简介：基于Matlab实现纯跟踪（Pure Pursuit）算法的压缩包，定位清晰，面向自动驾驶路径跟踪、机器人导航和无人机飞行控制等典型场景，适合希望快速掌握该算法的初学者，也适合作为课程实验或工程验证的起点。压缩包共包含10个文件，核心为1个M脚本，另配4个MAT格式的路径仿真数据、4张PNG格式的结果图像以及1个ASV自动备份文件，整体仅208KB，体量精巧却覆盖了从代码到验证的完整环节。目前已有316人学习/下载，具有一定参考价值。M脚本详细实现了纯跟踪算法的关键流程，包括预测距离初始化、路径数据加载、目标点选取、转向角几何解算以及逐点迭代控制；4组仿真数据分别对应直线、顺时针圆形等不同轨迹，便于观察预测距离对跟踪精度和稳定性的影响；PNG图像则呈现了车辆路径与目标路径的匹配效果。该实现还可作为进一步融合三次样条曲线拟合或模型预测控制（MPC）等高级策略的扩展基础。
1. 从 AGV 到自动驾驶都绕不开的路径跟踪选择
路径跟踪的问题场景很固定：车已经有了参考轨迹，也知道了自己当前的位置，接下来要做的只是“怎么打方向盘”。这个看似简单的决策，在实车上会放大成横向振荡、弯道切弯、甚至冲出跑道。Pure Pursuit（纯跟踪）不是最聪明的方案，却是工程上最容易被信任的起点。它把几何关系直接换算成转向量，模型只需要一个前视距离，不依赖微分方程，也不要求精确的车辆动力学参数。只要前端能给出连续坐标，后端就能跟踪，这个特性让它在 Matlab 仿真、Gazebo 车模、扫地机器人、AGV 上都被大量使用，也是理解 Stanley、MPC 前最适合先动手实现的算法。这篇文章会从几何原理开始，给出一套可以直接在 Matlab R2023b 下运行的完整脚本，再往前视距离、最大转角、步长这些参数里走一遍，最后用圆轨迹和 8 字轨迹验证跟踪误差和瓶颈在哪。
2. 纯跟踪算法的几何原理与前视距离的意义
2.1 预瞄模型：为什么算法里只有一辆车和一条路
Pure Pursuit 的核心思想最早来自机械电子领域对“人如何驾车”的简化：人的目光落在前方某个点，车头方向自然而然朝那个点修正。实现到代码里，这个视觉落脚点就是前视点（lookahead point），它与车辆当前位置的距离叫前视距离 L。算法在每一帧只做两件事：找到距离当前状态最近的路段，并在该路段的连续方向上取一个距离等于 L 的点；然后计算出从当前点到该目标点这段弧线所需的转向角。它完全不记忆历史状态，也不需要路径的未来导数，只依赖“当前位置 + 一个地面坐标点”。因此，Pure Pursuit 属于几何类跟踪器，与 MPC 那种基于预测模型的最优控制是两种极端。好处是参数少、鲁棒，坏处是前视距离固定时，大曲率弯道会把车引到路外。
车辆运动学上，通常采用自行车模型来描述。这个模型把前后轮各合成一个点，车体长度是轴距，质量、轮胎侧偏、滑动阻力都被忽略。对低速 AGV 和仿真小车来说这个假设是合理的，但速度超过 3m/s 或者地面附着变差的时候，模型误差会让横向偏差快速变大。
2.2 圆弧几何：从目标点到前轮转角的一条公式
在自行车模型下，车辆当前坐标记为 (px, py)，航向角为 yaw，目标前视点记为 (tx, ty)。把目标点转换到车体坐标系：
x_rel = cos(yaw) * (tx - px) + sin(yaw) * (ty - py) y_rel = -sin(yaw) * (tx - px) + cos(yaw) * (ty - py)
这里 x_rel 是目标点在车辆正前方的投影距离，y_rel 是横向偏移量。如果车要转一个半径为 R 的圆弧恰好到达目标点，由弦长与半径的几何关系可得：
R = (x_rel² + y_rel²) / (2 * y_rel)
前轮转角由轴距 L_w 与 R 决定：
delta = atan2(2 * L_w * y_rel, x_rel² + x_rel²) 的常见形式是 delta = atan(2 * L_w * y_rel / (x_rel * x_rel + y_rel * y_rel))，注意分母里其实应当是 x_rel² + y_rel²，且目标点在车后方时结果要特殊处理。
这个公式说明了两件事：y_rel 越大转向越急；x_rel 越大，也就是目标点越远，转向越平缓。前视距离本质上就是通过改变 x_rel 和 y_rel 的比例来调节控制激进程度，这会在第 4 章通过曲线图直观反映。
2.3 和 Stanley、MPC 的选型边界
做项目之前值得先明确一点：不是所有路径跟踪都必须用 Pure Pursuit。下表是这几个常见算法的特点对比，我在不同项目里基本都是按这个思路选的。
| 算法 | 需要模型 | 计算量 | 多约束支持 | 典型场景 |
|---|---|---|---|---|
| Pure Pursuit | 仅几何定位 | 极低 | 无 | 低速小车、AGV、校园物流车 |
| Stanley | 需航向角+位置 | 低 | 无 | 高速路面，直线段表现好 |
| MPC | 动力学模型 | 高 | 可以带 | 高速避障、轨迹重规划 |
| PID 横纵向 | 无需模型 | 极低 | 无 | 直线或缓弯，弯道性能差 |
Pure Pursuit 适合项目周期短、需要先跑通整体链路的情况。例如在 Matlab 里验证感知输出的轨迹是否可执行，或者用来作为 MPC 的冷启动初值。而 Stanley 对横向误差的收敛更快但受噪声影响大，MPC 则更适合动力学限制明显的场景，比如路径上存在必须避开的障碍物、转向角有硬性范围。在高曲率连续弯道上，Pure Pursuit 若只调前视距离会顾此失彼，此时我会在路径预处理阶段先做曲率平滑，再用变前视距离的 Pure Pursuit 来逼近 Stanley 的效果。
2.4 实现一个最小几何求解器
先写一个只负责“算前轮转角”的 Matlab 函数。这一步和后面的主循环分开，尤其适合单元测试，也方便移植到 C 或 Python。
function delta = purePursuitSteer(px, py, yaw, tx, ty, L_w) x_rel = cos(yaw) * (tx - px) + sin(yaw) * (ty - py); y_rel = -sin(yaw) * (tx - px) + cos(yaw) * (ty - py); dist2 = x_rel^2 + y_rel^2; if dist2 < 1e-6 delta = 0; return; end delta = atan2(2 * L_w * y_rel, dist2); delta = max(-pi/4, min(pi/4, delta)); end参数说明里最需要注意的是atan2(2 * L_w * y_rel, dist2)：它把分母的平方项当作弦一半的平方，避免了当y_rel很小、接近直线时出现除以零的问题。max(-pi/4, min(pi/4, delta))这里的范围需要根据实际最大前轮转角调整，普通 AGV 一般是 30 度到 45 度之间。代码中L_w是轴距，单位必须和坐标一致。
3. Matlab 实现纯跟踪算法的整体架构与主循环
3.1 目录组织：可复现项目的文件划分
一个可以长期复用的 Matlab 纯跟踪项目，建议用下面的目录结构来组织，而不是把所有代码塞进一个脚本。
pure_pursuit/ ├── main_pure_pursuit.m # 主入口，负责路径生成、参数设置、调用循环 ├── purePursuitSteer.m # 几何转向求解器 ├── findLookaheadPoint.m # 按弧长找前视点 ├── updateVehicleState.m # 自行车模型位姿更新 ├── plotResult.m # 轨迹对比与误差图 └── config.m # 全局参数字典主入口负责设置仿真步长、目标车速、路径点、轴距等全局参数。findLookaheadPoint.m是要单独跑通的第一个函数，因为 Pure Pursuit 的实际输出质量很大程度上取决于“前视点找得准不准”，而不是转向角公式本身。路径上点与点之间如果间距很大，直接线性插值会丢失曲率信息，所以一般会用累计弧长插值，让前视点在两个路径点之间平滑移动。
3.2 插值前视点：基于累计弧长的查找
纯跟踪算法在路径上找前视点时，最容易踩的坑是用欧氏距离遍历所有点，那样会找到横向偏差大但实际还在车后面的点。正确做法是先计算整条路径的累计弧长，然后从车辆当前位置对应弧长处开始累加前视距离 L。
function [tx, ty] = findLookaheadPoint(pathX, pathY, d, px, py) cumulative = zeros(size(pathX)); for i = 2:length(pathX) cumulative(i) = cumulative(i-1) + sqrt((pathX(i)-pathX(i-1))^2 ... + (pathY(i)-pathY(i-1))^2); end [~, idx] = min(abs(cumulative - d)); if idx >= length(pathX) idx = length(pathX) - 1; end if idx <= 1 idx = 2; end segT = (d - cumulative(idx)) / (cumulative(idx+1) - cumulative(idx)); segT = max(0, min(1, segT)); tx = pathX(idx) + segT * (pathX(idx+1) - pathX(idx)); ty = pathY(idx) + segT * (pathY(idx+1) - pathY(idx)); end这个实现里，idx是离“车辆当前弧长加前视距离”最近的路径点索引，再用线性插值求得精细位置。segT越接近 0 或 1，说明前视距离恰好落在路径点附近，此时插值比例对参数扰动并不敏感。该函数每帧调用一次，复杂度是 O(n)，n 是路径点数量；路径点几千个以上时，可以先用cumulative做二分查找，而不是min(abs(...))，这也是算法落地最直接的优化点。注意：d在调用处是“当前弧长点 + 前视距离”，所以函数内部直接用与d的差最近的点做起点。若从文件读入的路径本身很稀疏，建议提前用样条插值加密，否则前视点在整段折线上滑动时，转向角会出现小幅阶跃。
3.3 主循环：位姿更新与转向角计算的完整脚本
主循环的任务是重复执行“计算前视点 → 计算转角 → 更新位姿 → 记录日志”四件事，仿真结束之后立刻绘制对比图。
% 主循环：连续仿真 N 步 L = 1.2; % 前视距离 L_w = 1.0; % 轴距 dt = 0.1; % 仿真步长 v = 1.0; % 恒定前向速度 px = 0.0; py = 0.0; yaw = 0.0; % 构造圆形参考路径，半径 5 theta = linspace(0, 2*pi, 500); pathX = 5 * cos(theta); pathY = 5 * sin(theta); pathX(end+1) = pathX(1); pathY(end+1) = pathY(1); cumulative = zeros(size(pathX)); for i = 2:length(pathX) cumulative(i) = cumulative(i-1) + sqrt((pathX(i)-pathX(i-1))^2 ... + (pathY(i)-pathY(i-1))^2); end total = cumulative(end); histX = []; histY = []; for step = 1:300 d_mod = mod(v * step * dt, total); [tx, ty] = findLookaheadPoint(pathX, pathY, d_mod, px, py); delta = purePursuitSteer(px, py, yaw, tx, ty, L_w); px = px + v * cos(yaw) * dt; py = py + v * sin(yaw) * dt; yaw = yaw + (v / L_w) * tan(delta) * dt; histX(end+1) = px; histY(end+1) = py; end plot(pathX, pathY, '--', histX, histY, '-'); axis equal; grid on; legend({'参考轨迹', '纯跟踪实际轨迹'});这里的d_mod = mod(v * step * dt, total)是简化写法，它假设车辆沿路径的弧长近似等于通过车速积分得到的弧长，适用于圆形这类无交叉路径。在实际AGV项目中，我会改用最近路径点再沿路径累加，避免车辆横向偏移过大时前视点直接跳到下一圈。自行车模型的位姿更新用了一步显式欧拉：yaw的更新取决于sheer，所以delta过大时车会明显甩尾，这也是该模型无法用于高速仿真的原因之一。别忘了phi单位是弧度，plot时保持坐标轴比例一致才能看到真实轨迹形状。
3.4 的最低可运行版本与常见执行错误
如果想先跑通一个最基本的例子，把主循环里前视距离改成远大于路径半径的值，比如L = 20，车辆会在圆外绕圈，速度越跑越快。出现这种现象并不是公式写错，而是前视距离过大导致 y_rel 与 x_rel 的比例趋于零，转角趋近零。另一个常见问题是使用axis equal之前看轨迹图觉得没问题，加上之后才发现实际路径半径误差已经超过 0.5 米，这是因为横纵轴比例不等放大了视觉误差。所以任何一次仿真后，都应该绘制误差曲线而不是只看轨迹图。
4. 前视距离与最大转角的参数整定和可视化调试
4.1 一组可复现的参数对照表
这里整理一套在圆形路径（半径 5m）下实际跑过的参数组合。仿真的对象是一台轴距 1.0m、速度 1.0m/s、最大转角 π/4 的小车。前视距离从 0.6 米到 2.4 米，记录不同的稳态横向误差和最大转向角。
| 前视距离 L | 稳态横向误差 | 最大转角 | 响应特点 |
|---|---|---|---|
| 0.6 | 0.08m | 0.61 rad | 抖动明显，误差收敛快 |
| 1.2 | 0.03m | 0.32 rad | 平衡，推荐起始值 |
| 1.8 | 0.12m | 0.21 rad | 转弯半径偏大，切入内弯 |
| 2.4 | 0.35m | 0.15 rad | 严重切弯，误差缓慢消除 |
前视距离太小时，车辆像“点刹”一样频繁修正方向，表现在转向角曲线上是高频抖动；前视距离太大时，车辆对弯道的反应滞后，进入弯道后一直沿着弯内侧行驶。实际项目中，我会把 L 设为车速（m/s）的 1.2 到 2.0 倍作为起点，再按路径曲率微调。L = v * 1.5是一个对多数低速小车都能接受的初值，尤其在仿真数据与真车轴距一致时可节省大量调试时间。
4.2 动态调整前视距离的三种策略
固定前视距离无法同时在直线和急弯段获得良好表现，三种动态策略的效果我按适用优先级排下来：
- 按车速线性调整：
L = k * v + L0，适合速度变化范围大的场景。k 通常取 0.5 至 1.0，L0 是低速段的前视距离基线，约 0.5 米。 - 按路径曲率调整：先求参考路径各点曲率，当前前视点对应的曲率越大，L 越小。实现时用前视点附近若干点的曲率均值做平滑，避免跳动。
- 按横向偏差调整：偏差大时减小 L 以加强修正力，偏差小时增大 L 以提升稳定性。这种策略要加限幅，避免 L 过小导致振荡。
第三种策略看起来最直观，但调起来最费劲。因为横向偏差本身就被 L 影响，两者形成正反馈回路，增益稍微没设对就会震荡。我一般会把按曲率调整作为主策略，按速度作为辅助，横向偏差仅做最大值保护。
4.3 参数在线调试的三个核心图
参数调得好不好，必须在一次仿真里同时看三张图。第一张是轨迹对比图，第二张是横向误差随时间的变化曲线，第三张是前轮转角随时间变化的曲线。横向误差曲线能看出前视距引起的收敛带宽；转角曲线能看出响应压力。从转角曲线最容易识别的两类问题是：高频抖振说明前视距离过小；稳定的大幅滞后性转角说明前视距离过大。与只调一套工作点相比，在线改参数后重新仿真并对比曲线是效率更高的调试方式。
实现上，我建议把横向误差单独用一个脚本算出来：在每个仿真采样点找到车辆到参考路径的最近距离，记录为误差序列。注意最近距离的搜索不能只在所有路径点上做最小欧氏距离，因为路径点密集时结果与真实投影很接近，路径点稀疏时就可能有方向偏差。标准做法是对路径做分段处理后再求投影。
5. 从单位圆到 8 字路径：验证纯跟踪算法鲁棒性的两步检查法
纯跟踪算法写完以后，我通常用两步来验证它是否达到交付标准。第一步是单位圆路径，检查稳态误差是否收敛到预期范围；第二步是 8 字路径，检查变曲率场景下前视距离是否需要切换。单位圆路径的好处是曲率恒定，能明确暴露前视距离和最大转角是否匹配；8 字路径则在中心交叉区域引入曲率符号突变，可以用来验证前视点查找逻辑是否会在交叉点附近跳到另一段路径上。
8 字路径可以用以下参数方程生成：
t = linspace(0, 2*pi, 1000)'; x = 4 * sin(t); y = 2 * sin(2*t);这个路径在交叉点附近存在两条分支，用最短欧氏距离法找最近点很容易跳分支。解决方法是记录车辆上一时刻在前视点附近的路径索引，然后限定在这一索引附近的前后窗口内搜索下一帧的前视点，相当于给算法一个“路径连续性”约束。窗口大小与速度相关，一般取 2 到 3 倍前视距离所对应的路径点弧长。该方法对稀疏路径、重复线段同样有效。
另一个值得尝试的验证方式是切换不同前视距离做横向误差对比。我的习惯是直接绘制“前视距离-最大误差”曲线，会看到一个明显的 U 型。最佳点就是该项目中该车速下的整定值。这个曲线图很容易被误认为“越小越好”，但从 U 型图上看，L 小于最佳点后误差反而增大，原因是高频抖振导致实际走出来的路径更曲折。这组数据说明了纯跟踪算法的核心矛盾：它不是精度最高的算法，但它是能快速让整体系统跑通并暴露其它环节问题的算法，适合作为感知、路径规划、底层控制的中间验证层。用 Matlab 完成算法原型后，把函数翻译成 C++ 或移植到 ROS 中的成本很低，因为输入输出只有坐标、航向角和控制量这几个量。
本文还有配套的精品资源，点击获取
