# 宇树 G1 基于 ROS2 Nav2 的导航栈落地实践

> 原文链接：https://robotics.hellobike.com/wiki/g1-nav2-implementation-notes
> 频道：wiki | 更新日期：2026-10-10 | 标签：G1, Nav2, 路径规划, 速度控制, 避障恢复, 碰撞监控
> 核心摘要：记录宇树 G1 在机载计算单元运行 ROS2 Humble Nav2 的系统拓扑、通信隔离、步态适配速度控制、碰撞监控与避障恢复策略。

# 宇树 G1 基于 ROS2 Nav2 的导航栈落地实践

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

## 1. 系统架构与通信隔离

在宇树 G1 的机载计算单元（PC2，硬件平台 Jetson Orin，宿主系统 Ubuntu 20.04）上落地 Nav2 时，为解耦系统依赖，采用容器化方案运行 ROS2 Humble Nav2。

```text
控制台界面 ──(HTTP)──> 宿主机边缘主服务
                           │
                           │ 标准输入输出 JSONL 管道
                           ▼
                     Nav2 容器 (ROS2 Humble, --network host)
                           │
                           │ cmd_vel / TF / costmap (ROS_DOMAIN_ID=42)
                           ▼
                     底层运动适配器 ──(DDS)──> 机器人底盘 (ROBOT_DDS_DOMAIN=0)
```

关键隔离机制：
- **通信域隔离**：Nav2 容器内部使用 `ROS_DOMAIN_ID=42`，底盘通信与传感器点云使用 `ROBOT_DDS_DOMAIN=0`，避免 ROS 话题与底盘 DDS 话题相互干扰。
- **权限与速度下发唯一出口**：Nav2 不直接向底层硬件发送驱动报文，仅产生目标速度建议。速度建议必须经安全看门狗、加速度限制与死区判定后，由适配器独占调用 `LocoClient.SetVelocity` 执行。

## 2. 实时点云预处理与代价地图

### 2.1 Mid-360 点云滤波

机载 LiDAR 点云进入局部代价地图（costmap）前，执行以下预处理流水线：
1. **自体回波滤除**：剔除机身周围 `0.30m` 范围内的点，防止机体自身（手臂、双腿）形成伪障碍。
2. **体素下采样**：采用 `0.10m` 体素网格下采样，降低计算开销。
3. **噪点滤除**：过滤少于 3 个点的孤立点云簇，单帧最多保留 1200 个用于局部代价地图更新的有效点。
4. **有效高度截取**：将高度窗口限制在 `0.15m ～ 1.80m`，排除地面微小颠簸并覆盖人体与立柱障碍。

### 2.2 代价地图几何约束

- **机器轮廓（Footprint）**：采用矩形多边形 `[(±0.35m, ±0.28m)]` 并附加 `0.08m` 边距。
- **膨胀半径（Inflation Radius）**：生产参数采用紧凑的 `0.43m`（覆盖足迹边距后的外接圆下限），防止在走廊环境下因过度膨胀造成无路可走。

## 3. 双足步态速度控制策略

由于双足人形机器人的动力学特质，速度控制必须严格契合物理步态：

1. **有效行走下限**：G1 的运动控制存在起步步态死区。若输出线速度低于 `0.20m/s`，机器人往往仅出现双足踏步抖动而无法产生有效位移。因此局部规划器设置 `min_speed_xy = 0.20m/s`。
2. **平滑死区处理**：速度平滑器配置死区过滤 `[0.19, 0.0, 0.10]`。若避障或制动逻辑将目标线速度压缩至 `(0, 0.20m/s)` 区间，系统直接将整体速度指令清零（含角速度），避免在原地异常扭转。
3. **禁用横移**：双足步态行进中严格设置横向速度 `vy = 0`，仅允许前进 `vx` 与就地转向 `wz`。
4. **加速度与角速度限制**：线速度上限设为 `0.40m/s`，角速度上限设为 `0.30rad/s`。

## 4. 碰撞监控与避障恢复机制

### 4.1 Collision Monitor 保护

在速度平滑器之后配置 Collision Monitor 节点，采用二元 StopZone 机制：
- 当障碍物进入制动包络时，瞬时将下发速度归零；
- 避免配置多级按比例减速区域，防止低速输出落入双足步态死区引起振荡。

### 4.2 障碍物恢复行为树

当行进路线受阻时，遵循分级恢复策略：
1. **就地停等**：优先保持当前规划路径，原地停等 3 秒，等待临时路过的行人离开。
2. **后向净空检测与短距倒退（BackUp）**：
   - 检查机器人后方走廊（`x = [-1.00, -0.43]m`, `|y| ≤ 0.55m`, `z = [0.15, 1.80]m`）；
   - 在连续多帧确认后方完全无障碍点且雷达数据新鲜的前提下，允许执行单次短距倒退（距离 `0.15m`，速度 `0.20m/s`）；
   - 倒退后必须全轴停稳 `0.50s` 再触发全局重规划。
3. **等待操作员处理**：若后方存在障碍或多次重规划仍无可用路径，机器人进入暂停待命状态，由操作员确认现场情况后决定恢复或取消任务。

## 5. 到点容差与航向对齐

目标点完成判定遵循“位置先收敛，航向有界尽力”原则：
- **容差门限**：平面位置 XY 容差设为 `0.35m`，最终偏航角 yaw 容差设为 `0.50rad`。
- **航向原地对准**：当机器人进入 XY 容差半径后，锁定位置完成状态，线速度强制清零，仅允许以不超过 `0.30rad/s` 的角速度进行原地转向。
- **超时保护**：原地航向对齐设置 `6.0s` 严格超时。若超时仍未完全消除角度偏差，系统立即下发零速并标记到点完成，防止机器人在终点无限打转。

## 原始参考来源 (Sources)

- [Navigation2 Documentation](https://github.com/ros-navigation/navigation2)
- [unitree_sdk2_python](https://github.com/unitreerobotics/unitree_sdk2_python)