基于多傳感器融合的機器人位姿估計研究

羅耀耀，鐘 山，王 銳，王 楠，楊韻秋

（1.成都理工大學 核技術與自動化工程學院，四川 成都 610059；2.成都理工大學 信息科學與技術學院，四川 成都 610059）

隨著人類社會的發展，自動駕駛技術與移動機器人領域成為當今社會研究熱點，在移動機器人控制中，機器人定位與位姿估計是機器人研究的核心問題之一。受限于室內復雜環境，諸如GPS 等定位方法無法在室內使用，所以研究者多使用里程計，超聲波等傳感器完成位姿估計。

傳統機器人室內定位一般采用里程計航跡推測或慣性導航定位等方法，如文獻[1]利用慣性系統進行定位，但是其定位精度取決于初始位置，姿態信息的精確性以及推算過程中速度的航向信息求解。而且隨著時間推移其累積誤差很難消除，文獻[2]在傳統UMBmark 方法下改進校核系統參數，在一定情況下減小了編碼器里程計方法進行航跡推測時的累積誤差。但是對于車輪打滑等情況造成的漂移問題不能有效解決，累積誤差隨著時間增大。

為解決此類問題，有研究者采用超寬帶（ultrawideband,UWB）技術定位[3]，通過脈沖無線電測距和測向，獲得了比較好的定位精度，但是其受限于成本和頻譜利用率，目前難以大規模使用。文獻[4—5]提出的利用視覺傳感器定位方法雖然也能避免累積誤差的問題，但是其算法運算量極大，同時受限于視覺傳感器性能，是當前的研究熱點。Jaimez 等[6]提出RF2O方法，是一種利用激光雷達密集掃描的點云數據，制定范圍流方程和幾何約束獲得運動估計的方法，該方法和視覺定位方法相似，數據處理量遠小于視覺方法，所得位姿估計累積誤差也較小，具有優異的性能。然而由于激光雷達的轉動帶來的角度誤差，導致RF2O方法的定位存在一定的不確定性。

本文在RF2O 算法的基礎上，針對移動機器人建立5 自由度的運動模型，尋求通過擴展卡爾曼濾波來融合激光雷達的數據和IMU 的偏航數據及Y 軸加速度數據[7-10]，進而解決因為激光雷達機械干擾帶來的定位誤差。

1 RF2O 方法及激光雷達介紹

RF2O 方法是一種快速精確的方法，仿照密集的3D 視覺測距法的方式，對激光雷達從二維平面掃描的密集點云數據進行處理。對點云數據制定基于傳感器速度范圍流約束方程，對所得幾何約束的魯棒函數最小化，獲得運動估計。RF2O 方法運算量低于視覺方法，轉換評估較為準確。但是由于激光雷達自身的旋轉，會導致旋轉不準確，本文通過RF2O 和IMU 的傳感器數據融合解決這一問題。

激光雷達[7]是以發射激光束探測目標的位置、速度等特征量的雷達系統。其工作原理是向目標發射探測信號（激光束），然后將接收到的從目標反射回來的信號（目標回波）與發射信號進行比較，適當處理后，就可獲得目標的有關信息，如目標距離、方位、高度、速度、姿態、甚至形狀等參數。本文所使用激光雷達為鐳神LS01G 型二維激光雷達，能在一個二維平面上進行掃描，并將掃描到的數據轉換為點云數據，該激光雷達的主要參數如表1 所示。

表1 激光雷達參數

2 坐標系定義及姿態表示

本文坐標系為北東地坐標指向，設導航坐標系為Odom，車體中心坐標系為Base_link。坐標軸 XO，OY，ZO分別指向移動機器人前方、右方、上方。

移動機器人只在二維平面運動，所以在姿態解算時只用解算其偏航角，由姿態轉換矩陣易得[11]：

其中：2xr、ry2、rz2為空間矢量在Base_link 坐標系的投影，1xr、1yr、1zr 為在Odom 坐標系投影。φ 為繞Odom 坐標系Z 軸角度，式（1）記作：

3 系統運動學模型

為了確定移動機器人在Odom 下的位姿信息，建立Odom 坐標系與Base_link 坐標系之間的關系，如圖1所示。

圖1 位姿轉換關系

X_Odom、Y_Odom、X_Baselink、Y_Baselink分別為Odom坐標系和Base_link 坐標系的坐標軸。由于移動機器人在近似二維平面上運動，所以其在Odom 坐標系的位姿可以用點 P(x, y) 和角度φ 完全確定，因此，可以用方向向量oβ 和旋轉矩陣表示其狀態：

根據式（2）易得：

則t 時刻Odom 坐標系下的位置βo(t)可以由（t-1）時刻位置oβ 加t 時刻的得到，通過式（2）逆矩陣變換得：

本文所使用移動機器人為四輪驅動結構。前輪舵機控制轉向，后輪電機控制速度，所以其運動由后輪線速度速度v 和舵機轉向角速度ω 控制。則：

易得：

由上所述，定義移動機器人的狀態變量

將機器人線加速度及角加速度作為噪聲處理，移動機器人運動學模型可以表述為：

4 傳感器觀測模型

本文傳感器為激光雷達和IMU，通過建立觀測模型描述傳感器實際測量信號與系統狀態變量的聯系。

4.1 激光雷達觀測模型

移動機器人上配有激光雷達，由于安裝問題，激光雷達和移動機器人中心有一定距離，需要通過一次靜態TF 變換將激光雷達變換至移動機器人中心。如圖2 所示，其中Base_laser 為雷達坐標系，雷達坐標系與機器人本體坐標系Base_link 在Z 軸高度差為12.35 cm，X 軸差7.14 cm。

圖2 TF 變換示意圖

激光雷達通過掃描周圍環境獲取到點云數據，通過RF2O 方法對點云數據進行處理，獲得移動機器人位姿數據，其觀測模型為：

對應觀測矩陣的雅克比矩陣為：

則RF2O 的觀測方程為：

其中：υk_rf2o為RF2O 方法觀測噪聲，υk_rf2o為高斯白噪聲，υk_rf2o～ N(0, Rk_rf2o)。

4.2 IMU 觀測模型

對于二維平面運行的機器人，陀螺儀只用測量Z軸的角速度，其觀測模型為

本文中對IMU 測得Z 軸的角速度積分得到移動機器人偏航角φ。

同時對Y 軸方向加速度計測得加速度微分得到速度：

對其積分得到：

由此得IMU 觀測模型為：

本文將（t-1）時刻 himu值與t 時刻 himu值差分，得到 φΔ、 Δv 作為系統控制輸入量。

5 擴展卡爾曼濾波器設計

擴展卡爾曼濾波是一種使用的系統動態模型和一系列隨時間變化的觀測模型，包含統計噪聲和其他不確定性，用來產生估計未知變量的算法[12]。其作用對象是一個非線性控制系統，由于移動機器人的運動學模型存在非線性情況，所以本文采用EKF 融合RF2O數據和IMU 數據。如圖3 所示。

圖3 卡爾曼濾波框圖

5.1 系統模型及觀測方程

移動機器人狀態方程為：

其中：f為非線性系統模型，則f(Xk-1)為移動機器人（k-1）時刻的狀態值，uk1-表示（k-1）時刻機器人控制輸入。由式（19）差分：

其中wk1-為系統動態噪聲，為高斯白噪聲。其協方差矩陣為：

將（9）式運用于EKF 時，對其進行離散化非線性處理，以采樣周期 Δt 對系統模型離散化，其結果如下：

由于系統是非線性的，對函數f 線性化處理，得到雅克比矩陣：

則將狀態轉移函數一階泰勒展開得到的雅可比矩陣Fk帶入系統方程得，線性化后的系統狀態方程為：

由上述公式，根據卡爾曼濾波基本方程可得，濾波器的預測過程為：

5.2 擴展卡爾曼濾波器更新過程

k 時刻卡爾曼增益矩陣 Kk更新：

k 時刻后驗估計值更新：

k 時刻后驗估計協方差更新：

6 實驗測試與分析

6.1 實驗平臺設計

本文使用工控機作為主控制器，K-60 單片機作為下級驅動控制器，路由器為主機與從機提供信息交換。機器人實物如圖4 所示。

工控機搭載I3-7100U 處理器，4 G 內存，供電12 V，操作系統為Ubuntu16.04+ROS KINETIC。其主要作用是處理激光雷達和IMU 數據并將數據發送給單片機。K-60 單片機接收到工控機消息后輸出PWM信號控制舵機和電調驅動移動機器人運行。系統框架如圖5 所示。

圖4 機器人實物

圖5 移動機器人系統框圖

實驗過程如下：

（1）使用鍵盤控制機器人沿著Odom 坐標系y 方向走2 m，重復10 次。

（2）使機器人繞走廊通過Gmapping 建圖并進行路徑規劃。

6.2 實驗結果及分析

實驗結果如圖6—10 所示，為驗證本文融合IMU數據后的位姿估計算法性能，與傳統RF2O 方法的誤差情況比較，在進行實驗步驟1 的實驗后，選取其中4 次行走軌跡下的均方根誤差分析，其評價函數為

圖6 EKF 數據

圖7 RF2O 數據

圖8 EKF 位姿與真實位姿誤差

圖9 RF2O 方法與真實位姿誤差

圖10 EKF 與RF2O 方法誤差對比

表2 均方根誤差

可以明顯看出，在融合IMU 數據后，RF2O 算法的誤差約為融合前誤差的76.1%，下降23.9%，效果明顯。

使用本文算法所的里程計進行Gmapping 建圖效果如圖11 所示，所得地圖邊緣清楚，收斂性好同時距離估計準確。

圖11 Gmapping 建圖效果

7 結論

通過擴展卡爾曼濾波融合IMU 所得姿態信息與RF2O 所得數據。經過多組實地實驗。結果表明：該方法顯著降低了由于激光雷達旋轉造成的RF2O 方法的累積誤差，均方根誤差降低23.9%。實現了室內復雜環境下移動機器人的準確定位，該方法不借助編碼器等傳感器，對于輪胎打滑等造成累積誤差的情況能有效避免，在移動機器人室內位姿估計的算法中，具有很大的優越性和實用價值。