無里程計移動機器人多傳感器融合定位算法*

張禮睿，孫勇智，劉力銘，李津蓉，許 垚

(浙江科技學院自動化與電氣工程學院，杭州 310023)

0 引言

穩(wěn)定精確的定位結果是實現移動機器人在未知環(huán)境自主移動的首要條件。移動機器人的定位是指在運動過程中，機器人通過搭載的傳感器感知周圍環(huán)境并采集運動數據，確定自身的位置信息[1]。根據移動機器人定位過程，主要分為絕對定位和相對定位[2]。目前，移動機器人定位最常用的方案是使用輪式里程計和慣性測量單元(inertial measurement unit，IMU)測量機器人的運動數據，使用激光雷達傳感器或視覺傳感器測量周圍的障礙物信息，通過多傳感器數據融合算法完成室內定位和導航[3]。

然而在實際生產生活中，部分移動機器人因為機械結構的限制、工作環(huán)境或成本因素無法安裝輪式里程計[4-5]，從而影響機器人的定位。為解決這個問題，IBRAGIMOV、龔學銳等[6-7]提出使用視覺里程計代替輪式里程計，通過VIORB-SLAM、DPPTAM等圖像處理算法，從攝像頭采集的圖像中得到機器人的線速度、角速度等數據，從而完成室內定位任務。但視覺傳感器對CPU計算能力要求高、算法復雜且實現成本高，并不適合在低成本的移動機器人系統(tǒng)上安裝使用。除了使用視覺里程計代替輪式里程計，嚴小意等[8]提出了激光雷達和IMU相融合的導航定位系統(tǒng)，使用激光雷達提取環(huán)境特征和構建地圖，通過IMU采集姿態(tài)信息進行卡爾曼濾波，補償位置和姿態(tài)輸出的誤差。但IMU中的加速度傳感器測量噪聲大[9]，導致機器人運動數據并不準確。胡釗政等[10]提出基于卡爾曼濾波框架的WiFi、激光雷達與地圖的融合定位新方法，利用卡爾曼濾波器實現機器人高精度的融合定位。但機器人定位問題本質上是非線性系統(tǒng)狀態(tài)估計問題[11]，采用卡爾曼濾波處理非線性系統(tǒng)問題時，會造成收斂性較差，增大誤差的問題，進而影響定位精度。

RF2O(range flow-based 2D odometry)是一種基于測距流的平面運動估計算法[12]，其計算量小、精度高，可以代替輪式里程計，從激光雷達的連續(xù)范圍掃描中計算出移動機器人的運動數據。因此，本文提出使用擴展卡爾曼濾波(extended kalman filter，EKF)融合RF2O算法和IMU的數據，提高系統(tǒng)收斂性，得到相對定位數據，再使用自適應蒙特卡洛定位算法(adaptive monte carlo localization，AMCL)進行修正，減小誤差累計，實現無里程計移動機器人的精確定位。

1 擴展卡爾曼濾波

擴展卡爾曼濾波是卡爾曼濾波解決非線性系統(tǒng)問題的一種變種方法，其核心是將非線性系統(tǒng)經過線性化處理后再進行卡爾曼濾波計算[13]。

在機器人模型中，假設系統(tǒng)的真實狀態(tài)與上一時刻的狀態(tài)和當前系統(tǒng)輸入有關，系統(tǒng)的測量狀態(tài)與當前系統(tǒng)的真實狀態(tài)有關，則建立系統(tǒng)的狀態(tài)轉換模型和傳感器觀測模型如式(1)所示。

(1)

式中，k為離散時間；xk為在k時刻下系統(tǒng)的真實狀態(tài)；uk為輸入矩陣；wk為過程噪聲，假定符合w(k)～N(0,Qk)的多元高斯噪聲；zk為在k時刻下的測量；vk為測量噪聲，假定符合v(k)～N(0,Rk)的多元高斯噪聲；f(x,u)、h(x)分別為非線性可微的狀態(tài)轉換函數和測量函數。

基于上述模型，將擴展卡爾曼濾波求解分為狀態(tài)預測和測量更新兩個方面。

(2)

(3)

2 基于EKF的融合算法

進行移動機器人的定位首先需要獲得機器人的速度、加速度和角速度等數據。激光雷達可以感知機器人周圍的環(huán)境，通過發(fā)射激光信號獲取目標距離和速度等數據。IMU可以測量出機器人的角速度和加速度數據，但IMU中的加速度數據噪聲大，導致推算出的機器人速度數據誤差很大，從而影響機器人的定位。

為解決該問題，本文采用融合RF2O算法和IMU數據的方案。RF2O算法能夠估計激光雷達的平面運動，計算出機器人的運動數據。激光雷達的速度約束方程[12]如式(4)所示。

(4)

式中，r和θ為激光掃描點與雷達之間的距離和夾角；Rα和Rt為r對夾角θ和時間的偏導；x和vx,s、y和vy,s以及ωs為移動機器人的運動數據，即X、Y軸上的速度與位移以及機器人的角速度；kα為常數，與激光雷達的性能有關。

式(4)推導過程中假設周圍環(huán)境靜止，并且舍去了泰勒展開后的高次項，同時激光雷達在旋轉時還會產生角度誤差，導致計算出的角速度數據并不準確，所以需要在此基礎之上融合IMU的數據以得到精準的結果。

本文的機器人系統(tǒng)僅考慮平面上的運動，即X、Y軸上的平移運動與Z軸上的旋轉運動。根據EKF的設計原理，建立系統(tǒng)狀態(tài)轉換模型如式(5)所示。

(5)

式中，Xk、Yk和θt分別為移動機器人在X、Y軸的位移以及偏航角；vk、φk和ωk分別為X、Y軸的線速度和角速度[14]。

融合RF2O算法計算的機器人X、Y軸線速度和IMU測量的機器人角速度，建立傳感器觀測模型如式(6)所示。

(6)

在機器人系統(tǒng)運行的過程中，傳感器每次測量得到數據后，通過擴展卡爾曼濾波的預測式(2)和更新式(3)的不斷迭代，逐漸估計機器人系統(tǒng)的狀態(tài)。

3 AMCL算法修正

擴展卡爾曼濾波本質上是對速度和角速度進行積分運算得到機器人的位移和偏航角數據，因此隨著系統(tǒng)運行時間的增加，誤差累計也會隨之增加，進而影響機器人的定位結果。

自適應蒙特卡洛定位算法是一種估計機器人在地圖中的姿態(tài)的定位方法[15]。該算法通過產生隨機粒子來估計機器人的位姿狀態(tài)，每個粒子都代表機器人未來可能的狀態(tài)。在機器人運動過程中，AMCL算法會逐漸丟棄與觀測不符的粒子，保留并重新生成更多與觀測一致的粒子，最終粒子會不斷收斂至機器人實際所在位置，從而完成機器人的定位任務[16]。使用AMCL算法計算得到定位數據是機器人以地圖數據為參考的定位，因此沒有誤差累計的問題。但AMCL算法會遇到綁架問題[17]，即當機器人丟失了先前的位置信息或者得到了一個錯誤的位姿信息，機器人就無法依靠AMCL算法繼續(xù)計算當前的位置信息。

為解決該問題，本文提出擴展卡爾曼濾波和AMCL算法相結合的方案，即使用擴展卡爾曼濾波計算出機器人的定位數據后，再使用AMCL算法測量該定位數據的累計誤差得到修正值，最后使用擴展卡爾曼濾波的計算結果減去修正值，得到精度更高且沒有誤差累計問題的定位數據。

擴展卡爾曼濾波可以減少誤差累計對系統(tǒng)的影響，從而解決AMCL算法的綁架問題。由于AMCL算法僅起到修正累積誤差的作用，而累計誤差在短時間內變化較小，因此修正值的更新頻率可以遠遠低于擴展卡爾曼濾波算法，以此來減少定位系統(tǒng)的總體計算量。最終定位系統(tǒng)的傳感器數據處理框圖如圖1所示。

圖1 傳感器數據處理方案框圖

該方案只需要使用IMU和激光雷達傳感器。RF2O算法通過激光雷達數據計算得到機器人的線速度，IMU通過測量得到角速度，再經過擴展卡爾曼濾波融合得到相對定位數據。最后，為克服相對定位數據的誤差累計問題以及在該數據的基礎上進一步減少測量誤差，使用AMCL算法來修正機器人的相對定位，得到絕對定位數據。通過相對定位可以得到準確并且連續(xù)的機器人線速度、角速度和加速度數據，通過絕對定位則可以得到比較準確且沒有誤差累計的機器人位移、偏航角數據。

4 實驗與結果分析

4.1 Gazebo仿真實驗

為了定量比較不同定位方案的效果，本文在Gazebo軟件中進行仿真實驗。在IMU和激光雷達的測量上增加高斯噪聲，以更加貼近真實場景。在仿真環(huán)境下控制模擬機器人移動，記錄不同定位方案的計算結果并與真實值做比較。

本文采用均方根誤差(RMSE)來描述測量的誤差[18]。假設P1,…,Pn為預測的姿態(tài)數據序列；Q1,…,Qn為真實的姿態(tài)數據序列；Δ為每秒的數據間隔，即每秒鐘預測的姿態(tài)數據個數，本次實驗Δ=10。均方根誤差的計算表達式為：

(7)

仿真實驗中采集移動機器人的定位數據，計算得到的不同定位方案的均方根誤差，并記錄不同定位方案的最大測量偏差，如圖2和圖3所示。

圖2 不同定位方案的均方根誤差 圖3 不同定位方案的最大測量偏差

分析圖中數據可知，經過AMCL算法修正后的定位方案相比EKF融合方案，X軸和Y軸位移的均方根誤差分別下降了62.1%和39.5%，最大測量偏差不超過0.074 m。由于使用IMU測量得到機器人角速度數據，使得經過EKF融合后的偏航角誤差較小，引入AMCL算法反而增大了偏航角的測量誤差，但該誤差仍然較小，最大測量偏差不超過1.6°。

4.2 移動機器人定位實驗

為了驗證仿真實驗中得到的結果，本文使用真實的機器人在現實環(huán)境中完成定位任務。機器人平臺如圖4a所示，硬件采用阿克曼車型的底盤，下位機使用KEA128單片機主板控制機器人運動，同時裝載一臺LS01G型激光雷達和ART-IMU-02A型IMU，上位機為一臺CPU為i3的工控機，配有Ubuntu16.04以及Kinect版本的ROS操作系統(tǒng)。

在現實環(huán)境中由于無法測量出定位數據的真實值，也就無法對測量結果進行定量比較。因此，本節(jié)通過定位數據以及激光雷達測量的障礙物數據繪制地圖來定性地比較不同定位方案的性能，用于實驗的環(huán)境如圖4b所示，包含走廊和房間。

(a) 移動機器人平臺 (b) 實際環(huán)境圖4 實際環(huán)境定位實驗

遠程控制機器人在走廊上移動，不同定位方案得到的機器人運動軌跡以及建立的地圖如圖5所示。其中，圖5a為參考建圖，定位數據是由目前普遍使用的基于粒子濾波的SLAM算法融合RF2O算法結果和IMU數據產生，圖5b的定位數據全部源于RF2O算法，而圖5c中的定位數據則是來自于本文提出的算法方案。

(a) 參考建圖 (b) 使用RF2O算法得到定位數據建圖 (c) 使用本文方案得到定位數據建圖圖5 不同定位方案估計軌跡以及地圖對比

對比3種方案建立的地圖效果可知，僅使用RF2O算法計算得到的定位數據誤差最大，圖5b方框中的地圖邊緣模糊，出現明顯形變，這是RF2O算法計算得到的角速度誤差較大導致的。而圖5c使用本文提出的算法方案，最終建立的地圖邊緣清晰、距離估計準確，非常接近參考地圖，證明定位數據誤差較小。

表1為不同算法方案的運行時長。對比發(fā)現，本文提出的算法計算量遠遠小于基于粒子濾波的參考定位方案。算法運行初期，方案平均耗時為0.068 s。隨著AMCL算法逐漸收斂，方案的計算量逐漸減小，在系統(tǒng)運行10 s后，方案平均耗時逐漸穩(wěn)定，僅為0.025 s。

表1 不同算法方案運行時長 (s)

5 結論

本文針對無里程計移動機器人的定位問題，提出一種多傳感器融合定位算法，通過EKF融合IMU數據和RF2O算法計算出的機器人線速度和角速度，得到相對定位數據；使用AMCL算法修正機器人的相對定位數據，得到絕對定位數據，從而完成移動機器人的定位任務。實驗結果表明，本文提出的定位算法得到的定位數據誤差較小，具有定位精度高且計算量小的優(yōu)點，由于不依賴里程計數據，所以能夠避免輪式里程計因輪子打滑、長時間運行誤差累積而導致定位失敗的問題，實現在無里程計的情況下完成精確定位。

本文目前提出的定位算法依賴激光雷達傳感器，在動態(tài)場景下RF2O算法計算的機器人速度數據仍然會有較大誤差，因此下一步計劃引入人工路標，進行信息的深度融合，以輔助移動機器人進行定位與導航。