基于多傳感器的雙機編隊相對定位算法研究?

楊少帥 鄭 威

（1.中國西南電子技術研究所 成都 610036）（2.73602部隊 南京 210000）

1 引言

相對導航技術是編隊飛行器或網(wǎng)絡成員間相對位置確定的一項關鍵技術［1～2］。廣泛應用于編隊衛(wèi)星高精度的相對位置確定、航天器交會對接、戰(zhàn)斗機編隊飛行、無人機自主著艦、自主空中加油等領域［3～4］。目前國內(nèi)相對導航的研究主要集中在基于慣性與相對測距的相對導航［5～8］、基于偽距差分的相對導航［9］、基于衛(wèi)星和慣導組合的定位導航［10］，但是沒有將衛(wèi)星導航差分的高精度、慣性導航的自主性魯棒性以及測距測角信息的直接測量有效地結合起來。

慣性導航系統(tǒng)具有運行的自主性，能夠連續(xù)提供載體的位置、速度和姿態(tài)信息，但是測量誤差隨時間累積，需要初始對準和不斷修正。衛(wèi)星導航系統(tǒng)精度高，但信號容易受阻擋、干擾，信號中斷后需要數(shù)秒的重新捕獲時間。衛(wèi)星導航與慣性導航系統(tǒng)，兩者結合，取長補短，是公認的理想的組合導航方式［11］。協(xié)同成員間，基于時間同步技術和天線技術，可以獲取相對距離信息和相對角度信息。以慣導作為公共參考源，進行傳感器數(shù)據(jù)的濾波預測，并融合多源數(shù)據(jù)，提供一種可靠性好、精度高、魯棒性強的相對導航解決方案［12～13］。

本文以長機-僚機編隊的情景為例，基于相對運動誤差方程，推導了偽距差分、相對測距測角觀測方程。利用聯(lián)邦濾波器，估計相對位置、速度、姿態(tài)狀態(tài)。在傳感器觀測量都可用的情況系下，融合多源信息，得到精度高、可靠性好的結果；在衛(wèi)星導航差分結果不可用的情況下，僅利用慣導和測距測角，保證相對導航精度，并通過仿真驗證該方法的有效性。

2 相對導航方程

相對導航方程根據(jù)相對運動學關系推導而來，由相對姿態(tài)、相對位置、相對速度、以及陀螺、加速度計的漂移構成。由于誤差狀態(tài)方程濾波后精度高，所以本文先給出相對狀態(tài)方程，再給出相對誤差狀態(tài)方程，并利用誤差方程進行求解。

2.1 相對狀態(tài)方程

2.2 相對狀態(tài)誤差方程

3 相對導航觀測方程

3.1 偽距差分觀測方程

采用偽距差分方法時，長機并不單獨解算自己的絕對位置，而是將得到的偽距觀測量，通過鏈路傳到僚機，由僚機統(tǒng)一進行處理，如圖1所示。長機和僚機同時獲得相對于同一顆導航星的偽距進行差分。

圖1 偽距差分

將由偽距差分計算的觀測值Z?ρ與通過INS計算得到的相對偽距觀測值相減，推導偽距差分觀測方程。

3.2 測距與測角觀測方程

4 多傳感器信息融合

4.1 聯(lián)邦濾波器

相對導航手段除了傳統(tǒng)的INS，衛(wèi)導之外，還可利用測距、測角的等傳感器。多種觀測量的信息需要一種有效融合方式。多源組合導航系統(tǒng)濾波結構主要有集中濾波和聯(lián)邦濾波，集中濾波精度高、實時性好，但易受干擾；聯(lián)邦濾波容錯能力強，便于故障診斷與隔離。多傳感器相對導航的算法結構如圖2所示。

圖2 多傳感器相對導航算法結構圖

子濾波器1、2、3為標準卡爾曼濾波，主濾波器由三個局部濾波器得到各自得到相對狀態(tài)誤差估計值后，再在主濾波器中根據(jù)信息分配的原則進行融合可得最終相對狀態(tài)誤差估計值，融合過程如下：

式中：X1(k/k),X2(k/k),X3(k/k)分別是子濾波器1、2、3輸出的相對狀態(tài)誤差估計值，P1(k/k),P2(k/k),P3(k/k),PM(k/k)分別是子濾波器1、2、3和主濾波器的濾波誤差方差陣。

4.2 故障檢測策略

假設有n個傳感器，每個傳感器的測量模型：

ni,k是高斯白噪聲。

故障傳感器的自主檢測隔離是多種傳感器的融合系統(tǒng)的可靠保障，可以通過新息矢量vk來檢驗傳感器數(shù)據(jù)的有效性，首先歸一化的新息矢量為整主濾波器的信息分配系數(shù)，并結合殘差χ2檢測的檢測結果，達到在相對較大故障時隔離故障的效果。

5 算法仿真

5.1 仿真條件

利用軌跡發(fā)生器產(chǎn)生長機和僚機的理想慣導數(shù)據(jù)，在此基礎上，加上陀螺加速度計誤差進行慣導解算。測距觀測、測角觀測以及衛(wèi)導誤差由理想慣導數(shù)據(jù)加誤差計算得到。仿真條件如表1所示。

表1 仿真條件

5.2 聯(lián)邦濾波仿真

由于慣導自身姿態(tài)不準確，所以在計算相對導航誤差時，統(tǒng)一長機和僚機的相對導航計算的坐標系，在地固坐標系下統(tǒng)計導航誤差。相對導航誤差仿真結果如圖3所示。

圖3 相對導航仿真結果

取200s～400s之間的數(shù)據(jù)，濾波穩(wěn)定后，相對導航誤差均值和標準差結果見表2。

從圖3和表2可以看出，相對位置誤差很快就達到1m之內(nèi)。并且在穩(wěn)定后，XYZ三向的相對位置誤差估計能夠穩(wěn)定在1m之內(nèi)，相對速度誤差能夠穩(wěn)定在0.1m/s內(nèi)。

表2 相對導航誤差統(tǒng)計結果

5.3 差分故障相對導航仿真

由于差分需要長僚機實時交互衛(wèi)星導航觀測數(shù)據(jù)，而針對鏈路的不穩(wěn)定造成的差分解算不穩(wěn)定而帶來的較大偏差的情況下，采用基于χ2檢驗的方法檢測，從而選擇性地利用衛(wèi)導差分觀測量。仿真中自200s開始，對偽距差分結果加入100m的隨機誤差和200m常值誤差以模擬衛(wèi)導差分不可用。仿真結果如圖4所示。

圖4 偽距差分出現(xiàn)波動后的仿真結果

從圖4中可以看出，相對位置、速度解算誤差在200s左右，會出現(xiàn)一定程度上的增大。當偽距差分觀測不可用時，僅依靠相對測距和測角與慣性導航的組合方式，相對導航精度會比依靠衛(wèi)導差分有所降低，但仍然可以在衛(wèi)星導航信號不可用的時間內(nèi)，保證相對位置誤差在30m以內(nèi)，相對速度誤差在0.5m/s以內(nèi)。

6 結語

本文建立了相對導航誤差模型和偽距差分、相對測距、測角的觀測方程，研究了存在多種導航傳感器觀測量時的相對導航聯(lián)邦濾波算法。通過仿真分析發(fā)現(xiàn)，利用慣導、偽距差分、測距測角融合處理，能夠達到亞米級別的相對定位精度。以慣導系統(tǒng)為導航參考的聯(lián)邦濾波器能夠在差分結果不可信賴的情況下，利用相對測量，能夠保證相對導航的收斂和系統(tǒng)的魯棒性。