自动驾驶-PID控制器

在自动驾驶中控制车辆是指使用方向盘，刹车，油门将车开到目的地。控制车辆的算法一般称为控制器，PID控制器是最常见，最基础的控制器之一，PID控制器是Proportion Integration Differentiation 的缩写，由英文名可以看出，PID控制器分别代表比例，积分，差分。本章介绍PID控制器每一项的物理意义和代码实现。

前边的卡尔曼滤波和粒子滤波的章节分别介绍了识别其他车辆和定位自身的方法，另外控制器可以从地图得到道路轨迹，控制器就需要根据这些输入计算控制器的输出，使车辆能够平稳的沿着道路行驶。车辆的控制器应该满足以下要求：

1. 控制器必须准确， 避免车辆偏离目标轨迹；
2. 控制器具有可行性；
3. 控制器操作平稳；以免造成乘客的不适；

我们将根据地图计算出的车辆运行轨迹称为参考线，车辆与参考线的偏差称为CTE(cross track error), PID控制器的P代表比例，其表示车辆的航向角反比与CTE，即当CTE减小时，航向角也会相应减小，但是如下图所示，车辆实际行驶的路线会沿着参考轨迹周围摆动，所以P控制器并不稳定。

D控制器表示差分，差分项相当于给控制器增加了阻尼，使航向角改变的速度减慢，提达到平稳的效果，如下图所示，增加差分项以后，车辆的摆幅变小。

PID控制器的I代表积分项，负责纠正系统的系统性偏差，例如如果转向角失准，就要将方向盘偏移才能是车保持直线， 积分项会对系统的累积误差进行惩罚，以达到纠正系统误差的效果。

以下就是PID控制器的C++ 代码实现。

#include "PID.h"

using namespace std;

/*
* TODO: Complete the PID class.
*/

PID::PID() {}

PID::~PID() {}

void PID::Init(double Kp, double Ki, double Kd) {
    p_error = 0.0;
    i_error = 0.0;
    d_error = 0.0;
    
    Kp_ = Kp;
    Ki_ = Ki;
    Kd_ = Kd;
}

void PID::UpdateError(double cte) {
    d_error = cte - p_error;
    i_error +=cte;
    p_error = cte;
}


double PID::TotalError() {
    return -Kp_*p_error-Ki_*i_error-Kd_*d_error;
}

完整的项目代码请戳这里。