申紅雪， 丁國(guó)強(qiáng)

(1.鄭州輕工業(yè)大學(xué) 軟件學(xué)院，河南 鄭州 450002;2.鄭州輕工業(yè)大學(xué) 電氣信息工程學(xué)院，河南 鄭州 450002)

0 引 言

將運(yùn)載體從起始點(diǎn)引導(dǎo)到目的地的技術(shù)或方法稱為導(dǎo)航[1]，導(dǎo)航系統(tǒng)提供的信息主要有姿態(tài)、方位、速度和位置，甚至還包括加速度和角速率，這些信息可用于運(yùn)載體的正確操縱和控制。隨著技術(shù)的發(fā)展，導(dǎo)航系統(tǒng)的種類越來(lái)越多，比如慣導(dǎo)系統(tǒng)(Inertial Measurement Units, IMUs)、衛(wèi)星導(dǎo)航系統(tǒng)(Global Navigation Satellite System, GNSS)(如GPS)、磁羅盤、里程儀/多普勒測(cè)速儀/空速計(jì)、氣壓高度表/雷達(dá)高度表、地標(biāo)點(diǎn)/地圖匹配等[2]。這些導(dǎo)航系統(tǒng)各有特色，優(yōu)缺點(diǎn)并存，如慣導(dǎo)系統(tǒng)的優(yōu)點(diǎn)是自主性強(qiáng)、動(dòng)態(tài)性能好、導(dǎo)航信息全面且輸出頻率高，但其缺點(diǎn)是誤差隨時(shí)間不斷累積，長(zhǎng)期精度差；衛(wèi)星導(dǎo)航系統(tǒng)的優(yōu)點(diǎn)是精度高、誤差不隨時(shí)間增大，缺點(diǎn)是導(dǎo)航信息不夠全面、頻帶窄、信號(hào)容易受到干擾、在室內(nèi)等環(huán)境下接收不到衛(wèi)星信號(hào)而無(wú)法使用。在許多對(duì)導(dǎo)航性能要求苛刻的任務(wù)中，無(wú)論是精度還是可靠性要求高，任何單一的導(dǎo)航系統(tǒng)可能都無(wú)法滿足要求，這就需要使用多種導(dǎo)航系統(tǒng)同時(shí)對(duì)運(yùn)載體進(jìn)行導(dǎo)航信息測(cè)量，再對(duì)所有測(cè)量信息作綜合處理(包括檢測(cè)、結(jié)合、相關(guān)和估計(jì))，從而得到更為準(zhǔn)確和可靠的導(dǎo)航結(jié)果。這種對(duì)多種導(dǎo)航信息作綜合處理的技術(shù)稱為組合導(dǎo)航技術(shù)。從上述對(duì)慣導(dǎo)和衛(wèi)星導(dǎo)航的優(yōu)缺點(diǎn)描述中可以看出，兩者性能具有非常強(qiáng)的互補(bǔ)性，因而慣性/衛(wèi)星組合導(dǎo)航被公認(rèn)為是最佳的組合導(dǎo)航方案[3]，采用多元導(dǎo)航設(shè)備，如磁力計(jì)、全球?qū)Ш叫l(wèi)星系統(tǒng)接收機(jī)等與IMU組件組合而成的組合導(dǎo)航系統(tǒng)將會(huì)為艦船和飛行器等運(yùn)載體提供更加精確的運(yùn)載體位置、速度和姿態(tài)角信息，能夠進(jìn)一步提高和改善運(yùn)載體的導(dǎo)航性能[4]。

近幾年來(lái)關(guān)注更多的是基于非線性穩(wěn)定性理論[5]提出的非線性觀測(cè)器算法，它具有更好的靈活性，這類觀測(cè)器模型要求直接的姿態(tài)觀測(cè)信息，或者比較觀測(cè)向量和參考向量實(shí)施姿態(tài)計(jì)算，其基本原理由文獻(xiàn)[6]提出，如文獻(xiàn)[7-8]將其應(yīng)用到導(dǎo)航系統(tǒng)計(jì)算中，相比擴(kuò)展Kalman濾波方法，利用非線性觀測(cè)器方法的主要優(yōu)勢(shì)在于避免非線性系統(tǒng)函數(shù)的線性化操作，從而避免非線性函數(shù)的高階截?cái)嗾`差對(duì)計(jì)算精度的影響；非線性觀測(cè)器能夠獲得收斂界，提供穩(wěn)態(tài)收斂保證，觀測(cè)器整定參數(shù)更加直接有效，并且在非線性觀測(cè)器中，傳感器模型噪聲經(jīng)由濾波器參數(shù)適當(dāng)整定后的反饋增益矩陣表達(dá)出來(lái)，這樣做的好處是可以有效控制濾波器帶寬。相比擴(kuò)展Kalman方法[9]，非線性觀測(cè)器方法對(duì)非線性系統(tǒng)狀態(tài)參數(shù)計(jì)算具有較好的穩(wěn)定性能，系統(tǒng)動(dòng)態(tài)模型方程不需要線性化，計(jì)算量小，相比MEKF方法，其計(jì)算量減小23.6%[10]，在快速機(jī)動(dòng)場(chǎng)合中可以獲得較好的實(shí)時(shí)跟蹤效果。

目前GNSS/INS組合有很多種模式方法，相關(guān)的文獻(xiàn)也有很多，如松組合模式[11]，它利用INS和GNSS的位置組合方法，GNSS提供的位置信息實(shí)時(shí)修正INS輸出的位置量，而更進(jìn)一步的GNSS/INS緊耦合系統(tǒng)[12]則是利用GNSS導(dǎo)航天文中的偽距和偽距率觀測(cè)信息實(shí)時(shí)輔助修正組合系統(tǒng)的注入項(xiàng)，來(lái)自于C/A碼或者載波相位的偽距信息和偽距率測(cè)量信息中包含測(cè)量干擾或者誤差，如時(shí)鐘誤差、電離層誤差和多路徑效應(yīng)等，對(duì)時(shí)鐘誤差作為系統(tǒng)狀態(tài)變量計(jì)算，進(jìn)一步提高系統(tǒng)計(jì)算精度，對(duì)載波相位測(cè)量中引入整周模糊度的固定保持計(jì)算方法。

文中提出一種GNSS/INS緊耦合觀測(cè)器模型方法，利用GNSS偽距偽距率測(cè)量輔助INS導(dǎo)航系統(tǒng)，通過(guò)構(gòu)建姿態(tài)估計(jì)器模型，級(jí)聯(lián)了衛(wèi)星基座的偽距偽距率平移運(yùn)動(dòng)觀測(cè)器模型，在模型中把衛(wèi)星和接收機(jī)的時(shí)鐘誤差作為系統(tǒng)變量參與計(jì)算，提出一種新型的整周模糊度計(jì)算方法和偽距偽距率代數(shù)解計(jì)算方法。由計(jì)算機(jī)仿真驗(yàn)證，文中GNSS/INS緊耦合觀測(cè)器模型方法的計(jì)算效率高，在巡航導(dǎo)彈制導(dǎo)應(yīng)用中具有較好的實(shí)時(shí)性和較高的計(jì)算效率。

1 系統(tǒng)傳感器配置及其模型

文中主要目標(biāo)是設(shè)計(jì)RTK GNSS/INS緊耦合模型系統(tǒng)，估計(jì)計(jì)算運(yùn)載體的位置、線速度和姿態(tài)信息(PVA)，慣性測(cè)量組件安置在載體坐標(biāo)系(b系)中，衛(wèi)星測(cè)量系統(tǒng)的導(dǎo)航電文提供偽距、載波相位觀測(cè)量等信息，采用GNSS雙接收機(jī)模式，一個(gè)接收機(jī)位于地面站(Base station)，一個(gè)接收機(jī)固定在運(yùn)載體(Rover)上，考慮基于固定或者實(shí)值模糊度方法計(jì)算RTK基線解，并且假設(shè)地面站接收機(jī)位置是已知固定的，那么衛(wèi)星廣播信號(hào)來(lái)自于地面基站和運(yùn)載體接收機(jī)兩部分。定義載體坐標(biāo)系b，地球協(xié)議坐標(biāo)系e系(ECEF)和慣性坐標(biāo)系i系(Earth-centered Inertial Frame, ECI)，那么在地球系中可以獲得一組載體動(dòng)力學(xué)方程[13]為

(1)

四元數(shù)描述導(dǎo)彈載體姿態(tài)，如q:=[r;s]，其中r是標(biāo)量，s∈R3是三維向量，具有范數(shù)‖q‖=1；兩個(gè)四元數(shù)的乘積表示為，q1?q2，定義為

(2)

對(duì)于列向量x，x∈R3，其組成的斜對(duì)稱矩陣可表示為

(3)

那么兩個(gè)向量的叉積，如xa，xb，可表示為S(xa)xb。由四元數(shù)表達(dá)的旋轉(zhuǎn)矩陣表示為

(4)

基于捷聯(lián)假設(shè)IMU組件固定于載體系b中，慣性傳感器模型為

(5)

其中bb表示速率陀螺儀偏差，滿足條件‖bb‖2≤Mb，Mb是已知誤差邊界。

磁力計(jì)模型描述地球磁場(chǎng)三維向量[13]為

(6)

利用RTK技術(shù)要求兩個(gè)接收機(jī)，其中載體中固定的運(yùn)動(dòng)接收機(jī)觀測(cè)的GNSS偽距觀測(cè)模型為

(7)

式中：i——第i個(gè)衛(wèi)星;

yi——偽距觀測(cè)量；

β——待估計(jì)的偏差向量，β∈Rn;

ζi——描述了偏差向量元素對(duì)距離觀測(cè)量的影響。

當(dāng)考慮到Doppler偏移或者GNSS信號(hào)跟蹤時(shí)，需要進(jìn)一步偽距率觀測(cè)量，這樣做也是為了系統(tǒng)整理組合系統(tǒng)中的偏差誤差影響，偽距率觀測(cè)模型為

(8)

φi——偏差誤差向量β元素對(duì)偽距率觀測(cè)量的影響。

從前面的偽距和偽距率觀測(cè)向量方程可以看到，偽距偽距率方程是非線性的，很有必要從降低計(jì)算量角度探究偽距偽距率方程的二次特性來(lái)獲得相對(duì)簡(jiǎn)化的代數(shù)解問(wèn)題。

(9)

若假設(shè)已經(jīng)獲得4個(gè)偽距觀測(cè)向量yi(i=1,2,3,4)，以及3個(gè)線性不相關(guān)的視線向量，且有

和

且假設(shè)對(duì)所有的i=1,2,3,4，ζi=1，那么可定義

來(lái)給定

其中

(10)

其中

W=diag(1,1,1,-1)，

(11)

應(yīng)該說(shuō)明的是這里僅考慮緩慢時(shí)變系統(tǒng)誤差效應(yīng)，快速變化誤差如高頻噪聲等被忽略，因?yàn)榭焖贂r(shí)變?cè)肼曧?xiàng)可由增益整定處理或者高速時(shí)變?cè)肼暡挥绊懹^測(cè)器結(jié)構(gòu)。另外注意到偽距率方程在已知觀測(cè)基站或者接收機(jī)位置時(shí)成為線性方程，很容易求解計(jì)算速度，在典型的偽距觀測(cè)系統(tǒng)中，其余的觀測(cè)誤差很小，那么利用前面的定理很容易獲得觀測(cè)器的位置和速度初始化，即使有較小的位置和速度初始化誤差也是可以接受的，為了計(jì)算方便，文中給出載波相位誤差假設(shè)，在所有時(shí)間段內(nèi)，對(duì)所有的衛(wèi)星i=1,2,…,m，滿足

2 非線性姿態(tài)觀測(cè)模型

本模型的計(jì)算目標(biāo)是利用衛(wèi)星的偽距觀測(cè)量輔助，在載體系(b系)中的慣性測(cè)量組件(IMU)和衛(wèi)星導(dǎo)航系統(tǒng)組成緊耦合模式，估計(jì)載體位置、線速度和姿態(tài)信息(Position，Velocity，Attitude，PVA)。衛(wèi)星導(dǎo)航系統(tǒng)的導(dǎo)航電文提供了偽距、載波相位測(cè)量信息。為了計(jì)算表達(dá)方便,首先定義載體坐標(biāo)系b系，地球協(xié)議坐標(biāo)系e系和慣性導(dǎo)航坐標(biāo)系i系，并且標(biāo)記r代表運(yùn)動(dòng)載體，s代表衛(wèi)星基站。

2.1 姿態(tài)觀測(cè)模型

(12)

姿態(tài)觀測(cè)四元數(shù)誤差定義為

‖χ(t)‖2≤κe-λt≤‖χ(0)‖2,

(13)

可以表示為

從而注入項(xiàng)表達(dá)式可寫為

(14)

2.2 平移運(yùn)動(dòng)觀測(cè)模型

引入偽距和載波相位觀測(cè)方程，φ表示載波相位觀測(cè)量，單位是m，其與載波周期的關(guān)系為

φ=λφcycles，

其中λ表示載波信號(hào)的波長(zhǎng)，那么第i個(gè)衛(wèi)星的偽距觀測(cè)量可表示為

(15)

βr——接收機(jī)鐘差，可以表達(dá)為

βr=cΔc;

Δc——衛(wèi)星與接收機(jī)時(shí)鐘間的時(shí)鐘偏差；

對(duì)于運(yùn)動(dòng)載體和基站來(lái)說(shuō),環(huán)境誤差都是一樣的，因此有

和

考慮運(yùn)動(dòng)載體與衛(wèi)星基站間的單差分觀測(cè)問(wèn)題，它們接收第i顆衛(wèi)星的觀測(cè)信息，若進(jìn)一步考慮消除環(huán)境誤差因素，那么單差分偽距和載波相位觀測(cè)的差分表達(dá)式為

Δρi=Δψi+Δβ,

Δφi=Δψi+ΔΝiλ+Δβ,

(16)

其中

Δβ=βr-βs，

表示運(yùn)動(dòng)載體與基站之間的幾何基線；消除鐘差Δβ的方法可以采用在同一歷元中除了第i顆衛(wèi)星之外的第j顆衛(wèi)星輔助的二次差分觀測(cè)方法實(shí)現(xiàn)，

?Δρij=?Δψij,

?Δφij=?Δψij+?ΔΝijλ,

(17)

其中

?Δρij=Δρi-Δρj，

?Δψij=Δψi-Δψj，

?Δφij=Δφi-Δφj，

?ΔΝij=ΔΝi-ΔΝj。

實(shí)現(xiàn)這樣目的關(guān)鍵問(wèn)題是兩個(gè)接收機(jī)接收到的來(lái)自兩個(gè)衛(wèi)星的信號(hào)是同步性問(wèn)題，否則衛(wèi)星與接收機(jī)之間的幾何距離在觀測(cè)過(guò)程中會(huì)發(fā)生改變，導(dǎo)致差分觀測(cè)量不夠精確，涉及到衛(wèi)星信號(hào)的整周模糊度計(jì)算問(wèn)題，文獻(xiàn)中已經(jīng)有很多種整周模糊度的計(jì)算方法，文中討論一種稱之為“Fixing & Hold”的整周模糊度計(jì)算方法。

模糊度計(jì)算方法要求衛(wèi)星基站和接收機(jī)至少同時(shí)跟蹤5顆衛(wèi)星的信號(hào)，m≥5，模糊數(shù)可以看作一個(gè)復(fù)合變量表達(dá)為模糊數(shù)向量

考慮單差分觀測(cè)量或者雙差分觀測(cè)量的初始模糊數(shù)向量可由前面的偽距誤差量和載波相位誤差量表達(dá)式相減獲得

(18)

式中：Δρ，Δφ——單差分偽距觀測(cè)量;

?Δρ，?Δφ——雙差分偽距觀測(cè)量。

利用這兩個(gè)方程可以平均估計(jì)計(jì)算在一段時(shí)間內(nèi)歷元間的模糊數(shù)差分值，但是這兩個(gè)式子僅可以用于對(duì)初始模糊度的估計(jì)計(jì)算來(lái)確定每一個(gè)新衛(wèi)星的星座位置，并且衛(wèi)星經(jīng)由一段時(shí)間失鎖或者遮擋后，再次被捕獲時(shí)也可以采用這種初始模糊度估計(jì)計(jì)算方法來(lái)確定。

文中把模糊度向量作為觀測(cè)器狀態(tài)變量的一部分，可以實(shí)現(xiàn)實(shí)值模糊度的估計(jì)計(jì)算，在每一個(gè)歷元中對(duì)模糊度向量估計(jì)值進(jìn)行更新操作，在雙差分觀測(cè)情形中,可以測(cè)試模糊度實(shí)數(shù)估計(jì)值由一個(gè)整數(shù)集合最小化過(guò)程是否收斂到一個(gè)實(shí)數(shù)值來(lái)檢驗(yàn)，如

(19)

(20)

(21)

判斷是否成立，若該比率遠(yuǎn)大于某個(gè)閾值，檢測(cè)結(jié)果被接受，那么相應(yīng)于Ω1的整數(shù)被選擇出來(lái)作為固定的模糊度估計(jì)值，則有

(22)

此模糊度檢測(cè)過(guò)程在每一個(gè)歷元中都會(huì)被執(zhí)行一遍，它利用直到當(dāng)前歷元的所有模糊度數(shù)值做出決策，可以做到無(wú)遺漏決策判斷過(guò)程。

考慮載體坐標(biāo)系b與地球協(xié)議坐標(biāo)系e之間的旋轉(zhuǎn)，以及陀螺儀偏差和加速度計(jì)偏差，根據(jù)前面假設(shè)，在RTK GNSS觀測(cè)系統(tǒng)中考慮使用雙接收機(jī)配置，分別列出基站接收機(jī)和運(yùn)動(dòng)接收機(jī)的位置方程，根據(jù)前面的位置方程，在位置觀測(cè)量設(shè)計(jì)中考慮偽距誤差和載波相位誤差量的影響，引入運(yùn)動(dòng)載體位置量和偽距載波相位誤差的相關(guān)增益矩陣算子，可以獲得相應(yīng)的位置觀測(cè)方程；在基本速度方程的基礎(chǔ)上考慮運(yùn)動(dòng)載體速度觀測(cè)和偽距誤差、載波相位誤差的相關(guān)增益矩陣算子，可以獲得速度觀測(cè)方程；根據(jù)姿態(tài)注入項(xiàng)影響，引入輔助變量ξ來(lái)構(gòu)造加速度計(jì)比力觀測(cè)方程[18]，同時(shí)把鐘差和整周模糊度向量作為系統(tǒng)狀態(tài)變量設(shè)計(jì)出它們的觀測(cè)方程，整個(gè)系統(tǒng)模型算法數(shù)據(jù)流程如圖1所示。

圖1 GNSS/INS緊組合的姿態(tài)平移運(yùn)動(dòng)觀測(cè)器模型

比力估計(jì)表達(dá)式可由轉(zhuǎn)換運(yùn)動(dòng)觀測(cè)器獲得，轉(zhuǎn)換運(yùn)動(dòng)觀測(cè)器方程為

(23)

(24)

其中幾何距離差分估計(jì)項(xiàng)為

(25)

那么觀測(cè)誤差可表達(dá)為

(26)

由此引入系統(tǒng)估計(jì)誤差向量為

(27)

其中

(28)

(29)

(30)

利用同樣的步驟開展運(yùn)動(dòng)載體相關(guān)項(xiàng)的Taylor級(jí)數(shù)擴(kuò)展逼近計(jì)算，可以獲得直接的偽距觀測(cè)誤差量的Taylor級(jí)數(shù)擴(kuò)展逼近表達(dá)式為

(31)

其中

(32)

式中:h.o.t——Taylor級(jí)數(shù)展開式的高階項(xiàng)。

在觀測(cè)增益選擇中可以忽略，式(32)中Cρ,i的行向量表達(dá)式為

(33)

其中1i,m=[0,…,1,…,0]表示其中第i個(gè)元素非零，且有

(34)

由此可以獲得系統(tǒng)誤差動(dòng)力學(xué)方程為

(35)

其中矩陣

矩陣C是一個(gè)2m行的時(shí)變矩陣，

C=[Cρ,1;…;Cρ,m;Cφ,1;…;Cφ,m]，

并且式中干擾項(xiàng)定義為:

(36)

(37)

(38)

其中χ是由四元數(shù)的向量部分r和陀螺儀偏差b組成的復(fù)合變量，

χ=(r,b)，

(39)

同時(shí)也應(yīng)該看到對(duì)于一些常數(shù)γ3>0來(lái)說(shuō)，滿足

‖ρ2(t,χ)‖2≤γ3‖χ‖2

條件。

另外噪聲項(xiàng)

ε=(εy,1,…,εy,m;εv,1,…,εv,m)

的線性化結(jié)果導(dǎo)致了附加干擾項(xiàng)ρ3(t,x),滿足

條件等式。

3 系統(tǒng)Riccati方程的迭代計(jì)算過(guò)程

轉(zhuǎn)換運(yùn)動(dòng)觀測(cè)器方程可以利用修正-預(yù)測(cè)方法實(shí)施離散化操作，觀測(cè)器方程組被分成由注入項(xiàng)組成的線性修正部分和非線性預(yù)測(cè)部分，在IMU采樣時(shí)預(yù)測(cè)部分預(yù)測(cè)計(jì)算衛(wèi)星觀測(cè)量，預(yù)測(cè)計(jì)算衛(wèi)星觀測(cè)量來(lái)更新低速修正項(xiàng)部分；在IMU采樣中姿態(tài)觀測(cè)量實(shí)施歐拉角積分計(jì)算。因此整個(gè)觀測(cè)系統(tǒng)誤差方程被分成三個(gè)時(shí)間段開展修正預(yù)測(cè)計(jì)算步驟，其中IMU中姿態(tài)觀測(cè)量和轉(zhuǎn)換運(yùn)動(dòng)觀測(cè)器的預(yù)測(cè)部分以IMU采樣頻率速度執(zhí)行快速計(jì)算過(guò)程；轉(zhuǎn)換運(yùn)動(dòng)觀測(cè)器的修正部分以衛(wèi)星導(dǎo)航電文的接收速率來(lái)執(zhí)行，其中在接收導(dǎo)航電文數(shù)據(jù)中要求獲得衛(wèi)星位置估計(jì)數(shù)據(jù)，可以利用衛(wèi)星廣播星歷數(shù)據(jù)來(lái)估計(jì)計(jì)算。另外，衛(wèi)星的視線向量隨時(shí)間變化緩慢，轉(zhuǎn)換運(yùn)動(dòng)觀測(cè)器的增益矩陣K可以較低速率實(shí)時(shí)更新操作。從降低計(jì)算負(fù)擔(dān)角度來(lái)說(shuō)，觀測(cè)器增益可以數(shù)分鐘而不是以GNSS接收頻率來(lái)實(shí)施更新操作。

利用前面步驟中已經(jīng)確定的增益矩陣和方差矩陣來(lái)迭代計(jì)算離散時(shí)變Riccati方程，其步驟分為方差矩陣預(yù)測(cè)、增益矩陣計(jì)算和方差矩陣估計(jì)三個(gè)步驟完成,

(40)

式中：Ad——矩陣A的離散化表達(dá)。

將GNSS頻率作為采樣頻率，系統(tǒng)狀態(tài)變量為n維，系統(tǒng)狀態(tài)變量的初始方差為Q>0，觀測(cè)向量的初始方差陣為R>0，其由偽距噪聲方差組成對(duì)角元素，觀測(cè)器可由系統(tǒng)狀態(tài)變量的初始方差矩陣Q來(lái)整定，對(duì)應(yīng)于位置量和線速度量的噪聲元素設(shè)置得比較小,以利于獲得平滑估計(jì)，而比力噪聲分量元素設(shè)置的相對(duì)要大，同時(shí)最重要的是相應(yīng)于時(shí)鐘偏差的元素要設(shè)置得足夠大,以確保估計(jì)操作的快速收斂[18]。根據(jù)傳感器配置情況，設(shè)置系統(tǒng)狀態(tài)變量噪聲方差矩陣

(41)

觀測(cè)噪聲方差矩陣

R=diag{RΔρ,RΔψ,RΔv}。

(42)

4 系統(tǒng)仿真驗(yàn)證

4.1 仿真設(shè)置

以六旋翼飛行器作為運(yùn)動(dòng)載體，飛行器裝備了Xsens MTi 慣性測(cè)量組件ADIS 16488 IMU, 它由中等精度的陀螺儀、加速度計(jì)以及磁強(qiáng)計(jì)組成，其中陀螺儀具有20°/h(1σ)的偏差穩(wěn)定度，其工作頻率是410 Hz；u-Blox LEA-6T GNSS接收機(jī)以工作頻率5 Hz輸出偽距觀測(cè)量，同時(shí)還有一個(gè)在地面上相對(duì)固定位置的GNSS接收機(jī)通過(guò)實(shí)時(shí)動(dòng)態(tài)定位(RTK)計(jì)算提供導(dǎo)彈基準(zhǔn)位置信息，飛行器上的接收機(jī)和地面站接收機(jī)具有同樣特性，從而構(gòu)成無(wú)人飛行器載體GNSS/INS緊組合觀測(cè)模型實(shí)驗(yàn)測(cè)量平臺(tái)。若在直角坐標(biāo)系中目標(biāo)在空中做機(jī)動(dòng)運(yùn)動(dòng)，初始時(shí)刻相對(duì)運(yùn)動(dòng)位置為[50 km,60 km,25 km]，相對(duì)運(yùn)動(dòng)速度為[-18 00 m/s,-1 200 m/s,-500 m/s]，加速度矢量為[5g,6g,8g]，其中g(shù)表示目標(biāo)位置的重力加速度，導(dǎo)彈相對(duì)于運(yùn)動(dòng)目標(biāo)的航跡如圖2所示。

圖2 無(wú)人飛行器實(shí)驗(yàn)平臺(tái)

導(dǎo)彈載體觀測(cè)模型方程中的整定參數(shù)可由時(shí)變Riccati方程迭代計(jì)算獲得，設(shè)置如下：

k1=0.50，

k2=1.75，

kI=0.008，

kpp=0.06I，

kvp=0.11I，

kξp=0.006I,

θ=2，

λ=0.190 3 m，

其中I表示單位對(duì)角矩陣，還有投影算子參數(shù)

Mb=0.008 7，

增益矩陣選擇為

Kp=diag{0.08,0.04,0.06}。

作為鐘差初始值。

4.2 仿真數(shù)據(jù)分析

無(wú)人飛行器運(yùn)動(dòng)軌跡數(shù)據(jù)如圖3所示。

圖3 無(wú)人飛行器運(yùn)動(dòng)軌跡數(shù)據(jù)

由圖3可以看出，無(wú)人飛行器運(yùn)動(dòng)軌跡為比較系統(tǒng)模型算法性能，同時(shí)給出GNSS實(shí)時(shí)測(cè)量的估計(jì)數(shù)據(jù)，以及實(shí)際測(cè)試中的無(wú)人飛行器飛行軌跡數(shù)據(jù)。

系統(tǒng)模型算法計(jì)算誤差數(shù)據(jù)和實(shí)際測(cè)量誤差數(shù)據(jù)比較如圖4所示。

實(shí)驗(yàn)采用北東地坐標(biāo)系，由圖4可以看出，模型算法計(jì)算三個(gè)方向的位置誤差波動(dòng)比較小，誤差數(shù)據(jù)收斂快，數(shù)據(jù)計(jì)算穩(wěn)定性較強(qiáng)，系統(tǒng)模型算法計(jì)算獲得的路徑軌跡數(shù)據(jù)跟蹤實(shí)際數(shù)據(jù)效果比較好。

圖4 無(wú)人飛行器位置誤差數(shù)據(jù)比較

無(wú)人飛行器姿態(tài)數(shù)據(jù)比較如圖5所示。

圖5 無(wú)人飛行器姿態(tài)數(shù)據(jù)比較

由圖5可以看出，無(wú)人機(jī)載體姿態(tài)角實(shí)驗(yàn)數(shù)據(jù)為了比較方便，顯示了實(shí)際測(cè)試的姿態(tài)角數(shù)據(jù)和系統(tǒng)模型算法計(jì)算獲得的姿態(tài)角數(shù)據(jù)，特別是利用磁力計(jì)測(cè)量的航向角數(shù)據(jù)。

無(wú)人飛行器姿態(tài)方差數(shù)據(jù)如圖6所示。

圖6 無(wú)人飛行器姿態(tài)方差數(shù)據(jù)

由圖6可以看出，系統(tǒng)模型算法計(jì)算的姿態(tài)角誤差收斂快，表現(xiàn)出較強(qiáng)的計(jì)算穩(wěn)定性，但是姿態(tài)誤差角實(shí)際測(cè)量數(shù)據(jù)長(zhǎng)時(shí)間后明顯發(fā)散。另外與經(jīng)典的MEKF[19]算法相比，文中提出的多矢量觀測(cè)模型算法具有較小的計(jì)算量，MEKF算法需要經(jīng)過(guò)333 625次乘法和339 770次加法運(yùn)算，而多矢量觀測(cè)模型算法僅需要81 738次乘法和63 478次加法就可以完成算法計(jì)算流程，計(jì)算效率提高23.6%。因此，文中多矢量觀測(cè)模型算法計(jì)算效率高，很適合載體高速機(jī)動(dòng)運(yùn)動(dòng)過(guò)程中的快速計(jì)算要求。

通過(guò)數(shù)據(jù)對(duì)比表明，文中提出的多矢量觀測(cè)模型算法計(jì)算效率高，計(jì)算精度優(yōu)于常規(guī)算法，特別適合于運(yùn)動(dòng)載體高速機(jī)動(dòng)運(yùn)動(dòng)場(chǎng)合。

5 結(jié) 語(yǔ)

提出一種無(wú)人飛行器高速運(yùn)動(dòng)載體的RTK GNSS/INS緊組合多矢量觀測(cè)器模型，基于非線性穩(wěn)定性理論，利用非平行多觀測(cè)矢量方法構(gòu)建運(yùn)動(dòng)載體姿態(tài)觀測(cè)器和平移運(yùn)動(dòng)觀測(cè)器模型，模型中充分考慮IMU組件中的陀螺儀、加速度計(jì)和磁力計(jì)的觀測(cè)數(shù)據(jù)及其偏差影響，采用單差分GNSS方法和擴(kuò)展整周模糊度變量方法獲得一種新型的無(wú)人飛行器平移運(yùn)動(dòng)觀測(cè)，利用Kalman濾波的預(yù)測(cè)修正步驟計(jì)算系統(tǒng)方差矩陣的Riccati解。經(jīng)由無(wú)人飛行器實(shí)驗(yàn)平臺(tái)測(cè)試，文中提出的多矢量觀測(cè)模型算法計(jì)算量小，計(jì)算精度滿足小型無(wú)人飛行器飛行技術(shù)要求，驗(yàn)證了文中模型算法的有效性及其在實(shí)際使用中的巨大價(jià)值。