專利名稱：一種基于多傳感器服務(wù)機(jī)器人跟隨系統(tǒng)和方法
技術(shù)領(lǐng)域：
本發(fā)明屬于機(jī)器人領(lǐng)域，涉及一種機(jī)器人跟隨系統(tǒng)和方法，確切的說，是一種根據(jù)多傳感器融合的量測信息實(shí)現(xiàn)機(jī)器人自主跟隨目標(biāo)人的系統(tǒng)和方法。
背景技術(shù)：
目標(biāo)人跟隨是人機(jī)交互研究中的一個(gè)熱點(diǎn)問題，不僅在機(jī)器人領(lǐng)域具有廣泛的應(yīng)用需求，例如協(xié)助殘疾人和病人移動的機(jī)器人輪椅、跟在主人身后搬運(yùn)行李重物的機(jī)器人等，而且對于偵察型軍用機(jī)器人也具有應(yīng)用價(jià)值。跟隨過程往往發(fā)生在包括目標(biāo)人和非目標(biāo)人的多人自然環(huán)境中，因而機(jī)器人需要在避開障礙物的同時(shí)準(zhǔn)確跟隨指定目標(biāo)。目前已有的機(jī)器人跟隨系統(tǒng)主要采用單一激光傳感器，對目標(biāo)人的采集方法較為單一，易受環(huán)境中相似物體干擾。機(jī)器人跟隨的方法主要為基于采樣的聯(lián)合概率濾波算法 (SJPDAF)，此算法適于多目標(biāo)人跟蹤，但是過去此算法也只是采用單一激光傳感器。機(jī)器人在跟隨過程中環(huán)境較為復(fù)雜，可能出現(xiàn)靜態(tài)或動態(tài)障礙物。因此要求機(jī)器人在跟隨過程中有避障能力。目前的避障方法主要有動態(tài)窗口法(DWA)、勢場法和速度障礙法(VO)等。這些算法的復(fù)雜度較高，有容易陷入局部最小的缺陷。中國專利號200910101604. 4的專利公開了一種多機(jī)器人跟蹤移動目標(biāo)算法，使用移動目標(biāo)的當(dāng)前位置信息和前一時(shí)刻的位置信息構(gòu)造目標(biāo)的運(yùn)動模型，并用運(yùn)動預(yù)測模型構(gòu)造機(jī)器人運(yùn)動控制模型，使用協(xié)方差插值方法對移動目標(biāo)位置估計(jì)。該發(fā)明保證移動目標(biāo)始終處于多機(jī)器人可視范圍。但是該發(fā)明無法同時(shí)對多個(gè)目標(biāo)進(jìn)行跟蹤，也沒有考慮跟蹤過程中的避障問題。
發(fā)明內(nèi)容
本發(fā)明的目的是針對現(xiàn)有技術(shù)在機(jī)器人跟隨目標(biāo)人時(shí)存在的不足，提出一種基于多傳感器的機(jī)器人跟隨系統(tǒng)和方法，使得服務(wù)機(jī)器人在復(fù)雜室內(nèi)環(huán)境下跟隨目標(biāo)人的同時(shí)具有良好的避障能力。本發(fā)明解決技術(shù)問題的具體技術(shù)方案如下本發(fā)明是一種適于融合多傳感器的機(jī)器人跟隨系統(tǒng)，包括傳感器檢測部分、檢測融合部分、運(yùn)動執(zhí)行部分和上位主控機(jī)，傳感器檢測部分是采集視野中所有類似人的位置， 并將所有的量測信息發(fā)送給上位主控機(jī)，檢測融合部分是上位主控機(jī)采用基于采樣的聯(lián)合概率濾波算法對目標(biāo)人的位置進(jìn)行更新，運(yùn)動執(zhí)行部分是根據(jù)當(dāng)前目標(biāo)人信息和障礙物信息生成運(yùn)動目標(biāo)并執(zhí)行跟蹤與跟隨。本發(fā)明所述的傳感器檢測部分包括多種傳感器，多種傳感器包括激光測距儀和深度相機(jī)。本發(fā)明所述的檢測融合部分是上位主控機(jī)運(yùn)行的融合多種傳感器的基于采樣的聯(lián)合概率濾波算法。本發(fā)明所述的運(yùn)動執(zhí)行部分包括底盤和云臺。
本發(fā)明是一種適于融合多傳感器的機(jī)器人跟隨系統(tǒng)的使用方法，包括以下步驟(1)使用機(jī)器人的激光傳感器對視野所有目標(biāo)人定位，同時(shí)使用深度相機(jī)對所有目標(biāo)人進(jìn)行頭肩檢測并定位；(2)使用基于采樣的聯(lián)合概率濾波算法對所有目標(biāo)人的位置進(jìn)行更新，即根據(jù)量測信息生成每個(gè)目標(biāo)人當(dāng)前時(shí)刻的位置信息；(3)使用更新的被跟隨的目標(biāo)人信息，結(jié)合前方的障礙物信息進(jìn)行避障跟隨。本發(fā)明使用方法所述的激光傳感器采用圓弧檢測方法對類似人進(jìn)行檢測，深度相機(jī)采用基于AdaBoost的快速人臉檢測方法，進(jìn)行目標(biāo)人的頭肩特征的檢測。本發(fā)明使用方法融合了多種傳感器的量測信息，將其應(yīng)用于基于采樣的聯(lián)合概率濾波算法(SJPDAF)中。本發(fā)明所述的使用方法在更新目標(biāo)人位置的條件下，根據(jù)前方障礙物信息，采用了一種障礙物搜索半徑收縮的方法快速生成運(yùn)動規(guī)劃的目標(biāo)點(diǎn)。本發(fā)明的有益效果表現(xiàn)在，通過融合多種傳感器的量測信息，結(jié)合基于采樣的聯(lián)合概率濾波算法和避障算法對目標(biāo)人進(jìn)行無碰跟隨，保證機(jī)器人可良好地在室內(nèi)有干擾人的情況下實(shí)現(xiàn)自主跟隨，同時(shí)提高了跟隨的魯棒性和跟蹤精度。
圖1是機(jī)器人跟隨系統(tǒng)結(jié)構(gòu)示意圖；圖2是機(jī)器人動態(tài)規(guī)劃可行目標(biāo)點(diǎn)示意圖。
具體實(shí)施例方式下面結(jié)合附圖和具體實(shí)施例對發(fā)明的技術(shù)方案作進(jìn)一步詳細(xì)說明圖1是本發(fā)明提供的機(jī)器人跟隨系統(tǒng)的結(jié)構(gòu)示意圖。如圖1所示，本發(fā)明機(jī)器人跟隨系統(tǒng)包括傳感器檢測部分、檢測融合部分和運(yùn)動執(zhí)行部分。采用激光和深度相機(jī)進(jìn)行目標(biāo)人的采集，上位主控機(jī)采用基于采樣的聯(lián)合概率濾波算法(SJPDAF)對目標(biāo)人的位置進(jìn)行更新，云臺和底盤根據(jù)更新的目標(biāo)人位置進(jìn)行跟蹤與跟隨。傳感器檢測部分負(fù)責(zé)采集視野中所有類似人的位置和底盤障礙物信息，并將所有的量測信息發(fā)送給上位主控機(jī)。傳感器檢測部分包括多種傳感器，多種傳感器包括激光測距儀和深度相機(jī)，激光測距儀包括機(jī)器人的胸口激光測距儀和底盤激光測距儀。其中，胸口激光測距儀為HOKUYO UTM-30LX型激光測距儀，機(jī)器人頭部的深度相機(jī)為MESA SR4000相機(jī)，底盤激光測距儀為SICK-100型激光測距儀。檢測融合部分負(fù)責(zé)根據(jù)量測信息生成每個(gè)目標(biāo)人當(dāng)前時(shí)刻的位置信息，即上位主控機(jī)根據(jù)胸口激光測距儀和深度相機(jī)的量測信息生成當(dāng)前時(shí)刻視野內(nèi)每個(gè)人的位置信息， 同時(shí)根據(jù)底盤激光測距儀采集的障礙物信息規(guī)劃下一時(shí)刻機(jī)器人運(yùn)動目標(biāo)點(diǎn)。運(yùn)動執(zhí)行部分負(fù)責(zé)根據(jù)當(dāng)前目標(biāo)人信息和障礙物信息生成運(yùn)動目標(biāo)并執(zhí)行。本發(fā)明提供的基于多種傳感器的機(jī)器人跟隨方法步驟如下首先，使用機(jī)器人胸部的激光傳感器采用圓弧檢測方法，對視野內(nèi)的類似人進(jìn)行檢測并定位，同時(shí)使用深度相機(jī)采用基于AdaBoost的快速人臉檢測方法，對所有目標(biāo)人進(jìn)行頭肩檢測并定位，所有的傳感器量測信息都發(fā)送給上位主控機(jī)。
其次，使用基于采樣的聯(lián)合概率濾波算法(SJPDAF)對所有目標(biāo)人的位置進(jìn)行更新。假設(shè)t時(shí)刻有η個(gè)跟蹤目標(biāo)，mt個(gè)觀測。由于我們只能跟隨一個(gè)目標(biāo)運(yùn)動，所以只關(guān)注被跟隨目標(biāo)的t時(shí)刻的狀態(tài)，記為x(t) ；Z(t)表示t時(shí)刻所有傳感器的觀測的組合序列，記 Z(O = Iz1^)L、⑴}，Zj(t)表示t時(shí)刻這組觀測中的一個(gè)觀測。β」表示觀測j對目標(biāo)的關(guān)聯(lián)概率。假設(shè)被跟隨目標(biāo)的粒子濾波器有K個(gè)粒子。結(jié)合量測信息的多傳感器融合SJPDAF算法具體的步驟如下(1)在預(yù)測階段根據(jù)預(yù)測方程更新前一時(shí)刻粒子群的狀態(tài)；(2)根據(jù)下式計(jì)算各個(gè)觀測對于被跟蹤目標(biāo)的關(guān)聯(lián)概率β j,
mt ι Kβ=Σ^Π^Σ^ωι^(0)]
6 εΘJ=Ik=\其中θ表示觀測特征j與目標(biāo)的關(guān)聯(lián)集合，Θ表示將觀測j與目標(biāo)關(guān)聯(lián)的所有可能的關(guān)聯(lián)事件組合，xk (t)表示t時(shí)刻第k個(gè)粒子的狀態(tài)，α表示歸一化因子；(3)根據(jù)下式使用傳感器當(dāng)前時(shí)刻的量測信息估計(jì)被跟隨目標(biāo)的狀態(tài)，
mtp(x(t) I Z(O) = ^X β}ρ{ζ} (0 ι x{t))p{x{t) ι Z{t -1));
J=Ot時(shí)刻目標(biāo)第k個(gè)粒子的權(quán)重Wk (t)為
mtwk{t) = aYjβ]Ρ{ζ}(01 Xk(0)。
J=O最后，使用更新的被跟隨的目標(biāo)人信息，結(jié)合前方的障礙物信息進(jìn)行避障跟隨。動態(tài)規(guī)劃可行目標(biāo)點(diǎn)的思路如圖2示首先根據(jù)底盤激光檢測的信息以及機(jī)器人半徑擴(kuò)充障礙物區(qū)間；其次以目標(biāo)人方向?yàn)橹行南蜃笥曳謩e搜索可行目標(biāo)點(diǎn)，如果找到可行規(guī)劃目標(biāo)點(diǎn)P，算法退出；最后如果在當(dāng)前障礙物搜索半徑R范圍上沒有合適的可行路徑，適當(dāng)縮小搜索障礙物半徑R重復(fù)上面兩個(gè)步驟直到找到一個(gè)可行的目標(biāo)點(diǎn)。如果搜索半徑縮小到機(jī)器人底盤半徑大小還沒找到可行的目標(biāo)點(diǎn)，那么說明機(jī)器人前方?jīng)]有可行路徑，機(jī)器人保持靜止。機(jī)器人實(shí)物在復(fù)雜環(huán)境下跟隨目標(biāo)人無碰運(yùn)動。機(jī)器人在跟隨過程中有動態(tài)干擾人出現(xiàn)，同時(shí)地面也有靜態(tài)障礙物。機(jī)器人能夠通過SJPDAF算法跟蹤多個(gè)目標(biāo)人，使得機(jī)器人不被動態(tài)干擾人影響。機(jī)器人根據(jù)底盤激光測距儀采集的障礙物信息搜索合理的運(yùn)動目標(biāo)點(diǎn)，使得機(jī)器人能夠在狹小的空間內(nèi)無碰運(yùn)動，體現(xiàn)了避障方法的有效性。以上列舉的僅是本發(fā)明的一個(gè)具體實(shí)施例，顯然，本發(fā)明不限于以上實(shí)施例，還可以有許多變形，本領(lǐng)域的普通技術(shù)人員能從本發(fā)明公開的內(nèi)容直接導(dǎo)出或聯(lián)想到的所有變形，均應(yīng)認(rèn)為是本發(fā)明的保護(hù)范圍。
權(quán)利要求
1.一種適于融合多傳感器的機(jī)器人跟隨系統(tǒng)，包括傳感器檢測部分、檢測融合部分、運(yùn)動執(zhí)行部分和上位主控機(jī)，其特征在于，所述的傳感器檢測部分是采集視野中所有類似人的位置，并將所有的量測信息發(fā)送給上位主控機(jī)，所述的檢測融合部分是上位主控機(jī)采用基于采樣的聯(lián)合概率濾波算法對目標(biāo)人的位置進(jìn)行更新，所述的運(yùn)動執(zhí)行部分是根據(jù)當(dāng)前目標(biāo)人信息和障礙物信息生成機(jī)器人運(yùn)動規(guī)劃點(diǎn)并執(zhí)行跟蹤與跟隨。
2.如權(quán)利要求1所述的機(jī)器人跟隨系統(tǒng)，其特征在于，所述的傳感器檢測部分包括多種傳感器。
3.如權(quán)利要求2所述的機(jī)器人跟隨系統(tǒng)，其特征在于，所述的多種傳感器包括激光測距儀和深度相機(jī)。
4.如權(quán)利要求1所述的機(jī)器人跟隨系統(tǒng)，其特征在于，所述的檢測融合部分是上位主控機(jī)運(yùn)行的融合多種傳感器的基于采樣的聯(lián)合概率濾波算法。
5.如權(quán)利要求1所述的機(jī)器人跟隨系統(tǒng)，其特征在于，所述的運(yùn)動執(zhí)行部分包括底盤和云臺。
6.一種適于融合多傳感器的機(jī)器人跟隨系統(tǒng)的使用方法，其特征在于，包括以下步驟(1)使用機(jī)器人的激光傳感器對視野所有目標(biāo)人定位，同時(shí)使用深度相機(jī)對所有目標(biāo)人進(jìn)行頭肩檢測并定位；(2)使用基于采樣的聯(lián)合概率濾波算法對所有目標(biāo)人的位置進(jìn)行更新，即根據(jù)量測信息生成每個(gè)目標(biāo)人當(dāng)前時(shí)刻的位置信息；(3)使用更新的被跟隨的目標(biāo)人信息，結(jié)合前方的障礙物信息進(jìn)行避障跟隨。
7.如權(quán)利要求6所述的機(jī)器人跟隨系統(tǒng)的使用方法，其特征在于，所述的激光傳感器采用圓弧檢測方法對類似人進(jìn)行檢測，深度相機(jī)采用基于AdaBoost的快速人臉檢測方法， 進(jìn)行目標(biāo)人的頭肩特征的檢測。
8.如權(quán)利要求6所述的機(jī)器人跟隨系統(tǒng)的使用方法，其特征在于，融合了多種傳感器的量測信息，將其應(yīng)用于基于采樣的聯(lián)合概率濾波算法(SJPDAF)中。
9.如權(quán)利要求6所述的機(jī)器人跟隨系統(tǒng)的使用方法，其特征在于，在更新目標(biāo)人位置的條件下，根據(jù)前方障礙物信息，采用了一種障礙物搜索半徑收縮的方法快速生成運(yùn)動規(guī)劃的目標(biāo)點(diǎn)。
全文摘要
本發(fā)明公開一種根據(jù)多傳感器融合的量測信息實(shí)現(xiàn)機(jī)器人自主跟隨目標(biāo)人的系統(tǒng)和方法，包括傳感器檢測部分、檢測融合部分、運(yùn)動執(zhí)行部分和上位主控機(jī)，傳感器檢測部分是采集視野中所有類似人的位置，并將所有的量測信息發(fā)送給上位主控機(jī)，檢測融合部分是上位主控機(jī)采用基于采樣的聯(lián)合概率濾波算法對目標(biāo)人的位置進(jìn)行更新，運(yùn)動執(zhí)行部分是根據(jù)當(dāng)前目標(biāo)人信息和障礙物信息生成運(yùn)動目標(biāo)并執(zhí)行跟蹤與跟隨，本發(fā)明通過融合多種傳感器的量測信息，結(jié)合基于采樣的聯(lián)合概率濾波算法和避障算法對目標(biāo)人進(jìn)行無碰跟隨，保證機(jī)器人可良好地在室內(nèi)有干擾人的情況下實(shí)現(xiàn)自主跟隨，同時(shí)提高了跟隨的魯棒性和跟蹤精度。
文檔編號G05D1/02GK102411371SQ20111037057
公開日2012年4月11日 申請日期2011年11月18日 優(yōu)先權(quán)日2011年11月18日
發(fā)明者劉南洋, 李千山, 熊蓉, 王軍南, 褚健 申請人:浙江大學(xué)