基于SINS/TAN/ADS/MCP的無人機組合導航系統(tǒng)

湯郡郡，胡 偉，劉祥水，尹進軍，宋中建

（國營蕪湖機械廠，蕪湖 241007）

無人機武器系統(tǒng)是未來空中戰(zhàn)場的主力裝備，然而，戰(zhàn)場形勢瞬息萬變，具備快速反應、高機動性能、目標捕獲能力以及協(xié)同作戰(zhàn)、精確打擊的作戰(zhàn)模式是無人機發(fā)展的必然方向。為了保證生存能力和命中率，無人機必須能夠快速起飛，準確進入作戰(zhàn)區(qū)域和返還基地，這就要求無人機導航系統(tǒng)必須同時具備自主可靠、高精度、能快速定位的能力[1]。

捷聯(lián)慣性導航系統(tǒng)（Strapdown Inertial Navigation System, SINS）僅依靠自身就能在全天候條件下，在全球范圍內進行連續(xù)的導航和定位，它自主、隱蔽，能獲取載體完備的運動信息，但是其定位誤差會隨著時間的積累不斷增大；地形輔助導航系統(tǒng)（Terrain Aided Navigation, TAN）是一種利用地形高程特征來進行輔助定位的方法，它自主、隱蔽、連續(xù)、全天候，導航定位誤差不隨時間積累，但它需要有先驗的地形圖且依賴其它導航系統(tǒng)的位置輸出[2]；大氣數(shù)據(jù)系統(tǒng)（Air Data System, ADS）利用安裝在機體上的大氣傳感器提供的壓力、溫度等信息解算載體的空速、大氣高度等信息，不需要依賴外部條件，它可靠性高，不受高度、地形等因素的影響，是一種較為理想的輔助導航設備；電子磁羅盤（Magnetic Compass, MCP）能夠提供航向信息，且其輸出的導航信息誤差不隨時間的增長而增長，適合長時間高精度導航，但是電子磁羅盤受外界影響較大，精度也不易提高。

上述幾種導航方式都存在著一些自身難以克服的缺點，將多種導航方式進行融合，不僅可以取長補短提高導航精度，還可以適當降低對單一導航系統(tǒng)的要求，提高系統(tǒng)的可靠性和容錯性能，滿足無人機導航系統(tǒng)的高要求[3]。本文所述無人機組合導航系統(tǒng)總體方案如圖1所示。

圖1 UAV組合導航系統(tǒng)總體方案Fig.1 UAV integrated navigation system

1 SINS/TAN系統(tǒng)數(shù)學模型

SINS系統(tǒng)利用慣性器件（陀螺儀、加速度計）測量出載體的角速度和線加速度，經(jīng)過積分和各種算法得到載體的位置、速度及姿態(tài)信息。

1.1 SINS誤差模型

選取東北天坐標系為導航坐標系，載體坐標系x軸沿機體橫軸指向右，y軸沿機體縱軸指向前，z軸垂直于x軸和y軸所確定的平面構成右手坐標系。選擇位置誤差、速度誤差、姿態(tài)誤差、陀螺漂移和加速度計偏置作為狀態(tài)量：

其中：δL、δλ分別是緯度、經(jīng)度誤差；δVE、δVN分別是東向、北向速度誤差；EΦ、NΦ、UΦ分別是東向、北向、天向失準角；?bx、?by分別是x、y軸向的加速度計偏置；εbx、εby、εbz分別是x、y、z軸向的陀螺漂移；

系統(tǒng)狀態(tài)方程為

其中：

Cij為姿態(tài)轉移矩陣中的元素。

1.2 SINS/TAN狀態(tài)方程及觀測方程

由參考文獻[10]可知，SINS/TAN系統(tǒng)的狀態(tài)方程就是SINS的狀態(tài)方程，系統(tǒng)觀測量由SINS輸出的位置信息LSINS、λSINS減去TAN輸出的位置信息LTAN、形成，觀測方程可由下式表示：

其中：Vk為TAN子系統(tǒng)的觀測噪聲；

2 SINS/ADS系統(tǒng)數(shù)學模型

利用安裝在載體外側的壓力傳感器、總溫傳感器和攻角傳感器測量載體周圍流場的動壓、靜壓、總溫和攻角，并將這些信息送到計算機中進行解算，得到載體的氣壓高度和速度等導航信息。

2.1 ADS誤差模型

由大氣數(shù)據(jù)系統(tǒng)測量得到的靜壓值計算出的海拔高度，可直接提供給TAN使用。將大氣數(shù)據(jù)系統(tǒng)的速度誤差均建模為一階馬爾可夫過程：

2.2 SINS/ADS狀態(tài)方程及觀測方程

狀態(tài)方程為：

觀測方程為：

3 SINS/MCP系統(tǒng)數(shù)學模型

磁航向測量的誤差是由很多因素造成的，具體可以分為兩類：第一類是系統(tǒng)自身存在的誤差，包括制造誤差、安裝誤差，主要會對三軸磁阻傳感器的輸出信號產生影響；第二類誤差是由電子磁羅盤周圍環(huán)境因素而造成的誤差，主要指羅差。

3.1 MCP誤差模型

使用電子磁羅盤必須經(jīng)過嚴格的誤差補償。經(jīng)過誤差補償以后，電子磁羅盤的誤差可近似為一階馬爾可夫過程：

其中：τMCP、wMCP分別是一階馬爾可夫過程的相關時間和驅動白噪聲。

3.2 SINS/MCP狀態(tài)方程及觀測方程

狀態(tài)方程為：

觀測方程為：

4 組合導航系統(tǒng)設計

4.1 系統(tǒng)設計思想

系統(tǒng)設計框圖如圖2所示，其中：SINS為公共系統(tǒng)，提供位置、速度和姿態(tài)各項信息；SINS與 TAN構成位置局部濾波器，實現(xiàn)位置信息的綜合；SINS與ADS構成速度局部濾波器，實現(xiàn)速度信息的綜合；SINS與MCP構成航向局部濾波器，實現(xiàn)航向信息的綜合。

圖2 濾波器設計結構圖Fig.2 Design structure of filter

3個局部濾波器均為卡爾曼濾波器，它們輸出的數(shù)據(jù)送入全局濾波器進行融合。全局濾波器輸出的數(shù)據(jù)不直接反饋給各局部濾波器，因此不同局部濾波器之間不存在信息交流，某個局部濾波器故障不會影響另外的濾波器，所以整個系統(tǒng)具有很好的容錯性，可以滿足無人機導航系統(tǒng)高精度高可靠性的要求。

4.2 聯(lián)邦卡爾曼濾波器算法

1）局部濾波器算法

時間更新算法：

量測更新算法：

2）全局濾波器算法

全局濾波器不進行濾波處理，只是將各個局部濾波器的估計值按式(14)進行融合，得到全局最優(yōu)估計值和最優(yōu)估計方差陣：

3）信息分配

聯(lián)邦濾波中的信息分配原則如下：

式中：iβ為局部濾波器信息分配因子；mβ為全局濾波器信息分配因子，本文選擇融合重置模式下的聯(lián)邦濾波，有

聯(lián)邦濾波器可以獲得與卡爾曼濾波器相當?shù)墓烙嬀?，同時具有計算量小、容錯性強等優(yōu)點，而影響性能的關鍵是分配系數(shù)的選擇。由于各局部濾波器的估計精度高低可以通過估計誤差協(xié)方差矩陣來反應，所以考慮根據(jù)估計誤差協(xié)方差矩陣的特征值來確定分配系數(shù)，有：

5 仿真分析

仿真條件及參數(shù)設置如下：

UAV初始位置：北緯38°，東經(jīng)120°；SINS初始誤差：緯度 5″，經(jīng)度 5″；東、北、天三向速度誤差0.1 m/s；航向偏差0.5′；加速度計零偏和隨機漂移20 μg；陀螺常值漂移和隨機漂移0.1 (°)/h；TAN位置誤差：50 m；ADS速度誤差：1.2 m/s；電子磁羅盤精度 1.5°。

如圖3所示，從上到下分別為SINS/ADS、SINS/MCP、SINS/TAN局部濾波器以及 SINS/TAN/ADS/MCP聯(lián)邦濾波器位置誤差曲線。

如圖4所示，從上到下分別為SINS/ADS、SINS/MCP、SINS/TAN局部濾波器以及 SINS/TAN/ADS/MCP聯(lián)邦濾波器速度誤差曲線。

圖3 位置誤差曲線Fig.3 Curves of positioning errors

圖4 速度誤差曲線Fig.4 Curves of speed errors

圖5所示分別為SINS/ADS、SINS/MCP、SINS/TAN局部濾波器以及SINS/TAN/ADS/MCP聯(lián)邦濾波器航向誤差曲線。

圖5 航向誤差曲線Fig.5 Curves of heading errors

將各局部濾波器估計誤差與全局濾波器估計誤差進行分析比較，結果如表1所示。

從上述分析結果可以看出，SINS/ADS速度局部濾波器，由于速度信息得到修正，所以速度誤差較小而位置誤差和航向誤差較大；SINS/MCP航向局部濾波器，航向誤差較小，但位置和速度誤差的估計精度不理想；SINS/TAN位置局部濾波器，位置誤差較小，但航向和速度精度不高；相較而言，SINS/TAN/ADS/ MCP全局濾波器在位置、速度、航向上均具有較高的估計精度，可以滿足無人機導航系統(tǒng)高精度、高可靠性的要求。

表1 仿真結果分析比較Tab.1 Comparison on simulation results

6 結 論

本文提出的無人機導航系統(tǒng)是結合捷聯(lián)慣性導航系統(tǒng)、地形輔助導航系統(tǒng)、大氣數(shù)據(jù)系統(tǒng)以及磁羅經(jīng)導航的組合導航系統(tǒng)。采用聯(lián)邦卡爾曼濾波方式對導航信息進行融合，揚長避短。對比仿真實驗結果可以看出：SINS/TAN系統(tǒng)位置誤差較小，但航向誤差較大；SINS/ADS系統(tǒng)速度誤差較小且比較穩(wěn)定，但位置誤差隨時間發(fā)散；SINS/MCP系統(tǒng)航向誤差方差可達 0.3783′，但其位置和速度估計精度不理想；而該組合導航系統(tǒng)可以同時獲得較高的位置、速度以及航向精度，對于實現(xiàn)UAV高精度高可靠性導航具有重要的意義。

參考文獻（References）：

[1]方錚. 長航無人機多源信息融合自主導航及其仿真關鍵技術研究[D]. 南京: 南京航空航天大學, 2014.Fang Z. Research on Autonomous navigation with multi-source information fusion of long-endurance UAV[D]. Nanjing: Nanjing University of Aeronautics and Astronautics, 2014.

[2]徐曉蘇, 湯郡郡, 張濤, 等. 一種結合地形和環(huán)境特征的水下導航定位方法[J]. 中國慣性技術學報, 2015,23(5): 590-596.Xu X S, Tang J J, Zhang T, et al. Under- water navigation method based on terrain and environmental features[J].Journal of Chinese Inertial Technology, 2015, 23(5):590-596.

[3]Jiang D N, Mekonnen T M. INS/GPS/MNS Integrated navigation system with federated Kalman filtering[J].Advanced Materials Research, 2013, 718-720: 1207-1212.

[4]Xu T L. Research on federated Kalman filter for navigation system[C]//Proceeding of 2011 Third Pacific-Asia Conference on Circuits, Communications and System.2011: 1-4.

[5] 張科, 劉海鵬, 李恒年, 等. SINS/ GPS/CNS組合導航聯(lián)邦濾波算法[J]. 中國慣性技術學報, 2013, 21(2):226-230.Zhang K, Liu H P, Li H N, et al. SINS/GPS/CNS integrated navigation federal filtering algorithm[J].Journal of Chinese Inertial Technology, 2013, 21(2):226-230.

[6]Yang S J. Stellar refraction-based SINS/ CNS integrated navigation system for aerospace vehicles[J]. Journal of Aerospace Engineering, 2016, 29(2): 1-11.

[7]Deng H, Liu G B, Chen H M, et al. The Application of federated Kalman filtering in SINS/GPS/CNS integrated navigation system[C]//International Journal of Wireless and Microwave Technologies, 2012(2): 12-19.

[8]李佩娟, 徐曉蘇, 張濤. 信息融合技術在水下組合導航系統(tǒng)中的應用[J]. 中國慣性技術學報, 2009, 17(3):344-349.Li P J, Xu X S, Zhang T. Application of information fusion to integrated navigation system of underwater vehicle[J]. Journal of Chinese Inertial Technology, 2009,17(3): 344-349.

[9]Gao S S, Xue L, Zhang Y M, et al. Random weighting method for estimation of error characteristics in SINS/GPS/SAR integrated navigation system[J]. Aerospace Science and Technology, 2015, 46: 22-29.

[10]陳燕麗. SINS/TAN/GPS組合導航信息融合技術研究[D]. 哈爾濱: 哈爾濱工程大學, 2011.Cheng Y L. Research on information fusion technology of SINS/TAN/GPS integration navigation system[D].Harbin : Harbin Engineering University, 2011.

[11]孟陽, 高社生, 高兵兵, 等. 基于 UKF的 INS/GNSS/CNS組合導航最優(yōu)數(shù)據(jù)融合方法[J]. 中國慣性技術學報, 2016, 24(6): 746-751.Meng Y, Gao S S, Gao B B, et al. UKF-based optimal data fusion method for integrated INS/GNSS/CNS[J]. Journal of Chinese Inertial Technology, 2016, 24(6): 746-751.