1、在此之前首先深入了解PID算法设计思路。第一要明确设计目的，设计目标是制作一个能够保持平衡的两轮平衡小车，那么首先要保持平衡，其次要能够正常直线行走，再次能实现转向，一个小车就完成了。所以为了完成这些功能的设计，首先要有一个功能足够的PID体系。 比例（P）控制比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差（Steady-state error）。积分（I）控制在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的或简称有差系统（System wit
2、h Steady-state Error）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例+积分（PI）控制器，可以使系统在进入稳态后无稳态误差。微分（D）控制微分调节就是偏差值的变化率。例如，如果输入偏差值线性变化，则在调节器输出侧叠加一个恒定的调节量。大部分控制系统不需要调节微分时间。因为只有时间滞后的系统才需要附加这个参数。如果画蛇添足加上这个参数反而会使系统的控制受到影响。首先看PID的增量型公式：PID=U
3、k+KP*【E（k）-E（k-1）】+KI*E（k）+KD*【E（k）-2E（k-1）+E（k-2）】由公式可以看出，要实现PID算法的编程，所需的PID变量至少有，KP,KI,KD三个系数。实验过程中，这三个变量的值就是许多工程师在实际调试过程中要调整的值。了解这点之后就开始进行实际编码。2. 直立PID的设计两轮平衡小车不同于四轮的小车，开机之后就必须时刻保持直立，否则就会摔倒而无法进行任何操作，所以小车要完成的第一步就是保持直立。首先解释一下小车保持直立的原理。小车是一个直立的个体，那么当它要倒下之前，要首先倾斜，但这时如果小车提前预知了这个趋势，并且控制小车的轮子向倾斜的方向运动小段距
4、离，就能保持平衡。小车只要时刻都保持着这个运动，就可以实现一直保持直立。考虑到这个过程对于高响应速度的要求以及其依赖于对小车状态趋势的分析，这个过程使用P（比例）D（微分）驱动而不使用I（积分）。代码如下：int balance（float Angle,float Gyro） floatBias,kp=575,kd=2.7; int balance; Bias=Angle-ZHONGZHI; /求出平衡的角度中值，和机械相关 balance=kp*Bias+Gyro*kd; /计算平衡控制的电机PWM return balance;这个函数有两个形参，这两个形参分别是平衡倾角和平衡角速度，由单
5、片机控制的陀螺仪mpu6050实时传回的数据经过卡尔曼滤波计算得出。关于陀螺仪和卡尔曼滤波的算法与此处的PID算法无关不再叙述。上面的balance函数是计算PWM值的一个函数，在另一中断函数中，使用该函数为电机PWM赋值。Balance_Pwm =balance（Angle_Balance,Gyro_Balance）;简要说明，PWM在此处指的是单片机通过IO口传出的频率，占空比都可控的方波信号，单片机通过这些方波信号来控制电机的转动与转速，根据电机种类的不同而控制方式也不同。此处可简要理解为pwm的变量值越大，电机的转速越快。那么我们来看balance函数的具体内容，函数中的kp为比例系数
6、，kd为微分系数，该值为已经调试好的取值。输出一个值balance=kp*Bias+kd*Gyro，Angle为平衡倾角，而Bias=Angle-ZHONGZHI的意思就是求出平衡方向的倾角与实际要控制的值之间的差值，既是被控制量与被控制的值的差值，这个差值与kp的乘积就是比例系数的变量。然后再看另一个，另一个是平衡倾角角加速度，由加速度传感器直接经过计算而得，也就是现成的平衡倾角（被控制量）的微分变化，与kd的乘积就是微分系数的变量。两者相加就完成了直立PID的计算。3. 速度PID的设计解决了直立的问题，下一步就要让小车能够自由前行。既是实现速度PID控制。int velocity（int
7、encoder_left,intencoder_right）static float Velocity,Encoder_Least,Encoder,Movement; static float Encoder_Integral,Target_Velocity; floatkp=210,ki=1.05; if（1=Flag_Qian） Movement=Target_Velocity/Flag_sudu; else if（1=Flag_Hou） Movement=-Target_Velocity/Flag_sudu; else Movement=0; Encoder_Least=（encoder
8、_left+encoder_right）-0; Encoder *= 0.8; Encoder += Encoder_Least*0.2; Encoder_Integral +=Encoder; Encoder_Integral=Encoder_Integral-Movement; if（Encoder_Integral10000） Encoder_Integral=10000; if（Encoder_Integral则是为了防止积分变量过大，当系统处于特殊情况时积分变量不断增大可能会对系统的调整造成很大影响，于是在每次积分变量计算结束之后都对积分变量进行一个上限判断（正值上限与负值上限），如
9、果积分变量超过10000则把它的值限定在10000，这样做增加了系统的稳定性，同时在开发过程中这样的语句也有很大的意义。最后把积分变量和比例变量相加赋给电机pwm：Velocity=Encoder*kp+Encoder_Integral*ki;这样就能把每一次的计算结果传递给电机，让电机按照指定的计算结果转动，保证电机已给定的控制量方式转动。4. 转向环PID的设计两个电机都赋予了速度之后，小车就能被控制直走了，但是还需要一个转向的指令，这样小车才能够实现正常的功能。如下为转向PID函数。int turn（intencoder_left,intencoder_right,float gyro）
10、 Static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count; floatTurn_Amplitude=15/Flag_sudu,Kp=60,Kd=0; /=遥控左右部分=/ if（1=Flag_Left|1=Flag_Right） if（+Turn_Count=1） Encoder_temp=myabs（encoder_left+encoder_right）; Turn_Convert=50/Encoder_temp; if（Turn_Convert3）Turn_Convert=3; else Turn_Con
11、vert=0.9; Turn_Count=0; Encoder_temp=0; if（1=Flag_Left） Turn_Target-=Turn_Convert; else if（1=Flag_Right） Turn_Target+=Turn_Convert; elseTurn_Target=0;if（Turn_TargetTurn_Amplitude） Turn_Target=Turn_Amplitude; if（Turn_Target-Turn_Amplitude） Turn_Target=-Turn_Amplitude; if（Flag_Qian=1|Flag_Hou=1） Kd=1;
12、 elseKd=0; Turn=-Turn_Target*Kp -gyro*Kd; return Turn;转向部分使用PD控制，有kp，kd两个变量。函数的三个形参分别是左轮速度，右轮速度和当然角度。这个转向环PID使用的是带有陀螺仪获取方向的控制。那么分析函数本身，先判断flag_left或者flag_right是否为1，如果为1再进行转向PID的计算，如果不是，就把turn_convert置为初值0.9，turn_count置为初值0，encoder_temp也置为0（这几个变量在函数的开始就定义为静态变量可以一直存在并且保存其值大小）。若转向标志变量设置为1，则进行计算，若Turn_C
13、ount变量自加后为1，给encoder_temp变量赋值为当前两轮速度和。而Turn_Convert变量则赋值为50/encoder_temp，是根据速度越快转向越慢的调速，这样比较符合正常的感觉，在不同速度时转向变量大小若相同则会产生快速的时候转向过快的问题。同时如下两个语句：if（Turn_ConvertPR=15; /清楚line5上的中断标志位 Flag_Target=!Flag_Target; if（delay_flag=1） if（+delay_50=10） delay_50=0,delay_flag=0; /给主函数提供100ms精确延时 if（Flag_Target=1） /
14、5ms读取一次陀螺仪和加速度计的值，更高的采样频率可以改善滤波效果 Get_Angle（Way_Angle）; /更新姿态 return 0; /10ms控制一次，为保证测速时间的精准，首先读取编码器的值 Encoder_Left=-Read_Encoder（2）; /读取编码器的值，因为两个电机的反向，所以对其中一个取反，保证输出极性一致 Encoder_Right=Read_Encoder（4）; /读取编码器的值 Get_Angle（Way_Angle）; /更新姿态 Balance_Pwm =balance（Angle_Balance,Gyro_Balance）; /；平衡PID控制
15、Velocity_Pwm=velocity（Encoder_Left,Encoder_Right）; /速度环PID控制，此处速度环是正反馈，就是小车快的时候要慢下来就需要再跑快一点 Turn_Pwm=turn（Encoder_Left,Encoder_Right,Gyro_Turn）; /转向环PID控制 Moto1=Balance_Pwm-Velocity_Pwm+Turn_Pwm; /计算左轮电机最终PWM Moto2=Balance_Pwm-Velocity_Pwm-Turn_Pwm; /计算右轮电机最终PWM Xianfu_Pwm（）; /pwm限幅 Set_Pwm（Moto1,Mo
16、to2）; /赋值给PWM寄存器 return 0;该函数为单片机的中断函数，当PB5这个引脚的电平为低电平时，触发该中断。该终端由mpu6050的INT引脚触发，为5ms定时采样的定时中断，严格保证采样和数据处理的同步。该函数展示了系统把直立环，速度环和转向环三者结合的整个算法和思想，实现了在保持平衡小车直立的前提下对于小车的速度以及转向的控制。编者在每句后都加了注释在这里不再赘述。以下函数为PWM的配置函数，经过整个中断函数计算得出的电机PWM值经由这个函数配置给电机。voidSet_Pwm（int moto1,int moto2） if（moto10） AIN2=1, AIN1=0; e
17、lse AIN2=0, AIN1=1; PWMA=myabs（moto1）; if（moto2CCR1#define PWMB TIM1-CCR4将TIM1-CCR1这个寄存器宏定义给了PWMA，TIM-CCR2这个寄存器宏定义给了PWMB，这样定义增强了代码的可读性。上述函数中的mayabs函数是一个数学函数，取绝对值。以下函数为PWM限幅函数。其中PWM的最大值为7200是硬件限制。经过对定时器的计算得出的。具体计算过程在此不予说明。voidXianfu_Pwm（void） int Amplitude=6900; /=PWM的最大值为7200 限制在6900if（Moto1Amplitud
18、e） Moto1=Amplitude; if（Moto2Amplitude） Moto2=Amplitude;最后把主函数展示出来int main（void） Stm32_Clock_Init（9）; delay_init（72）; LED_Init（）; KEY_Init（）; OLED_Init（）; uart_init（72,128000）; uart3_init（36,9600）;MiniBalance_PWM_Init（7199,0）;MiniBalance_PWM_Init（9999,35） ; Encoder_Init_TIM2（）; Encoder_Init_TIM4（）; A
19、dc_Init（）; IIC_Init（）; MPU6050_initialize（）;DMP_Init（）; /TIM3_Cap_Init（0XFFFF,72-1）; EXTI_Init（）; while（1） delay_flag=1; delay_50=0; while（delay_flag）; While（1）之前的为各种外部设备和系统的初始化函数，while（1）内部为通过MPU6050的INT中断实现的50ms精确延时。下面再放一个简易的卡尔曼滤波的函数，不做解释，使用卡尔曼滤波或者互补滤波等可以使系统得到的加速度传感器传回值更稳定。voidKalman_Filter（float
20、Accel,float Gyro） angle+=（Gyro - Q_bias） * dt; / Pdot0=Q_angle - PP01 - PP10; Pdot1=-PP11; Pdot2=-PP11; Pdot3=Q_gyro; PP00 += Pdot0 * dt; PP01 += Pdot1 * dt; PP10 += Pdot2 * dt; PP11 += Pdot3 * dt; Angle_err = Accel - angle; PCt_0 = C_0 * PP00; PCt_1 = C_0 * PP10; E = R_angle + C_0 * PCt_0; K_0 = PC
21、t_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP01; PP00 -= K_0 * t_0; PP01 -= K_0 * t_1; PP10 -= K_1 * t_0; PP11 -= K_1 * t_1; angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; angle_dot = Gyro - Q_bias;四， 实验总结和经验实验程序经调试可以在小车上达到非常良好的行驶效果，实验程序上写的参数都是经过实际调试PID参数之后试验认为比较合适的参数，因个人的硬件设施不同，PID系数一定会产生差异，本文中的参数只能作为参考，具体如何调PID的参数本身也十分考究。实验过程中，笔者曾在陀螺仪数据处理的方式中选择使用dmp方式，最后可能是偶然原因导致dmp方式
