二輪自平衡車的控制與實現

劉玉宏

（河海大學常州校區，江蘇常州，213022）

二輪自平衡車的控制與實現

劉玉宏

（河海大學常州校區，江蘇常州，213022）

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

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

1 二輪自平衡車

1.1 二輪自平衡車系統模型

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

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

圖1 二輪平衡車結構圖

1.2 二輪自平衡車車身結構

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

圖2

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

2 車體平衡控制

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

2.1 車體狀態分析

圖3 車體平衡狀態

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

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

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

圖4 車體非平衡狀態

2.2 陀螺儀累積誤差消除

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

其中：

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

圖5 控制框圖

把F1和F2相加，然后取平均，如式2，中點斜率會有較大的權值，也就是階數越高，就會有更多的中點斜率取平均，就會獲得更大的權值。在程序中給定它的初值為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控制算法

車體的控制算法采用經典PID算法，主要是根據陀螺儀輸出的角速度控制伺服電機的方向和能量輸出來達到車體的直立平衡。控制框圖如圖5所示。

其中I的系數為0，即沒有采用I部分。P的系數直接采用伺服電機的tachometer count，而D的系數采用連續兩次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 }

從伺服電機的輸出功率數據來看，其數據不斷在正負功率之間跳動，導致伺服電機不斷地做正反轉動以維持車體直立平衡。

3 結論

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

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

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

[3] 郜圓圓等.兩輪自平衡機器人慣性傳感器濾波問題的研究[J].傳感器技術學報,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

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