一種改進的四旋翼飛行器自適應姿態融合算法

高怡，李東航

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

四旋翼飛行器(quadrotor)具有體積小、機動性強、隱蔽性好、安全性高等優點，目前被廣泛應用于農業、軍事、新聞等領域[1]。姿態航向參考系統(AHRS)是一種基于微機電系統(MEMS)的高性能、低成本捷聯式慣性導航測量裝置[2]，目前成為了四旋翼飛行器的主要慣性測量傳感器[3]。四旋翼飛行器姿態解算工作的速率與精度直接影響了其飛行的質量，所以設計出更加快速、精確的數據融合與姿態解算算法成為當前研究的熱點之一。

文獻[4]首次將梯度下降法(Gradient Descent,GD)應用在了飛行器姿態解算領域中。但為滿足四旋翼飛行器越來越高精度的姿態測量要求，不少學者又基于此算法提出了多種改進算法。文獻[5]將步長與飛行器的角速度關聯起來構成了動態步長。文獻[6]根據運動加速度值及加速度增量的變化得到自適應因子，從而提高了系統在運動狀態中的穩定性。文獻[7]將融合前的兩傳感器數據進行互補濾波，根據角速度和加速度大小自適應調整β參數，但該方法較為繁瑣，收斂速度較慢。文獻[8-9]引入動量以改進每次迭代的優化方向，提高了梯度下降收斂速度和精度，但精確的動量數據不易獲得。

該文以梯度下降法為基礎，提出一種變步長的自適應姿態融合算法——當設備在一段時間內的運動加速度數學期望值越大時，算法自適應地對加速度計的信任程度降低，即使梯度下降的補償成分降低，從而提高解算的精度。

1 飛行器姿態描述

1.1 坐標系與姿態角

選用地理坐標系東、北、天(O-ENU)作為參考坐標系(Reference Coordinate System)，坐標原點位于載體重心。載體坐標系(Carrier Coordinate System)X軸為機體平面向右方向，與機體軸線垂直，Y軸為機頭所對正前方方向，Z軸垂直于XOY平面向上，構成坐標系O-XYZ。載體坐標系與地理坐標系如圖1 所示，機體繞X軸旋轉產生俯仰角(Pitch)，繞Y軸旋轉產生滾轉角(Roll)，繞Z軸旋轉產生偏航角(Yaw)，分別用歐拉角θ、φ、ψ表示[10]。

圖1 載體坐標系與地理坐標系

1.2 四元數法姿態求解

四元數最早于19 世紀被提出，并建立了相關的數學理論。20 世紀中葉以來，隨著計算機的發明以及航天技術的發展，四元數才得到人們的重視[11]。

捷聯慣性導航系統中定義四元數Λ 為：

其中，i2=j2=k2=ijk=-1。

四元數微分方程定義如下[12]：

可采用離散方法求解四元數微分方程：若系統采樣周期為Ts，離散后的方程[13]為：

由四元數旋轉矩陣可解得3 個姿態角：

設初始四元數Λ0=[1 0 0 0]T，首先由式（3）、（4）可得到t時刻的四元數Λk，代入式（5）即可求得t時刻的3 個姿態角θ、φ、ψ。

2 傳感器數據融合與姿態解算

2.1 經典梯度下降算法姿態融合

梯度下降公式[14]為：

其中，?F(xn)為梯度值，μt為收斂步長。

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

設基于陀螺儀微分方程求解到的姿態為λω,t，則融合姿態為[15]：

其中，0 ≤α≤1。

由于α的值很大，取式（8）的近似值并將所有表達式代入式（9），可得最終融合算法公式為[7]：

其中，β為λω,t的收斂速率，由手冊查到。

磁力計和加速度計的數據融合與上述過程類似，不再贅述。

2.2 自適應變步長姿態融合

當飛行器處于運動狀態時，重力場的測量值會存在偏差，所以需要減少梯度下降的補償量從而減少對加速度計的信任程度[16]。換句話說，公式中需要引入一個可變因子，在運動加速度越大時使β值越小。

設加速度的實際測量值為a，重力加速度用g表示，運動加速度即為二者之差。t時刻的前n個采樣點的運動加速度數學期望值為En，引入自適應因子ρa與常數控制因子Cona，則有：

融合公式（10）也發生改變，如式（12）所示：

當四旋翼飛行器高速飛行時，可通過常數控制因子使自適應因子與期望值呈負相關。據此原理，算法可以自適應地控制補償量的變化：運動加速度數據的數學期望越大，梯度下降部分的補償成分越小。在實際應用中，可以通過調節常數控制因子Cona與采樣點數n，來減少高頻干擾，進而提高系統的穩定性。

自適應算法簡明流程圖如圖2 所示。

圖2 自適應算法簡明流程圖

3 實驗與分析

3.1 實驗平臺

實驗使用AHRS模塊搭建測試平臺，該AHRS模塊主控制芯片采用STM32F103T8，ARM 32-bit Cortex?-M3 內核，模塊傳感器部分組合使用了MPU6050 傳感器(其中包括加速度計、陀螺儀各一個)、HMC5883L傳感器(包括HMC118X 系列磁阻傳感器和相關集成電路)以及一個氣壓高度計BMP180，AHRS 模塊如圖3 所示，模塊實際尺寸為37.2 mm×21.4 mm。

圖3 AHRS模塊

AHRS 模塊與PC 通過USB-TTL 轉換模塊(以CP2102 為內核)連接，用于實現AHRS 設備的供電與通信，該轉換模塊如圖4 所示。串口所連接的PC 軟件可直接讀取和導出各傳感器數據，并給出設備的實時理論準確姿態曲線。

圖4 USB-TTL轉換模塊

測試平臺具有三軸旋轉維度，可模擬四旋翼飛行器在空間中的各種運動姿態。

3.2 實驗結果

為了驗證該算法的可靠性，選擇ψ角作為驗證角。實驗中，設定實驗平臺的軌跡為繞Z軸由0°旋轉至90°再旋轉至-90°，通過上位機軟件導出兩傳感器的數據并儲存。兩傳感器采集的數據如圖5(a)、(b)所示，其三軸分量以不同線型進行區分。

對自適應算法(AGD) 編程，各參數經調試后設置如下：采樣間隔T=1/256 s、融合步長β=0.1、常數控制因子Cona=0.95。實驗以互補濾波算法(CF)、經典梯度下降算法(GD)為對照組，將導出的傳感器數據分別讀入3 個算法程序進行解算，各算法對于同一軌跡的傳感器數據偏航角ψ的解算結果如圖5(c)、(d)所示。圖5 (c)中，實線為設備偏航角的理論精確姿態，從圖中可以大致看出，3 種算法均成功解算出了設備的姿態。圖5(d)為4～6 s 解算結果的局部放大圖，可以直觀比較3 種算法的解算效果。

圖5 算法測試圖

由表1 解算結果誤差數值比較可以看出，3 種算法的誤差平均值均在3°左右，這是由于該算法只進行基于運動加速度的變步長研究，未包含磁力計和氣壓高度計的補償。在標準差方面，CF 方法下的誤差波動較大，GD 算法次之，AGD 算法結果最為穩定；在平均值方面，就該實驗而言，AGD 算法的精度較其他兩種算法分別提高了0.557°和0.708°。

表1 解算結果誤差比較

4 結論

在四旋翼飛行器姿態測量中，高速且精確的姿態解算對飛行器控制有著至關重要的作用。該文提出了一種基于梯度下降的自適應變步長姿態融合算法，該算法可根據運動加速度的大小調整對加速度計數據的信任程度，從而提高解算精度。實驗中，該算法在解算AHRS 設備的偏航角時，收斂速度快，姿態輸出平穩，解算精度得到明顯提高。因此，該算法有較好的研究參考與工程應用價值。