面向林地環境的四足機器人自主定位方法*

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

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

1 引言

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

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

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

2 四足機器人自主定位總體框架

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

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

3 四足機器人自主定位方法實現

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

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

Figure 2 Structural diagram of quadruped robot圖2 四足機器人結構簡圖

(1)

(2)

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

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

si=RT(pi-r)+ni

(3)

其中,R表示機器人質心在世界坐標系下的姿態。

用v表示機器人質心的速度,定義機器人的狀態向量和觀測變量分別如式(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分別表示四足機器人中的第1～4條腿的速度。

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

(6)

(7)

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

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

(8)

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

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

3.2.1 點云畸變去除

樹林環境特征豐富,可以獲取大量穩定有效的點云信息,但由于四足機器人在林地環境運動中的機身抖動和滑移,且無法保證勻速運動,因此需要對激光雷達點云進行運動畸變去除,采用3.1節中獲取的腿部里程計進行點云畸變去除。相應符號設定如下:

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

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

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

(9)

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

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

(10)

Figure 4 Schematic diagram before and after point cloud distortion removal圖4 點云畸變去除前后示意圖

3.2.2 林地地面環境和樹干特征提取

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

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

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

(11)

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

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

(12)

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

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

Figure 6 Woodland environment feature point cloud圖6 林地地面環境特征點云圖

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

3.2.3 前端里程計

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

(13)

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

(14)

(15)

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

(16)

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

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

(17)

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

3.2.4 后端里程計

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

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

(18)

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

4 機器人林地環境仿真與實驗

4.1 仿真實驗與分析

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

Figure 10 Establishment of simulation environment in Webots圖10 Webots仿真環境的搭建

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方向上融合定位的效果比激光雷達定位效果較好,在x方向上定位效果接近,整體上提高了激光雷達定位精度,且融合頻率接近腿部里程計的頻率,在仿真中為500 Hz。

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

4.2 實驗驗證與分析

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

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

Figure 14 Experimental environment and platform圖14 實驗環境和平臺

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

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

計算得到不同里程計下的絕對軌跡誤差ATE(Absolute Trajectory Error)曲線如圖16所示(以第3圈為例),其中不同線型表示不同里程計。可以得到腿部里程計絕對誤差均大于激光雷達里程計和融合里程計,融合里程計絕對誤差小于激光雷達里程計。腿部里程計在不同的轉彎處與實際值的偏差會存在一定的波動,因為打滑存在不確定性,且在直線運動時會存在某個方向的偏差較小。

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

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

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

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

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

5 結束語

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

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