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

申紅雪， 丁國強

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

0 引 言

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

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

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

文中提出一種GNSS/INS緊耦合觀測器模型方法，利用GNSS偽距偽距率測量輔助INS導航系統，通過構建姿態估計器模型，級聯了衛星基座的偽距偽距率平移運動觀測器模型，在模型中把衛星和接收機的時鐘誤差作為系統變量參與計算，提出一種新型的整周模糊度計算方法和偽距偽距率代數解計算方法。由計算機仿真驗證，文中GNSS/INS緊耦合觀測器模型方法的計算效率高，在巡航導彈制導應用中具有較好的實時性和較高的計算效率。

1 系統傳感器配置及其模型

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

(1)

四元數描述導彈載體姿態，如q:=[r;s]，其中r是標量，s∈R3是三維向量，具有范數‖q‖=1；兩個四元數的乘積表示為，q1?q2，定義為

(2)

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

(3)

那么兩個向量的叉積，如xa，xb，可表示為S(xa)xb。由四元數表達的旋轉矩陣表示為

(4)

基于捷聯假設IMU組件固定于載體系b中，慣性傳感器模型為

(5)

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

磁力計模型描述地球磁場三維向量[13]為

(6)

利用RTK技術要求兩個接收機，其中載體中固定的運動接收機觀測的GNSS偽距觀測模型為

(7)

式中：i——第i個衛星;

yi——偽距觀測量；

β——待估計的偏差向量，β∈Rn;

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

當考慮到Doppler偏移或者GNSS信號跟蹤時，需要進一步偽距率觀測量，這樣做也是為了系統整理組合系統中的偏差誤差影響，偽距率觀測模型為

(8)

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

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

(9)

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

和

且假設對所有的i=1,2,3,4，ζi=1，那么可定義

來給定

其中

(10)

其中

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

(11)

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

2 非線性姿態觀測模型

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

2.1 姿態觀測模型

(12)

姿態觀測四元數誤差定義為

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

(13)

可以表示為

從而注入項表達式可寫為

(14)

2.2 平移運動觀測模型

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

φ=λφcycles，

其中λ表示載波信號的波長，那么第i個衛星的偽距觀測量可表示為

(15)

βr——接收機鐘差，可以表達為

βr=cΔc;

Δc——衛星與接收機時鐘間的時鐘偏差；

對于運動載體和基站來說,環境誤差都是一樣的，因此有

和

考慮運動載體與衛星基站間的單差分觀測問題，它們接收第i顆衛星的觀測信息，若進一步考慮消除環境誤差因素，那么單差分偽距和載波相位觀測的差分表達式為

Δρi=Δψi+Δβ,

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

(16)

其中

Δβ=βr-βs，

表示運動載體與基站之間的幾何基線；消除鐘差Δβ的方法可以采用在同一歷元中除了第i顆衛星之外的第j顆衛星輔助的二次差分觀測方法實現，

?Δρij=?Δψij,

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

(17)

其中

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

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

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

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

實現這樣目的關鍵問題是兩個接收機接收到的來自兩個衛星的信號是同步性問題，否則衛星與接收機之間的幾何距離在觀測過程中會發生改變，導致差分觀測量不夠精確，涉及到衛星信號的整周模糊度計算問題，文獻中已經有很多種整周模糊度的計算方法，文中討論一種稱之為“Fixing & Hold”的整周模糊度計算方法。

模糊度計算方法要求衛星基站和接收機至少同時跟蹤5顆衛星的信號，m≥5，模糊數可以看作一個復合變量表達為模糊數向量

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

(18)

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

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

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

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

(19)

(20)

(21)

判斷是否成立，若該比率遠大于某個閾值，檢測結果被接受，那么相應于Ω1的整數被選擇出來作為固定的模糊度估計值，則有

(22)

此模糊度檢測過程在每一個歷元中都會被執行一遍，它利用直到當前歷元的所有模糊度數值做出決策，可以做到無遺漏決策判斷過程。

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

圖1 GNSS/INS緊組合的姿態平移運動觀測器模型

比力估計表達式可由轉換運動觀測器獲得，轉換運動觀測器方程為

(23)

(24)

其中幾何距離差分估計項為

(25)

那么觀測誤差可表達為

(26)

由此引入系統估計誤差向量為

(27)

其中

(28)

(29)

(30)

利用同樣的步驟開展運動載體相關項的Taylor級數擴展逼近計算，可以獲得直接的偽距觀測誤差量的Taylor級數擴展逼近表達式為

(31)

其中

(32)

式中:h.o.t——Taylor級數展開式的高階項。

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

(33)

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

(34)

由此可以獲得系統誤差動力學方程為

(35)

其中矩陣

矩陣C是一個2m行的時變矩陣，

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

并且式中干擾項定義為:

(36)

(37)

(38)

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

χ=(r,b)，

(39)

同時也應該看到對于一些常數γ3>0來說，滿足

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

條件。

另外噪聲項

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

的線性化結果導致了附加干擾項ρ3(t,x),滿足

條件等式。

3 系統Riccati方程的迭代計算過程

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

利用前面步驟中已經確定的增益矩陣和方差矩陣來迭代計算離散時變Riccati方程，其步驟分為方差矩陣預測、增益矩陣計算和方差矩陣估計三個步驟完成,

(40)

式中：Ad——矩陣A的離散化表達。

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

(41)

觀測噪聲方差矩陣

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

(42)

4 系統仿真驗證

4.1 仿真設置

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

圖2 無人飛行器實驗平臺

導彈載體觀測模型方程中的整定參數可由時變Riccati方程迭代計算獲得，設置如下：

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表示單位對角矩陣，還有投影算子參數

Mb=0.008 7，

增益矩陣選擇為

Kp=diag{0.08,0.04,0.06}。

作為鐘差初始值。

4.2 仿真數據分析

無人飛行器運動軌跡數據如圖3所示。

圖3 無人飛行器運動軌跡數據

由圖3可以看出，無人飛行器運動軌跡為比較系統模型算法性能，同時給出GNSS實時測量的估計數據，以及實際測試中的無人飛行器飛行軌跡數據。

系統模型算法計算誤差數據和實際測量誤差數據比較如圖4所示。

實驗采用北東地坐標系，由圖4可以看出，模型算法計算三個方向的位置誤差波動比較小，誤差數據收斂快，數據計算穩定性較強，系統模型算法計算獲得的路徑軌跡數據跟蹤實際數據效果比較好。

圖4 無人飛行器位置誤差數據比較

無人飛行器姿態數據比較如圖5所示。

圖5 無人飛行器姿態數據比較

由圖5可以看出，無人機載體姿態角實驗數據為了比較方便，顯示了實際測試的姿態角數據和系統模型算法計算獲得的姿態角數據，特別是利用磁力計測量的航向角數據。

無人飛行器姿態方差數據如圖6所示。

圖6 無人飛行器姿態方差數據

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

通過數據對比表明，文中提出的多矢量觀測模型算法計算效率高，計算精度優于常規算法，特別適合于運動載體高速機動運動場合。

5 結 語

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