發(fā)射系下SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)算法及實現(xiàn)

潘加亮 熊智 趙慧 郁豐 王麗娜

(1 南京航空航天大學(xué)導(dǎo)航研究中心，南京 210016) (2 南京航空航天大學(xué)航天學(xué)院，南京 210016)

(3 北京航天自動控制研究所，北京 100854)

發(fā)射系下SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)算法及實現(xiàn)

潘加亮1熊智1趙慧1郁豐2王麗娜3

(1 南京航空航天大學(xué)導(dǎo)航研究中心，南京 210016) (2 南京航空航天大學(xué)航天學(xué)院，南京 210016)

(3 北京航天自動控制研究所，北京 100854)

首先建立了發(fā)射慣性系下的組合導(dǎo)航系統(tǒng)模型，據(jù)此設(shè)計了基于聯(lián)邦濾波器的SINS/GPS/CNS組合導(dǎo)航算法，最后研制了基于PC104硬件平臺的組合導(dǎo)航算法驗證樣機(jī)。通過實時半物理仿真測試得出，三組合導(dǎo)航系統(tǒng)的姿態(tài)誤差小于15″，位置誤差小于10 m,速度誤差小于0.2 m/s,表明所設(shè)計的組合導(dǎo)航系統(tǒng)算法正確，實現(xiàn)合理。

發(fā)射慣性系；組合導(dǎo)航；系統(tǒng)實現(xiàn)；硬件平臺

1 引言

隨著對空間動態(tài)載體導(dǎo)航定位精度要求的提高，多傳感器組合導(dǎo)航系統(tǒng)成為眾多學(xué)者的研究對象，并在空間導(dǎo)航技術(shù)領(lǐng)域得到廣泛應(yīng)用。但是典型的以慣性導(dǎo)航為基礎(chǔ)的慣性/衛(wèi)星單一組合導(dǎo)航系統(tǒng)，由于衛(wèi)星導(dǎo)航系統(tǒng)容易受到欺騙干擾，從而無法滿足飛行器對導(dǎo)航參數(shù)的高可靠、高精度的測量要求[1-3]。因此，考慮到單一組合導(dǎo)航系統(tǒng)難以勝任空間遠(yuǎn)程導(dǎo)彈的長航時高精度導(dǎo)航要求，本文設(shè)計了一種基于發(fā)射慣性系下的SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)，將捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)、全球衛(wèi)星定位系統(tǒng)(GPS)和天文導(dǎo)航系統(tǒng)(CNS)等多種導(dǎo)航系統(tǒng)的信息進(jìn)行最優(yōu)融合，構(gòu)建SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)是實現(xiàn)未來飛行器高精度導(dǎo)航的保證。

考慮到衛(wèi)星定位系統(tǒng)和天文導(dǎo)航系統(tǒng)的量測輸出參考坐標(biāo)系有較大的差異，因此，需要解決在統(tǒng)一的時空導(dǎo)航坐標(biāo)參考系下實現(xiàn)和SINS信息最優(yōu)融合的問題。傳統(tǒng)的SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)算法主要采用了當(dāng)?shù)氐乩韰⒖甲鴺?biāo)系作為統(tǒng)一的導(dǎo)航參考坐標(biāo)系[1]，其組合算法模型通常采用傳統(tǒng)的航空組合導(dǎo)航算法模型，在此基礎(chǔ)上，將組合獲得的導(dǎo)航參數(shù)經(jīng)過一定的參數(shù)轉(zhuǎn)換變換為發(fā)射慣性系下的導(dǎo)航參數(shù)進(jìn)行輸出，該算法實現(xiàn)復(fù)雜、需要經(jīng)過參數(shù)的多種轉(zhuǎn)換，無法直接獲得發(fā)射慣性系下的導(dǎo)航參數(shù)。

鑒于某些飛行器在發(fā)射過程中，通常需要直接獲得發(fā)射慣性系下的高精度導(dǎo)航參數(shù)這一需求現(xiàn)狀，本文研究了一種基于發(fā)射慣性系下的SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)算法及系統(tǒng)實現(xiàn)方案，直接建立發(fā)射慣性系下的組合導(dǎo)航系統(tǒng)模型，設(shè)計了基于聯(lián)邦濾波器的發(fā)射慣性系下的SINS/GPS/CNS組合導(dǎo)航算法[4-6]。為有效驗證所設(shè)計的算法性能，還研制了基于PC104嵌入式硬件平臺的組合導(dǎo)航樣機(jī)[7-9]，通過實時半物理仿真測試，表明所設(shè)計的SINS/GPS/CNS組合導(dǎo)航方案和算法能有效滿足遠(yuǎn)程導(dǎo)彈的高精度導(dǎo)航定位要求，為SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)的工程化實現(xiàn)和應(yīng)用提供了有效的支撐。

2 發(fā)射系下SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)總體方案

為直接獲得在發(fā)射慣性系下的高精度導(dǎo)航參數(shù)，本節(jié)設(shè)計了發(fā)射系下SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)算法總體實現(xiàn)方案如圖1所示。

圖1 SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)算法總體方案Fig.1 Overall plan of SINS/GPS/CNS muiti-integrated navigation system algorithm

圖1所給出的SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)總體方案分為三個關(guān)鍵部分：

1)捷聯(lián)慣性導(dǎo)航系統(tǒng)解算模塊(SINS)：根據(jù)陀螺和加速度計的原始測量數(shù)據(jù)，求解發(fā)射慣性系下的姿態(tài)、位置和速度等導(dǎo)航參數(shù)。

2)量測轉(zhuǎn)換子系統(tǒng)模塊：通過實時獲取GPS和CNS的輸出量測值，并轉(zhuǎn)換為發(fā)射慣性系下的位置值和姿態(tài)四元數(shù)，便于進(jìn)行濾波運(yùn)算。

3)聯(lián)邦濾波模塊：SINS/GPS組合子濾波器對慣導(dǎo)位置和GPS輸出進(jìn)行濾波處理，SINS/CNS組合子濾波器對慣導(dǎo)姿態(tài)和星敏感器輸出進(jìn)行濾波處理，主濾波器對兩個子濾波器獲得的導(dǎo)航參數(shù)進(jìn)行最優(yōu)融合，并對慣性導(dǎo)航系統(tǒng)的誤差進(jìn)行反饋修正。

3 多組合導(dǎo)航系統(tǒng)算法設(shè)計

3.1 發(fā)射慣性系下的組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型

(1)組合導(dǎo)航系統(tǒng)狀態(tài)方程建模

圖2 發(fā)射慣性系示意Fig.2 Launch inertial coordinate system schematic diagram

發(fā)射慣性坐標(biāo)系Of-XfYfZf定義為：原點Of設(shè)在發(fā)射點,Xf軸位于發(fā)射點的水平面內(nèi)，通過發(fā)射點指向發(fā)射方向，Yf軸垂直向上，Zf軸與Xf軸、Yf軸構(gòu)成右手坐標(biāo)系，發(fā)射慣性坐標(biāo)系的坐標(biāo)軸保持在慣性空間，不隨地球旋轉(zhuǎn)。發(fā)射慣性系如圖2所示。

本體坐標(biāo)系：Ob-XbYbZb定義為飛行器前上右坐標(biāo)系。

針對發(fā)射慣性系下的捷聯(lián)慣性導(dǎo)航算法，姿態(tài)、位置和速度解算方程如下：

(1)

(2)

(3)

陀螺和加速度計的測量模型如下：

(4)

式中ωr為陀螺隨機(jī)游走誤差；ωε為陀螺測量噪聲；fr為加速度計隨機(jī)游走誤差；fε為加速度計測量噪聲；ωn和fn分別為陀螺和加速度計隨機(jī)游走驅(qū)動噪聲。

該組合導(dǎo)航系統(tǒng)為非線性對象，根據(jù)公式(1)～(4)可以建立發(fā)射慣性系下的導(dǎo)航系統(tǒng)誤差增量方程如下：

(5)

基于上述分析，選擇導(dǎo)航系統(tǒng)誤差量為系統(tǒng)狀態(tài)方程，則可以獲得導(dǎo)航系統(tǒng)的狀態(tài)方程為

(6)

其中，狀態(tài)變量為

系統(tǒng)白噪聲矢量為

(2)組合導(dǎo)航系統(tǒng)量測方程建模

SINS/GPS子系統(tǒng)量測為捷聯(lián)慣性導(dǎo)航系統(tǒng)給出的三軸位置參數(shù)和GPS衛(wèi)星接收機(jī)給出的相應(yīng)信息(轉(zhuǎn)換到發(fā)射慣性系下)的差值；SINS/CNS子系統(tǒng)量測為慣導(dǎo)給出的姿態(tài)信息和天文星敏感器給出的姿態(tài)(轉(zhuǎn)換到發(fā)射慣性系下)差值。

對于SINS/GPS子系統(tǒng)，定義觀測矢量為

(7)

對于SINS/CNS子系統(tǒng)，定義子系統(tǒng)觀測矢量為姿態(tài)四元數(shù)的矢量部分，如下：

(8)

3.2 組合導(dǎo)航系統(tǒng)聯(lián)邦濾波器模型

各子濾波器進(jìn)行卡爾曼濾波，過程如下：

(9)

(10)

(11)

(12)

(13)

主濾波器進(jìn)行全局最優(yōu)融合，以得到高精度導(dǎo)航參數(shù)，算法如下：

(14)

為解決各子濾波器估計相關(guān)問題，聯(lián)邦濾波采用信息分配的方法使得各子濾波器估計狀態(tài)可以按照不相關(guān)的方式處理，算法如下：

(15)

4 組合導(dǎo)航系統(tǒng)實現(xiàn)及性能測試

4.1 組合導(dǎo)航系統(tǒng)原理樣機(jī)實現(xiàn)方案

為有效驗證所設(shè)計的SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)算法的正確性，本文設(shè)計了基于實時串口傳輸?shù)姆植际剿惴ò胛锢矸抡骝炞C環(huán)境。SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)算法驗證環(huán)境硬件分成導(dǎo)航計算機(jī)、IMU模擬器、GPS模擬器和星敏感器模擬器。其中仿真子系統(tǒng)模塊采用通用PC機(jī)，主要負(fù)責(zé)將模擬生成的三個傳感器數(shù)據(jù)傳送給導(dǎo)航計算機(jī)。組合導(dǎo)航計算機(jī)系統(tǒng)硬件分為導(dǎo)航計算機(jī)模塊、接口模塊、供電模塊和仿真子系統(tǒng)模塊。

核心的導(dǎo)航計算機(jī)硬件的研制采用基于PC104總線的嵌入式計算機(jī)系統(tǒng)，該類計算機(jī)具有強(qiáng)大的數(shù)據(jù)處理能力和豐富的外圍接口,主要對多個子系統(tǒng)的仿真數(shù)據(jù)或?qū)嵨飻?shù)據(jù)進(jìn)行濾波處理，并將導(dǎo)航結(jié)果傳輸?shù)狡聊伙@示出來；接口模塊采用八串口擴(kuò)展板；電源模塊采用PSI104模塊，與PC104相配套。采用標(biāo)準(zhǔn)C語言進(jìn)行系統(tǒng)軟件實現(xiàn)。

4.2 組合導(dǎo)航系統(tǒng)性能測試分析

為驗證多組合導(dǎo)航系統(tǒng)的算法性能，根據(jù)相應(yīng)的硬件條件，采用半物理仿真模式，首先由發(fā)射慣性系下的導(dǎo)彈動態(tài)航跡數(shù)據(jù)，模擬得到各類導(dǎo)航傳感器數(shù)據(jù)，再將傳感器數(shù)據(jù)由上位機(jī)通過串口實時發(fā)送給PC104導(dǎo)航計算機(jī)進(jìn)行實時導(dǎo)航解算，最后將發(fā)射慣性系下的導(dǎo)航結(jié)果與基準(zhǔn)航跡數(shù)據(jù)對比，進(jìn)行算法性能分析。

圖3 飛行軌跡圖Fig.3 Missile flight path map

飛行器發(fā)射初始經(jīng)、緯、高分別為：118°、32°、0m，初始航向角為90°，發(fā)射時間為2013年1月15日0時0分0秒，飛行時間為750s；捷聯(lián)解算周期為0.02s，聯(lián)邦濾波周期為1s；仿真中捷聯(lián)慣性傳感器仿真參數(shù)為：陀螺隨機(jī)游走驅(qū)動噪聲0.2(°)/h，陀螺白噪聲0.2(°)/h，加速度計隨機(jī)游走驅(qū)動噪聲0.000 1 gn，加速度計白噪聲0.000 1 gn；衛(wèi)星接收機(jī)和星敏感器仿真參數(shù)為：衛(wèi)星導(dǎo)航位置誤差15m，星光跟蹤儀誤差21″。飛行軌跡如圖3所示。

為有效驗證導(dǎo)航算法的實時性，首先在PC104平臺上進(jìn)行了算法實時性測試，設(shè)置仿真計算步長為0.02s，仿真總時間750s，分析了在不同工作模式下，算法運(yùn)行的實際總用時以及單步算法運(yùn)行的實際用時，如表1所示。

表1 算法實時性測試

從表1可以看出，不同的工作模式下，單步算法的實際用時均遠(yuǎn)小于0.02s，因而能夠滿足嵌入式算法的實時性要求。

此外，在實時仿真過程中，共分析了4種工作模式下的性能：純捷聯(lián)慣導(dǎo)、SINS/GPS組合、SINS/CNS組合和SINS/GPS/CNS三組合。為便于對比分析組合導(dǎo)航系統(tǒng)性能，下面給出了不同工作模式下的姿態(tài)、位置和速度誤差曲線對比圖，如圖4～6所示。

圖4 不同工作模式下的姿態(tài)誤差曲線Fig.4 Attitude error curves of different working modes

圖5 不同工作模式下的位置誤差曲線Fig.5 Position error curves of different working modes

圖6 不同工作模式下的速度誤差曲線Fig.6 Velocity error curves of different working modes

根據(jù)前面不同工作模式下的仿真數(shù)據(jù)，對SINS/GPS/CNS多組合導(dǎo)航系統(tǒng)在不同工作模式下的性能進(jìn)行比較分析，求取相應(yīng)的RMS誤差，如表2所示。

表2 SINS/GPS/CNS各種組合導(dǎo)航方式下誤差對比

由圖4～6以及表2可以看出，SINS/CNS組合導(dǎo)航系統(tǒng)能直接修正姿態(tài)誤差，提高姿態(tài)精度；SINS/GPS組合導(dǎo)航系統(tǒng)，通過對位置誤差直接觀測和對速度誤差間接觀測修正，使位置和速度誤差大大減小，保證彈著點的導(dǎo)航精度要求；最終的SINS/GPS/CNS三組合導(dǎo)航系統(tǒng)能同時修正姿態(tài)、位置和速度誤差，大大提高導(dǎo)航系統(tǒng)整體精度。

通過上述不同組合模式下的串口實時仿真結(jié)果，可以看出組合導(dǎo)航系統(tǒng)算法在不同組合模式下解算誤差合理，表明了算法能夠有效滿足導(dǎo)航解算的實時性和可靠性要求。

[1] 李艷華,房建成,賈志凱.INS/CNS/GPS組合導(dǎo)航系統(tǒng)仿真研究[J].中國慣性技術(shù)學(xué)報,2002,10(6):6-11,17.

LI YANHUA，F(xiàn)ANG JIANCHENG，JIA ZHIKAI.Simulation of INS/CNS/GPS integrated navigation[J]. Journal of Chinese Inertial Technology,2002,10(6):6-11,17.

[2] XIAOJUAN W, XINLONG W. A SINS/CNS deep integrated navigation method based on mathematical horizon reference[J]. Aircraft Engineering and Aerospace Technology, 2011, 83(1): 26-34.

[3] 溫永智,吳杰.彈道導(dǎo)彈捷聯(lián)慣導(dǎo)/天文組合導(dǎo)航方案研究[J].飛行力學(xué),2010,28(6):87-90.

WEN YONGZHI，WU JIE.Research on scheme of SINS/celestial integrated navigation for ballistic missile[J]. Flight Dynamics,2010,28(6):87-90.

[4] WANG R, XIONG Z, LIU J, et al. SINS/GPS/CNS information fusion system based on improved Huber filter with classified adaptive factors for high-speed UAVs[C].Position Location and Navigation Symposium (PLANS), 2012 IEEE/ION,IEEE, 2012:441-446.

[5] GAO S, ZHONG Y, ZHANG X, et al. Multi-sensor optimal data fusion for INS/GPS/SAR integrated navigation system[J]. Aerospace Science and Technology, 2009, 13(4): 232-237.

[6] 屈薔, 劉建業(yè), 熊智. 基于加性四元數(shù)誤差方程的慣性/天文姿態(tài)組合算法[J]. 中國慣性技術(shù)學(xué)報, 2011, 19(3):316-319.

QU QIANG，LIU JIANYE，XIONG ZHI.Inertial/Celestial attitude integrated algorithm based on additive quaternion[J]. Journal of Chinese Inertial Technology,2011,19(13):316-319.

[7] 祝燕華,蔡體菁,楊卓鵬.MEMS-IMU/GPS組合導(dǎo)航系統(tǒng)的實現(xiàn)[J].中國慣性技術(shù)學(xué)報,2009,17(5):552-556,561.

ZHU YANHUA，CAI TIJING，YANG ZHUOPENG. MEMS-IMU/GPS integrated navigation system[J]. Journal of Chinese Inertial Technology,2009,17(5):552-556,561.

[8] 鐘樂,胡延霖,陳永明,等.一種基于DSP的SINS/GPS組合導(dǎo)航系統(tǒng)實現(xiàn)[J].艦船電子工程,2011,31(2):55-57.

ZHONG LE，HU YANLIN，CHEN YONGMING，et al. Design of DSP-based SINS/GPS integrated navigation system[J]. Ship Electronic Engineering,2011,31(2):55-57.

[9] YAN J, XU X. Design and experiment of SINS/GPS integrated navigation system[J]. Journal of Computers, 2013, 8(8)：1973-1978.

[10] 張科,李海鵬,李恒年,等.SINS/GPS/CNS組合導(dǎo)航聯(lián)邦濾波算法[J].中國慣性技術(shù)學(xué)報,2013,21(2):226-230.

ZHANG KE，LI HAIPENG，LI HENGNIAN，et al. SINS/GPS/CNS integrated navigation federal filtering algorithm[J]. Journal of Chinese Inertial Technology,2013,21(2):226-230.

[11] XIONG ZHI, CHEN JIHUI, WANG RONG, et al. A new dynamic vector formed information sharing algorithm in federated filter[J]. Aerospace Science and Technology, 2013,29(1):37-46.

(編輯：楊嬋)

SINS/GPS/CNS Multi-integrated Navigation System Algorithm in Launch Inertial Coordinate System and Realization

PAN Jialiang1XIONG Zhi1ZHAO Hui1YU Feng2WANG Lina3

(1 Navigation Research Center, Nanjing University of Aeronautics and Astronautics,Nanjing 210016)

(2 College of Astronautics, Nanjing University of Aeronautics and Astronautics,Nanjing 210016)

(3 Beijing Aerospace Automatic Control Institute, Beijing 100854)

A single integrated navigation system can not meet the long-endurance high precision requirement of space long-range missile, so a SINS/GPS/CNS multi-integrated navigation system in launch inertial coordinate system was designed. Firstly, an integrated navigation model was built in the launch inertial coordinate system.Secondly,a SINS/GPS/CNS integrated navigation algorithm based on the federal Kalman filter was designed.Finally,an integrated navigation algorithm verification prototype based on PC104 was developed. Through the real time semi-physical simulation tests, the attitude error of triple integrated navigation system is below 15″, the position error is below 10 m and the velocity error is below 0.2 m/s. It indicates that the integrated navigation system algorithm designed is right and the realization of the system is reasonable.

Launch inertial coordinate system; Integrated navigation; System realization; Hardware platform

國家自然科學(xué)基金(61374115,91016019,61210306075,60904091,61203188);航空科學(xué)基金(2011ZC52044);國家留學(xué)基金委；江蘇省六大人才高峰(2013-JY-013)；江蘇省高校青藍(lán)工程；江蘇高校優(yōu)勢學(xué)科建設(shè)工程；中央高校基本科研業(yè)務(wù)費(fèi)專項資金(NZ2014406)資助項目

2014-09-03。收修改稿日期：2014-11-26

10.3780/j.issn.1000-758X.2015.02.002

潘加亮 1990年生，2012年畢業(yè)于南京航空航天大學(xué)探測制導(dǎo)與控制技術(shù)專業(yè)，現(xiàn)為南京航空航天大學(xué)導(dǎo)航、制導(dǎo)與控制專業(yè)碩士研究生，研究方向為航天器導(dǎo)航技術(shù)及組合導(dǎo)航技術(shù)。