文獻標識碼: A
DOI:10.16157/j.issn.0258-7998.173121
中文引用格式: 王勇軍,李智,李翔. 采用組合濾波算法的無人機航向測量系統(tǒng)研究[J].電子技術應用,2018,44(2):39-43.
英文引用格式: Wang Yongjun,Li Zhi,Li Xiang. Research of heading measurement system for UAV based on combined filter[J]. Application of Electronic Technique,2018,44(2):39-43.
0 引言
航向是無人飛行器的重要飛行參數(shù),在飛行過程中,實時準確地獲得飛行器的航向信息決定了飛行器的可靠性和穩(wěn)定性。如今隨著微電子技術的發(fā)展,小型低成本旋翼無人飛行器普遍采用基于微機電系統(tǒng)(MEMS)的三軸陀螺儀、三軸加速度計和三軸磁強計來測量航向[1]。由于MEMS傳感器自身存在非正交誤差,以及數(shù)據(jù)積分產(chǎn)生的發(fā)散誤差和外部干擾產(chǎn)生的隨機誤差,因此需要對多傳感器進行誤差處理并尋求最佳數(shù)據(jù)融合算法,才能得到精確的航向信息。
目前多數(shù)由陀螺儀和磁強計組成的航向測量系統(tǒng)主要采用羅差修正法和卡爾曼濾波算法來計算航向信息[2]。采用羅差修正的方法對磁強計進行校正,雖然能提高一定的精度,但僅針對磁傳感器校正仍然無法提高系統(tǒng)在受到長時間低頻干擾時的航向精度;而卡爾曼及其擴展算法可對磁強計和陀螺儀進行信息融合,能有效平滑曲線,使高頻誤差得到抑制,但無法消除磁強計受到的軟硬磁干擾誤差,而且應用時需要為其建立可靠穩(wěn)定的狀態(tài)方程,確定合適的量測噪聲和過程噪聲協(xié)方差矩陣,需要較大的時間開銷。
為此,本文設計一種基于MEMS慣性測量單元(IMU)和電子羅盤的航向測量系統(tǒng),采用梯度下降算法來對磁強計和陀螺儀進行誤差修正,而且在姿態(tài)解算過程只需進行乘法和加法運算,普通的微控制器即能滿足算法要求[3]。另外,在MAHONY R提出的補償濾波器[4]的基礎上進行優(yōu)化改進,結合二階互補濾波算法來進行數(shù)據(jù)融合,以期得到精密航向輸出。
1 無人機航向角描述與系統(tǒng)方案設計
1.1 坐標系定義及姿態(tài)矩陣
無人機的航姿信息主要包括航向角(也稱航偏角)、俯仰角θ、橫滾角γ。選取“東北天”為地理坐標系(記為ENU系),“右前上”為機體坐標系(記為NED系),由于NED系和ENU系均是直角坐標系,且各個軸之間始終保持垂直,因此,NED系和ENU系的空間位置關系即可理解為剛體的定點轉動[5]。地理坐標系與機體坐標系之間的角度轉換關系如圖1所示。
1.2 系統(tǒng)方案設計
該航向測量系統(tǒng)硬件主要由MEMS-IMU、MEMS電子羅盤和主控制器組成,如圖2所示。其中MEMS-IMU采用的是InvenSense公司的MPU-6500,它由一個三軸陀螺儀和三軸加速度計組成,可以通過SPI口輸出飛行器的角速度、加速度等姿態(tài)信息,用來計算俯仰角及橫滾角;所采用的MEMS電子羅盤是ST(意法半導體)公司的LSM303D,它內(nèi)部的三軸磁強計可通過SPI總線口輸出測得的磁場數(shù)據(jù),另外,它內(nèi)部的三軸加速度可用來測量重力矢量,獲得載體的傾角信息,并通過運算補償航向信息的輸出。主控器采用ST公司CM4內(nèi)核的32位ARM微控制器STM32F407,主要通過獲取MEMS傳感器的輸出數(shù)據(jù)進行姿態(tài)解算,完成飛行器航向的精密測量。
由于IMU中的陀螺儀、加速度計和電子羅盤中的加速度傳感器都可以測量加速度信息,因此該系統(tǒng)的傳感器配置也是具有一定冗余度,主控制器的另一任務就是將各傳感器的數(shù)據(jù)取長補短進行數(shù)據(jù)融合,提高姿態(tài)解算精度。主控制器讀取出傳感器采集到的原始數(shù)據(jù),利用梯度下降法優(yōu)化電子羅盤中加速度計和磁強計的輸出數(shù)據(jù),獲得靜態(tài)性較好的姿態(tài)四元數(shù),然后利用改進型互補濾波器將其與IMU獲得的動態(tài)性較好的姿態(tài)信息進行融合濾波,最后通過四元數(shù)的坐標換算解算出航向角。
2 航向數(shù)據(jù)組合濾波算法設計
2.1 梯度下降法
梯度下降算法是一種迭代求極值的算法,具體實現(xiàn)過程是按目標函數(shù)斜率的負方向來搜索尋優(yōu)。電子羅盤中的加速度計和磁強計具有良好的靜態(tài)特性,長時間使用不會引入積分誤差,但是加速度計受載體振動和運動影響較大,動態(tài)環(huán)境下,瞬時誤差大,而且磁強計容易受到周邊環(huán)境中軟硬磁干擾[7],因此本文首先使用梯度下降算法加對速度計和磁強計輸出數(shù)據(jù)進行優(yōu)化,獲得一個靜態(tài)特性較好的姿態(tài)四元數(shù),然后再進行下一步航向解算。
顯然f(q)≥0,進而對姿態(tài)四元數(shù)的優(yōu)化就可以轉換為對f(q)求最小值。
梯度下降法在姿態(tài)解算過程中只有簡單的加法和乘法運算,此時四元數(shù)更新方程可變?yōu)?sup>[8]:
2.2 改進型互補濾波算法
利用電子羅盤中三軸磁強計和加速度計測得的數(shù)據(jù)通過梯度下降法對姿態(tài)四元數(shù)進行修正轉換后得到姿態(tài)值記為。根據(jù)加速度計和磁強計的特性,不會引入累積誤差,但是在動態(tài)情況下,會引入較大瞬時誤差。相反IMU中的陀螺儀可測得瞬時精度高的姿態(tài)數(shù)據(jù),但是有累積誤差。設IMU中陀螺儀和加速度計直接積分解算出來的姿態(tài)值為,根據(jù)和在頻域上的互補特性,通過傳統(tǒng)的互補濾波方法融合兩者,可得到新的姿態(tài)值,雖然能同時消除低頻和高頻的干擾,實現(xiàn)姿態(tài)數(shù)據(jù)的融合,然而,互補濾波器[9]的低通阻帶衰減緩慢,誤差較大時,振動大,濾波效果差。因此在傳統(tǒng)互補濾波器的基礎上添加了PI(比例積分)環(huán)節(jié),構成改進型二階互補濾波器,如圖3所示。
由圖3可知:
2.3 航向角組合解算
系統(tǒng)基于四元數(shù)的姿態(tài)計算即對式(1)中的姿態(tài)四元數(shù)的微分方程進行求解,四元數(shù)微分方程常用的解算方法有[10]:單子樣旋轉適量法、多子樣旋轉矢量法、龍格庫塔法和泰勒展開法。旋轉矢量法多用于輸出是角增量的陀螺儀, 而MEMS陀螺儀的輸出形式是角速率??紤]到運算精度和速度, 本系統(tǒng)采用四階-龍格庫塔法求解微分方程:
式中,h為姿態(tài)數(shù)據(jù)采樣的周期,即四元數(shù)更新周期;ωb(t)、、ωb(t+h)分別為在更新周期h時間內(nèi)陀螺的采樣值。每個采樣周期內(nèi)提取陀螺儀的數(shù)據(jù),對式(10)進行迭代運算,便能夠?qū)崿F(xiàn)對四元數(shù)的實時更新,從而得出航向角。
3 實驗驗證分析
將設計的航向測量系統(tǒng)安裝在電子轉臺上,為避免鐵磁干擾,加工一個60 cm高的木架做固定支撐,并校正好零位,通過串口連接至上位機,進行傳感器數(shù)據(jù)采集,如圖4所示。
通過電子轉臺水平轉動,每次回到零位時利用上位機分別采集電子磁羅盤的數(shù)據(jù)和經(jīng)過梯度下降與互補濾波算法融合后的數(shù)據(jù)。通過對比分析可知,沒有使用濾波算法的磁強計測量的航偏角相對零位誤差較大,達到了±3°左右,磁強計受外界干擾嚴重。經(jīng)過組合濾波處理后得到的偏航角消除了部分干擾信號,誤差可穩(wěn)定保持在±1.0°以內(nèi)。
在電子轉臺上轉動測試可看作是靜態(tài)測試,為進一步驗證航向測量系統(tǒng)在多旋翼飛行器實際飛行過程中的性能,將該系統(tǒng)固定在PIX4飛控模塊屏蔽罩上,并一同安裝在六旋翼飛行器上,機架底部水平固定在萬向節(jié)一端,萬向節(jié)另外一端固定在水平的桌面上,如圖5所示。
啟動旋翼飛行器電機,將飛行器油門通道值保持在空中懸停狀態(tài),操作飛行器方向舵,讓其原地旋轉。然后,通過2.4G串口數(shù)傳電臺接收飛控航向數(shù)據(jù)和航向測量系統(tǒng)傳出的磁強計、陀螺儀原始數(shù)據(jù)以及組合解算后的數(shù)據(jù),并用MATLAB進行數(shù)據(jù)分析,如圖6所示。
圖6中采集的是飛行器水平懸停轉動(以飛控為基準)時的數(shù)據(jù),其中的電子羅盤和IMU輸出航向分別代表的是磁強計和陀螺儀數(shù)據(jù)處理的航向角誤差曲線,組合濾波輸出航向表示經(jīng)梯度下降和二階互補濾波算法融合后解算的航向角誤差曲線。旋翼飛行器運動過程中受電機工作引起的機架振動、載體重力分布不均、機械連接處阻尼問題等因素影響,導致航向角誤差比水平放置在轉臺上時的靜態(tài)零點誤差偏差相對較大且波動相對厲害,經(jīng)過組合濾波處理后,航向角誤差在±1.5°以內(nèi),可以滿足小型旋翼飛行器對航向數(shù)據(jù)的要求。
4 結論
航向信息作為無人飛行器導航定位的重要測量參數(shù),要求具有較高的精度。在小型旋翼無人飛行器系統(tǒng)上,利用MEMS IMU和電子磁羅盤各自的優(yōu)點及其互補性,配合ARM主控制器組成低成本航向測量系統(tǒng),采用梯度下降和二階互補濾波算法,使三軸陀螺儀、三軸加速度計、三軸磁強計經(jīng)過數(shù)據(jù)融合后,完成坐標變換和姿態(tài)解算,從而輸出較高精度的航向角。實驗結果表明:該航向測量系統(tǒng)較好地解決了噪聲干擾與航向最優(yōu)估計問題,并在實驗平臺上得到了驗證,航偏角誤差保持在±1.5°以內(nèi),在沒有外界信息輔助的情況下,可穩(wěn)定地輸出準確姿態(tài)數(shù)據(jù),滿足了低成本旋翼無人機對航向信息的要求。
參考文獻
[1] 何川,李翔,王勇軍.基于STM32的四旋翼飛行器的姿態(tài)最優(yōu)估計研究[J].電子技術應用,2015,41(12):61-64.
[2] 任愛愛,孫永榮,胡應東,等.無人機航向測量抗差補償濾波技術研究[J].傳感器與微系統(tǒng),2009,28(5):34-36.
[3] MADGWICK S O H,HARRISON A J L,VAIDYANATHAN R,et al.Estimation of IMU and MARG orientation using a gradient descent algorithm[C].IEEE International Conference on Rehabilitation Robotics,2011:1-7.
[4] MAHONY R,HAMEL T,PFLIMLIN J M.Nonlinear complementary filters on the special orthogonal group[C].IEEE Transactions on Automatic Control,2008,53(5):1203-1218.
[5] 秦永元.慣性導航[M].北京:科學出版社,2014.
[6] 曾憲陽,楊紅莉,郁漢琪.基于STM32單片機四旋翼飛行器建模分析與設計[J].電子技術應用,2016,12(42):65-68.
[7] 聶鵬,李佩華,李正強,等.基于卡爾曼濾波的小型無人機姿態(tài)估計算法研究[J].沈陽航空航天大學學報,2013,30(6):53-57.
[8] 譚廣超.四旋翼飛行器姿態(tài)控制系統(tǒng)的設計與實現(xiàn)[D].大連:大連理工大學,2013.
[9] 廖懿華,張鐵民,廖貽泳.基于模糊-比例積分偏差修正的多旋翼飛行器姿態(tài)測算系統(tǒng)[J].農(nóng)業(yè)工程學報,2014,30(20):19-27.
[10] 王彤,馬建倉,秦濤,等.基于旋轉四元數(shù)的姿態(tài)解算算法[J].彈箭與制導學報,2014,34(3):15-17.