背景

工业巡检、园区安防和物流运输等场景需要机器人在无磁条、无二维码的环境中自主移动。HelioBot 基础导航系统面向 Unitree 四足机器人，将底盘、激光雷达、建图、定位和 Navigation2 导航能力统一到 ROS2 Humble 工作空间中。

系统通过激光惯性里程计构建三维点云地图，再生成 Navigation2 可使用的二维栅格地图；运行时由 RCM 管理器统一启停底盘、雷达、建图和导航能力，并提供状态查询、暂停、恢复和全局停止接口。

核心优势

• 融合建图定位：FAST-LIO2 + Scan Context + GTSAM + Wheel Encoder，室外增加 RTK 全局约束
• Navigation2 导航：AMCL 定位、Smac Hybrid 全局规划、MPPI 控制与动态代价地图
• 统一能力管理：RCM action/service 统一管理各模块启停、状态和导航任务
• 多硬件适配：支持 Unitree GO2 / B2，以及 Livox、RoboSense、Hesai 雷达
• 可交付运行：支持 amd64 / arm64 Docker 镜像和硬件指纹 License 授权

适用平台

功能说明

系统架构

系统采用四层模块化架构，各能力通过 launch profile 和 RCM 接口组合：

数据流

LiDAR + IMU → FAST-LIO2 前端 → 激光惯性里程计 + 注册点云
Wheel Encoder → 运动约束 ┐
Scan Context  → 回环约束 ├→ GTSAM 因子图优化 → 优化位姿 + 三维点云地图
室外 RTK      → 全局约束 ┘
PCD 点云地图 → map_tools → PGM + YAML → Navigation2 Map Server
点云 → pointcloud_to_laserscan → 全局/局部 costmap → Smac + MPPI
RCM action/service → 能力控制器 → 底盘 / 雷达 / 建图 / 导航进程

SLAM 建图

系统采用统一的融合建图方案：FAST-LIO2 处理 LiDAR 与 IMU 数据并输出高频里程计；Wheel Encoder 提供独立运动约束；Scan Context 检测历史回环；GTSAM 将里程计、轮速和回环约束纳入因子图联合优化。室外模式进一步加入 RTK 全局位置约束。

• FAST-LIO2 前端：实时输出激光惯性位姿和注册点云，支持按指定路径保存 PCD
• Wheel Encoder：在快速运动、弱几何和短时点云退化场景中补充运动约束
• Scan Context：通过全局描述符检索回环候选，为长期运行提供闭环约束
• GTSAM 后端：联合优化激光惯性里程计、轮速、回环和室外 RTK 因子
• 地图输出：保存优化后的 PCD，并通过 map_tools 转换为 PGM + YAML 导航地图

导航引擎

导航系统基于 ROS2 Navigation2，使用 AMCL 在二维地图上定位，并通过 Behavior Tree 协调规划、控制和恢复行为。

• 全局规划：SmacPlannerHybrid 在静态地图和障碍物代价上生成可行路径
• 路径控制：MPPI Controller 根据局部代价地图滚动优化速度指令
• 环境感知：全局 costmap 使用静态层、障碍物层和膨胀层，局部 costmap 实时更新障碍物
• 恢复行为：支持旋转、后退、直行、辅助遥控和等待等 Behavior Server 行为

路点编辑

RCM Navigation Action 对单点和路径点序列提供统一控制：

• 单点导航：提交 map 坐标系下的目标位置和朝向
• 序列导航：通过 navigate_points 依次执行多个目标位姿
• 运行控制：支持导航启动、停止、暂停和恢复
• 状态反馈：Action 持续返回阶段、进度、当前目标与错误信息

RCM 能力管理

• 能力启停：分别管理 base、lidar、gazebo、mapping 和 navigation
• 状态查询：通过 /query_state 查询全部 capability 状态
• 故障收敛：通过 /shutdown_all 统一停止所有受管进程
• 配置驱动：通过 YAML 修改驱动、雷达、建图和导航 profile

授权机制

• 硬件指纹：使用设备指纹和 /etc/machine-id 绑定运行设备
• RSA 验签：验证 fingerprint 与 license 的 RSA-2048 签名，防止内容篡改
• 有效期校验：授权未过期且 device_id 匹配后才启动 RCM 管理器

安装部署

系统要求

• Ubuntu 22.04 LTS（Jammy）+ ROS2 Humble Hawksbill
• GCC 11+、C++17、colcon 与 ament_cmake
• Navigation2、PCL、OpenCV、GTSAM、yaml-cpp 与 OpenSSL

部署方式

源码环境可执行 ./setup.sh 安装依赖，并通过 ./build.sh 完整编译。交付环境支持 amd64 和 arm64 Docker 镜像，运行时仅包含编译产物；容器需使用 host 网络并挂载四个授权文件及宿主机 /etc/machine-id。

网络配置

ROS2 节点、机器人底盘和雷达需要处于可互通网络：

# 安装依赖并编译
./setup.sh
./build.sh

# 加载 ROS2 与项目环境
source /opt/ros/humble/setup.bash
source install/setup.bash

使用

启动系统

# 加载环境并启动 RCM 管理器
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch bringup rcm_manager.launch.py

RCM 管理器启动后，可通过 ROS2 Action/Service 或 src/bringup/scripts/rcm_test_client.py 控制底盘、雷达、建图和导航。

SLAM 建图

通过 RCM 完成建图全流程：

1. 启动底盘和对应雷达 capability
2. 启动 mapping，指定 fast_lio2 profile、室内/室外模式及地图保存路径；室外模式同时启用 RTK
3. 运行键盘遥控节点，使用 W/A/S/D 移动，Space 停止，缓慢覆盖目标区域
4. 停止 mapping；系统完成 GTSAM 图优化并调用 /map_save 保存优化点云，再停止建图进程
5. 使用 pcd_to_pgm.py 将 PCD 转换为 Navigation2 使用的 PGM + YAML

导航操作

准备好地图后，通过 RCM 启动导航：

1. 将 map.yaml 与 map.pgm 放入地图目录并检查图像路径
2. 启动 navigation action，并通过 launch_args 传入地图 YAML 路径
3. 通过 /initialpose 设置机器人在地图中的初始位置和朝向
4. 发送单点或多点导航目标，系统使用 SmacPlannerHybrid 与 MPPI 执行导航
5. 运行中可调用暂停、恢复或停止接口，并通过 Action feedback 查看执行状态

路点巡航

Navigation Action 支持多目标序列：

• 使用 navigate_point 发送单个目标位姿
• 使用 navigate_points 发送有序目标位姿列表
• Waypoint Follower 到达路点后按顺序推进，并可配置路点等待任务

地图管理与切换

• 地图格式：导航地图由 YAML 元数据和 PGM 栅格图组成，点云定位也可加载 PCD / PLY 全局地图
• 地图切换：停止当前导航后，使用新的地图路径重新启动 navigation capability

异常汇总