劉光偉，王 巍，祁賢雨，李明博

（北京航空航天大學(xué)機(jī)械工程及自動(dòng)化學(xué)院，北京 100083）

1 引言

同時(shí)定位與建圖（Simultaneous Localization and Mapping，SLAM）指的是移動(dòng)機(jī)器人在運(yùn)動(dòng)過(guò)程中根據(jù)實(shí)時(shí)獲取的傳感器數(shù)據(jù)進(jìn)行位姿估計(jì)，同時(shí)增量式的構(gòu)建環(huán)境地圖。作為移動(dòng)機(jī)器人實(shí)現(xiàn)自主導(dǎo)航的關(guān)鍵技術(shù)，SLAM自1986年在ICRA首次提出以來(lái)，經(jīng)過(guò)近幾十年的發(fā)展，已經(jīng)有了諸多適用于特定場(chǎng)景的解決方案；但這些SLAM算法都需人為控制機(jī)器人運(yùn)動(dòng)，為進(jìn)一步提升移動(dòng)機(jī)器人的智能水平和探索能力，即實(shí)現(xiàn)機(jī)器人在無(wú)人為干預(yù)的情況下自動(dòng)構(gòu)建環(huán)境地圖，文獻(xiàn)[1]提出了主動(dòng)SLAM。

主動(dòng)SLAM涉及到移動(dòng)機(jī)器人定位、建圖、路徑規(guī)劃等技術(shù)的結(jié)合，要求機(jī)器人在保證位姿估計(jì)精度的前提下，向能探測(cè)到盡可能多未知區(qū)域的方向運(yùn)動(dòng)。為解決該問(wèn)題，文獻(xiàn)[2]提出通過(guò)最優(yōu)控制法離散控制輸入，根據(jù)各輸入對(duì)機(jī)器人位姿估計(jì)精度和新增探索面積的影響，選擇出最優(yōu)的控制輸入進(jìn)行執(zhí)行，但實(shí)際使用中該方法可能導(dǎo)致機(jī)器人陷入局部最優(yōu)；文獻(xiàn)[3]使用線段特征表述環(huán)境，并結(jié)合線段特征特點(diǎn)制定目標(biāo)函數(shù)以評(píng)價(jià)系統(tǒng)的不確定度，并通過(guò)沿墻走來(lái)避免機(jī)器人陷入局部最優(yōu)；文獻(xiàn)[4]根據(jù)部分可觀測(cè)馬爾可夫決策過(guò)程模型從當(dāng)前地圖中選擇目標(biāo)點(diǎn)，但計(jì)算復(fù)雜度隨問(wèn)題規(guī)模呈指數(shù)級(jí)上升，一般只適用于求解小規(guī)模問(wèn)題。

對(duì)室內(nèi)環(huán)境中的定位與建圖問(wèn)題進(jìn)行了深入的研究、分析，并結(jié)合室內(nèi)場(chǎng)景特點(diǎn)，提出采用基于概率的方法進(jìn)行機(jī)器人的位姿估計(jì)和地圖構(gòu)建。首先通過(guò)機(jī)器人運(yùn)動(dòng)模型進(jìn)行位姿預(yù)測(cè)，然后根據(jù)激光雷達(dá)數(shù)據(jù)進(jìn)行位姿更新，從而實(shí)現(xiàn)高精度的定位和建圖；同時(shí)通過(guò)邊界探索不斷確定出下一步目標(biāo)點(diǎn)、規(guī)劃全局路徑，并通過(guò)動(dòng)態(tài)窗口法實(shí)現(xiàn)了機(jī)器人運(yùn)動(dòng)過(guò)程中的實(shí)時(shí)避障。

2 同時(shí)定位與建圖

移動(dòng)機(jī)器人同時(shí)定位與建圖指的是在已知機(jī)器人控制量（u1：t）和傳感器觀測(cè)量（z1：t）的情況下，估計(jì)環(huán)境地圖（m）及機(jī)器人位姿（x1：t），表示成數(shù)學(xué)形式即：p（x1：t，m|z1：t+1，u1：t）。

目前SLAM算法可分為概率和圖優(yōu)化兩種，但是由于室內(nèi)場(chǎng)景通常面積較小，且室內(nèi)移動(dòng)機(jī)器人自身的處理能力有限，所以選擇使用概率方法，并將SLAM問(wèn)題分解為對(duì)路徑的后驗(yàn)概率（位姿估計(jì)）和已知路徑情況下地圖的后驗(yàn)概率（地圖更新）兩部分[5]，如式（1）所示：

此外，由于柵格地圖具有易于構(gòu)建和保存、便于路徑規(guī)劃等優(yōu)點(diǎn)，所以采用柵格地圖來(lái)表示環(huán)境信息。

2.1 位姿估計(jì)

位姿估計(jì)是SLAM技術(shù)的核心，為提高機(jī)器人位姿估計(jì)精度，首先通過(guò)實(shí)時(shí)獲取的里程計(jì)數(shù)據(jù)進(jìn)行位姿預(yù)測(cè)，然后根據(jù)激光雷達(dá)數(shù)據(jù)進(jìn)行掃描匹配以實(shí)現(xiàn)位姿更新。

2.1.1 位姿預(yù)測(cè)

室內(nèi)環(huán)境中通常使用X=（x，y，θ）表示機(jī)器人的全局位姿，假設(shè)已知機(jī)器人t時(shí)刻位姿Xt、里程計(jì)觀測(cè)量ut以及t+1時(shí)刻里程計(jì)觀測(cè)量ut+1，則可根據(jù)里程計(jì)模型：Xt+1=Xt⊕（ut+1?ut）[6]對(duì)機(jī)器人的位姿Xt+1進(jìn)行預(yù)測(cè)。

2.1.2 位姿更新

輪子打滑、地面坑洼等因素都會(huì)導(dǎo)致里程計(jì)估計(jì)的位姿不準(zhǔn)確，長(zhǎng)時(shí)間運(yùn)行會(huì)產(chǎn)生較大的累計(jì)誤差。而通過(guò)使用激光雷達(dá)數(shù)據(jù)進(jìn)行掃描匹配，可以顯著提高位姿估計(jì)精度、減小累計(jì)誤差，所以通過(guò)掃描匹配進(jìn)行機(jī)器人位姿更新。位姿更新主要包括采集粒子和掃描匹配兩部分內(nèi)容。

（1）采集粒子

在里程計(jì)估計(jì)的位姿Xt+1周圍（（0.3×0.3）m的矩形區(qū)域）進(jìn)行采樣，為保證位姿估計(jì)的精度，在采集粒子時(shí)應(yīng)確保同一位置相鄰粒子間的角度差在一定范圍內(nèi)，這里的準(zhǔn)則是最遠(yuǎn)處激光點(diǎn)dmax在相鄰兩粒子間的偏移量不超過(guò)柵格分辨率r，由幾何關(guān)系可確定出相鄰粒子間的偏角δθ為：

根據(jù)柵格分辨率 r和窗口區(qū)域大小（Wx=Wy=0.3、Wθ=20°），可得采樣范圍為：

（2）掃描匹配

每個(gè)粒子都代表了機(jī)器人當(dāng)前時(shí)刻的一個(gè)可能位姿，按粒子位姿對(duì)當(dāng)前觀測(cè)的激光點(diǎn)進(jìn)行坐標(biāo)變換，得到其在柵格地圖坐標(biāo)系下的坐標(biāo)，然后根據(jù)已有地圖得到各激光點(diǎn)為占有柵格的概率Pxy。

累加各激光點(diǎn)所對(duì)應(yīng)的占有概率Pxy，可得當(dāng)前位姿下所有激光點(diǎn)的概率和；但柵格地圖的離散性會(huì)限制可以實(shí)現(xiàn)的精度，所以使用雙線性插值來(lái)計(jì)算Pxy以提高精度。依次遍歷所有粒子取得最大時(shí)的粒子即為機(jī)器人的后驗(yàn)位姿，也即機(jī)器人t+1時(shí)刻的最優(yōu)估計(jì)位姿Xt+1。

2.2 地圖更新

按照位姿Xt+1對(duì)當(dāng)前觀測(cè)到的激光點(diǎn)進(jìn)行坐標(biāo)轉(zhuǎn)換，得到其在柵格地圖坐標(biāo)系下的坐標(biāo)，從而實(shí)現(xiàn)地圖更新。更新原則為：激光點(diǎn)對(duì)應(yīng)的柵格為占有柵格，激光點(diǎn)與當(dāng)前位姿Xt+1之間的柵格為空白柵格，若這些柵格在之前的地圖中未探索過(guò)，則分別為其賦值0.6、0.4；若已經(jīng)探索過(guò)，則按式（5）進(jìn)行更新：

式中：P—該柵格前一時(shí)刻的概率；PN—更新后的概率；P0—占有柵格和空白柵格的初始概率，分別為0.6、0.4。

3 邊界探索

通過(guò)邊界提取不斷從當(dāng)前地圖中提取邊界[7]，并選擇出最優(yōu)邊界的中點(diǎn)作為機(jī)器人下一步的目標(biāo)點(diǎn)，同時(shí)規(guī)劃出全局路徑，從而不斷驅(qū)動(dòng)機(jī)器人向能探測(cè)到盡可能多未知區(qū)域的邊界運(yùn)動(dòng)，具體實(shí)現(xiàn)分為以下幾步：

（1）提取當(dāng)前地圖的邊界，即當(dāng)前柵格地圖中與未知柵格相鄰的空白柵格，兩種柵格；為防止碰撞，需剔除長(zhǎng)度小于機(jī)器人直徑的邊界，如圖1所示。

圖1 提取出的邊界及各柵格的距離變換值Fig.1 Extracted Boundaries and Distance Transform

（2）提取出所有剩余邊界的中點(diǎn)作為待定目標(biāo)點(diǎn)，如圖1中紅色柵格。并以各目標(biāo)點(diǎn)為中心，通過(guò)Flood-fill算法對(duì)已探索柵格進(jìn)行填充，為各柵格計(jì)算距離變換值CostOT（i）。

（3）為避免發(fā)生碰撞，在計(jì)算距離變換值的同時(shí)，為各柵格計(jì)算障礙物變換值CostOT（i）。障礙物變換值按式（6）進(jìn)行計(jì)算，各柵格CostOT（i），如圖2所示。

式中：Ω（i）—空白柵格距離障礙物柵格的距離；X—常數(shù)，根據(jù)機(jī)器人半徑確定。

（4）將各柵格距離變換值CostOT（i）和障礙物變換值CostOT（i）相加，可以得到各柵格的探索代價(jià)值，如圖3所示。然后從機(jī)器人當(dāng)前位置出發(fā)，沿探索代價(jià)值下降最快的方向進(jìn)行搜索，最先到達(dá)的目標(biāo)點(diǎn)即為下一步的目標(biāo)點(diǎn)，該過(guò)程所經(jīng)過(guò)的柵格即為機(jī)器人的全局路徑。

圖2 障礙物變換Fig.2 Obstacles Transform

圖3 探索代價(jià)值及全局路徑Fig.3 Exploration Transform and the Global Plan

（5）通過(guò)路徑追蹤算法驅(qū)動(dòng)機(jī)器人沿規(guī)劃出的全局路徑運(yùn)動(dòng)，直至無(wú)法繼續(xù)從已有地圖中提取出有效邊界。

4 實(shí)時(shí)避障

機(jī)器人的實(shí)際工作環(huán)境通常是動(dòng)態(tài)的，在沿全局路徑運(yùn)動(dòng)的過(guò)程中，為避免發(fā)生碰撞，需在全局路徑規(guī)劃的基礎(chǔ)上引入局部路徑規(guī)劃，以實(shí)現(xiàn)實(shí)時(shí)避障?？紤]到避障效果及實(shí)時(shí)性，選擇動(dòng)態(tài)窗口法（Dynamic Window Approach，DWA）[8]進(jìn)行局部路徑規(guī)劃。動(dòng)態(tài)窗口法主要包括以下幾個(gè)步驟：

4.1 速度空間的確定

機(jī)器人在t+1的速度vt+1可以由機(jī)器人當(dāng)前時(shí)刻的速度vt、角速度ωt確定，其范圍為：

式中：v˙a、v˙b—機(jī)器人的最大減速度、最大加速度；ω˙a、ω˙b—機(jī)器人的最大角減速度和最大角加速度。

4.2 推測(cè)下一周期的軌跡

計(jì)算得到機(jī)器人的速度空間后，可根據(jù)運(yùn)動(dòng)模型對(duì)下一周期的軌跡進(jìn)行預(yù)測(cè)，機(jī)器人在速度［v，ω］的作用下，下一周期內(nèi)運(yùn)動(dòng)軌跡為一段弧線，弧線方向與當(dāng)前速度方向相切，弧線半徑為v/ω，機(jī)器人在多組［v，ω］下得到的軌跡，如圖4所示。

圖4 動(dòng)態(tài)窗口法采樣得到的軌跡Fig.4 Trajectories Obtained by DWA

4.3 評(píng)價(jià)函數(shù)

根據(jù)第一節(jié)所述的地圖構(gòu)建方法，以機(jī)器人當(dāng)前位置為中心，建立一幅實(shí)時(shí)更新的（3×3）m的局部地圖，并從方位角（heading（v，ω））、距障礙物最短距離（dist（v，ω））、速度（velocity（v，ω））三方面對(duì)各條軌跡進(jìn)行評(píng)價(jià)。

4.3.1 方位角

方位角指的是機(jī)器人到達(dá)下一周期模擬軌跡末端時(shí)的朝向與目標(biāo)點(diǎn)之間的角度差，heading（v，ω）越小，代價(jià)值越大。

4.3.2 距障礙物最短距離

距障礙物最短距離指的是機(jī)器人在模擬軌跡上與局部地圖中最近的障礙物之間的距離，若該軌跡上沒(méi)有障礙物，則將其設(shè)定為常數(shù)；dist（v，ω）越大，代價(jià)值越大。

4.3.3 速度

速度指的是各模擬軌跡所對(duì)應(yīng)的速度，velocity（v，ω）越大，代價(jià)值越大。為了簡(jiǎn)化計(jì)算，算法假設(shè)機(jī)器人在模擬軌跡內(nèi)的速度大小不變。

4.3.4 歸一化

為綜合考慮方位角、距障礙物距離、速度大小對(duì)局部路徑規(guī)劃的影響，在計(jì)算出上述三項(xiàng)內(nèi)容后，需進(jìn)行歸一化處理，得到歸一化系數(shù)。歸一化計(jì)算公式為：

式中：n—采樣得到的總軌跡數(shù)；i—待評(píng)價(jià)的當(dāng)前軌跡。

將上述各項(xiàng)的計(jì)算結(jié)果帶入評(píng)價(jià)函數(shù)式（9），可得各軌跡的代價(jià)值；選擇代價(jià)值取得最大值時(shí)的速度作為機(jī)器人下一周期的速度，從而使得機(jī)器人及時(shí)避開障礙，以較快的速度向目標(biāo)點(diǎn)運(yùn)動(dòng)。

式中：α、β、γ—計(jì)算得到的歸一化系數(shù)。

5 實(shí)驗(yàn)

為驗(yàn)證算法，使用裝配有里程計(jì)和激光雷達(dá)的Kobuki移動(dòng)機(jī)器人平臺(tái)進(jìn)行了相關(guān)實(shí)驗(yàn)，其中激光雷達(dá)型號(hào)為RPLIDAR A2、掃描范圍360°、測(cè)量半徑8m，實(shí)驗(yàn)場(chǎng)地為兩室一廳結(jié)構(gòu)的家庭環(huán)境（12×12）m，如圖5所示。

圖5 實(shí)驗(yàn)場(chǎng)地及器材Fig.5 The Experiment Site and Equipment

移動(dòng)機(jī)器人上電后，在無(wú)人為干預(yù)情況下，使用所提算法自動(dòng)構(gòu)建的室內(nèi)環(huán)境地圖，該地圖為柵格地圖，柵格分辨率為5cm，如圖6所示。圖6中一條曲線為機(jī)器人自動(dòng)規(guī)劃的全局路徑，曲線端點(diǎn)即從當(dāng)前地圖中提取出的下一步目標(biāo)點(diǎn)，實(shí)驗(yàn)過(guò)程中機(jī)器人沿規(guī)劃的全局路徑運(yùn)動(dòng)。當(dāng)機(jī)器人前進(jìn)路徑上突然出現(xiàn)障礙物時(shí)（圖6中另一條線），機(jī)器人通過(guò)DWA算法規(guī)劃出局部路徑（圖6中一條線），及時(shí)調(diào)整速度方向，靈活的避開了障礙物。

圖6 主動(dòng)SLAM算法構(gòu)建的柵格地圖Fig.6 Grid Map Build by Active SLAM

為驗(yàn)證所提算法的精度，在同一實(shí)驗(yàn)場(chǎng)景中使用目前精度最高的開源激光SLAM算法Cartographer[9]（精度約5cm）進(jìn)行建圖實(shí)驗(yàn)，建圖方式為人為遙控，地圖構(gòu)建結(jié)果，如圖7所示。

圖7 Cartogerpher構(gòu)建的柵格地圖Fig.7 Grid Map Build by Cartogerpher

通過(guò)對(duì)比兩種方法所構(gòu)建的地圖，可以發(fā)現(xiàn)在小尺度的室內(nèi)家庭場(chǎng)景下（＜150m2），兩種方法的建圖精度非常接近。

為驗(yàn)證算法效率，在外界因素不變的情況下進(jìn)行了多次重復(fù)實(shí)驗(yàn)，并對(duì)每次實(shí)驗(yàn)的全程用時(shí)進(jìn)行統(tǒng)計(jì)。統(tǒng)計(jì)發(fā)現(xiàn)，人為遙控方式的平均用時(shí)約為4.3mins，所提主動(dòng)SLAM算法的平均用時(shí)約為6.8mins。

6 結(jié)論

通過(guò)對(duì)室內(nèi)動(dòng)態(tài)環(huán)境中同時(shí)定位和建圖問(wèn)題的研究及相關(guān)實(shí)驗(yàn)分析，可以得出如下結(jié)論：

（1）利用激光雷達(dá)高精度特性進(jìn)行掃描匹配，可以顯著提高機(jī)器人定位精度，從而構(gòu)建高精度的環(huán)境地圖。

（2）通過(guò)將激光SLAM與邊界探索相結(jié)合，可以在保證機(jī)器人位姿估計(jì)精度的前提下，實(shí)現(xiàn)機(jī)器人自動(dòng)、高效構(gòu)建全局地圖。

（3）局部路徑規(guī)劃可以使機(jī)器人靈活躲避環(huán)境中的隨機(jī)障礙，實(shí)現(xiàn)動(dòng)態(tài)環(huán)境中的同時(shí)定位和建圖。