第 14 章 仿真中的 3D SLAM 与 Nav2 升级¶
第 13 章我们把 Go2 在 Gazebo 里跑了起来,也把 /velodyne_points 和 /imu/data 这两路关键传感器喂通了。但那还只是"能走能扫"。这一章,我们正式把它升级成一条完整的 3D 感知链:先用 FAST-LIO 建图,再让 OctoMap 和 STVL 把 3D 信息送进 Nav2。
本章你将学到¶
比较 KISS-ICP、Point-LIO、FAST-LIO 三条 3D LIO 路线,明白为什么本书在仿真里选 FAST-LIO
看懂 FAST_LIO_ROS2/config/velodyne.yaml 里最关键的 6 个参数:lid_topic、imu_topic、extrinsic_T、extrinsic_R、scan_line、time_sync_en
按当前仿真代码的真实话题名跑通 FAST-LIO:/velodyne_points + /imu/data + /Odometry + /cloud_registered
把 3D 点云地图变成 Nav2 能消费的代价地图,理解 OctoMap 负责静态全局图,STVL 负责动态局部层
用一套更贴近真实工程的思路验证 3D 导航:桌面障碍、坡道、楼梯口都应该被"看见",即使 CHAMP 本身不会爬楼梯
背景与原理¶
从第 13 章走到这一章,差的到底是什么¶
第 13 章解决的是仿真底座:
Go2 能在 Gazebo 里稳定站立和移动
VLP-16 点云已经能通过 /velodyne_points 发布
Gazebo IMU 已经能通过 /imu/data 发布
use_slam:=true 这条开关也已经准备好,可以把 CHAMP 自带的 EKF 里程计先让出来
这一章要补上的,是另外两层能力:
3D 里程计与建图:机器人不能只会"扫",还得能把多帧点云叠起来,知道自己走到了哪里
3D 地图到导航的桥:Nav2 规划器本质上还是在平面上工作,所以你得把 3D 世界整理成它吃得下的形式
别小看第二层。很多人第一次做 3D 导航时最容易犯的错,就是以为"有了 3D 地图 = Nav2 自动会 3D 导航"。并没有。现实工程更常见的做法是:
前端感知和建图保留 3D
规划和控制继续消费 2D 或 2.5D 代价地图
这不是退而求其次,而是目前 ROS2 生态里最稳的一条路。
三条 3D LIO 路线怎么选¶
这一章我们只比较本项目里真的走过的三条线,不搞百科全书式横评。
方案
是否紧耦合
对 IMU 品质要求
典型强项
当前结论
KISS-ICP
否,纯几何 ICP
低
配置最少,上手快
Go2 这种快起快停的小型四足里容易漂
Point-LIO
是
很高
运动响应快,理论上很适合 LiDAR+IMU
对 IMU 契约和时间语义太敏感,前面在 L1 上已经吃过大亏
FAST-LIO / FAST-LIO2
是
中到高
iEKF + ikd-Tree 直接配准,退化场景更稳
这章主线就选它
把它们再说得更直白一点:
KISS-ICP:最省心,但也最容易在四足上先飘¶
KISS-ICP 的核心思路很朴素:只看点云几何,每一帧和上一帧去做 ICP 匹配。
优点确实明显:
不依赖 IMU
参数少
很适合做"我先看看点云能不能跑"的第一轮验证
但 Go2 这种底盘有一个坏消息:
起步、转弯、摆动都比较急
纯几何匹配没有 IMU 兜底时,漂移会来得很快
所以它更像一条试水路线,不是这章要交付的主线。
Point-LIO:很强,但它对输入契约真的很挑¶
Point-LIO 是紧耦合 LiDAR-IMU 里程计。理论上它反应快,对高速运动也更友好。
问题是,它对这几件事都很敏感:
IMU 的噪声和坐标系是否靠谱
点云逐点时间是否正确
加速度和角速度到底应该怎么喂
前面几周我们已经在实机 L1 路线上踩过很多坑,结论并不暧昧:
静态时好看
一动起来就容易炸
所以这里不是说 Point-LIO 不好,而是说它不适合拿来当这套仿真教程的第一条可复现主线。
FAST-LIO:参数多一点,但在当前仿真里最稳¶
FAST-LIO 同样是紧耦合路线,但它的前端做法和 Point-LIO、LIO-SAM 一类方案不完全一样。它把 LiDAR 特征和 IMU 状态都塞进迭代扩展卡尔曼滤波里,再配上 ikd-Tree 做快速地图匹配。
工程上它有三个很现实的优点:
社区资料最多,ROS2 移植版本也成熟
对 VLP-16 这类多线雷达已经有现成配置模板
在当前这套 Gazebo + 干净 IMU 的仿真里,它比 Point-LIO 和 LiORF 都更稳
当然,它也不是没缺点:
参数不少
外参、时间单位、线数这些契约配错了会直接炸
没有回环检测时,长时间建图还是会累积漂移
但这一章我们要的是先把前端 3D SLAM 和 3D costmap 桥接打通,FAST-LIO 正好最合适。
为什么 3D 地图最后还要服务 2D Nav2¶
这里要先把一个常见误解打掉:
Nav2 不是原生全 3D 路径规划器。
它最核心的 planner / controller 逻辑,仍然围着平面代价地图转。所以这章的正确目标不是"让 Nav2 直接在点云里飞",而是:
用 3D 地图表达复杂空间
再把适合规划的那部分投影或切片成 2D 代价地图
本项目里我们实际考虑过 3 种桥接方案:
方案
你得到什么
优点
缺点
结论
grid_map
2.5D 高程图
功能全,学术资料多
一个 xy 只能放一套高度表达,多层空间天然吃亏
否决
OctoMap
3D 八叉树 + 2D 投影
成熟、Humble 可装、支持多层结构
分辨率开太细会吃 CPU
作为静态全局图
STVL
直接面向 Nav2 的 3D voxel layer
吃 PointCloud2 方便,适合动态障碍
更偏局部层,不是整栋楼世界模型
作为动态局部层
本章最终采用的是这套分工:
OctoMap:把 FAST-LIO 的配准点云累成稳定的 3D 占据图,再投影成 /projected_map
STVL:直接消费原始 /velodyne_points,给 local_costmap 做动态障碍感知
这套"静态 + 动态"两层结构,是这一章最重要的工程价值。
为什么 grid_map 在这里被否了
grid_map 很适合做高程、坡度、接触地形分析,但它本质还是 2.5D。对于"首层大厅上面还有二层平台"这类有 xy 重叠的结构,它天然不如 OctoMap 这种真正 3D 占据表达。
楼梯在这一章的目标不是'爬上去'
CHAMP 这条仿真控制链不会突然因为你接了 3D 地图就学会爬楼梯。这里真正要验证的是:
3D 地图里能不能把楼梯口、台阶前沿看成障碍
Nav2 会不会再把楼梯当成平地硬冲
架构总览¶
先把整条链长什么样看清楚,后面你就不容易被 launch 和 YAML 绕晕。
flowchart LR
A[Gazebo + CHAMP + VLP-16] --> B["/velodyne_points"]
A --> C["/imu/data"]
B --> D[FAST-LIO]
C --> D
D --> E["/Odometry
camera_init -> body"]
D --> F["/cloud_registered"]
D --> G["/cloud_registered_body"]
D --> H["/path"]
E --> I[body -> base_link 静态桥]
I --> J[Nav2 / RViz]
F --> K[octomap_server]
K --> L["/octomap_full"]
K --> M["/projected_map"]
B --> N[STVL]
N --> O[local_costmap]
M --> P[global_costmap]
P --> J
O --> J
这张图里最容易混的有 3 个点:
FAST-LIO 的全局参考系不是 map,而是 camera_init
FAST-LIO 的机体坐标系是 body,而当前 Go2 仿真 URDF 根是 base_link
Nav2 全局层想要的是投影后的 2D 占据图,不是原始点云
所以我们会额外做两件小事:
用第 13 章已经准备好的 slam_tf_bridge 把 body -> base_link 补齐
在接 Nav2 时,先把 map -> camera_init 当成一条初始恒等变换
后面如果你再接回环后端,比如 SC-PGO,就让它接管 map -> camera_init。这也是前面周记里实际走过的那条路线。
环境准备¶
前置成果¶
开始前请确认你已经完成:
第 13 章的 Gazebo 仿真底座
go2_config gazebo_velodyne.launch.py 能正常启动
/velodyne_points 和 /imu/data 都有数据
你知道这套仿真里用的是 base_link,不是第 12 章实机里的 base
先用下面两条命令查一下:
# 先启动第 13 章的仿真底座,确认点云和 IMU 仍然正常
cd ~/go2_sim_ws
source install/setup.bash
ros2 launch go2_config gazebo_velodyne.launch.py use_slam:=true
另开终端:
# 确认关键话题都在
source ~/go2_sim_ws/install/setup.bash
ros2 topic list | grep -E "velodyne_points|imu/data|clock"
# 频率别拍脑袋,直接量
ros2 topic hz /imu/data
ros2 topic hz /velodyne_points
如果一切正常,你应该看到:
/imu/data 约 100 Hz
/velodyne_points 约 10 Hz
/clock 正在发布
新增依赖¶
这章要多装 4 类依赖:
OctoMap
STVL
pcl_viewer 所在工具包
FAST-LIO 自己的源码包
先装系统包:
# OctoMap + STVL + PCD 查看工具
sudo apt install -y \
ros-humble-octomap-ros \
ros-humble-octomap-server \
ros-humble-spatio-temporal-voxel-layer \
pcl-tools
再把 FAST-LIO 放进工作空间:
# 把 FAST-LIO 的 ROS2 版本放进当前教程工作空间
cd ~/go2_sim_ws/src
git clone
关于 Gazebo 的 VLP-16 插件
本书当前本地工程里,还额外放了一份源码版 velodyne_gazebo_plugins,它把每个点的 time 从全 0 修成了"相对扫描起点的时间偏移"。
这对 LiORF / LIO-SAM 一类更依赖 deskew 的算法非常重要。FAST-LIO 没那么脆,但如果你想让仿真和本书当前工程状态更一致,也可以把这份插件源码一起放进工作空间。
如果你也要跟到同一版仿真插件,步骤是:
# 可选:让 Gazebo 的 VLP-16 插件也走源码版,便于检查逐点 time 字段
cd ~/go2_sim_ws/src
git clone
然后把那些默认忽略构建的标记删掉:
# 这三个文件不删, colcon 根本不会编它
rm velodyne_simulator/velodyne_gazebo_plugins/COLCON_IGNORE
rm velodyne_simulator/velodyne_gazebo_plugins/AMENT_IGNORE
rm velodyne_simulator/velodyne_gazebo_plugins/CATKIN_IGNORE
先把"时间"这件事分清楚¶
这一章同时会遇到两种时间概念:
仿真时间:来自 /clock,由 use_sim_time: true 控制
点云逐点时间:点云里每个点相对一帧扫描起点的偏移,由 time 或 t 字段提供
这两者不是一回事。
use_sim_time 没开,TF 和消息时间会乱套
time 字段单位配错,去畸变和配准会变形
所以你要记住一句话:
use_sim_time 解决的是"大家跟谁对表"；timestamp_unit / time_sync_en 解决的是"雷达和 IMU 自己怎么对齐"。
实现步骤¶
步骤一:先把 FAST-LIO 的配置文件改对¶
我们先处理最核心的文件:src/FAST_LIO_ROS2/config/velodyne.yaml。
这份配置里,下面 6 个键必须和当前仿真代码严格对上:
common.lid_topic
common.imu_topic
common.time_sync_en
mapping.extrinsic_T
mapping.extrinsic_R
preprocess.scan_line
把文件整理成下面这样:
/**:
ros__parameters:
feature_extract_enable: false
point_filter_num: 4
max_iteration: 3
filter_size_surf: 0.5
filter_size_map: 0.5
cube_side_length: 1000.0
runtime_pos_log_enable: false
map_file_path: "./test.pcd"
common:
lid_topic: "/velodyne_points" # Gazebo VLP-16 发布的话题
imu_topic: "/imu/data" # Gazebo IMU 发布的话题
time_sync_en: false # 仿真里统一用 /clock,这里不要乱开软件同步
time_offset_lidar_to_imu: 0.0
preprocess:
lidar_type: 2 # 2 = Velodyne
scan_line: 16 # VLP-16 就是 16 线,这个键漏了基本必炸
scan_rate: 10 # 当前 xacro 里 update_rate = 10 Hz
timestamp_unit: 0 # 本仓库 Gazebo 插件的 time 字段按"秒"写入,所以填 0
blind: 0.5
mapping:
acc_cov: 0.1
gyr_cov: 0.1
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
fov_degree: 360.0
det_range: 30.0
extrinsic_est_en: false
extrinsic_T: [0.2, 0.0, 0.118] # 由 xacro 中的固定关节直接读出来
extrinsic_R: [1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0]
publish:
path_en: true
scan_publish_en: true
dense_publish_en: true
scan_bodyframe_pub_en: true
map_en: true
pcd_save:
pcd_save_en: true
interval: -1
filter_size_save: 0.1
ground_filter_z: -0.15
先把最重要的键一个个吃透:
lid_topic 和 imu_topic¶
这两个别凭感觉写。我们已经在第 13 章把真实话题查清楚了:
点云是 /velodyne_points
IMU 是 /imu/data
如果你这里沿用上游模板里的 Livox 话题名,FAST-LIO 就会一直在那里傻等。
scan_line¶
这个键就是告诉 FAST-LIO:你喂给我的到底是几线雷达。
当前仿真里用的是 VLP-16,所以一定是:
scan_line: 16
这个数写错后最常见的现象是:
点云能进来
但内部按错行号组织数据
地图开始扭、飘、或者直接不收敛
time_sync_en¶
这个参数最容易被误解。
它的意思不是"打开后就自动帮你处理所有时间问题",而是:
如果 LiDAR 和 IMU 头时间差比较大
又没有硬件或外部时间同步
那就让 FAST-LIO 做一个很朴素的软件对齐
在当前仿真里,我们的建议是保持 false。原因有两个:
Gazebo 本来就通过 /clock 给整套系统提供统一时间
这个开关更像兜底手段,不是常规首选项
别把 time_sync_en 和 use_sim_time 混为一谈
use_sim_time 是 ROS2 节点要不要跟 /clock 走。
time_sync_en 是 FAST-LIO 在 LiDAR 和 IMU 之间要不要做软件级补偿。
这俩名字里都有 time,但解决的问题完全不同。
timestamp_unit¶
这个键虽然不在本章的"六大主角"名单里,但它实际上一样关键。
它表示:
点云里 time 字段的单位是什么
FAST-LIO 该把它换算成什么量级去做去畸变
这一项最稳的做法不是背模板,而是对着你当前的 PointCloud2 实际字段来确认。
合法取值(由 FAST-LIO 源码决定):
0 = 秒(second)
1 = 毫秒(millisecond)
2 = 微秒(microsecond)
3 = 纳秒(nanosecond)
本书当前用的 velodyne_simulator 源码版 plugin(GazeboRosVelodyneLaser.cpp),time 字段的写入方式是:
数据类型:FLOAT32
数值含义:每个点相对于一帧扫描起点的时间偏移,单位是"秒"
所以本书配套的写法是:
timestamp_unit: 0
这是"代码级"结论,不是"经验之谈"
以上结论直接来自当前仓库 plugin 源码里的 PointField 定义。
如果你后续换了别的 plugin(比如某些实机/第三方插件把 time 按"微秒"或"纳秒"写入),就要对应把 timestamp_unit 改成 2 或 3。换插件必重核。
实测建议
理论上单位对齐后 FAST-LIO 去畸变就正常了。第一次跑通时还是建议你手动录一段 rosbag,在 RViz 里看 /cloud_registered 有没有明显的"拖尾 / 扭动",作为"单位真没配错"的双保险。后面常见问题里我们还会专门再说一次这个坑。
步骤二:从 xacro 里抄出 IMU 契约和外参¶
很多教程一讲到外参就开始说"去标定一下"。这话在真机上没错,但在仿真里先别自己吓自己:
传感器安装位置就是你写进 xacro 的
所以第一版外参,直接从 xacro 里读
先看 IMU:
再看 Gazebo 里的 IMU 插件:
这几行已经把 3 个关键事实说全了:
IMU frame 是 imu_link
IMU topic 是 /imu/data
IMU 发布频率是 100 Hz
再看雷达:
当前这个 Go2 模型里:
base_link -> trunk 是固定零变换
trunk -> imu_link 也是固定零变换
所以第一版你完全可以把 imu_link 看成和 base_link 共点。
于是 LiDAR 相对 IMU 的平移,直接就是:
x = 0.2
y = 0.0
z = 0.08 + 0.0377 = 0.1177
向上取一个教程里好抄的数,就是:
extrinsic_T: [0.2, 0.0, 0.118]
而这套仿真里所有相关固定关节的旋转都是零,所以:
extrinsic_R:
[1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0]
步骤三:启动 Gazebo 和 FAST-LIO¶
先把仿真底座拉起来:
# 终端 1: 启动 Gazebo 仿真,把 CHAMP 自带 EKF 让出来
cd ~/go2_sim_ws
source install/setup.bash
ros2 launch go2_config gazebo_velodyne.launch.py use_slam:=true rviz:=false
再起 FAST-LIO:
# 终端 2: 启动 FAST-LIO,记得显式打开仿真时间
cd ~/go2_sim_ws
source install/setup.bash
ros2 launch fast_lio mapping.launch.py \
config_file:=velodyne.yaml \
use_sim_time:=true
最后起键盘控制:
# 终端 3: 键盘遥控机器人在场景里走一圈
cd ~/go2_sim_ws
source install/setup.bash
ros2 run champ_teleop champ_teleop.py
如果你想先做最小检查,再动机器人,先看这 4 条:
# 终端 4: 最小联调检查
source ~/go2_sim_ws/install/setup.bash
ros2 topic hz /imu/data
ros2 topic hz /velodyne_points
ros2 topic hz /cloud_registered
ros2 topic echo --once /Odometry
正常现象应该是:
/cloud_registered 已经开始发布
/Odometry 的 header.frame_id 是 camera_init
/Odometry 的 child_frame_id 是 body
RViz 里把 Fixed Frame 设成 camera_init 后,轨迹和点云都能稳定累积
为什么这里是 camera_init,不是 map
这一章我们先做的是无回环的前端建图。
对 FAST-LIO 来说,它最自然的全局参考系就是 camera_init。
后面接回环或定位后端时,再让 map -> camera_init 这条 TF 去负责全局纠漂。
步骤四:把点云地图存成 .pcd¶
当前这份 FAST-LIO 源码里已经打开了地图保存服务,所以你有两种办法:
直接退出 FAST-LIO,让它自动保存
不退出,手动调 /map_save
推荐先用服务调用:
# 终端 4: 不停节点也能把当前累计地图存下来
source ~/go2_sim_ws/install/setup.bash
ros2 service call /map_save std_srvs/srv/Trigger "{}"
保存后去 src/FAST_LIO_ROS2/PCD/ 目录找生成的地图文件。
如果你想快速看看保存结果:
# 在工作空间根目录下直接看一眼保存的 PCD
cd ~/go2_sim_ws
pcl_viewer src/FAST_LIO_ROS2/PCD/<你的地图文件>.pcd
如果更喜欢在 RViz 里看,也可以把保存后的 PCD 再转成点云话题,或者直接用 PointCloud2 观察正在发布的 /Laser_map。
先别急着追求'整层楼一把过'
FAST-LIO 没有回环检测,在大场景里长时间跑一定会慢慢累积漂移。
这一章的目标不是世界级精度建图,而是先把前端 3D 建图、PCD 保存和 3D 导航桥接全部跑通。
步骤五:用 OctoMap 把 3D 地图投影成 Nav2 能吃的全局图¶
现在我们来做这一章最关键的一步:
FAST-LIO 持续输出 /cloud_registered
octomap_server 订阅这路配准后的点云
它一边维护 3D 八叉树,一边自动发布 2D 投影 /projected_map
先给 map 一个临时别名¶
我们前面说过,FAST-LIO 的全局参考系是 camera_init。
而 Nav2 这边更习惯叫它 map。
在还没接回环后端之前,最简单的做法就是先发布一条恒等变换:
# 文件: go2_config/launch/octomap_bridge.launch.py
# 作用:把 camera_init 临时别名成 map,再启动 octomap_server
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package="tf2_ros",
executable="static_transform_publisher",
name="map_alias",
arguments=["0", "0", "0", "0", "0", "0", "map", "camera_init"],
parameters=[{"use_sim_time": True}],
),
Node(
package="octomap_server",
executable="octomap_server_node",
name="octomap_server",
parameters=[{
"use_sim_time": True,
"frame_id": "map",
"base_frame_id": "base_link",
"resolution": 0.10,
"latch": True,
"sensor_model/max_range": 20.0,
"pointcloud_min_z": -0.20,
"pointcloud_max_z": 2.50,
"occupancy_min_z": 0.00,
"occupancy_max_z": 2.00,
}],
remappings=[
("cloud_in", "/cloud_registered"),
],
output="screen",
),
])
这里面你要特别注意两件事:
frame_id 设成 map,是为了让后面的 global_costmap 统一吃同一个全局坐标系
输入选 /cloud_registered,而不是原始 /velodyne_points,因为前者已经叠加了 FAST-LIO 估计出的位姿
启动 octomap_server¶
# 终端 4: 启动 OctoMap 桥接
cd ~/go2_sim_ws
source install/setup.bash
ros2 launch go2_config octomap_bridge.launch.py
起来之后,你可以马上检查:
# 看看 3D 地图和 2D 投影是不是都出来了
source ~/go2_sim_ws/install/setup.bash
ros2 topic list | grep -E "octomap|projected_map"
正常情况下应该至少能看到:
/octomap_full
/projected_map
步骤六:给 local_costmap 换成 STVL,把 3D 障碍直接送进 Nav2¶
现在来写 Nav2 的 3D 版参数。
这里的设计思路很明确:
global_costmap 读 /projected_map,负责全局静态环境
local_costmap 直接接 STVL,实时看原始 3D 点云
在 go2_config/config/autonomy/ 下新建 navigation_3d.yaml:
bt_navigator:
ros__parameters:
use_sim_time: true
global_frame: map
robot_base_frame: base_link
odom_topic: /Odometry
bt_loop_duration: 10
default_server_timeout: 20
enable_groot_monitoring: true
groot_zmq_publisher_port: 1666
groot_zmq_server_port: 1667
plugin_lib_names:
- nav2_compute_path_to_pose_action_bt_node
- nav2_compute_path_through_poses_action_bt_node
- nav2_follow_path_action_bt_node
- nav2_back_up_action_bt_node
- nav2_spin_action_bt_node
- nav2_wait_action_bt_node
- nav2_clear_costmap_service_bt_node
- nav2_is_stuck_condition_bt_node
- nav2_goal_reached_condition_bt_node
- nav2_goal_updated_condition_bt_node
- nav2_initial_pose_received_condition_bt_node
- nav2_reinitialize_global_localization_service_bt_node
- nav2_rate_controller_bt_node
- nav2_distance_controller_bt_node
- nav2_speed_controller_bt_node
- nav2_truncate_path_action_bt_node
- nav2_goal_updater_node_bt_node
- nav2_recovery_node_bt_node
- nav2_pipeline_sequence_bt_node
- nav2_round_robin_node_bt_node
- nav2_transform_available_condition_bt_node
- nav2_time_expired_condition_bt_node
- nav2_distance_traveled_condition_bt_node
- nav2_single_trigger_bt_node
- nav2_is_battery_low_condition_bt_node
- nav2_navigate_through_poses_action_bt_node
- nav2_navigate_to_pose_action_bt_node
- nav2_remove_passed_goals_action_bt_node
- nav2_planner_selector_bt_node
- nav2_controller_selector_bt_node
- nav2_goal_checker_selector_bt_node
controller_server:
ros__parameters:
use_sim_time: true
controller_frequency: 20.0
min_x_velocity_threshold: 0.001
min_y_velocity_threshold: 0.5
min_theta_velocity_threshold: 0.001
failure_tolerance: 0.3
progress_checker_plugin: progress_checker
goal_checker_plugins: [general_goal_checker]
controller_plugins: [FollowPath]
progress_checker:
plugin: nav2_controller::SimpleProgressChecker
required_movement_radius: 0.3
movement_time_allowance: 20.0
general_goal_checker:
stateful: true
plugin: nav2_controller::SimpleGoalChecker
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.35
FollowPath:
plugin: dwb_core::DWBLocalPlanner
debug_trajectory_details: true
min_vel_x: -0.1
min_vel_y: 0.0
max_vel_x: 0.3
max_vel_y: 0.0
max_vel_theta: 0.5
min_speed_xy: 0.0
max_speed_xy: 0.3
min_speed_theta: 0.0
acc_lim_x: 2.5
acc_lim_y: 0.0
acc_lim_theta: 3.2
decel_lim_x: -2.5
decel_lim_y: 0.0
decel_lim_theta: -3.2
vx_samples: 20
vy_samples: 5
vtheta_samples: 20
sim_time: 1.7
linear_granularity: 0.05
angular_granularity: 0.025
transform_tolerance: 0.5
xy_goal_tolerance: 0.25
trans_stopped_velocity: 0.05
short_circuit_trajectory_evaluation: true
stateful: true
critics: [RotateToGoal, Oscillation, BaseObstacle, GoalAlign, PathAlign, PathDist, GoalDist]
BaseObstacle.scale: 0.02
PathAlign.scale: 32.0
PathAlign.forward_point_distance: 0.1
GoalAlign.scale: 24.0
GoalAlign.forward_point_distance: 0.1
PathDist.scale: 32.0
GoalDist.scale: 24.0
RotateToGoal.scale: 24.0
RotateToGoal.slowing_factor: 3.0
RotateToGoal.lookahead_time: -1.0
local_costmap:
local_costmap:
ros__parameters:
use_sim_time: true
global_frame: camera_init
robot_base_frame: base_link
rolling_window: true
width: 4.0
height: 4.0
resolution: 0.05
robot_radius: 0.22
update_frequency: 5.0
publish_frequency: 2.0
plugins: [stvl_layer, inflation_layer]
inflation_layer:
plugin: nav2_costmap_2d::InflationLayer
cost_scaling_factor: 3.0
inflation_radius: 0.55
stvl_layer:
plugin: spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer
enabled: true
voxel_decay: 10.0
decay_model: 0
voxel_size: 0.05
track_unknown_space: true
unknown_threshold: 15
mark_threshold: 0
update_footprint_enabled: true
combination_method: 1
transform_tolerance: 0.2
origin_z: 0.0
publish_voxel_map: true
mapping_mode: false
observation_sources: velodyne_mark velodyne_clear
velodyne_mark:
data_type: PointCloud2
topic: /velodyne_points
marking: true
clearing: false
clear_after_reading: true
min_obstacle_height: 0.05
max_obstacle_height: 2.0
observation_persistence: 0.0
expected_update_rate: 0.0
velodyne_clear:
data_type: PointCloud2
topic: /velodyne_points
marking: false
clearing: true
clear_after_reading: true
min_z: -0.20
max_z: 2.20
vertical_fov_angle: 0.52
vertical_fov_padding: 0.05
horizontal_fov_angle: 6.28
model_type: 1
always_send_full_costmap: true
local_costmap_client:
ros__parameters:
use_sim_time: true
local_costmap_rclcpp_node:
ros__parameters:
use_sim_time: true
global_costmap:
global_costmap:
ros__parameters:
use_sim_time: true
global_frame: map
robot_base_frame: base_link
update_frequency: 2.0
publish_frequency: 1.0
resolution: 0.10
robot_radius: 0.22
track_unknown_space: true
plugins: [static_layer, inflation_layer]
static_layer:
plugin: nav2_costmap_2d::StaticLayer
map_topic: /projected_map
map_subscribe_transient_local: true
inflation_layer:
plugin: nav2_costmap_2d::InflationLayer
cost_scaling_factor: 3.0
inflation_radius: 0.55
always_send_full_costmap: true
global_costmap_client:
ros__parameters:
use_sim_time: true
global_costmap_rclcpp_node:
ros__parameters:
use_sim_time: true
planner_server:
ros__parameters:
use_sim_time: true
expected_planner_frequency: 20.0
planner_plugins: [GridBased]
GridBased:
plugin: nav2_navfn_planner/NavfnPlanner
tolerance: 0.5
use_astar: false
allow_unknown: true
behavior_server:
ros__parameters:
use_sim_time: true
costmap_topic: local_costmap/costmap_raw
footprint_topic: local_costmap/published_footprint
cycle_frequency: 10.0
behavior_plugins: [spin, backup, wait]
spin:
plugin: nav2_behaviors/Spin
backup:
plugin: nav2_behaviors/BackUp
wait:
plugin: nav2_behaviors/Wait
global_frame: camera_init
robot_base_frame: base_link
transform_timeout: 0.1
simulate_ahead_time: 2.0
max_rotational_vel: 1.0
min_rotational_vel: 0.4
rotational_acc_lim: 3.2
smoother_server:
ros__parameters:
use_sim_time: true
velocity_smoother:
ros__parameters:
use_sim_time: true
smoothing_frequency: 20.0
scale_velocities: false
feedback: OPEN_LOOP
max_velocity: [0.3, 0.0, 0.5]
min_velocity: [-0.3, 0.0, -0.5]
max_accel: [2.5, 0.0, 3.2]
max_decel: [-2.5, 0.0, -3.2]
odom_topic: /Odometry
odom_duration: 0.1
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
waypoint_follower:
ros__parameters:
use_sim_time: true
loop_rate: 20
stop_on_failure: false
waypoint_task_executor_plugin: wait_at_waypoint
wait_at_waypoint:
plugin: nav2_waypoint_follower::WaitAtWaypoint
enabled: true
waypoint_pause_duration: 200
这份参数里真正的核心只有两块:
global_costmap.static_layer.map_topic = /projected_map
local_costmap 用 STVL 直接订阅 /velodyne_points
如果你能把这两件事用自己的话解释清楚,这一章就算学到位了。
步骤七:启动 3D 版 Nav2 并发目标点¶
为了和当前仓库的 twist_mux 结构保持一致,我们新建一个 launch,延续前面 go2_config 里那套写法。
# 文件: go2_config/launch/nav2_octomap.launch.py
# 作用:把 OctoMap、twist_mux 和 Nav2 核心节点一起拉起来
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.descriptions import ParameterFile
from launch_ros.substitutions import FindPackageShare
from nav2_common.launch import RewrittenYaml
def generate_launch_description():
this_package = FindPackageShare("go2_config")
use_sim_time = LaunchConfiguration("use_sim_time")
params_file = LaunchConfiguration("params_file")
rviz = LaunchConfiguration("rviz")
twist_mux_params = PathJoinSubstitution(
[this_package, "config", "autonomy", "twist_mux.yaml"]
)
configured_params = ParameterFile(
RewrittenYaml(
source_file=params_file,
param_rewrites={"use_sim_time": use_sim_time},
convert_types=True,
),
allow_substs=True,
)
tf_remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
lifecycle_nodes = [
"controller_server",
"smoother_server",
"planner_server",
"behavior_server",
"bt_navigator",
"waypoint_follower",
"velocity_smoother",
]
rviz_config = PathJoinSubstitution(
[FindPackageShare("champ_navigation"), "rviz", "navigation.rviz"]
)
return LaunchDescription([
DeclareLaunchArgument("use_sim_time", default_value="true"),
DeclareLaunchArgument(
"params_file",
default_value=PathJoinSubstitution(
[this_package, "config", "autonomy", "navigation_3d.yaml"]
),
),
DeclareLaunchArgument("rviz", default_value="true"),
Node(
package="twist_mux",
executable="twist_mux",
name="twist_mux",
parameters=[twist_mux_params, {"use_sim_time": use_sim_time}],
remappings=[("cmd_vel_out", "/cmd_vel")],
output="screen",
),
Node(
package="nav2_controller",
executable="controller_server",
parameters=[configured_params],
remappings=tf_remappings + [("cmd_vel", "cmd_vel_nav")],
output="screen",
),
Node(
package="nav2_smoother",
executable="smoother_server",
name="smoother_server",
parameters=[configured_params],
remappings=tf_remappings,
output="screen",
),
Node(
package="nav2_planner",
executable="planner_server",
name="planner_server",
parameters=[configured_params],
remappings=tf_remappings,
output="screen",
),
Node(
package="nav2_behaviors",
executable="behavior_server",
name="behavior_server",
parameters=[configured_params],
remappings=tf_remappings + [("cmd_vel", "cmd_vel_behavior")],
output="screen",
),
Node(
package="nav2_bt_navigator",
executable="bt_navigator",
name="bt_navigator",
parameters=[configured_params],
remappings=tf_remappings,
output="screen",
),
Node(
package="nav2_waypoint_follower",
executable="waypoint_follower",
name="waypoint_follower",
parameters=[configured_params],
remappings=tf_remappings,
output="screen",
),
Node(
package="nav2_velocity_smoother",
executable="velocity_smoother",
name="velocity_smoother",
parameters=[configured_params],
remappings=tf_remappings + [
("cmd_vel", "cmd_vel_nav"),
("cmd_vel_smoothed", "cmd_vel_nav_smooth"),
],
output="screen",
),
Node(
package="nav2_lifecycle_manager",
executable="lifecycle_manager",
name="lifecycle_manager_navigation",
parameters=[{
"use_sim_time": use_sim_time,
"autostart": True,
"node_names": lifecycle_nodes,
}],
output="screen",
),
Node(
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config],
parameters=[{"use_sim_time": use_sim_time}],
condition=IfCondition(rviz),
output="screen",
),
])
这个 launch 不再去碰:
pointcloud_to_laserscan
map_server
因为这章的局部层已经直接走 3D 点云,全局层也直接吃 /projected_map 了。
编译与运行¶
先把这一章新增或改动的包编一遍:
# 编译 FAST-LIO 和 go2_config
# 如果你也放了源码版 velodyne 插件,把 velodyne_gazebo_plugins 一起加上
cd ~/go2_sim_ws
source /opt/ros/humble/setup.bash
colcon build --symlink-install --packages-select \
fast_lio \
go2_config
source install/setup.bash
下面按顺序开 5 个终端:
# 终端 1: Gazebo + Go2 + VLP-16
cd ~/go2_sim_ws
source install/setup.bash
ros2 launch go2_config gazebo_velodyne.launch.py use_slam:=true rviz:=false
# 终端 2: FAST-LIO 前端建图
cd ~/go2_sim_ws
source install/setup.bash
ros2 launch fast_lio mapping.launch.py \
config_file:=velodyne.yaml \
use_sim_time:=true
# 终端 3: 键盘控制
cd ~/go2_sim_ws
source install/setup.bash
ros2 run champ_teleop champ_teleop.py cmd_vel_topic:=/cmd_vel_teleop
# 终端 4: OctoMap 桥接
cd ~/go2_sim_ws
source install/setup.bash
ros2 launch go2_config octomap_bridge.launch.py
# 终端 5: 3D 版 Nav2
cd ~/go2_sim_ws
source install/setup.bash
ros2 launch go2_config nav2_octomap.launch.py \
params_file:=src/unitree-go2-ros2/robots/configs/go2_config/config/autonomy/navigation_3d.yaml
如果你只想先验证建图,可以暂时不启动终端 4 和终端 5。
如果你只想验证 OctoMap 投影,也可以先不开 Nav2,单独看 /projected_map。
结果验证¶
验证 1:FAST-LIO 是否真的跑通了¶
满足下面几条,就说明前端基本没问题:
RViz 里把 Fixed Frame 设成 camera_init,能看到 /cloud_registered 稳定累积
/Odometry 正在发布,而且机器人移动时轨迹连续
/path 不再抽风,转弯时不会瞬移
ros2 service call /map_save 能生成新的 .pcd
验证 2:OctoMap 和 /projected_map 是否对上了¶
下面这几个现象同时满足,说明 3D 到 2D 的桥已经搭上:
/octomap_full 在 RViz 里能看到体素占据
/projected_map 已经不是空白 OccupancyGrid
全局代价地图里,墙和柱子会稳定出现在规划层
你可以直接看话题:
# 看 projected_map 有没有持续发布
source ~/go2_sim_ws/install/setup.bash
ros2 topic hz /projected_map
验证 3:STVL 是否真的在拦 3D 障碍¶
局部层要看的不是"有没有图",而是"能不能拦住本来 2D 看不见的东西"。
最直接的检查方式:
让 Go2 朝一个有高度差的障碍物慢慢靠近
观察 local_costmap 是否先于机器人实体碰撞把代价抬高
如果你打开了 publish_voxel_map: true,还可以在 RViz 里把 STVL 的体素层也显示出来。
验证 4:按 3 个场景做检查¶
当前仓库现成的 world 主要还是 indoor.world。
所以这里我们把验证分成两类:
当前仓库立即可复现:先在 indoor.world 跑通整条链
下一步扩展场景:复制 world 后加桌面、坡道、楼梯结构
场景
当前仓库是否现成
你该怎么做
通过标准
室内基线(indoor.world)
是
直接启动整条链
墙、柱、转角都能稳定建图,Nav2 能在房间里到点
低矮桌面办公室
否
从 indoor.world 复制一份,加一个桌面高度约 0.7m ~ 0.8m 的 box
全局图和局部 STVL 都应把桌面下方不可穿越区域拦住
坡道走廊
否
复制 world,用倾斜 box 加一段缓坡
局部层不应把坡道口误判成完全平地穿透
首层 + 楼梯口 + 二层平台
否
复制 world,按多层测试场景清单拼出楼梯和平台
地图里能看见楼梯和平台边缘,Nav2 不再把楼梯当前方可通行平地
启动模式其实都一样,只是 world 文件换掉:
# 把 world 换成你复制出来的新场景
cd ~/go2_sim_ws
source install/setup.bash
ros2 launch go2_config gazebo_velodyne.launch.py \
use_slam:=true \
world:=src/unitree-go2-ros2/robots/configs/go2_config/worlds/<你的场景>.world
多层场景先别追求花里胡哨
第一版多层 world 用 box、cylinder 这类 primitive 几何体就够了。
目标是验证 3D 感知链和代价地图分工,不是先卷场景美术。
rosbag 回放建议¶
如果你想把 3 个验证场景都留一份回放素材,至少录下面这些:
# 建议每个场景单独录一包,后面排查参数更轻松
cd ~/go2_sim_ws
source install/setup.bash
ros2 bag record \
/velodyne_points \
/imu/data \
/cloud_registered \
/Odometry \
/octomap_full \
/projected_map \
/tf \
/tf_static
后面如果某个场景只在"坡道"或"楼梯口"出问题,你就不用每次都重跑 Gazebo 了,直接回放 bag 排参数。
常见问题¶
1. FAST-LIO 一直提示 Waiting for cloud and imu¶
现象:
点云和 IMU 看起来都在
但 FAST-LIO 就是不开始工作
先查 3 件事:
lid_topic / imu_topic 有没有写成上游模板里的默认值
所有节点是不是都开了 use_sim_time: true
ros2 topic list 里是不是真的有 /velodyne_points 和 /imu/data
这一章里,use_sim_time: true 不是可选项。
2. 点云跟着机器人一起飞,像整张地图粘在狗身上¶
原因:
外参填反了
extrinsic_T 把 LiDAR 相对 IMU 的方向写错了
或者 body -> base_link 没桥好
解决:
回头核对 xacro 里的固定关节
先确认 extrinsic_R 在当前仿真里应该是单位阵
再确认 gazebo_velodyne.launch.py 是不是带了 use_slam:=true
3. RViz 里模型不显示,或者 TF 一直 extrapolation¶
原因:
RViz 没跟 /clock
Fixed Frame 设错
解决:
RViz 本身也要 use_sim_time: true
跑 FAST-LIO 时先把 Fixed Frame 设成 camera_init
跑 Nav2 时,看全局图就切 map,看局部层就看 camera_init
4. timestamp_unit 配错后地图开始扭¶
现象:
直线走得还行
一转弯地图就明显扭曲
根因:
点云里的 time 字段单位,和 velodyne.yaml 里的 timestamp_unit 对不上
解决:
先确认你当前 PointCloud2 的 time 到底是秒、毫秒、微秒还是纳秒
再把 timestamp_unit 改成匹配值
别把这个坑当成'再调调 det_range 试试'
timestamp_unit 这种契约参数错了,后面再怎么调体素大小和量程都只是瞎忙。
5. octomap_server 吃满 CPU¶
原因:
分辨率设太细
场景又太大
解决:
先把 resolution 从 0.05 放宽到 0.10 或 0.15
把 sensor_model/max_range 控制在室内真正需要的范围内
6. /projected_map 和 Nav2 全局层对不上¶
现象:
代价地图有内容
但机器人位置和全局图不在一个地方
原因:
map -> camera_init 没统一
frame_id 写成了别的名字
解决:
先用这一章的 map_alias 把 map -> camera_init 固定成 identity
等后面接回环后端时,再让它接管这条 TF
7. Nav2 走到半路撞空气,或者局部层一直有幽灵障碍¶
原因:
STVL 的体素衰减时间太长
清除视锥参数太保守
观测持久化没有归零
解决:
先把 observation_persistence 保持 0.0
保持 clear_after_reading: true
必要时把 voxel_decay 再减小一点
8. Goal 一发就被拒绝,提示在 lethal zone¶
原因:
/projected_map 投影后把目标点周围也算进了膨胀区
解决:
先缩小 inflation_radius
或者把目标点往开阔区域挪一点
9. 多层场景看起来一团乱¶
原因:
你把所有高度的点都同时画出来了
解决:
RViz 里给 PointCloud2 加高度过滤
或者先只看 /octomap_full 和 /projected_map
10. 机器人对着楼梯口还想往前拱¶
先别急着骂算法。先分清楚是哪层出问题:
如果 /octomap_full 里楼梯边缘都没出来,是前端建图问题
如果 /projected_map 里没有台阶前沿,是 3D 到 2D 投影问题
如果局部层看见了但 controller 还在逼近,再去查 costmap 和膨胀参数
本章小结¶
这一章真正完成的,不是"又跑了一个新包",而是把仿真里的 Go2 从"能扫"升级成了"能用 3D 地图做导航决策"。
我们先比较了三条 3D LIO 路线,最后选了在当前仿真里最稳的 FAST-LIO。然后把它和 Gazebo 的 /velodyne_points、/imu/data 对齐,顺手把外参、时间同步和 scan_line 这些最容易踩坑的契约参数讲清楚了。
接着,我们没有犯"3D 地图直接塞给 Nav2"的低级错误,而是明确做了分工:
OctoMap 负责静态全局 3D 表达,并投影成 /projected_map
STVL 负责局部动态 3D 障碍
到这里,你应该已经能用自己的话说清楚:
为什么本章选 FAST-LIO
为什么 OctoMap 和 STVL 要分开干活
为什么楼梯在这章里是"识别为障碍",不是"直接爬上去"
下一步¶
仿真里的 3D 链已经基本打通了,但它终究还是仿真。下一章我们把这条路搬回实机,同时也把雷达从仿真里的 VLP-16 换成更像官方开发主线的 Livox MID-360。
继续阅读:第 15 章 实机升级 — 外接 MID-360 方案
拓展阅读¶
FAST-LIO / FAST-LIO2 论文与官方 README
OctoMap 论文与 octomap_mapping 项目文档
spatio_temporal_voxel_layer 官方 README 与示例配置
Nav2 官方 tuning guide
grid_map 文档:适合继续对比 2.5D 与真正 3D 世界模型的边界