王超杰，蘇 中，時(shí)佳斌，陳 庚

(北京信息科技大學(xué) 高動(dòng)態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京100101)

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

王超杰，蘇 中，時(shí)佳斌，陳 庚

(北京信息科技大學(xué) 高動(dòng)態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京100101)

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

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

0 引 言

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

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

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

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

(1)

其中，φ為偏航角(yaw)，θ為俯仰角(pitch)，γ為橫滾角(roll)，蛇形機(jī)器人運(yùn)動(dòng)過(guò)程中以這3個(gè)角作為姿態(tài)角。

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

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

1)初始對(duì)準(zhǔn)

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

(2)

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

寫成矩陣形式為

(3)

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

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

(4)

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

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

(5)

(6)

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

(7)

3 互補(bǔ)濾波器的設(shè)計(jì)[4]

3.1 設(shè)計(jì)原理

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

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

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

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

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

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

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

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

(8)

其中，(mx,my,mz)為磁強(qiáng)計(jì)測(cè)量的地磁場(chǎng)的矢量歸一化后的值，將其轉(zhuǎn)到導(dǎo)航坐標(biāo)系中得到(hx,hy,hz)，由于導(dǎo)航坐標(biāo)系為北東地，在y軸方向地磁場(chǎng)矢量應(yīng)該為0，對(duì)(hx,hy,hz)進(jìn)行一次校準(zhǔn)得導(dǎo)航坐標(biāo)系下的地磁場(chǎng)矢量(bx,by,bz)。

(9)

(10)

(11)

ω=ωg+δ.

(12)

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

4 實(shí)驗(yàn)驗(yàn)證

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

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

圖3 實(shí)驗(yàn)環(huán)境Fig 3 Experimental environment

圖4 橫滾角Fig 4 Roll angle

圖5 俯仰角Fig 5 Pitch angle

圖6 偏航角Fig 6 Yaw angle

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

5 結(jié) 論

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

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

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

[2] 趙 欣,王仕成,楊東方,等.一種改進(jìn)的高動(dòng)態(tài)捷聯(lián)慣導(dǎo)解算算法[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2011,19(2):163-169.

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

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

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

[6] 閻世梁,王銀玲,張 華.基于改進(jìn)互補(bǔ)濾波器的低成本微小飛行器姿態(tài)估計(jì)方法[J].計(jì)算機(jī)應(yīng)用，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

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

TP 212

A

1000—9787(2014)10—0137—04

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