基于多源信息的飛行姿態估計方法

辛琪，史忠科

（西北工業大學自動化學院，陜西西安 710072）

引言

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

為了實現對任意飛行狀態下姿態的精確估計，本文首先對平穩飛行的姿態測量方法進行修正，給出了可適應機動飛行的四種姿態測量方法，再采用基于混合EKF的馬爾柯夫估計方法對四種姿態測量進行融合估計。

1 基于混合EKF的姿態估計方法

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

式中，p，q，r為載體在x，y，z軸上對應的角速度，可通過陀螺敏感；φ，θ，ψ為定義在北東地（NED）坐標系下的姿態，分別為滾轉角、俯仰角和方位角。

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

姿態的輔助測量方法很多，對于任何一種全姿態測量，其規范化模型可表達為:

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

由于歐拉角動態方程是非線性連續時間方程，而測量方程是離散時間方程，因而需要采用混合EKF方法對狀態進行估計，步驟如下:

（2）采用四級四階Runge-Kutta算法對狀態預測值，k－1和狀態預測方差陣Pk，k－1進行更新。狀態更新方程和狀態預測方差陣的更新方程為:

（3）計算增益矩陣Kk、狀態矩陣和狀態方差矩陣Pk:

2 多源姿態融合估計

2.1 問題的提出

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

為了保證計算效率和濾波精度，需要對將多個姿態測量作為向量觀測的高維混合EKF姿態估計方法進行改進。

2.2 基本思路

如圖1所示，本文提出了采用馬爾柯夫估計和混合EKF相結合的方法對多種姿態觀測信息進行融合。

圖1 多源姿態融合估計結構圖

該方法首先將每路測量作為獨立觀測，采用混合EKF濾波建立了四路狀態估計，再采用馬爾柯夫估計對四路狀態估計的結果進行綜合，給出狀態和狀態方差陣的最優估計。其濾波步驟如下:

（1）對四路觀測分別采用基于混合EKF的姿態估計方法進行濾波，給出狀態和狀態方差陣。

（2）采用馬爾柯夫估計對四路狀態更新的結果進行綜合，式（8）給出了狀態估計和狀態方差陣的表達:

為了采用該方法對任意飛行狀態下的姿態進行精確估計，需要對常規的僅適合平穩飛行的姿態測量方法進行部分修正。

2.3 姿態輔助測量方法

2.3.1 基于加速度、磁強度和地速時間導數的姿態測量方法

式（9）給出了飛機對地速度的動力學方程:

當飛機水平時，其方位角為飛機機體y，x軸上測得的地磁強度分量Hny和Hnx之比的反正切值。而當飛機存在傾斜角時，將地理坐標系繞地向旋轉ψ之后的坐標系記為投影坐標系，則將地磁強度向量向投影坐標系上投影后，可得方位角的表達式如下:

式中，［HbxHbyHbz］為機體上測得的地磁強度。

2.3.2 基于加速度、磁強度、空速的姿態測量方法

當GPS的天線被遮蔽或GPS信號受到外界干擾時，GPS的觀測值極不可靠，常利用空速測量來彌補這一不足，飛機速度方程為:

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

2.3.3 基于圖像的近地點姿態測量方法

無人機上一般都裝有攝像頭，可采用基于機場跑道的姿態確定方法給出無人機在近地點的姿態。圖2給出了基于機場跑道的姿態確定原理結構圖。

圖2 基于圖像的姿態輔助測量原理圖

將攝像頭安裝在云臺上，調整云臺轉角，使機場跑道的著陸線P1P3與跑道線P1P2和P3P4總是處于鎖定狀態，采用圖像處理算法確定消失點P和跑道特征點P1和P3的圖像坐標位置，再根據成像原理，給出攝像機坐標系中P，P1，P3的圖像坐標，結合P1P3⊥OcP給出P1點相對于P3點的尺度因子k，從而給出攝像機坐標系的三軸單位向量:

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

姿態矩陣為:

2.3.4 基于多天線GPS的姿態測量方法

大型無人機體積較大，具有安裝多天線GPS的能力。基于多天線GPS姿態測量方法的關鍵是求解Wabba問題［10］，即通過最優化指標尋找公用旋轉變換，從而確定兩個坐標系間的關系。圖3為基于三天線GPS的姿態測量方法示意圖。

圖3 基于三天線GPS的姿態測量方法

通過 GPS可確定H點坐標（xh，yh，zh），F點坐標（xf，yf，zf）。通過OH與地理坐標系的空間關系可確定俯仰姿態和航向:

3 仿真分析

3.1 仿真實驗環境設置

按照式（19）配置載體的歐拉角動態:

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

3.2 仿真結果及分析

圖4為本文方法與基于未經校準的角速度姿態解算方法的對比。圖中，“未校準”為采用未扣除常值偏差的角速度解算的姿態；“真實”為導航級姿態儀表的輸出值；“本文方法”為基于多源測量信息的姿態融合估計結果。

圖4 仿真1:姿態算法對比

結果表明，不對陀螺進行校準，則會因角速度常值偏差導致姿態解算的發散。本文方法在估計姿態的同時，對角速度的常值偏差進行了估計，并在姿態更新過程中扣除了角速度常值偏差對姿態的影響，再進行姿態的時間更新和測量更新。仿真結果表明本文提出的多源姿態融合估計方法可保證姿態估計過程的收斂性。

圖5 仿真2:姿態誤差對比

圖5給出了采用本文方法和基于單個測量采用混合EKF的姿態估計誤差的對比結果。本文方法是四種基于單個測量采用混合EKF的姿態估計結果的馬爾柯夫估計，具有原理上最小的濾波誤差。試驗結果表明，本文方法估計的姿態誤差一致優于基于單個觀測采用混合EKF的估計結果。

圖6和圖7對本文方法和基于多個姿態測量的高維混合EKF方法的姿態估計誤差進行了對比。

圖6 仿真3:姿態誤差對比

圖7 仿真3:角速度常值偏差估計對比

可以看出，本文方法由于避免了對高維矩陣的求逆運算，降低了濾波增益矩陣的誤差，因而估計精度優于基于多個姿態測量的高維混合EKF方法。

表1給出了六種估計方法的均方誤差，其中方法0是本文方法，方法5是高維混合EKF方法，方法1～方法4順序對應基于單個測量的方法。由上述分析知，本文方法的濾波精度高于基于多個姿態測量的高維混合EKF，這兩種方法的估計精度高于任何一種基于單個觀測的混合EKF。

表1 六種估計方法的均方誤差

4 結束語

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

［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］趙昊昱.基于視覺的無人機著陸導航［D］.武漢:華中科技大學，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傳感器姿態測量系統［J］.清華大學學報（自然科學版），2006，46（11）:1857-1859，1863.

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

［8］LIU Cheng，ZHOU Zhaoying，FU 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.