王超杰，蘇 中，時佳斌，陳 庚

(北京信息科技大學 高動態(tài)導航技術北京市重點實驗室，北京100101)

蛇形機器人互補濾波和四元數(shù)的姿態(tài)解算*

王超杰，蘇 中，時佳斌，陳 庚

(北京信息科技大學 高動態(tài)導航技術北京市重點實驗室，北京100101)

針對蛇形機器人姿態(tài)解算問題，陀螺存在漂移特性，加速度計的測量值包含重力加速度和運動加速度，磁強計易受周圍環(huán)境地磁干擾，并且蛇形機器人采用嵌入式微處理器，需要減少計算量。設計了用互補濾波器來實現(xiàn)慣性傳感器的數(shù)據(jù)融合，用四元數(shù)進行姿態(tài)解算的方法。經(jīng)過實驗驗證表明:采用互補濾波和四元數(shù)進行姿態(tài)解算能有效融合各個慣性傳感器的數(shù)據(jù)，計算量小，能夠滿足蛇形機器人對精度和實時性的要求。

蛇形機器人；姿態(tài)解算；互補濾波器；慣性傳感器；四元數(shù)

0 引 言

蛇形機器人由于具有廣闊的應用前景，使其成為了當前的研究熱點。其中，姿態(tài)檢測是控制機器人在未知環(huán)境空間運動的關鍵問題之一，通過融合各種傳感器的數(shù)據(jù)來獲取機器人的運動姿態(tài)，進而實現(xiàn)對機器人的運動控制與路徑規(guī)劃。姿態(tài)解算的精度和速度將直接影響對蛇形機器人的控制的穩(wěn)定性和可靠性。隨著MEMS技術的發(fā)展與對成本因素的考慮，低成本的捷聯(lián)慣性測量單元越來越多的被應用到移動機器人中,其主要包括陀螺儀、加速度計和磁強計[1]。MEMS陀螺儀具有漂移特性，加速度計的測量值中同時包含了重力加速度和運動加速度，同時磁強計容易受到周圍環(huán)境中外部磁場的干擾。怎樣有效地融合這些慣性傳感器的數(shù)據(jù)，濾除外部干擾，得到高精度的姿態(tài)數(shù)據(jù)，是一項非常具有挑戰(zhàn)性的工作。同時，蛇形機器人采用STM32F103微處理器，不適用于計算量大的姿態(tài)解算算法。

本文設計了用互補濾波器實現(xiàn)慣性傳感器的數(shù)據(jù)融合、用四元數(shù)法進行姿態(tài)解算的方法能夠滿足蛇形機器人對精度和實時性的要求。

1 蛇形機器人姿態(tài)描述

為了描述蛇形機器人的俯仰、偏航、橫滾的姿態(tài)信息，需引入空間三維坐標系?，F(xiàn)選擇北東地作為導航坐標系(n系)，蛇形機器人的縱軸、橫軸和豎軸構成的右手坐標系作為載體坐標系(b系)。

(1)

其中，φ為偏航角(yaw)，θ為俯仰角(pitch)，γ為橫滾角(roll)，蛇形機器人運動過程中以這3個角作為姿態(tài)角。

2 姿態(tài)解算算法原理

姿態(tài)解算常用的算法有歐拉角法、方向余弦法和四元數(shù)法。歐拉角法在求姿態(tài)時存在奇點，不能用于全姿態(tài)的解算；方向余弦法可用于全姿態(tài)解算但計算量大，不能滿足實時性的要求。本文采用四元數(shù)法[3]，其計算量小、無奇點且可以滿足蛇形機器人運動過程中姿態(tài)的實時解算。

1)初始對準

姿態(tài)計算初始，將已知載體初始姿態(tài)角φ0,θ0,γ0代入下式，求出初始時刻的四元數(shù)，完成初始對準

(2)

2)四元數(shù)隨時間的更新與求解

寫成矩陣形式為

(3)

式中ωgx,ωgy,ωgz分別為陀螺儀在載體坐標系下測得的蛇形機器人x,y,z軸的角速度。設置采樣周期T，每隔一段時間讀取陀螺儀的數(shù)據(jù)，通過上式得到更新后的四元數(shù)，進而求得更新后的方向余弦矩陣。然而，MEMS陀螺儀存在溫度漂移，積分運算也有一定的累積誤差，所以，必須采用一定的補償算法，融合加速度計和磁強計的數(shù)據(jù)，進行誤差補償以實現(xiàn)精確的姿態(tài)解算。

考慮到蛇形機器人姿態(tài)解算的實時性和精度要求，四元數(shù)更新采用一階龍格—庫塔法，利用傳感器數(shù)據(jù)融合后得到的角速度ωx,ωy,ωz更新四元數(shù)

(4)

3)姿態(tài)角求解

每個周期更新完四元數(shù)，需要對四元數(shù)做歸一化處理

(5)

(6)

對照公式(1)和公式(6)可得姿態(tài)角

(7)

3 互補濾波器的設計[4]

3.1 設計原理

設計互補濾波器的目的是綜合加速度計、陀螺儀、磁強計各自的頻率響應特性，從頻域角度對3只傳感器的數(shù)據(jù)進行融合，以減小姿態(tài)解算的誤差。陀螺儀通過積分得到姿態(tài)角，能在較短的時間內提供準確的姿態(tài)數(shù)據(jù)，但長期精度較差；加速度計和磁強計的測量誤差不隨時間積累，但是短期內精度較差，動態(tài)響應較慢。利用其頻率上的互補特性，通過互補濾波能很好地結合陀螺儀角速度的動態(tài)性能和加速度計、磁強計的靜態(tài)精度。

設置互補濾波器的傳遞函數(shù)[5]為

G1(s)具有一階低通濾波特性，G2(s)具有一階高通特性,可以構造估計量

因此，通過互補濾波器能夠消除高頻噪聲和低頻誤差的積累，能夠很好地融合各傳感器的數(shù)據(jù)?；パa濾波器的原理框圖如圖1所示。

圖1 互補濾波器原理框圖Fig 1 Principle block diagram of complementary filter

互補濾波器在頻域上具有互補性，設計的濾波器傳遞函數(shù)滿足G1(s)+G2(s)=1，因此，角度的估計值是陀螺儀高通濾波和加速度計、磁強計低通濾波后的數(shù)據(jù)融合。

3.2 基于互補濾波和四元數(shù)的姿態(tài)求解[6,7]

圖2 基于互補濾波器和四元數(shù)的姿態(tài)解算方框圖Fig 2 Block diagram of attitude calculation based on complementary filter and quaternion

(8)

其中，(mx,my,mz)為磁強計測量的地磁場的矢量歸一化后的值，將其轉到導航坐標系中得到(hx,hy,hz)，由于導航坐標系為北東地，在y軸方向地磁場矢量應該為0，對(hx,hy,hz)進行一次校準得導航坐標系下的地磁場矢量(bx,by,bz)。

(9)

(10)

(11)

ω=ωg+δ.

(12)

其中，δ為誤差e通過一個比例積分模塊后生成的對陀螺漂移的補償量，ωg為陀螺儀測量得到的角速度，ω為融合加速度計和磁強計數(shù)據(jù)校正后的角速度，將ω代入公式(4)就可得到隨時間傳遞的準確的四元數(shù)，進而求得姿態(tài)矩陣和姿態(tài)角。

4 實驗驗證

蛇形機器人所用的MEMS慣性器件包括MPU6050和HMC5883，MPU6050集成3軸陀螺儀和3軸加速度計，HMC5883為3軸磁強計。為了驗證本文設計的姿態(tài)解算算法的性能，用日本北陽公司生產(chǎn)的高精度的LPMS—B模塊解算的角度作為參考角度，其姿態(tài)解算精度為動態(tài)情況下小于2°，靜態(tài)情況下小于0.5°。

將蛇形機器人所用的MEMS慣性器件與LPMS—B模塊一同固定在平整的電路板上，調整其基準一致，手動控制電路板模仿蛇形機器人的步態(tài)做各種動作，實驗環(huán)境如圖3所示。通過電腦的兩路USB串口分別接收2個慣導模塊姿態(tài)解算的數(shù)據(jù)，并Matlab繪圖如圖4～圖6所示。

圖3 實驗環(huán)境Fig 3 Experimental environment

圖4 橫滾角Fig 4 Roll angle

圖5 俯仰角Fig 5 Pitch angle

圖6 偏航角Fig 6 Yaw angle

通過實驗可以看出：通過手動控制電路板模擬蛇形機器人做運動時，俯仰角最大誤差約為±2°，橫滾角最大誤差約為±1.5°，偏航角最大誤差約為±3°，數(shù)據(jù)更新率為100 Hz，能夠滿足蛇形機器人對精度和實時性的要求。

5 結 論

本文針對蛇形機器人姿態(tài)解算的難點，設計了用互補濾波器來融合陀螺儀、加速度計和磁強計的測量數(shù)據(jù)，用四元數(shù)法進行姿態(tài)解算。

實驗結果表明：采用互補濾波器進行數(shù)據(jù)融合、四元數(shù)法進行姿態(tài)求解的方法能有效融合陀螺儀、加速度計和磁強計的數(shù)據(jù)，輸出的姿態(tài)角動態(tài)誤差小，計算量小，能夠滿足蛇形機器人在運動過程中對精度和實時性的要求。

[1] 王玲玲,富 立,陳 三.基于低成本IMU的捷聯(lián)航姿系統(tǒng)軟件的設計與實現(xiàn)[J].中國慣性技術學報,2009,17(4):379-382.

[2] 趙 欣,王仕成,楊東方,等.一種改進的高動態(tài)捷聯(lián)慣導解算算法[J].中國慣性技術學報，2011,19(2):163-169.

[3] 梁延德,程 敏,何福本，等.基于互補濾波器的四旋翼飛行器姿態(tài)解算[J].傳感器與微系統(tǒng),2011,30(11):56-61.

[4] 丁 君,趙忠華.AHRS航姿解算中的兩種濾波方法的比較研究[J].計算機仿真,2013,30(9):60-65.

[5] 蘭建軍,譚力弓,樸 亨,等.慣性傳感器和互補濾波器在姿態(tài)估計中的應用[J].制造業(yè)自動化,2013,17:65-67.

[6] 閻世梁,王銀玲,張 華.基于改進互補濾波器的低成本微小飛行器姿態(tài)估計方法[J].計算機應用，2013,33(7):2078-2082.

[7] Euston M,Coote P,Mahony R,et al．A complementary filter for attitude estimation of a fixed-wing UAV[C]∥IEEE/RSJ 2008 International Conference on Intelligent Robots and Systems，Nice，F(xiàn)rance，2008:340-345．

[8] Carminati M,Ferrari G,Sampietro M,et al．Fault detection and isolation enhancement of an aircraft attitude and heading reference system based on MEMS inertial sensors[J].Procedia Chemistry,2009,1(1):509-512.

[9] Brás Sérgio,Rosa Paulo,Silvestre Carlos,et al. Global attitude and gyro bias estimation based on set-valued observers[J].Systems & Control Letters,2013,62(10):937-942.

Attitude calculation of snake-like robot using complementary filtering and quaternion*

WANG Chao-jie, SU Zhong, SHI Jia-bin, CHEN Geng

(Beijing Key Laboratory of High Dynamic Navigation Technology,University of Beijing Information Science & Technology,Beijing 100101,China)

Aiming at problem of attitude calculation of snake-like robot，gyro exists drift characteristics,measured value of accelerometer includes gravity and motion acceleration,magnetometer vulnerable to ambient geographic magnetic interference,and snake-like robot use embedded microcontrollers,which reduce the need of computation amount.Therefore,design inertial sensor data fusion is achieved with complementary filtering and attitude calculation is carried out with quaternion method.Experimental verification show that using complementary filtering and quaternion method can effectively fuse sensor datas,amount of calculation is small, and can meet requirements of precision and real-time for snake-like robot.

snake-like robot; attitude calculation; complementary filter; inertial sensor; quaternion

10.13873/J.1000—9787(2014)10—0137—04

2014—03—12

國家自然科學基金資助項目(61261160497)；北京市科技計劃資助項目(Z121100001612007)；北京市教委科技成果轉化和產(chǎn)業(yè)化項目(PXM2013—014224—000074)

TP 212

A

1000—9787(2014)10—0137—04

王超杰(1988-)，男，山東煙臺人，碩士研究生，主要從事蛇形機器人多元信息融合的研究。