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

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

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

關鍵詞：蒙特卡洛定位;激光雷達;全局定位;特征匹配;機器人操作系統

中圖分類號：TP242.6? ? 文獻標識碼：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

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

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

1 移動機器人全局定位算法

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

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

步驟2：生成實時特征。將激光雷達的掃描數據進行分區，使得分區后每個區域內都包含一條連續的線段特征，對每個區域內的數據使用分割-合并算法提取出區域內的線段特征;

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

步驟4：生成最優預測位姿。將激光雷達生成的原始掃描數據按預測位姿集中的位姿進行轉換，使得所有掃描數據轉換到全局地圖坐標系下，計算掃描數據與全局地圖的距離[Dj]：

[Dj=i=1Pdi]

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

[Dmin=minD1，…，DN]

其中，[N]為預測位姿集的個數。

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

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

步驟7：應用運動模型估計粒子位姿。機器人產生位移，同時激光雷達產生一幀新的數據，使用運動模型的采樣函數[pxtxmt-1，ut]對粒子的位姿信息進行更新，

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

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

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

[wmt?pztxmt，m]

其中，[zt]表示激光雷達[t]時刻的測量數據，[wmt]表示該粒子的權重值;

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

[xt=m=1Mwmtxmt]

步驟10：粒子群重采樣。

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

2 實驗及仿真結果

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

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

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

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

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

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

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

3 結論

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

參考文獻：

[1] 王鵬，李書杰，陳宗海. 移動機器人定位方法研究綜述[C]. 第13屆中國系統仿真技術及其應用學術年會，安徽，中國， 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] 張弦，蘇志遠. 自主移動機器人定位技術研究綜述[J]. 機電產品開發與創新， 2010， 23（2）： 3-5.

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

[5] 操鳳萍，啟要. 基于自適應蒙特卡洛算法的實時定位研究[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] 劉松國，朱世強，劉瑜，等. 移動機器人的蒙特卡羅自主定位算法研究[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.

【通聯編輯：梁書】