基于Mahony濾波算法的姿態(tài)解算與應(yīng)用研究

徐恩松 陸文華 劉云飛 李寶磊 馮旺

摘 要：針對(duì)多旋翼無(wú)人機(jī)自駕儀在某一特定的飛行路徑中，飛行控制系統(tǒng)中存在姿態(tài)信息的實(shí)時(shí)獲取和精確解算誤差較大問(wèn)題，文章介紹了四旋翼飛行器姿態(tài)解算常用四元數(shù)法姿態(tài)解算方法與互補(bǔ)濾波融合算法原理，并研究了影響姿態(tài)解算的重要參數(shù)（比例系數(shù)Kp）的最佳取值。通過(guò)對(duì)Mahony算法給予改進(jìn)，最后在Matlab仿真試驗(yàn)和Mission Planner地面站軟件的測(cè)試下，比較不同的姿態(tài)解算結(jié)果，調(diào)試得出最佳的比例系數(shù)Kp值，得到了更好的姿態(tài)解算結(jié)果。四旋翼飛行器姿態(tài)信息的準(zhǔn)確性直接影響飛行器的飛行控制效果，實(shí)驗(yàn)結(jié)果表明，Mahony自適應(yīng)互補(bǔ)濾波算法濾波效果良好，可以有效降低低成本IUM系統(tǒng)的誤差累加，可以在實(shí)際應(yīng)用中得到較高的姿態(tài)精度，對(duì)飛行器的控制研究具有一定的參考價(jià)值與指導(dǎo)意義。

關(guān)鍵詞： 姿態(tài)解算;比例系數(shù)Kp;Mahony算法;Matlab仿真

【Abstract】 Aiming at the problem of real-time acquisition and accurate solution of attitude information in flight control system of multi-rotor UAV self-driving instrument in a specific flight path， in this paper， the quaternion method and the principle of complementary filtering fusion algorithm are introduced， and the optimum values of the important parameters （proportional coefficient Kp） affecting attitude calculation are studied. The Mahony algorithm is improved. Finally， under the test of Matlab simulation and Mission Planner ground station software， different attitude calculation results are compared， and the optimal proportional coefficient Kp value is debugged. The better attitude calculation results are obtained. The accuracy of attitude information of Four-rotor aircraft directly affects the flight control effect of the aircraft. The experimental results show that the filtering effect of Mahony adaptive complementary filtering algorithm is good， which can effectively suppress the error accumulation of low-cost IUM system. It can obtain high attitude accuracy in practical application. It has certain reference value and guiding significance for the control research of the aircraft.

【Key words】 ?attitude solution; proportional coefficient Kp; Mahony algorithm; Matlab simulation

0 引 言

0.1 飛行姿態(tài)解算的概念

姿態(tài)解算的原理：用不同的坐標(biāo)系表示同一個(gè)確定的向量時(shí)，2個(gè)坐標(biāo)系表示出的結(jié)果大小和方向一定相同。但是因?yàn)?個(gè)坐標(biāo)系的旋轉(zhuǎn)矩陣存在著誤差，即使得當(dāng)一個(gè)向量經(jīng)過(guò)一個(gè)帶有誤差的旋轉(zhuǎn)矩陣后，在另一個(gè)坐標(biāo)系中距理論值將出現(xiàn)一定偏差。因此可通過(guò)此偏差來(lái)修正這個(gè)旋轉(zhuǎn)矩陣。目前，常見(jiàn)的姿態(tài)表示方法有歐拉角法、方向余弦法和四元數(shù)法。其中，歐拉角法在求解姿態(tài)時(shí)存在奇點(diǎn)（萬(wàn)向節(jié)死鎖），不能用于全姿態(tài)解算;方向余弦可用于全姿態(tài)的解算，但計(jì)算量大，也無(wú)法滿足實(shí)時(shí)性要求;四元數(shù)法，計(jì)算量小，無(wú)奇點(diǎn)且可以滿足飛行器運(yùn)動(dòng)過(guò)程中姿態(tài)的實(shí)時(shí)解算要求。

0.2 姿態(tài)解算算法

四旋翼無(wú)人機(jī)的飛行控制系統(tǒng)中，姿態(tài)信息的實(shí)時(shí)獲取和精確解算尤為關(guān)鍵[1]。 目前常見(jiàn)的姿態(tài)解算算法有梯度下降算法[2]、互補(bǔ)濾波算法[3]和卡爾曼濾波算法[4]等。不同算法在不同情況下的解算效果不同，本文主要研究基于Mahony互補(bǔ)濾波算法的姿態(tài)解算[3]，對(duì)2個(gè)坐標(biāo)系相互轉(zhuǎn)化時(shí)旋轉(zhuǎn)矩陣存在偏差進(jìn)行修正，旋轉(zhuǎn)矩陣的元素是四元數(shù)，并于修正后給出了濾波的仿真結(jié)果分析。

1 四元數(shù)法姿態(tài)解算原理

1.1 四元數(shù)基本概念

2 Mahony互補(bǔ)濾波融合算法

互補(bǔ)濾波融合算法可根據(jù)加速度計(jì)測(cè)量數(shù)據(jù)利用梯度下降法得到最小誤差四元數(shù)的微分，再通過(guò)互補(bǔ)濾波算法與陀螺儀得出角速度微分四元數(shù)進(jìn)行融合，對(duì)融合后的姿態(tài)微分四元數(shù)進(jìn)行積分，估算出最優(yōu)四元數(shù)。

經(jīng)典互補(bǔ)濾波[8]利用陀螺儀和加速度計(jì)各自在動(dòng)態(tài)和靜態(tài)上的優(yōu)勢(shì)，在頻域上用加速度計(jì)測(cè)得的準(zhǔn)確瞬時(shí)值來(lái)彌補(bǔ)陀螺儀隨時(shí)間積累所產(chǎn)生的漂移誤差，即分別加入低通和高通濾波器提高姿態(tài)解算的精度[9]?；パa(bǔ)濾波的關(guān)鍵在于系數(shù) Kp的選擇，經(jīng)典低通環(huán)節(jié)阻帶衰減較慢，不能滿足四旋翼飛行器在飛行中的高動(dòng)態(tài)性，通過(guò)對(duì)Mahony算法做出改進(jìn)，最后在Matlab仿真試驗(yàn)和Mission Planner地面站軟件[10]的測(cè)試下，比較不同的姿態(tài)解算結(jié)果，調(diào)試得出最佳的比例系數(shù)Kp值，得到了更好的姿態(tài)解算結(jié)果。

3 Matlab姿態(tài)解算仿真實(shí)驗(yàn)

在實(shí)驗(yàn)前，研究將進(jìn)行試驗(yàn)數(shù)據(jù)的獲取，獲取飛行所需要的傳感器的姿態(tài)角數(shù)據(jù)。首先QGC與Pixhawk進(jìn)行連接[12]，并設(shè)置了多旋翼無(wú)人機(jī)的自主飛行路徑，然后在一段時(shí)間的飛行后，提取其飛行數(shù)據(jù)，主要涉及其未修正前的傳感器所測(cè)量的姿態(tài)角信息，再將其保存在Matlab能夠調(diào)用的dat數(shù)據(jù)文件夾中[13]。

這里，首先將特定路徑Pixhawk的飛行數(shù)據(jù)從日志中提取出來(lái)，提取的數(shù)據(jù)是飛行器未經(jīng)過(guò)Mahony算法解算[14]得到的的姿態(tài)角數(shù)據(jù)，Φ、θ、ψ分別為機(jī)體的俯仰角、偏航角和翻滾角，然后將這些數(shù)據(jù)以dat的格式保存起來(lái)。將數(shù)據(jù)加載到已經(jīng)封裝好的Mahony源程序中，傳入Matlab中進(jìn)行仿真實(shí)驗(yàn)[15]。

實(shí)驗(yàn)中，先后更改了3次Kp值，依次為0.6、1.1、2，得到了3張效果圖。分別如圖1～圖3所示。

4 仿真實(shí)驗(yàn)與結(jié)果分析

比例系數(shù)反映了姿態(tài)結(jié)果中加速計(jì)測(cè)量值對(duì)姿態(tài)結(jié)果的修正情況。Kp值越大說(shuō)明加速度計(jì)的結(jié)果對(duì)姿態(tài)解算的結(jié)果影響越大，又因?yàn)閷?duì)高頻震蕩非常敏感，這就有了結(jié)果中滾動(dòng)角和俯仰角震蕩頻率[16]隨著Kp值的增大而增大的情況。所以要不斷地進(jìn)行試驗(yàn)，根據(jù)特定的場(chǎng)景確定最適合的Kp值，Kp值并不是越小越好，當(dāng)Kp值越小時(shí)，陀螺儀對(duì)濾波結(jié)果的影響就較大，就會(huì)產(chǎn)生一些低頻的錯(cuò)誤信息。所以要綜合考慮Kp與Ki的值的大小。

由圖1～圖3中可以得到，當(dāng)Kp值小于1.1時(shí)，雖然濾波滾動(dòng)角和俯仰角有波動(dòng)，但波動(dòng)的頻率并不是特別大;當(dāng)Kp值大于等于1.1時(shí)，濾波的航向角波動(dòng)很大;當(dāng)Kp值為0.6～1.1之間，無(wú)人機(jī)自主導(dǎo)航的精確度更佳。

5 結(jié)束語(yǔ)

本文闡述了四旋翼飛行器姿態(tài)解算常用四元數(shù)法姿態(tài)解算方法與互補(bǔ)濾波融合算法原理，并研究了影響姿態(tài)解算的重要參數(shù)（比例系數(shù)Kp）的最佳取值。通過(guò)對(duì)Mahony算法做出改進(jìn)，最后在Matlab仿真試驗(yàn)和Mission Planner地面站軟件的測(cè)試下，比較不同的姿態(tài)解算結(jié)果，調(diào)試得出最佳的比例系數(shù)Kp值，得到了更好的姿態(tài)解算結(jié)果。四旋翼飛行器姿態(tài)信息的準(zhǔn)確性直接影響飛行器的飛行控制效果。實(shí)驗(yàn)結(jié)果表明，Mahony自適應(yīng)互補(bǔ)濾波算法濾波效果良好，可以有效降低低成本IUM系統(tǒng)的誤差累加，可以在實(shí)際應(yīng)用中得到較高的姿態(tài)精度，對(duì)飛行器的控制研究具有一定的參考價(jià)值與指導(dǎo)意義。

參考文獻(xiàn)

[1]趙偉，王志華，周兵. 基于.NET技術(shù)和MVC的新架構(gòu)模式[J]. 計(jì)算機(jī)工程與設(shè)計(jì)，2012，33（7）：2646-2651.

[2]高京都. 四旋翼飛行器控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[D]. 大連：大連理工大學(xué)，2015.

[3]李玉玲. 人工智能在計(jì)算機(jī)視覺(jué)及網(wǎng)絡(luò)領(lǐng)域中的應(yīng)用[J]. 電腦編程技巧與維護(hù)，2018（8）：156-157，161.

[4]段曉軍，吳成富. 嵌入式實(shí)時(shí)控制系統(tǒng)的快速原型化及其在無(wú)人機(jī)飛控系統(tǒng)設(shè)計(jì)與實(shí)時(shí)仿真中的應(yīng)用[C]//中國(guó)航空學(xué)會(huì)控制與應(yīng)用第十一屆學(xué)術(shù)年會(huì).煙臺(tái)：中國(guó)航空學(xué)會(huì)，2004：530-534.

[5]張錦. 三余度飛控計(jì)算機(jī)系統(tǒng)軟件的研究與設(shè)計(jì)[D].西安：西北工業(yè)大學(xué)，2006.

[6]劉耀欽，袁承芬. MVC設(shè)計(jì)模式在Web開(kāi)中的應(yīng)用與研究[J]. 信息安全與技術(shù)，2013，4（11）：78-80.

[7]LUO R C， LIN M H， SCHERP R S. Dynamic multi-sensor data fusion system for intelligent robots[J]. IEEE Journal of Robotics and Automation， 1988，15（4）： 386-396.

[8]KAMBEROVA G， MANDELBAUM R， MINTZ M， et al. Decision-theoretic approach to robust fusion of location data[J]. Journal of the Franklin Institute， 1999， 336（2）： 269-284.

[9]XU Lijia， CHEN Yangzhou， CUI Pingyuan. Improvement of D-S evidential theory in multisensor data fusion system[C]// Proceedings of the World Congress on Intelligent Control and Automation. Hangzhou：Zhejiang University， 2004： 3124-3128.

[10]MOORE J B. Direct Kalman filtering approach for GPS/INS integration[J]. IEEE Transactions on aerospace and electronic systems， 2002， 38（2）：687-693.

[11]SASIADEKT J Z， WANG Q， ZEREMBA M B. Fuzzy adaptive Kalman filtering for INS/GPS data fusion[C]// the 15th IEEE International Symposium on Intelligent Control （ISIC 2000）.RIO， Patras，Greece：IEEE，2000：181-186.

[12]陳華胄，諶海云. 基于自適應(yīng)互補(bǔ)濾波的四旋翼飛行器姿態(tài)解算[J]. 測(cè)控技術(shù)，2018，37（6）：81-84，89.

[13]江杰，王康，李剛. 一種四旋翼姿態(tài)解算與控制優(yōu)化方法設(shè)計(jì)分析[J].計(jì)算機(jī)仿真，2016，33（11）：42-47.

[14]CARLSON N A. Federal filter for fault-tolerant integrated navigation system[c]// IEEE. PLANS'88.， Position Location and Navigation Symposium， Reard， Navigation into the 21st Century'.Orlando， FL， USA：IEEE， 1988：110-119.

[15]洪世杰，李嘉欣，侯佳木，等. 自主巡線四旋翼無(wú)人機(jī)建模及姿態(tài)控制器設(shè)計(jì)[J]. 現(xiàn)代計(jì)算機(jī)（專業(yè)版），2018（16）：66-71.

[16]CUI Yongjun， LIU Jun， ZHANG Wendong. Research on adaptive Kalman filtering algorithm for integrated GPS/INS navigation system[C]// 6th International symposium on Test and Measurement. Dalian：[s.n.]，2005，8.

[17]黃曉瑞， 崔平遠(yuǎn)， 崔祜濤. GPS/INS組合導(dǎo)航系統(tǒng)自適應(yīng)濾波算法與仿真研究[J]. 飛行力學(xué)，2001， 19（2）：69-72，77.

[18]XIA Q， RAOAND M， YING Y. Adaptive fading Kalman filter with an application[J]. Automatica，1994， 30（8）：1333-1338.