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

沈念偉，于大泳

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

17853589237@163.com;825298801@qq.com

1 引言(Introduction)

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

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

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

2.1 里程計(jì)定位原理

圖1 里程計(jì)模型Fig.1 Odometer model

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

2.2 IMU定位原理

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

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

圖2 互補(bǔ)濾波原理Fig.2 Principle of complementary filtering

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 實(shí)驗(yàn)分析(Experimental analysis)

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

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

圖7 Cartographer建圖Fig.7 Cartographer mapping

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

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

每次機(jī)器人從起始點(diǎn)移動(dòng)到路標(biāo)處后就記錄下機(jī)器人的位姿信息，然后繼續(xù)朝向下一個(gè)路標(biāo)移動(dòng)，直到返回初始點(diǎn)，更換路標(biāo)坐標(biāo)，重復(fù)三次實(shí)驗(yàn)。單獨(dú)進(jìn)行單一里程計(jì)定位、單一激光雷達(dá)定位及多傳感器信息融合定位，誤差結(jié)果如表1所示，根據(jù)誤差結(jié)果的絕對(duì)值繪制誤差折線圖如圖9所示。通過(guò)對(duì)比可知，多信息融合的定位誤差最小，且相對(duì)于單一傳感器，其平均定位精度提高了約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ī)器人的定位問(wèn)題一直是機(jī)器人導(dǎo)航研究的難點(diǎn)，僅靠單一的傳感器無(wú)法達(dá)到較為理想的定位狀態(tài)。合理利用里程計(jì)、IMU和激光雷達(dá)，通過(guò)對(duì)多傳感器的信息進(jìn)行融合，能夠極大提高定位精度。通過(guò)實(shí)驗(yàn)結(jié)果可知，相對(duì)于單一傳感器而言，多信息融合的定位精度提升了60%，從而使得后期機(jī)器人的導(dǎo)航精度也會(huì)有所提升。