慣性傳感器和互補(bǔ)濾波器在姿態(tài)估計(jì)中的應(yīng)用

蘭建軍，譚力弓，樸 亨，王 鵬

LAN Jian-jun, TAN Li-gong, PIAO Heng, WANG Peng

（東北電力大學(xué) 自動(dòng)化工程學(xué)院，吉林 132012）

0 引言

機(jī)器人的姿態(tài)控制研究對(duì)于揭示飛行器等系統(tǒng)的控制原理和方法具有重要的理論和實(shí)際應(yīng)用意義[1]。姿態(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ī)劃。機(jī)器人的導(dǎo)航和控制是利用各種傳感器（如激光、視覺(jué)等傳感器）數(shù)據(jù)和數(shù)字信號(hào)處理技術(shù)的方法實(shí)現(xiàn)，而測(cè)量數(shù)據(jù)基本都是通過(guò)慣性傳感器來(lái)獲取的，因此慣性測(cè)量方法作為一種首選的方案得到廣泛的應(yīng)用[2,3]。目前慣性傳感器主要包含三軸加速度計(jì)、三軸角速率計(jì)和三軸磁場(chǎng)計(jì)等，通過(guò)融合這些傳感器的數(shù)據(jù)實(shí)現(xiàn)機(jī)器人的姿態(tài)預(yù)估。

加速度計(jì)和陀螺儀是慣性測(cè)量中最常用的傳感器，鑒于陀螺儀和加速度的頻率響應(yīng)特性，單獨(dú)使用獨(dú)立的傳感器進(jìn)行姿態(tài)檢測(cè)，都具有較大的誤差[4]。因此，在實(shí)際應(yīng)用研究中，融合陀螺儀和加速度計(jì)傳感器數(shù)據(jù)進(jìn)行姿態(tài)預(yù)估的方案得到許多研究者的青睞[5,6]。

常用的數(shù)據(jù)融合算法有卡爾曼、互補(bǔ)濾波算法，本次研究在現(xiàn)有互補(bǔ)濾波算法的基礎(chǔ)上，對(duì)互補(bǔ)濾波算法進(jìn)行改進(jìn)，提出一種自適應(yīng)互補(bǔ)濾波算法，算法可實(shí)現(xiàn)互補(bǔ)濾波器參數(shù)自動(dòng)調(diào)節(jié)。解決了噪聲干擾與姿態(tài)最優(yōu)估計(jì)問(wèn)題，并在雙輪直立行走機(jī)器人裝置上進(jìn)行了實(shí)驗(yàn)驗(yàn)證，給出了實(shí)驗(yàn)測(cè)試數(shù)據(jù)。

1 姿態(tài)估計(jì)原理

1.1 姿態(tài)估計(jì)

慣性坐標(biāo)系中把X、Y、Z方向定義成北、東、天方向，簡(jiǎn)稱北東天坐標(biāo)系，繞慣性坐標(biāo)系X、Y、Z三軸旋轉(zhuǎn)的角度分別定義為橫滾角（φ）、俯仰角（θ）和航向角（ψ）。旋轉(zhuǎn)物體的方向通常用物體坐標(biāo)系描述，運(yùn)動(dòng)系統(tǒng)的旋轉(zhuǎn)角度通常以歐拉角的形式描述。兩個(gè)坐標(biāo)系之間的轉(zhuǎn)換關(guān)系如圖1所示。利用慣性坐標(biāo)系三維坐標(biāo)空間Cn和相對(duì)于三維坐標(biāo)空間的3×3轉(zhuǎn)換矩陣來(lái)描述物體的姿態(tài)變化。矩陣的列對(duì)應(yīng)于基準(zhǔn)空間的三軸坐標(biāo) X，Y ，Z ，矩陣的行對(duì)應(yīng)于轉(zhuǎn)移后的坐標(biāo)空間x?，，，二者轉(zhuǎn)換關(guān)系可用通過(guò)式（1）來(lái)描述。

圖1 慣性坐標(biāo)系和物體坐標(biāo)系下的歐拉角構(gòu)建示意圖

1.2 姿態(tài)解算

傳統(tǒng)的姿態(tài)測(cè)控方案通常采用高精度航姿參考系統(tǒng)（AHRS-Attitude and Heading Reference System），其雖然具有較高的測(cè)量精度，但其價(jià)格成本較高。隨著微機(jī)電系統(tǒng)（MEMS）技術(shù)的發(fā)展，出現(xiàn)了許多性價(jià)比較高的中低精度的加速度計(jì)和陀螺儀。如果用加速度計(jì)測(cè)出重力加速度后，就可利用式（1）通過(guò)投影向量來(lái)解算歐拉角，但是該方案需要準(zhǔn)確地進(jìn)行轉(zhuǎn)移矩陣的實(shí)時(shí)更新[7]，計(jì)算和迭代量較大。采用加速度計(jì)進(jìn)行慣性測(cè)量時(shí)，如果忽略其他加速度影響，可直接利用式（2）通過(guò)重力加速度信息計(jì)算姿態(tài)[8]。gx，gy，gz分別為x軸、y軸、z軸方向上的重力加速度分量；θ為仰俯角，φ為橫滾角。

單獨(dú)使用加速度計(jì)雖然也可以實(shí)現(xiàn)姿態(tài)估計(jì)，但是加速度傳感器受動(dòng)態(tài)加速度變化影響較大，一旦受外力干擾或加速度發(fā)生變化時(shí)，姿態(tài)測(cè)量誤差較大，因此采用單一加速度傳感器進(jìn)行物體姿態(tài)檢測(cè)的方案采用較少。陀螺儀以其動(dòng)態(tài)性能好、不受加速度變化影響的優(yōu)點(diǎn)，可以有效解決加速度變化的影響，但是陀螺儀存在累積漂移誤差，并且溫漂現(xiàn)象較為明顯，靜態(tài)性能較差。因此，綜合采用加速度計(jì)和陀螺儀的測(cè)量方法是目前低成本慣性測(cè)量中使用最為廣泛的方法[9]。

2 互補(bǔ)濾波器設(shè)計(jì)

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

互補(bǔ)濾波算法的目的就是綜合加速度計(jì)和陀螺儀各自的頻率響應(yīng)優(yōu)勢(shì)，從頻域角度對(duì)兩個(gè)傳感器數(shù)據(jù)進(jìn)行融合，以減小測(cè)量和估計(jì)的誤差。為了獲取傳感器的頻率響應(yīng)范圍，對(duì)研究采用的陀螺儀和加速度計(jì)分別進(jìn)行了頻率響應(yīng)測(cè)試。通過(guò)圖2的曲線數(shù)據(jù)表明，加速度計(jì)在低頻范圍時(shí)表現(xiàn)出較好的動(dòng)態(tài)響應(yīng)，當(dāng)頻率超過(guò)2.35rad/s時(shí)出現(xiàn)了偏離現(xiàn)象。陀螺儀在低頻范圍時(shí)，測(cè)量數(shù)據(jù)誤差較大，當(dāng)頻率超過(guò)3.48 rad/s時(shí)對(duì)角度跟蹤的質(zhì)量逐漸變好。

圖2 傳感器動(dòng)態(tài)響應(yīng)曲線

盡管兩個(gè)傳感器無(wú)法在所有頻率范圍中都有良好的動(dòng)態(tài)響應(yīng)，但是兩個(gè)傳感器的動(dòng)態(tài)響應(yīng)頻率覆蓋了整個(gè)頻率范圍。選用從頻率域來(lái)處理噪聲的互補(bǔ)濾波算法具有很大的優(yōu)勢(shì)，互補(bǔ)濾波器的基本原理框圖如圖3所示。

圖3 互補(bǔ)濾波器原理框圖

其中，u1，u2分別為傳感器測(cè)量中引入的高頻和低頻噪聲，加速度計(jì)中的高頻噪聲u1用低通濾波器 F1(s)消除，陀螺儀中的低頻噪聲u2用高通濾波器 F2(s)消除。為了充分發(fā)揮二者在各自頻率范圍的動(dòng)態(tài)響應(yīng)優(yōu)勢(shì)，設(shè)計(jì)了如圖4所示的互補(bǔ)濾波觀測(cè)器，兩個(gè)濾波器的傳遞函數(shù)計(jì)算如下。

圖4 互補(bǔ)濾波器

兩個(gè)濾波器在頻域上具有互補(bǔ)性，選用的濾波器傳遞函數(shù)滿足 F1(s)1+F2(s)=1，因此角度估計(jì)值是陀螺儀積分后高通濾波和加速度計(jì)低通濾波后的數(shù)據(jù)融合，互補(bǔ)濾波器設(shè)計(jì)的關(guān)鍵在于高、低通濾波器轉(zhuǎn)接頻率的確定。

2.2 自適應(yīng)互補(bǔ)濾波器

文獻(xiàn)[10]提出一種通過(guò)選定互補(bǔ)濾波器的權(quán)重系數(shù)來(lái)確定互補(bǔ)濾波器的轉(zhuǎn)接頻率的方法，計(jì)算公式如式（5）所示，系數(shù)的確定是依據(jù)時(shí)間常數(shù)和采樣時(shí)間來(lái)選定的，該方法計(jì)算簡(jiǎn)單、計(jì)算量小。

其中，agy為陀螺儀權(quán)重系數(shù)，aac為加速度計(jì)權(quán)重系數(shù)，滿足agy+aac=1。

由于權(quán)重系數(shù)的固定，當(dāng)被測(cè)對(duì)象加速度發(fā)生變化或者有外力干擾的情況時(shí)，估計(jì)誤差較大。因此，自適應(yīng)互補(bǔ)濾波器設(shè)計(jì)的關(guān)鍵問(wèn)題在于根據(jù)測(cè)量對(duì)象的運(yùn)行狀態(tài)，自行選擇合適的權(quán)重系數(shù)進(jìn)行互補(bǔ)濾波，以獲得最佳的姿態(tài)估計(jì)。根據(jù)圖2所示的傳感器動(dòng)態(tài)響應(yīng)曲線，低頻時(shí)加速度計(jì)測(cè)量值更為可靠，因此加速度權(quán)重系數(shù)可以增加；高頻時(shí)，陀螺儀測(cè)量效果好，可以加大陀螺儀權(quán)重系數(shù)。依據(jù)實(shí)驗(yàn)曲線中獲取的關(guān)鍵轉(zhuǎn)接頻率點(diǎn)，可以在兩個(gè)轉(zhuǎn)點(diǎn)頻率范圍內(nèi)，根據(jù)物體實(shí)際轉(zhuǎn)動(dòng)頻率來(lái)自動(dòng)調(diào)整權(quán)重系數(shù)，權(quán)重系數(shù)自適應(yīng)算法如式（6）所示：

n表示采樣更新次序，λ1，2λ都為大于0的常數(shù)，取值和連續(xù)兩次采樣的角度差和采樣時(shí)間有關(guān)，m值為在0～0.92范圍內(nèi)變化的變量，根據(jù)連續(xù)兩次采樣的角度差計(jì)算得出。

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

為了驗(yàn)證方案的可行性，設(shè)計(jì)了雙輪直立行走機(jī)器人控制裝置對(duì)自適應(yīng)互補(bǔ)濾波器進(jìn)行效果驗(yàn)證。慣性傳感器分別選用MMA7361加速度計(jì)（測(cè)量范圍：±6g）和ENC-03（測(cè)量范圍：±300(deg/s)）陀螺儀，采用頻率200Hz。以俯仰角（θ）測(cè)量為例，分別進(jìn)行了靜態(tài)和動(dòng)態(tài)測(cè)試。

靜態(tài)測(cè)試實(shí)驗(yàn)時(shí)，將雙輪直立行走機(jī)器人保持固定的17°傾斜角度靜止于地面，加速度計(jì)、陀螺儀和自適應(yīng)互補(bǔ)濾波器輸出的仰俯角測(cè)量曲線如圖5(a)所示。動(dòng)態(tài)測(cè)試時(shí)，控制機(jī)器人按預(yù)定的17°傾角運(yùn)動(dòng)，測(cè)試曲線如圖5(b)所示。實(shí)驗(yàn)數(shù)據(jù)表明，通過(guò)動(dòng)態(tài)調(diào)整加速度計(jì)和陀螺儀的權(quán)重系數(shù)，能夠獲得良好的互補(bǔ)濾波效果，對(duì)角度跟蹤和預(yù)估準(zhǔn)確。

4 結(jié)論

本文通過(guò)加速度計(jì)和陀螺儀傳感器設(shè)計(jì)了運(yùn)動(dòng)物體姿態(tài)檢測(cè)系統(tǒng)，通過(guò)自適應(yīng)互補(bǔ)濾波器對(duì)加速度計(jì)和陀螺儀數(shù)據(jù)進(jìn)行融合，能夠根據(jù)運(yùn)動(dòng)物體狀態(tài)自適應(yīng)調(diào)整互補(bǔ)濾波器的交接頻率，有效消除加速度變化和外力干擾引起的姿態(tài)預(yù)估誤差。在雙輪直立行走機(jī)器人上分別進(jìn)行的靜態(tài)和動(dòng)態(tài)實(shí)驗(yàn)，實(shí)驗(yàn)數(shù)據(jù)表明，利用自適應(yīng)互補(bǔ)濾波算法可以準(zhǔn)確可靠的獲取機(jī)器人的姿態(tài)信息，系統(tǒng)的姿態(tài)預(yù)估精度在±2°內(nèi)，可實(shí)現(xiàn)較高精度的運(yùn)動(dòng)物體姿態(tài)預(yù)估。

圖5 自適應(yīng)互補(bǔ)濾波器測(cè)試曲線

[1]EI-Hawwary M I,Elshafei A L, Emara H M.Adaptive fuzzy control of the inverted pendulum problem[J].Transactions on Control Systems Technology,2009,14(6)：1135-1144.

[2]解旭輝,劉危,張明亮,等.微慣性測(cè)量組合關(guān)鍵技術(shù)與應(yīng)用.光線精密工程[J].2002,10(2)：154-158.

[3]丁明理,王祁.無(wú)陀螺慣性測(cè)量組合研究現(xiàn)狀概述.中國(guó)慣性技術(shù)學(xué)報(bào)[J].2005,13(4)：83-88.

[4]秦勇,臧希喆,王曉宇,等.基于MEMS慣性傳感器的機(jī)器人姿態(tài)檢測(cè)系統(tǒng)的研究[J].傳感技術(shù)學(xué)報(bào),2007,20(2)：298-301.

[5]趙建偉,阮曉鋼.柔性兩輪直立式自平衡仿人機(jī)器人的建模及控制[J].機(jī)器人,2009,31(2)：179-182.

[6]元震宇.基于姿態(tài)控制的平行輪智能電動(dòng)車[J].機(jī)電一體化,2003,9(6)：38-40.

[7]劉亞,康波.基于MEMS陀螺儀的慣性跟蹤裝置的設(shè)計(jì)及算法實(shí)現(xiàn)[J].電子測(cè)量,2010,(9)：15-17.

[8]史智寧,韓波.徐玉,等.基于重力場(chǎng)自適應(yīng)互補(bǔ)濾波的無(wú)人直升機(jī)水平姿態(tài)估計(jì)方法[J].傳感技術(shù)學(xué)報(bào),2009,22(7)：993-996.

[9]Zhu Rong, Sun Dong, Zhou Zhaoying et al, A Linear Fusion Algorithm for Attitude Determination Using Low Cost MEMS-Based Sensors[J].Journal of the International Measurement Confederation,2007,40(3)：322-328.

[10]張吉昌,程凱,鄭榮兒.單軸雙輪自平衡車姿態(tài)檢測(cè)方案設(shè)計(jì).中國(guó)海洋大學(xué)學(xué)報(bào)[J].2009,30(增刊)：467-470.