航天器相對視覺/IMU導航量測修正多速率濾波

曾占魁，曹喜濱，張世杰，華 冰，吳云華

(1.哈爾濱工業(yè)大學衛(wèi)星技術研究所，150008哈爾濱;2.南京航空航天大學微小衛(wèi)星工程技術研究中心，210016南京)

微小衛(wèi)星編隊已成為美國空軍實驗室以及NASA宇航局眾多在研空間科學任務如電子偵查、立體成像、光學干涉等任務的關鍵技術之一［1］，也是實現(xiàn)空間攻防的有效手段，受到國內(nèi)外航天機構的普遍關注.編隊衛(wèi)星協(xié)同工作能滿足多種未來航天任務需求［1-4］.相對狀態(tài)確定是衛(wèi)星編隊的關鍵技術之一，同時也是衛(wèi)星編隊的基本前提.相對軌道與姿態(tài)估計方法有基于差分GPS及類GPS的測量方法，前者為半自主式測量方法，受GPS信號覆蓋范圍影響僅適用于LEO軌道編隊［5］.后者由于無線電接收天線相對距離較近，存在多路徑效應的缺陷［6］.鑒于上述方法的不足，美國德克薩斯A＆M大學研制了視覺相對測量敏感器，通過特征點在焦平面上的投影坐標獲取特征點方向矢量，此方法能夠提供較為精確的相對狀態(tài)測量［7-10］.

單目視覺導航常用于合作航天器，例如在航天器交會對接階段，可事先設置特殊構型的特征點系統(tǒng)，因此可以保證在交會對接期間的相對位姿始終滿足視覺測量條件.針對交會對接的繞飛階段，可以在追蹤航天器上布設2～3個視覺相機，同時對追蹤/目標航天器進行姿態(tài)控制，可以保證目標航天器一直在追蹤航天器的視場內(nèi);針對交會對接的逼近階段，由于雙方相對姿態(tài)保持穩(wěn)定，因此可以一直保證目標航天器的可觀測性.但限于視覺相機的作用距離，該方法適用于近距離編隊飛行的相對導航.隨著距離的增大，視覺導航精度會嚴重下降.

由此可見，基于單目視覺的相對導航能夠滿足多種航天任務的相對狀態(tài)測量需求，但由于視覺敏感器的尺寸和象元限制，以及圖像處理計算量大等特點，使其有效測量距離較短且測量頻率低.慣導(IMU)是航天器導航的首選配置，其可靠性高，屬于全自主導航，短期精度好，能夠以較高的頻率提供位置、速度和姿態(tài)等信息，但其誤差隨時間逐漸變大.本文針對視覺和IMU系統(tǒng)的各自優(yōu)缺點，有機地將兩種系統(tǒng)組合起來，針對近距離航天器編隊研究采用單目視覺/IMU組合的相對導航方法，提高組合導航系統(tǒng)的相對狀態(tài)估計性能和可靠性.針對視覺導航系統(tǒng)與慣導系統(tǒng)輸出頻率不同造成的異速問題，以及由于視覺敏感元件自身特性而造成的視覺系統(tǒng)量測噪聲不確定的特點，本文提出基于量測修正的視覺/IMU多速率卡爾曼算法，以提高濾波器性能，并通過數(shù)學仿真驗證其有效性.

1 問題描述

1.1 坐標系定義

首先給出相關坐標系定義:

1)地心慣性坐標系OXIYIZI:其坐標原點位于地心O，XIYI平面位于赤道平面內(nèi)，XI軸指向春分點Υ，ZI軸指向北極與地球自轉(zhuǎn)軸重合.

2)相對運動參考坐標系oxyz:目標航天器的質(zhì)心為坐標原點o，x軸由地心指向目標星，z軸方向為軌道角動量方向.相對運動參考坐標系又被稱為Hill坐標系，用r表示.

3)體坐標系:原點定義在衛(wèi)星質(zhì)心OC處，XC軸指向主軸方向，YC軸在垂直于主軸的橫截面內(nèi).

需要指出的是本文所用坐標系均為右手坐標系.

1.2 相對運動模型

記為向量s在坐標系N中的時間導數(shù)，sN為s在坐標系N中的分量形式.

目標航天器和追蹤航天器的軌道動力學方程分別為

其中rc和rd分別為目標和追蹤航天器在慣性坐標系中的位置.

則追蹤航天器相對目標航天器的位置為

其二階導數(shù)為

式(1)在相對運動坐標系中可表示為

式中ωo為目標航天器的軌道角速度矢量，在相對運動坐標系中有ωo=［0，0，］T.

略去下標，并假設星間距離遠小于目標航天器的地心距，上式可簡化為

式中rc為目標航天器的地心距，θ為目標航天器的真近點角.

追蹤航天器體坐標系相對于Hill坐標系的相對姿態(tài)采用四元數(shù)q=［q1，q2，q3，q4］T來描述，則相對姿態(tài)運動學方程為

式中ω=［ω1，ω2，ω3］T是追蹤航天器體坐標系相對于Hill坐標系的角速度，有

式中ωd是追蹤航天器的慣性角速度.

在慣性系中求導，有

可進一步描述為

對于追蹤航天器，其姿態(tài)動力學方程可表示為

式中Jd是追蹤航天器的慣量矩陣，Td是所受外部力矩.

經(jīng)整理可得

則追蹤航天器相對目標航天器的姿態(tài)動力學方程為

1.3 組合導航方案

視覺/慣性組合相對導航方案如圖1所示［11］.假設陀螺和加速度計采用正裝方式安裝，其測量即為追蹤航天器體坐標系下的運動參數(shù).陀螺輸出的是追蹤航天器相對慣性坐標系的姿態(tài)角速率在追蹤航天器體坐標系中的分量，加速度計輸出的是加速度在追蹤航天器本體系中的比力投影.在近距離相對導航中可把目標航天器視為相對靜止的目標，而追蹤航天器進行逼近機動.以目標航天器體坐標系為慣性導航參考坐標系.慣性導航系統(tǒng)的加速度計和陀螺儀等慣性敏感器能夠?qū)教炱飨鄬\動產(chǎn)生的加速度和角速度信息進行高速率測量，利用慣性導航算法估計出位置、速度、姿態(tài)角和角速度等多種相對狀態(tài).由于慣性敏感器自身的噪聲漂移將導致系統(tǒng)會產(chǎn)生隨時間累積的誤差，因此本文采用單目視覺系統(tǒng)對慣導系統(tǒng)進行修正.單目視覺相機對目標航天器成像后經(jīng)圖像處理獲取目標的特征信息，然后利用單目視覺導航算法解算出相對位姿.因視覺和慣導系統(tǒng)的采樣周期不同，所以傳統(tǒng)的單速率卡爾曼濾波理論不適用于本文提出的組合導航系統(tǒng)，因此本文提出一種多速率卡爾曼信息融合算法以解決組合系統(tǒng)更新速率不同的問題.

圖1 視覺/慣性組合相對導航方案

2 多速率濾波算法

2.1 視覺/IMU組合導航系統(tǒng)模型

2.1.1 狀態(tài)方程

組合導航系統(tǒng)狀態(tài)量包括平臺失準角誤差、加速度誤差、速度誤差和位置誤差，下面分別加以描述.

假設慣性導航系統(tǒng)IMU中陀螺儀的漂移速度與導航坐標系漂移速度相同，忽略加速度測量計誤差中二階以上小量，則加速度的測量誤差方程有

其分量形式為

式中:f表示加速度計測量的比力;φ表示平臺失準角;▽表示加速度計偏置;w▽a表示加速度計白噪聲;Cn

b表示從追蹤航天器本體系到導航坐標系的姿態(tài)轉(zhuǎn)移矩陣.

在導航坐標系中，追蹤航天器的位置/速度誤差方程為

式中:d14～d36為引力加速度對位置矢量的相對導數(shù)，此處假設為零;(x，y，z)為追蹤航天器在導航坐標系下的坐標分離，可轉(zhuǎn)換為追蹤航天器在目標航天器體坐標系下的位置.

忽略陀螺誤差的二階及二階以上小量，平臺失準角的誤差方程可表示為

其分量形式為

式中ε表示陀螺儀常值漂移，wε表示陀螺儀測量白噪聲.

結合上述3個誤差方程，則組合導航系統(tǒng)狀態(tài)方程為等號右側(cè)的5組分量分別為姿態(tài)誤差角、速度誤差、位置誤差、陀螺漂移和加速度偏置.式(2)中F(t)是狀態(tài)轉(zhuǎn)移矩陣，G(t)和W(t)是系統(tǒng)噪聲矩陣，分別有且W(t)滿足

式中wεx，wεy，wεz表示陀螺隨機漂移;w▽x，w▽y，w▽z表示加速度計隨機偏置.

將狀態(tài)方程離散化為

式中Φk+1·k為一步轉(zhuǎn)移陣，可由下式計算:

式中:T為濾波周期;Wk為系統(tǒng)噪聲序列，滿足:

Qk為系統(tǒng)噪聲序列方差陣，為非負定陣，有Qk=

2.1.2 量測方程

假設單目視覺相機已經(jīng)過標定，視覺導航的關鍵問題在于根據(jù)特征點在目標坐標系中和相機坐標系中的坐標來求解相對位姿參數(shù).一般可采用迭代估計方法，依次對景深和相對位姿進行估計，在此基礎上求解新的景深，不斷迭代上述過程直至解趨于收斂.Haralick［11］提出了一種同時計算目標位置和景深的迭代算法，該方法采用特征值分解來求解相對位姿，本文也采用該算法對視覺導航數(shù)據(jù)進行仿真.

組合導航系統(tǒng)以慣性/視覺導航系統(tǒng)估計的相對位置和姿態(tài)的差值作為濾波器測量值.那么組合導航系統(tǒng)量測方程為

狀態(tài)變量X(t)可表示為

且有

其中:下標INS表示慣導系統(tǒng)測量量;Vis表示視覺導航系統(tǒng)測量量;γ，φ和θ為三軸姿態(tài)角;x，y和z表示位置.Vk為量測噪聲，滿足高斯白噪聲過程，即E［Vk］=0，cov［Vk，Vj］=E［VkVTj］=Rkδkj.同時，Vk和Wk還滿足cov［Wk，Vj］=E［WkVTj］=0，Rk為系統(tǒng)噪聲方差陣，且為正定陣.

2.2 多速率濾波建模

多速率卡爾曼信息融合算法的核心思想是:以導航子系統(tǒng)中最小濾波周期作為組合導航系統(tǒng)的濾波周期，從而提高狀態(tài)更新率.組合導航系統(tǒng)的濾波周期可以分為時間更新和量測更新兩個過程.當濾波時刻沒有慢速率視覺測量值，濾波器僅進行時間更新;當濾波時刻出現(xiàn)慢速率視覺量測值，濾波器同時進行時間更新和量測更新［12-15］.

多速率卡爾曼信息融合濾波算法的優(yōu)點:

1)濾波器能夠始終以最快采樣周期進行狀態(tài)估計，使系統(tǒng)不再受采樣周期不等的影響，提高了數(shù)據(jù)的更新率;

2)提高了數(shù)據(jù)利用率，有助于提高濾波性能;

3)由于在濾波時刻對有無慢速率視覺量測信息進行了判斷，即使視覺導航采樣周期不固定，或者在濾波時刻視覺導航采樣點出現(xiàn)異常中斷，濾波系統(tǒng)即認為沒有出現(xiàn)視覺量測信息，繼續(xù)進行濾波的時間更新.這樣提高了系統(tǒng)冗余性，解決了傳統(tǒng)集中式濾波器因某一子系統(tǒng)的量測更新失敗就會造成濾波性能下降的問題.

組合導航濾波周期內(nèi)的卡爾曼濾波器更新過程包含時間更新和量測更新.卡爾曼濾波迭代過程分為［14］:

1)時間更新.狀態(tài)一步預測為

一步預測誤差方差陣為

2)量測更新.濾波增益矩陣為

狀態(tài)估計為

估計誤差方差陣為

若沒有測量信息輸出，則濾波器僅進行時間更新;有量測信息輸出時，卡爾曼濾波器則要進行正常的時間更新以及量測更新.

從原理上解釋，在濾波時刻k時，如果缺少視覺導航的量測信息輸入，那么濾波器認為系統(tǒng)出現(xiàn)了粗大誤差，此時系統(tǒng)的觀測噪聲方差陣Rk會趨向于無窮大，然后濾波增益矩陣計算中的會趨向于零，濾波增益矩陣也趨近于零，則

表示量測更新過程沒有起到作用，只進行時間更新過程.

記慣性導航解算周期為TINS，視覺導航解算周期為TVision，濾波周期為TFilter，且有

其中N和M均為正整數(shù).

取N=1，M=2～3(視覺導航估計周期不固定).假設視覺導航估計周期和濾波周期均為慣性導航的整數(shù)倍，可認為每個采樣點都有慣性導航的量測值.由于視覺導航采樣頻率慢，且輸出周期并不恒定，當無視覺導航的量測信息的t1，t3，t4時刻，系統(tǒng)濾波增益趨近于零，濾波器僅進行時間更新;在有視覺導航量測信息的t0，t2，t5時刻，濾波器進行時間和量測更新，所有濾波方程都得到更新.可以看出，異速卡爾曼濾波器縮短了濾波周期，增加了對測量信息的判別，提高了數(shù)據(jù)使用率及系統(tǒng)的可靠性.

3 多速率濾波器的量測修正

本文設計的異速濾波只對導航量測信息的有無起到有效隔離作用，但對量測信號的質(zhì)量卻不加以控制，只做了最優(yōu)假設.實際上，視覺與慣性測量信息均可能出現(xiàn)野值、故障、粗大誤差等情況，若不控制的加以修正，會造成估計偏差.本節(jié)研究如何在多速率濾波的基礎上對量測信號質(zhì)量和濾波修正效果加以控制.

3.1 量測噪聲陣的量測估計

在k時刻，系統(tǒng)的觀測值Zk的預測估計值為

記k時刻觀測值為Zk，它與預測值，k-1的差值k為

根據(jù)濾波理論:

記協(xié)方差陣為Bk，則

根據(jù)新息的n組歷史數(shù)據(jù)，可以估算出新息協(xié)方差陣:

則量測噪聲陣的估計值為

濾波量測噪聲陣與量測噪聲估計的差值為

則修正后的量測噪聲估計陣為

為了避免量測數(shù)據(jù)長度不足、野值、故障、粗大誤差等原因造成的估計偏離，量測修正采用一定的比例系數(shù):

3.2 狀態(tài)估計誤差協(xié)方差陣估計

估計誤差方差陣:

設X*

k為狀態(tài)理想值，理想濾波誤差協(xié)方差矩陣為

誤差方差陣差值為

為了控制誤修正的影響程度，狀態(tài)估計誤差協(xié)方差陣修正也采用一定的比例系數(shù):

4 仿真與分析

濾波初值選取如下.相對姿態(tài)角誤差為0.5°、0.5°、0.5°，相對位置誤差為3 m、3 m、3 m;相對速度誤差為 0.01 m/s、0.01 m/s、0.01 m/s;陀螺測量誤差:常值漂移0.035°/h，隨機漂移方差σ2=(0.035°/3h)2;加速度誤差:常值偏置30 μg，隨機偏置方差σ2=(30μg/3)2;視覺導航采樣周期為Tvision=0.05 s，慣性導航采樣周期為TINS=0.01 s.同時為了驗證測量修正對濾波的影響，也對未采用量測修正的多速率相對導航進行仿真，仿真結果如圖2～4所示.

圖2 相對姿態(tài)角估計誤差

圖3 相對速度估計誤差

圖4 相對位置估計誤差

從仿真曲線可以看出兩種多速率濾波方法都 能夠有效處理量測信息異步的問題，在相同的仿真條件下，本文提出的算法具有更好的收斂效果.需要說明的是在無視覺輔助情況下，純慣性組件測量時姿態(tài)趨于發(fā)散，因此不在論文中給出.

此外，對仿真結果進行了統(tǒng)計分析.表1比較了修正前后三軸姿態(tài)角、速度和距離誤差的穩(wěn)態(tài)均值.姿態(tài)角、速度和距離精度最高分別提高了0.03 °、0.02 m/s和0.3 m，表明采用量測修正的多速率組合導航濾波器具有更好的性能.因此數(shù)學仿真驗證了本文提出的視覺/慣性異速卡爾曼濾波融合算法的有效性，能夠完成相對導航任務需求.

表1 濾波器數(shù)據(jù)對比

5 結語

針對近距離航天器編隊或在軌服務等航天任務的相對導航問題，本文采用單目視覺和慣導系統(tǒng)組合的相對測量方式，研究了基于測量修正的多速率卡爾曼信息融合算法.該算法既克服了慣導系統(tǒng)的漂移問題，又解決了視覺導航測量數(shù)據(jù)更新率低的問題.針對視覺敏感元件測量噪聲不確定的缺點，提出了利用測量信息對濾波量測噪聲陣和狀態(tài)估計誤差協(xié)方差陣進行后驗修正的方法.數(shù)學仿真結果表明該方法的有效性和可行性.

［1］ WU Yunhua，CAO Xibin，XING Yanjun，et al.Relative motion coupled control for formation flying spacecraft via convex optimization［J］.Aerospace Science and Technology，2010，14:415-428.

［2］WU Yunhua，CAO Xibin，XING Yanjun，et al.Relative motion integrated coupled control for spacecraft formation via GaussPseudospectralmethod［J］.Transactions of the Japan Society for Aeronautical and Space Sciences，2009，52(177):125-134.

［3］ WU Baolin，WANG Danwei， ENG K P， et al.Nonlinearoptimization oflow-thrusttrajectory for satellite formation:Legendre Pseudospectral approach［J］.Journal of Guidance，Control，and Dynamics，2009，32(4):1371-1381.

［4］吳云華，曹喜濱，張世杰，等.編隊衛(wèi)星相對軌道與姿態(tài)一體化耦合控制方法研究［J］.南京航空航天大學學報，2010，42(1):13-20.

［5］李晨光，韓潮.編隊飛行衛(wèi)星群相對軌道測量研究［J］.北京航空航天大學學報，2005，31(6):614-617.

［6］崔小松，王惠南.基于類GPS技術的編隊衛(wèi)星相對參數(shù)測量研究［J］.飛行器測控學報，2012，31(3):85-89.

［7］WU Yunhua，GAO Yang，LIN Jiawei，et al.Low-cost，high-performance monocularvision system forair bearing table attitude determination［J］.Journal of Spacecraft and Rockets，2014，51(1):66-75.

［8］CRASSIDIS J L，ALONSO R，JUNKINS J L.Optimal attitude and position determination from line-of-sight measurements［J］.The Journal of the Astronautical Sciences，2000，48(2/3):891-896.

［9］CHENG Y，CRASSIDIS J L，MARKLEY F L.Attitude estimation for large field-of-view sensors［J］.Journal of the Astronautical Sciences，2006，54(3/4):433-448.

［10］YANG Y，CAO X.Design and development of the small satellite attitude control system simulator［C］//AIAA Modeling and Simulation Technologies Conference and Exhibit.Reston VA:AIAA，2006:21-24.

［11］HARALICK R M，JOO H，LEE C，et al.Pose estimation from corresponding point data［J］.IEEE Transactions on Systems，Man and Cybernetics，1989，19(6):1426-1446.

［12］曾占魁.視覺/慣性組合相對導航算法及物理仿真研究［D］.哈爾濱:哈爾濱工業(yè)大學，2007.

［13］NING X，F(xiàn)ANG J.Autonomous celestial navigation method for LEO satellite based on unscented Kalman Filter and information fusion［J］.Aerospace Science and Technology，2007，11(2/3):222-228.

［14］GAO S，ZHONG Y，LI W.Robust adaptive filtering method for SINS/SAR integrated navigation system ［J］.Aerospace Science and Technology，2010，15(6):425-430.

［15］XING Yanjun，ZHANG Shijie，ZHANG Jinxiu，et al.Robust-extended Kalman Filterforsmallsatellite attitude estimation in the presence of measurement uncertainties and faults［J］. Proceedingsofthe Institution of Mechanical Engineers.Part G:Journal of Aerospace Engineering，2012，226(1):30-41.