1、 基于特征匹配和MCL的全局融合定位算法研究 章洋 李星博 胡丁文 楊帆 蘭長(zhǎng)勇Summary：傳統(tǒng)的蒙特卡洛定位（Monte Carlo Localization， MCL）算法在實(shí)時(shí)性以及定位精度上，都還存在改進(jìn)的余地。通過(guò)將特征匹配和蒙特卡洛定位進(jìn)行融合，對(duì)基于特征匹配和MCL的全局融合定位算法進(jìn)行了研究。融合定位算法在蒙特卡洛定位的基礎(chǔ)上，增加了線段特征匹配的過(guò)程，用于縮小定位的范圍，從而改善定位精度和實(shí)時(shí)性。在MATLAB軟件以及機(jī)器人的測(cè)試實(shí)驗(yàn)中，通過(guò)三角形運(yùn)動(dòng)路徑來(lái)對(duì)比蒙特卡洛定位算法與融合定位算法的定位精度，實(shí)驗(yàn)結(jié)果表明融合定位算法的定位精度比蒙特卡洛定位算法提高了78%。Ke

2、y：蒙特卡洛定位;激光雷達(dá);全局定位;特征匹配;機(jī)器人操作系統(tǒng)：TP242.6 ：A：1009-3044（2019）19-0186-03Abstract： The traditional Monte Carlo Localization algorithm still has room for improvement in real-time and positioning accuracy. By combining feature matching and Monte Carlo positioning， the global fusion localization algorithm b

3、ased on feature matching and Monte Carlo is studied. Based on Monte Carlo Localization， the Fusion Localization algorithm increases the process of line segment feature matching， which is used to narrow the localization range， thus improving localization accuracy and real-time performance. In the MAT

4、LAB software and robot test experiments， the localization accuracy of Monte Carlo Localization algorithm and the Fusion Localization algorithm is compared by the triangle motion path. Experimental results show that the localization accuracy of the Fusion Localization algorithm is 78% higher than tha

5、t of Monte Carlo Localization algorithm.Key words： Monte Carlo Localization; Lidar; Global Localization; Feature Matching; Robot Operating System移動(dòng)機(jī)器人定位是指機(jī)器人通過(guò)感知獲取環(huán)境信息，經(jīng)過(guò)相關(guān)的信息處理而確定自身及目標(biāo)位姿的過(guò)程1。機(jī)器人導(dǎo)航的相關(guān)問(wèn)題更形象的一種表達(dá)方式是：“我在哪兒”“我要去哪兒”及“我如何到達(dá)那兒”2，而其中的第一個(gè)問(wèn)題“我在哪兒”即機(jī)器人的定位問(wèn)題，是實(shí)現(xiàn)機(jī)器人導(dǎo)航的前提。針對(duì)這一實(shí)際問(wèn)題，前人已經(jīng)提出了許多機(jī)器人定位的

6、方法，例如比較流行的基于概率的定位方法和基于地圖的定位方法3，尤其是基于概率的機(jī)器人定位方法，近年來(lái)受到大量學(xué)者的關(guān)注4?，F(xiàn)有的研究和應(yīng)用表明：概率定位方法是一種能夠有效實(shí)現(xiàn)巡檢機(jī)器人精確定位和自主導(dǎo)航的方法5。而基于概率的定位方法中，蒙特卡洛定位算法由于其較高的計(jì)算效率，在許多場(chǎng)景中得到應(yīng)用。在文獻(xiàn)6中，Dellaert等人基于馬爾可夫定位方法，提出了一種利用粒子群描述概率分布的方法，即粒子濾波（particle filter， PF）算法。PF算法保留了馬爾可夫定位法定位精度高的特點(diǎn)，同時(shí)，該算法易于實(shí)現(xiàn)，對(duì)硬件運(yùn)算資源的要求較低，由于PF算法是受蒙特卡洛思想的啟發(fā)，Dellaert等人也

7、將這種定位方法稱為蒙特卡洛定位，并且這一術(shù)語(yǔ)已經(jīng)成為移動(dòng)機(jī)器人定位研究領(lǐng)域的通用說(shuō)法。但是經(jīng)典的蒙特卡洛定位算法存在定位失效和機(jī)器人綁架的問(wèn)題，很多學(xué)者針對(duì)這一問(wèn)題提出了改進(jìn)的方法，其中Fox等7人提出了自適應(yīng)蒙特卡洛定位（Adaptive Monte Carlo Localization， AMCL）算法，AMCL有效地解決了MCL算法存在的定位失效的問(wèn)題?；谝陨戏治?，本文對(duì)基于特征匹配和MCL的全局融合定位算法進(jìn)行了研究。全局融合定位算法以開(kāi)源機(jī)器人操作系統(tǒng)（Robot Operating System，ROS）的機(jī)器人為平臺(tái)，首先使用線段提?。↙ine Segment Detector

8、， LSD）算法11提取全局地圖的特征數(shù)據(jù)，然后使用分開(kāi)合并（Split-and-Merge，S-M）算法12提取機(jī)器人采集的激光雷達(dá)傳感器數(shù)據(jù)中的線段特征，再將這些特征數(shù)據(jù)與數(shù)據(jù)庫(kù)中存儲(chǔ)的全局地圖的特征數(shù)據(jù)進(jìn)行匹配，匹配后可以得出機(jī)器人當(dāng)前位姿的一個(gè)估計(jì)值，將這一位姿估計(jì)值應(yīng)用于MCL算法中，這樣大大提高了MCL算法的收斂速度，最終實(shí)現(xiàn)機(jī)器人的快速全局定位。1 移動(dòng)機(jī)器人全局定位算法本文在MCL的基礎(chǔ)上引入特征匹配實(shí)現(xiàn)融合定位功能，融合定位的主要思想是將特征匹配估算的位姿作為MCL算法中粒子群的初始位姿，融合定位算法流程如下所示：步驟1：生成柵格地圖的特征數(shù)據(jù)庫(kù)。將柵格地圖轉(zhuǎn)換為二值圖像，使

9、用LSD算法提取柵格地圖的所有線特征，組成柵格地圖的特征數(shù)據(jù)庫(kù);步驟2：生成實(shí)時(shí)特征。將激光雷達(dá)的掃描數(shù)據(jù)進(jìn)行分區(qū)，使得分區(qū)后每個(gè)區(qū)域內(nèi)都包含一條連續(xù)的線段特征，對(duì)每個(gè)區(qū)域內(nèi)的數(shù)據(jù)使用分割-合并算法提取出區(qū)域內(nèi)的線段特征;步驟3：生成預(yù)測(cè)位姿集。根據(jù)實(shí)時(shí)特征中其中一條線段的線段長(zhǎng)度，查找特征數(shù)據(jù)庫(kù)中相同線段長(zhǎng)度的線段特征，得到若干個(gè)預(yù)測(cè)位姿，將這些位姿組成預(yù)測(cè)位姿集;步驟4：生成最優(yōu)預(yù)測(cè)位姿。將激光雷達(dá)生成的原始掃描數(shù)據(jù)按預(yù)測(cè)位姿集中的位姿進(jìn)行轉(zhuǎn)換，使得所有掃描數(shù)據(jù)轉(zhuǎn)換到全局地圖坐標(biāo)系下，計(jì)算掃描數(shù)據(jù)與全局地圖的距離Dj：Dj=i=1Pdi其中，Dj為預(yù)測(cè)位姿集中第j個(gè)位姿對(duì)應(yīng)的距離，di為掃

10、描數(shù)據(jù)中第i個(gè)數(shù)據(jù)點(diǎn)距最近柵格點(diǎn)的距離，P為掃描數(shù)據(jù)中數(shù)據(jù)點(diǎn)的總數(shù)。最終，計(jì)算預(yù)測(cè)位姿集中所有位姿對(duì)應(yīng)距離的最小值Dmin，最小值Dmin所對(duì)應(yīng)的位姿作為最優(yōu)預(yù)測(cè)位姿。Dmin=minD1，DN其中，N為預(yù)測(cè)位姿集的個(gè)數(shù)。步驟5：根據(jù)最優(yōu)預(yù)測(cè)位姿初始化MCL算法的粒子群，生成M個(gè)粒子。其中粒子的位姿初始化為最優(yōu)預(yù)測(cè)位姿，所有粒子權(quán)值初始化為1M，應(yīng)用測(cè)量模型計(jì)算粒子權(quán)重，計(jì)算粒子群的平均權(quán)重;步驟6：判斷粒子群的平均權(quán)重是否大于閾值，如果大于閾值，則進(jìn)入步驟7，否則，返回，步驟2;步驟7：應(yīng)用運(yùn)動(dòng)模型估計(jì)粒子位姿。機(jī)器人產(chǎn)生位移，同時(shí)激光雷達(dá)產(chǎn)生一幀新的數(shù)據(jù)，使用運(yùn)動(dòng)模型的采樣函數(shù)pxtxmt

11、-1，ut對(duì)粒子的位姿信息進(jìn)行更新，xtm?pxtxmt-1，ut其中，xmt-1表示t-1時(shí)刻粒子群t-1中第m個(gè)粒子的位姿，ut表示從t-1時(shí)刻至t時(shí)刻之間機(jī)器人的運(yùn)動(dòng)數(shù)據(jù)，xtm表示t時(shí)刻該粒子的位姿;步驟8：用測(cè)量模型計(jì)算粒子權(quán)重。使用測(cè)量模型的采樣函數(shù)pztxmt，m計(jì)算粒子的權(quán)重值wmt?pztxmt，m其中，zt表示激光雷達(dá)t時(shí)刻的測(cè)量數(shù)據(jù)，wmt表示該粒子的權(quán)重值;步驟9：估計(jì)位姿。計(jì)算粒子群t中粒子的位姿期望得到估計(jì)位姿xtxt=m=1Mwmtxmt步驟10：粒子群重采樣。綜上所述，基于特征匹配和MCL的全局融合定位算法的流程圖如下圖1所示：2 實(shí)驗(yàn)及仿真結(jié)果在測(cè)試全局定位時(shí)

12、，將真實(shí)環(huán)境的柵格地圖導(dǎo)入進(jìn)來(lái)，在ROS系統(tǒng)上機(jī)器人按照固定的運(yùn)行指令進(jìn)行運(yùn)動(dòng)，機(jī)器人身上的傳感器記錄數(shù)據(jù)同時(shí)人為的記錄真實(shí)路徑，在MATLAB上分別通過(guò)蒙特卡洛定位和本文提出的融合定位兩種定位方法計(jì)算出估計(jì)路徑，對(duì)于定位誤差的評(píng)估分別使用位置誤差i和路徑誤差p來(lái)描述。位置誤差的定義為i=xi-xi2+yi-yi2其中，i表示路徑上第i次采集數(shù)據(jù)的位置誤差，xi表示第i次采集數(shù)據(jù)時(shí)x方向的真實(shí)坐標(biāo)，xi表示第i次采集數(shù)據(jù)時(shí)x方向的估計(jì)坐標(biāo)，yi表示第i次采集數(shù)據(jù)時(shí)y方向的真實(shí)坐標(biāo)，yi表示第i次采集數(shù)據(jù)時(shí)y方向的估計(jì)坐標(biāo)。路徑誤差p定義為路徑中位置誤差的均方根誤差p=i=1N2iN其中，i為路

13、徑上第i次采集數(shù)據(jù)的位置誤差，N為路徑上采集數(shù)據(jù)的總次數(shù)。本次實(shí)驗(yàn)的真實(shí)環(huán)境是電子科技大學(xué)清水河校區(qū)科研樓B區(qū)2樓217附近的空間區(qū)域，機(jī)器人運(yùn)動(dòng)路徑在真實(shí)環(huán)境中示意圖，如圖2所示。3 結(jié)論本文在傳統(tǒng)的蒙特卡洛定位上，針對(duì)全局定位存在收斂時(shí)間，且路徑誤差較大，通過(guò)引入線段特征匹配，完成位姿的最初估計(jì)，再使用蒙特卡洛定位完成位姿跟蹤，對(duì)基于特征匹配和MCL的全局融合定位算法進(jìn)行了研究，在真實(shí)環(huán)境中，通過(guò)三角形的測(cè)試定位路徑，并對(duì)測(cè)試路徑分別測(cè)試20次，完成全局定位的測(cè)試。通過(guò)測(cè)試數(shù)據(jù)，我們發(fā)現(xiàn)融合定位的平均位置誤差都小于蒙特卡洛定位的平均位置誤差，不存在定位收斂時(shí)間，在路徑誤差上，融合定位的平均

14、路徑誤差比蒙特卡洛定位提高了78%。Reference：1 王鵬，李書(shū)杰，陳宗海. 移動(dòng)機(jī)器人定位方法研究綜述C. 第13屆中國(guó)系統(tǒng)仿真技術(shù)及其應(yīng)用學(xué)術(shù)年會(huì)，安徽，中國(guó)， 2011， 911-915.2 Leonard J， Durrant-Whyte H F. Mobile robot localization by tracking geometric beaconsJ. IEEE Transactions on Robotics and Automation， 1991， 7（3）： 376-3823 張弦，蘇志遠(yuǎn). 自主移動(dòng)機(jī)器人定位技術(shù)研究綜述J. 機(jī)電產(chǎn)品開(kāi)發(fā)與創(chuàng)新， 2010，

15、23（2）： 3-5.4 時(shí)也，吳懷宇，徐文霞，等. 基于擴(kuò)展卡爾曼濾波器的移動(dòng)機(jī)器人SLAM研究J. 電子設(shè)計(jì)工程， 2012， 20（1）： 104-106.5 操鳳萍，啟要. 基于自適應(yīng)蒙特卡洛算法的實(shí)時(shí)定位研究J. 計(jì)算機(jī)工程， 2018， 4（9）： 8-32.6 Dellaert F， Burgard W， Fox D. Monte Carlo Localization for Mobile RobotsA. IEEE International Conference on Robotics and AutomationC， 1999.7 Fox D， Burgard W， Del

16、laert F， et al. Monte Carlo Localization：Efficient Position Estimation for Mobile RobotsA. National Conference on ArtificialIntelligenceC， 1999： 343-349.8 Gordon N J，Salmond D J ， Smith A F M.Novel approach to nonlinear /non-Gaussian Bayesian state estimationJ.Proceedings F-Radar and Signal Processi

17、ng，1993，140（ 2）： 107-113.9 劉松國(guó)，朱世強(qiáng)，劉瑜，等. 移動(dòng)機(jī)器人的蒙特卡羅自主定位算法研究J. 機(jī)電工程， 2005， 22（4）：38-42.10 Doucet A， Godsill S. On Sequential Monte Carlo Sampling Methods for Bayesian FilteringR. Cambrige： University of Cambrige， 1998： 1-36.11 R G Von Gioi， J Jakubowicz， J M Morel， et al. LSD： a line segment detectorJ. Image Processing On Line， 2012， 2： 35-5