寧明峰,張世杰, 張翰墨
(1.哈爾濱工業(yè)大學(xué) 衛(wèi)星技術(shù)研究所,黑龍江 哈爾濱 150080; 2.上海航天控制技術(shù)研究所,上海 201109)
基于主動(dòng)特征選擇的非合作航天器魯棒視覺(jué)導(dǎo)航方法研究
寧明峰1,張世杰1, 張翰墨2
(1.哈爾濱工業(yè)大學(xué) 衛(wèi)星技術(shù)研究所,黑龍江 哈爾濱 150080; 2.上海航天控制技術(shù)研究所,上海 201109)
面向非合作目標(biāo)航天器近距離操作任務(wù),針對(duì)采用自然特征的單目視覺(jué)相對(duì)位姿參數(shù)確定過(guò)程中特征提取與匹配導(dǎo)致的粗大誤差增加導(dǎo)致結(jié)果不準(zhǔn)確甚至錯(cuò)誤,以及特征數(shù)量多增大計(jì)算量等問(wèn)題,提出一種融合隨機(jī)采樣一致性(RANSAC)算法和主動(dòng)特征選擇的魯棒視覺(jué)導(dǎo)航方法。用RANSAC算法剔除有粗大誤差的特征點(diǎn),給出了基于RANSAC的特征點(diǎn)選擇步驟;根據(jù)不同特征點(diǎn)組合所計(jì)算的克拉美羅(CRLB)不同,用參數(shù)化CRLB下限選擇對(duì)位姿確定精度有顯著影響的點(diǎn)以減少參與計(jì)算的特征數(shù)量,給出了基于CRLB的特征點(diǎn)選擇流程。仿真結(jié)果表明:綜合RANSAC和CRLB的特征點(diǎn)選擇方法可顯著減少特征點(diǎn)數(shù)量,提高了位姿解算精度。
非合作目標(biāo); 視覺(jué)導(dǎo)航; 特征點(diǎn)選擇; RANSAC; CRLB; 特征點(diǎn)數(shù); 魯棒性; 位姿精度
目前,航天器在軌維修等非合作航天器接近任務(wù)是航天器相對(duì)導(dǎo)航的主要應(yīng)用之一。與合作航天器目標(biāo)不同,非合作目標(biāo)航天器未安裝目標(biāo)標(biāo)識(shí)器,追蹤航天器不能直接獲得固定的標(biāo)識(shí)器特征信息。這增大了目標(biāo)航天器特征點(diǎn)提取與匹配的難度,降低了位姿解算的精度。因此,選擇非合作目標(biāo)表面特征點(diǎn)以提高解算精度,是目前亟需解決的問(wèn)題之一。航天器有明顯的邊緣點(diǎn)和角點(diǎn),可作為目標(biāo)特征點(diǎn),結(jié)合航天器自身結(jié)構(gòu)信息,將航天器模型簡(jiǎn)化為特征點(diǎn)模型,進(jìn)而可基于這些特征點(diǎn)解算相對(duì)狀態(tài)信息。用這種方法,可充分利用航天器表面特征,在不安裝目標(biāo)標(biāo)識(shí)器條件下能計(jì)算相對(duì)狀態(tài),適于非合作目標(biāo)的相對(duì)狀態(tài)測(cè)量[1-2]。文獻(xiàn)[3]針對(duì)模型尺寸已知的非合作目標(biāo),在目標(biāo)三維特征點(diǎn)和圖像二維特征點(diǎn)坐標(biāo)已知但匹配關(guān)系未知的條件下,對(duì)基于SoftPOSIT算法的非合作目標(biāo)航天器間相對(duì)位姿估計(jì)方法進(jìn)行了研究,所得結(jié)果精度較高。但因空間光照及航天器表面特征不明顯,導(dǎo)致特征點(diǎn)提取難度大且易造成個(gè)別特征點(diǎn)的提取和匹配誤差大[4]。若將這些特征直接用于相對(duì)狀態(tài)解算,則會(huì)導(dǎo)致計(jì)算結(jié)果偏差過(guò)大甚至出現(xiàn)計(jì)算結(jié)果錯(cuò)誤。另外,考慮不同特征點(diǎn)對(duì)解算精度的影響各異,需用一種新方法從提取的特征點(diǎn)中選取一定數(shù)量的特征點(diǎn),以提高目標(biāo)航天器與追蹤航天器的相對(duì)狀態(tài)信息解算精度。為此,本文對(duì)基于主動(dòng)特征選擇的非合作航天器魯棒視覺(jué)導(dǎo)航方法進(jìn)行了研究,采用RANSAC算法剔除特征點(diǎn)中有粗大誤差的點(diǎn),利用參數(shù)化CRLB的特征有效模型,從RANSAC計(jì)算的內(nèi)點(diǎn)中選取使CRLB最小的特征點(diǎn)集合用于求解相對(duì)狀態(tài)參數(shù)。由于RANSAC消除了誤差大的點(diǎn),用CRLB模型從已有點(diǎn)中選取最優(yōu)解算的最小集合,提出的魯棒性特征點(diǎn)選擇算法有較高的精度。
圖像坐標(biāo)系:原點(diǎn)位于圖像平面的左上角;μ,ν分別為像素的水平與垂直坐標(biāo)。圖像的像素坐標(biāo)為(μ,ν)。
相機(jī)坐標(biāo)系:以投影中心為坐標(biāo)原點(diǎn);Oczc軸與光軸重合;Ocxc、Ocyc軸分別與圖像像素坐標(biāo)系中的μ、ν軸平行且方向一致,Ocxc、Ocyc、Oczc軸構(gòu)成右手坐標(biāo)系。
目標(biāo)航天器體坐標(biāo)系OI-XIYIZI:為固連坐標(biāo)系,原點(diǎn)為目標(biāo)航天器質(zhì)心;三軸分別沿三個(gè)慣性主軸,構(gòu)成右手坐標(biāo)系。
各坐標(biāo)系如圖1所示。
圖1 相機(jī)模型及坐標(biāo)系定義Fig.1 Camera model and coordinate system definitions
(1)
式中:q為姿態(tài)四元數(shù);符號(hào)“*”表示共軛;Cct為目標(biāo)航天器坐標(biāo)系至追蹤航天器體坐標(biāo)系的轉(zhuǎn)換矩陣;t為目標(biāo)航天器坐標(biāo)系質(zhì)心在相機(jī)坐標(biāo)中的坐標(biāo)[5-6]。另在圖像坐標(biāo)系中,有
(2)
2.1 總體方案
在視覺(jué)導(dǎo)航過(guò)程中,獲取的目標(biāo)圖像包含各種特征,可能有背景特征、航天器尖銳的角點(diǎn)特征、太陽(yáng)帆板等有明顯幾何形狀的特征。航天器視覺(jué)導(dǎo)航特征選擇的基本任務(wù)是從航天器眾多的特征點(diǎn)中剔除圖像處理產(chǎn)生的粗大誤差及虛假無(wú)效特征,從而獲得最優(yōu)表決集。在此基礎(chǔ)上,用CRLB減少冗余特征并選擇空間構(gòu)型優(yōu)異的特征,減少計(jì)算量并提高視覺(jué)導(dǎo)航算法的魯棒性??傮w方案如圖2所示。
圖2 魯棒性特征點(diǎn)選擇方法總體方案Fig.2 Robust feature point selection method overall scheme
假設(shè)目標(biāo)航天器模型已知,追蹤航天器中有目標(biāo)航天器的表面特征信息。當(dāng)獲取目標(biāo)航天器圖像后,處理得到目標(biāo)航天器表面特征特征點(diǎn)信息。獲得圖像后,將其與追蹤航天器中保存的圖像或上一幅圖進(jìn)行特征對(duì)比并匹配,獲取其對(duì)應(yīng)特征點(diǎn)先驗(yàn)信息。這些特征點(diǎn)作為待選特征點(diǎn),經(jīng)由RANSAC,CRLB選點(diǎn)后,獲得所需的特征點(diǎn),將最后得到的特征點(diǎn)用于相對(duì)位姿計(jì)算。
2.2 RANSAC算法
傳統(tǒng)的視覺(jué)導(dǎo)航相對(duì)狀態(tài)估計(jì)算法,如最小二乘法等,利用提供的所有測(cè)量值最優(yōu)化擬合目標(biāo)函數(shù)。這些算法未采取任何措施判斷和剔除粗大誤差,均建立在平滑假設(shè)基礎(chǔ)上,因此無(wú)論測(cè)量集的大小,總存在足夠多的數(shù)據(jù)平滑任何粗大誤差[7-8]。但實(shí)際的相對(duì)狀態(tài)解算過(guò)程并不能保證平滑假設(shè)成立,即測(cè)量結(jié)果中包含不可平滑的粗大誤差。為處理這種情況,常先用所有數(shù)據(jù)計(jì)算模型參數(shù),再確定偏離該模型最遠(yuǎn)的數(shù)據(jù),假設(shè)其為粗大誤差予以剔除,重復(fù)上述過(guò)程直至最大數(shù)據(jù)偏差小于某預(yù)先設(shè)定的閾值或沒(méi)有足夠的數(shù)據(jù)進(jìn)行上述過(guò)程。當(dāng)一組有效數(shù)據(jù)中混入一個(gè)粗大誤差時(shí)上述探索性方法易失效。因此,平均法不是處理含粗大誤差數(shù)據(jù)的合適算法,使用特征點(diǎn)前必須降低這些粗大誤差的影響。
RANSAC算法可有效解決此問(wèn)題。其要點(diǎn)是首先根據(jù)具體問(wèn)題設(shè)計(jì)某種目標(biāo)函數(shù),然后通過(guò)反復(fù)提取最小點(diǎn)集估計(jì)該函數(shù)中的初值,利用這些初始參數(shù)值根據(jù)一定的判斷準(zhǔn)則將所有的數(shù)據(jù)分為“內(nèi)點(diǎn)”(滿足估計(jì)參數(shù)的點(diǎn))和“外點(diǎn)”(不滿足估計(jì)的參數(shù)點(diǎn)),由所有內(nèi)點(diǎn)組成的點(diǎn)集稱(chēng)為最小點(diǎn)集的表決集,利用滿足一定條件的有效表決集內(nèi)的所有“內(nèi)點(diǎn)”重新計(jì)算和估計(jì)函數(shù)的參數(shù)。本文基于RANSAC的特征點(diǎn)選擇方法過(guò)程如下。
步驟1:從所有特征點(diǎn)中隨機(jī)選擇6個(gè),建立估計(jì)模型,恢復(fù)相對(duì)狀態(tài)信息。
步驟2:用步驟1建立的模型測(cè)試其它特征點(diǎn)數(shù)據(jù),若某個(gè)特征點(diǎn)適于估計(jì)模型,則將其作為“內(nèi)點(diǎn)”;否則,認(rèn)為該點(diǎn)為“外點(diǎn)”。
步驟3:若有足夠多的“內(nèi)點(diǎn)”滿足某一估計(jì)模型,則認(rèn)為該模型足夠合理,滿足該模型的所有“內(nèi)點(diǎn)”即為RANSAC所篩選的特征點(diǎn),結(jié)束篩選。
步驟4:若測(cè)試完所有特征點(diǎn)仍沒(méi)有足夠多的“內(nèi)點(diǎn)”滿足某一模型,則認(rèn)為該模型不合理,重新從步驟1開(kāi)始。
步驟5:若以上過(guò)程被執(zhí)行預(yù)先設(shè)定次數(shù)后,每次都因無(wú)足夠多“內(nèi)點(diǎn)”形成模型被舍棄,則使用RANSAC選擇特征點(diǎn)失敗。這表明此圖像不適于解算相對(duì)狀態(tài)信息,舍棄該圖像。
RANSAC是一種簡(jiǎn)單但具有高魯棒性的算法,用其可基本剔除粗大誤差的特征點(diǎn),滿足平滑假設(shè)。
2.3 基于CRLB的特征點(diǎn)自適應(yīng)選擇方法
CRLB表示了一個(gè)確定參數(shù)估計(jì)的協(xié)方差的下界,是最簡(jiǎn)單的形式——邊界狀態(tài)。如一個(gè)無(wú)偏估計(jì)達(dá)到此下界,可以稱(chēng)為全效率。此結(jié)果在所有無(wú)偏估計(jì)方法中達(dá)到了一個(gè)最小的均方誤差,因此也可稱(chēng)作是最小協(xié)方差(MVU)估計(jì)[9-11]。
CRLB可表示為
(3)
(4)
式中:p(x;θ)為待估計(jì)變量θ與觀測(cè)值x的極大似然函數(shù)。
相對(duì)位姿視覺(jué)測(cè)量的CRLB模型建立如下。假設(shè)特征點(diǎn)x、y軸均受到高斯白噪聲(0,σ2)影響,特征點(diǎn)數(shù)為N,估計(jì)所求變量
的最低方差,其極大似然函數(shù)
(5)
式中:q0,q1,q2,q3為q的四個(gè)分量。
式(5)取對(duì)數(shù),有
(6)
(7)
式中:d=1,2,3;i=1,2,3,…,N??傻脤?duì)各估計(jì)量求二階偏導(dǎo)表達(dá)式如下。
a)期望Et1t1
(8)
b)dt1t2的期望Et1t2
Et1t2=dt1t2=0.
(9)
c)dt1t3的期望Et1t3
(10)
d)dt1q的期望Et1q
(11)
e)dt2t2的期望Et2t2
(12)
f)dt2t3的期望Et2t3
(13)
g)dt2q的期望Et2q
(14)
h)dt3t3的期望Et3t3
(15)
i)dt3q的期望Et3q
(16)
j)dqq的期望Eqq
Eqq=
(17)
可得基于特征點(diǎn)的位姿參數(shù)估計(jì)的方差滿足
(18)
式中:Mθ為位姿參數(shù)估計(jì)的參數(shù);
(19)
2.4 基于CRLB特征點(diǎn)選擇
RANSAC剔除了提取匹配特征點(diǎn)中具有粗大誤差的特征點(diǎn),由于各“內(nèi)點(diǎn)”誤差及位置不同,其對(duì)解算結(jié)果的影響亦不相同,必然會(huì)導(dǎo)致不同“內(nèi)點(diǎn)”解算的相對(duì)狀態(tài)精度影響各異。
在所有“內(nèi)點(diǎn)”中,如采用誤差小且具有較好相對(duì)位置的部分“內(nèi)點(diǎn)”組合解算相對(duì)狀態(tài),其解算精度必高于將所有“內(nèi)點(diǎn)”作為特征點(diǎn)解算。為能從所有“內(nèi)點(diǎn)”中搜索滿足高精度要求的“內(nèi)點(diǎn)”組合,需要一個(gè)精度評(píng)價(jià)標(biāo)準(zhǔn)以評(píng)判所搜索的“內(nèi)點(diǎn)”組合能有效提高精度。
相對(duì)位姿視覺(jué)測(cè)量的CRLB表示的含義是,對(duì)不同給定特征點(diǎn)其所解算的相對(duì)狀態(tài)方差最小不同。因此,可將該模型作為這個(gè)精度評(píng)價(jià)標(biāo)準(zhǔn),當(dāng)選擇“內(nèi)點(diǎn)”組合后,將該組合解算的CRLB下限與其它組合解算的CRLB下限進(jìn)行比較,以選定能解算出更高精度的特征點(diǎn)組合。
因相對(duì)位姿的CRLB模型對(duì)姿態(tài)四元數(shù)與位移進(jìn)行參數(shù)化,計(jì)算所得的CRLB下限由相應(yīng)的7個(gè)CRLB構(gòu)成。為能對(duì)特征點(diǎn)進(jìn)行比較,本文將各變量的CRLB計(jì)算結(jié)果加權(quán)相加。借鑒RANSAC算法,CRLB下限特征點(diǎn)選擇開(kāi)始時(shí)依然選擇6個(gè)特征點(diǎn)計(jì)算其CRLB下限的加權(quán)值,并將其與所有RANSAC選擇的“內(nèi)點(diǎn)”CRLB加權(quán)值比較,若所選的6個(gè)特征點(diǎn)CRLB加權(quán)值大于所有“內(nèi)點(diǎn)”CRLB加權(quán)值,則表明,所選擇的點(diǎn)組合不合理,重新選擇6個(gè)計(jì)算;若每次所選擇初始特征點(diǎn)的CRLB加權(quán)值均大于所有“內(nèi)點(diǎn)”組合的CRLB加權(quán)值,且選擇次數(shù)超過(guò)閾值,則認(rèn)為經(jīng)過(guò)RANSAC所選的“內(nèi)點(diǎn)”較合理,之后用所有“內(nèi)點(diǎn)”進(jìn)行相對(duì)狀態(tài)計(jì)算。
若選擇的初始特征點(diǎn)CRLB加權(quán)值小于所有“內(nèi)點(diǎn)”CRLB下限加權(quán)值,則將剩余“內(nèi)點(diǎn)”逐個(gè)加入特征點(diǎn)組合并將新組合與之前組合CRLB加權(quán)值比較,若使CRLB加權(quán)值變小,則保留該點(diǎn)在特征點(diǎn)組合直至測(cè)試完所有“內(nèi)點(diǎn)”。最終的特征點(diǎn)組合即為選擇的特征點(diǎn),用這組特征點(diǎn)組合計(jì)算相對(duì)狀態(tài)信息。
本文基于CRLB下限的特征點(diǎn)選擇借鑒RANSAC剔除流程如圖3所示。
圖3 基于CRLB特征點(diǎn)選擇流程Fig.3 CRLB-based feature point selection process
非合作目標(biāo)航天器表面無(wú)特定標(biāo)識(shí),只能利用其表面特征。在空間環(huán)境中,特征點(diǎn)提取與匹配的難度大于合作目標(biāo)特征點(diǎn),同時(shí)其誤差亦較大。以在軌服務(wù)非合作目標(biāo)相對(duì)位姿測(cè)量為背景,用仿真驗(yàn)證本文的魯棒特征點(diǎn)選擇方法。
假設(shè)目標(biāo)航天器表面特征已知,實(shí)際相對(duì)位姿參數(shù)為相對(duì)位置矢量[1 1.2 10] m;相對(duì)姿態(tài)角[15° 20° 10°];相機(jī)焦距0.01 m。仿真產(chǎn)生的數(shù)據(jù)在22 m2平面上,平面在目標(biāo)航天器1 m處隨機(jī)生成。每次仿真初始特征點(diǎn)介于160~180個(gè)不等,90%特征點(diǎn)誤差隨機(jī)采用方差0.02,0.1 mm2中的一種;10%特征點(diǎn)的方差3 mm2。
連續(xù)仿真300次,所得單獨(dú)RANSAC計(jì)算與采用CRLB及RANSAC組合后的位置與姿態(tài)角誤差分別如圖4、5所示。
圖4 CRLB選擇后位置誤差Fig.4 Position error comparison
圖5 CRLB選擇后姿態(tài)誤差Fig.5 Attitude error comparison
由圖4、5可知:經(jīng)CRLB選點(diǎn)后的特征點(diǎn)解算的相對(duì)位置狀態(tài)信息精度高于只采用RANSAC方法,其解算精度有明顯提高。
為驗(yàn)證使用該算法后特征點(diǎn)數(shù)變化,連續(xù)仿真80次,所得特征點(diǎn)數(shù)分布結(jié)果如圖6所示。
圖6 仿真80次特征點(diǎn)選擇數(shù)分布Fig.6 Number of feature points distribution aftersimulation 80 times
由圖6可知:經(jīng)CRLB篩選后的特征點(diǎn)數(shù)僅為只用RANSAC方法選點(diǎn)數(shù)的約三分之二,這將顯著提高后續(xù)解算速度;仿真時(shí)出現(xiàn)采用CRLB選點(diǎn)前后數(shù)一樣多,表明此次仿真沒(méi)有符合CRLB選點(diǎn)的特征點(diǎn)組合,直接采用RANSAC選擇的特征點(diǎn)解算相對(duì)位姿。
本文針對(duì)在軌維修等非合作目標(biāo)航天器空間視覺(jué)導(dǎo)航過(guò)程中,特征點(diǎn)提取與匹配過(guò)程中出現(xiàn)的粗大誤差,影響解算精度的問(wèn)題,在RANSAC算法的基礎(chǔ)上,提出了一種有高魯棒性的RANSAC與CRLB結(jié)合的選點(diǎn)方法。該法顯著減少了有效特征點(diǎn)數(shù)量,同時(shí)提高了位姿解算精度。對(duì)本文的算法進(jìn)行了數(shù)值仿真,驗(yàn)證了其在對(duì)特征點(diǎn)數(shù)量減少與解算精度提高的有效性。
[1] MATTHIES L, BROCKERS R, KUWATAY, et al. Stereo vision-based obstacle avoidance for micro air vehicles using disparity space[C]∥ 2014 IEEE
International Conference on Robotics and Automation (ICRA). IEEE, 2014: 3242-3249.
[2] WEEDEN B C, CHOW T, LUKASZCZYK A, et al. International perspectives on on-orbit satellite servicing and active debris removal and recommendations for a sustainable path forward[C]// 64thInternational Astronautical Congress. Beijing: [s. n.], 2013: IAC-13, E34, 7, X16786.
[3] 張?chǎng)? 張雅聲, 程文華, 等. 基于SoftPOSIT算法的單目視覺(jué)非合作目標(biāo)相對(duì)位姿估計(jì)[J]. 上海航天, 2016, 33(3): 124-129.
[4] WU Y H, GAO Y, LIN J W, et al. Low-cost, high-performance monocular vision system for air bearing table attitude determination[J]. Journal of Spacecraft and Rockets, 2013, 51(1): 66-75.
[5] TWEDDLE B E, SAENZ-OTERO A. Relative computer vision-based navigation for small inspection spacecraft[J]. Journal of Guidance, Control, and Dynamics, 2014, 38(5): 969-978.
[6] TWEDDLE B E, SAENZ-OTERO A. Relative computer vision-based navigation for small inspection spacecraft[J]. Journal of Guidance, Control, and Dynamics, 2014, 38(5): 969-978.
[7] ZHANG K, ZHANG L, YANG M H, et al. Robust object tracking via active feature selection[J]. IEEE Transactions on Circuits and Systems for Video Technology, 2013, 23(11): 1957-1967.
[8] 張世杰, 譚校納, 曹喜濱. 非合作航天器相對(duì)位姿的魯棒視覺(jué)確定方法[J]. 哈爾濱工業(yè)大學(xué)學(xué)報(bào), 2009, 41(7): 6-10.
[9] ZUO L, NIU R, VARSHNEY P K. Posterior CRLB based sensor selection for target tracking in sensor networks[C]// 2007 IEEE International Conference on Acoustics, Speech and Signal Processing-ICASSP'07. [S. l.]: IEEE, 2007, 2: Ⅱ-1041-Ⅱ-1044.
[10] GRISHINV A. A Cramer-Rao bound for the measurement accuracy of motion parameters and the accuracy of reconstruction of a surface profile observed by a binocular vision system[J]. Pattern Recognition and Image Analysis, 2008, 18(3): 507-513.
[11] LENG M, TAY W P, SEE C M S, et al. Modified CRLB for cooperative geolocation of two devices using signals of opportunity[J]. IEEE Transactions on Wireless Communications, 2014, 13(7): 3636-3649.
Robust Method Study of Active Feature Selection for Non-Cooperative Spacecraft Vision-Based Navigation
NING Ming-feng1, ZHANG Shi-jie1, ZHANG Han-mo2
(1. Research center of Satellite Technology, Harbin Institute of Technology, Harbin 150080, Heilongjiang, China; 2. Shanghai Institute of Spaceflight Control Technology, Shanghai 201109, China)
To solve the problem that the error of feature points extracting or matching and the number of feature points would lead to inaccurate results or the wrong results and huge amount of calculation on the relative position and attitude parameter determination during non-cooperative target spacecraft proximity operations, a robust method for vision navigation fusing the random sample consensus (RANSAC) algorithm and an active feature selection method was put forward in this paper. First the gross error was eliminated by RANSAC algorithm. The selection steps for feature points were given based on RANSAC algorithm. Then the different points, which had significant impact on determining precision based on Cramér-Rao lower bound (CRLB), were selected to reduce the number of feature involved in the calculation according to the different CRLB calculated from various feature points sets. The features selection flowchart was given based on CRLB. The simulation results showed the selected points by combining of RANSAC and CRLB could be reduced and the precision of position and attitude had been improved.
Non-cooperative spacecraft; Visual navigation; Feature point selection; Random sample consensus (RANSAC); Cramér-Rao lower bound (CRLB); Feature point number; Robust; Precision of position and attitude
1006-1630(2016)06-0136-06
2016-07-05;
2016-11-10
上海航天科技創(chuàng)新基金資助(SAST201444)
寧明峰(1989-),男,博士生, 主要研究方向?yàn)楹教炱饕曈X(jué)導(dǎo)航與控制。
V448.2
A
10.19328/j.cnki.1006-1630.2016.06.020