机器人 / 系统课时

用 Pure Pursuit 跟踪路径

用 Pure Pursuit 跟踪路径

给定一条包含直线和弯道的导航路径,Pure Pursuit 会反复寻找机器人前方的目标点,将几何偏差换算为线速度与角速度。本篇用可运行仿真比较前视距离造成的弯道误差,再把对应参数接入 ROS 2 Jazzy 的 Nav2 控制器。

学习目标

  • 理解前视点如何决定差速底盘的转弯曲率。
  • 计算线速度、角速度,并用仿真量化弯道横向误差。
  • 配置 Nav2 的 Regulated Pure Pursuit,并按固定路线验证效果。

选取前视点

Pure Pursuit 的输入是规划器给出的路径和机器人当前位姿。控制器先在路径上寻找靠近机器人、且尚未通过的位置,再沿路径向前选一个距机器人约 LdL_d 的前视点(lookahead point,也称 carrot)。真实系统要先借助 TF 将路径点转换到 base_link:车体 xx 轴朝前,yy 轴朝左。

对于每隔约 0.02 m 采样的路径,可以从最近点向前搜索,取第一个与车体距离不小于 LdL_d 的采样点。这样实现简单,但选中点的实际距离可能略大于设定值;生产实现通常会利用线段插值改善精度。前视点应沿路径前进选取,不能直接拿离机器人最近的点当目标,否则到弯道时可能反复修正已走过的路段。

Pure Pursuit 前视点与弯道跟踪误差

把几何曲率变成速度

设前视点在车体系中的坐标为 (xL,yL)(x_L,y_L),到车体的直线距离为 L=xL2+yL2L=\sqrt{x_L^2+y_L^2}。让一段与车头方向相切的圆弧经过该点,其曲率和差速底盘的角速度为:

κ=2yLxL2+yL2,ω=vκ\kappa=\frac{2y_L}{x_L^2+y_L^2},\qquad \omega=v\kappa

其中 vv 的单位是 m/s,ω\omega 的单位是 rad/s。取 v=0.20v=0.20 m/s,前视点为 (0.60,0.20)(0.60,0.20) m,则距离 L≈0.632L\approx0.632 m,曲率 κ=1.00\kappa=1.00 1/m,输出 ω=0.20\omega=0.20 rad/s;正号表示向左转。若 yL=0y_L=0,角速度为零。分母应检查是否接近零,防止目标点与机器人重合导致除零。

这里算出的是理想几何命令;它没有考虑电机加速度、障碍物、速度饱和或终点姿态。Nav2 的 Regulated Pure Pursuit(RPP)会在几何计算之外进行速度调节和碰撞预测,因此实际发布的速度可能更低。

用仿真比较弯道误差

用 Python 3 标准库建立一条直线、半径 0.8 m 的 90° 左弯、再接直线的路径。初始机器人在路径右侧偏离 0.12 m,固定线速度 0.20 m/s,控制周期 0.05 s,连续运行 210 次。每一步先选前视点,再计算 ω\omega,最后用欧拉积分更新位姿。为突出弯道表现,只统计处在圆弧段时的最大横向距离;距离按最近离散路径点近似,不是 Nav2 实测日志。

python
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")

预期输出:

text
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 配置中,不要整段覆盖原有的进度检查器、目标检查器与代价图配置。

yaml
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 后执行:

bash
ros2 param get /controller_server FollowPath.lookahead_dist

预期输出:

text
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 延迟、定位噪声和轮速反馈。

前视距离的取值要通过同路径、同速度、同起点的重复测量来判断:误差很小但频繁摆动,和轨迹平滑却明显切弯,都是需要继续调参的信号。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

用 Pure Pursuit 跟踪路径

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通