浅谈两轮平衡车的控制原理（续）

前言：上次云里雾里的说了一通，不知道对平衡车的控制有没有说到点子上。单纯的讲解原理可能会很无聊，但是作为一个技术宅来说，就算头皮发麻也要接着看下去。哈哈，吾理小子争取用通俗的语言把自己懂的知识讲解出来。

好了，闲话少说，进入正题。上文已经做好了平衡车站立起来的全部准备工作，接下来就是控制的核心了，如果对上面讲到的内容还没有看到，建议先看上一篇，否则会有莫名其妙的感觉。

首先，说说陀螺仪的安装位置，建议有条件的话装在电机轴的正上方，两轮子中点处，这样做的好处是角速度响应比较平均。但是，吾理小子的条件有限，陀螺仪装在了右侧轮子附近，安装位置看下图。（PS：陀螺仪一定要装稳）

安装好陀螺仪之后，接下来开始写程序。关于MPU6050的程序，吾理小子不想啰嗦，直接说比较关键的地方吧。相信用过STM32的小伙伴对原子哥不会陌生吧，小编也是原子哥的受益者，在这里顺便感谢一下原子哥。

小编移植了原子哥的MPU6050程序，所以要说一下初始化问题。我们知道陀螺仪上电之后要初始化，然后要自校准。原子哥的程序是以上电的时候陀螺仪的姿态为参考，解算姿态。也就是说，每次测量的姿态都是以上电时刻为基准的。那么可能有人和小编一样纳闷，如果每次都是相对的参考平面，那岂不是要每次扶着平衡车上电初始化啊。考虑到这个问题之后，小编尝试寻找MPU6050的绝对参考平面。

原子哥初始化程序中有一个DMP初始化，打开看看

看到下面有一个自检函数，Go to你会发现有这样一段代码：

敲黑板，画重点啦。这就是获取当前加速度计的值，然后将其设置为基础值。各位看官明白什么意思了吧，就是自检函数中获取加速度计的值，将此时的值设置为基础值，也就是参考平面啦！

啦啦啦，那我们把这个值设置为零就和初始位置无关了吧，所以将此时的加速度计的值置零就行了，修改之后如下：

至此，我们上电之后的参考平面就是水平面啦，是不是很简单呢！

好了，接下来就是绝对无聊的代码与思路了。首先，简单描绘一下控制思路

各位请看上图，虽然它很简陋，但是基本能够说明问题。读取陀螺仪数据，通过PID控制器计算得到PWM的占空比输出，控制电机将车架姿态调整，继续读取陀螺仪数据，如此反复，就完成了动态调节。

程序中要实现这样的效果，一般会使用单片机内部定时器，开启定时器中断，在固定的时间节点去读取陀螺仪数据，然后进行PID计算，最后控制电机进行一次调节。小编也是这样实现PID调节的，定时器10ms中断一次。定时器的配置与中断服务函数就不贴了，只着重描述直立环的PID代码。 

void PID_Balance_Cal(Balance_PID * PID,float Angle,float Gyro)
{
	PID->Now  = Angle - 2;	
	PID->Out  = PID->Kp * PID->Now
	          + PID->Kd * Gyro;
}

这是直立环PID代码，入口参数有三个。第一个是PID结构体，第二个是测量的角度值，第三个是角速度值。后两个量与MPU6050安装方位有关。小编的陀螺仪安装位置与姿态第一幅图拍过了，小车前倾时roll角度变小，后仰时roll角度变大。小车前倾与后仰时，陀螺仪的数据也就是角速度变化的是x轴的，也就是gyrox。因此，调用PID的形参就是roll和gyrox。

再来看函数具体内容，第一行是计算当前误差，2代表的是小编平衡车的机械中值，这个在上一篇的开头中有说明。车架与重量分布不一样，这个值也不一样，一般都是在0°左右。第二行是PID的输出，就是常规位置式PID的公式。这里我们只用到了PD项，没有用到积分项，这是借鉴前人的经验而已。至于微分项为什么乘以gyro，现在进行一点自己的理解与说明。

大家调试过位置式PID的同志们都知道，计算公式是

PID->Out=(PID->Kp * PID->Error) + (PID->Ki * PID->Error_I) + (PID->Kd * PID->Error_D); 

在平衡车中用到的PID形式好像有点看不懂了。有没有这种感觉，其实并不是这样的，上面两个实质是一样的。一般来说，求微分项就是求角度的变化趋势，也就是求角度的微分，他的物理意义实际就是角速度，因此我们直接用陀螺仪角速度乘以微分项系数来表示微分结果，这是合情合理的吧！各位道友，不知小编的理解是否正确，欢迎各位批评指正。

好了，直立环的核心代码就这么几行，接下来就是参数整定了。

嗯……再三考虑，还是先把定时器中断里面的代码贴一下吧！

typedef struct
{
	float Kp;
	float Ki;
	float Kd;
	
	float Out;
	
	float Now;
	float Last;
	float Earlier;
	
	float Error;
	float Error_I;
	float Error_D;

} Balance_PID;  //定义直立环结构体，很多参数都是冗余。小编太懒，不想改

Balance_PID  Balance_pid; //初始化直立环结构体

void TIM5_IRQHandler(void)
{
    if (TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET) 
	{
	    TIM_ClearITPendingBit(TIM5, TIM_IT_Update); //清中断标志
		
	    if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)  //读取陀螺仪数据
	    { 
		    MPU_Get_Accelerometer(&aacx,&aacy,&aacz);	//获取加速度计数据
		    MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);	//获取陀螺仪数据

		    PID_Balance_Cal(&Balance_pid,roll,gyrox);	//调用直立环 
		    Set_Motor_PWM((int)Balance_pid.Out,(int)Balance_pid.Out);  //设置PWM
	    }
	}
}

代码应该没有什么说的，自行看注释就好。

先确定参数正负，再确定大小。

设置Kp=100。烧代码，拿起小车，向前倾，发现车轮向后加速。这样的效果也就是说轮子的响应是加速小车倒下，在不是我们想要的结果，肯定是正负错了。

设置Kp=-100。烧代码，拿起小车，向前倾，车轮向前加速，向后倾，车轮向后加速，如此这就是正常的效果了。

同样的实验过程确定Kd：

设置Kp=0，Kd=0.5。烧代码，拿起小车，猛向前倾，发现车轮向后加速。这样的效果也就是说正负错了。

设置Kp=0，Kd=-0.5。烧代码，拿起小车，猛向前倾，车轮向前加速，猛向后倾，车轮向后加速，正常的效果。

上面的参数大小为什么这样取值，和PWM的计数上限有关。参数大小范围，可以这样评估。假如计数器上限是2000，车架的角度变化是-9°~9°，那么我们的Kp大概就在（2000/18=）100左右。我们知道P的系数是调整的步幅，步子太大，响应速度快，但是不稳定，步子太小可能达不到期望值。基本就是这个意思了。上面设置d的系数很小，是因为小车猛向前或者后时，gyrox的值可能达到上万的数值，所以这个只取0.5。好了，具体的细节就不多说了，参数整定多了，自然会明白。

至此，直立环就剩下参数的整定啦。各位可参见平衡小车之家的整定教程。讲道理，参数整定的好的话，小车应该可以很听话的站立起来了。但是小编遇见了下面的问题：

基本现象是小车能够站立，也能够看出在不断调整，但是小车总会慢慢旋转，就是右轮的动作幅度很大，左轮几乎不动，看到的现象就是小车在慢慢的原地转圈。其实小车稳定性还是很高的，但是就是会莫名的转圈，再怎样调整参数，都无法解决这个问题。小编有点抓狂了，好难受。（抱住肉肉的自己）

随后，仔细回顾代码发现，PD计算出的PWM控制两个电机是相同的值。灵光一闪，我已经知道问题在哪里啦，肯定是驱动板不对称或者电机参数不对称造成的，也就是说给相同的PWM两个电机响应的不一样。我决定尝试用编码器来测试一下，看来土豪级的编码器还是排上用场了。具体这样操作的：

 

初始化左右两电机的编码器，设置PWM：Set_Motor_PWM(1200,1200);

void Get_Speed_Data(void)          //获取当前速度
{
	Left_encode_val = TIM_GetCounter(TIM8);
	Right_encode_val= TIM_GetCounter(TIM4);	//读计数器值
	
	TIM8->CNT = 15000;    //将计数值赋初值
	TIM4->CNT = 15000;
	
	Left_Speed_Val= (15000-Left_encode_val)*2.578;  //uint:m/s    *1.0035
	Right_Speed_Val=(15000-Right_encode_val)*2.578;  //uint:m/s //计算实际速度
}

编写读取编码器数据子函数，为了放置缩小误差，我们直接看编码器的值。在定时器中断中调用上面子函数，然后把得到的值实时显示在SPI接口屏幕上。（豪华配置，一个个都体现了出来）如果没有屏幕，此时可以用串口看数据。在死循环中调用显示函数，具体代码

	while(1)
	{
//		sprintf(s,"%.3f",roll);
//		Gui_DrawFont_GBK16(60,16,RED,GRAY0,(unsigned char*)s);
			
		sprintf(s,"%d",Left_encode_val-15000);
		Gui_DrawFont_GBK16(60,80,RED,GRAY0,(unsigned char*)s);
		
		sprintf(s,"%d",Right_encode_val-15000);
		Gui_DrawFont_GBK16(60,96,RED,GRAY0,(unsigned char*)s);
	} 

将左右轮的编码器值显示在屏幕上。如此，在PWM均为1200情况下，在10ms（定时器中断间隔）时间段内，左右电机编码器的值，左轮数值22左右，右轮数值34左右。

我的天呐！各位发现了没有，10ms的时间居然相差这么大，那么为什么会原地转动，原因和预料的一样。

怎么办嘞？补偿呗，通过补偿发现，当设置左轮为1370时，和右轮编码器数值几乎相等，这就圆满啦（哈哈哈，兴奋一下）。注意，反相同样需要补偿。

补偿之后，还原原来的代码，烧代码，看效果。

哇，见证奇迹的时刻到了，小车直立环效果感人啊！站立的 非常稳，速度几乎为零。

各位道友可能看到说仅一个直立环是不足以让小车站稳的，那么小编的结论并非如此，仅仅直立环也可以站稳而且很稳。

PS：仅直立环是否能够站稳和小车的安装、结构、重量分布等等都有关系，因此，调试过程中一定以自己的现象为准，切记不能盲从。

最关键的直立环调试完了，效果感人，那么今天的内容也接近尾声啦！

随后的速度与转向环，吾理小子不想再接着写了，因为没有特别难的，各位自己修行吧，哈哈哈！多说一句，速度环不是常规的负反馈，而是正反馈。这样描述一下，就是小车速度要想降下来，必须先加速然后才能减速。欲知具体效果，需自身体验，谢谢！

感谢原子哥和平衡小车之家，有了你们这些大佬的默默付出，才会有我们这群小妖怪的茁壮成长！

最后，写的有点凌乱，有什么不对的地方请各位批评指正！