夏文強(qiáng),王書(shū)涵,曾理湛,羅 欣

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

1 引言

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

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

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

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

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

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

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

3.1 基于卡爾曼濾波的腿部里程計(jì)

四足機(jī)器人結(jié)構(gòu)簡(jiǎn)圖和簡(jiǎn)化坐標(biāo)系如圖2所示,機(jī)器人的質(zhì)心位置和IMU基本重合。圖中,l1、l2、l3分別表示為對(duì)應(yīng)關(guān)節(jié)之間的連桿長(zhǎng)度。機(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}不存在角度變換。每個(gè)角度均可以通過(guò)編碼器獲取。

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

(1)

(2)

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

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

si=RT(pi-r)+ni

(3)

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

用v表示機(jī)器人質(zhì)心的速度,定義機(jī)器人的狀態(tài)向量和觀測(cè)變量分別如式(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àn)預(yù)測(cè)方程為:

(6)

(7)

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

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

(8)

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

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

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

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

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

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

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

(9)

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

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

(10)

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

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

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

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

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

(11)

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

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

(12)

Figure 5 Schematic diagram of tree trunk feature point cloud segmentation圖5 樹(shù)干特征點(diǎn)云分割原理圖

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

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

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

3.2.3 前端里程計(jì)

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

(13)

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

(14)

(15)

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

(16)

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

Figure 8 Schematic diagram of feature point distance constraints圖8 特征點(diǎn)距離約束示意圖

(17)

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

3.2.4 后端里程計(jì)

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

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

(18)

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

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

4.1 仿真實(shí)驗(yàn)與分析

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

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

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

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

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

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

4.2 實(shí)驗(yàn)驗(yàn)證與分析

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

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

Figure 14 Experimental environment and platform圖14 實(shí)驗(yàn)環(huán)境和平臺(tái)

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

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

計(jì)算得到不同里程計(jì)下的絕對(duì)軌跡誤差A(yù)TE(Absolute Trajectory Error)曲線如圖16所示(以第3圈為例),其中不同線型表示不同里程計(jì)。可以得到腿部里程計(jì)絕對(duì)誤差均大于激光雷達(dá)里程計(jì)和融合里程計(jì),融合里程計(jì)絕對(duì)誤差小于激光雷達(dá)里程計(jì)。腿部里程計(jì)在不同的轉(zhuǎn)彎處與實(shí)際值的偏差會(huì)存在一定的波動(dòng),因?yàn)榇蚧嬖诓淮_定性,且在直線運(yùn)動(dòng)時(shí)會(huì)存在某個(gè)方向的偏差較小。

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

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

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

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

5 結(jié)束語(yǔ)

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

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