面向林地環(huán)境的四足機(jī)器人自主定位方法*

夏文強,王書涵,曾理湛,羅 欣

(華中科技大學(xué)機(jī)械科學(xué)與工程學(xué)院,湖北 武漢 430074)

1 引言

林地是一種典型的野外環(huán)境,是四足機(jī)器人常見的作業(yè)場景,實現(xiàn)四足機(jī)器人在野外林地環(huán)境中的自主作業(yè)對于發(fā)揮和拓展其應(yīng)用有著重要的價值和意義[1,2]。林地環(huán)境具有樹木多且間距小、地形復(fù)雜、地面松軟、凹凸不平和光線明暗變化大等特點。在這種具有挑戰(zhàn)性的環(huán)境中,四足機(jī)器人需要更為準(zhǔn)確和高頻的定位信息來實現(xiàn)按設(shè)定路線的快速導(dǎo)航。四足機(jī)器人獲取其定位信息的方法是利用傳感器數(shù)據(jù)進(jìn)行里程計推算,常用的傳感器主要有2種:一種是內(nèi)部傳感器,例如慣性測量單元IMU(Intertial Measurement Unit)和編碼器等;另一種是外部傳感器,例如激光雷達(dá)LiDAR(Light Detection And Ranging)和相機(jī)等。在林地環(huán)境中采用基于內(nèi)部傳感器的方法[3,4,5]往往會因為林地地面松軟、凹凸不平等使得落足點滑動進(jìn)而使得定位估計值出現(xiàn)較大誤差,因此需要借助外部傳感器對腿部里程計進(jìn)行修正,例如,Hartley和Wisth等[6,7]考慮采用加入視覺信息進(jìn)行因子圖構(gòu)建的融合,結(jié)果表明慣性運動學(xué)視覺方法優(yōu)于融合信息較少的方法。然而,由于基于因子圖的方法使用沿整個軌跡的測量值進(jìn)行平滑處理,對于實時導(dǎo)航控制而言,更新頻率較低。在滿足高頻率的定位下,通?？紤]采用卡爾曼濾波的方式融合觀測信息,Ma等[8]提出了一種誤差狀態(tài)卡爾曼濾波器,將慣性測量單元、腿部運動學(xué)與立體視覺里程計相融合,但該方法未考慮林地環(huán)境特征,仍是以腿部里程計作為先驗,接收外部數(shù)據(jù)后需要構(gòu)建觀測模型,更新協(xié)方差矩陣,模型和計算復(fù)雜度較大。

近年來,隨著同步定位與建圖SLAM(Simultaneous Localization And Mapping)技術(shù)的發(fā)展,在機(jī)器人上使用激光雷達(dá)或者視覺相機(jī)作為主要傳感器,可以實現(xiàn)精確的定位,比較有代表性的SLAM方法有LOAM[9]、LIO-SAM[10]、ORB-SLAM[11]和ORB-SLAM2[12]等。采用視覺信息的ORB-SLAM類方法易受光線條件影響,無法滿足機(jī)器人在林地環(huán)境執(zhí)行任務(wù)的需求。因此,林地導(dǎo)航的首選SLAM方案是3D激光雷達(dá),其可以不受光線影響,但輸出頻率較低,無法滿足實時導(dǎo)航的需求。

因此,面向林地環(huán)境的四足機(jī)器人設(shè)定路線的快速導(dǎo)航問題,本文提出了一種適用于林地環(huán)境的自主定位方法,考慮了在林地環(huán)境激光雷達(dá)定位的適用性以及腿部里程計的高頻連續(xù)性,改進(jìn)LOAM方法,采用腿部里程計去除激光雷達(dá)的點云畸變,并分別提取林地地面和樹干信息進(jìn)行匹配,提高激光雷達(dá)定位精度;在激光雷達(dá)2次定位之間采用中值和窗口濾波融合腿部里程計的插值數(shù)據(jù),提高定位頻率,降低計算復(fù)雜度低的同時保證了精度和頻率。

2 四足機(jī)器人自主定位總體框架

激光雷達(dá)在林地環(huán)境中可以獲取豐富的特征信息,但在掃描過程中由于機(jī)器人的抖動,獲取的點云會產(chǎn)生運動畸變。因此,本文在構(gòu)建腿部里程計的同時采用其短時間位姿估計的準(zhǔn)確性去除點云畸變,且由于林地中存在較多的樹葉或雜草等,其不能作為特征提取的一部分,考慮提取較為明顯的樹干特征和地面特征進(jìn)行匹配,可以提高激光雷達(dá)精度。在保證激光雷達(dá)精度和時間對齊的基礎(chǔ)上,推導(dǎo)2次定位之間腿部里程計多次位姿變換,采用中值和窗口濾波融合其插值數(shù)據(jù),可以提高其頻率,從而滿足四足機(jī)器人設(shè)定路線快速導(dǎo)航的定位頻率和精度。本文方法的框架如圖1所示,主要分為3部分:第1部分是通過擴(kuò)展卡爾曼濾波構(gòu)建腿部里程計;第2部分是對于激光雷達(dá)三維點云,去除畸變,構(gòu)建激光里程計,圖1中,tx、ty和tz分別表示激光里程計在x、y和z方向上的位移,θroll、θpitch和θyaw分別表示激光里程計的橫滾角、俯仰角和偏航角;第3部分是將兩者融合得到最終的里程計。

Figure 1 Framework of quadruped robot autonomous localization method圖1 四足機(jī)器人自主定位方法框架

3 四足機(jī)器人自主定位方法實現(xiàn)

3.1 基于卡爾曼濾波的腿部里程計

四足機(jī)器人結(jié)構(gòu)簡圖和簡化坐標(biāo)系如圖2所示,機(jī)器人的質(zhì)心位置和IMU基本重合。圖中,l1、l2、l3分別表示為對應(yīng)關(guān)節(jié)之間的連桿長度。機(jī)器人的質(zhì)心坐標(biāo)系為{OB}。以右后腿為例分析,設(shè)腿部坐標(biāo)系為{OH},髖部橫滾關(guān)節(jié)坐標(biāo)系{O0}與坐標(biāo)系{OH}之間存在繞x軸的橫滾角θ1,髖部俯仰關(guān)節(jié)坐標(biāo)系{O1}與{O0}之間存在繞z軸的俯仰角θ2,膝部俯仰關(guān)節(jié)坐標(biāo)系{O2}與{O1}之間存在繞z軸的俯仰角θ3,足端坐標(biāo)系{O3}與{O2}不存在角度變換。每個角度均可以通過編碼器獲取。

Figure 2 Structural diagram of quadruped robot圖2 四足機(jī)器人結(jié)構(gòu)簡圖

(1)

(2)

其中,si表示第i條腿相對于機(jī)器人質(zhì)心坐標(biāo)系的位置向量,equ(·)表示基于上述運動學(xué)求解si所建立的方程,ni表示噪聲,將四足機(jī)器人存在的滑移等效為噪聲[4,14],假定滑動的大小為0.3～0.5步長的高斯噪聲。在本文中不考慮加工、裝配和編碼器噪聲等誤差。

同樣式(2)也可以表示為在世界坐標(biāo)系下機(jī)器人落足點的位置pi減去機(jī)器人質(zhì)心位置r的差值,即:

si=RT(pi-r)+ni

(3)

其中,R表示機(jī)器人質(zhì)心在世界坐標(biāo)系下的姿態(tài)。

用v表示機(jī)器人質(zhì)心的速度,定義機(jī)器人的狀態(tài)向量和觀測變量分別如式(4)和式(5)所示:

x=[r,v,p1,p2,p3,p4]

(4)

z=[r-p1,r-p2,r-p3,r-p4,v1,v2,v3,v4]

(5)

其中,v1～v4分別表示四足機(jī)器人中的第1～4條腿的速度。

由IMU預(yù)積分可得到第k次迭代的先驗預(yù)測方程為:

(6)

(7)

由式(3)可以得到觀測方程的更新為:

zk+1=Ck+1xk+1+wk+1

(8)

其中,C表示狀態(tài)觀測矩陣,wk+1表示觀測誤差。由此可以得到2種不同傳感器信息的更新方程,采用卡爾曼濾波[15]融合,狀態(tài)向量中的r與R即表示機(jī)器人在世界坐標(biāo)系下的速度和位姿。

3.2 基于特征點云匹配的激光雷達(dá)定位

3.2.1 點云畸變?nèi)コ?/p>

樹林環(huán)境特征豐富,可以獲取大量穩(wěn)定有效的點云信息,但由于四足機(jī)器人在林地環(huán)境運動中的機(jī)身抖動和滑移,且無法保證勻速運動,因此需要對激光雷達(dá)點云進(jìn)行運動畸變?nèi)コ?采用3.1節(jié)中獲取的腿部里程計進(jìn)行點云畸變?nèi)コ?。相?yīng)符號設(shè)定如下:

(1)Pk表示在第k次掃描周期的所有點云集合。

(2)一次掃描的時間段為[t0,t1],在這個時間段內(nèi)得到時間對齊后的腿部里程計位姿集合為T={Tj|j=0,1,…,m},其中j=0時對齊時間為t0,j=m時對齊時間為t1。

由集合T可以推導(dǎo)得到在不同時刻下相對t0時刻的位姿變換如下:

(9)

Figure 3 Time series diagram of point cloud by LiDAR圖3 激光雷達(dá)采樣點云時間序列圖

Pk中每個點的采樣時刻很難精確獲取,使用其在掃描中所對應(yīng)的水平角來估計在時間段[t0,t1]中的線性占比,即可對應(yīng)至集合T中相對位姿變換下標(biāo)j為:

(10)

Figure 4 Schematic diagram before and after point cloud distortion removal圖4 點云畸變?nèi)コ昂笫疽鈭D

3.2.2 林地地面環(huán)境和樹干特征提取

激光雷達(dá)在林地環(huán)境中獲取穩(wěn)定有效的特征,對實現(xiàn)其精確定位有較大的影響[16]。因此,在林地環(huán)境中獲取準(zhǔn)確可靠的地面和樹干特征,去除如樹葉、草叢等不穩(wěn)定的特征,可以避免錯誤匹配,提高定位精度。

對于地面特征,本文采用以模型擬合為基礎(chǔ)的RANSAC(RANdom SAmple Consensus)算法[17]進(jìn)行獲取,機(jī)器人搭載16線激光雷達(dá)離地面高度h大約為0.5 m,取激光雷達(dá)下面6根線束,則前方地面點云x方向的范圍為z/tan 15°～z/tan 3°,即1.87 m～9.54 m。根據(jù)此距離進(jìn)行范圍限定并對選定點云的z方向排序,選取高度較小的部分點云進(jìn)行平面擬合得到方程a0x+b0y+c0z+d0=0。

在激光雷達(dá)前后限定范圍內(nèi)的第l個點云pl=[x,y,z],如果是平面點則與平面的距離滿足一定閾值dmax,且由于地面估計參數(shù)的不確定性,隨著點云與坐標(biāo)原點p0=[x0,y0,z0]的距離越遠(yuǎn),其為平面點的可能性越低,則該點與原點的連線和擬合平面的夾角也應(yīng)該滿足一定閾值θmax,即存在以下約束公式:

(11)

在林地環(huán)境中另一個重要特征是樹干,屬于一類特征。對于激光雷達(dá)光束而言,如果從同一個物體反射得到較多近鄰點則這些點云屬于相同分割。將激光雷達(dá)點云按照線束投影為二維深度圖像,其中圖像的行數(shù)為線束,列數(shù)為總旋轉(zhuǎn)角除以水平分辨率,像素為點的深度。采用聚類分割方法[18]進(jìn)行樹干的點云分割,原理如圖5所示。在圖5中右側(cè)部分為示意的樹干特征,A和B表示任意2個鄰近點,d1和d2表示連續(xù)且相鄰的2道激光束。連接A、B為直線LAB,α為OA與OB的夾角,β為OA與LAB的夾角,設(shè)定閾值為βmin,如果β>βmin且A和B之間的歐氏距離較小,則認(rèn)為A和B均屬于同一個物體,也即存在約束公式如式(12)所示:

β=arctan(d2sinα/(d1-d2cosα))>βmin

(12)

Figure 5 Schematic diagram of tree trunk feature point cloud segmentation圖5 樹干特征點云分割原理圖

由上述方法得到分割后的地面點云和樹干點云分別如圖6和圖7所示。可以看出分割得到的地面特征和樹干特征較為明顯且合理,為激光雷達(dá)點云匹配提供了可靠和明確的點云。

Figure 6 Woodland environment feature point cloud圖6 林地地面環(huán)境特征點云圖

Figure 7 Point cloud map of tree trunk characteristics in woodland environments圖7 林地環(huán)境樹干特征點云圖

3.2.3 前端里程計

本文借鑒LOAM思想[9],采用點的曲率評估局部表面的平滑度,并以此提取當(dāng)前的特征信息,點的曲率計算公式如式(13)所示:

(13)

假設(shè)Ek和Hk分別表示第k次掃描中曲面特征點集合和平面特征點集合,Ek+1和Hk+1分別表示第k+1次掃描提取到的曲面特征點集合和平面特征點集合。第k+1幀相對于第k幀的旋轉(zhuǎn)運動采用3-2-1歐拉角表示,變換矩陣為:

(14)

(15)

其中,c′=cos(·),s′=sin(·)。R表示旋轉(zhuǎn)矩陣,t表示3×1的位移向量。為了求解平移和旋轉(zhuǎn)6個變量,對于曲面特征匹配,采取點到直線的匹配來估計[tz,β,γ];對于平面特征匹配,采取點到平面的匹配來估計[tx,ty,α],其中β,γ和α分別表示為橫滾角、俯仰角和偏航角。構(gòu)建距離約束關(guān)系如式(16)所示:

(16)

其中,fE(·)和fH(·)表示的是約束關(guān)系,優(yōu)化變量是dE和dH。構(gòu)建約束關(guān)系如圖8所示,其中,圖8a表示曲面特征點r和q到上一幀最鄰近點p組成直線的距離約束關(guān)系,圖8b表示平面特征點p到上一幀3個鄰近點s、q、r組成平面的距離約束關(guān)系。根據(jù)點到直線和點到平面所對應(yīng)的求解公式即可建立優(yōu)化方程。

Figure 8 Schematic diagram of feature point distance constraints圖8 特征點距離約束示意圖

(17)

其中,0表示起始幀,激光雷達(dá)以10 Hz頻率不斷獲取每一幀的點云信息,計算2幀之間的位姿變換,根據(jù)上述公式遞推得到前端里程計。

3.2.4 后端里程計

為了更多利用每一幀得到的信息,在后端考慮利用若干歷史幀(在這里選取前面5幀)的信息,從效果上相當(dāng)于一個滑動窗口,里面存放了當(dāng)前的局部地圖,其不局限于相鄰幀的間距,而是擴(kuò)展到了相鄰若干幀的間距,幀圖匹配精度比幀間匹配精度高很多。

后端優(yōu)化的思路與前面雷達(dá)前端里程計的思路基本一致,對于當(dāng)前幀的邊緣特征點與平面特征點,同樣是從第k-5幀到第k幀所有特征點云所組成的地圖中尋找當(dāng)前幀的匹配特征點建立距離約束的殘差表達(dá)式。最終通過高斯牛頓迭代求解優(yōu)化得到6個變量值,獲取雷達(dá)后端優(yōu)化的齊次變換矩陣。后端輸出的激光雷達(dá)里程計頻率較低,可以結(jié)合前端里程計進(jìn)行融合,具體計算如式(18)所示:

(18)

3.3 基于中值和窗口濾波的融合里程計

4 機(jī)器人林地環(huán)境仿真與實驗

4.1 仿真實驗與分析

本文仿真環(huán)境為Webots,在仿真中可以獲取四足機(jī)器人搭載激光雷達(dá)的點云數(shù)據(jù)、IMU數(shù)據(jù)、編碼器數(shù)據(jù)和真實位姿值?；赗OS(Robot Operating System)發(fā)布和接收腿部里程計、激光雷達(dá)里程計(LOAM方法)、融合里程計以及真實值。四足機(jī)器人林地仿真環(huán)境如圖10所示。分析四足機(jī)器人自主定位的效果,控制四足機(jī)器人直線運動,在這里對比四足機(jī)器人定位變量x,y,如圖11和圖12所示,其中不同的線型表示不同的里程計值,從圖11和圖12中可以看出,在前面時間中有一部分省略,因為在此方向上基本無運動。

Figure 10 Establishment of simulation environment in Webots圖10 Webots仿真環(huán)境的搭建

Figure 11 Variation of localization variable x with time圖11 定位變量x隨時間變化圖

Figure 12 Variation of localization variable y with time圖12 定位變量y隨時間變化圖

由圖11和圖12可以得到,融合里程計的定位值不會像腿部里程計一樣累計誤差,在某些時刻接近真實值。計算得到不同里程計下x,y方向絕對定位均方根誤差RMSE(Root Mean Square Error)如表1所示,可以得到在y方向上融合定位的效果比激光雷達(dá)定位效果較好,在x方向上定位效果接近,整體上提高了激光雷達(dá)定位精度,且融合頻率接近腿部里程計的頻率,在仿真中為500 Hz。

Table 1 Root mean square error of location with different odometers in x and y directions表1 不同里程計在x,y方向定位的均方根誤差

4.2 實驗驗證與分析

本文采用宇樹公司出品的四足機(jī)器人AI進(jìn)行實驗。圖13所示是系統(tǒng)的整體硬件框架。編碼器角度通過接口程序獲取,激光雷達(dá)是速騰聚創(chuàng)公司的RS-LiDAR-16,IMU型號為MTi-G-710,AGX與交換機(jī)通過網(wǎng)線連接四足機(jī)器人,遠(yuǎn)程終端調(diào)試使用代碼,編譯后發(fā)送控制指令,實現(xiàn)四足機(jī)器人的運動切換,實驗環(huán)境和平臺如圖14所示。

Figure 13 Hardware configuration framework圖13 硬件配置框架

Figure 14 Experimental environment and platform圖14 實驗環(huán)境和平臺

根據(jù)實際情況,設(shè)定路線為x方向10 m,y方向4.5 m,控制機(jī)器人按照長方形路徑行走4圈(實際中會存在小范圍的偏差),得到不同定位下的曲線如圖15所示。

Figure 15 Location curves with different odometers圖15 不同里程計下的定位曲線圖

計算得到不同里程計下的絕對軌跡誤差A(yù)TE(Absolute Trajectory Error)曲線如圖16所示(以第3圈為例),其中不同線型表示不同里程計?？梢缘玫酵炔坷锍逃嫿^對誤差均大于激光雷達(dá)里程計和融合里程計,融合里程計絕對誤差小于激光雷達(dá)里程計。腿部里程計在不同的轉(zhuǎn)彎處與實際值的偏差會存在一定的波動,因為打滑存在不確定性,且在直線運動時會存在某個方向的偏差較小。

Figure 16 Absolute error of the third circle in robot walking with different localization odometers圖16 不同里程計下機(jī)器人行走第3圈的絕對誤差

每一圈回到起點的絕對軌跡誤差如表2所示。從表2中可以看出,激光雷達(dá)里程計和融合里程計最大均方根誤差分別為1.36 m和0.19 m,均優(yōu)于腿部里程計,且最終融合里程計精度相較于激光雷達(dá)里程計提升了94%。從圖15中也可以得到融合里程計更接近真實值,符合機(jī)器人實際行走軌跡。

Table 2 Absolute trajactory error with different odometers returning to the starting point at each turn表2 不同里程計在每一圈回到起點的絕對軌跡誤差

根據(jù)林地實際狀況,設(shè)定的直線路線與上述驗證方法相同,路線上存在樹木并且需要進(jìn)行穿越,設(shè)定四足機(jī)器人行走速度為1.0 m/s,轉(zhuǎn)彎半徑為1.5 m,最大轉(zhuǎn)彎速度為0.6 rad/s,四足機(jī)器人采用相同的控制方法,得到在不同里程計下的定位數(shù)據(jù)如圖17所示。由圖17可以得出,在四足機(jī)器人設(shè)定路線快速導(dǎo)航中且在相同的控制方法和規(guī)劃下,通過激光雷達(dá)定位在前期能保持較好的定位精度。但是在避開樹木轉(zhuǎn)彎的過程中,由于定位頻率較低,局部導(dǎo)航無法順利調(diào)節(jié),導(dǎo)致轉(zhuǎn)彎性能較差,最終由于撞到樹木而無法完成任務(wù)要求。通過融合里程計和腿部里程計的定位方法,能夠進(jìn)行順利調(diào)節(jié),完成相應(yīng)的工作任務(wù),但從圖中得到腿部里程計誤差偏移較多,機(jī)器人在實際實驗運動過程中非常接近樹木,最大誤差為0.8 m,而融合后的里程計最大偏移為0.2 m,在保證高頻率的前提下,仍然有著較高的精度,順利地完成了四足機(jī)器人在林地環(huán)境中的設(shè)定路線快速導(dǎo)航任務(wù)。

Figure 17 Location values and expected location values of quadruped robot with different localization odometers圖17 不同里程計下的四足機(jī)器人定位值和期望定位值

5 結(jié)束語

面向林地環(huán)境的四足機(jī)器人自主定位問題,本文利用多種傳感器的數(shù)據(jù),對于編碼器和IMU采用卡爾曼濾波的方法,而對于激光雷達(dá)的點云數(shù)據(jù),考慮了去除畸變,提取林地地形和樹干特征進(jìn)行匹配,并采用中值和窗口濾波的融合方法,該方法可以較好滿足四足機(jī)器人自主定位的實時性和精確性,彌補了激光雷達(dá)定位頻率低和腿部里程計定位累計誤差大的不足。本文通過仿真和實驗驗證了該定位方法的有效性,并在設(shè)定路線的快速導(dǎo)航中驗證了該方法的實用性。

由于林地環(huán)境的復(fù)雜性、機(jī)器人續(xù)航等因素限制,本文將打滑簡化處理為高斯噪聲,但是實際過程中四足機(jī)器人運動中加減速的不對稱,是一個有偏的模型,盡管如此并不影響本文結(jié)論的正確性。對于腿部里程計可能存在的打滑以及長距離下定位漂移等問題,應(yīng)考慮建模進(jìn)一步減少位姿估計誤差或者融合其他傳感器信息修正。在實際導(dǎo)航中期望定位值與里程計定位值的偏差存在一定波動,后續(xù)考慮緊耦合并和結(jié)合濾波的方法進(jìn)行多傳感器數(shù)據(jù)的融合,可以達(dá)到更好的效果和實用性。