雷达与 IMU 建图笔记：四足和无人机共用一个组件

同一个建图组件能否用于四足和无人机？这次把 Go2 的步行记录和四旋翼的飞行记录分别接入同一套 ros2_mapping，让两种机器人使用不同的传感器话题、坐标系和安装外参，验证建图代码能否直接复用。

两套演示都实际运行了 Cartographer 3D，生成了点云地图和估计轨迹。下面的 MP4 同时展示机器人运动与 SLAM 结果。

1. Go2 四足：边走边积累点云

原始运动来自已有的 Go2 导航仿真：上层速度命令进入 ONNX 步行策略，再经关节 PD 驱动 MuJoCo。这里取出实际记录的基座和关节状态，在机体上定义虚拟雷达与 IMU，重新生成建图输入。

2. 四旋翼：起飞、绕圈和降落中的建图

无人机的运动记录来自四旋翼 OCS2、姿态 PD 与统一控制器笔记。飞行控制原理和执行器分配仍由那篇说明；本篇关注同样的运动怎样变成建图组件的输入。

两段视频默认显示静态封面，点击后才加载播放，可暂停、拖动或全屏查看。右侧显示高度低于 2.2 m 的点云以便看清室内，导出的地图保留完整高度。播放过程积累在线注册点云，末尾切换到使用最终优化轨迹重投影的地图。

3. 复用的边界：算法共用，传感器配置各自明确

建图节点只消费标准 PointCloud2、Imu、时间和固定安装变换，无需知道机器人有几条腿或几个旋翼。两套演示没有复制两份建图算法；同一个 mapping.launch.py 启动输入校验节点与 Cartographer 3D。

外参定义为 p_imu = R_imu_lidar × p_lidar + t_imu_lidar。安装位置或方向不同，点云必须先转换到跟踪坐标系，再由估计位姿放入地图。无人机的 90° 安装旋转用于验证这一变换，不能省略或直接照抄到真实设备。

这与统一控制器的组织思路相同：固定组件输入输出，把机器人差异留在适配配置中。控制器输出关节或旋翼命令，建图组件输出地图与位姿；两者各有自己的职责。

4. 本轮验证了什么

误差仅通过首个位姿做一次刚体坐标系对齐，不对整条轨迹拟合，也不改变尺度。两种机器人都通过输入有效性、停止后的断流检测、输出频率、轨迹覆盖、三维地图完整度和 PBSTREAM 保存检查。原输入契约测试与新增传感器几何、IMU 约定测试共 6 项通过；本轮指标与视频哈希记录保留了对应数据。

这轮验证使用实际 MuJoCo 运动记录 + 虚拟传感器回放 + 实际 ROS SLAM。50 Hz 运动记录插值生成 100 Hz IMU，比力保留重力；雷达采用 24×360 条瞬时射线，加入 12 mm 测距噪声和 1.5% 丢点。扫描已经去畸变，不模拟转动扫描的时间畸变、自遮挡或高频接触振动。

Go2 保留原障碍物并增加外围房间，无人机在原飞行轨迹周围加入静态墙体与箱体供射线求交。这些新增传感器与场景没有重新参与动力学求解。真值只用于生成模拟传感器、显示机器人和事后评估，不把真值位姿或里程计发送给 SLAM。

当前 SLAM 结果没有反馈到导航或飞行控制。因此这两段演示证明了组件的跨机器人输入适配；无真值定位的控制闭环、真实设备标定、同步与去畸变仍需后续独立验证。两台机器人本轮分开运行，也不代表已经实现多机器人同时建图或地图合并。

5. 如何复现和换机器人

以下入口对应本次本地开发工作区，依赖已有的 ROS、控制器和 MuJoCo 基础镜像；笔记媒体本身不包含整套后端环境。

# 使用已有 Go2 与四旋翼运动记录，验收并生成两个 MP4
./bin/site.sh ros2-mapping-robots

# 指定新的输出目录和自己的两份运动记录
./bin/site.sh ros2-mapping-robots build/ros2-mapping/my-robots \
  build/ros2-navigation/my-go2 build/quadrotor/my-flight/ocs2

代码集中在 docker/ros2-humble-dev/control_ws/src/ros2_mapping/：config/robots/ 保存两套示例配置，robot_sensors.py 生成虚拟传感器，robot_replay.py 负责演示录制，input_node.py 与 mapping.launch.py 是共用建图入口。包内 ROBOT_DEMOS.md 和 INTERFACES.md 说明复现与真实驱动接入契约。

换成真实机器人时，由驱动发布符合契约的点云和 IMU，配置话题、frame、标定外参与时间同步，再独立检查数据质量和建图误差。运动回放器只是本次 demo 的数据源，真实设备不需要依赖它。