基于實(shí)測(cè)軌跡的高精度捷聯(lián)慣導(dǎo)模擬器

嚴(yán)恭敏,WANG Jinling,周馨怡

(1.西北工業(yè)大學(xué)自動(dòng)化學(xué)院,西安 710072; 2.School of Civil and Environment Engineering,University of New South Wales,Sydney,NSW 2052,Australia)

基于實(shí)測(cè)軌跡的高精度捷聯(lián)慣導(dǎo)模擬器

嚴(yán)恭敏1,WANG Jinling2,周馨怡1

(1.西北工業(yè)大學(xué)自動(dòng)化學(xué)院,西安 710072; 2.School of Civil and Environment Engineering,University of New South Wales,Sydney,NSW 2052,Australia)

針對(duì)傳統(tǒng)的純數(shù)學(xué)模型SINS慣性器件模擬器仿真效果不夠逼真的問(wèn)題,本文提出了一種基于已有的真實(shí)導(dǎo)航系統(tǒng)飛行記錄數(shù)據(jù)的慣性器件模擬器方法。該方法利用高精度載波相位差分GNSS與中等精度IMU融合的組合導(dǎo)航算法,結(jié)合Kalman濾波的部分反饋修正和三次樣條擬合方法,生成平滑的軌跡參數(shù),再通過(guò)推導(dǎo)SINS反演算法實(shí)現(xiàn)了慣性器件的模擬采樣仿真,反演算法中充分考慮了姿態(tài)圓錐誤差和速度劃船誤差的補(bǔ)償。通過(guò)機(jī)載飛行實(shí)驗(yàn)數(shù)據(jù)驗(yàn)證,結(jié)果表明所提慣性器件模擬器具有極高的精度性能和良好的頻率特性,滿足高精度SINS對(duì)數(shù)據(jù)源仿真精度和頻率復(fù)雜性的要求。

GNSS;SINS模擬器;組合導(dǎo)航;實(shí)際飛行數(shù)據(jù)

0 引言

在慣性導(dǎo)航系統(tǒng)及其組合導(dǎo)航的仿真研究中,運(yùn)載體的運(yùn)動(dòng)軌跡參數(shù)和慣性器件(陀螺和加速度計(jì))的數(shù)據(jù)源仿真是項(xiàng)基礎(chǔ)性工作,特別是研究高精度的捷聯(lián)慣導(dǎo)系統(tǒng)(strapdown inertial navigation system,SINS)算法,更是需要高精度的數(shù)據(jù)模擬器作為支撐[1]。以機(jī)載SINS仿真為例,傳統(tǒng)的基于純數(shù)學(xué)模型的軌跡模擬器,只能給出由靜止、加速、爬升、巡航和盤(pán)旋轉(zhuǎn)彎等簡(jiǎn)單飛行動(dòng)作組合的載機(jī)運(yùn)動(dòng)軌跡,運(yùn)動(dòng)形式過(guò)于簡(jiǎn)單,效果不夠逼真,若要進(jìn)一步考慮載機(jī)飛控特性和氣動(dòng)環(huán)境的影響,物理和數(shù)學(xué)模型往往又過(guò)于復(fù)雜,并且理想模型與實(shí)際飛行情況之間或多或少總會(huì)存在一些差距[2]。

本文基于已有的真實(shí)導(dǎo)航傳感器飛行記錄數(shù)據(jù),利用高精度載波相位差分全球衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system,GNSS)與中等精度慣性測(cè)量單元(inertial measurement unit, IMU)融合的組合導(dǎo)航算法,結(jié)合卡爾曼(Kalman)濾波的部分反饋修正和三次樣條函數(shù)擬合和插值方法,生成平滑的軌跡參數(shù),即姿態(tài)、速度和位置信息。借助傳統(tǒng)的高精度SINS數(shù)值更新算法[3-5],推導(dǎo)了由姿態(tài)、速度和位置信息反向求解慣性器件增量采樣輸出的方法,稱其為SINS反演算法,在反演算法中考慮了姿態(tài)圓錐誤差和速度劃船誤差效應(yīng)的影響,進(jìn)行了相應(yīng)誤差的補(bǔ)償。利用SINS反演算法實(shí)現(xiàn)了慣性器件的模擬采樣輸出。通過(guò)飛行實(shí)驗(yàn)數(shù)據(jù)驗(yàn)證和對(duì)比分析,慣性器件模擬采樣輸出具有很高的導(dǎo)航精度,其精度僅受限于數(shù)值算法的計(jì)算誤差;慣性器件的模擬采樣輸出與原始真實(shí)采樣輸出在低頻段的功率譜密度基本一致,說(shuō)明了模擬采樣輸出能夠較好地反映出載機(jī)的飛行環(huán)境特性。

1 傳統(tǒng)的捷聯(lián)慣導(dǎo)算法

1.1 捷聯(lián)慣導(dǎo)系統(tǒng)微分方程

首先,簡(jiǎn)要說(shuō)明文中涉及到的幾個(gè)坐標(biāo)系:慣性坐標(biāo)系表示為i系,地球坐標(biāo)系表示為e系,“東-北-天”導(dǎo)航坐標(biāo)系為n系,“右-前-上”載體坐標(biāo)系為b系。

SINS導(dǎo)航算法包含一組微分方程,即姿態(tài)微分方程、速度微分方程和位置微分方程,可分別表示為[6]

1.2 捷聯(lián)慣導(dǎo)系統(tǒng)數(shù)值更新算法

SINS數(shù)值更新算法是一種航位推算算法,它根據(jù)上一時(shí)刻(tm-1時(shí)刻)的已知姿態(tài)、速度和位置導(dǎo)航信息,結(jié)合慣性器件在更新周期內(nèi)的采樣輸出,遞推計(jì)算當(dāng)前時(shí)刻(tm時(shí)刻)的導(dǎo)航信息。記Tm=tm-tm-1為SINS更新周期。文中假設(shè)陀螺采樣輸出為角增量并且加速度計(jì)輸出為速度增量,高精度的慣導(dǎo)系統(tǒng)往往都采取這種采樣方式。

考慮到數(shù)值更新算法的精度和為了后續(xù)推導(dǎo)反演算法的方便,下面以“單子樣+前一周期”誤差補(bǔ)償方法給出SINS數(shù)值更新算法的主體框架[3-6]。

1.2.1 姿態(tài)更新算法

下面給出由等效旋轉(zhuǎn)矢量Φ計(jì)算變換矩陣C的公式

1.2.2 速度更新算法

1.2.3 位置更新算法

2 GNSS/SINS組合導(dǎo)航

2.1 GNSS/SINS組合Kalman濾波

采用高精度載波相位GNSS與SINS構(gòu)成組合導(dǎo)航系統(tǒng),GNSS可達(dá)厘米級(jí)的定位精度并且載機(jī)的機(jī)動(dòng)大、飛行速度高,因此必需考慮到GNSS 和SINS之間的安裝桿臂誤差δl以及兩者之間的時(shí)間不同步誤差δt影響。這里建立19維的GNSS/ SINS組合Kalman濾波器,狀態(tài)選取及濾波器構(gòu)造為

式(18)～式(20)中各符號(hào)的含義以及詳細(xì)系統(tǒng)參數(shù)可參見(jiàn)文獻(xiàn)[6-7]。

2.2 Kalman濾波的部分反饋修正原理

為了保持慣導(dǎo)系統(tǒng)的導(dǎo)航誤差始終為小量,即保證慣導(dǎo)誤差方程的線性,提高濾波器模型精度,在組合導(dǎo)航過(guò)程中需將Kalman濾波的狀態(tài)估計(jì)值不斷反饋給慣導(dǎo)系統(tǒng),進(jìn)行慣導(dǎo)修正。傳統(tǒng)的反饋方法是:選定某些狀態(tài)參與反饋,將Kalman濾波器中的這些狀態(tài)的估計(jì)值一次性全部反饋給慣導(dǎo)系統(tǒng),修正慣導(dǎo)參數(shù),再將濾波器中所有參與反饋的狀態(tài)清零。但是,上述全反饋方法容易造成慣導(dǎo)導(dǎo)航輸出劇烈的鋸齒狀波動(dòng),從而影響后續(xù)SINS反演算法的效果,在慣性器件模擬器上產(chǎn)生很大的間歇性脈沖失真。為了避免全反饋的不良影響,下面提出部分反饋修正原理,以經(jīng)度誤差的狀態(tài)反饋修正為例,表示為

雖然GNSS/SINS組合導(dǎo)航的量測(cè)更新周期一般遠(yuǎn)大于SINS更新周期,但如果選擇反饋修正周期與SINS更新周期Tm一致,并利用上述部分反饋修正方法,則可得到比較平滑的導(dǎo)航參數(shù)輸出,為慣性模擬器的反演算法提供更加逼真的參考軌跡。當(dāng)然,如欲進(jìn)一步提高參考軌跡參數(shù)的求解精度,還可采用雙向?yàn)V波或平滑等算法[8]。

3 捷聯(lián)慣導(dǎo)反演算法

反演算法可以看作是常規(guī)SINS數(shù)值更新算法的逆過(guò)程。在反演算法中,假設(shè)已知姿態(tài)角和位置序列,前者為角運(yùn)動(dòng)信息而后者為線運(yùn)動(dòng)信息,再通過(guò)改造常規(guī)SINS更新算法,反向求解陀螺角增量和加速度計(jì)速度增量,實(shí)現(xiàn)慣性器件的高精度仿真和模擬。SINS反演算法的關(guān)鍵技術(shù)敘述如下。

3.1 姿態(tài)和位置的三次樣條函數(shù)擬合

以位置信息中的經(jīng)度為例,對(duì)組合導(dǎo)航輸出的經(jīng)度序列…,λm-1,λm,λm+1,…進(jìn)行分段三次樣條函數(shù)擬合,假設(shè)在時(shí)間區(qū)間t∈[tm-1,tm]上的擬合三次多項(xiàng)式函數(shù)為

式(24)中,am0,am1,am2,am3為樣條擬合系數(shù)。

經(jīng)度與慣導(dǎo)東向速度之間的關(guān)系為

一般區(qū)間[tm-1,tm]很小,將式(25)中的L(t) 和RNh(t)取成tm-1/2時(shí)刻的值。由于位置函數(shù)已知,這里L(fēng)m-1/2可通過(guò)樣條函數(shù)插值精確獲得,而不象常規(guī)SINS更新算法中的那樣需通過(guò)線性外推進(jìn)行估計(jì)。

由式(25)移項(xiàng),再將式(24)代入,即可求得東向速度函數(shù)

同理,對(duì)緯度和高度序列進(jìn)行處理,可求得北向速度和天向速度,從而獲得任意時(shí)刻的位置函數(shù)p(t)和速度函數(shù)vn(t);若對(duì)歐拉角序列進(jìn)行同樣處理,可獲得歐拉角分段擬合函數(shù),不妨記為A(t)。值得注意的是,如果歐拉角表示姿態(tài)存在奇異點(diǎn),則需做特殊處理,這里不再詳述。

隨著我國(guó)教育事業(yè)的不斷發(fā)展，教學(xué)模式也越來(lái)越多樣化，在對(duì)人才的教育教學(xué)過(guò)程中教師應(yīng)該不僅僅是進(jìn)行課堂教學(xué)，通過(guò)利用數(shù)字媒體技術(shù)來(lái)提高學(xué)生的學(xué)習(xí)經(jīng)驗(yàn)和個(gè)人素質(zhì)素養(yǎng)。對(duì)于每個(gè)人一生的教育學(xué)習(xí)來(lái)說(shuō)，專業(yè)核心能力的教育培養(yǎng)非常重要。加強(qiáng)人才培養(yǎng)過(guò)程中的數(shù)字媒體技術(shù)的運(yùn)用，堅(jiān)持社會(huì)主義教學(xué)事業(yè)的快速發(fā)展，推動(dòng)科學(xué)的教育事業(yè)發(fā)展，做好對(duì)孩子德智體美的全面發(fā)展工作，做好人才的前期的培養(yǎng)工作，為中國(guó)未來(lái)的發(fā)展奠定有利的教育基礎(chǔ)。

3.2 陀螺的角增量模擬

假設(shè)SINS反演算法生成慣性器件的解算周期為T(mén)k=tk-tk-1,它可以不同于Tm,一般情況下小于Tm,Tk越小則解算精度越高。

3.3 加速度計(jì)的速度增量模擬

這里同樣可假設(shè)初值Δv0=0。至此,根據(jù)式(29)和式(31)即可求解得加速度計(jì)的速度增量,完成慣性器件的模擬采樣仿真。

4 實(shí)驗(yàn)數(shù)據(jù)處理與分析

現(xiàn)有一組機(jī)載導(dǎo)航實(shí)驗(yàn)數(shù)據(jù),其SINS中陀螺的隨機(jī)常值漂移約為0.03°/h,加速度計(jì)的逐次啟動(dòng)隨機(jī)常值偏值約為200μg(特別說(shuō)明:g= 9.8 m/s2,下同),原始數(shù)據(jù)輸出頻率200 Hz;載波相位差分GPS的定位精度為5 cm,數(shù)據(jù)輸出頻率1 Hz。載機(jī)飛行軌跡包括靜止、滑跑起飛、轉(zhuǎn)彎、巡航和在作業(yè)區(qū)域?qū)嵤┳鳂I(yè)等階段,圖1給出了1 h飛行軌跡的水平投影圖,其中左下角“☆”為軌跡起始點(diǎn)。

圖1 飛行軌跡

對(duì)實(shí)驗(yàn)數(shù)據(jù)進(jìn)行處理,大致過(guò)程如下:

(1)采用二子樣姿態(tài)圓錐誤差和速度劃船誤差補(bǔ)償算法進(jìn)行SINS導(dǎo)航更新,同時(shí)進(jìn)行GPS/ SINS組合導(dǎo)航Kalman濾波,使用部分反饋修正方法修正慣導(dǎo)誤差,這些算法均基于PSINS工具箱實(shí)現(xiàn)[9];

(2)對(duì)組合導(dǎo)航輸出的100 Hz姿態(tài)和位置數(shù)據(jù)作三次樣條函數(shù)擬合,獲得分段連續(xù)的軌跡函數(shù)A(t)、vn(t)和p(t);

(3)對(duì)軌跡函數(shù)A(t)、vn(t)和p(t)作200 Hz等間隔插值,得到序列Ak和pk;

(4)根據(jù)SINS反演算法生成慣性器件模擬采樣值Δθk和Δvk,比較IMU原始采樣數(shù)據(jù)與模擬采樣數(shù)據(jù)的功率譜密度(power spectrum density, PSD),參見(jiàn)圖2,圖中顯示的是x軸陀螺和x軸加速度計(jì)在1 000～1 100 s的結(jié)果,其它軸和時(shí)間段的效果類似,不再給出;

(5)選用二子樣誤差補(bǔ)償算法,由模擬采樣Δθk和Δvk進(jìn)行純慣導(dǎo)SINS解算,但對(duì)慣導(dǎo)的高度通道進(jìn)行了阻尼,純慣導(dǎo)解算結(jié)果與(1)中的組合導(dǎo)航結(jié)果比較,誤差參見(jiàn)圖3。

圖2 原始采樣數(shù)據(jù)與模擬采樣數(shù)據(jù)的功率譜密度對(duì)比

圖3 基于模擬采樣數(shù)據(jù)的純慣導(dǎo)誤差

從圖2可看出,原始采樣數(shù)據(jù)與模擬采樣數(shù)據(jù)的功率譜密度在低頻段吻合得很好,說(shuō)明了慣性器件模擬器能夠模擬出載機(jī)的低頻運(yùn)動(dòng)特性。圖3表明,模擬采樣數(shù)據(jù)具有很高的導(dǎo)航精度,在1 h內(nèi)純慣導(dǎo)的姿態(tài)誤差為0.001″量級(jí)、位置誤差為0.1 m量級(jí)。

5 結(jié)束語(yǔ)

根據(jù)傳統(tǒng)的高精度SINS數(shù)值更新算法,它由慣性器件輸出求解姿態(tài)、速度和位置等導(dǎo)航信息,提出了SINS反演算法,即如果已知姿態(tài)和位置軌跡信息,反向模擬求解慣性器件采樣值,實(shí)現(xiàn)慣性器件模擬器的仿真。針對(duì)實(shí)際載機(jī)的飛行數(shù)據(jù),需要結(jié)合GNSS進(jìn)行組合導(dǎo)航和使用部分反饋修正技術(shù)處理,提高導(dǎo)航精度和獲得平滑的軌跡參數(shù),才能作為慣性器件模擬器的理想輸入。實(shí)驗(yàn)數(shù)據(jù)處理結(jié)果表明,所提慣性模擬器具有在1 h內(nèi)純慣導(dǎo)位置誤差0.1 m量級(jí)的精度,完全滿足高精度慣導(dǎo)系統(tǒng)對(duì)數(shù)據(jù)源仿真精度的要求,如果增加模擬器求解頻率,還有望進(jìn)一步減少仿真計(jì)算誤差;慣性器件的模擬采樣數(shù)據(jù)頻譜與實(shí)際采樣數(shù)據(jù)頻譜在低頻段基本相同,如果有更高的高頻一致性要求,可通過(guò)增加實(shí)際數(shù)據(jù)的采集頻率或者采用單子樣算法進(jìn)行SINS解算來(lái)實(shí)現(xiàn)。

當(dāng)然,在高精度的模擬器數(shù)據(jù)源之后,還可針對(duì)特定的任務(wù)需求,再加入隨機(jī)漂移誤差和不正交安裝誤差等建模誤差,進(jìn)行更深層次的慣性導(dǎo)航或組合導(dǎo)航仿真。

[1] HUDDLE J,GRETHEL R.Radio astro inertial Doppler error simulator[EB/OL].[2014-06-12].http://www.industrycortex.com.

[2] GPSoft.Sat Nav toolbox 3.0 for Matlab[EB/OL].[2014-06-12].http://gpsoftnav.com/products/satellite-navigation-satnav-toolbox-3-0/.

[3] PAUL G S.Strapdown inertial navigation integration algorithm design part 1:attitude algorithms[J].Journal of Guidance,Control and Dynamics,1998,21(1):19-28.

[4] PAUL G S.Strapdown inertial navigation integration algorithm design part 2:velocity and position algorithms[J].Journal of Guidance,Control and Dynamics,1998,21(2):208-221.

[5] WU Yuanxin,WANG Jinling,HU Dewen.A new technique for INS/GNSSattitude and parameter estimation using online optimization[J].IEEE Trans.on Signal Processing,2014,62(10):2642-2655.

[6] 嚴(yán)恭敏.車載定位定向系統(tǒng)研究[D].西安:西北工業(yè)大學(xué),2006:16-27,82-84.

[7] 趙春蓮.航空攝影測(cè)量中高精度POS導(dǎo)航算法研究[D].西安:西北工業(yè)大學(xué),2014:60-64.

[8] 郝萬(wàn)亮,孫付平,崔建勇,等.雙向?yàn)V波平滑在GNSS/INS組合導(dǎo)航中的應(yīng)用[C]//中國(guó)衛(wèi)星導(dǎo)航系統(tǒng)管理辦公室學(xué)術(shù)交流中心.第四屆中國(guó)衛(wèi)星導(dǎo)航學(xué)術(shù)年會(huì)論文集.北京:中國(guó)衛(wèi)星導(dǎo)航系統(tǒng)管理辦公室,2013:1-4.

[9] 嚴(yán)恭敏.高精度捷聯(lián)慣性導(dǎo)航系統(tǒng)Matlab工具箱[EB/OL].(2013-09-16)[2014-06-12].http://blog.sina.com.cn/s/ blog_40edfdc90101heg0.html.

High-Precision Simulator for Strapdown Inertial Navigation Systems Based on Real Dynamics

YAN Gongmin1,WANG Jinling2,ZHOU Xinyi1
(1.School of Automation,Northwestern Polytechnical University,Xi’an 710072,China; 2.School of Civil and Environment Engineering,University of New South Wales,Sydney 2052,Australia)

Traditional strapdown inertial navigation systems(SINS)simulator based on pure mathematical model cannot reflect the reality of complex dynamics of a carrier.In this paper,based on the actual flight data of an airborne navigation system,navigation algorithms of high-precision carrier phase differences global navigation satellite system(GNSS)and mediumprecision inertial measurement unit(IMU)are used,and combined with the partial feedback principle of Kalman filter and cubic spline interpolation method,to generate smooth trajectory parameters.Then,the simulation of inertial sensors is achieved with the inversed SINSalgorithm,and the new algorithm considers the impacts of attitude coning error and velocity sculling error compensation.Numerical verifications of airborne flight test have demonstrated that the inertial sensor simulator has high accuracy and good frequency characteristic,and can meet the high-accuracy SINS requirements for simulated inertial sensor data source and frequency complexity.

GNSS;SINS simulator;Integrated navigation;Actual flight data

V249.3

A

2095-4999(2015)-04-0027-05

2014-10-16

嚴(yán)恭敏(1977—),福建建甌人,男,博士,副教授,主要從事慣性導(dǎo)航與信息融合理論研究。

注:本文于2015年5月在第六屆中國(guó)衛(wèi)星導(dǎo)航學(xué)術(shù)年會(huì)(CSNC2015)宣讀過(guò)。

嚴(yán)恭敏,WANG Jinling,周馨怡.基于實(shí)測(cè)軌跡的高精度捷聯(lián)慣導(dǎo)模擬器[J].導(dǎo)航定位學(xué)報(bào),2015,3(4):27-31,37.YAN Gongmin, WANG Jinling,ZHOU Xinyi.High-Precision Simulator for Strapdown Inertial Navigation Systems Based on Real Dynamics[J].Journal of Navigation and Positioning,2015,3(4):27-31,37.

10.16547/j.cnki.10-1096.20150406