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