冉 騰，袁 亮，吳金強，羅 高

（新疆大學(xué)機械工程學(xué)院，新疆 烏魯木齊 830047）

1 引言

SLAM（simultaneous localization and mapping）全稱同時定位與建圖，這一概念于1986 年提出，各國學(xué)者一致將基于概率估計的定位與環(huán)境地圖創(chuàng)建定為移動機器人的基本問題[1]。精準(zhǔn)的定位與地圖構(gòu)建是移動機器人高效且安全作業(yè)的前提，因此使機器人對環(huán)境進(jìn)行更準(zhǔn)確的感知并且提高SLAM 精度是十分重要的。

起初基于Rao-Blackwellized 的粒子濾波算法被用于解決SLAM 問題[2-3]，但是構(gòu)建精確地圖需要大量的粒子，并且重采樣過程極易發(fā)生粒子耗盡的問題。文獻(xiàn)[4]提出改進(jìn)的RBPF 方法將觀測信息與里程計信息同時用于機器人軌跡預(yù)測，并減少重采樣次數(shù)降低粒子耗盡的風(fēng)險，算法在一定粒子數(shù)的前提下能得到較理想的SLAM 結(jié)果。里程計數(shù)據(jù)通?？赏ㄟ^編碼器獲得，而編碼器自身精度受環(huán)境影響較大，并且隨著機器人移動范圍的擴(kuò)大里程計數(shù)據(jù)變的不可靠，因此激光雷達(dá)等設(shè)備成為大規(guī)模環(huán)境下精確建圖的主要傳感器[5]，但由于其測距范圍有限導(dǎo)致位置估計仍存在較大誤差，因此在大規(guī)模的室內(nèi)外場景中利用環(huán)境路標(biāo)輔助定位的方法被廣泛應(yīng)用。文獻(xiàn)[6]在路標(biāo)組定位的基礎(chǔ)上提出多元M 序列的人工路標(biāo)設(shè)置方法，簡化了路標(biāo)種類優(yōu)化出的方案表現(xiàn)出較好的性能。文獻(xiàn)[7]提出一種幾何路標(biāo)關(guān)系法，將二維激光雷達(dá)掃描數(shù)據(jù)轉(zhuǎn)化為位姿不變直方圖，該方法在室內(nèi)外場景中的環(huán)境識別與匹配均達(dá)到出較好的效果。文獻(xiàn)[8]在室外大規(guī)模場景中，利用視覺和雷達(dá)傳感器將檢測到的物體作為自然路標(biāo)并創(chuàng)建稀疏的路標(biāo)地圖用于無人車的定位與導(dǎo)航文獻(xiàn)[9]利用激光雷達(dá)結(jié)合提出的改進(jìn)相交圓的人工路標(biāo)方法對托卡馬克轉(zhuǎn)運車進(jìn)行定位，并驗證了該定位方法具有較高的精度。文獻(xiàn)[10]針對不攜帶編碼器的大型移動平臺，提出利用激光雷達(dá)提取環(huán)境路標(biāo)外形特征，通過擴(kuò)展卡爾曼濾波算法對移動平臺進(jìn)行運動估計和位姿校正，實驗結(jié)果驗證了定位系統(tǒng)的精度和可靠性。

大規(guī)模場景中的SLAM 依賴高精度大量程的激光雷達(dá)，當(dāng)移動機器人配備低成本激光雷達(dá)時定位精度受到極大影響，因此使用環(huán)境路標(biāo)輔助機器人進(jìn)行實時定位能夠較好估計機器人位姿提高SLAM 精度。傳統(tǒng)的融合路標(biāo)信息進(jìn)行移動機器人定位仍然存在許多不足，針對路標(biāo)設(shè)置過于復(fù)雜以及有關(guān)室內(nèi)基于人工路標(biāo)SLAM 的研究尚少，提出利用差速移動機器人在人工路標(biāo)輔助下進(jìn)行有效的室內(nèi)大范圍場景激光雷達(dá)SLAM。通過實驗驗證，提出的方法具有較好的效果和實用性，有效提高移動機器人SLAM 的精度。

2 差速底盤運動模型

圖1 機器人運動模型示意圖Fig.1 Schematic Diagram of Robot Motion Model

實驗平臺采用差速驅(qū)動底盤，機器人左右電機分別由兩個驅(qū)動器控制，底盤運動模型，如圖1 所示。機器人移動的過程中依靠左右兩側(cè)編碼器進(jìn)獲取里程計數(shù)據(jù)，不考慮誤差的情況下通過實時采集編碼器數(shù)據(jù)并對其進(jìn)行解算能夠得到機器人每一時刻的速度和位姿，如圖1 所示。L為底盤軸距，R為運動時轉(zhuǎn)彎半徑，假設(shè)機器人（t-1）時刻位姿為（Xt-1，Yt-1，θt-1），采樣時間間隔為dt，采樣時刻左輪速度為Vl，右輪速度為Vr，線速度、角速度分別為Vt和Wt，采樣時間間隔內(nèi)的坐標(biāo)變換和偏航角分別為dx，dy，dθ，則有：

根據(jù)已知左右輪速度即可得到式（1）中機器人線速度、角速度以及轉(zhuǎn)彎半徑，同時依據(jù)采樣時間可得式（2）中的航向角變化值和式（3）中的坐標(biāo)位置，即確定機器人實時的估計位姿。

3 基于路標(biāo)的激光雷達(dá)觀測模型

二維激光雷達(dá)安裝在機器人底盤正上方，根據(jù)三角測距原理可以得到障礙物相對于激光雷達(dá)的距離及角度。若以機器人初始位置為坐標(biāo)系原點，以路標(biāo)為固定參考物則機器人移動過程中相對于參考物的位置始終已知，如圖2 所示。機器人0 時刻處于初始位置O1，下一時刻t處于位置O2，始終以右側(cè)路標(biāo)角點O為參考則可以得到機器人移動過程中相對初始位置的任意位置坐標(biāo)，即用激光雷達(dá)進(jìn)行機器人定位和軌跡的確定。

圖2 基于路標(biāo)的激光雷達(dá)觀測模型Fig.2 Lidar Observation Model Based on Landmarks

4 基于Rao-Blackwellized 的粒子濾波SLAM算法原理

Rao-Blackwellized 思想是將同時定位與建圖問題轉(zhuǎn)化為已知位姿求解地圖的形式，因此對聯(lián)合后驗概率函數(shù)因式分解。

式中：x—機器人軌跡；m—觀測的地圖；z—觀測信息；u—里程計信息，地圖的后驗概率p（m|x1:t，z1:t）將按照已知機器人位姿建圖進(jìn)行處理[2]，機器人軌跡的后驗概率p（x1:t，|z1:t，u1:t-1）可用粒子濾波方法計算，每個粒子都代表一條可能的機器人軌跡并關(guān)聯(lián)一張地圖，通過賦予粒子權(quán)重最終得到估計機器人運動軌跡。重要性重采樣（sampling importance resampling，SIR）濾波是常用的一種粒子濾波方法，通常包括四個部分：

（1）采樣粒子：根據(jù)提議分布進(jìn)行粒子采樣，新一代的粒子集得到，并且通常將里程計運動模型作為提議分布。（2）重要性權(quán)值：根據(jù)觀測值計算每個粒子的重要性權(quán)值，使得提議分布盡可能與真實目標(biāo)分布一致。（3）重采樣：根據(jù)權(quán)重結(jié)果重新分布粒子。（4）地圖估計：對于每個粒子，根據(jù)其采樣軌跡和觀測數(shù)據(jù)進(jìn)行地圖估計。里程計運動模型作為重要性概率密度函數(shù)計算簡單，但是由于其本身誤差將使估計分布偏離真實目標(biāo)分布，因此文獻(xiàn)[4]將里程計信息與觀測信息融合并入重要性概率密度函數(shù)，使粒子分布估計更接近真實分布?；谖墨I(xiàn)[4]研究成果，移動機器人在配備低成本雷達(dá)進(jìn)行大規(guī)模環(huán)境下同時定位與建圖時，融合人工路標(biāo)即可創(chuàng)建更精確的地圖。

5 融合人工路標(biāo)信息的移動機器人SLAM

大規(guī)模環(huán)境下進(jìn)行地圖構(gòu)建時里程計信息變的不可靠，因此機器人需要依賴高精度大范圍傳感器，而配備低成本雷達(dá)時，機器人可以利用環(huán)境中人工路標(biāo)輔助定位并修正里程計信息。由激光雷達(dá)觀測模型可以知道，機器人通過激光雷達(dá)確定某一路標(biāo)的相對位置，然后以當(dāng)前路標(biāo)為參考物進(jìn)行定位，如圖3 所示。機器人在初始位置O1確定1#路標(biāo)的外側(cè)邊AB，通過A和B的坐標(biāo)得到該邊中點坐標(biāo)，以該坐標(biāo)為參考進(jìn)行同時定位與建圖，移動到O2點時同時觀測到1#路標(biāo)和2#路標(biāo)，在確定AB中點的基礎(chǔ)上可以確定CD中點坐標(biāo)，進(jìn)而在移動過程中不斷獲取新的路標(biāo)進(jìn)行定位修正里程計信息，構(gòu)建與真實環(huán)境更接近的地圖。

圖3 融合路標(biāo)信息的輔助定位Fig.3 Auxiliary Positioning of Integrated Landmark Information

實驗室部分走廊真實場景，如圖4（a）所示。該段走廊平面圖，如圖4（b）所示。移動機器人配備低成本雷達(dá)利用RBPF-SLAM 算法構(gòu)建的二維柵格地圖，如圖4（c）所示。在走廊中設(shè)置人工路標(biāo)后移動機器人構(gòu)建的柵格地圖，如圖4（d）所示。可以發(fā)現(xiàn)未設(shè)置人工路標(biāo)時，柵格地圖的縱向長度明顯小于實際尺寸，而在人工路標(biāo)輔助移動機器人定位的情況下，創(chuàng)建的地圖尺寸基本接近實際尺寸。

圖4 設(shè)置人工路標(biāo)前后SLAM 效果對比Fig.4 Comparison of SLAM Performance before and after Setting up Artificial Landmarks

6 實驗結(jié)果與分析

6.1 實驗平臺

圖5 硬件平臺Fig.5 Hardware Platform

實驗平臺硬件，如圖5 所示。小車為兩輪差速驅(qū)動。實驗采用的激光雷達(dá)為RPlidar A2，接收驅(qū)動指令后可以對環(huán)境輪廓進(jìn)行360°掃描并返回檢測到的路標(biāo)數(shù)據(jù)，最大測量范圍為（6～8）m，頻率10Hz。根據(jù)激光雷達(dá)的工作特性，實驗中選用的人工路標(biāo)為（300*200*500）mm 長方體紙箱，無需考慮顏色等特性。箱體的平面及其不透明材質(zhì)便于2D 激光雷達(dá)快速檢測到直線特征，路標(biāo)按照一定間距在走廊一側(cè)進(jìn)行布置。

軟件部分基于ROS 平臺進(jìn)行開發(fā)，結(jié)構(gòu)如圖6 所示。移動機器人底盤控制指令通過ROS 平臺發(fā)布，控制模塊經(jīng)串口接收指令進(jìn)行PID 計算并驅(qū)動電機工作，同時接收里程計數(shù)據(jù)進(jìn)行解算發(fā)送至上位機。ROS 中融合里程計數(shù)據(jù)與激光雷達(dá)掃描數(shù)據(jù)進(jìn)行SLAM，同時發(fā)布路標(biāo)估計位置與機器人運動軌跡。

圖6 軟件結(jié)構(gòu)Fig.6 Software Structure

6.2 實驗結(jié)果

移動機器人在圖4（a）所示的走廊中行走一圈（176m）進(jìn)行SLAM 實驗，記錄移動機器人進(jìn)行同時定位與建圖時的估計運動軌跡，并最終生成完整室內(nèi)走廊地圖，在有無路標(biāo)的實驗中各選取一幅地圖，無路標(biāo)參考的環(huán)境地圖，如圖7 所示。設(shè)置路標(biāo)創(chuàng)建的地圖，如圖8 所示?？梢钥闯鰣D7 下半部分有傾斜趨勢，右側(cè)閉環(huán)部分出現(xiàn)明顯偏移，圖8 走廊整體垂直度較好，并且右側(cè)閉環(huán)效果理想。依上述實驗過程按設(shè)置人工路標(biāo)和無人工路標(biāo)兩種情況在路標(biāo)間隔分別為5m、7m 和10m 進(jìn)行SLAM 實驗，共得到六組軌跡長度，分別與實際走廊長度176m 進(jìn)行比較，軌跡估計精度提高6%左右，結(jié)果統(tǒng)計，如表1 所示。運動軌跡，如圖9 所示。在ROS 中獲取機器人發(fā)布的軌跡信息并將相關(guān)數(shù)據(jù)導(dǎo)入Matlab，將實驗結(jié)果進(jìn)行均值化處理，得到設(shè)置路標(biāo)前后的平均軌跡并進(jìn)行顯示?？梢钥闯龌谌斯ぢ窐?biāo)參考的移動機器人運動軌跡估計能較好的貼合真實走廊路徑，無路標(biāo)的軌跡則偏離較大，甚至無法實現(xiàn)閉環(huán)。

圖7 設(shè)置路標(biāo)前整體走廊地圖Fig.7 The Overall Corridor Map Before Setting the Landmarks

圖8 設(shè)置路標(biāo)后整體走廊地圖Fig.8 The Overall Corridor Map After Setting the Landmarks

表1 設(shè)置路標(biāo)前后及路標(biāo)間距改變時的SLAM 軌跡長度比較（單位：m）Tab.1 Comparison of the SLAM Track Length Before and after the Setting of Landmakes and When the Distance Have Changed Between Landmarks

圖9 移動機器人估計運動軌跡Fig.9 Mobile Robot Estimating Motion Track

7 結(jié)論

實驗證明，基于人工路標(biāo)的室內(nèi)移動機器人SLAM 方法能夠有效提高室內(nèi)地圖創(chuàng)建的精度，移動機器人在移動過程中根據(jù)路標(biāo)進(jìn)行準(zhǔn)確的位姿估計使得估計運動軌跡更接近真實運動軌跡，其中人工路標(biāo)間隔在（5～10）m 范圍時，移動機器人在室內(nèi)走廊行進(jìn)軌跡的精度平均提高6%左右，解決了室內(nèi)移動機器人配備低成本雷達(dá)時由于測距有限帶來的位姿估計偏差過大的問題。未來將在SLAM 的基礎(chǔ)上，研究基于已知地圖的移動機器人非結(jié)構(gòu)環(huán)境下自主導(dǎo)航與實時避障。