基于全局SLAM的四足機器人路徑規(guī)劃研究

劉宇飛，江 磊，邢伯陽，王志瑞

（中國北方車輛研究所兵器地面平臺無人中心，北京 100072）

1 引 言

為了實現(xiàn)四足機器人在未知崎嶇地形行走，盡管可以通過本體傳感器獲得機器人的行走狀態(tài)，但如果沒有感知系統(tǒng)，機器人將無法在崎嶇地形上自主行走。基于環(huán)境感知的四足機器人路徑規(guī)劃可以實現(xiàn)足式機器人的自主行走。

波士頓動力公司開發(fā)的小狗機器人是經(jīng)典的四足機器人，此機器人已經(jīng)在環(huán)境建模和運動規(guī)劃中進行了研究[1-2]。研究人員使用視覺傳感器來構(gòu)建地形環(huán)境，并基于視覺傳感器生成三維地圖以完成路徑規(guī)劃[3]。波士頓動力公司開發(fā)的大型四足機器人配備了GPS、IMU、激光和雙目相機傳感器[4]。實現(xiàn)了視覺測量、環(huán)境地圖構(gòu)建和定位，可以通過感知信息判斷機器人的合適落足點[5]。

Belter[6]研究了基于視覺傳感器的機器人環(huán)境感知和建模。視覺和慣性導(dǎo)航傳感器的組合可以實現(xiàn)姿態(tài)和運動參數(shù)的估計。研究內(nèi)容包括模擬環(huán)境中的地形識別與感知、落足點選擇和路徑規(guī)劃[7]。Fankhauser[8]提出了一種基于外部視覺感知的四足機器人運動規(guī)劃方法。根據(jù)獲得的地形高程圖，規(guī)劃擺動腿落足點未知，并且基于優(yōu)化方法完成四足機器人攀爬障礙物和樓梯。控制框架包含地圖建模、狀態(tài)估計、運動控制和規(guī)劃，并將所提出的方法在四足機器人上進行了驗證。

基于EKF 的SLAM 方法是解決定位問題的最早方法[9]，但由于EKF 的計算復(fù)雜度較高，因此會降低定位效率。早期的稀疏EIF 算法使用稀疏結(jié)構(gòu)來減少計算量，但結(jié)構(gòu)的稀疏性將導(dǎo)致結(jié)果的發(fā)散，因此學(xué)者開始研究SLAM 問題的稀疏結(jié)構(gòu)[10-11]。基于姿態(tài)濾波和稀疏特征點提出了稀疏濾波方法[12]。為了提高濾波算法的精度，提出了一種滑動窗口濾波的方法來提高精度，包含了前一時刻的姿態(tài)信息。

足式機器人的導(dǎo)航規(guī)劃可以分為全局地圖的路徑規(guī)劃和局部地圖的落足點規(guī)劃兩部分。全局路徑規(guī)劃可以歸結(jié)為優(yōu)化問題，其中優(yōu)化的目標函數(shù)是路徑。優(yōu)化問題的約束條件是速度約束以及起點和終點的位置約束，學(xué)者們通過將約束問題轉(zhuǎn)化為非約束問題來解決此優(yōu)化問題[13]。路徑規(guī)劃問題轉(zhuǎn)化為一種圖形方法，包括幾何方法和采樣方法。在成本函數(shù)方面，采用了人為設(shè)計的成本函數(shù)[13]，但是人為設(shè)計的函數(shù)很難覆蓋潛在的問題空間，因此最終提出了一種基于示教的成本函數(shù)方法。

本文的主要貢獻是開發(fā)一種旨在實現(xiàn)自主行走的感知和導(dǎo)航算法，提出了全局SLAM 定位算法來實現(xiàn)地圖的重建和定位，提出了用于全局和局部地圖的路徑規(guī)劃以實現(xiàn)自主導(dǎo)航和避障任務(wù)。

2 基于激光雷達的地形構(gòu)建

激光雷達可以得到地形環(huán)境下的點云信息，通過對點云信息的分析和處理，建立三維地形圖。為了更準確地描述地形信息，建立了基于地形建模的4 個坐標系系統(tǒng)，如圖1 所示。坐標系系統(tǒng)分別為大地坐標系OW、本體坐標系OB、傳感器坐標系OV和腿部坐標系OL[14]。腿部坐標系為傳感坐標系為為了建立基于機體質(zhì)心的地形信息，將激光雷達信息轉(zhuǎn)換到機體坐標系下：

圖1 地形建?？蚣芟碌淖鴺讼迪到y(tǒng)Fig.1 Coordinate system for terrain modeling framework

式中，TBW為機體坐標系和大地坐標系之間的轉(zhuǎn)換矩陣，它通過IMU 數(shù)據(jù)和機器人本體數(shù)據(jù)得到；TVW為傳感坐標系和大地坐標系之間的轉(zhuǎn)換矩陣。

通過激光雷達得到的點云信息生成地形高程圖。在地形坐標系上，高度測量被近似成一個高斯概率分布，其中高度平均值為p，標準差為σ2p。如果通過激光傳感器可以得到障礙物在傳感器坐標系下的位置VrVP，即可以得到在大地坐標系下的高度測量值：

由于在高度測量過程中，物體在兩個坐標系統(tǒng)下的橫縱坐標不變，因此矩陣P=[0 0 1]。高度測量的標準差如下所示：

式中，ΣV為激光傳感器模型的協(xié)方差矩陣；ΣP,q為傳感器坐標系旋轉(zhuǎn)的協(xié)方差矩陣；JV為傳感器測量的雅克比矩陣；Jq為傳感器坐標旋轉(zhuǎn)的雅克比矩陣。

針對地形高程圖的云圖更新，采用擴展卡爾曼濾波的方式對更新數(shù)據(jù)進行數(shù)據(jù)融合。擴展卡爾曼濾波方程中的狀態(tài)變量等同于地圖信息中的高度信息。擴展卡爾曼濾波更新方程如下所示：

在狀態(tài)更新方程中，狀態(tài)變量即是高度信息，Hk為單位矩陣，協(xié)方差矩陣Pk等同于高度估計的標準差。因此，將地形高程圖估計值通過擴展卡爾曼濾波的方式融合到高度測量值中。

最終可以得到實時更新的地形建模高度信息和高度的標準差，從而得到實時的地形信息。

3 SLAM 全局定位模塊構(gòu)建

3.1 自主定位模塊化設(shè)計

自主定位模塊將融合GPS、IMU、里程計等導(dǎo)航定位信息與全局地圖信息、激光雷達信息，完成四足機器人自主行走中準確位姿信息的自主獲取，保證機器人能夠進行準確的人員與軌跡跟蹤。

在本系統(tǒng)的自主定位中，考慮足式機器人大環(huán)境自足行走時，需要考慮環(huán)境過大帶來的地圖全局一致性無法滿足的問題，以及地圖過大帶來的占用內(nèi)存過大的問題。本系統(tǒng)采用拓撲度量定位框架下的地圖動態(tài)加載方法，將全局地圖劃分成一系列局部地圖，并以帶尺度的拓撲地圖表示。在定位中，系統(tǒng)采用當前定位結(jié)果動態(tài)加載鄰近地圖的方式進行信息融合，并采用記憶機制來克服運動中定位融合匹配失敗問題。

3.2 拓撲度量定位框架設(shè)計

在已有全局地圖的前提下，系統(tǒng)將全局地圖劃分成一系列局部地圖，并保證有一定的重疊。地圖的表示形式為拓撲地圖，具體為無向圖，圖中的節(jié)點為局部地圖，邊為局部地圖之間的相對變換。在定位過程中，主要分為兩層定位步驟，其中第一層負責選取一個節(jié)點作為參考點，即根據(jù)前一次定位結(jié)果，融合GPS、IMU、里程計信息，推算距離當前位姿最近的一個節(jié)點作為參考點，用于后續(xù)定位的子地圖，并將機器人表示在該節(jié)點的坐標系下；隨著機器人的不斷移動，參考點不斷變化，即子地圖將不斷切換。第二層將融合地圖信息，通過選取參考點一定大小鄰域的若干子地圖，并與實時獲得的激光數(shù)據(jù)進行迭代最近點法（ICP）匹配，根據(jù)匹配結(jié)果，得到當前機器人在參考點下的位姿，完成系統(tǒng)自主定位。

對于Codom，該項約束由多傳感器插值里程獲得。即從上一個獲取激光的時刻開始的IMU 數(shù)據(jù)，在下一個激光數(shù)據(jù)獲得前，均通過四足機器人的運動模型積分得到相對于上一個時刻的位姿。當下一次激光獲取時，以IMU 積分的數(shù)據(jù)為初值，通過和上一個時刻的激光數(shù)據(jù)進行迭代最近點法（ICP）匹配，得到相鄰兩幀的增量式相對變換。作為當前時刻相對于上一個時刻的位姿，IMU 數(shù)據(jù)的積分清零，清除IMU 積分的累積誤差。通過累積一系列相對變換可以最終得到機器人的里程軌跡。有了該軌跡，可以約束變量位姿之間的相對位姿變換需要盡可能和里程一致。以長度為3 的窗口為例，定義里程軌跡的位姿為U，優(yōu)化代價函數(shù)Codom如下：

根據(jù)代價函數(shù)，取當前時刻和子地圖之間的位姿變換，即為當前在子地圖下的定位。生成的全局定位地圖如圖2 所示，機器人可以根據(jù)全局地圖來規(guī)劃自己的運動軌跡。

圖2 全局地圖生成Fig.2 Global map generation

4 全局和局部地圖的路徑規(guī)劃

4.1 基于Dijkstra 算法的全局路徑導(dǎo)航

路徑規(guī)劃模塊將完成四足機器人在全局導(dǎo)航定位地圖下的全局路徑規(guī)劃任務(wù)，其將跟蹤指定的人為設(shè)定軌跡點或行人軌跡，根據(jù)局部環(huán)境信息、障礙物信息等規(guī)劃安全無碰撞的局部行走路徑點信息。系統(tǒng)將采用Dijkstra 算法進行局部路徑點規(guī)劃，再采用TEB 平滑算法進行全局路徑的平滑優(yōu)化處理。

系統(tǒng)采用Dijkstra 算法在局部地圖構(gòu)建輸出的實時局部柵格地圖中進行路徑規(guī)劃，Dijkstra算法的起點為機器人當前所在柵格，目標點為下一個要到達的目標節(jié)點所在柵格。啟發(fā)式算法（如A*算法）相比于Dijkstra 的搜索空間縮小很多，即在計算量上大大降低。Dijkstra 算法的具體步驟如下。

記柵格地圖為G={V,E}，V為柵格，即頂點，E為兩點之間的邊：

（1）初始時令S={V0},T=V-S={其余頂點}；

（2）從T中選取一個與S中頂點有關(guān)聯(lián)邊且權(quán)值最小的頂點W，加入到S中；

（3）對其余T中頂點的距離值進行修改：若加進W作中間頂點，從V0到Vi的距離值縮短，則修改此距離值；

（4）重復(fù)上述步驟2、3，直到S中包含目標頂點。

經(jīng)過Dijkstra 搜索，從當前柵格到目標柵格的路徑可以得到。

由于柵格地圖的分辨率較低，生成的局部路徑需要進一步處理。先將柵格路徑進行離散采樣，再放入TEB 算法進行優(yōu)化處理。TEB 算法是一個相對較新的全局導(dǎo)航算法，可以輸出移動機器人的路徑點以及速度，它采用了g2o 優(yōu)化：將局部路徑節(jié)點當作節(jié)點，即待解量；將節(jié)點之間的約束作為條件。經(jīng)過優(yōu)化后，局部路徑相對會更加平滑，并且可以調(diào)節(jié)距離障礙物之間的距離。

4.2 基于勢場理論的局部地圖避障

在局部地圖中通過相鄰的網(wǎng)格信息來判斷障礙物的位置和高度信息，當檢測到障礙物高度和尺寸超過了腿部擺動閾值，需要通過避障實現(xiàn)局部地圖通過。通過障礙檢測的方式去感知物體在全局坐標系下的位置，并依據(jù)勢場理論完成機器人的自主避障[15]。

設(shè)定地圖的柵格尺寸為m×n，單個柵格尺寸為a×a，設(shè)置柵格信息格式為如式(10)所示：

障礙物在柵格地圖中的高度信息可以表示為h[p,q]，代表了指定區(qū)域地形范圍內(nèi)的平均高度。

式中，k為指定柵格中的點云數(shù)量。

通過變量h[p,q]去評價所在地形區(qū)域的平坦程度和地形的幾何信息。根據(jù)采集得到的點云數(shù)據(jù)，通過各個區(qū)域的點云平均高度差進行比較，變量δWh(i)為每個柵格間的高度差：

柵格信息包含了采集區(qū)域的水平和高度值，首先對采集區(qū)域的地形綜合評估，對高度差進行檢測，得到前方路面的高度最低區(qū)域，此區(qū)域即為機器人的安全通過區(qū)域。本文把機器人本體和四條腿近似等價成一個虛擬機體模型，提出了基于人工勢場理論的四足機器人自主避障策略，模擬引力場的原理來表達目標位置、障礙物位置以及機器人運動方向的關(guān)系。定義障礙物對機器人產(chǎn)生排斥力Fo，目標對機器人產(chǎn)生吸引力Fd，系統(tǒng)的勢場Us可以表示為：

根據(jù)Khatib 提出的模型，吸引力產(chǎn)生的勢函數(shù)為：

障礙物體和機器人之間的排斥力產(chǎn)生的勢函數(shù)為：

根據(jù)勢函數(shù)可得，機器人和障礙物產(chǎn)生的排斥力為：

為了保證障礙物和目標的安全距離，在勢場函數(shù)中考慮障礙物和目標的距離，修改后的勢函數(shù)為：

結(jié)合吸引力和排斥力產(chǎn)生的勢函數(shù)，可以得到機器人避障的評價函數(shù)：

取機器人在地圖中的位置為Gn,n，如圖3 所示。對于備選的5 個備選柵格單元，其鄰域單元分別為通過勢場法計算機器人的行走方向，建立虛擬機體模型包絡(luò)本體和各腿的運動范圍，根據(jù)機器人和障礙物之間的安全距離得到四足機器人最優(yōu)避障運動方向。

圖3 虛擬機體模型最優(yōu)運動方向Fig.3 Optimal locomotion direction for virtual trunk body model

基于本體傳感器采集得到的數(shù)據(jù)，建立一個以機器人本體質(zhì)心為圓心，質(zhì)心到腿部足端位置為半徑的虛擬機體模型；以包絡(luò)的虛擬機體模型作為避障運動的采樣區(qū)域，根據(jù)地形高程圖得到的數(shù)據(jù)，計算機器人質(zhì)心和障礙物之間的垂直距離dOV和質(zhì)心沿最優(yōu)方向的距離dOT，基于人工勢場理論建立避障評價函數(shù)，得到四足機器人的避障最優(yōu)方向和最短距離，通過計算圓切點M和障礙邊界點N的連線，得到四足機器人避障偏航角度φ：

四足機器人根據(jù)感知得到的避障偏航角度實現(xiàn)最優(yōu)運動方向的局部地圖避障。

5 試驗驗證

根據(jù)提出的全局和局部地圖導(dǎo)航定位和路徑規(guī)劃方法，將感知系統(tǒng)安裝在四足機器人上，并進行了野外環(huán)境自主試驗，四足機器人和感知系統(tǒng)如圖4 所示。

圖4 四足機器人和感知系統(tǒng)Fig.4 Quadruped robot and perception system

試驗結(jié)果證明，四足機器人滿足上述路徑跟蹤和速度要求，感知規(guī)劃決策頻率為 5 Hz，障礙避障決策頻率和定位迭代頻率分別為 10 Hz和 50 Hz。四足機器人的野外環(huán)境性能試驗如圖5 所示。

圖5 四足機器人野外環(huán)境性能試驗Fig.5 Field experimental performance test of quadruped robot

如圖5 所示，感知生成的路徑點為20 個，第一個目標點為47.25 m，機器人跟蹤的最近路徑點為0.3 m，基于Dijkstra 算法生成的最優(yōu)綠色軌跡保證了四足機器人可以根據(jù)全局SLAM 定位中的最優(yōu)路徑行走，四足機器人在行走過程中的路徑變化如圖6 所示。

圖6 四足機器人在行走過程中的路徑跟蹤Fig.6 Path tracking diversification of quadruped robot during walking

基于拓撲度量相結(jié)合的定位方法減少了整體采集特征點的數(shù)量，采用了點特征和魯棒線特征。為了顯示定位方法的效果，作者計算了特定軌跡 上不同方法的成功定位次數(shù)，并找到了相應(yīng)的正確特征匹配的數(shù)量。將所提出的方法與 P3P 定位方法進行了對比分析，從統(tǒng)計結(jié)果可以看出，提出的定位方法可以利用點特征和更魯棒的線特征進行定位，總體定位成功的次數(shù)比其他方法多，絕對軌跡誤差更低。對比了 P3P 方法與本論文方法的正確定位特征匹配的數(shù)量，結(jié)果如圖7 所示，箭頭所指紅色代表定位成功地點，藍色代表定位失敗地點?？梢钥闯觯疚牡亩ㄎ怀晒Υ螖?shù)明顯更多，且找到的正確特征匹配的數(shù)量也更多，從而表明本論文的定位方法的魯棒性更強。

圖7 兩種方法的成功定位次數(shù)Fig.7 Number of successful localization between two methods

為了驗證四足機器人可以根據(jù)雷達信息自主避開局部地圖中的障礙物，將基于雷達的偏航角與基于運動學(xué)的偏航角進行了對比，如圖8 所示。圖中的紅色實線代表通過雷達信息和評價函數(shù)得到的機器人最優(yōu)可行轉(zhuǎn)向角度，藍色星線代表了四足機器人本體傳感計算得到的偏航角度，通過對比驗證了機器人本體可以實時跟蹤雷達反饋回的偏航角度。四足機器人運動過程中由于足端運動離散性會有一定的滯后，導(dǎo)致跟蹤效果無法實時。由試驗結(jié)果得知，四足機器人最初與障礙物的距離很近為不可通過地形，通過避障評價函數(shù)計算得出機器人的偏航角度，當避開障礙物后偏航角度逐漸減小并繼續(xù)沿目標點行走。

圖8 四足機器人偏航角對比試驗Fig.8 Comparative experiment of quadruped robot for the yaw angle

四足機器人的避障運動過程如圖9 所示，由于局部地圖中障礙物的包絡(luò)較大，因此機器人會根據(jù)局部地圖和雷達檢測的障礙物信息，實時完成自主避障運動。

圖9 四足機器人虛擬機體模型最優(yōu)避障運動方向Fig.9 Optimal locomotion direction for virtual trunk body model

6 結(jié) 論

本文針對四足機器人感知定位和導(dǎo)航算法旨在完成野外自主導(dǎo)航任務(wù)，并在四足原理樣機上進行了試驗驗證，得出了以下主要結(jié)論：

（1）提出了SLAM 全局定位算法，實現(xiàn)了野外環(huán)境的地圖重構(gòu)和定位；

（2）采用了基于Dijkstra 算法的路徑規(guī)劃，以實現(xiàn)四足機器人的全局地形自主導(dǎo)航任務(wù)；

（3）在考慮四足機器人的運動和振動前提下，應(yīng)用了基于人工勢場理論的四足機器人局部地圖自主避障策略；

（4）對所提出的算法進行了綜合性試驗，試驗結(jié)果驗證了理論分析的有效性。

未來將對四足機器人在野外地形環(huán)境下的局部地圖落足點規(guī)劃進行進一步的研究，并考慮自主避障的高跟蹤特性。