二轮自平衡车的控制与实现

刘玉宏

（河海大学常州校区，江苏常州，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