移動機器人多傳感器融合的SLAM新方法

馬國力，朱曉春，陳子濤

（1.江蘇省連云港工貿高等職業技術學校，連云港 222000；2.南京工程學院 江蘇省先進數控技術重點實驗室，南京 211100）

0 引言

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

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

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

1.1 RBPF-SLAM算法原理

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

RBPF-SLAM算法步驟如下：

1）粒子重要性采樣。利用給定的重要性建議分布q(x1:t|z1:t,u1:t)采樣得到N個初始粒子，每個粒子對應一個權值，其中表示t時刻第i個粒子的權值；粒子權值計算。根據重要性采樣原則，粒子的權重定義為建議分布與目標分布的比值。粒子權值計算公式為：

歸一化處理得：

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

當Neff>Nth，不做重采樣；當Neff＜Nth，需要進行重采樣。

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

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

1:t,z1:t)。

1.2 多傳感器融合SLAM算法

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

1.2.1 基于里程計-IMU-激光雷達的多傳感器數據融合

在工作環境中，機器人SLAM算法通過里程計計算機器人線速度與角速度，獲取機器人實時位姿。但里程計自身會出現一定的累計誤差[11]。IMU傳感器由加速度計、地磁計、陀螺儀組成，能提供穩定的機器人姿態信息，本文利用IMU傳感器短時間內精度高、響應快的特點對里程計誤差進行修正。

針對移動機器人，對采集到的輪式里程計數據以及IMU數據進行建模，其空間狀態量表達式定義如下：

式(5)中Xt、Yt分別為移動機器人X、Y方向位移量；θ為機器人的姿態角。

t+1時刻的位姿可表示為：

系統的運動方程為：

設置閾值θ0，求出當前IMU傳感器與里程計測得的姿態角度之差，進行差值判斷，如果差值大于給定閾值，則采用IMU測出姿態角進行機器人的姿態估計，否則通過加權平均求得姿態角并對機器人進行姿態估計。IMU與里程計的融合過程如圖1所示。

圖1 IMU-里程計融合示意圖

傳統的RBPF-SLAM算法中通過里程計的信息構建機器人SLAM的運動模型。針對里程計易受外界信息擾動的特點，本文將精確的激光雷達數據融合到建議分布函數中，通過將里程計與IMU數據融合得到的運動模型與基于激光雷達的觀測模型聯合，獲得新的建議分布函數，改進后的建議分布函數為：

代入粒子權重計算公式可得：

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

圖2 觀測模型與運動模型的似然函數分布

1.2.2 改進粒子重采樣策略

在得到改進的建議分布函數后，利用估計結果從所提出的分布函數中提取一組新的粒子。為了緩解粒子耗散的問題，本文改進粒子重采樣策略。該策略通過構造新的分布函數，求出各粒子對應分布函數值，保留分布函數值在之間的粒子作為新粒子集，選取權值前三的粒子，最后在指定區間進行重采樣，具體步驟如下；

1）對粒子權值進行降序排序。

2）構造分布函數H(wk)，求出粒子分布函數值。粒子權重越小，對應分布函數值越大。分布函數值在之間的粒子權重小，與機器人實際位姿偏差較大。故保留分布函數值小于的粒子，舍棄分布函數值在的粒子：

3）根據分布函數值進行篩選，取出權值排名前三的粒子，然后任意選取一個粒子，設其在新粒子集中對應的位置為A，分布函數值為的粒子在新粒子集中對應的位置為B，最后在[A，B]之間執行重采樣。

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

步驟1：使用EKF方法融合里程計與IMU數據建立機器人運動模型。

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

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

步驟4：計算并更新粒子的重要性權值。

步驟5：根據改進的粒子重采樣策略執行重采樣操作。

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

2 實驗與分析

2.1 算法建圖仿真環境實驗與分析

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

圖3 仿真構建地圖

在仿真建圖實驗中，傳統算法平均建圖時間為375s，最低粒子數為40。本文算法需要平均建圖時間為302s，最低粒子數為15。圖3(a)和圖3(b)分別為傳統算法與本文多傳感器融合SLAM算法構建的仿真環境地圖。

實驗結果分析：當運行時間較長、粒子數較多時，傳統算法生成的地圖出現偏差，精度差。本文算法則維持了粒子多樣性，有效較少了粒子耗散，提高了算法的運行效率，可以獲取機器人精確的實時姿態，有效提高了地圖的精度。

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

圖4 機器人誤差X、Y軸誤差估計曲線

2.2 算法建圖實體環境實驗與分析

2.2.1 移動機器人硬件平臺

本文實驗平臺長度為45cm，寬度為38cm，高度為30cm，如圖5所示。移動平臺采用兩輪差速底盤，可以實現全方位移動。傳感器部分由激光雷達、慣性測量傳感器（IMU）、輪式里程計組成，激光雷達掃描半徑12米，可以按照設定的頻率進行掃描匹配。IMU提供穩定的實時加速度與角速度，里程計記錄機器人走過的路程并推算機器人的運動軌跡。控制系統部分包括微型工控機與電機驅動控制器。微型工控機裝有Ubuntu 16.04系統，配備Kinetic版ROS并利用里程計、IMU信息和激光雷達數據信息在線實時構建地圖，通過Rviz可視化軟件顯示地圖。電機驅動控制器為STM32F1，主要負責電機的驅動以及部分傳感器信息的收集。

圖5 機器人平臺

2.2.2 機器人軟件系統

機器人操作系統（ROS）是一個面向機器人的軟件平臺。ROS分為三個級層：文件系統級、計算圖級、開源社區級[12]。在ROS中，節點（NODE）為最小的進程單位，節點包括發布端與接收端，系統通過節點實現具體功能，如傳感器數據獲取、傳輸、發布。SLAM通過節點將傳感器數據傳輸至地圖模塊，實現機器人定位與建圖功能。

2.2.3 實體環境實驗與分析

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

表1 構建一致性地圖參數列表

圖6 實驗環境

圖7 兩種算法分別構建柵格地圖

3 結語

本文提出了移動機器人多傳感器融合SLAM新方法，研究了里程計數據與IMU數據融合的具體實現，得到更精準的機器人運動模型；在建議分布中融合激光雷達的觀測信息，有效減少系統所需的粒子個數；通過改進粒子重采樣策略，緩解重采樣導致的粒子耗散問題。最后分別在仿真環境和實體環境進行建圖實驗，實驗結果表明：本文SLAM方法使用更少運行時間同時地圖構建的精度更優。