想做成什么样的项目

我们希望机器人能够用激光雷达观察环境，建立一张二维地图，在地图中知道自己的位置；收到目标点后，它先规划一条全局路线，再根据附近的实时障碍物不断调整速度和方向，最终安全到达目标。

这条链路可以概括为：

激光雷达 /scan ─┐
轮式里程计 /odom ├─→ SLAM 或定位 ─→ 地图、机器人位姿、TF
机器人模型 / TF ┘                         ↓
                                 Nav2 Costmap
                                  ↙         ↘
                         全局路径规划       局部控制与避障
                            A*                 DWB
                                               ↓
                                         底盘速度指令
                                               ↓
                                      新的雷达和里程计数据

重点不是把几个算法名放在一起，而是让它们使用同一套坐标、地图和机器人运动限制。A*、DWB、SLAM 分别负责不同层次的问题：SLAM 提供地图和位置估计，全局规划器决定“总体怎么走”，局部控制器决定“此刻怎么动”。

一、先把算法关系讲清楚

1. SLAM：同时估计位置并构建地图

SLAM 是“同步定位与建图”：机器人一边移动，一边估计自己在环境中的位置，同时逐步建立环境地图。定位和建图互相依赖：没有地图时，机器人要靠连续的运动估计和激光扫描判断周围结构；地图逐渐形成后，又能用新扫描匹配已建区域，修正自身的位置估计。

二维平面中的机器人位姿通常由地图坐标系下的位置和朝向表示：

xt=(xt,yt,θt)\mathbf{x}_t=(x_t,y_t,\theta_t)

这里的坐标原点通常是建图时选定的参考原点，并非 GPS 坐标。刚启动、还没有地图时，系统需要给一个初始位姿或从任意参考位姿开始；随着机器人移动，里程计根据轮速等信息估计相邻时刻的相对位移，帮助预测“我大概移动到了哪里”。这类估计会随时间累积误差。

激光雷达则提供周围障碍物的距离扫描。SLAM 将扫描与已有扫描或地图结构对齐，校正里程计带来的漂移，并用校正后的位置把观测写入地图。可以把每一时刻的运动预测和雷达观测概念化地写成：

xt≈f(xt−1,ut)+ϵt\mathbf{x}_t \approx f(\mathbf{x}_{t-1},\mathbf{u}_t)+\boldsymbol{\epsilon}_t zt≈h(xt,M)+ηt\mathbf{z}_t \approx h(\mathbf{x}_t,\mathcal{M})+\boldsymbol{\eta}_t

其中，运动输入代表里程计给出的相对运动，雷达观测代表当前扫描，地图代表已经累积的环境信息；误差项表示传感器噪声和模型误差。这是帮助理解的简化表达，不是 slam_toolbox 内部实现公式。当机器人回到曾经到过的区域并识别出相同环境时，回环检测会建立新旧观测之间的约束，优化一段轨迹，从而减少长期累计漂移。

在 ROS 2 中，slam_toolbox 是 Nav2 文档支持的 2D SLAM 方案之一。常见学习方式是先用它在线建图，再把地图保存下来。进入重复导航阶段后，通常加载已保存的地图，并用 AMCL 等定位方案估计机器人在地图中的位置。建图和静态地图定位是两种不同运行模式，初学时分开验证会更容易定位问题。Nav2 建图与定位指南，SLAM Toolbox 项目

SLAM 不是只接上雷达就能工作。它还依赖连续、方向正确的里程计，以及雷达相对机器人本体的安装位姿。雷达数据时间戳、扫描频率、机器人运动速度和环境特征都会影响地图质量。

2. A*：在地图上搜索全局路线

A* 要回答的是：在已知地图里，从起点到目标，哪一条路线总代价较低？

把地图抽象成加权图，表示为：

G=(V,E)G=(V,E)

其中，集合 V 表示可搜索的栅格节点，集合 E 表示相邻格子之间的连边：

V={可搜索栅格},E={相邻栅格之间的连边}V=\{\text{可搜索栅格}\}, \qquad E=\{\text{相邻栅格之间的连边}\}

在二维栅格地图中，每个格子可以当成一个节点。机器人当前位置的 XY 坐标落在哪个格子，那个格子就是起点；目标位姿的 XY 坐标落在哪个格子，就是搜索终点。格子之间是否相邻，取决于采用四邻域（上下左右）还是八邻域（再加四个对角方向）。

格坐标与世界坐标通过地图分辨率和原点对应。忽略地图原点旋转时，格子中心可写为：

x=x0+(i+12)r,y=y0+(j+12)rx = x_0 + (i + \tfrac{1}{2})r, \qquad y = y_0 + (j + \tfrac{1}{2})r

搜索时，A* 会为每个候选格子计算：

f(n) = g(n) + h(n)
  • g(n)：从起点走到当前格子，实际累计的路线代价。
  • h(n)：从当前格子到目标的剩余代价估计，也叫启发式函数。
  • f(n)：经过当前格子到达目标的总代价估计。f 越小，越优先展开。

g 和 h 扮演不同角色：g 让搜索记住已经走过的路有多贵；h 则让搜索朝目标方向推进。若只按 g 搜索，算法会像 Dijkstra 一样向周围均匀扩张；加入合理的 h 后，A* 通常会把计算集中到更接近目标的区域。若启发式从不高估实际剩余代价，A* 在所定义的图和代价规则下可以找到最优路径。

设一次移动从节点 u 到节点 v，抵达节点 v 的最低累计代价可写为：

u,vu, v g(v)=min⁡u∈N(v)[g(u)+c(u,v)]g(v) = \min_{u \in N(v)} \bigl[g(u) + c(u,v)\bigr]

其中，邻居集合包含所有能直接连到 v 的节点；边代价表示从一个节点移动到另一个节点的代价：

N(v)={u∣(u,v)∈E},c(u,v)=从 u 移动到 v 的代价N(v)=\{u\mid (u,v)\in E\}, \qquad c(u,v)=\text{从 }u\text{ 移动到 }v\text{ 的代价}

一个便于理解的 Costmap 加权模型是：

c(u,v)=d(u,v) [1+λ q(v)]c(u,v) = d(u,v)\,[1 + \lambda\,q(v)]

其中，移动距离由相邻格子的几何关系决定；目标格的 Costmap 风险值越高，额外惩罚通常越大。参数 lambda 控制规划器对高代价格子的规避程度。直走一个格子和斜走一个格子的距离分别为：

dstraight=r,ddiagonal=2rd_{straight}=r, \qquad d_{diagonal}=\sqrt{2}r

真实 Nav2 插件会使用各自的代价转换和参数；以上公式用于讲解“路程代价 + 风险代价”，不是 Nav2 源码的精确公式。

启发函数应与邻接方式和移动代价匹配。下面公式中的单位移动代价下界用于保证启发值不会高估剩余路线代价：

h4(n)=wmin⁡r(∣Δi∣+∣Δj∣)h_4(n) = w_{\min}r(|\Delta i| + |\Delta j|) h8(n)=wmin⁡r[max⁡(∣Δi∣,∣Δj∣)+(2−1)min⁡(∣Δi∣,∣Δj∣)]h_8(n) = w_{\min}r\left[\max(|\Delta i|,|\Delta j|) + (\sqrt{2}-1)\min(|\Delta i|,|\Delta j|)\right]

四邻域使用 Manhattan 距离；八邻域使用 octile 距离。它们只估算几何距离，不把绕障碍造成的额外路程算进去，因此不会高估真实路线。启发函数中的代价下界应满足：

0≤wmin⁡≤实际的最低单位移动代价0 \le w_{\min} \le \text{实际的最低单位移动代价}

不确定时可令启发值为零：

h(n)=0h(n)=0

此时搜索退化为 Dijkstra，仍能求得最小代价路径，但通常会扩展更多节点。A* 的最优性条件可参看 Hart、Nilsson 和 Raphael 的原始论文。A* 原始论文

一轮搜索具体做什么

算法维护一个按 f 从小到大排列的优先队列（Open Set），以及已经处理过的节点集合（Closed Set）。每轮取出 f 最小的格子，检查它的邻居；遇到障碍就跳过，遇到更便宜的新路线就更新邻居的 g 值和父节点。搜索到终点后，沿父节点反向回溯，就能恢复完整路径。

下面是教学用伪代码。它把 Costmap 的风险值简化成额外代价，目的是说明 A* 如何搜索，并不等同于 Nav2 规划器源码里的精确代价公式：

function AStar(start, goal, costmap):
    open = priority_queue()             # 按 f 值从小到大取出
    g[start] = 0
    parent[start] = NONE
    open.push(start, priority = h(start, goal))

    while open is not empty:
        current = open.pop_min()

        if current == goal:
            return reconstruct_path(parent, goal)

        if current in closed:
            continue
        closed.add(current)

        for next in neighbors(current):
            if is_blocked(next, costmap):
                continue
            if diagonal_cuts_corner(current, next, costmap):
                continue

            distance = move_distance(current, next)  # 直走 r，对角线 √2·r
            q = normalized_cost(costmap[next])        # 归一化风险代价
            step_cost = distance * (1 + λ * q)
            candidate_g = g[current] + step_cost

            if next not in g or candidate_g < g[next]:
                g[next] = candidate_g
                parent[next] = current
                f = candidate_g + h(next, goal)
                open.push_or_update(next, priority = f)

    return NO_PATH

以仓库走廊为例：如果机器人正前方有障碍，A* 会跳过被判定为不可通行的格子，从相邻通道继续扩展。如果两条路线都能到目标，但其中一条靠墙格子的代价更高，搜索可能选择稍长、但安全余量更大的路线。最后返回的是一串栅格路径点；它还需要交给平滑器或局部控制器处理，不能直接当成轮子的速度指令。

Costmap 为什么会改变 A* 的路线

真实 Nav2 项目里，搜索空间通常来自 Costmap，而不是单纯的黑白图。Costmap 将静态地图、激光雷达看到的障碍物、机器人外形以及障碍物周围的膨胀代价组合起来。规划器会避开不可通行区域，也会受到高代价区域的影响；是否允许经过未知区域，同样由配置决定。Nav2 Costmap 概念

可以把这理解为：障碍格子规定“不能走”，高代价格子表示“尽量别贴着走”。膨胀半径太小，路线可能贴墙；太大，狭窄通道可能被整个封住。因此调 A* 之前，要先在 RViz 检查 Costmap 是否准确反映了环境和机器人尺寸。

Nav2 里该选哪个规划器

Nav2 使用插件切换全局规划算法。SmacPlanner2D 是面向二维栅格的 A*，适合差速或全向底盘；NavFn 可以在 A* 和 Dijkstra 之间选择。普通 2D A* 不会自动保证路径满足车辆的转弯半径等运动学约束。对阿克曼小车这类有明确转向约束的平台，应考虑 Hybrid-A*；需要自定义控制集合或车辆模型时，可以评估 State Lattice。Smac Planner 文档，Nav2 规划器插件说明

全局路径只表达“从整体上经过哪里”。DWB 等局部控制器还会结合机器人当前速度、雷达和局部 Costmap，在路径附近决定当前应该如何运动。

3. DWA：在可达速度中挑选一条安全轨迹

Dynamic Window Approach（DWA）回答的是：机器人这一刻应该发出什么线速度和角速度？ 它不在栅格里搜索整条路线，而是在每个控制周期的速度空间中，筛掉当前无法达到或不安全的速度，再从剩下的候选里选一个。

运动学模型：速度如何变成轨迹

对理想平面差速机器人，状态由平面位置和朝向组成，控制量为线速度和角速度。不考虑侧滑时，运动学方程为：

x˙=vcos⁡θ,y˙=vsin⁡θ,θ˙=ω\dot{x}=v\cos\theta, \qquad \dot{y}=v\sin\theta, \qquad \dot{\theta}=\omega

在小时间步内把速度视为常量，用欧拉法预测下一状态：

xk+1=xk+vcos⁡θkΔt,yk+1=yk+vsin⁡θkΔtx_{k+1}=x_k+v\cos\theta_k\Delta t, \qquad y_{k+1}=y_k+v\sin\theta_k\Delta t θk+1=θk+ωΔt\theta_{k+1}=\theta_k+\omega\Delta t

连续迭代到预测时长后，得到每组候选线速度和角速度对应的轨迹。角速度为零时轨迹近似直线；角速度不为零时轨迹近似圆弧。真实控制器会按自己的轨迹生成器和离散步长进行预测。

动态窗口：限制为“当前够得着”的速度

底盘自身的速度限制定义可行速度集合：

Vs=[vmin⁡,vmax⁡]×[ωmin⁡,ωmax⁡]V_s = [v_{\min},v_{\max}] \times [\omega_{\min},\omega_{\max}]

设当前实测线速度和角速度分别为 v0、omega0，线加速度和角加速度上限分别为 av、aomega。一个控制周期后可达到的范围近似为：

Vd=[v0−avΔt, v0+avΔt]×[ω0−aωΔt, ω0+aωΔt]V_d = [v_0-a_v\Delta t,\,v_0+a_v\Delta t] \times [\omega_0-a_\omega\Delta t,\,\omega_0+a_\omega\Delta t]

实现时还要与底盘速度限制集合求交；若加速和减速能力不对称，区间两端应分别使用对应限制。由此，算法不会选择虽然符合最大速度、但机器人不可能在当前周期内达到的指令。

安全窗口：确保有空间刹停

对每个候选速度，沿它的预测轨迹测量到最近障碍物的距离。在简化的直线制动模型中，最大制动减速度为正值时，线速度对应的理想刹停距离为：

dstop=v22abd_{stop}=\frac{v^2}{2a_b}

因此可用下面的条件做直观筛选：

dstop+dmargin<dobs(v,ω)d_{stop}+d_{margin}<d_{obs}(v,\omega)

安全余量是给机器人外形、传感器延迟和定位误差预留的距离。这个公式是简化的制动安全模型；实际系统应对完整机器人 footprint 和预测轨迹做碰撞检查，并根据底盘实测减速度、控制延迟和传感器更新率设定余量。经典 DWA 的关键步骤就是同时考虑动态可达性与沿轨迹安全停车能力。DWA 原始论文，原始论文 PDF

把速度限制、动态窗口和安全窗口相交，可得本周期允许搜索的集合：

Vr=Vs∩Vd∩VaV_r = V_s \cap V_d \cap V_a

其中，安全速度集合只包含通过安全检查的候选速度。

评分函数：比较安全候选的优劣

经典 DWA 在允许搜索的速度集合中选择使收益最大的速度组合：

(v∗,ω∗)=arg⁡max⁡(v,ω)∈VrG(v,ω)(v^*,\omega^*)=\arg\max_{(v,\omega)\in V_r}G(v,\omega)

一个常见的概念化评分式是：

G(v,ω)=αH(v,ω)+βD(v,ω)+γV(v,ω)G(v,\omega)=\alpha H(v,\omega)+\beta D(v,\omega)+\gamma V(v,\omega)
  • heading：预测轨迹的前进方向与目标方向有多一致。
  • clearance：轨迹与最近障碍物之间的间距。
  • velocity：前进速度偏好，鼓励在安全情况下有效前进。
  • alpha、beta、gamma：三项指标的权重。

实际使用时，先把各项归一化到相近范围，再比较加权结果；发生碰撞的轨迹应直接判为无效，不能只靠一个有限的低分来惩罚。

算法过程可以写成：

每个控制周期：
    读取当前位姿、里程计速度、局部 Costmap 和目标/全局路径
    用速度上限、加速度上限生成动态窗口 Vd
    候选速度集合 Vr = Vs ∩ Vd

    对 Vr 中每个采样速度 (v, ω)：
        按机器人运动学预测短时轨迹 τ(v, ω)
        若轨迹碰撞，或不能在障碍前安全制动：丢弃
        计算 heading、clearance、velocity 等指标
        计算 G = α·heading + β·clearance + γ·velocity

    若存在有效轨迹：输出 G 最大的 (v, ω)
    否则：输出安全停止指令，并交由上层恢复/重规划
    执行一个短周期后重新感知、重新计算

Nav2 DWB 与经典 DWA 的区别

Nav2 的 DWB Controller 是受 DWA 启发的 ROS 2 局部控制器，但它的实际评分不是上面三项公式的固定实现。DWB 使用轨迹生成器产生候选轨迹，再按参数中启用的 critics 评分。可用下面的抽象式理解它的加权代价：

τ∗=arg⁡min⁡τ∈Tvalid∑isiCi(τ)\tau^*=\arg\min_{\tau\in\mathcal{T}_{valid}}\sum_i s_i C_i(\tau)

有效轨迹集合只包含满足运动限制且没有碰撞的轨迹；每个 critic 计算一项代价，例如 BaseObstacle 检查障碍、PathAlign 和 PathDist 检查路径跟随、GoalDist 检查目标进展；每项代价再乘上配置的权重。经典 DWA 通常写成“最大化收益”，DWB 则常组织为“最小化总代价”，符号方向不同，但都在候选轨迹之间权衡。Nav2 DWB Controller 文档

DWB 每个周期只输出当前选定的线速度和角速度，执行短时间后，再用新的雷达和里程计数据重新预测。这种滚动闭环能对新出现的障碍做出反应。报告项目时应同时写出使用的控制器插件、critic 列表、速度/加速度限制、预测时间和 Costmap 参数；只写“使用 DWA”无法复现实验。

二、ROS 2 中各模块如何接起来

1. 先打通传感器、里程计和 TF

导航系统至少要能正确获得下面这条坐标关系：

map → odom → base_link → laser_frame

含义是：

  • map → odom 通常由 SLAM 或全局定位模块提供。
  • odom → base_link 通常由底盘里程计提供，也可以由 robot_localization 融合轮速计和 IMU 后发布。
  • base_link → laser_frame 表示雷达相对于机器人本体的固定安装位置，通常由 URDF 和 robot_state_publisher 发布。

Nav2 需要把雷达扫描、机器人位姿和地图变换到正确的坐标系里。只要 TF 树缺一段、方向反了、frame 名称不一致或数据时间戳对不上，RViz 里就可能出现雷达点云偏离机器人、Costmap 空白、规划失败等现象。Nav2 TF 配置指南

动手前先核对这些数据：

ros2 topic list
ros2 topic echo /scan --once
ros2 topic echo /odom --once
ros2 topic hz /scan
ros2 run tf2_tools view_frames

实际项目的话题名可能是 /scan_filtered、/wheel/odom 等，应在 Nav2 参数里使用机器人真正发布的话题名。

2. 用 SLAM 建图并验证地图

先启动底盘或仿真环境，再启动 slam_toolbox。仿真时需让所有相关节点使用同一个仿真时钟；实体机器人则使用真实时间。开始时用低速遥控机器人沿环境边缘移动，观察 RViz 中 /scan 与地图是否对齐、墙面是否重复、转弯处是否出现明显拉伸。

Nav2 官方仿真入口可以将 TurtleBot 仿真、机器人状态发布、RViz 和 Nav2 一起拉起。安装方式和 launch 参数会随 ROS 2 发行版变化，开始前应按自己使用的发行版查看对应文档：Nav2 仿真 launch 文件。

示例：

export TURTLEBOT3_MODEL=waffle
ros2 launch nav2_bringup tb3_simulation_launch.py slam:=True

地图质量达到要求后保存地图：

mkdir -p ~/maps
ros2 run nav2_map_server map_saver_cli -f ~/maps/lab

保存后会得到地图图像和描述地图分辨率、原点、占用阈值等信息的 YAML 文件。不要只检查文件是否生成，还要重新加载地图确认方向、比例和障碍边界正确。

3. 配置全局 A* 与局部 DWB

Nav2 以服务器和插件的方式组织规划功能。planner_server 根据目标调用全局规划器，controller_server 根据全局路径和当前机器人状态调用局部控制器。规划器 ID 和控制器 ID 由参数文件中的插件列表管理，实际参数层级应以所用发行版的 Nav2 示例配置为准。

Smac 2D 的核心配置形态如下：

planner_server:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner::SmacPlanner2D"

DWB 的核心配置形态如下：

controller_server:
  ros__parameters:
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "dwb_core::DWBLocalPlanner"
      critics:
        - "RotateToGoal"
        - "Oscillation"
        - "BaseObstacle"
        - "GoalAlign"
        - "PathAlign"
        - "PathDist"
        - "GoalDist"

这只是展示插件连接关系的精简片段，不能直接替代一份完整的 Nav2 参数文件。完整配置还需包含机器人速度与加速度限制、Costmap、坐标系、进度检查器、目标检查器、行为树等参数。

4. 将激光雷达送入局部 Costmap

静态地图层为全局规划提供长期不变的墙体等信息；局部 obstacle layer 则持续从 /scan 读取最新扫描，用于标记当前障碍物并沿雷达射线清除已离开的障碍物。障碍物周围的 inflation layer 让机器人留出安全余量。这样，动态障碍可以先影响局部控制；如果局部长期无法绕过，行为树中的重规划流程再让全局路径适应变化。

示意配置：

local_costmap:
  local_costmap:
    ros__parameters:
      plugins: ["obstacle_layer", "inflation_layer"]
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: true
        observation_sources: "scan"
        scan:
          topic: /scan
          data_type: "LaserScan"
          marking: true
          clearing: true
          obstacle_max_range: 2.5
          raytrace_max_range: 3.0
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"

传感器量程、机器人尺寸和速度不同，示例范围不能直接照搬。障碍物标记范围应小于等于雷达可靠探测范围；射线清除范围也要与雷达返回数据的语义匹配。Nav2 Obstacle Layer 参数

5. 导航时的完整闭环

用户在 RViz 指定目标后，Nav2 会通过行为树协调导航流程：

  1. Planner Server 从当前位姿和目标位姿计算全局路径。
  2. Controller Server 获取机器人附近的一段路径和局部 Costmap。
  3. DWB 生成候选速度轨迹，检查障碍物碰撞风险和路径进展。
  4. 控制器输出速度命令，底盘驱动轮子运动。
  5. 激光雷达与里程计返回新数据，Costmap 和机器人状态随之更新。
  6. 控制器持续重新评估；如果无法推进，行为树可请求重新规划或执行恢复行为。

这就是“全局规划 + 局部动态避障”的融合：A* 为局部控制提供方向，DWB 为全局路径补上短时运动决策。

三、推荐的项目学习路线

项目一：Nav2 官方 TurtleBot 仿真，先跑通整条链路

从 Nav2 的 tb3_simulation_launch.py 开始，在仿真里验证机器人、地图、定位、全局路径和速度控制。这个项目适合先熟悉 RViz、Nav2 生命周期节点、行为树和参数文件。先用官方默认参数记录一组可复现基线，不急着改算法。Nav2 官方仿真启动文件

建议完成这些小目标：

  1. 在 RViz 看到机器人、雷达扫描和 Costmap。
  2. 指定一个开阔区域目标，确认全局路径生成且机器人能到达。
  3. 在路径旁放置静态障碍，观察全局路径如何绕行。
  4. 在机器人行驶中移动一个障碍物，观察局部 Costmap 和 DWB 轨迹如何变化。
  5. 保存参数文件和 RViz 截图，作为后续比较的基线。

项目二：Nav2 Tutorials 的 Sam Bot，理解“换成自己的机器人”

Nav2 的 navigation2_tutorials 仓库包含从机器人描述、传感器、里程计、Costmap 到 Nav2 接入的一系列教程，适合从仿真样例过渡到自定义差速底盘。重点学习机器人 URDF、robot_state_publisher、/odom 和 TF 如何提供给导航栈，而不是只看 launch 命令。Nav2 官方教程仓库

可以把 TurtleBot 的参数逐步替换成自己的底盘：轮距、轮径、最大线速度、最大角速度、加减速度、footprint、雷达安装位姿。每次只替换一类参数，再重新验证 TF、里程计和 Costmap。

项目三：SLAM Toolbox，单独研究建图和定位误差

使用 slam_toolbox 的在线模式，比较慢速与快速转弯、不同雷达频率、不同里程计质量下地图的变化。然后保存地图，关掉在线建图，切到静态地图定位。这样可以明确区分两类问题：地图本身画错了，还是机器人在地图中的位姿估计漂移了。SLAM Toolbox 官方仓库

项目四：做一组 A* 与 DWB 的可量化调参实验

固定同一张地图、同一台机器人和同一组起终点，只改一个参数或一组相关参数。可以逐步测试：

实验改动观察指标
A* 代价Costmap inflation / traversal cost路径长度、离障碍物距离、拐弯数量
DWB 速度采样vx_samples、vtheta_samples计算开销、轨迹选择、动作平滑度
预测范围sim_time、轨迹采样间隔动态障碍反应距离、停滞情况
路径偏好PathAlign、PathDist 权重是否贴着全局路线、是否频繁横摆
安全距离footprint、inflation 半径最小障碍距离、窄通道通过率

每组至少重复多次，记录成功率、碰撞次数、完成时间、路径长度、最小障碍距离和恢复行为次数。否则一次运行偶然成功，很难证明调参有效。

四、调参顺序：先校准机器人，再调规划器

第一步：机器人尺寸与运动学

先确认底盘确实能执行发出的速度命令。差速底盘要量清轮距与轮径；用低速直行和原地旋转检查里程计方向、比例和角速度。footprint 应覆盖底盘实际外轮廓，不能为了“通过窄门”而设得比机器人真实尺寸小。

第二步：地图、TF 和雷达

检查墙面和雷达点是否对齐，观察机器人旋转时扫描是否稳定。确定雷达坐标系、扫描方向、有效量程和遮挡范围。若定位或 TF 本身不稳定，调 DWB 评分权重只会让问题更难判断。

第三步：Costmap

先调 global / local Costmap 的分辨率、更新频率、静态层、障碍层、清除射线范围和 inflation。局部地图应及时显示新障碍，并在障碍物离开视野后合理清除。对体积较大的机器人，应结合真实 footprint 与 inflation 留出余量。

第四步：全局 A*

先检查路径是否穿过障碍、是否允许进入未知区域、狭窄处是否符合底盘尺寸。若路径过于贴墙，优先查 Costmap 代价和 inflation；若路径不符合转弯约束，则换用更适配运动学的规划器，而不是只提高 A* 的权重。

第五步：DWB

按真实底盘填写最大速度、最大加速度和减速度；然后根据机器人的制动距离调整预测时长和采样。提高障碍 critic 的权重不一定总能解决避障问题：如果雷达数据太迟、清除设置不对、速度上限太高或 footprint 错了，评分再高也无法弥补输入错误。

现象排查可从这里开始：

  • 路径上左右摆动：检查里程计噪声、控制频率、速度采样和路径对齐 critic。
  • 贴障太近：核对 footprint、inflation、obstacle layer 是否接入正确。
  • 离障碍很远就不敢走：检查 inflation 代价范围、障碍物误报和清除射线。
  • 面对动态障碍反复停住：检查传感器视野、障碍更新频率、DWB 的预测时间及行为树重规划策略。
  • 原地打转或到点不结束：核对目标容差、角速度限制、Goal Checker 和 odom 方向。
  • 规划器无路可走：确认地图坐标、起点与终点、Costmap footprint，以及未知区域策略。

五、真实机器人上的安全边界

仿真里能避开障碍，并不代表真实机器人具有足够的安全制动距离。雷达刷新频率、扫描延迟、CPU 负载、底盘刹车能力、地面摩擦、盲区和人的移动都会影响结果。应先限制低速，在空旷区域验证急停和失联行为，再逐渐提高速度。急停应有独立于 Nav2 的硬件或底盘保护路径；Nav2 的局部规划不能代替安全认证或物理防护。

总结

这个 ROS 2 项目的主线是：先让雷达、里程计和 TF 可信，再用 SLAM 得到地图和位姿；接着把地图、实时障碍和机器人轮廓合成 Costmap；然后由 A* 规划全局路线，再由 DWB 结合局部雷达数据选择短时速度，最后让底盘执行并进入下一轮反馈。

按“官方仿真 → SLAM 建图 → 静态地图定位 → Costmap → A* → DWB → 实体底盘”的顺序推进，每一步都能独立观察和记录结果。这样不仅能完成导航演示，也能把项目写成一篇有原理、有工程结构、有实验数据的 ROS 2 学习记录。