0 引言

    航向是無人飛行器的重要飛行參數(shù),，在飛行過程中，實時準確地獲得飛行器的航向信息決定了飛行器的可靠性和穩(wěn)定性,。如今隨著微電子技術(shù)的發(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é)方差矩陣，需要較大的時間開銷,。

    為此,，本文設(shè)計一種基于MEMS慣性測量單元(IMU)和電子羅盤的航向測量系統(tǒng)，采用梯度下降算法來對磁強計和陀螺儀進行誤差修正,而且在姿態(tài)解算過程只需進行乘法和加法運算,，普通的微控制器即能滿足算法要求^[3],。另外，在MAHONY R提出的補償濾波器[4]的基礎(chǔ)上進行優(yōu)化改進,，結(jié)合二階互補濾波算法來進行數(shù)據(jù)融合,，以期得到精密航向輸出。

1 無人機航向角描述與系統(tǒng)方案設(shè)計

1.1 坐標系定義及姿態(tài)矩陣

    無人機的航姿信息主要包括航向角(也稱航偏角),、俯仰角θ,、橫滾角γ,。選取“東北天”為地理坐標系(記為ENU系)，“右前上”為機體坐標系(記為NED系),，由于NED系和ENU系均是直角坐標系,，且各個軸之間始終保持垂直，因此,，NED系和ENU系的空間位置關(guān)系即可理解為剛體的定點轉(zhuǎn)動^[5],。地理坐標系與機體坐標系之間的角度轉(zhuǎn)換關(guān)系如圖1所示。

1.2 系統(tǒng)方案設(shè)計

    該航向測量系統(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ù)組合濾波算法設(shè)計

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)化就可以轉(zhuǎn)換為對f(q)求最小值,。

    梯度下降法在姿態(tài)解算過程中只有簡單的加法和乘法運算,，此時四元數(shù)更新方程可變?yōu)?sup>[8]：

2.2 改進型互補濾波算法

    利用電子羅盤中三軸磁強計和加速度計測得的數(shù)據(jù)通過梯度下降法對姿態(tài)四元數(shù)進行修正轉(zhuǎn)換后得到姿態(tài)值記為。根據(jù)加速度計和磁強計的特性,，不會引入累積誤差,，但是在動態(tài)情況下，會引入較大瞬時誤差,。相反IMU中的陀螺儀可測得瞬時精度高的姿態(tài)數(shù)據(jù),，但是有累積誤差。設(shè)IMU中陀螺儀和加速度計直接積分解算出來的姿態(tài)值為,，根據(jù)和在頻域上的互補特性,，通過傳統(tǒng)的互補濾波方法融合兩者，可得到新的姿態(tài)值,，雖然能同時消除低頻和高頻的干擾,，實現(xiàn)姿態(tài)數(shù)據(jù)的融合，然而,，互補濾波器^[9]的低通阻帶衰減緩慢,，誤差較大時，振動大,，濾波效果差,。因此在傳統(tǒng)互補濾波器的基礎(chǔ)上添加了PI（比例積分）環(huán)節(jié)，構(gòu)成改進型二階互補濾波器,，如圖3所示,。

    由圖3可知：

2.3 航向角組合解算

    系統(tǒng)基于四元數(shù)的姿態(tài)計算即對式（1）中的姿態(tài)四元數(shù)的微分方程進行求解，四元數(shù)微分方程常用的解算方法有^[10]：單子樣旋轉(zhuǎn)適量法,、多子樣旋轉(zhuǎn)矢量法,、龍格庫塔法和泰勒展開法。旋轉(zhuǎn)矢量法多用于輸出是角增量的陀螺儀, 而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 實驗驗證分析

    將設(shè)計的航向測量系統(tǒng)安裝在電子轉(zhuǎn)臺上，為避免鐵磁干擾,，加工一個60 cm高的木架做固定支撐,，并校正好零位，通過串口連接至上位機,，進行傳感器數(shù)據(jù)采集,，如圖4所示。

    通過電子轉(zhuǎn)臺水平轉(zhuǎn)動,，每次回到零位時利用上位機分別采集電子磁羅盤的數(shù)據(jù)和經(jīng)過梯度下降與互補濾波算法融合后的數(shù)據(jù),。通過對比分析可知，沒有使用濾波算法的磁強計測量的航偏角相對零位誤差較大,，達到了±3°左右,，磁強計受外界干擾嚴重。經(jīng)過組合濾波處理后得到的偏航角消除了部分干擾信號,，誤差可穩(wěn)定保持在±1.0°以內(nèi),。

    在電子轉(zhuǎn)臺上轉(zhuǎn)動測試可看作是靜態(tài)測試，為進一步驗證航向測量系統(tǒng)在多旋翼飛行器實際飛行過程中的性能,，將該系統(tǒng)固定在PIX4飛控模塊屏蔽罩上,，并一同安裝在六旋翼飛行器上，機架底部水平固定在萬向節(jié)一端,，萬向節(jié)另外一端固定在水平的桌面上,，如圖5所示。

    啟動旋翼飛行器電機,，將飛行器油門通道值保持在空中懸停狀態(tài),，操作飛行器方向舵，讓其原地旋轉(zhuǎn),。然后,，通過2.4G串口數(shù)傳電臺接收飛控航向數(shù)據(jù)和航向測量系統(tǒng)傳出的磁強計、陀螺儀原始數(shù)據(jù)以及組合解算后的數(shù)據(jù),，并用MATLAB進行數(shù)據(jù)分析,，如圖6所示。

    圖6中采集的是飛行器水平懸停轉(zhuǎn)動（以飛控為基準）時的數(shù)據(jù)，其中的電子羅盤和IMU輸出航向分別代表的是磁強計和陀螺儀數(shù)據(jù)處理的航向角誤差曲線,，組合濾波輸出航向表示經(jīng)梯度下降和二階互補濾波算法融合后解算的航向角誤差曲線,。旋翼飛行器運動過程中受電機工作引起的機架振動、載體重力分布不均,、機械連接處阻尼問題等因素影響,，導致航向角誤差比水平放置在轉(zhuǎn)臺上時的靜態(tài)零點誤差偏差相對較大且波動相對厲害，經(jīng)過組合濾波處理后,，航向角誤差在±1.5°以內(nèi),，可以滿足小型旋翼飛行器對航向數(shù)據(jù)的要求。

4 結(jié)論

    航向信息作為無人飛行器導航定位的重要測量參數(shù),，要求具有較高的精度,。在小型旋翼無人飛行器系統(tǒng)上，利用MEMS IMU和電子磁羅盤各自的優(yōu)點及其互補性,，配合ARM主控制器組成低成本航向測量系統(tǒng)，采用梯度下降和二階互補濾波算法,，使三軸陀螺儀,、三軸加速度計、三軸磁強計經(jīng)過數(shù)據(jù)融合后,，完成坐標變換和姿態(tài)解算,，從而輸出較高精度的航向角。實驗結(jié)果表明：該航向測量系統(tǒng)較好地解決了噪聲干擾與航向最優(yōu)估計問題,，并在實驗平臺上得到了驗證,，航偏角誤差保持在±1.5°以內(nèi)，在沒有外界信息輔助的情況下,，可穩(wěn)定地輸出準確姿態(tài)數(shù)據(jù),，滿足了低成本旋翼無人機對航向信息的要求。

參考文獻

[1] 何川,，李翔,，王勇軍.基于STM32的四旋翼飛行器的姿態(tài)最優(yōu)估計研究[J].電子技術(shù)應用，2015,，41(12)：61-64.

[2] 任愛愛,，孫永榮，胡應東,，等.無人機航向測量抗差補償濾波技術(shù)研究[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單片機四旋翼飛行器建模分析與設(shè)計[J].電子技術(shù)應用,，2016，12(42)：65-68.

[7] 聶鵬,，李佩華,，李正強，等.基于卡爾曼濾波的小型無人機姿態(tài)估計算法研究[J].沈陽航空航天大學學報,，2013,，30(6)：53-57.

[8] 譚廣超.四旋翼飛行器姿態(tài)控制系統(tǒng)的設(shè)計與實現(xiàn)[D].大連：大連理工大學，2013.

[9] 廖懿華,，張鐵民,，廖貽泳.基于模糊-比例積分偏差修正的多旋翼飛行器姿態(tài)測算系統(tǒng)[J].農(nóng)業(yè)工程學報，2014,，30(20)：19-27.

[10] 王彤,，馬建倉，秦濤,，等.基于旋轉(zhuǎn)四元數(shù)的姿態(tài)解算算法[J].彈箭與制導學報,，2014，34(3)：15-17.