CNS/SINS組合導航系統的UKF算法研究

朱璐瑛 林雪原 王 萍 許家龍 陳祥光,3

1 煙臺南山學院工學院，山東省龍口市大學路12號，265713 2 航天晨光股份有限公司，南京市天元中路188號，211100 3 北京理工大學化學與化工學院，北京市中關村南大街5號，100081

CNS抗干擾能力強，誤差不隨時間積累，精度高達角秒級[1]，在航天飛機、載人飛船及深空探測等領域得到廣泛應用[2]；同時，SINS和CNS都是完全自主的導航系統，將二者組合也是目前的研究重點。組合導航系統的濾波方法主要分為線性和非線性，其中線性濾波方法以常規卡爾曼濾波為代表，而非線性濾波方法包括擴展卡爾曼濾波(EKF)、不敏卡爾曼濾波(UKF)及粒子濾波(PF)等。相對于擴展卡爾曼濾波和粒子濾波，不敏卡爾曼濾波因具有精度高及易于實現的特點而得到廣泛應用[3]。

目前，基于UKF的GNSS/SINS組合導航系統得到深入研究，主要原因是其速度和位置的樣本點、均值及方差較容易生成，且可通過線性疊加產生[4-6]；由于姿態角不是普通的矢量，故對基于UKF的CNS/SINS組合導航系統的研究相對繁瑣[7-8]。本文將UKF方法運用于CNS/SINS組合導航系統中，建立其非線性狀態方程及線性量測方程，并提出了一種基于UKF的CNS/SINS組合導航系統濾波算法。

1 基于UKF的組合導航濾波模型

1.1 標準UKF簡介

考慮如下形式的n維離散時間非線性系統：

(1)

式中，f(·)、h(·)為非線性函數；k為離散時刻；Xk為k時刻的系統狀態，假設其方差矩陣為Pk；uk為系統確定性輸入項；Zk為量測值；Wk和Vk分別為系統噪聲和量測噪聲，其方差矩陣分別為Qk和Rk。

由式(1)可知，標準UKF算法包含初始化、樣點計算、時間更新、量測更新及濾波更新等過程。因式(1)中狀態方程及量測方程均為典型的非線性方程，故UKF中的時間更新及量測更新過程也是典型的非線性運算，算法復雜度高。同時，標準UKF算法將系統狀態向量、系統噪聲和量測噪聲增廣為系統狀態，也進一步增大了濾波算法的運算量。

1.2 CNS/SINS組合導航系統濾波模型

結合式(1)的非線性系統模型，基于CNS/SINS組合導航系統的UKF算法，以導航參數直接作為被估計的狀態，采用捷聯慣性導航系統的機械編排方程作為狀態方程，且狀態方程無需進行線性化處理。選取N、E、U地理坐標系作為導航坐標系，慣導系統采取指北方位機械編排的形式，其狀態方程可表示為：

(2)

依據SINS工作原理，展開式如下：

(3)

CNS的輸出可轉換為地理坐標系下的橫滾角、俯仰角及航向角，故選取CNS輸出作為觀測量，則系統的量測方程可表示為：

(4)

對式(2)和式(4)進行離散化處理，可以得到離散化的狀態方程及量測方程，且假設導航系統的先驗狀態、過程噪聲方差和測量噪聲方差相互獨立。

1.3 CNS/SINS組合導航的簡化UKF算法

基于式(2)和式(4)可以看出，CNS/SINS組合導航系統具有狀態方程非線性而量測方程線性的特點，為此僅將系統狀態向量和系統噪聲增廣為狀態向量[9]，則簡化的系統增廣狀態向量為：

(5)

式中，χa、χX和χW分別為Xa、X和W的樣點，其維數分別定義為La、Lx和Lw，且La=Lx+Lw。

簡化后的UKF具體計算步驟如下[10-11]。

步驟1)：初始化。

(6)

步驟2)：樣點計算。

(7)

(8)

其中，λ=α2(n+κ)-La，α、β和κ為比例因子。

步驟3)：時間更新。

(9)

(10)

步驟4)：量測更新。

(11)

而

(12)

同理，有：

PXZ=Pk|k-1HT

(13)

步驟5)：濾波更新。

對比標準UKF算法，簡化UKF算法的計算復雜度得到了降低。

2 組合導航系統中姿態融合的設計

與位置和速度不同，計算姿態與真實姿態之間的誤差不能通過簡單的線性減法來實現，必須借助于捷聯姿態誤差分析原理進行。在捷聯式慣性導航系統中，用數字平臺坐標系模擬導航坐標系，由于平臺有誤差，故數字平臺坐標系(n′)和導航坐標系(n)之間存在著平臺角誤差，將這種平臺角誤差寫成列向量，可表示為[12]：

(15)

式中，φx、φy和φz分別為x、y和z方向平臺角誤差。則2個坐標系之間的轉換矩陣為：

(16)

對捷聯式系統計算的姿態陣為：

(17)

即

(18)

2.1 系統狀態向量協方差矩陣的設計

UKF算法中，位置、速度及IMU噪聲的樣點可以通過式(7)進行簡單的線性疊加得到，并且位置和速度對應的協方差矩陣也可通過式(10)計算得到。根據捷聯導航原理可知，為避免姿態計算過程中出現奇點(即方程退化的問題)，需要通過四元數乘法或方向余弦矩陣等算法來計算姿態角，是典型的非線性計算。為此，本文以方向余弦矩陣為工具，計算姿態角樣點、均值和方差，具體過程如下。

(19)

2.2 量測值一步預測誤差的確定

(20)

根據式(16)的定義，可以得到量測值一步預測誤差為：

(21)

3 仿真結果及分析

3.1 仿真條件

設定飛行器初始經度、緯度和高度分別為118°、29°、50 m，初始航向角、俯仰角和橫滾角分別為90°、0°和0°，飛行時間為3 600 s，捷聯解算周期為0.02 s，濾波周期為1 s。濾波初始參數設置如下：三維位置誤差均為5 m，三維速度誤差均為0.1 m/s，三維姿態角誤差均為0.5°；陀螺隨機游走驅動噪聲及陀螺白噪聲均為1°/h，加速度計隨機游走驅動噪聲及陀螺白噪聲均為10-4g；CNS測角誤差為300″，其采樣周期為1 s。系統仿真在MATLAB7.1環境下進行，仿真軌跡如圖1所示。

圖1 載體飛行軌跡Fig.1 Flight trajectory of carrier

3.2 仿真結果及分析

根據仿真初始條件的設定，對于相同導航傳感器仿真的原始數據，本文分別進行了基于下述濾波模型的仿真實驗：1)基于常規卡爾曼濾波[7-8]；2)基于簡化UKF算法；3)基于EKF算法[13]。圖2～4分別給出3組仿真實驗結果的位置誤差、速度誤差及姿態誤差對比曲線，可以看出，3種算法均可有效抑制位置及速度的發散，使姿態角處于收斂狀態，且本文簡化UKF算法的效果要優于常規卡爾曼濾波及EKF算法。

圖2 位置誤差曲線對比Fig.2 Position error curve

圖3 速度誤差曲線對比Fig.3 Velocity error curve

圖4 姿態誤差曲線對比Fig.4 Attitude error curve

為更加具體形象地說明3種濾波算法對姿態角的濾波效果，表1給出實驗結果數據統計的對比分析。可以看出，簡化UKF算法的姿態角濾波精度比EKF算法提高了約9%，比常規卡爾曼濾波算法提高了約14%。

表1 姿態均方誤差統計對比

為研究本文算法對濾波器初始三維姿態角誤差的敏感度，分別在初始三維姿態角誤差為1°及300″的情況下，對三維姿態角的均方差(即平臺角誤差)進行仿真，時長為600 s。圖5給出了平臺角誤差的協方差仿真曲線，可以看出，在2種濾波器初始三維姿態角誤差的條件下，本文算法得到的平臺角誤差協方差曲線完全重合，即本文算法對濾波器初始三維姿態角誤差的敏感度極低。為對濾波器初始階段協方差曲線的細節有更完全的描述，僅取圖5中1～4 s時間段進行對比，具體如圖6所示。可以看出，在2種初始條件下，經過3次濾波后本文算法得到的平臺角誤差協方差曲線完全重合，進一步說明了本文算法對濾波器初始三維姿態角誤差的魯棒性極高。

圖5 不同濾波器初始姿態誤差角條件下的平臺誤差角協方差對比曲線Fig.5 Platform angle error covariance contrast curve under the condition of different filter initial attitude error

圖6 圖5中起始時間段內的平臺角誤差協方差對比曲線Fig.6 Platform angle error covariance contrast curve during the starting period of Fig.5

基于圖2～4的仿真實驗，對標準UKF算法及本文簡化UKF算法的計算量進行對比分析。從表2可以看出，簡化UKF算法的計算時間比標準UKF算法減少了18.5%，有效地降低了算法的復雜度。

表2 算法計算量分析

4 結 語

CNS/SINS組合導航系統采用姿態融合的方式，其狀態方程具有強非線性、量測方程具有線性的特點。當采用UKF算法對系統進行濾波時，由于姿態角樣點、樣點均值和樣點方差的生成不同于以往GNSS/SINS組合導航系統模式，本文首先對標準UKF算法進行簡化，在生成姿態角樣點的基礎上，借助于平臺角誤差這一概念設計了姿態角樣點均值及方差的生成過程；同樣，借助于平臺角誤差，設計了量測更新過程中的量測一步預測誤差生成過程。仿真結果表明，本文算法具有比常規卡爾曼濾波及EKF算法更高的導航精度，同時本文算法對濾波器初始三維姿態角誤差具有更高的魯棒性。