夏育泓，鄧三鵬，張志軍，祁宇明，呂松杰

（1.天津職業(yè)技術(shù)師范大學(xué) 機(jī)器人及智能裝備研究院，天津 300222；2.天津市智能機(jī)器人技術(shù)及應(yīng)用企業(yè)重點(diǎn)實(shí)驗(yàn)室，天津 300350；3.天津博諾智創(chuàng)機(jī)器人技術(shù)有限公司，天津 300350）

0 引言

同時(shí)定位與建圖技術(shù)是當(dāng)前移動(dòng)機(jī)器人領(lǐng)域的研究重點(diǎn)。SLAM 技術(shù)是指利用移動(dòng)機(jī)器人本身所帶有的傳感器來探測(cè)周圍環(huán)境信號(hào)，進(jìn)而形成當(dāng)前區(qū)域的局部地圖，然后再利用局部地圖形成全局地圖的技術(shù)。

目前，國(guó)內(nèi)外研究人員已經(jīng)開源了許多單一傳感器的SLAM 方法，例如基于相機(jī)的視覺SLAM 算法ORB-SLAM[1]、RGBD-SLAM[2]等，基 于 激 光 雷 達(dá) 的Gmapping[3]、Cartographer[4]等。但是這些SLAM 算法使用單一傳感器，所以會(huì)由于傳感器本身的缺陷導(dǎo)致算法存在不足。GPS 可以在室外獲得位置信息卻無法獲得室內(nèi)的位置信息；二維激光雷達(dá)，雖然能獲得較高的位置精度，但只能掃描雷達(dá)所在平面的二維信息；RGB-D 相機(jī)能獲得外界環(huán)境的顏色和深度信息，但是精度較差且會(huì)受到光照的影響；編碼器來估計(jì)機(jī)器人位姿但實(shí)際中會(huì)由于移動(dòng)機(jī)器人打滑導(dǎo)致出現(xiàn)定位誤差。IMU 利用加速度計(jì)和陀螺儀得到機(jī)器人的角加速度和加速度，從而計(jì)算出位移和角度信息，但是在運(yùn)行的過程中隨著溫度的升高數(shù)據(jù)漂移會(huì)不斷增加。因此研究人員根據(jù)各種傳感器的優(yōu)點(diǎn)進(jìn)行信息的融合來提高SLAM 效果已經(jīng)成為常態(tài)。王錦凱等[5]通過擴(kuò)展卡爾曼濾波器的原理結(jié)合了激光雷達(dá)數(shù)據(jù)與RGB-D 相機(jī)數(shù)據(jù)，當(dāng)相機(jī)與地圖匹配錯(cuò)誤時(shí)可以通過把激光雷達(dá)的數(shù)據(jù)與相機(jī)數(shù)據(jù)加以互補(bǔ)，而得到地圖。李全峰等[6]將視覺里程計(jì)、激光里程計(jì)采用Kalman濾波算法進(jìn)行融合，將融合后的數(shù)據(jù)與輪式里程計(jì)使用動(dòng)態(tài)二次加權(quán)的方式融合，提高了里程計(jì)的精度。

為了創(chuàng)建的地圖能顯示更多的信息并提高移動(dòng)機(jī)器人里程計(jì)的精度，提出將激光雷達(dá)數(shù)據(jù)和RGBD 相機(jī)數(shù)據(jù)融合提高對(duì)環(huán)境信息的感知，將里程計(jì)，IMU，激光雷達(dá)的信息進(jìn)行融合提高里程計(jì)的精度，并結(jié)合Gmapping 建圖算法，使得建圖精度得到一定程度提升，最后通過仿真驗(yàn)證了方法的可行性。

1 基本原理

要進(jìn)行數(shù)據(jù)融合，首先需要將數(shù)據(jù)轉(zhuǎn)換到同一坐標(biāo)系，再進(jìn)行濾波處理并融合，最后將激光雷達(dá)和相機(jī)融合的數(shù)據(jù)轉(zhuǎn)換成的激光雷達(dá)數(shù)據(jù)。

1.1 聯(lián)合標(biāo)定

數(shù)據(jù)融合需要使激光雷達(dá)數(shù)據(jù)和相機(jī)數(shù)據(jù)處在同一個(gè)坐標(biāo)系下，采用文獻(xiàn)[7]的方法對(duì)相機(jī)和激光雷達(dá)進(jìn)行聯(lián)合標(biāo)定。由于相機(jī)和雷達(dá)的位置在移動(dòng)機(jī)器人處相對(duì)位置一般都是固定的，直接使用ROS 中的static_transform_publisher 對(duì)雷達(dá)和相機(jī)進(jìn)行靜態(tài)坐標(biāo)變換。

1.2 數(shù)據(jù)融合

采用點(diǎn)云融合的方式進(jìn)行數(shù)據(jù)融合，經(jīng)聯(lián)合標(biāo)定、坐標(biāo)變換等處理使激光雷達(dá)數(shù)據(jù)和相機(jī)數(shù)據(jù)處于同一坐標(biāo)系下，對(duì)相機(jī)數(shù)據(jù)進(jìn)行濾波處理，最后將激光雷達(dá)數(shù)據(jù)和相機(jī)數(shù)據(jù)融合在一起，具體步驟如下：

（1）使用深度相機(jī)的原始點(diǎn)云會(huì)因?yàn)閿?shù)據(jù)過于龐大導(dǎo)致處理數(shù)據(jù)的時(shí)間過長(zhǎng)，從而使得極坐標(biāo)的發(fā)布頻率過低，最終出現(xiàn)誤差較大的地圖。所以通過體素濾波對(duì)深度相機(jī)點(diǎn)云進(jìn)行降采樣處理。

（2）去除地面和過高的點(diǎn)云：使用索引濾波器，過濾地面點(diǎn)云；使用直通濾波器，在Z 軸上設(shè)置高度閾值，將高于閾值的點(diǎn)云過濾。

（3）激光雷達(dá)數(shù)據(jù)截?。杭す饫走_(dá)掃描范圍為360毅，本次選取小車前進(jìn)方向?yàn)橹休S線，左右截取90毅，所以當(dāng)前掃描范圍為前進(jìn)方向正向的180毅。

（4）數(shù)據(jù)融合：通過PCL 中的指針變量將雷達(dá)和相機(jī)點(diǎn)云進(jìn)行拼接。

（5）使用pointcloud2_to_laserscan 將融合后生成的點(diǎn)云轉(zhuǎn)化為激光雷達(dá)數(shù)據(jù)。只有激光雷達(dá)的效果如圖1 中（a）所示，融合后的效果如圖1 中（b）所示。

圖1 點(diǎn)云效果

2 多傳感器里程計(jì)融合

對(duì)激光里程計(jì)數(shù)據(jù)、IMU 數(shù)據(jù)和輪式里程計(jì)數(shù)據(jù)進(jìn)行融合。常見的激光里程計(jì)的基本原理是采用點(diǎn)-點(diǎn)匹配的ICP 及其變種算法，該算法采用掃描激光雷達(dá)上的相鄰關(guān)鍵幀的方法實(shí)現(xiàn)定位，不過由于激光的高離散性，可能導(dǎo)致匹配失敗。采用基于距離流的RF2O 來獲取激光里程計(jì)信息，該方法相對(duì)于ICP 等點(diǎn)云匹配算法受到環(huán)境的影響較小，在平移信息的測(cè)量精度都高于ICP 及其變種算法。輪式里程計(jì)使用電機(jī)的光電編碼器獲得機(jī)器人的位移，同時(shí)結(jié)合左右輪的移動(dòng)激勵(lì)差來進(jìn)一步計(jì)算位姿變化。IMU 的姿態(tài)結(jié)算常使用的是方向余弦算法。擴(kuò)展卡爾曼濾波是卡爾曼濾波在非線性情況下的濾波方法，而DS 證據(jù)理論在對(duì)測(cè)量數(shù)據(jù)的信息處理和不確定的問題方面有良好的效果。所以采用擴(kuò)展卡爾曼濾波結(jié)合DS 證據(jù)理論方式融合IMU，輪式里程計(jì)，激光雷達(dá)里程計(jì)數(shù)據(jù)。

2.1 擴(kuò)展卡爾曼濾波系統(tǒng)模型及觀測(cè)方程

移動(dòng)機(jī)器人狀態(tài)方程：

其中：Xk為移動(dòng)機(jī)器人位置，f為機(jī)器人的非線性系統(tǒng)模型，則f（Xk-1）為在k-1 時(shí)刻的狀態(tài)值，uk-1表示k-1 時(shí)刻機(jī)器人控制狀態(tài)。wk-1是動(dòng)態(tài)噪聲，為高斯白噪聲。其協(xié)方差矩陣：

其中：Qk為過程噪聲的協(xié)方差矩陣。

將狀態(tài)矩陣進(jìn)行離散化處理，并進(jìn)行非線性分析，再以吟t對(duì)系統(tǒng)模型離散化，結(jié)果如下：

其中：xk為機(jī)器人x方向上k時(shí)刻的位置，yk為機(jī)器人y方向上k時(shí)刻的位置，漬k為機(jī)器人k時(shí)刻的偏航角，vk為機(jī)器人k時(shí)刻的線速度，wk為機(jī)器人k時(shí)刻的角速度。

由于是非線性系統(tǒng)，對(duì)函數(shù)f線性化處理，得到雅可比矩陣：

狀態(tài)轉(zhuǎn)移函數(shù)使用一階泰勒展開，將得到的雅可比矩陣Fk帶入系統(tǒng)方程，形成的系統(tǒng)狀態(tài)方程：

根據(jù)卡爾曼濾波基本方程，濾波器的預(yù)測(cè)方程：

2.2 擴(kuò)展卡爾曼濾波器更新過程

更新k時(shí)刻卡爾曼增益矩陣Kk：

更新K時(shí)刻的后驗(yàn)估計(jì)值：

更新k時(shí)刻后驗(yàn)估計(jì)協(xié)方差：

2.3 DS 證據(jù)理論在權(quán)值分配上的應(yīng)用

DS 證據(jù)理論是一種不確定性推理方法，能夠在沒有先驗(yàn)概率的前提下，對(duì)不確定性建模。借鑒文獻(xiàn)[8]的做法，利用DS 證據(jù)理論，在權(quán)值分配上對(duì)傳感器的測(cè)量信息進(jìn)行融合處理。

將多種傳感器測(cè)量的信息Zi與經(jīng)過濾波器的信息作為接收到的信息證據(jù)。

將證據(jù)按照DS 證據(jù)理論的計(jì)算方式加以處理，可以得到各個(gè)時(shí)刻傳感器信息所占據(jù)的權(quán)重比例：

其中k表示時(shí)刻，i表示傳感器的個(gè)數(shù)。

將信息進(jìn)行數(shù)據(jù)融合得到了新的信息序列：

T表示轉(zhuǎn)置。再對(duì)新的序列經(jīng)過濾波處理，獲得更為理想的濾波效果。

2.4 多傳感器融合里程計(jì)流程

先利用輪式里程計(jì)的數(shù)據(jù)和IMU 姿態(tài)數(shù)據(jù)利用DS 證據(jù)理論在權(quán)值分配上的算法處理融合接收到的測(cè)量數(shù)據(jù)，改善移動(dòng)機(jī)器人的轉(zhuǎn)向誤差，再利用融合后的里程計(jì)數(shù)據(jù)作為預(yù)測(cè)量，由激光里程計(jì)RF2O 得到的數(shù)據(jù)作為觀測(cè)量，經(jīng)過擴(kuò)展卡爾曼濾波方式融合，改善移動(dòng)機(jī)器人的位置精度。融合算法流程如圖2 所示。

圖2 融合算法流程

3 實(shí)驗(yàn)測(cè)試與分析

3.1 移動(dòng)機(jī)器人仿真模型和仿真環(huán)境搭建

為了驗(yàn)證移動(dòng)機(jī)器人SLAM 建圖效果，采用ROS系統(tǒng)下的Gazebo 與Rviz 平臺(tái)進(jìn)行仿真實(shí)驗(yàn)。機(jī)器人操作系統(tǒng)（robot operating system，ROS）是免費(fèi)且開放的框架，包含多種工具、庫(kù)，并且支持C++Python 等語言，為機(jī)器人的開發(fā)人員提供了很多的便利。通過使用ROS 中的Gazebo（3D 動(dòng)態(tài)模擬器），能夠設(shè)置需要的模擬環(huán)境，以及搭建攜帶包括激光雷達(dá)、IMU、相機(jī)、里程計(jì)等多種傳感器的機(jī)器人仿真模型。通過使用Rviz（三維可視化工具），可以實(shí)現(xiàn)對(duì)環(huán)境信息和傳感器信息等外部信息用圖形化的方式顯示，并且能夠發(fā)布信息，達(dá)到檢測(cè)機(jī)器人數(shù)據(jù)以及控制機(jī)器人運(yùn)動(dòng)的效果。為驗(yàn)證算法在障礙物無法被激光雷達(dá)掃描時(shí)的效果，設(shè)置了兩處低于掃描平面的障礙物。最終的仿真環(huán)境如圖3 所示。

圖3 仿真環(huán)境

3.2 實(shí)驗(yàn)結(jié)果與分析

在仿真環(huán)境中分別進(jìn)行了多次激光雷達(dá)建圖和激光雷達(dá)和相機(jī)融合建圖，建圖的效果如圖4 所示。

圖4 建圖效果

為了檢驗(yàn)多傳感器融合里程計(jì)的效果，將融合里程計(jì)所獲得的軌跡數(shù)據(jù)，原始里程計(jì)所獲得的軌跡數(shù)據(jù)與真實(shí)軌跡數(shù)據(jù)進(jìn)行比較，計(jì)算均方根誤差并進(jìn)行誤差分析。其中一次實(shí)驗(yàn)的輪式里程計(jì)軌跡和實(shí)際軌跡路線圖對(duì)比如圖5 所示，融合里程計(jì)軌跡和實(shí)際軌跡路線圖對(duì)比如圖6 所示。一部分實(shí)驗(yàn)的融合里程計(jì)和輪式里程計(jì)與實(shí)際軌跡對(duì)比的均方根誤差數(shù)據(jù)如表1 所示。

圖5 輪式里程計(jì)軌跡和實(shí)際軌跡路線圖對(duì)比

圖6 融合里程計(jì)軌跡和實(shí)際軌跡路線對(duì)比

表1 融合里程計(jì)和輪式里程計(jì)與真實(shí)軌跡對(duì)比的均方根誤差

從圖5，圖6 可以看出，融合激光里程計(jì)、輪式里程計(jì)、IMU 數(shù)據(jù)的里程計(jì)比只使用輪式里程計(jì)對(duì)移動(dòng)機(jī)器人的位姿估計(jì)效果有提升，尤其是在移動(dòng)機(jī)器人發(fā)生轉(zhuǎn)向時(shí)，融合里程計(jì)的軌跡比輪式里程計(jì)軌跡更加接近真實(shí)軌跡。經(jīng)過多次實(shí)驗(yàn)，融合里程計(jì)軌跡與真實(shí)軌跡對(duì)比，均方根誤差為0.0875，而輪式里程計(jì)軌跡與真實(shí)軌跡對(duì)比，均方根誤差為0.1791，融合后的里程計(jì)誤差約為只使用輪式里程計(jì)的誤差的48.86%，下降51.14%，效果明顯。

4 結(jié)語

為了提升移動(dòng)機(jī)器人建圖效果，提出了一種激光雷達(dá)與相機(jī)融合，并結(jié)合多傳感器融合里程計(jì)信息的建圖方法。該方法融合了激光雷達(dá)數(shù)據(jù)精度和相機(jī)數(shù)據(jù)豐富的優(yōu)勢(shì)，相對(duì)單一使用激光雷達(dá)的建圖方式，將激光雷達(dá)無法掃描到的物體加入到了地圖中，有效增加了地圖顯示的內(nèi)容。而且使用多傳感器融合的里程計(jì)，也提升了建圖中所需要的里程計(jì)信息的精確程度。實(shí)驗(yàn)結(jié)果表明，融合相機(jī)數(shù)據(jù)的建圖方法可以得到激光雷達(dá)所無法掃描物體的信息，并且在地圖中顯示。使用多傳感器融合里程計(jì)方式相比只使用輪式里程計(jì)的建圖方式，降低了在移動(dòng)機(jī)器人轉(zhuǎn)向時(shí)造成的累計(jì)誤差，提高了位置精度，融合里程計(jì)軌跡與輪式里程計(jì)軌跡對(duì)比，均方根誤差下降了51.14%，效果明顯。