二輪自平衡車的控制與實現(xiàn)

劉玉宏

（河海大學(xué)常州校區(qū)，江蘇常州，213022）

二輪自平衡車的控制與實現(xiàn)

劉玉宏

（河海大學(xué)常州校區(qū)，江蘇常州，213022）

本文使用了Runge-Kutta算法來消除陀螺儀的累積誤差，并使用PID控制算法來控制二輪自平衡車的車身平衡。文中給出了這兩種算法的數(shù)學(xué)公式、框圖和部分程序，最后驗證了這兩種算法的有效性。

自平衡車；Runge-Kutta算法；PID算法

1 二輪自平衡車

1.1 二輪自平衡車系統(tǒng)模型

二輪自平衡車也可稱為二輪自平衡機器人，最早來源于美國DEKA研制的segway。Segway是以幫助腿部殘疾人士自由行動為目的的，但是人們發(fā)現(xiàn)它在汽車所不能行走的場合大有用武之地，例如機場、車站等地可以幫助人們快速通行，因此對它的研究也就熱起來。

二輪自平衡車的模型是一個高階次、不穩(wěn)定、多變量、非線性、強耦合的系統(tǒng)， 它實際上是一個可以行走的一級倒立擺，它以雙輪差速方式布置，每個輪子由直流電機通過減速器直接驅(qū)動， 以電機軸心線為中心前后轉(zhuǎn)動[1]。如圖1所示。對于兩輪機器人來說, 在靜止?fàn)顟B(tài)下不能穩(wěn)定平衡， 若要其穩(wěn)定必須采用動態(tài)平衡[2]， 機器人的平衡是一個動態(tài)過程，機器人在平衡點附近不停的變化進行調(diào)節(jié)以保持平衡。

圖1 二輪平衡車結(jié)構(gòu)圖

1.2 二輪自平衡車車身結(jié)構(gòu)

為了驗證所設(shè)計的算法正確和有效性，搭建了二輪平衡車，并在其上進行編程驗證。如圖2所示。

圖2

此車身采用lego公司的NXT機器人套件搭建。中間為NXT，兩邊為帶減速機構(gòu)的伺服電機，背后是NXT Gyro 公司的陀螺儀。所設(shè)計算法在NXT上運行并控制兩個伺服電機的輸出；陀螺儀檢測車體Z軸的角速度，并以333HZ的頻率輸出給NXT。

2 車體平衡控制

車身平衡控制的基本思路是：首先由陀螺儀輸出角速度，經(jīng)Runge-Kutta算法積分求出車體的直立平衡的正確位置。然后經(jīng)PID算法控制伺服電機的輸出功率和轉(zhuǎn)動方向來維持車身的平衡。

2.1 車體狀態(tài)分析

圖3 車體平衡狀態(tài)

要使車體能達到直立平衡的狀態(tài)，必需放置車體時讓它基本處于直立平衡狀態(tài)，否則任何控制算法都無能為力。把車體的姿態(tài)分為3種情況：直立平衡、車體前傾和車體后仰。

直立平衡如圖3所示。z是車體的垂直軸，也是陀螺儀的垂直軸，a、b是左右車輪的中心。o是ab車軸的中心，也是垂直軸坐落的中心。

車體前仰和后仰的姿態(tài)如圖4所示。從圖中可以看出當(dāng)兩個輪子向前運動時，z1軸就向后運動，補償偏離原垂直軸的角度，達到平衡狀態(tài)。

圖4 車體非平衡狀態(tài)

2.2 陀螺儀累積誤差消除

陀螺儀輸出的是角速度，其動態(tài)特性好，適合測量快速變化的信號，其數(shù)據(jù)輸出經(jīng)過積分可以得到車體的傾斜角度，但在累積誤差[3]。我們采用Runge-Kutta算法來解決這個問題。

其中：

式1是二階Runge-Kutta算法的公式。

圖5 控制框圖

把F1和F2相加，然后取平均，如式2，中點斜率會有較大的權(quán)值，也就是階數(shù)越高，就會有更多的中點斜率取平均，就會獲得更大的權(quán)值。在程序中給定它的初值為500。y(x0)給定的初值為0。程序如下：

Input : gyro_bias = 600,t_scale = 500

Output : theta

Begin

while(true)

f2 = (readgyrovalue - gyrobias)/gyroscale;

time = get current time;

theta = theta_old + (f1+2*f2)*(time - t_old)/t_scale;

theta_old = theta;

f1 = (readgyrovalue -gyrobias)/gyroscale;

END{ Runge-Kutta }

通過此程序我們就可以得到下述PID算法的輸入角度值。

2.3 車體的PID控制算法

車體的控制算法采用經(jīng)典PID算法，主要是根據(jù)陀螺儀輸出的角速度控制伺服電機的方向和能量輸出來達到車體的直立平衡?？刂瓶驁D如圖5所示。

其中I的系數(shù)為0，即沒有采用I部分。P的系數(shù)直接采用伺服電機的tachometer count，而D的系數(shù)采用連續(xù)兩次tachometer count的差值。

程序如下：

Input : k1=0.0 , k2=50.0 , k3=8.0 , k4=0.0 , x=0.0 , x_ old=0.0;

Output: Power;

Begin{ PIDcontrol }

x = Motor.B. tachometer count ();

x_dot = x - x_old;

x_old = x;

Gyro P= k3*theta;

Gyro D= k4*f1;Motor P= k1*x;

Motor D= k2*x_dot;

Power =Gyro P + Gyro D + Motor P + Motor D;

Motor.A.( Power );

Motor.B.( Power );

END{ PID controll }

從伺服電機的輸出功率數(shù)據(jù)來看，其數(shù)據(jù)不斷在正負(fù)功率之間跳動，導(dǎo)致伺服電機不斷地做正反轉(zhuǎn)動以維持車體直立平衡。

3 結(jié)論

通過Java語言把上述算法編寫成程序，并在NXT運行證明：PID算法和Runge-Kutta算法可以控制二輪平衡車的直立平衡。

[1] 任紅格, 阮曉鋼. 基于Skinner操作條件反射的兩輪機器人自平衡控制[J]. 控制理論與應(yīng)用,2010,27(10):1423-1428

[2] 屠運武, 徐俊艷, 張培仁, 等. 自平衡控制系統(tǒng)的建模與仿真[J]. 系統(tǒng)仿真學(xué)報, 2004, 16(4): 830-841.

[3] 郜圓圓等.兩輪自平衡機器人慣性傳感器濾波問題的研究[J].傳感器技術(shù)學(xué)報,2010,23(5):696-700

The Control and realize of two-wheel self-balanced car

Liu Yuhong
（ Hohai University ChangZhou,Jiangsu Changzhou,213022）

Runge-Kutta algorithm is used to eliminate the accumulated error in this paper，which come from the gyroscope. PID algorithm is utilized to control the balance of two-wheel self-balanced car.The mathematical formula, the diagram and the program are given in this paper.At last,the effectiveness of two algorithms are confirmed.

self-balanced car, Runge-Kutta algorithm, PID algorithm

此項工作得到國家自然科學(xué)基金資助，項目編號分別為 61504038