基于A*和TEB融合的行人感知無碰跟隨方法

龐磊，曹志強，喻俊志

1. 中國科學院自動化研究所 復雜系統管理與控制國家重點實驗室，北京 100190 2.中國科學院大學 人工智能學院，北京 100049 3.北京大學 工學院 先進制造與機器人系，北京 100871

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

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

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

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

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

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

1 多傳感器行人跟隨系統

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

(1)

2 行人運動狀態估計

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

2.1 狀態預測

(2)

χi=

(3)

(4)

(5)

(6)

式中：Q為預測噪聲的協方差矩陣。

2.2 數據關聯和狀態更新

(7)

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

(8)

(9)

(10)

式中：R為測量噪聲協方差矩陣。

(11)

(12)

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

2.3 狀態更新和行人運動狀態估計

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

(13)

(14)

式中：PZkZk為Sigma點在測量空間的自相關運算。

然后，基于卡爾曼增益Kk和預測觀測值與真實測量值之間的差值更新系統狀態Xk和相應的協方差矩陣Pk：

(15)

(16)

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

3.1 分層式代價地圖

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

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

(17)

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

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

3.2 路徑規劃模塊

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

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

4 實驗驗證

4.1 行人運動狀態估計與動態行人避障實驗

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

在圖3(a)中創建了行人0的跟蹤軌跡，而此時行人1未達到創建跟蹤軌跡的條件，直到圖3(b)時才創建行人1的跟蹤軌跡。隨著行人1的運動，行人0在圖3(c)所示時刻被行人1完全遮擋，此時行人0的運動狀態由UKF的預測狀態提供。當行人0再次被檢測到(見圖3(d))，他被原有的跟蹤器繼續跟蹤而未切換ID。與圖3(c)類似的遮擋情況在圖3(e)和圖3(g)中再次出現，在這兩個場景中分別發生了行人2遮擋行人1以及行人3遮擋行人0和行人1。可以看出遮擋消失后的跟蹤軌跡依然能夠維持原有跟蹤軌跡ID，較好地實現了對行人狀態的預測。

圖3 多行人運動狀態估計實驗結果Fig.3 Experimental results of motion estimation of multiple pedestrians

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

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

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

4.2 機器人長距離跟隨實驗

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

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

5 結論和展望

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

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