激光SLAM移動機器人室內(nèi)定位研究

嚴小意，郭 杭

(南昌大學信息工程學院，江西 南昌 330031)

移動機器人在未知室內(nèi)環(huán)境導航中，穩(wěn)定可靠的定位結(jié)果是規(guī)劃路徑的關(guān)鍵先決條件[1-2]。在室外環(huán)境，GNSS或GNSS/INS融合導航系統(tǒng)是一種有效的導航定位解決方法。然而，GNSS在室內(nèi)環(huán)境卻變得無效，而室內(nèi)避障對于移動機器人定位顯得尤為重要，必須根據(jù)環(huán)境場景、路徑規(guī)劃和指令這些信息來控制移動機器人沿著路徑移動[3-5]。因此，有必要采用一些導航傳感器來獲得這些信息，移動機器人一般采用聲吶、激光雷達、相機等傳感器來檢測環(huán)境。由于激光傳感器具有測距實時性快、不受光線環(huán)境限制、測距精度高、誤差相對恒定等特點，因此在構(gòu)建地圖、機器人的導航定位、環(huán)境建模等各方面得到普遍應(yīng)用[6]。

LiDAR掃描匹配技術(shù)雖然能夠提供高精度的位置、航向定位結(jié)果，但是遇到環(huán)境特征不明顯的情況，LiDAR的定位精度便大幅度下降，容易受到外界環(huán)境的干擾，同時LiDAR采樣頻率低[7]。慣性導航技術(shù)相對于LiDAR定位技術(shù)而言，它不受外界環(huán)境的影響，具備自主導航的能力，可以提供髙頻率的速度、位置、姿態(tài)全導航參數(shù)。可是，隨著時間延長，IMU的導航定位誤差會逐漸累積，因此單獨的慣性導航僅僅能在很短的時間內(nèi)提供高精度的導航定位結(jié)果。而LiDAR/IMU組合導航結(jié)合了LiDAR和IMU這兩種導航定位技術(shù)的優(yōu)勢，取長補短，可實現(xiàn)在室內(nèi)連續(xù)工作的高采樣率、高精度的組合系統(tǒng)。

綜上所述，本文采用基于LiDAR/IMU組合導航的信息融合技術(shù)，將LiDAR掃描匹配算法解算得到的位置、姿態(tài)觀測量作為組合卡爾曼濾波器的輸入量，與IMU得到的位置、姿態(tài)結(jié)果進行組合，以提高室內(nèi)移動機器人的運動軌跡定位精度。

1 傳感器介紹

1.1 慣性測量單元

IMU是用來測量物體三軸姿態(tài)角及加速度的傳感器[8]。本文僅對加速度計和陀螺儀的數(shù)據(jù)進行處理，而物體在載體坐標系下的加速度通過三軸加速度計獲取，載體相對于導航坐標系的角速度通過三軸陀螺儀獲取，根據(jù)在三維空間中物體的角速度和加速度，進而解算出物體的姿態(tài)。由于不可避免的各種干擾因素，導致陀螺儀及加速度計從初始對準開始產(chǎn)生誤差，其導航誤差即隨時間而增長，因此，應(yīng)該考慮它的誤差和噪聲。用字母b代表誤差，字母n代表噪聲。噪聲假設(shè)是零均值高斯白噪聲，且進行建模為隨機過程。實際值與角速度和線加速度測量值可以表達為

wt=wm-bw-nw

(1)

at=C(qt)(am-ba-na)+g

(2)

式中，wt和wm分別為角速度的實際值和測量值；g為重力矢量；qt為真實姿態(tài)的四元數(shù)；C(qt)為真實姿態(tài)qt的旋轉(zhuǎn)矩陣；nw和na分別為陀螺儀和加速度計的噪聲；bw和ba表示相關(guān)誤差。由于這些誤差，IMU測量會隨著時間推移產(chǎn)生漂移，為了避免這種漂移，IMU通常與其他傳感器融合，如相機或激光雷達。

1.2 激光雷達

LiDAR是一種測量從傳感器到被測物的角度和距離的光學傳感器。依據(jù)從發(fā)射激光脈沖到接收脈沖的時間計算傳感器與被測物體間的距離[9-10]。這種技術(shù)常用來創(chuàng)建高分辨率地圖，如SLAM是一個實質(zhì)性的應(yīng)用，其目的是構(gòu)建一個未知環(huán)境的地圖且同時追蹤機器人的位姿。激光雷達徑向掃描平面中的環(huán)境，為了獲得每個掃描點的坐標，采用方程式

式中，d為從激光雷達到掃描點的距離；φ為光束角度。由于(x，y)是在激光雷達載體坐標系下掃描點的坐標，并非導航坐標，因此需要將它們轉(zhuǎn)換為導航坐標。

2 組合導航方案

由于單一的激光導航測量會隨著時間延長存在累積誤差，GPS在室內(nèi)丟失信號而無法定位，因此，有必要引入其他導航傳感器來獲取移動機器人精準的位姿信息以改善單一傳統(tǒng)的導航系統(tǒng)帶來的累積缺陷[11]。總之，基于LiDAR/IMU的卡爾曼濾波組合導航系統(tǒng)可用來減少LiDAR定位誤差，從而提高移動機器人在室內(nèi)自主導航中的定位精度和穩(wěn)定。卡爾曼濾波[12]是一種最優(yōu)的線性估計方法，它使用狀態(tài)空間描述方法來描述系統(tǒng)，并連續(xù)估計各種傳感器輸出的估計誤差，從而得到所有傳感器的最優(yōu)信息。

測量更新和時間更新是卡爾曼濾波的主要過程[13]。時間更新是指根據(jù)系統(tǒng)前一個時刻的狀態(tài)和協(xié)方差矩陣，得到當前時刻狀態(tài)先驗估計和協(xié)方差先驗估計，為兩個相鄰時刻系統(tǒng)的狀態(tài)更新過程。測量更新是指將當前時刻獲得的測量信息對時間更新過程中所得的狀態(tài)先驗估計和協(xié)方差先驗估計進行更新校正的過程，以得到比先驗估計要精確的狀態(tài)后驗估計和協(xié)方差后驗估計的值[14-15]。依據(jù)當前時刻的狀態(tài)后驗估計和協(xié)方差后驗估計又能知道下一時刻的狀態(tài)先驗估計和協(xié)方差先驗估計，這就是卡爾曼濾波作用的優(yōu)勢。由于使用的狀態(tài)方程是基于誤差的，本文不涉及擴展卡爾曼濾波，這里使用構(gòu)成反饋脈沖的濾波。

卡爾曼濾波初始化

(3)

(4)

(5)

式中，k為離散時間；Xk∈Rn為k時刻離散系統(tǒng)的狀態(tài)向量；Ak∈Rn×n，為離散系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣；Bk∈Rn×l，為離散系統(tǒng)的輸入控制矩陣；uk∈Rl，為離散系統(tǒng)的輸入向量。

(2) 協(xié)方差更新可表示為

(6)

(3) 卡爾曼濾波增益可表示為

(7)

(4) 當前狀態(tài)最優(yōu)估計可表示為

(8)

(5) 更新當前時刻的協(xié)方差矩陣可表示為

(9)

式(7)—式(9)為卡爾曼濾波算法的測量更新過程。

3 室內(nèi)定位試驗分析

為了驗證卡爾曼濾波融合后的定位精度，本文使用安裝有Rfans16線激光雷達傳感器和IMU傳感器的WX-DP203移動機器人試驗平臺(由實驗室設(shè)計和開發(fā))，沿著實驗室進行了室內(nèi)試驗。將激光雷達作為外部傳感器，通過USB接口實現(xiàn)與PC的通信，利用ROS操作系統(tǒng)分布式的特點，PC機為主機，該產(chǎn)品數(shù)據(jù)采集頻率為10 Hz，測距精度小于3 cm，在最遠距離處測量誤差小于8 mm，在測量范圍為1.5 m內(nèi)，誤差小于0.5 mm。IMU用100 Hz頻率采集數(shù)據(jù)。為了比較最后結(jié)果的定位精度，預(yù)先計劃好相關(guān)路徑，設(shè)置包括起始點的4個控制點，并利用全站儀獲得局部坐標系下控制點的精準位置。原點設(shè)為起始點坐標，測量的控制點坐標見表1。

表1 控制點位置坐標 m

在試驗過程中，機器人向前移動了8.8 m之后回到起點，每秒移動不超過20 cm，總共獲得211個點。其中，機器人在ROS操作系統(tǒng)下形成的地圖與真實定位軌跡和LiDAR/IMU組合定位軌跡的結(jié)果分別如圖2、圖3所示。表2為設(shè)置的4個控制點與LiDAR/IMU卡爾曼濾波融合后位置誤差分析。真實軌跡與LiDAR/IMU組合定位軌跡的X方向、Y方向和歐拉距離的誤差分析如圖4所示。

表2 LiDAR/IMU卡爾曼濾波融合和控制點的位置比較 m

從表2可以看出，當機器人移動到左上拐角3.49 m處時，誤差緩慢累積，定位精度為0.024/3.49×100%=0.57%，這屬于比較高的定位精度，隨著機器人平臺移動，誤差累積的更快，定位精度是0.056/3.79×100%=1.47%，另外，從圖4中可以更加明顯地看到誤差累積變化。考慮到機器人本身可能在操作過程中側(cè)滑以及LiDAR傳感器容易受到噪聲和其他因素的影響，在試驗結(jié)果中存在一定程度的誤差。但是從定位精度來看，LiDAR/IMU卡爾曼濾波融合在減小累積誤差方面起著非常好的作用，這兩種傳感器組合可以實現(xiàn)更高的定位精度。機器人轉(zhuǎn)彎前累積誤差比較小，轉(zhuǎn)彎后累積誤差稍微變大，之后又慢慢變小。上述結(jié)果表明，短距離融合定位誤差累積非常小，定位精度高。在最初的計算階段IMU使軌跡從不穩(wěn)定變?yōu)榉€(wěn)定，當位姿發(fā)生變化大時(如沿路線轉(zhuǎn)彎)時，IMU傳感器作用顯得比較重要，能改善方向角的定位精度，并且還能提高方向角的穩(wěn)定性。

4 結(jié) 論

本文將LiDAR/IMU兩個傳感器通過卡爾曼濾波器相融合進行綜合定位系統(tǒng)研究，卡爾曼濾波器的綜合定位結(jié)果利用IMU傳感器減小LiDAR產(chǎn)生的漂移，進而減小其軌跡誤差，提高了室內(nèi)機器人的定位精度。Rfan16線LiDAR具有普通激光傳感器的優(yōu)點，且定位精度高，由于IMU的高采樣率和相對較高的精度增加了數(shù)據(jù)采集數(shù)量，LiDAR和IMU集成系統(tǒng)相互受益，克服了各自的缺點，從而提高了系統(tǒng)的穩(wěn)定性。然而，由于研究剛剛開始，該方法仍具有精度限制，還有許多工作等待后續(xù)處理。將來的研究工作是增加移動機器人的測量范圍并提高計算運動參數(shù)的準確度，因為這種方法很容易受到噪音的影響。此外，還將嘗試在機器人轉(zhuǎn)動時減小擺動幅度，從而減少姿態(tài)誤差。