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

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

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

0 引言

機器人的姿態(tài)控制研究對于揭示飛行器等系統(tǒng)的控制原理和方法具有重要的理論和實際應(yīng)用意義[1]。姿態(tài)檢測是控制機器人在未知環(huán)境空間運動的關(guān)鍵問題，通過融合各種傳感器的數(shù)據(jù)來獲取機器人的運動姿態(tài)，進而實現(xiàn)對機器人的運動路徑規(guī)劃。機器人的導(dǎo)航和控制是利用各種傳感器（如激光、視覺等傳感器）數(shù)據(jù)和數(shù)字信號處理技術(shù)的方法實現(xiàn)，而測量數(shù)據(jù)基本都是通過慣性傳感器來獲取的，因此慣性測量方法作為一種首選的方案得到廣泛的應(yīng)用[2,3]。目前慣性傳感器主要包含三軸加速度計、三軸角速率計和三軸磁場計等，通過融合這些傳感器的數(shù)據(jù)實現(xiàn)機器人的姿態(tài)預(yù)估。

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

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

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

1.1 姿態(tài)估計

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

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

1.2 姿態(tài)解算

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

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

2 互補濾波器設(shè)計

2.1 設(shè)計原理

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

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

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

圖3 互補濾波器原理框圖

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

圖4 互補濾波器

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

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

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

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

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

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

3 實驗與結(jié)果分析

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

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

4 結(jié)論

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

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

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

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

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

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

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

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

[8]史智寧,韓波.徐玉,等.基于重力場自適應(yīng)互補濾波的無人直升機水平姿態(tài)估計方法[J].傳感技術(shù)學(xué)報,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)檢測方案設(shè)計.中國海洋大學(xué)學(xué)報[J].2009,30(增刊)：467-470.