1、平衡原理 一、平衡小车原理 平衡小车是通过两个电机运动下实现小车不倒下直立行走的多功能智能小车，在外力的推拉下，小车依然保持不倒下。这么一说可能还没有很直观的了解究竟什么是平衡小车， 不过这个平衡小车实现的原理其实是在人们生活中的经验得来的。如果通过简单的练习，一般人可以通过自己的手指把木棒 直立 而不倒的放在指尖上， 所以练习的时候， 需要学会的两个条件： 一是放在指尖上可以 移动 ，二是通过眼睛观察木棒的 倾斜角度 和 倾斜趋势 （角速度） 。通过手指的移动去 抵消 木棒倾斜的角度和趋势，使得木棒能直立不倒。这样的条件是不可以缺一的，实际上加入这两个条件，控制过程中就是 负反馈机制。 而世
2、界上没有任何一个人可以蒙眼不看，就可以直立木棒的，因为没有眼睛的负反馈，就不知道笔的倾斜角度和趋势。这整个过程可以用一个执行式表达： 平衡小车也是这样的过程，通过 负反馈实现平衡 。与上面保持木棒直立比较则相对简单，因为小车有两个轮子着地，车体只会在轮子滚动的方向上发生倾斜。控制轮子 转动 ， 抵消 在一个维度上倾斜的趋势便可以保持车体平衡了。 所以根据上述的原理， 通过测量小车的 倾角和倾角速度 控制小车车轮的加速度来 消除小车的倾角 。因此，小车倾角以及倾角速度的测量成为控制 小车直立的关键。我们的 亚博智能平衡小车 使用了测量倾角和倾角速度的集成传感器陀螺仪-MPU6050（模块详细介绍
3、在亚博智能平衡小车光盘资料3.硬件资料中）。 二、角度（物理分析 PD 算法） 图 1 图2 控制平衡小车，使得它作加速运动。这样站在 小车上（非惯性系，以车轮作为坐标原点）分析倒立摆受力，它就会受到 额外的惯性力，该力与车轮的加速度方向相反，大小成正比。这样倒立摆(如图 2)所受到的回复力为：公式 1 F = mg sin -ma c os mg -mk 1 式1中，由于 很小，所以进行了线性化。假设负反馈控制是车轮加速度 a 与偏角成正比，比例为 k1。如果比例k1g， （g是重力加速度）那么 回复力的方向便于位移方向相反了 。 而为了让倒立摆能够尽快回到垂直位置稳定下 来，还需要增加阻尼
4、力。增加的阻尼力与偏角的速度成正比， 方向相反，因此公式1可改为： F = mg -mk 1 -mk 2 按照上述倒立摆的模型，可得出控 制小车车轮加速度的算法： a =k1+k 2 式中为小车角度,为角速度。k 1 k2都是比例系数 根据上述内容，建立速度的比例微分负反馈控 制，根据基本控制理论讨论小车通过闭环控制保持稳定的条件（ 这里需要对控制理论有基本了解 ）。 假设外力干扰引起车模产生角加速度 x（ t） 。沿着垂直于车模地盘方向进行受力分析，可以得到车模倾角与车轮运动加速度以及外力干扰加速度 a（ t） x（ t） 之间的运动方程。如图 3所示。 图3 在角度反馈控制中，与角度成比例
5、的控制量是称为比例控制；与角速度成比例的控制量称为 微分控制 （角速度是角度的微分） 。因此上面系数 k1,k2 分别称为 比例 和 微分 控制参数。 其中微分参数相当于阻尼力，可以有效抑制车模震荡。通 过微分抑制控制震荡的思想在后面的速度和方向控制中也同样适用。 总结控制车模直立稳定的条件如下： （ 1）能够精确测量车模倾角的大小和角速度 的大小； （ 2）可以控制车轮的加速度。 上述控制实际结果是小车与地面不是严格垂直， 而是存在一个对应的倾角。在 重力 的作用下， 小车会朝着一个方面加速前进 。为了保持小车的 静止或者匀速运动 需要 消除 这个安装误差。 在实际小车制作过程中需要进行 机
6、械调整 和 软件参数设置 。 另外需要通过软件中的速度控制来实现速度的稳定性。在小车 角度控制中出现的小车倾角偏差，使得小车在倾斜的方向上产生加 速。这个结果可以用来进行小车的速度控制。下面将利用这个原理来调节小车的速度。 三、测速（物理模型 建立数学模型 传递函数 PD 算法） 假设小车在上面 直立控制调节 下已经能够保持平衡了， 但是由于安装误差，传感器实际测量的角度与 车模角度有偏差，因此小车实际不是保持与地面垂直，而是存在一个倾角。在重力的作用下，小车就会 朝倾斜的方向加速前进 。 控制速度 只要通过控制小车的倾角就可以实现了。具体实现需要解决三个问题： （ 1）如何测量小车速度？ （
7、 2）如何通过小车直立控制实现小车倾角的改变？ （ 3）如何根据速度误差控制小车倾角？ 第一个问题 可以通过安装在电机输出轴上的 霍尔测速 来测量得到小车的车轮速度。如图 4 所示。利用控制单片机的 外部中断 IO 口在不间断测速，速度为 脉冲信号的个数 可以反映电机的转速 。 图4 第二个问题 可以通过角度控制给定值来解决。 给定小车直立控制的设定值，在角度控制调节下， 小车将会自动维持在一个角度。通过前面小车直立控制算法可以知道，小 车倾角最终是跟踪重力加速度Z轴的角度。因此小车的 倾角给定值 与 重力加速度Z轴角度 相减，便可以最终决定小车的 倾角 第三个问题 分析起来相对比较困难， 远
8、比直观进行速度负反馈分析复杂。首先对一个简单例子进行分析。 假设小车开始保持静止，然后增加给定速度，为此需要小车往前倾斜以便 获得加速度 。在小车直立控制下，为了能够有一个往前的 倾斜角度 ，车轮需要 往后 运动，这样会引起车轮速度下降（因为车轮往 负方向运动了）。由于负反馈，使得小车往前倾角需要更大。如此循环，小车很快就会 倾倒 。原本利用负反馈进行速度控制反而成了“正”反馈。 为什么负反馈控制在这儿失灵了呢？原来在直立控制下的小车速度与小车倾角之间传递函数具有 非最小相位 特性（在此省略了分析），在反馈控制下容易造成系统的 不稳定性 。 为了保证系统稳定，往往取的小车倾角控制 时间常数 T
9、z很大。这样便会引起系统产生两个共轭极点，而且极点的实部变得很小，使得系统的速度控制会产生的 震荡现象 。 这个现象在实际 参数整定 的时候可以观察到。那么如何消除速度控制过程中的震荡呢？ 要解决控制震荡问题，在前面的小车 角度控制中已经有了经验，那就是在控制反馈中增加 速度微分控制 。 但由于车轮的速度反馈信号中往往存在着 噪声 ，对速度进行微分运算会进一步加大噪声的影响。为此需要对上面控制方法进行 改进 。 原系统中倾角调整过程时间常数往往很大，因此可以将该系统近似为一个 积分环节 。将原来的 微分环节和这个积分环节合并，形成一个比例控制环节 。这样可以保持系统控制传递函数不变，同时避免了
10、微分计算。 但在控制反馈中，只是使用反馈信号的 比例和微分 ，没有利误差积分，所以最终这个速度控制是 有残差的控制 。但是直接引入误差积分控制环节，会增加系统的复杂度，为此就不再增加积分控制，而是通过与 角度控制 相结合后在进行 改进 。 要求小车在原地停止，速度为0。但是由于采用的是 比例控制 ，如果此时陀螺仪有 漂移 ，或者加速度传感器安装有误差，最终小车倾角不会最终调整到0，小车会朝着倾 斜的方向恒速运行下去。注意此时车模不会像没有速度控制那样加速运行了， 但是速度不会最终为0。为了消除这个误差， 可以将小车倾角设定量直接积分补偿在角度控制输出中，这样就会彻底消除速度控制误差。 第二点
11、，由于加入了 速度控制 ，它可以补偿陀螺仪和重力加速度的 漂移和误差 。所以此时重力加速度传感器实际上没有必要了。 此时小车在控制启动的时候，需要保持小车的垂直状态。此时陀螺仪的积分角度也初始化为0 。当然如果电路中已经包括了重力加速度传感器，也可以保留这部分，从 而提高小车的稳定性。在后面的最终给定的控制方案中，保留了这部分的控制回路。 四、转向控制（ PD 算法） 通过左右电机 速度差 驱动小车转向消除小车距离道路中心的偏差。通过调整小车的方向，再加上车 前行运动，可以逐步消除小车距离中心线的距离差别。这个过程是一个 积分过程 ，因此小车差动控制一般只需要进行 简单的比例控制 就可以完成小
12、车方向控制。 但是由于小车本身安装有电池等比较重的物体，具有很大的 转动惯量 ，在调整过程中会出现小车转向 过冲 现象，如果不加以 抑制 ，会使得小车过度转向而倒下。根据前面 角度 和 速度 控制的经验，为了消除小车方向控制中的过冲，需要增加角度 微分控制 。 五、全方案整合 通过上面介绍，将车模直立行走主要的控制算法集中起来，如图5 图5 为了实现小车 直立行走 ，需要采集如下信号： （1）小车 倾角速度 陀螺仪信号，获得小车的倾角和角速度。 （2） 重力加速度信号 （z轴信号），补偿陀螺仪的漂移。 该信号可以省略，有速度控制替代。 （3） 小车电机 转速脉冲信号 ，获得小车运动速度，进行速
13、度控制。 （4） 小车转动速度陀螺仪信号 ，获得小车转向 角速度 ，进行方向控制。 在小车控制中的 直立、速度 和 方向 控制三个环节中，都使用了 比例微分（PD）控制 ，这三种控制算法的输出量最终通过 叠加 通过电机运动来完成。 （1）小车直立控制：使用小车倾角的PD（比例、微分）控制； mpu.getMotion6( /IIC获取MPU6050六轴数据 ax ay az gx gy gz Angletest(); /获取angle 角度和卡曼滤波 angleout(); /角度环 PD控制 /角度PD / void angleout() Output = kp * (angle + ang
14、le0) + kd * Gyro_x; /PD 角度环控制 （2）小车速度控制：使用PD（比例、微分）控制； Outputs = ksi * (setp0 - positions) + ksp * (setp0 - speeds_filter); /速度环控制 PI （3）小车方向控制：使用PD（比例、微分）控制。 turnoutput = -turnout * ktp - Gyro_z * ktd;/旋转PD算法控制 融合速度和Z轴旋转定位。 可通过单片机软件实现上述控制算法。 在上面控制过程中， 车模的角度控制和方向控制都是直接将输出电压叠加后控制电机的转速实现的。 而车模的速度控制本质上
15、是通过调节车模的倾角实现的，由于车模是一个 非最小相位系统，因此该反馈控制如果比例和速度过大，很容易形成正反馈，使得车模失控，造成系统的不稳定性。因此速度的调节过程需要非常缓慢和平滑。 六、 PID 算法 图 6 控制相关的软件函数包括： 1. Angletest：小车倾角计算函数。根据采集到的陀螺仪和重力加速度传感器的数值计算小车角度和角速度。如果这部分的算法由外部一个运放实现，那么采集得到的直接是小车的角度和角速度，这部分算法可以省略。该函数是 每 5 毫秒调用一次 。 2. angleout：小车 直立控制 函数。根据小车角度和角速度计算小车电机的控制量。直立控制是 5 毫秒调用一次 。
16、 3. speedpiout：小车 速度控制 函数。根据小车采集到的电机转速和速度设定值，计算电机的控制量。该函数是 50 毫秒调用一次 。 4. turnspin： 方向控制 函数输出平滑函数。将方向控制的输出变化量平均分配到 2 步 5 毫秒的控制周期中 。 5. pwma：电机输出量汇集函数。根据前面的 直立控制 、 速度控制 和 方向控制 所得到的控制量进行叠加，分别得到左右两个电极的输出电压控制量。对叠加后的输出量进行饱和处理。函数调用 周期 5 毫秒 。在此请大家注意速度控制量叠加的极性是负。 七、程序（只给出一部分内容，程序使用的是库函数形式） （ 1） 时序总算法 void i
17、nter() sei(); countpluse(); /脉冲叠加子函数 mpu.getMotion6( /IIC 获取MPU6050 六轴数据 ax ay az gx gy gz kalmanfilter.Angletest(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro,R_angle,C_0,K1); /获取 angle 角度和卡曼滤波 angleout(); /角度环 PD 控制 speedcc+; if (speedcc = 8) /50ms 进入速度环控制 Outputs = balancecar.speedpiout(kp_speed,ki
18、_speed,kd_speed,front,back,setp0); speedcc = 0; turncount+; if (turncount 2) /10ms 进入旋转控制 turnoutput = balancecar.turnspin(turnl,turnr,spinl,spinr,kp_turn,kd_turn,kalmanfilter.Gyro_z); /旋转子函数 turncount = 0; balancecar.posture+; balancecar.pwma(Outputs,turnoutput,kalmanfilter.angle,kalmanfilter.angle
19、6,turnl,turnr,spinl,spinr,front,back,kalmanfilter.accelz,IN1M,IN2M,IN3M,IN4M,PWMA,PWMB); /小车总 PWM 输出 （ 2） 平衡程序 /卡曼滤波计算角度 ,一阶滤波换算姿体 / void Angletest() /平衡参数 Angle = atan2(ay , az) * 57.3; /角度计算公式 Gyro_x = (gx - 128.1) / 131; /角度转换 Kalman_Filter(Angle, Gyro_x); /卡曼滤波 /旋转角度 Z 轴参数 if (gz 32768) gz -= 65
20、536; /强制转换 2g 1g Gyro_z = -gz / 131; /Z 轴参数转换 /姿态识别 angleAx=atan2(ax,az)*180/PI;/计算与 x 轴夹角 Gyro_y=-gy/131.00;/计算角速度 Yijielvbo(angleAx,Gyro_y);/一阶滤波 /卡曼滤波计算角度 / /kalman/ void Kalman_Filter(double angle_m, double gyro_m) angle += (gyro_m - q_bias) * dt; angle_err = angle_m - angle; Pdot0 = Q_angle - P
21、01 - P10; Pdot1 = - P11; Pdot2 = - P11; Pdot3 = Q_gyro; P00 += Pdot0 * dt; P01 += Pdot1 * dt; P10 += Pdot2 * dt; P11 += Pdot3 * dt; PCt_0 = C_0 * P00; PCt_1 = C_0 * P10; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * P01; P00 -= K_0 * t_0; P01 -= K_0 * t_1; P10
22、 -= K_1 * t_0; P11 -= K_1 * t_1; angle += K_0 * angle_err; /最优角度 q_bias += K_1 * angle_err; angle_dot = gyro_m - q_bias; /最优角速度 /kalman/ /一阶滤波 / void Yijielvbo(float angle_m, float gyro_m) angle6 = K1 * angle_m+ (1-K1) * (angle6 + gyro_m * dt); /速度 PI/ void speedpiout() speeds = (smal + smar) * 1.0;
23、 /车速 脉冲值 smar = smal = 0; speeds_filterold *= 0.7; /一阶互补滤波 speeds_filter = speeds_filterold + speeds * 0.3; speeds_filterold = speeds_filter; positions += speeds_filter; positions += front; /全进控制量融合 positions += back; /全进控制量融合 positions = constrain(positions, -16550, 16550); /抗积分饱和 Outputs = ksi * (
24、setp0 - positions) + ksp * (setp0 - speeds_filter); /速度环控制 PI speedcc = 0; /pwm/ void pwma() pwm1 = -Output - Outputs - turnoutput; /Left 电机 PWM 输出值 pwm2 = -Output - Outputs + turnoutput;/Right 电机 PWM输出值 /幅度值限制 if (pwm1 255) pwm1 = 255; if (pwm1 255) pwm2 = 255; if (pwm2 30 | angle = 0) digitalWrite
25、(IN2M, 0); digitalWrite(IN1M, 1); analogWrite(PWMA, pwm1); else digitalWrite(IN2M, 1); digitalWrite(IN1M, 0); analogWrite(PWMA, -pwm1); /电机的正负输出判断 右电机判断 if (pwm2 = 0) digitalWrite(IN4M, 0); digitalWrite(IN3M, 1); analogWrite(PWMB, pwm2); else digitalWrite(IN4M, 1); digitalWrite(IN3M, 0); analogWrite
26、(PWMB, -pwm2); /turn/ int turnmax = 10; /旋转输出幅值 int turnmin = -10; /旋转输出幅值 void turnspin() float turnspp; turnvert=0.9； if (turnl = 1 | spinl = 1)/根据方向参数叠加 turnout += turnvert; else if (turnr = 1 | spinr = 1)/根据方向参数叠加 turnout -= turnvert; else turnout = 0; if (turnout turnmax) turnout = turnmax;/幅值最
27、大值设置 if (turnout turnmin) turnout = turnmin;/幅值最小值设置 turnoutput = -turnout * ktp - Gyro_z * ktd;/旋转 PD 算法控制 融合速度和 Z 轴旋转定位。 （ 3） 蓝牙 /bluetooth/ void kongzhi() while (Serial.available() /等待蓝牙数据 switch (Serial.read() /读取蓝牙数据 case 0x01: front=600; break; /前进 case 0x02: back=-600; break; /后退 case 0x03: t
28、urnl=1; break; /左转 case 0x04: turnr=1; break; /右转 case 0x05: spinl=1; break; /左旋转 case 0x06: spinr=1; break; /右旋转 case 0x07: turnl = 0; turnr = 0; front = 0; back = 0; spinl = 0; spinr = 0; break; /确保按键松开后为停车操作 case 0x08: spinl = 0; spinr = 0; front = 0; back = 0; turnl = 0; turnr = 0; break; /确保按键松开后为停车操作 case 0x09: front = 0; back = 0; turnl = 0; turnr = 0; spinl = 0; spinr = 0; turnoutput = 0; break; / 确保按键松开后为停车操作 default: front = 0; back = 0; turnl = 0; turnr = 0; spinl = 0; spinr = 0; turnoutput = 0; break; 亚博智能 Arduino 智能平衡小车
