基于多信息融合的輪式機器人定位算法研究

沈念偉，于大泳

(上海理工大學機械工程學院，上海 200093)

17853589237@163.com;825298801@qq.com

1 引言(Introduction)

機器人在室內定位的過程中，位姿是極為重要的，精確地獲取位姿能夠大大減小導航的誤差。機器人位姿主要依靠傳感器去獲取，但是單一傳感器存在一定的缺陷，而多種傳感器的信息融合卻能夠產生更加準確、穩定以及可靠的定位精度。目前用于定位的傳感器的種類繁多，例如人們所熟知的GPS導航定位系統、激光雷達傳感器、IMU傳感器和里程計，等等。這些傳感器都可以用于不同場景的導航，GPS依據天上的衛星系統來進行定位導航，在室外的定位效果最佳，但是在室內卻差強人意，一般用于汽車、手機等產品上；激光雷達傳感器是通過發射激光束獲取點云數據來進行定位的，但是在一些較為窄長的通道地圖中定位效果卻不太理想；IMU是通過加速度計和陀螺儀來獲取機器人的線速度和角速度進行定位的，其最大的優勢是在短時內能夠產生極佳的定位效果，但是隨著時間的推移，IMU也會產生累積誤差，之后的定位效果也不盡人意；里程計主要通過安裝在車輪上的光電編碼器來獲取機器人的位姿信息，但是在機器人的運動過程中難免會出現車輪打滑的情況，其定位效果也會大打折扣。

綜上所述，使用單一的傳感器進行定位，肯定無法適應不同的場景，其定位效果會有欠缺，很難達到理想的狀態，因此考慮使用多傳感器的信息進行融合來提高機器人的定位精度。

2 傳感器定位原理(Sensor positioning principle)

2.1 里程計定位原理

圖1 里程計模型Fig.1 Odometer model

里程計傳感器在車輪打滑的情況下會產生一定的定位誤差，這樣的誤差會在很大程度上降低里程計的精度。因此，使用里程計時一般都會配備IMU傳感器來進行校正，這樣就可以很好地克服這個缺點。

2.2 IMU定位原理

IMU傳感器主要是依靠陀螺儀和加速度計來進行定位的，首先陀螺儀傳感器可以獲得機器人的角速度數據，加速度計傳感器可以獲得機器人的加速度數據，對加速度數據進行兩次積分后得到機器人的位移數據。

對于IMU傳感器的姿態解算，目前常用且較為優秀的算法為方向余弦矩陣算法(Direction Cosine Matrix，DCM)。這種算法本質上是一種互補濾波算法，是一個用低通濾波器濾除高頻噪聲，用高通濾波器濾除低頻噪聲，分別把相對應測量中的噪聲去除，然后融合兩種信號的過程，如圖2所示。

圖2 互補濾波原理Fig.2 Principle of complementary filtering

其具體流程主要分三步進行：

(1)構建方向余弦矩陣方程。機器人在運動過程中，其在空間中的位姿是繞著、、軸運動所形成的，繞軸旋轉的順序不相同，所使用的分解方式也不相同。最常用的分解方法是歐拉角法，其原理是把機器人的位姿變換通過在、、軸的旋轉順序來表示，即：

圖3 歐拉角示意圖Fig.3 Euler angle diagram

(2)方向余弦矩陣對時間進行求導。在得到機器人的方向余弦矩陣之后，對其進行求導，可得到歐拉角的微分方程，即：

可以采用四階龍格庫塔法對歐拉角的微分方程進行求解，結果即為機器人的實時位姿。

2.3 激光雷達定位原理

對于激光雷達傳感器的位姿解算算法而言，目前比較常用就是最近點搜索法(Iterative Closest Point，ICP)。ICP的基本原理及推導過程如下：

圖4 ICP算法的匹配原理Fig.4 Matching principle of ICP algorithm

3 多傳感器信息融合算法的設計(Design of multisensor information fusion algorithm)

通過第2部分的傳感器定位原理可知，依靠單一的傳感器進行定位時，機器人可能會存在比較大的定位誤差，這些誤差會導致機器人“迷失方向”。因此，本文提出一種多傳感器信息融合的定位算法，其流程圖如圖5所示。

圖5 多傳感器信息融合定位流程圖Fig.5 Flow chart of multi-sensor information fusion positioning

3.1 里程計融合器的研究設計

里程計融合器是通過擴展卡爾曼濾波算法融合里程計傳感器和IMU傳感器的信息。在運用擴展卡爾曼濾波過程中，用里程計的運動模型作為預測量，用IMU的數據作為觀測量。根據公式(4)可得擴展卡爾曼濾波的系統狀態方程：

對擴展卡爾曼濾波的系統狀態方程進行泰勒展開的一階線性化處理，其結果即為雅可比矩陣(Jacobian Matrix)：

根據公式(13)和公式(14)可以求出系統的觀測方程：

3.2 激光雷達融合器的研究設計

由于激光雷達求解機器人位姿會因為沒有精準的初始位姿導致定位失敗，因此本文設計了激光雷達融合器，其最主要的作用是融合激光雷達的信息以及里程計融合器輸出的位姿信息來對機器人進行位姿解算，信息融合的算法依舊是運用擴展卡爾曼濾波算法。

在激光雷達融合器的設計中，激光雷達的數據作為觀測量，激光雷達的觀測模型如公式(16)所示：

在第2部分中介紹了使用ICP算法進行激光雷達的定位，使用這種方法的優點就是定位精度高，但是該算法的使用比較依賴位姿的初始值，一旦初始值效果不理想，就容易導致算法陷入局部最優的情況，這樣就會導致定位精度降低。激光雷達融合器的設計流程如下所示：

(1)里程計融合器的系統狀態方程為：

(2)根據ICP算法，其算法的初始值是里程計融合器輸出的位姿，其激光雷達的測量值為：

(4)跳轉到第一步進行迭代，可以不斷地輸出激光雷達融合器的值。

3.3 加權融合器的研究設計

4 實驗分析(Experimental analysis)

本課題為了驗證多信息融合定位算法的優勢，在上海理工大學綜合樓進行了實驗驗證。實驗環境如圖6所示，機器人位姿解算的參考值是ROS系統中move_base功能包所發布的話題數據，通過Cartographer算法建立地圖，所建地圖如圖7所示。

圖6 實驗的真實環境Fig.6 Real environment of experiment

圖7 Cartographer建圖Fig.7 Cartographer mapping

首先，在實驗場景中標記一個初始點，記為坐標(0，0)，初始點位于機器人底盤的中心位置。以機器人正前方為軸，垂直軸方向為軸建立直角坐標系，在復雜的實驗場景中隨機設置四個路標，并且測量和記錄機器人在坐標系中的位置。然后，在終端輸入rosrun turtlesim turtle_teleop_key，操作機器人移動，機器人運動到路標位置時，計算機終端會輸出機器人的位姿數據。在教室中隨機設置的四個路標如圖8所示。

圖8 機器人在地圖中的路標位置Fig.8 Robot＇s landmark location on the map

每次機器人從起始點移動到路標處后就記錄下機器人的位姿信息，然后繼續朝向下一個路標移動，直到返回初始點，更換路標坐標，重復三次實驗。單獨進行單一里程計定位、單一激光雷達定位及多傳感器信息融合定位，誤差結果如表1所示，根據誤差結果的絕對值繪制誤差折線圖如圖9所示。通過對比可知，多信息融合的定位誤差最小，且相對于單一傳感器，其平均定位精度提高了約60%。

表1 在復雜環境中機器人的定位誤差Tab.1 Positioning error of robot in complex environment

圖9 定位誤差折線圖Fig.9 Line chart of positioning error

5 結論(Conclusion)

機器人的定位問題一直是機器人導航研究的難點，僅靠單一的傳感器無法達到較為理想的定位狀態。合理利用里程計、IMU和激光雷達，通過對多傳感器的信息進行融合，能夠極大提高定位精度。通過實驗結果可知，相對于單一傳感器而言，多信息融合的定位精度提升了60%，從而使得后期機器人的導航精度也會有所提升。