馮遠(yuǎn)靜,黃良鵬,張文安

(浙江工業(yè)大學(xué)信息工程學(xué)院,浙江杭州 310023)

1 引言

基于視覺傳感器的剛體位姿估計(jì)是計(jì)算機(jī)視覺領(lǐng)域中一個(gè)重要的研究方向,廣泛應(yīng)用于機(jī)械臂跟蹤[1-2]、手術(shù)器械位姿估計(jì)[3-6]、風(fēng)洞模型姿態(tài)測(cè)量[7]、機(jī)器人導(dǎo)航[8-10]等領(lǐng)域.

目前,大部分位姿估計(jì)的研究都是在相機(jī)內(nèi)參已知的前提下,通過采集圖像并加以處理,得到二維特征信息,利用二維、三維坐標(biāo)系間的對(duì)應(yīng)關(guān)系設(shè)計(jì)位姿估計(jì)方法,從而實(shí)現(xiàn)對(duì)目標(biāo)位姿的估計(jì).在現(xiàn)有的研究方法中,大部分研究都是基于點(diǎn)特征的,也存在一些相對(duì)成熟的點(diǎn)特征位姿估計(jì)方法,例如正交迭代(orthogonal iteration,OI)[11]、高效n 點(diǎn)透視(efficient perspective-n-point,EPnP)[12]、自適應(yīng)擴(kuò)展卡爾曼(adaptive extended Kalman filter,AEKF)[13]以及迭代自適應(yīng)擴(kuò)展卡爾曼(iterative adaptive extended Kalman filter,IAEKF)[14].OI正交迭代和EPnP的估計(jì)精度過分依賴觀測(cè)噪聲.當(dāng)噪聲過大時(shí),估計(jì)精度便會(huì)急劇下降[15],因此人們提出了AEKF[16],通過對(duì)每一個(gè)采樣步長中的過程噪聲以及觀測(cè)噪聲進(jìn)行采樣從而克服環(huán)境中因噪聲過大或者環(huán)境異變時(shí)產(chǎn)生的影響,使得系統(tǒng)更加魯棒.與AEKF相比,IAEKF通過將AEKF的結(jié)果進(jìn)行迭代,得到最小的估計(jì)方差,提高了精度,但同時(shí)也使得計(jì)算量大幅增加.在某些狀況下,如初始值誤差過大、相機(jī)標(biāo)定誤差過大,基于單個(gè)視覺傳感器的位姿估計(jì)方法所得到的結(jié)果誤差將會(huì)過大甚至發(fā)散[17].從而出現(xiàn)了基于多個(gè)視覺傳感器的集中式融合方法[18],通過結(jié)合多個(gè)視覺傳感器采集的圖像信息,并利用AEKF或IAEKF對(duì)目標(biāo)進(jìn)行位姿估計(jì),一定程度上彌補(bǔ)了單個(gè)視覺傳感器方法的缺陷,提高了系統(tǒng)估計(jì)精度,但觀測(cè)矩陣維數(shù)會(huì)隨著觀測(cè)信息的增加而上升,計(jì)算量也會(huì)隨之成平方增長,造成系統(tǒng)的響應(yīng)速度大幅下降[19],并且對(duì)于多視覺傳感器位姿估計(jì)系統(tǒng),若某個(gè)局部子系統(tǒng)出現(xiàn)遮擋,集中式卡爾曼方法將會(huì)失效.

而分布式融合估計(jì)方法先對(duì)各個(gè)子系統(tǒng)單獨(dú)進(jìn)行估計(jì),然后將估計(jì)結(jié)果融合.對(duì)于缺失觀測(cè)數(shù)據(jù)的子系統(tǒng),通常只對(duì)其進(jìn)行狀態(tài)預(yù)測(cè),再將剩余子系統(tǒng)的估計(jì)結(jié)果與其預(yù)測(cè)結(jié)果融合[20],然而在觀測(cè)數(shù)據(jù)連續(xù)丟失的情況下,相應(yīng)子系統(tǒng)的估計(jì)結(jié)果將過度依賴于模型,往往導(dǎo)致跟蹤偏差過大.遮擋過程正是一個(gè)觀測(cè)數(shù)據(jù)連續(xù)缺失的過程,因此這類方法不適用于遮擋情況下多相機(jī)位姿估計(jì)系統(tǒng).為了解決遮擋情況下多視覺傳感器對(duì)剛體的位姿估計(jì)問題,本文針對(duì)多視覺傳感器位姿估計(jì)系統(tǒng)的特點(diǎn),提出了一種基于自適應(yīng)無跡卡爾曼濾波的分布式融合方法,可有效地解決多相機(jī)位姿估計(jì)系統(tǒng)中的遮擋問題.首先,建立了多相機(jī)系統(tǒng)的狀態(tài)空間模型,進(jìn)而設(shè)計(jì)了AUKF和矩陣加權(quán)分布式融合估計(jì)方法,然后將遮擋情況劃分為部分遮擋情況和嚴(yán)重遮擋情況,并針對(duì)不同遮擋情況提出了對(duì)應(yīng)的解決方案.系統(tǒng)未出現(xiàn)遮擋時(shí),利用AUKF求出相機(jī)子系統(tǒng)的狀態(tài)信息,進(jìn)而篩選出基準(zhǔn)估計(jì)值,通過引入改進(jìn)的Sage-Husa噪聲估計(jì)器[21]更新過程噪聲.相機(jī)子系統(tǒng)出現(xiàn)嚴(yán)重遮擋時(shí),該時(shí)刻不對(duì)其進(jìn)行計(jì)算,等待剩余子系統(tǒng)融合結(jié)束,利用融合結(jié)果對(duì)其進(jìn)行初始化;子系統(tǒng)出現(xiàn)部分遮擋時(shí),利用基準(zhǔn)估計(jì)值和先驗(yàn)知識(shí)估計(jì)出缺失的特征點(diǎn)坐標(biāo),繼而利用無跡卡爾曼濾波(unscented Kalman filter,UKF)獲取子系統(tǒng)狀態(tài)信息.結(jié)合正常、部分遮擋子系統(tǒng)的狀態(tài)信息,利用分布式融合方法得到最終結(jié)果,實(shí)現(xiàn)對(duì)運(yùn)動(dòng)剛體的位姿估計(jì).最后,通過仿真獲取了區(qū)分遮擋情況的閾值,并進(jìn)一步通過實(shí)驗(yàn)驗(yàn)證表明,所提出的設(shè)計(jì)方法能夠有效提升系統(tǒng)在遮擋情況下的估計(jì)精度與魯棒性.

2 問題描述

2.1 投影模型

考慮一類有多個(gè)特征點(diǎn)位于平面上的投影模型,如圖1所示.點(diǎn)O為像素坐標(biāo)系原點(diǎn),位于圖像左上角;U,V 兩軸分別穿過原點(diǎn),平行于圖像的水平邊緣與垂直邊緣;點(diǎn)OI為圖像坐標(biāo)系原點(diǎn),位于圖像中心;XI,YI兩軸分別平行于U,V 兩軸;點(diǎn)Oc為相機(jī)坐標(biāo)系原點(diǎn),位于相機(jī)的光心;Zc軸穿過相機(jī)光心,垂直于圖像平面,與圖像交于圖像坐標(biāo)系原點(diǎn)OI;Xc,Yc軸分別平行于XI,YI軸.物體固定于一個(gè)六自由度并聯(lián)平臺(tái),將物體下表面中心視為物體坐標(biāo)系原點(diǎn),與并聯(lián)平臺(tái)坐標(biāo)系重合.在本模型中,相機(jī)位姿保持不變,通過控制六自由度并聯(lián)平臺(tái)實(shí)現(xiàn)目標(biāo)剛體的運(yùn)動(dòng),在實(shí)驗(yàn)前對(duì)相機(jī)進(jìn)行標(biāo)定,獲得世界坐標(biāo)系和相機(jī)坐標(biāo)間的轉(zhuǎn)換矩陣.

圖1 特征點(diǎn)在圖像平面的投影示意圖Fig.1 Projection sketch of feature points on image plane

剛體上標(biāo)有8個(gè)特征點(diǎn),假設(shè)第j(j=1,···,8)個(gè)特征點(diǎn)Pj在物體坐標(biāo)系和相機(jī)坐標(biāo)系中的坐標(biāo)分別為,且轉(zhuǎn)換關(guān)系如下:

其中:cRb為坐標(biāo)系Ob?XbYbZb與Oc?XcYcZc之間的旋轉(zhuǎn)矩陣;為平移向量,表示物體坐標(biāo)系原點(diǎn)相對(duì)于相機(jī)坐標(biāo)系原點(diǎn)的位置關(guān)系.

像素坐標(biāo)系與相機(jī)坐標(biāo)系下特征點(diǎn)之間的關(guān)系可以通過針孔模型描述:

其中:[ujvj]T表示第j個(gè)特征點(diǎn)在像素坐標(biāo)系下的坐標(biāo);f為相機(jī)焦距;dx,dy分別為單個(gè)像素點(diǎn)在圖像坐標(biāo)系中沿x軸、y 軸方向的物理距離,可通過標(biāo)定得到相機(jī)內(nèi)參,即fx=f/dx,fy=f/dy,u0,v0,(u0,v0)表示圖像中心點(diǎn)在像素坐標(biāo)系下的坐標(biāo).為了方便與六自由度并聯(lián)平臺(tái)反饋的位姿作對(duì)比,利用物體坐標(biāo)系原點(diǎn)在世界坐標(biāo)系下的位姿表示系統(tǒng)狀態(tài)量.由于Haug[22]提出,對(duì)于位姿估計(jì)系統(tǒng),使用二階加速度模型具有更好的估計(jì)結(jié)果,因此采用二階加速度模型來描述剛體的運(yùn)動(dòng).

2.2 位姿估計(jì)系統(tǒng)描述

考慮一類基于多相機(jī)的運(yùn)動(dòng)剛體位姿估計(jì)系統(tǒng),其中運(yùn)動(dòng)剛體的運(yùn)動(dòng)學(xué)模型描述如下:

其中:?t為系統(tǒng)采樣時(shí)間,I3×3為3×3的單位矩陣.

對(duì)于第i(i=1,···,N)個(gè)相機(jī)子系統(tǒng),其觀測(cè)模型如下:

其中:r為量測(cè)噪聲,滿足均值為0、協(xié)方差為R的高斯分布;

通過標(biāo)定可求得k1,k2,p1,p2四個(gè)參數(shù);可通過狀態(tài)信息以及先驗(yàn)知識(shí)獲得:

其中Rod1,Rod2,Rod3滿足如下關(guān)系:

本文將采用多相機(jī)融合和自適應(yīng)UKF分布式濾波方法解決如圖1所示的存在遮擋情況的運(yùn)動(dòng)剛體位姿估計(jì)問題.

3 基于AUKF的分布式融合方法

3.1 基于AUKF的局部濾波器設(shè)計(jì)

由式(3)和(5)可知,運(yùn)動(dòng)剛體位姿估計(jì)系統(tǒng)是一個(gè)非線性系統(tǒng),一種簡便的處理方法是將系統(tǒng)進(jìn)行線性化后再做進(jìn)一步處理,而在忽略高階項(xiàng)的同時(shí),勢(shì)必會(huì)使系統(tǒng)產(chǎn)生較大誤差.UKF[16]使用確定性采樣策略逼近非線性函數(shù)的概率密度分布,計(jì)算精度較高[23],且無需求解雅克比矩陣[24],故本文采用UKF作為系統(tǒng)局部濾波器.

由于剛體運(yùn)動(dòng)過程中系統(tǒng)噪聲未知,故不能簡單地將過程噪聲協(xié)方差Q 定為一個(gè)常數(shù),Min等[21]針對(duì)系統(tǒng)噪聲時(shí)變的系統(tǒng)提出了改進(jìn)的次優(yōu)Sage-Husa方法,通過引入噪聲估計(jì)器,更新系統(tǒng)噪聲,一定程度上補(bǔ)償了系統(tǒng)噪聲的影響.然而同時(shí)更新Q和觀測(cè)噪聲協(xié)方差R可能導(dǎo)致結(jié)果發(fā)散,可通過實(shí)驗(yàn)事先校準(zhǔn)獲得較準(zhǔn)確的R[21].

本文所設(shè)計(jì)的基于AUKF的局部濾波器設(shè)計(jì)步驟包括:

步驟1初始化.

步驟2預(yù)測(cè).

同時(shí),計(jì)算其權(quán)值:

式中:n 是狀態(tài)維數(shù),在系統(tǒng)(3)中,n=18;xi(i=0,1,···,2n)為選定的sigma點(diǎn);分別為計(jì)算均值及其協(xié)方差的權(quán)值系數(shù);λ=α2(n+κ)?n,α通常設(shè)為一個(gè)較小的正數(shù),設(shè)置為1e?4;κ是一個(gè)比例參數(shù),設(shè)置為3?n.

將sigma點(diǎn)通過非線性變換對(duì)系統(tǒng)狀態(tài)進(jìn)行預(yù)測(cè),得到各個(gè)sigma點(diǎn)的狀態(tài)預(yù)測(cè)值:

式中:A是狀態(tài)轉(zhuǎn)移矩陣;利用sigma點(diǎn)的狀態(tài)預(yù)測(cè)和均值的權(quán)值系數(shù),得到系統(tǒng)狀態(tài)預(yù)測(cè)值:

同時(shí),獲取各個(gè)sigma點(diǎn)的狀態(tài)預(yù)測(cè)誤差:

類似地,可以得到各個(gè)sigma點(diǎn)的觀測(cè)預(yù)測(cè)值、系統(tǒng)觀測(cè)預(yù)測(cè)值及各個(gè)sigma點(diǎn)觀測(cè)預(yù)測(cè)誤差:

步驟3更新.

利用各個(gè)sigma點(diǎn)狀態(tài)預(yù)測(cè)誤差、觀測(cè)預(yù)測(cè)誤差和協(xié)方差權(quán)值系數(shù)求取狀態(tài)誤差方差矩陣、觀測(cè)誤差方差矩陣及狀態(tài)觀測(cè)之間的互協(xié)方差矩陣:

進(jìn)而獲取系統(tǒng)的卡爾曼增益:

在狀態(tài)預(yù)測(cè)值基礎(chǔ)上,利用k時(shí)刻的量測(cè)信息便可獲得更新的狀態(tài)估計(jì)值和狀態(tài)誤差方差矩陣:

步驟4調(diào)整過程噪聲協(xié)方差Q.

將改進(jìn)的Sage-Husa噪聲估計(jì)器應(yīng)用于UKF:

其中:da,k?1=(1?b)/(1?bk);b為遺忘因子,滿足0

修正系統(tǒng)過程噪聲后，再執(zhí)行步驟2.

3.2 矩陣加權(quán)分布式融合估計(jì)器設(shè)計(jì)

在卡爾曼濾波器中,若使用集中式融合,將會(huì)增加觀測(cè)矩陣維數(shù),從而造成計(jì)算量大幅增加.而分布式融合通過將各個(gè)局部估計(jì)結(jié)果按照一定的方法進(jìn)行加權(quán),從而得到全局最優(yōu)值,同時(shí)避免了觀測(cè)擴(kuò)維,減少了計(jì)算量.在線性情況下,下列所述的矩陣加權(quán)分布式融合已被證明等價(jià)于集中式融合[25],該方法也適用于非線性系統(tǒng),以AUKF為局部估計(jì)器的矩陣加權(quán)分布式融合方法可描述如下:首先,根據(jù)先驗(yàn)信息計(jì)算k時(shí)刻系統(tǒng)的融合狀態(tài)預(yù)測(cè)值以及對(duì)應(yīng)的誤差協(xié)方差,參考UKF 中對(duì)狀態(tài)預(yù)測(cè)以及誤差協(xié)方差的計(jì)算,求取相應(yīng)的sigma點(diǎn):

進(jìn)而求取融合狀態(tài)預(yù)測(cè)值和預(yù)測(cè)誤差:

進(jìn)而求取融合狀態(tài)預(yù)測(cè)協(xié)方差:

然后結(jié)合每個(gè)相機(jī)子系統(tǒng)的狀態(tài)估計(jì)值、狀態(tài)預(yù)測(cè)值、誤差協(xié)方差估計(jì)值及其預(yù)測(cè)值,更新融合后的狀態(tài)估計(jì)值以及融合誤差協(xié)方差矩陣:

式中:i=1,2,···,L,L 表示局部子系統(tǒng)的個(gè)數(shù);分別表示k時(shí)刻第i個(gè)局部子系統(tǒng)狀態(tài)的估計(jì)值和預(yù)測(cè)值;分別為對(duì)應(yīng)的誤差協(xié)方差.

4 遮擋情況下的位姿估計(jì)方法

在進(jìn)行位姿估計(jì)的過程中,常常會(huì)因?yàn)檠９?、外部噪聲干擾、人為干預(yù)等問題而出現(xiàn)特征信息被遮擋或無法識(shí)別的情況.對(duì)于視覺傳感器,遮擋的產(chǎn)生會(huì)造成目標(biāo)特征信息不穩(wěn)定甚至丟失.而對(duì)于位姿估計(jì)算法而言,其關(guān)鍵在于利用足夠的特征信息來完成對(duì)目標(biāo)的估計(jì),因此遮擋的出現(xiàn)會(huì)給剛體位姿估計(jì)帶來很大的困難.本文根據(jù)局部相機(jī)子系統(tǒng)觀測(cè)到的特征點(diǎn)個(gè)數(shù)對(duì)遮擋情況進(jìn)行區(qū)分,假定n為剛體上特征點(diǎn)的總個(gè)數(shù),ns為局部子系統(tǒng)當(dāng)前時(shí)刻能夠觀測(cè)到的特征點(diǎn)數(shù)量,nL為閾值,用于區(qū)分嚴(yán)重遮擋和部分遮擋,通過多次仿真獲得.若局部子系統(tǒng)滿足nL≥ns

4.1 嚴(yán)重遮擋下的改進(jìn)策略

嚴(yán)重遮擋情況下,有效特征點(diǎn)數(shù)量過少,相應(yīng)相機(jī)子系統(tǒng)的估計(jì)結(jié)果無法對(duì)整體系統(tǒng)估計(jì)精度作出有效貢獻(xiàn).因此對(duì)于當(dāng)前時(shí)刻出現(xiàn)嚴(yán)重遮擋的子系統(tǒng),本文不對(duì)其進(jìn)行單獨(dú)估計(jì),等待其他子系統(tǒng)融合完成后,利用融合結(jié)果對(duì)該子系統(tǒng)進(jìn)行初始化.

假設(shè)在第k時(shí)刻,第j個(gè)局部相機(jī)子系統(tǒng)出現(xiàn)嚴(yán)重遮擋.對(duì)部分遮擋和無遮擋的子系統(tǒng)進(jìn)行局部估計(jì),并利用分布式融合方法求得狀態(tài)估計(jì)值和狀態(tài)誤差協(xié)方差,然后對(duì)出現(xiàn)嚴(yán)重遮擋的子系統(tǒng)進(jìn)行初始化:

從而保證該局部相機(jī)子系統(tǒng)在初始化時(shí)獲得較高精度的初始值,使得局部濾波器能夠正常運(yùn)行.

4.2 部分遮擋下的改進(jìn)策略

部分遮擋情況下,目標(biāo)剛體上的部分特征點(diǎn)的觀測(cè)信息缺失,此時(shí)無法直接利用上文所提的卡爾曼濾波器進(jìn)行位姿估計(jì),為此,本文提出一種改進(jìn)的方案,通過利用先驗(yàn)信息對(duì)遮擋處的特征點(diǎn)進(jìn)行修復(fù),間接獲得其觀測(cè)值,提高了數(shù)據(jù)利用率,同時(shí)改善了估計(jì)結(jié)果.

定義1假設(shè)在第k個(gè)時(shí)刻,未出現(xiàn)遮擋的局部子系統(tǒng)有m個(gè),利用UKF分別求得各個(gè)局部子系統(tǒng)的狀態(tài)估計(jì)值及其誤差協(xié)方差矩陣,分別記為x1,x2,···,xm和P1,P2,···,Pm,計(jì)算各個(gè)局部子系統(tǒng)誤差協(xié)方差的跡,并將與最小的協(xié)方差跡對(duì)應(yīng)的狀態(tài)估計(jì)值稱為基準(zhǔn)估計(jì)值,用下式表示:

其中:tr(Pi)為第i個(gè)局部子系統(tǒng)所求得的誤差協(xié)方差的跡;s表示局部子系統(tǒng)的序號(hào),即對(duì)于第s個(gè)局部子系統(tǒng),若其誤差協(xié)方差的跡是最小的,那么其狀態(tài)估計(jì)值xs被稱為基準(zhǔn)估計(jì)值.

假設(shè)在第k個(gè)時(shí)刻,第j個(gè)出現(xiàn)部分遮擋的子系統(tǒng)中,目標(biāo)剛體上第l個(gè)特征點(diǎn)被遮擋,該特征點(diǎn)在物體坐標(biāo)系下的坐標(biāo)為.針對(duì)部分遮擋情況,本文處理方法如下:

步驟1對(duì)于未出現(xiàn)遮擋的m個(gè)局部子系統(tǒng),利用UKF估計(jì)其狀態(tài)值和誤差協(xié)方差,進(jìn)而計(jì)算基準(zhǔn)估計(jì)值xs,并利用第s個(gè)系統(tǒng)更新過程噪聲協(xié)方差Q.

步驟2基準(zhǔn)估計(jì)值xs包含位置向量及姿態(tài)向量,分別表示為(α,β,γ)將姿態(tài)向量代入式(11),可計(jì)算獲得物體坐標(biāo)系相對(duì)于世界坐標(biāo)系的旋轉(zhuǎn)矩陣,從而可將其坐標(biāo)轉(zhuǎn)換矩陣表示為.通過標(biāo)定可獲得每個(gè)子系統(tǒng)中世界坐標(biāo)系相對(duì)于相機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣cjRw和平移矩陣cjtw,從而可將其坐標(biāo)轉(zhuǎn)換矩陣表示為.已知第l個(gè)特征點(diǎn)在物體坐標(biāo)系下的位置,則可通過下式獲得該點(diǎn)在世界坐標(biāo)系下的坐標(biāo)

該點(diǎn)在相應(yīng)相機(jī)坐標(biāo)系中坐標(biāo)Pcjl 可進(jìn)一步由下式求得:

相應(yīng)坐標(biāo)系之間轉(zhuǎn)換關(guān)系如圖2所示.

圖2 相應(yīng)坐標(biāo)系之間轉(zhuǎn)換關(guān)系Fig.2 Conversion relations between corresponding coordinate systems

步驟3利用相機(jī)坐標(biāo)系與像素坐標(biāo)系之間的轉(zhuǎn)換關(guān)系,修復(fù)被遮擋的特征點(diǎn),通過下式可求得被修復(fù)的特征點(diǎn)坐標(biāo)(ul,vl):

圖3 補(bǔ)全特征信息Fig.3 Completion of characteristic information

步驟4補(bǔ)全被遮擋的特征點(diǎn)后,利用UKF估計(jì)局部子系統(tǒng)狀態(tài)信息.

綜上所述,遮擋處理流程如圖4所示:假設(shè)共有M個(gè)局部相機(jī)子系統(tǒng),首先根據(jù)觀測(cè)信息判斷每個(gè)子系統(tǒng)的遮擋情況,若子系統(tǒng)處于嚴(yán)重遮擋狀態(tài),則放棄對(duì)該子系統(tǒng)進(jìn)行計(jì)算,并等待剩余子系統(tǒng)融合結(jié)束,反之,則判斷是否處于部分遮擋狀態(tài),若特征點(diǎn)均未被遮擋,利用UKF估計(jì)局部子系統(tǒng)的狀態(tài)信息,進(jìn)而篩選獲得基準(zhǔn)估計(jì)值,并更新過程噪聲協(xié)方差Q;若處于部分遮擋狀態(tài),利用基準(zhǔn)估計(jì)值以及先驗(yàn)知識(shí)修復(fù)該子系統(tǒng)中被遮擋的特征點(diǎn)然后再通過UKF計(jì)算獲得該子系統(tǒng)的狀態(tài)信息.利用分布式融合方法,結(jié)合未出現(xiàn)遮擋的局部子系統(tǒng)的狀態(tài)信息,計(jì)算獲得k時(shí)刻最終的融合結(jié)果,最后利用該融合結(jié)果對(duì)處于嚴(yán)重遮擋狀態(tài)的局部子系統(tǒng)進(jìn)行初始化.

圖4 遮擋處理流程圖Fig.4 Flow chart of processing scheme under occlusion

5 實(shí)驗(yàn)

5.1 平臺(tái)搭建

本文所用實(shí)驗(yàn)平臺(tái)包括一臺(tái)由PI公司生產(chǎn)的精度達(dá)0.1μm的六自由度并聯(lián)平臺(tái)、一臺(tái)配套的并聯(lián)平臺(tái)控制器、一臺(tái)PC機(jī)以及多臺(tái)普通固焦相機(jī).六自由度并聯(lián)平臺(tái)可在一定范圍內(nèi)任意移動(dòng).相機(jī)型號(hào)為今貴S9,能夠采集分辨率為640×480的圖像,如圖5所示.

實(shí)驗(yàn)場景如圖6所示.用一個(gè)帶有8個(gè)特征點(diǎn)的高精度定制方木塊作為被跟蹤剛體,固定于高精度定制角件上,并將其安裝于六自由度并聯(lián)平臺(tái),從而剛體可隨六自由度并聯(lián)平臺(tái)的移動(dòng)而移動(dòng),且剛體與并聯(lián)平臺(tái)相對(duì)位姿保持不變.固焦相機(jī)通過云臺(tái)固定于三腳架上,并保持相機(jī)和三腳架位置、姿態(tài)不變.

圖5 實(shí)驗(yàn)平臺(tái)Fig.5 Experimental platform

圖6 實(shí)驗(yàn)場景Fig.6 Experimental scene

六自由度并聯(lián)平臺(tái)精度可達(dá)0.1μm,因此可將其反饋的位置和姿態(tài)信息作為參考真值.實(shí)驗(yàn)過程中,通過固焦相機(jī)采集目標(biāo)運(yùn)動(dòng)圖像,發(fā)送至上位機(jī)系統(tǒng)處理,進(jìn)而利用所提算法對(duì)目標(biāo)剛體進(jìn)行位姿估計(jì),同時(shí),通過六自由度并聯(lián)平臺(tái)控制器,獲取同一時(shí)刻六自由度并聯(lián)平臺(tái)的坐標(biāo)并保存.

六自由度并聯(lián)平臺(tái)坐標(biāo)系的原點(diǎn)位于上圓盤中心位置,且其X,Y,Z軸方向保持固定.實(shí)驗(yàn)前,通過控制器對(duì)并聯(lián)平臺(tái)進(jìn)行初始化,將其位移與姿態(tài)值置零.并聯(lián)臺(tái)移動(dòng)時(shí),控制器向PC機(jī)反饋六自由度并聯(lián)平臺(tái)坐標(biāo)系相對(duì)于初始時(shí)刻改變的位姿.實(shí)驗(yàn)中,為方便與參考真值對(duì)比,令物體坐標(biāo)系Ob?XbYbZb與六自由度并聯(lián)平臺(tái)坐標(biāo)系OS?XSYSZS重合,同時(shí)令世界坐標(biāo)系Ow?XwYwZw與初始時(shí)刻六自由度并聯(lián)平臺(tái)坐標(biāo)系重合,并將世界坐標(biāo)系下物體坐標(biāo)系原點(diǎn)的位姿作為系統(tǒng)估計(jì)狀態(tài)值,從而使計(jì)算獲得的估計(jì)值與真實(shí)值直接進(jìn)行對(duì)比,如圖7所示.

圖7 世界坐標(biāo)系與六自由度并聯(lián)平臺(tái)坐標(biāo)系之間關(guān)系Fig.7 The relation between world coordinate system and 6-DOF parallel platform coordinate system

5.2 仿真實(shí)驗(yàn)

采用帶有兩個(gè)局部相機(jī)子系統(tǒng)的系統(tǒng)進(jìn)行仿真,并利用UKF進(jìn)行局部估計(jì).仿真時(shí),使其中一個(gè)局部子系統(tǒng)始終處于未被遮擋狀態(tài)(簡稱為正常子系統(tǒng)),對(duì)另一個(gè)局部子系統(tǒng)進(jìn)行遮擋處理,使其只能觀測(cè)到部分特征點(diǎn)(簡稱為非正常子系統(tǒng)).對(duì)缺失0至7個(gè)特征點(diǎn)的情況依次進(jìn)行仿真,記錄系統(tǒng)融合結(jié)果和正常子系統(tǒng)的估計(jì)結(jié)果,仿真結(jié)束后,計(jì)算正常子系統(tǒng)的平均誤差以及每次系統(tǒng)融合誤差.通過對(duì)比分析兩者結(jié)果,從而選擇合適的閾值,以此來區(qū)分子系統(tǒng)處于何種遮擋情況.

對(duì)于正常子系統(tǒng),通過下式計(jì)算仿真后的平均狀態(tài)誤差:

其中:j為當(dāng)前缺失特征點(diǎn)的個(gè)數(shù),Nf表示實(shí)驗(yàn)場景中的特征點(diǎn)總數(shù),Ns為每種缺失特征點(diǎn)的仿真狀態(tài)下的仿真次數(shù),Nk表示每次仿真的觀測(cè)采樣次數(shù),表示局部子系統(tǒng)的狀態(tài)估計(jì)值,對(duì)應(yīng)于缺失j個(gè)特征點(diǎn)的仿真實(shí)驗(yàn)中的第i 個(gè)采樣時(shí)刻,表示在該時(shí)刻的狀態(tài)參考真值.系統(tǒng)分布式融合誤差通過下式計(jì)算獲得:

仿真時(shí),相機(jī)水平焦距設(shè)定為fx1=fx2=500,垂直焦距設(shè)為fy1=fy2=500,光心橫坐標(biāo)設(shè)置為u01=u02=300,縱坐標(biāo)為v01=v02=200,采樣周期?t=0.05 s,系統(tǒng)噪聲協(xié)方差的初值設(shè)為Q0=0.1×diag{0,0,0,5,5,5,0.1,0.1,0.1,0,0,0,2,2,2,0.1,0.1,0.1},觀測(cè)噪聲協(xié)方差初值設(shè)為R0=0.1×I16×16,I16×16為16×16的單位矩陣,設(shè)置系統(tǒng)的初始方差為P0=I18×18,I18×18為18×18的單位矩陣,遺忘因子b=0.9.世界坐標(biāo)系相對(duì)于相機(jī)1和相機(jī)2坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣分別設(shè)置為

針對(duì)本文實(shí)驗(yàn)場景,特征點(diǎn)總數(shù)Nf=8,每種仿真狀態(tài)進(jìn)行1000次仿真,即Ns=1000,每次仿真時(shí)的觀測(cè)采樣次數(shù)設(shè)置為Nk=1000.

圖8和圖9分別展示了系統(tǒng)融合結(jié)果的位置誤差和角度誤差與缺失特征個(gè)數(shù)之間的關(guān)系.

圖8 系統(tǒng)融合結(jié)果的位置誤差與缺失特征點(diǎn)個(gè)數(shù)關(guān)系Fig.8 The relationship between the position error of fusion results and the number of missing feature points

圖9 系統(tǒng)融合結(jié)果的角度誤差與缺失特征個(gè)數(shù)關(guān)系Fig.9 The relationship between the angle error of fusion results and the number of missing features

從中可以獲得以下結(jié)論:1)系統(tǒng)融合結(jié)果的誤差隨缺失特征點(diǎn)個(gè)數(shù)增加而增大;2)當(dāng)目標(biāo)特征點(diǎn)總個(gè)數(shù)Nf=8,且缺失的特征點(diǎn)個(gè)數(shù)不超過2個(gè)時(shí),融合結(jié)果平均誤差小于正常子系統(tǒng)的平均估計(jì)誤差.分析可得,當(dāng)遮擋的特征點(diǎn)個(gè)數(shù)超過2個(gè)時(shí),通過修補(bǔ)特征點(diǎn)獲取丟失觀測(cè)信息的非正常子系統(tǒng)無法對(duì)整體系統(tǒng)產(chǎn)生有效貢獻(xiàn).因此針對(duì)本文實(shí)驗(yàn)場景,用于區(qū)分嚴(yán)重遮擋和部分遮擋的閾值設(shè)定為6,即nL=6.對(duì)于特征點(diǎn)總數(shù)為Nf的實(shí)驗(yàn)場景,可根據(jù)上述仿真方法計(jì)算正常子系統(tǒng)的平均誤差以及每次系統(tǒng)融合誤差,獲取誤差大小與缺失特征點(diǎn)個(gè)數(shù)之間的對(duì)應(yīng)關(guān)系,從而進(jìn)行閾值選取.閾值選取的原則為:當(dāng)局部子系統(tǒng)能夠觀測(cè)到特征點(diǎn)數(shù)量不小于閾值時(shí),系統(tǒng)融合誤差不大于正常子系統(tǒng)的平均誤差,即滿足nL≥ns

5.3 實(shí)際實(shí)驗(yàn)

實(shí)驗(yàn)采用兩個(gè)相機(jī)對(duì)目標(biāo)剛體進(jìn)行位姿估計(jì).兩個(gè)相機(jī)的內(nèi)部參數(shù)通過攝像機(jī)標(biāo)定后獲取,其各項(xiàng)數(shù)值分別如下:=813.3459,=803.1460,=813.0550,=803.0452,=358.9625,=229.8308,=380.9250,=233.6312;畸變參數(shù)為=0.2124,=?0.0145,=0.235,=?0.0084,====0.設(shè)置過程噪聲協(xié)方差初始值均為

每個(gè)局部子系統(tǒng)的觀測(cè)噪聲協(xié)方差初始值均為R0=0.05·I16×16,其中:I16×16為16×16的單位矩陣,遺忘因子b=0.9,六自由度并聯(lián)平臺(tái)的初始位置和姿態(tài)都為0,因此系統(tǒng)的初始狀態(tài)量為零向量,設(shè)置系統(tǒng)的初始方差為P0=0.01·I18×18,為18×18的單位矩陣.世界坐標(biāo)系相對(duì)于相機(jī)1與相機(jī)2坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣分別為

目標(biāo)剛體上標(biāo)記的8個(gè)特征點(diǎn)在物體坐標(biāo)系下的坐標(biāo)為(35,?80,50),(35,?10,50),(?35,?10,50),(?35,?80,50),(20,?65,50),(20,?10,50),(?20,?65,50),(?20,?25,50),單位為mm.在實(shí)驗(yàn)過程中,利用不透明的物體對(duì)兩個(gè)相機(jī)鏡頭輪流進(jìn)行遮擋,遮擋過程效果如圖10所示.

圖10 遮擋過程Fig.10 Occlusion process

在實(shí)驗(yàn)效果圖中,為了方便觀察位姿估計(jì)結(jié)果,將出現(xiàn)遮擋的局部子系統(tǒng)對(duì)應(yīng)狀態(tài)量設(shè)置為零.圖11展示了遮擋情況下目標(biāo)的重投影效果圖,其中實(shí)線部分表示利用本文方法獲得位姿,虛線部分表示出現(xiàn)遮擋的子系統(tǒng)所對(duì)應(yīng)的位姿.

圖11 遮擋情況下的重投影效果圖Fig.11 The re-projection rendering under occlusion

圖12展示了帶有遮擋的情況下目標(biāo)位姿的估計(jì)結(jié)果,分析圖像后可得到以下結(jié)論:

1)本文算法在遮擋環(huán)境下依然能夠有效地跟蹤并且估計(jì)目標(biāo)剛體的位姿;

2)當(dāng)某一局部子系統(tǒng)出現(xiàn)遮擋時(shí),系統(tǒng)整體融合結(jié)果并不會(huì)與另一有效的局部估計(jì)結(jié)果完全重合,原因在于融合過程中綜合了另一局部估計(jì)信息和之前的先驗(yàn)知識(shí),使其得到自己的結(jié)果,但是隨著時(shí)間的推移,系統(tǒng)整體融合結(jié)果會(huì)趨向有效的局部估計(jì)結(jié)果.

圖12 各個(gè)軸跟蹤軌跡Fig.12 Tracking trajectory of each axis

為了客觀驗(yàn)證算法的有效性,對(duì)系統(tǒng)的累積誤差進(jìn)行了統(tǒng)計(jì).為計(jì)算方便,令某一時(shí)刻出現(xiàn)嚴(yán)重遮擋的子系統(tǒng)的誤差與該時(shí)刻系統(tǒng)整體融合結(jié)果的誤差相等.通過圖13可以看到,本文算法在遮擋情況下的累計(jì)誤差小于每個(gè)局部估計(jì)的累計(jì)誤差.從圖14可以發(fā)現(xiàn),本文算法在整個(gè)跟蹤過程中的累積誤差小于局部子系統(tǒng)使用AUKF的累積誤差,證明了所提出算法的有效性.綜上所述,過程噪聲自適應(yīng)分布式融合方法在系統(tǒng)出現(xiàn)遮擋的情況下仍然能有效地估計(jì)目標(biāo)剛體的位姿,且其估計(jì)結(jié)果優(yōu)于各個(gè)局部估計(jì).

圖13 遮擋時(shí)刻各個(gè)軸累計(jì)誤差Fig.13 Accumulative error of each axis during occlusion time

圖14 各個(gè)軸累計(jì)誤差Fig.14 Accumulative error of each axis

6 小結(jié)

針對(duì)在剛體位姿估計(jì)過程中出現(xiàn)的遮擋情況,本文提出了AUKF 分布式融合方法,通過引入改進(jìn)的Sage-Husa噪聲估計(jì)器更新過程噪聲,并針對(duì)不同的遮擋情況采用對(duì)應(yīng)的方案,詳細(xì)描述了所提出的兩種遮擋問題的處理方法,最后,通過仿真確定了區(qū)分部分遮擋和嚴(yán)重遮擋的閾值,并進(jìn)行實(shí)驗(yàn)驗(yàn)證了該方法具有很強(qiáng)的魯棒性和有效性.本文所提方法適用于遮擋情況下的剛體位姿估計(jì),后續(xù)工作將針對(duì)遮擋情況下的手術(shù)器械位姿跟蹤進(jìn)行深入研究.