title: 平衡车PID分析
date: 2020-06-04 20:46:21
tags:
categories: STM32学习记录
初步认识PID
对于我这个非控制院的学生来说，这方面的理论知识本来就不足，加上以前基本上干的都是点灯的事，没有涉及控制算法的编写，所以写PID的程序是很懵逼的，因此首先要明白究竟什么是PID。
首先，要明白什么情况下要用到PID控制：用户设定一个期望值，我们希望控制一个受控物体，通常是一个物理量，能够尽可能快的达到一个值，并且能够稳定住。那么这个控制器就需要一个或多个输入值，一个是用户的期望值，其他则是测量物理量的传感器的值。
P：proportion，比例，就是用户期望值，与传感器返回值的差值，偏差。
I：integral，积分，记录了从某一时刻开始，所有的偏差的累积。
D：derivative，微分，是偏差的偏差，相当于偏差的导数，偏差变化的速率。
在实际的运用中，我们不一定要用这里面的所有，但是一定是基于P的控制，积分和微分都是在数据上对偏差进行处理，可以有PI控制，可以有PD控制，也存在PID控制。
结合平衡车，深度理解PID
比例控制：把握当下
那么，究竟什么时候用PI，什么时候用PD呢？这就要从原理说起。
比如我做一个平衡车，希望通过一种控制算法来实现平衡，最容易想到的方法就是比例控制：
1、我希望平衡车稳定达到平衡状态（用户期望值）是平衡车的机械平衡角度（目标物理量），以及受控物体的角度，MPU6050传感器的DMP度数，
2、两个数据的偏差，作为输入控制器的值，在控制器中乘上一个增益，经过输出限幅，然后输出给执行器件：电机，
3、电机的工作状态，反过来又会影响平衡车的角度，角度又会被传感器收集，重新计算偏差，输入控制器。
于是，这样一个偏差控制器就形成了一个闭环。
理论上，一个理想的P控制器，只能使受控物理量无限接近目标值，因为当偏差逐渐减小，趋于零时，控制器的输入趋于零，那么无论有多大增益，输出也会趋于零，这样就会停在一个固定值。
但是在实际中，我们要考虑多种因素，第一，输出趋于零，小车可不会静止在一个角度，而是迅速倒下，第二，实际上第一种情况也是不存在的，因为从控制器输出PWM给电机，到平衡车角度改变，本身就是有很长时间的延时，会有惯性，这种控制影响总是延后造成。因此，真正的情况是，平衡车的角度已经很接近机械零度了，电机的输出也已经快要停止了，然而，平衡车却有一个角速度，方向正是最初纠正平衡车的方向，这样，就算停掉电机，小车依然会越过平衡点，越过平衡点之后，偏差为负数，但是数值却很小，偏差经过控制器增益、限幅、输出给电机，电机却无法产生足够及时纠正偏离的转动，于是到小车反向倒下足够大的角度时，电机输出的转速才足够纠正，最终的结果就是，小车开始出现大幅度的震荡，根本无法平衡。
比例+微分控制：预测未来，控制平衡
如何改善这种震荡呢，经过前面的分析，我们知道，出现震荡的根本原因是机械系统的惯性，导致输出对受控物体的影响总是延后的。问题就出在，当角度接近机械零度时，我们的P控制器不工作了，控制器认为完成了任务，然而控制器忽略了一种对下一个时刻的预测，那么这个对下一个时刻的预测，指的就是角度偏差的导数，角度变化的速率，就是角速度。
有了这个认识，我们继续分析，角度偏差接近零度，但是系统存在角速度，那么下一刻就一定会有角度偏差，并且方向与角速度方向相反，通过数据，获取到角速度，就能够通过角速度，来修正这种状态，有预见性的输出给电机，让电机在下一个时刻就开始纠正偏差的倾向。
于是，我们加入D控制，微分项，相当于一个阻尼力：在偏差项P占主导时，微分项D的影响相对来说非常小，然而当偏差趋于零时，微分项的作用就是，减慢受控物理量的变化速率。
这样做的核心原理就是，既然控制存在滞后，那我就提前控制，就达到了抑制震荡，促进系统在平衡点稳定的目的。
比例+积分控制：总结过去，控制位置
上面的PD控制系统，基本上可以让平衡小车处于一个短期稳定平衡的状态，但是肯定不能长时间平衡，原因呢，要从具体的平衡控制结构来分析。
我们的传感器是单一的MPU6050，通过DMP库，输出的是角度以及角速度信号，而我们控制的是电机，也就是说，我们不考虑电机的感受，只要求平衡车的角度能够保持在机械零度，那么一旦平衡车收到扰动，或者控制闭环的任何一个环节，出现了微小的干扰，平衡车就会向一个方向加速，角度还是平衡的，但是平移的速度却没有限制，这样一旦驱动电机的PWM信号达到了限幅的数值，电机不再加速旋转，小车的执行器件就失去了执行能力，就会向这个方向倒下。
经过分析，我们需要再参考一个物理量，就是编码器的读数，既然使用编码电机，编码器的读数就需要纳入测量范围。然后问题又来了，如何处理读到的编码器数据呢？我们设置定时器模式为AB相编码器模式，那么每次读编码器，得到的是0-65535这个区间的一个值，电机转动，编码器的读数就会变化，我们希望小车在一个位置静止，就需要在平衡PD控制的基础上，加上一个位置控制，两种信号经过处理，输入控制器，控制器经过融合，输出给电机。
现在的问题是，位置控制使用什么样的方法？显然，位置控制并不像角度的平衡控制，电机转动就能够较快地达到效果，因此，我们需要一段时间，一步一步地接近我们设定的位置，也就是说，需要使用速度控制，对速度的一步步累积，使小车的位置（编码器的读数）逐渐趋于某一个值，这就要用到PI算法，即比例+积分控制来实现速度控制。
继续分析，我们假设用户输入的目标值是放下小车时刻，编码器的读数就是小车（车轮）的速度，这里可能就有点问题了，明明编码器的读数是脉冲信号，是位置啊，为什么说是速度呢？这就涉及到对编码器的数据处理，我们每隔一段时间，我们读取CNT寄存器，然后软件清零，意味着我们每次读取的数据，都是一段时间的计数值变化，不就是速度吗。弄清楚这个，我们开始分析怎么实现比例+积分的控制。
每隔一定周期，我们获取一次编码器的速度，然后与用户期望的速度，得到一个偏差，这个偏差就是P（比例），很显然偏差越大，我们需要的回复力就越大。到这里，我们吧前面的平衡PD控制，加上速度P控制，经过调整三个增益的大小以及比例，在控制器中对信号进行简单的叠加（叠加式PID，另外还有串联式，这里没有用到），就能够完美的实现小车的长时间稳定了。完美的状态是，推动小车，小车会前进一段距离，然后重新进入平衡。
那么，我们如果想要实现小车能够记住自己的位置，在受到外部干扰后自动回复到初始位置，该如何操作呢？注意，这里我提到了记住这么一个概念，就不难想到，这种控制，需要历史的全部速度信息，对离散的速度进行积分，得到从初始时刻，到当前时刻的位置信息。为了做到这一点，我们需要引入速度PI控制，比例+积分。具体的操作就是：从开始时刻，固定周期将速度偏差进行累加（具体可能需要滤波，这里讲解原理，略去），这个累加的值，就是偏离的距离，把这个距离，经过一个增益，也叠加进前面的3个参数，一共4个参数，共同作为输入，经过控制器输出给电机，这样就实现了在一个位置的PD+PI控制算法。
通过代码，分析PID的各个组成部分
比例+微分：直立环
比例输入：期望角度与当前角度的偏差
期望角度，就是我们通过提前测量，得到的平衡车直立时的6050的读数，因为传感器的安装是存在误差的，并且小车的质心投影也不一定在两轮中心连线上，因此有必要提前测量机械角度中值，作为期望角度。
#define ZHONGZHI -4.5 //平衡机械中值，与MPU6050的安装位置有关
然后我们通过DMP的方式，读取到小车当前的角度：
void Read_DMP(void)
{
unsigned long sensor_timestamp;
unsigned char more;
long quat[4];
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if (sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
Roll= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}
}
在这里，我们设定一个全局变量Roll，通过四元数算出的值就是小车的滚转角，与中值做差，就是角度的偏差：
Read_DMP(); //===读取加速度、角速度、倾角
Angle_Balance=-Roll; //===更新平衡倾角
Bias=Angle-ZHONGZHI; //===求出平衡的角度偏差
微分输入：两个周期得到的角度偏差的差值
Gyro_Balance=-gyro[0]; //===更新滚转角速度
这里的gyro是一个DMP库自带的一个数组，存储了xyz三个轴的角速度AD值，传感器能读出角速度，我们就不需要再去自己计算微分了。
融合：比例+微分信号
balance=Balance_Kp*Bias+Balance_Kd*Gyro;
这样就得到了直立PD控制所需的PWM信号，当然，现在还没有输出，因为后面需要融合速度PI控制，之后再进行输出。
比例+积分：速度环
比例输入：期望速度与当前速度的偏差
这里先介绍最简单的，期望速度为0。我们配置定时器为编码器模式，不分频，向上计数，0-65535，CH1+CH2四倍频。这样，CNT计数寄存器中的值，就记录的小车的位置，我们每隔10ms读取一次寄存器的值，然后清零，具体的函数：
int Read_Encoder(u8 TIMX)
{
int Encoder_TIM;
switch(TIMX)
{
case 2: Encoder_TIM= (short)TIM2 -> CNT; TIM2 -> CNT=0;break;
case 4: Encoder_TIM= (short)TIM4 -> CNT; TIM4 -> CNT=0;break;
default: Encoder_TIM=0;
}
return Encoder_TIM;
}
这里一定要注意，并且理解这个强制类型转换的作用：
我们设定编码器模式，向上计数，那么电机正转，T计数器从0增加，增加到65535时，就会归零，再增加，这样循环。那么一旦反转，就会从0递减到65535，这样的速度偏差就是65535，这显然不是我们想看到的。但是从底层原理解释，本来读寄存器，都是我们经过强转类型，把寄存器赋值给一个我们规定类型的变量。那这里我们把这个变量转成有符号的不就有了正负？于是，强转成short类型，这个计数的范围就变成了-32768到+32767循环计数，这样，偏差就有了正负之分。
Encoder_Left = Read_Encoder(2); //===读取编码器1的值
Encoder_Right = Read_Encoder(4); //===读取编码器2的值
由于是周期性读取编码器，那么这个读取的值，就是小车的当前速度。
积分输入：两个周期得到的比例的累积
static float Velocity,Encoder_Least,Encoder,Movement;
这里定义了几个static变量，可以保存上一次的变化值，是积分实现的编程基础。
Encoder_Least =(encoder_left+encoder_right)-0;
这一句，左轮速度加右轮速度，就得到了小车的几何中心的速度，可以代表小车的速度。
Encoder *= 0.7;
Encoder += Encoder_Least*0.3;
这是一个简单的低通滤波器，作用是减缓速度的变化。因为我们只是将速度环和直立环做了一个简单的线性叠加，而直立是一切的基础，我们不希望速度环对直立环造成过大的干扰，所以在这里需要做一个滤波的处理。
Encoder_Integral += Encoder;
这一步，就做了一个积分的操作，每执行一次，这个变量都会累加之前的速度偏差，得到速度的积分：位移。
融合：比例+积分信号
Velocity=Encoder*Velocity_Kp+Encoder_Integral*Velocity_Ki;
这样就得到了速度PI控制所需的电机PWM信号，下一步就是融合两个闭环控制。
最终融合：直立PD+速度PI
Balance_Pwm = Get_balance_PWM(Angle_Balance,Gyro_Balance); //===平衡PD控制
Velocity_Pwm = Get_velocity_PWM(Encoder_Left,Encoder_Right); //===速度环PI控制
Moto1 = Balance_Pwm - Velocity_Pwm; //===计算左轮电机最终PWM
Moto2 = Balance_Pwm - Velocity_Pwm; //===计算右轮电机最终PWM
Xianfu_Pwm(); //===PWM限幅
Set_Pwm(Moto1,Moto2); //===赋值给PWM寄存器
这一步，就实现了控制器的信号输出，设定驱动电机工作的PWM信号，调节Kp(balance)，Kd(balance)，Kp(velocity)，Ki(velocity)，四个参数，就可以实现小车平衡在某一个位置了。
总结：PID总体框图
