SUN Jinqiu，YOU Youpeng，F(xiàn)U Zhongyun

(1.College of Mechanical and Electrical Engineering，Nanjing University of Aeronautics and Astronautics，Nanjing 210016，China; 2.Nanhang Jincheng College，Nanjing 211156，China)

Attitude Estimation Based on Conjugate Gradient and Complementary Filter*

SUN Jinqiu1，2*，YOU Youpeng1，F(xiàn)U Zhongyun2

(1.College of Mechanical and Electrical Engineering，Nanjing University of Aeronautics and Astronautics，Nanjing 210016，China; 2.Nanhang Jincheng College，Nanjing 211156，China)

In order to improve the precision of attitude estimation，a novel attitude estimation algorithm based on conjugate gradient and complementary filter is proposed.The quaternion representation used in the proposed system for attitude estimation，is optimized based on conjugate gradient algorithm with magnetometer and accelerometer data，and then fused with the updated quaternion from gyroscope data based on complementary filter.Finally，experiment results verify that the proposed algorithm based on conjugate gradient and complementary filter improves the static and dynamic properties of attitude estimation system，and has better performance than the Kalman filter and gradient descent filter especially when the attitude has drastic changes.

sensor;attitude estimation;conjugate gradient;complementary filter;quaternion

為提高捷聯(lián)慣導(dǎo)系統(tǒng)的姿態(tài)解算精度，人們開展了大量的研究工作:一方面尋找更合適的姿態(tài)解算方法，另一方面對多種傳感器的測量值進(jìn)行數(shù)據(jù)融合。

常見的姿態(tài)解算方法有:歐拉角法、方向余弦法、四元數(shù)法、等效旋轉(zhuǎn)矢量法、對偶四元數(shù)法。歐拉角法直觀，容易理解，但存在三角函數(shù)計(jì)算問題和方程奇異現(xiàn)象。方向余弦法避免了歐拉角法的方程奇異問題，但要求解九個微分方程，計(jì)算量大。四元數(shù)法只需要求解四個微分方程，計(jì)算量小，實(shí)際中多被采用，但存在有限轉(zhuǎn)動引起不可交換性誤差的問題。等效旋轉(zhuǎn)矢量法彌補(bǔ)了四元數(shù)的不足，但計(jì)算量有所增加［1］。對偶四元數(shù)法是描述一般性剛體運(yùn)動最簡潔、最有效的數(shù)學(xué)工具。但其技術(shù)還需要進(jìn)一步完善［2］。

捷聯(lián)慣導(dǎo)系統(tǒng)中，通常采用多種傳感器組成組合導(dǎo)航系統(tǒng)，所以要選擇一個最優(yōu)的在線數(shù)據(jù)融合策略，將多種傳感器的數(shù)據(jù)進(jìn)行融合。常用的數(shù)據(jù)融合算法有:擴(kuò)展卡爾曼濾波EKF(Extended Kalman Filter)，無跡卡爾曼濾波UKF(Unscented Kalman Filter)，梯度下降法，容積卡爾曼濾波CKF(Cubature Kalman Filter)，粒子濾波PF(Particle Filter)，互補(bǔ)濾波CF(Complementary Filter)等。EKF是應(yīng)用最廣泛的有一種非線性系統(tǒng)的濾波方法，但其存在線性化誤差、截?cái)嗾`差及發(fā)散問題［3－7］。朱豐超［8］、Lin Zhao［9］、Yue Pan［10］等人將UKF應(yīng)于姿態(tài)檢測中，實(shí)驗(yàn)結(jié)果表明，其收斂速度、估計(jì)精度、穩(wěn)定性都要高于EKF。Tang Xiaojun［11］將CKF應(yīng)在航天器的姿態(tài)估計(jì)中，CKF估計(jì)精度又要高于UKF，但計(jì)算量有所增加?；パa(bǔ)濾波是從頻率分辨和消除噪聲，原理簡單，計(jì)算量最小的一種濾波方式，但其高、低通濾波器轉(zhuǎn)接頻率難以確定。文獻(xiàn)［12］中將梯度下降法應(yīng)用于姿態(tài)解算，性能優(yōu)于卡爾曼濾波。該方法具有計(jì)算量少，對初始點(diǎn)要求低的優(yōu)點(diǎn)，但與牛頓法和共軛梯度法相比，收斂較慢。

本文提出一種共軛梯度優(yōu)化方法與互補(bǔ)濾波相結(jié)合的數(shù)據(jù)融合策略，共軛梯度法既克服了梯度下降法收斂慢的不足，又可避免牛頓法計(jì)算量大的缺點(diǎn)。其中，姿態(tài)解算采用四元數(shù)法;利用加速度計(jì)和磁強(qiáng)計(jì)的輸出數(shù)據(jù)，通過共軛梯度方法對姿態(tài)四元數(shù)進(jìn)行尋優(yōu)估計(jì)，再將其和利用陀螺儀輸出數(shù)據(jù)更新的四元數(shù)進(jìn)行互補(bǔ)濾波，最終解算出歐拉角。

1 基于四元數(shù)法的姿態(tài)解算

1.1 姿態(tài)的坐標(biāo)描述

通常將陀螺儀、磁強(qiáng)計(jì)和加速度計(jì)的測量數(shù)據(jù)定義到一個坐標(biāo)系，稱為載體坐標(biāo)系b。與之相對應(yīng)的絕對坐標(biāo)系稱為導(dǎo)航坐標(biāo)系n(本文選用北、東、地的地理坐標(biāo)系)。載體坐標(biāo)系和導(dǎo)航坐標(biāo)系之間的夾角就是描述載體航向的姿態(tài)角。γ、θ和ψ分別為載體的橫滾角、俯仰角和航向角，如圖1所示［1］。

圖1 載體坐標(biāo)系和導(dǎo)航坐標(biāo)系以及歐拉角

1.2 姿態(tài)的四元數(shù)解算法

導(dǎo)航系統(tǒng)中，載體坐標(biāo)系相對于導(dǎo)航坐標(biāo)系的轉(zhuǎn)動則可以運(yùn)用轉(zhuǎn)動四元數(shù)來表示:

四元數(shù)微分方程:

2 多傳感器的數(shù)據(jù)融合

陀螺儀在工作時易受到載體震動的干擾而影響測量精度，并在積分過程中產(chǎn)生較大的累積誤差。為獲取更加準(zhǔn)確的姿態(tài)，需要用加速度計(jì)和磁強(qiáng)計(jì)的測量數(shù)據(jù)對陀螺儀進(jìn)行補(bǔ)償。通常用加速度計(jì)數(shù)據(jù)去補(bǔ)償俯仰角和橫滾角，磁強(qiáng)計(jì)數(shù)據(jù)補(bǔ)償航向角。但加速度計(jì)無法區(qū)分重力加速度和運(yùn)動加速度，磁強(qiáng)計(jì)容易受到周圍磁場的干擾，且這類誤差是隨機(jī)的，無法預(yù)先消除。因此本文先用加速度計(jì)和磁強(qiáng)計(jì)所測的數(shù)據(jù)，采用共軛梯度法對姿態(tài)四元數(shù)進(jìn)行尋優(yōu)處理。

2.1 基于共軛梯度法的四元數(shù)最優(yōu)估計(jì)

加速度計(jì)和磁強(qiáng)計(jì)的三軸分別沿著載體坐標(biāo)系以X，Y，Z的三軸方向安裝，磁強(qiáng)計(jì)在載體坐標(biāo)系三軸上的分量分別為mbx，mby，mbz，加速度計(jì)在載體坐標(biāo)系三軸上的分量分別abx，aby，abz。理想情況下，重力場的參考方向?yàn)閒g=［00g］T;地磁場的參考量為H=［Hn0Hd］］T。其中g(shù)是重力加速度，Hn和Hd分別是地磁場在北東地坐標(biāo)系中北向分量和垂直分量。將fg和H補(bǔ)零，并單位化可得

理想的均勻重力場和地磁場的參考方向可通過載體導(dǎo)航坐標(biāo)系下四元數(shù)轉(zhuǎn)換到載體坐標(biāo)系下，表示為:

其中Q*=q0－q1i－q2j－q3k，為Q的共軛四元數(shù)。

將式(6)和式(7)所得數(shù)據(jù)與加速度計(jì)與磁強(qiáng)計(jì)在載體坐標(biāo)系下的測量數(shù)據(jù)相減便可得到測量誤差。定義誤差函數(shù)為:

則四元數(shù)的優(yōu)化問題可轉(zhuǎn)換為目標(biāo)函數(shù)f(q)= e(q)Te(q)求最小值問題。

根據(jù)共軛梯度法，可按照式(9)更新載體姿態(tài)的四元數(shù)。

其中，λk為最優(yōu)步長，dk為搜索方向。根據(jù)FR共軛梯度法，兩者的計(jì)算如下［13］:

定義:▽f=JT(q)·e(q)

式中，JT(q)為e(q)的雅可比矩陣。

每次更新一個新的姿態(tài)后都需要迭代數(shù)次以得到最優(yōu)解，實(shí)際應(yīng)用中每次釆樣只迭代一次也可以。為了跟下文的互補(bǔ)濾波相結(jié)合，本文每次采樣只迭代一次。

2.2 互補(bǔ)濾波

陀螺儀短時間內(nèi)可以提供高動態(tài)的姿態(tài)數(shù)據(jù)，但因漂移的影響，長時間后精度會降低。加速度計(jì)和磁強(qiáng)計(jì)測量誤差不會隨時間累計(jì)，但其短期內(nèi)的精度差。利用兩者在頻域上的互補(bǔ)特性，采用互補(bǔ)濾波器對二者進(jìn)行數(shù)據(jù)融合可提高姿態(tài)測量的精度?；パa(bǔ)濾波的公式為:

其中，r為加權(quán)系數(shù)，取值為0～1之間，本文取經(jīng)驗(yàn)值0.02;Qam(k+1)為通過共軛梯度法尋優(yōu)得到的四元數(shù);Qg(k+1)為由陀螺儀測量數(shù)據(jù)更新的四元數(shù)。

將式(4)、式(9)代入式(10)并簡化，可得:

圖2給出了本文姿態(tài)解算的全過程。利用陀螺儀輸出數(shù)據(jù)，根據(jù)四元數(shù)微分方程得到˙Qg(k);將加速度計(jì)和磁強(qiáng)計(jì)的輸出數(shù)據(jù)，與通過四元數(shù)估計(jì)值變換得到的相應(yīng)數(shù)據(jù)相減，求差值e(k)，并建立誤差函數(shù)f(q)，再利用共軛梯度法求解四元數(shù)更新的步長和梯度方向λkdk;最后根據(jù)互補(bǔ)濾波計(jì)算出Q(k+1)估計(jì)值。

圖2 共軛梯度法和互補(bǔ)濾波相結(jié)合的原理圖

3 實(shí)驗(yàn)測試與結(jié)果分析

3.1 測試平臺

實(shí)驗(yàn)測試平臺以STM32F103C8為CPU，傳感器選用MPU－6050模塊(整合了三軸加速度計(jì)和陀螺儀)，并通過I2C協(xié)議與CPU通訊。處理器完成姿態(tài)解算后，通過SPI將數(shù)據(jù)經(jīng)2.4 GHz無線數(shù)據(jù)傳輸模塊發(fā)送到上位機(jī)。上位機(jī)采用C#編寫顯示界面，可顯示各傳感器的測量數(shù)據(jù)和姿態(tài)歐拉角的數(shù)據(jù)和波形，并可將數(shù)據(jù)導(dǎo)到EXCELL中，進(jìn)行進(jìn)一步分析。系統(tǒng)的硬件結(jié)構(gòu)和顯示界面如圖3所示。系統(tǒng)以100 Hz頻率對傳感器進(jìn)行采樣，可存儲500個點(diǎn)的數(shù)據(jù)。因暫未采用磁強(qiáng)計(jì)，故只對俯仰角和橫滾角進(jìn)行了靜態(tài)測試、水平滑動和動態(tài)測試。

圖3 系統(tǒng)的硬件電路和顯示界面

3.2 測試結(jié)果與分析

3.2.1 靜態(tài)測試

將姿態(tài)測量單元置于水平臺面上，理想的俯仰角應(yīng)該為0°。當(dāng)姿態(tài)檢測單元處于靜止時，單獨(dú)使用加速度計(jì)時，如圖4所示，輸出的俯仰角信號帶有一定的干擾噪聲，最大誤差為0.23°。單獨(dú)使用陀螺儀，得到的俯仰角在5 s內(nèi)角度值從9.0°累計(jì)到11.2°。使用本文所設(shè)計(jì)濾波方法，對陀螺儀和加速度計(jì)的數(shù)據(jù)進(jìn)行融合后，靜態(tài)誤差可控制在0.023°以內(nèi)。

圖4 靜態(tài)時俯仰角估計(jì)

圖5 有運(yùn)動加速度時俯仰角和橫滾角的估計(jì)

3.2.2 水平滑動測試

加速計(jì)最大缺點(diǎn)是無法區(qū)分運(yùn)動加速度和重力加速度，難以單獨(dú)使用。本文將姿態(tài)測量單元放在水平桌面上，作快速滑動。理論上，俯仰角和橫滾角均應(yīng)該為0°。但直接由加速度計(jì)測量值經(jīng)過解算，得到的俯仰角誤差達(dá)到了22.3°，且運(yùn)動加速度越大，該誤差越大，如圖5所示。經(jīng)過本文設(shè)計(jì)的濾波器，將陀螺儀和加速度計(jì)的數(shù)據(jù)進(jìn)行融合后，這個誤差被減小到2°之內(nèi)，橫滾角誤差在2.6°之內(nèi)?？梢?，本文濾波方法對運(yùn)動加速度有較好濾除作用。

3.2.3 動態(tài)測試

動態(tài)測試又分姿態(tài)變化較慢和姿態(tài)變化較快時兩種情況。圖6分別為姿態(tài)變換較慢時，采用卡爾曼濾波、梯度下降法以及本文濾波方法得到的估計(jì)值與AHRS(Attitude and Heading Reference System)得到的估計(jì)值對比效果。AHRS系統(tǒng)采用AHRS3000成品，該成品基于三軸陀螺儀、三軸加速度計(jì)和三軸磁力計(jì)，并采用自適應(yīng)Kalman數(shù)據(jù)融合算法。

圖7分別表示了對應(yīng)于圖6中各個濾波方法的估計(jì)誤差。表1給出了各種濾波算法誤差絕對值的均值、最大值和最小值?？傻?，本文所提出的濾波算法性能優(yōu)于卡爾曼濾波和梯度下降法。

表1 姿態(tài)變換較慢時俯仰角估計(jì)誤差表

為進(jìn)一步驗(yàn)證本文濾波方法的有效性，還模擬了載體姿態(tài)變化較快時的情況。圖8分別為采用卡爾曼濾波、梯度下降法和本文設(shè)計(jì)的共軛梯度法得到的俯仰角估計(jì)值和用加速度計(jì)得到實(shí)際測量值比較結(jié)果?？梢钥闯觯藨B(tài)劇烈變化的時候，卡爾曼濾波有明顯的滯后，1 s左右才會跟蹤上，且有穩(wěn)態(tài)誤差。梯度下降法有輕度滯后，200 ms跟蹤上。本文設(shè)計(jì)的共軛梯度法幾乎沒有滯后，很好的跟蹤了實(shí)際姿態(tài)的變化。說明了共軛梯度法比梯度下降法具有更快的收斂速度。

圖6 姿態(tài)變化較慢時的俯仰角估計(jì)

圖7 姿態(tài)變化較慢時的俯仰角估計(jì)誤差

圖8 姿態(tài)變化較快時的俯仰角估計(jì)

4 結(jié)論

本文提出了共軛梯度法和互補(bǔ)濾波相結(jié)合的方式，將多傳感器的數(shù)據(jù)進(jìn)行了融合，以提高姿態(tài)的解算精度。實(shí)驗(yàn)測試表明，這種融合策略，可以使姿態(tài)檢測系統(tǒng)具有較好的靜態(tài)特性和動態(tài)特性。尤其姿態(tài)變化劇烈的時候，本文提出的濾波方法性能明顯優(yōu)于卡爾曼濾波和梯度下降法濾波。如果提高一個解算周期內(nèi)共軛梯度法的迭代次數(shù)，姿態(tài)的估計(jì)精度還可進(jìn)一步提高，是后續(xù)研究的重點(diǎn)之一。

［1］丁軍.基于微慣性傳感器的姿態(tài)算法研究［D］.上海:上海交通大學(xué)，2013.

［2］李貴珍.基于對偶四元數(shù)的捷聯(lián)慣導(dǎo)算法研究［D］.太原:中北大學(xué)，2012.

［3］葉锃鋒，馮恩信.基于四元數(shù)和卡爾曼濾波的兩輪車姿態(tài)穩(wěn)定方法［J］.傳感技術(shù)學(xué)報(bào)，2012，25(4):524－528.

［4］苑艷華，李四海，南江.基于卡爾曼濾波器的航姿系統(tǒng)測姿算法研究［J］.傳感技術(shù)學(xué)報(bào)，2011，24(12):1723－1727.

［5］Jung Keun Lee，Edward J Park，Stephen N.Robinovitch.Estimation of Attitude and External Acceleration Using Inertial Sensor Measurement During Various Dynamic Conditions［J］.IEEE Transactions on Instrumentation and Measurement，2012，61(8):2262－2273.

［6］Ruan Xiaogang，Song Keke.An Adaptive Extended Kalman Filter for Attitude Estimation of Self-Balancing Two-Wheeled Robot［C］//International Conference on Electric Information and Control Enginee(ICEICE)，2011:4760－4763.

［7］Lou Lu，Xu Xin.An Approach to Improving Attitude Estimation U-sing Sensor Fusion for Robot Navigation［C］//Procedia Engineering，2011，15:5601－5605.

［8］朱豐超，姚敏立，賈維敏.基于低成本陀螺和傾角儀的姿態(tài)估計(jì)［J］.宇航學(xué)報(bào)，2011，32(8):1728－1732.

［9］Zhao Lin，Nie Qi，Guo Qiufen.Unscented Kalman Filtering for SINS Attitude Estimation［C］//IEEE International Conference on Control and Automation，2007:228－232.

［10］Pan Yue.Attitude Estimation of Miniature Unmanned Helicopter Using Unscented Kalman Filter［C］//International Conference on Transportation，Mechanical，and Electrical Engineering(TMEE).2011:1548－1551.

［11］Tang Xiaojun，Wei Jianli，Chen Kai.Square-Root Adaptive Cubature Kalman Filter with Application to Spacecraft Attitude Estimation［C］//International Conference on Information Fusion.2012: 1406－1412.

［12］Madgwick S.An Efficient Orientation Filter for Inertial and Inertial/Magnetic Sensor Arrays［R］.Report x-io and University of Bristol(UK)，2010.

［13］余剛.無約束優(yōu)化問題共軛梯度法的研究［D］.北京:北京郵電大學(xué)，2007.

孫金秋(1979－)，女，博士生，講師，主要從事機(jī)器人以及先進(jìn)控制理論應(yīng)用研究工作，523106551@qq.com;

游有鵬(1960－)，男，博導(dǎo)，教授，主要從事機(jī)電一體化控制，機(jī)器人以及數(shù)控技術(shù)等研究工作，youypeng@163.com。

基于共軛梯度法和互補(bǔ)濾波相結(jié)合的姿態(tài)解算算法*

孫金秋1，2*，游有鵬1，傅忠云2
(1.南京航空航天大學(xué)機(jī)電學(xué)院，南京210016;2.南京航空航天大學(xué)金城學(xué)院，南京211156)

為了提高姿態(tài)解算精度，提出了一種基于共軛梯度和互補(bǔ)濾波相結(jié)合的多傳感器數(shù)據(jù)融合策略。系統(tǒng)采用四元數(shù)方法進(jìn)行姿態(tài)解算。利用加速度計(jì)和磁強(qiáng)計(jì)的輸出數(shù)據(jù)，通過共軛梯度方法對姿態(tài)四元數(shù)進(jìn)行尋優(yōu)估計(jì)，再將其和利用陀螺儀輸出數(shù)據(jù)更新的四元數(shù)進(jìn)行互補(bǔ)濾波，解算出姿態(tài)角。實(shí)驗(yàn)測試表明，這種融合策略使姿態(tài)檢測系統(tǒng)靜態(tài)性能和和動態(tài)性能均有所提高，尤其在姿態(tài)劇烈變化時，其性能明顯優(yōu)于卡爾曼濾波和梯度下降法。

傳感器;姿態(tài)估計(jì);共軛梯度法;互補(bǔ)濾波;四元數(shù)

TP391.9

A

1004－1699(2014)04－0524－05

2014－01－20修改日期:2014－04－04

C:6330

10.3969/j.issn.1004－1699.2014.04.019

項(xiàng)目來源:國家自然科學(xué)基金項(xiàng)目(51175263);基于9-SPS冗余并聯(lián)機(jī)構(gòu)的六維加速度傳感器研究項(xiàng)目