簡(jiǎn)化七階CQKF及其在SINS大失 準(zhǔn)角初始對(duì)準(zhǔn)中的應(yīng)用

孟 東，繆玲娟，邵?？?，沈 軍

(北京理工大學(xué)自動(dòng)化學(xué)院，北京 100081)

0 引 言

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

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

近年來(lái)，在信號(hào)處理的實(shí)際應(yīng)用中，數(shù)據(jù)并行處理技術(shù)、高頻芯片技術(shù)等技術(shù)發(fā)展不斷提升了硬件功能，數(shù)據(jù)處理能力大幅度提升。因此，新的硬件系統(tǒng)會(huì)減輕或者消除高運(yùn)算量所帶來(lái)的問(wèn)題；同時(shí)，高階算法的實(shí)用性能也得到了加強(qiáng)。

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

1 三階容積卡爾曼濾波CKF

1.1 考慮如下非線性系統(tǒng)：

(1)

式中：xk為k時(shí)刻的狀態(tài)向量，g(xk-1)和h(xk)為非線性函數(shù)，xk和zk分別是n維狀態(tài)向量和m維量測(cè)向量；wk-1和vk分別為系統(tǒng)的過(guò)程噪聲和量測(cè)噪聲，互為不相關(guān)的零均值高斯白噪聲，其統(tǒng)計(jì)特性為：

(2)

式中：Qk-1≥0為系統(tǒng)噪聲方差矩陣,Rk-1>0為量測(cè)噪聲方差陣矩陣,δ(k-1)j為Kronecker符號(hào)。

傳統(tǒng)的CKF濾波器建立在非線性離散動(dòng)態(tài)系統(tǒng)上，對(duì)任意階函數(shù) ，其積分可表示為[9]：

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

(3)

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

exp(-r2/2)d(r)

(4)

(5)

三階CKF化簡(jiǎn)為由一組2n個(gè)等權(quán)值的容積點(diǎn)(采樣點(diǎn))來(lái)實(shí)現(xiàn)積分的數(shù)值逼近，共有2n個(gè)采樣點(diǎn)。這就是CKF算法的基本思想。

2 改進(jìn)的簡(jiǎn)化七階容積卡爾曼濾波

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

2.1 改進(jìn)的七階半徑線積分準(zhǔn)則

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

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

(6)

(7)

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

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

2.2 簡(jiǎn)化七階球面積分準(zhǔn)則

根據(jù)文獻(xiàn)[11,13]中的七階球面簡(jiǎn)化準(zhǔn)則，球面積分有(n+1)(n2+8n+6)/3個(gè)容積點(diǎn)，有：

(8)

(9)

其中，點(diǎn)集yk,uk,wk分別表示為：

(10)

(11)

(12)

將式(9)簡(jiǎn)化為：

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

ωh3f(h3)+ωh4f(h4)

(13)

則參數(shù)具體表示為：

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

2.3 簡(jiǎn)化七階球-半徑準(zhǔn)則

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

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

(14)

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

3 七階正交簡(jiǎn)化容積卡爾曼濾波

高階簡(jiǎn)化CKF算法中的改進(jìn)線積分準(zhǔn)則，存在高階擴(kuò)展性難的問(wèn)題。正交容積卡爾曼(CQKF)算法將線積分準(zhǔn)則正交化，能有效地解決這個(gè)問(wèn)題。

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

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

3.1 七階正交簡(jiǎn)化線積分準(zhǔn)則

對(duì)于公式(3)，根據(jù)文獻(xiàn)[8-9]，應(yīng)用高斯-拉蓋爾正交準(zhǔn)則(GGLQ)，線積分可用正交點(diǎn)近似為：

(15)

其中，n′為近似階次。當(dāng)維數(shù)n確定后，解方程可以得到n′個(gè)值，即n′個(gè)正交采樣點(diǎn)，其相應(yīng)的權(quán)重為：

3.2 七階正交簡(jiǎn)化球-半徑準(zhǔn)則

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

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

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

(16)

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

(17)

則其相應(yīng)的容積點(diǎn)和權(quán)值為：

εi=

(18)

ωi=

(19)

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

4 晃動(dòng)基座下SINS大失準(zhǔn)角初始對(duì)準(zhǔn)仿真校驗(yàn)

4.1 SINS的非線性初始對(duì)準(zhǔn)模型[18-19]

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

(20)

(21)

4.2 仿真校驗(yàn)

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

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

(22)

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

表1 姿態(tài)角誤差統(tǒng)計(jì)結(jié)果

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

5 結(jié) 論

本文提出了7th-SCQKF算法，進(jìn)行了理論推導(dǎo)，提高了濾波精度。工作主要包括以下幾個(gè)方面：

1)根據(jù)簡(jiǎn)化CKF理論，本文推導(dǎo)出改進(jìn)簡(jiǎn)化七階CKF算法(7th-MSSRCKF)，改進(jìn)了原簡(jiǎn)化七階CKF(7th-SSRCKF)的推導(dǎo)。

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

3)晃動(dòng)基座下SINS大失準(zhǔn)角仿真試驗(yàn)對(duì)比表明，7th-SCQKF算法比7th-SSRCKF算法濾波精度高、穩(wěn)定性好，說(shuō)明了本文提出算法的有效性。