沈念偉，于大泳

(上海理工大學(xué)機(jī)械工程學(xué)院，上海 200093)

17853589237@163.com;825298801@qq.com

1 引言(Introduction)

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

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

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

2.1 里程計定位原理

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

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

2.2 IMU定位原理

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

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

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

其具體流程主要分三步進(jìn)行：

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

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

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

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

2.3 激光雷達(dá)定位原理

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

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

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

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

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

3.1 里程計融合器的研究設(shè)計

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

對擴(kuò)展卡爾曼濾波的系統(tǒng)狀態(tài)方程進(jìn)行泰勒展開的一階線性化處理，其結(jié)果即為雅可比矩陣(Jacobian Matrix)：

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

3.2 激光雷達(dá)融合器的研究設(shè)計

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

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

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

(1)里程計融合器的系統(tǒng)狀態(tài)方程為：

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

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

3.3 加權(quán)融合器的研究設(shè)計

4 實驗分析(Experimental analysis)

本課題為了驗證多信息融合定位算法的優(yōu)勢，在上海理工大學(xué)綜合樓進(jìn)行了實驗驗證。實驗環(huán)境如圖6所示，機(jī)器人位姿解算的參考值是ROS系統(tǒng)中move_base功能包所發(fā)布的話題數(shù)據(jù)，通過Cartographer算法建立地圖，所建地圖如圖7所示。

圖6 實驗的真實環(huán)境Fig.6 Real environment of experiment

圖7 Cartographer建圖Fig.7 Cartographer mapping

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

圖8 機(jī)器人在地圖中的路標(biāo)位置Fig.8 Robot＇s landmark location on the map

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

表1 在復(fù)雜環(huán)境中機(jī)器人的定位誤差Tab.1 Positioning error of robot in complex environment

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

5 結(jié)論(Conclusion)

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