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仿真代码链接（非免费哦，技术问题可以私信交流）

路径跟踪算法之PID路径跟踪与PP跟踪相关推荐

极值跟踪算法 c语言,快速约束极值子空间跟踪算法_魏志强.pdf
快速约束极值子空间跟踪算法_魏志强 34 4 Vol. 34 No. 4 第卷第期华北水利水电学院学报 2013 8 Journal of North China Instit ...
无人车采用纯跟踪算法跟随离线路径（ROS，C++实现）第一部分
一前言最近是由于实验室需求,做了一个简单的无人车轨迹跟踪算法,最主要要完成的功能还是希望无人车能够跟随离线轨迹动起来,因为自己也是在学习阶段,第一次接触这一块的知识,所以就写了这个博客,希望自己能 ...
a算法和a*算法的区别_机器人路径规划算法，全局路径规划与局部路径规划究竟有哪些区别？...
若步智能移动这一简单动作,对于人类来说相当容易,但对机器人而言就变得极为复杂,说到机器人移动就不得不提到路径规划,路径规划是移动机器人导航最基本的环节,指的是 ...
python路径规划算法可视化_路径规划问题：DIJKSTRA算法以及Python实现
一. DJKSTRA算法概述我们可以将地图抽象为Graph的数据结构,然后利用Graph的广度优先遍历算法(Breadth-First Search, BFS)解决无权重的High-Level的地图 ...
基于MeanShift的目标跟踪算法及实现
from: http://blog.csdn.net/jinshengtao/article/details/30258833 一.简介首先扯扯无参密度估计理论,无参密度估计也叫做非参数估计,属于数 ...
基于强化学习的智能机器人路径规划算法研究（附代码）
目录一.摘要二.路径规划技术的研究进展 1.研究现状 2.算法分类 2.1 全局路径规划算法 2.2 局部路径规划算法三.本文采用的路径规划算法--强化学习 1. 概念 2. 与其他机器学习方式 ...
基于MeanShift的目标跟踪算法及实现（转载）
版权声明:本文为博主原创文章,未经博主允许不得转载. https://blog.csdn.net/jinshengtao/article/details/30258833 这次将介绍基于MeanShi ...
DWA局部路径规划算法论文阅读：The Dynamic Window Approach to Collision Avoidance。
DWA(动态窗口)算法是用于局部路径规划的算法,已经在ROS中实现,在move_base堆栈中:http://wiki.ros.org/dwa_local_planner DWA算法第一次提出应该是1 ...
目标跟踪算法_Camshift函数(学习笔记)
在这一节中,主要讲目标跟踪的一个重要的算法Camshift,因为它是连续自使用的meanShift,所以这2个函数opencv中都有,且都很重要.为了让大家先达到一个感性认识.这节主要是看懂和运行op ...

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