园区自动驾驶实车平台决策规划控制系统（二）——基于PID算法的纵向控制（C++实现）

相比横向控制，纵向控制的实现不是那么直观。因此在讲实现过程之前，先回顾下本科学过的汽车知识。

上篇提到的横向控制是通过：方向盘->前轮转角->车的航向角 的直观控制过程。

纵向控制的原理和实现b站老王讲的很棒，链接附在文末了，感兴趣的话建议看一下。

纵向控制需要分成加速和制动来分别分析：

加速过程：油门踏板->发动机功率增大->发动机转速(控制车速v)和发动机扭矩(控制加速度a>0)增大 ->车加速->速度增大->位置变化率+

制动过程：刹车踏板->制动压力增大->制动盘摩擦增大->阻力增大->车减速>速度减小->位置变化率-

由于项目结题迫在眉睫，人力时间都十分有限，本文对控制方法做了简化处理，没有具体做发动机功率和油门开度的标定，也没有做刹车踏板和制动盘摩擦力的标定。而是把PID的速度控制输出u和油门刹车开度线性处理了，即：

int k_cmd=6;    //pid输出的比例系数
// 输出小于0，需要刹车减速；输出大于0，则是油门加速
if (pid_output < 0)
{
    cmd_acc_ = 0;
    cmd_brk_ = -k_cmd* pid_output;
    // 为了应对紧急工况，允许刹车踏板踩到底
    if (cmd_brk_ > 100)
    {
        cmd_brk_ = 100;
    }
    else if (cmd_brk_ < 0)
    {
        cmd_brk_ = 0;
    }
}
else
{
    cmd_acc_ = k_cmd*pid_output;
    cmd_brk_ = 0;
    // 由于是低速场景，对最大油门开度进行限制
    if (cmd_acc_ > 40)
    {
        cmd_acc_ = 40;
    }
}

纵向控制的输入是由决策层发来的期望速度和传感器反馈的当前速度，输出的是控制速度的油门/刹车踏板开度。横纵向控制对接的都是线控系统，线控系统是已经做好了的，因此，完成横纵向控制后，就可以实现基本的循迹功能。

园区自动驾驶（二）——基于PID的纵向控制

PID是一种广泛应用于各种工业控制场合，无论是无人机的飞行姿态和飞行速度还是机器人的移动方向和移动速度都能使用PID控制。最新的研究有用遗传算法等方法来寻找最优Kp，Ki和Kd参数，但在实际工程领域，根据工程经验和实际场景调试参数Kp，Ki和Kd还是占多数。PID的流程图和公式如下所示，后面会举例说明。

比例控制方法

一文读懂PID控制算法（抛弃公式，从原理上真正理解PID控制）_确定有穷自动机的博客-CSDN博客_pid控制

这位大佬对PID的举例很清晰，之前如果没了解过PID的建议看看原文。对PID的理解可以这样解释：

先说明PID中最基础的比例控制P，I和D先不考虑。假设无人驾驶车目前是静止状态，现在将车辆的预期车速target设为10km/h（期望车速保持在10km/h），那么当前时刻的车速actual与目标车速target之间就存在了误差error，且error=target-actual（这里的误差不能做绝对值处理，正负不能反），error=10。假设现在踩下油门踏板，让车速先控制到车速 u 。如果只考虑比例项，就是指每次改变的速度u和误差error是成正比的。即：

假设Kp取0.5，

那么t=1时（表示第1次加速，也就是第1次对系统施加控制），那么u=0.5*10=5，所以这一次加速会使速度在0的基础上提速5，达到5 km/h。第二次循环时当前车速actual=5，target=10，则u=0.5*(target-actual)=7.5。如此循环下去，最终会达到目标车速10 km/h。 

但是，上述过程只是我们想象的车速控制，实际控制中会存在稳态误差。什么叫稳态误差呢？

上文的车速控制的改变量 u 是通过油门踏板实现的，认为只要有油门踏板就会实现速度改变量u。但现实中踩下油门踏板后不一定速度会增加，由于道路阻力(指坡度阻力+滚动阻力)的存在，踩下油门踏板车速不一定增加，也可能不变(动力和道路阻力平衡)，甚至可能减小(只要坡度够大)。这里道路阻力和动力平衡的稳态即是速度控制的稳态误差。

因此，只要当error减小到一定程度，就会出现比例输出 u 对应的油门踏板产生的动力与道路阻力平衡的稳态误差，导致无法达到预期的控制效果。

所以仅靠比例控制是不够的，还要加入积分控制。

积分控制方法

为了解决稳态误差的问题，引入了积分项，该项和误差的积分成正比，于是u变为了：

继续刚才的例子，假设当前车速actual=7.5，error=2.5，u=1.25时出现稳态误差，如果没有积分项，error会一直等于2.5。但加入积分项后，第一次error=10，第二次error=5，第三次error=2.5，积分项 ∫error 不断增大，使得输出 u 也越来越大，直到油门踏板对应的动力输出大于道路阻力，消除稳态误差。

微分控制方法

由于怕噪声产生干扰，且低速行驶下超调量也不会太高，因此本文最后只用到了PI两项的控制。

基于C++的PID算法实现(核心代码)

//计算误差
PID_position::PID_position(float p, float i, float d)
    : kp(p), ki(i), kd(d), target(0), actual(0), integral(0), integral_limit(100)
{
    e = target - actual;
    e_pre = e;
}

// 设置积分上限，正负号分别代表油门和刹车
float PID_position::pid_control(float tar, float act)
{
    float u;
    target = tar;
    actual = act;
    e = target - actual;
    integral += e;
    if ((ki * integral) > integral_limit)
    {
        integral = integral_limit / ki;
    }
    else if (integral < (-1 * integral_limit))
    {
        integral = -1 * integral_limit;
    }
    u = kp * e + ki * integral + kd * (e - e_pre);
    e_pre = e;
    return u;
}

方法缺陷

本方法优点是可以很快进行落地，但因为时间仓促，采用方法也有很多不足之处：纵向控制的控制方法很粗糙，只是实现了基本的车速控制，没有制定精确的油门/刹车开度；且纵向控制应该包括纵向加速度、纵向速度和纵向位置的控制，本方法只能控制纵向速度，而不能控制纵向加速度和纵向位置。

参考

【基础】自动驾驶控制算法第九讲 纵向控制开篇_哔哩哔哩_bilibili

基于C++的PID控制器_pid c++_Alexon Xu的博客-CSDN博客