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

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

1 引 言

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

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

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

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

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

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

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

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

圖1 地形建?？蚣芟碌淖鴺?biāo)系系統(tǒng)Fig.1 Coordinate system for terrain modeling framework

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

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

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

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

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

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

最終可以得到實(shí)時(shí)更新的地形建模高度信息和高度的標(biāo)準(zhǔn)差，從而得到實(shí)時(shí)的地形信息。

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

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

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

在本系統(tǒng)的自主定位中，考慮足式機(jī)器人大環(huán)境自足行走時(shí)，需要考慮環(huán)境過大帶來的地圖全局一致性無法滿足的問題，以及地圖過大帶來的占用內(nèi)存過大的問題。本系統(tǒng)采用拓?fù)涠攘慷ㄎ豢蚣芟碌牡貓D動(dòng)態(tài)加載方法，將全局地圖劃分成一系列局部地圖，并以帶尺度的拓?fù)涞貓D表示。在定位中，系統(tǒng)采用當(dāng)前定位結(jié)果動(dòng)態(tài)加載鄰近地圖的方式進(jìn)行信息融合，并采用記憶機(jī)制來克服運(yùn)動(dòng)中定位融合匹配失敗問題。

3.2 拓?fù)涠攘慷ㄎ豢蚣茉O(shè)計(jì)

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

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

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

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

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

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

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

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

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

（1）初始時(shí)令S={V0},T=V-S={其余頂點(diǎn)}；

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

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

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

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

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

4.2 基于勢(shì)場(chǎng)理論的局部地圖避障

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 試驗(yàn)驗(yàn)證

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

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

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

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

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

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

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

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

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

圖8 四足機(jī)器人偏航角對(duì)比試驗(yàn)Fig.8 Comparative experiment of quadruped robot for the yaw angle

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

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

6 結(jié) 論

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

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

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

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

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

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