馬國力，朱曉春，陳子濤

（1.江蘇省連云港工貿(mào)高等職業(yè)技術(shù)學(xué)校，連云港 222000；2.南京工程學(xué)院 江蘇省先進(jìn)數(shù)控技術(shù)重點(diǎn)實(shí)驗(yàn)室，南京 211100）

0 引言

SLAM（simultaneous localization and mapping，SLAM）是移動(dòng)機(jī)器人實(shí)現(xiàn)精確自主導(dǎo)航的基礎(chǔ)，定位與建圖是機(jī)器人自主導(dǎo)航的關(guān)鍵技術(shù)。粒子濾波原理是以貝葉斯推理和重要性采樣為基本框架的非參濾波器，它可以處理非線性、多峰分布等問題[1，2]。Thrun等人首先提出了基于粒子濾波的SLAM方法，將空間狀態(tài)中的粒子賦予權(quán)值，得到機(jī)器人系統(tǒng)狀態(tài)的后驗(yàn)概率分布，使定位更加準(zhǔn)確，但隨著粒子數(shù)的增加，地圖構(gòu)建復(fù)雜度也隨之增加[3]。Murphy等人提出用Rao-Blackwillised particle filter（RBPF）方法來解決SLAM問題[4，5]。RBPF-SLAM將SLAM問題分解為機(jī)器人姿態(tài)估計(jì)和地圖估計(jì)，使得粒子濾波的SLAM方法計(jì)算量大幅度減少。國內(nèi)羅榮華等人提出基于自適應(yīng)重采樣的RBPF-SLAM方法，提高了SLAM的運(yùn)算效率[6]?；趩我粋鞲衅鞯腟LAM方法在室內(nèi)環(huán)境條件差、機(jī)器人運(yùn)動(dòng)速度或轉(zhuǎn)向過快時(shí)表現(xiàn)不穩(wěn)定，例如：激光雷達(dá)傳感器掃描觀測(cè)距離有限、易受到環(huán)境中復(fù)雜幾何結(jié)構(gòu)影響。相機(jī)對(duì)機(jī)器人周圍環(huán)境的照明條件有一定的要求。編碼器電機(jī)經(jīng)過長(zhǎng)時(shí)間工作會(huì)產(chǎn)生累計(jì)誤差。羅元等人融合里程計(jì)與雷達(dá)數(shù)據(jù)，優(yōu)化建議分布函數(shù)，有效降低了預(yù)測(cè)階段機(jī)器人位姿的不確定性，減少了SLAM所需粒子數(shù)量[7]。

本文在以上方法的基礎(chǔ)上，將2D激光雷達(dá)、IMU、輪式里程計(jì)等多傳感器進(jìn)行融合，提出一種基于多傳感器融合的SLAM新方法。首先利用IMU對(duì)里程計(jì)數(shù)據(jù)進(jìn)行校正，建立機(jī)器人運(yùn)動(dòng)模型，再引入激光雷達(dá)觀測(cè)數(shù)據(jù)修正機(jī)器人運(yùn)動(dòng)模型，聯(lián)合優(yōu)化建議分布，提高機(jī)器人位姿后驗(yàn)概率模型的精度。同時(shí)針對(duì)RBPF-SLAM算法存在粒子耗散問題，改進(jìn)粒子重采樣策略。為了驗(yàn)證本文算法的有效性，分別在仿真環(huán)境與實(shí)體環(huán)境進(jìn)行試驗(yàn)。

1 基于多傳感器融合的SLAM方法

1.1 RBPF-SLAM算法原理

基本的SLAM算法描述為：通過在給定傳感器數(shù)據(jù)的情況下，利用機(jī)器人的運(yùn)動(dòng)模型p(xt|ut,xt-1)與觀測(cè)模型p(zt|xt,m)，來預(yù)測(cè)機(jī)器人下一時(shí)刻的位姿狀態(tài)[8]?；赗BPF（Rao-Blackwellized partical filter）的SLAM算法是通過蒙特卡洛方法解決機(jī)器人在大型復(fù)雜環(huán)境下的狀態(tài)估計(jì)問題[9]。其基本原理是：計(jì)算機(jī)器人運(yùn)動(dòng)軌跡x1:t和地圖m的聯(lián)合后驗(yàn)概率p(x1:t|z1:t,u1:t)，通過貝葉斯濾波器將其分解為如式1所示的軌跡估計(jì)和地圖估計(jì)兩個(gè)后驗(yàn)概率的乘積，降低了估計(jì)值的維度。

RBPF-SLAM算法步驟如下：

1）粒子重要性采樣。利用給定的重要性建議分布q(x1:t|z1:t,u1:t)采樣得到N個(gè)初始粒子，每個(gè)粒子對(duì)應(yīng)一個(gè)權(quán)值，其中表示t時(shí)刻第i個(gè)粒子的權(quán)值；粒子權(quán)值計(jì)算。根據(jù)重要性采樣原則，粒子的權(quán)重定義為建議分布與目標(biāo)分布的比值。粒子權(quán)值計(jì)算公式為：

歸一化處理得：

重采樣。根據(jù)粒子權(quán)值的大小，重新篩選粒子，淘汰小權(quán)值粒子，復(fù)制大權(quán)值粒子。為了減少重采樣次數(shù)，用有效粒子數(shù)Neff來衡量粒子權(quán)值的退化程度：

當(dāng)Neff>Nth，不做重采樣；當(dāng)Neff＜Nth，需要進(jìn)行重采樣。

4）狀態(tài)估計(jì)與地圖更新。根據(jù)所有的粒子信息，得到機(jī)器人的位姿狀態(tài)x(i)

1:t，根據(jù)傳感器的觀測(cè)信息z(i)1:t，求出環(huán)境地圖p(m(i)|x(i)

1:t,z1:t)。

1.2 多傳感器融合SLAM算法

傳統(tǒng)的RBPF-SLAM算法通過里程計(jì)與激光雷達(dá)傳感器信息分別提供機(jī)器人運(yùn)動(dòng)模型與觀測(cè)模型，單獨(dú)將運(yùn)動(dòng)模型作為粒子濾波的建議分布，在估計(jì)機(jī)器人位姿時(shí)存在較大的累計(jì)誤差[10]，使粒子計(jì)算的地圖后驗(yàn)分布與真實(shí)環(huán)境有較大的偏差，存在粒子內(nèi)存爆炸問題。本文針對(duì)上述問題，提出多傳感器融合SLAM方法，第一步融合IMU-里程計(jì)建立機(jī)器人運(yùn)動(dòng)模型，第二步聯(lián)合激光雷達(dá)觀測(cè)信息進(jìn)行二次融合，優(yōu)化建議分布函數(shù)；第三步改進(jìn)粒子重采樣策略，減緩粒子耗散問題。

1.2.1 基于里程計(jì)-IMU-激光雷達(dá)的多傳感器數(shù)據(jù)融合

在工作環(huán)境中，機(jī)器人SLAM算法通過里程計(jì)計(jì)算機(jī)器人線速度與角速度，獲取機(jī)器人實(shí)時(shí)位姿。但里程計(jì)自身會(huì)出現(xiàn)一定的累計(jì)誤差[11]。IMU傳感器由加速度計(jì)、地磁計(jì)、陀螺儀組成，能提供穩(wěn)定的機(jī)器人姿態(tài)信息，本文利用IMU傳感器短時(shí)間內(nèi)精度高、響應(yīng)快的特點(diǎn)對(duì)里程計(jì)誤差進(jìn)行修正。

針對(duì)移動(dòng)機(jī)器人，對(duì)采集到的輪式里程計(jì)數(shù)據(jù)以及IMU數(shù)據(jù)進(jìn)行建模，其空間狀態(tài)量表達(dá)式定義如下：

式(5)中Xt、Yt分別為移動(dòng)機(jī)器人X、Y方向位移量；θ為機(jī)器人的姿態(tài)角。

t+1時(shí)刻的位姿可表示為：

系統(tǒng)的運(yùn)動(dòng)方程為：

設(shè)置閾值θ0，求出當(dāng)前IMU傳感器與里程計(jì)測(cè)得的姿態(tài)角度之差，進(jìn)行差值判斷，如果差值大于給定閾值，則采用IMU測(cè)出姿態(tài)角進(jìn)行機(jī)器人的姿態(tài)估計(jì)，否則通過加權(quán)平均求得姿態(tài)角并對(duì)機(jī)器人進(jìn)行姿態(tài)估計(jì)。IMU與里程計(jì)的融合過程如圖1所示。

圖1 IMU-里程計(jì)融合示意圖

傳統(tǒng)的RBPF-SLAM算法中通過里程計(jì)的信息構(gòu)建機(jī)器人SLAM的運(yùn)動(dòng)模型。針對(duì)里程計(jì)易受外界信息擾動(dòng)的特點(diǎn)，本文將精確的激光雷達(dá)數(shù)據(jù)融合到建議分布函數(shù)中，通過將里程計(jì)與IMU數(shù)據(jù)融合得到的運(yùn)動(dòng)模型與基于激光雷達(dá)的觀測(cè)模型聯(lián)合，獲得新的建議分布函數(shù)，改進(jìn)后的建議分布函數(shù)為：

代入粒子權(quán)重計(jì)算公式可得：

對(duì)似然分布函數(shù)進(jìn)行分析。如圖所示，激光信息匹配的重要性權(quán)重方差非常小，加入精確的激光雷達(dá)掃描匹配信息來表示建議分布，則可以修正基于里程計(jì)信息的機(jī)器人運(yùn)動(dòng)模型，將采樣范圍限制在一個(gè)相對(duì)較小得到區(qū)域內(nèi)，如圖2所示，融入激光信息的建議分布能更接近真實(shí)目標(biāo)分布，用較少的粒子就可以表示機(jī)器人位姿的后驗(yàn)概率分布，有效減少了所需粒子的數(shù)量。

圖2 觀測(cè)模型與運(yùn)動(dòng)模型的似然函數(shù)分布

1.2.2 改進(jìn)粒子重采樣策略

在得到改進(jìn)的建議分布函數(shù)后，利用估計(jì)結(jié)果從所提出的分布函數(shù)中提取一組新的粒子。為了緩解粒子耗散的問題，本文改進(jìn)粒子重采樣策略。該策略通過構(gòu)造新的分布函數(shù)，求出各粒子對(duì)應(yīng)分布函數(shù)值，保留分布函數(shù)值在之間的粒子作為新粒子集，選取權(quán)值前三的粒子，最后在指定區(qū)間進(jìn)行重采樣，具體步驟如下；

1）對(duì)粒子權(quán)值進(jìn)行降序排序。

2）構(gòu)造分布函數(shù)H(wk)，求出粒子分布函數(shù)值。粒子權(quán)重越小，對(duì)應(yīng)分布函數(shù)值越大。分布函數(shù)值在之間的粒子權(quán)重小，與機(jī)器人實(shí)際位姿偏差較大。故保留分布函數(shù)值小于的粒子，舍棄分布函數(shù)值在的粒子：

3）根據(jù)分布函數(shù)值進(jìn)行篩選，取出權(quán)值排名前三的粒子，然后任意選取一個(gè)粒子，設(shè)其在新粒子集中對(duì)應(yīng)的位置為A，分布函數(shù)值為的粒子在新粒子集中對(duì)應(yīng)的位置為B，最后在[A，B]之間執(zhí)行重采樣。

1.3 基于多傳感器融合的SLAM算法流程

步驟1：使用EKF方法融合里程計(jì)與IMU數(shù)據(jù)建立機(jī)器人運(yùn)動(dòng)模型。

步驟2：根據(jù)地圖m(i)t-1，激光雷達(dá)傳感器使用迭代最近鄰（ICP）算法進(jìn)行掃描匹配得到觀測(cè)模型，融合機(jī)器人運(yùn)動(dòng)模型與觀測(cè)模型得到新的建議分布函數(shù)。

步驟3：從步驟2中的建議分布中，進(jìn)行粒子的采樣。

步驟4：計(jì)算并更新粒子的重要性權(quán)值。

步驟5：根據(jù)改進(jìn)的粒子重采樣策略執(zhí)行重采樣操作。

步驟6：根據(jù)機(jī)器人位姿x(i)t和觀測(cè)信息zt計(jì)算m(i)t，對(duì)地圖進(jìn)行更新。

2 實(shí)驗(yàn)與分析

2.1 算法建圖仿真環(huán)境實(shí)驗(yàn)與分析

采用開源的計(jì)算機(jī)仿真數(shù)據(jù)集檢驗(yàn)算法的性能，對(duì)兩種SLAM方法進(jìn)行仿真實(shí)驗(yàn)。在仿真實(shí)驗(yàn)中，建圖仿真環(huán)境尺寸為300m×300m，機(jī)器人最大行駛速度為4m/s，激光雷達(dá)傳感器最大觀測(cè)范圍為20m，速度誤差σv=0.3m/s，距離誤差為0.1m，角度誤差為1°，為了保證客觀性，所有仿真在同臺(tái)主機(jī)完成，系統(tǒng)類型為64位操作系統(tǒng)，處理器為Intel i5 8300，運(yùn)行內(nèi)存為8.00GB，仿真平臺(tái)為MATLAB 2016a。建圖實(shí)驗(yàn)結(jié)果圖如圖3所示。

圖3 仿真構(gòu)建地圖

在仿真建圖實(shí)驗(yàn)中，傳統(tǒng)算法平均建圖時(shí)間為375s，最低粒子數(shù)為40。本文算法需要平均建圖時(shí)間為302s，最低粒子數(shù)為15。圖3(a)和圖3(b)分別為傳統(tǒng)算法與本文多傳感器融合SLAM算法構(gòu)建的仿真環(huán)境地圖。

實(shí)驗(yàn)結(jié)果分析：當(dāng)運(yùn)行時(shí)間較長(zhǎng)、粒子數(shù)較多時(shí)，傳統(tǒng)算法生成的地圖出現(xiàn)偏差，精度差。本文算法則維持了粒子多樣性，有效較少了粒子耗散，提高了算法的運(yùn)行效率，可以獲取機(jī)器人精確的實(shí)時(shí)姿態(tài)，有效提高了地圖的精度。

為更好的驗(yàn)證算法的性能，使用MATLAB繪制機(jī)器人在仿真環(huán)境下的X、Y軸誤差曲線。曲線如圖4所示。多傳感器融合SLAM算法在X、Y軸的誤差比傳統(tǒng)RBPF算法誤差更小，更接近真實(shí)狀態(tài)。隨著時(shí)間的增加，總體上傳統(tǒng)算法的誤差曲線波動(dòng)幅度較大，本文基于多傳感器融合的SLAM算法得到的誤差曲線相對(duì)穩(wěn)定。表明本文算法在位姿估計(jì)時(shí)的精度高于傳統(tǒng)算法。

圖4 機(jī)器人誤差X、Y軸誤差估計(jì)曲線

2.2 算法建圖實(shí)體環(huán)境實(shí)驗(yàn)與分析

2.2.1 移動(dòng)機(jī)器人硬件平臺(tái)

本文實(shí)驗(yàn)平臺(tái)長(zhǎng)度為45cm，寬度為38cm，高度為30cm，如圖5所示。移動(dòng)平臺(tái)采用兩輪差速底盤，可以實(shí)現(xiàn)全方位移動(dòng)。傳感器部分由激光雷達(dá)、慣性測(cè)量傳感器（IMU）、輪式里程計(jì)組成，激光雷達(dá)掃描半徑12米，可以按照設(shè)定的頻率進(jìn)行掃描匹配。IMU提供穩(wěn)定的實(shí)時(shí)加速度與角速度，里程計(jì)記錄機(jī)器人走過的路程并推算機(jī)器人的運(yùn)動(dòng)軌跡?？刂葡到y(tǒng)部分包括微型工控機(jī)與電機(jī)驅(qū)動(dòng)控制器。微型工控機(jī)裝有Ubuntu 16.04系統(tǒng)，配備Kinetic版ROS并利用里程計(jì)、IMU信息和激光雷達(dá)數(shù)據(jù)信息在線實(shí)時(shí)構(gòu)建地圖，通過Rviz可視化軟件顯示地圖。電機(jī)驅(qū)動(dòng)控制器為STM32F1，主要負(fù)責(zé)電機(jī)的驅(qū)動(dòng)以及部分傳感器信息的收集。

圖5 機(jī)器人平臺(tái)

2.2.2 機(jī)器人軟件系統(tǒng)

機(jī)器人操作系統(tǒng)（ROS）是一個(gè)面向機(jī)器人的軟件平臺(tái)。ROS分為三個(gè)級(jí)層：文件系統(tǒng)級(jí)、計(jì)算圖級(jí)、開源社區(qū)級(jí)[12]。在ROS中，節(jié)點(diǎn)（NODE）為最小的進(jìn)程單位，節(jié)點(diǎn)包括發(fā)布端與接收端，系統(tǒng)通過節(jié)點(diǎn)實(shí)現(xiàn)具體功能，如傳感器數(shù)據(jù)獲取、傳輸、發(fā)布。SLAM通過節(jié)點(diǎn)將傳感器數(shù)據(jù)傳輸至地圖模塊，實(shí)現(xiàn)機(jī)器人定位與建圖功能。

2.2.3 實(shí)體環(huán)境實(shí)驗(yàn)與分析

在實(shí)際情況下，機(jī)器人的真實(shí)運(yùn)動(dòng)環(huán)境比仿真環(huán)境更加復(fù)雜，存在許多不確定因素。分別利用本文多傳感器融合SLAM算法和傳統(tǒng)RBPF-SLAM算法進(jìn)行SLAM實(shí)體環(huán)境實(shí)驗(yàn)，實(shí)驗(yàn)環(huán)境為科創(chuàng)中心的實(shí)驗(yàn)室與辦公區(qū)域（實(shí)驗(yàn)室與外部走廊），如圖6所示。實(shí)驗(yàn)結(jié)果如圖7所示，圖7(a)為常規(guī)算法構(gòu)建的實(shí)驗(yàn)室柵格圖，圖7(b)為本文算法構(gòu)建的實(shí)驗(yàn)室柵格圖。圖7(c)為常規(guī)算法構(gòu)建的辦公區(qū)域柵格地圖，圖7(d)為本文算法構(gòu)建的辦公區(qū)域柵格地圖。由表1可以看出，本文SLAM算法在實(shí)體環(huán)境建圖時(shí)，所需的粒子數(shù)量少于傳統(tǒng)算法，縮短了構(gòu)建地圖的時(shí)間，極大地提高了算法的效率。由圖6可以看出，由于傳統(tǒng)算法只使用里程計(jì)作為建議分布，里程計(jì)的累積誤差隨著時(shí)間的推移越來越大，機(jī)器人繞行一段時(shí)間后，紅圈部分構(gòu)建地圖出現(xiàn)丟失和假墻現(xiàn)象，而當(dāng)使用多傳感器融合的SLAM算法時(shí)，所構(gòu)建的地圖相對(duì)穩(wěn)定準(zhǔn)確。

表1 構(gòu)建一致性地圖參數(shù)列表

圖6 實(shí)驗(yàn)環(huán)境

圖7 兩種算法分別構(gòu)建柵格地圖

3 結(jié)語

本文提出了移動(dòng)機(jī)器人多傳感器融合SLAM新方法，研究了里程計(jì)數(shù)據(jù)與IMU數(shù)據(jù)融合的具體實(shí)現(xiàn)，得到更精準(zhǔn)的機(jī)器人運(yùn)動(dòng)模型；在建議分布中融合激光雷達(dá)的觀測(cè)信息，有效減少系統(tǒng)所需的粒子個(gè)數(shù)；通過改進(jìn)粒子重采樣策略，緩解重采樣導(dǎo)致的粒子耗散問題。最后分別在仿真環(huán)境和實(shí)體環(huán)境進(jìn)行建圖實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明：本文SLAM方法使用更少運(yùn)行時(shí)間同時(shí)地圖構(gòu)建的精度更優(yōu)。