一種改進的四旋翼飛行器自適應(yīng)姿態(tài)融合算法

高怡，李東航

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

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

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

該文以梯度下降法為基礎(chǔ)，提出一種變步長的自適應(yīng)姿態(tài)融合算法——當(dāng)設(shè)備在一段時間內(nèi)的運動加速度數(shù)學(xué)期望值越大時，算法自適應(yīng)地對加速度計的信任程度降低，即使梯度下降的補償成分降低，從而提高解算的精度。

1 飛行器姿態(tài)描述

1.1 坐標系與姿態(tài)角

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

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

1.2 四元數(shù)法姿態(tài)求解

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

捷聯(lián)慣性導(dǎo)航系統(tǒng)中定義四元數(shù)Λ 為：

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

四元數(shù)微分方程定義如下[12]：

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

由四元數(shù)旋轉(zhuǎn)矩陣可解得3 個姿態(tài)角：

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

2 傳感器數(shù)據(jù)融合與姿態(tài)解算

2.1 經(jīng)典梯度下降算法姿態(tài)融合

梯度下降公式[14]為：

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

假設(shè)目標姿態(tài)為λ?,t，由梯度下降法有：

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

其中，0 ≤α≤1。

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

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

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

2.2 自適應(yīng)變步長姿態(tài)融合

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

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

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

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

自適應(yīng)算法簡明流程圖如圖2 所示。

圖2 自適應(yīng)算法簡明流程圖

3 實驗與分析

3.1 實驗平臺

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

圖3 AHRS模塊

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

圖4 USB-TTL轉(zhuǎn)換模塊

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

3.2 實驗結(jié)果

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

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

圖5 算法測試圖

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

表1 解算結(jié)果誤差比較

4 結(jié)論

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