MAPSAC與EKF結(jié)合的單目相機(jī)魯棒定位估計(jì)算法與實(shí)驗(yàn)驗(yàn)證

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

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

0 引 言

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

針對單目相機(jī)定位過程中，路標(biāo)與圖像中成像特征點(diǎn)的數(shù)據(jù)關(guān)聯(lián)存在錯(cuò)誤關(guān)聯(lián)的問題，在ICNN數(shù)據(jù)關(guān)聯(lián)基礎(chǔ)上，提出了MAPSAC(Maximum A Posteriori SAmple Consensus，MAPSAC)[9]與EKF(Extended Kalman Filter，EKF)[10，11]結(jié)合的路標(biāo)與圖像中成像特征點(diǎn)錯(cuò)誤關(guān)聯(lián)數(shù)據(jù)剔除的單目相機(jī)定位魯棒估計(jì)方法，給出的MAPSAC損失函數(shù)不僅可靠地剔除了錯(cuò)誤關(guān)聯(lián)的數(shù)據(jù)，而且對數(shù)據(jù)關(guān)聯(lián)的內(nèi)點(diǎn)進(jìn)行了有效的區(qū)分；通過MAPSAC的最小化損失函數(shù)，可以實(shí)現(xiàn)單目相機(jī)定位的魯棒估計(jì)，增強(qiáng)算法的環(huán)境適應(yīng)性.結(jié)合單目相機(jī)狀態(tài)的EKF更新方程，提高了數(shù)據(jù)關(guān)聯(lián)的效率，通過仿真和實(shí)驗(yàn)分析，該方法可以實(shí)現(xiàn)相機(jī)魯棒定位估計(jì).

1 基于RANSAC的單目相機(jī)定位魯棒估計(jì)分析

根據(jù)RANSAC單目相機(jī)定位魯棒估計(jì)算法的原理，對其選擇正確樣本的計(jì)算復(fù)雜度較高以及損失函數(shù)對內(nèi)點(diǎn)不做區(qū)分的問題進(jìn)行了分析.

1.1 基于RANSAC的單目相機(jī)定位魯棒估計(jì)原理

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

1.2 RANSAC的定位估計(jì)樣本選擇分析

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

當(dāng)路標(biāo)與特征點(diǎn)關(guān)聯(lián)存在錯(cuò)誤關(guān)聯(lián)時(shí)，在內(nèi)點(diǎn)比例一定的情況下，獲取一個(gè)定位矩陣模型估計(jì)的正確觀測樣本概率與觀測樣本的大小是非線性關(guān)系.假設(shè)路標(biāo)與特征點(diǎn)關(guān)聯(lián)的內(nèi)點(diǎn)比例w=0.45，獲取一個(gè)好的路標(biāo)與特征點(diǎn)關(guān)聯(lián)的觀測樣本的概率z=0.98的情況下，當(dāng)觀測樣本大小為n時(shí)，抽樣到一個(gè)正確的路標(biāo)與特征點(diǎn)關(guān)聯(lián)的觀測樣本所需的次數(shù)[14]

(1)

由式(1)可知，當(dāng)觀測樣本J所需路標(biāo)與特征點(diǎn)數(shù)據(jù)關(guān)聯(lián)個(gè)數(shù)n增多時(shí)，抽到一個(gè)正確的路標(biāo)與特征點(diǎn)關(guān)聯(lián)的觀測樣本的次數(shù)K急劇增加，它們之間具體的對應(yīng)數(shù)值如表 1 所示.

從表 1 可以看出，6對路標(biāo)與特征點(diǎn)作為一個(gè)樣本抽樣到一個(gè)好觀測樣本的次數(shù)約是1對點(diǎn)時(shí)的78倍.所以，需要減少抽樣次數(shù)，提高獲得一個(gè)好觀測樣本的效率，從而加快數(shù)據(jù)關(guān)聯(lián)的效率.

表 1 樣本大小與抽樣正確樣本次數(shù)的關(guān)系

1.3 RANSAC的損失函數(shù)分析

設(shè)測量誤差為ei；測量標(biāo)準(zhǔn)差為σ；被接受為內(nèi)點(diǎn)的距離閾值為T，RANSAC算法的最大一致集實(shí)際上是對式(2)損失函數(shù)[15]的最小化

(2)

式中：

(3)

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

2 MAPSAC與EKF結(jié)合的單目相機(jī)定位魯棒估計(jì)算法設(shè)計(jì)

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

2.1 基于EKF更新的單目相機(jī)的初始定位估計(jì)模型

RANSAC在選用樣本進(jìn)行射影成像的初始定位估計(jì)時(shí)，參數(shù)估計(jì)的一個(gè)樣本需要6對關(guān)聯(lián)數(shù)據(jù)，得到一個(gè)正確樣本所需的抽樣次數(shù)較高.為了降低該抽樣次數(shù)，結(jié)合單目相機(jī)的運(yùn)動(dòng)方程進(jìn)行更新，采用一對關(guān)聯(lián)數(shù)據(jù)的EKF更新確定初始定位，一對關(guān)聯(lián)數(shù)據(jù)是指樣本中s=1，就可以實(shí)現(xiàn)定位模型的擬合，大大降低了正確定位模型的抽樣次數(shù).該算法充分利用了EKF更新階段得到的單目相機(jī)的定位狀態(tài)，從路標(biāo)與成像特征中選取1對關(guān)聯(lián)數(shù)據(jù)，即可確定單目相機(jī)的初始定位模型[19].

假設(shè)經(jīng)過數(shù)據(jù)匹配的路標(biāo)集為Mi，相應(yīng)的特征測量數(shù)據(jù)集為zIC={z1，z2，…，zi,……,zn}，組成的關(guān)聯(lián)數(shù)據(jù)集為X={M，zIC}.在單目相機(jī)定位過程中，根據(jù)慣導(dǎo)測量相機(jī)的狀態(tài)，設(shè)其運(yùn)動(dòng)方程[10]為

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

(4)

單目相機(jī)測量方程為

(5)

(6)

2.2 基于MAPSAC損失函數(shù)最小化一致集

在得到射影成像的初始定位估計(jì)模型后，就可以按照此模型確定其一致集.當(dāng)關(guān)聯(lián)數(shù)據(jù)與此模型的距離誤差小于距離閾值時(shí)，則關(guān)聯(lián)數(shù)據(jù)為該定位估計(jì)模型的內(nèi)點(diǎn)集zinliers，否則為外點(diǎn)集.

針對RANSAC算法對內(nèi)點(diǎn)不做區(qū)分，距離閾值根據(jù)先驗(yàn)知識(shí)確定的問題，結(jié)合后驗(yàn)概率模型，Torr等人隨后提出了MAPSAC模型估計(jì)算法[9]，該算法使用最大化后驗(yàn)對數(shù)確定距離閾值

(7)

式中：ξ為內(nèi)點(diǎn)的先驗(yàn)期望；σ為測量數(shù)據(jù)的標(biāo)準(zhǔn)差；ν為測量數(shù)據(jù)有界區(qū)域的體積；D為測量數(shù)據(jù)的維數(shù)；d為定位模型的維數(shù).

在單目相機(jī)定位估計(jì)中，在給定定位估計(jì)模型參數(shù)Θ的情況下，對數(shù)據(jù)關(guān)聯(lián)測量坐標(biāo)分量X邊緣化的同時(shí)，優(yōu)化對內(nèi)點(diǎn)與外點(diǎn)距離閾值Tmap的選擇，可以通過式(8)最小化來完成

(8)

式中：

(9)

從式(9)可以看出，每個(gè)外點(diǎn)的損失函數(shù)相同，而對內(nèi)點(diǎn)卻根據(jù)他們擬合模型的好壞程度加以區(qū)別評價(jià)，這樣降低了選擇距離閾值的準(zhǔn)確性，并且不會(huì)增加額外的計(jì)算.

2.3 MAPSAC與EKF結(jié)合的魯棒定位估計(jì)算法

2) 從關(guān)聯(lián)數(shù)據(jù)點(diǎn)集X中隨機(jī)地抽取一對相容性關(guān)聯(lián)數(shù)據(jù)J，根據(jù)式(6)的EKF更新估計(jì)初始定位參數(shù)ΘJ；

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

4) 當(dāng)定位參數(shù)一致集S(ΘJ)中關(guān)聯(lián)數(shù)據(jù)的數(shù)量#S(ΘJ)大于數(shù)量閾值th2時(shí)，根據(jù)式(5)，用一致集進(jìn)行EKF更新，得到定位參數(shù)ΘJ，并輸出關(guān)聯(lián)數(shù)據(jù)一致集及相應(yīng)的定位參數(shù)；當(dāng)S(ΘJ)中關(guān)聯(lián)數(shù)據(jù)的數(shù)量小于數(shù)量閾值th2，回到 2)；

通過以上步驟，則得到了一個(gè)最大一致集，即是單目相機(jī)定位估計(jì)所需要的正確的數(shù)據(jù)關(guān)聯(lián)對，其它的相容性對應(yīng)點(diǎn)都是外點(diǎn)集.濾除了誤關(guān)聯(lián)數(shù)據(jù)，同時(shí)獲得了單目相機(jī)的定位估計(jì).

3 數(shù)值仿真結(jié)果與分析

為了對提出的MAPSAC數(shù)據(jù)關(guān)聯(lián)算法進(jìn)行有效性的驗(yàn)證，采用直觀的曲線模型擬合進(jìn)行仿真驗(yàn)證.46個(gè)真實(shí)二維點(diǎn)分布如圖 1 所示.

圖 1 模擬曲線的二維數(shù)據(jù)

以圖 1 的點(diǎn)作為真實(shí)點(diǎn)，并給每個(gè)真實(shí)點(diǎn)增加2個(gè)模擬測量數(shù)據(jù)，總共構(gòu)成92個(gè)均值為[0,0]的模擬測量數(shù)據(jù)，并產(chǎn)生20個(gè)均值為[2,2]的高斯分布噪聲，它們的協(xié)方差分別為

(10)

20個(gè)高斯分布噪聲數(shù)據(jù)作為錯(cuò)誤數(shù)據(jù)，用實(shí)心圓表示，如圖 2 所示.

圖 2 誤差數(shù)據(jù)和錯(cuò)誤數(shù)據(jù)分布

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

通過圖 3 可知，RANSAC與MAPSAC數(shù)據(jù)關(guān)聯(lián)方法找到的關(guān)聯(lián)數(shù)據(jù)相近，都比較正確地找出了散點(diǎn)的聚集函數(shù)，說明兩種方法數(shù)據(jù)關(guān)聯(lián)結(jié)果差別很小，都是有效的數(shù)據(jù)關(guān)聯(lián)方法.

(a) RANSAC數(shù)據(jù)關(guān)聯(lián)的內(nèi)點(diǎn)結(jié)果

通過表 2 RANSAC與MAPSAC擬合曲線數(shù)據(jù)對比可知，兩種方法找到的數(shù)據(jù)關(guān)聯(lián)點(diǎn)有一點(diǎn)兒區(qū)別，擬合的曲線方程很相近，但MAPSAC的擬合誤差比RANSAC小很多，說明MAPSAC數(shù)據(jù)關(guān)聯(lián)模型估計(jì)方法的準(zhǔn)確性.

表 2 RANSAC與MAPSAC擬合曲線數(shù)據(jù)

4 實(shí)驗(yàn)驗(yàn)證及精度分析

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

圖 4 試驗(yàn)現(xiàn)場布置

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

模擬破片穿孔數(shù)量為20個(gè)，通過表 3 可以看出，檢測出模擬破片穿孔數(shù)量20個(gè)，破片穿孔坐標(biāo)誤差最大為0.86 mm，滿足單個(gè)破片穿孔位置測量精度，說明設(shè)計(jì)的相機(jī)定位算法精度較高.通過實(shí)驗(yàn)驗(yàn)證，系統(tǒng)測量精度滿足項(xiàng)目的技術(shù)指標(biāo)要求.

表 3 模擬破片穿孔坐標(biāo)

5 結(jié) 論

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