# 宇树 G1 SLAM 建图与受控自主导航技术概览

> 原文链接：https://robotics.hellobike.com/wiki/g1-slam-navigation-overview
> 频道：wiki | 更新日期：2026-10-10 | 标签：G1, SLAM, Nav2, 激光雷达, 自主导航, FAST-LIO2
> 核心摘要：介绍宇树 G1 人形机器人在受控环境下的建图、定位与自主导航技术架构，涵盖 Mid-360 激光雷达点云处理、FAST-LIO2 建图与 Nav2 动态避障。

# 宇树 G1 SLAM 建图与受控自主导航技术概览

> 本文为宇树 G1 EDU 真机实操记录，固件/SDK 版本以文中标注为准。

## 1. 范围与安全模型

本文描述宇树 G1 在已建图的受控区域内的自主导航技术管线。该场景面向结构化室内或受控巡检路线，由操作员在控制界面点选目标，机器人自主规划路径并完成动态避障。

核心安全原则：
- **受控运行**：非开放环境自主穿行人群；当感知检测到人员持续挡路时执行安全停等，而非强行穿插。
- **过程监控**：导航期间持续评估越界、超速、通信链路状态、低电量与感知阻挡；任一异常立即触发减速或暂停。
- **操作员在环**：所有导航目标下发前需操作员确认，物理急停开关随时保持可达。

```text
点云与 IMU (DDS) ──> FAST-LIO2 (建图) ──> 地图资产 (raw/dense/grid)
                             │
                             ▼
实时扫描 (DDS) ──> 点云 ICP 配准 (定位 map→odom) ──> Nav2 规划器 ──> 安全限幅 ──> 底盘控制
```

## 2. 硬件与传感器接口

### 2.1 激光雷达点云与 IMU 数据流

宇树 G1 标配 Livox Mid-360 激光雷达（倒装安装在头部/胸腔位置）。机载系统通过 DDS 话题对外分发传感器数据：

| 话题名称 | 消息类型 | 频率 / 坐标系 | 说明 |
|---|---|---|---|
| `rt/utlidar/cloud_livox_mid360` | `sensor_msgs/PointCloud2` | 10Hz / `livox_frame` | 点云输入，用于 FAST-LIO2 建图与实时避障 |
| `rt/utlidar/imu_livox_mid360` | `sensor_msgs/Imu` | 200Hz | IMU 数据，用于里程计融合与姿态解算 |

雷达硬件配置与服务管理要点：
- **固件与驱动要求**：LiDAR Driver 版本需 ≥ 1.0.0.5。
- **点云服务启停**：雷达点云发布服务在机器人冷启动后默认可能处于未开启状态。可通过机载 `robot_state` 服务的 `ServiceSwitch` 接口传入雷达服务名动态启停，无需整机重启。
- **点云数据格式**：数据包为小端序，单点步长（`point_step`）为 22 字节。解析字段包括：
  - `x`: offset 0, float32
  - `y`: offset 4, float32
  - `z`: offset 8, float32
  - `intensity`: offset 12, float32
  - 其余 `ring` (offset 16, uint16) 与 `time` (offset 18, float32) 字段在常规障碍物投影时可忽略。

## 3. 建图、定位与规划架构

### 3.1 FAST-LIO2 建图与地图资产化

建图阶段通过订阅原始点云与 IMU 话题，运行容器化的 FAST-LIO2 算法实时生成高精度 3D 点云。建图完成后对原始点云（raw）进行地面拟合、噪点过滤与静态化处理，输出紧凑点云（dense）与二维占据栅格图（grid），确保规划与定位复用同源地图。

### 3.2 点云 ICP 定位

在已建图区域中，定位模块接收当前实时扫描点云与里程计位姿，结合 Open3D ICP 算法将实时点云与先验 dense 地图进行匹配，计算出全局位姿校正量并发布 `map → odom` 坐标变换，具备位姿跳变检测与短暂失鲜抑制能力。

### 3.3 Nav2 规划与双足速度整形

规划栈采用 ROS2 Nav2，适配双足机器人的动力学约束：
- **全局路径规划**：基于 `SmacPlanner2D`，考虑栅格代价与安全边距。
- **局部轨迹跟踪**：采用针对双足步态优化的局部跟踪器（或 DWB 本地规划器作为回退方案）。严禁侧向平移（`vy = 0`），仅输出前进速度 `vx` 与旋转角速度 `wz`。
- **双足速度死区控制**：G1 在非零超低速指令下容易出现原地踏步抖动。因此工程上设定有效行走线速度下限为 `0.20m/s`，最大巡航速度通常设为 `0.40m/s`，角速度上限设为 `0.30rad/s`。
- **碰撞监控**：在速度平滑输出后接 Collision Monitor 的 StopZone 机制。若实时点云进入刚性包络，速度立即强制归零，并触发就地停等或动态绕行重规划。

## 4. 关键参数与调优经验

| 参数项 | 推荐值 | 调优考量 |
|---|---|---|
| 机器人包络半径 | `0.35m` | 匹配 G1 双足直立状态的外形轮廓 |
| 足迹外扩填充 | `0.08m` | 保证摆臂与摆腿过程中的动态间距 |
| 代价地图膨胀半径 | `0.43m` | 收紧至 `0.35 + 0.08m`，避免将较窄走廊误判为不可通行 |
| 行走线速度下限 | `0.20m/s` | 消除双足机械踏步死区 |
| 旋转角速度上限 | `0.30rad/s` | 保证回转平稳，防止角速度过快导致质心失稳 |
| 障碍暂停重规划间隔 | `3.0s` | 给移动障碍离开留出观察缓冲，避免高频振荡 |

## 原始参考来源 (Sources)

- [G1 LiDAR services interface](https://support.unitree.com/home/zh/G1_developer/lidar_services_interface)
- [Navigation2](https://github.com/ros-navigation/navigation2)