孟 騰，章 政

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

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

孟 騰，章 政

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

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

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

0 引 言

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

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

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

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

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

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

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

(1)

當角度θ很小時，運動方程簡化為

(2)

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

(3)

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

(4)

(5)

圖2 系統(tǒng)結構框圖Fig 2 Structure block diagram of system

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

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

(6)

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

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

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

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

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

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

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

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

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

3.1 系統(tǒng)硬件設計

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

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

圖4 系統(tǒng)硬件結構圖Fig 4 Structure diagram of system hardware

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

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

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

圖6 加速度計檢測電路Fig 6 Detection circuit of accelerometer

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

圖7 電機驅動模塊電路Fig 7 Circuit of motor driving module

3.2 系統(tǒng)軟件設計

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

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

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

4 實驗結果

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

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

圖9 實驗結果Fig 9 Experimental results

5 結束語

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

[1] 阮曉鋼,任紅格.消兩輪自平衡機器人動力學建模及其平衡控制[J].計算機應用研究,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].北京:北京交通大學,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)設計[M]．北京：北京電子工業(yè)出版社,2010.

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

[8] 于 瑋,封維忠,武建軍.基于MEMS加速度傳感器的數(shù)字水平儀[J].儀表技術與傳感器,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

冶金裝備及其控制教育部重點實驗室開放基金資助項目(2013B06)；湖北省自然科學基金資助項目(2013CFC077)

TP 391

A

1000—9787(2014)10—0061—04

孟 騰(1988-)，男，湖北黃岡人，研究生，研究方向為控制理論與控制工程。