平衡小车直立控制调试

author：Songyibiao

整体思路

平衡小车由角度环和速度环控制。其中，角度环为内环，速度环为外环。

角度环使用PD（比例微分）控制器，其实一般的控制系统，例如水温控制，单纯的P控制或者PI控制就够用了，但是那些对干扰要做出迅速响应的控制过程需要D（微分）控制。

下面是直立PD控制的代码：

/***************************************************************
** 作　  者: Songyibiao
** 官    网：http://www.miaowlabs.com
** 淘    宝：http://miaowlabs.taobao.com
** 日　  期: 2015年11月29日
** 函数名称: AngleControl
** 功能描述: 角度环控制函数           
** 输　  入:   
** 输　  出:   
** 备    注: 
********************喵呜实验室版权所有**************************
***************************************************************/
void AngleControl(void)	 
{  
	//去除零点偏移,计算得到加速度传感器的角度（弧度）
	g_fGravityAngle = (float)(g_iAccelInputVoltage_X_Axis - GRAVITY_OFFSET) / 16384.0f;
	// 57.2957795=180/3.1415926535898 弧度转换为度
	g_fGravityAngle = g_fGravityAngle * 57.2957795f ;
	//陀螺仪去零点偏移，乘比例因子
	g_fGyroAngleSpeed = (g_iGyroInputVoltage_Y_Axis - GYRO_OFFSET) / GYROSCOPE_ANGLE_RATIO *(-1.0);
	//互补滤波
	g_fCarAngle = 0.99f*(g_fCarAngle + g_fGyroAngleSpeed * 0.008f) + 0.01f * g_fGravityAngle;
	//角度环PID控制
	g_fAngleControlOut = (CAR_ANGLE_SET - g_fCarAngle)* g_tAnglePID.P+ \
	(CAR_ANGULARSPEED_SET - g_fGyroAngleSpeed )* g_tAnglePID.D;	   
}

代码共五行，第一、二行是对加速度值的处理，第三行是对陀螺仪的处理，第四行是互补滤波，对加速度和陀螺仪数据进行数据融合，第五行是PID控制器计算。

其中，g_iAccelInputVoltage_X_Axis、g_iGyroInputVoltage_Y_Axis为MPU6050的加速度和陀螺仪数据，这里用了X轴加速度和Y轴陀螺仪。具体使用哪个轴，要看MPU6050的安装方位。

GRAVITY_OFFSET是小车的机械中值，所谓机械中值，把平衡小车放在地面上，绕电机轴旋转平衡小车，找到接近平衡的（能直立一两秒）角度，查看这时加速度的值，就是机械中值，这时应该为0，这时看到的不为0的任何值就是偏移值，所以要减去这个零点偏移。16384.0f是固定转换值，具体可以看MPU6050的规格书。

同样，陀螺仪也要减去零点偏移，并且乘于一个比例因子，这个GYROSCOPE_ANGLE_RATIO比例因子是不完全跟规格书里的，而是一个工程值，需要根据具体情况来选取，这个后面说明。接着，就是互补滤波，互补滤波比较简单，一条公式就可以妥妥的干活。不明白互补滤波工作原理的同学，自己到喵呜实验室官网看Mwbalanced 8051百科里面的教程。

最后是PID控制器计算角度环PWM输出值。

在调试直立环的时候，我们要屏蔽速度环和转向环。除了角度环，其他函数先屏蔽就行。

SampleInputVoltage();		//采集数据函数
AngleControl();				//直立控制函数			
//GetMotorPulse();			//采集脉冲函数
//SpeedControl();			//速度控制函数	
MotorOutput();				//电机输出函数

确定平衡小车的机械中值

把平衡小车放在地面上，绕电机轴旋转平衡小车，找到接近平衡的（能直立一两秒）角度，查看这时加速度的值，就是机械中值，这时应该为0，这时看到的不为0的任何值就是偏移值，所以要减去这个零点偏移。我在头文件里定义宏GRAVITY_OFFSET。

确定角度环P的极性

此时，只加入P值，D值设0。

根据工程整定法，我们可以估计P的取值范围，我们的PWM量程为1000，即赋值为0占空比为0%，赋值为100占空比100%，假如我们设定P值为100，那么平衡小车在±20°的时候就会全速转动电机。根据我们的感性认识，这显然太大了，那我们就可以估计kp值在0~100之间。首先大概我们给一个值kp=-50，我们可以观察到，小车往哪边倒，电机会往那边加速让小车更快地倒下，就是一个我们不愿看到的正反馈的效果，说明P值的极性反了，这里表明P值应为正值。接下来我们设定kp=50,这个时候可以看到平衡小车有直立的趋势，虽然响应太慢，但是，我们可以确定kp值是正的。具体的数据接下来再仔细调试。

确定角度环P值的大小（令D=0，要结合角度环控制函数理解）

参数整定思路：P值一直增加，直到小车出现大幅度的来回摆动。在自动化专业，这个现象叫振荡。 设定P值=30,这个时候我们可以看到，小车虽然有平衡的趋势，但是显然响应太慢了。 设定P值=60,这个时候我们可以看到，小车虽然有平衡的趋势，而且响应有所加快，但是响应还是不足以让小车保持平衡。 设定P值=80,这个时候我们可以看到，小车的响应明显加快，而且来回推动小车的时候，会有大幅度的摆动。说明这个时候kp值已经足够大了，需要增加微分控制削弱P控制，抑制由P控制引发的振荡。

确定D值的极性

此时令P值为0。

我们先设定kd=-0.5，当我们旋转小车的时候，车轮会反向转动，这会另小车加快倾倒，并没有能够实现跟随效果。这说明了D值的极性反了。接下来，我们设定D值=0.5,这个时候我们可以看到，当我们旋转小车的时候，车轮会同向以相同的速度跟随转动，这说明我们实现了角速度闭环，至此，我们可以确定D值的极性是正的。具体的数据接下来再仔细调试。

确定kd值的大小（令kp=80）

参数整定思路：D值一直增加，直到出现高频抖动。 设定D值=2,这个时候我们可以看到，由P值引起的振荡已经基本消除。设定D值=2.5,这个时候我们可以看到，整体性能已经非常棒。 设定kd=3.5,这个时候我们可以看到，小车开始出现高频剧烈抖动(调试过程遇到这种情况请马上关闭小车，长时间高频抖动会导致驱动被烧坏的) 至此，我们可以确定得到kp=80,kd=3.5是P、D参数的最大值。我们将这两个最大值适当减少，得到76、3.0，这个时候我们可以看到，小车没有任何的抖动，非常平稳，但是依然无法保持长时间的直立，直立很短一段时间后会往一个方向加速倒下。只有角度环的控制，平衡小车是不具备抗干扰能力，就像浮在冰面上的冰山，一旦受到干扰，就会发生移动。

增强鲁棒性，要加入速度环。角度环调试到此结束。

常见问题

１车模往一个方向偏，为什么？ 这是因为你目标角度为０，然后陀螺仪中值测量不准确，导致计算出的角度和目标０度角有偏差，可以修改加速度偏移值GRAVITY_OFFSET，其中加大向后，减少向前。

２容易出现的一个错误是利用融合之后的角度判断电机正反转，实际是需要利用ＰＤ参数计算的最终数值计算正反。

3需要注意传感器的正负方向，当角度向前倒下，小车电机向前，当角度向后倒下，小车电机向后，最终融合的角度和角速度大小和变化趋势，根据虚拟示波器调试波形趋势。原则是当融合的角度为正，角速度也需要按照同方向增大。角速度相当于正向反馈。否则当两者方向判断相反，小车不能直立。