融合圖特征的多機器人柵格地圖拼接方法

黃小杭，曾 碧，劉建圻，汪明慧

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

對未知環境構建地圖是移動機器人技術中的一項基本挑戰，而地圖的構建通常需要對機器人位姿的精確估計，因此目前移動機器人主要通過同步定位與建圖（simultaneous localization and mapping，SLAM）技術[1-2]構建環境地圖，目前已有多種較為成熟的單機器人SLAM算法[3-5]，而在大規模未知環境中，單機器人的地圖構建精度、效率和魯棒性都存在限制，在過去的十年中，多機器人協作已經成為了目前研究的熱點，多機器人的引入有助于突破上述單機器人SLAM算法存在的限制。在機器人SLAM算法中，其地圖類型可分為特征地圖、拓撲地圖和柵格地圖[1-2]，其中柵格地圖可直觀地表達真實環境的結構，存儲方便，并在精確定位與導航方面更具優勢。

在多機器人SLAM中，各機器人在構建各自的局部地圖之后，如何拼接局部地圖并確定各機器人之間的相對位姿是個熱點問題。

Konolige等人指出，機器人地圖拼接問題是一個有意義且具有挑戰性的問題，但是它并沒有像SLAM等其他機器人問題一樣受到關注[6]。

其中基于優化和特征匹配的方法是較為主流的思路。Carpin和Brik將地圖匹配問題建模為優化問題，然后通過隨機搜索算法進行求解[7-8]。在他們的后續工作中通過使用刪除異常點的機制和更復雜的方法來優化搜索來提高先前工作的性能[9]。Carpin提出了一種使用Hough變換對搜索空間建模的方法，并將其分解為平移和旋轉估計，以合并多機器人系統中的占據柵格地圖[10]，該方法拼接速度快，但由于Hough變換存在參數離散化現象，且匹配效果容易受到地圖噪聲影響，魯棒性較差。

對于基于特征匹配的拼接方法，其中利用SIFT（scale-invariant feature transform）[11-13]、SURF（speeded up robust features）[14]、PCA-SIFT（principal component analysis-based SIFT）[15]、ORB（oriented fast and rotated brief）[16]等圖像特征，將地圖視為圖像，此時柵格地圖拼接可視為圖像配準的一種特殊情況。由于室內環境的自相似性，與一般的圖像相比，柵格地圖提供的特征較少，僅適用于重疊率較大的地圖，這些方法容易受到局部最小值的影響，尤其是在沒有準確的初值估計的情況下。

地圖拼接問題目前主要面臨以下挑戰：

（1）柵格地圖的拼接需要較大的重疊區域。

（2）由于SLAM算法本身存在累積誤差和觀測噪聲，因此構建的地圖往往存在一定的非剛性形變。

（3）需要考慮地圖拼接的處理時間，尤其是對于大型地圖，其計算會非常密集，因此會阻礙某些同時運行的具有實時需求的程序，例如機器人定位和導航程序。

針對上述問題，本文提出通過融合圖特征的柵格地圖拼接方法，所提出的方法基于ORB特征點，通過建立關特征點之間的中值K近鄰圖來表征局部關系，建立最優傳輸模型并求解最優特征點匹配，實現柵格地圖魯棒的配準和拼接。本文方法具備較高的拼接精度，可拼接重疊度較低的地圖，且具備一定抗非剛性形變干擾的能力，在本文實驗中，本文方法也展現出了較快的計算速度，尤其是在面對大規模場景。

1 柵格地圖拼接的問題描述

移動機器人創建的柵格地圖，通過將環境劃分為等分辨率的平面柵格，這些柵格通過矩陣形式存儲于計算機中，可視為圖像。假設系統中存在兩臺機器人A和B，其構建的柵格地圖表示為圖像IA和IB，其中對于所有像素pij∈I都存在三種可能的狀態：占據、空閑和未知。其中未知狀態主要受機器人自身觀測影響而非真實世界的描述，因此本文將空閑和未知的狀態均歸為“其他”狀態，此時待處理的柵格地圖可二值化為兩個狀態：占據pij=1或其他pij=0，定義兩臺機器人的柵格地圖點集數據為：

且兩者存在重疊區域即A?B≠?，柵格地圖拼接問題即求解A到B的剛體變換T={R,t}。其中R∈SO(2)為旋轉矩陣，t∈?2為平移向量：

經過變換后點集A與點集B的重疊部分配準，即可進一步將柵格地圖拼接表示為如下的最小化問題：

2 融合圖特征的柵格地圖拼接方法

2.1 ORB特征提取與粗匹配

本文采用ORB[17]特征。ORB是一種FAST特征點檢測和BRIEF特征描述符融合的圖像特征提取算法，其具有較高精度，較小的計算量，以及良好的旋轉不變性和尺度不變性。

ORB特征提取和粗匹配步驟如下：

步驟1利用高斯濾波器對待匹配的柵格地圖進行適當的模糊，使二值圖像的邊緣產生連續平滑的梯度，本文使用尺寸為3×3，標準差σ=1的高斯卷積核。

步驟2提取兩張地圖圖像中具有方向信息的多尺度FAST特征點并采用非極大抑制算法去除Harris響應低的FAST特征點，得。

步驟3以FAST特征點的方向為基準，提取BRIEF特征描述子。

步驟4利用FLANN（fast library for approximate nearest neighbors）庫[18]的多探針局部敏感哈希（localitysensetive Hashing，LSH）算法快速搜索兩個特征點集中距離最近的匹配點對，同時引入Lowe[11]所提出比率測試方法，剔除不合理的特征點，得到匹配點對，。

經過ORB特征的提取與粗匹配，提前剔除較為明顯的誤匹配點，降低后續算法的計算規模。

2.2 中值K近鄰圖的構建

為了度量粗匹配結果的幾何關系合理程度，用于優化匹配結果，本文受到Aguilar等人[19]提出的圖變換匹配（graph transformation matching，GTM）的啟發，通過對每張地圖中粗匹配后的特征點構建中值K近鄰圖，即每個特征點和其滿足中值約束條件的最近的K個特征點建立近鄰圖的邊，本文以點集構建中值K近鄰圖Gp=(Vp,Ep)為例，首先定義一個頂點vi對應于每個點pi，即Vp={v1,v2,…,vnm}，無向邊(vi,vj)連接著與pi距離最近的K個鄰居pj，并且‖pi-pj‖≤η，其中，η為所有頂點對之間的距離的中間數，即：

K最近鄰條件可表達其局部幾何結構，而中值約束主要在于過濾由于偏遠點引起的結構變形。對待拼接的兩張地圖特征點分別構建中值K近鄰圖Gp=(Vp,Ep)和Gq=(Vq,Eq)，本文K值取10。

2.3 最優傳輸模型優化對應關系

2.3.1 最優傳輸模型構建

為了融合特征描述子和中值K近鄰圖的特征，優化特征點間的對應關系，和傳統特征點匹配方法不同，本文將ORB特征點之間的匹配問題轉換為Gp和Gq的圖匹配問題，由此可將該圖匹配問題轉換為指派問題，假設通過粗匹配后待匹配點集為，經過粗匹配后np=nq,為不失一般性，該指派問題可松弛為最優傳輸問題[20]并表達為求一個軟匹配矩陣P,即可建立最優傳輸模型：

其中，μp和υq表示特征點的總質量，即特征點的權重之和。傳輸代價矩陣為特征點pi到qc(j)的傳輸成本，在本文中，特征點權重相等，每個特征點質量可設為1，即：

式（5）中對集合的線性約束確保Vp傳遞到Vq的質量保持一致，即從集合Vp轉移質量到Vq后，系統的總質量保持不變，這就要求此優化問題中Vp和Vq必須具有相同質量，并且每個節點始終以一個質量單位傳輸，這種情況需要保證最終匹配結果中節點數量np=nq，在實踐中可知，由于粗匹配結果中存在無法正確匹配的點對（即離群值），因此傳輸的質量不一定保持一致。為了移除離群值，同時滿足最優傳輸問題的質量約束，本文引入兩個增廣節點gp∈Vp和gq∈Vq，用于放置離群值的質量。定義α為離群值權重，用于估計上述最優傳輸模型中的離群值。增廣節點的引入使得式（5）增加了如下約束：

其中：

式（8）的參數ω只與離群值的數量有關，可控制離群值篩選的嚴格程度。為了在求解上述最優傳輸問題的過程中將離群值吸引至增廣節點gp和gq，需要在代價矩陣C中增加關于增廣節點傳輸代價，本文選用C中行或者列的最小值作為其他節點到增廣節點的傳輸代價。在求解該最優傳輸問題后，所有與增廣節點匹配的節點可以認為是離群值，通過從P中刪除與增廣節點有關的行和列來獲得最終的對應關系，示意圖如圖1所示。

圖1 增廣節點去除離群值示意圖Fig.1 Using additional nodes to remove outliers

2.3.2 最優傳輸代價矩陣構建

式（5）中的傳輸代價矩陣C主要由兩個部分組成：由Vp和Vq的ORB特征描述子之間的歸一化漢明距離構成的節點到節點的相異性矩陣S和表示圖Gp和Gq結構相異性矩陣R組成，將代價映射至[0,1)范圍內傳輸代價矩陣可定義為：

其中對于節點結構相異性矩陣R構建，本文對待匹配柵格地圖做如下假設：待匹配柵格地圖占據點之間的關系約束是保持一致的，因此第一張柵格地圖特征點的相鄰點對應于第二張地圖的特征點的相鄰點，若所有對應關系正確，該對節點的鄰接結構應保持一致，非正確對應的匹配點將導致兩圖增加節點間結構相異性。根據這個假設，本文通過計算節點結構相異性矩陣Rij=來度量中值K近鄰圖的結構相似度，其中Ap和Aq分別為Gp和Gq的鄰接矩陣。

2.3.3 最優傳輸模型求解

待匹配特征點的數量大約在100～2 000這個數量級，若要求式（5）精確解，可利用網絡流求解器求解，然而其時間復雜度為O(n3)（其中n與np和nq成正比），時間復雜度較高，因此本文Sinkhorn算法[21]近似求解該最優傳輸問題，其時間復雜度為O((np+1)×(nq+1))。對式（5）熵正則化：

其中：

步驟1初始化；

步驟2計算；

步驟3計算P=diag(a)Kdiag(b)并代入式（10）計算Sinkhorn距離ds；

步驟4重復步驟2和步驟3，直到兩次迭代的ds之差小于終止條件閾值eps，解得。

2.4 計算拼接結果

2.4.1 變換估計

利用上述方法給出的特征點的對應關系，通過最小二乘法求解式（3）可估計柵格地圖IA到柵格地圖IB像素之間的剛體變換{R,t}，為了實現較為魯棒的變換估計，采用隨機采樣一致性（random sample consensus，RANSAC）算法估計剛體變換參數。相對于傳統基于特征點匹配的柵格地圖拼接算法而言，本文的特征點匹配關系已經過優化，RANSAC算法僅需較少的迭代次數即可估計精確的剛體變換參數。

2.4.2 地圖拼接

創建空白柵格地圖IW，柵格地圖IA變換為柵格地圖T(IA)，其對應像素為：

柵格地圖T(IA)和IB均置于空白柵格地圖IW，本文采用文獻[13]中的融合函數融合對應像素：

其中，x、y分別為柵格地圖均為柵格地圖的像素值。拼接結果為：

2.5 融合圖特征的柵格地圖拼接實現

融合圖特征的柵格地圖拼接方法的具體實現步驟如下：

步驟1輸入兩張柵格地圖圖像IA和IB，提取ORB特征并粗匹配。

步驟2構建每張柵格地圖特征點的中值K近鄰圖。首先建立特征點的K維樹（KD-Tree），查找每個特征點最近的K個鄰居特征點，然后在鄰居中篩選出滿足中值約束的鄰居特征點建立無向邊，構建中值K近鄰圖鄰接矩陣Ap和Aq。

步驟3構建最優傳輸代價矩陣C。分別計算節點相異性矩陣R和圖結構相異性矩陣S,代入式（9）計算最優傳輸代價矩陣C,將C中行或者列的最小值作為其他節點到增廣節點的傳輸代價。設置最優傳輸權重向量μp和υq,其中特征點節點的權重為均為1，增廣節點的權重由式（8）給出。

步驟4將節點權重μp和υq,代價矩陣C和熵正則化系數λ和終止條件閾值eps代入Sinkhorn-Knopp算法中迭代求解軟匹配矩陣P,取P每行（或列）最大值的行列索引為節點匹配結果，去除增廣節點所匹配的節點后，得到優化后的特征點之間的對應關系。

步驟5將特征點對應關系代入式（3）并通過RANSAC算法估計剛體變換參數{R,t}，創建空白柵格地圖IW，地圖IA通過式（12）變換后，再通過式（13）所述的融合函數進行像素融合，最后得到拼接結果IW。

3 實驗結果

3.1 實驗平臺、數據集與評估指標

本文方法在一臺主頻2.5 GHz四核CPU，內存8 GB的筆記本電腦上完成所有實驗程序的運行。為了驗證本文方法的可行性和性能，并分析所述參數對算法性能的影響，本文主要在TUMindoor數據集[22]和多機器人采集的柵格地圖數據集進行實驗。

其中TUMindoor數據集采用精度為0.1 m的柵格地圖經過人工按一定重疊率裁切，而本文采集的數據集采用圖2所示的兩臺搭載2D激光雷達的微型輪式機器人分別通過Gmapping算法建立柵格地圖，所用數據的具體參數如表1，其中T-前綴為TUMindoor數據集，R-前綴為通過多機器人采集的柵格地圖數據集。

這里對本文所提出的算法從以下三個方面進行評估：

（1）針對特征點匹配結果，評估算法得出的匹配數，該匹配數由RANSAC算法估計，匹配數可在一定程度上評估特征匹配算法的魯棒性：當匹配數過低時，經過RANSAC算法估計符合剛體變換規律的特征點較少，故魯棒性較低；當匹配數足夠多時，魯棒性會相對較好，但過多的匹配數往往會引入假匹配點對和額外的計算量，影響配準精度和運行耗時。可結合后續指標進一步評估。

（3）針對算法實時性的評估，采用算法運行耗時對比的方式評估。

3.2 分析實驗

本文方法的匹配數主要受離群值權重ω和正則化系數λ影響，在R-ESAC509數據集中，其參數取值與匹配數的關系如圖3所示。

圖3 參數ω 和λ 對匹配數的影響Fig.3 Effect of parameters ω and λ on number of matches

可以看出，離群值權重ω取值越大，算法對離群值過濾越嚴格，若該值過大會導致將正確的匹配歸入離群值。熵正則化系數λ越大，算法會盡可能地傾向均勻分配，而該值越小則越傾向于尋找代價最小的匹配，本文的代價矩陣中增廣節點的匹配代價最小，因此該值越小過濾離群值的效果越強。在后續實驗中，本文選用參數ω=0.02,λ=5。

Sinkhorn-Knopp算法迭代過程中各數據集的Sinkhorn距離收斂曲線如圖4。Sinkhorn距離可在4～6輪迭代實現收斂，在工程實現上，直接設定合適的迭代次數而免去Sinkhorn距離的計算可降低迭代計算量。

圖4 各數據集的Sinkhorn距離收斂曲線Fig.4 Sinkhorn distance convergence curves for each dataset

3.3 對比實驗

本文方法將分別與基于SIFT方法和基于ORB方法進行對比，其中魯棒配準算法均采用RANSAC方法。對比實驗數據如表2。圖5為地圖拼接實驗對比，圖6為特征匹配結果對比柵格地圖。

對于匹配數，基于ORB的拼接方法擁有最高的匹配數，但引入了較多冗余匹配，增加了一定額外計算量。本文算法由于剔除了誤差較大的離群值、優化了特征點之間的對應關系，整體匹配數略少，但相對于基于SIFT的匹配方法匹配數量仍然可觀，本文算法同時也具有較高的匹配魯棒性。

對于角度誤差和平移誤差，本文算法在TUMindoor數據集和多機器人建圖數據集均表現出較高的拼接精度和魯棒性，而基于SIFT的拼接方法在重疊率較低且重疊部分紋理不明顯的R-ESAC509中出現配準失誤，而基于ORB的拼接方法在數據集R-ESAC507和R-ESAC509中也出現較明顯的角度誤差。

對于運行耗時，本文算法的平均耗時最短，基于ORB的拼接方法耗時與本文接近，基于SIFT的拼接方法平均耗時最長。盡管在本文算法中最優傳輸模型的求解部分引入了較多的計算量，但由于該部分如Sinkhorn-Knopp算法主要以矩陣運算為主，且迭代收斂速度快，利用基礎線性代數子程序庫（basic linear algebra subprograms，BLAS）或GPU并行計算可較為容易地有效提升相關運算速度。在本文實驗中僅使用了CPU和BLAS庫。

表2 對比實驗結果Table 2 Results of comparison experiment

圖5 柵格地圖拼接結果對比Fig.5 Comparison of results of grid map stitching

圖6 特征匹配結果對比Fig.6 Comparison of feature matching results

4 結論

針對基于特征匹配的柵格地圖拼接方法面臨的地圖特征較少并存在自相似性，依賴柵格地圖較大重疊區域、柵格地圖存在非剛性形變等問題，本文提出的融合圖特征的柵格地圖拼接方法，所提出的方法基于ORB特征點，通過建立關特征點之間的中值K近鄰圖來表征局部關系，并通過最優傳輸模型求解最優特征點匹配，實現柵格地圖的配準和拼接。本文方法具備較高的拼接精度，可拼接重疊度較低的地圖，且具備一定抗非剛性形變干擾的能力，在本文實驗中，充分分析了相關參數對實驗結果的影響，同時本文方法分別與基于SIFT的拼接方法和基于ORB的拼接方法進行了對比，本文方法展現出了較快的計算速度和較高的精度和魯棒性。