段航琪 張峰源 杜嘯宇 劉源 臧博

摘 要：機(jī)器人同時定位與建圖技術(shù)在移動機(jī)器人、無人駕駛中占有核心地位。按照傳感器不同大體分為激光SLAM和視覺SLAM兩種，按照算法的不同又可以分為基于濾波器的SLAM算法和基于圖優(yōu)化的SLAM算法。現(xiàn)有的SLAM開源算法有g(shù)mapping、cartographyer、ORB-SLAM等。在二位柵格地圖上，主要使用A*算法和DWA算法完成導(dǎo)航。

關(guān)鍵詞：機(jī)器人SLAM；自主導(dǎo)航；A*算法；DWA算法

一、SLAM的起源

移動機(jī)器人在進(jìn)入一個陌生環(huán)境時，要解決“我在哪”以及“我周圍的環(huán)境是怎樣的”這兩個問題，才可以更好地進(jìn)行移動和工作。而在位姿估計(jì)的同時建立環(huán)境地圖，就是機(jī)器人SLAM技術(shù)。

二、SLAM的原理

SLAM主要解決位姿估計(jì)和環(huán)境地圖建立這兩個問題，這兩個部分是相互依存的。

1.定位

機(jī)器人估計(jì)自身位姿有兩種方案。一是靠里程計(jì)信息進(jìn)行估計(jì)，里程計(jì)信息來源主要有輪式編碼器、慣性測量單元（imu）、攝像頭（視覺里程計(jì)）這幾類傳感器；另一種是靠觀測路標(biāo)點(diǎn)進(jìn)行位姿估計(jì)，通過激光雷達(dá)掃描得到的深度信息或者攝像頭拍照，對機(jī)器人當(dāng)前位置周圍的特征點(diǎn)進(jìn)行提取，然后將提取到的特征點(diǎn)與之前的路標(biāo)進(jìn)行匹配，根據(jù)機(jī)器人對路標(biāo)點(diǎn)的觀測量可以得出機(jī)器人當(dāng)前相對于路標(biāo)點(diǎn)的位姿。理論上，這兩種方法在沒有測量誤差的情況下都可以單獨(dú)估計(jì)機(jī)器人位姿。但由于數(shù)據(jù)測量噪聲的存在，單獨(dú)使用其中一種無法得到機(jī)器人準(zhǔn)確的位姿，因此需要將兩種數(shù)據(jù)進(jìn)行融合，得到效果更好的位姿估計(jì)量。融合的方法分為濾波器和圖優(yōu)化兩種，經(jīng)典的SLAM算法是基于濾波器的，而近年來的研究熱點(diǎn)為基于圖優(yōu)化的視覺SLAM。

2.建圖

機(jī)器人在定位過程中同時建立環(huán)境地圖。最基本的，機(jī)器人在定位過程中，將自身位置周圍的landmark（地標(biāo)）位置記錄下來，便構(gòu)成一張記錄地標(biāo)位置的地圖。機(jī)器人可以通過觀測路標(biāo)并與路標(biāo)地圖比較，從而實(shí)現(xiàn)兩種定位方法中的觀測路標(biāo)法。

三、SLAM在機(jī)器人上的應(yīng)用——自主導(dǎo)航

1.柵格地圖

SLAM在機(jī)器人上的應(yīng)用主要為機(jī)器人自主導(dǎo)航。機(jī)器人通過環(huán)境地圖中的路標(biāo)信息與傳感器觀測值來估計(jì)自身位置，并根據(jù)導(dǎo)航目標(biāo)點(diǎn)和地圖來進(jìn)行路徑規(guī)劃（path planning）和軌跡規(guī)劃（trajectory planning）。而前面介紹的SLAM原理中的建圖環(huán)節(jié)的地圖，只包含路標(biāo)信息，而不包括路標(biāo)之外的信息，這種地圖稱為“稀疏地圖”。顯然，稀疏地圖并不能體現(xiàn)各路標(biāo)點(diǎn)之間的道路狀況，如果機(jī)器人使用稀疏地圖進(jìn)行導(dǎo)航，比如直奔某個路標(biāo)點(diǎn)，那么極有可能在中途碰到障礙物。因此，導(dǎo)航需要的是包含路標(biāo)信息以及路標(biāo)之間道路狀況的地圖，我們稱這種包含信息更多的地圖為“稠密地圖”。稠密地圖中應(yīng)用較廣的一種是“占據(jù)柵格地圖”， 機(jī)器人對環(huán)境地圖的描述的方式最常見的為柵格地圖（Grid map）或者稱為Occupancy Map，如下圖所示。柵格地圖就是把環(huán)境劃分成一系列柵格，其中每一柵格給定一個可能值，表示該柵格被占據(jù)的概率。

下面以柵格地圖來介紹機(jī)器人導(dǎo)航算法。導(dǎo)航算法分為全局路徑規(guī)劃與局部路徑規(guī)劃兩部分。

2.全局路徑規(guī)劃——A*導(dǎo)航算法

A*導(dǎo)航算法，俗稱A星算法。這是一種在圖形平面上，有多個節(jié)點(diǎn)的路徑，求出最低通過成本的算法。常用于游戲中的NPC的移動計(jì)算，或網(wǎng)絡(luò)游戲的BOT的移動計(jì)算上。如下圖，考慮具有許多障礙的方格，我們給出起始單元和目標(biāo)單元。我們希望盡可能快地從起始單元到達(dá)目標(biāo)單元。該算法綜合了最良優(yōu)先搜索和Dijkstra算法的優(yōu)點(diǎn)：在進(jìn)行啟發(fā)式搜索提高算法效率的同時，可以保證找到一條最優(yōu)路徑（基于評估函數(shù)）。

在此算法中，如果以 表示從起點(diǎn)到任意頂點(diǎn) 的實(shí)際距離， 表示任意頂點(diǎn) 到目標(biāo)頂點(diǎn)的估算距離（根據(jù)所采用的評估函數(shù)的不同而變化），那么A*算法的估算函數(shù)為：

這個公式遵循以下特性：

如果 為0，即只計(jì)算任意頂點(diǎn) 到目標(biāo)的評估函數(shù) ，而不計(jì)算起點(diǎn)到頂點(diǎn) 的距離，則算法轉(zhuǎn)化為使用貪心策略的最良優(yōu)先搜索，速度最快，但可能得不出最優(yōu)解；

如果 不大于頂點(diǎn) 到目標(biāo)頂點(diǎn)的實(shí)際距離，則一定可以求出最優(yōu)解，而且 越小，需要計(jì)算的節(jié)點(diǎn)越多，算法效率越低，常見的評估函數(shù)有——?dú)W幾里得距離、曼哈頓距離、切比雪夫距離；

如果 為0，即只需求出起點(diǎn)到任意頂點(diǎn) 的最短路徑 ，而不計(jì)算任何評估函數(shù) ，則轉(zhuǎn)化為單源最短路徑問題，即Dijkstra算法，此時需要計(jì)算最多的頂點(diǎn)；

3.局部導(dǎo)航算法——DWA

機(jī)器人局部路徑規(guī)劃常用DWA算法，動態(tài)窗口法主要是在速度（v，w）空間中采樣多組速度，并模擬機(jī)器人在這些速度下一定時間（sim_period）內(nèi)的軌跡。在得到多組軌跡后，對于這些軌跡進(jìn)行評價，選取最優(yōu)軌跡所對應(yīng)的速度來驅(qū)動機(jī)器人運(yùn)動。該算法突出點(diǎn)在與動態(tài)窗口這個名詞，它的含義是依據(jù)移動機(jī)器人的加減速性能限定速度采樣空間在一個可行的動態(tài)范圍內(nèi)。該算法主要包含速度采樣和評價函數(shù)兩部分。

（1）速度采樣

建立機(jī)器人的軌跡運(yùn)動模型后，根據(jù)速度就可以推算出軌跡。因此只需采樣很多速度，推算軌跡，然后評價這些軌跡優(yōu)劣。速度采樣的過程為：在速度（v，w）的二維空間中，存在無窮多組速度。但是根據(jù)機(jī)器人本身的限制和環(huán)境限制可以將采樣速度控制在一定范圍內(nèi)。

（2）評價函數(shù)

在采樣的速度組中，有若干組軌跡是可行的，因此采用評價函數(shù)的方式為每條軌跡進(jìn)行評價。

參考文獻(xiàn)：

作者簡介：

段航琪（1999.04.23-） 男，漢族，河南省開封市，本科生，研究方向：自動化

張峰源（1998.10.26-） 男，漢族，河南省周口市，本科生，研究方向：機(jī)械工程

杜嘯宇（1998.10.8-）男 漢族 河南省信陽市淮濱縣 本科 研究方向：汽車工程

劉源 （1999.4.2-）男 ，漢族，河南省鄭州市，本科生，研究方向：自動化

臧博（1998.6.18-）男，漢族，山東省泰安市，本科生，研究方向：機(jī)械工程