路径跟踪算法之PID路径跟踪与PP跟踪

1路径跟踪原理

在运用好的路径规划算法，规划好一条从起点到目标点最优的路径后，如何
让机器人来寻着这条最优的路径走，便是路径跟踪(Path tracking)需要解决的问题。基于平面坐标系下设定一条理想的几何路径，然后要求机器人从某一处出发，按照某种控制规律到达该路径上，并实现其跟踪运动。

路径跟踪的实质是通过控制车辆的运动来减少车辆与参考轨迹之间的空间
误差，如果考虑轨迹的话，那么还包含了时间上的误差。

2 常见的路径跟踪算法

常见的路径跟踪算法可分为几何跟踪和模型跟踪：
几何跟踪是基于车辆的运动学几何结构，推导出控制量实现跟踪，常见的几何跟踪算法有纯跟踪算法（purePuresuit）、Stanley method 前轴控制、Rear_wheel_feedback 后轴控制等，其中后两者都是基于阿克曼模型实现的，而 pp 跟踪是在双轮乃至四轮差速模型和阿克曼模型中都相应的应用。

模型跟踪则是基于算法求解模型，结合机器人运动学与动力学的控制规律，通过最优解或稳定性调节等方式计算得到跟踪控制量的跟踪控制方式，常见的模型跟踪算法有很多，比如经典的 PID 控制、滑膜控制、线性二次型调节器控制、模型预测控制（MPC）等，不同的控制方式对应着不同的模型系统，可以是线性的，也可以是非线性的，可以是模型精度高的，也可以是不需要明确模型的，建模方式呈现多样化。

下面对最常见的路径跟踪算法-PP跟踪算法和PID跟踪算法的原理实现及应用进行展开叙述.

2.1 Pure puresuit(pp)纯跟踪

纯跟踪算法（以下简称 pp 算法）在低速情况下运用得非常广泛，其主要原理基于机器人圆弧轨迹运动的特性，建立目标点和机器人当前位置之间的几何关系表达式，从而得出机器人当前时刻的角速度控制量。
以双轮差速移动机器人模型为例，根据机器人圆弧运动的特性，在物理学上得出小车运动线速度v,角速度w及运动半径r之间的关系如下：

曲率与半径之间的固定关系如下：


如图 所示，以小车自身的局部坐标系为基准坐标系，小车的当前位置P(x0,y0)，为原点，目标点选定为P(xg,yg)。目标点与小车当前位置的距离 L 通常称为前视距离，在坐标系中表达式如下：

另，从圆弧运动的几何关系上可得：

联立式 1-3，式 1-4，式 1-5 可得基于前视距离和横向距离的运动半径表达式：

联立 1-1，式 1-2，式 1-6，得出给定线速度下的角速度调节量：

设置固定的前视距离和路径曲率肯定无法适应不同的路径，因此就需要对于
前视距离的计算方法进行研究改进。从平时我们开车时的习惯来讲，我们会根据不同驾驶速度和不同路段，进行判断合适的视线跟踪点。因此，我们就可以将这个过程抽象出来，加以处理，形成一个选择前视距离的规则。一般情况下，纵向速度在路径曲率较大的地方应做减速处理，而前视距离与纵向速度呈正比关系。
下图是双轮差速模型 pp 算法跟踪仿真效果图。

其实根据 pure pursuit 的原理可以知道，其跟踪效果很大程度上取决于前视距离 ld 的选择。该算法多数用于一些简单低速的场景；对于一些复杂的路径跟踪效果可能会比较差，例如 U 型/S 型等曲线路径。但是 pp 跟踪的算法实现直观简洁，跟踪流程设计稳定高效，在低速导航领域应用广泛。

2.2 PID 跟踪

PID 控制方法具有结构简单、调整方便等特点，被广泛应用。当被控对象的模型结构和参数不全，可以采用 PID 控制，其参数通过经验和现场调试来确定。

在路径跟踪控制中，根据轨迹的纵向/横向误差、角度/曲率误差或者若干车
辆姿态状态变量的综合误差，利用比例、积分、微分参数计算出跟踪已有路径所需的方向盘转角，完成机器人的轨迹跟踪目。
PID 控制算法在无人驾驶路径跟踪中普遍运用，apollo 的开源平台代码的轨迹跟踪部分的纵向控制便是基于三个 PID 控制环控制刹车、油门、档位等实现对车速的控制。PID 控制算法不需要被控对象的模型结构和参数，只需要得到车辆与目标轨迹之间的偏差就可以达到很好的控制效果。如图所示是基于机器人与路径横/纵向误差及航向量误差综合控制律简单实现的 PID 路径跟踪仿真图。

PID 算法的缺点在于，针对不同的模型或者路径状况，可能需要不同的 PID参数才能实现稳定控制，PID 参数的标定有时需要大量的实验测试；同时纯 PID的鲁棒性并不好，抗干扰能力不强，由于是基于反馈控制的方式，控制量是滞后输出的。PID 控制的一些改进控制方式包括模糊 PID，神经网络 PID 等。

这里附上参考博客：
基于pid的路径跟踪_智驾技术 | 路径跟踪控制

这里附上相关仿真代码链接：
1.pp跟踪仿真代码链接（非免费哦，技术问题可以私信交流）
2.pid仿真代码链接（非免费哦，技术问题可以私信交流）