高怡,李東航
(西安石油大學(xué)電子工程學(xué)院,陜西 西安 710065)
四旋翼飛行器(quadrotor)具有體積小、機(jī)動(dòng)性強(qiáng)、隱蔽性好、安全性高等優(yōu)點(diǎn),目前被廣泛應(yīng)用于農(nóng)業(yè)、軍事、新聞等領(lǐng)域[1]。姿態(tài)航向參考系統(tǒng)(AHRS)是一種基于微機(jī)電系統(tǒng)(MEMS)的高性能、低成本捷聯(lián)式慣性導(dǎo)航測(cè)量裝置[2],目前成為了四旋翼飛行器的主要慣性測(cè)量傳感器[3]。四旋翼飛行器姿態(tài)解算工作的速率與精度直接影響了其飛行的質(zhì)量,所以設(shè)計(jì)出更加快速、精確的數(shù)據(jù)融合與姿態(tài)解算算法成為當(dāng)前研究的熱點(diǎn)之一。
文獻(xiàn)[4]首次將梯度下降法(Gradient Descent,GD)應(yīng)用在了飛行器姿態(tài)解算領(lǐng)域中。但為滿足四旋翼飛行器越來(lái)越高精度的姿態(tài)測(cè)量要求,不少學(xué)者又基于此算法提出了多種改進(jìn)算法。文獻(xiàn)[5]將步長(zhǎng)與飛行器的角速度關(guān)聯(lián)起來(lái)構(gòu)成了動(dòng)態(tài)步長(zhǎng)。文獻(xiàn)[6]根據(jù)運(yùn)動(dòng)加速度值及加速度增量的變化得到自適應(yīng)因子,從而提高了系統(tǒng)在運(yùn)動(dòng)狀態(tài)中的穩(wěn)定性。文獻(xiàn)[7]將融合前的兩傳感器數(shù)據(jù)進(jìn)行互補(bǔ)濾波,根據(jù)角速度和加速度大小自適應(yīng)調(diào)整β參數(shù),但該方法較為繁瑣,收斂速度較慢。文獻(xiàn)[8-9]引入動(dòng)量以改進(jìn)每次迭代的優(yōu)化方向,提高了梯度下降收斂速度和精度,但精確的動(dòng)量數(shù)據(jù)不易獲得。
該文以梯度下降法為基礎(chǔ),提出一種變步長(zhǎng)的自適應(yīng)姿態(tài)融合算法——當(dāng)設(shè)備在一段時(shí)間內(nèi)的運(yùn)動(dòng)加速度數(shù)學(xué)期望值越大時(shí),算法自適應(yīng)地對(duì)加速度計(jì)的信任程度降低,即使梯度下降的補(bǔ)償成分降低,從而提高解算的精度。
選用地理坐標(biāo)系東、北、天(O-ENU)作為參考坐標(biāo)系(Reference Coordinate System),坐標(biāo)原點(diǎn)位于載體重心。載體坐標(biāo)系(Carrier Coordinate System)X軸為機(jī)體平面向右方向,與機(jī)體軸線垂直,Y軸為機(jī)頭所對(duì)正前方方向,Z軸垂直于XOY平面向上,構(gòu)成坐標(biāo)系O-XYZ。載體坐標(biāo)系與地理坐標(biāo)系如圖1 所示,機(jī)體繞X軸旋轉(zhuǎn)產(chǎn)生俯仰角(Pitch),繞Y軸旋轉(zhuǎn)產(chǎn)生滾轉(zhuǎn)角(Roll),繞Z軸旋轉(zhuǎn)產(chǎn)生偏航角(Yaw),分別用歐拉角θ、φ、ψ表示[10]。
圖1 載體坐標(biāo)系與地理坐標(biāo)系
四元數(shù)最早于19 世紀(jì)被提出,并建立了相關(guān)的數(shù)學(xué)理論。20 世紀(jì)中葉以來(lái),隨著計(jì)算機(jī)的發(fā)明以及航天技術(shù)的發(fā)展,四元數(shù)才得到人們的重視[11]。
捷聯(lián)慣性導(dǎo)航系統(tǒng)中定義四元數(shù)Λ 為:
其中,i2=j2=k2=ijk=-1。
四元數(shù)微分方程定義如下[12]:
可采用離散方法求解四元數(shù)微分方程:若系統(tǒng)采樣周期為T(mén)s,離散后的方程[13]為:
由四元數(shù)旋轉(zhuǎn)矩陣可解得3 個(gè)姿態(tài)角:
設(shè)初始四元數(shù)Λ0=[1 0 0 0]T,首先由式(3)、(4)可得到t時(shí)刻的四元數(shù)Λk,代入式(5)即可求得t時(shí)刻的3 個(gè)姿態(tài)角θ、φ、ψ。
梯度下降公式[14]為:
其中,?F(xn)為梯度值,μt為收斂步長(zhǎng)。
假設(shè)目標(biāo)姿態(tài)為λ?,t,由梯度下降法有:
設(shè)基于陀螺儀微分方程求解到的姿態(tài)為λω,t,則融合姿態(tài)為[15]:
其中,0 ≤α≤1。
由于α的值很大,取式(8)的近似值并將所有表達(dá)式代入式(9),可得最終融合算法公式為[7]:
其中,β為λω,t的收斂速率,由手冊(cè)查到。
磁力計(jì)和加速度計(jì)的數(shù)據(jù)融合與上述過(guò)程類似,不再贅述。
當(dāng)飛行器處于運(yùn)動(dòng)狀態(tài)時(shí),重力場(chǎng)的測(cè)量值會(huì)存在偏差,所以需要減少梯度下降的補(bǔ)償量從而減少對(duì)加速度計(jì)的信任程度[16]。換句話說(shuō),公式中需要引入一個(gè)可變因子,在運(yùn)動(dòng)加速度越大時(shí)使β值越小。
設(shè)加速度的實(shí)際測(cè)量值為a,重力加速度用g表示,運(yùn)動(dòng)加速度即為二者之差。t時(shí)刻的前n個(gè)采樣點(diǎn)的運(yùn)動(dòng)加速度數(shù)學(xué)期望值為En,引入自適應(yīng)因子ρa(bǔ)與常數(shù)控制因子Cona,則有:
融合公式(10)也發(fā)生改變,如式(12)所示:
當(dāng)四旋翼飛行器高速飛行時(shí),可通過(guò)常數(shù)控制因子使自適應(yīng)因子與期望值呈負(fù)相關(guān)。據(jù)此原理,算法可以自適應(yīng)地控制補(bǔ)償量的變化:運(yùn)動(dòng)加速度數(shù)據(jù)的數(shù)學(xué)期望越大,梯度下降部分的補(bǔ)償成分越小。在實(shí)際應(yīng)用中,可以通過(guò)調(diào)節(jié)常數(shù)控制因子Cona與采樣點(diǎn)數(shù)n,來(lái)減少高頻干擾,進(jìn)而提高系統(tǒng)的穩(wěn)定性。
自適應(yīng)算法簡(jiǎn)明流程圖如圖2 所示。
圖2 自適應(yīng)算法簡(jiǎn)明流程圖
實(shí)驗(yàn)使用AHRS模塊搭建測(cè)試平臺(tái),該AHRS模塊主控制芯片采用STM32F103T8,ARM 32-bit Cortex?-M3 內(nèi)核,模塊傳感器部分組合使用了MPU6050 傳感器(其中包括加速度計(jì)、陀螺儀各一個(gè))、HMC5883L傳感器(包括HMC118X 系列磁阻傳感器和相關(guān)集成電路)以及一個(gè)氣壓高度計(jì)BMP180,AHRS 模塊如圖3 所示,模塊實(shí)際尺寸為37.2 mm×21.4 mm。
圖3 AHRS模塊
AHRS 模塊與PC 通過(guò)USB-TTL 轉(zhuǎn)換模塊(以CP2102 為內(nèi)核)連接,用于實(shí)現(xiàn)AHRS 設(shè)備的供電與通信,該轉(zhuǎn)換模塊如圖4 所示。串口所連接的PC 軟件可直接讀取和導(dǎo)出各傳感器數(shù)據(jù),并給出設(shè)備的實(shí)時(shí)理論準(zhǔn)確姿態(tài)曲線。
圖4 USB-TTL轉(zhuǎn)換模塊
測(cè)試平臺(tái)具有三軸旋轉(zhuǎn)維度,可模擬四旋翼飛行器在空間中的各種運(yùn)動(dòng)姿態(tài)。
為了驗(yàn)證該算法的可靠性,選擇ψ角作為驗(yàn)證角。實(shí)驗(yàn)中,設(shè)定實(shí)驗(yàn)平臺(tái)的軌跡為繞Z軸由0°旋轉(zhuǎn)至90°再旋轉(zhuǎn)至-90°,通過(guò)上位機(jī)軟件導(dǎo)出兩傳感器的數(shù)據(jù)并儲(chǔ)存。兩傳感器采集的數(shù)據(jù)如圖5(a)、(b)所示,其三軸分量以不同線型進(jìn)行區(qū)分。
對(duì)自適應(yīng)算法(AGD) 編程,各參數(shù)經(jīng)調(diào)試后設(shè)置如下:采樣間隔T=1/256 s、融合步長(zhǎng)β=0.1、常數(shù)控制因子Cona=0.95。實(shí)驗(yàn)以互補(bǔ)濾波算法(CF)、經(jīng)典梯度下降算法(GD)為對(duì)照組,將導(dǎo)出的傳感器數(shù)據(jù)分別讀入3 個(gè)算法程序進(jìn)行解算,各算法對(duì)于同一軌跡的傳感器數(shù)據(jù)偏航角ψ的解算結(jié)果如圖5(c)、(d)所示。圖5 (c)中,實(shí)線為設(shè)備偏航角的理論精確姿態(tài),從圖中可以大致看出,3 種算法均成功解算出了設(shè)備的姿態(tài)。圖5(d)為4~6 s 解算結(jié)果的局部放大圖,可以直觀比較3 種算法的解算效果。
圖5 算法測(cè)試圖
由表1 解算結(jié)果誤差數(shù)值比較可以看出,3 種算法的誤差平均值均在3°左右,這是由于該算法只進(jìn)行基于運(yùn)動(dòng)加速度的變步長(zhǎng)研究,未包含磁力計(jì)和氣壓高度計(jì)的補(bǔ)償。在標(biāo)準(zhǔn)差方面,CF 方法下的誤差波動(dòng)較大,GD 算法次之,AGD 算法結(jié)果最為穩(wěn)定;在平均值方面,就該實(shí)驗(yàn)而言,AGD 算法的精度較其他兩種算法分別提高了0.557°和0.708°。
表1 解算結(jié)果誤差比較
在四旋翼飛行器姿態(tài)測(cè)量中,高速且精確的姿態(tài)解算對(duì)飛行器控制有著至關(guān)重要的作用。該文提出了一種基于梯度下降的自適應(yīng)變步長(zhǎng)姿態(tài)融合算法,該算法可根據(jù)運(yùn)動(dòng)加速度的大小調(diào)整對(duì)加速度計(jì)數(shù)據(jù)的信任程度,從而提高解算精度。實(shí)驗(yàn)中,該算法在解算AHRS 設(shè)備的偏航角時(shí),收斂速度快,姿態(tài)輸出平穩(wěn),解算精度得到明顯提高。因此,該算法有較好的研究參考與工程應(yīng)用價(jià)值。