机器人 / 系统课时
用路径平滑减少急转与抖动
用路径平滑减少急转与抖动
栅格规划得到的路径常由直线段拼接,连续急转会让差速底盘频繁调整角速度。本篇通过带避障约束的平滑算例和 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 的间隔采样线段检测扩张障碍区。
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)}")预期输出:
原始: 长度=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 的配置片段;已有同名段应合并,不能重复声明。首先关闭二次细化,便于观察一次平滑效果。
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: falsew_data 越大,路径越接近原始折线;w_smooth 增大通常能减轻局部曲折,但也更可能切近障碍。代价图话题与 base_link 必须和当前机器人命名一致。SimpleSmoother 适用于 NavFn、Smac 2D 等二维几何路径;对于 Hybrid-A*、State Lattice 的曲率或倒车约束路径,优先评估 ConstrainedSmoother,避免破坏原先的运动学可行性。
接着在现有 NavigateToPose 行为树中,将原来的规划步骤替换成下列XML 片段示例。它依赖已存在的 {goal} 黑板变量、名为 GridBased 的规划器及后续读取 {path} 的 FollowPath 节点;不是一份独立的完整行为树文件。
<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 环境的终端执行:
ros2 lifecycle get /smoother_server
ros2 action list -t | grep smooth_path预期输出(默认命名空间下):
active [3]
/smooth_path [nav2_msgs/action/SmoothPath]实际命名空间可能不同。如果没有动作服务器,先核对 Nav2 是否启动了 smoother_server、生命周期是否激活,以及插件类名是否拼对;不能只看到 YAML 存在就认为平滑已接入。
在 RViz2 中为原始规划路径(通常为 /plan)和平滑路径 /smoothed_path 各添加一个 Path 显示,使用相同地图坐标系,对同一起终点发起导航。逐项检查:平滑后的折角是否减小、长度是否合理、轨迹是否远离障碍膨胀区,以及控制器实际跟踪的是否为平滑结果。障碍紧贴拐角时允许保留明显转弯,绝不能为追求长度更短而穿越代价图禁入区。
路径平滑只改变几何路径,不等同于对 /cmd_vel 做加速度限幅。如果轨迹已经平顺,底盘却仍出现角速度抖动,应继续检查定位噪声、路径跟踪参数与速度限幅,而不是一味增大 w_smooth。