具有協(xié)方差反饋控制的無人機INS/GNSS組合自適應(yīng)CKF算法

李文敏，劉明威，高兵兵，胡高歌

（西北工業(yè)大學(xué)自動化學(xué)院，西安 710072）

1 引 言

慣性導(dǎo)航/全球?qū)Ш叫l(wèi)星系統(tǒng)（INS/GNSS）組合已成為無人機應(yīng)用中最常用的導(dǎo)航策略。然而，其非線性本質(zhì)以及動態(tài)屬性嚴(yán)重限制了無人機在復(fù)雜任務(wù)環(huán)境下的應(yīng)用[1-2]。

信息融合是實現(xiàn)組合導(dǎo)航的關(guān)鍵技術(shù)[3-4]。王文益和陳晨應(yīng)用線性卡爾曼濾波（KF），設(shè)計了一種基于衛(wèi)星/INS 組合導(dǎo)航系統(tǒng)的無人機誘捕方法[5]。然而，由于組合導(dǎo)航系統(tǒng)的非線性本質(zhì)，KF 已很難適用，特別是在復(fù)雜應(yīng)用環(huán)境中。擴展卡爾曼濾波（EKF）和無跡卡爾曼濾波（UKF）是常用的非線性濾波器[5-6]。然而，由于系統(tǒng)模型的線性化，EKF 對于強非線性系統(tǒng)的估計精度較差[6]，而UKF 在無跡變換中權(quán)值會出現(xiàn)負(fù)值，導(dǎo)致濾波結(jié)果不穩(wěn)定，特別是對于高維（三維以上）非線性系統(tǒng)，例如INS/GNSS 組合導(dǎo)航系統(tǒng)[7]。

容積卡爾曼濾波（CKF）是一種新興的非線性高斯濾波策略，其利用球面徑向容積法則逼近貝葉斯后驗概率密度函數(shù)的數(shù)值積分，使得狀態(tài)的后驗均值和協(xié)方差至少具有二階近似精度[3-7]。在初始誤差大、可觀測性弱的情況下，CKF 比EKF 具有更好的性能[6]。此外，文獻[8]已經(jīng)證實，對于高維濾波問題，CKF 相比UKF 具有更好的穩(wěn)定性和精度。此外，CKF 的計算負(fù)擔(dān)也小于UKF，因為它包含2n個容積點，而UKF 包含2n+1個Sigma 點，從而提高了系統(tǒng)的計算性能[9]。因此，對于系統(tǒng)維數(shù)至少為15 維的INS/GNSS 系統(tǒng)，CKF 是一個更優(yōu)的濾波選擇。Muhammad 研究了基于EKF 和CKF 的INS/GPS 組合導(dǎo)航濾波算法，表明了CKF 相對于EKF 的優(yōu)越性，有效提高了導(dǎo)航濾波器的非線性逼近性能[10]。

然而，KF 框架中的高斯濾波器有一個共同的局限性，即它們的濾波解算嚴(yán)重依賴系統(tǒng)噪聲統(tǒng)計參數(shù)的精確性[9,11-12]，CKF 也是如此。如果在濾波過程中使用有偏的系統(tǒng)噪聲統(tǒng)計，濾波結(jié)果會導(dǎo)致較大的估計誤差，甚至發(fā)散。因此，系統(tǒng)噪聲的統(tǒng)計特性是提高INS/GNSS 組合導(dǎo)航定位精度的關(guān)鍵。在INS/GNSS 組合導(dǎo)航系統(tǒng)中，量測噪聲協(xié)方差R與GNSS 接收機的導(dǎo)航誤差有關(guān)。借助先進的衛(wèi)星導(dǎo)航技術(shù)，使用高精度的GNSS 觀測設(shè)備或大量冗余的觀測數(shù)據(jù)可以保證R的精度[13]。然而，由于狀態(tài)模型的噪聲統(tǒng)計量總是與系統(tǒng)動力學(xué)具有強耦合的關(guān)系，所以很難獲取過程噪聲協(xié)方差矩陣Q的精確信息。因此，研究如何抑制有偏過程噪聲方差對INS/GNSS 組合系統(tǒng)解算精度的影響，提高系統(tǒng)的自適應(yīng)能力是十分必要的。

自適應(yīng)濾波是處理有偏過程噪聲統(tǒng)計影響的常用方法[14-15]。基于不同的自適應(yīng)策略，如協(xié)方差匹配技術(shù)、Sage-Husa 估計、貝葉斯推斷理論等，學(xué)者設(shè)計了大量的自適應(yīng)濾波，以抑制有偏過程噪聲方差對系統(tǒng)濾波解算的影響。協(xié)方差匹配技術(shù)是一種通過保持新息協(xié)方差與其理論值一致，從而在線估計過程噪聲協(xié)方差的自適應(yīng)策略[11]。然而，該方法在估計過程中會產(chǎn)生一定的穩(wěn)態(tài)估計誤差，導(dǎo)致濾波精度改善效果不佳。Sage-Husa估計器基于最大后驗原理獲取過程噪聲的統(tǒng)計信息[14]。然而，其不能保證收斂到正確的過程噪聲協(xié)方差，因此可能導(dǎo)致濾波發(fā)散[11]。貝葉斯推斷理論自適應(yīng)濾波器是基于時不變動態(tài)誤差假設(shè)建立的，并且需要大量的計算負(fù)荷，因而不適用于INS/GNSS 組合導(dǎo)航系統(tǒng)[16]。

對于提高CKF 濾波器自適應(yīng)能力的研究，Cui 等基于Sage-Husa 估計器，建立了一種用于INS/GNSS 組合導(dǎo)航的迭代CKF 算法，通過過程噪聲統(tǒng)計估計來抑制過程噪聲不確定性對濾波解算的影響[14]。然而，該方法中遺忘因子的選取嚴(yán)重依賴人工經(jīng)驗，改進性能不佳。通過將極大似然準(zhǔn)則與期望最大化方法相結(jié)合，Zhou 和Liu 提出了一種自適應(yīng)平方根CKF（ASCKF），用于在線估計和調(diào)整過程噪聲協(xié)方差矩陣[15]。然而，該方法的計算量會隨著時間的推移而無限增加，不適用于長時間的無人機導(dǎo)航。更重要的是，上述自適應(yīng)方法不能避免過程噪聲協(xié)方差估計的負(fù)定結(jié)果，需要額外的措施來確保過程噪聲協(xié)方差估計值的半正定性，這在一定程度上是有效的，但在濾波精度上是折衷的[16-17]。總體而言，在INS/GNSS 組合精確導(dǎo)航方面，如何更好地抑制過程噪聲協(xié)方差對CKF 解算的影響還缺乏有效的解決方案。

針對INS/GNSS 組合系統(tǒng)過程噪聲方差（Q）具有不確定性，本文提出了一種具有先驗狀態(tài)協(xié)方差反饋控制的自適應(yīng)CKF 算法，以提高無人機INS/GNSS 組合導(dǎo)航對過程噪聲方差的自適應(yīng)能力。在該算法中，將系統(tǒng)后驗狀態(tài)和誤差協(xié)方差信息反饋至濾波過程中，然后基于最大似然準(zhǔn)則，利用估計窗口內(nèi)的反饋狀態(tài)和誤差協(xié)方差信息，建立了一種先驗狀態(tài)協(xié)方差在線反饋控制策略。提出的方法有效抑制了過程噪聲方差不確定性對濾波解的影響，提高了CKF 用于無人機 INS/ GNSS 組合導(dǎo)航解算時的自適應(yīng)能力。通過無人機INS/GNSS 組合導(dǎo)航仿真實驗對提出算法的有效性進行了評估，并與經(jīng)典CKF 和ASCKF 進行了比較分析。

2 基于四元數(shù)的INS/GNSS 組合導(dǎo)航非線性數(shù)學(xué)模型

2.1 狀態(tài)模型

選擇E-N-U（東北天）地理坐標(biāo)系（g系）作為無人機導(dǎo)航坐標(biāo)系（n系）。將機體坐標(biāo)系表示為b系，慣性坐標(biāo)系表示為i系，地球坐標(biāo)系表示為e系。因此，系統(tǒng)的狀態(tài)變量為

其中，[δq0δq1δq2δq3]T表示加性姿態(tài)誤差四元數(shù)，[δvEδvNδvU]T為速度誤差，[δLδλ δh]T為位置誤差，[εbxεbvεbz]T為陀螺儀的常值漂移，[?bx?bv?bz]T為加速度計的零偏。

采用四元數(shù)來描述姿態(tài)誤差，可以將INS 姿態(tài)誤差方程寫成[18]

INS 的速度誤差方程為

INS 位置誤差的傳播方程為

其中，式(2)～式(4)中參數(shù)的含義參見文獻[18]。

接下來，陀螺的輸出誤差可描述為[15]

其中，bε為陀螺的常值漂移，gw是陀螺的白噪聲。

類似地，加速度計輸出誤差公式為[17]

其中，b?為加速度計零偏，aw是加速度計的白噪聲。

將式(2)～式(4)、式(6)和式(8)組合在一起，可以得到INS/GNSS 組合導(dǎo)航系統(tǒng)的狀態(tài)模型

其中，f(·)為連續(xù)形式的非線性系統(tǒng)函數(shù)，w(t)為系統(tǒng)過程噪聲。

2.2 量測模型

INS/GNSS 組合導(dǎo)航量測模型可描述如下[19]

3 具有協(xié)方差反饋控制的自適應(yīng)CKF 算法

3.1 經(jīng)典CKF 算法

為了更清楚地說明提出的自適應(yīng)CKF 算法的推導(dǎo)過程，首先簡要回顧經(jīng)典CKF 算法的計算過程。

采用改進的Euler 方法[20]對式(9)進行離散化，可以得到一般形式的非線性離散系統(tǒng)模型

其中，xk∈Rn和zk∈Rm表示在k時刻系統(tǒng)的狀態(tài)向量和量測向量；kw和kv為不相關(guān)的零均值高斯白噪聲序列，其方差矩陣為和為描述狀態(tài)模型的非線性函數(shù)，Hk是量測矩陣。

基于容積法則，經(jīng)典CKF 的計算過程總結(jié)如下。

步驟1：預(yù)測。應(yīng)用容積法則近似先驗狀態(tài)及其誤差協(xié)方差

步驟2：更新。由于量測模型為線性模型，則更新過程與卡爾曼濾波一致。

步驟3：返回步驟1，直到處理完所有樣本。

由式(14)可以看出，過程噪聲協(xié)方差信息Qk-1是由先驗狀態(tài)協(xié)方差來進行傳播的，其通過式(18)～式(20)來影響濾波增益Kk，從而影響系統(tǒng)狀態(tài)估計式(21)的結(jié)果。因此，當(dāng)過程噪聲協(xié)方差矩陣存在跳變或偏差時，濾波增益Kk將會失去最優(yōu)性，從而導(dǎo)致系統(tǒng)狀態(tài)估計精度變差，即組合導(dǎo)航誤差變大。

3.2 先驗狀態(tài)協(xié)方差反饋控制

針對上述問題，本節(jié)設(shè)計一種具有先驗狀態(tài)協(xié)方差反饋控制的自適應(yīng)CKF 算法。首先將系統(tǒng)后驗狀態(tài)和誤差協(xié)方差信息反饋至濾波過程中，構(gòu)成CKF 協(xié)方差傳播的閉環(huán)結(jié)構(gòu)；然后，基于極大似然準(zhǔn)則，利用估計窗口內(nèi)的反饋狀態(tài)和誤差協(xié)方差信息，建立一種先驗狀態(tài)協(xié)方差在線反饋控制策略，從而克服了經(jīng)典CKF 的缺陷，有效抑制過程噪聲方差不確定性對濾波解的影響，提高了濾波器的自適應(yīng)能力。

定義濾波器的新息向量為

其中，m為量測向量的維數(shù)表示矩陣的行列式，為新息向量的協(xié)方差矩陣，可由式(18)獲得。

在一個固定長度的估計窗口中，對似然函數(shù)(25)的對數(shù)求取關(guān)于的偏導(dǎo)數(shù)，并應(yīng)用矩陣的導(dǎo)數(shù)理論[21]，可以得出

其中，s和t分別表示矩陣的第s行和第t列。

由式(27)可以看出，上述極大似然估計問題已被轉(zhuǎn)化為計算新息協(xié)方差關(guān)于( 1,s,t=2, ,n… )的偏導(dǎo)數(shù)。

將式(18)代入式(27)得到

文獻[21-22]表明，當(dāng)估計窗口內(nèi)的濾波過程達到穩(wěn)態(tài)時，將趨于收斂，式(28)中的微分項將趨于1，因此可得

由式(21)和式(22)，有以下關(guān)系

因此，可以得到

3.3 算法實現(xiàn)

根據(jù)3.1 節(jié)和3.2 節(jié)的描述，提出的自適應(yīng)CKF 的算法流程如圖1 所示，其步驟可總結(jié)如下。

圖1 提出的自適應(yīng)CKF 算法流程圖Fig.1 Flow chart of proposed adaptive CKF

步驟1: 初始化。設(shè)置濾波器的初始狀態(tài)估計及其誤差協(xié)方差矩陣。

步驟2：預(yù)測。計算容積點并執(zhí)行經(jīng)典CKF預(yù)測步驟式(13)和式(14)。

步驟3：更新。執(zhí)行經(jīng)典CKF 更新步驟式(17)～ 式(22)，獲取系統(tǒng)的狀態(tài)估計及其誤差協(xié)方差矩陣。

通過式(34)調(diào)整先驗狀態(tài)協(xié)方差矩陣，然后，重新執(zhí)行經(jīng)典CKF 更新步驟式(17)～式(22)，獲取系統(tǒng)的狀態(tài)估計及其誤差協(xié)方差矩陣。

步驟4：返回步驟2，直到處理完所有樣本。

4 仿真驗證與分析

將提出的自適應(yīng)CKF 算法應(yīng)用于INS/GNSS組合導(dǎo)航系統(tǒng)，并通過蒙特卡洛仿真評估提出算法的有效性。

仿真驗證模擬固定翼無人機飛行，其主要參數(shù)如表1 所示以及運動模型見文獻[23]。根據(jù)無人機的基本運動狀態(tài)，設(shè)計一條包括平飛、勻速、加速、轉(zhuǎn)彎、爬升、下降等運動狀態(tài)的飛行軌跡，對無人機的飛行過程進行模擬。假設(shè)無人機的初始姿態(tài)為(0 ,0 ,0 )° ° ° ，初始速度為（0 m/s, 150 m/s, 0 m/s），初始位置為東經(jīng)108.997°、北緯34.246°、高度5000 m；軌跡仿真時間為1000 s，模擬的飛行軌跡如圖2 所示。

圖2 無人機飛行軌跡Fig.2 UAV flight trajectory

表1 無人機主要參數(shù)Table 1 Main parameters of UAV

仿真計算中，模擬過程噪聲統(tǒng)計存在不確定性的情形，以驗證提出的自適應(yīng)CKF 算法在過程噪聲統(tǒng)計存在不確定性時的濾波效果。在(300 s, 700 s)內(nèi)，濾波器使用的過程噪聲方差突變?yōu)?5×Qk-1，即

導(dǎo)航系統(tǒng)仿真參數(shù)設(shè)置如表2 所示，濾波周期1 s，蒙特卡洛次數(shù)20 次。

表2 仿真參數(shù)設(shè)置Table 2 Simulation parameters setting

圖3 和圖 4 分別給出了采用經(jīng)典CKF、ASCKF 和本文提出的自適應(yīng)CKF 算法得到的無人機位置和姿態(tài)的均方根誤差(RMSE)曲線。分析圖3 和圖4 可以看出：

圖3 經(jīng)典CKF、ASCKF 和本文提出的自適應(yīng)CKF 得到的無人機位置RMSE 曲線圖Fig.3 RMSEs of UAV position obtained by classical CKF、ASCKF and proposed adaptive CKF

圖4 經(jīng)典CKF、ASCKF 和本文提出的自適應(yīng)CKF 得到的無人機姿態(tài)RMSE 曲線圖Fig.4 RMSEs of UAV attitude obtained by classical CKF、ASCKF and proposed adaptive CKF

（1）在(300 s, 700 s)內(nèi)，濾波器中使用的過程噪聲方差突變，即過程噪聲統(tǒng)計存在不確定性。經(jīng)典CKF 由于自適應(yīng)能力較差，因而估計的導(dǎo)航參數(shù)誤差最大，明顯大于過程噪聲統(tǒng)計精確已知的時間段；而ASCKF 能夠通過噪聲統(tǒng)計的在線估計弱化過程噪聲統(tǒng)計突變的影響，并提高經(jīng)典CKF 的濾波精度，其估計誤差明顯小于經(jīng)典CKF；與上述兩種算法相比，本文提出的自適應(yīng)CKF 通過先驗狀態(tài)協(xié)方差的自適應(yīng)反饋控制，可

以獲得比ASCKF 更高的導(dǎo)航精度。這是由于提出的方法有效避免了對過程噪聲協(xié)方差負(fù)定估計結(jié)果的額外處理，保證了先驗狀態(tài)協(xié)方差的正定性，從而提高了無人機導(dǎo)航系統(tǒng)的自適應(yīng)能力和導(dǎo)航精度。

（2）在(0 s, 300 s)和(700 s, 1000 s)內(nèi)，由于過程噪聲協(xié)方差未出現(xiàn)突變，即過程噪聲統(tǒng)計特性精確已知，經(jīng)典CKF 能夠獲得最優(yōu)的估計結(jié)果。因而相比于ASCKF 和提出的自適應(yīng)CKF 算法，經(jīng)典CKF 具有最小的估計誤差；而ASCKF和本文提出的自適應(yīng)CKF 在提高系統(tǒng)自適應(yīng)能力的同時會犧牲部分過程噪聲統(tǒng)計特性精確已知情形的估計精度，在該情形下具有一定的次優(yōu)性，其原因是重新修正后的先驗協(xié)方差矩陣失去了最優(yōu)性。此外，進一步可以看出本文提出的自適應(yīng)CKF 在最大限度地提高系統(tǒng)自適應(yīng)能力的同時，能夠盡可能小地犧牲過程噪聲統(tǒng)計特性精確已知情形的估計精度。因而，提出的自適應(yīng)CKF 在復(fù)雜環(huán)境下具有相比于ASCKF 更優(yōu)的性能。

圖5 和圖6 描繪了經(jīng)典CKF、ASCKF 和提出的自適應(yīng)CKF 算法分別在(300 s, 700 s)內(nèi)得到的無人機位置和姿態(tài)的RMSE 均值的直觀比較。圖5 和圖6 的統(tǒng)計結(jié)果同樣表明，本文提出的自適應(yīng)CKF 顯著改善了經(jīng)典CKF 的自適應(yīng)性，從而減小了INS/GNSS 組合導(dǎo)航系統(tǒng)的導(dǎo)航誤差，提高了無人機導(dǎo)航系統(tǒng)的自適應(yīng)能力。

圖5 無人機在(300s, 700s)內(nèi)的位置RMSE 均值比較Fig.5 Comparison of mean RMSE of UAV position in (300s, 700s)

圖6 無人機在(300s, 700s)內(nèi)的姿態(tài)RMSE 均值比較Fig.6 Comparison of mean RMSE of UAV attitude in (300s, 700s)

5 結(jié) 論

本文提出了一種具有先驗狀態(tài)協(xié)方差反饋控制的自適應(yīng)CKF 算法，以提高無人機INS/GNSS組合導(dǎo)航對過程噪聲方差的自適應(yīng)能力。該算法是一種利用系統(tǒng)后驗狀態(tài)和誤差協(xié)方差信息，對先驗狀態(tài)協(xié)方差進行反饋調(diào)整控制的方法，有效地抑制過程噪聲方差不確定性對濾波解的影響，并保證了調(diào)整過程中先驗狀態(tài)協(xié)方差的正定性，從而提高了CKF 用于無人機INS/GNSS 組合導(dǎo)航解算時的自適應(yīng)能力。仿真實驗結(jié)果表明：在過程噪聲方差具有不確定性的情況下，提出方法的性能明顯優(yōu)于經(jīng)典CKF 和ASCKF，顯著改善了CKF 濾波器的自適應(yīng)性，提高了無人機 INS/ GNSS 組合系統(tǒng)的導(dǎo)航精度。