簡化七階CQKF及其在SINS大失 準角初始對準中的應用

孟 東，繆玲娟，邵海俊，沈 軍

(北京理工大學自動化學院，北京 100081)

0 引 言

捷聯慣性導航系統(Strapdown inertial navigation system, SINS)的初始對準在受到外界干擾或晃動基座的影響時，經過粗對準后，其失準角較大；在此基礎上進行的大失準角精對準，其狀態方程具有嚴重的非線性。因此，需要引入非線性卡爾曼濾波方法濾波。目前，非線性卡爾曼濾波算法主要有擴展卡爾曼濾波(EKF)[1-2]、無跡卡爾曼濾波(UKF)[3-5]、容積卡爾曼濾波(CKF)[6-7]、正交容積卡爾曼濾波(CQKF)[9-10]、簡化容積卡爾曼濾波(SSRCKF)[10-13]和粒子濾波(PF)[14]等，已被廣泛應用到工業環境中。

傳統非線性濾波算法一般最高精確到三階精度，近年來，高階濾波算法不斷被提出來。五階UKF[15]、五階CKF[16]、五階ECKF[17]、五階ICKF[18]、高階CQKF[9]、七階SSRCKF[13]等高階算法的提出，都提高了原三階算法的濾波精度。高階算法理論精度高，但其復雜度較高。

近年來，在信號處理的實際應用中，數據并行處理技術、高頻芯片技術等技術發展不斷提升了硬件功能，數據處理能力大幅度提升。因此，新的硬件系統會減輕或者消除高運算量所帶來的問題；同時，高階算法的實用性能也得到了加強。

為進一步提高濾波精度，本文提出了簡化七階CQKF(7th-SCQKF)濾波算法，并應用于SINS的大失準角初始對準中。首先，本文提出改進的簡化七階CKF(7th-MSSRCKF)濾波算法，重新推導了該算法的球-半徑準則公式；其次，在7th-MSSRCKF基礎上，引入正交半徑準則，提出了7th-SCQKF算法，提高了濾波精度。SINS的大失準角初始對準仿真試驗證明，新方法提升了濾波精度。

1 三階容積卡爾曼濾波CKF

1.1 考慮如下非線性系統：

(1)

式中：xk為k時刻的狀態向量，g(xk-1)和h(xk)為非線性函數，xk和zk分別是n維狀態向量和m維量測向量；wk-1和vk分別為系統的過程噪聲和量測噪聲，互為不相關的零均值高斯白噪聲，其統計特性為：

(2)

式中：Qk-1≥0為系統噪聲方差矩陣,Rk-1>0為量測噪聲方差陣矩陣,δ(k-1)j為Kronecker符號。

傳統的CKF濾波器建立在非線性離散動態系統上，對任意階函數 ，其積分可表示為[9]：

(x-μ)TM-1(x-μ))d(x)

(3)

式(3)可被分解為兩類積分，分別為線積分和球面積分，則有：

exp(-r2/2)d(r)

(4)

(5)

三階CKF化簡為由一組2n個等權值的容積點(采樣點)來實現積分的數值逼近，共有2n個采樣點。這就是CKF算法的基本思想。

2 改進的簡化七階容積卡爾曼濾波

針對多維濾波的高運算量問題，文獻[10-11]提出了簡化采點濾波的方法，降低了算法復雜度；文獻[12]根據簡化采點方法，提出了簡化容積卡爾曼濾波方法(SSRCKF)，分析了估計精度為三階和五階的SSRCKF濾波算法；文獻[13]在[12]基礎上，將估計精度擴展為七階，提出七階SSRCKF(7th-SSRCKF)算法。本文重新推導了原7th-SSRCKF的半徑線積分準則，彌補了其不足，提出改進的七階SSRCKF(7th-MSSRCKF)算法。

2.1 改進的七階半徑線積分準則

文獻[16]已經證明，如果積分公式(3)中的I(f)要達到七階精度，就必須滿足兩類積分同時達到七階精度。因此，七階SSRCKF算法要達到七階精度，需滿足七階半徑積分準則和球面積分準則均達到七階近似精度。改進的七階SSRCKF算法的半徑積分準則推導如下：

根據以上分析，對于七階的近似精度，m=3，有4個近似方程，對于不同的k的取值，式(4)擴展為：

(6)

(7)

式(7)即為所求的七階半徑準則。

對比文獻[13]，原7th-SSRCKF的七階線積分準則，m=2，只能達到五階的近似精度，不能達到七階的濾波精度，而式(7)所得的七階半徑準則可以精確到七階。因此，式(7)積分準則的推導，彌補了文獻[13]的不足，提升了濾波精度。

2.2 簡化七階球面積分準則

根據文獻[11,13]中的七階球面簡化準則，球面積分有(n+1)(n2+8n+6)/3個容積點，有：

(8)

(9)

其中，點集yk,uk,wk分別表示為：

(10)

(11)

(12)

將式(9)簡化為：

U(f)=ωh1f(h1)+ωh2f(h2)+

ωh3f(h3)+ωh4f(h4)

(13)

則參數具體表示為：

式(13)完成了簡化七階球面積分的分解，共有采樣點(n+1)(n2+8n+6)/3個。

2.3 簡化七階球-半徑準則

根據式(7)和式(13)，將改進的七階線積分準則和七階球面簡化積分準則結合起來，組成簡化七階球-半徑準則。在零均值和單位方差前提下，令λ=r2/2,七階正交簡化球-半徑準則可以表示為：

ωi′ωh2f(h2)+ωi′ωh3f(h3)+ωi′ωh4f(h4)

(14)

式(14)就是改進的簡化七階容積卡爾曼濾波(7th-MSSRCKF)的基本公式。根據式(4)～式(13)，式(14)中的各量均已知。由于文獻[13]中七階線積分準則的推導有缺陷，理論上就不能達到七階的濾波精度；從理論分析可知，本文提出改進簡化七階容積卡爾曼濾波(7th-MSSRCKF)能達到七階的濾波精度，提高了原7th-SSRCKF算法的性能。

3 七階正交簡化容積卡爾曼濾波

高階簡化CKF算法中的改進線積分準則，存在高階擴展性難的問題。正交容積卡爾曼(CQKF)算法將線積分準則正交化，能有效地解決這個問題。

CQKF算法[8]在CKF算法的基礎上，對線積分使用切比雪夫-拉蓋爾多項式得到正交點，實現對線積分的采樣，提高了濾波精度，并降低了容積準則的擴展難度。文獻[9]中的高階CQKF算法擴展了正交容積準則，將近似精度提高到五階。正交準則比普通的半徑準則濾波精度高[8]。

本文在高階簡化CKF基礎上，引入正交容積準則，擴展了其中的半徑線積分準則，提出七階正交簡化CKF(7th-SCQKF)算法，提高了濾波精度。

3.1 七階正交簡化線積分準則

對于公式(3)，根據文獻[8-9]，應用高斯-拉蓋爾正交準則(GGLQ)，線積分可用正交點近似為：

(15)

其中，n′為近似階次。當維數n確定后，解方程可以得到n′個值，即n′個正交采樣點，其相應的權重為：

3.2 七階正交簡化球-半徑準則

根據文獻[11，13]，參照第3.2節中的式(8)～式(13)，球面積分S7(r)有(n+1)(n2+8n+6)/3個容積點。式(13)是七階正交簡化CKF(7th-SCQKF)算法球面簡化積分的基本形式。

根據式(13)和式(15)，將七階正交簡化線積分準則和七階球面簡化積分準則結合起來，組成七階正交簡化球-半徑準則。在零均值和單位方差前提下，令λ=r2/2,七階正交簡化球-半徑準則表示為：

ωi′ωh3f(h3)+ωi′ωh4f(h4)

(16)

式(16)就是七階容積卡爾曼濾波(7th-SCQKF)的基本公式。根據式(8)～式(15)，式(16)中的各量均已知。式(16)的簡化模型為：

(17)

則其相應的容積點和權值為：

εi=

(18)

ωi=

(19)

式(6)～式(19)就是7th-SCQKF的基本公式，其采樣點總數為7(n+1)(n2+8n+6)/3。

4 晃動基座下SINS大失準角初始對準仿真校驗

4.1 SINS的非線性初始對準模型[18-19]

由于在導航系統中存在著各種誤差，所以SINS解算出的導航坐標系n′與理想的導航坐標系n并不完全重合。假設從n系到n′系可通過依次繞東西軸、天向軸、北向軸進行旋轉得到，旋轉角度分別為φE,φU,φN，其矢量形式定義為φ=[φEφUφN]T。此3次旋轉所對應的姿態變換矩陣可分別定義為Cφ,N，Cφ,E，Cφ,U；于是得到n系到n′系的姿態變換矩陣：

(20)

(21)

4.2 仿真校驗

為了進行對比，在相同SINS仿真條件下，分別利用以下3種濾波算法對整個初始對準過程進行濾波估計，3種方法分別是：UKF算法、改進簡化七階CQKF(7th-MSSRCKF)算法、七階正交簡化CKF(7th-SCQKF)算法。

在整個仿真過程中，為了檢驗本文所提出的濾波算法對于外界干擾的濾波能力，進行晃動基座下的初始對準仿真。假設SINS系統受到晃動基座的影響，航向角，俯仰角，橫滾角作周期變化，搖擺頻率分別為1 Hz，1.5 Hz，2 Hz，搖擺幅值分別為1°，3°，1°，其表達式為：

(22)

由圖1～圖3可知，航向角誤差變化較大，俯仰角和橫滾角誤差變化平穩， 3種濾波算法航向角誤差均在大約500 s后進入穩態， 7th-SCQKF的收斂速度比7th-MSSRCKF更快；取最后100 s航向角誤差的算術平均值作為穩態誤差，經過100次的Monte-Carlo仿真，3種算法姿態角穩態誤差的統計結果如表1所示。

表1 姿態角誤差統計結果

上述試驗結果證明了本文提出的7th-SCQKF算法高于UKF,7th-MSSRCKF算法的濾波精度。

5 結 論

本文提出了7th-SCQKF算法，進行了理論推導，提高了濾波精度。工作主要包括以下幾個方面：

1)根據簡化CKF理論，本文推導出改進簡化七階CKF算法(7th-MSSRCKF)，改進了原簡化七階CKF(7th-SSRCKF)的推導。

2)在七階7th-MSSRCKF算法基礎上，結合正交CKF算法，提出了7th-SCQKF算法，詳細介紹了其濾波步驟。

3)晃動基座下SINS大失準角仿真試驗對比表明，7th-SCQKF算法比7th-SSRCKF算法濾波精度高、穩定性好，說明了本文提出算法的有效性。