基于陀螺儀與加速度計(jì)的二輪自平衡控制系統(tǒng)設(shè)計(jì)*

孟 騰，章 政

(武漢科技大學(xué) 信息科學(xué)與工程學(xué)院, 湖北 武漢 430081)

基于陀螺儀與加速度計(jì)的二輪自平衡控制系統(tǒng)設(shè)計(jì)*

孟 騰，章 政

(武漢科技大學(xué) 信息科學(xué)與工程學(xué)院, 湖北 武漢 430081)

為了實(shí)現(xiàn)二輪自平衡系統(tǒng)的穩(wěn)定控制，提高系統(tǒng)姿態(tài)傾角的可靠性，提出了基于陀螺儀與加速度計(jì)的二輪自平衡系統(tǒng)控制方法。建立二輪自平衡系統(tǒng)的簡易動力學(xué)模型，以Lyapunov方法對穩(wěn)定性進(jìn)行分析，得到系統(tǒng)穩(wěn)定控制的條件，采用陀螺儀和加速度計(jì)采集姿態(tài)傾角數(shù)據(jù)，經(jīng)融合濾波后得出高精度姿態(tài)傾角，最后控制二輪自平衡系統(tǒng)電機(jī)實(shí)現(xiàn)穩(wěn)定運(yùn)行。通過二輪自平衡控制系統(tǒng)的硬軟件設(shè)計(jì)，成功驗(yàn)證了該方法的可行性。

二輪自平衡；陀螺儀；加速度計(jì)；融合濾波

0 引 言

二輪自平衡系統(tǒng)本質(zhì)上是一種多變量、強(qiáng)耦合的變階非線性二輪系統(tǒng)，它具有體積小、運(yùn)動靈活、零轉(zhuǎn)彎半徑等特點(diǎn)，在軍用和民用領(lǐng)域有著廣泛的應(yīng)用，近十年來，二輪自平衡系統(tǒng)的研究應(yīng)用成為了一個熱點(diǎn)[1]。二輪自平衡系統(tǒng)是一個欠驅(qū)動系統(tǒng)，其自平衡是一個動態(tài)過程，系統(tǒng)在運(yùn)行時姿態(tài)傾角要通過不斷追逐跟隨平衡點(diǎn)而保持平衡，因此,二輪自平衡系統(tǒng)中如何建立系統(tǒng)動力學(xué)模型和提高姿態(tài)傾角的可靠性對于實(shí)現(xiàn)系統(tǒng)的自平衡控制尤為重要。Grasser F等人[2]采用牛頓法推導(dǎo)出了系統(tǒng)的動力學(xué)方程；Salemo A等人[3]根據(jù)倒立擺特性，以系統(tǒng)的旋轉(zhuǎn)角度為變量得到系統(tǒng)的動力學(xué)方程。姿態(tài)傾角的測量大多選用傾角計(jì)、陀螺儀、加速度計(jì)等傳感器。

本文基于Lagrange方法對二輪自平衡系統(tǒng)建立了簡易的動力學(xué)模型，通過對系統(tǒng)穩(wěn)定性的分析提出了基于陀螺儀與加速度計(jì)結(jié)合的控制方法，實(shí)現(xiàn)了系統(tǒng)的穩(wěn)定控制，提高了姿態(tài)傾角的可靠性。在此理論研究的基礎(chǔ)上,完成了二輪自平衡控制系的硬軟件設(shè)計(jì)，通過實(shí)驗(yàn)驗(yàn)證了該方法的有效性。

1 二輪自平衡系統(tǒng)動力學(xué)模型

二輪自平衡系統(tǒng)主要是由核心控制器、傳感器、參數(shù)相同的2個輪子組成。2個輪子共軸，分別由各自直流電機(jī)獨(dú)立驅(qū)動，以差速方式完成行駛。

建立動力學(xué)模型得到系統(tǒng)傳遞函數(shù)，通過穩(wěn)定性分析得出系統(tǒng)穩(wěn)定運(yùn)行的條件。系統(tǒng)模型如圖1所示，假設(shè)模型高度為L，質(zhì)量為M，傾斜角度為外力干擾引起系統(tǒng)產(chǎn)生角速度為x(t)。沿著垂直于系統(tǒng)底盤方向進(jìn)行受力分析，可以得到系統(tǒng)姿態(tài)傾角θ(t)與輪子運(yùn)動加速度a(t)以及外力引起角加速度x(t)的運(yùn)動方程。

圖1 二輪系統(tǒng)模型Fig 1 Model of two-wheeled system

采用Lagrange方法[4,5]分析系統(tǒng)運(yùn)動情況，運(yùn)動方程為

(1)

當(dāng)角度θ很小時，運(yùn)動方程簡化為

(2)

系統(tǒng)靜止時,a(t)=0,此時有

(3)

對應(yīng)系統(tǒng)靜止時，其輸入輸出的傳遞函數(shù)為

(4)

(5)

圖2 系統(tǒng)結(jié)構(gòu)框圖Fig 2 Structure block diagram of system

結(jié)合系統(tǒng)動力學(xué)方程可得到控制輪子加速a(t)的條件

a(t)=k1θ(t)+k2x(t).

(6)

由上面分析可知，二輪自平衡系統(tǒng)實(shí)現(xiàn)穩(wěn)定控制的條件如下：

1)能精確測量姿態(tài)傾角的大小和角速度大小;

2)可以控制系統(tǒng)車輪的加速度。

其中,角度與角速度的測量通過陀螺儀、加速度計(jì)完成，系統(tǒng)車輪的加速度由控制直流電機(jī)的轉(zhuǎn)速來實(shí)現(xiàn)。

2 加速度計(jì)與陀螺儀數(shù)據(jù)融合濾波過程

陀螺儀測量物體旋轉(zhuǎn)運(yùn)動所發(fā)生的角度輸出角速度信號，對信號積分可獲得姿態(tài)傾角，但經(jīng)過積分運(yùn)算后會形成累積誤差。加速度計(jì)測量物體線性運(yùn)動所產(chǎn)生的加速度輸出姿態(tài)傾角信號，對信號微分就可以得到傾角速度，但由于系統(tǒng)自身的擺動會產(chǎn)生很大干擾。這2種傳感器的特性說明，單獨(dú)使用其中一種很難保證測量結(jié)果的精確性，因此,將加速度計(jì)和陀螺儀結(jié)合使用。

采用融合濾波的方法實(shí)現(xiàn)2種傳感器的數(shù)據(jù)融合，以克服加速度計(jì)的動態(tài)誤差和陀螺儀的漂移誤差，即以加速度計(jì)獲得的角度信息對陀螺儀輸出信息進(jìn)行校正，通過對比積分所得到的角度與加速度所得到的角度之間的偏差來改變陀螺儀的輸出，從而使積分的角度逐步跟蹤到加速度傳感器所得到的角度。該方法利用加速度計(jì)所獲得的角度信息θg，與陀螺儀積分后的角度θ進(jìn)行比較，將比較的誤差信號經(jīng)過比例系數(shù)1/Tg放大后與陀螺儀輸出的角速度信號疊加然后進(jìn)行積分。對于加速度計(jì)給定的角度θg，經(jīng)過比例、積分環(huán)節(jié)之后產(chǎn)生的角度θ必然最終等于θ。數(shù)據(jù)濾波融合過程如圖3所示。

圖3 數(shù)據(jù)融合濾波過程Fig 3 Process of data fusion filtering

3 二輪自平衡系統(tǒng)的實(shí)現(xiàn)

3.1 系統(tǒng)硬件設(shè)計(jì)

由前面得出的二輪自平衡系統(tǒng)實(shí)現(xiàn)穩(wěn)定控制的條件可知，系統(tǒng)的硬件設(shè)計(jì)主要包括角度與角速度檢測模塊和直流電機(jī)驅(qū)動控制模塊，角度和角速度檢測模塊由加速度計(jì)和陀螺儀組成，直流電機(jī)驅(qū)動控制模塊由驅(qū)動橋芯片和檢測速度的光電編碼器組成。除此之外，系統(tǒng)還有主控制器、電源模塊等。

主控制器選用飛思卡爾MC9S12系列微控制器[6]，如圖4所示。MC9S12是一個16位控制器件，有16位中央處理單元，一個16通道10位A/D轉(zhuǎn)換器和一個8通道PWM。MC9S12豐富的硬件資源和高速運(yùn)算性能，使得系統(tǒng)硬件電路簡單、可靠性高。由主控制器MC9S12完成各模塊采集的數(shù)據(jù)處理，得到系統(tǒng)姿態(tài)傾角，然后控制電機(jī)驅(qū)動模塊實(shí)現(xiàn)系統(tǒng)的姿態(tài)調(diào)整，最終達(dá)到系統(tǒng)的自平衡控制。系統(tǒng)硬件結(jié)構(gòu)圖如圖4所示。

圖4 系統(tǒng)硬件結(jié)構(gòu)圖Fig 4 Structure diagram of system hardware

角度檢測模塊主要有陀螺儀和加速度計(jì)組成。陀螺儀選用ENC—03[7]，由于其動態(tài)輸出信號很小(0.67 mV/(°)/s)，若直接對陀螺儀輸出信號進(jìn)行AD采樣，則達(dá)不到足夠的分辨率，因此，需要加一級放大級電路，如圖5所示。

圖5 陀螺儀檢測電路Fig 5 Detection circuit of gyroscope

加速度計(jì)選用MMA7361[8]，它是一種三軸低gn半導(dǎo)體加速度計(jì)，可以同時輸出3個方向上的加速度模擬信號，其最大輸出靈敏度為800 mV/gn，無需進(jìn)行放大可直接送到單片機(jī)進(jìn)行 A/D 轉(zhuǎn)換，即可得到相應(yīng)軸上的加速度值。其電路圖如圖6所示。

圖6 加速度計(jì)檢測電路Fig 6 Detection circuit of accelerometer

電機(jī)驅(qū)動模塊設(shè)計(jì)為采用International Rectifier公司生產(chǎn)的IR2104S半橋驅(qū)動芯片和IRF3205 MOSFET組成的H橋型直流電機(jī)驅(qū)動電路。用2個IR2104S各自驅(qū)動一個半橋，最后組成全橋，如圖7所示。

圖7 電機(jī)驅(qū)動模塊電路Fig 7 Circuit of motor driving module

3.2 系統(tǒng)軟件設(shè)計(jì)

系統(tǒng)的軟件主要包括主控制器MC9S12初始化[9]，姿態(tài)角數(shù)據(jù)檢測程序、角速度檢測程序，數(shù)據(jù)融合濾波程序、數(shù)字PID控制[10]程序和直流電機(jī)PWM控制程序等部分。主程序的流程框圖如圖8所示。

圖8 主程序流程圖Fig 8 Flow chart of main program

系統(tǒng)啟動后，程序首先進(jìn)入到主控制器MC9S12初始化，包括單片機(jī)的分頻、IO端口的定義、A/D轉(zhuǎn)換的初始化、中斷的定義等。然后系統(tǒng)中斷讀取加速度計(jì)和陀螺儀分別采集到的角度與角速度的值，經(jīng)過A/D轉(zhuǎn)換后由數(shù)據(jù)融合濾波算法得到姿態(tài)傾角和傾角速度的優(yōu)化值，與此同時,同樣采用中斷的方式讀取由光電編碼器采集的直流電機(jī)轉(zhuǎn)速數(shù)據(jù)，隨后將得到的優(yōu)化值與直流電機(jī)轉(zhuǎn)速數(shù)據(jù)通過已得數(shù)學(xué)關(guān)系運(yùn)算得到電機(jī)轉(zhuǎn)速的給定值，最后完成直流電機(jī)的PID控制程序，經(jīng)主控制器集成的PWM模塊輸出控制電機(jī)轉(zhuǎn)動，從而使二輪系統(tǒng)自平衡運(yùn)行，如此循環(huán)。

4 實(shí)驗(yàn)結(jié)果

通過上位機(jī)軟件讀取二輪自平衡系統(tǒng)在各種實(shí)際運(yùn)行過程中姿態(tài)傾角的情況，實(shí)驗(yàn)結(jié)果如圖9所示。

二輪自平衡系統(tǒng)以一定速度正常行駛時，系統(tǒng)的姿態(tài)傾角趨近穩(wěn)定于垂直位置，如圖9(a)所示。當(dāng)系統(tǒng)行駛過程中因需要而改變速度時，姿態(tài)傾角出現(xiàn)一個左右的變化響應(yīng)，最終會向前偏離垂直位置很小角度并穩(wěn)定下來，并且系統(tǒng)速度越快偏離角度越大，如圖9(b)所示，系統(tǒng)2次速度加大時姿態(tài)傾角波形圖。當(dāng)系統(tǒng)行駛過程中遇到因路面不平等因素帶來的干擾時，姿態(tài)傾角同樣會出現(xiàn)一個短暫的變化響應(yīng)并最終穩(wěn)定于一定角度，如圖9(c)所示，系統(tǒng)在行駛過程中先后遇到2次干擾時姿態(tài)傾角波形圖。

圖9 實(shí)驗(yàn)結(jié)果Fig 9 Experimental results

5 結(jié)束語

本文介紹了基于陀螺儀與加速度計(jì)的二輪自平衡控制系統(tǒng)的設(shè)計(jì)方法，在建立二輪自平衡系統(tǒng)的簡易動力學(xué)模型的基礎(chǔ)上，從分析系統(tǒng)穩(wěn)定性的角度上入手，得到了系統(tǒng)自平衡的條件，通過數(shù)據(jù)融合濾波有效地得到了高精度姿態(tài)傾角，最終實(shí)現(xiàn)了二輪自平衡系統(tǒng)的穩(wěn)定運(yùn)行。實(shí)驗(yàn)結(jié)果表明:該方法應(yīng)用在二輪自平衡控制系統(tǒng)中不僅能實(shí)現(xiàn)系統(tǒng)的穩(wěn)定運(yùn)行，并且在不同的運(yùn)行情況下姿態(tài)傾角能穩(wěn)定保持在±5°內(nèi)。

[1] 阮曉鋼,任紅格.消兩輪自平衡機(jī)器人動力學(xué)建模及其平衡控制[J].計(jì)算機(jī)應(yīng)用研究,2009,26(1):99-101.

[2] Grasser F,D` Arrigo A,Colombi S,et al.Joe:A mobile,inverted pendulum[J].IEEE Trans on Industrial Electronics,2002,49(1):107-114.

[3] Salerno A,Angeles J.On the nonlinear controllability of aquasiholonomic mobile robot[C]∥Proc of IEEE International Confe-rence on Robotics and Automation,2003:3379-3384.

[4] 李凡紅.兩輪自平衡小車系統(tǒng)[D].北京:北京交通大學(xué),2010.

[5] Jian H. Sliding-modeling velocity control of mobile-wheeled-inverted-pendulum systems[J].IEEE Transactions on Robotics,2010,26(4):750-758

[6] 吳 曄,張 陽,滕 勤.基于HCS12的嵌入式系統(tǒng)設(shè)計(jì)[M]．北京：北京電子工業(yè)出版社,2010.

[7] 李俊博,朱 濤,鄒艷忠．陀螺儀穩(wěn)定系統(tǒng)參數(shù)測試儀設(shè)計(jì)[J].計(jì)算機(jī)測量與控制,2011,19(2):481-482.

[8] 于 瑋,封維忠,武建軍.基于MEMS加速度傳感器的數(shù)字水平儀[J].儀表技術(shù)與傳感器,2011(1):24-26,48.

[9] Motorola Inc M68HC12 & HCS12 microcontrollers CPU12 refe-rence manual rev.3[Z]. 2005.

[10] Udo K. Elin praxisnahe einstellregel fuer PID-regeler[J]. Automatisier umgsteahmische Praxis,2005,37(5):1-5.

Design of two-wheeled self-balancing control system based on gyroscope and accelerometer*

MENG Teng，ZHANG Zheng

(School of Information Science and Engineering,Wuhan University of Science and Technology,Wuhan 430081,China)

In order to realize stable control of two-wheeled self-balancing system and improve reliability of system attitude inclination angle,a method of two-wheeled self-balancing system control based on gyroscope and accelerometer is presented. Establish brief dynamics model for two-wheeled self-balancing control system,Lyiapunov method is used to analyze stability in order to get stability control condition of system,datas of attitude inclination angle with high precision is acquired by gyroscope and accelerometer after fusion filtering,at last,control motor of two-wheeled self-balancing system to achieve stable operation. Through hardware and software design,it verifies the feasibility of this method successfully.

two-wheeled self-balancing; gyroscope; accelerometer; fusion filtering

10.13873/J.1000—9787(2014)10—0061—04

2014—02—22

冶金裝備及其控制教育部重點(diǎn)實(shí)驗(yàn)室開放基金資助項(xiàng)目(2013B06)；湖北省自然科學(xué)基金資助項(xiàng)目(2013CFC077)

TP 391

A

1000—9787(2014)10—0061—04

孟 騰(1988-)，男，湖北黃岡人，研究生，研究方向?yàn)榭刂评碚撆c控制工程。