辛琪，史忠科

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

引言

飛行姿態(tài)是導(dǎo)航制導(dǎo)的關(guān)鍵因素，其檢測(cè)精度決定了大型無人機(jī)的安全性能。為了實(shí)現(xiàn)對(duì)可安裝多種小型測(cè)量設(shè)備的大型無人機(jī)的姿態(tài)進(jìn)行高精度估計(jì)，客觀上要求采用多源姿態(tài)融合估計(jì)方法實(shí)現(xiàn)。文獻(xiàn)［1-3］給出了采用三天線GPS的姿態(tài)測(cè)量方法，但精度受GPS天線間基線長度的影響較大，且易受外界干擾。文獻(xiàn)［4］給出了基于圖像的姿態(tài)測(cè)量方法，但僅適合于近地點(diǎn)的載體姿態(tài)測(cè)量。文獻(xiàn)［1，5-7］給出了基于加速度計(jì)和磁強(qiáng)度計(jì)的測(cè)量方法，但僅適合于平穩(wěn)飛行時(shí)的姿態(tài)測(cè)量。文獻(xiàn)［8］給出了基于加速度計(jì)、磁強(qiáng)度計(jì)和空速的姿態(tài)測(cè)量方法，但由于空速對(duì)誤差的補(bǔ)償不足，導(dǎo)致精度不高。多源信息融合的最常用方法是將多個(gè)測(cè)量作為向量觀測(cè)，采用高維混合擴(kuò)展卡爾曼濾波（EKF）法進(jìn)行姿態(tài)估計(jì)［9］，但該方法需要解算高維矩陣的逆，增加了計(jì)算的復(fù)雜度和濾波誤差。

為了實(shí)現(xiàn)對(duì)任意飛行狀態(tài)下姿態(tài)的精確估計(jì)，本文首先對(duì)平穩(wěn)飛行的姿態(tài)測(cè)量方法進(jìn)行修正，給出了可適應(yīng)機(jī)動(dòng)飛行的四種姿態(tài)測(cè)量方法，再采用基于混合EKF的馬爾柯夫估計(jì)方法對(duì)四種姿態(tài)測(cè)量進(jìn)行融合估計(jì)。

1 基于混合EKF的姿態(tài)估計(jì)方法

飛機(jī)姿態(tài)具有多種表達(dá)方式，包括歐拉角、歐拉軸/旋轉(zhuǎn)角、四元數(shù)、方向余弦矩陣、羅德里格參數(shù)等，其中歐拉角因具有明確的物理意義、最少參數(shù)的表達(dá)以及無約束的解算，使得歐拉角成為最經(jīng)典的姿態(tài)表達(dá)方法。對(duì)于任意的角度α，記cα=cosα，sα=sinα，式（1）給出了采用歐拉角描述的飛行姿態(tài)動(dòng)態(tài)方程［1］:

式中，p，q，r為載體在x，y，z軸上對(duì)應(yīng)的角速度，可通過陀螺敏感；φ，θ，ψ為定義在北東地（NED）坐標(biāo)系下的姿態(tài)，分別為滾轉(zhuǎn)角、俯仰角和方位角。

采用陀螺測(cè)量的角速度常包含一定的誤差，如角速度常值偏差和高斯白噪聲等，pt=pm+Δp+wp（pt為p的真值；pm為p的測(cè)量值；Δp為pm引入的常值偏差；wp為pm引入的高斯白噪聲）。為了對(duì)姿態(tài)進(jìn)行精確估計(jì)，需同時(shí)考慮對(duì)角速度常值漂移誤差的估計(jì)。取估計(jì)狀態(tài)為x=［φθψΔpΔqΔr］T，則估計(jì)狀態(tài)的線性化模型為:

姿態(tài)的輔助測(cè)量方法很多，對(duì)于任何一種全姿態(tài)測(cè)量，其規(guī)范化模型可表達(dá)為:

式中，H=［E303］；vk為測(cè)量噪聲，服從N（03×1，R）分布，R=E［vkvkT］。

由于歐拉角動(dòng)態(tài)方程是非線性連續(xù)時(shí)間方程，而測(cè)量方程是離散時(shí)間方程，因而需要采用混合EKF方法對(duì)狀態(tài)進(jìn)行估計(jì)，步驟如下:

（2）采用四級(jí)四階Runge-Kutta算法對(duì)狀態(tài)預(yù)測(cè)值，k－1和狀態(tài)預(yù)測(cè)方差陣Pk，k－1進(jìn)行更新。狀態(tài)更新方程和狀態(tài)預(yù)測(cè)方差陣的更新方程為:

（3）計(jì)算增益矩陣Kk、狀態(tài)矩陣和狀態(tài)方差矩陣Pk:

2 多源姿態(tài)融合估計(jì)

2.1 問題的提出

大型無人機(jī)上可安裝多種姿態(tài)測(cè)量裝置，為了充分利用各種測(cè)量結(jié)果對(duì)姿態(tài)進(jìn)行修正，可采用基于混合EKF的姿態(tài)估計(jì)方法進(jìn)行姿態(tài)估計(jì)。該方法只需將基于混合EKF的姿態(tài)估計(jì)方法中的姿態(tài)測(cè)量矩陣、噪聲方差陣改為式（7）即可。但與基于單姿態(tài)測(cè)量采用混合EKF的姿態(tài)估計(jì)方法相比，該方法在實(shí)現(xiàn)多源姿態(tài)信息融合估計(jì)時(shí)存在求解高維矩陣的逆的問題，導(dǎo)致計(jì)算效率和濾波精度下降。

為了保證計(jì)算效率和濾波精度，需要對(duì)將多個(gè)姿態(tài)測(cè)量作為向量觀測(cè)的高維混合EKF姿態(tài)估計(jì)方法進(jìn)行改進(jìn)。

2.2 基本思路

如圖1所示，本文提出了采用馬爾柯夫估計(jì)和混合EKF相結(jié)合的方法對(duì)多種姿態(tài)觀測(cè)信息進(jìn)行融合。

圖1 多源姿態(tài)融合估計(jì)結(jié)構(gòu)圖

該方法首先將每路測(cè)量作為獨(dú)立觀測(cè)，采用混合EKF濾波建立了四路狀態(tài)估計(jì)，再采用馬爾柯夫估計(jì)對(duì)四路狀態(tài)估計(jì)的結(jié)果進(jìn)行綜合，給出狀態(tài)和狀態(tài)方差陣的最優(yōu)估計(jì)。其濾波步驟如下:

（1）對(duì)四路觀測(cè)分別采用基于混合EKF的姿態(tài)估計(jì)方法進(jìn)行濾波，給出狀態(tài)和狀態(tài)方差陣。

（2）采用馬爾柯夫估計(jì)對(duì)四路狀態(tài)更新的結(jié)果進(jìn)行綜合，式（8）給出了狀態(tài)估計(jì)和狀態(tài)方差陣的表達(dá):

為了采用該方法對(duì)任意飛行狀態(tài)下的姿態(tài)進(jìn)行精確估計(jì)，需要對(duì)常規(guī)的僅適合平穩(wěn)飛行的姿態(tài)測(cè)量方法進(jìn)行部分修正。

2.3 姿態(tài)輔助測(cè)量方法

2.3.1 基于加速度、磁強(qiáng)度和地速時(shí)間導(dǎo)數(shù)的姿態(tài)測(cè)量方法

式（9）給出了飛機(jī)對(duì)地速度的動(dòng)力學(xué)方程:

當(dāng)飛機(jī)水平時(shí)，其方位角為飛機(jī)機(jī)體y，x軸上測(cè)得的地磁強(qiáng)度分量Hny和Hnx之比的反正切值。而當(dāng)飛機(jī)存在傾斜角時(shí)，將地理坐標(biāo)系繞地向旋轉(zhuǎn)ψ之后的坐標(biāo)系記為投影坐標(biāo)系，則將地磁強(qiáng)度向量向投影坐標(biāo)系上投影后，可得方位角的表達(dá)式如下:

式中，［HbxHbyHbz］為機(jī)體上測(cè)得的地磁強(qiáng)度。

2.3.2 基于加速度、磁強(qiáng)度、空速的姿態(tài)測(cè)量方法

當(dāng)GPS的天線被遮蔽或GPS信號(hào)受到外界干擾時(shí)，GPS的觀測(cè)值極不可靠，常利用空速測(cè)量來彌補(bǔ)這一不足，飛機(jī)速度方程為:

再利用式（11）確定航向。

2.3.3 基于圖像的近地點(diǎn)姿態(tài)測(cè)量方法

無人機(jī)上一般都裝有攝像頭，可采用基于機(jī)場(chǎng)跑道的姿態(tài)確定方法給出無人機(jī)在近地點(diǎn)的姿態(tài)。圖2給出了基于機(jī)場(chǎng)跑道的姿態(tài)確定原理結(jié)構(gòu)圖。

圖2 基于圖像的姿態(tài)輔助測(cè)量原理圖

將攝像頭安裝在云臺(tái)上，調(diào)整云臺(tái)轉(zhuǎn)角，使機(jī)場(chǎng)跑道的著陸線P1P3與跑道線P1P2和P3P4總是處于鎖定狀態(tài)，采用圖像處理算法確定消失點(diǎn)P和跑道特征點(diǎn)P1和P3的圖像坐標(biāo)位置，再根據(jù)成像原理，給出攝像機(jī)坐標(biāo)系中P，P1，P3的圖像坐標(biāo)，結(jié)合P1P3⊥OcP給出P1點(diǎn)相對(duì)于P3點(diǎn)的尺度因子k，從而給出攝像機(jī)坐標(biāo)系的三軸單位向量:

式中，f為攝像機(jī)的焦距；（u，v），（u1，v1），（u3，v3）分別為P點(diǎn)、P1點(diǎn)、P3點(diǎn)的圖像坐標(biāo)；尺度因子k為（f2+uu3+vv3）/（f2+uu1+vv1）。則地理坐標(biāo)系到攝像機(jī)坐標(biāo)系的方向余弦矩陣為:

姿態(tài)矩陣為:

2.3.4 基于多天線GPS的姿態(tài)測(cè)量方法

大型無人機(jī)體積較大，具有安裝多天線GPS的能力?；诙嗵炀€GPS姿態(tài)測(cè)量方法的關(guān)鍵是求解Wabba問題［10］，即通過最優(yōu)化指標(biāo)尋找公用旋轉(zhuǎn)變換，從而確定兩個(gè)坐標(biāo)系間的關(guān)系。圖3為基于三天線GPS的姿態(tài)測(cè)量方法示意圖。

圖3 基于三天線GPS的姿態(tài)測(cè)量方法

通過 GPS可確定H點(diǎn)坐標(biāo)（xh，yh，zh），F(xiàn)點(diǎn)坐標(biāo)（xf，yf，zf）。通過OH與地理坐標(biāo)系的空間關(guān)系可確定俯仰姿態(tài)和航向:

3 仿真分析

3.1 仿真實(shí)驗(yàn)環(huán)境設(shè)置

按照式（19）配置載體的歐拉角動(dòng)態(tài):

式中，φ0=40°；θ0=0.5°；ψ0=110 － 15 sin（0.2）U（t10），U（tτ）為單位階躍函數(shù)，tτ=t－τ。陀螺的常值漂移誤差為2（°）/s，陀螺噪聲標(biāo)準(zhǔn)差為0.05（°）/s。姿態(tài)測(cè)量方法1的測(cè)量誤差為2°，方法2的測(cè)量誤差為3.5°，方法3的測(cè)量誤差為3°，方法4的測(cè)量誤差為2.5°。狀態(tài)初值為零向量，初始噪聲方差陣為mE3，m取為25。T=0.1 s，姿態(tài)更新周期和濾波周期均為0.1 s，試驗(yàn)100 s。

3.2 仿真結(jié)果及分析

圖4為本文方法與基于未經(jīng)校準(zhǔn)的角速度姿態(tài)解算方法的對(duì)比。圖中，“未校準(zhǔn)”為采用未扣除常值偏差的角速度解算的姿態(tài)；“真實(shí)”為導(dǎo)航級(jí)姿態(tài)儀表的輸出值；“本文方法”為基于多源測(cè)量信息的姿態(tài)融合估計(jì)結(jié)果。

圖4 仿真1:姿態(tài)算法對(duì)比

結(jié)果表明，不對(duì)陀螺進(jìn)行校準(zhǔn)，則會(huì)因角速度常值偏差導(dǎo)致姿態(tài)解算的發(fā)散。本文方法在估計(jì)姿態(tài)的同時(shí)，對(duì)角速度的常值偏差進(jìn)行了估計(jì)，并在姿態(tài)更新過程中扣除了角速度常值偏差對(duì)姿態(tài)的影響，再進(jìn)行姿態(tài)的時(shí)間更新和測(cè)量更新。仿真結(jié)果表明本文提出的多源姿態(tài)融合估計(jì)方法可保證姿態(tài)估計(jì)過程的收斂性。

圖5 仿真2:姿態(tài)誤差對(duì)比

圖5給出了采用本文方法和基于單個(gè)測(cè)量采用混合EKF的姿態(tài)估計(jì)誤差的對(duì)比結(jié)果。本文方法是四種基于單個(gè)測(cè)量采用混合EKF的姿態(tài)估計(jì)結(jié)果的馬爾柯夫估計(jì)，具有原理上最小的濾波誤差。試驗(yàn)結(jié)果表明，本文方法估計(jì)的姿態(tài)誤差一致優(yōu)于基于單個(gè)觀測(cè)采用混合EKF的估計(jì)結(jié)果。

圖6和圖7對(duì)本文方法和基于多個(gè)姿態(tài)測(cè)量的高維混合EKF方法的姿態(tài)估計(jì)誤差進(jìn)行了對(duì)比。

圖6 仿真3:姿態(tài)誤差對(duì)比

圖7 仿真3:角速度常值偏差估計(jì)對(duì)比

可以看出，本文方法由于避免了對(duì)高維矩陣的求逆運(yùn)算，降低了濾波增益矩陣的誤差，因而估計(jì)精度優(yōu)于基于多個(gè)姿態(tài)測(cè)量的高維混合EKF方法。

表1給出了六種估計(jì)方法的均方誤差，其中方法0是本文方法，方法5是高維混合EKF方法，方法1～方法4順序?qū)?yīng)基于單個(gè)測(cè)量的方法。由上述分析知，本文方法的濾波精度高于基于多個(gè)姿態(tài)測(cè)量的高維混合EKF，這兩種方法的估計(jì)精度高于任何一種基于單個(gè)觀測(cè)的混合EKF。

表1 六種估計(jì)方法的均方誤差

4 結(jié)束語

本文首先對(duì)混合EKF估計(jì)方法進(jìn)行了分析，提出了基于多個(gè)測(cè)量的高維混合EKF估計(jì)方法存在解算高維矩陣逆的問題。針對(duì)該問題，給出了對(duì)四種基于單個(gè)測(cè)量的混合EKF的狀態(tài)估計(jì)采用馬爾柯夫估計(jì)進(jìn)行綜合的方法，該方法可精確估計(jì)出角速度的常值偏差，保證濾波的收斂性；同時(shí)該方法的濾波結(jié)果優(yōu)于任何一種基于單個(gè)觀測(cè)的混合EKF的濾波結(jié)果和基于多個(gè)測(cè)量的高維EKF濾波結(jié)果。

［1］Gebre-Egziabher D，Hayward R C，Powell J D.Design of multi-sensor attitude determination systems［J］.IEEE Transactions on Aerospace and Electronic Systems，2004，40（2）:627-649.

［2］Gebre-Egziabher D，Hayward R C，Powell J D.A low cost GPS/inertial attitude heading reference system（AHRS）for general aviation applications［C］//IEEE Position Location and Navigation Symposium.Palm Springs:IEEE，1998:518-525.

［3］Cohen C E.Attitude determination using GPS ［D］.Palo Alto:Stanford University，1992.

［4］趙昊昱.基于視覺的無人機(jī)著陸導(dǎo)航［D］.武漢:華中科技大學(xué)，2006.

［5］Gebre-Egziabher D，Elkaim G H，Powell，J D，et al.A gyro-free quaternion-based attitude determination system suitable for implementation using low cost sensors［C］//IEEE Position Location and Navigation Symposium.San Diego:IEEE，2000:185-192.

［6］付旭，周兆英，熊沈蜀，等.基于EKF的多MEMS傳感器姿態(tài)測(cè)量系統(tǒng)［J］.清華大學(xué)學(xué)報(bào)（自然科學(xué)版），2006，46（11）:1857-1859，1863.

［7］王松，田波，戰(zhàn)榆莉，等.基于修正 EKF的微小型飛行器姿態(tài)估計(jì)［J］.高技術(shù)通訊，2011，21（6）:612-618.

［8］LIU Cheng，ZHOU Zhaoying，F(xiàn)U Xu.Attitude determination for MAVs using a Kalman filter［J］.Tsinghua Science and Technology，2008，13（5）:593-597.

［9］Dan Simon.Optimal state estimation:Kalman，H∞，and nonlinear approaches［M］.First Edition.New Jersey:John Wiley ＆ Sons，Inc Publication，2006:403-407.

［10］Wahba G.Problem 65-1:a least squares estimate of satellite attitude［J］.SIAM Review，1965，7（3）:409-409.