基于自適應(yīng)互補濾波算法的輪式倒立擺控制*

蘭建軍，李佳璐，劉 穎，張秀宇

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

基于自適應(yīng)互補濾波算法的輪式倒立擺控制*

蘭建軍，李佳璐，劉 穎，張秀宇

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

針對倒立擺控制系統(tǒng)中需要建立復(fù)雜的數(shù)學(xué)模型問題，提出一種利用加速度計和陀螺儀進行倒立擺姿態(tài)檢測和控制的慣性測量方法。為準確獲取輪式倒立擺的姿態(tài)信息，采用自適應(yīng)互補濾波算法對兩種慣性傳感器的測量數(shù)據(jù)進行融合，并將控制方案在設(shè)計的輪式倒立擺裝置上進行了實驗測試。實驗數(shù)據(jù)表明，利用慣性測量和自適應(yīng)互補濾波算法的方案無需建立復(fù)雜的倒立擺數(shù)學(xué)模型，同樣可以準確可靠的獲取倒立擺的姿態(tài)信息，實現(xiàn)倒立擺的姿態(tài)控制。

倒立擺;姿態(tài)控制;慣性測量;自適應(yīng)互補濾波器

0 引言

倒立擺控制系統(tǒng)的研究對于揭示機器人和飛行器等系統(tǒng)的控制原理和方法具有重要的理論和實際應(yīng)用意義[1]。倒立擺系統(tǒng)作為典型的非線性、強耦合、多變量系統(tǒng)，要求控制系統(tǒng)應(yīng)當具有良好的穩(wěn)定性、魯棒性以及隨動性等性能。許多學(xué)者和科技工作者對旋轉(zhuǎn)倒立擺的控制也提出相應(yīng)的控制方案，其中較為典型是根據(jù)牛頓力學(xué)等原理建立倒立擺的數(shù)學(xué)模型后實施不同的控制策略的方案[2-4]。上述方案的一個普遍特點是需要建立復(fù)雜的數(shù)學(xué)模型，并且模型都是在忽略空氣阻力和摩擦力的前提下建立的，同時控制方案的實施都是依托PC機實現(xiàn)，控制成本較高，不適合旋轉(zhuǎn)倒立擺系統(tǒng)的實際應(yīng)用控制。

為了提高旋轉(zhuǎn)倒立擺控制系統(tǒng)的實用性，同時簡化控制難度和降低控制成本。本文采樣慣性測量方案對倒立擺的姿態(tài)進行控制，研究以MC9S12XS128單片機為控制器，采用加速度計和陀螺儀為角度傳感器，在現(xiàn)有互補濾波算法基礎(chǔ)上，設(shè)計自適應(yīng)互補濾波器對加速度計和陀螺儀的數(shù)據(jù)進行融合，并在設(shè)計的輪式倒立擺實驗裝置上對方案進行驗證，給出相關(guān)實驗數(shù)據(jù)。

1 輪式倒立擺系統(tǒng)

1.1 系統(tǒng)概述

輪式倒立擺系統(tǒng)結(jié)構(gòu)圖如圖1所示，系統(tǒng)主要由控制測量單元、電機驅(qū)動單元以及相應(yīng)的機械連接和固定裝置組成。為實現(xiàn)輪式倒立擺的轉(zhuǎn)向，系統(tǒng)采用獨立的雙電機驅(qū)動結(jié)構(gòu)。控制測量單元主要包括主控制器模塊(單片機)、角度傳感器模塊(加速度計和陀螺儀)、測速模塊(500線雙向編碼器)、電機驅(qū)動模塊和電源模塊(鎳鎘電池)。

圖1 輪式倒立擺系統(tǒng)結(jié)構(gòu)

1.2 基于歐拉角的姿態(tài)估計

為實現(xiàn)輪式倒立擺的控制，姿態(tài)檢測是控制輪式倒立擺運動的關(guān)鍵問題。運動物體的姿態(tài)通常采用慣性坐標系中的橫滾角(φ)、俯仰角(θ)和航向角(ψ)來描述[5]。旋轉(zhuǎn)物體的方向通常用物體坐標系描述，運動系統(tǒng)的旋轉(zhuǎn)角度通常以歐拉角的形式描述，因此輪式倒立擺的歐拉角構(gòu)建示意圖如圖2所示。

圖2 輪式倒立擺的歐拉角構(gòu)建示意圖

在輪式倒立擺姿態(tài)檢測中，只需要測量仰俯角和橫滾角即可。如果采用慣性測量方案進行姿態(tài)檢測，在忽略其他加速度影響的前提下，可直接利用式(1)通過重力加速度信息計算姿態(tài)。gx，gy，gz分別為x軸、y軸、z軸方向上的重力加速度分量；θ為仰俯角，φ為橫滾角。因此，使用三軸加速度計獲取x軸、y軸和z軸三軸上的重力加速度分量就可實現(xiàn)輪式倒立擺的姿態(tài)估計與測量。

(1)

1.3 姿態(tài)檢測實施方案

盡管加速度計可以實現(xiàn)姿態(tài)估計，但加速度傳感器受動態(tài)加速度變化影響較大，一旦受外力干擾或加速度發(fā)生變化時，姿態(tài)測量誤差較大。陀螺儀以其動態(tài)性能好、不受加速度變化影響的優(yōu)點，被廣泛應(yīng)用與慣性測量中。但陀螺儀存在累積誤差和溫漂現(xiàn)象，靜態(tài)性能較差。因此，綜合采用加速度計和陀螺儀的測量方案是目前低成本慣性測量中使用較為廣泛的方法[6]，其目的就是綜合加速度計和陀螺儀各自的頻率響應(yīng)優(yōu)勢，采用相應(yīng)的控制算法對兩種傳感器的數(shù)據(jù)進行融合，以獲取較高的測量精度。

2 傳感器數(shù)據(jù)融合

目前對于加速度計和陀螺儀數(shù)據(jù)融合算法使用較多的是卡爾曼濾波和互補濾波兩種算法[7-9]，但是采用卡爾曼濾波方法通常需要創(chuàng)建系統(tǒng)的狀態(tài)方程和測量方程，并且需要根據(jù)采樣值對方程進行實時更新，計算和迭代量較大。圖3給出了本次研究所使用的加速度計和陀螺儀的動態(tài)響應(yīng)曲線，根據(jù)動態(tài)響應(yīng)曲線可知，加速度計低頻響應(yīng)效果較好，陀螺儀高頻特性較好，兩個傳感器的動態(tài)響應(yīng)頻率覆蓋了整個頻率范圍。因此選用從頻率域來處理噪聲的互補濾波算法具有很大的優(yōu)勢，本次研究在現(xiàn)有互補濾波算法的基礎(chǔ)上，對互補濾波算法進行改進，采用自適應(yīng)互補濾波算法對傳感器的數(shù)據(jù)進行融合。

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

2.1 互補濾波原理

互補濾波器的基本原理框圖如圖4所示。加速度計輸出經(jīng)低通濾波器，濾除高頻干擾；陀螺儀輸出經(jīng)高通濾波器，消除低頻干擾，通過互補濾波器對加速度計和陀螺儀的數(shù)據(jù)進行融合。

圖4 互補濾波器原理框圖

其中，u1，u2分別為傳感器測量中引入的高頻和低頻噪聲，加速度計中的高頻噪聲u1用低通濾波器F1(s)消除，陀螺儀中的低頻噪聲u2用高通濾波器F2(s)消除。

為了充分發(fā)揮加速度計和陀螺儀在各自頻率范圍的動態(tài)響應(yīng)優(yōu)勢，依據(jù)圖3所示的傳感器動態(tài)響應(yīng)曲線，設(shè)計了如圖5所示的互補濾波觀測器，兩個濾波器的傳遞函數(shù)如式(2)和式(3)所示。

圖5 互補濾波器

(2)

(3)

兩個濾波器在頻域上具有互補性，選用的濾波器傳遞函數(shù)滿足F1(s)1+F2(s)=1，因此角度估計值是陀螺儀積分后高通濾波和加速度計低通濾波后的數(shù)據(jù)融合，互補濾波器設(shè)計的關(guān)鍵在于高、低通濾波器轉(zhuǎn)接頻率的確定。張吉昌提出一種通過選定固定互補濾波器的權(quán)重系數(shù)來確定互補濾波器的轉(zhuǎn)接頻率的方法[10]，系數(shù)的確定是依據(jù)時間常數(shù)和采樣時間來選定的，該方法計算簡單、計算量小,因此程序?qū)崿F(xiàn)簡單。

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

采用文獻[10]提出的固定權(quán)重系數(shù)方法雖然可以有效的實現(xiàn)輪式倒立擺的姿態(tài)檢測，但是一旦輪式倒立擺加速度發(fā)生變化或者出現(xiàn)外力干擾的情況時，姿態(tài)估計誤差較大。因此，自適應(yīng)互補濾波器設(shè)計的需要解決的問題是如何根據(jù)控制對象的實際運行狀態(tài)，自適應(yīng)的調(diào)整權(quán)重系數(shù)從而實現(xiàn)更好的互補濾波，以獲得姿態(tài)估計的最佳值。

根據(jù)圖3所示的傳感器動態(tài)響應(yīng)曲線可知，低頻時加速度計測量值更為可靠，因此加速度權(quán)重系數(shù)可以占主導(dǎo)部分；高頻時，陀螺儀測量效果好，陀螺儀權(quán)重系數(shù)占主要部分；而在高、低頻轉(zhuǎn)接頻率點附近，可以根據(jù)輪式倒立擺的實際轉(zhuǎn)動頻率情況自動調(diào)整權(quán)重系數(shù)大小。按照上述原理，設(shè)置的權(quán)重系數(shù)自適應(yīng)算法如式(4)所示：

(4)

式中，agy和aac分別表示陀螺儀和加速度計的權(quán)重系數(shù)；angle表示估算的角度；n表示采樣更新次序；λ1，λ2都為大于0的常數(shù)，取值和連續(xù)兩次采樣的角度差和采樣時間有關(guān)；m值為在0-0.92范圍內(nèi)變化的變量，根據(jù)連續(xù)兩次采樣的角度差計算得出。

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

為了驗證方案的可行性，設(shè)計了輪式倒立擺裝置對自適應(yīng)互補濾波器和自適應(yīng)算法進行效果驗證。慣性傳感器分別選用MMA8451Q加速度計(測量范圍：±6g)和ENC-03RC(測量范圍：±300(deg/s))陀螺儀，單片機A/D采樣頻率100Hz。以俯仰角(θ)測量為例，分別進行了靜態(tài)和動態(tài)測試。

靜態(tài)測試實驗時，將輪式倒立擺垂直靜止于地面，加速度計、陀螺儀和自適應(yīng)互補濾波器輸出的仰俯角測量曲線如圖6a所示。動態(tài)測試時，輪式倒立擺按設(shè)定的17°傾角運動，測試曲線如圖6b所示。實驗數(shù)據(jù)表明，采用自適應(yīng)互補濾波算法動態(tài)調(diào)整加速度計和陀螺儀的權(quán)重系數(shù)的方法，能夠獲得良好的互補濾波效果，輪式倒立擺的仰角跟蹤和預(yù)估準確。

4 結(jié)論

本文利用三軸加速度計和陀螺儀為傳感器，采用慣性測量方法對輪式倒立擺的姿態(tài)進行預(yù)估。針對加速度計和陀螺儀的動態(tài)響應(yīng)特性，為保證控制器能根據(jù)運動物體運動狀態(tài)有效消除加速度變化和外力干擾引起的姿態(tài)預(yù)估誤差，在現(xiàn)有互補濾波算法的基礎(chǔ)上，提出一種采用自適應(yīng)互補濾波算法對加速度計和陀螺儀數(shù)據(jù)進行融合的方法，并在設(shè)計的輪式倒立擺裝置上對算法分別進行了靜態(tài)和動態(tài)測試。實驗數(shù)據(jù)表明，利用自適應(yīng)互補濾波算法可以準確可靠的獲取輪式倒立擺的姿態(tài)信息，系統(tǒng)的姿態(tài)預(yù)估誤差在±2°內(nèi)，該方法可應(yīng)用于運動物體姿態(tài)估計精度要求較高的場合。

圖6 自適應(yīng)互補濾波器測試曲線 [參考文獻]

[1] 張建海.單片機控制的二級倒立擺系統(tǒng)的研究[D].天津:河北工業(yè)大學(xué),2005.

[2] 葉曉燕,傅保川,王儉,等.基于模糊控制的旋轉(zhuǎn)倒立擺系統(tǒng)[J].電機與控制學(xué)報,2005,9(5):508-511.

[3] 段旭東,許可.單級旋轉(zhuǎn)倒立擺的建模與控制仿真[J].機器人技術(shù)與應(yīng)用,2002(5):43-45.

[4] 張姝,朱善安.基于Lagrange方程建模的旋轉(zhuǎn)倒立擺的分析與控制[J].工業(yè)控制計算機,2003,16(8):30-32.

[5] Xiaowei Shen, Minli Yao, Weimin Jia et al. Adaptive complementary filter using fuzzy logic and simultaneous perturbation stochastic approximation algorithm [J]. Measurement,2012(45):1257-1265.

[6] 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.

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

[8] 吳紅芳,王志.基于擴展卡爾曼濾波器的航天器姿控飛輪故障診斷[J].航天控制,2012,30(3):34-37.

[9] J.F. Vasconcelos, C.Silvestre, et al. Discrete Time-Varying Attitude Complementary Filter, American Control Conference,2009:4056-4061.

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

(編輯 趙蓉)

Control of Wheeled Inverted Pendulum Based on Adaptive Complementary Filter Algorithm

LAN Jian-jun, LI Jia-lu, LIU Ying, ZHANG Xiu-yu

(School of Automation Engineering ,Northeast Dianli University ,Jilin Jilin 132012, China)

Aiming at problematic complexity of mathematical modeling in inverted pendulum control system, an inertial measurement method is brought forward. To reduce the error of attitude estimation, an adaptive complementary filter algorithm is adopted to fuse the data from inertial sensors, and the control scheme is tested in inverted pendulum equipment. The experimental data show that without complex mathematical model, the inverted pendulum’s attitude information can be obtained faithful with the method of inertial measurement and adaptive complementary algorithm filter.

inverted pendulum, attitude control, inertial measurement, adaptive complementary filter

1001-2265(2014)06-0066-03

10.13462/j.cnki.mmtamt.2014.06.018

2013-10-16

國家自然科學(xué)基金資助項目(61304013)

蘭建軍(1977—)，男，畬族，福建邵武人，東北電力大學(xué)講師，碩士，主要從事智能測控技術(shù)及裝置、無線傳感器網(wǎng)絡(luò)方向的教學(xué)和科研工作，(E-mail)cyzx1@mail.nedu.edu.cn。

TH166;TG65

A