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)在受到長時間低頻干擾時的航向精度；而卡爾曼及其擴展算法可對磁強計和陀螺儀進行信息融合，能有效平滑曲線，使高頻誤差得到抑制，但無法消除磁強計受到的軟硬磁干擾誤差，而且應(yī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（意法半導(dǎo)體）公司的LSM303D，它內(nèi)部的三軸磁強計可通過SPI總線口輸出測得的磁場數(shù)據(jù)，另外，它內(nèi)部的三軸加速度可用來測量重力矢量，獲得載體的傾角信息，并通過運算補償航向信息的輸出。主控器采用ST公司CM4內(nèi)核的32位ARM微控制器STM32F407，主要通過獲取MEMS傳感器的輸出數(shù)據(jù)進行姿態(tài)解算，完成飛行器航向的精密測量。

    由于IMU中的陀螺儀、加速度計和電子羅盤中的加速度傳感器都可以測量加速度信息，因此該系統(tǒng)的傳感器配置也是具有一定冗余度，主控制器的另一任務(wù)就是將各傳感器的數(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)梯度下降和二階互補濾波算法融合后解算的航向角誤差曲線。旋翼飛行器運動過程中受電機工作引起的機架振動、載體重力分布不均、機械連接處阻尼問題等因素影響，導(dǎo)致航向角誤差比水平放置在轉(zhuǎn)臺上時的靜態(tài)零點誤差偏差相對較大且波動相對厲害，經(jīng)過組合濾波處理后，航向角誤差在±1.5°以內(nèi)，可以滿足小型旋翼飛行器對航向數(shù)據(jù)的要求。

4 結(jié)論

    航向信息作為無人飛行器導(dǎo)航定位的重要測量參數(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ù)應(yīng)用，2015，41(12)：61-64.

[2] 任愛愛，孫永榮，胡應(yīng)東，等.無人機航向測量抗差補償濾波技術(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] 秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社，2014.

[6] 曾憲陽，楊紅莉，郁漢琪.基于STM32單片機四旋翼飛行器建模分析與設(shè)計[J].電子技術(shù)應(yīng)用，2016，12(42)：65-68.

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

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

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

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