Nav2 工业级导航：多源融合定位与精准对接

扫地机器人级别的导航进不了工厂。开个玩笑，但这话认真讲只有三分玩笑——家用和工业 AMR（自主移动机器人）隔着一整条工程鸿沟：8 小时连续运行不能重启、长走廊里激光雷达特征退化不能丢定位、回充电桩要毫米级对接不能撞。这三关每一关都需要新武器，凑齐了才敢开进产线。

这篇的主线：家用导航的"够用哲学"在工厂逐关失效，每一关补一件什么武器——轮式里程计打滑用 IMU 补、传感器各怀死穴用 EKF 融、导航状态用生命周期节点管、对接精度用三板斧抠。

一、差距清单：家用与工业之间隔着什么

把扫地机器人的配置单拉出来对照工厂需求，差距一目了然：

维度	家用扫地机	工业 AMR
运行时长	断续、可重启	8 小时连班、不许丢定位
环境特征	家具多、特征丰富	长走廊、立库巷道、特征退化
定位精度	`±10 cm` 够用	巡航 `±5 cm`、对接 `±5 mm`
动态障碍	人、宠物，低速容忍	叉车、料车，高速高危
对接要求	大致回桩即可	充电桩毫米级，且每次都得成

最后一行值得强调：每一次对接都得成。一天几十次对接，95% 的成功率意味着每天必有几次失败要人工干预，产线停得起吗？这就是为什么工业导航的每个环节都要冗余、都要可验证。

8 小时连续运行这条也不只是"电池大一点"的事。家用机卡死了原地重启，用户无所谓；工业 AMR 在立库巷道里重启，调度系统里它的任务全悬着，堵住的车道后面排一串料车。连续运行考验的是整个软件栈的稳定性：内存不能泄漏（雷达驱动一晚泄漏 200 MB，三天就 OOM）、TF 树不能断（一个传感器掉线拖垮整棵树）、恢复行为不能死循环（卡住→旋转→还是卡住→再旋转）。Nav2 的很多设计决策，只有放在"8 小时不许停"的语境下才能读懂。

长走廊丢定位这条要单独说，因为它是"重现场景"——平时一切正常，一进长走廊就犯病，等人工赶到现场早就恢复正常，复盘全靠猜。复现方法很朴素：找条 30 m 以上的走廊或立库巷道，让 AMR 以工作速度跑十个来回，录全 rosbag 带回家分析。重点看雷达匹配的协方差曲线：进走廊瞬间协方差沿走廊方向分量飙升，就是几何退化的实锤。

二、多源融合定位：三个各怀死穴的传感器

先看单传感器各自的死穴，才知道融合在救什么。传感器选型阶段就该把这张"死穴清单"过一遍——不是选"最好的传感器"，是选"死穴不会在现场环境里被踩中的传感器"。

轮式里程计：靠编码器积分算位移，死穴是打滑。算个账：轮径 80 mm 的驱动轮，编码器每转 4096 线，分辨率 (π × 0.08) / 4096 ≈ 0.061 mm/线，看着很精。但经过一段洒水车间门口，轮子打滑 2% 而系统毫不知情，跑 50 m 就累计 50 m × 2% = 1 m 的虚假位移——误差不随时间收敛，只随里程累积，而且打滑这件事里程计自己完全检测不到。

更麻烦的是打滑不是小概率事件：叉车式的急加减速、地面油污水渍、载重变化导致的轮地压力波动，都是打滑的日常来源。里程计在干燥地面、匀速直线时是三个传感器里短期最准的，可它的死穴偏偏全藏在工业现场最常见的工作条件里。

IMU（惯性测量单元）：加速度计+陀螺仪，短时间航向无比精准，死穴是漂移。消费级 MEMS 陀螺零偏不稳定性典型 10°/h，不加修正的话一小时后航向凭空偏 10°，位置积分更是灾难——误差随时间平方发散。

这里有个反直觉的事实：IMU 越贵未必越救命。战术级 IMU 固然能把零偏压到 0.1°/h 量级，但单价顶半台 AMR；而 10°/h 的消费级 IMU 在 EKF 里持续被雷达位姿修正，漂移根本没机会累积。融合系统的哲学是"便宜的传感器 + 聪明的滤波"，单点堆料是土豪玩法，这个思路后面还会反复出现。

激光雷达：SLAM 特征匹配定位，不怕漂移，死穴是环境退化。长走廊里两侧墙面平行又光滑，雷达看到的特征点沿走廊方向完全对称，沿走廊方向的位移解不出来（几何退化），AMR 就地"迷路打转"。立库超长巷道是同款死穴。工程师的土办法是在退化走廊的柱子上贴几张反光板或挂个箱子，人为制造特征——听起来糙，但比换雷达便宜两个数量级。

三个死穴恰好互补：里程计短期精度高但会累积、IMU 航向稳但会漂、雷达绝对准但会退化。robot_localization 的 EKF（扩展卡尔曼滤波）就是让三者互相补台：用里程计的短期速度做预测、用 IMU 的角速度锁航向、用雷达的位姿做观测修正，任何单传感器掉线，融合解依然平滑输出。工程上这叫冗余设计——不是三选一的保险，而是任意坏一个、系统降级但不停摆的韧性。

还有一条隐性收益：EKF 输出的不只是位姿，还有协方差——对"自己有多不确定"的量化。下游可以拿它做决策：协方差小说明定位可信，全速跑；协方差突然膨胀说明传感器打架或退化，主动降速甚至停下求援。把不确定性当信号用，而不是只在丢定位后才反应，这是工业系统和演示 demo 的分水岭之一。

ekf.yaml 的关键段（两个 EKF 级联是标准做法：一个融合 odom 里程计+IMU 输出平滑 odom，一个再融合雷达位姿输出 map）：

# ekf_filter_node_odom.yaml —— 第一级：里程计 + IMU → odom
frequency: 50.0
sensor_timeout: 0.1
two_d_mode: true          # 地面机器人锁 Z/横滚/俯仰

odom0: wheel/odom
odom0_config: true, true, false,      # x, y 直接信
              false, false, true,     # 只信偏航角
              true, true, false,      # vx, vy 速度也信
              false, false, true,     # 偏航角速度
              false, false, false

imu0: imu/data
imu0_config: false, false, false,     # 位置不信（会漂）
             false, false, true,      # 偏航角信
             false, false, false,
             false, false, true,      # 偏航角速度最信
             true, true, true         # 加速度计做辅助
imu0_differential: true   # 差分模式：把绝对姿态转成增量再融合
imu0_relative: true        # 相对模式：丢弃上电时的初始偏移

两行注释值得展开：imu0_differential: true 是把 IMU 的绝对姿态读数差分成角增量再融合，避免 IMU 初始航向与里程计坐标系打架；odom0_config 里把 x/y 位置设为 true 而 vy 设 false，是因为差速底盘横向不打滑（理想情况），速度约束帮滤波器压住横向漂移。配置文件里每个 true/false 都在回答"信谁不信谁"，这是融合调参的全部哲学。

怎么验证融合效果？空口说"更稳了"不算数，两个可量化的验收指标：闭环测试——让 AMR 沿 20 m × 20 m 矩形跑十圈，回到起点的偏差 < 5 cm 算合格，纯里程计跑同样路线通常已经偏出半米；创新性序列（NIS）监控——robot_localization 会输出每个传感器的测量残差，某传感器的 NIS 持续超阈值说明它"在撒谎"（里程计打滑、雷达退化），运维面板上盯着这条曲线，传感器故障能在定位丢失前被发现。

三、Lifecycle 与行为树：导航状态机的工业级管法

Nav2 把所有节点实现成 ROS 2 生命周期节点（Lifecycle Node），把"配置"和"激活"拆开：Unconfigured → Inactive → Active 三态，配置阶段加载代价地图参数、分配缓冲区，激活阶段才真正订阅话题、发布速度。收益是产线上换班、重定位、故障恢复时可以 deactivate/reactivate 单个模块，而不是杀掉整个导航栈重启——8 小时连班的机器人，重启一次可能就是一次磕碰事故。

这个设计还带来一个隐性福利：参数在 Inactive 态就能改。要给某台 AMR 调膨胀半径，deactivate → 改参数 → activate，几秒完成且全程可控；反观非生命周期节点，改个参数得杀进程重启，产线上正在跑的任务直接中断。"不重启就能改"在产线上不是便利，是纪律——运维 SOP 里写的就是这套流程。

行为树（BT）负责把"到点"拆成一串带优先级的动作。对接充电桩这种精细活，原生 BT 节点不够用，得写自定义节点。骨架：先开到桩前预停点，二次精定位，再慢速逼近：

// dock_approach_action.hpp —— "到桩前先二次精定位再对接"的 BT 动作节点
#include "rclcpp/rclcpp.hpp"
#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"

namespace nav2_behavior_tree
{
class DockApproachAction : public BtActionNode<nav2_msgs::action::NavigateToPose>
{
public:
  DockApproachAction(const std::string & name, const BT::NodeConfiguration & conf)
  : BtActionNode(name, conf, "dock_approach") {}

  static BT::PortsList providedPorts()
  {
    return {BT::InputPort<double>("pre_dock_distance", 1.0, "预停距离"),
            BT::InputPort<double>("approach_speed", 0.05, "最后逼近速度"),
            BT::OutputPort<bool>("relocalize_ok", "二次定位是否成功")};
  }

  void on_tick() override
  {
    // 阶段 1：到预停点后触发反光板二次定位
    if (!relocalized_) {
      goal_.pose = compute_pre_dock_pose();
      relocalized_ = call_reflector_localization();  // 反光板/二维码精定位
      setOutput("relocalize_ok", relocalized_);
    } else {
      // 阶段 2：以 approach_speed 慢速直线逼近，视觉伺服微调
      goal_.pose = compute_final_dock_goal(approach_speed_);
    }
  }

  BT::NodeStatus on_success() override
  {
    return relocalized_ ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
  }

private:
  bool relocalized_{false};
  double approach_speed_{0.05};
};
}  // namespace nav2_behavior_tree

// 注册进插件列表，BT XML 里直接当动作节点用：
// <DockApproachAction pre_dock_distance="1.0" approach_speed="0.05"/>

对应的行为树 XML 片段，把"导航→精定位→对接"串成带失败重试的序列：

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RetryUntilSuccessful num_attempts="3">
      <Sequence name="dock_sequence">
        <NavigateToPose goal="{pre_dock_goal}" />
        <DockApproachAction pre_dock_distance="1.0"
                            approach_speed="0.05"
                            relocalize_ok="{rel_ok}"/>
        <FallbackStar>
          <Sequence>
            <IsOK condition="{rel_ok}"/>
            <Log text="对接完成，精度达标"/>
          </Sequence>
          <RetryNode name="回退到预停点重来"/>
        </FallbackStar>
      </Sequence>
    </RetryUntilSuccessful>
  </BehaviorTree>
</root>

BT 的价值是把"对接策略"从代码里搬到配置里：产线调整对接流程，改 XML 就行，不用重新编译导航栈；失败重试、超时回退的优先级逻辑也在树里一目了然，出问题时照着树排查。

写自定义 BT 节点时有两个坑值得提前说：一是 providedPorts() 的端口名必须和 XML 里完全一致，拼错不报编译错、只在运行时静默失败，排查极痛苦；二是 on_tick() 每个周期都会被调用，有状态的逻辑（比如上面的 relocalized_ 标志）要自己管理好复位——对接失败重试时忘了把标志清回 false，第二次进来会直接跳过精定位，这就是"偶尔对接歪一次"类的幽灵 bug 来源。

调试 BT 也有趁手工具：Grootono 或 Nav2 自带的 BT 日志能把整棵树的节点状态（RUNNING/SUCCESS/FAILURE）实时染上颜色，对接失败时看一眼哪个节点红了，比翻终端日志快一个量级。产线运维把"打开 BT 可视化再看现场"写进排障手册第一页。

值得强调的是，BT 和 Lifecycle 是配合关系而非两套独立机制：BT 决定"什么时候做什么"，Lifecycle 决定"模块以什么状态待命"。对接流程里二次定位失败要重试三次，第三次失败就得回退到安全点并上报——这套流程逻辑长在 BT 上；而重试过程中需要 deactivate 雷达节点换参数再 activate，动作由 BT 触发、状态由 Lifecycle 管理。两者拼起来才是完整的"导航状态机"，缺一个都只能算半个。

四、工程级导航策略：地图上的三条规矩

融合定位解决了"我在哪"，接下来是"我怎么走"。工厂地图上要立三条规矩：

Keepout 禁行区：costmap 叠一层 keepout layer，把人行通道、消防栓区域、AGV 专用道之外的区域画成禁区，规划器从代价地图层面绕开，而不是靠工程师脑记。还有一个容易忽略的用途：新机器人上线前的"电子围栏"——先给它划一小片测试区域跑通流程，验收后再逐步放开活动范围，比一上来就给全图权限稳妥得多。配置段：

# nav2_params.yaml —— 全局代价地图叠 Keepout 层
global_costmap:
  ros__parameters:
    plugins: ["static_layer", "obstacle_layer", "keepout_layer"]
    keepout_layer:
      plugin: "nav2_costmap_2d::KeepoutLayer"
      enabled: true
      subscribe_to_updates: true
      map_topic: "keepout_map"        # 独立的禁区地图
      transform_tolerance: 0.1

禁区地图就是一张普通栅格图，产线改布局时运维直接改图、subscribe_to_updates 热生效，不动导航参数——把安全边界从代码挪到数据，是工业系统的通用思路。

限速区：人机混行区、转弯口限速，用 nav2 速度控制器+Smoother 配合，在特定多边形区域内把 max_vel_x 从 1.2 m/s 压到 0.3 m/s。

走廊窄道减速：局部代价地图膨胀层根据两侧障碍距离动态减速——窄道里膨胀代价抬高，Smoother 自动压低曲率与速度，实测 1.4 m 窄双向巷道能稳定错车。配套细节是膨胀半径要按"机器人半宽 + 货物伸出量 + 余量"算，AMR 拖着料车时等效宽度比裸机大 20 cm，忘了算这笔账，窄道里就是贴墙蹭货。

三条规矩共同的底层逻辑：把"怎么走安全"编码进代价地图，而不是编码进某个控制器的 if-else。代价地图是所有规划器的公共输入，规矩立在地图层，全局规划器、局部控制器、恢复行为全都自动遵守。反例是早期某项目把"经过涂装车间要减速"写死在控制器代码里，后来产线改造挪了车间位置，代码里的坐标忘了改，AMR 在空地上龟速爬行、真正的涂装区全速冲过——规矩写进数据，改一张图；写进代码，埋一颗雷。

三条规矩的落地还有个共同前提：地图要有版本管理。禁区图、限速区多边形、导航原图，全部进 git 仓库，产线改造走"改图 → 评审 → 发版"流程。听起来重，但比起 AMR 冲进禁区的停产损失，这套流程轻得像羽毛。

动态障碍是三条规矩之外的另一道日常考题。工厂里的叉车、料车不会给 AMR 让路，Nav2 的应对是分层：远距离靠全局代价地图重新规划绕行，中距离靠局部代价地图减速避让，近距离触发恢复行为（停走/绕圈/后退）。分层的关键是"尽早用最便宜的手段"——早 10 米绕行只是多走两秒，等贴脸了才急刹，后面跟着的 AMR 就要连环追尾。把局部避障的预测时域调长一点（代价是计算量），换来的是更从容的动态避障。

五、精准对接三板斧：从 `±5 cm` 到 `±5 mm`

导航定位精度 ±5 cm，充电桩对接要求 ±5 mm，中间差一个数量级，靠三板斧逐级收：

反光板/二维码二次定位：桩前 1 m 预停，用导航雷达扫桩旁反光板（精度可达 ±5 mm 级）或相机读桩面二维码（AprilTag 位姿精度 ±2 mm），把 EKF 定位"洗"一遍，消除巡航累积误差。反光板方案的关键是布置几何：三块以上反光板、围成多边形、各朝向错开，几何约束越"胖"定位越稳，两块反光板排在一条线上时会退化成走廊同款问题；
视觉伺服微调：最后 1 m 用对接相机闭环，图像误差直接映射到横向/航向修正，不走全局定位，帧率闭环 30 Hz，响应比重定位快一个量级。伺服的停止条件也有讲究——别用"图像误差小于阈值"单条件，加上"连续 N 帧都小于阈值"的时间维度过滤，单帧反光干扰就不会让 AMR 误判到位；
最后 10 cm 慢速逼近：0.05 m/s 蛇形逼近，撞上缓冲胶垫前速度归零，留给控制系统的响应余量足够吃掉残余误差。

三板斧在硬件上各有依托：反光板是雷达的事、二维码和伺服是相机的事、慢速逼近是底盘控制器的事——三件武器分属三套硬件，所以对接系统的 BOM 里才同时出现反光板、对接相机和带低速模式的底盘。

三板斧下来，精度路径是：±5 cm（EKF 巡航）→ ±5 mm（二次定位）→ ±2 mm（视觉伺服）→ ±0.5 mm（机械导向销最终落位）。注意精度不是一步到位，是逐级收敛的——每一级只解决上一级的残差，成本也逐级递增，按需取用，别上来就视觉伺服。

对接成功率验收也要讲方法。测试时别拿"今天下午对接了 20 次都对"当结论——样本太小、胎温胎压都是理想态。像样的验收是：满电和低电（电压低时轮端扭矩特性会变）各测 100 次、地面干燥和微湿各一半、统计成功率与横向/航向残差的分布。工业级的"能用"是分布意义上的能用，不是最好一次的能用，这个观念在验收环节和设计环节同样重要。

六、软硬联动

导航的"软"离不开底层的"硬"，两篇前作正好是左右护法：

里程计和雷达时间戳对不上，点云就重影、定位就抖动——多传感器对时的硬件方案见多传感器硬件时间同步：触发电路与编码器接口，EKF 融合的地基；
轮子打滑怎么在里程计毫不知情时被发现？相电流突变就是铁证——三相 FOC 驱动板：栅极驱动与电流采样，打滑检测的底层信号源。

顺带把话题收回来：这篇的三关——融合定位、Lifecycle/BT 状态机、精准对接三板斧——对应的是工厂现场三个最常翻车的场景。把 ekf.yaml 的每个 true/false 调明白、把对接 BT 树在仿真里跑顺、把验收测试做成分布统计，AMR 才算真正从演示品变成生产资料。导航的软件栈再复杂，最终服务的不过是两个朴素的问句："我在哪"和"我怎么走"——工业级只是把这两个问句的容错空间压到了毫米。

参考文献

Moore, T., Stouch, D., et al. "Nav2: Multi-Robot Navigation System." ROS World / IEEE ICRA Workshop, 2022.（Nav2 架构与 Lifecycle 设计）
Nav2 Documentation. "Navigation2 ROS 2 Docs — Costmap 2D and Behavior Trees." docs.nav2.org, 2024.（Keepout 层与 BT 插件接口）
Moore, T., & Stouch, D. "A Generalized Extended Kalman Filter Implementation for the Robot Operating System." Proceedings of ROBOTIK, 2016.（robot_localization EKF 原理与配置）
Tom Moore. "robot_localization Package Documentation." docs.ros.org, 2024.（ekf.yaml 参数语义）
Michel, H., et al. "Navigation2: A ROS 2 Framework for Mobile Robot Navigation." arXiv:2306.10143, 2023.（Nav2 系统设计与实验评估）
ROS 2 Documentation. "TF2 and ROS Coordinates Frames Convention." docs.ros.org, 2024.（odom/map 坐标系语义）