基于四元數的低成本姿態測量系統設計

陳建翔，萬子敬，王向軍

（天津大學微光機電教育部重點實驗室，天津300072）

基于四元數的低成本姿態測量系統設計

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

（天津大學微光機電教育部重點實驗室，天津300072）

針對運動載體姿態測量的需求，以MEMS慣性測量器件MPU6050為核心，設計了一種低成本姿態測量系統。系統采用四元數作為姿態解算的基礎，融合陀螺儀和加速度計數據以修正陀螺儀累積誤差引起的姿態漂移。為了降低MCU負擔提高系統實時性，數據融合采用運算量較小的梯度下降和互補濾波相結合的方法。實驗結果表面：此測量系統實時性好，輸出姿態角穩定，且成本低，具有一定的實用價值。

MPU6050；梯度下降；互補濾波；姿態解算

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

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

陀螺儀動態性能好，但存在漂移問題，且積分累積誤差的存在，長期使用時需要對其進行修正。加速度計和磁強計不存在積分誤差的問題，靜態性能好，但是分別受載體振動和外界磁場干擾嚴重，動態性能很差。融合三者數據，即可得到同時具有良好的動態性能和靜態性能的姿態角［4］。

相比傳統傳感器，MEMS傳感器體積小、質量輕、功耗低、成本低，MEMS技術的發展使得低成本姿態測量系統有了更廣泛的應用［5］。本文以InvenSense公司生產的MPU6050作為核心，設計了一個低成本的雙軸姿態測量系統，以單片機為數據處理器，采用一種基于梯度下降和互補濾波相結合的計算量低且效果較好的數據融合算法，使用四元數進行姿態解算。

1　基于四元數的姿態計算方法

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

角θ、γ和ψ表示載體相對于導航坐標系N的姿態，即俯仰角、橫滾角和航向角。由3個姿態角可定義導航坐標系N到載體坐標系B的旋轉矩陣為：

定義一個用來表示姿態信息的姿態四元數Q，其中Q=q0+q1i0+q2j0+q3z0且‖‖Q=1，則式（1）所示的旋轉矩陣可用四元數表示為

根據式（1）和式（2）可得姿態角與四元數之間的關系，分別表示為：

姿態四元數Q的微分方程用矩陣形式可表示為：

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

2　基于梯度下降和互補濾波的數據融合方法

2.1梯度下降法

加速度計具有良好的靜態特性，長時間使用不會引入積分誤差，但是加速度計受載體振動和運動影響大，非靜態環境下使用，瞬時誤差大，本文先使用加速度計數據對姿態四元數進行優化，得到一個靜態特性較好的姿態四元數，優化過程使用梯度下降法。

梯度下降法，又叫最速下降法，是一種迭代求極值的算法［8-9］，具體實現是按負梯度方向搜索尋優的過程。

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

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

對e（Q）取2范數的平方，取為梯度下降法尋優目標函數：

進而對姿態四元數的優化就可以轉換為對f（Q）求最小值。

根據梯度下降法，姿態四元數可按下式進行修正［10］：

其中η為搜索步長。當利用四元數法得到當前時刻的姿態四元數后，用上式進行數次迭代得到四元數最優值。為了降低運算量，本文實際程序中只進行一次迭代。

2.2互補濾波法

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

其中k為濾波系數，在程序調試過程中調整至最佳值。

將Q代入式（3）即可解算得到實時姿態角。

3　系統硬件和程序設計

3.1硬件設計

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

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

MCU方面，考慮到性能和功耗，選用飛思卡爾生產的MK10DN32VFM5單片機作為系統的控制芯片進行傳感器數據處理、姿態四元數的解算和數據的融合。

硬件框圖如圖1所示，MCU將陀螺儀加速度計數據和解算出的姿態角通過串口傳輸到上位機以供使用。

圖1　系統硬件框圖

3.2程序設計

陀螺儀與加速度計的原始數據存在誤差，在用于姿態解算前用滑動均值濾波［13］進行平滑處理，并對陀螺儀數據進行零偏補償。

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

其中姿態四元數計算即對式（4）的姿態四元數微分方程求解，本文采用畢卡求解法［14］：

其中

4　實驗結果分析

4.1靜止實驗分析

將傳感器置于水平桌面上，采集俯仰角（Pitch）并繪制俯仰角隨時間變化曲線。其中，圖3為由三軸陀螺儀數據使用四元數法直接解算出的俯仰角隨時間變化曲線，因為陀螺儀數據積分導致的累積誤差，4 s內，俯仰角漂移了約3°。圖4為將加速度計數據與陀螺儀數據使用前述融合算法進行融合后解算出的俯仰角隨時間變化曲線，如圖4所示，加速度計數據具有靜態穩定性，不會隨時間漂移，修正了陀螺儀數據的積分誤差，俯仰角長期保持穩定。

圖3　靜止，未融合

圖4　靜止，融合

靜止條件下，以高精度轉臺的輸出作為俯仰角真值，與傳感器輸出俯仰角進行對比，角度越大，誤差越大，正負90°靜態精度可達2°，正負30內靜態精度可達0.3°。部分數據如表1所示。

表1　靜態精度測試

4.2動態實驗分析

如圖5所示，將傳感器固定于三軸轉臺，傳感器俯仰軸與轉臺的俯仰軸平行，轉臺繞俯仰軸以20°/s的速率在正負25°之間來回擺動。圖6為傳感器輸出俯仰角隨時間變化曲線，如圖，曲線光滑，25°處最大誤差為0.52°，可見此傳感器隨動性較好，低速情況下，具有較好的動態性能。

圖5　轉臺實驗

圖6　動態測試

4.3結論

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

［1］彭孝東，張鐵民，李繼宇，陳瑜.基于傳感器校正與融合的農用小型無人機姿態估計算法［J］.自動化學報，2015，41（4）：854-860.

［2］劉凱，梁曉庚.基于陀螺儀和磁強計的姿態解算方法研究［J］.計算機仿真，2014，31（5）：39-42.

［3］丁君.基于微慣性傳感器的姿態算法研究［D］：［碩士學位論文］，上海：上海交通大學，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傳感器姿態測量系統［J］.傳感技術學報，2011，24（5）：642-646.

［6］秦永元.慣性導航［M］.北京：科學出版社，2014：288-297.

［7］葉锃鋒，馮恩信.基于四元數和卡爾曼濾波的兩輪車姿態穩定方法［J］.傳感技術學報2012，25（4）：524-528.

［8］王俊，魯曉天.對四軸飛行器基于姿態算法的分析與實現［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］.應用數學學報，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模塊角度算法處理及在嵌入式中的應用［J］.江西科技學院學報，2014，9（2）：26-29.

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

［14］邢質皙，王愛民.捷聯慣導系統姿態算法比較［J］.艦船電子工程，2009，181（7）：81-84.

陳建翔（1990-），男，浙江金華人，天津大學精密儀器與光電子工程學院碩士，研究方向為姿態測量，email：jianxiang_chen@ 126.com；

王向軍（1955-），男，黑龍江哈爾濱人，博士，天津大學精密儀器與光電子工程學院教授，博士生導師，主要研究方向為精密測試技術與儀器、光電探測與傳感技術、影像與視覺測量，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