基于特征匹配和MCL的全局融合定位算法研究

章洋 李星博 胡丁文 楊帆 蘭長勇

摘要：傳統(tǒng)的蒙特卡洛定位（Monte Carlo Localization， MCL）算法在實時性以及定位精度上，都還存在改進(jìn)的余地。通過將特征匹配和蒙特卡洛定位進(jìn)行融合，對基于特征匹配和MCL的全局融合定位算法進(jìn)行了研究。融合定位算法在蒙特卡洛定位的基礎(chǔ)上，增加了線段特征匹配的過程，用于縮小定位的范圍，從而改善定位精度和實時性。在MATLAB軟件以及機(jī)器人的測試實驗中，通過三角形運動路徑來對比蒙特卡洛定位算法與融合定位算法的定位精度，實驗結(jié)果表明融合定位算法的定位精度比蒙特卡洛定位算法提高了78%。

關(guān)鍵詞：蒙特卡洛定位;激光雷達(dá);全局定位;特征匹配;機(jī)器人操作系統(tǒng)

中圖分類號：TP242.6? ? 文獻(xiàn)標(biāo)識碼：A

文章編號：1009-3044（2019）19-0186-03

Abstract： 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 based 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 MATLAB 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 that of Monte Carlo Localization algorithm.

Key words： Monte Carlo Localization; Lidar; Global Localization; Feature Matching; Robot Operating System

移動機(jī)器人定位是指機(jī)器人通過感知獲取環(huán)境信息，經(jīng)過相關(guān)的信息處理而確定自身及目標(biāo)位姿的過程[1]。機(jī)器人導(dǎo)航的相關(guān)問題更形象的一種表達(dá)方式是：“我在哪兒”“我要去哪兒”及“我如何到達(dá)那兒”[2]，而其中的第一個問題“我在哪兒”即機(jī)器人的定位問題，是實現(xiàn)機(jī)器人導(dǎo)航的前提。針對這一實際問題，前人已經(jīng)提出了許多機(jī)器人定位的方法，例如比較流行的基于概率的定位方法和基于地圖的定位方法[3]，尤其是基于概率的機(jī)器人定位方法，近年來受到大量學(xué)者的關(guān)注[4]。現(xiàn)有的研究和應(yīng)用表明：概率定位方法是一種能夠有效實現(xiàn)巡檢機(jī)器人精確定位和自主導(dǎo)航的方法[5]。而基于概率的定位方法中，蒙特卡洛定位算法由于其較高的計算效率，在許多場景中得到應(yīng)用。在文獻(xiàn)[6]中，Dellaert等人基于馬爾可夫定位方法，提出了一種利用粒子群描述概率分布的方法，即粒子濾波（particle filter， PF）算法。PF算法保留了馬爾可夫定位法定位精度高的特點，同時，該算法易于實現(xiàn)，對硬件運算資源的要求較低，由于PF算法是受蒙特卡洛思想的啟發(fā)，Dellaert等人也將這種定位方法稱為蒙特卡洛定位，并且這一術(shù)語已經(jīng)成為移動機(jī)器人定位研究領(lǐng)域的通用說法。但是經(jīng)典的蒙特卡洛定位算法存在定位失效和機(jī)器人綁架的問題，很多學(xué)者針對這一問題提出了改進(jìn)的方法，其中Fox等[7]人提出了自適應(yīng)蒙特卡洛定位（Adaptive Monte Carlo Localization， AMCL）算法，AMCL有效地解決了MCL算法存在的定位失效的問題。

基于以上分析，本文對基于特征匹配和MCL的全局融合定位算法進(jìn)行了研究。全局融合定位算法以開源機(jī)器人操作系統(tǒng)（Robot Operating System，ROS）的機(jī)器人為平臺，首先使用線段提?。↙ine Segment Detector， LSD）算法[11]提取全局地圖的特征數(shù)據(jù)，然后使用分開合并（Split-and-Merge，S-M）算法[12]提取機(jī)器人采集的激光雷達(dá)傳感器數(shù)據(jù)中的線段特征，再將這些特征數(shù)據(jù)與數(shù)據(jù)庫中存儲的全局地圖的特征數(shù)據(jù)進(jìn)行匹配，匹配后可以得出機(jī)器人當(dāng)前位姿的一個估計值，將這一位姿估計值應(yīng)用于MCL算法中，這樣大大提高了MCL算法的收斂速度，最終實現(xiàn)機(jī)器人的快速全局定位。

1 移動機(jī)器人全局定位算法

本文在MCL的基礎(chǔ)上引入特征匹配實現(xiàn)融合定位功能，融合定位的主要思想是將特征匹配估算的位姿作為MCL算法中粒子群的初始位姿，融合定位算法流程如下所示：

步驟1：生成柵格地圖的特征數(shù)據(jù)庫。將柵格地圖轉(zhuǎn)換為二值圖像，使用LSD算法提取柵格地圖的所有線特征，組成柵格地圖的特征數(shù)據(jù)庫;

步驟2：生成實時特征。將激光雷達(dá)的掃描數(shù)據(jù)進(jìn)行分區(qū)，使得分區(qū)后每個區(qū)域內(nèi)都包含一條連續(xù)的線段特征，對每個區(qū)域內(nèi)的數(shù)據(jù)使用分割-合并算法提取出區(qū)域內(nèi)的線段特征;

步驟3：生成預(yù)測位姿集。根據(jù)實時特征中其中一條線段的線段長度，查找特征數(shù)據(jù)庫中相同線段長度的線段特征，得到若干個預(yù)測位姿，將這些位姿組成預(yù)測位姿集;

步驟4：生成最優(yōu)預(yù)測位姿。將激光雷達(dá)生成的原始掃描數(shù)據(jù)按預(yù)測位姿集中的位姿進(jìn)行轉(zhuǎn)換，使得所有掃描數(shù)據(jù)轉(zhuǎn)換到全局地圖坐標(biāo)系下，計算掃描數(shù)據(jù)與全局地圖的距離[Dj]：

[Dj=i=1Pdi]

其中，[Dj]為預(yù)測位姿集中第[j]個位姿對應(yīng)的距離，[di]為掃描數(shù)據(jù)中第[i]個數(shù)據(jù)點距最近柵格點的距離，[P]為掃描數(shù)據(jù)中數(shù)據(jù)點的總數(shù)。最終，計算預(yù)測位姿集中所有位姿對應(yīng)距離的最小值[Dmin]，最小值[Dmin]所對應(yīng)的位姿作為最優(yōu)預(yù)測位姿。

[Dmin=minD1，…，DN]

其中，[N]為預(yù)測位姿集的個數(shù)。

步驟5：根據(jù)最優(yōu)預(yù)測位姿初始化MCL算法的粒子群，生成[M]個粒子。其中粒子的位姿初始化為最優(yōu)預(yù)測位姿，所有粒子權(quán)值初始化為[1M]，應(yīng)用測量模型計算粒子權(quán)重，計算粒子群的平均權(quán)重;

步驟6：判斷粒子群的平均權(quán)重是否大于閾值，如果大于閾值，則進(jìn)入步驟7，否則，返回，步驟2;

步驟7：應(yīng)用運動模型估計粒子位姿。機(jī)器人產(chǎn)生位移，同時激光雷達(dá)產(chǎn)生一幀新的數(shù)據(jù)，使用運動模型的采樣函數(shù)[pxtxmt-1，ut]對粒子的位姿信息進(jìn)行更新，

[xt[m]?pxtxmt-1，ut]

其中，[xmt-1]表示[t]-1時刻粒子群[χt-1]中第[m]個粒子的位姿，[ut]表示從[t]-1時刻至[t]時刻之間機(jī)器人的運動數(shù)據(jù)，[xt[m]]表示[t]時刻該粒子的位姿;

步驟8：用測量模型計算粒子權(quán)重。使用測量模型的采樣函數(shù)[pztxmt，m]計算粒子的權(quán)重值

[wmt?pztxmt，m]

其中，[zt]表示激光雷達(dá)[t]時刻的測量數(shù)據(jù)，[wmt]表示該粒子的權(quán)重值;

步驟9：估計位姿。計算粒子群[χ′t]中粒子的位姿期望得到估計位姿[xt]

[xt=m=1Mwmtxmt]

步驟10：粒子群重采樣。

綜上所述，基于特征匹配和MCL的全局融合定位算法的流程圖如下圖1所示：

2 實驗及仿真結(jié)果

在測試全局定位時，將真實環(huán)境的柵格地圖導(dǎo)入進(jìn)來，在ROS系統(tǒng)上機(jī)器人按照固定的運行指令進(jìn)行運動，機(jī)器人身上的傳感器記錄數(shù)據(jù)同時人為的記錄真實路徑，在MATLAB上分別通過蒙特卡洛定位和本文提出的融合定位兩種定位方法計算出估計路徑，對于定位誤差的評估分別使用位置誤差[εi]和路徑誤差[εp]來描述。位置誤差的定義為

[εi=xi-xi2+yi-yi2]

其中，[εi]表示路徑上第[i]次采集數(shù)據(jù)的位置誤差，[xi]表示第[i]次采集數(shù)據(jù)時[x]方向的真實坐標(biāo)，[xi]表示第[i]次采集數(shù)據(jù)時[x]方向的估計坐標(biāo)，[yi]表示第[i]次采集數(shù)據(jù)時[y]方向的真實坐標(biāo)，[yi]表示第[i]次采集數(shù)據(jù)時[y]方向的估計坐標(biāo)。

路徑誤差[εp]定義為路徑中位置誤差的均方根誤差

[εp=i=1Nε2iN]

其中，[εi]為路徑上第[i]次采集數(shù)據(jù)的位置誤差，[N]為路徑上采集數(shù)據(jù)的總次數(shù)。

本次實驗的真實環(huán)境是電子科技大學(xué)清水河校區(qū)科研樓B區(qū)2樓217附近的空間區(qū)域，機(jī)器人運動路徑在真實環(huán)境中示意圖，如圖2所示。

3 結(jié)論

本文在傳統(tǒng)的蒙特卡洛定位上，針對全局定位存在收斂時間，且路徑誤差較大，通過引入線段特征匹配，完成位姿的最初估計，再使用蒙特卡洛定位完成位姿跟蹤，對基于特征匹配和MCL的全局融合定位算法進(jìn)行了研究，在真實環(huán)境中，通過三角形的測試定位路徑，并對測試路徑分別測試20次，完成全局定位的測試。通過測試數(shù)據(jù)，我們發(fā)現(xiàn)融合定位的平均位置誤差都小于蒙特卡洛定位的平均位置誤差，不存在定位收斂時間，在路徑誤差上，融合定位的平均路徑誤差比蒙特卡洛定位提高了78%。

參考文獻(xiàn)：

[1] 王鵬，李書杰，陳宗海. 移動機(jī)器人定位方法研究綜述[C]. 第13屆中國系統(tǒng)仿真技術(shù)及其應(yīng)用學(xué)術(shù)年會，安徽，中國， 2011， 911-915.

[2] Leonard J， Durrant-Whyte H F. Mobile robot localization by tracking geometric beacons[J]. IEEE Transactions on Robotics and Automation， 1991， 7（3）： 376-382

[3] 張弦，蘇志遠(yuǎn). 自主移動機(jī)器人定位技術(shù)研究綜述[J]. 機(jī)電產(chǎn)品開發(fā)與創(chuàng)新， 2010， 23（2）： 3-5.

[4] 時也，吳懷宇，徐文霞，等. 基于擴(kuò)展卡爾曼濾波器的移動機(jī)器人SLAM研究[J]. 電子設(shè)計工程， 2012， 20（1）： 104-106.

[5] 操鳳萍，啟要. 基于自適應(yīng)蒙特卡洛算法的實時定位研究[J]. 計算機(jī)工程， 2018， 4（9）： 8-32.

[6] Dellaert F， Burgard W， Fox D. Monte Carlo Localization for Mobile Robots[A]. IEEE International Conference on Robotics and Automation[C]， 1999.

[7] Fox D， Burgard W， Dellaert F， et al. Monte Carlo Localization：Efficient Position Estimation for Mobile Robots[A]. National Conference on ArtificialIntelligence[C]， 1999： 343-349.

[8] Gordon N J，Salmond D J ， Smith A F M.Novel approach to nonlinear /non-Gaussian Bayesian state estimation[J].Proceedings F-Radar and Signal Processing，1993，140（ 2）： 107-113.

[9] 劉松國，朱世強(qiáng)，劉瑜，等. 移動機(jī)器人的蒙特卡羅自主定位算法研究[J]. 機(jī)電工程， 2005， 22（4）：38-42.

[10] Doucet A， Godsill S. On Sequential Monte Carlo Sampling Methods for Bayesian Filtering[R]. Cambrige： University of Cambrige， 1998： 1-36.

[11] R G Von Gioi， J Jakubowicz， J M Morel， et al. LSD： a line segment detector[J]. Image Processing On Line， 2012， 2： 35-55.

[12] V. Nguyen， A. Martinelli， N. Tomatis， et al. A comparison of line extraction algorithms using 2D laser rangefinder for indoor mobile robotics[C]. 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems. Edmonton， Alta， 2005， 1929-1934.

【通聯(lián)編輯：梁書】