从零搭建 Gazebo 仿真系统：SLAM 建图、地图保存与 Nav2 导航全流程实战
本文以搭载 Jetson Orin Nano 的 ROSOrin Pro 为对象，在 Ubuntu 22.04、ROS 2 Humble 与 Gazebo Classic 组合环境中，完整复现机器人模型加载、传感器仿真、二维 SLAM 建图、地图保存、单目标导航和多目标点导航。重点不只是“把程序跑起来”，而是说明每一层数据为什么存在、如何验证、出现异常时应从哪里排查。
一、实验目标：最终要得到什么
这次实验不是单独打开一个 Gazebo 窗口，也不是只在 RViz 中看到一张地图，而是打通从物理仿真到自主移动的完整闭环。完成后应得到以下结果：
- Gazebo 能稳定载入室内世界和 ROSOrin Pro 模型，机器人不会下落、弹飞、持续抖动或穿透地面。
- ROS 2 中能够持续收到激光雷达、IMU、里程计和仿真时钟数据，消息时间戳连续且频率基本稳定。
- TF 树能够形成连续的
map -> odom -> base_footprint/base_link -> laser/imu关系，不存在断链或重复发布。 - 键盘控制指令可以驱动仿真机器人运动，Gazebo 中的位姿变化与 RViz 中的机器人姿态保持一致。
- SLAM 运行后，RViz 能逐步生成边界清晰的二维占据栅格地图，闭环区域不会出现明显双墙或大范围错层。
- 地图可以成功保存为图像与元数据，并在新的导航会话中正确加载。
- 完成初始位姿设定后，AMCL 能稳定定位，粒子云逐渐收敛。
- 发送单个目标位姿后，Nav2 能生成全局路径，局部控制器能够持续输出速度并使机器人到达目标附近。
- 设置多个航点后，机器人可以按照队列依次执行，界面能够显示当前任务状态。
- 当目标不可达、定位错误或局部规划受阻时，能够根据 TF、话题、代价地图和生命周期状态定位问题，而不是盲目重启全部节点。
为了便于理解，可以把整个实验分为五层：
任何上层功能都依赖下层成立。例如，导航没有路径不一定是规划器参数问题，也可能是地图没有加载、AMCL 未定位、TF 断开或激光数据没有进入代价地图。因此，本文始终按照“先验证底层数据，再启动上层算法”的顺序推进。
二、实验对象与总体架构
2.1 ROSOrin Pro 在本实验中的抽象
ROSOrin Pro 实机是一套由高性能 ROS 主控、底层运动控制器、编码器电机、IMU、二维激光雷达以及扩展执行机构组成的移动机器人。具体到 Gazebo 建图与导航，本实验最关心的不是机械臂或视觉模块，而是以下四类基础能力：
- 可控底盘： 接收二维速度指令，完成前进、后退和转向；物理版本可采用麦克纳姆轮等底盘结构，但二维导航的基本闭环仍围绕线速度、角速度、里程计和底盘坐标系建立。
- 二维激光雷达： 输出周围障碍物的距离扫描，是 SLAM、AMCL 和局部代价地图的主要观测来源。
- 轮式里程计与 IMU： 提供短时间内连续的运动估计。它们并不能单独消除累计误差，但能够帮助算法维持运动连续性。
- 统一坐标系： 用 TF 描述地图、里程计、底盘和传感器之间的空间关系，使每一条传感器数据都能被转换到算法所需坐标系。
Gazebo 中的机器人模型应尽量复现这些接口。模型外形是否精细，只影响视觉效果；碰撞几何、惯性参数、轮地接触、传感器安装位姿和话题接口是否正确，才决定建图与导航能否可靠运行。
2.2 仿真计算在哪里执行
本文以 ROSOrin Pro 为机器人对象，但主要仿真计算运行在 PC 虚拟机中。Jetson Orin Nano 实机不需要参与 Gazebo 物理计算，这样可以避免硬件尚未就绪时阻塞算法调试。仿真通过后，再把 ROS 2 节点、参数和接口迁移到机器人主控。
这种架构带来三个直接好处：
- 可重复。 每次都能从相同世界、相同出生点和相同地图开始，便于对比参数修改前后的效果。
- 低风险。 规划器发出错误速度时，最多表现为虚拟机器人碰撞，不会损伤电机、雷达和机械结构。
- 易观测。 Gazebo 可查看真实仿真位姿，RViz 可查看算法估计位姿，两者对比能快速判断误差来自物理模型还是算法层。
2.3 ROS 2 数据闭环
建图阶段的数据流如下：
导航阶段的数据流则变为：
注意建图和导航对 map -> odom 的发布者不同：建图时通常由 SLAM 节点发布；载入静态地图导航时，通常由 AMCL 发布。两者不应在同一时刻争抢同一 TF，否则会出现坐标跳变、机器人模型闪烁或规划结果反复重置。
三、环境选型与资源规划
3.1 推荐软件组合
本文实验基于以下组合：
| 层级 | 建议配置 | 作用 |
|---|---|---|
| 宿主系统 | Windows 10/11 64 位 | 运行虚拟化软件与图形界面 |
| 虚拟化软件 | VMware Workstation | 承载 Ubuntu 仿真系统 |
| 客体系统 | Ubuntu 22.04 LTS | ROS 2 Humble 的主流目标系统 |
| ROS 发行版 | ROS 2 Humble | 节点、话题、TF、SLAM 与导航框架 |
| 仿真器 | Gazebo Classic | 机器人动力学、碰撞和传感器模拟 |
| 可视化工具 | RViz2 | 显示地图、激光、TF、规划路径和代价地图 |
| 建图模块 | SLAM Toolbox 类二维 SLAM | 在线构建占据栅格地图 |
| 导航模块 | Nav2 | 定位、规划、控制、恢复与任务管理 |
这里的“Gazebo”特指与 ROS 2 Humble 工程接口配套的 Gazebo Classic 体系。不要在同一次实验中混用 Gazebo Classic 的启动方式和新版 Gazebo Sim 的 gz sim 命令，因为两者的插件、资源和桥接机制并不相同。
3.2 宿主机资源建议
Gazebo、RViz、SLAM 和 Nav2 同时运行时会持续占用 CPU、内存与图形资源。建议为虚拟机预留：
- 4 个或更多逻辑处理器；复杂场景可分配 6 至 8 个，但不要把宿主机全部核心都交给虚拟机。
- 8 GB 内存作为可运行下限，12 GB 至 16 GB 更适合同时打开 Gazebo、RViz 和多个终端。
- 至少 40 GB 可用磁盘空间，避免编译产物、日志和快照占满系统盘。
- 1920×1080 或更高显示分辨率，便于同时观察 Gazebo 与 RViz。
- BIOS/UEFI 中启用 CPU 虚拟化能力，Windows 中避免多个虚拟化后端互相争用。
资源并不是越多越好。虚拟机分配过多 CPU 后，宿主系统无法及时处理图形、磁盘和输入事件，反而可能导致 Gazebo 界面卡顿。判断性能时应重点关注 Gazebo 的实时因子：如果仿真时间长期明显落后于墙上时间，机器人控制和传感器频率都会被拖慢。
3.3 为什么建议使用桥接网络
仅在本机完成仿真时，NAT 网络通常也能工作；如果后续要让虚拟机与 ROSOrin Pro 实机、另一台开发电脑或局域网工具通信，桥接模式更直观。桥接后，虚拟机作为局域网中的独立主机获取地址，便于执行跨设备的 ROS 2 DDS 发现和远程调试。
配置桥接时需要确认：
- 虚拟网卡桥接到当前真正联网的物理网卡，而不是断开的有线接口或虚拟适配器。
- 宿主机与虚拟机处于同一网段，并能互相连通。
- 防火墙没有阻断 DDS 使用的 UDP 组播和动态端口。
- 多机 ROS 2 使用相同的
ROS_DOMAIN_ID，且没有误设只允许本机通信的选项。
3.4 3D 加速的取舍
在部分 Windows 显卡驱动与 VMware 组合中，启用虚拟机 3D 加速会出现 Gazebo 黑屏、模型透明、界面花屏或直接崩溃。遇到这些现象时，优先关闭虚拟机显示设置中的 3D 图形加速并重新启动虚拟机。关闭后渲染性能可能下降，但稳定性通常更好。
如果关闭 3D 加速后仍无法正常显示，可在终端检查 OpenGL 渲染器。软件渲染可以用于功能验证，但复杂世界下帧率较低。工程上应先保证物理仿真和 ROS 数据链稳定，再追求更高的画面帧率。
四、搭建 Ubuntu、ROS 2 与 Gazebo 环境
4.1 导入现成系统与全新安装的区别
环境准备有两种方式：
- 导入已经配置好的 Ubuntu 虚拟机。 优点是 ROS 2、Gazebo、依赖和仿真功能包通常已经集成，适合快速复现实验；缺点是需要确认虚拟机的 CPU、内存、网络和显示设置是否适合当前电脑。
- 从 Ubuntu 22.04 全新安装。 优点是环境透明、便于长期维护；缺点是需要自行配置 ROS 2 软件源、安装依赖、编译仿真功能包并处理版本匹配。
无论采用哪一种方式，都建议先完成系统快照。一个“刚完成依赖安装但尚未修改参数”的干净快照，可以在环境损坏时快速回退。
4.2 基础依赖安装
如果使用全新系统，先更新软件包索引，然后安装 ROS 2 桌面环境、Gazebo 接口、机器人模型工具、SLAM 与 Nav2 组件。以下命令给出一套覆盖本实验的典型依赖：
sudo apt update
sudo apt install ros-humble-desktop \
ros-humble-gazebo-ros-pkgs \
ros-humble-xacro \
ros-humble-robot-state-publisher \
ros-humble-joint-state-publisher-gui \
ros-humble-slam-toolbox \
ros-humble-navigation2 \
ros-humble-nav2-bringup \
ros-humble-tf2-tools \
python3-colcon-common-extensions
若使用已经准备好的虚拟机，不要为了“确保完整”而无条件重复安装全部组件。先执行版本检查和包检查，只有确认缺失时再补装，能够减少软件源变化引起的依赖替换。
4.3 ROS 2 环境检查
打开新终端后，依次检查 ROS 发行版、命令行工具和系统诊断结果：
echo $ROS_DISTRO
ros2 --help
ros2 doctor --report
预期 ROS_DISTRO 为 humble。ros2 doctor 的报告可能包含网络接口、RMW 实现和软件版本提示，应重点排除“ROS 环境未加载”“同一终端混入其他 ROS 发行版”“RMW 实现不一致”等问题。
如果多个终端分别启动 Gazebo、SLAM 和导航，每个终端都必须加载相同的 ROS 2 基础环境与同一个工作空间覆盖层。常见现象是第一个终端能找到仿真包，第二个终端提示找不到包，本质上不是功能包消失，而是第二个终端没有加载环境。
4.4 导入并编译仿真功能包
将 ROSOrin Pro 仿真功能包放入 ROS 2 工作空间的源集合后，在工作空间根级执行：
colcon build --symlink-install
--symlink-install 对 Python、启动脚本和配置文件迭代较方便，修改后不必每次复制资源。编译完成后应看到所有目标包成功结束；如果出现失败，不要只看最后一行，要向上找到第一个报错包，因为后续“aborted”通常只是依赖连锁反应。
加载工作空间环境后，可用以下命令确认核心包已经被 ROS 2 索引：
ros2 pkg prefix robot_gazebo
ros2 pkg executables robot_gazebo
第一条命令应返回包的安装前缀，第二条应至少列出本实验需要的可执行节点。如果包无法发现，检查编译结果和环境加载；如果包能发现但启动文件无法发现，再检查功能包安装规则是否包含启动与配置资源。
4.5 最小运行验证
在启动复杂室内世界前，先验证 ROS 2 和 Gazebo 能否独立通信。启动 ROS 2 主节点后查看节点与话题：
ros2 node list
ros2 topic list
Gazebo 启动后通常能看到与仿真时钟、模型状态或底盘插件相关的节点和话题。最关键的是 /clock：使用仿真时间的节点依赖该话题推进内部时间。如果 Gazebo 暂停，/clock 也会停止，SLAM、AMCL 和控制器可能表现为不更新，这属于正常机制。
检查仿真时间：
ros2 topic hz /clock
ros2 topic echo /clock --once
如果 /clock 不存在，说明 Gazebo ROS 插件没有正常加载；如果 /clock 存在但上层节点仍使用系统时间，应检查这些节点的 use_sim_time 参数是否开启。
五、理解 ROSOrin Pro 的 URDF/Xacro 与 Gazebo 模型
5.1 为什么模型正确比外观精细更重要
URDF 用连杆和关节描述机器人结构，Xacro 在此基础上提供宏、变量和文件复用能力。一个可用于 Gazebo 导航的模型至少包含三套几何信息：
visual：决定用户在 Gazebo 和 RViz 中看到什么。collision：决定机器人与墙体、地面及障碍物如何碰撞。inertial：决定质量、质心和惯性矩阵，直接影响运动稳定性。
很多“模型能显示但不能运动”的问题，来自只配置了视觉模型，却遗漏碰撞体、惯性参数或传动插件。反过来，碰撞几何过于复杂也会显著增加物理计算量。工程上通常使用精细网格表现外观，使用简单盒体、圆柱体近似碰撞轮廓。
5.2 连杆、关节与底盘基准坐标系
移动机器人通常以 base_link 表示机体主坐标系，并可增加贴合地面的 base_footprint。常见约定是：
- X 轴指向机器人前方。
- Y 轴指向机器人左侧。
- Z 轴指向机器人上方。
- 偏航角绕 Z 轴旋转，逆时针为正。
激光雷达坐标系和 IMU 坐标系通过固定关节连接到底盘。如果雷达在模型中位于车体前上方，那么 TF 中也必须反映相同平移和旋转。传感器插件输出的数据带有 frame_id，该名称必须能在 TF 树中找到；否则 RViz 会提示无法转换，SLAM 也无法把扫描点投影到机器人位姿上。
理想 TF 主链如下：
map
└── odom
└── base_footprint
└── base_link
├── laser_frame
└── imu_link
其中：
map -> odom负责修正里程计累计漂移，由 SLAM 或 AMCL 发布。odom -> base_footprint表示局部连续运动，通常由底盘或 Gazebo 插件发布。base_link -> laser_frame/imu_link是传感器安装外参，由机器人状态发布器提供。
5.3 Gazebo 传感器插件需要关注什么
激光雷达插件通常包含扫描角范围、角分辨率、最小/最大测距、更新频率、噪声模型、输出话题和坐标系名称。用于二维 SLAM 时，最重要的不是把最大测距设置得很远，而是让扫描范围、频率和噪声接近实机。过于理想的零噪声雷达可能让仿真地图看起来漂亮，却不能暴露实机中的匹配敏感性。
IMU 插件应明确输出角速度、线加速度以及对应协方差。若算法没有使用 IMU，可以保留数据用于后续融合验证，但不要误以为仅启动 IMU 话题就会自动改善定位；只有状态估计节点订阅并正确配置后，IMU 才会进入融合链路。
底盘插件需要把速度指令转换为轮子运动，同时发布里程计或 TF。对 ROSOrin Pro 这类移动平台，需要检查：
- 速度指令话题名称是否与遥控节点、Nav2 控制器一致。
- 轮子关节名称和旋转方向是否正确。
- 轮距、轮径或全向运动学参数是否与模型一致。
- 里程计坐标系和底盘坐标系名称是否统一。
- 是否只有一个节点发布
odom -> base，避免重复 TF。
5.4 惯性参数错误的典型表现
若模型一出生就弹飞、旋转或穿透地面，应优先检查：
- 质量是否为零或小到不合理。
- 惯性矩阵是否为正定，主对角项是否大于零。
- 碰撞体是否与地面深度重叠。
- 轮子与地面的摩擦系数是否异常。
- 固定关节是否形成冲突约束。
- 模型出生高度是否足以让轮子自然落到地面。
不要通过把重力关闭来掩盖惯性错误。关闭重力可能让模型暂时“稳定”，但底盘接触、轮胎摩擦和导航运动都会失去物理意义。
六、启动 ROSOrin Pro 模型并完成第一轮自检
6.1 启动基础世界
加载模型与基础世界：
ros2 launch robot_gazebo worlds.launch.py
Gazebo 正常打开后，应看到工具栏(1)、菜单栏(2)、动作栏(3)和场景(4)，界面布局大致如下。
Gazebo 常用交互包括：鼠标左键选择或平移视图，鼠标中键配合拖动旋转视角，滚轮缩放；工具栏可切换选择、移动和旋转模式。调试时可以临时移动模型观察碰撞，但建图与导航实验开始后不要手工拖拽机器人，因为这会让仿真真实位姿瞬间变化，而里程计和定位算法未必同步接受这一跳变。
模型成功载入后的效果如下。
6.2 观察模型静止状态
启动后先不要立即发送运动指令，等待 5 至 10 秒，观察：
- 机器人是否平稳落地。
- 四个轮子是否保持合理接触。
- 机体是否持续缓慢漂移。
- 雷达或传感器模型是否与车体分离。
- Gazebo 是否保持运行而不是暂停状态。
- 实时因子是否长期过低。
“看起来没动”不是充分条件。可检查里程计是否在静止时持续积累明显位移。如果静止机器人每秒都出现厘米级位置变化，后续地图会产生模糊和重影，应先解决底盘物理或里程计噪声问题。
6.3 检查节点和关键话题
在第二个终端查看：
ros2 node list
ros2 topic list
不同实现的具体名称可能不同，但至少应能找到以下语义的数据：
| 数据 | 常见消息类型 | 用途 |
|---|---|---|
| 激光扫描 | sensor_msgs/msg/LaserScan |
SLAM、AMCL、局部避障 |
| IMU | sensor_msgs/msg/Imu |
姿态与运动状态估计 |
| 里程计 | nav_msgs/msg/Odometry |
局部连续位姿 |
| 速度指令 | geometry_msgs/msg/Twist |
遥控和 Nav2 控制输出 |
| TF | tf2_msgs/msg/TFMessage |
坐标变换 |
| 仿真时钟 | rosgraph_msgs/msg/Clock |
统一仿真时间 |
使用 ros2 topic info 核对发布者、订阅者与消息类型。例如：
ros2 topic info /scan -v
ros2 topic info /odom -v
ros2 topic info /cmd_vel -v
如果实际工程采用不同话题名，应以 ros2 topic list 的结果为准。不要仅凭经验修改上层参数，因为仿真包可能已经通过重映射完成接口适配。
6.4 检查频率和数据内容
ros2 topic hz /scan
ros2 topic hz /odom
ros2 topic echo /scan --once
ros2 topic echo /odom --once
检查重点：
LaserScan.header.frame_id是否对应雷达坐标系。ranges是否包含大量有效有限值，而不是全部为inf、零或nan。range_min小于range_max，角度起止范围合理。- 里程计消息中的位置与姿态在运动时连续变化。
- 所有消息时间戳随
/clock推进。
扫描频率不是越高越好。若 Gazebo 实时因子下降，标称 10 Hz 的雷达在墙上时间中可能只有几 Hz。SLAM 仍可能运行，但控制延迟和地图匹配质量会下降。
6.5 检查 TF 树
ros2 run tf2_ros tf2_echo odom base_link
ros2 run tf2_ros tf2_echo base_link laser_frame
如果工程使用 base_footprint 或其他雷达坐标系名称，应替换为实际名称。第一条变换在机器人运动时应连续变化；第二条属于固定安装关系，应保持不变。
RViz 中也可以添加 TF 显示，打开坐标轴和名称。出现以下任一现象都应停止继续启动 SLAM：
- 雷达坐标轴在机器人运动时脱离车体。
- 同一个底盘坐标系在画面中来回跳动。
- RViz 固定坐标系设为
odom后仍看不到激光。 - 终端持续出现 extrapolation into the future/past。
- TF 树中同时存在两条互相矛盾的底盘链。
6.6 验证速度控制闭环
可以短暂启动键盘控制节点：
ros2 run robot_gazebo teleop_key_control
将输入焦点放在该终端，根据界面提示发送前进、后退、左转和右转指令。基础按键关系为：
| 按键 | 行为 | 操作建议 |
|---|---|---|
w |
持续前进 | 点按，小步移动 |
s |
持续后退 | 点按，避免长距离倒车 |
a |
向左转向 | 视转角短按或长按 |
d |
向右转向 | 视转角短按或长按 |
第一轮只移动一小段距离，然后检查三件事：Gazebo 中机器人确实运动；里程计方向与实际运动一致；雷达点云在 RViz 的固定坐标系下与环境关系合理。如果按前进后里程计 X 反向变化，或左转时偏航角符号错误，应先修正底盘运动学和坐标约定。
七、在正式建图前建立可观测性
7.1 RViz 显示项建议
建图 RViz 建议至少添加：
RobotModel：确认机器人描述与 TF 正常。TF：查看坐标树和各坐标轴方向。LaserScan：确认扫描点与墙面重合。Map：观察占据栅格地图生成过程。Odometry：观察里程计轨迹和朝向。
固定坐标系在建图阶段设置为 map。刚启动 SLAM 时，map 坐标可能尚未建立，短暂报错可以接受；一旦收到有效扫描，map -> odom 应出现并持续更新。
7.2 仿真时间必须统一
Gazebo 中所有算法节点都应使用仿真时间。如果一部分节点使用系统时间，另一部分使用 /clock，最常见后果是 TF 数据看似存在却始终无法在消息时间戳处查询。检查参数：
ros2 param get /slam_toolbox use_sim_time
导航阶段可对 AMCL、地图服务器、控制器和代价地图节点做同样检查。返回 True 才符合纯仿真实验预期。
7.3 建图前的五项门禁
在正式启动 SLAM 前，建议执行以下门禁：
| 门禁项 | 通过条件 | 不通过的直接影响 |
|---|---|---|
| 模型稳定 | 静置无明显漂移、抖动或穿模 | 地图边缘模糊、运动不可控 |
| 激光有效 | 有稳定扫描且坐标系可转换 | SLAM 无地图或持续丢匹配 |
| 里程计连续 | 运动方向正确、无大幅跳变 | 地图错层、闭环失败 |
| TF 完整 | odom 到雷达链路连续 |
扫描无法投影到地图 |
| 仿真时间统一 | 算法节点均使用 /clock |
消息过滤器丢数据、TF 超时 |
只有五项全部通过，才值得进入建图。这样做看似多花几分钟，实际上能避免在 SLAM 参数中寻找一个根本不存在的“算法问题”。
八、Gazebo 室内场景与 SLAM 建图实验
8.1 启动室内仿真世界
先启动包含墙体、通道和障碍物的室内场景：
ros2 launch robot_gazebo room_worlds.launch.py
正常结果应包含室内环境与 ROSOrin Pro 模型。场景启动效果如下。
此时先观察机器人出生点：机体不应与墙面、家具或地面重叠；雷达水平扫描面不应被车体自身结构大面积遮挡。若出生点过于靠近障碍物，局部碰撞会让机器人刚启动就产生位移，导致初始地图质量下降。
8.2 启动 SLAM
保持 Gazebo 终端运行，在新的终端启动建图：
ros2 launch robot_gazebo slam.launch.py
启动过程通常会拉起 SLAM 节点与 RViz 配置。正常界面应能看到机器人、激光扫描和正在扩展的地图区域。
不要一看到 RViz 窗口就立即高速遥控。等待节点进入工作状态，确认固定坐标系为 map，激光点能显示，地图话题已有发布者，然后再开始探索。
可检查：
ros2 topic info /map -v
ros2 topic hz /map
ros2 topic hz /scan
地图不一定按固定高频率发布，因为许多 SLAM 实现只在达到时间或位移阈值后更新。只要机器人移动后地图能持续扩展，就不必追求很高的 /map 频率。
8.3 启动键盘遥控
在第三个终端执行：
ros2 run robot_gazebo teleop_key_control
将鼠标焦点放入遥控终端后再按键。如果机器人没有反应，先检查终端是否真的接收到按键，再检查速度话题是否有消息：
ros2 topic hz /cmd_vel
ros2 topic echo /cmd_vel --once
如果速度话题有数据但机器人不动，问题在底盘插件、话题重映射或 Gazebo 暂停状态；如果速度话题没有数据，则问题在遥控节点或终端焦点。
8.4 推荐的建图行驶策略
高质量建图依赖可观测、平稳和有重叠的扫描，不依赖高速覆盖。建议按以下顺序行驶：
- 原地小角度观察。 出生后缓慢转动少量角度，让 SLAM 建立初始环境特征。
- 沿外边界探索。 贴近但不要紧贴外墙行驶，优先形成地图整体轮廓。
- 进入内部通道。 在每个转角前减速，使雷达连续观察转角两侧结构。
- 覆盖支路与凹槽。 对门口、短走廊和家具背面进行补扫，减少未知区域。
- 返回已知区域。 重新经过起点或明显结构，给算法提供闭环约束。
- 检查后再补图。 在 RViz 中寻找断墙、双墙、大片灰区或未闭合边界，有针对性地再次经过。
一条实用原则是：直线段保持低速，转弯更慢，避免一边高速平移一边急转。二维激光每一帧扫描需要与里程计和 TF 匹配，角速度过大会放大扫描畸变与时间同步误差。
8.5 建图过程中的画面
遥控 ROSOrin Pro 在 Gazebo 中逐步探索环境时，可以同时观察物理场景和 RViz 地图。
在占据栅格地图中，一般可这样理解颜色：
- 黑色或深色：被判断为占据的区域，通常对应墙体和固定障碍物。
- 白色：被激光确认可通行的自由空间。
- 灰色：尚未观测或无法确认的未知空间。
地图不是 Gazebo 世界的俯视截图，而是算法基于激光、里程计和位姿估计得到的概率表示。因此墙体厚度、边缘锯齿和局部空洞都受分辨率、传感器噪声、运动轨迹与匹配质量影响。
8.6 什么叫“地图已经建完”
地图覆盖到场景边界并不等于质量合格。建议同时满足：
- 主要可通行区域已经转为白色，不再被大块未知区域分割。
- 外墙和主要内墙连续，无明显断口。
- 同一堵墙没有平行的双重轮廓。
- 回到起点附近后，地图没有整体跳动或错层。
- 机器人在 RViz 中的位置与 Gazebo 中所处房间一致。
- 雷达点大体落在地图边界上，而不是与墙体持续错开。
- 狭窄通道宽度足以容纳机器人 footprint 和安全膨胀层。
完成一次覆盖并补扫后的地图效果如下。
如果地图出现局部瑕疵，不要立刻从头开始。先让机器人平稳返回该区域，以不同观察角度补扫；如果整张地图已发生大尺度扭曲或重复，才考虑重新建图并降低运动速度。
8.7 保存地图
地图质量确认后，在配置好的地图输出位置执行：
ros2 run nav2_map_server map_saver_cli -f map_01 \
--ros-args -p map_subscribe_transient_local:=true
map_01 是本次地图名称，可以替换为有意义的版本名。map_subscribe_transient_local:=true 让保存器使用适合静态地图的 QoS 获取最近一次地图，即使保存器启动时间晚于地图发布，也能收到已保留的数据。
正常情况下会生成一张地图图像和一个元数据文件。终端应显示正在接收地图、写入图像和写入元数据，且没有超时或权限错误。保存后不要只相信“成功”日志，还要检查图像能否打开、尺寸是否合理、元数据中的图像名称与分辨率是否正确。
典型元数据包含：
image: map_01.pgm
mode: trinary
resolution: 0.05
origin: [0.0, 0.0, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25
其中 resolution 表示每个像素对应的实际米数；origin 给出地图图像原点在世界坐标中的位姿；占据与自由阈值决定灰度值如何转换为地图状态。不要随意修改 origin，否则导航时地图与机器人初始坐标会发生整体偏移。
8.8 地图版本管理建议
实验中不要反复覆盖同一个地图名称。建议使用能表达场景和迭代信息的名称，例如：
room_clean_v1
room_fullscan_v2
room_nav_tuned_v3
每张地图同时记录：仿真世界版本、机器人模型版本、SLAM 参数、建图日期、是否完成闭环、已知缺陷和推荐导航参数。地图本质上是导航系统的基础数据，应像配置一样可追溯，而不是一张来源不明的图片。
8.9 正确结束建图会话
地图保存并验证后，再逐个终止遥控、SLAM 和 Gazebo。建议先停止遥控，确保没有残留速度；再停止 SLAM；最后关闭 Gazebo。不要在地图写入过程中直接关闭终端，以免只生成一个文件或写入不完整。
至此，建图阶段的完整成果应包括：可重复启动的室内世界、稳定的 ROSOrin Pro 模型、通过验证的传感器与 TF、覆盖完整的二维地图，以及能被 Nav2 重新加载的地图元数据。
九、从建图切换到导航：系统角色发生了什么变化
9.1 为什么不能直接在建图会话中启动导航
建图阶段的目标是同时估计机器人轨迹和未知地图，导航阶段的目标则是在一张已知静态地图中估计机器人位姿并执行路径。两者都需要维护 map -> odom，但发布者不同：
- 建图时由 SLAM 根据扫描匹配和闭环结果修正里程计漂移。
- 导航时由 AMCL 根据静态地图、激光扫描和里程计估计全局位姿。
如果不关闭 SLAM 就再启动 AMCL，两个节点可能同时发布同一坐标变换。表面现象包括机器人在 RViz 中跳跃、激光与地图时而重合时而错开、规划路径频繁刷新、控制器报告机器人位姿超时。因此，最稳妥的实验方式是保存地图后结束建图会话，再以“静态地图 + AMCL”的方式启动一个干净的导航会话。
9.2 启动导航模式下的仿真世界
首先启动与建图时一致的室内世界，并启用导航模式：
ros2 launch robot_gazebo room_worlds.launch.py nav:=true
这里有两个关键点。第一，导航使用的 Gazebo 世界必须与保存地图时的几何布局一致；如果墙体、家具或出生点发生大幅变化，静态地图就不再对应真实仿真环境。第二，nav:=true 由启动系统切换到适合导航的运行组合，具体行为以当前仿真包配置为准，不应额外启动一套重复的底盘、TF 或传感器节点。
启动后再次做最小检查：
ros2 topic hz /clock
ros2 topic hz /scan
ros2 topic hz /odom
三个数据源都稳定后，再加载地图和 Nav2。这样一旦导航启动失败，可以明确问题发生在算法层，而不是 Gazebo 尚未准备完成。
9.3 加载地图并启动 Nav2
在新的终端执行：
ros2 launch robot_gazebo navigation.launch.py map:=map_01
map 参数与建图保存时使用的地图名称保持一致。正常启动后，RViz 应显示静态地图、机器人模型、激光扫描、全局和局部代价地图等内容。
如果 RViz 打开但地图区域为空，先确认地图服务器是否已经激活以及 /map 是否有数据，不要立即怀疑 RViz 配置：
ros2 topic info /map -v
ros2 topic echo /map_metadata --once
地图元数据中的宽、高、分辨率和原点都应为有效值。若 /map 没有发布者，应检查地图名称是否正确、地图文件是否成对存在以及地图服务器生命周期状态。
9.4 Nav2 各模块的职责
Nav2 不是一个单独的“导航节点”，而是一组通过生命周期和行为树协作的服务器。理解模块职责后，故障定位会清晰很多。
| 模块 | 主要职责 | 典型异常表现 |
|---|---|---|
| Map Server | 加载并发布静态地图 | RViz 无地图、AMCL 无法工作 |
| AMCL | 根据激光与地图估计机器人位姿 | 粒子不收敛、机器人在地图中漂移 |
| Planner Server | 计算从当前位置到目标点的全局路径 | 目标发送后没有全局路径 |
| Controller Server | 跟踪路径并输出速度 | 有路径但机器人不动或持续振荡 |
| Behavior Server | 执行旋转、后退、等待等恢复动作 | 受阻后无法脱困或恢复反复触发 |
| BT Navigator | 按行为树组织规划、控制和恢复 | 导航动作提前失败或任务状态异常 |
| Waypoint Follower | 管理多目标点队列 | 单点正常、多点任务不连续 |
| Costmap | 融合静态地图与实时障碍物 | 路径穿墙、空地被判定为障碍 |
Nav2 节点多采用 ROS 2 生命周期机制。节点进程存在并不代表已经提供服务，它可能处于 unconfigured、inactive 或 active 状态。导航不能工作时，可检查关键节点：
ros2 lifecycle nodes
ros2 lifecycle get /map_server
ros2 lifecycle get /amcl
ros2 lifecycle get /planner_server
ros2 lifecycle get /controller_server
ros2 lifecycle get /bt_navigator
正常导航前，这些核心节点应处于 active。如果某个节点停留在 inactive，向上查看启动日志，通常能找到地图读取、插件加载、参数类型或 TF 等直接原因。
9.5 导航前的六项门禁
建议在第一次点击目标工具前完成以下检查：
- 静态地图已经显示，方向和比例与建图结果一致。
- Gazebo 机器人出生位置与静态地图中的可通行区域对应。
- 激光扫描能够转换到
map坐标系，扫描端点与墙体大体重合。 - AMCL、规划器、控制器和行为树导航器都处于活动状态。
- 全局代价地图与局部代价地图能显示机器人 footprint 和障碍物膨胀区。
- 速度输出话题与 Gazebo 底盘实际订阅的话题相同。
如果任何一项未通过，先修复再发送目标。Nav2 的行为树可能在失败后自动重试和恢复，日志会迅速变多，反而掩盖最初的根因。
十、AMCL 初始定位与定位质量判断
10.1 为什么必须设置初始位姿
静态地图只描述环境，不知道机器人当前位于哪里。AMCL 是基于粒子滤波的二维定位方法，需要一个初始位置和大致朝向，再利用激光观测不断调整粒子权重。即使 Gazebo 中的机器人出生点与建图起点完全一致，也建议显式设置一次初始位姿，以建立可重复的实验流程。
RViz 工具栏中常见的导航工具如下。
选择 2D Pose Estimate，在地图中机器人实际所在位置按住鼠标左键并沿机器人朝向拖动，松开后发布初始位姿。起点表示位置，箭头表示朝向。位置误差越小、方向越接近真实朝向，AMCL 收敛越快。
10.2 如何判断初始定位是否成功
设置完成后观察以下现象：
- 机器人模型出现在正确房间和通道内。
- 激光点与地图墙体基本重合。
- 粒子云从较分散状态逐渐聚集到机器人周围。
map -> odom变换稳定，不再发生大幅跳跃。- 轻微移动机器人后，模型与激光能连续跟随。
还可查看定位消息：
ros2 topic echo /amcl_pose --once
ros2 topic hz /amcl_pose
/amcl_pose 除位姿外还包含协方差。协方差反映估计的不确定性，不宜只凭粒子点“看起来聚在一起”判断定位质量。刚初始化时不确定性较大是正常的；机器人进行少量平移和旋转、观察到具有区分度的墙角后，位置和偏航相关协方差应趋于稳定。
10.3 对称环境为何容易定位错误
如果房间或走廊高度对称，多个位置可能产生相似的激光轮廓。此时 AMCL 可能在错误位置形成一个看似集中的粒子簇。判断方法不是只看粒子集中程度，而是看激光点是否与当前地图边界一致，并让机器人小范围移动，观察估计轨迹是否符合环境结构。
改善方式包括：
- 将初始位姿设置得更接近真实位置和朝向。
- 在包含转角、凹槽或不对称障碍物的位置初始化。
- 先缓慢旋转，再做短距离平移，增加观测差异。
- 检查雷达最大有效范围和 AMCL 激光模型配置。
- 避免把粒子数量设置得过低，以免假设过早坍缩。
10.4 初始位姿常见误操作
最常见的错误是只点击没有拖动，导致位置正确但朝向采用默认值；其次是把箭头方向理解为“机器人要去的方向”，实际上它表示机器人当前朝向。若朝向相差 180°，激光在局部对称环境中仍可能短暂匹配，但机器人一运动就会出现定位跳变。
设置错误时可以重新使用 2D Pose Estimate，不必重启整个导航系统。重新设置后等待粒子收敛，再发送目标。
十一、Gazebo 单目标导航实验
11.1 发送目标位姿
定位稳定后，选择 RViz 中的 2D Nav Goal 或当前配置中等价的目标位姿工具。在地图白色可通行区域按住鼠标左键并拖动：按下位置是目标坐标，拖动方向是机器人到达后的期望朝向。
目标点应满足：
- 位于静态地图的自由空间，而不是灰色未知区。
- 与墙体保持足够距离，能够容纳机器人 footprint 和膨胀半径。
- 不落在桌腿、窄缝或局部代价地图中的实时障碍物上。
- 第一轮测试距离适中，路径中至少包含一个转弯，但不要跨越整个地图。
目标发布后，行为树导航器接收任务，全局规划器计算路径，控制器按局部代价地图生成速度。完整过程可概括为：
11.2 观察全局路径、局部路径和代价地图
发送目标后，RViz 中应首先出现从机器人当前位置连接到目标点的全局路径。随后机器人开始运动，局部规划结果会随着机器人位置和附近障碍物持续更新。
可以把画面分成四层理解：
- 静态地图层。 提供墙体和固定障碍物的基础占据信息。
- 膨胀层。 在障碍物周围形成渐变代价值，让机器人保留安全距离。
- 全局路径。 从起点到目标的整体路线，通常不会每个控制周期都变化。
- 局部轨迹。 控制器根据当前速度、机器人模型和局部障碍物选出的短时运动方案。
如果全局路径紧贴墙角，不一定是规划器“错误”，可能是 footprint 或膨胀半径设置过小；如果路径在宽阔区域绕行很远，可能是代价地图残留了错误障碍或地图中存在未清理的占据像素。
11.3 单点导航过程演示
以下三张图对应机器人从起点出发、沿路径行驶和接近目标点的连续状态。
从结果链可以确认：静态地图已成功加载；AMCL 提供了连续的全局位姿；全局规划器生成了可行路线；控制器持续输出速度；Gazebo 模型在物理场景中与 RViz 位姿同步移动；机器人最终进入目标位置与朝向容差。
11.4 如何确认“真正到达”而不是“看起来靠近”
导航成功由行为树和控制器的目标检查器判定，通常同时考虑位置误差和偏航误差。建议从三个层面确认：
- 界面层： 机器人模型进入目标箭头附近，最终朝向与箭头基本一致，路径不再继续延伸。
- 动作层： 导航动作返回成功，行为树不再处于运行状态。
- 运动层： 速度指令回到零，机器人保持静止，不再执行恢复旋转。
可以监听导航动作状态与速度：
ros2 topic echo /cmd_vel
ros2 action list
ros2 action info /navigate_to_pose
如果机器人停在目标附近但任务迟迟不结束，通常是朝向容差过严、定位噪声较大，或控制器在低速区无法稳定满足目标检查器。不要简单把所有容差调得很大；先观察最终位置误差和角度误差，再有依据地调整。
11.5 取消或替换目标
导航过程中再次发送新目标，系统通常会抢占当前任务并规划到新目标。测试时可利用这一机制验证重新规划，但首轮验收不建议频繁改变目标，因为难以区分正常抢占和异常中断。
发生路径错误或机器人即将进入异常状态时，优先在 RViz 中取消任务，或终止导航动作，再检查问题。直接关闭 Gazebo 会让所有上层节点同时失去时钟、TF 和传感器，日志中会出现大量派生错误，不利于定位原始问题。
十二、Gazebo 多目标点导航实验
12.1 多点导航与连续发送单点的区别
连续手工发送单点依赖操作者在每次到达后再发布目标；多点导航则先建立航点队列，由航点跟随模块按顺序提交导航任务。它更适合验证室内巡检、定点采集、仓储搬运和教学演示等流程。
多点任务至少包含三类状态：
- 尚未执行的航点。
- 当前正在导航的航点。
- 已完成或失败的航点。
具体策略可以是某一点失败后停止全部任务，也可以记录失败并继续下一个点。本文所用界面通过 RViz 面板添加目标点并启动队列，执行策略由当前导航配置决定。
12.2 设置航点
打开 RViz 左下角的多点导航面板，进入航点设置状态。
选择发布点工具后，在地图中依次设置多个目标位姿。每个点都应拖动指定朝向，而不是只给出位置。推荐第一轮设置 3 至 5 个点，并遵循以下原则：
- 点位分布在不同房间或通道，能够覆盖直行、转弯和回程。
- 相邻点之间有明确可行路径，不把点放在门框、墙边和膨胀区。
- 至少一个点要求明显的终点朝向调整，以验证角度控制。
- 队列最后一个点可放回起点附近，观察长路径后定位是否仍稳定。
航点数量不是越多越好。过密航点会使机器人频繁减速、停止和重新规划，降低巡航效率；过疏则无法表达必须经过的关键位置。航点应描述任务约束，而不是手工勾勒每一段路径。
12.3 启动航点任务
确认点位顺序后点击开始导航。机器人将按照队列逐点执行，界面可显示任务进度。执行效果如下。
观察重点包括：
- 当前点完成后是否自动切换到下一点。
- 每次切换时是否生成新的全局路径。
- 机器人是否在狭窄通道发生长时间振荡。
- 行驶较长距离后激光与地图是否仍然重合。
- 回到曾经经过的区域时 AMCL 是否保持稳定。
- 最后一个点完成后速度是否归零、任务是否正常结束。
12.4 多点任务的工程化扩展
在实际应用中，航点不仅是位置，还可以绑定“到点动作”，例如等待、采集传感器、拍照、播报、机械臂操作或调用业务服务。推荐把导航与业务动作通过行为树或任务状态机解耦：导航模块只负责可靠到达，业务模块处理到点后的动作和失败策略。
一个典型巡检逻辑如下：
仿真阶段可先验证导航队列，再逐步增加业务动作。这样当流程失败时，能够区分“没有到达”与“到达后动作失败”。
十三、决定导航效果的关键参数
13.1 Footprint：机器人到底有多大
Nav2 使用 footprint 判断机器人是否能穿过某个区域。footprint 可以用多边形描述真实外轮廓，也可以用 robot_radius 近似圆形。对于外形接近矩形、带轮子和突出部件的 ROSOrin Pro，多边形通常比圆形更能反映转弯占用空间。
设置时应覆盖底盘最外侧轮廓，并保留少量安全余量。若 footprint 过小，规划路径可能允许车体擦墙甚至穿过实际无法通过的缝隙；若过大，原本可通行的门口会被判定为封闭。仿真模型的碰撞体、Nav2 footprint 和实机外形三者应尽量一致。
13.2 膨胀半径与代价衰减
膨胀层把障碍物周围区域赋予更高代价。inflation_radius 决定影响范围，代价衰减参数决定离开障碍物后代价下降速度。
- 膨胀范围太小：全局路径贴墙，转角容易擦碰。
- 膨胀范围太大：窄通道在代价地图中闭合，无路可走。
- 衰减过快：路径仍可能紧靠障碍物。
- 衰减过慢：机器人偏向通道中心，但可能产生较长绕行。
调参时先保证 footprint 正确，再调膨胀层。如果机器人几何本身就错了，只靠膨胀参数无法获得一致结果。
13.3 全局代价地图与局部代价地图
全局代价地图覆盖较大区域，为长距离规划提供静态地图和障碍信息；局部代价地图通常围绕机器人滚动，用于实时避障和短时控制。常见配置差异如下：
| 项目 | 全局代价地图 | 局部代价地图 |
|---|---|---|
| 范围 | 整张地图或大范围 | 机器人附近窗口 |
| 主要用途 | 生成整体路径 | 跟踪路径、实时避障 |
| 静态地图层 | 通常启用 | 可按需要启用 |
| 障碍物层 | 可启用 | 通常必须启用 |
| 更新频率 | 相对较低 | 相对较高 |
| 坐标系 | 常用 map |
常用 odom |
如果 RViz 中全局路径正常，但机器人遇到并不存在的障碍后停住，重点检查局部代价地图；如果目标明显可达却完全规划不出路径，重点检查全局代价地图、静态地图和 footprint。
13.4 速度与加速度约束
仿真中把最大速度调得很高并不能证明导航性能更强。速度越高，控制器留给制动、转向和重新规划的时间越短，虚拟机计算延迟也更容易转化为轨迹误差。建议按以下顺序调节：
- 使用较低最大线速度和角速度完成首次全流程。
- 确认加减速平滑、转角不振荡、目标点能稳定停车。
- 逐步提高速度，每次只修改一类限制。
- 在最长直道、最窄通道和最急转角分别验证。
- 观察实际速度是否因实时因子下降而无法达到指令值。
对于麦克纳姆底盘，还需明确导航配置是否允许横向速度。如果控制器配置为全向模型，而 Gazebo 插件或实机底盘只响应前进和旋转，预测轨迹与真实运动会不一致。反之，如果底盘支持横移但导航按非全向模型运行，系统仍可工作，只是不会利用横向自由度。首次实验优先采用与现有仿真配置一致的运动模型。
13.5 AMCL 参数关注点
AMCL 调参应围绕运动模型、激光模型和粒子管理进行：
- 运动模型必须与底盘类型和里程计特征匹配。
- 激光最大使用距离不应超过有效仿真扫描范围。
- 更新位移和角度阈值过大，会导致机器人移动较远才更新定位。
- 粒子数量太少，复杂或对称环境中容易丢失多峰假设；太多则增加计算负担。
- 初始协方差过大，收敛时间长；过小且初始位姿错误，可能难以恢复。
- 激光似然场参数影响扫描与地图边界的匹配宽容度。
仿真里程计通常比实机理想，因此在仿真中工作良好的 AMCL 参数不能原封不动地视为实机最优值。迁移时应加入实机轮滑、地面不平和雷达噪声的影响重新验证。
13.6 目标容差与停止行为
目标检查器通常有 XY 位置容差和偏航容差。过严会导致机器人在目标附近反复微调，过松则可能在视觉上明显偏离目标就宣布成功。合理容差应结合机器人尺寸、定位噪声和业务需求确定。
调试方法是记录多次导航停止时的实际位置与目标位置差，而不是凭一次体验修改。若位置误差稳定很小但偏航始终达不到，可单独调整角度控制、最小角速度或偏航容差；若机器人在低速下无法克服物理摩擦，则需要同步检查控制器最小速度和 Gazebo 接触参数。
十四、实验结果与验收方法
14.1 本次实验已经完成的功能性结果
结合前文界面与连续过程图，本次仿真实验完成了以下功能性验证：
- ROSOrin Pro 模型能够在 Gazebo 中稳定生成，场景和机器人几何关系正常。
- 室内世界能够作为建图与导航的统一测试场，机器人可以接受键盘控制进行探索。
- SLAM 运行期间，RViz 能同步显示机器人、激光扫描和逐步扩展的二维占据栅格。
- 经过外边界、内部通道和局部盲区补扫后，地图形成完整的室内轮廓。
- 地图保存器能够将当前地图持久化，供新的导航会话加载。
- Nav2 启动后能够恢复静态地图，并在 RViz 中显示导航相关图层。
- 设置单个目标位姿后，系统能够生成路径，驱动机器人从起点移动到目标区域并完成朝向调整。
- 设置多个航点后，系统能够进入航点任务模式并依次执行导航。
这些结果证明了“模型、传感器、TF、SLAM、地图、定位、规划、控制”链路在当前仿真配置下具备完整性。不过，功能跑通不等于性能已经达到最终应用要求。若文章用于科研复现、课程考核或项目验收，还应采集定量指标。
14.2 建图结果如何量化
二维地图可以从覆盖度、一致性、清晰度和闭环稳定性四个方面评价。
覆盖度关注需要使用的自由空间是否已经被观测。可将 Gazebo 场景中理论可通行区域作为参考，统计地图中已知自由区域占参考区域的比例。若没有自动化真值工具，也可按房间、走廊、转角和支路建立人工检查表。
墙体一致性关注同一物理墙面在地图中是否形成单一连续边界。明显的平行双墙通常意味着轨迹错位；墙线随行驶方向变成波浪状，通常与转弯过快、里程计误差或扫描时间匹配有关。
地图清晰度关注边缘厚度和孤立噪点。分辨率固定时，理想静态墙面不应扩散成大面积模糊带。少量锯齿属于离散栅格的正常结果，但大范围散点需要检查雷达噪声、反射范围和动态物体。
闭环稳定性关注机器人再次回到起点或已知区域后，地图是否能保持拓扑一致。可记录闭环前后同一墙角的位置差，或者比较轨迹修正前后的地图重合程度。
建议使用以下记录表，真实数值应在实验时采集，不应根据截图臆测：
| 指标 | 记录方法 | 建议判据 |
|---|---|---|
| 场景覆盖率 | 已观测有效区域/目标区域 | 主要作业区域基本无大片未知区 |
| 边界连续性 | 检查外墙、内墙和门口 | 不出现影响规划的断墙 |
| 双墙数量 | 统计明显重复轮廓 | 主通道和闭环处不应出现明显双墙 |
| 闭环位姿变化 | 对比回到起点前后估计 | 修正后地图整体保持一致 |
| 雷达地图重合 | RViz 叠加观察 | 静止和低速运动时扫描贴合墙面 |
| 地图可加载性 | 新会话载入并显示 | 图像、元数据、原点和分辨率均正确 |
14.3 导航结果如何量化
导航性能至少应统计成功率、耗时、路径效率、终点误差和安全性。
导航成功率可定义为：
成功率 = 成功到达且动作正常结束的任务数 / 总任务数 × 100%
路径效率可以用实际行驶长度与规划参考长度的比值衡量。比值越接近 1，说明绕行和振荡越少；但在需要避障或平滑转弯时，实际轨迹略长是正常的。
终点误差分为位置误差与角度误差：
位置误差 = sqrt((x_actual - x_goal)^2 + (y_actual - y_goal)^2)
角度误差 = 归一化后的 |yaw_actual - yaw_goal|
在 Gazebo 中可以把仿真真实位姿作为参考，与 AMCL 估计和目标位姿分别比较。这样能区分两类问题：如果 AMCL 显示已经到达，但 Gazebo 真值偏差较大，问题主要在定位；如果估计和真值一致但都没有进入目标容差，问题主要在规划控制或目标检查。
建议至少设计以下测试集：
- 近距离直线目标 3 次，验证基础速度闭环。
- 包含 90° 转弯的目标 3 次，验证路径跟踪和转向。
- 跨房间长距离目标 3 次，验证定位长期稳定性。
- 窄通道目标 3 次，验证 footprint 与膨胀层。
- 不同终点朝向目标 3 次，验证最终角度控制。
- 3 至 5 点航点队列重复 3 轮，验证任务连续性。
14.4 推荐的实验记录字段
每次任务建议记录：场景版本、地图版本、参数版本、起点、目标点、规划是否成功、任务是否成功、规划长度、实际长度、耗时、终点位置误差、终点角度误差、恢复行为次数、最小障碍距离、失败原因和日志编号。
记录失败同样重要。只保留成功视频会让系统看起来稳定，却无法支持参数改进。失败原因应使用结构化分类，例如“初始定位错误”“全局路径不可达”“局部振荡”“传感器超时”“行为树取消”，不要统一写成“导航失败”。
十五、全链路故障排查：从现象回到根因
15.1 统一排障顺序
机器人软件栈层级较多，排查时应保持固定顺序：
核心原则是：先证明输入存在且正确，再讨论算法参数；先找到第一个异常层，不被后续连锁报错干扰。
15.2 Gazebo 无法打开、黑屏或异常退出
可能原因： 虚拟机 3D 加速与显卡驱动不兼容、OpenGL 渲染失败、内存不足、模型资源加载卡住、已有 Gazebo 进程占用服务。
排查方法：
- 先关闭 3D 加速并重启虚拟机。
- 检查终端中第一条渲染或插件错误，而不是只看进程退出提示。
- 确认系统有足够可用内存和磁盘空间。
- 查看是否存在未退出的 Gazebo 服务进程，避免多个实例争用。
- 用空世界验证 Gazebo 本体，再逐步加入室内世界和机器人模型。
若空世界也无法显示，问题在 Gazebo/图形环境；若空世界正常、加载机器人后失败，问题更可能在模型网格、插件或世界资源。
15.3 模型弹飞、侧翻、抖动或轮子空转
可能原因： 惯性参数不合理、碰撞体互相重叠、出生高度错误、轮子关节轴向错误、接触摩擦异常、控制增益过大。
排查方法：
- 暂停仿真，检查碰撞几何是否相交。
- 单独生成机器人，不加载复杂环境，排除场景碰撞。
- 逐步禁用附加部件，定位引发约束冲突的连杆。
- 检查每个动态连杆质量和惯性矩阵。
- 低速发送短脉冲指令，观察每个轮子的转动方向。
- 对比 Gazebo 中实际速度与里程计速度，判断是否存在严重轮滑。
轮子在视觉上旋转但车体不动，通常是接触、摩擦或关节传动问题；车体运动但里程计不变，则是里程计发布接口问题。
15.4 有雷达话题但 RViz 看不到扫描
可能原因： RViz 固定坐标系不正确、雷达 frame_id 不在 TF 树中、扫描范围全为无效值、消息时间与 TF 时间不一致、显示项的话题选错。
排查方法：
- 查看一帧
LaserScan，确认frame_id和ranges。 - 查询固定坐标系到雷达坐标系的 TF。
- 建图前先把 RViz 固定坐标设为
odom，缩短转换链验证。 - 检查
use_sim_time和/clock。 - 查看 RViz 显示项状态栏的具体错误。
若扫描在雷达坐标系下能显示，但在 odom 或 map 下不能显示，传感器插件本身正常，问题位于 TF 链。
15.5 TF 报过去或未来外推错误
可能原因： 一部分节点使用系统时间，另一部分使用仿真时间；仿真曾暂停；CPU 负载导致消息延迟；静态变换被错误地当成动态变换发布；多个发布者时间基准不一致。
处理顺序：
- 确认
/clock正在推进。 - 确认所有算法节点启用仿真时间。
- 检查消息时间戳是否接近当前仿真时间。
- 查找同一 TF 的重复发布者。
- 降低雷达频率或场景复杂度，判断是否由性能不足引起。
不要第一时间把 TF 容忍时间无限增大。较大的容忍度只能掩盖有限延迟，无法修复时钟源混用。
15.6 SLAM 启动后没有地图
可能原因： 没有激光输入、缺少里程计到雷达的变换、SLAM 订阅话题与实际话题不一致、节点未激活、机器人没有达到更新阈值、仿真暂停。
排查方法：
- 确认
/scan与/odom有稳定频率。 - 确认
odom -> base -> laser可查询。 - 查看
/map是否有发布者。 - 缓慢移动和转动机器人，触发位姿更新。
- 检查 SLAM 日志中的消息过滤器丢弃原因。
- 确认雷达扫描平面与地面平行且能命中墙体。
如果地图只有起点周围一小块，机器人运动后仍不扩展，应重点看里程计是否变化和扫描时间是否能转换。
15.7 地图重影、双墙或整体扭曲
可能原因： 运动速度过快、里程计方向或尺度错误、TF 延迟、雷达外参错误、场景特征过少、闭环错误、仿真实时因子过低。
改进顺序：
- 降低线速度和角速度，特别是转角速度。
- 对比机器人实际位移与里程计位移，修正尺度和方向。
- 检查雷达相对底盘的安装位姿。
- 确保扫描频率和 TF 更新频率稳定。
- 采用外圈到内圈、最后回到起点的轨迹。
- 在结构单一的长走廊中保持平稳，不做高速原地旋转。
- 若已经发生全局错层，重新开始一张干净地图。
参数优化应建立在运动学正确基础上。错误的轮径或轮距会让每次转弯都产生系统性里程计误差，SLAM 只能有限修正。
15.8 地图保存超时或导航无法加载
保存超时通常意味着地图保存器没有收到 /map，应检查地图话题、QoS 与 SLAM 是否仍在运行。导航加载失败则需检查图像与元数据是否同时存在、名称是否匹配、元数据格式是否有效、进程是否有读取权限。
地图能显示但方向或位置错误时，检查 origin；地图显示为反色或障碍判断异常时，检查 negate、占据阈值和自由阈值。不要在图像编辑器中随意缩放地图图像，因为像素尺寸变化后，原分辨率和原点不再对应。
15.9 AMCL 粒子不收敛或定位跳变
可能原因： 初始位姿差距太大、箭头朝向设置错误、地图与世界不一致、雷达地图匹配差、运动模型不适配底盘、里程计突跳、粒子数量不足。
排查方法：
- 在正确位置重新设置初始位姿，并明确拖出朝向。
- 静止观察激光与墙体是否重合。
- 小范围旋转和平移，增加观测信息。
- 对比 Gazebo 真值、里程计和 AMCL 三条轨迹。
- 检查是否还有 SLAM 节点发布
map -> odom。 - 检查静态地图与当前世界是否为同一版本。
如果定位每次在同一个转角跳变，通常是地图局部质量、环境对称或外参误差，而不是随机故障。
15.10 能定位但规划不出路径
先看起点和目标是否在全局代价地图的自由区域。常见原因包括目标点落在墙内、机器人 footprint 与障碍重叠、膨胀层封闭窄通道、未知区不允许通行、全局代价地图没有正确接收地图或机器人位姿。
排查时在 RViz 同时显示 footprint、全局代价地图和全局路径。若机器人起点周围已是致命障碍，规划器自然无法出发；若起点正常而目标周围为致命障碍，应重新设置目标或修正地图；若整条通道被膨胀层封闭，再评估机器人几何和膨胀参数。
15.11 有全局路径但机器人不动
沿数据链逐级检查：
- 控制器是否处于活动状态。
/cmd_vel是否有非零数据。- Gazebo 底盘订阅的话题是否与控制器输出一致。
- 速度限制器是否把指令全部裁剪为零。
- Gazebo 是否暂停。
- 机器人是否因碰撞卡住。
如果 /cmd_vel 有值但底盘不动，问题在控制输出之后；如果没有速度，查看控制器日志和局部代价地图。机器人起点被判定为碰撞、路径转换失败或局部规划无有效轨迹时，控制器不会盲目输出速度。
15.12 机器人贴墙、振荡或在目标附近绕圈
贴墙通常与 footprint、膨胀层或路径平滑有关；通道内左右振荡通常与速度过高、控制前视距离、角速度增益和运动模型有关；目标附近绕圈则多与最小速度、角度容差和定位噪声有关。
一次只改变一组参数，并使用同一起点和目标重复对比。建议按“几何参数 → 代价地图 → 速度限制 → 控制器权重 → 目标容差”的顺序调整。若同时修改十几个参数，即使结果变好也无法知道真正起作用的是哪一项。
15.13 多点导航中途停止
先确定是当前单点任务失败，还是航点管理器没有继续提交下一个点。查看当前目标是否不可达、恢复行为是否耗尽、失败策略是否配置为终止全队列。若每次都在同一个航点失败，应检查该点位置和朝向；若失败点随机，检查定位稳定性、计算负载和传感器超时。
十六、性能优化与稳定运行
16.1 关注实时因子而不是只看画面流畅度
Gazebo 的实时因子是仿真时间推进速度与真实时间的比值。接近 1 表示一秒现实时间约推进一秒仿真时间；明显低于 1 表示仿真在慢放。画面帧率低不一定影响物理计算，但实时因子长期过低会降低传感器和控制在墙上时间中的有效频率。
性能优化可依次考虑：
- 降低不必要的相机分辨率与帧率。
- 关闭本实验未使用的深度相机或高负载传感器。
- 简化视觉网格与碰撞几何。
- 降低雷达过高的采样点数和更新率。
- 减少 RViz 中重复或高负载显示项。
- 给虚拟机分配合理 CPU 和内存，但保留宿主系统资源。
- 避免同时运行无关编译、录屏和大型应用。
16.2 建议录制 rosbag 复盘
对于偶发定位跳变、规划失败或传感器超时，现场观察往往不够。可录制关键话题用于离线复盘。建议至少包含激光、里程计、TF、静态 TF、地图、AMCL 位姿、规划路径、速度指令和导航状态。
录制前应控制范围，避免把高带宽图像全部加入，导致磁盘写入反过来影响仿真。复盘时可以比较故障前后的消息频率、时间戳、TF 连续性和代价地图变化，从而判断是数据中断还是算法决策。
16.3 建立可重复的基准场景
为了公平对比参数，应固定：
- Gazebo 世界和障碍物布局。
- 机器人出生位置与朝向。
- 地图版本。
- 起点、目标点和航点顺序。
- 虚拟机资源配置。
- 最大速度与传感器频率。
- 测试时是否有动态障碍。
每次只修改一个主要变量，并保留参数版本与结果记录。这样才能把“这次感觉更顺”转化为可验证的工程结论。
16.4 启动与关闭顺序
推荐启动顺序为：Gazebo 世界与机器人 → 检查时钟和传感器 → SLAM 或 Nav2 → RViz 观察 → 遥控或目标任务。推荐关闭顺序为：停止任务和速度 → 停止上层算法 → 停止 Gazebo。
顺序的意义在于让上层节点启动时已有数据源，让系统退出时机器人先停止。尤其在实机上，任何退出流程都应先显式发布停止或由底盘超时保护清零速度。
十七、常见问题简答
17.1 Gazebo 中地图和 RViz 中地图是什么关系？
Gazebo 显示的是仿真世界的几何与物理状态，RViz 中的二维地图是 SLAM 根据激光和运动估计生成的占据栅格。前者相当于实验环境，后者相当于机器人对环境的认知结果。两者应在结构上对应，但不会像截图一样像素完全一致。
17.2 为什么激光点能看到，SLAM 仍然不建图？
SLAM 不只需要激光，还需要在消息时间戳处获得里程计到底盘、底盘到雷达的 TF。RViz 在某些固定坐标下能显示一帧扫描，不代表 SLAM 的完整时间同步条件一定满足。应同时检查 TF、时间和订阅话题。
17.3 建图时能否高速行驶？
可以让机器人运动，但不建议高速。高速平移与急转会放大扫描畸变、里程计误差和计算延迟。先以低速得到结构正确的地图，再评估提高速度后的影响。
17.4 导航是否一定要设置初始位姿？
只要系统没有从其他可靠来源获得全局初始位姿，就应设置。即使仿真出生点固定，显式初始化也能消除启动顺序和默认值带来的不确定性，并形成可复现流程。
17.5 单点导航成功，多点导航为什么仍可能失败？
多点任务持续时间更长、行驶范围更大，还涉及航点队列和失败策略，因此会暴露长期定位漂移、某个航点不可达、任务切换或恢复次数耗尽等问题。应先确认失败发生在某个单点导航，还是发生在航点管理层。
17.6 仿真导航成功是否意味着实机一定成功？
不意味着。仿真证明算法接口和基本逻辑成立，实机还需要处理真实运动学误差、轮滑、传感器噪声、网络和安全约束。仿真的最大价值是提前排除软件架构错误，并提供一套可验证的参数起点。