嚴(yán)小意，郭 杭

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

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

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

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

1 傳感器介紹

1.1 慣性測(cè)量單元

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

wt=wm-bw-nw

(1)

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

(2)

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

1.2 激光雷達(dá)

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

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

2 組合導(dǎo)航方案

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

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

卡爾曼濾波初始化

(3)

(4)

(5)

式中，k為離散時(shí)間；Xk∈Rn為k時(shí)刻離散系統(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) 當(dāng)前狀態(tài)最優(yōu)估計(jì)可表示為

(8)

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

(9)

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

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

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

表1 控制點(diǎn)位置坐標(biāo) m

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

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

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

4 結(jié) 論

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