开源 | 四足机器人完整导航栈：Autonomy Stack for Unitree Go2【项目解读】

无宜

94人浏览 · 2026-08-15 13:40:16

无宜 · 2026-08-15 13:40:16 发布

项目：Autonomy Stack for Unitree Go2
项目定位：面向 Unitree Go2 EDU 的完整自主导航栈，覆盖激光雷达—惯性里程计、地形分析、局部避障、路径跟踪、FAR 全局/路由规划、仿真与实机控制
开源地址：https://github.com/jizhang-cmu/autonomy_stack_go2
主要维护者：Ji Zhang（CMU）等
默认分支：foxy-humble
ROS：ROS 2 Foxy / ROS 2 Humble
机器人：Unitree Go2 EDU
核心传感器：Go2 内置 Unitree L1 LiDAR + L1 内置 IMU
相关算法：Point-LIO、FAR Planner、CMU Autonomous Exploration Development Environment
Point-LIO：https://github.com/hku-mars/Point-LIO
FAR Planner：https://github.com/MichaelFYang/far_planner

1. 项目解决的是什么问题

autonomy_stack_go2 的目标并不是单独实现一个局部规划器或一个 SLAM 算法，而是把 定位建图、地形理解、长距离路径规划、近距离避障、路径跟踪和 Unitree Go2 运动接口 串成一套可以直接运行的四足机器人自主导航系统。

从根目录 README 和总 Launch 文件可以看出，系统支持两类使用方式：

自主导航：用户给出目标点，Go2 一边在线建图，一边规划并自主移动到目标点；
智能遥控：用户通过手柄给出期望运动方向，系统保留局部碰撞检测和避障能力。

项目只依赖 Go2 EDU 机身自带的 L1 激光雷达和雷达内部 IMU，不要求额外安装 3D 雷达、相机或外部定位系统。实机既可以直接运行在 Go2 板载计算机上，也可以通过以太网把 ROS 2 导航栈运行在外部电脑上。

这套系统真正解决的是一个完整的工程问题：

如何让一台仅依赖机载 LiDAR + IMU 的 Go2，在未知三维环境中完成在线定位、地形理解、全局路径搜索、局部避障和速度控制，并最终把导航结果转换为四足机器人可执行的运动指令。

图 1　Autonomy Stack Go2 使用的机载传感器：Unitree L1 LiDAR 与内置 IMU

2. 项目的核心意义

传统 ROS 导航系统通常采用：

SLAM / Localization
        ↓
2D Occupancy Grid
        ↓
Global Planner
        ↓
Local Costmap
        ↓
DWA / TEB / MPC
        ↓
cmd_vel

autonomy_stack_go2 并没有照搬 Nav2 的标准 costmap_2d + planner_server + controller_server 框架，而是采用了一条更加偏向三维机器人导航的技术路线：

Unitree L1 LiDAR + IMU
        │
        ▼
Point-LIO
        │
        ├──────────────► 在线里程计 /state_estimation
        │
        └──────────────► 配准点云 /registered_scan
                              │
                              ▼
                 terrain_analysis
                              │
                ┌─────────────┴─────────────┐
                ▼                           ▼
          /terrain_map               /terrain_map_ext
                │                           │
                │                     FAR Planner
                │                           │
                │            Visibility Graph / Route
                │                           │
                │                     /way_point
                │                           │
                └──────────────┬────────────┘
                               ▼
                         localPlanner
               预计算轨迹库 + 点云碰撞查表
                               │
                             /path
                               ▼
                         pathFollower
             前视跟踪 + 航向控制 + 速度限幅
                               │
                     ┌─────────┴─────────┐
                     ▼                   ▼
                  /cmd_vel       /api/sport/request
                                         │
                                         ▼
                                Unitree Sport API
                                         │
                                         ▼
                                      Go2

从这个结构可以看出，系统把导航问题拆成四层：

层级	核心模块	主要任务	输出
状态估计	Point-LIO	LiDAR-IMU 融合定位与建图	`/state_estimation`、`/registered_scan`
环境理解	terrain_analysis / terrain_analysis_ext	地面估计、相对高差、未知区域和地形连通性	`/terrain_map`、`/terrain_map_ext`
规划	FAR Planner + localPlanner	长距离路由搜索 + 局部避障	`/way_point`、`/path`
控制接口	pathFollower + Unitree Sport API	路径跟踪、速度约束和实机命令转换	`/cmd_vel`、`/api/sport/request`

这种架构最大的工程价值是：高层导航不需要理解四足机器人的关节和足端控制。

导航栈只输出机身期望速度

$\mathbf{u} =\begin{bmatrix} v_x & v_y & \omega_z \end{bmatrix}^{T},$

随后通过 Unitree Sport API 的 Move(vx, vy, vyaw) 交给 Go2 自带的运动控制器。步态生成、足端轨迹、关节力矩等底层问题由机器人本体完成。

因此，这个项目解决的是“自主导航到机身速度指令”这一层，而不是重新实现 Go2 的全身控制器。

3. 仓库整体结构

源码可以分为四个主要部分。

autonomy_stack_go2/
├── README.md
├── img/
│
├── src/
│   ├── base_autonomy/
│   │   ├── local_planner/
│   │   ├── sensor_scan_generation/
│   │   ├── terrain_analysis/
│   │   ├── terrain_analysis_ext/
│   │   ├── vehicle_simulator/
│   │   ├── visualization_tools/
│   │   └── waypoint_example/
│   │
│   ├── route_planner/
│   │   ├── boundary_handler/
│   │   ├── far_planner/
│   │   ├── graph_decoder/
│   │   └── visibility_graph_msg/
│   │
│   ├── slam/
│   │   └── point_lio_unilidar/
│   │
│   └── utilities/
│       ├── ROS-TCP-Endpoint/
│       ├── calibrate_imu/
│       ├── goalpoint_rviz_plugin/
│       ├── teleop_rviz_plugin/
│       ├── teleop_rviz_plugin_plus/
│       ├── transform_sensors/
│       ├── unitree_pkgs/
│       └── waypoint_rviz_plugin/
│
├── system_simulation.sh
├── system_simulation_with_route_planner.sh
├── system_real_robot.sh
├── system_real_robot_with_route_planner.sh
└── unitree_setup.sh

3.1 `base_autonomy`

这是系统的局部自主导航层。

其中最重要的是：

terrain_analysis：从配准后的 3D 点云中估计地面和障碍高度；
terrain_analysis_ext：生成范围更大的地形信息，并为 FAR Planner 提供扩展地图；
local_planner：在预生成轨迹库中进行碰撞筛选和路径打分；
pathFollower：把局部路径转换成机身速度；
vehicle_simulator：仿真和总系统 Launch；
sensor_scan_generation：仿真传感器扫描数据生成；
visualization_tools：RViz 可视化。

3.2 `route_planner`

这一部分集成 FAR Planner。

主要包括：

far_planner：FAR 主规划器；
graph_decoder：可见图信息解码；
boundary_handler：导航边界处理；
visibility_graph_msg：可见图自定义 ROS 2 消息。

3.3 `slam`

目前核心包为：

point_lio_unilidar

它是在 Point-LIO 基础上针对 Unitree LiDAR L1 做的适配版本，并在当前工程中进一步移植到 ROS 2。

3.4 `utilities`

主要处理工程接口：

Unitree Go2 Sport API；
Unitree ROS 2 消息；
L1 点云/IMU 坐标变换；
IMU 标定；
RViz Goalpoint / Waypoint / Teleop 插件；
Unity 与 ROS 2 之间的 ROS-TCP-Endpoint；
Go2 H.264 视频流转 ROS Image。

4. 系统输入、输出与 ROS 话题关系

整个导航链最关键的话题可以整理为：

Topic	类型/来源	作用
`/utlidar/transformed_cloud`	L1 LiDAR	Point-LIO 输入点云
`/utlidar/transformed_imu`	L1 IMU	Point-LIO IMU 输入
`/state_estimation`	Point-LIO / 仿真器	机器人状态估计
`/registered_scan`	Point-LIO / 仿真器	世界坐标系配准点云
`/terrain_map`	terrain_analysis	近场地形高差点云
`/terrain_map_ext`	terrain_analysis_ext	FAR Planner 使用的扩展地形地图
`/goal_point`	RViz Goalpoint	FAR Planner 最终目标
`/way_point`	FAR Planner / RViz	局部规划器当前目标
`/navigation_boundary`	FAR Planner / waypoint_example	局部规划约束边界
`/path`	localPlanner	局部可执行路径
`/cmd_vel`	pathFollower	机身速度命令
`/api/sport/request`	pathFollower	Go2 Sport API 请求
`/joy`	joy_node	手柄输入

启用 FAR Planner 后，Launch 文件中的主要 remap 为：

FAR /odom_world          ← /state_estimation
FAR /terrain_cloud       ← /terrain_map_ext
FAR /scan_cloud          ← /terrain_map
FAR /terrain_local_cloud ← /registered_scan

FAR Planner 再把当前局部导航目标发布到：

/way_point

因此 FAR Planner 和局部规划器之间并不是通过完整的稠密全局轨迹强耦合，而是采用“长距离路由 → 下一导航点 → 局部路径规划”的层级方式连接。

5. Point-LIO：LiDAR-IMU 状态估计

5.1 为什么使用 Point-LIO

Go2 自带 L1 LiDAR 的视场较大，扫描模式为非重复扫描，同时包含 IMU。项目使用 Point-LIO 把这两类信息融合起来，实时输出：

机器人位姿；
速度；
IMU 偏置；
重力方向；
LiDAR-IMU 外参；
配准后的局部点云/地图。

仓库中的状态定义可以直接在：

src/slam/point_lio_unilidar/src/Estimator.h

中看到。

输入状态为：

$\mathbf{x} =\left[ \mathbf{p}, \mathbf{R}, \mathbf{R}_{LI}, \mathbf{t}_{LI}, \mathbf{v}, \mathbf{b}_g, \mathbf{b}_a, \mathbf{g} \right].$

其中：

$p\mathbf{p}$ ：IMU 在世界坐标系中的位置；
$R\mathbf{R}$ ：IMU 姿态；
$RLI\mathbf{R}_{LI}$ ：LiDAR 到 IMU 的旋转外参；
$tLI\mathbf{t}_{LI}$ ：LiDAR 到 IMU 的平移外参；
$v\mathbf{v}$ ：速度；
$bg\mathbf{b}_g$ ：陀螺仪零偏；
$ba\mathbf{b}_a$ ：加速度计零偏；
$g\mathbf{g}$ ：重力向量。

5.2 IMU 状态传播

源码 get_f_input() 中对应的连续时间模型可以写成：

$\dot{\mathbf{p}}=\mathbf{v},$

$\dot{\mathbf{R}} =\mathbf{R} \left[ \boldsymbol{\omega}_m-\mathbf{b}_g \right]_{\times},$

$\dot{\mathbf{v}} =\mathbf{R} \left( \mathbf{a}_m-\mathbf{b}_a \right) + \mathbf{g}.$

其中：

$ωm\boldsymbol{\omega}_m$ ：IMU 测得的角速度；
$am\mathbf{a}_m$ ：IMU 测得的线加速度；
$[⋅]×[\cdot]_{\times}$ ：向量对应的反对称矩阵。

实际系统使用 IKFoM 中的误差状态迭代 Kalman Filter 进行流形上的状态估计。

5.3 点到平面残差

Point-LIO 会把当前 LiDAR 点变换到世界坐标系，并利用 ikd-Tree 搜索附近地图点。

LiDAR 点 $pL\mathbf{p}^{L}$ 首先经过外参：

$\mathbf{p}^{I} =\mathbf{R}_{LI}\mathbf{p}^{L} + \mathbf{t}_{LI},$

再变换到世界坐标系：

$\mathbf{p}^{W} =\mathbf{R}\mathbf{p}^{I} + \mathbf{p}.$

若邻域拟合平面为：

$\mathbf{n}^{T}\mathbf{x}+d=0,$

则点到平面的残差为：

$=\mathbf{n}^{T}\mathbf{p}^{W}+d.$

源码中对应：

pd2 = a * point_world.x
    + b * point_world.y
    + c * point_world.z
    + d;

满足平面质量和距离条件的点才会进入滤波更新。

ikd-Tree 的作用是维护一个可在线增删和最近邻搜索的增量 KD-Tree，从而避免每一帧重新构建整张地图的数据结构。

5.4 Go2 L1 配置

config/utlidar.yaml 中的重要默认参数包括：

lid_topic: /utlidar/transformed_cloud
imu_topic: /utlidar/transformed_imu

filter_size_surf_min: 0.2
filter_size_map_min: 0.2

imu_time_inte: 0.01
acc_norm: 9.81
lidar_meas_cov: 0.01
plane_thr: 0.1

extrinsic_est_en: false

项目默认关闭在线外参估计，直接使用预先设置的 LiDAR-IMU 外参。

6. 地形分析：从三维点云得到可通行性信息

局部规划器没有直接把所有原始点云都视为障碍物，而是先执行 terrain_analysis。

这一步非常关键。

如果只根据点云是否存在来判断障碍，那么：

地面本身会被误判为障碍；
小台阶与墙体无法区分；
地形起伏无法转化成连续代价；
未观测区域缺乏安全处理。

项目通过局部地面估计计算每个点相对于地面的高度。

6.1 滚动体素地图

terrainAnalysis.cpp 维护两级网格：

terrain voxel：
  voxel size = 1.0 m
  width      = 21 × 21

planar voxel：
  voxel size = 0.2 m
  width      = 51 × 51

大体素负责保存一定时间范围内的点云，小平面栅格负责估计局部地面。

机器人移动时，terrain voxel 会随机器人位置滚动，而不是建立无限增长的全局稠密栅格。

点云还带有时间衰减机制：

$t_{\text{now}}-t_i < T_{\text{decay}},$

默认：

$T_{\text{decay}}=2\,\text{s}.$

这样可以减少旧点云对动态环境的长期影响。

6.2 地面高度估计

对于平面栅格 $c$ ，收集邻域点高度：

$\mathcal{Z}_c =\{z_1,z_2,\ldots,z_N\}.$

源码默认：

useSorting: true
quantileZ: 0.25

因此先排序：

$z_{(1)} \le z_{(2)} \le \cdots \le z_{(N)},$

再取：

$=\left\lfloor qN \right\rfloor, \qquad q=0.25,$

得到局部地面：

$z_g(c)=z_{(k)}.$

相比直接取最低点，分位数估计对少量离群点更稳定。

6.3 相对地面高差

对于点 $pi\mathbf{p}_i$ ：

$$
h_i

z_i-z_g(c_i).
$$

源码最终把：

$I_i=h_i$

写入 PointXYZI::intensity。

因此 /terrain_map 的 intensity 并不是普通激光反射强度，而是该点相对局部地面的高度差。

这也是理解后续局部规划器的关键。

默认配置中：

obstacleHeightThre: 0.3
groundHeightThre: 0.1

因此可以粗略理解为：

$h < 0.1$ m：近似地面；
$\sim 0.3$ m：可作为软地形代价；
$h > 0.3$ m：直接按障碍物处理。

实际是否允许通过还会受到 useCost、路径碰撞点数量和其他参数影响。

6.4 未观测区域处理

配置：

noDataObstacle: true

意味着局部区域如果缺少足够点云，系统可以主动将其按障碍处理。

这对楼梯边缘、坑洞、悬崖和 LiDAR 无回波区域尤其重要，因为“没有点”并不等于“可以走”。

源码通过：

minBlockPointNum
maxElevBelowVeh
noDataAreaMinX / MaxX
noDataAreaMinY / MaxY

判断机器人前方某块区域是否缺少有效地面支撑。

7. 局部规划器：预计算轨迹库，而不是 DWA/TEB

local_planner/src/localPlanner.cpp 是整套系统中最值得分析的部分之一。

它并不是：

DWA；
TEB；
MPC；
MPPI；
A* 在线搜索。

它使用的是：

离线预生成候选轨迹 + 在线点云碰撞查表 + 路径方向/地形代价评分。

7.1 候选路径库

源码固定：

const int pathNum = 343;
const int groupNum = 7;

即：

343 条候选曲线路径；
7 个路径组。

轨迹文件位于：

src/base_autonomy/local_planner/paths/
├── paths.ply
├── startPaths.ply
├── pathList.ply
├── correspondences.txt
└── path_generator.m

其中 path_generator.m 用于离线生成路径集合。

在线运行时不需要重新求解这些轨迹，而是直接加载。

7.2 36 个旋转搜索方向

局部规划器进一步把路径库旋转到 36 个方向：

$\theta_r =10r-180^\circ, \qquad r=0,\ldots,35.$

所以理论上被检查的“路径—朝向”组合数量达到：

$343\times36=12348.$

但它没有对每个组合逐点进行完整几何碰撞检测，而是使用提前建立的空间索引表。

7.3 路径缩放与旋转

候选路径原始点：

$\mathbf{p} =\begin{bmatrix} x\\ y \end{bmatrix}.$

经过比例 $s$ 与旋转 $θ\theta$ 后：

$\mathbf{p}' =s \begin{bmatrix} \cos\theta & -\sin\theta\\ \sin\theta & \cos\theta \end{bmatrix} \mathbf{p}.$

源码对应：

x' = pathScale * (cos(rotAng) * x - sin(rotAng) * y);
y' = pathScale * (sin(rotAng) * x + cos(rotAng) * y);

pathScale 还可以随当前速度变化。

如果当前尺度找不到路径，程序会逐渐：

减小 pathScale；
再减小 pathRange；

直到找到可行路径或退化为停车路径。

7.4 为什么需要 `correspondences.txt`

这是整个局部规划器高效运行的关键。

离线阶段已经知道：

某个空间网格被占用时，哪些候选路径会撞上这个网格。

于是建立：

$\mathcal{C}(v) =\{p_1,p_2,\ldots\},$

其中 $v$ 是空间 voxel， $C(v)\mathcal{C}(v)$ 表示会穿过该 voxel 的候选路径集合。

在线检测到障碍点后，只需：

把障碍点投影到对应网格；
查询 correspondences[ind]；
给相关路径的碰撞计数加一。

因此复杂度从“障碍点 × 所有路径 × 路径点”变成了稀疏查表。

7.5 障碍碰撞判定

对于旋转方向 $r$ 、候选路径 $p$ ，定义碰撞计数：

$N_{r,p} =\sum_{v\in\mathcal{O}} \mathbb{I} \left( p\in\mathcal{C}(v) \right).$

源码默认：

pointPerPathThre: 2

所以当：

$N_{r,p}<2$

时，该路径仍可被认为是候选可行路径。

这相当于允许少量离散噪声点，而不是一个点就把整条轨迹永久否决。

7.6 地形软代价

如果开启 useCost，低于障碍阈值但高于地面阈值的地形会形成软惩罚。

源码中的惩罚项近似为：

$P_h =1- \frac{h_{\max}}{h_{\text{cost}}},$

并进行下限截断：

$P_h =\max(P_h,P_{\min}).$

代码对应：

penaltyScore =
    1.0 - pathPenaltyList[i] / costHeightThre;

if (penaltyScore < costScore)
    penaltyScore = costScore;

这使规划器不仅能做“碰撞 / 不碰撞”的二值判断，也能在多个可通行方向之间优先选择更平坦的地形。

7.7 方向评分函数

对于候选轨迹末端方向与目标方向之间的误差：

$\Delta\theta =\left| \theta_{\text{goal}} -\theta_{\text{path}} -\theta_r \right|.$

角度会被归一化到：

$[0,180^\circ].$

正常情况下源码评分为：

$=\left( 1-\sqrt[4]{w_d\Delta\theta} \right) W_r^4 P_h.$

其中：

$w_d$ 对应 dirWeight；
$W_r$ 是旋转方向权重；
$P_h$ 是地形惩罚项。

程序只累加：

$S > 0$

的候选路径分数，并以“路径组”为单位累计。

接近目标点时，评分切换为：

$S_{\text{goal}} =\left( 1-\sqrt[4]{w_d\Delta\theta} \right) W_g^2 P_h,$

更加关注路径组本身与目标的匹配。

最终选择：

$(r^, g^) = \underset{r,g}{\mathrm{arg\,max}}\; S_{r,g}$

7.8 这个局部规划器的优势

计算量稳定

不需要在线迭代求解非线性优化问题。

直接使用三维点云

不依赖传统 2D costmap。

对动态障碍响应快

新的点云一到即可重新筛选轨迹。

候选路径具有运动连续性

轨迹并不是栅格搜索产生的折线，而是提前设计好的平滑路径族。

7.9 局限

这类轨迹库方法也存在明确边界：

能执行的运动受离线轨迹库覆盖范围限制；
343 条路径并不代表任意轨迹空间；
碰撞模型主要围绕机器人机身几何，而不是四足接触可达性；
没有显式优化能耗、足端落点或动力学稳定裕度；
在特别狭窄的环境中，轨迹分辨率可能成为限制。

8. Path Follower：前视点路径跟踪

局部规划器只输出：

/path

真正产生速度的是：

local_planner/src/pathFollower.cpp

节点运行频率为：

$f=100\text{ Hz}.$

8.1 前视点选择

设当前机器人相对路径起点的位置为：

$x_v,y_v),$

路径点为：

$x_i,y_i).$

距离：

$d_i =\sqrt{ (x_i-x_v)^2+(y_i-y_v)^2 }.$

程序不断向前推进路径索引，直到：

$d_i\ge L_d,$

其中默认：

$L_d=0.5\text{ m}.$

这个点作为当前前视目标。

8.2 期望方向

前视方向：

$\theta_p =\operatorname{atan2} (y_i-y_v,x_i-x_v).$

航向误差：

$e_\psi =\operatorname{wrap} ( \psi-\psi_0-\theta_p ).$

这里 $ψ0\psi_0$ 是收到该局部路径时记录的机器人航向。

8.3 航向控制

正常运动时：

$\omega_z =-k_\psi e_\psi.$

速度较低时使用另外一个增益：

$\omega_z =-k_{\psi,stop}e_\psi.$

同时限制：

$|\omega_z| \le \omega_{\max}.$

默认 Launch 中：

yawRateGain: 1.5
stopYawRateGain: 1.5
maxYawRate: 80 deg/s

8.4 线速度加速度限制

目标速度为：

$v_d=v_{\max}u_{\text{joy}},$

控制循环 100 Hz，因此每周期最大变化：

$\Delta v =\frac{a_{\max}}{100}.$

如果：

$v<v_d,$

则：

$v_{k+1} =v_k+\frac{a_{\max}}{100}.$

反之逐步减速。

这是一种简单直接的离散速度斜坡限制。

8.5 接近终点减速

设局部路径终点距离为 $d_e$ ，减速距离阈值为 $d_s$ 。

当：

$\frac{d_e}{d_s}<u_{\text{joy}},$

目标速度按距离比例缩小：

$v_d \leftarrow v_d \frac{d_e}{d_s}.$

所以机器人不会保持最大速度直接冲向 waypoint。

8.6 全向速度输出

最终速度转换为：

$v_x =v\cos(e_\psi),$

$v_y =-v\sin(e_\psi),$

$\omega_z =-k_\psi e_\psi.$

发布：

/cmd_vel

实机模式下，同样的三个量继续通过：

sport_req.Move(req, vx, vy, vyaw);

转换为：

/api/sport/request

发送给 Go2。

9. FAR Planner：可见图长距离路由规划

如果只运行：

./system_real_robot.sh

系统主要工作在基础自主导航模式，waypoint 通常需要设置在机器人附近。

如果运行：

./system_real_robot_with_route_planner.sh

则会额外启用 FAR Planner。

FAR Planner 负责解决一个更高层的问题：

目标点在局部感知范围之外时，机器人应该通过哪些拓扑节点逐步接近目标？

图 2　启用 FAR Planner 后的 RViz 界面与在线 Visibility Graph

9.1 为什么采用 Visibility Graph

FAR 不在整个空间内维护高分辨率的全局二维栅格，而是从障碍物轮廓中提取具有导航意义的节点，再建立可见关系。

定义图：

$\mathcal{G} =(\mathcal{V},\mathcal{E}),$

其中：

$V\mathcal{V}$ ：障碍物轮廓节点、机器人节点、目标节点；
$E\mathcal{E}$ ：两节点之间满足可见性和碰撞约束时建立的连接边。

边代价通常采用欧氏距离：

$c_{ij} =\| \mathbf{p}_i-\mathbf{p}_j \|_2.$

9.2 FAR 的在线处理流程

源码 far_planner.cpp 的主循环顺序非常清晰：

Terrain / Scan Cloud
        ↓
Contour Detector
        ↓
提取障碍轮廓
        ↓
Contour Graph
        ↓
轮廓节点匹配
        ↓
Dynamic Graph
        ↓
更新全局 Visibility Graph
        ↓
Graph Planner
        ↓
搜索到 Goal 的路径
        ↓
选择 Next Waypoint
        ↓
/way_point

其中 main_run_freq 默认：

$5\text{ Hz}.$

因此它是一个低频路由层，不承担 100 Hz 的实时底盘避障。

9.3 图搜索

GraphPlanner::UpdateGraphTraverability() 从当前 odometry 节点开始维护：

$g (s) = 0.$

对相邻节点：

$=\min \left[ g(v), g(u)+c(u,v) \right].$

源码使用按 gscore 排序的优先队列进行扩展，因此这里本质上属于 Dijkstra / Uniform-Cost Search 风格的图遍历。

它不仅计算普通可达状态，还单独维护：

is_free_traversable
fgscore
free_parent

用于“只走已知自由空间”的规划模式。

9.4 Free Navigation 与 Attemptable Navigation

这是 FAR Planner 很有特点的地方。

Free-space navigation

只允许利用已经被传感器覆盖并确认可通行的图节点。

可以写成：

$Vfree={v∈V∣v.is_covered=true}. \mathcal{V}_{free} =\{ v\in\mathcal{V} \mid v.\text{is\_covered}=true \}.$

规划：

$P_free=arg⁡min⁡P⊆Gfree∑(i,j)∈Pcij. P^ \_{free}=\arg\min_{P\subseteq\mathcal{G}_{free}}\sum_{(i,j)\in P}c_{ij}.$

优点是保守。

Attemptable navigation

当已知自由空间不能连接目标时，允许利用未知区域进行尝试。

其核心不是认为未知区域一定安全，而是：

允许高层规划向未知方向推进，新的传感器数据到来后继续更新可见图，再决定下一步。

这使系统适合在线探索式导航。

根目录 README 中 RViz 颜色也对应这一设计：

绿色：已知自由空间路径；
蓝色：考虑未知空间的 attemptable path。

9.5 FAR Planner 为什么只输出 waypoint

FAR 并不直接控制 Go2。

它最终将图路径中的下一个导航节点发布到：

/way_point

于是形成：

$\text{Goal} \rightarrow \text{FAR Route} \rightarrow \text{Next Waypoint} \rightarrow \text{Local Planner} \rightarrow \text{Path Follower}.$

这种分层设计可以把 FAR 的全局拓扑规划和高频局部避障彻底解耦。

即使全局路径当前有效，只要局部突然出现障碍，localPlanner 仍会基于最新 /terrain_map 重新挑选轨迹。

10. FAR Planner 与局部规划器的关系

这是理解整个项目最重要的一点。

二者不是“两个并列规划器”。

它们分别处理不同空间尺度。

模块	空间尺度	输入	输出	频率
FAR Planner	长距离 / 全局拓扑	Goal、terrain、scan、odom	`/way_point`	5 Hz
localPlanner	机器人附近	waypoint、terrain map	`/path`	点云触发，100 Hz 主循环
pathFollower	控制层	`/path`、odom	`cmd_vel / Sport API`	100 Hz

所以可以把整个导航任务理解为：

$\mathbf{g}_{global} \xrightarrow{\text{FAR}} \mathbf{g}_{local} \xrightarrow{\text{Trajectory Library}} \mathbf{P}_{local} \xrightarrow{\text{Follower}} \mathbf{u}.$

11. 三种运行模式

项目在基础自主导航层设计了三种模式。

11.1 Smart Joystick Mode

用户决定希望向哪个方向移动，但局部规划器仍然执行碰撞检测。

Joystick Direction
      ↓
localPlanner
      ↓
Collision-aware path
      ↓
pathFollower

因此它并不是直接遥控。

11.2 Waypoint Mode

用户或 FAR Planner 设置：

/way_point

局部规划器计算避障轨迹并自动前进。

FAR Planner 工作时，基础系统默认运行在这个模式。

11.3 Manual Mode

手柄直接决定：

$v_x,\quad v_y,\quad\omega_z,$

不执行局部碰撞规避。

这个模式主要用于人工接管。

图 3　RViz 控制面板与 PS3 手柄操作方式

12. 仿真系统

项目仿真不是 Gazebo，而是：

Unity + ROS-TCP-Endpoint + ROS 2

运行：

./system_simulation_with_route_planner.sh

脚本会先启动：

src/base_autonomy/vehicle_simulator/mesh/unity/environment/Model.x86_64

等待 3 秒后再执行：

ros2 launch vehicle_simulator system_simulation_with_route_planner.launch

总 Launch 中包含：

joy
local_planner
terrain_analysis
terrain_analysis_ext
vehicle_simulator
sensor_scan_generation
visualization_tools
ros_tcp_endpoint
sim_image_repub
far_planner

ROS-TCP-Endpoint 默认：

0.0.0.0:10000

负责 Unity 和 ROS 2 之间的数据交互。

图 4　基础自主导航仿真运行时的 RViz 界面

13. 软件依赖

13.1 ROS 2 与系统环境

项目根 README 给出的测试环境包括：

ROS 2 Foxy；
ROS 2 Humble；
Go2 板载端 Ubuntu 20.04 + ROS 2 Foxy；
仿真可使用项目给出的 Foxy/Humble 配置。

从工程实践角度，更推荐：

Ubuntu 20.04 → ROS 2 Foxy
Ubuntu 22.04 → ROS 2 Humble

以尽量贴近 ROS 2 各发行版的标准支持环境。

13.2 根 README 中直接要求的系统依赖

Foxy：

sudo apt update
sudo apt install \
  libusb-dev \
  ros-foxy-perception-pcl \
  ros-foxy-sensor-msgs-py \
  ros-foxy-tf-transformations \
  ros-foxy-joy \
  ros-foxy-rmw-cyclonedds-cpp \
  ros-foxy-rosidl-generator-dds-idl \
  python3-colcon-common-extensions \
  python-is-python3

pip install transforms3d pyyaml

Humble：

sudo apt update
sudo apt install \
  libusb-dev \
  ros-humble-perception-pcl \
  ros-humble-sensor-msgs-py \
  ros-humble-tf-transformations \
  ros-humble-joy \
  ros-humble-rmw-cyclonedds-cpp \
  ros-humble-rosidl-generator-dds-idl \
  python3-colcon-common-extensions \
  python-is-python3

pip install transforms3d pyyaml

外部计算机如果还需要 Go2 H.264 视频：

sudo apt install \
  gstreamer1.0-plugins-bad \
  gstreamer1.0-libav

13.3 源码层核心库

根据各包的 package.xml 和源码包含关系，主要依赖如下：

类别	依赖
ROS 2	`rclcpp`、`rclpy`、`ament_cmake`、`colcon`
消息	`std_msgs`、`sensor_msgs`、`nav_msgs`、`geometry_msgs`、`visualization_msgs`
点云	PCL、`pcl_ros`、`pcl_conversions`
坐标变换	`tf2`、`tf2_ros`、`tf2_geometry_msgs`、`tf2_sensor_msgs`
数学	Eigen3
图像	OpenCV4
同步	`message_filters`
DDS	CycloneDDS
仿真通信	ROS-TCP-Endpoint
机器人接口	`unitree_api`、`unitree_go`、Go2 Sport API
LIO 内部	IKFoM、ikd-Tree
Python	`transforms3d`、`PyYAML`

其中 IKFoM、ikd-Tree、Unitree 消息和部分 JSON 头文件已经包含在仓库中。

14. 仿真复现步骤

14.1 克隆

git clone https://github.com/jizhang-cmu/autonomy_stack_go2.git
cd autonomy_stack_go2

建议确认当前分支：

git branch --show-current

目标应为：

foxy-humble

14.2 编译

colcon build \
  --symlink-install \
  --cmake-args -DCMAKE_BUILD_TYPE=Release

编译完成：

source install/setup.bash

14.3 下载 Unity 场景

根据根 README，需要把官方提供的 Unity 环境模型放到：

src/base_autonomy/vehicle_simulator/mesh/unity/

目录大致为：

mesh/
└── unity/
    └── environment/
        ├── Model_Data/
        ├── Model.x86_64
        ├── UnityPlayer.so
        ├── Dimensions.csv
        ├── Categories.csv
        ├── map.ply
        ├── object_list.txt
        ├── traversable_area.ply
        ├── map.jpg
        └── render.jpg

14.4 只运行基础自主导航

./system_simulation.sh

适合研究：

terrain analysis；
local planner；
smart joystick；
waypoint navigation；
collision avoidance。

14.5 启用 FAR Planner

./system_simulation_with_route_planner.sh

在 RViz 使用：

Goalpoint

设置远距离目标。

如果只测试局部 planner，不需要启动 FAR Planner。

14.6 单独发布 waypoint

source install/setup.bash
ros2 launch waypoint_example waypoint_example.launch

该节点还会发送：

navigation boundary；
speed。

15. Go2 实机部署

15.1 硬件前提

项目明确要求：

Go2 EDU

原因是需要 SDK 支持。

基础导航只使用：

Go2 L1 LiDAR；
L1 内部 IMU。

图 5　项目给出的 Go2 端和控制站硬件配置

图 6　官方 README 展示的完整辅助硬件

15.2 板载运行

Go2 板载计算机使用：

Ubuntu 20.04
ROS 2 Foxy

安装依赖并编译后即可运行。

15.3 外部计算机运行

项目推荐外部计算机使用：

Ubuntu 20.04 + ROS 2 Foxy

外部电脑通过 Go2 后方以太网接口连接。

README 推荐：

PC IP   : 192.168.123.100
Netmask : 255.255.255.0
Gateway : 192.168.123.1

并按照 Unitree ROS 2 文档配置 CycloneDDS。

启动系统前要先：

source unitree_ros2_setup.sh

然后检查：

ros2 topic list

确认可以看到 Go2 DDS 话题。

16. IMU 标定

项目要求每台 Go2 对 L1 内部 IMU 做一次标定。

运行：

source install/setup.bash
ros2 run calibrate_imu calibrate_imu

README 给出的操作过程为：

Go2 站立；
踏步约 2 s；
静止约 10 s；
原地旋转约 20 s。

标定结果保存为：

~/Desktop/imu_calib_data.yaml

系统启动时自动读取。

不要随意修改：

文件名；
保存目录。

图 7　Go2 启动与 L1 IMU 标定流程

17. 实机运行命令

17.1 基础自主导航

./system_real_robot.sh

启动：

Point-LIO
terrain_analysis
terrain_analysis_ext
localPlanner
pathFollower

17.2 带 FAR Planner 的完整导航

./system_real_robot_with_route_planner.sh

该脚本本身非常简单：

source ./install/setup.bash
ros2 launch vehicle_simulator \
  system_real_robot_with_route_planner.launch

总 Launch 进一步启动：

joy
Point-LIO
local_planner
terrain_analysis
terrain_analysis_ext
FAR Planner
TF

17.3 启动 Go2 相机

source install/setup.bash
ros2 run go2_h264_repub go2_h264_repub

输出：

/camera/image/raw

相机不是当前自主导航主链的必要传感器。

17.4 记录 L1 数据

ros2 bag record \
  /utlidar/cloud \
  /utlidar/imu

离线回放：

ros2 bag play bagfile_name.db3

如果在外部电脑回放实机 bag，README 特别提醒：

不要同时通过网线连接 Go2，否则实时传感器数据和 bag 数据可能同时进入系统。

图 8　项目 README 展示的实机碰撞规避效果

18. 系统关键参数

18.1 Local Planner

默认 Launch 中值得重点调整的参数：

参数	默认值	含义
`vehicleLength`	0.3 m	碰撞模型长度
`vehicleWidth`	0.7 m	碰撞模型宽度
`adjacentRange`	3.0 m	局部规划范围
`obstacleHeightThre`	0.3 m	障碍物高度阈值
`groundHeightThre`	0.1 m	地面/低障碍阈值
`pointPerPathThre`	2	路径碰撞点阈值
`pathScale`	0.75	候选路径尺度
`minPathScale`	0.5	最小路径尺度
`maxSpeed`	1.0 m/s	最大速度
`dirWeight`	0.02	方向代价权重
`goalClearRange`	0.5 m	目标附近障碍裁剪余量

18.2 Path Follower

参数	默认值
`lookAheadDis`	0.5 m
`yawRateGain`	1.5
`maxYawRate`	80 deg/s
`maxAccel`	2.0 m/s²
`stopDisThre`	0.3 m
`slowDwnDisThre`	0.75 m
`goalCloseDis`	0.4 m

18.3 Terrain Analysis

参数	默认值
`scanVoxelSize`	0.05 m
`decayTime`	2.0 s
`quantileZ`	0.25
`noDataObstacle`	true
`minBlockPointNum`	10
`maxElevBelowVeh`	-0.6 m
`vehicleHeight`	1.5 m

18.4 FAR Planner

默认 default.yaml：

参数	默认值
`main_run_freq`	5 Hz
`voxel_dim`	0.1 m
`robot_dim`	0.6 m
`sensor_range`	10.0 m
`terrain_range`	7.5 m
`local_planner_range`	2.5 m
`is_attempt_autoswitch`	true
`world_frame`	`map`

这些参数决定 FAR 的感知范围、图更新范围和 free/attemptable 模式切换策略。

19. 数据流的完整一次闭环

假设用户在 RViz 中点一个远距离 Goal。

Step 1：Goal 进入 FAR

/goal_point

FAR 把目标加入 Visibility Graph。

Step 2：FAR 更新图

通过：

/state_estimation
/terrain_map
/terrain_map_ext
/registered_scan

更新轮廓与图连接。

Step 3：图搜索

得到：

$P_G =\{ v_0,v_1,\ldots,v_n \}.$

Step 4：选择下一导航点

FAR 不把整条路线直接交给局部控制，而是发布：

/way_point

Step 5：局部轨迹筛选

localPlanner 根据当前地形点：

查询 voxel-to-path correspondence；
排除碰撞轨迹；
计算地形软代价；
计算方向评分；
选择最佳路径组。

输出：

/path

Step 6：Path Follower

计算：

$e_\psi, \quad v_x, \quad v_y, \quad \omega_z.$

Step 7：Go2 Sport API

生成：

/api/sport/request

Step 8：机器人运动

Go2 自带低层运动控制器完成：

步态；
足端轨迹；
姿态稳定；
关节执行。

Step 9：Point-LIO 更新

机器人产生新的 LiDAR/IMU 数据，再回到 Step 1～8 的闭环。

20. 这个项目是否需要先验地图

不需要。

这是该项目很重要的特点之一。

Point-LIO 在线建立局部/全局点云地图；

FAR Planner 在线构建 Visibility Graph；

机器人向未知区域推进时继续感知并更新图。

因此完整流程属于：

无先验地图的在线 SLAM + 在线规划导航。

但“不需要先验地图”不等于“不维护地图”。

它仍然会在线维护：

Point-LIO 点云地图；
terrain map；
FAR Visibility Graph。

21. 它是不是完整的四足机器人导航栈

如果“完整导航栈”指：

传感器
→ 定位
→ 地形
→ 全局规划
→ 局部避障
→ 路径跟踪
→ 四足机器人速度接口

答案是：

是。

如果“完整”进一步要求：

足端落点规划
→ 接触序列规划
→ MPC/WBC
→ 关节力矩控制

那么答案是：

不是。

这个项目依赖 Unitree 自带 Sport Mode 来完成底层 locomotion。

换句话说：

autonomy_stack_go2
负责：
“机器人机身下一步应该往哪里走”

Unitree 内部控制器
负责：
“十二个关节具体怎么动”

22. 能不能直接规划上下楼梯

从源码结构看，这套系统没有显式的楼梯识别、足端落点优化、接触序列规划或楼梯专用 locomotion planner。

地形分析主要给出：

局部地面；
点相对地面的高差；
未观测区域；
地形连通性。

Local Planner 仍以机身轨迹和点云碰撞作为主要决策依据。

因此：

它能够利用 Go2 本体的运动能力通过一定坡度或复杂地形，但“导航栈能够生成路线”与“机器人一定能够稳定爬某种楼梯”不是同一件事。

对于规则楼梯，如果台阶在 terrain_analysis 中被持续判定为高障碍或无数据区域，局部规划器可能直接拒绝该方向。

如果需要稳定多层楼梯导航，更合理的扩展是：

Point-LIO
   ↓
3D / Multi-layer Global Planner
   ↓
Terrain Classification / Stair Recognition
   ↓
Locomotion-aware Local Planner
   ↓
Go2 locomotion controller

或者将 FAR 替换/并联为更适合多层拓扑和三维通行性的全局规划器。

23. 与 Nav2 的差别

对比项	autonomy_stack_go2	Nav2 常见方案
地图	3D 点云 + terrain map + visibility graph	2D Occupancy Grid / Costmap
全局规划	FAR Visibility Graph	NavFn / Smac
局部规划	预计算轨迹库	DWB / TEB / MPPI Controller
障碍表示	点云相对地面高差	costmap cell cost
控制接口	Unitree Sport API	`cmd_vel`
未知空间	FAR attemptable navigation	通常由 costmap unknown policy 处理
四足适配	直接面向 Go2	通用移动底盘

所以这个项目更适合研究：

三维未知环境导航；
四足机器人导航；
基于点云的局部避障；
无先验地图探索式导航。

而不是传统二维室内轮式机器人导航。

24. 项目的主要工程贡献

24.1 把多个成熟模块真正接成了 Go2 可运行系统

项目最有价值的地方不是提出一个全新的单一算法，而是完成：

L1 LiDAR
+ Point-LIO
+ Terrain Analysis
+ FAR Planner
+ Trajectory Library Planner
+ Unitree Sport API

之间的完整接口适配。

24.2 只依赖 Go2 内置感知

没有要求额外的：

Livox；
Velodyne；
Realsense；
RTK；
Motion Capture。

这降低了实机复现门槛。

24.3 全局拓扑规划与高频局部避障解耦

FAR 的 5 Hz 图搜索不需要承担高速避障。

Local Planner 和 Path Follower 保持 100 Hz 主循环。

这一层级划分很适合移动机器人系统工程。

24.4 局部规划计算开销可控

利用：

offline path library
+
offline voxel-path correspondence

把大量在线几何碰撞计算提前到了离线阶段。

24.5 仿真—实机接口基本一致

仿真和实机共用：

terrain analysis；
local planner；
path follower；
FAR Planner；
RViz 操作逻辑。

主要差异只在：

仿真传感器/状态由 Unity 和 vehicle simulator 提供；
实机状态由 Point-LIO 提供；
实机速度额外发送到 Unitree Sport API。

25. 项目局限与需要注意的地方

25.1 Point-LIO 子目录 README 已经落后于当前总仓库

point_lio_unilidar/README.md 仍然描述 ROS 1 Noetic + catkin_make。

但是当前总仓库中：

<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>

总 Launch 也通过 ROS 2 调起。

因此复现整个 autonomy_stack_go2 时，应以：

根目录 README
+
foxy-humble 当前源码

为准，而不是照搬 Point-LIO 子目录旧 README 的 ROS 1 命令。

25.2 不是 Nav2 插件化框架

如果希望替换 planner，需要自己处理：

topic；
frame；
waypoint；
terrain map；
boundary；
speed command。

不能直接把一个 Nav2 plugin XML 塞进去就完成替换。

25.3 本身没有腿式接触规划

复杂台阶、碎石和高落差的真正可通过性仍受 Go2 本体步态能力限制。

25.4 Terrain 参数高度依赖机器人与场景

例如：

obstacleHeightThre: 0.3
vehicleWidth: 0.7
adjacentRange: 3.0

这些参数不能机械照搬到其他四足机器人。

更换机器人后至少需要重新标定：

footprint；
LiDAR 安装位置；
地面高差阈值；
最大速度；
waypoint 距离；
local planner range。

25.5 FAR 的“attemptable”不是无条件穿越未知空间

它只是允许路由层向未知区域推进。

真正执行时仍然会经过：

terrain analysis
+
local collision checking

因此高层“尝试”与底层“安全执行”是两层逻辑。

26. 如果要改造成自己的四足机器人导航框架

这个项目非常适合作为系统骨架。

可以把接口理解成：

Localization:
    输出 /state_estimation
    输出 /registered_scan

Terrain:
    输入 /registered_scan
    输出 /terrain_map

Global Planner:
    输入 Goal + Map + Odom
    输出 /way_point

Local Planner:
    输入 /way_point + /terrain_map
    输出 /path

Controller:
    输入 /path
    输出 vx, vy, wz

因此可以逐模块替换。

例如：

Point-LIO
    ↓
PCT Planner
    ↓
SCAN-Planner / 自定义地形局部规划
    ↓
Go2 Sport API

也可以保留 FAR，只替换局部规划器：

Point-LIO
    ↓
FAR Planner
    ↓
/way_point
    ↓
你的 Local Planner
    ↓
cmd_vel

真正需要保证的不是“算法名称一致”，而是：

坐标系一致；
topic 语义一致；
高层输出满足低层接口；
局部规划器仍具有实时碰撞保护。

27. 推荐的阅读源码顺序

如果准备二次开发，不建议从 FAR 的所有 C++ 文件开始硬看。

推荐顺序：

第一阶段：总系统

README.md
system_real_robot_with_route_planner.sh
vehicle_simulator/launch/system_real_robot_with_route_planner.launch

先理解节点连接关系。

第二阶段：局部导航

local_planner/launch/local_planner.launch
local_planner/src/localPlanner.cpp
local_planner/src/pathFollower.cpp

这是最容易直接修改和验证的部分。

第三阶段：地形

terrain_analysis/src/terrainAnalysis.cpp
terrain_analysis_ext/src/terrainAnalysisExt.cpp

重点看：

planarVoxelElev
intensity
noDataObstacle
terrain connectivity

第四阶段：全局路由

far_planner/src/far_planner.cpp
far_planner/src/graph_planner.cpp
far_planner/src/dynamic_graph.cpp
far_planner/src/contour_graph.cpp
far_planner/src/contour_detector.cpp

第五阶段：定位

point_lio_unilidar/src/Estimator.h
point_lio_unilidar/src/Estimator.cpp
point_lio_unilidar/src/laserMapping.cpp
point_lio_unilidar/include/ikd-Tree/
point_lio_unilidar/include/IKFoM/

最后再研究滤波器内部数学细节。

28. 一套推荐的实机启动检查顺序

在真正让 Go2 走起来之前，建议逐层验证。

1. DDS

ros2 topic list

确认能收到 Unitree topic。

2. LiDAR

检查：

/utlidar/cloud
/utlidar/imu

3. 传感器变换

检查：

/utlidar/transformed_cloud
/utlidar/transformed_imu

4. Point-LIO

检查：

/state_estimation
/registered_scan

在 RViz 中确认点云不会明显漂移。

5. Terrain Analysis

检查：

/terrain_map
/terrain_map_ext

重点确认地面 intensity 接近 0，高障碍 intensity 明显增大。

6. Local Planner

手动给一个近距离：

/way_point

检查：

/path
/free_paths

7. Path Follower

先架空机器人或限速测试：

/cmd_vel

8. Go2 Sport API

最后才允许：

/api/sport/request

真正驱动机器人。

9. FAR Planner

局部闭环稳定以后，再启动：

./system_real_robot_with_route_planner.sh

测试远距离目标。

这样的调试顺序比“一次启动整个系统再找问题”高效得多。

29. 总结

autonomy_stack_go2 的核心价值不是某一个孤立算法，而是形成了一条完整的四足机器人自主导航链：

$\boxed{ \text{LiDAR + IMU} \rightarrow \text{Point-LIO} \rightarrow \text{Terrain Analysis} \rightarrow \text{FAR Planner} \rightarrow \text{Local Trajectory Library} \rightarrow \text{Path Follower} \rightarrow \text{Go2 Sport API} }$

其中：

Point-LIO 解决“机器人在哪里”；
Terrain Analysis 解决“附近地形是什么”；
FAR Planner 解决“远距离应该往哪里走”；
Local Planner 解决“眼前应该走哪条安全轨迹”；
Path Follower 解决“怎样把轨迹转换成机身速度”；
Unitree Sport API 解决“怎样把机身速度真正交给 Go2 执行”。

从源码实现看，这套系统并不是传统 Nav2 的二维导航框架，而是一套围绕三维 LiDAR、地形高差和在线 Visibility Graph 设计的自主导航系统。对于希望研究 Go2 无先验地图导航、四足机器人局部避障、FAR Planner、Point-LIO 或自定义全局/局部规划器接入 的开发者，这个仓库具有较高的参考价值。

30. 主要源码索引

项目入口

项目主页：
https://github.com/jizhang-cmu/autonomy_stack_go2
根 README：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/README.md

系统 Launch

实机 + Route Planner：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/src/base_autonomy/vehicle_simulator/launch/system_real_robot_with_route_planner.launch
仿真 + Route Planner：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/src/base_autonomy/vehicle_simulator/launch/system_simulation_with_route_planner.launch

Local Planner

localPlanner.cpp：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/src/base_autonomy/local_planner/src/localPlanner.cpp
pathFollower.cpp：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/src/base_autonomy/local_planner/src/pathFollower.cpp
local_planner.launch：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/src/base_autonomy/local_planner/launch/local_planner.launch

Terrain

terrainAnalysis.cpp：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/src/base_autonomy/terrain_analysis/src/terrainAnalysis.cpp
terrainAnalysisExt.cpp：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/src/base_autonomy/terrain_analysis_ext/src/terrainAnalysisExt.cpp

FAR Planner

far_planner.cpp：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/src/route_planner/far_planner/src/far_planner.cpp
graph_planner.cpp：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/src/route_planner/far_planner/src/graph_planner.cpp
default.yaml：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/src/route_planner/far_planner/config/default.yaml

Point-LIO

Estimator.h：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/src/slam/point_lio_unilidar/src/Estimator.h
Estimator.cpp：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/src/slam/point_lio_unilidar/src/Estimator.cpp
utlidar.yaml：
https://github.com/jizhang-cmu/autonomy_stack_go2/blob/foxy-humble/src/slam/point_lio_unilidar/config/utlidar.yaml

Unitree Go2

go2_sport_api：
https://github.com/jizhang-cmu/autonomy_stack_go2/tree/foxy-humble/src/utilities/unitree_pkgs/go2_sport_api
unitree_api：
https://github.com/jizhang-cmu/autonomy_stack_go2/tree/foxy-humble/src/utilities/unitree_pkgs/unitree_api
unitree_go：
https://github.com/jizhang-cmu/autonomy_stack_go2/tree/foxy-humble/src/utilities/unitree_pkgs/unitree_go

DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

现场换下来的旧件，为什么不能直接放回备件箱？

机器人现场换完一个部件，报警消失，任务也重新跑通。新件留在机器人上，换下来的旧件被顺手装进袋子，放到一旁。有人问了一句：“这个已经确认坏了吗？现场常见的回答是：“换完以后正常了，应该就是它。可过几天再打开那个袋子，问题往往更多：它从哪台机器人、哪个位置拆下来？当时是什么现象？有没有单独确认过？现在是故障件、待分析件，还是还能继续使用的备件？如果这些都说不清，它就会变成一个只知道“被拆下来”，却不知

DAMO开发者矩阵

《VLA 系列》MEM | 多尺度具身记忆 | 15 分钟长任务 | 论文解析

DAMO开发者矩阵

RynnVLA-002 复现工作记录

配套目录：0. 目标态与验收标准重建成功需同时满足：权重约 70GB（A–F）+ libero_goal hdf5 ~6GB + 输出数 GB；建议数据盘空闲 ≥100GB。评测环境变量：安装 Miniconda 到（若无）。确认为 12.8。2. 获取代码与重建制品必须同步本机与本手册到新机同路径（官方仓库不含这些文件）。若制品在数据盘根目录，执行：权重建议另下或，不要打进小 tar。3.2

DAMO开发者矩阵

所有评论(0)

查看更多评论

无宜

@qq_56908984

已为社区贡献39条内容

开源 | 四足机器人完整导航栈：Autonomy Stack for Unitree Go2【项目解读】

无宜

1. 项目解决的是什么问题

2. 项目的核心意义

3. 仓库整体结构

3.1 base_autonomy

3.2 route_planner

3.3 slam

3.4 utilities

4. 系统输入、输出与 ROS 话题关系

5. Point-LIO：LiDAR-IMU 状态估计

5.1 为什么使用 Point-LIO

5.2 IMU 状态传播

5.3 点到平面残差

5.4 Go2 L1 配置

6. 地形分析：从三维点云得到可通行性信息

6.1 滚动体素地图

6.2 地面高度估计

6.3 相对地面高差

$$ h_i

6.4 未观测区域处理

7. 局部规划器：预计算轨迹库，而不是 DWA/TEB

7.1 候选路径库

7.2 36 个旋转搜索方向

7.3 路径缩放与旋转

7.4 为什么需要 correspondences.txt

7.5 障碍碰撞判定

7.6 地形软代价

7.7 方向评分函数

(r∗,g∗)=arg maxr,g Sr,g (r^*, g^*) = \underset{r,g}{\mathrm{arg\,max}}\; S_{r,g} (r∗,g∗)=r,gargmax​Sr,g​

7.8 这个局部规划器的优势

计算量稳定

直接使用三维点云

对动态障碍响应快

候选路径具有运动连续性

7.9 局限

8. Path Follower：前视点路径跟踪

8.1 前视点选择

8.2 期望方向

8.3 航向控制

8.4 线速度加速度限制

8.5 接近终点减速

8.6 全向速度输出

9. FAR Planner：可见图长距离路由规划

9.1 为什么采用 Visibility Graph

9.2 FAR 的在线处理流程

9.3 图搜索

9.4 Free Navigation 与 Attemptable Navigation

Free-space navigation

Attemptable navigation

9.5 FAR Planner 为什么只输出 waypoint

10. FAR Planner 与局部规划器的关系

11. 三种运行模式

11.1 Smart Joystick Mode

11.2 Waypoint Mode

11.3 Manual Mode

12. 仿真系统

13. 软件依赖

13.1 ROS 2 与系统环境

13.2 根 README 中直接要求的系统依赖

13.3 源码层核心库

14. 仿真复现步骤

14.1 克隆

14.2 编译

14.3 下载 Unity 场景

14.4 只运行基础自主导航

14.5 启用 FAR Planner

14.6 单独发布 waypoint

15. Go2 实机部署

15.1 硬件前提

15.2 板载运行

15.3 外部计算机运行

16. IMU 标定

17. 实机运行命令

17.1 基础自主导航

17.2 带 FAR Planner 的完整导航

17.3 启动 Go2 相机

17.4 记录 L1 数据

18. 系统关键参数

18.1 Local Planner

3.1 `base_autonomy`

3.2 `route_planner`

3.3 `slam`

3.4 `utilities`

$$
h_i

7.4 为什么需要 `correspondences.txt`

$(r^, g^) = \underset{r,g}{\mathrm{arg\,max}}\; S_{r,g}$