劉玉宏

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

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

劉玉宏

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

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

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

1 二輪自平衡車

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

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

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

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

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

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

圖2

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

2 車體平衡控制

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

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

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

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

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

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

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

2.2 陀螺儀累積誤差消除

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

其中：

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

圖5 控制框圖

把F1和F2相加，然后取平均，如式2，中點(diǎn)斜率會有較大的權(quán)值，也就是階數(shù)越高，就會有更多的中點(diǎn)斜率取平均，就會獲得更大的權(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ù)陀螺儀輸出的角速度控制伺服電機(jī)的方向和能量輸出來達(dá)到車體的直立平衡。控制框圖如圖5所示。

其中I的系數(shù)為0，即沒有采用I部分。P的系數(shù)直接采用伺服電機(jī)的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 }

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

3 結(jié)論

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

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

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

[3] 郜圓圓等.兩輪自平衡機(jī)器人慣性傳感器濾波問題的研究[J].傳感器技術(shù)學(xué)報(bào),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

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