王 寧，王 堅(jiān)，李麗華

一種改進(jìn)的AMCL機(jī)器人定位方法

王 寧1，王 堅(jiān)1，李麗華2

（1. 北京建筑大學(xué) 測(cè)繪與城市空間信息學(xué)院，北京 102616；2. 中國(guó)地質(zhì)大學(xué) 土地科學(xué)技術(shù)學(xué)院，北京 100083）

針對(duì)移動(dòng)機(jī)器人技術(shù)中的蒙特卡羅定位算法存在計(jì)算量大、實(shí)時(shí)處理能力差、粒子退化等問(wèn)題，提出一種基于AMCL算法的自適應(yīng)定位模型：改進(jìn)ROS中的move_base節(jié)點(diǎn)的路徑規(guī)劃功能，并加載學(xué)院樓CAD地圖實(shí)現(xiàn)路徑規(guī)劃；基于ROS中slam_gmapping節(jié)點(diǎn)在未知環(huán)境下創(chuàng)建OGM地圖。仿真實(shí)驗(yàn)結(jié)果表明：AMCL定位模型能夠減小定位誤差，提高路徑規(guī)劃的準(zhǔn)確性和實(shí)時(shí)性；采樣部分利用重采樣與KLD采樣交替進(jìn)行的方法，根據(jù)粒子在狀態(tài)空間的分布情況，能夠?qū)崟r(shí)在線調(diào)整粒子數(shù)，有效減少算法計(jì)算量。

自適應(yīng)蒙特卡羅定位模型；機(jī)器人操作系統(tǒng)；靜態(tài)地圖；移動(dòng)機(jī)器人；路徑規(guī)劃

0 引言

移動(dòng)機(jī)器人在各種復(fù)雜環(huán)境下的自主操作中，自主定位是其最基本的任務(wù)之一。移動(dòng)機(jī)器人的自主定位是基于上一時(shí)刻的姿態(tài)估計(jì)，利用已有的環(huán)境地圖信息和傳感器檢測(cè)感受到的被測(cè)量信息，再經(jīng)過(guò)信息的傳輸、處理、顯示、存儲(chǔ)、控制等，最后利用當(dāng)前時(shí)刻的姿態(tài)估計(jì)來(lái)對(duì)移動(dòng)機(jī)器人在環(huán)境地圖中進(jìn)行定位[1-3]。

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

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

2 AMCL自適應(yīng)定位

2.1 MCL算法

式中表示樣本估計(jì)概率。

產(chǎn)生新的粒子集為

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

自適應(yīng)樣本大小對(duì)基于線性傳感器的模型是極其有益的，因?yàn)閼?yīng)用這些模型將會(huì)需要大量的幾何計(jì)算。

2.2 KLD采樣

根據(jù)（8）式，KLD定義為

式中：表示K-L距離。

在重采樣中，復(fù)制權(quán)重較大的粒子，權(quán)重較小的粒子予以忽略。利用KLD采樣確定下一次重采樣在粒子復(fù)制過(guò)程中所需的粒子數(shù)，實(shí)現(xiàn)了粒子數(shù)的在線調(diào)整，減少了算法計(jì)算量。

2.3 重采樣和KLD采樣交替進(jìn)行的AMCL

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

2）將當(dāng)前粒子集中粒子的權(quán)重作歸一化處理。

3）對(duì)更新處理后的粒子集進(jìn)行重采樣和KLD交替采樣，利用式（14）對(duì)重采樣的粒子數(shù)進(jìn)行更新。

式中E[]為數(shù)學(xué)期望。

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

3 ROS框架下的模型驗(yàn)證

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

3.1 仿真研究

機(jī)器人3D模型或部分結(jié)構(gòu)模型主要用于仿真機(jī)器人或者簡(jiǎn)化開(kāi)發(fā)者的日常工作，在ROS中模型文件有URDF文件和XACRO文件。仿真環(huán)境由rviz或gazebo節(jié)點(diǎn)控制。

為了在rviz節(jié)點(diǎn)中查看仿真機(jī)器人3維模型，需要編寫啟動(dòng)文件（display.launch），ROS中的啟動(dòng)文件可以同時(shí)啟動(dòng)節(jié)點(diǎn)管理器（roscore）和多個(gè)節(jié)點(diǎn)，啟動(dòng)命令為roslaunch package-name launch- file-name。rviz仿真結(jié)果如圖1所示。

Gazebo是ROS系統(tǒng)下模擬室內(nèi)室外復(fù)雜3維環(huán)境，并可以在3維場(chǎng)景中添加多個(gè)機(jī)器人、不同傳感器和常見(jiàn)物體來(lái)模擬真實(shí)世界對(duì)物體的物理響應(yīng)和實(shí)際反饋的仿真環(huán)境。在Gazebo中進(jìn)行機(jī)器人仿真，就需要添加物理屬性和碰撞屬性，否則Gazebo無(wú)法使用這些模型。本文使用柳樹(shù)車庫(kù)（Willow Garage）辦公室模型地圖。Gazebo允許使用左邊菜單插入和放置物體，本文添加了書架、桌子、罐子，來(lái)監(jiān)測(cè)傳感器對(duì)它們?nèi)绾畏磻?yīng)。

圖1 rviz仿真結(jié)果

最后創(chuàng)建一個(gè)新的啟動(dòng)文件（gazebo.launch）來(lái)同時(shí)加載地圖和機(jī)器人。Gazebo仿真結(jié)果如圖2所示。

圖2 Gazebo仿真結(jié)果

3.2 基于AMCL算法的移動(dòng)機(jī)器人定位

自適應(yīng)蒙特卡羅定位[9-10]是一種用于2維環(huán)境下移動(dòng)機(jī)器人概率統(tǒng)計(jì)定位方法。這種算法在ROS框架下的具體實(shí)現(xiàn)主要是通過(guò)在已知地圖上使用粒子濾波算法跟蹤機(jī)器人的位姿。

自適應(yīng)蒙特卡羅定位節(jié)點(diǎn)主要使用激光掃描和激光雷達(dá)地圖。本文以北京建筑大學(xué)測(cè)繪與城市空間信息學(xué)院二層地圖為例進(jìn)行模擬。圖3所示是加載學(xué)院樓CAD地圖的情況。以此為基礎(chǔ)，模擬激光掃描和激光雷達(dá)數(shù)據(jù)，采用本文算法，通過(guò)傳遞消息完成位姿估計(jì)的計(jì)算。

圖3 學(xué)院樓地圖

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

圖4 tf坐標(biāo)變換

圖5 傳感器數(shù)據(jù)

然后針對(duì)ROS框架提供的各個(gè)初始化參數(shù)，完成自適應(yīng)蒙特卡羅定位算法粒子濾波器的初始化。如果沒(méi)有設(shè)定初始化位姿，自適應(yīng)蒙特卡羅定位算法會(huì)假定機(jī)器人從坐標(biāo)系原點(diǎn)運(yùn)行，如此會(huì)增加計(jì)算的復(fù)雜度。文中利用rviz界面中2D Pose Estimate按鈕來(lái)設(shè)定初始化位姿。機(jī)器人位姿利用采樣所得的粒子分布來(lái)跟蹤，采樣結(jié)果如圖6所示。

最后ROS通過(guò)nav_msgs/Odometry消息發(fā)布關(guān)于2個(gè)坐標(biāo)系之間的位移變換信息。該消息提供機(jī)器人的位姿信息和速度信息。ROS通過(guò)sensor_msgs/LaserScan或sensor_msgs/PointCloud2消息發(fā)布傳感器數(shù)據(jù)。該消息中的stamp域存儲(chǔ)時(shí)間信息，領(lǐng)域存儲(chǔ)激光掃描數(shù)據(jù)來(lái)實(shí)現(xiàn)機(jī)器人的動(dòng)態(tài)避障。

圖6 采樣結(jié)果

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

圖7 AMCL算法定位結(jié)果

在Linux終端輸入rosrun rqt_graph rqt_graph可以調(diào)出ROS系統(tǒng)在線檢測(cè)工具。圖9是move_base功能包運(yùn)行時(shí)節(jié)點(diǎn)之間的相互調(diào)用情況。

ROS框架下的模型驗(yàn)證實(shí)驗(yàn)中不同節(jié)點(diǎn)發(fā)布不同的主題，并訂閱其他節(jié)點(diǎn)所發(fā)布的主題，通過(guò)調(diào)用主題之上的載體消息來(lái)完成彼此之間的溝通，保證移動(dòng)機(jī)器人定位數(shù)據(jù)的同步更新和其路徑規(guī)劃數(shù)據(jù)的實(shí)時(shí)關(guān)聯(lián)。同時(shí)，在ROS系統(tǒng)下進(jìn)行移動(dòng)機(jī)器人的程序開(kāi)發(fā)會(huì)提高效率，其分布式的管理結(jié)構(gòu)使得代碼和功能包可以方便地移植應(yīng)用到其他機(jī)器人系統(tǒng)開(kāi)發(fā)中。

圖9 ROS系統(tǒng)在線檢測(cè)節(jié)點(diǎn)狀態(tài)

4 結(jié)束語(yǔ)

本文在ROS系統(tǒng)下研究了移動(dòng)機(jī)器人在路徑規(guī)劃中的問(wèn)題，分析了AMCL定位的數(shù)學(xué)模型。通過(guò)分析MCL算法與傳統(tǒng)KLD自適應(yīng)粒子濾波算法中的優(yōu)缺點(diǎn)，確定采用重采樣和KLD采樣交替進(jìn)行的AMCL算法。該算法改進(jìn)了ROS中的move_base節(jié)點(diǎn)的路徑規(guī)劃功能；在路徑規(guī)劃方面，著重研究了基于激光雷達(dá)的傳感器數(shù)據(jù)關(guān)聯(lián)問(wèn)題；通過(guò)該定位模型實(shí)現(xiàn)了移動(dòng)機(jī)器人的局部規(guī)劃與全局規(guī)劃，并利用slam_gmapping節(jié)點(diǎn)在未知環(huán)境下創(chuàng)建OGM地圖。但本文沒(méi)有考慮3維環(huán)境下移動(dòng)機(jī)器人如何定位，只是利用該定位模型驗(yàn)證分析了2維環(huán)境中移動(dòng)機(jī)器人的定位情況。因此對(duì)于該算法模型的優(yōu)化提升以及在3維環(huán)境下移動(dòng)機(jī)器人的定位問(wèn)題等方面是今后仍需研究的方向。

[1] 羅元, 龐冬雪, 張毅, 等. 基于自適應(yīng)多提議分布粒子濾波的蒙特卡洛定位算法[J]. 計(jì)算機(jī)應(yīng)用, 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] 張霄漢, 陳小平, 李嘉玲, 等. 一種基于視覺(jué)的步行機(jī)器人Monte Carlo自定位系統(tǒng)[J]. 機(jī)器人, 2006, 28(4): 415-421.

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

[9] MARTINEZ A, FERNANDEZ E, MAHTANI A, et al. ROS機(jī)器人程序設(shè)計(jì)[M]. 劉錦濤, 張瑞雷, 譯. 北京: 機(jī)械工業(yè)出版社, 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] 殷波. 正弦信號(hào)頻率估計(jì)算法研究[D]. 哈爾濱: 哈爾濱工程大學(xué), 2015.

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

[16] 陳卓, 蘇衛(wèi)華, 安慰寧, 等. 移動(dòng)機(jī)器人SLAM與路徑規(guī)劃在ROS框架下的實(shí)現(xiàn)[J]. 醫(yī)療衛(wèi)生裝備, 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

國(guó)家重點(diǎn)研發(fā)計(jì)劃項(xiàng)目（2016YFC0803103）。

王寧（1995—），男，山西長(zhǎng)治人，碩士生，研究方向?yàn)闇y(cè)繪工程、基于ROS的移動(dòng)機(jī)器人定位等。

王堅(jiān)（1980—），男，江蘇淮陰人，博士，教授，研究方向?yàn)榇蟮販y(cè)量學(xué)與測(cè)量工程、應(yīng)急定位、衛(wèi)星導(dǎo)航應(yīng)用服務(wù)等。

王寧，王堅(jiān)，李麗華.一種改進(jìn)的AMCL機(jī)器人定位方法[J].導(dǎo)航定位學(xué)報(bào),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.