陳建翔，萬子敬，王向軍

（天津大學(xué)微光機(jī)電教育部重點(diǎn)實(shí)驗(yàn)室，天津300072）

基于四元數(shù)的低成本姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)

陳建翔，萬子敬，王向軍*

（天津大學(xué)微光機(jī)電教育部重點(diǎn)實(shí)驗(yàn)室，天津300072）

針對(duì)運(yùn)動(dòng)載體姿態(tài)測(cè)量的需求，以MEMS慣性測(cè)量器件MPU6050為核心，設(shè)計(jì)了一種低成本姿態(tài)測(cè)量系統(tǒng)。系統(tǒng)采用四元數(shù)作為姿態(tài)解算的基礎(chǔ)，融合陀螺儀和加速度計(jì)數(shù)據(jù)以修正陀螺儀累積誤差引起的姿態(tài)漂移。為了降低MCU負(fù)擔(dān)提高系統(tǒng)實(shí)時(shí)性，數(shù)據(jù)融合采用運(yùn)算量較小的梯度下降和互補(bǔ)濾波相結(jié)合的方法。實(shí)驗(yàn)結(jié)果表面：此測(cè)量系統(tǒng)實(shí)時(shí)性好，輸出姿態(tài)角穩(wěn)定，且成本低，具有一定的實(shí)用價(jià)值。

MPU6050；梯度下降；互補(bǔ)濾波；姿態(tài)解算

EEACC：2575doi：10.3969/j.issn.1004-1699.2016.05.014

近年來，四旋翼飛機(jī)、自平衡電動(dòng)車、機(jī)器人等項(xiàng)目十分流行。在這些項(xiàng)目中，都需要實(shí)時(shí)進(jìn)行載體或部件的姿態(tài)測(cè)量，以實(shí)現(xiàn)精確控制。常見的姿態(tài)測(cè)量單元，一般的包含三軸陀螺儀、三軸加速度計(jì)和三軸磁強(qiáng)計(jì)［1］。常見的姿態(tài)解算方法有：四元數(shù)法，歐拉角法，方向余弦法，等效旋轉(zhuǎn)矢量法等［2］。其中，四元數(shù)法雖對(duì)有限轉(zhuǎn)動(dòng)引起的不可交換誤差的補(bǔ)償程度不夠，但是能滿足大多數(shù)低速情況下的需求，且四元數(shù)法只涉及四個(gè)線性微分方程，計(jì)算量小、算法簡(jiǎn)單，故在工程中常被采用［3］。

陀螺儀動(dòng)態(tài)性能好，但存在漂移問題，且積分累積誤差的存在，長(zhǎng)期使用時(shí)需要對(duì)其進(jìn)行修正。加速度計(jì)和磁強(qiáng)計(jì)不存在積分誤差的問題，靜態(tài)性能好，但是分別受載體振動(dòng)和外界磁場(chǎng)干擾嚴(yán)重，動(dòng)態(tài)性能很差。融合三者數(shù)據(jù)，即可得到同時(shí)具有良好的動(dòng)態(tài)性能和靜態(tài)性能的姿態(tài)角［4］。

相比傳統(tǒng)傳感器，MEMS傳感器體積小、質(zhì)量輕、功耗低、成本低，MEMS技術(shù)的發(fā)展使得低成本姿態(tài)測(cè)量系統(tǒng)有了更廣泛的應(yīng)用［5］。本文以InvenSense公司生產(chǎn)的MPU6050作為核心，設(shè)計(jì)了一個(gè)低成本的雙軸姿態(tài)測(cè)量系統(tǒng)，以單片機(jī)為數(shù)據(jù)處理器，采用一種基于梯度下降和互補(bǔ)濾波相結(jié)合的計(jì)算量低且效果較好的數(shù)據(jù)融合算法，使用四元數(shù)進(jìn)行姿態(tài)解算。

1　基于四元數(shù)的姿態(tài)計(jì)算方法

假設(shè)導(dǎo)航坐標(biāo)系為N，其中坐標(biāo)軸Xn指正東，Yn指正北，Zn指天，坐標(biāo)系B為與載體相固定的載體坐標(biāo)系，三軸加速度計(jì)和三軸陀螺儀分別固定到載體坐標(biāo)系的X、Y、Z三軸上。

角θ、γ和ψ表示載體相對(duì)于導(dǎo)航坐標(biāo)系N的姿態(tài)，即俯仰角、橫滾角和航向角。由3個(gè)姿態(tài)角可定義導(dǎo)航坐標(biāo)系N到載體坐標(biāo)系B的旋轉(zhuǎn)矩陣為：

定義一個(gè)用來表示姿態(tài)信息的姿態(tài)四元數(shù)Q，其中Q=q0+q1i0+q2j0+q3z0且‖‖Q=1，則式（1）所示的旋轉(zhuǎn)矩陣可用四元數(shù)表示為

根據(jù)式（1）和式（2）可得姿態(tài)角與四元數(shù)之間的關(guān)系，分別表示為：

姿態(tài)四元數(shù)Q的微分方程用矩陣形式可表示為：

其中，在忽略地球自轉(zhuǎn)、位置速率等因素影響下wx、wy、wz為陀螺儀輸出的三軸角速率。給定初始四元數(shù)Q0（即初始姿態(tài)角已知），由上述微分方程求得當(dāng)前時(shí)刻的四元數(shù)Q，代入式（3）進(jìn)而可得當(dāng)前時(shí)刻的姿態(tài)角［6-7］。

2　基于梯度下降和互補(bǔ)濾波的數(shù)據(jù)融合方法

2.1梯度下降法

加速度計(jì)具有良好的靜態(tài)特性，長(zhǎng)時(shí)間使用不會(huì)引入積分誤差，但是加速度計(jì)受載體振動(dòng)和運(yùn)動(dòng)影響大，非靜態(tài)環(huán)境下使用，瞬時(shí)誤差大，本文先使用加速度計(jì)數(shù)據(jù)對(duì)姿態(tài)四元數(shù)進(jìn)行優(yōu)化，得到一個(gè)靜態(tài)特性較好的姿態(tài)四元數(shù)，優(yōu)化過程使用梯度下降法。

梯度下降法，又叫最速下降法，是一種迭代求極值的算法［8-9］，具體實(shí)現(xiàn)是按負(fù)梯度方向搜索尋優(yōu)的過程。

假設(shè)重力加速度垂直向下，則在導(dǎo)航坐標(biāo)系下重力加速度矢量為，其中g(shù)是地表重力加速度的大小。利用旋轉(zhuǎn)四元數(shù)矩陣將g轉(zhuǎn)換到載體坐標(biāo)系，記為a=［ax′，ay′，az′］，其中

載體坐標(biāo)系下實(shí)際測(cè)得的載體加速度矢量為a=［ax，ay，az］，其中ax、ay、az分別為三軸加速度計(jì)測(cè)得的加速度大小。理想情況下，a=a′。實(shí)際上兩者之間存在一個(gè)測(cè)量誤差，根據(jù)此測(cè)量誤差，定義誤差函數(shù)為：

對(duì)e（Q）取2范數(shù)的平方，取為梯度下降法尋優(yōu)目標(biāo)函數(shù)：

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

根據(jù)梯度下降法，姿態(tài)四元數(shù)可按下式進(jìn)行修正［10］：

其中η為搜索步長(zhǎng)。當(dāng)利用四元數(shù)法得到當(dāng)前時(shí)刻的姿態(tài)四元數(shù)后，用上式進(jìn)行數(shù)次迭代得到四元數(shù)最優(yōu)值。為了降低運(yùn)算量，本文實(shí)際程序中只進(jìn)行一次迭代。

2.2互補(bǔ)濾波法

利用三軸加速度計(jì)測(cè)得的數(shù)據(jù)根據(jù)梯度下降法對(duì)姿態(tài)四元數(shù)進(jìn)行修正后，得到優(yōu)化的四元數(shù)記為Qa。由于加速度計(jì)的特性，Qa不會(huì)引入累積誤差，但是非靜止情況下，Qa會(huì)引入較大的瞬時(shí)誤差。相反，根據(jù)陀螺儀測(cè)得的姿態(tài)瞬時(shí)精度高，但是會(huì)隨時(shí)間累積誤差。記陀螺儀測(cè)量數(shù)據(jù)直接解算得出的姿態(tài)四元數(shù)為Qg。利用Qa和Qg在頻域上的互補(bǔ)特性，通過互補(bǔ)濾波方法［11］融合兩者，可得到新姿態(tài)四元數(shù)Q，同時(shí)具有較高的瞬時(shí)精度且不會(huì)引入累積誤差而產(chǎn)生漂移?；パa(bǔ)濾波公式如下：

其中k為濾波系數(shù)，在程序調(diào)試過程中調(diào)整至最佳值。

將Q代入式（3）即可解算得到實(shí)時(shí)姿態(tài)角。

3　系統(tǒng)硬件和程序設(shè)計(jì)

3.1硬件設(shè)計(jì)

傳感器方面，選擇InvenSense公司推出的一款6軸傳感器模塊MPU6050，整合了三軸加速度計(jì)和三軸陀螺儀，其中加速度計(jì)可編程測(cè)量范圍為±2 gn、±4 gn、±8 gn和±16 gn，陀螺儀可編程測(cè)量范圍為±250°/s、±500°/s、±1 000°/s和±2 000°/s。MCU芯片可通過I2C接口與MPU6050進(jìn)行傳感器初始化控制和數(shù)據(jù)交換。

作為一種整合性MEMS器件，它的優(yōu)點(diǎn)是體積小、成本低，而且MPU6050作為全球首例整合性6軸運(yùn)動(dòng)處理組件，相對(duì)于多個(gè)獨(dú)立的加速度計(jì)陀螺儀組合的多組件方案，有效的免除了安裝陀螺儀和加速度計(jì)時(shí)軸間誤差的問題，且大大減少了占用尺寸［12］。并且MPU6050內(nèi)部集成了低通濾波器，可在不增加MCU運(yùn)算量的前提下對(duì)陀螺儀和加速度數(shù)據(jù)進(jìn)行濾波處理，提高原始數(shù)據(jù)質(zhì)量。

MCU方面，考慮到性能和功耗，選用飛思卡爾生產(chǎn)的MK10DN32VFM5單片機(jī)作為系統(tǒng)的控制芯片進(jìn)行傳感器數(shù)據(jù)處理、姿態(tài)四元數(shù)的解算和數(shù)據(jù)的融合。

硬件框圖如圖1所示，MCU將陀螺儀加速度計(jì)數(shù)據(jù)和解算出的姿態(tài)角通過串口傳輸?shù)缴衔粰C(jī)以供使用。

圖1　系統(tǒng)硬件框圖

3.2程序設(shè)計(jì)

陀螺儀與加速度計(jì)的原始數(shù)據(jù)存在誤差，在用于姿態(tài)解算前用滑動(dòng)均值濾波［13］進(jìn)行平滑處理，并對(duì)陀螺儀數(shù)據(jù)進(jìn)行零偏補(bǔ)償。

系統(tǒng)上電后，對(duì)MPU6050和MCU進(jìn)行初始化工作，隨后依次完成數(shù)據(jù)采集、數(shù)據(jù)濾波、原始姿態(tài)四元數(shù)解算、梯度下降和互補(bǔ)濾波數(shù)據(jù)融合、最終姿態(tài)角輸出。其中，其中加速度計(jì)測(cè)量范圍取±2 gn，陀螺儀測(cè)量范圍設(shè)為±2 000°/s。陀螺儀和加速度數(shù)據(jù)以及解算出來的俯仰橫滾姿態(tài)角以串口形式輸出，供上層處理。

其中姿態(tài)四元數(shù)計(jì)算即對(duì)式（4）的姿態(tài)四元數(shù)微分方程求解，本文采用畢卡求解法［14］：

其中

4　實(shí)驗(yàn)結(jié)果分析

4.1靜止實(shí)驗(yàn)分析

將傳感器置于水平桌面上，采集俯仰角（Pitch）并繪制俯仰角隨時(shí)間變化曲線。其中，圖3為由三軸陀螺儀數(shù)據(jù)使用四元數(shù)法直接解算出的俯仰角隨時(shí)間變化曲線，因?yàn)橥勇輧x數(shù)據(jù)積分導(dǎo)致的累積誤差，4 s內(nèi)，俯仰角漂移了約3°。圖4為將加速度計(jì)數(shù)據(jù)與陀螺儀數(shù)據(jù)使用前述融合算法進(jìn)行融合后解算出的俯仰角隨時(shí)間變化曲線，如圖4所示，加速度計(jì)數(shù)據(jù)具有靜態(tài)穩(wěn)定性，不會(huì)隨時(shí)間漂移，修正了陀螺儀數(shù)據(jù)的積分誤差，俯仰角長(zhǎng)期保持穩(wěn)定。

圖3　靜止，未融合

圖4　靜止，融合

靜止條件下，以高精度轉(zhuǎn)臺(tái)的輸出作為俯仰角真值，與傳感器輸出俯仰角進(jìn)行對(duì)比，角度越大，誤差越大，正負(fù)90°靜態(tài)精度可達(dá)2°，正負(fù)30內(nèi)靜態(tài)精度可達(dá)0.3°。部分?jǐn)?shù)據(jù)如表1所示。

表1　靜態(tài)精度測(cè)試

4.2動(dòng)態(tài)實(shí)驗(yàn)分析

如圖5所示，將傳感器固定于三軸轉(zhuǎn)臺(tái)，傳感器俯仰軸與轉(zhuǎn)臺(tái)的俯仰軸平行，轉(zhuǎn)臺(tái)繞俯仰軸以20°/s的速率在正負(fù)25°之間來回?cái)[動(dòng)。圖6為傳感器輸出俯仰角隨時(shí)間變化曲線，如圖，曲線光滑，25°處最大誤差為0.52°，可見此傳感器隨動(dòng)性較好，低速情況下，具有較好的動(dòng)態(tài)性能。

圖5　轉(zhuǎn)臺(tái)實(shí)驗(yàn)

圖6　動(dòng)態(tài)測(cè)試

4.3結(jié)論

綜合靜止實(shí)驗(yàn)和動(dòng)態(tài)實(shí)驗(yàn)分析，本文設(shè)計(jì)的基于MEMS器件MPU6050的低成本姿態(tài)測(cè)量系統(tǒng)具有較好的動(dòng)態(tài)性能和靜態(tài)性能，在低速場(chǎng)合下具有一定的實(shí)用價(jià)值。采用梯度下降和互補(bǔ)濾波相結(jié)合的數(shù)據(jù)融合算法，不僅計(jì)算量小實(shí)時(shí)性高，而且具有良好的性能。由于航向角（RAW）的改變并不會(huì)影響三軸加速度計(jì)的數(shù)據(jù)，即三軸加速度計(jì)數(shù)據(jù)不包含橫滾角信息，所以與三軸陀螺儀數(shù)據(jù)融合后，不能修正RAW角的漂移。如需使用RAW角，需融合磁強(qiáng)計(jì)（電子羅盤）數(shù)據(jù)，對(duì)其進(jìn)行修正。

［1］彭孝東，張鐵民，李繼宇，陳瑜.基于傳感器校正與融合的農(nóng)用小型無人機(jī)姿態(tài)估計(jì)算法［J］.自動(dòng)化學(xué)報(bào)，2015，41（4）：854-860.

［2］劉凱，梁曉庚.基于陀螺儀和磁強(qiáng)計(jì)的姿態(tài)解算方法研究［J］.計(jì)算機(jī)仿真，2014，31（5）：39-42.

［3］丁君.基于微慣性傳感器的姿態(tài)算法研究［D］：［碩士學(xué)位論文］，上海：上海交通大學(xué)，2013.

［4］GebreEgziabher D，Hayward R C，Powell J D.Design of Multi Sen?sor Attitude Determination Systems［J］.IEEE Transactions onAerospace and Electronic System，2004，40（2）：627-649.

［5］趙鶴，王喆垚.基于UKF的MEMS傳感器姿態(tài)測(cè)量系統(tǒng)［J］.傳感技術(shù)學(xué)報(bào)，2011，24（5）：642-646.

［6］秦永元.慣性導(dǎo)航［M］.北京：科學(xué)出版社，2014：288-297.

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

［8］王俊，魯曉天.對(duì)四軸飛行器基于姿態(tài)算法的分析與實(shí)現(xiàn)［J］.河南科技，2015，556（2）：51-53.

［9］Madgwick S.An Efficient Orientation Filter for Inertial and Iner?tial/Magnetic Sensor Arrays［R］.Report x-io and University of Bristol（UK），2010.

［10］吳秋峰.Stiefel流形上的梯度下降法［J］.應(yīng)用數(shù)學(xué)學(xué)報(bào)，2012，35（4）：719-726.

［11］Won M，Kim S S.Design and Control of a Marine Satellite Antenna［J］.Journal of Mechanical Science and Technology，2005，19（1）：473-480.

［12］邱云平，伍寶玉.MPU-6050模塊角度算法處理及在嵌入式中的應(yīng)用［J］.江西科技學(xué)院學(xué)報(bào)，2014，9（2）：26-29.

［13］樊毅，秦會(huì)斌.基于STM32的高精度電子天平［J］機(jī)電工程，2013，30（1）：51-54.

［14］邢質(zhì)皙，王愛民.捷聯(lián)慣導(dǎo)系統(tǒng)姿態(tài)算法比較［J］.艦船電子工程，2009，181（7）：81-84.

陳建翔（1990-），男，浙江金華人，天津大學(xué)精密儀器與光電子工程學(xué)院碩士，研究方向?yàn)樽藨B(tài)測(cè)量，email：jianxiang_chen@ 126.com；

王向軍（1955-），男，黑龍江哈爾濱人，博士，天津大學(xué)精密儀器與光電子工程學(xué)院教授，博士生導(dǎo)師，主要研究方向?yàn)榫軠y(cè)試技術(shù)與儀器、光電探測(cè)與傳感技術(shù)、影像與視覺測(cè)量，xdocuxjw@ vip.163.com。

Design of Low-Cost Attitude Determination System Based on Quaternion

CHEN Jianxiang，WAN Zijing，WANG Xiangjun*
（MOEMS Education Ministry Key Lab，Tianjin University，Tianjin 300072，China）

To satisfy the requirements of attitude determination of moving carrier，a low-cost attitude determination system is designed.The attitude algorithm is based on quaternion representation.To correct drift error caused by cu?mulative error of the gyroscope，it combines gyroscope data and accelerometer data by the algorithm based on gradi?ent descent and complementary filter，which can reduce the load of MCU and improve real timing by costing less computation amounts.The experimental results indicate that the low-cost system can work in real time and output stable attitude.It has practical value of engineering.

MPU6050；gradient descent；complementary filter；attitude algorithm

TP212.9

A

1004-1699（2016）05-0706-05

2015-10-21修改日期：2016-01-18