0 引言

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

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

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

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

2 基于粒子濾波的SLAM原理

2.1 擴(kuò)展卡爾曼濾波與粒子濾波

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

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

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

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

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

3 在Cortex-A53平臺(tái)上實(shí)現(xiàn)SLAM的方法

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

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

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

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

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

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

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

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

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

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

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

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

4 算法優(yōu)化

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

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

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

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

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

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

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

5 總結(jié)

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

參考文獻(xiàn)

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

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

[3] 高云峰，周倫，呂明睿，等.自主移動(dòng)機(jī)器人室內(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.