袁千賀,田 昕,沈斯杰

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

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

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

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

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

1 移動機器人定位系統(tǒng)

1.1 機器人運動學模型

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

圖1 移動機器人的運動學模型

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

其中,v,ω分別為移動機器人繞其旋轉中心的線速度和角速度.

1.2 機器人定位模型

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

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

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

圖2 移動機器人定位模型

2 傳感器定位原理

2.1 里程計定位原理

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

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

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

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

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

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

2.2 IMU 定位原理

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

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

則機器人坐標系至全局坐標系的變換矩陣Cbn為:

2.3 激光雷達定位原理

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

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

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

3 定位實驗和結果分析

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

實驗選用定位誤差作為定位精度的評價指標,定位誤差指的是機器人的估計位置與真實位置之間的偏差.假設移動機器人的真實位姿為(xm,ym,θm)T,估算所得位姿為(xn,yn,θn)T,則兩者之間的定位誤差de,航向角誤差 θe表達式為:

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

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

圖3 EKF 算法流程

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

圖4 機器人運動軌跡

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

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

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

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

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

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

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

圖6 Gmapping 建圖

圖7 實驗環(huán)境地圖

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

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

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

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

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

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

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

圖8 算法流程圖

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

圖9 機器人初始位姿和粒子分布

圖10 機器人最終位姿和粒子分布

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

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

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

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

3.3 定位結果分析

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

4 結語

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

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