MAPSAC與EKF結合的單目相機魯棒定位估計算法與實驗驗證

孫 浩，馮孝周，張見升，王艷艷，高俊釵

(1.西安工業大學，陜西 西安 710065；2.中國兵器工業試驗測試研究院，陜西 華陰 714200)

0 引 言

路標與圖像中成像特征點的正確數據關聯關系到單目相機定位估計的準確性，是單目相機定位方法的關鍵[1，2].由于單目相機獲取環境信息的平滑性、重復紋理的存在以及圖像處理引入的噪聲等，已有的數據關聯方法ICNN[3-5](Individual Compatibility Nearest Neighbour，ICNN)對環境路標和成像特征進行數據關聯時會存在模糊關聯的情況，即一個路標可能關聯多個成像特征，或者一個成像特征可能與多個路標關聯，從而引起錯誤的數據關聯.對含有錯誤關聯的數據直接采用最小二乘法或者卡爾曼濾波方法估計單目相機的定位，會導致定位估計的不準確性，所以定位估計時需要剔除錯誤關聯的數據.RANSAC(RANdom SAmple Consensus，RANSAC)[6-8]是一種考慮了剔除錯誤數據關聯的統計模型魯棒估計方法，它采用隨機抽樣尋找最大一致集的方法剔除錯誤關聯數據(外點)，并同時可以利用正確的關聯數據(內點)對單目相機進行較準確的定位估計.但該方法有一個假設，即在所有的關聯數據定位估計模型中，正確關聯數據定位估計的是主要模型.在自然環境中，錯誤關聯數據是隨機的，其正確定位估計有時不會占主要模型，從而不滿足這個假設.而且RANSAC數據關聯方法對內點不再進行區分，使得單目相機定位估計的準確度受到影響，并且其定位模型估計樣本需要的數據對較多，估計效率不高.

針對單目相機定位過程中，路標與圖像中成像特征點的數據關聯存在錯誤關聯的問題，在ICNN數據關聯基礎上，提出了MAPSAC(Maximum A Posteriori SAmple Consensus，MAPSAC)[9]與EKF(Extended Kalman Filter，EKF)[10，11]結合的路標與圖像中成像特征點錯誤關聯數據剔除的單目相機定位魯棒估計方法，給出的MAPSAC損失函數不僅可靠地剔除了錯誤關聯的數據，而且對數據關聯的內點進行了有效的區分；通過MAPSAC的最小化損失函數，可以實現單目相機定位的魯棒估計，增強算法的環境適應性.結合單目相機狀態的EKF更新方程，提高了數據關聯的效率，通過仿真和實驗分析，該方法可以實現相機魯棒定位估計.

1 基于RANSAC的單目相機定位魯棒估計分析

根據RANSAC單目相機定位魯棒估計算法的原理，對其選擇正確樣本的計算復雜度較高以及損失函數對內點不做區分的問題進行了分析.

1.1 基于RANSAC的單目相機定位魯棒估計原理

在單目相機EKF 定位過程中，RANSAC的數據關聯算法通過反復選擇路標與成像特征點數據中的一組隨機樣本，得到主要的單目相機定位模型.首先，隨機選取最小的一個樣本假設為單目相機定位模型的內點集，基于直接線性方法確定單目相機定位模型；然后對其它關聯數據用得到的單目相機定位模型去檢驗決策，如果符合估計的單目相機定位模型或具有較小的距離誤差，則判斷為該定位模型一致集的內點，并用所有一致集的內點進行單目相機定位模型估計.如果內點越多，估計的單目相機定位模型就越穩定；最后根據設定的迭代次數重復執行上述過程，統計單目相機定位模型所含的內點數量對定位模型進行選擇，舍棄內點太少的單目相機定位模型，得到含有最大一致集內點的單目相機定位模型.

1.2 RANSAC的定位估計樣本選擇分析

對于單目相機定位算法中路標與成像特征點關聯的數據，由于單目相機的射影成像，三維路標-二維特征成像點數據關聯滿足單目的射影成像關系，成像關系的內參數是不變的，單目相機定位外參數自由度為6，包含3個平移和3個旋轉參數.由PnP[12，13]的n點定位原理可知：至少需要6對正確的數據關聯點作為一個樣本，可唯一地確定單目相機定位模型.為了估計的方便性，可以直接建立定位矩陣模型是3×4的矩陣，而不再需要估計具體的外參數.一個樣本至少需要 6對點可唯一確定定位矩陣模型.

當路標與特征點關聯存在錯誤關聯時，在內點比例一定的情況下，獲取一個定位矩陣模型估計的正確觀測樣本概率與觀測樣本的大小是非線性關系.假設路標與特征點關聯的內點比例w=0.45，獲取一個好的路標與特征點關聯的觀測樣本的概率z=0.98的情況下，當觀測樣本大小為n時，抽樣到一個正確的路標與特征點關聯的觀測樣本所需的次數[14]

(1)

由式(1)可知，當觀測樣本J所需路標與特征點數據關聯個數n增多時，抽到一個正確的路標與特征點關聯的觀測樣本的次數K急劇增加，它們之間具體的對應數值如表 1 所示.

從表 1 可以看出，6對路標與特征點作為一個樣本抽樣到一個好觀測樣本的次數約是1對點時的78倍.所以，需要減少抽樣次數，提高獲得一個好觀測樣本的效率，從而加快數據關聯的效率.

表 1 樣本大小與抽樣正確樣本次數的關系

1.3 RANSAC的損失函數分析

設測量誤差為ei；測量標準差為σ；被接受為內點的距離閾值為T，RANSAC算法的最大一致集實際上是對式(2)損失函數[15]的最小化

(2)

式中：

(3)

RANSAC算法距離閾值T的確定可以根據測量誤差的先驗分布或者根據先驗經驗選取，所以，RANSAC算法的一個不足之處是損失函數中對路標與特征點數據關聯內點的處理方法，如果距離閾值T設置太高，此時內點之間將沒有什么區別，而對外點的懲罰是一個常數，從而射影成像關聯矩陣模型估計的性能將會變差[16].因此，距離閾值T越高，有相同C的估計性能越差.例如，若T足夠大，則損失將會相同，因為每個數據關聯對都將是內點，所以RANSAC的一個主要缺點是由于對路標與特征點數據關聯的內點成因不明地用了均勻分布建模，所以對合適的距離閾值有較高的要求.而合適距離閾值的選取是一個比較困難的問題，所以為了降低距離閾值選擇的難度，需要對內點集進行評價使用，有效提高定位估計的準確性.同時在自然環境中，錯誤關聯數據是隨機的，其正確定位估計有時不會占主要模型，RANSAC數據關聯確定的最大一致集確定的參數模型可能不是正確的相機定位.

2 MAPSAC與EKF結合的單目相機定位魯棒估計算法設計

針對RANSAC單目相機定位魯棒估計算法的計算復雜度高、損失函數中距離閾值T確定以及環境適應性等問題，結合EKF更新選擇單目相機的初始定位，提高了選擇正確初始定位的概率，降低了計算復雜度.給出的MAPSAC損失函數確定定位估計的最小損失一致集，有效地衡量了內點定位的質量[17，18].

2.1 基于EKF更新的單目相機的初始定位估計模型

RANSAC在選用樣本進行射影成像的初始定位估計時，參數估計的一個樣本需要6對關聯數據，得到一個正確樣本所需的抽樣次數較高.為了降低該抽樣次數，結合單目相機的運動方程進行更新，采用一對關聯數據的EKF更新確定初始定位，一對關聯數據是指樣本中s=1，就可以實現定位模型的擬合，大大降低了正確定位模型的抽樣次數.該算法充分利用了EKF更新階段得到的單目相機的定位狀態，從路標與成像特征中選取1對關聯數據，即可確定單目相機的初始定位模型[19].

假設經過數據匹配的路標集為Mi，相應的特征測量數據集為zIC={z1，z2，…，zi,……,zn}，組成的關聯數據集為X={M，zIC}.在單目相機定位過程中，根據慣導測量相機的狀態，設其運動方程[10]為

rk=f(rk-1，uk-1)+wk-1.

(4)

單目相機測量方程為

(5)

(6)

2.2 基于MAPSAC損失函數最小化一致集

在得到射影成像的初始定位估計模型后，就可以按照此模型確定其一致集.當關聯數據與此模型的距離誤差小于距離閾值時，則關聯數據為該定位估計模型的內點集zinliers，否則為外點集.

針對RANSAC算法對內點不做區分，距離閾值根據先驗知識確定的問題，結合后驗概率模型，Torr等人隨后提出了MAPSAC模型估計算法[9]，該算法使用最大化后驗對數確定距離閾值

(7)

式中：ξ為內點的先驗期望；σ為測量數據的標準差；ν為測量數據有界區域的體積；D為測量數據的維數；d為定位模型的維數.

在單目相機定位估計中，在給定定位估計模型參數Θ的情況下，對數據關聯測量坐標分量X邊緣化的同時，優化對內點與外點距離閾值Tmap的選擇，可以通過式(8)最小化來完成

(8)

式中：

(9)

從式(9)可以看出，每個外點的損失函數相同，而對內點卻根據他們擬合模型的好壞程度加以區別評價，這樣降低了選擇距離閾值的準確性，并且不會增加額外的計算.

2.3 MAPSAC與EKF結合的魯棒定位估計算法

2) 從關聯數據點集X中隨機地抽取一對相容性關聯數據J，根據式(6)的EKF更新估計初始定位參數ΘJ；

3) 選擇與定位參數ΘJ幾何距離小于距離閾值Tmap的相容性關聯數據，用S(ΘJ)表示，為該定位參數ΘJ的一致集；

4) 當定位參數一致集S(ΘJ)中關聯數據的數量#S(ΘJ)大于數量閾值th2時，根據式(5)，用一致集進行EKF更新，得到定位參數ΘJ，并輸出關聯數據一致集及相應的定位參數；當S(ΘJ)中關聯數據的數量小于數量閾值th2，回到 2)；

通過以上步驟，則得到了一個最大一致集，即是單目相機定位估計所需要的正確的數據關聯對，其它的相容性對應點都是外點集.濾除了誤關聯數據，同時獲得了單目相機的定位估計.

3 數值仿真結果與分析

為了對提出的MAPSAC數據關聯算法進行有效性的驗證，采用直觀的曲線模型擬合進行仿真驗證.46個真實二維點分布如圖 1 所示.

圖 1 模擬曲線的二維數據

以圖 1 的點作為真實點，并給每個真實點增加2個模擬測量數據，總共構成92個均值為[0,0]的模擬測量數據，并產生20個均值為[2,2]的高斯分布噪聲，它們的協方差分別為

(10)

20個高斯分布噪聲數據作為錯誤數據，用實心圓表示，如圖 2 所示.

圖 2 誤差數據和錯誤數據分布

采用RANSAC進行數據關聯，設置最大迭代次數為100，通過迭代，找到符合擬合模型的內點數據，如圖3(a)所示的“⊕”，疊加在原始數據上.同樣條件下采用MAPSAC進行數據關聯，通過迭代，找到符合擬合模型的內點數據，如圖 3(b)所示的“⊕”，疊加在原始數據上.數據關聯過程中擬合曲線數據對比如表 2 所示.

通過圖 3 可知，RANSAC與MAPSAC數據關聯方法找到的關聯數據相近，都比較正確地找出了散點的聚集函數，說明兩種方法數據關聯結果差別很小，都是有效的數據關聯方法.

(a) RANSAC數據關聯的內點結果

通過表 2 RANSAC與MAPSAC擬合曲線數據對比可知，兩種方法找到的數據關聯點有一點兒區別，擬合的曲線方程很相近，但MAPSAC的擬合誤差比RANSAC小很多，說明MAPSAC數據關聯模型估計方法的準確性.

表 2 RANSAC與MAPSAC擬合曲線數據

4 實驗驗證及精度分析

通過算法研究，利用該算法編制了專用數據處理軟件，并結合相關實驗進行了測量精度的對比驗證.實驗中采用3塊木板搭建模擬飛散破片場的穿孔位置和大小，在地面用標志點模擬被試品所在位置及其方向，現場布置如圖 4 所示.

圖 4 試驗現場布置

利用相機在不同位置進行成像，獲取具有重疊區域的序列圖像，采用本文所設計的算法，對相機成像時的位姿進行定位估計，結合匹配的破片穿孔位置進行三維重建，測量計算模擬破片穿孔位置.經過破片穿孔位置計算軟件模塊處理與計算，得到的模擬破片穿孔位置坐標如表 3 所示，并與用精度為0.03 mm的掃描儀得到的模擬破片穿孔位置坐標進行對比.

模擬破片穿孔數量為20個，通過表 3 可以看出，檢測出模擬破片穿孔數量20個，破片穿孔坐標誤差最大為0.86 mm，滿足單個破片穿孔位置測量精度，說明設計的相機定位算法精度較高.通過實驗驗證，系統測量精度滿足項目的技術指標要求.

表 3 模擬破片穿孔坐標

5 結 論

本文針對單目相機定位中的含錯誤數據關聯的定位估計問題進行了研究.在含有錯誤數據關聯的情況下，結合單目相機運動方程的預測和更新，提出了MAPSAC與EKF結合的魯棒定位估計算法，提高了定位估計的效率，給出了MAPSAC損失函數，有效提高了定位模型估計的準確性以及環境適應性.通過曲線模型數據關聯仿真，說明所設計的定位估計算法可靠的抑制了噪聲數據的影響，為單目相機定位估計提供了可靠的保障.結合該算法，編制了相關應用軟件，在室外構建了模擬對象，對基于該算法的定位測量精度進行了對比驗證，取得了良好的測試效果.后期將結合具體的項目需求，對算法模型和開發軟件的實用性進行不斷改進，進一步提高測試精度，并加強該技術的推廣應用.