基于梯度下降算法的飛行器姿態解算

高 怡，李東航，郭 飄

（西安石油大學電子工程學院，陜西西安 710065）

獲得精確的實時飛行姿態是實現飛行器控制的前提條件。對于傳統的慣性測量單元(Inertial Measurement Unit,IMU)，陀螺儀與加速度計的測量性能各有利弊：加速度計在實際使用中的動態性能很差，物體高速運動下所測的數據有很大的延遲；陀螺儀可以有效處理高速運動下的動態數據，可對加速度計的數據進行補償。但僅憑加速度計和陀螺儀的數據融合不能很好地求解偏航角的值，還需要引入磁力計來修正。

針對上述問題，國內外學者提出了多種姿態解算方法。常見的方法有互補濾波(Complementary Filtering,CF)[1]法、擴展卡爾曼濾波(Extended Kalman Filter,EKF)法[2-3]、梯度下降(Gradient Descent)法[4-6]等。

文獻[1]使用互補濾波算法，針對陀螺儀在低頻段的動態響應特性較差，使用高通濾波器抑制噪聲；加速度計、磁力計在高頻段的動態響應特性較差，使用低通濾波器抑制噪聲，從而實現傳感器動態響應特性的互補。該方法實用性強，但不適用于高精度要求的導航系統中。文獻[2]中基于擴展卡爾曼濾波算法提出了一種改進算法，解算偏航角時加入一個方差矩陣，可以自適應改變卡爾曼增益值，從而減小了磁場對偏航角解算的影響。文獻[3]提出了高階線性互補濾波與擴展卡爾曼濾波相結合的融合算法，明顯地去除了噪聲并抑制了姿態角的漂移。EKF 算法雖然能求解出高精確度的姿態，但該算法計算量很大，對處理器的運算速度和精度要求很高，不適用于低成本飛行器應用。文獻[4]首次將梯度下降(Gradient Descent,GD)法應用在了姿態解算領域中。GD 算法具有可快速跟隨高速運動中的姿態變化、收斂速度快等優點，適用于低成本飛行器的研究。

1 飛行器姿態描述

1.1 建立坐標系

選用地理坐標系東、北、天（O-ENU）作為參考坐標系（Reference Coordinate System）。載體坐標系（Carrier Coordinate System）X軸為載體平面向右方向，Y軸為機頭所對的方向，Z軸與載體橫截面垂直向上，構成坐標系O-XYZ，如圖1 所示。

圖1 載體坐標系

1.2 姿態角

載體的姿態角偏航角、滾轉角和俯仰角[7-8]——載體繞Z軸旋轉，載體Y軸在參考坐標系E-O-N平面上的投影與參考坐標系N軸之間的夾角稱為偏航角(Yaw)，用字母ψ表示；載體繞Y軸旋轉，載體Z軸在參考坐標系E-O-U平面上的投影與參考坐標系U軸之間的夾角稱為滾轉角(Roll)，用字母γ表示。載體繞X軸旋轉，載體Y軸在參考坐標系U-O-N平面上的投影與參考坐標系N軸的夾角稱為俯仰角(Pitch)，用字母θ表示。

1.3 坐標變換

設坐標系O-X0Y0Z0中有一向量P，P在該坐標系中的投影為，令坐標系繞OZ0軸轉動一個角度α，得到新坐標系O-X1Y1Z1，P在新坐標系中的投影為，坐標變換關系如圖2所示。

圖2 坐標變換關系示意圖

由圖2 可得如下關系：

1.4 方向余弦姿態矩陣

兩個坐標系之間的位置關系都可以看作是有限次基本旋轉的組合，坐標變換矩陣等于每次基本旋轉矩陣的左乘積[9]。

若規定坐標旋轉次序為Z→Y→X，旋轉矩陣由3 個基本旋轉矩陣依次左乘求得：

1.5 四元數姿態矩陣

設矢量P在兩個坐標系中的投影為R=x1i+y1j+z1k,R′=x2i+y2j+z2k，設四元數為Λ=λ0+λ1i+λ2j+λ3k。

兩坐標系之間的旋轉關系可以由R′=Λ ?R?ΛR′得到[10]（Λ′為共軛四元數），整理得四元數姿態矩陣為：

2 數據融合與姿態解算

飛行器的慣性測量單元(Inertial Measurement Unit,IMU)通常搭載了加速度傳感器與陀螺儀來測量實時姿態數據，并且使用磁力計來修正偏航角的值。

梯度下降算法的基本思想如下[11-12]：首先，通過陀螺儀的角速度初步計算傳感器方向偏移；然后通過對重力加速度和地磁兩個恒定量在傳感器坐標系下的投影計算傳感器的方向偏移；最后通過梯度下降法對這兩個偏移量進行數據融合，從而得到一個更加準確的解算結果。

經典梯度下降法公式為[13]：

其中，?F(xn)是該函數在xn點處的梯度，μt為步長。

用加速度計的數據表征四元數矩陣的誤差，則誤差函數為：

求導，得到矩陣：

根據梯度計算公式有：

假設目標姿態為λ?,t，由梯度下降法公式有：

由于加速度計在實際使用中的動態性能很差，在物體高速運動下所測的數據會有很大的延遲，而在慣性導航中，陀螺儀能有效處理高速運動下的動態數據[14]。考慮到二者特點互補，需要對它們的數據進行融合。

設基于陀螺儀的四元數微分方程求解到的姿態為λω,t，則融合后的四元數為：

式中，0 ≤α≤1。等號右邊由兩部分組成：一是利用梯度下降法得到的低速姿態四元數λ?,t，二是基于陀螺儀的微分方程求解到的高速姿態四元數λω,t[15]。

α的取值應該使λ?,t的收斂速度等于λω,t的收斂速度β：

由于α的值一般都很大，故式（8）有近似表達式，如下：

將所有表達式代入式（9）中，可得最終融合算法公式，如下：

加速度計與磁力計的誤差函數可以寫成一個6行1 列的矩陣，對其求導得：

代入梯度公式可得：

后續工作與加速度計和陀螺儀的融合過程類似，最終也可得加速度計和磁力計的融合算法公式。

3 試驗與分析

姿態航向參考系統(Attitude and Heading Reference System,AHRS)是一種基于微機電系統(Micro Electro-Mechanical Systems,MEMS)技術的高性能、低成本捷聯慣導測量裝置[16]。為驗證算法的可行性，設定AHRS 設備分別繞Z、X、Y軸從0°旋轉至+90°，再旋轉至-90°，將3 次旋轉過程采集到的傳感器數據進行降噪處理，輸出的9 組傳感器數據及采樣時間數據以.mat 文件的形式存儲。經過程序解算，該AHRS 3個預定軌跡的解算結果如圖3所示。

圖3 試驗解算曲線

圖3(b2)中，當俯仰角在±90°附近時，其他兩個角的數值是無效的，這是由于“萬向節鎖”現象造成的——當俯仰角等于±90°時，滾轉和偏航兩個旋轉軸對于飛行器的旋轉效果相同，從而丟失了偏航效果[17]。但由于使用四元數法解算姿態，相比于歐拉角法，不會出現方程奇異解的情形，俯仰角依然解算成功。

由圖3(a2)、(b2)、(c2)可以看出，當AHRS 設備分別繞Z、X、Y軸從0°旋轉至+90°，再旋轉至-90°時，姿態解算結果曲線與設備預定運動情況完全符合，由此說明了該姿態解算程序的可行性。

4 結論

在飛行器姿態解算中，鑒于各傳感器測量特性的利弊，需要對多個傳感器的數據進行融合，使用梯度下降算法可以使加速度計測得的低速姿態信息與陀螺儀數據求解到的高速姿態相融合，并且還能與磁力計的數據融合來修正偏航角的解算值。令AHRS 設備分別繞Z、X、Y軸旋轉，通過算法程序解算，得出了與該設備預設旋轉情形完全相同的姿態角結果，驗證了算法的可行性。

該解算算法具有在較慢速運動中精度高、可快速跟隨高速運動中的姿態變化、無需獲知當地地磁傾角、收斂速度快等優點，有一定的工程應用價值，適用于低成本飛行器姿態測量與控制的研究。下一步工作考慮如何自適應地選擇步長μt，從而在飛行器機動狀態下獲得更加精確的姿態。