基于AFKF的多旋翼無人機組合導航技術研究

高彥釗，章衛國，郭 鑫，黃 海2，劉小雄

(1.西北工業大學 自動化學院, 西安 710072；2.中國船舶重工集團公司第七〇五研究所，西安 710000)

0 引言

捷聯慣性導航系統(strapdown inertial navigation system, SINS)是一種被廣泛應用于無人機制導、水下導航等領域的自主式導航系統，具有體積小、結構簡單、成本低等優點。在此基礎上，引入全球定位系統(global positioning system, GPS)的位置信息、速度信息作為觀測量，使用卡爾曼濾波器作為數據融合算法，使兩者形成優勢互補，便可克服單一SINS的誤差會隨時間積累的缺點，即為目前應用最為廣泛的GPS/SINS組合導航系統。

目前國內外在旋翼無人機組合導航系統上使用的數據融合算法主要是擴展卡爾曼濾波算法(extend kalman filter, EKF)。文獻[1]用松組合與常規卡爾曼濾波的方式，進行組合導航解算。文獻[2]針對多旋翼無人機自身的特性，為了改善組合導航系統的解算效果，在擴展卡爾曼濾波的基礎上，提出使用序貫處理、自適應濾波以及強跟蹤濾波等一系列改進算法。

傳統的擴展卡爾曼濾波算法的系統噪聲協方差矩陣和量測噪聲協方差矩陣均設定為常值，但是相對的，實際中系統的噪聲是隨機的，因此會產生濾波誤差。系統模型的準確與否能夠對擴展卡爾曼濾波算法的精確性產生深遠的影響，但是在實際系統的建立過程中，有目的的近似和簡化無法避免，導致模型沒有包含足夠的系統真實特征的信息，進而對最終濾波結果產生影響。

自適應漸消卡爾曼濾波(adaptive fading kalmanfilter, AFKF)就是為了解決常規的擴展卡爾曼濾波系統模型不準確的問題而被提出的。該方法通過實時計算遺忘因子，不斷調整系統的協方差陣，削減過去的數據權重，以此限制卡爾曼濾波器的“記憶”，即模型誤差累積。

國內外一些學者已經在自適應漸消卡爾曼濾波領域做了很多的研究工作。文獻[3]提出了加入自適應遺忘因子對系統模型進行實時調整的方法，并且深入研究了3種求取最佳遺忘因子的算法；文獻[8]針對上述方法進行了分析研究，對其直接對協方差陣進行加權漸消的處理存在合理性提出了質疑；文獻[4]針對文獻[8]所提出問題進行了深入研究，證明了文獻[3]算法的正確性和合理性。

基于上述研究成果，本文提出使用自適應漸消卡爾曼濾波作為旋翼無人機組合導航系統的融合算法，對系統模型進行實時的修正，以提高濾波結果的精度，最終改善旋翼無人機的導航解算效果。仿真結果表明了本文所使用算法的有效性。

1 旋翼無人機組合導航系統及濾波器模型

一般而言，旋翼無人機大多使用組合導航系統，由全球定位系統與捷聯慣性導航系統相組合，再融入如磁力計等外置傳感器，不僅克服了單一慣導系統會隨時間累計誤差的缺點，并且不受外界干擾，導航信息全面。

濾波方式共有兩種：

1)直接濾波法。使用系統的導航參數作為狀態量，直接對系統的導航參數進行估計。

2)間接濾波法。使用導航參數的誤差量作為狀態量，即對誤差量進行估計。

兩種濾波方式各有優缺點如下:

1)直接濾波法：

a)由于直接使用導航系統解算方程作為系統方程，所以該方法能夠準確地反映出系統狀態的真實變化情況。

b)對于濾波算法的計算周期要求嚴格，為了達到理想的濾波效果，需要將周期控制得很短。

c)直接濾波法所使用的狀態量在數量級上不統一，會大大增加計算復雜度，進而影響估計精度。

2)間接濾波法：

a)由于使用導航系統的誤差方程作為系統方程，所以該方法只能間接地反映系統狀態的變化，準確性不足。

b)對計算周期的要求比較寬松，在保證濾波器性能的前提下，該方法的周期可以在幾秒到一分鐘之內選取。

c)間接濾波法使用誤差作為狀態量，各個量之間相差很小，計算簡單。

綜上所述，由于間接濾波法對于量測更新的時間間隔沒有嚴格要求，并且計算量較小，結合旋翼無人機本身的特點，本文選取間接濾波方式，則系統的誤差量就是濾波器的狀態量。

選取系統的18個狀態量如下：

X=[δLδλδhδVEδVNδVUφEφNφU

εbxεbyεbzεrxεryεrz▽x▽y▽z]

(1)

其中:δL，δλ，δh分別是捷聯慣性導航系統的經度、緯度、高度誤差；δVE,δVN,δVU分別是捷聯慣性導航系統的東向、北向、垂直方向速度誤差；φE,φN,φU是系統的3個姿態角誤差；εbx,εby,εbz是陀螺儀的3個隨機漂移；εrx,εry,εrz是陀螺儀3個方向的相關漂移；▽x, ▽y, ▽z是加速度計3個方向的隨機漂移。

由卡爾曼濾波基本理論可知，系統的狀態方程可以表達為：

(2)

系統的白噪聲W(t)為：

W(t)=[ωgxωgyωgzωrxωryωrzωaxωayωaz]T

(3)

其中:ωgx，ωgy，ωgz是陀螺儀隨機漂移在載體坐標系下的白噪聲；ωrx，ωry，ωrz是載體坐標系下陀螺儀相關漂移的白噪聲；ωax，ωay，ωaz是載體坐標系下加速度計隨機漂移的白噪聲。

系統的噪聲分配矩陣G(t)為：

(4)

而系統的狀態轉移矩陣為：

(5)

本文中組合導航系統采用速度和位置組合模式。因此，系統的觀測量是捷聯慣性導航系統解算的速度信息和位置信息，與全球定位系統給出的速度信息和位置信息的差值。

定義速度的觀測量為：

(6)

由卡爾曼濾波的基本方程可知，量測方程的形式如下：

Zvet(t)=Hvet(t)X(t)+Wvet(t)

(7)

綜上可以得到速度的量測矩陣為：

(8)

定義位置的觀測量為：

(9)

量測方程的形式為：

Zpos(t)=Hpos(t)X(t)+Wpos(t)

(10)

綜上可以得到位置的量測矩陣為：

(11)

綜合式(7)和式(9)，可以知道系統的量測方程為：

(12)

式中，量測矩陣H(t)為：

(13)

量測噪聲為：

(14)

2 自適應漸消卡爾曼濾波基本原理

如下是一個線性、離散時間、隨機多變量系統：

x(k+1)=Φ(k+1,k)x(k)+G(k)w(k)

(15)

y(k)=H(k)x(k)+v(k)

(16)

其中:x(k)是n×1維狀態向量,y(k)是m×1維量測向量，Φ(k+1,k)和H(k)分別是狀態轉移矩陣和量測矩陣。w(k)和v(k)表示具有零均值的不相關高斯隨機向量序列，其協方差矩陣為:

E[w(k)wT(j)]=Q(k)δkj

(17)

E[v(k)vT(j)]=R(k)δkj

(18)

初始狀態被指定為一個隨機高斯向量。

(19)

(20)

如果系統是完全可觀的，那么描述最優估計的方程(常規卡爾曼濾波)是：

(21)

(22)

其中:

K(k)=P(k|k-1)HT(k)×

[H(k)P(k|k-1)HT(k)+R(k)]-1

(23)

P(k+1|k)=Φ(k+1,k)P(k)ΦT(k+1,k)+

G(k)Q(k)GT(k)

(24)

P(k)=[I-K(k)H(k)]P(k|k-1)

(25)

自適應漸消卡爾曼濾波就是在計算一步預測均方誤差時加入遺忘因子λ(k)：

P(k+1|k)=λ(k+1)Φ(k+1,k)P(k)ΦT(k+1,k)+

G(k)Q(k)GT(k)

(25)

其中:λ(k)≥1。

除了遺忘因子λ(k)外，方程(21)～(25)中描述自適應漸消卡爾曼濾波的方程與正規卡爾曼濾波的方程是相同的。

分析上述基本原理，可以發現，標準卡爾曼濾波器魯棒性差的根本原因，在于其最優增益的計算嚴重依賴驗前數據，但是其系統模型的建立過程中又會不可避免地存在近似或簡化。因此常規擴展卡爾曼濾波的估計誤差會不斷增大，嚴重影響濾波效果，甚至導致發散。

相對應的，自適應漸消卡爾曼濾波器在濾波過程中，不斷地通過遺忘因子對增益陣進行修正，如果估計誤差不斷增大，則遺忘因子λ(k)增大，相應的一步預測均方誤差增大，最終導致增益K(k)增大。換言之，過去時刻的誤差會導致遺忘因子對模型的調節，加大新息的權重，也就減小了過去時刻誤差對估計結果的影響。

3 遺忘因子的算法

自適應漸消卡爾曼濾波效果的好壞直接決定于遺忘因子λ(k)的求取，下面介紹一種求取遺忘因子的方法。

3.1 最優濾波器的性質

自適應漸消卡爾曼濾波希望通過求取一個最優的遺忘因子，使得其成為最優濾波器。換言之，遺忘因子應當滿足最優濾波器的性質。

最優濾波器具有一條重要的性質：在使用最優濾波增益時，下面方程所定義的殘差是一個白噪聲序列。

(27)

對于任意增益K(k)，可以證明殘差的協方差為:

Co(k)=E[z(k)zT(k)]=H(k)P(k|k-1)HT(k)+R(k)

(28)

殘差的自協方差為:

Cj(k)=E[z(k+j)zT(k)]=H(k+j)Φ(k+j,k+j-1)×

[I-K(k+j-1)H(k+j-1)]...Φ(k+2,k+1)×

[I-K(k+1)H(k+1)]...Φ(k+1,k)×

9[P(k|k-1)HT(k)-K(k)Co(k)]

?j=1,2,3...

(29)

顯然，由于白噪聲序列的性質，殘差序列的自協方差，即方程(29)應當等于零。觀察展開之后的方程式，可以發現C1(k)僅取決于最后一項，其他項均只包含將來時刻的增益K(k+j-1)，由此便將最優濾波器的性質轉換為了下述等價條件：

P(k|k-1)HT(k)-K(k)Co(k)=0

(30)

3.2 遺忘因子的求取

給定系統狀態方程(15)～(20)，將等價條件(30)展開，得到：

P(k|k-1)HT(k)×

{I-[H(k)P(k|k-1)HT(k)+R(k)]-1Co(k)}=0

(31)

假設1：Q(k),R(k),P(0)都是正定的。

假設2：量測矩陣H(k)是滿秩的。

在上述假設前提下，顯然可以將方程(31)等價為下式：

H(k)P(k|k-1)HT(k)=Co(k)-R(k)

(32)

展開后得到：

λ(k)M(k)=N(k)

(33)

其中，

M(k)=H(k)Φ(k,k-1)×P(k-1)ΦT(k,k-1)HT(k)

(34)

N(k)=Co(k)-[H(k)G(k-1)Q(k-1)×

GT(k-1)HT(k)]-R(k)

(35)

則最佳遺忘因子可由下式計算：

λ(k)=max{1,trace[N(k)]/trace[M(k)]}

(36)

注意：在實際應用中，式中的Co(k)是根據實測數據計算的，而不是根據理想化條件下的方程(28)計算的。

算法1自適應漸消卡爾曼濾波濾波器更新

1.時間更新

dx_k=fai*dx

P_k=fai*P*fai'+Q

P_k=(P_k+P_k')/2

2.構造量測量

z=[lat-INSGPS.GPS(GPS_loc,8)*deg2rad;

lng-INSGPS.GPS(GPS_loc,9)*deg2rad;

h-INSGPS.GPS(GPS_loc,10);

ve-INSGPS.GPS(GPS_loc,11)*

sin(INSGPS.GPS(GPS_loc,12)*deg2rad);

vn-INSGPS.GPS(GPS_loc,11)*

cos(INSGPS.GPS(GPS_loc,12)*deg2rad);

vu+INSGPS.GPS(GPS_loc,13)];

3.計算殘差

innovation=z-H*dx_k;

4.計算殘差的協方差

Cok=innovation*innovation';

5.計算Mk和Nk

Mk=H*fai*P*fai'*H'

Nk=Cok-H*Q*H'-R

6.計算遺忘因子

fading_factor=max(1,trace(Nk)/trace(Mk));

7.加入遺忘因子

P_k=fading_factor*P_k;

8.進行量測更新

K=P_k*H'*inv(H*P_k*H'+R);

dx=dx_k+K*(z-H*dx_k);

P=P_k-K*H*P_k;

4 仿真驗證

程序仿真環境為Matlab環境，仿真中使用的原始數據是四旋翼平臺試飛時保存的數據，使仿真程序最大程度的接近真實情況。

實驗所用旋翼無人機搭載的是Pixhawk。Pixhawk是一款開源飛控，屬于PX4開源項目，由3DR公司生產制造。Pixhawk的導航系統應用的也是以擴展卡爾曼濾波為基礎的組合導航方式，相比廣泛應用的GPS/SINS組合導航系統，融合了更多傳感器的數據，算法更復雜，所以可以以此作為基準，驗證設計算法的效果好壞。

在原始數據中，慣性測量單元20 ms保存數據的時間間隔是20 ms，相對應的，在仿真程序中三軸加速度計和三軸陀螺儀的更新頻率是50 Hz，比Pixhawk中組合導航系統的運行頻率要低很多。另一方面，全球定位系統兩組數據之間的時間間隔是200 ms，即全球定位系統數據的更新頻率是5 Hz，這是全球定位系統接收機正常的更新頻率。

擴展卡爾曼濾波的初始化數據如下所示：

P=diag([(2/Re)2,(2/Re)2,22,

0.052,0.052,0.052,

(0.5*pi/180)2,(1*pi/180/60)2,

(1*pi/180/60)2,(0.1*pi/180/3600)2,

(0.1*pi/180/3600)2,(0.1*pi/180/3600)2,

(0.1*pi/180/3600)2,(0.1*pi/180/3600)2,

(0.1*pi/180/3600)2,(50E-6*g0)2,

(50E-6*g0)2,(50E-6*g0)2])

(37)

q=diag([(0.04*pi/180)2,

(0.04*pi/180)2,(0.04*pi/180)2,

(0.04*pi/180)2,(0.04*pi/180)2,

(0.04*pi/180)2,(400E-6*g0)2,

(400E-6*g0)2,(400E-6*g0)2])

(38)

R=diag([(2.5/Re)2,(2.5/Re)2,32,

0.0052,0.0052,0.0052,])

(39)

4.1 算法仿真結果

下面給出用全球定位系統輸出的速度信息和位置信息，通過擴展卡爾曼濾波算法與自適應漸消卡爾曼濾波算法對捷聯慣導系統進行校正的仿真結果。

圖1 姿態角曲線

圖1中三幅圖分別為俯仰角、滾轉角、航向角的曲線圖。觀察以上曲線圖可以看出：

1)俯仰角方面。擴展卡爾曼濾波與自適應漸消卡爾曼濾波的濾波結果曲線均能夠較好的跟蹤基準曲線，誤差在1度左右，濾波效果良好。

2)滾轉角方面。擴展卡爾曼濾波與自適應漸消卡爾曼濾波的濾波結果曲線均能夠較好的跟蹤基準曲線，誤差較小，偶有野值，但是對總體濾波效果影響不大。

3)航向角方面。擴展卡爾曼濾波與自適應漸消卡爾曼濾波的濾波結果曲線與基準曲線相差不大，偶有野值，在250 s之后略有波動，但是基本不影響濾波效果。

圖2 速度曲線

圖2中三幅圖分別為東向速度、北向速度、天向速度的曲線圖。觀察以上曲線圖可以看出：

4)東向速度方面。擴展卡爾曼濾波與自適應漸消卡爾曼濾波的濾波結果曲線與基準曲線基本吻合，濾波效果良好。

5)北向速度方面。擴展卡爾曼濾波與自適應漸消卡爾曼濾波的濾波結果曲線均能夠較好的跟蹤基準曲線，誤差較小，但是對總體濾波效果影響不大。

6)天向速度方面。擴展卡爾曼濾波與自適應漸消卡爾曼濾波的濾波結果曲線與基準曲線相差不大，總體誤差在0.15m/s左右，濾波效果較好。

圖3 位置曲線

圖3中，三幅圖分別為緯度、精度、高度的曲線圖。觀察以上曲線圖可以看出：

在位置方面，常規擴展卡爾曼濾波由于全球定位系統硬件精度所限制，在緯度、經度、高度3個方面均具有較大誤差，自適應漸消卡爾曼濾波則明顯減小了誤差：

1)緯度方面，200～250 s之間，自適應漸消卡爾曼濾波將誤差縮小了50%以上； 250～350 s之間，自適應漸消卡爾曼濾波基本將誤差消除，與基準曲線重合。

2)經度方面， 200～350 s之間，自適應漸消卡爾曼濾波基本將誤差消除。

3)高度方面， 200～350 s之間,常規擴展卡爾曼濾波與基準曲線之間產生了較大誤差，約為2～3 m；自適應漸消卡爾曼濾波則大幅抑制了誤差，將仿真曲線限制在基準曲線附近。

通過上述仿真結果可得，當系統建模不夠準確或是硬件精度受限時，常規卡爾曼濾波會因此產生誤差，導致新量測值對估計值的修正作用下降，舊量測值對估計值得修正作用上升，繼而嚴重影響最終的濾波結果，采用自適應漸消卡爾曼濾波可以實時調整系統的協方差陣，較好地抑制濾波結果的誤差，提高濾波精度。

5 結論

本文首先介紹了目前無人機上常用的組合導航系統——GPS/SINS組合導航系統，其使用的數據融合算法以擴展卡爾曼濾波為主流。接著針對幾篇文獻以此為基礎進行了簡要分析，從而引出了作為改進算法的自適應漸消卡爾曼濾波，其相比于擴展卡爾曼濾波擁有精度高、模型準確的優點，分析了國內外學者對此進行的研究。

本文建立了濾波器模型、狀態方程以及量測方程，對自適應漸消卡爾曼濾波的原理進行詳細講解，深入解析了一種求取最優遺忘因子的方法，最后通過Matlab仿真程序，使用旋翼無人機飛行數據，對算法進行了仿真驗證，分析結果得到如下結論：

對于弱非線性系統，擴展卡爾曼濾波算法可以達到較好的濾波效果，但是在其建模過程中，無可避免的對其線性模型進行了近似和簡化，通常會導致系統模型缺乏足夠的關于真實系統特征的信息，繼而影響其濾波效果。自適應漸消卡爾曼濾波在擴展卡爾曼濾波基礎上，引入了遺忘因子，實時調整系統的協方差陣，削減過去的數據權重，以此限制卡爾曼濾波器的模型誤差，大幅提升濾波精度，對于改善旋翼無人機飛行效果具有重要意義。