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

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

0 引言

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

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

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

1 擴(kuò)展卡爾曼濾波

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

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

(1)

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

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

(2)

(3)

2 基于EKF的融合算法

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

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

(4)

式中，r和θ為激光掃描點(diǎn)與雷達(dá)之間的距離和夾角；Rα和Rt為r對(duì)夾角θ和時(shí)間的偏導(dǎo)；x和vx,s、y和vy,s以及ωs為移動(dòng)機(jī)器人的運(yùn)動(dòng)數(shù)據(jù)，即X、Y軸上的速度與位移以及機(jī)器人的角速度；kα為常數(shù)，與激光雷達(dá)的性能有關(guān)。

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

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

(5)

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

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

(6)

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

3 AMCL算法修正

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

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

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

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

圖1 傳感器數(shù)據(jù)處理方案框圖

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

4 實(shí)驗(yàn)與結(jié)果分析

4.1 Gazebo仿真實(shí)驗(yàn)

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

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

(7)

仿真實(shí)驗(yàn)中采集移動(dòng)機(jī)器人的定位數(shù)據(jù)，計(jì)算得到的不同定位方案的均方根誤差，并記錄不同定位方案的最大測量偏差，如圖2和圖3所示。

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

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

4.2 移動(dòng)機(jī)器人定位實(shí)驗(yàn)

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

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

(a) 移動(dòng)機(jī)器人平臺(tái) (b) 實(shí)際環(huán)境圖4 實(shí)際環(huán)境定位實(shí)驗(yàn)

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

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

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

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

表1 不同算法方案運(yùn)行時(shí)長 (s)

5 結(jié)論

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

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