机器人 / 系统课时

用路径平滑减少急转与抖动

用路径平滑减少急转与抖动

栅格规划得到的路径常由直线段拼接,连续急转会让差速底盘频繁调整角速度。本篇通过带避障约束的平滑算例和 ROS 2 Jazzy 的 Nav2 配置,量化比较路径转角、长度与安全性。

学习目标

  • 解释折线拐角、障碍物膨胀区与底盘控制抖动的关系。
  • 运行局部平滑示例,比较路径长度和最大转角,并检查碰撞。
  • 在 Nav2 中配置 SimpleSmoother,让控制器接收平滑后的路径。

判断哪些拐角可以平滑

nav_msgs/msg/Path 包含按顺序排列的路径位姿。像 A*、NavFn 这样的二维规划结果可能出现多个 90° 折角;控制器经过折角时,期望航向迅速改变,容易表现为减速、原地转向或角速度来回修正。平滑可以减弱这种变化,但不能把路径直接拉成起终点连线。

设一个圆形差速底盘半径为 0.20 m,额外安全余量为 0.05 m,则本例对矩形障碍物做 0.25 m 的保守轴对齐扩张。平滑后的所有线段都必须留在禁入区之外,首尾位置保持不动;这是比“视觉上更圆”更优先的约束。

折线路径与平滑路径的转角、长度及障碍安全区对比

对离散路径的每个内部点,可用“原始点回拉”抑制偏离,用“前后邻点拉近”减轻锯齿。权重先取 w_data=0.2 和 w_smooth=0.3;每次移动候选点后,重新检查它连接的两段路径,发生碰撞就拒绝这次移动。

运行平滑与指标对比

下面是独立的 Python 3 标准库示例,不依赖 ROS,也不是 Nav2 插件源码。将它保存为 path_smooth_demo.py,执行 python3 path_smooth_demo.py。示例在两处转弯的折线上迭代移动中间点,以 0.02 m 的间隔采样线段检测扩张障碍区。

python
from math import acos, degrees, hypot

raw = [(0.0, 0.0), (1.0, 0.0), (1.0, 1.0), (1.0, 2.0),
       (1.0, 3.0), (2.0, 3.0), (3.0, 3.0), (4.0, 3.0)]
obstacle = (1.65, 1.85, 2.25, 2.40)  # 矩形障碍物边界,单位 m
padding = 0.25  # 半径 0.20 m + 安全余量 0.05 m

def segment_clear(a, b):
    # 按 0.02 m 采样,并检查是否进入保守扩张的障碍矩形
    steps = max(1, int(hypot(b[0] - a[0], b[1] - a[1]) / 0.02) + 1)
    x0, y0, x1, y1 = obstacle
    for k in range(steps + 1):
        t = k / steps
        x = a[0] + (b[0] - a[0]) * t
        y = a[1] + (b[1] - a[1]) * t
        if x0 - padding <= x <= x1 + padding and y0 - padding <= y <= y1 + padding:
            return False
    return True

def path_clear(points):
    return all(segment_clear(a, b) for a, b in zip(points, points[1:]))

def metrics(points):
    distance = sum(hypot(b[0] - a[0], b[1] - a[1])
                   for a, b in zip(points, points[1:]))
    turns = []
    for a, b, c in zip(points, points[1:], points[2:]):
        u = (b[0] - a[0], b[1] - a[1])
        v = (c[0] - b[0], c[1] - b[1])
        cosine = sum(u[d] * v[d] for d in (0, 1)) / (hypot(*u) * hypot(*v))
        turns.append(degrees(acos(max(-1.0, min(1.0, cosine)))))
    return distance, max(turns, default=0.0)

smoothed = [list(p) for p in raw]
for _ in range(200):
    biggest_move = 0.0
    for i in range(1, len(raw) - 1):  # 起点、终点保持不动
        candidate = [
            smoothed[i][d] + 0.2 * (raw[i][d] - smoothed[i][d])
            + 0.3 * (smoothed[i - 1][d] + smoothed[i + 1][d] - 2 * smoothed[i][d])
            for d in (0, 1)
        ]
        # 只接受不会让相邻线段进入扩张障碍区的更新
        if segment_clear(smoothed[i - 1], candidate) and segment_clear(candidate, smoothed[i + 1]):
            biggest_move = max(biggest_move, hypot(
                candidate[0] - smoothed[i][0], candidate[1] - smoothed[i][1]))
            smoothed[i] = candidate
    if biggest_move < 1e-6:
        break

for name, points in (("原始", raw), ("平滑", smoothed)):
    distance, turn = metrics(points)
    print(f"{name}: 长度={distance:.2f} m, 最大转角={turn:.1f}°, 安全={path_clear(points)}")

预期输出:

text
原始: 长度=7.00 m, 最大转角=90.0°, 安全=True
平滑: 长度=5.84 m, 最大转角=61.8°, 安全=True

长度按相邻点欧氏距离求和,最大转角是相邻两段运动方向的最大夹角。此例缩短约 1.16 m(16.6%),最大转角降低 28.2°,同时通过了示例的碰撞检查;靠近障碍的拐弯不能无限制地圆滑。

接入 Nav2 Jazzy 的平滑器

在 Ubuntu 24.04、ROS 2 Jazzy 且 Nav2 已经能够规划和导航的环境中,把以下内容并入正在使用的 nav2_params.yaml。这是 smoother_server 的配置片段;已有同名段应合并,不能重复声明。首先关闭二次细化,便于观察一次平滑效果。

yaml
smoother_server:
  ros__parameters:
    costmap_topic: global_costmap/costmap_raw
    footprint_topic: global_costmap/published_footprint
    robot_base_frame: base_link
    transform_tolerance: 0.1
    smoother_plugins: ["simple_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      max_its: 1000
      w_data: 0.2
      w_smooth: 0.3
      do_refinement: false

w_data 越大,路径越接近原始折线;w_smooth 增大通常能减轻局部曲折,但也更可能切近障碍。代价图话题与 base_link 必须和当前机器人命名一致。SimpleSmoother 适用于 NavFn、Smac 2D 等二维几何路径;对于 Hybrid-A*、State Lattice 的曲率或倒车约束路径,优先评估 ConstrainedSmoother,避免破坏原先的运动学可行性。

接着在现有 NavigateToPose 行为树中,将原来的规划步骤替换成下列XML 片段示例。它依赖已存在的 {goal} 黑板变量、名为 GridBased 的规划器及后续读取 {path} 的 FollowPath 节点;不是一份独立的完整行为树文件。

xml
<Sequence name="ComputeAndSmoothPath">
  <ComputePathToPose goal="{goal}" path="{raw_path}" planner_id="GridBased"/>
  <SmoothPath unsmoothed_path="{raw_path}" smoothed_path="{path}"
              smoother_id="simple_smoother" max_smoothing_duration="1.0"
              check_for_collisions="true"/>
</Sequence>

SmoothPath 调用平滑动作服务器,把输出写入 {path},交给后续路径跟踪;check_for_collisions="true" 让动作检查平滑结果的碰撞情况。将 bt_navigator.ros__parameters.default_nav_to_pose_bt_xml 指向修改后的完整 BT XML,重启 Nav2,使参数和行为树生效;平滑失败时应进入错误处理或重新规划,不要直接跟踪未验证的候选路径。具体端口用法可对照 Nav2 Jazzy 的 SmoothPath 文档。

验收平滑效果与失效边界

Nav2 启动、生命周期节点激活后,在另一个已加载 ROS 2 Jazzy 环境的终端执行:

bash
ros2 lifecycle get /smoother_server
ros2 action list -t | grep smooth_path

预期输出(默认命名空间下):

text
active [3]
/smooth_path [nav2_msgs/action/SmoothPath]

实际命名空间可能不同。如果没有动作服务器,先核对 Nav2 是否启动了 smoother_server、生命周期是否激活,以及插件类名是否拼对;不能只看到 YAML 存在就认为平滑已接入。

在 RViz2 中为原始规划路径(通常为 /plan)和平滑路径 /smoothed_path 各添加一个 Path 显示,使用相同地图坐标系,对同一起终点发起导航。逐项检查:平滑后的折角是否减小、长度是否合理、轨迹是否远离障碍膨胀区,以及控制器实际跟踪的是否为平滑结果。障碍紧贴拐角时允许保留明显转弯,绝不能为追求长度更短而穿越代价图禁入区。

路径平滑只改变几何路径,不等同于对 /cmd_vel 做加速度限幅。如果轨迹已经平顺,底盘却仍出现角速度抖动,应继续检查定位噪声、路径跟踪参数与速度限幅,而不是一味增大 w_smooth。

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

目录与阅读设置

阅读设置

17

记录一个想法

用路径平滑减少急转与抖动

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

登录哪都通