0 引言

    自主移動機器人^[1-3]是近幾年的研究熱點，要實現(xiàn)機器人的自主移動，關(guān)鍵是要實現(xiàn)SLAM^[4-7]（Simultaneous Localization and Mapping），也就是同時定位與地圖構(gòu)建。

    在移動機器人上實現(xiàn)SLAM目前有兩種主流的方法，一是基于相機的SLAM；二是基于激光雷達^[8]的SLAM。在SLAM的實現(xiàn)中最常用的硬件處理器平臺是Intel x86平臺，如TurtleBot移動機器人上的硬件處理器平臺就是一臺筆記本。在移動機器人上實現(xiàn)SLAM的硬件成本比較高，這是自主移動機器人難以進入服務(wù)市場的主要原因之一。為了解決這個問題，本文在基于Cortex-A53的處理器平臺上配以激光雷達實現(xiàn)了室內(nèi)環(huán)境下的SLAM。

1 關(guān)于用低成本實現(xiàn)SLAM的思考

    在移動機器人中用嵌入式處理器實現(xiàn)SLAM是一種趨勢，一是由于嵌入式處理器平臺對工業(yè)現(xiàn)場環(huán)境有較好的適應(yīng)性，二是成本相對較低。但是嵌入式處理器相對于SLAM問題來說，其性能不夠高，所以在這種相對低性能的處理器上實現(xiàn)SLAM要解決的主要問題就是要降低計算量，并且需要對算法進行優(yōu)化，使得所實現(xiàn)的SLAM能滿足實際使用要求。基于激光雷達的SLAM相對于基于相機的SLAM來說，它的算法相對簡單，計算量相對較小，對處理器的性能要求相對較低，所以才可以在性能較低的ARM平臺上實現(xiàn)，所以選擇用激光雷達作為SLAM的測量輸入。

2 基于粒子濾波的SLAM原理

2.1 擴展卡爾曼濾波與粒子濾波

    目前實現(xiàn)SLAM的兩種最主要的方法是擴展卡爾曼濾波和粒子濾波。基于擴展卡爾曼濾波(EKF)的SLAM算法^[8]對非線性的運動模型和觀測模型采用線性化來解決，其線性化的方法是在目標點附近做泰勒展開并去除其高階部分，如果模型的非線性程度很大，就很容易使算法產(chǎn)生較大誤差，因此非線性就成了這種SLAM算法中的一個很嚴重的問題^[9]，且EKF還是基于高斯假設(shè)的，運動模型和觀測模型中的噪聲都要滿足高斯分布，這樣使得算法的使用受到限制。粒子濾波中用一定大小的采樣樣本的頻率分布來表示概率分布，當(dāng)樣本數(shù)量趨于無窮時，粒子集的概率密度函數(shù)可以近似于任意形式的概率密度分布，因此粒子濾波可以處理任意形式的狀態(tài)空間模型，而不局限于高斯噪聲模型。

    在粒子濾波中，把從后驗分布中獲得的采樣稱作粒子，粒子是對真實世界中待確定的狀態(tài)的一種假設(shè)。采樣后的粒子集S_t表示為：

2.2 基于粒子濾波的SLAM的實現(xiàn)步驟

    SLAM是這樣的一個過程：當(dāng)機器人處于一個未知環(huán)境中且自身位置未知時，移動機器人在向前運動過程中可以構(gòu)建環(huán)境地圖，并同時利用構(gòu)建的地圖來估計自身的位置。本文中的SLAM是基于粒子濾波算法來實現(xiàn)的。

    在一個陌生的環(huán)境中，機器人位置的初始信度未知，所以認為機器人可能位于當(dāng)前環(huán)境中的任何一個位置。從表示當(dāng)前環(huán)境的狀態(tài)空間的均勻分布中隨機采樣N個粒子來表示機器人的初始信度分布。本文中用于地圖構(gòu)建的SLAM算法描述如下：

3 在Cortex-A53平臺上實現(xiàn)SLAM的方法

    整個系統(tǒng)的構(gòu)建過程如下：

    (1)軟件平臺中使用的操作系統(tǒng)是支持ARM架構(gòu)處理器的Ubuntu mate16.04 LTS系統(tǒng)；

    (2)使用的ROS(Robot Operating System)機器人操作系統(tǒng)是kinetic版本；

    (3)進行網(wǎng)絡(luò)配置，把處理器平臺上的WiFi模塊配置成Access Point(AP)模式，讓上位機可以接入AP與Cortex-A53處理器平臺通信，這樣就可以在個人計算機上通過遠程登錄來操控機器人；

    (4)把上位機配置成時間服務(wù)器，讓Cortex-A53平臺每次上電后能與上位機的時間保持同步；

    (5)構(gòu)建地圖使用了粒子濾波方法。在未知環(huán)境中是通過粒子濾波來實現(xiàn)機器人定位的，并在定位的基礎(chǔ)上根據(jù)激光雷達的觀測數(shù)據(jù)來構(gòu)建地圖；

    (6)在已知地圖的基礎(chǔ)上定位是通過粒子濾波實現(xiàn)的，機器人可以根據(jù)當(dāng)前的激光雷達掃描數(shù)據(jù)和里程計數(shù)據(jù)實現(xiàn)對自身的定位；

    (7)有了環(huán)境地圖，并且機器人可以對自身定位，再通過路徑規(guī)劃就可以實現(xiàn)機器人導(dǎo)航。全局路徑規(guī)劃使用Dijkstra算法，在成本地圖上尋找從起點到終點的最低成本路徑。局部路徑規(guī)劃的算法思想為：在機器人可以運動的速度和旋轉(zhuǎn)角度范圍內(nèi)對速度和角度進行離散采樣；根據(jù)機器人的當(dāng)前狀態(tài)對每一個采樣速度進行向前的運動模擬，根據(jù)模擬過程中機器人與障礙物的接近程度、與目標的接近程度以及與全局路徑的接近程度等來給每條模擬軌跡打分，并放棄不合理的路線；最后選擇得分最高的路線并給底座發(fā)送相應(yīng)的速度指令來讓機器人運動。此種算法在局部路徑規(guī)劃過程中會在整個向前運動模擬過程中持續(xù)對速度采樣；

    (8)對程序進行優(yōu)化，減小Cortex-A53處理器平臺的計算壓力。

    系統(tǒng)構(gòu)建完成后就可以在上位機上實時觀察構(gòu)建的地圖，并可以在上位機上用軟件來給機器人指定目標，實現(xiàn)機器人自主導(dǎo)航。

    移動底座和激光雷達與Cortex-A53平臺都是通過串口來通信的。在基于Cortex-A53處理器的平臺上處理激光雷達的掃描數(shù)據(jù)以及底座中采集的里程計數(shù)據(jù)，結(jié)合激光雷達的數(shù)據(jù)和里程計數(shù)據(jù)實現(xiàn)機器人的定位，并在定位的基礎(chǔ)上利用雷達掃描數(shù)據(jù)實現(xiàn)二維地圖構(gòu)建。在對應(yīng)的人機接口上可以通過軟件在掃描地圖的基礎(chǔ)上給機器人指定目的地來進行導(dǎo)航。

    系統(tǒng)的架構(gòu)如圖1所示。

4 算法優(yōu)化

4.1 構(gòu)圖算法的優(yōu)化

    Cortex-A53處理器平臺結(jié)合激光雷達實現(xiàn)地圖構(gòu)建。圖2為未優(yōu)化的地圖。

    從前文可知，地圖構(gòu)建是通過匹配得分最高的最優(yōu)粒子結(jié)合激光雷達的掃描數(shù)據(jù)來實現(xiàn)的。構(gòu)圖算法的優(yōu)化方法為：通過給掃描匹配得分設(shè)定一個閾值minimumScore來提高所得地圖的精度。

    這個閾值表示在粒子濾波過程中進行掃描匹配時要求的最小匹配得分，這個值設(shè)置的越大，就表示對掃描匹配的要求越高。如果閾值設(shè)置過高，很容易導(dǎo)致匹配失敗，機器人會選擇使用里程計的數(shù)據(jù)，這樣就會使得對機器人姿態(tài)估計的準確度降低，從而導(dǎo)致構(gòu)圖精度降低；如果閾值設(shè)置過低，掃描匹配很容易成功，會導(dǎo)致地圖中出現(xiàn)大量噪聲。設(shè)置合理的閾值可以加快機器人定位過程的收斂，得到精度更高的地圖。本次實驗過程中測得的閾值范圍約為60~80，閾值范圍取決于激光雷達的測量范圍、角度分辨率、測量精度以及環(huán)境特征。圖3為優(yōu)化后的地圖。

4.2 路徑規(guī)劃算法的優(yōu)化

    在系統(tǒng)構(gòu)建過程中發(fā)現(xiàn)在Cortex-A53處理器平臺上使用的局部路徑規(guī)劃算法無法實現(xiàn)局部路徑規(guī)劃，因為這種算法的計算量太大，在路徑規(guī)劃過程中處理器CPU占用率達到100%，但路徑規(guī)劃依然無法執(zhí)行，此平臺無法滿足該算法的要求。

    使用改進后的局部路徑規(guī)劃算法機器人可以正常執(zhí)行導(dǎo)航功能。改進后的路徑規(guī)劃算法只對一步向前運動模擬進行速度采樣。這就使得速度采樣的樣本空間大大減小，因而改進后的局部路徑規(guī)劃算法更高效，對計算能力的要求會相對較低。但是當(dāng)對機器人的加速度限制較小時，改進后的算法性能可能會不如改進前的算法性能好。不過在室內(nèi)環(huán)境下，機器人運動的加速度會相對比較小，所以改進后的算法適合在室內(nèi)條件下進行局部路徑規(guī)劃。

5 總結(jié)

    本文提出了在Cortex-A53嵌入式處理器平臺上實現(xiàn)激光雷達SLAM的方法，實現(xiàn)了地圖構(gòu)建和地圖優(yōu)化，以及移動機器人室內(nèi)環(huán)境下的導(dǎo)航、實時避障等功能。實踐證明，該方法效果較好，對處理器性能要求較低。后續(xù)的研究將集中在如何使用消費級的激光雷達來實現(xiàn)SLAM上，從而更有效地降低移動機器人實現(xiàn)SLAM的成本。

參考文獻

[1] 張弦，蘇志遠.自主移動機器人定位技術(shù)研究綜述[J].機電產(chǎn)品開發(fā)與創(chuàng)新，2010(2)：3-5.

[2] 曾品善，妙全興，徐磊.自主移動機器人即時定位與地圖構(gòu)建方法研究[J].電子科技，2013(9)：177-178，181.

[3] 高云峰，周倫，呂明睿，等.自主移動機器人室內(nèi)定位方法研究綜述[J].傳感器與微系統(tǒng)，2013(12)：1-5，9.

[4] DISSANAYAKE G，DURRANT-WHYTE H，BAILEY T.A computationally efficient solution to the simultaneous localization and map building(SLAM) problem[C].Proceedings of the IEEE International Conference on Robotics and Automation(ICRA)，San Francisco，CA，USA：IEEE，2000：1009-1014.

[5] MONTELERLO M，THRUN S，KOLLER D，et al.FastSLAM：A factored solution to the simultaneous localization and mapping problem[C].In Proceedings of the National Conference on Artificial Intelligence(AAAI)，Edmonton，Canada：AAAI Press，2002：593-598.

[6] ELIAZAR A，PARR R.DP-SLAM：Fast，robust simultaneous localization and mapping without predetermined landmarks[C].In Proc.of the Int.Conf.on Artificial Intelligence(IJCAI)，Acapulco，Mexico：IJCAI Press，2003，3：1135-1142.

[7] DURRANT-WHYTE H，BAILEY T.Simultaneous localization and mapping：part I[J].IEEE Robotics & Automation Magazine，2006，13(2)：99-110.

[8] WILLIAMS S B，DISSANAYAKE G，DURRANT-WHYTE H.Field deployment of the simultaneous localization and mapping algorithm[C].Proceedings of the 2002 IFAC World Congress on Automatic Control.Barcelona，Spain：IFAC Press，2002：861-866.

[9] JULIER S J，UHLMANN J K.A counter example to the theory of simultaneous localization and map building[C].Proceedings of the IEEE International Conference on Robotics and Automation(ICRA)：IEEE，2001：4238-4243.