一種自適應(yīng)擴(kuò)展卡爾曼濾波在無人直升機(jī)導(dǎo)航系統(tǒng)中的應(yīng)用

杜佳新

中國直升機(jī)設(shè)計(jì)研究所

精準(zhǔn)的導(dǎo)航信息對于無人直升機(jī)穩(wěn)定飛行控制起著重要的作用。針對慣性導(dǎo)航系統(tǒng)由于慣性測量元件產(chǎn)生的誤差會(huì)隨著導(dǎo)航解算的過程不斷累積的問題，通過全球定位系統(tǒng)進(jìn)行組合導(dǎo)航，以四元數(shù)法為基礎(chǔ)建立慣性導(dǎo)航系統(tǒng)的非線性誤差模型。同時(shí)針對實(shí)際系統(tǒng)中噪聲信號統(tǒng)計(jì)特性不完全會(huì)干擾濾波精度而影響誤差估計(jì)的問題，提出一種自適應(yīng)擴(kuò)展卡爾曼濾波方法，該濾波方法可以在線估計(jì)噪聲特性以此保證濾波精度。仿真結(jié)果表明，該濾波方法可以提高組合導(dǎo)航系統(tǒng)的性能。

無人直升機(jī)由于具有可以在空中實(shí)現(xiàn)懸停、低空域飛行以及低速持續(xù)飛行的特點(diǎn)，在森林救火、軍事偵察、環(huán)境勘探、農(nóng)業(yè)種植等軍用和民用領(lǐng)域有著廣泛的應(yīng)用。無人直升機(jī)可以減小自身受飛行環(huán)境的影響，滿足在各種復(fù)雜環(huán)境下完成飛行任務(wù)。無人直升機(jī)通過姿態(tài)解算獲取當(dāng)前的姿態(tài)值，并將當(dāng)前的姿態(tài)值作為反饋輸入給飛控系統(tǒng)，再進(jìn)行控制律解算。因此，無人直升機(jī)能保持穩(wěn)定飛行的必要條件之一就是精確的姿態(tài)解算。

無人直升機(jī)可以通過慣性導(dǎo)航系統(tǒng)（INS）獲取當(dāng)前的姿態(tài)信息。慣性導(dǎo)航系統(tǒng)具有自主性的特點(diǎn)，通過慣性測量單元即陀螺儀和加速度計(jì)、計(jì)算機(jī)和穩(wěn)定平臺組成。陀螺儀測量載體相對于慣性空間的角速度，加速度計(jì)測量載體相對于慣性空間的線加速度，再通過導(dǎo)航解算得到載體相對于某一基準(zhǔn)坐標(biāo)系的導(dǎo)航參數(shù)。但是由于無人直升機(jī)在飛行時(shí)存在自身固有頻率的振動(dòng)以及電磁干擾，同時(shí)慣性測量元件的陀螺漂移和加速度計(jì)零偏產(chǎn)生的誤差會(huì)隨著導(dǎo)航解算過程不斷累積，這些因素都會(huì)造成單獨(dú)使用慣性導(dǎo)航系統(tǒng)去進(jìn)行導(dǎo)航解算無法滿足無人直升機(jī)穩(wěn)定飛行控制的精度要求。因此，需要通過另一種導(dǎo)航系統(tǒng)對慣性導(dǎo)航系統(tǒng)（INS）量測數(shù)據(jù)進(jìn)行實(shí)時(shí)修正即進(jìn)行組合導(dǎo)航。

由于全球衛(wèi)星定位系統(tǒng)（GPS）具有范圍遍布全球、定位精度高、觀測速度快以及受地域和時(shí)間的限制很小等多方面的優(yōu)點(diǎn)，因此可以通過全球定位系統(tǒng)（GPS）來修正慣性導(dǎo)航系統(tǒng)（INS）在導(dǎo)航解算時(shí)的累積誤差，這樣即保留了導(dǎo)航解算過程的自主性又提升了定位結(jié)果的準(zhǔn)確性和魯棒性。組合導(dǎo)航系統(tǒng)從其本質(zhì)上來看是將系統(tǒng)內(nèi)各個(gè)傳感器獲取的導(dǎo)航信息進(jìn)行綜合處理，針對這一處理過程目前國內(nèi)外已經(jīng)有多種理論方法，如最小二乘法、加權(quán)融合法、貝葉斯估計(jì)、卡爾曼濾波以及神經(jīng)網(wǎng)絡(luò)法等多種方法。

其中，應(yīng)用于導(dǎo)航領(lǐng)域最為廣泛的是卡爾曼濾波法，通過遞推手段可以實(shí)時(shí)解算出當(dāng)前的狀態(tài)量，同時(shí)還可以對下一次的狀態(tài)量進(jìn)行預(yù)測，但卡爾曼濾波僅適用于系統(tǒng)模型線性下精度較高，而針對此改進(jìn)的擴(kuò)展卡爾曼濾波可以將系統(tǒng)模型非線性的部分進(jìn)行線性化處理，從而提高濾波精度。同樣擴(kuò)展卡爾曼濾波也不適用于所有的情況，在考慮到實(shí)際系統(tǒng)模型不完全準(zhǔn)確或不能完全知悉干擾信號的噪聲特性下，會(huì)使得直接使用擴(kuò)展卡爾曼濾波方法進(jìn)行狀態(tài)估計(jì)的精度下降，甚至有可能導(dǎo)致濾波發(fā)散。

基于此，本文以INS/GPS組合導(dǎo)航作為無人直升機(jī)的導(dǎo)航模塊，在考慮到復(fù)雜飛行環(huán)境下噪聲統(tǒng)計(jì)特性未知的條件下，通過在線實(shí)時(shí)估計(jì)量測噪聲和狀態(tài)噪聲的自適應(yīng)擴(kuò)展卡爾曼濾波來進(jìn)行導(dǎo)航參數(shù)解算，并通過仿真驗(yàn)證該方法的有效性。

INS/GPS組合導(dǎo)航基本原理

組合導(dǎo)航技術(shù)是通過兩種或更多導(dǎo)航設(shè)備來獲取同一運(yùn)載體的導(dǎo)航信息，并將獲取到的導(dǎo)航信息進(jìn)行綜合處理，以此來提升導(dǎo)航信息的精度。以INS/GPS組合導(dǎo)航系統(tǒng)為例，GPS通過接收機(jī)獲取無人直升機(jī)的速度和位置信息，INS通過陀螺儀和加速度計(jì)獲取無人直升機(jī)的速度和位置信息，將兩個(gè)導(dǎo)航子系統(tǒng)位置和速度信息的量測量作差值，通過設(shè)計(jì)好的濾波器來進(jìn)行狀態(tài)估計(jì)得到導(dǎo)航子系統(tǒng)的誤差量，再將估計(jì)出的慣導(dǎo)系統(tǒng)導(dǎo)航參數(shù)的誤差值作為反饋來校正慣導(dǎo)系統(tǒng)的導(dǎo)航參數(shù)，以此來提升導(dǎo)航參數(shù)的精度。

基于INS/GPS組合導(dǎo)航系統(tǒng)的工作原理如圖1所示。整個(gè)組合導(dǎo)航系統(tǒng)的核心部分在于設(shè)計(jì)恰當(dāng)?shù)臑V波器，對導(dǎo)航信息進(jìn)行數(shù)據(jù)處理以此得到導(dǎo)航參數(shù)誤差的最優(yōu)估計(jì)，然后再根據(jù)最小均方誤差的規(guī)律對慣性導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)進(jìn)行修正，提高整個(gè)導(dǎo)航系統(tǒng)的精度，使無人直升機(jī)的飛行控制更加穩(wěn)定。

圖1 INS/GPS組合導(dǎo)航系統(tǒng)的工作原理。

INS/GPS組合導(dǎo)航系統(tǒng)誤差模型

INS的誤差狀態(tài)方程

對于慣性導(dǎo)航系統(tǒng)來說，最常見的姿態(tài)解算的方法為四元數(shù)法，既便于計(jì)算又可以保證解算精度。本文就通過四元數(shù)法解算慣性導(dǎo)航系統(tǒng)的姿態(tài)矩陣，從而推導(dǎo)出慣性導(dǎo)航系統(tǒng)的誤差模型，從而估計(jì)出慣性導(dǎo)航系統(tǒng)的誤差并對其進(jìn)行修正。

導(dǎo)航坐標(biāo)系為東北地坐標(biāo)系，機(jī)體坐標(biāo)系為與無人直升機(jī)機(jī)體本身固聯(lián)的坐標(biāo)系，其中原點(diǎn)O為無人直升機(jī)重心位置，縱軸OX沿著機(jī)身縱軸方向且其正方向與機(jī)頭朝向一致，垂向軸OZ與無人直升機(jī)的槳轂軸平行且與OX軸形成的平面在機(jī)體的對稱平面，其中正方向指向機(jī)體的下方，橫軸OX與XOZ平面垂直且正方向指向機(jī)體的右側(cè)。

若Q=[q0,q1,q2,q3]T為INS真實(shí)的四元數(shù)，而T為通過解算后得到的計(jì)算四元數(shù)，則有=Q+δQ其中δQ為真實(shí)的姿態(tài)四元數(shù)與計(jì)算四元數(shù)的偏差值，根據(jù)慣導(dǎo)解算的四元數(shù)微分方程可得出δQ的具體表達(dá)式即為INS的姿態(tài)誤差方程：

其中，

在式（1）～（6）中，用b代表機(jī)體坐標(biāo)系，用n代表導(dǎo)航坐標(biāo)系，用i代表慣性坐標(biāo)系，姿態(tài)誤差四元數(shù)表達(dá)式為δQ=[δq0,δq1,δq2,δq3]T，陀螺測量誤差表達(dá)式為=[εx,εy,εz]T，將陀螺測量誤差進(jìn)行補(bǔ)償后得到的輸出其表達(dá)式為=[ωx,ωy,ωz]T，無人直升機(jī)在導(dǎo)航坐標(biāo)系下的運(yùn)動(dòng)相對于慣性空間的角速度在導(dǎo)航坐標(biāo)系下的投影為[ωN,ωE,ωD]T,ωin的誤差表達(dá)式為=[δωN,δωE,δωD]T，INS的速度誤差表達(dá)式為δVn=[δVN,δVE,δVD]T，INS的位置誤差表達(dá)式為δP=[δL,δλ,δh]T（其中L表示無人機(jī)的緯度，λ表示無人機(jī)的經(jīng)度，h表示無人機(jī)的高度），用ωie代表地球自轉(zhuǎn)角速度，用RM代表沿子午圈的曲率半徑，用RN代表沿卯酉圈的曲率半徑。

若Vn=[VN,VE,VD]T為INS的真實(shí)速度，而[VN,VE,VD]T為經(jīng)過解算后得到的速度，由比力方程分別將Vn、展開，兩者作差忽略δgn可以得到INS的速度誤差方程為：

其中，

其 中，εb=[εbx,εby,εbz]T表 示 陀 螺 零 偏 向 量，εr=[εrx,εry,εrz]T表示一階馬爾可夫過程噪聲向量，Tr表示相關(guān)時(shí)間常數(shù)，wr=[wrx,wry,wrz]T表示驅(qū)動(dòng)噪聲向量，wg=[wgx,wgy,wgz]T表示陀螺觀測噪聲向量。 把加速度計(jì)量測誤差認(rèn)為是一階馬爾可夫過程，則可以得到加速度計(jì)誤差Δb為：

其中，Ta表示相關(guān)時(shí)間常數(shù)，wa=[wax,way,waz]T表示驅(qū)動(dòng)噪聲向量。 選取INS的誤差狀態(tài)量為 X(t)=[δq0,δq1,δq2,δq3,δVN,δVE,δVD,δL,δλ,δh,εbx,εby,εbz,εrx,εry,εrz,Δx,Δy,Δz]T，則由式（1）、（7）、（10）、（14）、（15）、（16）組成INS誤差狀態(tài)方程表示為：

W(t)=[wgx,wgy,wgz,wrx,wry,wrz,wax,way,waz]T表示過程噪聲向量。

INS/GPS組合導(dǎo)航系統(tǒng)觀測方程

將INS與GPS分別對運(yùn)載體的量測信息即速度和位置信息作差，并將其作為INS/GPS組合導(dǎo)航的觀測量，由此可得觀測方程為：

其中，MN、ME、MD表示GPS的速度誤差，NN、NE、ND表示GPS的位置誤差。

自適應(yīng)擴(kuò)展卡爾曼濾波

將INS/GPS組合導(dǎo)航系統(tǒng)狀態(tài)方程（17）和觀測方程（18）進(jìn)行離散化處理，可得到非線性方程為：

其中，Wk和ηk為高斯噪聲，有如下關(guān)系：

對于實(shí)際系統(tǒng)來說，可能存在著模型不完全準(zhǔn)確或者噪聲信號統(tǒng)計(jì)特性不完全的情況，這會(huì)使得標(biāo)準(zhǔn)卡爾曼濾波和擴(kuò)展卡爾曼濾波估計(jì)精度下降甚至引起濾波發(fā)散，因此在進(jìn)行濾波之前對于系統(tǒng)噪聲方差Qk和觀測噪聲方差Rk的實(shí)時(shí)估計(jì)就顯得至關(guān)重要，下面具體從狀態(tài)預(yù)測以及狀態(tài)更新兩方面來闡述濾波過程。

狀態(tài)預(yù)測部分由狀態(tài)一步預(yù)測方程和一步預(yù)測均方誤差所組成，如下。

狀態(tài)一步預(yù)測方程：

一步預(yù)測均方誤差：

狀態(tài)更新部分由濾波增益、狀態(tài)估計(jì)方程和估計(jì)均方誤差所組成，如下。

濾波增益：

狀態(tài)估計(jì)方程：

估計(jì)均方誤差：

為了能應(yīng)對在復(fù)雜環(huán)境下系統(tǒng)噪聲方差和觀測噪聲方差可能會(huì)發(fā)生變化而影響濾波器的精度，現(xiàn)通過對噪聲特性進(jìn)行實(shí)時(shí)估計(jì)以此來得到狀態(tài)量的最優(yōu)估計(jì)值?，F(xiàn)以k時(shí)刻為例，定義新息dk為濾波器實(shí)際量測值Zk和估計(jì)的觀測值的差值，則有：

由式（27）可知，新息dk把新的觀測值以及濾波器模型參數(shù)的信息全部包括進(jìn)來，這樣提升了在復(fù)雜環(huán)境下濾波器的抗干擾性，新息dk的實(shí)時(shí)估計(jì)方差通過開窗估計(jì)法可得：

其中，W表示滑動(dòng)數(shù)據(jù)窗口長度。

殘差方差為：

濾波增益為：

在式（34）兩邊右乘Rk可得

將式（35）代入式（32）得Rk估計(jì)值為

同理可得系統(tǒng)噪聲方差Qk-1估計(jì)值為

仿真及分析

圖2 無人直升機(jī)的速度估計(jì)誤差。

圖3 無人直升機(jī)的位置估計(jì)誤差。

結(jié)論

本文從分析組合導(dǎo)航原理出發(fā)以GPS來修正INS導(dǎo)航參數(shù)信息，選取INS的速度誤差以及位置誤差等為狀態(tài)量，并通過四元數(shù)法列寫INS的誤差方程即為狀態(tài)方程；選取INS和GPS對無人直升機(jī)速度和位置量測信息的差值作為觀測量并列寫觀測方程；針對實(shí)際系統(tǒng)中噪聲信號統(tǒng)計(jì)特性不完全的情況，提出一種對噪聲特性實(shí)時(shí)估計(jì)的自適應(yīng)擴(kuò)展卡爾曼濾波方法。

仿真結(jié)果表明，在噪聲統(tǒng)計(jì)特性未知的情況下此種濾波方法能夠有效估計(jì)出INS/GPS組合導(dǎo)航系統(tǒng)的速度以及位置誤差。但本文只驗(yàn)證了此種自適應(yīng)擴(kuò)展卡爾曼濾波方法可以估計(jì)出組合導(dǎo)航系統(tǒng)的速度與位置誤差，其濾波效果并未與基本擴(kuò)展卡爾曼濾波進(jìn)行比較，還需要通過仿真對比不同噪聲特性環(huán)境下兩種濾波的效果。