这一章主要学习小车控制的PID算法。
如图是PID的基本概念。
因为我的专业是自动化，对这些基本概念比较清楚，就不对一些很常见的概念做一些叙述了。
这幅图是一个烧水壶的例子，用来说明PID控制的效果。
右上角出现震荡是因为加热具有延时，会产生一段超调。
小车项目里，不是使用传统的调参，而是建模分析，根据指标计算KP，KI，KD。
第一部分 控制系统的结构和关键指标
控制系统：使用控制器控制执行器，让被控对象的输出跟踪设定值。
控制系统的组成
上图是系统的关键指标的通俗易懂的解释。v
其中，对于准的概念，上图具体做了一个解释，因为水壶本身会散热，所以最终的实际温度到不了设定温度，造成稳态误差，是系统自身的缺陷，需要额外加大力度调整才能消除。
上图是对控制系统的关键指标的专业术语解释。
主要要记住几个量的专业名词方面后面编程、
第二部分 电机的电学方程
这是控制系统各个部分对应小车项目的现实部分。
简易框图如下
更简化如下
想确定PID的参数，首先要确定电机的数学模型。
首先复习一下 理想的电机模型的相关参数。其中重要的是反电动势根据感应电动势推倒出的公式，因为B和L是定值，我们可以近似认为角速度和电源电压成正比。
根据电学知识列出上述公式，写出微分方程。
第三部分 电机的机械方程
除了知道电机的电学方程，还需要知道电机的机械方程。电机转速主要受到安培力大小，转子惯性，还有摩擦力的影响。
电机转动可以等效成刚体转动，和刚体平动相关参数对比如下
其中，平动已经非常熟悉了，刚体转动的扭矩，计算方法如下
转动惯量的计算如下
用一道例题巩固知识点
求出Jw就能求出角加速度了。
以上是J的计算公式
用微积分求出等效模型的转动惯量。
以上计算过程是对等效圆环求转动惯量。
那么最后就可以求出Jw的值了
进而求出角加速度，最后求出时间
把电路部分和机械部分联动起来，得到下面这张图
联立等式就可以求出机械的微分方程。
这就是两个方程，电机的电学方程和机械方程。
第四部分 拉普拉斯建模
拉普拉斯变换将时域变换成复频域，求解问题更加简单。
首先对电学方程进行套用规则，保持常数不变。
接着是机械方程
之后就可以通过这两个方程联立求出传递函数了。
第五部分 结构图化简
首先介绍了传递函数的概念
求出输出与输入的拉氏变换之比，就是传递函数。
对上次的电学微分方程化简
如图是电机的结构框图，输入是Ua，输出是ws。
此处忽略Tl（为了计算方便），再对两个方程联立求解传递函数，消去Ia（s），就可以得到Ua（s）和w（s）的关系。
计算过程如图。
用计算好的结果代入，得到如图的结果。
上面是通过方程组的方式得到传递函数，另外还有一种画出结构图的方式来获取传递函数。
上图两段蓝色的有笔误，应该除以跟等式一样的式子。
将相同的部分连接起来就行了，接下去进行化简。
上图的反馈环节有错误，（分子）上面是G1（s）。
第一步，忽略TL，对化简影响太大了。
最终通过结构图化简的方式，同样得出了传递函数。
第六部分 电机参数测量
这部分是测量基本参数的，涉及大量公式，可以不掌握，直接获取参数即可。
第七部分 波特图
如图，波特图是用来表示系统在3个频段的性能，求出原始电机的性能，加上PID的性能矫正，就能得到PID控制好的系统。
绘制步骤如下
标准型：
型别看幂次，比如2次就是2型。图上的分母括号里代表尾1型，表示加的常数是1。
例题：
画波特图的步骤如上。
引入自动控制理论里的波特图，是一种特殊的坐标图
极点画叉，零点画圈，负号省略。
先在w=1处确定20logk的数值，然后确定穿过的斜率，由型别决定。
例如
在基准线的基础上，遇到极点斜率减20，遇到零点斜率加20。
根据上节课跳过的数据，把传递函数表示出来以后，绘制电机模型的波特图。
补充：除了幅频特性曲线，还有相频特性曲线。
与幅频特性相关，相频特性表示将x（t）经过G（s）后得到的y（t）相位的值。
首先看型别，v是几次方，就滞后几个90°，在零点处超前90°，形状类似于arctanx的函数，在极点处滞后90°，形状类似于arctanx的函数。
因为没有零点，所以画两条极点对应的曲线，合成后大概是图上红线的形状。
第八部分 PID参数整定
这部分包含三段论的知识，如下图
低频是第一个极点左边的部分，中频是曲线穿过x轴附近的区域，剩下是高频段，这三段分别和一些指标有关系。
理想的波特图的状况如上。
低频段，k越大w=1对应的值越大，越陡，稳态误差越小。
中频段，尽量以-20db穿过，且穿越的范围尽可能大，范围越大，超调量越小。wc，也就是穿过x轴的交点的值，也是越大越好，振荡越快，越容易趋于稳定。
高频段尽量低，下降越快越好，抗噪声能力越好。
用以上指标分析原有电机模型：
低频段，斜率为0，需要变大成-20或者-40；
中频段：穿过的点最好在频段的中心，需要上移；
高频段：斜率大，衰减能力强。
综上所述，需要改进的位置在低频段和中频段。
利用PID控制器叠加在原有电机上，改进电机的性能，如图。
在低频段，不仅变陡了，而且w=1的位置上升了。
在中频段，wc的位置右移了，振荡加快，且中频段穿越x轴两边的部分尽可能相等了。
在高频段，因为原本性能不搓，所以没什么变动。
想画出PID的波特图，步骤如下
将PID传递函数画出波特图，各种不同的波特图如图。
根据上面的练习，设计电机的PID
此处的数据为估算，大致让改进后的系统达到要求而已。
对于小车来说有3个现实缺陷，导致电机控制非线性：
1.电机控制轮胎存在减速差，因为变速齿轮存在间隙。
2.编码器测速波形并非理想，不光滑。
3.启动扭矩很大。
综上所述，增益K需要减小一些，这些数字都是up经过实验得出的。
第九部分 PID控制器代码编写
大致的PID控制流程在代码的体现如下
创建一个结构体用来保存PID参数里面需要长期保存的参数。
新建一个pid.c和pid.h的文件，在头文件里创建结构体
#ifndef PID_H #define PID_H #include"stm32f10x.h" typedef struct { float Kp;//比例系数 float Ki;//积分项的系数 float Kd;//微分项的系数 float SP;//用户的设定值 }PID_TypeDef; #endif接着创建两个函数用来初始化PID和改变设定值SP
// //@简介：对PID控制器进行初始化 //@参数 kp - 比例系数 //@参数 ki - 积分项系数 //@参数 kd - 微分项系数 void PID_Init(PID_TypeDef *PID,float Kp,float Ki,float Kd) { PID->Kp=Kp; PID->Ki=Ki; PID->Kd=Kd; PID->SP=0.0f; PID->t_k_1=0; PID->err_k_1=0; PID->err_int_k_1=0; } // //@简介：改变设定值SP //@参数 SP - 新的设定值 // void PID_ChangeSP(PID_TypeDef *PID,float SP) { PID->SP=SP; }创建一个计算参数，用于执行一次PID计算，里面的参数都需要经过运算得出。将PID的比例积分微分项累积就可以得出结果了。但是积分和微分比较难实现，需要借助误差的图像。
上图解释了积分和微分的图解法。
具体算法如下
用三角形的斜率求微分，用梯形的面积求积分
// //@简介：执行一次PID运算 //@参数 FB - 反馈的值，也就是传感器采回的值 //@返回值：PID控制器计算的结果 // float PID_Compute(PID_TypeDef *PID,float FB) { float err=PID->SP - FB; uint64_t t_k=GetUs(); float deltaT=(t_k - PID->t_k_1)* 1.0e-6f; float err_dev=(err - PID->err_k_1) / deltaT; float err_int=PID->err_int_k_1 + (PID->err_k_1 + err)*deltaT * 0.5f; float COp=err*PID->Kp; float COi=err*err_int; float COd=err*err_dev; float CO=COp + COi + COd; //更新 PID->t_k_1=t_k; PID->err_int_k_1=err_int; PID->err_k_1=err; return CO; }注意：在执行完一次计算后，需要迭代更新下一次计算的值。
第十部分 PID测试
上图是PID控制的思路，其中compute函数上一个部分已经完成编写了。把所有的电机调速代码写在app_motor.h和app_motor.c里面。
依照先前的思路，先对左右电机进行初始化，再反复执行电机调速的进程函数。
声明左右两个电机的结构体，然后将前面算好的PID的3个参数传进去，完成PID控制器（左右电机）的初始化。
static PID_TypeDef pid_motor_L;//左电机调速系统的PID控制器 static PID_TypeDef pid_motor_R;//右电机调速系统的PID控制器 // //@简介：初始化左右电机的调速系统 // void App_motor_Init(void) { PID_Init(&pid_motor_L,0.5,7,0); PID_Init(&pid_motor_R,0.5,7,0); }接着编写进程函数
// //@简介：电机调速系统的进程函数 // void App_motor_Proc(void) { PERIODIC(1); //#1.通过编码器获取左右电机旋转的角速度 float omega_L=App_Encoder_Getspeed_L(); float omega_R=App_Encoder_Getspeed_L(); //#2.计算PID控制器的输出 float ua_L=PID_Compute(&pid_motor_L,omega_L); float ua_R=PID_Compute(&pid_motor_R,omega_R); //#3.将电压Ua设置到电机两端 //获取电池电压Vbat float Vbat =App_Bat_Get(); //计算左右电机的PWM信号的占空比 float duty_L=ua_L/Vbat*100.0f; float duty_R=ua_R/Vbat*100.0f; App_PWM_Set_L(duty_L); App_PWM_Set_R(duty_R); }其中设置电压的部分，之前的学习中，是通过设置占空比来间接设置的。上图是占空比和Ua的转换关系。
最后需要给用户留一个设置左右电机转速的接口
// //@简介：用来设置左右电机的转速omega_L和omega_R的值 //@参数：omega - 表示电机的转速，单位是rad/s // void App_motor_SetOmega_L(float omega) { PID_ChangeSP(&pid_motor_L,omega); } void App_motor_SetOmega_R(float omega) { PID_ChangeSP(&pid_motor_R,omega); }写完以后，对上面的测试代码进行测试。
每隔1s加大电机转速，让电机实际转速跟踪设定转速，观察跟踪是否紧密。
实验思路如下
具体测试的初始化和进程函数我已经卸载main,c里面了。
下面是呈现阶梯状转速的思路
两个整数相除会忽略余数，示例中13.5s的时候，首先是毫秒单位13500ms，转换成13s，然后取余数3s，乘2结果变成6rad/s。
下面是根据上面的思路编写的测试函数
#include "stm32f10x.h" #include "bat_test.h" #include "app_bat.h" #include "app_button.h" #include "app_pwm.h" #include "pwm_test.h" #include "encoder_test.h" #include "MPU6050_test.h" #include "app_encoder.h" #include "app_motor.h" #include "delay.h" #include "app_usart2.h" #include "task.h" static float targetOmega; static void USART2_Proc(void); int main(void) { NVIC_PriorityGroupConfig(NVIC_PriorityGroup_0); MPU6050_EularAngleTest(); // MPU6050_Test(); // Encoder_T_Method_Test(); // Encoder_Test(); // bat_test(); // pwm_test(); App_USART2_Init(); App_Bat_Init(); App_Button_Init(); App_PWM_Init(); App_Encoder_Init(); App_motor_Init(); while(1) { targetOmega=(GetTick()/1000)%10*2.0f; App_motor_SetOmega_L(targetOmega); App_motor_SetOmega_R(targetOmega); App_Bat_Proc(); App_Button_Proc(); App_motor_Proc(); USART2_Proc(); } } static void USART2_Proc(void) { PERIODIC(10) float omega_l=App_Encoder_Getspeed_L(); float omega_r=App_Encoder_Getspeed_R(); My_USART_Printf(USART2,"%.3f,%.3f,%.3f\n",targetOmega,omega_l,omega_r); }测试代码完成后，在PID控制器方面还有缺陷，需要改进后再进行测试。
第十一部分 PID改进
改进的方面主要有以上四个方面
输出限幅主要限制CO的值，要根据被控对象的现实物理条件进行合理的限制。
比如Ua的输出值，由于Vbat的限制，控制0~100%的占空比，范围在-8.4~+8.4，超过这个范围的都应该被强制拉到最高或者最低值。
上图是代码实现的具体思路。先在结构体里定义这两个变量，在初始化的时候设置好上下限，图上表示无穷大和无穷小。再加入一个设置上下限的接口。在计算PID的函数里，再对输出值进行判断，进行限幅。
具体的代码呈现和上图代码差不多。
积分限幅方面，如下
// //@简介：执行一次PID运算 //@参数 FB - 反馈的值，也就是传感器采回的值 //@返回值：PID控制器计算的结果 // float PID_Compute(PID_TypeDef *PID,float FB) { float err=PID->SP - FB; uint64_t t_k=GetUs(); float deltaT=(t_k - PID->t_k_1)* 1.0e-6f; float err_dev=(err - PID->err_k_1) / deltaT; float err_int=PID->err_int_k_1 + (PID->err_k_1 + err)*deltaT * 0.5f; float COp=err*PID->Kp; float COi=err*err_int; float COd=err*err_dev; float CO=COp + COi + COd; //更新 PID->t_k_1=t_k; PID->err_int_k_1=err_int; PID->err_k_1=err; //输出限幅 if(CO>PID->upperlimit) CO= PID->upperlimit; if(CO<PID->lowerlimit) CO= PID->lowerlimit; //积分限幅 if(PID->err_int_k_1>PID->upperlimit) PID->err_int_k_1=PID->upperlimit; if(PID->err_int_k_1<PID->lowerlimit) PID->err_int_k_1=PID->lowerlimit; return CO; }在compute函数里加入输出限幅和积分限幅。
PID复位就是通过小车上的按钮控制电机的开关时，需要在小车重启的时候把相关参数复位，避免残留数据对新的运行产生影响。
主要是这些临时存储的数据需要清零。
// //@简介：对PID控制器进行复位 // void PID_RESET(PID_TypeDef *PID) { PID->err_int_k_1=0.0f; PID->err_k_1=0.0f; PID->t_k_1=0; }编写一个复位函数
// //@简介：开关电机 //@参数: on - 控制电机的开关，0 - 关闭 非零 - 开启 // void App_motor_Cmd(uint8_t on) { App_PWM_Cmd(on); //在开关电机的同时需要对PID控制器进行复位 PID_RESET(&pid_motor_L); PID_RESET(&pid_motor_R); }并且在按下按钮的时候调用复位函数，回到之前编写的按钮相关函数里面
static void OnUserKey_Clicked(uint8_t clicks) { if(clicks==1) { //翻转电机状态 if(pwm_on==0) { pwm_on=1; } else { pwm_on=0; } App_motor_Cmd(pwm_on); } }根据状态量控制电机的状态。这里之前写的是App_pwm_Cmd(pwm_on)，现在把这个嵌入在motor的Cmd里面了。
最后的改进，首次运行时忽略积分和微分。
第一次运行的时候只有积分项，没有微分项和积分项，所以要忽略。解决的办法是判断一下三个临时参数是不是0，就知道是不是第一次运行了。
uint64_t t_k_1;//t[k-1],上次运行PID的时间 float err_k_1;//err[k-1],上次运行PID时的误差 float err_int_k_1;//err_int[k-1],上次运行PID时的积分值就是如图的这三项
float err_dev=0.0f; float err_int=0.0f; if(PID->t_k_1!=0) { err_dev=(err - PID->err_k_1) / deltaT; err_int=PID->err_int_k_1 + (PID->err_k_1 + err)*deltaT * 0.5f; }首先先令积分项和微分项为0，然后判断一下是否为0，第一次为0就不进行运算了。
接着测试一下效果。
测试结果如下，PID控制器能够正常跟踪。
