企业官网建设流程全解析

ROS2 导航里最容易被低估的环节就是自定位。很多人把 Nav2 跑起来后，看到机器人能动就以为导航成了，结果 RViz2 里目标点一下，机器人要么原地转圈，要么朝墙冲，根因往往不是规划器，而是 map 到 odom 的自定位漂了。这篇 ROS2 极简总结，我想把导航简介里的自定位单独拎出来讲清楚：它是什么、在 TF 树里干什么活、AMCL 怎么工作、参数怎么调、出问题怎么查。适合刚接触 Nav2 的 ROS2 开发者，也适合已经在调巡检机器人、AGV、差速小车和 3D 雷达平台的同行。你不需要把概率机器人学整本书啃完，但得知道粒子云为什么散、map->odom 为什么不动、初始位姿为什么点了没反应。把这些搞明白，导航调试会少走很多弯路。

1. ROS2导航栈里，自定位到底在算什么

Nav2 的导航链路看起来很长：BT Navigator 管行为树，Planner 出全局路径，Controller 跟局部轨迹，Costmap 维护障碍，Recovery 负责脱困。自定位不在这个链路的最显眼位置，却决定了整条链路能不能成立。因为全局路径是建在 map 坐标系里的，局部控制又依赖机器人当前在 map 里的准确位姿。如果自定位偏了 0.5 米，全局路径可能贴着墙走，局部 costmap 里的障碍也会整体错位，控制器就会一边走一边修，严重时直接顶墙或者绕着空气障碍转圈。

自定位要回答的问题很朴素：机器人现在在已知地图的哪个位置，朝向哪里。输出通常是一个二维位姿 x、y、yaw，外加一个协方差表示“我有多确定”。在 ROS2 里，这个结果不是直接塞给控制器，而是变成 TF 树里的一段变换 map->odom。很多人第一次看 TF 树会懵：为什么要有 odom，为什么还要 map，底盘不是已经有里程计了吗。关键就在于里程计是相对量，它不知道自己在全局地图的哪里。自定位节点的任务，就是不断把里程计的漂移补回来。

1.1 TF树里的分工：map->odom是自定位节点的“签名”

ROS2 导航的标准 TF 链一般是 map -> odom -> base_footprint -> base_link -> laser。map 到 odom 这一段由自定位节点发布，Nav2 里默认是 AMCL。odom 到 base_footprint 由底盘驱动或者 robot_localization 发布，base_footprint 到 base_link 通常是静态变换，base_link 到激光雷达也是静态变换。这个分工不能乱。底盘只负责告诉系统“我从上一次到现在走了多少”，AMCL 负责告诉系统“你在地图里的绝对位置大概在哪里”。

我用ros2 run tf2_ros tf2_echo map odom查定位是否在更新时，最直观的现象是：机器人移动后，map 到 odom 的平移和旋转会变化，而且变化方向通常和机器人运动方向相反。因为 AMCL 在修正 odom 累积的误差，它不是在发布机器人位姿本身，而是在发布“地图坐标系相对于里程计坐标系的偏移”。如果这条命令输出一直不动，而tf2_echo odom base_footprint在动，基本可以判断 AMCL 没有正常工作，或者 scan 没有参与更新，或者生命周期节点没激活。

注意：map->odom 不动不一定是 AMCL 挂了。如果机器人没有移动、没有新激光、没有达到更新阈值，AMCL 也可能不更新。先让机器人动一段距离，或者手动给一个 2D Pose Estimate，再看 TF 是否变化。

1.2 里程计、惯性导航与激光定位各管哪一段

轮式里程计短时间很平滑，尤其是差速底盘，左右轮编码器一算就能得到线速度和角速度。但它有两个硬伤：轮子打滑、轮胎磨损、地面不平、负载变化都会让标定参数失准；长时间积分后，角度误差会积累成位置误差。你让机器人跑十分钟，可能回到起点时已经偏了一米。惯性导航里的 IMU 能补角速度和短时姿态，但加速度积分成速度再积分成位置，漂移也很快，而且零偏不校准会越来越离谱。

激光雷达对已知地图做匹配，能提供绝对修正，但激光也有局限：长廊里几何特征重复，玻璃和镜面反射会骗人，动态人群和货物会挡住视线，地图本身如果是旧的就更容易出错。工程上常见的做法是分工：轮式里程计和 IMU 先融合成连续的 odom->base_link，保证短时平滑和局部控制稳定；AMCL 用激光和地图修正 map->odom，保证全局定位不飘。这样局部控制器能拿到高频平滑的 odom，全局规划又能拿到地图坐标系下的绝对位姿。

我自己调车时，如果底盘 odom 质量差，AMCL 会非常痛苦。粒子云刚收敛，机器人一转弯又散开，因为运动模型预测的粒子分布和真实运动差太多。这个时候不要急着调 AMCL 的激光参数，先把轮距、轮径、编码器分辨率、角速度标定做准，再考虑用 robot_localization 的 EKF 融合 IMU。底盘 odom 是地基，AMCL 是装修，地基歪了，装修再漂亮也白搭。

1.3 自定位误差如何影响全局路径、局部控制和恢复行为

自定位误差对导航的影响是分层的。小误差比如 5 厘米、2 度，控制器还能靠局部 costmap 和轨迹跟踪修正，表现为轻微画龙。中等误差比如 20 到 30 厘米，全局路径会明显偏离通道中心，局部 costmap 里的障碍物位置整体偏移，机器人可能贴着墙走，或者把可通行区域当成障碍。大误差比如半米以上，或者朝向错了 30 度，全局规划器可能规划出一条穿过墙的路径，局部控制器直接报错，恢复行为频繁触发，机器人原地旋转、后退、再规划，看起来像“导航抽风”。

更麻烦的是误差会污染 costmap。障碍层用的是激光点云，如果 map->odom 错了，激光点投影到地图坐标系后也会错位，本来在墙上的点可能被投到通道里，形成幽灵障碍。局部代价地图一膨胀，机器人就无路可走。很多“明明前面没东西，机器人却不敢走”的问题，最后查出来不是 costmap 参数，而是自定位偏了。所以我在排查导航问题时，习惯先看 RViz2 里激光扫描和地图是否重合，再看粒子云是否集中，最后才看规划器和控制器参数。

2. 自定位核心原理：粒子滤波、卡尔曼与传感器融合

自定位的数学本质是贝叶斯估计：机器人不知道自己在哪里，就用一组可能的位姿和对应的概率来描述。每来一次运动，就根据运动模型预测这些可能位姿会跑到哪里；每来一次观测，就根据传感器模型判断哪个可能位姿更像当前看到的激光数据。概率高的位姿保留，概率低的淘汰。AMCL 用的是粒子滤波，把“可能位姿”用一堆粒子表示。粒子云集中的地方，就是机器人最可能的位置。

你不用被“蒙特卡洛”“重要性重采样”这些词吓到，可以把它想成一群候选人在猜位置。每个人手里有一个坐标和朝向，机器人一动，所有候选人都按同样的运动模型移动，但加入随机噪声，所以队伍会散开。激光一来，每个候选人拿自己所在位置的地图激光和真实激光对比，像的人加分，不像的人减分。然后按分数重新抽一批人，分数高的更容易被抽中，分数低的被淘汰。反复几轮后，候选人会聚集到真实位置附近。全局定位时一开始候选人可能撒满整个地图，跟踪定位时候选人只在当前位置附近小范围波动。

2.1 用“扔粒子”理解贝叶斯定位

粒子滤波最适合解释 AMCL 的行为。你给一个初始位姿，粒子云会围绕这个位姿散布；机器人移动，粒子云跟着预测移动；激光更新后，粒子云收缩。如果地图和激光匹配得好，粒子云会越跑越集中。如果匹配得差，粒子云会散成一片，甚至分成几团，表示系统在几个可能位置之间犹豫。这个时候 RViz2 里的 ParticleCloud 非常有用，它比看数字直观得多。

全局定位和跟踪定位要区别对待。跟踪定位假设机器人大概知道自己在哪，初始位姿离真实位置不远，粒子数可以少一些，更新也更稳定。全局定位假设完全不知道位置，粒子要撒满地图，粒子数要多，收敛时间更长，而且容易在相似环境里认错。很多仓库、走廊、机房都有重复结构，激光看到的两面墙可能和地图里另一处一模一样，粒子云就会分叉。这个时候不要只靠 AMCL 硬扛，移动一段距离、增加观测差异、加入 IMU 或者人工确认初始位姿，都比盲目调参数有效。

我通常会做一个简单测试：在 RViz2 里给一个明显错误的初始位姿，看 AMCL 能不能在一段移动后收敛回来。如果收敛不回来，说明地图质量、激光配置或者运动模型有问题。如果能收敛，但很慢，说明粒子数和恢复参数还有优化空间。这个测试比只看最终导航效果更能暴露定位问题。

2.2 AMCL的六个关键步骤

AMCL 的内部循环可以拆成六步。第一步是预测，根据 odom 的变化和运动噪声模型，把每个粒子按运动增量移动，并加入 alpha1 到 alpha4 表示的噪声。第二步是观测更新，把每个粒子所在位置的地图激光和真实激光做比较，用 z_hit、z_rand、z_max、z_short、sigma_hit 这些参数算权重。第三步是重采样，按权重重抽粒子，权重高的粒子更容易留下。第四步是自适应粒子数，用 KLD 采样控制粒子数量，在不确定时增加粒子，确定时减少粒子。第五步是发布位姿，把粒子群的加权平均作为/amcl_pose。第六步是发布 TF，维护 map->odom 变换。

其中观测模型对效果影响很大。likelihood_field模型会预先计算地图上每个点到最近障碍的距离，激光点离障碍越近得分越高，它对地图噪声和激光噪声更宽容，适合大多数 2D 激光导航。beam模型更接近真实激光射线，理论上更锐利，但对地图和激光一致性要求高，计算也更重。我大多数项目用likelihood_field，只有在环境特征非常干净、地图非常准时才会考虑 beam 模型。max_beams控制每次更新用多少条激光，60 到 120 比较常见，太小会欠采样，太大会吃 CPU。

2.3 卡尔曼滤波与惯性导航在工程里怎么配合

卡尔曼滤波和粒子滤波不是二选一。卡尔曼滤波适合连续状态、高斯噪声的估计，比如用 IMU 和轮式里程计融合出平滑的 odom。粒子滤波适合多峰、非高斯的全局定位，比如 AMCL 在多个可能位置之间犹豫。工程里常见组合是：robot_localization 的 EKF 节点融合 wheel odom 和 IMU，输出 odom->base_footprint；AMCL 用激光和地图输出 map->odom。两者各司其职，TF 树不打架。

惯性导航的核心是陀螺仪和加速度计。陀螺仪测角速度，积分得到姿态，短时准但零偏会积累；加速度计测比力，静态时能估计俯仰和横滚，动态时噪声大。EKF 用协方差矩阵描述“我多信轮式里程计，多信 IMU”，然后做加权融合。如果 IMU 零偏没校准，或者时间戳不同步，融合后反而更差。我见过有人把 IMU 直接塞进 EKF，结果机器人静止时 odom 也在缓慢漂，最后查出来是 IMU 的 yaw 零偏和协方差设置太自信。

提示：融合 IMU 之前，先确认 IMU 的安装方向、坐标系、角速度单位和时间戳。ROS2 里常见单位是 rad/s 和 m/s^2，如果驱动发的是角度制，EKF 会直接疯掉。

3. ROS2 Nav2 自定位最小实操：从地图到RViz2初始位姿

理论说再多，不如把一套最小系统跑起来。下面以 ROS2 Humble 或 Jazzy 为例，假设你已经有一张 2D 占据栅格地图和一个能发布/scan的 2D 激光雷达。仿真环境也可以用 Gazebo 或 Ignition 里的 TurtleBot3，但真实底盘更能暴露问题。目标是用 Nav2 的 localization_launch 启动 AMCL 和 map_server，再在 RViz2 里给初始位姿，观察粒子云和 TF 是否正常。

3.1 版本与依赖准备

先确认 ROS2 版本和 Nav2 包是否装好。Humble 和 Jazzy 的 Nav2 参数命名有少量差异，但 AMCL 核心参数基本一致。安装命令很简单，注意把$ROS_DISTRO换成你的发行版名称。

sudo apt update sudo apt install ros-$ROS_DISTRO-nav2-bringup \ ros-$ROS_DISTRO-nav2-amcl \ ros-$ROS_DISTRO-map-server \ ros-$ROS_DISTRO-rviz2 \ ros-$ROS_DISTRO-tf2-tools source /opt/ros/$ROS_DISTRO/setup.bash

如果你用的是源码工作空间，记得colcon build后 sourceinstall/setup.bash。检查ros2 pkg list | grep nav2能看到nav2_amcl、nav2_map_server、nav2_bringup基本就齐了。有些精简系统没有nav2_bringup，只装了nav2_amcl也可以手动启动，但新手建议直接用 bringup，少踩生命周期节点的坑。

3.2 地图文件和AMCL参数怎么写

地图一般由 SLAM Toolbox、Cartographer 或其他建图工具生成，包含一个 pgm 图片和一个 yaml 描述文件。yaml 里最关键的是分辨率、原点和占据阈值。分辨率 0.05 表示一个像素 5 厘米，原点[-10.0, -10.0, 0.0]表示地图左下角在 map 坐标系里的位置。如果原点写错，地图和激光会整体偏移，AMCL 再努力也对不上。

image: room.pgm resolution: 0.05 origin: [-10.0, -10.0, 0.0] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196

AMCL 参数通常放在nav2_params.yaml的amcl命名空间下。下面是一份偏保守、适合室内差速底盘的片段。注意base_frame_id要和你的底盘 TF 一致，常见是base_footprint或base_link；scan_topic要和雷达实际话题一致。set_initial_pose为 true 时可以在启动时给一个初始位姿，适合机器人固定在充电桩上的场景。

amcl: ros__parameters: use_sim_time: false alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 base_frame_id: base_footprint beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 do_beamskip: false global_frame_id: map lambda_short: 0.1 laser_likelihood_max_dist: 2.0 laser_max_range: 12.0 laser_min_range: 0.1 laser_model_type: likelihood_field max_beams: 60 max_particles: 2000 min_particles: 500 odom_frame_id: odom pf_err: 0.05 pf_z: 0.99 recovery_alpha_fast: 0.0 recovery_alpha_slow: 0.0 resample_interval: 1 robot_model_type: nav2_amcl::DifferentialMotionModel save_pose_rate: 0.5 sigma_hit: 0.2 tf_broadcast: true transform_tolerance: 1.0 update_min_a: 0.2 update_min_d: 0.25 z_hit: 0.5 z_max: 0.05 z_rand: 0.5 z_short: 0.05 scan_topic: scan set_initial_pose: false

laser_max_range不要直接写成雷达标称的 100 米，室内通常 10 到 20 米就够，太长会把远处不可靠的数据拉进来。transform_tolerance给 1.0 秒是为了容忍 TF 时间戳的小抖动，但如果设太大，机器人快速旋转时位姿会显得滞后。update_min_d和update_min_a控制移动多少距离或角度才做一次激光更新，太小会浪费 CPU，太大定位更新不及时。

3.3 启动定位、导航与RViz2

如果只是定位，可以只启动 localization_launch。如果要完整导航，用 bringup_launch。下面命令以完整导航为例，地图路径替换成你的实际路径。

ros2 launch nav2_bringup bringup_launch.py \ map:=/home/user/maps/room.yaml \ use_sim_time:=false \ params_file:=/home/user/config/nav2_params.yaml

启动后，另开终端检查生命周期节点状态。AMCL 和 map_server 都是生命周期节点，必须先 configure 再 activate。bringup 通常会自动激活，但如果你手动启动单个节点，可能会发现话题存在却没有 TF。

ros2 lifecycle get /amcl ros2 lifecycle get /map_server ros2 topic echo /amcl_pose --once ros2 run tf2_ros tf2_echo map odom

RViz2 里需要添加几个显示：Map、LaserScan、TF、PoseArray（话题/particle_cloud）、Path。然后用工具栏的“2D Pose Estimate”在地图上点一下并拖出朝向。这个操作会向/initialpose发布一个初始位姿，AMCL 收到后会把粒子云撒到该位置附近。如果点了没反应，先看/initialpose有没有发布，再看 AMCL 是否激活，最后看scan_topic是否正确。

3.4 验证定位质量：TF、amcl_pose、粒子云

判断定位是否靠谱，我一般看四个东西。第一，RViz2 里激光扫描是否和地图墙壁重合。重合得好，说明 map->odom 基本正确；明显错位，说明初始位姿或地图原点有问题。第二，粒子云是否集中。如果粒子云像一团雾散开，或者分成几团，说明 AMCL 不确定。第三，/amcl_pose的协方差。协方差小表示自信，协方差大表示犹豫。第四，机器人移动后 map->odom 是否平滑变化，odom->base_footprint 是否连续。

ros2 topic echo /amcl_pose --field pose.covariance ros2 topic hz /scan ros2 topic hz /amcl_pose ros2 run tf2_ros tf2_echo map base_footprint

实测下来，如果激光和地图重合，粒子云集中，/amcl_pose频率稳定，导航基本就稳了一半。如果激光和地图在旋转方向上差一点，可能是雷达安装角度或者静态 TF 的 yaw 没写对。如果平移方向整体偏，可能是地图 origin 或者初始位姿偏了。别急着调 AMCL 内部参数，先把这些低级问题排掉。

4. AMCL参数调优：粒子数、激光模型和更新阈值怎么选

AMCL 参数很多，但真正影响日常调试的就那么几类：粒子数、激光模型、运动噪声、更新门限、恢复参数。新手容易犯的错是把所有参数都改一遍，结果不知道哪个起了作用。我的建议是每次只动一类，记录现象，逐步收敛。下面按影响从大到小说。

4.1 粒子数范围与KLD自适应

min_particles和max_particles是最直观的参数。粒子越多，定位越稳，但 CPU 和内存也越高。室内差速小车，500 到 2000 通常够用；大仓库、长走廊、全局定位场景可以到 3000 到 5000。如果机器人上算力紧张，不要盲目上 10000，先优化激光更新频率和max_beams。KLD 自适应通过pf_err和pf_z控制粒子数动态调整，pf_err越小，粒子越多，定位越精细但越慢。0.05 和 0.99 是常用组合。

我遇到过一次粒子云在走廊里分成两团，机器人走到岔路口才收敛。后来把max_particles从 2000 提到 4000，同时把update_min_d从 0.25 降到 0.1，粒子云明显更早收敛。代价是 CPU 占用涨了约 15%，但在可接受范围。记住，粒子数解决的是“不确定性”，不是“地图错误”。如果地图本身和现实差很多，粒子再多也收敛不到正确位置。

4.2 激光模型：likelihood_field vs beam

laser_model_type选likelihood_field还是beam，取决于地图质量和环境动态程度。likelihood_field对地图中的小误差不敏感，适合大多数室内场景，参数重点是sigma_hit、laser_likelihood_max_dist、z_hit、z_rand。z_hit表示激光点由真实障碍反射的概率，z_rand表示随机噪声概率，两者加起来通常接近 1。sigma_hit越大，模型越宽容，但太大也会让定位迟钝。

beam模型会模拟每条激光射线，遇到障碍的距离和真实距离比较，理论上更精确，但它对地图和激光的几何一致性要求很高。如果地图是 2D 栅格，雷达又有安装角误差，beam 模型很容易出现权重极端化，粒子云突然全灭或者跳变。我一般先用 likelihood_field 把系统跑稳，再考虑是否切换。max_beams也不要一上来就 360，60 到 90 条已经能提供足够约束，尤其是低成本雷达。

4.3 运动噪声alpha1-alpha5与更新门限怎么调

alpha1到alpha4描述差速运动模型里的旋转和平移噪声，alpha5用于全向模型。默认 0.2 偏保守，适合大多数差速底盘。如果底盘打滑严重、轮距标定不准、地面湿滑，粒子预测会偏离真实运动，表现为粒子云跟不上机器人。这时可以适当增大 alpha 值，让粒子扩散范围更大，容错更强，但也会让定位更“松散”。如果底盘非常精准，可以降低 alpha 值，让粒子云更集中。

update_min_d和update_min_a决定移动多少才更新。设太小，静止时也频繁计算，浪费 CPU；设太大，机器人已经走出半米才更新一次，定位滞后。室内小车常用 0.1 到 0.25 米和 0.1 到 0.2 弧度。transform_tolerance影响 TF 发布的时间戳容差，通常 0.5 到 1.0 秒。如果机器人旋转快，可以适当调小，但太小会导致 TF 查询超时。

4.4 恢复参数不是越大越好

recovery_alpha_slow和recovery_alpha_fast控制 AMCL 的随机粒子注入，用来应对“ kidnapped robot ”问题，也就是机器人被搬走或者定位完全丢失。默认 0.0 表示关闭增强恢复。开启后，长时间观测似然低会触发随机粒子，帮助全局重定位。但这两个参数不是越大越好。在动态环境里，行人、叉车、货物会频繁遮挡激光，如果恢复太激进，AMCL 会不断注入随机粒子，导致粒子云发散、定位跳变。

我的经验是：如果场景动态但机器人基本在固定区域工作，保持恢复关闭，靠人工重新给初始位姿更可靠；如果机器人可能被搬动、上电位置不固定，再开启慢恢复，recovery_alpha_slow设 0.001 左右，recovery_alpha_fast设 0.1 左右，同时提高max_particles。开启后一定要在 RViz2 里观察粒子云，确认它不会在正常情况下乱散。

5. 常见问题与排查：定位漂移、TF超时、粒子云发散

自定位问题看起来千奇百怪，其实排查路径可以很固定：先看数据有没有，再看 TF 通不通，再看时间戳和坐标系，最后才看算法参数。下面这张表是我自己常用的速查表，基本覆盖了八成现场问题。

5.1 症状到根因速查表

这张表不能替代思考，但能让你在现场快速缩小范围。我最常遇到的是 QoS 问题。ROS2 的传感器话题默认可能是best_effort，而 AMCL 订阅如果设成reliable，双方握手失败，话题能echo到但节点收不到。用ros2 topic info /scan --verbose看发布者和订阅者的 QoS，把 AMCL 的scan订阅 QoS 设成sensor_data或best_effort，很多时候问题立刻消失。

5.2 时间同步与QoS导致scan收不到

ROS2 里时间戳是定位的命门。仿真用use_sim_time:=true，真实机器人用false，所有节点必须一致。如果雷达驱动用系统时间，AMCL 用仿真时间，TF 查询会一直失败。检查/clock话题是否存在，ros2 param get /amcl use_sim_time是否符合预期。静态 TF 发布时也要注意时间戳，static_transform_publisher发布的是无限有效期，但动态 TF 不行。

QoS 另一个常见坑是激光雷达驱动发布best_effort，AMCL 默认订阅可能也是best_effort，但中间经过 relay 或 record 后变成reliable，就会断流。我的做法是先用ros2 topic hz /scan确认数据稳定，再用ros2 topic info /scan --verbose看 QoS。如果 AMCL 收不到，可以在参数里把scan的 QoS 覆盖成sensor_data，或者把雷达驱动改成 reliable。不要同时改两边，容易越改越乱。

5.3 多传感器融合时的TF重复发布问题

用了 robot_localization 之后，TF 重复发布是高发问题。底盘驱动可能发布 odom->base_footprint，EKF 也发布 odom->base_footprint，AMCL 又发布 map->odom。如果两个节点发布同一段 TF，RViz2 里机器人会抖动、跳变，AMCL 也会因为 odom 跳变而粒子乱飞。解决方法是保证每段 TF 只有一个发布者。通常让底盘驱动发布原始 odom 话题但不发 TF，让 EKF 融合后发布 odom->base_footprint；AMCL 只发布 map->odom。

检查命令很简单：ros2 run tf2_tools view_frames生成 TF 树，看每段变换的发布者。如果发现同一段有多个 broadcaster，就要在参数里关掉多余的publish_tf。我踩过一次坑：底盘驱动和 EKF 同时发 odom->base_footprint，机器人静止时 TF 也在两个值之间跳，AMCL 的/amcl_pose跟着抖。关掉底盘驱动的 TF 发布后，世界立刻安静了。

5.4 3D雷达和八叉树地图接入Nav2的注意点

3D 雷达越来越便宜，很多项目想直接用 3D 雷达做导航。但 Nav2 的 AMCL 默认吃的是 2DLaserScan，不是PointCloud2。你可以用pointcloud_to_laserscan把点云压成 2D 扫描，选一个合适的高度切片，比如机器人本体上方 0.3 到 1.0 米，避开地面和头顶。切片太矮会把地面点当成障碍，太高会漏掉低矮障碍。滤波方面，先做 passthrough 或 voxel grid 降采样，再转 scan，能显著降低噪声。

八叉树地图适合 3D 占据表达，但 Nav2 的 costmap 主要是 2D 的。你可以用 octomap_server 生成 3D 地图，再投影成 2D 栅格给 AMCL 和全局规划用，或者在 costmap 里加 voxel layer 处理 3D 障碍。注意 3D 地图内存和更新频率，大场景下很容易把工控机吃满。我的建议是：定位仍然用稳定的 2D 地图加 2D scan，3D 点云只用于局部避障和地形分析。这样分工明确，调试也简单。

6. 工程化自定位：上电策略、长期运行和参数模板

把单次跑通不难，难的是让机器人在现场每天稳定工作。自定位的工程化要考虑上电时机器人知不知道自己在哪、长时间运行后定位会不会漂、异常时怎么恢复、多楼层多地图怎么切换。这些问题不是调一个参数能解决的，而是要在系统设计阶段就想清楚。

6.1 已知初始位姿与未知初始位姿策略

如果机器人每次都从固定充电桩出发，可以用已知初始位姿策略。在 AMCL 参数里设置set_initial_pose: true，并给出initial_pose的 x、y、z、yaw。这样启动后 AMCL 直接在该位置附近撒粒子，收敛快，适合 AGV、巡检机器人。但前提是充电桩位置和地图一致，机器人不能被人为搬动。如果机器人可能停在任意位置，就需要未知初始位姿策略：启动时粒子撒满地图，或者让操作员在 RViz2 里点 2D Pose Estimate，或者用二维码、反光板、UWB 等外部手段给一个粗初始位置。

未知初始位姿下，AMCL 需要更多粒子、更长时间收敛。你可以让机器人先原地旋转一圈，或者缓慢前进一段，增加激光观测差异。如果环境重复性太强，比如长走廊两边一模一样，全局定位很容易认错。这个时候不要迷信算法，增加人工确认或者环境特征标记往往更可靠。我在一个机房项目里就遇到过类似问题，最后在入口处贴了反光标识，配合激光强度信息，定位才稳定下来。

6.2 长期运行监控与自动恢复

长期运行时，AMCL 可能因为动态障碍、地图变化、轮子打滑而逐渐偏掉。监控/amcl_pose的协方差是一个简单有效的手段。协方差对角线元素变大，说明 AMCL 不确定。你可以写一个监控节点，订阅/amcl_pose，当 x、y 方差超过阈值时触发重定位，或者让 Nav2 进入恢复行为。RViz2 里的粒子云也要定期看，粒子云发散往往早于导航失败。

自动恢复要谨慎。直接重启 AMCL 会丢失当前位姿，可能让机器人更迷茫。更好的流程是：先停止导航，让机器人原地旋转观察，如果粒子云收敛就继续；如果不收敛，再让操作员介入。对于无人值守场景，可以设置多级恢复：一级是局部重定位，二级是全局重定位，三级是停车报警。不要让机器人带着错误定位继续跑，那是安全事故的温床。

6.3 一份可抄的AMCL参数片段

下面这份参数适合室内差速底盘、2D 激光、动态程度中等的场景。它不是万能钥匙，但可以作为起点。关键改动是提高了粒子数上限，降低了更新门限，开启了轻微恢复，并把激光范围限制在合理区间。复制后记得改base_frame_id、odom_frame_id、scan_topic和use_sim_time。

amcl: ros__parameters: use_sim_time: false alpha1: 0.25 alpha2: 0.25 alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 base_frame_id: base_footprint global_frame_id: map odom_frame_id: odom scan_topic: scan robot_model_type: nav2_amcl::DifferentialMotionModel laser_model_type: likelihood_field max_beams: 90 min_particles: 800 max_particles: 4000 pf_err: 0.05 pf_z: 0.99 update_min_d: 0.15 update_min_a: 0.15 resample_interval: 1 transform_tolerance: 0.8 save_pose_rate: 0.5 z_hit: 0.6 z_rand: 0.4 z_max: 0.05 z_short: 0.05 sigma_hit: 0.2 lambda_short: 0.1 laser_likelihood_max_dist: 2.0 laser_max_range: 15.0 laser_min_range: 0.1 recovery_alpha_slow: 0.001 recovery_alpha_fast: 0.1 set_initial_pose: false

这份参数里，max_particles提到 4000 是为了应对岔路口和相似环境，如果 CPU 吃紧可以降到 2500。recovery_alpha_slow和fast开启后，动态环境里如果出现粒子乱散，可以先把fast降到 0.05 或直接归零。laser_max_range设 15 米，是因为大多数室内 2D 雷达在 15 米外已经噪声很大，保留太远的数据反而增加计算量。

6.4 从2D到3D的扩展思路

2D 自定位跑稳之后，再考虑 3D 扩展。第一步是用 SLAM Toolbox 建一张高质量的 2D 地图，确保地图和现实一致，墙角、柱子、门框位置准确。第二步是融合 IMU，用 robot_localization 输出更平滑的 odom。第三步是接入 3D 雷达，用点云做局部避障和地形分析，定位仍然以 2D scan 加 AMCL 为主。第四步如果有多楼层，可以做多地图管理，在不同楼层切换不同的 map 和 AMCL 实例，或者用一张大图加高度层。

多楼层场景要特别注意初始位姿和地图切换。电梯口、楼梯口是定位最容易出错的地方，因为上下层地图可能相似。可以在关键位置设置人工确认点，或者用楼层标识辅助。3D 雷达和八叉树地图能提供更多几何信息，但也会增加系统复杂度和算力消耗。我的原则是：定位链路越简单越可靠，能 2D 解决的不要上 3D，能单地图解决的不要多地图。每增加一层复杂度，现场调试时间都会成倍增加。

最后再分享一个小技巧：把/amcl_pose的协方差和 TF 的跳动情况记录成 rosbag，出问题时回放比现场猜快得多。尤其是偶发的定位跳变，现场可能只出现一次，rosbag 能让你反复观察粒子云、激光和 TF 的对应关系。很多看似玄学的问题，回放几遍就能找到规律。