陳 亮，楊柳慶，肖前貴

（1.南京航空航天大學(xué) 自動(dòng)化學(xué)院，江蘇 南京210016；2.南京航空航天大學(xué) 無人機(jī)研究院，江蘇 南京210016）

基于梯度下降法和互補(bǔ)濾波的航向姿態(tài)參考系統(tǒng)

陳 亮1，楊柳慶1，肖前貴2

（1.南京航空航天大學(xué) 自動(dòng)化學(xué)院，江蘇 南京210016；2.南京航空航天大學(xué) 無人機(jī)研究院，江蘇 南京210016）

針對(duì)微型無人機(jī)航向姿態(tài)參考系統(tǒng)低成本、小型化的工程實(shí)現(xiàn)需求，基于三軸陀螺儀、加速度計(jì)和磁力計(jì)，提出了一種在線實(shí)時(shí)姿態(tài)估計(jì)算法。該算法采用四元數(shù)描述系統(tǒng)模型，采用改進(jìn)的梯度下降法預(yù)處理加速度計(jì)和磁力計(jì)的姿態(tài)信息，然后采用互補(bǔ)濾波融合陀螺儀的姿態(tài)信息，實(shí)現(xiàn)高精度實(shí)時(shí)姿態(tài)估計(jì)。最后通過在線性能測試，來驗(yàn)證算法的有效性。結(jié)果表明，該算法測量誤差小、運(yùn)算量小、實(shí)時(shí)性高，具有較高的工程應(yīng)用價(jià)值。

航姿系統(tǒng)；梯度下降法；互補(bǔ)濾波；四元數(shù)

近年來隨著科技和社會(huì)發(fā)展，無人機(jī)發(fā)展日新月異，其中微小型無人機(jī)在軍用和民用領(lǐng)域以其低成本、小尺寸、生存性能強(qiáng)等特點(diǎn)發(fā)展尤其迅猛，在軍事偵察、生態(tài)監(jiān)測、視頻拍攝和搶險(xiǎn)救災(zāi)等有著廣泛應(yīng)用前景。航向姿態(tài)參考系統(tǒng)是無人機(jī)導(dǎo)航與飛行控制系統(tǒng)的重要組成部分，其精度直接影響到無人機(jī)的姿態(tài)控制精度及導(dǎo)航定位精度。微小型無人機(jī)自身有效載荷小，決定了在一段時(shí)間內(nèi)都無法安裝激光陀螺、光纖陀螺等高精度的姿態(tài)測量裝置，于是以MEMS陀螺儀和加速度計(jì)，以及磁力計(jì)（MARG）等組成的低成本微小型航向姿態(tài)參考系統(tǒng)（AHRS）成為一個(gè)研究熱點(diǎn)[1]。然而低成本的MEMS陀螺儀雖然瞬態(tài)性能較好，但誤差漂移會(huì)隨著時(shí)間逐漸累積，導(dǎo)致測量精度降低；加速度計(jì)沒有積分誤差，但當(dāng)被測對(duì)象處于高動(dòng)態(tài)時(shí)，會(huì)受到附加平動(dòng)或轉(zhuǎn)動(dòng)加速度干擾；同樣地，磁力計(jì)也會(huì)受到周圍的軟磁或硬磁的干擾。因此，單獨(dú)采用MEMS陀螺儀或者加速度計(jì)和磁力計(jì)都無法獨(dú)立給出足夠好的姿態(tài)估計(jì)[2]。

一般的，姿態(tài)估計(jì)是采用卡爾曼濾波算法及其衍生形式，如擴(kuò)展卡爾曼濾波（EKF），無跡卡爾曼濾波（UKF），聯(lián)邦卡爾曼濾波等，在文獻(xiàn)[3-7]中進(jìn)行了不同方面深入的研究，分別采用了EKF，無跡卡爾曼濾波，粒子濾波等算法來提高姿態(tài)估計(jì)的精度。其中擴(kuò)展卡爾曼濾波器算法是一種高精度的在飛行器中應(yīng)用非常廣泛的姿態(tài)解算算法，但EKF存在3大缺陷：1）在一般情況下計(jì)算雅可比矩陣是不容易實(shí)現(xiàn)的過程而且其計(jì)算量很大；2）當(dāng)線性化假設(shè)不成立時(shí)，線性化會(huì)導(dǎo)致濾波器極度不穩(wěn)定；3）實(shí)際應(yīng)用中，噪聲難以符合白噪聲的要求[8-9]。一般情況下卡爾曼濾波需要構(gòu)造多維的狀態(tài)方程和量測方程，運(yùn)算量較大，對(duì)嵌入式處理器資源占用率過高。文獻(xiàn)[10]提出一種自適應(yīng)顯式互補(bǔ)濾波（AECF）來提高無人機(jī)的姿態(tài)估計(jì)精度，有效地解決了一般互補(bǔ)濾波器需要對(duì)重構(gòu)無人機(jī)姿態(tài)估計(jì)的缺陷。文獻(xiàn)[11]中提出采用一種梯度下降算法對(duì)加速度計(jì)和磁強(qiáng)計(jì)的數(shù)據(jù)進(jìn)行預(yù)處理，然后利用互補(bǔ)濾波器將處理后的結(jié)果與陀螺儀的積分進(jìn)行融合，但是在融合過程中參數(shù)為定值，導(dǎo)致算法在飛機(jī)高機(jī)動(dòng)狀態(tài)下的姿態(tài)估計(jì)誤差較大。在實(shí)際情況中，微小型無人機(jī)的飛控和航姿系統(tǒng)是集成在一個(gè)模塊中，由一個(gè)內(nèi)核來處理所有數(shù)據(jù)，同時(shí)無人機(jī)飛控和航姿系統(tǒng)對(duì)實(shí)時(shí)性要求較高[12]。為了減小微型無人機(jī)中處理器的運(yùn)算負(fù)擔(dān)，降低航姿算法的解算復(fù)雜度和增加算法的實(shí)時(shí)性，保證一定的精確性并增強(qiáng)工程可實(shí)現(xiàn)性，文中分析并設(shè)計(jì)了基于梯度下降法和互補(bǔ)濾波的四元數(shù)混合濾波。

1 基本原理

1.1 算法總體結(jié)構(gòu)

文中所設(shè)計(jì)的混合濾波算法總體結(jié)構(gòu)為:首先，利用陀螺儀輸出的角速度和四元數(shù)微分方程得出角速度微分四元數(shù)；然后運(yùn)用梯度下降法對(duì)加速度計(jì)和磁力計(jì)測得數(shù)據(jù)進(jìn)行預(yù)處理，得到最小的誤差四元數(shù)的微分；再用互補(bǔ)濾波將兩者進(jìn)行融合，盡可能的減小陀螺儀的漂移誤差和加速度計(jì)的高頻干擾，以及磁力計(jì)引起的姿態(tài)估計(jì)誤差；最后對(duì)互補(bǔ)濾波后的姿態(tài)微分四元數(shù)進(jìn)行積分，估算出最優(yōu)的姿態(tài)值。該混合濾波算法運(yùn)算過程不涉及三角和反三角函數(shù)運(yùn)算，全部使用簡單的四元數(shù)加減乘除運(yùn)算，計(jì)算工作量很小，極大地提高了運(yùn)算效率。

圖1 算法總體結(jié)構(gòu)圖

1.2 四元數(shù)姿態(tài)描述

通常將慣性測量單元中陀螺儀、加速度計(jì)和磁力計(jì)的測量數(shù)據(jù)定義到機(jī)體坐標(biāo)系，取機(jī)體的重心為機(jī)體坐標(biāo)系原點(diǎn)，3個(gè)軸分別與機(jī)體的縱軸、橫軸和豎軸相互重合，分別定義為右前上（XYZ）。相對(duì)應(yīng)的絕對(duì)坐標(biāo)系稱為導(dǎo)航坐標(biāo)系，文中采用東北天ENU坐標(biāo)系n。機(jī)體坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的轉(zhuǎn)換可采用旋轉(zhuǎn)四元數(shù)表示[13]：

又由姿態(tài)四元滿足微分方程：

設(shè)在很小的時(shí)間段（t，t+Δt）內(nèi)，Δt為姿態(tài)四元數(shù)更新時(shí)間，wbnb值不變?？梢缘玫交谕勇輧x輸出的姿態(tài)四元數(shù)的迭代方程為：

方向余弦矩陣與四元數(shù)的關(guān)系可表示為：

但由于四元數(shù)實(shí)質(zhì)上是旋轉(zhuǎn)矢量，它的4個(gè)變量并不獨(dú)立，必須對(duì)其規(guī)范化處理。本文所用四元數(shù)均為規(guī)范化的模值為1的四元數(shù)，即：

并且在每次濾波周期，都要對(duì)解算得到的四元數(shù)進(jìn)行規(guī)范化處理。

姿態(tài)角與四元數(shù)之間的關(guān)系可表示為

2 互補(bǔ)濾波器的設(shè)計(jì)

2.1 互補(bǔ)濾波器原理

互補(bǔ)濾波簡單地來說，就是把濾波器的兩部分基于一階微分系統(tǒng)來融合在一起，成為一個(gè)新濾波器，從而得到一個(gè)更加準(zhǔn)確的線性估計(jì)輸出。在無人機(jī)航姿系統(tǒng)中，由于陀螺儀雖然短時(shí)間內(nèi)可提供高精度的姿態(tài)數(shù)據(jù)，但因漂移的影響，精度隨著時(shí)間就會(huì)降低。加速度計(jì)和磁力計(jì)則相反，其量測誤差不會(huì)隨著時(shí)間累積，長時(shí)間內(nèi)有良好的穩(wěn)定性，但瞬態(tài)的加速度和磁場干擾對(duì)測量精度影響較大。從頻域上來說，陀螺儀具有高通特性，而加速度計(jì)和磁力計(jì)有低通特性，因此它們正好在頻域上相互補(bǔ)充[15]。

式中ya為加速度計(jì)和磁力計(jì)姿態(tài)量測信息；yb為陀螺儀輸出值；wa、wb表示系統(tǒng)量測噪聲；b為陀螺漂移。一般工程中，互補(bǔ)濾波器結(jié)構(gòu)可用如圖2所示。

圖2 互補(bǔ)濾波器結(jié)構(gòu)示意圖

2.2 基于梯度下降算法的四元數(shù)最優(yōu)估計(jì)

梯度下降法是在沿梯度下降的方向求解表達(dá)式的最小值。其迭代公式為yk+1=yk-ρkgk，其中g(shù)k表示函數(shù)的梯度，前面加負(fù)號(hào)表示梯度的負(fù)方向，即梯度下降方向；ρk表示在梯度方向上的搜索步長，即求取一點(diǎn)使函數(shù)的值達(dá)到最小。具體到文中，設(shè)經(jīng)過單位規(guī)范化后的機(jī)體加速度計(jì)、磁力計(jì)三軸輸出分別為 ab=[0 axayaz]T、mb=[0 mxmymz]T，當(dāng)?shù)貐⒖贾亓κ噶繛間n=[0 0 0 1]T，當(dāng)?shù)貐⒖即艌鍪噶繛閙n=[0 px0 pz]T，令

故對(duì)重力場，其誤差向量矩陣為：

同樣地，對(duì)磁場，其誤差向量矩陣為：

誤差向量F的梯度為：

則誤差向量‖F(xiàn)‖下降最快的方向時(shí)的單位四元數(shù)梯度即是：

式中w是陀螺儀角速率構(gòu)成的四元數(shù)向量，即w=[0 wxwywz]。

綜上可得，基于梯度下降算法的四元數(shù)最優(yōu)估計(jì)的迭代方程為：

其中qam，t為由加速度計(jì)和磁力計(jì)輸出數(shù)據(jù)經(jīng)梯度下降算法處理后得到的姿態(tài)更新四元數(shù)。

2.3 互補(bǔ)濾波

利用陀螺儀的高通特性，加速度計(jì)和磁力計(jì)的低通特性，在頻域上的互補(bǔ)關(guān)系，采用互補(bǔ)濾波對(duì)兩者的信息進(jìn)行融合，來提高航姿系統(tǒng)的姿態(tài)測量精度。由上文2.1節(jié)分析，為使算法更加簡單高效，提高系統(tǒng)姿態(tài)結(jié)算的快速性和實(shí)時(shí)性，令kp=K，ki=1，則互補(bǔ)濾波融合后的姿態(tài)估計(jì)值為：

其時(shí)域表達(dá)式為：

其中θa，m為加速度計(jì)和磁力計(jì)輸出數(shù)據(jù)基于梯度下降法得到的姿態(tài)角估計(jì)，可由迭代式（16）得到；wcor為陀螺儀經(jīng)過校正補(bǔ)償后的角速率，可由下文3.2節(jié)式（20）、式（21）得到；為融合后的姿態(tài)四角估計(jì)；適當(dāng)選取低通濾波器系數(shù)K值，可使系統(tǒng)具有恰當(dāng)?shù)慕刂诡l率，K值的大小主要根據(jù)無人機(jī)的飛行狀態(tài)來變化。

3 誤差補(bǔ)償

3.1 有害加速度的考慮

當(dāng)無人機(jī)加速狀態(tài)下，由于加速度計(jì)會(huì)敏感到有害加速度，因此在長時(shí)間機(jī)動(dòng)后姿態(tài)測量會(huì)出現(xiàn)較大誤差，導(dǎo)致濾波發(fā)散，所以必須對(duì)有害加速度進(jìn)行考慮。設(shè)其中分別為加速度計(jì)白噪聲零偏的均方差，根據(jù)試驗(yàn)經(jīng)驗(yàn)取高加速飛行狀態(tài)閥值為ζ=0.3gn，則

當(dāng)λt≤η，無人機(jī)處于靜止或勻速飛行狀態(tài)，K值不變，根據(jù)多次試驗(yàn)取經(jīng)驗(yàn)值Kt0=20；

當(dāng)λt＞ζ，K值應(yīng)趨于無窮小，可取Kt=0，即無人機(jī)在高機(jī)動(dòng)飛行狀態(tài)主要依靠基于陀螺儀的姿態(tài)導(dǎo)出。

3.2 陀螺儀誤差補(bǔ)償

陀螺的隨機(jī)誤差模型[10-16]表示為：

其中式中εb為隨機(jī)常值漂移，εm為可以用一階馬爾可夫過程描述的相關(guān)漂移，wg為零均值高斯白噪聲。由于系統(tǒng)的采樣頻率遠(yuǎn)高于陀螺零偏的變化速率，采用隨機(jī)游走描述陀螺儀隨機(jī)漂移，因此可以忽略式中第二項(xiàng)相關(guān)漂移。按照四元數(shù)微分方程可以反求得到，陀螺儀角速率誤差wε與誤差四元數(shù)微分之間的關(guān)系可以表示為：

陀螺零偏wb為陀螺漂移wε中的常值部分，可以通過下式來消除：

其中wcor，t為陀螺角速率在零偏補(bǔ)償后組成的四元數(shù)矢量。

4 硬件平臺(tái)與算法驗(yàn)證

文中采用MPU9150九軸慣性傳感器測量單元進(jìn)行測試，MPU9150由MPU6050傳感器（其中包括三軸加速度計(jì)和三軸陀螺儀）與HMC5883L三軸磁力計(jì)集成，數(shù)據(jù)通過I2C總線進(jìn)行讀取，利用文中設(shè)計(jì)的濾波算法對(duì)傳感器輸出的原始數(shù)據(jù)進(jìn)行處理來獲得機(jī)體準(zhǔn)確的姿態(tài)信息，并通過SPI口將解算后的數(shù)據(jù)經(jīng)由2.4 GHz無線數(shù)傳模塊上傳到上位機(jī)中。上位機(jī)監(jiān)控顯示界面采用C++編寫，可顯示各傳感器的測量數(shù)據(jù)和姿態(tài)角的數(shù)據(jù)，航姿系統(tǒng)以50 Hz頻率對(duì)傳感器進(jìn)行采樣，并將數(shù)據(jù)導(dǎo)出到Excel中，以便進(jìn)一步地分析。系統(tǒng)硬件結(jié)構(gòu)圖如圖3。

圖3 系統(tǒng)硬件結(jié)構(gòu)示意圖

將本系統(tǒng)加載到某型微小型無人機(jī)中，采集實(shí)際飛行數(shù)據(jù)，其中主要分為四段飛行狀態(tài)，分別為無人機(jī)加速、轉(zhuǎn)彎、平飛和大幅連續(xù)轉(zhuǎn)彎過程。實(shí)驗(yàn)結(jié)果如圖4，5，6和表1所示。

圖4 俯仰角曲線圖

圖5 偏航角曲線圖

圖6 滾轉(zhuǎn)角曲線圖

表1 各飛行狀態(tài)下的無人機(jī)姿態(tài)誤差

在0～400 s時(shí)段，無人機(jī)在進(jìn)行加減速運(yùn)動(dòng)，由上圖4和表1可知基于梯度下降算法的四元數(shù)互補(bǔ)濾波能夠較好地跟蹤無人機(jī)真實(shí)姿態(tài)。在400～700 s時(shí)段內(nèi)，無人機(jī)連續(xù)轉(zhuǎn)彎，表1給出了此階段梯度下降算法的姿態(tài)誤差平均值和誤差均方差值，3個(gè)姿態(tài)角的誤差值均小于1°，姿態(tài)誤差均方差也都小于1.8°，因此改進(jìn)后的梯度下降算法也能較好地跟蹤無人機(jī)連續(xù)機(jī)動(dòng)狀態(tài)下的姿態(tài)變化。在800～1 100 s時(shí)段，無人機(jī)在進(jìn)行平穩(wěn)飛行，表1給出了此階段無人機(jī)的姿態(tài)誤差，可以看出其誤差平均值和誤差均方差都很小，很好地完成姿態(tài)跟蹤。在1 100～1 500 s，飛機(jī)在大幅連續(xù)轉(zhuǎn)彎過程中，算法解算結(jié)果仍能較好地完成無人機(jī)姿態(tài)確定。

5 結(jié)束語

文中提出了基于梯度下降法和互補(bǔ)濾波相結(jié)合的混合濾波算法，加速度計(jì)和磁力計(jì)測量數(shù)據(jù)，經(jīng)過梯度下降算法處理后，反饋到參數(shù)可調(diào)的四元數(shù)互補(bǔ)濾波中，完成對(duì)陀螺漂移校正，進(jìn)而獲得高精度的姿態(tài)信息。實(shí)驗(yàn)結(jié)果表明，該算法具有較高的姿態(tài)估算精度，快速性好，對(duì)系統(tǒng)負(fù)載較低，具有較高的工程實(shí)用價(jià)值，適合低成本的航姿平臺(tái)工程應(yīng)用。

[1]劉興川，張盛，李麗哲，等.基于四元數(shù)的MARG傳感器姿態(tài)測量算法[J].清華大學(xué)學(xué)報(bào)：自然科學(xué)版 ，2012，52（5）: 627-631.

[2]張浩，任芊.四旋翼飛行器航姿測量系統(tǒng)的數(shù)據(jù)融合方法[J].兵工自動(dòng)化，2013，32（1）:28-31.

[3]Tarhan M，Altu E.EKF based attitude estimation and stabilization of a quadrotor UAV using vanishing points in catadioptricimages[J].Journal of Intelligent&Robotic Systems，2011，62（3-4）:587-607.

[4]Jing L，Xu L，Li X，et al.A federal UKF algorithm in INS/ GPS/aerial image integrated attitude determination system[C] //Imaging Systems and Techniques（IST），2013 IEEE International Conference on.IEEE，2013:60-63.

[5]Kai Xiong，Tang Liang，Lei Peng.Multiple Model Kalman Filter for Attitude Determination of Precision Pointing Spac-ecraft[J].ActaAstro-Nautica，2011（68）:843-845.

[6]Marina H G，Espinosa F，Santos C.Adaptive UAV Attitude Estimation Employing Unscented Kalman Filter，F(xiàn)OAM and Low-Cost MEMS Sensors[J].Sensors，2012，12（7）:9566-9585.

[7]Won S P，Melek W W，Golnaraghi F.A Kalman/Particle Filter-based Position and Orientation Estimation Method Using a Position Sensor/Inertial Measurement Unit Hybrid System[J].IEEETransactionsonIndustrialElectronics，2010，57（5）:1787-1798.

[8]趙琳，王小旭，丁繼成，等.組合導(dǎo)航系統(tǒng)非線性濾波算法綜述[J].中國慣性技術(shù)學(xué)報(bào)，2009，17（1）:46-58.

[9]Fourati H，Manamanni N，Afilal L，et al.A complementary observer-based approach for the estimation of motion in rigid bodies using inertial and magnetic sensors[C]//Control Applications（CCA），2010IEEEInternationalConferenceon.IEEE，2010:422-427.

[10]Euston M，Coote P，Mahony R，et al.A complementary filter for attitude estimation of a fixed-wing UAV[C]//Intelligent Robots and Systems，2008.IROS 2008.IEEE/RSJ International Conference on.IEEE，2008:340-345.

[11]Madgwick S O H，Harrison A J L，Vaidyanathan R.Estimation of IMU and MARG orientation using a gradient descent algorithm [C]//Rehabilitation Robotics（ICORR），2011 IEEE International Conference on.IEEE，2011:1-7.

[12]徐玉，李平，韓波.一種面向機(jī)動(dòng)的低成本姿態(tài)測量系統(tǒng)[J].傳感技術(shù)學(xué)報(bào)，2008（10）:2272-2275.

[13]Markley F L.Attitude error representations for Kalman filtering[J].Journal of Guidance，Control，and Dynamics，2003，26（2）:311－317．

[14]劉建業(yè)，曾慶化，趙偉，等.導(dǎo)航系統(tǒng)理論與應(yīng)用[M].西安:西北工業(yè)大學(xué)出版社，2010.

[15]Mahony R，Hamel T，Pflimlin J M.Complementary filter design on the special orthogonal group so（3）[C]//Proc of the IEEE Conference on Decision and Control and European Control Conference，2005:1477-1484.

[16]Robert Mahony，Tarek Hamel，Pascal Morin，et al.Nonlinear complementary filters on the special linear group[J]. International Journal of Control，2012，85（10）:1557-1573.

Attitude and heading system based on gradient descent algorithm and quaternion complementary filter

CHEN Liang1，YANG Liu-qing1，XIAO Qian-gui2
（1.College of Automation Engineering，Nanjing University of Aeronautics and Astronautics，Nanjing 210016，China；2.Research Institute of Unmanned Aircraft，Nanjing University of Aeronautics and Astronautics，Nanjing 210016，China）

In order to satisfy the requirement of the low cost micro unmanned aerial vehicles'attitude and heading reference system（AHRS），the online real-time attitude determination method is proposed based on the combination of triaxial gyroscope，accelerometer and magnetometer.In the system，the system model is described by quaternion method，and an optimized gradient descent algorithm is used to process the data measured by accelerometer and magnetometer.And the complementary filter whose parameters are adjustable is used to fuse the data with triaxial gyroscope measurement to obtain high-precision attitude information.Online experiments were conducted to assess the performance of algorithm and verify the effectiveness of the algorithm.And the results show that the proposed scheme has characteristics of low error of attitude tracking，parameter adjustment，low computer load，and easy to realize，therefore the designed algorithm is applicable for low-cost and small-sized attitude and heading reference system of micro UAV.

attitude and heading reference system；gradient-descent algorithm；complementary filter；quaternion method

TN967.2

A

1674－6236（2016）24-0038-04

2015-11-30 稿件編號(hào)：201511291

陳 亮（1991—），男，安徽滁州人，碩士研究生。研究方向：無人機(jī)導(dǎo)航制導(dǎo)與控制。