路径跟踪算法之PID、PP、Stanley详细理解

一、前言

今天又来补作业了！

---

在跟踪控制领域，PID（Proportional-Integral-Derivative, 分别为比例、积分、微分）、PP（ Pure-Puresuit, 纯跟踪）、Stanley（前轮反馈控制）是三种最为常见的路径跟踪算法。

下面将会以 如何使用 如何最简单理解 的角度进行说明，文章的表达方式比较随意，有稿纸书写的，也有电子记录的，也有代码片段的，终极目的是通俗易懂，希望大家都能有所收获。

二、PID算法

先看一段PID进行横向跟踪的核心代码实现：

// 计算机器人位置到参考位置的夹角
double alpha = atan2(refer_path.y-robot_state.y, refer_path.x-robot_state.x);
// 及算法机器人位置到参考位置的距离
double l_d = sqrt(pow(refer_path.x-robot_state.x,2)+pow(refer_path.y-robot_state.y,2));
// 计算机器人朝向角与参考位置的角度
double theta_e = alpha-robot_state.psi;
// 计算机器人与参考位置的y向距离偏差
double e_y = l_d*sin(theta_e);
// pid求解转向控制量
double delta_f = PID.calOutput(e_y);

///
// PID控制的实现
/**
  * kp 代表比例系数
  * ki 代表积分系数
  * kd 代表微分系数
  * sum_error 表示累计误差
  * lower 代表最小控制量
  * upper 代表最大控制量
  * pre_error 代表上一次误差
*/
double PID_controller::calOutput(double error) {
    double u = error * this->kp + this->sum_error * this->ki + (error - this->pre_error) * this->kd;
    if (u < this->lower)
        u = this->lower;
    else if (u > this->upper)
        u = this->upper;
    this->pre_error = error;
    this->sum_error = this->sum_error + error;
    return u;
}

其实PID的核心代码就只有上面一小段！

计算机器人与参考位置的y向距离偏差 可以看下面的示意图

如何理解PID算法呢？直接给出公式，$\delta \left(k\right)$就是下一时刻的控制量。

PID的三个参数如何调整呢？直接给出调节口诀
先是比例后积分，最后再把微分加
曲线振荡很频繁，比例度盘要放大
曲线漂浮绕大湾，比例度盘往小扳
曲线偏离回复慢，积分时间往下降
曲线波动周期长，积分时间再加长
曲线振荡频率快，先把微分降下来

下面也给出比较推荐的几篇文章：
通俗理解：https://blog.csdn.net/m0_38106923/article/details/109545445
原理理解：https://zhuanlan.zhihu.com/p/562593474
调参技巧：https://zhuanlan.zhihu.com/p/85020348

三、Pure-Puresuit 算法

以最简单的差速模型来进行分析


对于其他模型，基本原理也是一样，只是要根据对应模型去进行推导就行。

车辆模型具体可以参考：https://blog.csdn.net/WaiNgai1999/article/details/132062188

四、Stanley算法

Stanley基本上都用于车辆模型，核心思想是基于车辆 前轴中心点 的路径跟踪偏差量对方向盘转向控制量进行计算（pp算法是以后轴中心定义的），下面简单以车辆模型进行推导。

控制模型：

控制量（转向角）推导如下：

Stanley算法收敛性的验证：

五、总结

1、PP 和 Stanley 都是几何跟踪算法，其最大的区别是PP以后轮为中心，Stanley以前轮为中心；
2、PID仍然是目前运用最为广泛的跟踪算法，其计算简单，但调参需要经验；
3、PP 和 Stanley 能够较好的对未来的路径进行调控，而PID更容易出现超调现象；