两轮自平衡小车的控制策略设计

詹长书 邢博坤 朴光宇 巨生龙 苏德新 马璐

摘要：本文在总结和归纳国内外两轮自平衡小车的研究现状后，在使用现有小车的情况下，选用适当的控制器、执行电机和传感器，设计出两轮自平衡小车的驱动电路，实现了两轮小车的硬件控制系统。其工作原理是系统以姿态传感器（陀螺仪、加速度计）来监测车身所处的俯仰状态和状态变化率，通过高速中央处理器计算出适当数据和指令后，驱动电动机产生前进或后退的加速度来达到车体前后平衡的效果。本次实验将陀螺仪与加速度传感器集成在一张板上，对传感器两者所采集的数据进行互补滤波的优化处理，补偿陀螺仪的漂移误差和加速度计的动态误差，得到一个更优的倾角近似值，之后使用PID控制器来进行系统设计，最终实现小车的直立。

关键词：自平衡小车；互补滤波；积分误差；陀螺仪

中图分类号：S776.3

文献标识码：A

文章编号：1001-O05X（2015）04-0073-04

两轮自平衡小车体积小、结构简单、运动灵活，适用于狭小和危险的工作空间，拥有广泛的应用前景。两轮自平衡小车具有多变量、非线性、强耦合、时变、参数不确定性等特性。近年来，国内外多家科研单位和企业以及自平衡代步车的爱好者都对自平衡理论的发展做出了许多有益的贡献，大大推进了自平衡技术的发展，同时也出现了一批有代表性的自平衡机器人和载人代步车作品。国外的机器人起步较早，所研制的样机比较多，理论也比较完善。和国外比，国内的产品尚处于理论研究阶段。小车硬件采用MK60单片机作为主控芯片，陀螺仪和加速度传感器检测车身重力方向的倾斜角度和车身轮轴方向上的旋转加速度。由于两轮自平衡小车存在陀螺仪存在温漂和积分误差，加速度传感器动态响应较慢，而且直立容易受干扰等问题。针对积分误差的问题，本文将着重使用互补滤波算法将陀螺仪和加速度传感器数据融合，得到一个优化的角度近似值。采用双环PID控制策略，控制电机调整小车状态，使小车保持动平衡。

1 系统方案设计及改进

1.1 主控芯片选择

采用MK60单片机作为主控芯片。相对其他单片机，MK60外设丰富，主频高，价格便宜，有专门的软件库，操作简单，调试方便，低功耗。基本型时钟频率为36MHz，增强型系列时钟频率达到72MHz，是同类产品中性能最高的产品。考虑系统的复杂度，主控芯片需要与传感器进行IIc通讯，输出灵活可控制的信号，以及进行大量的数学运算。从性能和价格综合考虑，即用MK60作为本系统的主控芯片，具体型号为MK60DN512。

1.2 姿态检测方案选择

姿态检测采用加速度传感器与陀螺仪传感器。通过融合算法，提取出加速度传感器的静态效果和陀螺仪的动态效果。融合算法比较复杂，优点是能测出准确稳定的倾角，但是由于准确稳定的倾角正是本文要讨论的话题，因此最终选择此方案，即加速度传感器与陀螺仪传感器数据融合测量倾角。并为了简化电路，最终选择了均为nc接口的陀螺仪与加速器一体的ENC -03RC +MMA7361板。

1.3 电机选择

直流有刷电机具有机械特性硬，响应速度快，调速范围宽的特点，满足两轮自平衡小车对灵敏性、快速性等要求。虽然电机的电刷会使电机的寿命缩短，还会引发电磁干扰。但是由于本设计负载较轻，换向器和电刷的损耗较低。小车布置采用多层机械结构，电机驱动电路与其他电路分离，有效降低电磁干扰。因此，使用两个6V带有减速齿轮的直流有刷电机驱动两轮自平衡小车。

2 主要芯片与驱动电路

2.1 加速度传感器的选择

加速度传感器MMA7361检测的是它受到的惯性力。加速度传感器实际上是用MEMS技术检测惯性力造成的微小形变。所以把加速度传感器水平静止放在桌子上，它的Z轴输出的是1g的加速度，因为Z轴方向被重力向下拉出了一个形变。从刚才的分析可以发现重力在不产生加速度的情况下对加速度传感器造成形变，在产生加速度的时候不造成形变，而其他力都做不到。

所以，当系统在三维空间做变速运动时，它的输出就不正确了或者说它的输出不能表明物体的姿态和运动状态。所以说，只靠加速度传感器来估计小车的姿态是不可取的。

2.2 陀螺仪传感器ENC-03RC

陀螺仪ENC-03RC可以测量角速度，具有高动态特性。但是它是一个间接测量器件，它测量的是角度的导数，角速度，显然我们要将角速度对时间积分才能得到角度。

因此综合考虑，加速度计是极易受外部干扰的传感器，但是测量值随时间的变化相对较小。陀螺仪可以积分得到角度关系，动态性能好，受外部干扰小，但测量值随时间变化比较大。它们优缺点互补，结合起来才能准确地测量小车倾角。

2.3 电机驱动电路BTN 7971b

本设计中使用减速直流有刷电机作为两轮自平衡车的驱动电机，电机采用H桥驱动方式，使用脉宽调制方式调节电机两端电压有效值，达到调速的目的。

3 系统软件设计

3.1 互补滤波器的设计

由于加速度计对小车的加速度比较敏感，取瞬时值计算倾角误差比较大；而陀螺仪积分得到的角度不受小车加速度的影响，但是随着时间的增加积分漂移和温度漂移带来的误差比较大。所以这两个传感器正好可以弥补相互的缺点。互补滤波就是在短时间内采用陀螺仪得到的角度作为最优，定时對加速度采样来的角度进行取平均值来校正陀螺仪得到的角度，由图可看出该滤波器的输入有两个数据，一是加速度计测得的角度，二是陀螺仪测得的角速度，两者经过一定的融合运算后得出一个稳定可靠的倾角。以下将介绍数据的融合过程。首先建立一个以主控板为基准的空间坐标系，如图1所示。

R代表电路板的加速度矢量，Rn，（n可以是x，y，z）代表R在各个轴的分量，Ann，（A代表角度an：gle，nn，可以是xz，yz，如Axz）代表R与各个平面的夹角。因此，从加速度计测出来的RxAcc，Ry-Acc，RzAcc（ac代表accelerometer）便可合成矢量Racc，即Racc=[Raccx，Raccy，Raccz]。由Racc的分量即可计算出Axz，Ayz，但如前文讨论的，此时的Axz，Ayz并不稳定可靠。

假设最后计算出的稳定可靠的矢量（实际上是单纯的重力加速度矢量）为Rest=[RxEst，RyEst，RzEst]（Est代表：estimate）。程序中，第一次计算Rest直接取值于加速度计的Racc，即：

Rest（0） =Racc（0）。

此后的Rest（如Rest（1），Rest（2），Rest（3）…Rest（n））则由陀螺仪参与调整。在计算Rest（n）时，可以通过RxEst（n-1）和RzEst（n-1）计算出Axz（n-1），即：

Axz（n-1）= atan2（RxEst（n-1），RzEst（n-1））。

接着由Axz（n-1）和RateAxz（n）可计算出Axz（n），即：

Axz（n）=Axz（n-1）+RateAxz（n）×T，

其中RateAxz（n）为绕y轴的陀螺仪角速度，T为计算Axz（n-1）到计算Axz（n）的时间差值。同样道理可得：

Ayz（n）=Ayz（n-1）+RateAyz（n）×T。

再由以下公式可得：

同理得：

到此，计算出了两个单位为1的向量：Racc（n），Rgyro（n），通过一个加权平均公式便可得到Rest（n），即：

Rest（n）=（RaccXwl+RgyroXw2）/（wl+ w2）。

令w2/wl=wGyro，可得

Rest（n）=（Racc +Rgyro XwGyro）/（1+wGyro）。

即：

RxEst（n）=（RxAcc+ RxGyro×wGyro）/

（1+WGyro）。

RyEst（n）=（RyAcc+ RyGyro×wGyro）/

（1+wGyro）。

RzEst（n）=（RzAcc+ RzGyro×wGyro）/

（1+wGyro）。

而小车的倾角为：

RzEst（n）=（RzAcc+ RzGyro×+wGyro）/

（1+wGyro）。

3.2

PID控制器的设计

PID控制器的输入输出关系为：

PID控制的模拟和数宇控制：模拟电子电路调节器，调节器，所测量的信号与给定值进行比较，然后将致动器之后比较PID电路动作之间的差来改变供给量，以达到调整的目。PID数字由计算机操作时，其结果将被转换成一个模拟输出，以控制致动器。

比例控制系统的动态性能影响：当增加Kp时，会使系统的响应速度增加，当Kp过大，会使系统的振荡频率调整时间变长；当Kp为过小，则系统响应缓慢。确定Kp的选择，以产生输出響应为4：1衰减处理是适当的。在系统稳定，增加的Kp可以减少稳态误差，但它不能消除稳态误差。

积分时间T_i影响系统性能：积分控制通常会影响系统的稳定性。T_i太小，使系统不稳定和冲击更频繁；T_i太大，对系统的影响将减弱；T在适当的时候，系统更加理性的过程的过度性质。积分控制系统有助于消除稳态误差。

微分时间T_D对系统性能的影响：增强的微分作用可以改善动态特性，如减少超调，缩短调整时间，适当增加比例控制，它可以减少稳态误差，提高控制精度，另一方面微分动作将放大系统噪声和降低系统抵制干扰。可以实现鉴别程序错误，或在瞬间，通过检验偏移控制的偏差，得出尽早纠正的措施，有助于提高系统的稳定性。

数字PID算法为用计算机实现，用数值逼近和连续信号离散化实现的PID控制规律，有两种实现方式：位置性数字PID、增量型数字PID。

对于位置式PID算法，由于公式 位置性数字PID控制算法可以由公式（1）得到。

结合公式（1）和公式（2），可以得到增量式数字PID控制算法公式（3）：

3.3

PID测试结果

在实现小车的动态平衡的过程中，互补滤波器输出值将继续根据身体姿势偏转调节数字PID输出值，调整PWM占空比，从而控制电机的转向和速度，以控制适当的均衡的目的，系统控制过程如图2所示。

在两轮自平衡车的功能中，最重要的是保证车体的动态平衡。通过对PID控制器的调试，可以以实验的方式确定控制系统的性能是否稳定以及算法是否有效，平衡测试曲线如图3所示。

从图3两轮自平衡车平衡曲线中可以看出，从开始时的20。倾角的平衡，迅速由趋于平衡到振动，然后汽车的倾斜不继续广泛震荡，证明了PID控制器的有效性。要验证的两轮自平衡车辆的鲁棒性来控制外部影响的能力的作用下，PID控制器，外力施加到小车，其响应干扰图的姿态如图4所示。自平衡小车在受到外力施加后，3s内立即向初始平衡点响应，保持动态平衡，说明PID控制器能满足设计要求，防干扰系统的性能得到了检验。图4中的数据可以看到，在当两轮自平衡小车被外力干扰时，陀螺仪将在第一时间来改变汽车的角度，根据陀螺仪反馈到主控制器检测到的信号，两轮自平衡车辆通过角度变化来调整车体回到平衡。

4 结论

首先完成了两轮自平衡小车硬件电路的设计，通过实验验证了硬件电路基本满足系统的要求。其次设计了互补滤波器，通过对加速度传感器和陀螺仪传感器数据的融合处理，得到一个小车倾角的最优近似解，便于控制器处理。最后设计了双环PID控制器，通过实验的方式验证了算法基本满足设计要求，完成小车的自平衡控制。