二轮平衡车调节平衡

stm32f4平衡车

通过设定小车的偏差值写 balance 函数，作用是计算出小车直立 PD 控制所需的 PWM 信号。比例输入为期望角度与当前角度的偏差。微分输入为两个周期得到的比例的累积。 

 void Get_Angle(void)  
{
mpu_dmp_get_data(&pitch, &roll, &yaw);     
//从 DMP 读取四元数后算出 Pitch Roll Yaw，并且把加速度、角速度数组更新到全局  
 
Angle_Balance=-roll;             //===更新平衡倾角   
Gyro_Balance=-Gyro[0];            //===更新平衡角速度  
Gyro_Turn=Gyro[2];               //===更新转向角速度   
Acceleration_Z=Accel[2];         //===更新 Z 轴加速度计  

}    
写在 Callback 函数中   
Balance_Pwm = balance(Angle_Balance,Gyro_Balance);            //===平衡 PID 控制  

通过 Get_Angle 函数得到小车通过 DMP 读取的四元数，计算出 Pitch、Roll 和 Yaw， 直立环主要运用得变量为 Roll。然后计算出期望角度，就是我们通过提前测量，得到的平 衡车直立时的 MPU6050 的读数，因为传感器的安装是存在误差的，应当提前测量机械角 度中值，作为期望角度。令滚转角与中值做差，得到角度的偏差。微分通过 Gyro 数组自 动读出角速度，无需再次计算。 

融合 PD，在 Callback 函数中调用获取小车直立 PD 控制平衡时输出 的 PWM 信号。 

int balance(float Angle,float Gyro)  
{     
float Bias;     
int balance;     
Bias=Angle-Zhongzhi; //偏差计算，zhongzhi 为设置的小车机械中值     
balance=Balance_Kp*Bias+Gyro*Balance_Kd;   
//===计算平衡控制的电机PWM，PD控制：kp是P系数 kd 是D系数     
return balance;  
} 

在 PID 参数整定之前首先要找平衡车的机械中值，机械中值就是不在任何外力的干扰 下，小车能够保持平衡时间最长的位置。首先在 OLED 显示屏上显示对应的角度或者通过 串口实时反馈角度。观察角度，一只手把平衡从一个方向往另一个方向托，在其快速倒下 的瞬间我们记住当前的角度值。另一个方向同理，我们将两个角度值相加除以 2 得到的中 间值作为机械中值。

 
整体 PID 参数设置的原理：位置控制的调节经验可以总结为:先只使用 Р 控制，增大 Р 系数至系统震荡之后加入微分控制以增大阻尼,消除震荡之后再根据系统对响应和静差等 的具体要求，调节 Р 和Ⅰ参数。 

直立环 PD 参数调节，首先判断 Kp、Kd 的极性，当倾角增大时 PWM 占空比应增大， 所以显然它们都应该为正值，然后先确定 Kp 再确定 Kd，Kp 逐渐增大直至小车有明显低 频震荡，注意，Kp 的值越大，系统的反应越快，但是平衡曲线的震荡就较大，我们需要 引入 Kd 参数来减缓震荡，定住 Kd 后从零逐渐增大 Kd，直至小车对对角度变化十分敏感， 且几乎没有震荡时，这时 PD 调节器的参数整定就完成了。根据工程经验，将整定后的 PD 调节器的参数乘 0.6 得到最终的 PD 调节器参数。 

最终调节结果为以下代码所示。 

float Balance_Kp=300,Balance_Kd=1.5;   
float Zhongzhi=-0.8;        
//机械中值 

在执行直立平衡控制之后，小车虽然能够直立行走，但无法控制它稳定在一个位置。 因此加入了下面的速度控制与之结合。