文獻(xiàn)標(biāo)識碼： A
DOI：10.16157/j.issn.0258-7998.170898
中文引用格式： 李洋，余先濤，賀勇，等. 基于Cortex-A53平臺的激光雷達(dá)SLAM實(shí)現(xiàn)[J].電子技術(shù)應(yīng)用，2018，44(2)：105-108.
英文引用格式： Li Yang，Yu Xiantao，He Yong，et al. Cortex-A53 platform based laser radar SLAM implementation[J]. Application of Electronic Technique，2018，44(2)：105-108.
0 引言
自主移動機(jī)器人[1-3]是近幾年的研究熱點(diǎn)，要實(shí)現(xiàn)機(jī)器人的自主移動，關(guān)鍵是要實(shí)現(xiàn)SLAM[4-7]（Simultaneous Localization and Mapping），也就是同時定位與地圖構(gòu)建。
在移動機(jī)器人上實(shí)現(xiàn)SLAM目前有兩種主流的方法，一是基于相機(jī)的SLAM；二是基于激光雷達(dá)[8]的SLAM。在SLAM的實(shí)現(xiàn)中最常用的硬件處理器平臺是Intel x86平臺，如TurtleBot移動機(jī)器人上的硬件處理器平臺就是一臺筆記本。在移動機(jī)器人上實(shí)現(xiàn)SLAM的硬件成本比較高，這是自主移動機(jī)器人難以進(jìn)入服務(wù)市場的主要原因之一。為了解決這個問題，本文在基于Cortex-A53的處理器平臺上配以激光雷達(dá)實(shí)現(xiàn)了室內(nèi)環(huán)境下的SLAM。
1 關(guān)于用低成本實(shí)現(xiàn)SLAM的思考
在移動機(jī)器人中用嵌入式處理器實(shí)現(xiàn)SLAM是一種趨勢，一是由于嵌入式處理器平臺對工業(yè)現(xiàn)場環(huán)境有較好的適應(yīng)性，二是成本相對較低。但是嵌入式處理器相對于SLAM問題來說，其性能不夠高，所以在這種相對低性能的處理器上實(shí)現(xiàn)SLAM要解決的主要問題就是要降低計(jì)算量，并且需要對算法進(jìn)行優(yōu)化，使得所實(shí)現(xiàn)的SLAM能滿足實(shí)際使用要求?；诩す饫走_(dá)的SLAM相對于基于相機(jī)的SLAM來說，它的算法相對簡單，計(jì)算量相對較小，對處理器的性能要求相對較低，所以才可以在性能較低的ARM平臺上實(shí)現(xiàn)，所以選擇用激光雷達(dá)作為SLAM的測量輸入。
2 基于粒子濾波的SLAM原理
2.1 擴(kuò)展卡爾曼濾波與粒子濾波
目前實(shí)現(xiàn)SLAM的兩種最主要的方法是擴(kuò)展卡爾曼濾波和粒子濾波?；跀U(kuò)展卡爾曼濾波(EKF)的SLAM算法[8]對非線性的運(yùn)動模型和觀測模型采用線性化來解決，其線性化的方法是在目標(biāo)點(diǎn)附近做泰勒展開并去除其高階部分，如果模型的非線性程度很大，就很容易使算法產(chǎn)生較大誤差，因此非線性就成了這種SLAM算法中的一個很嚴(yán)重的問題[9]，且EKF還是基于高斯假設(shè)的，運(yùn)動模型和觀測模型中的噪聲都要滿足高斯分布，這樣使得算法的使用受到限制。粒子濾波中用一定大小的采樣樣本的頻率分布來表示概率分布，當(dāng)樣本數(shù)量趨于無窮時，粒子集的概率密度函數(shù)可以近似于任意形式的概率密度分布，因此粒子濾波可以處理任意形式的狀態(tài)空間模型，而不局限于高斯噪聲模型。
在粒子濾波中，把從后驗(yàn)分布中獲得的采樣稱作粒子，粒子是對真實(shí)世界中待確定的狀態(tài)的一種假設(shè)。采樣后的粒子集St表示為：
2.2 基于粒子濾波的SLAM的實(shí)現(xiàn)步驟
SLAM是這樣的一個過程：當(dāng)機(jī)器人處于一個未知環(huán)境中且自身位置未知時，移動機(jī)器人在向前運(yùn)動過程中可以構(gòu)建環(huán)境地圖，并同時利用構(gòu)建的地圖來估計(jì)自身的位置。本文中的SLAM是基于粒子濾波算法來實(shí)現(xiàn)的。
在一個陌生的環(huán)境中，機(jī)器人位置的初始信度未知，所以認(rèn)為機(jī)器人可能位于當(dāng)前環(huán)境中的任何一個位置。從表示當(dāng)前環(huán)境的狀態(tài)空間的均勻分布中隨機(jī)采樣N個粒子來表示機(jī)器人的初始信度分布。本文中用于地圖構(gòu)建的SLAM算法描述如下：
3 在Cortex-A53平臺上實(shí)現(xiàn)SLAM的方法
整個系統(tǒng)的構(gòu)建過程如下：
(1)軟件平臺中使用的操作系統(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ò)配置，把處理器平臺上的WiFi模塊配置成Access Point(AP)模式，讓上位機(jī)可以接入AP與Cortex-A53處理器平臺通信，這樣就可以在個人計(jì)算機(jī)上通過遠(yuǎn)程登錄來操控機(jī)器人；
(4)把上位機(jī)配置成時間服務(wù)器，讓Cortex-A53平臺每次上電后能與上位機(jī)的時間保持同步；
(5)構(gòu)建地圖使用了粒子濾波方法。在未知環(huán)境中是通過粒子濾波來實(shí)現(xiàn)機(jī)器人定位的，并在定位的基礎(chǔ)上根據(jù)激光雷達(dá)的觀測數(shù)據(jù)來構(gòu)建地圖；
(6)在已知地圖的基礎(chǔ)上定位是通過粒子濾波實(shí)現(xiàn)的，機(jī)器人可以根據(jù)當(dāng)前的激光雷達(dá)掃描數(shù)據(jù)和里程計(jì)數(shù)據(jù)實(shí)現(xiàn)對自身的定位；
(7)有了環(huán)境地圖，并且機(jī)器人可以對自身定位，再通過路徑規(guī)劃就可以實(shí)現(xiàn)機(jī)器人導(dǎo)航。全局路徑規(guī)劃使用Dijkstra算法，在成本地圖上尋找從起點(diǎn)到終點(diǎn)的最低成本路徑。局部路徑規(guī)劃的算法思想為：在機(jī)器人可以運(yùn)動的速度和旋轉(zhuǎn)角度范圍內(nèi)對速度和角度進(jìn)行離散采樣；根據(jù)機(jī)器人的當(dāng)前狀態(tài)對每一個采樣速度進(jìn)行向前的運(yùn)動模擬，根據(jù)模擬過程中機(jī)器人與障礙物的接近程度、與目標(biāo)的接近程度以及與全局路徑的接近程度等來給每條模擬軌跡打分，并放棄不合理的路線；最后選擇得分最高的路線并給底座發(fā)送相應(yīng)的速度指令來讓機(jī)器人運(yùn)動。此種算法在局部路徑規(guī)劃過程中會在整個向前運(yùn)動模擬過程中持續(xù)對速度采樣；
(8)對程序進(jìn)行優(yōu)化，減小Cortex-A53處理器平臺的計(jì)算壓力。
系統(tǒng)構(gòu)建完成后就可以在上位機(jī)上實(shí)時觀察構(gòu)建的地圖，并可以在上位機(jī)上用軟件來給機(jī)器人指定目標(biāo)，實(shí)現(xiàn)機(jī)器人自主導(dǎo)航。
移動底座和激光雷達(dá)與Cortex-A53平臺都是通過串口來通信的。在基于Cortex-A53處理器的平臺上處理激光雷達(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)建。在對應(yīng)的人機(jī)接口上可以通過軟件在掃描地圖的基礎(chǔ)上給機(jī)器人指定目的地來進(jìn)行導(dǎo)航。
系統(tǒng)的架構(gòu)如圖1所示。
4 算法優(yōu)化
4.1 構(gòu)圖算法的優(yōu)化
Cortex-A53處理器平臺結(jié)合激光雷達(dá)實(shí)現(xiàn)地圖構(gòu)建。圖2為未優(yōu)化的地圖。
從前文可知，地圖構(gòu)建是通過匹配得分最高的最優(yōu)粒子結(jié)合激光雷達(dá)的掃描數(shù)據(jù)來實(shí)現(xiàn)的。構(gòu)圖算法的優(yōu)化方法為：通過給掃描匹配得分設(shè)定一個閾值minimumScore來提高所得地圖的精度。
這個閾值表示在粒子濾波過程中進(jìn)行掃描匹配時要求的最小匹配得分，這個值設(shè)置的越大，就表示對掃描匹配的要求越高。如果閾值設(shè)置過高，很容易導(dǎo)致匹配失敗，機(jī)器人會選擇使用里程計(jì)的數(shù)據(jù)，這樣就會使得對機(jī)器人姿態(tài)估計(jì)的準(zhǔn)確度降低，從而導(dǎo)致構(gòu)圖精度降低；如果閾值設(shè)置過低，掃描匹配很容易成功，會導(dǎo)致地圖中出現(xiàn)大量噪聲。設(shè)置合理的閾值可以加快機(jī)器人定位過程的收斂，得到精度更高的地圖。本次實(shí)驗(yàn)過程中測得的閾值范圍約為60~80，閾值范圍取決于激光雷達(dá)的測量范圍、角度分辨率、測量精度以及環(huán)境特征。圖3為優(yōu)化后的地圖。
4.2 路徑規(guī)劃算法的優(yōu)化
在系統(tǒng)構(gòu)建過程中發(fā)現(xiàn)在Cortex-A53處理器平臺上使用的局部路徑規(guī)劃算法無法實(shí)現(xiàn)局部路徑規(guī)劃，因?yàn)檫@種算法的計(jì)算量太大，在路徑規(guī)劃過程中處理器CPU占用率達(dá)到100%，但路徑規(guī)劃依然無法執(zhí)行，此平臺無法滿足該算法的要求。
使用改進(jìn)后的局部路徑規(guī)劃算法機(jī)器人可以正常執(zhí)行導(dǎo)航功能。改進(jìn)后的路徑規(guī)劃算法只對一步向前運(yùn)動模擬進(jìn)行速度采樣。這就使得速度采樣的樣本空間大大減小，因而改進(jìn)后的局部路徑規(guī)劃算法更高效，對計(jì)算能力的要求會相對較低。但是當(dāng)對機(jī)器人的加速度限制較小時，改進(jìn)后的算法性能可能會不如改進(jìn)前的算法性能好。不過在室內(nèi)環(huán)境下，機(jī)器人運(yùn)動的加速度會相對比較小，所以改進(jìn)后的算法適合在室內(nèi)條件下進(jìn)行局部路徑規(guī)劃。
5 總結(jié)
本文提出了在Cortex-A53嵌入式處理器平臺上實(shí)現(xiàn)激光雷達(dá)SLAM的方法，實(shí)現(xiàn)了地圖構(gòu)建和地圖優(yōu)化，以及移動機(jī)器人室內(nèi)環(huán)境下的導(dǎo)航、實(shí)時避障等功能。實(shí)踐證明，該方法效果較好，對處理器性能要求較低。后續(xù)的研究將集中在如何使用消費(fèi)級的激光雷達(dá)來實(shí)現(xiàn)SLAM上，從而更有效地降低移動機(jī)器人實(shí)現(xiàn)SLAM的成本。
參考文獻(xiàn)
[1] 張弦，蘇志遠(yuǎn).自主移動機(jī)器人定位技術(shù)研究綜述[J].機(jī)電產(chǎn)品開發(fā)與創(chuàng)新，2010(2)：3-5.
[2] 曾品善，妙全興，徐磊.自主移動機(jī)器人即時定位與地圖構(gòu)建方法研究[J].電子科技，2013(9)：177-178，181.
[3] 高云峰，周倫，呂明睿，等.自主移動機(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.