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

楊少帥 鄭 威

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

1 引言

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

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

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

2 相對導航方程

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

2.1 相對狀態方程

2.2 相對狀態誤差方程

3 相對導航觀測方程

3.1 偽距差分觀測方程

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

圖1 偽距差分

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

3.2 測距與測角觀測方程

4 多傳感器信息融合

4.1 聯邦濾波器

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

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

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

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

4.2 故障檢測策略

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

ni,k是高斯白噪聲。

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

5 算法仿真

5.1 仿真條件

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

表1 仿真條件

5.2 聯邦濾波仿真

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

圖3 相對導航仿真結果

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

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

表2 相對導航誤差統計結果

5.3 差分故障相對導航仿真

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

圖4 偽距差分出現波動后的仿真結果

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

6 結語

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