6.PID算法解析及实现

星火计划2.0基础课：https://apollo.baidu.com/community/online-course/2
星火计划2.0专项课：https://apollo.baidu.com/community/online-course/12

串级PID

PID调试方法:

Apollo代码介绍：

Apollo中的PID代码实现：

输入量是偏差值以及模块执行的频率，输出量为PID的计算结果。

积分遇限消弱法是判断当前的偏差量是否和控制量处于同方向，当控制量的输出值已经达到限幅了，说明控制已经跟随不上了，此时积分也就没什么作用了

反馈抑制抗饱和法是判断当系统处于饱和状态的时候，通过对积分项加入一个负反馈，即将u限幅后与原来的u做差值，判断差值的大小，如果限幅后的数值比原来更大，u可能本来就是负的，说明其为反方向的控制量，那么计算增益的时候引入kaw变量，其设置的范围为0.3到3(ki/kp)这里默认使用系数为1.

以上为纵向控制代码结构，右侧为纵向控制标定表

现在来看纵向控制代码：

首先是构造函数：

其初始化了调试过程中csv，将不同变量的标题写入

然后是加载纵向控制的配置参数

首先拿到纵向控制的配置文件

然后做一些安全性的检查

将配置文件中的一些数值读出来

定义一些补偿的环节(超前滞后补偿)

初始化位置pid参数

可以从配置文件看到积分是未启用的，且PID目前只使用了Kp

在速度PID中一开始初始化了低速PID的配置参数(ki kp)

针对控制中的一些问题，比如数字滤波器，主要处理pitch俯仰角的补偿。然后加载标定表，因为控制代码中并没有加入纵向动力学模型，单纯就是根据标定表来对控制结果进行输出查找

在加载标定表的过程中还会引插值

配置文件中根据当前所使用的车辆，根据当前的速度和加速度给出油门踏板的输出值。不同的车输出的类型可能不同，有输出踏板的，有输出加速度的，有输出扭矩的。

在控制计算过程中，最主要的部分为如下函数：

其可以根据定位的信息，底盘的信息，planning给出的轨迹计算出控制量，放入到cmd变量(指针)中

首先检测刚才初始化的标定表是否是有效的

然后判断planning传过来的指针是否为空

并且初始化一个trajectory_analyzer_，其为常用的轨迹分析中包含的工具，其会计算当前车辆的状态偏差(比如横纵向偏差)

debug存储了当前车辆计算出的控制量

初始化刹车油门，拿到配置文件中模块的执行频率，设置预瞄时间，根据未来时刻的偏差值考虑是否需要变化当前的控制量的增益以便适应未来的变化。

坐标系的转换函数以及匹配点的查找，计算出车辆状态后保存到debug中，输出到日志中，可以打印一下这些状态信息看一下车辆的运动状态。

根据档位判断初始化了不同的参数，比如倒车的时候设计了位置和速度不同的PID，如果引入超前滞后调节还会引入不同的PID.

这部分比较重要，为位置PID的求解

其参数为：

其计算出的offset值又作用到了

speed__controller_input的输入端,其结合位置环求解出补偿再加上当前速度的偏差共同作用，限幅后放入到speed_pid_controller_中

计算出一个加速度闭环的数值。

位置环计算出的是一个速度值，速度环计算出的是一个加速度的值。通过这两环相串，计算出一个加速度的值。

根据加速度的值结合其他的补偿(考虑到车辆的俯仰角，即上下坡)，得出最后加速度的cmd

限幅后进行插值：

然后得到标定表的结果

即首先根据位置的偏差计算出一个加速度输入的offset，然后将该值和速度的偏差输入到PID里面，输出一个加速度的控制指令，通过一系列后处理，从标定表中插值出最终的控制结果，这个控制结果限幅之后就赋给了油门和刹车指令

最终给cmd赋值