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

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

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

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

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

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

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

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

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

2 3D Zernike矩配準算法

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

(1)

圖1 系統(tǒng)結構Fig.1 System structure

(2)

(3)

(4)

(5)

(6)

(7)

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

(8)

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

(9)

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

(10)

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

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

1) 將導航計算機中空間站外表面三維點云作為3D Zernike矩匹配的基準圖。

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

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

(11)

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

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

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

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

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

(12)

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

(13)

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

(14)

6) 對G進行特征值分解，求得最大的特征值以及其對應的特征向量：

(15)

(16)

T=μq-Rμp

(17)

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

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

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

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

fc=f0+fb+fε

(18)

ωc=ω0+ωb+ωε

(19)

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

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

(20)

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

(21)

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

(22)

(23)

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

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

(24)

3.2 觀測方程的建立

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

(25)

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

同樣對觀測方程進行線性化：

(26)

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

4 實驗及結果分析

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

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

圖2 實驗平臺Fig.2 Experimental platform

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

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

圖4 匹配結果Fig.4 Matching results

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

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

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

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

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

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

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

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

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

4.2 組合導航算法驗證及分析

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

(27)

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

在表1和表2的仿真條件下進行校驗，得到相對位置和姿態(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

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

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

表3 配準誤差及耗時Table 3 Registration errors and time-consumption

由表3實驗結果可知，3D Zernike矩配準算法在精度及耗時方面優(yōu)于“SAC-IA+ICP”算法；此外，設計的組合導航算法精度優(yōu)于3D Zernike矩算法，能夠估計出巡檢飛行器與空間站間的最優(yōu)相對位姿參數(shù)，實現(xià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 結 論

以空間站表面為“特殊地形”，提出了一種大型航天器表面巡檢的相對導航算法。創(chuàng)新地采用3D Zernike矩匹配算法實現(xiàn)了TOF相機采集的局部實時圖在先驗基準圖上的定位，并解算實時圖與基準圖間的相對位姿參數(shù)，同時進一步分析了影響3D Zernike矩匹配算法精度及耗時的主要因素。以該相對位姿參數(shù)為觀測量設計擴展卡爾曼濾波器對慣性位姿推算系統(tǒng)的誤差進行校正，從而估計出巡檢飛行器與空間站間的最優(yōu)相對導航參數(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.