基于3D Zernike矩的巡檢器與空間站的相對導(dǎo)航算法

王潤，郁豐,*，周士兵，劉方武

1. 南京航空航天大學(xué) 空間光電探測與感知工業(yè)和信息化部重點實驗室，南京 211106 2. 南京航空航天大學(xué) 航天學(xué)院，南京 211106 3. 中國科學(xué)院 上海技術(shù)物理研究所，上海 200083

近些年來，隨著空間任務(wù)的日益復(fù)雜，各個國家和研究機(jī)構(gòu)針對空間在軌服務(wù)技術(shù)展開了大量的研究工作[1-2]。作為在軌服務(wù)領(lǐng)域中不可或缺的一部分，航天器表面自主在軌巡檢技術(shù)對于一些大型航天器的長期安全運行、空間操作輔助等具有十分重要的價值[3]。

相對位姿測量是在軌巡檢的前提，也是目前國內(nèi)外科研人員的研究熱點[4]。美國NASA約翰遜空間中心的工程師設(shè)計開發(fā)了一顆自主艙外機(jī)器人相機(jī)(Mini AERCam)[5]。旨在幫助宇航員及地面成員在任務(wù)執(zhí)行期間觀察航天器外部情況。但上述研究存在一定的局限性，Mini AERCam采用慣性/全球定位系統(tǒng)(Global Positioning System, GPS)導(dǎo)航，雖然GPS精度相對較高，但在GPS信號較弱或不存在的情況下，該導(dǎo)航方式會失效[6]。因此，開展新的面向自主在軌巡檢的相對導(dǎo)航算法具有十分重要的意義。

目前，視覺測量系統(tǒng)和慣性推算系統(tǒng)廣泛應(yīng)用于航天器相對導(dǎo)航領(lǐng)域。由于TOF(Time of Flight)相機(jī)具有快速獲取場景深度信息、不受光照環(huán)境限制等優(yōu)點[7-8]，因此近些年來多采用TOF相機(jī)對目標(biāo)航天器表面點云進(jìn)行采集，再利用圖像處理算法解算航天器間的相對位姿參數(shù)[9]。例如，德國錫根大學(xué)、基爾大學(xué)和柏林大學(xué)的研究人員利用TOF相機(jī)快速、實時獲取周圍場景深度信息，而后采用ICP(Iterative Closest Point)算法完成位姿解算[10]。雖然ICP算法在航天領(lǐng)域應(yīng)用廣泛并進(jìn)行了諸多改進(jìn)，但其仍存在對初值的精度要求高、耗時長等弊端[11-12]。故而，目前多采用特征描述子對目標(biāo)關(guān)鍵點進(jìn)行描述，得到點對點的匹配后利用ICP算法對匹配結(jié)果進(jìn)行優(yōu)化，進(jìn)而達(dá)到較好的導(dǎo)航效果。例如，Endres等提出了RGBD-SLAM(RGB Depth map-Simultaneous Localization and Mapping)算法[13]，該算法采用SIFT(Scale-Invariant Feature Transform)/SURF(Speeded Up Robust Features)/ORB(Oriented FAST and Rotated BRIEF)進(jìn)行特征提取和匹配，然后采用ICP算法改善位姿估計，能夠穩(wěn)定可靠地實現(xiàn)匹配定位。但現(xiàn)有的視覺SLAM采用的圖像特征的語義級別低，造成了特征的可區(qū)別性較弱，可利用性有限[14]，不適用于目標(biāo)表面特征不明顯的情況。SAC-IA(SAmple Consensus Initial Aligment)+ICP算法先采用FPFH(Fast Point Feature Histograms)描述子獲取粗匹配結(jié)果，后利用ICP算法進(jìn)行精確匹配[15-16]，該算法精度較高且對表面特征不明顯的目標(biāo)具有較強(qiáng)的適用性，但其計算量較大，導(dǎo)致匹配耗時較長。Novotni和Klein和提出了一種用于描述三維曲面特征的3D Zernike描述子[17]，使算法具有識別能力強(qiáng)、抗噪及抗旋轉(zhuǎn)性能好等優(yōu)勢；該算法屬于面匹配算法，直接利用三維地形曲面代替整個三維地形，保證了曲面上全部點均可參與匹配，使特征點能夠很好地描述三維地形，獲取高精度的匹配結(jié)果。3D Zernike矩匹配算法的耗時與其匹配階次和目標(biāo)表面結(jié)構(gòu)有關(guān)，耗時上具有可控性[18-19]，因而近些年來逐漸應(yīng)用于導(dǎo)航系統(tǒng)中。但目前基于3D Zernike矩的導(dǎo)航研究僅實現(xiàn)了匹配定位功能，尚未能解算出目標(biāo)的位姿參數(shù)。因此，研究如何利用3D Zernike矩算法解算航天器間的相對位姿參數(shù)是實現(xiàn)兩航天器間相對導(dǎo)航的關(guān)鍵，具有一定的現(xiàn)實意義。但由于TOF相機(jī)無法直接獲得速度和角速度信息，且受測量距離、外界環(huán)境情況等影響較大，因此，僅利用圖像處理技術(shù)進(jìn)行航天器間的相對導(dǎo)航有時無法達(dá)到預(yù)期的效果。相比之下，慣性位姿推算系統(tǒng)則可連續(xù)輸出載體的加速度、角速度信息且不依賴于外界環(huán)境，具有短期高精度和高穩(wěn)定性等優(yōu)點，但由于慣性元件存在初始誤差、漂移和噪聲等因素，長時間導(dǎo)航會產(chǎn)生較大的積累誤差[20]。

鑒于視覺測量系統(tǒng)和慣性位姿推算系統(tǒng)均存在其各自的優(yōu)缺點，本文設(shè)計了“視覺/慣性”組合導(dǎo)航系統(tǒng)，以空間站表面為“特殊地形”，利用TOF相機(jī)獲取空間站表面“三維地形曲面”。采用一種基于3D Zernike矩的面匹配算法，以空間站表面三維地形曲面代替空間站表面三維地形，通過地形曲面的匹配實現(xiàn)三維地形的匹配[21-22]，求解出匹配前后兩曲面間的相對位姿參數(shù)，將該位姿參數(shù)與慣性系統(tǒng)推算的相對位姿參數(shù)進(jìn)行信息融合，從而實現(xiàn)航天器間的相對導(dǎo)航，以期有效、高精度地估計空間站與巡檢飛行器間的相對位姿信息，滿足相關(guān)應(yīng)用領(lǐng)域的需求。

1 系統(tǒng)總體設(shè)計

以空間站表面巡檢為背景，研究“視覺/慣性”組合系統(tǒng)的航天器相對導(dǎo)航算法，系統(tǒng)總體結(jié)構(gòu)如圖1所示。

首先通過TOF深度相機(jī)獲取空間站表面局部三維點云數(shù)據(jù)，以該數(shù)據(jù)為實時圖，以導(dǎo)航計算機(jī)中空間站外表面先驗點云數(shù)據(jù)為基準(zhǔn)圖，然后在3D Zernike矩框架下實現(xiàn)實時圖與基準(zhǔn)圖的匹配，并解算實時圖與匹配上的基準(zhǔn)圖間的相對位姿，最后將該相對位姿與慣性系統(tǒng)推算的相對位姿通過擴(kuò)展卡爾曼濾波器(EKF)進(jìn)行信息融合，從而實現(xiàn)兩航天器間的相對導(dǎo)航。

2 3D Zernike矩配準(zhǔn)算法

2.1 3D Zernike矩的定義及性質(zhì)

(1)

圖1 系統(tǒng)結(jié)構(gòu)Fig.1 System structure

(2)

(3)

(4)

(5)

(6)

(7)

式中：Mrst為幾何矩，定義為

(8)

當(dāng)x為三維曲面上的點時，f(x)=1；當(dāng)位于其他位置時，f(x)=0。故而Mrst的離散形式為

(9)

式中：N為點云中所有點的個數(shù)。

(10)

2.2 基于點云的3D Zernike矩匹配算法

由2.1節(jié)中3D Zernike矩的定義及性質(zhì)可知，3D Zernike矩與空間站三維曲面存在一一對應(yīng)關(guān)系，且在匹配過程中，整個三維地形曲面上的點均可參與匹配，這保證了曲面特征的完備性，能夠較好地描述地形特征[25-26]。因而提出了一種基于3D Zernike矩的航天器相對導(dǎo)航算法。利用3D Zernike矩表示空間站三維外表面模型，從而將曲面匹配轉(zhuǎn)化為3D Zernike矩的匹配問題。具體算法步驟如下：

1) 將導(dǎo)航計算機(jī)中空間站外表面三維點云作為3D Zernike矩匹配的基準(zhǔn)圖。

2) 根據(jù)描述三維曲面的精度要求和計算時間要求，預(yù)先設(shè)置匹配所需要的3D Zernike矩的計算階次n。

6) 由于3D Zernike矩描述子向量各元素間的數(shù)量級不盡相同，故而以Canberra距離作為特征向量間的距離測度來消除該問題所產(chǎn)生的判別誤差。Canberra距離d越小，實時圖與當(dāng)前窗口對應(yīng)的基準(zhǔn)圖越相似，Canberra距離定義為

(11)

式中：Fxi和Fyi分別為Fx和Fy的組成元素，且Fxi≥0，F(xiàn)yi≥0，F(xiàn)xi+Fyi≠0。

7) 當(dāng)窗口在基準(zhǔn)圖上遍歷一次后，得到一組Fxi與Fyi之間的Canberra距離di。將di從小到大排序，di最小的窗口位置即為實時圖與基準(zhǔn)圖相似度最大的位置，從而完成實時圖在基準(zhǔn)圖上的匹配。

2.3 相對導(dǎo)航參數(shù)的求解

在完成3D Zernike矩匹配后，進(jìn)一步求解實時圖與相似度最大的基準(zhǔn)圖窗口間的相對位置、相對姿態(tài)，從而確定巡檢器飛行器與空間站間的相對導(dǎo)航參數(shù)。具體算法步驟如下：

3) 點集pi和點集qi的中心點分別為μp和μq：

(12)

4) 點集pi和點集qi中心化處理后，其協(xié)方差矩陣為

(13)

5) 由矩陣K中各元素kij(i,j= 1, 2, 3, 4)構(gòu)造出四維對稱矩陣G：

(14)

6) 對G進(jìn)行特征值分解，求得最大的特征值以及其對應(yīng)的特征向量：

(15)

(16)

T=μq-Rμp

(17)

8) 利用第7)步求得的旋轉(zhuǎn)矩陣R和平移矩陣T對pi進(jìn)行旋轉(zhuǎn)和平移變換，得到新的對應(yīng)點集p′i=Rpi+T。

3 相對位姿參數(shù)濾波器的設(shè)計

3.1 狀態(tài)方程的建立

利用慣性測量單元測量巡檢器的加速度和角速度信息，測量值分別為fc和ωc，由于慣性測量單元中加速度計和陀螺儀的隨機(jī)測量誤差一般含有多種成分的有色隨機(jī)噪聲，建立其數(shù)學(xué)模型必須進(jìn)行“白色化”處理，假設(shè)：

fc=f0+fb+fε

(18)

ωc=ω0+ωb+ωε

(19)

式中：f0為加速度真值；fb為加速度計漂移；fε為加速度計測量噪聲；ω0為角速度真值；ωb為陀螺漂移；ωε為陀螺測量噪聲。

由于主要研究航天器表面巡檢，巡檢器與空間站間的相對距離較小，Hill方程具有足夠的精確性。根據(jù)Hill方程，在空間站軌道坐標(biāo)系下建立兩航天器質(zhì)心間的相對位置運動學(xué)模型：

(20)

假設(shè)空間站軌道坐標(biāo)系為導(dǎo)航坐標(biāo)系，推導(dǎo)巡檢過程中兩航天器間的相對姿態(tài)運動學(xué)模型為

(21)

由式(19)、式(21)可推導(dǎo)出巡檢器相對于空間站的姿態(tài)運動方程：

(22)

(23)

式中：fbε和ωbε分別為加速度計和陀螺的漂移噪聲。

對式(20)、式(22)和式(23)中非線性部分線性化處理推導(dǎo)出系統(tǒng)的線性化狀態(tài)方程：

(24)

3.2 觀測方程的建立

以TOF相機(jī)獲取的空間站表面局部點云數(shù)據(jù)為實時圖，以空間站表面先驗點云數(shù)據(jù)為基準(zhǔn)圖。由于深度相機(jī)采集的點云數(shù)據(jù)往往帶有噪聲，為避免其對匹配性能的影響，需先對實時圖與基準(zhǔn)圖的原始點云數(shù)據(jù)進(jìn)行濾波降噪處理，后利用3D Zernike矩匹配算法進(jìn)行相對位姿的測量，以獲得較好的相對位姿參數(shù)。慣性位姿推算系統(tǒng)提供測量時的相對位置ρ和相對姿態(tài)q。則觀測方程為

(25)

式中：ρc和qc分別為巡檢器相對于導(dǎo)航坐標(biāo)系的位置和姿態(tài)觀測值；ρε和qε分別為實時圖與基準(zhǔn)圖間的相對位置和姿態(tài)誤差。

同樣對觀測方程進(jìn)行線性化：

(26)

式中：δqc13和qε13分別為δqc和qε的矢量部分。

4 實驗及結(jié)果分析

4.1 3D Zernike矩匹配算法的驗證

為驗證所設(shè)計算法的有效性，進(jìn)行了實驗平臺的搭建。假設(shè)空間中空間站的長度為10 m，巡檢器與空間站間的距離為6 m。巡檢器位于空間站下方6 m處，而后沿空間站軌道系X軸正方向運動。根據(jù)TOF相機(jī)的視場范圍，空間中實時圖的大小約為(3×4) m。將上述場景等比例縮小10倍，在搭建的實驗場景中模擬航天器在空間中的運動如圖2所示，空間站模型長度約為1.0 m， 巡檢器與空間站模型間的距離約為0.6 m， 運動過程中TOF相機(jī)拍攝的模型實時圖大小約為(0.3×0.4) m。由于巡檢器以及空間站的三軸姿態(tài)可能均存在小角度的偏差，這種偏差會導(dǎo)致實時圖與基準(zhǔn)圖邊緣點云數(shù)據(jù)的差異，為避免邊緣差異點對匹配結(jié)果的影響，以實時圖的重心為中心，在實時圖內(nèi)部截取(0.2×0.2) m的矩形作為匹配模板，并將實時圖依次與基準(zhǔn)圖進(jìn)行配準(zhǔn)，得到實時的相對位置、姿態(tài)參數(shù)作為后續(xù)信息融合的觀測量。

圖2 實驗平臺Fig.2 Experimental platform

3D Zernike矩的計算耗時與其匹配階次成指數(shù)關(guān)系，而匹配階次的選取與地形起伏情況有關(guān)，地形起伏越劇烈，所需匹配階次越高。根據(jù)采用的空間站模型外表面起伏變化情況、配準(zhǔn)精度及時間要求，預(yù)先設(shè)置匹配階次n=4。假設(shè)巡檢飛行器運行過程中某一時刻實時圖如圖3所示，此時實時圖與基準(zhǔn)圖間的相對位姿分別為[0.1, 0, 0] m、[1, 1, 1]°。將該實時圖與基準(zhǔn)圖在3D Zernike矩框架下進(jìn)行匹配及相對位姿解算，結(jié)果如圖4所示。

圖3 實時圖Fig.3 Real-time map

圖4 匹配結(jié)果Fig.4 Matching results

由圖4的實驗結(jié)果可知，基于3D Zernike矩的匹配算法能夠?qū)崿F(xiàn)實時圖在基準(zhǔn)圖上的匹配，相對位置精度優(yōu)于0.005 m，相對姿態(tài)精度優(yōu)于0.3°。但在實際情況中，實時圖與基準(zhǔn)圖間的位姿差異及點云數(shù)量的不同會導(dǎo)致配準(zhǔn)效果的不同，從而對導(dǎo)航系統(tǒng)的精度產(chǎn)生一定的影響。故而進(jìn)一步研究了3D Zernike矩匹配精度及速度的主要影響因素。

4.1.1 姿態(tài)差異對配準(zhǔn)精度的影響

假設(shè)實時圖與基準(zhǔn)圖間三軸位置差異值為[0.1, 0, 0] m；將上述實時圖與基準(zhǔn)圖間的三軸角度差異值依次設(shè)置為[1, 1, 1]°、[2, 2, 2]°、[3, 3, 3]°、[4, 4, 4]°，則采用3D Zernike矩匹配后解算出的相對位姿誤差如圖5所示。

圖5 相對位姿誤差Fig.5 Relative pose errors

由圖5可知，實時圖與基準(zhǔn)圖間的姿態(tài)差異越大，3D Zernike矩匹配誤差越大，解算出的相對位姿參數(shù)誤差越大。此外，由于三軸角度變化過程中俯仰角的變化會導(dǎo)致實時圖點云在俯仰軸上的大幅度移動，故而Y軸的相對位姿誤差最大，而航向軸作為平面內(nèi)軸向，受三軸角度變化的影響最小，因而Z軸的相對位姿誤差最小。

4.1.2 點云數(shù)量對配準(zhǔn)耗時的影響

在保證相同位姿參數(shù)條件下，采用點數(shù)分別為8 000、9 000、10 000、11 000的實時圖依次與基準(zhǔn)圖進(jìn)行匹配，3D Zernike矩匹配的計算耗時如圖6所示。

由圖6可知，預(yù)先匹配階次及基準(zhǔn)圖一定的前提下，參與匹配的實時圖點云數(shù)量與3D Zernike矩匹配的計算耗時成正比關(guān)系，點云數(shù)量越多，計算耗時越長。

4.1.1節(jié)和4.1.2節(jié)的兩組實驗證明，姿態(tài)差異越大、點云數(shù)量越多，3D Zernike矩匹配算法測量的觀測值誤差及延遲越大，故而在后續(xù)信息融合過程中需充分考慮上述影響因素，以達(dá)到較好的導(dǎo)航精度。

4.2 組合導(dǎo)航算法驗證及分析

根據(jù)第3節(jié)中建立的系統(tǒng)模型，分別設(shè)置濾波器的各項參數(shù)，濾波初值的協(xié)方差矩陣為

(27)

表1 濾波參數(shù)Table 1 Filtering parameters

在表1和表2的仿真條件下進(jìn)行校驗，得到相對位置和姿態(tài)估計誤差如圖7和圖8所示。

表2 初始仿真誤差參數(shù)Table 2 Initial simulation error parameters

圖7 相對位置估計誤差Fig.7 Relative position estimation errors

圖8 相對姿態(tài)估計誤差Fig.8 Relative attitude estimation errors

仿真結(jié)果表明，設(shè)計的信息融合方法能有效獲取巡檢器與空間站間的相對導(dǎo)航參數(shù)。相對位置和姿態(tài)在很短的時間內(nèi)收斂，濾波效果較好。

為進(jìn)一步證明設(shè)計的“3D Zernike矩+慣性”組合導(dǎo)航算法的有效性，將該組合導(dǎo)航算法與上述3D Zernike矩算法以及相同實驗條件下導(dǎo)航效果相對較好的“SAC-IA+ICP”算法進(jìn)行實驗對比。3種算法的配準(zhǔn)誤差及耗時結(jié)果如表3所示。

表3 配準(zhǔn)誤差及耗時Table 3 Registration errors and time-consumption

由表3實驗結(jié)果可知，3D Zernike矩配準(zhǔn)算法在精度及耗時方面優(yōu)于“SAC-IA+ICP”算法；此外，設(shè)計的組合導(dǎo)航算法精度優(yōu)于3D Zernike矩算法，能夠估計出巡檢飛行器與空間站間的最優(yōu)相對位姿參數(shù)，實現(xiàn)高精度相對導(dǎo)航。

此外，還對加速度計、陀螺儀的漂移誤差進(jìn)行了估計。在表1和表2的仿真條件下，加速度計和陀螺儀的漂移估計誤差如圖9和圖10所示。

圖9 加速度計漂移估計誤差Fig.9 Accelerometer drift estimation errors

圖10 陀螺儀漂移估計誤差Fig.10 Gyroscope drift estimation errors

由圖9和圖10可知，慣性測量單元加速度計和陀螺儀漂移誤差能夠很快收斂，收斂后幾乎無波動；加速度計漂移估計精度優(yōu)于0.005 m/s2，陀螺儀漂移估計精度優(yōu)于16.5 (°)/h；表明了本文算法的有效性，可以在一定程度上補償加速度計和陀螺的漂移。

5 結(jié) 論

以空間站表面為“特殊地形”，提出了一種大型航天器表面巡檢的相對導(dǎo)航算法。創(chuàng)新地采用3D Zernike矩匹配算法實現(xiàn)了TOF相機(jī)采集的局部實時圖在先驗基準(zhǔn)圖上的定位，并解算實時圖與基準(zhǔn)圖間的相對位姿參數(shù)，同時進(jìn)一步分析了影響3D Zernike矩匹配算法精度及耗時的主要因素。以該相對位姿參數(shù)為觀測量設(shè)計擴(kuò)展卡爾曼濾波器對慣性位姿推算系統(tǒng)的誤差進(jìn)行校正，從而估計出巡檢飛行器與空間站間的最優(yōu)相對導(dǎo)航參數(shù)，為大型航天器近距離在軌巡檢操作的安全可靠性提供了技術(shù)支持。

[21] WANG K D, ZHU T Q, WANG J L. Real-time terrain matching based on 3D zernike moments[J]. The Journal of Navigation, 2018, 71(6): 1441-1459.