0 引言

    航向是無(wú)人飛行器的重要飛行參數(shù)，在飛行過(guò)程中，實(shí)時(shí)準(zhǔn)確地獲得飛行器的航向信息決定了飛行器的可靠性和穩(wěn)定性。如今隨著微電子技術(shù)的發(fā)展，小型低成本旋翼無(wú)人飛行器普遍采用基于微機(jī)電系統(tǒng)（MEMS）的三軸陀螺儀、三軸加速度計(jì)和三軸磁強(qiáng)計(jì)來(lái)測(cè)量航向^[1]。由于MEMS傳感器自身存在非正交誤差，以及數(shù)據(jù)積分產(chǎn)生的發(fā)散誤差和外部干擾產(chǎn)生的隨機(jī)誤差，因此需要對(duì)多傳感器進(jìn)行誤差處理并尋求最佳數(shù)據(jù)融合算法，才能得到精確的航向信息。

    目前多數(shù)由陀螺儀和磁強(qiáng)計(jì)組成的航向測(cè)量系統(tǒng)主要采用羅差修正法和卡爾曼濾波算法來(lái)計(jì)算航向信息^[2]。采用羅差修正的方法對(duì)磁強(qiáng)計(jì)進(jìn)行校正，雖然能提高一定的精度，但僅針對(duì)磁傳感器校正仍然無(wú)法提高系統(tǒng)在受到長(zhǎng)時(shí)間低頻干擾時(shí)的航向精度；而卡爾曼及其擴(kuò)展算法可對(duì)磁強(qiáng)計(jì)和陀螺儀進(jìn)行信息融合，能有效平滑曲線，使高頻誤差得到抑制，但無(wú)法消除磁強(qiáng)計(jì)受到的軟硬磁干擾誤差，而且應(yīng)用時(shí)需要為其建立可靠穩(wěn)定的狀態(tài)方程，確定合適的量測(cè)噪聲和過(guò)程噪聲協(xié)方差矩陣，需要較大的時(shí)間開銷。

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

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

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

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

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

    該航向測(cè)量系統(tǒng)硬件主要由MEMS-IMU、MEMS電子羅盤和主控制器組成，如圖2所示。其中MEMS-IMU采用的是InvenSense公司的MPU-6500，它由一個(gè)三軸陀螺儀和三軸加速度計(jì)組成，可以通過(guò)SPI口輸出飛行器的角速度、加速度等姿態(tài)信息，用來(lái)計(jì)算俯仰角及橫滾角；所采用的MEMS電子羅盤是ST（意法半導(dǎo)體）公司的LSM303D，它內(nèi)部的三軸磁強(qiáng)計(jì)可通過(guò)SPI總線口輸出測(cè)得的磁場(chǎng)數(shù)據(jù)，另外，它內(nèi)部的三軸加速度可用來(lái)測(cè)量重力矢量，獲得載體的傾角信息，并通過(guò)運(yùn)算補(bǔ)償航向信息的輸出。主控器采用ST公司CM4內(nèi)核的32位ARM微控制器STM32F407，主要通過(guò)獲取MEMS傳感器的輸出數(shù)據(jù)進(jìn)行姿態(tài)解算，完成飛行器航向的精密測(cè)量。

    由于IMU中的陀螺儀、加速度計(jì)和電子羅盤中的加速度傳感器都可以測(cè)量加速度信息，因此該系統(tǒng)的傳感器配置也是具有一定冗余度，主控制器的另一任務(wù)就是將各傳感器的數(shù)據(jù)取長(zhǎng)補(bǔ)短進(jìn)行數(shù)據(jù)融合，提高姿態(tài)解算精度。主控制器讀取出傳感器采集到的原始數(shù)據(jù)，利用梯度下降法優(yōu)化電子羅盤中加速度計(jì)和磁強(qiáng)計(jì)的輸出數(shù)據(jù)，獲得靜態(tài)性較好的姿態(tài)四元數(shù)，然后利用改進(jìn)型互補(bǔ)濾波器將其與IMU獲得的動(dòng)態(tài)性較好的姿態(tài)信息進(jìn)行融合濾波，最后通過(guò)四元數(shù)的坐標(biāo)換算解算出航向角。

2 航向數(shù)據(jù)組合濾波算法設(shè)計(jì)

2.1 梯度下降法

    梯度下降算法是一種迭代求極值的算法，具體實(shí)現(xiàn)過(guò)程是按目標(biāo)函數(shù)斜率的負(fù)方向來(lái)搜索尋優(yōu)。電子羅盤中的加速度計(jì)和磁強(qiáng)計(jì)具有良好的靜態(tài)特性，長(zhǎng)時(shí)間使用不會(huì)引入積分誤差，但是加速度計(jì)受載體振動(dòng)和運(yùn)動(dòng)影響較大，動(dòng)態(tài)環(huán)境下，瞬時(shí)誤差大，而且磁強(qiáng)計(jì)容易受到周邊環(huán)境中軟硬磁干擾[7]，因此本文首先使用梯度下降算法加對(duì)速度計(jì)和磁強(qiáng)計(jì)輸出數(shù)據(jù)進(jìn)行優(yōu)化，獲得一個(gè)靜態(tài)特性較好的姿態(tài)四元數(shù)，然后再進(jìn)行下一步航向解算。

    顯然f(q)≥0，進(jìn)而對(duì)姿態(tài)四元數(shù)的優(yōu)化就可以轉(zhuǎn)換為對(duì)f(q)求最小值。

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

2.2 改進(jìn)型互補(bǔ)濾波算法

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

    由圖3可知：

2.3 航向角組合解算

    系統(tǒng)基于四元數(shù)的姿態(tài)計(jì)算即對(duì)式（1）中的姿態(tài)四元數(shù)的微分方程進(jìn)行求解，四元數(shù)微分方程常用的解算方法有^[10]：?jiǎn)巫訕有D(zhuǎn)適量法、多子樣旋轉(zhuǎn)矢量法、龍格庫(kù)塔法和泰勒展開法。旋轉(zhuǎn)矢量法多用于輸出是角增量的陀螺儀, 而MEMS陀螺儀的輸出形式是角速率?？紤]到運(yùn)算精度和速度, 本系統(tǒng)采用四階-龍格庫(kù)塔法求解微分方程：

式中，h為姿態(tài)數(shù)據(jù)采樣的周期，即四元數(shù)更新周期；ω_b(t)、、ω_b(t+h)分別為在更新周期h時(shí)間內(nèi)陀螺的采樣值。每個(gè)采樣周期內(nèi)提取陀螺儀的數(shù)據(jù)，對(duì)式(10)進(jìn)行迭代運(yùn)算，便能夠?qū)崿F(xiàn)對(duì)四元數(shù)的實(shí)時(shí)更新，從而得出航向角。

3 實(shí)驗(yàn)驗(yàn)證分析

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

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

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

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

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

4 結(jié)論

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

參考文獻(xiàn)

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

[2] 任愛愛，孫永榮，胡應(yīng)東，等.無(wú)人機(jī)航向測(cè)量抗差補(bǔ)償濾波技術(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] 曾憲陽(yáng)，楊紅莉，郁漢琪.基于STM32單片機(jī)四旋翼飛行器建模分析與設(shè)計(jì)[J].電子技術(shù)應(yīng)用，2016，12(42)：65-68.

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

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

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

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