基于多傳感器融合的移動(dòng)機(jī)器人定位①

袁千賀,田 昕,沈斯杰

1(上海理工大學(xué) 光電信息與計(jì)算機(jī)工程學(xué)院,上海 200093)

2(上海理工大學(xué) 理學(xué)院,上海 200093)

對(duì)于移動(dòng)機(jī)器人來說,最重要的是解決如何識(shí)別周圍的環(huán)境情況、如何移動(dòng)到目標(biāo)位置、以及如何避開障礙物3 個(gè)問題.這些問題的關(guān)鍵都在于如何在環(huán)境中實(shí)現(xiàn)移動(dòng)機(jī)器人的精確定位[1],可以說,高精度的定位是機(jī)器人控制系統(tǒng)中最重要的部分.因此,近年來,移動(dòng)機(jī)器人的定位技術(shù)逐漸成為機(jī)器人相關(guān)領(lǐng)域的研究熱點(diǎn).盡管在定位技術(shù)上已經(jīng)有很多方面的研究,但依舊存在著各種挑戰(zhàn)與問題.

在實(shí)際定位中,不同的傳感器對(duì)于位姿和姿態(tài)的測(cè)量精度各有優(yōu)劣,可以利用各傳感器的優(yōu)勢(shì)估計(jì)機(jī)器人位姿,因此,基于多傳感器數(shù)據(jù)融合的定位方法[2,3]受到了廣大專家學(xué)者的青睞.Du 等[4]提出了一種基于仿生極化天窗傳感器的多傳感器融合SLAM 方法,減少了定位誤差和航向角誤差.柯顯信等[5]設(shè)計(jì)的多傳感器定位系統(tǒng),由紅外定位系統(tǒng)、聽覺定位系統(tǒng)和視覺定位系統(tǒng)融合而成,可適應(yīng)復(fù)雜場(chǎng)景,提高了機(jī)器人的定位精度和可靠性.但其中的視覺傳感器容易受到光照強(qiáng)度的影響.尹皓等[6]提出一種基于高斯牛頓方程的多傳感器非線性融合定位算法,該算法非線性融合輪式里程計(jì)、慣性測(cè)量單元 (inertial measurement unit,IMU)和激光雷達(dá)的測(cè)量信息,從而得到最優(yōu)位姿估計(jì),然而卻面臨算法運(yùn)算量大,定位速度慢等問題.He 等[7]以激光傳感器為核心設(shè)計(jì)了多傳感器融合定位導(dǎo)航系統(tǒng),通過實(shí)驗(yàn)驗(yàn)證了系統(tǒng)的可行性和可靠性.Rigatos 等[8]分別利用EKF和PF 兩種濾波算法融合里程計(jì)和聲吶的測(cè)量數(shù)據(jù),針對(duì)實(shí)驗(yàn)的傳感器融合問題,表明了粒子濾波具有更好的性能,而蒙特卡洛定位算法便是基于粒子濾波.此外,在靜態(tài)環(huán)境下,基于地圖匹配的定位方法也可以提高機(jī)器人的定位精度,將傳感器的掃描信息與先驗(yàn)地圖信息做匹配,得到最相符的匹配結(jié)果,從而提高精度[9-11].

綜上所述,本文考慮是否已知環(huán)境信息以及針對(duì)不同傳感器定位效果的情況下,提出了基于里程計(jì)、IMU和激光雷達(dá)的多傳感器融合移動(dòng)機(jī)器人定位方法.所提出的定位方法具有以下貢獻(xiàn):1)將定位實(shí)現(xiàn)分為是否已知先驗(yàn)環(huán)境地圖信息進(jìn)行,突出了環(huán)境對(duì)定位的重要性;2)將EKF 融合里程計(jì)和IMU的位姿作為AMCL 算法粒子集預(yù)測(cè)的初始位姿,并輔助粒子更新;3)利用激光雷達(dá)獲取的地圖環(huán)境信息,彌補(bǔ)了里程計(jì)和IMU 只適用于短時(shí)間,短距離內(nèi)位姿估計(jì)的缺點(diǎn).

1 移動(dòng)機(jī)器人定位系統(tǒng)

1.1 機(jī)器人運(yùn)動(dòng)學(xué)模型

對(duì)于常用的兩輪差速移動(dòng)機(jī)器人,由于噪聲、傳感器等因素的影響,機(jī)器人的運(yùn)動(dòng)狀態(tài)難以描述,通過對(duì)機(jī)器人建立恰當(dāng)?shù)倪\(yùn)動(dòng)學(xué)模型,可以更加準(zhǔn)確地模擬其運(yùn)動(dòng)過程.本文研究機(jī)器人在二維平面內(nèi)的運(yùn)動(dòng)情況,將傳感器坐標(biāo)系與機(jī)器人坐標(biāo)系合并,機(jī)器人坐標(biāo)系建立在機(jī)器人中心位置并隨其移動(dòng),全局坐標(biāo)系保持固定.移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型如圖1所示.

圖1 移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型

在圖1中,Xg-Yg為全局坐標(biāo)系,X-Y為機(jī)器人坐標(biāo)系,用(x,y,θ)T(T表示矩陣轉(zhuǎn)置)表示移動(dòng)機(jī)器人在全局坐標(biāo)系下的位姿,其中 (x,y)表示機(jī)器人的位置信息,θ表示機(jī)器人的朝向信息(航向角).假定移動(dòng)機(jī)器人左右兩輪的中心距離為2d,車輪半徑為r,線速度分別為VL、VR,則可得其運(yùn)動(dòng)學(xué)方程:

其中,v,ω分別為移動(dòng)機(jī)器人繞其旋轉(zhuǎn)中心的線速度和角速度.

1.2 機(jī)器人定位模型

由于最終要求解的是移動(dòng)機(jī)器人在全局坐標(biāo)系下的位姿數(shù)據(jù)[12],而傳感器測(cè)量的數(shù)據(jù)是在機(jī)器人坐標(biāo)系下的數(shù)據(jù),因此定位問題可以看作是機(jī)器人坐標(biāo)系至全局坐標(biāo)系的轉(zhuǎn)換問題.機(jī)器人坐標(biāo)系下的任意一點(diǎn)坐標(biāo) (xa,ya) 通過旋轉(zhuǎn)變換矩陣R,平移變換矩陣T可轉(zhuǎn)換至全局坐標(biāo)系下.其中,矩陣R,T分別為:

此外,在移動(dòng)機(jī)器人定位中,基于概率的估計(jì)方法經(jīng)常被使用,常用的概率定位方法有卡爾曼濾波[13]和粒子濾波[14],他們的主要區(qū)別是概率分布函數(shù)的不同.機(jī)器人的條件概率可表示為:

其中,xt-1表示機(jī)器人在t-1時(shí)刻的位姿;xt表示機(jī)器人在t時(shí)刻的位姿;ut表示t時(shí)刻的運(yùn)動(dòng)控制.模型表示在t-1時(shí)刻,已得機(jī)器人的位姿時(shí),對(duì)其執(zhí)行ut后,所得到的后驗(yàn)概率分布.假設(shè)m為周圍環(huán)境地圖信息,z為觀測(cè)狀態(tài)信息,則可得圖2所示的機(jī)器人定位模型.

圖2 移動(dòng)機(jī)器人定位模型

2 傳感器定位原理

2.1 里程計(jì)定位原理

里程計(jì)(odometer)結(jié)構(gòu)簡(jiǎn)單,可以在短時(shí)間內(nèi)較精確地測(cè)量到移動(dòng)機(jī)器人的位姿.理論上,若里程計(jì)沒有各因素的影響,僅使用單一里程計(jì)就可以實(shí)現(xiàn)精確定位.但在實(shí)際情況下,機(jī)器人的輪子難免會(huì)發(fā)生漂移或打滑等現(xiàn)象,因此僅使用里程計(jì)定位并不準(zhǔn)確,與機(jī)器人的真實(shí)位姿會(huì)有較大的差別.

假設(shè)輪子的編碼器參數(shù)為K線/轉(zhuǎn),在Δt時(shí)間內(nèi)輸出N個(gè)脈沖,則車輪的移動(dòng)距離ΔS為:

由式(6)可得機(jī)器人左輪移動(dòng)距離 ΔSL,右輪移動(dòng)距離ΔSR,則可得:

將式(7)代入式(1),可得移動(dòng)機(jī)器人繞其旋轉(zhuǎn)中心的線速度v和角速度 ω.此外,根據(jù)幾何關(guān)系,移動(dòng)機(jī)器人的移動(dòng)距離 ΔSt,旋轉(zhuǎn)角度 Δθ和旋轉(zhuǎn)半徑R可分別表示為:

假設(shè) Δt很小,機(jī)器人在t1時(shí)刻的位姿為(x1,y1,θ1)T,可得與之時(shí)長(zhǎng)相差 Δt的t2時(shí)刻的位姿(x2,y2,θ2)T為:

機(jī)器人的輪子不可避免會(huì)發(fā)生磨損變形,從而導(dǎo)致輪子半徑r發(fā)生變化,所以單一里程計(jì)定位會(huì)出現(xiàn)定位誤差,且誤差會(huì)隨著移動(dòng)的時(shí)間和距離不斷累積.

2.2 IMU 定位原理

IMU 體積小、成本低,通過加速度計(jì),陀螺儀和磁力計(jì)的測(cè)量數(shù)據(jù)計(jì)算得出移動(dòng)機(jī)器人的位姿信息.

機(jī)器人的位姿信息可以看作是在空間內(nèi)經(jīng)過一系列旋轉(zhuǎn)平移而得到的結(jié)果.假設(shè)機(jī)器人繞Z 軸旋轉(zhuǎn) α角度,繞X 軸旋轉(zhuǎn) β角度,繞Y 軸旋轉(zhuǎn)δ 角度,可得變換矩陣分別為:

則機(jī)器人坐標(biāo)系至全局坐標(biāo)系的變換矩陣Cbn為:

2.3 激光雷達(dá)定位原理

結(jié)合圖2,對(duì)激光雷達(dá)測(cè)量值建立概率學(xué)模型,可以用條件概率分布來表示其數(shù)據(jù)觀測(cè)模型:

其中,zt為t時(shí)刻的激光雷達(dá)觀測(cè)狀態(tài);xt為t時(shí)刻的機(jī)器人位姿;m為周圍環(huán)境地圖信息.由于激光雷達(dá)在某個(gè)測(cè)量時(shí)刻會(huì)得到一系列的測(cè)量值,因此可得:

其中,j為zt狀態(tài)測(cè)量值的數(shù)目.假設(shè)激光雷達(dá)得到的每個(gè)測(cè)量值之間相互獨(dú)立,則激光雷達(dá)的數(shù)據(jù)觀測(cè)模型條件概率分布可表示為:

3 定位實(shí)驗(yàn)和結(jié)果分析

移動(dòng)機(jī)器人定位實(shí)驗(yàn)基于ROS (robot operating system)系統(tǒng)平臺(tái)[15]進(jìn)行,分為未知環(huán)境定位與已知環(huán)境定位.實(shí)驗(yàn)對(duì)robot_localization 功能包[16]進(jìn)行了一定的改進(jìn)設(shè)置,功能包的每個(gè)狀態(tài)估計(jì)節(jié)點(diǎn)在收到傳感器的測(cè)量結(jié)果后便會(huì)估計(jì)機(jī)器人狀態(tài).根據(jù)機(jī)器人實(shí)際的傳感器模型參數(shù)進(jìn)行配置,修改了功能包原有的參數(shù)配置文件,保證了與實(shí)際機(jī)器人的物理模型參數(shù)一致.

實(shí)驗(yàn)選用定位誤差作為定位精度的評(píng)價(jià)指標(biāo),定位誤差指的是機(jī)器人的估計(jì)位置與真實(shí)位置之間的偏差.假設(shè)移動(dòng)機(jī)器人的真實(shí)位姿為(xm,ym,θm)T,估算所得位姿為(xn,yn,θn)T,則兩者之間的定位誤差de,航向角誤差 θe表達(dá)式為:

3.1 未知環(huán)境定位

在未知環(huán)境地圖的情況下,首先利用單一里程計(jì)對(duì)移動(dòng)機(jī)器人進(jìn)行定位研究,然后利用EKF 算法[17]融合里程計(jì)和IMU 進(jìn)行定位研究.EKF是借助線性濾波理論求解非線性問題的算法,如圖3所示,算法可以分為兩部分,一是狀態(tài)預(yù)測(cè),二是觀測(cè)更新:

圖3 EKF 算法流程

為了驗(yàn)證所提出的定位方法的有效性,進(jìn)行了對(duì)比實(shí)驗(yàn).控制機(jī)器人沿著既定的路線移動(dòng),比較單一里程計(jì)定位軌跡,EKF 算法融合里程計(jì)、IMU 定位軌跡和真實(shí)軌跡之間的差異,如圖4所示.

圖4 機(jī)器人運(yùn)動(dòng)軌跡

從圖4可以看到,在移動(dòng)機(jī)器人只有單一里程計(jì)定位時(shí),定位軌跡與真實(shí)軌跡之間的差距逐漸增大,存在較大的定位誤差,這會(huì)對(duì)后續(xù)的導(dǎo)航避障工作造成很大的不利影響.當(dāng)利用EKF 算法融合里程計(jì)和IMU定位后,定位軌跡雖與真實(shí)軌跡仍有差距,但與單一里程計(jì)的定位軌跡相比,與真實(shí)軌跡明顯相近,定位誤差有所降低.根據(jù)軌跡圖可發(fā)現(xiàn)每當(dāng)機(jī)器人改變航向角時(shí),其定位誤差會(huì)明顯增加.

為進(jìn)一步驗(yàn)證分析未知環(huán)境地圖下的定位效果,根據(jù)所測(cè)得的機(jī)器人的位姿數(shù)據(jù),選取12 個(gè)定位測(cè)試點(diǎn)進(jìn)行對(duì)比,分別記錄真實(shí)位置與估計(jì)位置,如表1所示.比較這兩種定位方法所估計(jì)的位置分別與真實(shí)位置之間的誤差以分析定位精度,處理結(jié)果如圖5所示.

圖5 未知環(huán)境下測(cè)試點(diǎn)定位誤差

表1 未知環(huán)境下真實(shí)位置與估計(jì)位置 (m)

由上述實(shí)驗(yàn)結(jié)果可知,移動(dòng)機(jī)器人在未知環(huán)境下,單一里程計(jì)定位存在較大的定位誤差,且會(huì)隨著移動(dòng)的時(shí)間和距離不斷累積,所取定位測(cè)試點(diǎn)的定位誤差均值為0.22 m 左右;EKF 算法融合定位后的定位誤差均值為0.1 m 左右,相對(duì)而言明顯降低,定位精度比單一里程計(jì)的精度高,但誤差仍會(huì)不斷累積,不能通過先驗(yàn)環(huán)境地圖信息進(jìn)行校正.

3.2 已知環(huán)境定位

實(shí)驗(yàn)利用ROS 開源社區(qū)中SLAM 算法Gmapping功能包[18]構(gòu)建環(huán)境地圖,如圖6所示.Gmapping 對(duì)于小環(huán)境地圖的構(gòu)建計(jì)算量小且精度高,所得到的實(shí)驗(yàn)環(huán)境地圖如圖7所示.

圖6 Gmapping 建圖

圖7 實(shí)驗(yàn)環(huán)境地圖

對(duì)于已知環(huán)境地圖定位,利用AMCL 算法對(duì)移動(dòng)機(jī)器人進(jìn)行定位研究.AMCL 算法是一種基于粒子濾波的,適用于二維環(huán)境下移動(dòng)機(jī)器人的概率統(tǒng)計(jì)定位方法[19].

AMCL 算法是MCL (Monte Carlo localization)算法的一種改進(jìn)[20].MCL 算法在對(duì)移動(dòng)機(jī)器人進(jìn)行定位時(shí),會(huì)將粒子均勻的散布在已知的環(huán)境地圖上,表示著機(jī)器人此時(shí)刻所有可能的位姿信息.當(dāng)機(jī)器人移動(dòng)時(shí),粒子均會(huì)隨之移動(dòng),但會(huì)存在與機(jī)器人移動(dòng)方向不一致的粒子.然后,MCL 算法會(huì)逐漸去除這些粒子,保留與機(jī)器人移動(dòng)方向一致的粒子.經(jīng)過這些粒子的收斂,從而得到機(jī)器人所處的實(shí)際位置.然而,MCL 算法將粒子去除后,無法注入新的粒子,可能會(huì)在粒子數(shù)量很小的情況下,丟失正確位置附近的粒子,這樣就無法解決機(jī)器人綁架問題.而AMCL 算法解決了這一問題,該定位算法在機(jī)器在面對(duì)機(jī)器人綁架問題時(shí),會(huì)注入新的粒子.AMCL 算法在已知環(huán)境地圖的情況下,通過激光雷達(dá)對(duì)環(huán)境的掃描觀測(cè)信息,會(huì)去除與觀測(cè)結(jié)果不一致的粒子,同時(shí)生成與結(jié)果一致的粒子.最終隨著機(jī)器人的移動(dòng),粒子收斂至其附近,從而獲得機(jī)器人的實(shí)際位置.

AMCL 算法采用粒子集合來描述機(jī)器人所處的所有可能的位置,且這些粒子帶有不同的權(quán)重信息.粒子集合St可表示為:

1)預(yù)測(cè)階段.首先,將粒子散布在已知的環(huán)境地圖上;然后,基于機(jī)器人的運(yùn)動(dòng)學(xué)模型,根據(jù)里程計(jì)的定位原理,結(jié)合式(5)對(duì)每個(gè)粒子的狀態(tài)進(jìn)行預(yù)測(cè).

2)權(quán)值更新階段.該階段需要激光雷達(dá)的掃描信息作為輸入.由式(14)-式(16)得到激光雷達(dá)的觀測(cè)數(shù)據(jù),對(duì)粒子的權(quán)重進(jìn)行更新,并進(jìn)行歸一化處理.

3)采樣更新階段.隨著機(jī)器人的移動(dòng),權(quán)重大小發(fā)生改變.從而去除權(quán)重小的粒子,保留權(quán)重大的粒子,同時(shí)產(chǎn)生新的預(yù)測(cè)粒子避免粒子退化現(xiàn)象.經(jīng)過反復(fù)迭代,使得多數(shù)粒子收斂至機(jī)器人的實(shí)際位置,實(shí)現(xiàn)對(duì)其定位.

傳統(tǒng)的AMCL 算法是利用單一的里程計(jì)測(cè)量模型作為運(yùn)動(dòng)模型采樣,但由于里程計(jì)容易受到多種因素的影響,其采樣結(jié)果與真實(shí)值有一定的誤差,導(dǎo)致定位效果較差.為了改善AMCL的定位效果,在已知環(huán)境地圖的定位過程中,本文的具體的算法流程如圖8.

圖8 算法流程圖

首先在預(yù)測(cè)階段,利用EKF 算法融合里程計(jì)和IMU的測(cè)量信息得到的移動(dòng)機(jī)器人位姿數(shù)據(jù)作為AMCL 算法運(yùn)動(dòng)模型的采樣源;然后在權(quán)值更新階段,充分利用激光雷達(dá)掃描的點(diǎn)云信息和先驗(yàn)地圖信息,更新給定粒子權(quán)值,并基于粒子的權(quán)重不斷迭代估計(jì)當(dāng)前位姿,最終得到移動(dòng)機(jī)器人在全局環(huán)境地圖下的最優(yōu)位姿.該方法的定位效果如圖9、圖10所示,圖中的粒子表示機(jī)器人此刻可能的位姿狀態(tài).從圖10中可以看出隨著機(jī)器人的運(yùn)動(dòng),粒子收斂至機(jī)器人的實(shí)際位置周圍,改進(jìn)后的AMCL 算法可以有效地對(duì)機(jī)器人定位.

圖9 機(jī)器人初始位姿和粒子分布

圖10 機(jī)器人最終位姿和粒子分布

為驗(yàn)證分析已知環(huán)境地圖下的定位效果,控制機(jī)器人移動(dòng)并從中選取定位測(cè)試點(diǎn)進(jìn)行對(duì)比,記錄所測(cè)得的位置數(shù)據(jù),如表2.比較該方法所估計(jì)的位置與真實(shí)位置之間的誤差以分析定位精度,處理結(jié)果如圖11.

圖11 已知環(huán)境下測(cè)試點(diǎn)定位誤差

表2 已知環(huán)境下真實(shí)位置與估計(jì)位置 (m)

由上述實(shí)驗(yàn)結(jié)果可得,移動(dòng)機(jī)器人在已知環(huán)境下,AMCL 算法所得的定位誤差均值為0.07 m 左右,相對(duì)于未知環(huán)境下的兩種定位方法的定位誤差明顯降低.由于已知先驗(yàn)環(huán)境地圖信息,因此該方法的定位誤差較小,校正了未知環(huán)境定位下的累積誤差,有效地提高了定位精度.

3.3 定位結(jié)果分析

針對(duì)移動(dòng)機(jī)器人單一傳感器定位精度低與環(huán)境地圖的重要性問題,實(shí)驗(yàn)分為未知環(huán)境定位與已知環(huán)境定位兩種情況.對(duì)于未知環(huán)境下的單一里程計(jì)定位、EKF 算法融合定位,環(huán)境地圖構(gòu)建以及已知環(huán)境下的AMCL 算法做出了一定的理論分析與實(shí)驗(yàn)研究.實(shí)驗(yàn)結(jié)果表明,未知環(huán)境定位會(huì)產(chǎn)生明顯的累積誤差,與真實(shí)位置有較大的差別,因此不適用于遠(yuǎn)距離定位;已知環(huán)境定位利用本文方法可以對(duì)累積誤差進(jìn)行一定的校正,相較于未知環(huán)境下的單一里程計(jì)定位與EKF 算法融合定位,誤差均值分別減少了68%、30%,提高了移動(dòng)機(jī)器人的定位精度,改善了定位效果,驗(yàn)證了所提出定位方法的有效性以及環(huán)境地圖的重要性.

4 結(jié)語

本文提出了基于里程計(jì)、IMU、激光雷達(dá)融合的移動(dòng)機(jī)器人定位方法,并通過實(shí)驗(yàn)加以驗(yàn)證.相較于未知環(huán)境定位,所提出的方法具有較高的定位精度,其具有以下特點(diǎn):1)多傳感器融合定位方法利用了各傳感器的優(yōu)勢(shì),克服了單一傳感器定位精度低的問題;2)通過激光雷達(dá)構(gòu)建環(huán)境地圖,校正了未知環(huán)境下定位的累積誤差.

為了進(jìn)一步研究所提出的多傳感器融合定位方法并提高移動(dòng)機(jī)器人定位的準(zhǔn)確性,后續(xù)計(jì)劃進(jìn)行以下工作:1)增加GPS 傳感器,以提高機(jī)器人的定位精度;2)增加環(huán)境的復(fù)雜性,研究機(jī)器人在表面不平整環(huán)境中的定位精度.