机器人 / 系统课时
用 Pure Pursuit 跟踪路径
用 Pure Pursuit 跟踪路径
给定一条包含直线和弯道的导航路径,Pure Pursuit 会反复寻找机器人前方的目标点,将几何偏差换算为线速度与角速度。本篇用可运行仿真比较前视距离造成的弯道误差,再把对应参数接入 ROS 2 Jazzy 的 Nav2 控制器。
学习目标
- 理解前视点如何决定差速底盘的转弯曲率。
- 计算线速度、角速度,并用仿真量化弯道横向误差。
- 配置 Nav2 的 Regulated Pure Pursuit,并按固定路线验证效果。
选取前视点
Pure Pursuit 的输入是规划器给出的路径和机器人当前位姿。控制器先在路径上寻找靠近机器人、且尚未通过的位置,再沿路径向前选一个距机器人约 的前视点(lookahead point,也称 carrot)。真实系统要先借助 TF 将路径点转换到 base_link:车体 轴朝前, 轴朝左。
对于每隔约 0.02 m 采样的路径,可以从最近点向前搜索,取第一个与车体距离不小于 的采样点。这样实现简单,但选中点的实际距离可能略大于设定值;生产实现通常会利用线段插值改善精度。前视点应沿路径前进选取,不能直接拿离机器人最近的点当目标,否则到弯道时可能反复修正已走过的路段。

把几何曲率变成速度
设前视点在车体系中的坐标为 ,到车体的直线距离为 。让一段与车头方向相切的圆弧经过该点,其曲率和差速底盘的角速度为:
其中 的单位是 m/s, 的单位是 rad/s。取 m/s,前视点为 m,则距离 m,曲率 1/m,输出 rad/s;正号表示向左转。若 ,角速度为零。分母应检查是否接近零,防止目标点与机器人重合导致除零。
这里算出的是理想几何命令;它没有考虑电机加速度、障碍物、速度饱和或终点姿态。Nav2 的 Regulated Pure Pursuit(RPP)会在几何计算之外进行速度调节和碰撞预测,因此实际发布的速度可能更低。
用仿真比较弯道误差
用 Python 3 标准库建立一条直线、半径 0.8 m 的 90° 左弯、再接直线的路径。初始机器人在路径右侧偏离 0.12 m,固定线速度 0.20 m/s,控制周期 0.05 s,连续运行 210 次。每一步先选前视点,再计算 ,最后用欧拉积分更新位姿。为突出弯道表现,只统计处在圆弧段时的最大横向距离;距离按最近离散路径点近似,不是 Nav2 实测日志。
import math
# 直线、四分之一圆弧、直线;路径点间距约 0.02 m
path = [(-0.8 + i * 0.02, 0.0) for i in range(41)]
path += [(0.8 * math.sin(math.pi * i / 160),
0.8 * (1 - math.cos(math.pi * i / 160))) for i in range(1, 81)]
path += [(0.8, 0.8 + i * 0.02) for i in range(1, 31)]
def run(lookahead):
x, y, yaw = -0.8, -0.12, 0.0
speed, dt = 0.2, 0.05
curve_errors = []
for _ in range(210):
nearest = min(range(len(path)),
key=lambda i: math.hypot(path[i][0] - x, path[i][1] - y))
if 41 <= nearest <= 120:
curve_errors.append(math.hypot(path[nearest][0] - x,
path[nearest][1] - y))
# 从最近路径点向前找到落在前视圆外的第一个采样点
j = nearest
while j + 1 < len(path) and math.hypot(
path[j][0] - x, path[j][1] - y) < lookahead:
j += 1
dx, dy = path[j][0] - x, path[j][1] - y
local_y = -math.sin(yaw) * dx + math.cos(yaw) * dy
curvature = 2 * local_y / max(dx * dx + dy * dy, 1e-6)
omega = max(-1.0, min(1.0, speed * curvature))
# 差速底盘平面运动学:先使用本周期姿态,再更新航向
x += speed * math.cos(yaw) * dt
y += speed * math.sin(yaw) * dt
yaw += omega * dt
return max(curve_errors)
for ld in (0.3, 0.6, 0.9):
print(f"Ld={ld:.1f} m, 弯道最大横向误差={run(ld):.3f} m")预期输出:
Ld=0.3 m, 弯道最大横向误差=0.019 m
Ld=0.6 m, 弯道最大横向误差=0.041 m
Ld=0.9 m, 弯道最大横向误差=0.085 m同样的路线和速度下,较大的前视距离更早瞄准弯道远处,因而更容易切弯。小前视距离在这次理想仿真里误差更小,但真实底盘如果存在定位噪声、控制延迟或轮速响应不足,也可能因修正过于频繁而摆动;不能仅凭这组数字决定实车参数。
在 Nav2 中启用路径跟踪
以下适用于 ROS 2 Jazzy + Nav2。先确保地图定位、map 到 odom 再到 base_link 的 TF 链、局部代价图和底盘速度接口已经工作。将下面的 片段示例 合并到现有 nav2_params.yaml 的 controller_server 配置中,不要整段覆盖原有的进度检查器、目标检查器与代价图配置。
controller_server:
ros__parameters:
controller_frequency: 20.0
controller_plugins: ["FollowPath"]
FollowPath:
plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
desired_linear_vel: 0.20
lookahead_dist: 0.60
use_velocity_scaled_lookahead_dist: false
use_regulated_linear_velocity_scaling: true
regulated_linear_scaling_min_speed: 0.15
use_collision_detection: true
max_allowed_time_to_collision_up_to_carrot: 1.0这里关闭的是速度自适应前视距离,便于只比较 lookahead_dist;并没有关闭曲率减速和碰撞检测。该控制器也会根据代价图中的障碍物调节速度;相关膨胀层和障碍层应保持有效。重新用该参数文件启动 Nav2 后执行:
ros2 param get /controller_server FollowPath.lookahead_dist预期输出:
Double value is: 0.6如果提示参数不存在,先确认 controller_server 节点已启动、参数文件确实被加载、控制器 ID 是否仍为 FollowPath。官方参数释义可查阅 Nav2 Jazzy RPP 文档。
观察误差并调整参数
在 RViz 2 将 Fixed Frame 设为 map,显示规划路径与 RobotModel,使用导航目标工具在同一地图中重复驶过同一处 90° 弯道。每次只把 lookahead_dist 改为 0.30、0.60、0.90 m 之一,并在重启或确认参数已生效后重复测试;其余速度、起点、目标点和代价图参数不变。RPP 的 /lookahead_point 可辅助检查前视点是否跳到路径后方。
评估时记录机器人在 map 坐标系中的轨迹,计算各时刻车体位置到参考路径线段的最短距离,比较弯道最大误差与平均误差,而不只看终点是否到达。另记下角速度是否左右频繁换向,以及弯道内侧是否存在切角风险。先用 0.60 m 作为低速起点:切弯明显就尝试减小到 0.40 m;出现左右摆动则适当增大前视距离或降低速度,并排查 TF 延迟、定位噪声和轮速反馈。
前视距离的取值要通过同路径、同速度、同起点的重复测量来判断:误差很小但频繁摆动,和轨迹平滑却明显切弯,都是需要继续调参的信号。