龐磊，曹志強(qiáng)，喻俊志

1. 中國科學(xué)院自動化研究所 復(fù)雜系統(tǒng)管理與控制國家重點實驗室，北京 100190 2.中國科學(xué)院大學(xué) 人工智能學(xué)院，北京 100049 3.北京大學(xué) 工學(xué)院 先進(jìn)制造與機(jī)器人系，北京 100871

機(jī)器人跟隨行人可以簡化人與機(jī)器人之間的交互，使機(jī)器人跟隨指定行人到達(dá)期望地點的方式較為便捷，可以視為一種特殊的導(dǎo)航方式。行人跟隨在地面機(jī)器人、空中機(jī)器人和水下機(jī)器人中均有重要的應(yīng)用[1]?，F(xiàn)階段行人跟隨多在地面機(jī)器人中發(fā)揮作用，在助老助殘[2-3]、載物載貨[4]及軍事[5]等領(lǐng)域具有重要的應(yīng)用前景。

機(jī)器人實現(xiàn)行人跟隨需要具備定位目標(biāo)行人的能力[6]，并需要保持對目標(biāo)行人的跟隨狀態(tài)。在目標(biāo)行人定位方面，視覺傳感器和激光雷達(dá)被廣泛使用，此外，IMU(Inertial Measurement Unit)、 超聲波傳感器、RFID(Radio-Frequency IDentification)標(biāo)簽、UWB(Ultra Wide Band)定位模塊、藍(lán)牙傳感器等也可用于行人跟隨[7-9]。以穩(wěn)定的目標(biāo)行人定位信息為基礎(chǔ)，機(jī)器人以適當(dāng)?shù)姆绞脚c目標(biāo)行人保持一定的位置關(guān)系進(jìn)而維持跟隨狀態(tài)。根據(jù)不同的應(yīng)用場景，機(jī)器人需要與目標(biāo)行人保持不同的相對位置關(guān)系，常見的有身后跟隨、身前跟隨和身旁跟隨等模式。為了實現(xiàn)相應(yīng)的跟隨模式，常見的方法有基于目標(biāo)行人軌跡、基于目標(biāo)行人方向指引與局部避障以及基于路徑規(guī)劃的跟隨方法。

機(jī)器人沿著目標(biāo)行人的運動軌跡進(jìn)行跟隨是一種直觀的實現(xiàn)方案，需要機(jī)器人記錄目標(biāo)行人的歷史運動軌跡，并按照記錄的軌跡以一定的滯后距離或時間運動。Doisy等[10]提出了一種跟隨目標(biāo)行人路徑的跟隨方法，機(jī)器人通過記錄目標(biāo)行人運動軌跡并進(jìn)行采樣和篩選、平滑處理以控制機(jī)器人跟隨目標(biāo)行人。上述嚴(yán)格沿著目標(biāo)行人運動軌跡跟隨的方法僅適用于身后跟隨模式，對于身旁跟隨和身前跟隨模式來說，可以通過估計行人運動狀態(tài)、運動軌跡等方式實現(xiàn)跟隨。同時，基于預(yù)測目標(biāo)行人運動軌跡的方法提供了目標(biāo)行人在機(jī)器人傳感器視野短暫脫離問題的解決方案。Hu等[11]基于行人運動模型和無跡卡爾曼濾波器預(yù)測目標(biāo)行人的運動軌跡，使機(jī)器人可以做出期望的運動決策，從而在身后、身前和身旁模式下均可以有效跟隨目標(biāo)行人。

目標(biāo)行人方向指引以機(jī)器人與行人之間的距離和相對角度作為反饋，從而控制機(jī)器人的速度以保持對目標(biāo)行人的跟隨[12]。目標(biāo)行人方向指引通常表現(xiàn)為控制機(jī)器人使目標(biāo)行人位于傳感器的中間位置。在開闊場景中，直接基于方向指引能夠?qū)崿F(xiàn)機(jī)器人的行人跟隨，但是在復(fù)雜工作環(huán)境中，單純方向指引并不能保證機(jī)器人安全、有效地運行，為此需要添加額外的避障算法。文獻(xiàn)[13-14] 中的跟隨機(jī)器人均采用方向指引的方式跟隨目標(biāo)行人，并分別基于VFH(Vector Field Histogram)算法以及VFH與PFM(Potential Field Methods)相結(jié)合的避障算法躲避機(jī)器人與目標(biāo)行人之間的障礙物。

基于路徑規(guī)劃的跟隨方法是在獲取目標(biāo)行人與機(jī)器人相對位姿的基礎(chǔ)上，通過規(guī)劃機(jī)器人與目標(biāo)行人之間的最優(yōu)路徑實現(xiàn)跟隨。該類方法在考慮保持與目標(biāo)行人相對位置關(guān)系的同時兼顧避障，主要包括基于全局地圖輔助的方法[15]和基于相對位姿規(guī)劃的方法[16]。全局地圖輔助的方法通過在環(huán)境地圖中確定機(jī)器人與目標(biāo)行人的全局位置，進(jìn)而規(guī)劃兩者之間的最優(yōu)無碰路徑以實現(xiàn)行人跟隨，地圖的建立和機(jī)器人定位通常使用SLAM(Simultaneous Localization and Mapping)技術(shù)加以解決。基于相對位姿規(guī)劃的方法并不考慮機(jī)器人和目標(biāo)行人的全局位置，更傾向于以反應(yīng)式跟隨策略規(guī)劃機(jī)器人到目標(biāo)行人的局部路徑以實現(xiàn)跟隨，因此相對基于全局地圖輔助的方式更加靈活。Tasaki等[16]根據(jù)目標(biāo)行人在機(jī)器人坐標(biāo)系下的定位信息，使用RRT(Rapidly-exploring Random Tree)算法在機(jī)器人局部環(huán)境地圖上規(guī)劃無碰可行的路徑，進(jìn)而利用PID(Proportional-Integral-Derivative)控制器控制機(jī)器人跟隨路徑。

為使機(jī)器人在行人跟隨中具有較好的環(huán)境適應(yīng)能力，同時在行人跟隨過程中兼顧避障，提出了一種基于路徑規(guī)劃的無碰行人跟隨方法。基于激光點云分割提供的靜態(tài)障礙物信息及基于3D行人檢測的多行人UKF(Unscented Kalman Filter)跟蹤器估計的行人運動狀態(tài)，分別生成靜態(tài)障礙代價地圖和動態(tài)行人代價地圖，在此基礎(chǔ)上，通過低頻全局規(guī)劃和高頻局部規(guī)劃結(jié)合的方式實現(xiàn)對目標(biāo)行人安全、無碰地跟隨。

1 多傳感器行人跟隨系統(tǒng)

基于路徑規(guī)劃的無碰行人跟隨方法由感知模塊和規(guī)劃模塊組成。感知模塊負(fù)責(zé)目標(biāo)行人和環(huán)境中其他行人的檢測和定位問題，同時負(fù)責(zé)環(huán)境中障礙物的感知。為保證復(fù)雜環(huán)境下穩(wěn)定和準(zhǔn)確的行人定位，感知模塊基于由外參數(shù)標(biāo)定的RGB相機(jī)與多線激光雷達(dá)組成的多傳感器方案實現(xiàn)。首先，基于深度神經(jīng)網(wǎng)絡(luò)的YOLOv3[17]檢測器被用于在圖像中確定全部行人的位置；其次，激光雷達(dá)獲取的點云數(shù)據(jù)被分割為地面點云和非地面點云簇，其中非地面點云簇在后續(xù)機(jī)器人跟隨過程中將被進(jìn)一步用于避障；此外，通過在圖像坐標(biāo)系中融合由行人檢測器提供的視覺行人語義信息與非地面物體點云簇，可以實現(xiàn)3D行人(包括目標(biāo)行人和干擾行人)定位和點云簇分類，這里點云簇被分為行人點云簇和非行人點云簇。基于獲得的3D行人定位信息，機(jī)器人需要進(jìn)一步在多個行人之間確認(rèn)目標(biāo)行人，為此卡爾曼濾波器結(jié)合行人重識別(person re-identification)模塊的數(shù)據(jù)關(guān)聯(lián)方法被用于在連續(xù)的圖像幀間持續(xù)地關(guān)聯(lián)檢測數(shù)據(jù)，進(jìn)而確定目標(biāo)行人的身份。除目標(biāo)行人之外的其他行人將被進(jìn)一步跟蹤并估計運動信息，從而結(jié)合靜態(tài)障礙物信息實現(xiàn)機(jī)器人行人跟隨過程中行人感知的避障。感知模塊中算法的具體描述參考文獻(xiàn)[18-19]。

(1)

2 行人運動狀態(tài)估計

以感知模塊提供的3D行人檢測結(jié)果為基礎(chǔ)，通過基于UKF的跟蹤器預(yù)測環(huán)境中其他行人的運動狀態(tài)，進(jìn)而基于行人的運動方向和速度預(yù)測行人未來一段時間的運動軌跡，使機(jī)器人可以預(yù)先規(guī)劃更合理的路徑繞過行人，從而更好地保障環(huán)境中行人的安全以及提高自身運動效率。

2.1 狀態(tài)預(yù)測

(2)

χi=

(3)

(4)

(5)

(6)

式中：Q為預(yù)測噪聲的協(xié)方差矩陣。

2.2 數(shù)據(jù)關(guān)聯(lián)和狀態(tài)更新

(7)

Zk,i=h(χk,i,0)i=0,1,…,2nx

(8)

(9)

(10)

式中：R為測量噪聲協(xié)方差矩陣。

(11)

(12)

式中：B為常數(shù)，負(fù)責(zé)處理由于檢測、遮擋、錯誤引起的非均勻概率。在NN-JPDA算法中，最多僅有一個測量值與已有的跟蹤軌跡對應(yīng)，將最大βk,ji對應(yīng)的第i個測量值和第j個跟蹤軌跡進(jìn)行配對，而后按照同樣的思路對剩余的M-1個跟蹤軌跡和N-1個測量值進(jìn)行配對，直到全部的跟蹤軌跡或測量值完成配對，或者滿足βk,ji<η為止，其中η為預(yù)設(shè)閾值。

2.3 狀態(tài)更新和行人運動狀態(tài)估計

當(dāng)跟蹤軌跡獲得與之匹配的測量值時，將更新UKF的系統(tǒng)狀態(tài)。首先，計算tk時刻Sigma點在狀態(tài)空間和測量空間的互相關(guān)運算Ck，并根據(jù)Ck和Sk計算卡爾曼增益Kk：

(13)

(14)

式中：PZkZk為Sigma點在測量空間的自相關(guān)運算。

然后，基于卡爾曼增益Kk和預(yù)測觀測值與真實測量值之間的差值更新系統(tǒng)狀態(tài)Xk和相應(yīng)的協(xié)方差矩陣Pk：

(15)

(16)

3 基于路徑規(guī)劃的無碰行人跟隨方法

3.1 分層式代價地圖

根據(jù)感知模塊提供的靜態(tài)障礙物信息和動態(tài)行人的運動狀態(tài)分別生成靜態(tài)障礙代價地圖和動態(tài)行人代價地圖，其中代價地圖使用分層式代價地圖框架[21]進(jìn)行維護(hù)和更新。分層式代價地圖框架中的每一個代價地圖層均采用柵格地圖形式，能夠獨立處理來自不同信息源的障礙物并生成相應(yīng)的代價地圖。圖1給出了基于分層式代價地圖的路徑規(guī)劃示意圖。靜態(tài)障礙代價地圖源自環(huán)境中的非行人障礙物，利用感知模塊提供的非行人點云簇在地面投影區(qū)域生成障礙柵格和相應(yīng)膨脹，如圖1中的藍(lán)色區(qū)域所示；依據(jù)3D行人檢測結(jié)果(目標(biāo)行人除外)預(yù)測行人運動狀態(tài)，并生成動態(tài)行人代價地圖，如圖1中的粉色線段所示。動態(tài)行人代價地圖考慮環(huán)境中干擾行人的運動狀態(tài)，有利于機(jī)器人更好地躲避動態(tài)障礙物。

圖1 基于分層式代價地圖的路徑規(guī)劃示意圖Fig.1 Schematic diagram of path planning with layered cost maps

(17)

式中：ρ為速度系數(shù)；v為行人速度。此外，在行人運動的反方向設(shè)立固定長度Rd的虛擬障礙。這兩部分虛擬障礙構(gòu)成完整的動態(tài)行人代價地圖，如圖2(b)粉色線段所示。

圖2 動態(tài)行人代價地圖生成示意圖Fig.2 Schematic diagram of dynamic pedestrian cost map

3.2 路徑規(guī)劃模塊

路徑規(guī)劃模塊包括全局規(guī)劃器和局部規(guī)劃器。全局規(guī)劃器以靜態(tài)障礙代價地圖作為輸入，在規(guī)劃路徑時不考慮機(jī)器人的機(jī)械性能和運動學(xué)約束，通過A*算法[22]規(guī)劃出從機(jī)器人當(dāng)前位置到期望目標(biāo)位置的最優(yōu)路徑，并為局部規(guī)劃器提供初始值。局部規(guī)劃器在全局最優(yōu)路徑上采樣路徑點，并優(yōu)化機(jī)器人當(dāng)前位置與采樣路徑點之間的全局路徑子集，同時結(jié)合靜態(tài)障礙代價地圖和動態(tài)行人代價地圖，在跟隨全局路徑的同時將機(jī)器人的外形、動力學(xué)模型、運動性能等納入局部規(guī)劃的范圍，這里使用TEB(Timed-Elastic-Band)算法[23]實現(xiàn)?？紤]全局規(guī)劃器和局部規(guī)劃器不同的規(guī)劃范圍等因素，兩者分別以1 Hz和5 Hz的頻率運行。

機(jī)器人執(zhí)行已規(guī)劃的路徑需具備局部定位能力，使用機(jī)器人底盤的里程計提供機(jī)器人短時定位信息和運動狀態(tài)反饋。局部規(guī)劃器以機(jī)器人里程計作為反饋輸出機(jī)器人控制指令以跟隨規(guī)劃的路徑，從而實現(xiàn)對目標(biāo)行人的持續(xù)跟隨。由于本方法以較高的頻率進(jìn)行路徑規(guī)劃，且期望的目標(biāo)位姿相對于機(jī)器人坐標(biāo)系計算，因此所提方法在長距離跟隨中并不會受累積誤差的影響。

4 實驗驗證

4.1 行人運動狀態(tài)估計與動態(tài)行人避障實驗

首先對基于UKF的多行人跟蹤器進(jìn)行測試，同時考慮行人交錯的情況用于驗證行人跟蹤軌跡的持續(xù)性。實驗機(jī)器人保持靜止?fàn)顟B(tài)以便于更好地測試跟蹤器性能。實驗結(jié)果如圖3所示，其中第1行給出了圖像中被跟蹤行人的位置，第2行中藍(lán)色3D包圍框表示三維行人檢測方法輸出的結(jié)果，多行人跟蹤器輸出的結(jié)果用虛擬行人模型給出，其頭上數(shù)字為跟蹤軌跡ID、箭頭指示行人運動方向，地面上的彩色曲線描述了各行人的歷史運動軌跡。

在圖3(a)中創(chuàng)建了行人0的跟蹤軌跡，而此時行人1未達(dá)到創(chuàng)建跟蹤軌跡的條件，直到圖3(b)時才創(chuàng)建行人1的跟蹤軌跡。隨著行人1的運動，行人0在圖3(c)所示時刻被行人1完全遮擋，此時行人0的運動狀態(tài)由UKF的預(yù)測狀態(tài)提供。當(dāng)行人0再次被檢測到(見圖3(d))，他被原有的跟蹤器繼續(xù)跟蹤而未切換ID。與圖3(c)類似的遮擋情況在圖3(e)和圖3(g)中再次出現(xiàn)，在這兩個場景中分別發(fā)生了行人2遮擋行人1以及行人3遮擋行人0和行人1?？梢钥闯稣趽跸Ш蟮母欆壽E依然能夠維持原有跟蹤軌跡ID，較好地實現(xiàn)了對行人狀態(tài)的預(yù)測。

圖3 多行人運動狀態(tài)估計實驗結(jié)果Fig.3 Experimental results of motion estimation of multiple pedestrians

在基于UKF的多行人跟蹤器基礎(chǔ)上，結(jié)合動態(tài)行人代價地圖，對機(jī)器人避讓行人的能力進(jìn)行測試。圖4給出了實驗結(jié)果，其中每組數(shù)據(jù)由3部分組成：上方圖像為第三視角圖像，左下角圖像為機(jī)器人相機(jī)采集的圖像及其圖像處理結(jié)果，右下角圖片包含了激光點云數(shù)據(jù)、干擾行人跟蹤結(jié)果和路徑規(guī)劃結(jié)果。

當(dāng)機(jī)器人運行到圖4(a)所示位置時，多行人跟蹤器開始跟蹤和預(yù)測干擾行人A的運動狀態(tài)。當(dāng)行人A運動到圖4(b)位置時，機(jī)器人根據(jù)實時代價地圖規(guī)劃從行人A的前方繞過，接著行人A快速到達(dá)機(jī)器人前面，機(jī)器人轉(zhuǎn)而減速并重新規(guī)劃為從行人A的身后繞行，如圖4(c)所示。之后干擾行人B在機(jī)器人與目標(biāo)行人之間穿行，如圖4(d)所示，此時機(jī)器人規(guī)劃從行人B身后繞過。需要說明的是，在行人B脫離機(jī)器人相機(jī)視野后，雖然3D行人檢測已經(jīng)無法提供行人B的測量數(shù)據(jù)，但是跟蹤器仍然會利用運動模型繼續(xù)預(yù)測行人的運動狀態(tài)，如圖4(e)所示，直到行人B的運動軌跡被清除。該實驗表明了機(jī)器人能夠在跟隨過程中躲避動態(tài)行人。

圖4 機(jī)器人行人跟隨過程中的動態(tài)避障實驗結(jié)果Fig.4 Experimental results of dynamic obstacle avoidance in process of mobile robot’s person following

4.2 機(jī)器人長距離跟隨實驗

為進(jìn)一步驗證所提方法的性能，進(jìn)行長距離行人跟隨實驗。圖5給出了跟隨實驗結(jié)果，其中中間圖像為衛(wèi)星地圖以及運動路線，周圍圖像描述了衛(wèi)星圖中各標(biāo)記位置對應(yīng)的機(jī)器人跟隨情況。實驗中機(jī)器人的平均運動速度約0.9 m/s，全程總計800余米。實驗表明基于所提方法機(jī)器人可以在室外環(huán)境中穩(wěn)定跟隨目標(biāo)行人。

圖5 移動機(jī)器人長距離跟隨實驗結(jié)果Fig.5 Results of long-distance person following of mobile robot

5 結(jié)論和展望

提出了一種基于路徑規(guī)劃的無碰行人跟隨方法，基于激光點云分割提供的靜態(tài)障礙物信息及基于3D行人檢測的多行人UKF跟蹤器估計的行人運動狀態(tài)，分別生成靜態(tài)障礙代價地圖和動態(tài)行人代價地圖，在此基礎(chǔ)上，通過低頻全局規(guī)劃和高頻局部規(guī)劃結(jié)合的方式實現(xiàn)對目標(biāo)行人安全、無碰地跟隨。所提機(jī)器人跟隨方法同時適用于室內(nèi)和室外環(huán)境，但是在特別狹窄的場景中路徑規(guī)劃方法的規(guī)劃速度會受到影響，因此不太適用于過于狹窄的場景。此外，由于感知模塊需要使用RGB相機(jī)提供的圖像語義信息，因此無法在夜晚暗光條件下提供準(zhǔn)確的定位信息。

在后續(xù)工作中，將繼續(xù)提高機(jī)器人在人類環(huán)境中運行的安全性以及密集人群場景下機(jī)器人社會性的研究，在機(jī)器人跟隨中考慮靠右行走、文明禮讓等更多社會規(guī)范。