蛇形機器人互補濾波和四元數的姿態解算*

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

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

蛇形機器人互補濾波和四元數的姿態解算*

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

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

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

蛇形機器人；姿態解算；互補濾波器；慣性傳感器；四元數

0 引 言

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

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

1 蛇形機器人姿態描述

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

(1)

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

2 姿態解算算法原理

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

1)初始對準

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

(2)

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

寫成矩陣形式為

(3)

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

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

(4)

3)姿態角求解

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

(5)

(6)

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

(7)

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

3.1 設計原理

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

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

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

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

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

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

3.2 基于互補濾波和四元數的姿態求解[6,7]

圖2 基于互補濾波器和四元數的姿態解算方框圖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為陀螺儀測量得到的角速度，ω為融合加速度計和磁強計數據校正后的角速度，將ω代入公式(4)就可得到隨時間傳遞的準確的四元數，進而求得姿態矩陣和姿態角。

4 實驗驗證

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

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

圖3 實驗環境Fig 3 Experimental environment

圖4 橫滾角Fig 4 Roll angle

圖5 俯仰角Fig 5 Pitch angle

圖6 偏航角Fig 6 Yaw angle

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

5 結 論

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

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

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

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

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

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

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

[6] 閻世梁,王銀玲,張 華.基于改進互補濾波器的低成本微小飛行器姿態估計方法[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，France，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)；北京市教委科技成果轉化和產業化項目(PXM2013—014224—000074)

TP 212

A

1000—9787(2014)10—0137—04

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