一、纯跟踪算法：从几何直觉到工程实现
各位同学好，我是你们的老朋友。今天咱们聊聊路径跟踪里一个非常经典的方法——纯跟踪算法（Pure Pursuit）。
说实话，我第一次接触这个算法是在五年前的一个园区物流项目里。当时车辆要在狭窄通道里走S弯，用PID控制总是 overshoot，后来换成纯跟踪，效果出奇的好。嗯，从那以后我就对这个算法情有独钟了。
1.1 基本原理：一个几何问题
纯跟踪算法的核心思想，说白了就是一句话：让车辆像追逐一个移动目标点一样去跟踪路径。
你想象一下：在车辆前方某处，有一个虚拟的“目标点”在路径上移动。车辆通过调整前轮转角，让自己始终朝着这个目标点前进。这就是“纯跟踪”这个名字的由来——纯粹地追逐那个点。
具体怎么算呢？我们来看几何关系：
核心公式：
δ = arctan( 2L·sin(α) / ld )
其中：
- δ — 前轮转角（我们要算的东西）
- L — 车辆轴距（固定值）
- α — 车辆航向与目标点方向的夹角
- ld — 前视距离（关键参数）
这个公式怎么来的？其实它基于一个简单的几何模型——阿克曼转向几何。车辆转弯时，后轴中心会画出一个圆弧。纯跟踪算法就是让这个圆弧恰好经过目标点。
我在项目中遇到过一个问题：公式里的α角怎么算？很多人直接用车辆朝向和目标点方向的夹角，但要注意坐标系转换。我习惯用车辆坐标系下的角度，这样更直观。
1.2 前视距离选择：最关键的调参
纯跟踪算法里，前视距离 ld 是最重要的参数。没有之一。
为什么这么说？你想想看：
- ld 太大：车辆反应迟钝，过弯时容易切内线，甚至冲出道路
- ld 太小：车辆频繁调整方向，出现“蛇形”振荡
我个人的经验是：前视距离应该与车速成正比。一个常用的公式是：
ld = k · v + ld_min
其中 k 是比例系数，v 是车速，ld_min 是最小前视距离。
我的调参建议：
先固定车速，从较大的 ld 开始试（比如 5 米），然后逐步减小，直到车辆出现轻微振荡，再回调 20%。这个“临界振荡法”我在三个项目里都用过，效果不错。
另外，不同曲率的弯道也需要不同的 ld。我记得有一次在高速匝道上测试，曲率半径只有 30 米，用固定 ld 怎么调都不行。后来改成根据路径曲率动态调整 ld，问题就解决了。
1.3 横向误差计算：怎么评价跟踪效果？
做控制，你得知道控制得好不好。横向误差就是最直观的指标。
横向误差的定义很简单：车辆当前位置到目标路径的最短距离。
计算方式有两种：
- 最近点法：找到路径上离车辆最近的点，计算距离
- 投影法：将车辆位置投影到路径上，计算垂直距离
我个人更推荐投影法，因为它能反映车辆相对于路径前进方向的偏差。最近点法在路径曲率变化大的地方容易跳变。
注意：横向误差有正负之分。我习惯定义：车辆在路径左侧为正，右侧为负。这样在调试时能直观看出车辆偏向哪边。
1.4 Python实现：从公式到代码
好了，理论讲完了，咱们来看看代码怎么写。下面是一个完整的纯跟踪算法实现：
import numpy as np
import math
class PurePursuitController:
def __init__(self, wheelbase=2.5, k=0.5, ld_min=2.0):
self.L = wheelbase # 轴距（米）
self.k = k # 前视距离比例系数
self.ld_min = ld_min # 最小前视距离（米）
def compute_steering_angle(self, vehicle_state, path_points, velocity):
"""
计算前轮转角
:param vehicle_state: [x, y, yaw] 车辆状态
:param path_points: [[x1,y1], [x2,y2], ...] 路径点
:param velocity: 当前车速（m/s）
:return: 前轮转角（弧度）
"""
# 1. 计算前视距离
ld = self.k * velocity + self.ld_min
# 2. 找到目标点
target_point = self._find_target_point(
vehicle_state, path_points, ld
)
if target_point is None:
return 0.0
# 3. 计算目标点在车辆坐标系下的角度
dx = target_point[0] - vehicle_state[0]
dy = target_point[1] - vehicle_state[1]
# 转换到车辆坐标系
alpha = math.atan2(dy, dx) - vehicle_state[2]
# 4. 计算前轮转角
steering_angle = math.atan2(
2.0 * self.L * math.sin(alpha), ld
)
# 限制最大转角
max_steer = math.radians(30)
steering_angle = np.clip(
steering_angle, -max_steer, max_steer
)
return steering_angle
def _find_target_point(self, vehicle_state, path_points, ld):
"""找到前视距离处的目标点"""
min_dist = float('inf')
target_idx = 0
# 找到最近点
for i, point in enumerate(path_points):
dist = math.hypot(
point[0] - vehicle_state[0],
point[1] - vehicle_state[1]
)
if dist < min_dist:
min_dist = dist
target_idx = i
# 从最近点开始，找到距离为 ld 的点
accumulated_dist = 0.0
for i in range(target_idx, len(path_points) - 1):
segment_dist = math.hypot(
path_points[i+1][0] - path_points[i][0],
path_points[i+1][1] - path_points[i][1]
)
accumulated_dist += segment_dist
if accumulated_dist >= ld:
# 线性插值
ratio = (accumulated_dist - ld) / segment_dist
x = path_points[i+1][0] - ratio * (
path_points[i+1][0] - path_points[i][0]
)
y = path_points[i+1][1] - ratio * (
path_points[i+1][1] - path_points[i][1]
)
return [x, y]
# 如果路径不够长，返回最后一个点
return path_points[-1]
def compute_lateral_error(self, vehicle_state, path_points):
"""计算横向误差"""
# 找到最近路径点
min_dist = float('inf')
nearest_point = None
for point in path_points:
dist = math.hypot(
point[0] - vehicle_state[0],
point[1] - vehicle_state[1]
)
if dist < min_dist:
min_dist = dist
nearest_point = point
# 计算横向误差（带符号）
dx = nearest_point[0] - vehicle_state[0]
dy = nearest_point[1] - vehicle_state[1]
# 路径方向角
path_yaw = math.atan2(
path_points[-1][1] - path_points[0][1],
path_points[-1][0] - path_points[0][0]
)
# 横向误差 = 距离 × sin(相对角度)
lateral_error = math.hypot(dx, dy) * math.sin(
math.atan2(dy, dx) - path_yaw
)
return lateral_error这段代码我用了好几年，基本框架没怎么变过。有几个细节我想强调一下：
- 目标点查找：我用了累积距离法，比直接找最近点更稳定
- 线性插值：路径点稀疏时，插值能避免转角跳变
- 转角限幅：别忘了限制最大转角，否则实车会出问题
1.5 仿真验证：看看效果
光有代码不行，咱们得跑个仿真看看效果。下面是一个简单的仿真框架：
import matplotlib.pyplot as plt
def simulate_pure_pursuit():
# 创建控制器
controller = PurePursuitController(
wheelbase=2.5, k=0.5, ld_min=2.0
)
# 生成参考路径（圆形）
theta = np.linspace(0, 2*np.pi, 100)
radius = 10.0
path = np.column_stack([
radius * np.cos(theta),
radius * np.sin(theta)
])
# 初始状态
state = [radius, 0.0, math.pi/2] # x, y, yaw
velocity = 3.0 # 3 m/s
# 记录轨迹
trajectory = [state[:2]]
lateral_errors = []
# 仿真循环
dt = 0.05 # 50ms
for _ in range(500):
# 计算控制量
steer = controller.compute_steering_angle(
state, path, velocity
)
# 更新车辆状态（简单运动学模型）
state[0] += velocity * math.cos(state[2]) * dt
state[1] += velocity * math.sin(state[2]) * dt
state[2] += velocity / controller.L * math.tan(steer) * dt
# 记录
trajectory.append(state[:2])
lateral_errors.append(
controller.compute_lateral_error(state, path)
)
# 绘制结果
plt.figure(figsize=(12, 4))
plt.subplot(1, 2, 1)
plt.plot(path[:,0], path[:,1], 'b--', label='参考路径')
traj = np.array(trajectory)
plt.plot(traj[:,0], traj[:,1], 'r-', label='实际轨迹')
plt.axis('equal')
plt.legend()
plt.title('路径跟踪效果')
plt.subplot(1, 2, 2)
plt.plot(lateral_errors)
plt.xlabel('时间步')
plt.ylabel('横向误差 (m)')
plt.title('横向误差变化')
plt.tight_layout()
plt.show()
# 运行仿真
simulate_pure_pursuit()运行这段代码，你会看到车辆沿着圆形路径行驶，横向误差逐渐收敛到零附近。这就是纯跟踪算法的魅力——简单、直观、有效。
1.6 知识体系总览
为了让大家对纯跟踪算法有个整体认识，我画了一张结构图：
这张图把纯跟踪算法的知识体系梳理得很清楚。从基本原理出发，到前视距离和横向误差两个关键点，最后落地到代码实现。每个环节都环环相扣，缺一不可。
我的学习建议：
别急着调参，先把几何关系搞明白。我曾经带过一个实习生，上来就调 ld，调了两天没调好。后来我让他画了个几何图，五分钟就理解了问题出在哪。
好了，这一章的内容就到这里。纯跟踪算法虽然简单，但用好了威力很大。下一章我们会聊更高级的跟踪方法，到时候你会发现，很多复杂算法其实都是在纯跟踪的基础上改进的。
可可精品资料库