基于全局特征點匹配的全局定位方法

2023-02-14 10:32:08彭澤鑫劉建圻

計算機工程與應用 2023年3期

關鍵詞：特征

彭澤鑫，曾碧，劉建圻

廣東工業大學計算機學院，廣州 510006

移動機器人的定位問題是同步定位與建圖（simultaneous localization and mapping，SLAM）[1-2]的一個基本問題，指的是移動機器人根據傳感器信息去推斷當前的位姿。定位問題大致可以分為局部定位與全局定位[3]。局部定位指的是在已知機器人的初始位姿的情況下，通過適應機器人運動噪聲和傳感器數據來推測機器人下一幀的位姿。全局定位指的是在未知機器人的初始位姿，通過傳感器數據和地圖數據等進行匹配推測出機器人當前的位姿。全局定位是在缺少初始位姿的情況下推測出當前的位姿，所以它比局部定位更加困難。

全局定位問題根據使用的傳感器大致可以分為基于視覺的定位方法和基于雷達數據的定位方法。基于視覺的全局定位方法通過把當前幀所對應的圖片與圖片數據庫中的信息進行匹配，以估計機器人當前的位姿，大致可以分為圖片到圖片、圖片到地圖的匹配方法[4]。在經典的視覺方案中，大部分都是通過將當前的圖片幀進行某種方式的編碼，然后再與數據庫的關鍵幀進行對比，從而實現場景的匹配，例如，Singh等人則采用了Gist描述子的方式對地圖進行編碼，并用之來實現場景的識別[5]。FAB-MAP[6]、RTAB-MAP[7]和ORB-SLAM3[8]所采用的則是基于詞袋模型（bag-of-words，BoW）的來實現場景的匹配。基于圖像的全局定位方法只能夠在機器人曾經采集過關鍵幀的位置附近定位成功，而且隨著數據量的增大，圖像與數據庫的匹配將越來越困難，誤匹配的概率也將隨之提高。Kendall等人首次將卷積神經網絡（convolutional neural network，CNN）應用到了視覺定位中，他們通過訓練PoseNet的方式實現了通過單張RGB圖實現了相機在一個6-DOF的空間的定位[9]。Li等人則通過RGB-D相機，采集了點線特征點，并且將這些點線特征通過深度神經網絡的形式完成場景的識別[10]。盡管基于神經網絡的方法彌補了傳統方法的許多缺點，但在局部特征表現良好的典型情況下，其定位性能仍然不如傳統方法。

在基于雷達數據的全局定位方法中，比較有代表性的方法之一是蒙特卡洛自適應算法AMCL[11]，它是一個十分經典的多峰概率估計的實現方式，在粒子足夠多的情況下，一般能夠收斂到較優的位置，然而，AMCL有明顯的缺點，例如粒子收斂速度緩慢，計算量較大。Hess等人提出了Cartographer[12]，使用了基于搜索窗口的方法將當前的雷達幀與地圖相匹配來實現全局定位，然而，Cartographer基于搜索窗的方法只有當機器人在當前幀的搜索窗口范圍內時才可全局定位成功，雖然Cartographer使用了深度優先算法和分支限界法來加速搜索的過程，但是計算量還是十分大。Wang等人提出了一種利用幾何結構信息將點云注冊到全局點云的方法實現了全局定位，通過在當前點云幀構建全等三角形，并將之在全局點云中尋找與之全等的三角形完成點云注冊[13]，然而該方法僅僅考慮了當前幀的點云局部信息，并且匹配速度較低。Liu等人提出了LPD-Net，通過使用網絡來對點云提取出一個全局描述子通過該描述子完成點云注冊的任務[14]。Kong等人提出了基于語義構建圖網絡，再通過圖網絡匹配的方式完成了點云注冊[15]。Xiang等人通過對點云數據使用結合編碼和機器學習的方法，提出了FastLCD[16]，實現了閉環檢測，得到了相對可靠和精確的結果。采用這些方法雖然在算力足夠以及特征足夠的情況下能夠取得較好的效果，但是依賴于GPU，計算量較大，在計算力較低的平臺很難有效的運用。

針對以上幾類方法存在的缺點，本文提出了一種基于全局特征點匹配的全局定位方法，其計算量小，效率高。本文的主要創新點如下：（1）提出了實時穩定的輕量級特征點提取算法，有效減少了無用特征點的數量。（2）建立了點與點之間的約束，構建了全局描述子，針對該描述子，提出了樸素匹配算法和隨機匹配算法，實現了全局特征點的同步匹配，具有較高的匹配精度和匹配速度。

1 系統框架

本文所提出的基于全局定位方法主要由三部分組成：特征點提取、特征點匹配以及全局定位結果的求解。如圖1所示，算法事先獲取了全局地圖特征點，通過實時獲取局部地圖特征點，將局部地圖特征點與全局地圖特征點通過隨機匹配算法實時匹配，再使用隨機抽樣一致算法（random sample consensus，RANSAC）根據特征點的匹配結果求解出全局位姿，完成全局定位。全局地圖特征點獲取階段發生在全局地圖的構建過程中，即讓機器人在運行SLAM算法構建全局地圖的過程中，實時運行該算法，當全局地圖構建完畢后，再進入運行全局定位算法的階段，通過運行與構建全局地圖時一致的算法，實時獲得局部地圖以及局部地圖特征點。

圖1 全局定位方法框架Fig.1 System framework of global localization mehod

2 特征點提取

為了后續特征點匹配算法的效率、準確率以及對實時性的要求，本文提出了一種輕量級穩定的特征點實時提取算法。該特征點提取算法具備以下特點：（1）在不同方位掃描的時候都可以提取出該特征點。（2）具有一定的魯棒性，能夠有效地消除噪聲影響。（3）所需算力極低。

2.1 特征點的粗提取

如圖2（a）所示，算法首先提取當前雷達幀數據H，通過當前機器人的位姿ξ映射到地圖坐標系，根據相鄰兩點間的距離對雷達幀進行切片，同時，為了保證數據的可靠性，舍棄了包含與雷達射線夾角較小的邊緣的雷達片段，因為當激光以較小的夾角打到物體平面時，數據不夠穩定。再對片段中的每個點進行打分，根據得分閾值得到待篩選的特征點，如圖2（b）所示。

圖2 特征點提取算法示意圖Fig.2 Schematic diagram of the feature point extraction

這里所采用的得分評估函數是Zhang等人在LOAM[17]中提取特征點時所采用的評估函數。該函數根據當前點的平滑程度進行打分，得分越高的點表示越不光滑。使用該函數可以粗略地提取出角點，后續算法將基于這些提取出來的粗特征點進行篩選，而不是基于原始的雷達數據，大大減小了后續的計算量。將當前雷達幀的點記作H={hk}k=1,2,…,K,hk?R2，當前雷達幀所切割出來的片段記作Hi,i=1,2,…,N，Hi?H，Hi∩Hj=?,i≠j，N為根據距離間隔切割出來的片段的數量，使用評估函數對每個點hk∈Hi進行打分得到c。

式中，W指hk當前所在的滑動窗口。將得分c大于閾值的點保存下來，記錄為待篩選的特征點。

2.2 特征點的篩選

在特征點的粗提取中得到的特征點包括直線的端點，噪音所引起的局部噪點，兩段線段相交形成的角點。在三類特征點中，兩段線段相交形成的角點僅與實際環境有關，這正是所尋求的穩定特征點，其余兩類都屬于不穩定的特征點。

通過實驗可知，由于雷達在機器人的運動過程中，對直線往往只能觀測到其一部分，這時候直線的端點會被算法識別為特征點，其產生的端點在地圖上的位置與當前雷達所處的位置有直接的關系。另一方面，由于雷達本身的數據是自帶噪聲的，當特征點提取的窗口范圍內的噪聲比較大時，算法容易將一段實際是光滑的平面識別為特征點，其產生的特征點與雷達觀測到這段平面的相對位姿和雷達自身的工作情況有關。這兩類特征點在建圖過程中都會呈現出一種極度的不穩定性，因此需要過濾掉。

為了篩選出穩定的特征點，提出了向量特征點（vector feature points，VFP）。VFP主要通過當前特征點與其附近的點共同形成一個角，通過判斷角的兩邊是否符合條件以及角度是否滿足條件，從而達到篩選的目的。篩選的效果如圖3所示。

圖3 特征點篩選示意圖Fig.3 Schematic diagram of feature point filtering

將待篩選的特征點記作pk∈P，尋找距離pk最近的特征點pl，pl∈P，l＜k，||pk-pl||＞tneigh，以及點pr,pr∈P,r＞k,||pk-pr||＞tneigh，這里tneigh為距離參數，主要是為了算法的穩定性，保證不要采集到太近的點。如果在符合要求的范圍內找不到pl或者pr時，便舍棄這個點，如圖3（a）所示。再對所有點pi,l＜i＜k,pi∈P，以及點pj,k＜j＜r,pj∈P分別計算到向量：

和向量：

的距離：

如果所有的di、dj都小于一定的值，則計算向量-L和R之間的夾角a：

當夾角a大于一定的值時，將pk保存為當前幀的特征點。當角度過小時則舍棄，如圖3（b）所示。最終當前幀所剩下的特征點如圖2（c）所示。

由于雷達掃描到的數據是離散的，不能夠保證每次掃描同一個障礙物時，所提取出來的特征點都是同一個，所以需要根據角度a保存下鄰域內最大的特征點。當雷達的掃描情況不夠理想時，所計算出來的角度a會比理想情況下的小，如圖3（c）、（d）所示。

當所有的數據處理完成之后，將當前幀的特征點加入到地圖的特征點數據庫Pmap中，再對數據庫中的點，根據特征點的夾角a進行非極大值抑制（non maximum suppression，NMS），可以保證當計算出與數據庫中已有的點近似的特征點時，只保留下一個效果最好的，如圖2（d）所示。

本文所提出的特征點提取算法運行效率非常高，時間復雜度為O(n+m2)，這里的n為一幀雷達的點數量，m為數據庫中已經有的特征點數量，同時提取的特征點非常穩定，為后續的匹配算法提供了較好的基礎。具體的特征點提取算法可以表述如下：

考慮到SLAM算法在構建較大的全局地圖時會對地圖整體做優化，導致地圖與特征點生成的時候不一致。針對這種情況，可以通過保存關鍵幀信息與軌跡的方式，根據優化后的軌跡，重新通過本算法離線生成特征點，此時，新生成的特征點將與地圖一致。

3 特征點匹配

3.1 全局特征描述子

本文通過將局部地圖中的特征點與全局地圖中的特征點進行匹配，根據兩者間的匹配關系，通過使用RANSAC算法求解出全局地圖到局部地圖的轉換關系Tξ，從而求解全局定位問題。本文提出的匹配算法基于這一事實：當機器人運行著同一個算法在不同的時刻經過同一個環境兩次時，兩次所構建的地圖應該十分相似，記局部地圖特征點為集合Plocal={plocal,i}i=1,2,…,M，全局地圖特征點為集合Pglobal={pglobal,j}j=1,2,…,N，待求解的全局位姿ξ所對應的轉換關系為Tξ。此時有關系：

基于這一關系，當Plocal、Pglobal存在：

時，記vlocal,12=plocal,i2-plocal,i1，vglobal,12=pglobal,j2-pglobal,j1則有：

基于式（8）～（10），構建了如下的描述子。首先獲取地圖特征點，如圖4（a）所示。然后對地圖特征點取pi∈P，然后計算所有dim=||pm-pi||，i≠m，以及使用式（10）計算向量pm-pi與所形成的夾角aim，角度范圍取[0,2π)，如圖4（b）所示。記地圖特征點數量為M，則每個點pi的描述子可表述為一個長度為M-1的向量desi={(dim,aim,m)}。為了加速后面對描述子的匹配，本算法對描述子desi里的元素按照dim升序進行排列，如圖4（c）所示。

圖4 描述子示意圖Fig.4 Schematic diagram of descriptor

理論上來說，每一個描述子都包含了地圖特征點的所有有用信息，但是為了算法的魯棒性，當式（7）退化成：

時，算法仍然能夠正常工作，對所有的特征點pi都采用了一樣的算法得到描述子desi。最終地圖的描述子可表示為des=[desk]k=1,2,…,M。除此之外，由于噪聲的存在，在不同時間地點采集到的特征點不可能嚴格意義上的一致，為了解決這個問題，同時提高后續匹配算法的速度，對描述子里面的夾角aim和距離dim進行了離散化處理。

式中，ares和dres分別為所允許的最大角度誤差和距離誤差，round為四舍五入函數。描述子的生成算法時間復雜度為O(n2+n2lgn)，這里n為地圖特征點的數量。具體的算法流程可以表述如下：

3.2 描述子匹配算法

每一個特征點描述子都包含了與地圖其他所有特征點的距離以及兩者形成的向量與x的夾角。因此全局特征點之間的約束和關聯體現在相應特征點的距離和角度兩個方面：一方面，由于距離并不會隨著旋轉而發生改變，因此當兩個特征點相匹配時，必須與足夠多的特征點保持著距離一致關系，同時，另一方面，理論上所有距離一致的點之間的角度差值將是相等的，這是由于該角度僅與局部地圖和全局地圖之間的相對旋轉相關，且所有差值都與所旋轉的角度相等。因此，可以通過統計所有距離一致的匹配點之間角度的差值，選取數量最多的差值所對應的匹配關系，此時即可完成兩個描述子的匹配。

特征點的數量越多，相互之間的約束會變多，從而魯棒性增強。當環境發生變化，或者噪音影響時，會出現一些原本不存在的特征點或者漏掉一些原本存在的特征點，這些將有可能導致錯誤的匹配關系。但是當因噪聲而產生的新特征點數量少于局部地圖與全局地圖特征點交集的數量時，算法依舊能夠有效消除噪音所帶來的影響，因為算法本質上是讓能夠完成距離一致約束的特征點對自動投票出一個最優結果，當最優結果所得的票數大于一定閾值時，便認為這個結果是有效的。

描述子的匹配過程如圖5所示，首先取局部地圖的任意一個描述子deslocal,i={(dlocal,im,aim,m)}，與全局地圖的任意一個描述子desglobal,j={(dglobal,jn,ain,n)}，根據式（9），首先按照距離一致的關系，給deslocal,i里的每個元素(dlocal,im,aim,m)找到desglobal,j里面匹配的元素(dglobal,jn,ajn,n)，然后通過角度差值相等關系進行角度匹配。如果相匹配元素里面角度差值為amax的元素對最多，則有：

圖5 兩個描述子匹配過程示意圖Fig.5 Matching process of two descriptors

如果角度差值為amax的元素數量超過一定的閾值，那么認為滿足這個約束的點對為匹配的點對(m,n)。

任意兩個描述子的匹配時間最優情況下復雜度為O(m+n)，最差情況下復雜度為O((m+n)n)，m、n分別為兩個描述子的元素數量，算法可以表示如下：

3.3 樸素匹配算法

由于每個地圖里面的特征點同時都與其他的特征點產生了聯系，建立了點與點之間的約束，所以當將局部地圖中的一個描述子與全局地圖中對應的描述子成功匹配后，即可得到所有特征點對之間的匹配關系。提出了一種樸素匹配算法（Naive match algorithm，NMA），實現了多個特征點與多個特征點的同步匹配，極大地提高了匹配精度和匹配速度。NMA算法的思路如圖6。

圖6 求解全局位姿示意圖Fig.6 Schematic diagram of global localization

記Plocal為局部地圖特征點集合，如圖6（a）所示；Pglobal為全局地圖特征點集合，如圖6（b）所示，記deslocal,i與desglobal,j完成匹配后所產生的匹配點對集合為matchij={(m,n)},pm∈Plocal,pn∈Pglobal，為了保證匹配算法的魯棒性，對所有的deslocal,i與desglobal,j都進行匹配，記所有的匹配結果為match={matchij}，最終選取一個匹配對數最多的結果，如圖6（c）所示。

在獲得了匹配的特征點matchmax={(m,n)},pm∈Plocal,pn∈Pglobal后，將點對代入模型：

然后通過RASAC算法求解兩個點集之間的轉換關系Tξ，將局部地圖在全局地圖里的位姿乘上機器人當前在局部地圖里面的位姿Tlocal，即可得到機器人全局定位的結果，如圖6（d）所示。

通過完成匹配后，可以得到一個比較理想的全局定位結果。當需要更高精度的定位結果時，可以將當前的定位結果當作初值，然后通過迭代最近點（iterative closest point，ICP）算法，將當前雷達幀的數據和全局地圖進行匹配，即可收斂到局部最優值。當初值比較理想的時候，此時的局部最優值便是全局最優值。

3.4 隨機匹配算法

由于每一個匹配成功的描述子理論上都包含了所有的可匹配的點對，為了降低計算耗時同時取得一個與NMA在多數情況下一致的表現，本文在NMA的基礎上又提出了隨機匹配算法（random match algorithm，RMA），用隨機的方式完成描述子之間的匹配。

NMA將所有的局部地圖特征點集合Plocal和全局地圖特征點集合Pglobal進行匹配，并在所有匹配成功的結果中選取了一個最好的結果。由于每個特征點所對應的描述子是由地圖上所有的特征點共同形成的約束所組成，每個完成匹配的描述子都會同步地完成局部地圖與全局地圖的交集中的所有特征點的匹配。當Plocal是Pglobal子集，即符合式（7）時，每個描述子所匹配的結果都將一致。實際上NMA需要窮舉所有匹配結果，正是因為局部地圖的特征點僅僅與全局地圖存在交集，而不是其子集，即當式（7）退化成式（11）時，這導致了每個描述子匹配的結果不一定一致。處于交集中的任意特征點所對應的描述子完成匹配時，其結果將是一致的，同時是最好的。因此，如果能夠選中局部地圖中處于交集中的特征點，并且將之與全局地圖的所有描述子進行匹配，那么其匹配結果將與NMA一致。RMA正是基于這一思想，采用了隨機抽取局部地圖的特征點所對應的描述子與全局地圖的所有描述子進行匹配的方法。只要抽取的次數足夠多時，便能夠保證抽取到交集中的特征點所對應的描述子。

4 實驗結果

4.1 實驗環境

本文的所有實驗都在處理器型號為i5-6300HQ 2.30 GHz，操作系統為Ubuntu 18.04的電腦上運行，算法通過C++程序語言實現，實驗基于機器人操作系統（robot operating system，ROS）進行。

4.2 特征點提取算法測試

在本節測試中，主要對特征點提取算法進行測試。測試的方式為構建一個仿真的地圖，然后生成雷達數據，同時使雷達數據帶上不同程度的噪音，用本文提出的特征點提取算法VFP，與HARRIS特征點、ORB特征點和LOAM里面所提出的特征點提取算法進行對比。主要對比這些算法在不同程度噪音下的精確率（precision）、召回率（recall）和運行耗時（ms）。本次所使用的地圖如圖7（a）所示，地圖的分辨率為0.02 m，長度為10 m，寬度為10 m。雷達的角分辨率為0.25°，掃描范圍為360°，放置在地圖x=7.5 m，y=5 m的位置。三次測試數據使用的噪音標準差分別為0、0.03和0.07。為了保證算法結果易于觀察，同時量化指標，對多種算法的識別結果均采用了NMS算法，保證每個半徑為1 m的范圍內，只保留下響應值最大的特征點。通過將識別出來的特征點距離參考特征點小于0.1 m的特征點記錄為正確識別。識別效果分別如圖7（b）、（c）、（d）和表1、表2、表3所示。可以發現，HARRIS算法的耗時遠超其他算法的耗時，而ORB算法并不適合于這種紋理較為簡單的雷達圖，導致了其在所有測試中都表現較差。隨著噪聲程度的加劇，HARRIS、LOAM算法的精確率有了較大程度的下降，同時NMS的耗時大幅度提高，而召回率卻只出現較小幅度的下降，這表明這兩個算法的抗噪能力比較弱，提取出了較多的特征點，導致NMS的耗時大幅提高，以及精確率下降。

表1 噪聲標準差為0時不同算法對比Table 1 Comparison of different algorithms when noise standard deviation is 0

表2 噪聲標準差為0.03時不同算法對比Table 2 Comparison of different algorithms when noise standard deviation is 0.03

表3 噪聲標準差為0.07時不同算法對比Table 3 Comparison of different algorithms when noise standard deviation is 0.07

圖7 在帶噪聲情況下不同算法的對比Fig.7 Comparison of different algorithms in the case of noise

在不同程度的測試中，VFP算法的表現最好，在沒有在噪音程度較小的情況下，VFP算法的精確率和召回率都能保持在較高的水平，同時總耗時極低。例如，在噪音標準差為0的時候，算法甚至達到了100%的精確率和100%的召回率，在噪音標準差為0.03時，算法準確率稍有下降，而召回率卻依然能夠保持在100%。這表明VFP算法比較穩定，抗噪能力較強，能夠為后續的匹配算法提供一個較理想的描述子；計算資源消耗較低，不會給系統本身造成較大的負擔，實時性能夠得到保障。

4.3 匹配算法的計算性能測試

在本節測試中，主要是對匹配算法的計算性能進行測試。測試的方式為用程序隨機生成足夠多的地圖特征點，模擬全局定位的真實情況，統計算法的實時計算耗時。對全局地圖特征點數量50、100、200和300個點分別進行了局部地圖特征點數量從0到49、99、199和299個的樸素匹配算法和隨機匹配算法的耗時統計，隨機匹配算法參數為，結果如圖8所示。可以看出，在當前參數下，在任何規模的數據量下隨機匹配算法的時間消耗都少于樸素匹配算法。并且，隨著數據規模的增大，隨機匹配算法的運行速度優勢越來越明顯，同時兩者在大于95%的情況下，兩者取得的結果是一致的。雖然樸素匹配算法能夠嚴格意義上的找到全局最優匹配，但是與隨機匹配的算法的性能差距較大，并且在實際運行過程中，算法以非常高頻的運行，使用隨機匹配算法在絕大多數的情況下能夠實時取得一個理想的結果。

圖8 匹配算法消耗的時間Fig.8 Time cost of matching algorithm

可以觀察到，不管是樸素匹配算法還是隨機匹配算法隨著局部特征點數量的增加計算的耗時都呈現出增加的趨勢。對樸素匹配算法而言，這主要是由于匹配次數和單次匹配所需的時間的增加；對隨機匹配算法而言，匹配的次數會保持穩定，然而單次匹配所需要的時間會大體上呈現出線性增長。

4.4 在不同場景下的定位效果

在本節測試中，主要對算法在不同場景下的定位效果進行測評。測試的方式為通過在仿真軟件GAZEBO中搭建一個虛擬的世界，并且運行本算法。本次測試的所使用的場景為長25 m、寬25 m，所使用的雷達角分辨率為0.25°，掃描范圍為180°。測試場景主要有靜態環境、發生了一定變化的動態環境以及具有相似子結構的場景。

靜態場景下的測試結果如圖9所示，在機器人自轉一周之后，采集到了足夠多的特征點時，算法完成了匹配。這是由于在這個場景中，機器人剛好處于一個比較理想的位置，且實時運行的時候場景與建圖時完全一致，當采集到的特征點數量滿足所設置的閾值時，算法便能完成全局定位任務。

圖9 靜態環境下的全局定位效果Fig.9 Global localization in static environment

動態場景下的測試結果如圖10所示。通過在運行全局定位算法之前，把多個原本不存在的物體擺放在機器人的啟動位置附近，以及把機器人擺放在一個比較差的初始位置，此時便不能夠像靜態場景中一樣理想，快速完成全局定位任務。由于機器人啟動位置附近，干擾物較多，同時特征點數量較少，機器人必須移動，直到采集到足夠多的特征點。當機器人采集到足夠多的特征點時，算法同樣能夠完成全局定位任務，如圖10（b）所示。

圖10 動態環境下的全局定位效果Fig.10 Global localization in dynamic environment

相似環境下的測試結果如圖11所示。由于場景本身有比較多近似的局部區域，如果設定的匹配點閾值較低時，算法容易產出誤匹配，如圖11（a）、（b）所示。但是隨著機器人的運行，采集到的特征點越來越多，它們之間所形成的約束更加復雜時，算法便能夠有效過濾掉局部相似的所帶來的影響，成功實現全局定位，如圖11（c）、（d）所示。

圖11 相似環境下的全局定位效果Fig.11 Global localization in similar environment

4.5 定位效果對比實驗

本節主要是為對算法在實際場景下的全局定位效果的測試。測試的方式為，在Deutsches Museum Dataset[12]和Mit Stata Center Dataset[18]上分別運行了RMA、AMCL和Cartographer三個算法。測試的具體方法為，首先針對數據集，通過Cartographer構建全局地圖，然后將這個全局地圖用于全局定位。實驗分別在同個場景、不同時間所采集的數據集上選取了4個不同的時間、地點作為全局定位算法的啟動位置，然后通過統計成功實現全局定位時機器人從上電到當前的時間，以及每一幀的平均耗時和定位成功時的精度。由于這兩個數據集的場景都比較大，為了盡可能地定位成功，將AMCL的粒子數量設置為20 000；將Cartographer的搜索層數設定為7層，搜索窗邊長為7 m，搜索角度設置為30°。

全局定位所需要的運行時間如圖12所示，可以看出，在Deutsches Museum Dataset上，本文提出的算法所需要機器人運行時間明顯更少，而AMCL在地點3甚至無法在規定的時間（180 s）內完成全局定位。在Mit Stata Center Dataset數據集上，可以看出本文的算法都能夠在規定的時間內完成全局定位算法，與AMCL算法對比，機器人運行時間相近，而Cartographer則無法在規定的時間內在地點2、3和4完成全局定位。

圖12 全局定位所需的運行時間Fig.12 Runtime required for global localization

平均每幀耗時和定位精度如表4、表5所示，可以看出RMA算法平均每幀運行所需要的時間遠低于AMCL和Cartographer算法。在定位精度上，三者的精度較為接近，此時的主要誤差來源于雷達本身數據的噪音和柵格地圖分辨率的限制。算法的全局定位效果如圖13所示，可以看出，本文所提出的算法可以很好地完成全局定位。

表4 在Deutsches Museum Dataset上不同算法對比Table 4 Comparison of different algorithms on Deutsches Museum Dataset

表5 在Mit Stata Center Dataset上不同算法對比Table 5 Comparison of different algorithms on Mit Stata Center Dataset

圖13 全局定位的實際運算效果Fig.13 Global localization in real world

5 結論

本文提出了一種基于全局特征點匹配的移動機器人全局定位算法。一方面設計了一種高效穩定的特征點提取算法，另一方面建立了全局所有特征點之間的相互關聯和約束，并且基于這些關聯和約束，采用高效的隨機匹配算法，同步完成了多個特征點對的匹配，大大提高了算法的穩定性。在運算性能方面，算法總體維持在了一個較低的復雜度和運算時間，在效果上，與AMCL和Cartographer算法對比，全局定位的效果在多數情況下更優。