什么是纯追踪算法？

回帖（1）

王禅

2021-11-22 09:30:10

纯追踪算法

自行车模型

在了解纯追踪算法之前，需要先了解车辆模型，一个简化的自行车模型。
运动学自行车模型和动力学自行车模型
轨迹追踪

目前主流的轨迹跟踪方法有两类，基于几何的追踪方法和基于模型预测的方法，纯追踪就是基于几何追踪的方法。下图是一个运动学自行车模型：

它是对阿卡曼转向几何的简化，将车辆四轮模型简化为两轮模型，并且假定车辆只在平面上行驶，那么前轮转角δ、轴距和转向半径将遵循以下关系：

其中δ是前轮的转向角，L为轴距，R为当前转角下的转向半径。
从自行车模型出发，纯追踪算法以车后轴为切点、车辆纵向车身为切线，通过控制前轮转向角，使车辆可以沿着一条经过目标路经点的圆弧行驶，如下图所示：

图中（gx, gy）是我们下一个要追踪的路点，它位于我们已经规划好的全局路径上，现在需要控制车辆是的车辆的后轴经过该路点，表示车辆当前位置（即后轴位置）到目标路点的距离，表示目前车身姿态和目标路点的夹角，那么更具正弦定理我们可以推导出如下转换式：

上式也可以表示为：

其中 k 是计算出来的圆弧的曲率，那么前轮的转角 δ 的表达式为：

结合以上两式，我们可以得出纯追踪算法控制量的的最终表达式：

这里我们把时间考虑进来，在知道t时刻车身和目标路点的夹角 α(t) 和距离目标路点的前视距离 ld 的情况下，由于车辆轴距 L固定，我们可以利用上式估计出应该作出的前轮转角 δ，为了更好的理解纯追踪控制器的原理，我们定义一个新的量：el—— 车辆当前姿态和目标路点在横向上的误差，由此可得夹角正弦：

圆弧的弧度就可重写为：

考虑到本质是横向上的CTE，由上式可知纯追踪控制器其实是一个横向转角的P控制器，其P系数为2/ld2，这个P控制器受到参数ld（即前视距离）的影响很大，如何调整前视距离变成纯追踪算法的关键，通常来说，ld被认为是车速的函数，在不同的车速下需要选择不同的前视距离。
一种最常见的调整前视距离的方法就是将前视距离表示成车辆纵向速度的线形函数，即ld=kVx，那么前轮的转角公式就变成了：

那么纯追踪控制器的调整就变成了调整系数k，通常来说，会使用最大，最小前视距离来约束前视距离，越大的前视距离意味着轨迹的追踪越平滑，小的前视距离会使得追踪更加精确（当然也会带来控制的震荡），下面我们使用Python实现一个简单的纯追踪控制器。

本文使用Matlab和Simulink构建了一个简单的纯追踪模型并进行了仿真,整体流程框图如下：