非合作紋理目標(biāo)單目位姿計算

馮肖維，謝安安，肖健梅，王錫淮

(上海海事大學(xué) 電氣自動化系，上海 201306)

1 引 言

非合作目標(biāo)位姿測量廣泛應(yīng)用于機器人環(huán)境感知、空間交會對接和虛擬現(xiàn)實等領(lǐng)域[1-4]。由于無法事先獲得目標(biāo)物體外形信息，使非合作目標(biāo)位姿計算變得復(fù)雜與困難，必須首先恢復(fù)目標(biāo)表面三維信息(如紋理特征、外形輪廓等)，然后利用這些信息提供的空間約束關(guān)系解算位姿[5]。

單目視覺系統(tǒng)無法直接獲得景深信息，只能利用目標(biāo)與相機間的相對運動來恢復(fù)場景三維結(jié)構(gòu)(Structure From Motion，SFM)[6]。而單目位姿計算不僅要恢復(fù)目標(biāo)三維結(jié)構(gòu)，還需要實時精確估計相機與目標(biāo)之間的相對位姿。Segal[7]首先恢復(fù)目標(biāo)三維信息，然后利用擴展卡爾曼濾波算法迭代求解位姿，由于需要事先恢復(fù)目標(biāo)的三維結(jié)構(gòu)，無法實時在線應(yīng)用。在一些非結(jié)構(gòu)化應(yīng)用場景中，由于無法事先恢復(fù)目標(biāo)完整外形信息，只能根據(jù)逐漸恢復(fù)的目標(biāo)三維表面結(jié)構(gòu)信息來確定目標(biāo)與相機間的相對位姿，此過程類似于同時定位與地圖構(gòu)建(Simultaneous Localization and Mapping，SLAM)[8]。Augenstein[9]利用粒子濾波實現(xiàn)目標(biāo)同步三維重建與位姿估計，依靠粒子濾波的多假設(shè)估計提高算法抗干擾能力。為了實現(xiàn)目標(biāo)特征和相對位姿的聯(lián)合估計，前述方法需要濾波器對每幀圖像進行處理，將時間消耗在處理具有極少新增信息的冗余圖像幀中，且忽略了累積線性化誤差。為此，Klein[4]和Mouragnon[10]提出基于關(guān)鍵幀的單目視覺方法，僅使用選定的圖像幀重構(gòu)三維信息，因此可以執(zhí)行更費時但更準確的非線性優(yōu)化。Strasdat等人證明在相同的計算成本下，基于關(guān)鍵幀的優(yōu)化技術(shù)比濾波法更為準確[11]。

在單目視覺SLAM中，環(huán)境中的所有特征都可以用于解算位姿，當(dāng)鏡頭移動時，連續(xù)圖像幀之間存在豐富的共視特征，保證視覺SLAM具有較高穩(wěn)定性與抗干擾能力。但在指定目標(biāo)的位姿計算中，目標(biāo)只占圖像的局部區(qū)域，提取的特征數(shù)量有限，使得空間約束關(guān)系變少，算法的穩(wěn)定性變差，特別是當(dāng)背景特征豐富時，會進一步給目標(biāo)特征辨識造成困難，導(dǎo)致計算失敗。此外，單目視覺具有尺度漂移問題，目標(biāo)模型在增量式恢復(fù)過程中會產(chǎn)生累積誤差，進而影響位姿計算的精度。Mur-Artal等[8]利用關(guān)鍵幀思想增量式恢復(fù)環(huán)境三維共視特征地圖，并依靠回環(huán)檢測與位姿圖優(yōu)化保證地圖尺度一致性，實現(xiàn)相機相對非結(jié)構(gòu)環(huán)境的位姿計算，但是無法針對具體目標(biāo)計算相對位姿。在此基礎(chǔ)上，劉宗明等[12]實現(xiàn)非合作目標(biāo)基于關(guān)鍵幀的純旋轉(zhuǎn)位姿測量，由于沒有建立目標(biāo)模型，無法分離前景與背景，因此該方法只能在結(jié)構(gòu)化環(huán)境中使用。并且前述方法在優(yōu)化時都只考慮成像模型約束，而沒有考慮特征點與目標(biāo)輪廓之間的幾何關(guān)系。

非合作目標(biāo)位姿計算的關(guān)鍵是如何在線恢復(fù)目標(biāo)外形信息、抑制累積誤差以及避免環(huán)境干擾。本文選擇具有良好共視特性的圖像幀——模型幀——構(gòu)建共視特征模型，實現(xiàn)非合作紋理目標(biāo)三維信息的合作化。利用網(wǎng)格模型對目標(biāo)表面輪廓及其拓補關(guān)系進行估計，增量式恢復(fù)目標(biāo)表面未知區(qū)域特征點與非共視特征點三維信息，增加模型包含特征的數(shù)量。并利用網(wǎng)格法向場引導(dǎo)特征模型的優(yōu)化調(diào)整，減少由于網(wǎng)格近似造成的特征恢復(fù)誤差，從而提高目標(biāo)位姿的計算精度。在此基礎(chǔ)上引入模型幀閉環(huán)優(yōu)化，抑制尺度漂移形成的累積誤差。依靠運動預(yù)測模型跟蹤共視特征實現(xiàn)目標(biāo)相對位姿的實時計算。

2 目標(biāo)特征模型恢復(fù)

2.1 共視特征模型

(1)

考慮實時性與盡可能多的引入視差，模型幀在選擇時需要滿足如下條件：

(1)模型幀的間隔大于15個連續(xù)圖像幀；

(2)模型幀必須包含αm=40個以上目標(biāo)表面特征點，從而提供盡可能多的約束條件，便于三維信息的恢復(fù)與優(yōu)化；

(3)新增加的模型幀與某個已有模型幀之間共視特征的比率要小于βm=85%，使鄰近模型幀之間具有較大的視覺變化，提高匹配特征之間的視差。

由于使用了共視特征模型，位姿計算與模型優(yōu)化可以在局部區(qū)域進行，而與圖像幀率無關(guān)。同時，為了控制共視特征模型M中模型幀的總體數(shù)量，每增加新的模型幀都會伴隨冗余幀檢測，剔除模型中特征匹配率高于γm=90%的模型幀，使得在增量式建模過程中可以靈活地擴展目標(biāo)模型，當(dāng)連續(xù)觀測到目標(biāo)表面相同區(qū)域時，模型無需被重復(fù)構(gòu)建。

2.2 模型初始化

2.3 三維信息恢復(fù)

圖1 特征點三維重構(gòu)Fig.1 3D reconstruction of feature points

(2)

(3)

(4)

2.4 特征點的選擇

為了只恢復(fù)目標(biāo)表面的特征，本文利用網(wǎng)格模型F的拓撲關(guān)系對位于網(wǎng)格外的特征點進行篩選，避免背景特征加入共視特征模型。

(5)

2.5 模型法向約束優(yōu)化

由于三角網(wǎng)格模型是實際物體表面的一個近似逼近，因此根據(jù)網(wǎng)格模型重投影恢復(fù)的特征三維信息含有誤差，影響位姿計算的精度。當(dāng)共視特征模型被更新，即有新的模型幀被加入時，本文所述方法會對共視模型中與新加入模型幀具有共視關(guān)系的區(qū)域進行優(yōu)化調(diào)整。

假設(shè)鏡頭無畸變，相機的內(nèi)參矩陣為K，則成像模型π可以表示為：

(6)

其中si,k為尺度系數(shù)，取默認值si,k=1，則第i個特征點對第k個模型幀的重投影誤差為：

(7)

(8)

圖2 特征模型局部優(yōu)化Fig.2 Local optimization of feature model

如圖2所示，目標(biāo)表面三角網(wǎng)格模型F中局部區(qū)域f的某個三角面片f∈f的法向量為nf。則根據(jù)f中每個三角面片的法向量與其3條邊之間的正交關(guān)系，定義如下優(yōu)化正則項：

(9)

其中?f表示面f對應(yīng)的邊。則最終的基于面法向場約束的模型局部優(yōu)化表示為：

(10)

其中參數(shù)θ用于控制投影約束優(yōu)化與模型法向約束優(yōu)化之間的平衡。本文取θ=1，使重投影誤差與法向調(diào)整誤差具有相同重要性。本文使用Levenberg-Marquardt法進行非線性優(yōu)化求解。

2.6 模型閉環(huán)尺度優(yōu)化

如果相機再次采集到目標(biāo)表面某個已建模區(qū)域，則可以通過模型中已建立的模型幀調(diào)整當(dāng)前模型幀的尺度。

(11)

(12)

(13)

(14)

(15)

其中Λi,j=I7×7為誤差項的協(xié)方差矩陣。

(16)

3 位姿跟蹤計算

圖3 特征共視關(guān)系的建立Fig.3 Establishing covisibility between features

(17)

(18)

(19)

4 實驗結(jié)果分析

實驗中相機內(nèi)參K已知，圖像分辨率為640×480。程序用C++語言編寫，運行于一臺Intel core i5@2.40 GHz雙核筆記本電腦，內(nèi)存為8 GB，安裝的操作系統(tǒng)為Ubuntu18.04 LTS。

圖4 圓筒增量式建模結(jié)果Fig.4 Incremental modelling results of cylinder

圖4顯示圓筒目標(biāo)共視特征模型恢復(fù)過程(彩圖見期刊電子版)。圖左側(cè)顯示恢復(fù)的所有三維特征點，以及經(jīng)過辨識后根據(jù)有效特征點構(gòu)建的三維網(wǎng)格模型，綠色方框表示相機當(dāng)前位姿。同時，將左側(cè)三維網(wǎng)格模型根據(jù)計算出的目標(biāo)位姿重投影到右側(cè)對應(yīng)二維圖像幀上，右圖中綠色小方框為提取的ORB特征。隨著計算的推進，重投影網(wǎng)格與目標(biāo)輪廓保持重合，說明本文所述方法可以對非合作目標(biāo)進行在線同步建模與位姿計算。此外，圖4(b)中有2個背景特征點被誤加入模型，使模型產(chǎn)生了畸變，隨著視角的變化，新信息的到來，干擾特征點被辨識，并從模型中剔除，如圖4(c)所示，說明本文所述方法的特征選擇機制可以有效區(qū)分目標(biāo)與背景特征，從而保證只有目標(biāo)表面的特征用于位置計算。

圖5顯示了圖4所示建模過程中，模型幀的數(shù)量隨時間變化的情況。本文所述方法的冗余幀回收機制使模型幀的數(shù)量保持在一個穩(wěn)定的數(shù)量上，而不會隨時間持續(xù)增長。同時由于目標(biāo)識別機制，背景的變化并不會使新模型幀被加入，保證本文方法可以長時間高效運行。圖中柱狀圖形表示不同時刻共視特征模型中每個模型幀的共視模型幀的平均數(shù)量，可以保持在5～10 frame之間，說明共視特征模型可以提供足夠的特征進行位姿計算。

圖5 模型幀數(shù)量及其共視關(guān)系的變化Fig.5 Evolution of the number of model frames and covisiblity

為了直觀顯示模型優(yōu)化的效果，圖6為目標(biāo)三維網(wǎng)格模型在優(yōu)化過程中的變化情況?？梢园l(fā)現(xiàn)，隨著特征點不斷優(yōu)化，網(wǎng)格模型逐漸逼近真實目標(biāo)輪廓，說明在建模過程中由于網(wǎng)格近似造成的特征恢復(fù)誤差可以得到有效抑制。

圖6 目標(biāo)網(wǎng)格模型優(yōu)化結(jié)果Fig.6 Optimization results of target mesh model

圖7顯示了插入第40個模型幀后，模型優(yōu)化前后特征模型恢復(fù)與特征重投影的結(jié)果(彩圖見期刊電子版)。每幅圖從左到右分別為特征模型與對應(yīng)的真實目標(biāo)外形線框模型、當(dāng)前幀提取的特征點(綠色圓圈)和根據(jù)目標(biāo)當(dāng)前位姿得到的反投影特征點(紅色交叉)及目標(biāo)線框模型、特征重投影放大視圖。圖7(a)是未進行模型優(yōu)化時的結(jié)果，由于初始估計的位姿與恢復(fù)的目標(biāo)共視特征模型存在誤差，重投影的特征與線框模型都與實際值具有較大偏差。圖7(b)是模型經(jīng)過無法向約束優(yōu)化后的結(jié)果，通過共視特征點與位姿的優(yōu)化，重投影誤差得到明顯改善，但是特征模型并沒有收斂到目標(biāo)真實輪廓。圖7(c)是經(jīng)過本文所述法向約束優(yōu)化后的結(jié)果，重投影誤差得到改善的同時，共視特征模型也得到優(yōu)化，特征點朝著目標(biāo)真實表面輪廓調(diào)整。

圖7 特征模型與重投影誤差比較Fig.7 Comparison of feature model and reprojection error

(20)

(21)

圖8 特征重投影誤差與偏離誤差隨優(yōu)化的變化Fig.8 Evolution of reprojection error and normalized deviation error w.r.t. optimization

為了比較不同方法位姿計算精度，將多面體目標(biāo)固定于高精度6自由度云臺上做空間運動，利用固定于支架上的相機對目標(biāo)進行連續(xù)拍攝，如圖11所示。由于相機和目標(biāo)之間的相對位姿真值未知，而云臺提供的平移距離和旋轉(zhuǎn)角度只能反映目標(biāo)姿態(tài)的變化量，所以根據(jù)測量位姿的變化量來衡量位姿計算精度。

圖9 精度分析實驗平臺Fig.9 Experiment platform of precision analysis

圖10顯示了，目標(biāo)做20次位姿變換，平移距離變化量和旋轉(zhuǎn)變化量的絕對和相對測量誤差。將目標(biāo)平移量測量值與對應(yīng)云臺平移量相比較，利用該比值對測量值進行尺度恢復(fù)后與云臺實際平移量進行比較，得到平移誤差。而旋轉(zhuǎn)誤差根據(jù)式(22)計算[16]：

(22)

圖10 回環(huán)優(yōu)化對于共視特征模型的影響Fig.10 Influence of loop detection on covisibility

為了分析算法的時效性，對程序中各個功能模塊的運行時間進行了統(tǒng)計，如表1所示。由于建模過程中網(wǎng)格數(shù)量是逐漸增長的，實驗中等待模型幀數(shù)量穩(wěn)定后再進行統(tǒng)計。從表中看出，位姿計算率維持在8 Hz，4個模塊中最耗時的是網(wǎng)格構(gòu)建，由于各模塊采用多線程機制，因此其他3個模塊對位姿計算的實時性影響較小。網(wǎng)格構(gòu)建與模型優(yōu)化過程中，當(dāng)有新的模型幀插入時，局部優(yōu)化會被終止，因此耗時變化較大。

表1 位姿計算時效分析

5 結(jié) 論

本文根據(jù)機器視覺和計算機圖形學(xué)相關(guān)理論，實現(xiàn)了非合作紋理目標(biāo)在線同步建模與位姿計算。從連續(xù)圖像幀序列中選擇具有良好特征共視關(guān)系的圖像幀構(gòu)建基于ORB特征的共視特征模型，實現(xiàn)目標(biāo)的合作化。利用網(wǎng)格模型指導(dǎo)目標(biāo)未知區(qū)域特征點的增量式恢復(fù)，避免背景特征干擾和目標(biāo)整體建模。在共視模型的局部重投影優(yōu)化中引入法向約束，提高特征模型的恢復(fù)質(zhì)量，并對模型進行閉環(huán)尺度優(yōu)化，減少累積誤差的同時提高位姿計算精度。實驗結(jié)果表明，本文所述方法能夠?qū)Ψ呛献骷y理目標(biāo)進行實時的特征建模與位姿計算，為基于單目視覺的三維感知與測量建模提供一種有效方法。

在今后的研究中，可以考慮引入基于像素深度匹配的光流法進行非紋理目標(biāo)的位姿計算；并利用如直線等特征施加更加穩(wěn)定的空間約束關(guān)系，進一步提高算法的精度與穩(wěn)定性。