路径跟踪算法---PID实现

---

前言

在获取机器人路径以后，下一步任务就是控制机器人沿着给定的路径进行运动，常用的控制算法有PID控制、纯路径跟踪算法（PurePursuit）、线性二次型调节器（LQR）、模型预测控制（MPC）等，本文首先介绍如何使用PID算法路径跟踪。

---

一、PID控制

1. PID简介

PID算法是一种经典、简单、高效的动态速度调节方式，Kp为比例系数，Ki积分系数，Kd为微分系数。
$$U(t)=K_p\ast e(t)+K_i{\int }_0^{t}e(t)d(t)+K_{d}d(e_t)/dt$$
通常依据控制器输出与执行机构的对应关系，将基本数字PID算法分为位置式PID和增量式PID两种，参考博客：空虚法师
a.Kpa.err + a.Kia.integral + a.Kd*(a.err-a.err_last);
位置式PID：
$$K_p\ast e(k)+K_i\ast \sum _{i=0}e(i)+K_d\ast (e(k)-e(k-1))$$
e(k)：误差 = 目标值 - 当前值
$\sum e(i)$：误差累加
e(k) - e(k-1)： 本次误差- 上次误差
增量式PID：
  K p ∗ ( e ( k ) − e ( k − 1 ) ) + K i ∗ e ( k ) + K d ( e ( k ) − 2 e ( k − 1 ) + e ( k − 2 ) ) \ K_p*(e(k) - e(k-1)) + K_i*e(k) +K_d(e(k) - 2e(k-1) + e(k-2))  Kp​∗<