小型無人飛行器的GNSS/INS緊組合觀測模型算法

申紅雪， 丁國強(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)的種類越來越多，比如慣導(dǎo)系統(tǒng)(Inertial Measurement Units, IMUs)、衛(wèi)星導(dǎo)航系統(tǒng)(Global Navigation Satellite System, GNSS)(如GPS)、磁羅盤、里程儀/多普勒測速儀/空速計(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í)間不斷累積，長期精度差；衛(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)而無法使用。在許多對(duì)導(dǎo)航性能要求苛刻的任務(wù)中，無論是精度還是可靠性要求高，任何單一的導(dǎo)航系統(tǒng)可能都無法滿足要求，這就需要使用多種導(dǎo)航系統(tǒng)同時(shí)對(duì)運(yùn)載體進(jìn)行導(dǎo)航信息測量，再對(duì)所有測量信息作綜合處理(包括檢測、結(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]。

近幾年來關(guān)注更多的是基于非線性穩(wěn)定性理論[5]提出的非線性觀測器算法，它具有更好的靈活性，這類觀測器模型要求直接的姿態(tài)觀測信息，或者比較觀測向量和參考向量實(shí)施姿態(tài)計(jì)算，其基本原理由文獻(xiàn)[6]提出，如文獻(xiàn)[7-8]將其應(yīng)用到導(dǎo)航系統(tǒng)計(jì)算中，相比擴(kuò)展Kalman濾波方法，利用非線性觀測器方法的主要優(yōu)勢在于避免非線性系統(tǒng)函數(shù)的線性化操作，從而避免非線性函數(shù)的高階截?cái)嗾`差對(duì)計(jì)算精度的影響；非線性觀測器能夠獲得收斂界，提供穩(wěn)態(tài)收斂保證，觀測器整定參數(shù)更加直接有效，并且在非線性觀測器中，傳感器模型噪聲經(jīng)由濾波器參數(shù)適當(dāng)整定后的反饋增益矩陣表達(dá)出來，這樣做的好處是可以有效控制濾波器帶寬。相比擴(kuò)展Kalman方法[9]，非線性觀測器方法對(duì)非線性系統(tǒng)狀態(tài)參數(shù)計(jì)算具有較好的穩(wěn)定性能，系統(tǒng)動(dòng)態(tài)模型方程不需要線性化，計(jì)算量小，相比MEKF方法，其計(jì)算量減小23.6%[10]，在快速機(jī)動(dò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)航天文中的偽距和偽距率觀測信息實(shí)時(shí)輔助修正組合系統(tǒng)的注入項(xiàng)，來自于C/A碼或者載波相位的偽距信息和偽距率測量信息中包含測量干擾或者誤差，如時(shí)鐘誤差、電離層誤差和多路徑效應(yīng)等，對(duì)時(shí)鐘誤差作為系統(tǒng)狀態(tài)變量計(jì)算，進(jìn)一步提高系統(tǒng)計(jì)算精度，對(duì)載波相位測量中引入整周模糊度的固定保持計(jì)算方法。

文中提出一種GNSS/INS緊耦合觀測器模型方法，利用GNSS偽距偽距率測量輔助INS導(dǎo)航系統(tǒng)，通過構(gòu)建姿態(tài)估計(jì)器模型，級(jí)聯(lián)了衛(wèi)星基座的偽距偽距率平移運(yùn)動(dòng)觀測器模型，在模型中把衛(wèi)星和接收機(jī)的時(shí)鐘誤差作為系統(tǒng)變量參與計(jì)算，提出一種新型的整周模糊度計(jì)算方法和偽距偽距率代數(shù)解計(jì)算方法。由計(jì)算機(jī)仿真驗(yàn)證，文中GNSS/INS緊耦合觀測器模型方法的計(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)，慣性測量組件安置在載體坐標(biāo)系(b系)中，衛(wèi)星測量系統(tǒng)的導(dǎo)航電文提供偽距、載波相位觀測量等信息，采用GNSS雙接收機(jī)模式，一個(gè)接收機(jī)位于地面站(Base station)，一個(gè)接收機(jī)固定在運(yùn)載體(Rover)上，考慮基于固定或者實(shí)值模糊度方法計(jì)算RTK基線解，并且假設(shè)地面站接收機(jī)位置是已知固定的，那么衛(wèi)星廣播信號(hào)來自于地面基站和運(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ì)模型描述地球磁場三維向量[13]為

(6)

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

(7)

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

yi——偽距觀測量；

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

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

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

(8)

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

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

(9)

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

和

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

來給定

其中

(10)

其中

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

(11)

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

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

本模型的計(jì)算目標(biāo)是利用衛(wèi)星的偽距觀測量輔助，在載體系(b系)中的慣性測量組件(IMU)和衛(wèi)星導(dǎo)航系統(tǒng)組成緊耦合模式，估計(jì)載體位置、線速度和姿態(tài)信息(Position，Velocity，Attitude，PVA)。衛(wèi)星導(dǎo)航系統(tǒng)的導(dǎo)航電文提供了偽距、載波相位測量信息。為了計(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)觀測模型

(12)

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

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

(13)

可以表示為

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

(14)

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

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

φ=λφcycles，

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

(15)

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

βr=cΔc;

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

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

和

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

Δρi=Δψi+Δβ,

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

(16)

其中

Δβ=βr-βs，

表示運(yùn)動(dòng)載體與基站之間的幾何基線；消除鐘差Δβ的方法可以采用在同一歷元中除了第i顆衛(wèi)星之外的第j顆衛(wèi)星輔助的二次差分觀測方法實(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)鍵問題是兩個(gè)接收機(jī)接收到的來自兩個(gè)衛(wèi)星的信號(hào)是同步性問題，否則衛(wèi)星與接收機(jī)之間的幾何距離在觀測過程中會(huì)發(fā)生改變，導(dǎo)致差分觀測量不夠精確，涉及到衛(wèi)星信號(hào)的整周模糊度計(jì)算問題，文獻(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ù)向量

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

(18)

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

?Δρ，?Δφ——雙差分偽距觀測量。

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

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

(19)

(20)

(21)

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

(22)

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

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

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

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

(23)

(24)

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

(25)

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

(26)

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

(27)

其中

(28)

(29)

(30)

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

(31)

其中

(32)

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

在觀測增益選擇中可以忽略，式(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來說，滿足

‖ρ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ì)算過程

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

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

(40)

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

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

(41)

觀測噪聲方差矩陣

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

(42)

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

4.1 仿真設(shè)置

以六旋翼飛行器作為運(yùn)動(dòng)載體，飛行器裝備了Xsens MTi 慣性測量組件ADIS 16488 IMU, 它由中等精度的陀螺儀、加速度計(jì)以及磁強(qiáng)計(jì)組成，其中陀螺儀具有20°/h(1σ)的偏差穩(wěn)定度，其工作頻率是410 Hz；u-Blox LEA-6T GNSS接收機(jī)以工作頻率5 Hz輸出偽距觀測量，同時(shí)還有一個(gè)在地面上相對(duì)固定位置的GNSS接收機(jī)通過實(shí)時(shí)動(dòng)態(tài)定位(RTK)計(jì)算提供導(dǎo)彈基準(zhǔn)位置信息，飛行器上的接收機(jī)和地面站接收機(jī)具有同樣特性，從而構(gòu)成無人飛行器載體GNSS/INS緊組合觀測模型實(shí)驗(yàn)測量平臺(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 無人飛行器實(shí)驗(yàn)平臺(tái)

導(dǎo)彈載體觀測模型方程中的整定參數(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ù)分析

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

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

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

系統(tǒng)模型算法計(jì)算誤差數(shù)據(jù)和實(shí)際測量誤差數(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 無人飛行器位置誤差數(shù)據(jù)比較

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

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

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

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

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

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

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

5 結(jié) 語

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