1. 医院多楼层配送仿真到底在做什么先把场景说清楚。医院配送和普通仓储AGV最大的区别在于楼层多、电梯是必经节点、地图要频繁切换、配送任务有优先级急救药品和餐食的时效要求完全不同。这套仿真要解决的核心问题就是——让一台双足walking机器人在Gazebo里跑通“取货→跨楼层→送达”的完整链路并且每进入一个新楼层导航系统能自动加载对应的地图不需要人工干预。我选择用ROS2 Jazzy来做这套仿真原因很直接Jazzy是当前的LTS版本Nav2的成熟度比Foxy时期好了不止一个档次尤其是行为树Behavior Tree的插件化配置和多地图加载的灵活性。如果你还在用Foxy或者Humble大部分逻辑是通用的但Jazzy在生命周期节点管理上更省心。整套仿真涉及几个核心模块walking机器人的URDF建模与步态控制、多楼层地图的构建与存储、楼层间的电梯过渡逻辑、Nav2导航栈的配置、以及最关键的实时地图切换机制。适合有ROS2基础但没做过多楼层场景的开发者也适合做医院物流系统验证的工程人员参考。注意本文所有操作基于Ubuntu 22.04 ROS2 Jazzy Gazebo Harmonic。如果你用的是Gazebo Classic部分插件接口需要调整我会在对应位置标注。2. 整体方案设计与核心思路拆解2.1 为什么不用轮式底盘而选walking机器人医院环境里轮式AGV的痛点很明显门槛、地毯边缘、电梯轿厢与楼层地面的缝隙这些都会导致轮式底盘卡住或者打滑。walking机器人双足或四足在越障能力上有天然优势代价是控制复杂度高得多。在仿真里我用的是简化版双足模型12个自由度每条腿6个步态采用基于ZMP的预观控制。你可能会问仿真里步态控制有必要做这么细吗我的经验是如果你的配送场景只关心导航层面可以用简化模型但如果要验证机器人在电梯轿厢内的稳定性电梯启停有加速度步态就不能太粗糙。2.2 多楼层地图的管理策略这是整套方案的核心设计决策。常见做法有三种方案思路优点缺点单一大图把所有楼层拼成一张3D地图全局一致性好内存占用大导航效率低多张2D图楼层索引每层一张2D栅格图用楼层ID索引轻量、切换快需要手动管理切换逻辑分层3D地图用OctoMap做3D表示天然支持多层计算量大实时性差我选的是第二种每层一张2D栅格图配合一个楼层状态管理器。理由很实际——医院每层布局差异大拼成一张图会导致Nav2的代价地图膨胀得厉害规划效率直线下降。而每层独立建图单层地图可以控制在2000×2000栅格以内Nav2跑起来很流畅。2.3 电梯过渡的逻辑设计电梯是连接楼层的唯一通道。仿真里我用了一个“虚拟电梯”方案在每层地图的电梯位置设置一个特殊区域机器人到达该区域后触发楼层切换事件。具体流程是机器人导航到当前楼层的电梯等待点发布电梯呼叫信号仿真里用ROS2 topic模拟电梯到达后机器人进入轿厢导航到轿厢内的目标点触发楼层切换保存当前状态加载目标楼层地图重新初始化定位机器人从目标楼层的电梯出口点开始导航到最终目的地这个逻辑听起来简单但实际调试时最容易出问题的就是第4步——地图切换后AMCL的重新定位。如果处理不好机器人会“以为”自己还在旧楼层导致导航目标完全错乱。3. 核心细节解析与实操要点3.1 walking机器人URDF建模的关键参数先看腿部结构。我用的是串联12自由度构型每条腿的关节分配是髋关节3个roll/pitch/yaw、膝关节1个pitch、踝关节2个pitch/roll。这个配置在仿真里足够表达基本步态又不会让计算量爆炸。URDF里几个容易踩坑的地方碰撞体简化视觉模型可以用精细mesh但碰撞体一定要用简单几何体圆柱球体组合。我试过直接用mesh做碰撞体Gazebo的物理引擎直接卡死。关节阻尼膝关节的damping值建议设在0.5~1.0之间。太小了腿会抖太大了步态僵硬。足底摩擦系数mu1和mu2都设成1.0以上否则在电梯轿厢里容易打滑。!-- 膝关节配置示例 -- joint nameknee_joint typerevolute axis xyz0 1 0/ limit lower-2.0 upper0.0 effort100 velocity5.0/ dynamics damping0.8 friction0.1/ /joint步态控制我用了一个简化的预观控制节点发布关节轨迹到/joint_trajectory_controller/command。步频设在1.2Hz步长0.3m这个参数在仿真里走起来比较稳。3.2 多楼层地图的构建与存储规范建图用SLAM Toolbox每层单独建。关键操作是每建完一层用nav2_map_server的map_saver_cli保存命名规范建议用floor_编号.yaml比如floor_1.yaml、floor_2.yaml。地图分辨率我设的是0.05m/pixel这个精度对医院走廊通常2~3米宽足够用。如果走廊特别窄可以调到0.03但地图文件会大不少。每层地图的元数据里要额外记录两个信息电梯入口坐标和电梯出口坐标。我是在YAML文件里加了一个自定义字段image: floor_1.pgm resolution: 0.05 origin: [-10.0, -10.0, 0.0] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196 elevator_entry: [12.5, 8.3] elevator_exit: [13.0, 9.0]这个自定义字段不会被map_server解析但我的楼层管理节点会读取它用来设置电梯等待点和出口点。3.3 Nav2参数配置的楼层适配Nav2的配置文件需要针对每层地图做微调。主要调整的是代价地图的膨胀半径——医院走廊窄膨胀半径设太大机器人会“无路可走”。我的经验值是全局代价地图膨胀半径0.3m局部代价地图膨胀半径0.25m机器人半径0.2m双足站立时的投影半径如果走廊宽度小于1米膨胀半径要降到0.2以下同时把cost_scaling_factor调大建议5.0以上让代价衰减更快。实操心得每次切换楼层后一定要重新发布/initialpose让AMCL在新地图上重新定位。我一开始偷懒没做这一步结果机器人直接“穿墙”导航到错误位置。4. 实操过程与核心环节实现4.1 环境搭建与依赖安装先装ROS2 Jazzy。如果你已经装好了跳过这步。没装的话用鱼香ROS的一键安装脚本最省事wget http://fishros.com/install -O fishros . fishros选Jazzy版本桌面版。装完之后验证source /opt/ros/jazzy/setup.bash ros2 topic list能正常列出topic就说明基础环境OK。接下来装Gazebo Harmonic和Nav2sudo apt install ros-jazzy-gazebo-ros-pkgs ros-jazzy-nav2-bringup sudo apt install ros-jazzy-slam-toolbox ros-jazzy-robot-localizationwalking机器人的仿真包我是自己写的核心依赖是ros2_control和gazebo_ros2_controlsudo apt install ros-jazzy-ros2-control ros-jazzy-gazebo-ros2-control4.2 启动仿真与建图流程启动顺序很重要我踩过坑先启动Gazebo再启动机器人状态发布最后启动SLAM。# 终端1启动Gazebo世界 ros2 launch hospital_sim hospital_world.launch.py floor:1 # 终端2加载机器人模型 ros2 launch walking_robot spawn_robot.launch.py x:0.0 y:0.0 z:0.5 # 终端3启动SLAM Toolbox ros2 launch slam_toolbox online_async_launch.py建图时用键盘遥控机器人走遍整层ros2 run teleop_twist_keyboard teleop_twist_keyboard走完之后保存地图ros2 run nav2_map_server map_saver_cli -f floor_1这里有个细节保存路径建议统一放在~/hospital_maps/目录下后面楼层管理节点会从这个目录读取。4.3 楼层切换节点的实现这是整套方案的核心代码。我用Python写了一个floor_manager节点订阅/floor_switch_request话题收到请求后执行切换逻辑。import rclpy from rclpy.node import Node from std_msgs.msg import String from nav2_msgs.srv import LoadMap import subprocess class FloorManager(Node): def __init__(self): super().__init__(floor_manager) self.current_floor 1 self.subscription self.create_subscription( String, /floor_switch_request, self.switch_callback, 10) self.map_client self.create_client(LoadMap, /map_server/load_map) def switch_callback(self, msg): target_floor int(msg.data) if target_floor self.current_floor: return self.get_logger().info(fSwitching from floor {self.current_floor} to {target_floor}) # 调用map_server的load_map服务 req LoadMap.Request() req.map_url f/home/user/hospital_maps/floor_{target_floor}.yaml future self.map_client.call_async(req) future.add_done_callback(self.map_loaded_callback) def map_loaded_callback(self, future): response future.result() if response.result 0: self.get_logger().info(Map loaded successfully) self.current_floor target_floor # 重新发布初始位姿 self.publish_initial_pose() else: self.get_logger().error(Map load failed)这个节点启动后配合一个电梯触发节点检测机器人是否到达电梯区域就能实现自动切换。4.4 电梯过渡的完整流程调试电梯过渡我拆成了三个阶段来调试阶段一单层内导航到电梯等待点。这个用Nav2的标准NavigateToPose action就能搞定。目标点从地图YAML里的elevator_entry字段读取。阶段二进入轿厢。轿厢在仿真里是一个封闭的box机器人需要导航到轿厢中心。这里要注意轿厢内的代价地图需要单独配置因为空间小膨胀半径要临时调小。阶段三楼层切换后重新定位。这是最容易出问题的环节。我的做法是切换地图后直接发布一个已知的初始位姿电梯出口点让AMCL从那里开始定位。不要用全局重定位因为医院楼层布局相似全局重定位容易匹配到错误楼层。# 手动发布初始位姿的示例 ros2 topic pub /initialpose geometry_msgs/PoseWithCovarianceStamped header: frame_id: map pose: pose: position: {x: 13.0, y: 9.0, z: 0.0} orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} 注意电梯出口点的坐标一定要在每层地图建图时精确记录。我建议在电梯出口地面贴一个标记物仿真里可以用一个红色方块建图时把标记物中心作为出口点坐标。5. 常见问题与排查技巧实录5.1 地图切换后机器人定位漂移这是最高频的问题。现象是切换楼层后机器人在RViz里的位置和实际Gazebo里的位置对不上导航目标点偏移好几米。排查思路检查/initialpose是否在切换后正确发布。用ros2 topic echo /initialpose确认。检查AMCL的set_initial_pose参数是否设为true。如果为false发布的初始位姿不会生效。检查新旧地图的origin是否一致。如果两层地图的origin差异大AMCL的坐标系会错乱。我的解决方案是在楼层管理节点里加一个延时地图加载完成后等2秒再发布初始位姿给AMCL留出初始化时间。5.2 电梯轿厢内导航失败轿厢空间小Nav2的局部规划器经常报“无法找到有效路径”。原因是膨胀半径太大把整个轿厢都填满了。解决办法为轿厢区域单独配置一个代价地图层用static_layerinflation_layer但膨胀半径设为0.15m。或者更简单粗暴进入轿厢时切换到DWB规划器把min_vel_x设为0.05让机器人慢慢挪进去。5.3 walking机器人步态发散仿真里步态跑着跑着就“劈叉”了关节角度越来越大。这是典型的步态控制参数不匹配。检查清单检查项正常范围异常表现步频1.0~1.5Hz过高导致振荡膝关节阻尼0.5~1.0过低导致抖动足底摩擦1.0过低导致打滑控制周期1~5ms过长导致累积误差我最后是把控制周期从10ms降到2ms步态就稳了。代价是CPU占用上去了但仿真里无所谓。5.4 Gazebo物理引擎卡顿多楼层场景下如果所有楼层模型同时加载Gazebo会非常卡。我的做法是只加载当前楼层的模型其他楼层用include但设置pose到很远的地方比如z1000物理引擎就不会计算它们。include urimodel://floor_2/uri pose0 0 1000 0 0 0/pose /include切换楼层时用Gazebo的set_model_pose服务把目标楼层移回来把旧楼层移走。5.5 常见问题速查表问题现象可能原因快速修复切换后RViz无地图map_server未加载新图检查load_map服务返回码机器人原地转圈AMCL定位丢失重新发布initialpose导航目标不可达代价地图膨胀过大调小inflation_radius步态关节超限控制周期过长降低到2msGazebo帧率低多楼层模型同时加载移走非当前楼层模型6. 后续扩展与个人实操体会这套仿真跑通之后我做了几个扩展效果不错。一个是把配送任务做成了优先级队列急救药品任务插队到最前面Nav2的行为树里加一个RateController节点控制任务切换频率。另一个是加了多机器人支持两台walking机器人共享电梯资源用了一个简单的互斥锁机制防止电梯冲突。我个人在实际操作中的体会是多楼层仿真的难点不在单点技术而在状态管理。地图状态、定位状态、任务状态这三者必须严格同步。我一开始把切换逻辑写得太随意结果调试了两天才发现是地图加载和初始位姿发布的时序问题。后来加了一个状态机来管理整个切换流程代码清晰了问题也少了。如果你要复现这套方案建议先从单层导航跑通再加电梯逻辑最后做多楼层切换。不要一上来就搞全套那样出了问题根本不知道是哪一层的问题。另外地图的命名和存储路径一定要规范我见过有人把地图存得乱七八糟切换时找不到文件排查半天以为是代码bug。
1. 医院多楼层配送仿真到底在做什么先把场景说清楚。医院配送和普通仓储AGV最大的区别在于楼层多、电梯是必经节点、地图要频繁切换、配送任务有优先级急救药品和餐食的时效要求完全不同。这套仿真要解决的核心问题就是——让一台双足walking机器人在Gazebo里跑通“取货→跨楼层→送达”的完整链路并且每进入一个新楼层导航系统能自动加载对应的地图不需要人工干预。我选择用ROS2 Jazzy来做这套仿真原因很直接Jazzy是当前的LTS版本Nav2的成熟度比Foxy时期好了不止一个档次尤其是行为树Behavior Tree的插件化配置和多地图加载的灵活性。如果你还在用Foxy或者Humble大部分逻辑是通用的但Jazzy在生命周期节点管理上更省心。整套仿真涉及几个核心模块walking机器人的URDF建模与步态控制、多楼层地图的构建与存储、楼层间的电梯过渡逻辑、Nav2导航栈的配置、以及最关键的实时地图切换机制。适合有ROS2基础但没做过多楼层场景的开发者也适合做医院物流系统验证的工程人员参考。注意本文所有操作基于Ubuntu 22.04 ROS2 Jazzy Gazebo Harmonic。如果你用的是Gazebo Classic部分插件接口需要调整我会在对应位置标注。2. 整体方案设计与核心思路拆解2.1 为什么不用轮式底盘而选walking机器人医院环境里轮式AGV的痛点很明显门槛、地毯边缘、电梯轿厢与楼层地面的缝隙这些都会导致轮式底盘卡住或者打滑。walking机器人双足或四足在越障能力上有天然优势代价是控制复杂度高得多。在仿真里我用的是简化版双足模型12个自由度每条腿6个步态采用基于ZMP的预观控制。你可能会问仿真里步态控制有必要做这么细吗我的经验是如果你的配送场景只关心导航层面可以用简化模型但如果要验证机器人在电梯轿厢内的稳定性电梯启停有加速度步态就不能太粗糙。2.2 多楼层地图的管理策略这是整套方案的核心设计决策。常见做法有三种方案思路优点缺点单一大图把所有楼层拼成一张3D地图全局一致性好内存占用大导航效率低多张2D图楼层索引每层一张2D栅格图用楼层ID索引轻量、切换快需要手动管理切换逻辑分层3D地图用OctoMap做3D表示天然支持多层计算量大实时性差我选的是第二种每层一张2D栅格图配合一个楼层状态管理器。理由很实际——医院每层布局差异大拼成一张图会导致Nav2的代价地图膨胀得厉害规划效率直线下降。而每层独立建图单层地图可以控制在2000×2000栅格以内Nav2跑起来很流畅。2.3 电梯过渡的逻辑设计电梯是连接楼层的唯一通道。仿真里我用了一个“虚拟电梯”方案在每层地图的电梯位置设置一个特殊区域机器人到达该区域后触发楼层切换事件。具体流程是机器人导航到当前楼层的电梯等待点发布电梯呼叫信号仿真里用ROS2 topic模拟电梯到达后机器人进入轿厢导航到轿厢内的目标点触发楼层切换保存当前状态加载目标楼层地图重新初始化定位机器人从目标楼层的电梯出口点开始导航到最终目的地这个逻辑听起来简单但实际调试时最容易出问题的就是第4步——地图切换后AMCL的重新定位。如果处理不好机器人会“以为”自己还在旧楼层导致导航目标完全错乱。3. 核心细节解析与实操要点3.1 walking机器人URDF建模的关键参数先看腿部结构。我用的是串联12自由度构型每条腿的关节分配是髋关节3个roll/pitch/yaw、膝关节1个pitch、踝关节2个pitch/roll。这个配置在仿真里足够表达基本步态又不会让计算量爆炸。URDF里几个容易踩坑的地方碰撞体简化视觉模型可以用精细mesh但碰撞体一定要用简单几何体圆柱球体组合。我试过直接用mesh做碰撞体Gazebo的物理引擎直接卡死。关节阻尼膝关节的damping值建议设在0.5~1.0之间。太小了腿会抖太大了步态僵硬。足底摩擦系数mu1和mu2都设成1.0以上否则在电梯轿厢里容易打滑。!-- 膝关节配置示例 -- joint nameknee_joint typerevolute axis xyz0 1 0/ limit lower-2.0 upper0.0 effort100 velocity5.0/ dynamics damping0.8 friction0.1/ /joint步态控制我用了一个简化的预观控制节点发布关节轨迹到/joint_trajectory_controller/command。步频设在1.2Hz步长0.3m这个参数在仿真里走起来比较稳。3.2 多楼层地图的构建与存储规范建图用SLAM Toolbox每层单独建。关键操作是每建完一层用nav2_map_server的map_saver_cli保存命名规范建议用floor_编号.yaml比如floor_1.yaml、floor_2.yaml。地图分辨率我设的是0.05m/pixel这个精度对医院走廊通常2~3米宽足够用。如果走廊特别窄可以调到0.03但地图文件会大不少。每层地图的元数据里要额外记录两个信息电梯入口坐标和电梯出口坐标。我是在YAML文件里加了一个自定义字段image: floor_1.pgm resolution: 0.05 origin: [-10.0, -10.0, 0.0] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196 elevator_entry: [12.5, 8.3] elevator_exit: [13.0, 9.0]这个自定义字段不会被map_server解析但我的楼层管理节点会读取它用来设置电梯等待点和出口点。3.3 Nav2参数配置的楼层适配Nav2的配置文件需要针对每层地图做微调。主要调整的是代价地图的膨胀半径——医院走廊窄膨胀半径设太大机器人会“无路可走”。我的经验值是全局代价地图膨胀半径0.3m局部代价地图膨胀半径0.25m机器人半径0.2m双足站立时的投影半径如果走廊宽度小于1米膨胀半径要降到0.2以下同时把cost_scaling_factor调大建议5.0以上让代价衰减更快。实操心得每次切换楼层后一定要重新发布/initialpose让AMCL在新地图上重新定位。我一开始偷懒没做这一步结果机器人直接“穿墙”导航到错误位置。4. 实操过程与核心环节实现4.1 环境搭建与依赖安装先装ROS2 Jazzy。如果你已经装好了跳过这步。没装的话用鱼香ROS的一键安装脚本最省事wget http://fishros.com/install -O fishros . fishros选Jazzy版本桌面版。装完之后验证source /opt/ros/jazzy/setup.bash ros2 topic list能正常列出topic就说明基础环境OK。接下来装Gazebo Harmonic和Nav2sudo apt install ros-jazzy-gazebo-ros-pkgs ros-jazzy-nav2-bringup sudo apt install ros-jazzy-slam-toolbox ros-jazzy-robot-localizationwalking机器人的仿真包我是自己写的核心依赖是ros2_control和gazebo_ros2_controlsudo apt install ros-jazzy-ros2-control ros-jazzy-gazebo-ros2-control4.2 启动仿真与建图流程启动顺序很重要我踩过坑先启动Gazebo再启动机器人状态发布最后启动SLAM。# 终端1启动Gazebo世界 ros2 launch hospital_sim hospital_world.launch.py floor:1 # 终端2加载机器人模型 ros2 launch walking_robot spawn_robot.launch.py x:0.0 y:0.0 z:0.5 # 终端3启动SLAM Toolbox ros2 launch slam_toolbox online_async_launch.py建图时用键盘遥控机器人走遍整层ros2 run teleop_twist_keyboard teleop_twist_keyboard走完之后保存地图ros2 run nav2_map_server map_saver_cli -f floor_1这里有个细节保存路径建议统一放在~/hospital_maps/目录下后面楼层管理节点会从这个目录读取。4.3 楼层切换节点的实现这是整套方案的核心代码。我用Python写了一个floor_manager节点订阅/floor_switch_request话题收到请求后执行切换逻辑。import rclpy from rclpy.node import Node from std_msgs.msg import String from nav2_msgs.srv import LoadMap import subprocess class FloorManager(Node): def __init__(self): super().__init__(floor_manager) self.current_floor 1 self.subscription self.create_subscription( String, /floor_switch_request, self.switch_callback, 10) self.map_client self.create_client(LoadMap, /map_server/load_map) def switch_callback(self, msg): target_floor int(msg.data) if target_floor self.current_floor: return self.get_logger().info(fSwitching from floor {self.current_floor} to {target_floor}) # 调用map_server的load_map服务 req LoadMap.Request() req.map_url f/home/user/hospital_maps/floor_{target_floor}.yaml future self.map_client.call_async(req) future.add_done_callback(self.map_loaded_callback) def map_loaded_callback(self, future): response future.result() if response.result 0: self.get_logger().info(Map loaded successfully) self.current_floor target_floor # 重新发布初始位姿 self.publish_initial_pose() else: self.get_logger().error(Map load failed)这个节点启动后配合一个电梯触发节点检测机器人是否到达电梯区域就能实现自动切换。4.4 电梯过渡的完整流程调试电梯过渡我拆成了三个阶段来调试阶段一单层内导航到电梯等待点。这个用Nav2的标准NavigateToPose action就能搞定。目标点从地图YAML里的elevator_entry字段读取。阶段二进入轿厢。轿厢在仿真里是一个封闭的box机器人需要导航到轿厢中心。这里要注意轿厢内的代价地图需要单独配置因为空间小膨胀半径要临时调小。阶段三楼层切换后重新定位。这是最容易出问题的环节。我的做法是切换地图后直接发布一个已知的初始位姿电梯出口点让AMCL从那里开始定位。不要用全局重定位因为医院楼层布局相似全局重定位容易匹配到错误楼层。# 手动发布初始位姿的示例 ros2 topic pub /initialpose geometry_msgs/PoseWithCovarianceStamped header: frame_id: map pose: pose: position: {x: 13.0, y: 9.0, z: 0.0} orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0} 注意电梯出口点的坐标一定要在每层地图建图时精确记录。我建议在电梯出口地面贴一个标记物仿真里可以用一个红色方块建图时把标记物中心作为出口点坐标。5. 常见问题与排查技巧实录5.1 地图切换后机器人定位漂移这是最高频的问题。现象是切换楼层后机器人在RViz里的位置和实际Gazebo里的位置对不上导航目标点偏移好几米。排查思路检查/initialpose是否在切换后正确发布。用ros2 topic echo /initialpose确认。检查AMCL的set_initial_pose参数是否设为true。如果为false发布的初始位姿不会生效。检查新旧地图的origin是否一致。如果两层地图的origin差异大AMCL的坐标系会错乱。我的解决方案是在楼层管理节点里加一个延时地图加载完成后等2秒再发布初始位姿给AMCL留出初始化时间。5.2 电梯轿厢内导航失败轿厢空间小Nav2的局部规划器经常报“无法找到有效路径”。原因是膨胀半径太大把整个轿厢都填满了。解决办法为轿厢区域单独配置一个代价地图层用static_layerinflation_layer但膨胀半径设为0.15m。或者更简单粗暴进入轿厢时切换到DWB规划器把min_vel_x设为0.05让机器人慢慢挪进去。5.3 walking机器人步态发散仿真里步态跑着跑着就“劈叉”了关节角度越来越大。这是典型的步态控制参数不匹配。检查清单检查项正常范围异常表现步频1.0~1.5Hz过高导致振荡膝关节阻尼0.5~1.0过低导致抖动足底摩擦1.0过低导致打滑控制周期1~5ms过长导致累积误差我最后是把控制周期从10ms降到2ms步态就稳了。代价是CPU占用上去了但仿真里无所谓。5.4 Gazebo物理引擎卡顿多楼层场景下如果所有楼层模型同时加载Gazebo会非常卡。我的做法是只加载当前楼层的模型其他楼层用include但设置pose到很远的地方比如z1000物理引擎就不会计算它们。include urimodel://floor_2/uri pose0 0 1000 0 0 0/pose /include切换楼层时用Gazebo的set_model_pose服务把目标楼层移回来把旧楼层移走。5.5 常见问题速查表问题现象可能原因快速修复切换后RViz无地图map_server未加载新图检查load_map服务返回码机器人原地转圈AMCL定位丢失重新发布initialpose导航目标不可达代价地图膨胀过大调小inflation_radius步态关节超限控制周期过长降低到2msGazebo帧率低多楼层模型同时加载移走非当前楼层模型6. 后续扩展与个人实操体会这套仿真跑通之后我做了几个扩展效果不错。一个是把配送任务做成了优先级队列急救药品任务插队到最前面Nav2的行为树里加一个RateController节点控制任务切换频率。另一个是加了多机器人支持两台walking机器人共享电梯资源用了一个简单的互斥锁机制防止电梯冲突。我个人在实际操作中的体会是多楼层仿真的难点不在单点技术而在状态管理。地图状态、定位状态、任务状态这三者必须严格同步。我一开始把切换逻辑写得太随意结果调试了两天才发现是地图加载和初始位姿发布的时序问题。后来加了一个状态机来管理整个切换流程代码清晰了问题也少了。如果你要复现这套方案建议先从单层导航跑通再加电梯逻辑最后做多楼层切换。不要一上来就搞全套那样出了问题根本不知道是哪一层的问题。另外地图的命名和存储路径一定要规范我见过有人把地图存得乱七八糟切换时找不到文件排查半天以为是代码bug。
