一種改進的AMCL機器人定位方法

王 寧，王 堅，李麗華

一種改進的AMCL機器人定位方法

王 寧1，王 堅1，李麗華2

（1. 北京建筑大學 測繪與城市空間信息學院，北京 102616；2. 中國地質大學 土地科學技術學院，北京 100083）

針對移動機器人技術中的蒙特卡羅定位算法存在計算量大、實時處理能力差、粒子退化等問題，提出一種基于AMCL算法的自適應定位模型：改進ROS中的move_base節點的路徑規劃功能，并加載學院樓CAD地圖實現路徑規劃；基于ROS中slam_gmapping節點在未知環境下創建OGM地圖。仿真實驗結果表明：AMCL定位模型能夠減小定位誤差，提高路徑規劃的準確性和實時性；采樣部分利用重采樣與KLD采樣交替進行的方法，根據粒子在狀態空間的分布情況，能夠實時在線調整粒子數，有效減少算法計算量。

自適應蒙特卡羅定位模型；機器人操作系統；靜態地圖；移動機器人；路徑規劃

0 引言

移動機器人在各種復雜環境下的自主操作中，自主定位是其最基本的任務之一。移動機器人的自主定位是基于上一時刻的姿態估計，利用已有的環境地圖信息和傳感器檢測感受到的被測量信息，再經過信息的傳輸、處理、顯示、存儲、控制等，最后利用當前時刻的姿態估計來對移動機器人在環境地圖中進行定位[1-3]。

自適應蒙特卡羅定位（adaptive Monte Carlo localization,AMCL）算法是一種基于粒子濾波的適用于2維環境下移動機器人的魯棒的概率統計定位方法。它所有可能的姿態假設及其概率分布通過使用一系列加權粒子來表示。機器人的初始位姿及其路徑規劃過程中的位姿通過采樣一定數量的粒子來跟蹤。采樣的方法是重采樣（在首次采樣之后用于確定路徑規劃過程中機器人新位姿時對當前粒子集進行采樣）和Kullback-Leibler距離即K-L距離（Kullback-Leibler distance, KLD）采樣交替進行。機器人的運動模型、傳感器模型和實時數據通過迭代算法不斷連續更新粒子及其權重，直至趨于真實概率分布。AMCL可以有效地處理非線性和非高斯系統。因此，它廣泛地應用在移動機器人導航定位等領域[4-10]。

本文研究和改進AMCL定位算法：采用自適應定位模型，定位原理依賴重采樣和KLD采樣交替進行的原則；并在機器人操作系統（robot operating system, ROS）上加載學院樓計算機輔助設計（computer aided design, CAD）地圖，通過全局規劃和局部規劃對移動機器人定位中的重要狀態進行跟蹤；然后在不同的條件下調整和控制AMCL參數和重采樣規則，利用slam_gmapping節點在未知環境下創建覆蓋柵格地圖（occupancy grid map, OGM）。

2 AMCL自適應定位

2.1 MCL算法

式中表示樣本估計概率。

產生新的粒子集為

并且使其對所有粒子都滿足[1-2]

自適應樣本大小對基于線性傳感器的模型是極其有益的，因為應用這些模型將會需要大量的幾何計算。

2.2 KLD采樣

根據（8）式，KLD定義為

式中：表示K-L距離。

在重采樣中，復制權重較大的粒子，權重較小的粒子予以忽略。利用KLD采樣確定下一次重采樣在粒子復制過程中所需的粒子數，實現了粒子數的在線調整，減少了算法計算量。

2.3 重采樣和KLD采樣交替進行的AMCL

基于重采樣和KLD采樣交替進行的AMCL算法流程[1,9,14]為：

2）將當前粒子集中粒子的權重作歸一化處理。

3）對更新處理后的粒子集進行重采樣和KLD交替采樣，利用式（14）對重采樣的粒子數進行更新。

式中E[]為數學期望。

該算法的濾波效率與傳統的自適應濾波算法相比得到了提升，它的濾波額外計算復雜度得到了降低。這種改進在粒子數目龐大的情況下，極大地提高了系統的性能。

3 ROS框架下的模型驗證

ROS是基于機器人的開放源碼的元操作系統。它可以提供許多類似于傳統操作系統的功能，如底層設備控制、硬件抽象、進程間消息傳遞和功能包管理、公共功能實現等[9]。此外，ROS采用分布式網絡系統，不同節點之間的通信利用主題、服務或參數服務器來實現。比如：在移動機器人導航定位中，tf節點控制坐標轉換；激光雷達和里程計采集環境數據，并通過laser scan節點接收傳輸數據；AMCL算法接收傳感器數據并對其進行處理，將處理結果通過amcl節點顯示出來。某個節點訂閱其他節點發布的不同主題，自身發布的主題也被其他節點所訂閱；消息是主題的載體，彼此的溝通通過消息來完成。這種系統結構確保實時更新定位數據和同步傳輸。節點是ROS計算圖級中主要的計算執行進程。它們之間相互獨立，將代碼和功能解耦，提高了系統容錯率和可維護性。ROS主要在Linux系統下開發，在Ubuntu下支持最為完全；整合多個主流函數庫，擁有多傳感器驅動程序[15-16]。

3.1 仿真研究

機器人3D模型或部分結構模型主要用于仿真機器人或者簡化開發者的日常工作，在ROS中模型文件有URDF文件和XACRO文件。仿真環境由rviz或gazebo節點控制。

為了在rviz節點中查看仿真機器人3維模型，需要編寫啟動文件（display.launch），ROS中的啟動文件可以同時啟動節點管理器（roscore）和多個節點，啟動命令為roslaunch package-name launch- file-name。rviz仿真結果如圖1所示。

Gazebo是ROS系統下模擬室內室外復雜3維環境，并可以在3維場景中添加多個機器人、不同傳感器和常見物體來模擬真實世界對物體的物理響應和實際反饋的仿真環境。在Gazebo中進行機器人仿真，就需要添加物理屬性和碰撞屬性，否則Gazebo無法使用這些模型。本文使用柳樹車庫（Willow Garage）辦公室模型地圖。Gazebo允許使用左邊菜單插入和放置物體，本文添加了書架、桌子、罐子，來監測傳感器對它們如何反應。

圖1 rviz仿真結果

最后創建一個新的啟動文件（gazebo.launch）來同時加載地圖和機器人。Gazebo仿真結果如圖2所示。

圖2 Gazebo仿真結果

3.2 基于AMCL算法的移動機器人定位

自適應蒙特卡羅定位[9-10]是一種用于2維環境下移動機器人概率統計定位方法。這種算法在ROS框架下的具體實現主要是通過在已知地圖上使用粒子濾波算法跟蹤機器人的位姿。

自適應蒙特卡羅定位節點主要使用激光掃描和激光雷達地圖。本文以北京建筑大學測繪與城市空間信息學院二層地圖為例進行模擬。圖3所示是加載學院樓CAD地圖的情況。以此為基礎，模擬激光掃描和激光雷達數據，采用本文算法，通過傳遞消息完成位姿估計的計算。

圖3 學院樓地圖

首先通過tf坐標變換（如圖4所示）節點來配置機器人。利用tf坐標變換功能包對機器人進行坐標變換，并定義了不同坐標系之間的偏移量。從圖上可以看出，本文的移動機器人使用base_link基座坐標系和hokuyo_link激光傳感器2個坐標系。為了實現機器人的避障功能，通過訂閱主題把激光傳感器的數據發送給機器人。之后利用基座坐標系來控制機器人的移動方向和旋轉姿態，因此有必要構造base_link和hokuyo_link之間的變換矩陣。為此先定義base_link基本連接坐標系，在visual字段中定義它的幾何形狀（本文定義為立方體）、材質（顏色為白色，無紋理）和原點；然后創建廣播器（tf_broadcaster），主要用來發布base_link和hokuyo_link的轉換四元數和激光傳感器相對于base_link坐標系原點的位置；接著創建偵聽器（tf_listener），主要用來監聽激光傳感器采集到的移動機器人上各點的坐標偏移量或者關節之間的相對位置，同時將偏移量或者位置數據發布到主題robot_tf_listener上。激光傳感器采集到的環境數據和機器人偏移數據如圖5所示。廣播器通過訂閱這個主題來獲取機器人的偏移數據，最后經過四元轉換把機器人偏移數據發布到主題robot_tf_ broadcaster上。

圖4 tf坐標變換

圖5 傳感器數據

然后針對ROS框架提供的各個初始化參數，完成自適應蒙特卡羅定位算法粒子濾波器的初始化。如果沒有設定初始化位姿，自適應蒙特卡羅定位算法會假定機器人從坐標系原點運行，如此會增加計算的復雜度。文中利用rviz界面中2D Pose Estimate按鈕來設定初始化位姿。機器人位姿利用采樣所得的粒子分布來跟蹤，采樣結果如圖6所示。

最后ROS通過nav_msgs/Odometry消息發布關于2個坐標系之間的位移變換信息。該消息提供機器人的位姿信息和速度信息。ROS通過sensor_msgs/LaserScan或sensor_msgs/PointCloud2消息發布傳感器數據。該消息中的stamp域存儲時間信息，領域存儲激光掃描數據來實現機器人的動態避障。

圖6 采樣結果

根據上述步驟逐一編寫代碼，最后在ROS系統下利用重采樣和KLD采樣交替進行的AMCL算法改進move_base功能包，基于離散粒子的權值來對機器人的位姿PoseArray進行估計。AMCL定位算法的結果如圖7所示。進而在圖3學院樓靜態地圖下實現局部規劃（如圖8(a)所示）與全局規劃（如圖8(b)所示）。slam_gmapping功能包可以在未知環境地圖下由激光掃描器節點（/robot/laser/scan）發布的數據創建地圖。圖8(c)為激光掃描器創建的OGM地圖。

圖7 AMCL算法定位結果

在Linux終端輸入rosrun rqt_graph rqt_graph可以調出ROS系統在線檢測工具。圖9是move_base功能包運行時節點之間的相互調用情況。

ROS框架下的模型驗證實驗中不同節點發布不同的主題，并訂閱其他節點所發布的主題，通過調用主題之上的載體消息來完成彼此之間的溝通，保證移動機器人定位數據的同步更新和其路徑規劃數據的實時關聯。同時，在ROS系統下進行移動機器人的程序開發會提高效率，其分布式的管理結構使得代碼和功能包可以方便地移植應用到其他機器人系統開發中。

圖9 ROS系統在線檢測節點狀態

4 結束語

本文在ROS系統下研究了移動機器人在路徑規劃中的問題，分析了AMCL定位的數學模型。通過分析MCL算法與傳統KLD自適應粒子濾波算法中的優缺點，確定采用重采樣和KLD采樣交替進行的AMCL算法。該算法改進了ROS中的move_base節點的路徑規劃功能；在路徑規劃方面，著重研究了基于激光雷達的傳感器數據關聯問題；通過該定位模型實現了移動機器人的局部規劃與全局規劃，并利用slam_gmapping節點在未知環境下創建OGM地圖。但本文沒有考慮3維環境下移動機器人如何定位，只是利用該定位模型驗證分析了2維環境中移動機器人的定位情況。因此對于該算法模型的優化提升以及在3維環境下移動機器人的定位問題等方面是今后仍需研究的方向。

[1] 羅元, 龐冬雪, 張毅, 等. 基于自適應多提議分布粒子濾波的蒙特卡洛定位算法[J]. 計算機應用, 2016, 36(8): 2352-2356.

[2] HANTEN R, BUCK S, OTTE S, et al. Vector-AMCL: vector based adaptive Monte Carlo localization for indoor maps[EB/OL]. [2018-08-08]. https: //www. researchgate. net/publication/305999066_Vector-AMCL_Vector_ based_Adaptive_Monte_Carlo_Localization_for_Indoor_Maps.

[3] THRUN S, FOX D, BURGARD W, et al. Robust Monte Carlo localization for mobile robots[J]. Artificial Intelligence, 2001, 128(1/2): 99-141.

[4] DELLAERT F,FOX D,BURGARD W,et al.Monte Carlo localization for mobile robots[EB/OL.[2018-08-27].https://www.cc.gatech.edu/~dellaert/pub/Dellaert99icra.pdf.

[5] R?FER T,JüNGEL M.Vision-based fast and reactive Monte-Carlo localization[EB/OL.[2018-08-27].http://www.cs.utexas.edu/~pstone/Courses/393Rfall13/readings/german_localize_icra03.pdf.

[6] THOMPSON S, KAGAMI S.Humanoid robot localisation using stereo vision[C]//The Institute of Electrical and Electronic Engineers(IEEE).Proceedings of the 5th IEEE-RAS International Conference on Humanoid Robots.Tsukuba,Japan：IEEE, 2005:19 - 25.DOI: 10.1109/ICHR.2005.1573539.

[7] 張霄漢, 陳小平, 李嘉玲, 等. 一種基于視覺的步行機器人Monte Carlo自定位系統[J]. 機器人, 2006, 28(4): 415-421.

[8] 洪偉, 周長久, 田彥濤. 一種針對人形足球機器人的分域自適應蒙特卡洛定位方法[J]. 機器人, 2012, 34(6): 652-659.

[9] MARTINEZ A, FERNANDEZ E, MAHTANI A, et al. ROS機器人程序設計[M]. 劉錦濤, 張瑞雷, 譯. 北京: 機械工業出版社, 2014.

[10] MACENSKI S. Wiki: amcl[EB/OL]. (2018-09-24)[2018-08-08]. http: //www. ros. org/wiki/amcl.

[11] THRUN S, BURGARD W, FOX D. Probabilistic robotics (intelligent robotics and autonomous agents)[M]. Cambridge: The MIT Press, 2005.

[12] FOX D, BURGARD W, DELLAERT F, et al. Monte Carlo localization: efficient position estimation for mobile robots[EB/OL]. [2018-08-08]. http: //www. aaai. org/Papers/AAAI/1999/AAAI99-050. pdf.

[13] FOX D. Adapting the sample size in particle filters through KLD-sampling[J]. International Journal of Robotics Research, 2016, 22(12): 985-1003.

[14] 殷波. 正弦信號頻率估計算法研究[D]. 哈爾濱: 哈爾濱工程大學, 2015.

[15] 方洵. 基于ROS的移動機器人研究與設計[J]. 科技資訊, 2017(36): 76-80.

[16] 陳卓, 蘇衛華, 安慰寧, 等. 移動機器人SLAM與路徑規劃在ROS框架下的實現[J]. 醫療衛生裝備, 2017, 38(2): 109-113.

An improved adaptive Monte Carlo localization method for robot

WANG Ning1, WANG Jian1, LI Lihua2

（1. School of Geomatics and Urban Spatial Informatics, Beijing University of Civil Engineering and Architecture, Beijing 102616, China;2. School of Land Science and Technology, China University of Geosciences, Beijing 100083, China）

Aiming at the problems of large computation, poor real-time processing ability and particle degradation in Monte Carlo location algorithm, the paper proposed an adaptive positioning model based on AMCL algorithm: the path planning function of move_base node in ROS was improved, and the CAD map of the college building was loaded to realize the path planning; an OGM map was created in an unknown environment based on slam_gmapping node in ROS. Simulational result showed that AMCL positioning model could reduce the positioning errors and improve the accuracy and real time of path planning; and the alternate sampling between resampling and KLD sampling could adjust the number of particles online in real time so that the calculation of the algorithm could be reduced effectively according to the distribution of the particles in the state space.

adaptive Monte Carlo localization model; robot operating system; static map; mobile robot; path planning

P228

A

2095-4999(2019)03-0031-07

2018-10-11

國家重點研發計劃項目（2016YFC0803103）。

王寧（1995—），男，山西長治人，碩士生，研究方向為測繪工程、基于ROS的移動機器人定位等。

王堅（1980—），男，江蘇淮陰人，博士，教授，研究方向為大地測量學與測量工程、應急定位、衛星導航應用服務等。

王寧，王堅，李麗華.一種改進的AMCL機器人定位方法[J].導航定位學報,2019,7(3):31-37.（WANG Ning,WANG Jian,LI Lihua.An improved adaptive Monte Carlo localization method for robot[J].Journal of Navigation and Positioning,2019,7(3):31-37.）

10.16547/j.cnki.10-1096.20190306.