基于kuavo机器人的导航demo

本demo同时适配kuavo4与kuavo5
调试本demo需要一定的基础，如ROS、navigation、坐标变换、点云处理等

1. 系统概述

本demo是基于KUAVO导航解决方案，可以实现：

这仅仅是示例，还有一些局限性，比如建图时对于动态障碍物的处理并不好，会把动态障碍物经过的路线识别成障碍物，所以建图时尽量在静态场景，对于地图中的噪点可以通过地图编辑解决。
代码只能部署到机器人上位机

• 🗺️ 自动建图：扫描环境并生成地图
• 📍 定位：在地图中确定机器人的位置
• 🚀 自主导航：自动规划路径并避开障碍物到达目标，在导航时可以动态避障

1.1 KUAVO使用介绍（必读）

本demo面向KUAVO实机（kuavo4/kuavo5）+ Livox MID360，在机器人上位机直接运行。

robot_version 用于区分 KUAVO 不同版本上的雷达安装方向，驱动和FAST-LIO会按该参数切换对应配置：

• kuavo4：robot_version=45 或 49
• kuavo5：robot_version=52
• robot_version=45 或 49：使用 MID360_config.json + mid360.yaml
• robot_version=52：使用 MID360_config_52.json + mid360_52.yaml

默认参数为 robot_version=45（兼容45/49安装方向），因此 kuavo4 常规使用可以不传该参数。
只有 kuavo5（52安装方向）需要显式传 robot_version:=52。

常见参考坐标系说明：

• camera_init：FAST-LIO启动时建立的全局参考系
• body：机器人本体参考坐标系
• livox_frame：雷达坐标系
• odom：里程计坐标系（本demo中通过静态TF与 camera_init 关联）

1.2 快速使用路径

1. 建图：roslaunch kuavo_nav_demo kuavo_build_map.launch
2. 保存地图：roslaunch kuavo_nav_demo save_pgm_map_auto_fix.launch
3. 导航：roslaunch kuavo_nav_demo kuavo_nav_test.launch

仅 kuavo5（52安装方向）需要在建图和导航命令后追加 robot_version:=52。

1.3 系统主要特点

✅ 开箱即用：预配置好的参数，新手也能快速上手
✅ 功能完整：从建图到导航的一站式解决方案
✅ 实时处理：基于FAST-LIO的SLAM算法
✅ 基于雷达纯定位：只用关注body的坐标，不用关注机器人本身在地图上的坐标

1.4 工作空间速查

本项目是一个通用 ROS1/catkin 工作空间，源码统一放在 src/ 下，主要面向 KUAVO 实机导航验证。

• 工作空间目录统一命名为 research_navigation_demo
• research_navigation_demo 是工作空间根目录，也就是包含 src/、devel/、build/ 的目录
• 编译入口：在 research_navigation_demo 下执行 catkin build，再 source devel/setup.bash
• 主入口包：kuavo_nav_demo

实际部署到机器人上位机时，先进入自己的工作空间目录并加载环境：

cd research_navigation_demo
catkin build
source devel/setup.bash

1.5 工作空间包关系

本工作空间把传感器驱动、SLAM/定位、地图处理、导航和 KUAVO 示例封装在同一个 catkin 工作空间内。常用包关系如下：

包/目录	作用	主要连接
livox_ros_driver2	Livox MID360 驱动	读取 MID360_config*.json，发布 /livox/lidar、/livox/imu
FAST_LIO	建图与里程计	订阅 /livox/lidar、/livox/imu，发布 /Odometry、/cloud_registered_body，建图时保存 PCD/scans.pcd
FAST_LIO_LOCALIZATION	基于 PCD 的全局定位	加载 FAST_LIO/PCD/scans.pcd，运行 global_localization.py 和 transform_fusion.py，发布 /localization
pointcloud_to_laserscan	点云转 2D scan	订阅 /cloud_registered_body，发布导航代价地图使用的 /scan
octomap_*	点云投影/八叉树地图支持	Pointcloud2Map.launch 订阅 /cloud_registered_body，输出 /projected_map
navigation	ROS Navigation Stack	提供 move_base、costmap、全局/局部规划器
velocity_smoother_ema	速度平滑	订阅 /cmd_vel_nav，平滑后发布 /cmd_vel
kuavo_nav_demo	KUAVO 启动、参数、地图和辅助脚本	集成建图、定位、move_base、地图保存与故障恢复
ros_map_editor	栅格地图编辑工具	直接编辑 kuavo_nav_demo/maps/*.pgm

1.6 主 launch 连接关系

建图链路

主入口：

roslaunch kuavo_nav_demo kuavo_build_map.launch

实际连接：

kuavo_build_map.launch
  -> livox_ros_driver2/msg_MID360.launch
       -> livox_lidar_publisher2
       -> /livox/lidar, /livox/imu
  -> fast_lio/fastlio_mapping
       -> /Odometry, /cloud_registered_body, FAST_LIO/PCD/scans.pcd
  -> fast_lio/Pointcloud2Map.launch
       -> /cloud_registered_body -> /projected_map
  -> fast_lio_localization/PointsCloud2toLaserscan.launch
       -> /cloud_registered_body -> /scan
  -> static TF:
       body -> livox_frame
       camera_init -> odom

建图完成后的 2D 地图保存入口：

roslaunch kuavo_nav_demo save_pgm_map_auto_fix.launch

该入口默认从 /projected_map 保存 kuavo_nav_demo/maps/test_map.pgm 和 test_map.yaml，并启动 map_yaml_monitor.py 修复 YAML 中可能出现的 nan。

定位导航链路

主入口：

roslaunch kuavo_nav_demo kuavo_nav_test.launch

实际连接：

kuavo_nav_test.launch
  -> livox_ros_driver2/msg_MID360.launch
       -> /livox/lidar, /livox/imu
  -> fast_lio_localization/kuavo_localize.launch
       -> fast_lio/fastlio_mapping
       -> global_localization.py
       -> transform_fusion.py
       -> pcl_ros/pcd_to_pointcloud
       -> map_server /map:=prior_map
       -> /Odometry, /localization, /map, /prior_map, /scan
  -> kuavo_movebase.launch
       -> move_base /map:=prior_map /odom:=Odometry
       -> velocity_smoother_ema
       -> /cmd_vel_nav -> /cmd_vel

导航时最重要的操作话题：

• /initialpose：RViz 的 2D Pose Estimate
• /move_base_simple/goal：RViz 的 2D Nav Goal
• /move_base/status、/move_base/result：导航状态与结果
• /move_base/clear_costmaps：清理代价地图服务
• /cmd_vel_nav：move_base 输出的原始速度
• /cmd_vel：平滑后给机器人底层使用的速度

1.7 地图与坐标系文件连接

定位导航同时依赖 3D PCD 地图和 2D 栅格地图：

• 3D 定位地图：src/FAST_LIO/PCD/scans.pcd
• 2D 导航地图：src/kuavo_nav_demo/maps/test_map.yaml 和 test_map.pgm
• 建图投影输出：/projected_map
• 导航加载地图：map_server 将 test_map.yaml 发布为 /prior_map

常用坐标系连接：

camera_init -> robot_foot_init -> odom
body -> livox_frame

其中 camera_init 是 FAST-LIO 启动时建立的全局参考系，body 是当前导航配置里的机器人本体参考系，livox_frame 是 MID360 雷达坐标系。当前实物导航链路主要围绕 camera_init / robot_foot_init / odom / body / livox_frame 工作，不依赖传统 base_link 链路。

2. 环境准备

需要使用远程桌面运行程序，方便查看效果，和实现导航

安装依赖：

pip3 install open3d==0.16.0
pip3 install watchdog==4.0.2
sudo apt-get install libsdl-image1.2-dev
# 上位机若为NX或AGX即用户名为leju_Kuavo，执行下面一行命令，反之不用
sudo apt install ros-noetic-tf2-sensor-msgs

下载代码并编译：

git clone https://gitcode.com/OpenLET/research_navigation_demo.git
cd research_navigation_demo
catkin build
source devel/setup.bash

---

3. 硬件连接

3.1 连接Livox激光雷达

步骤1：测试连接

# 测试网络连通性
ping 192.168.1.191  # 出厂设置，需要确认没问题

# 如果无法ping通，检查：
# 1. 有线是否连接正常
# 2. IP地址是否在同一网段
# 3. 激光雷达是否正常供电

正常连接输出：

4. 建图操作

4.1 基础建图流程

步骤1：启动建图系统

# 进入工作空间
cd research_navigation_demo
source devel/setup.bash
# 启动基础建图
roslaunch kuavo_nav_demo kuavo_build_map.launch

仅 kuavo5（52安装方向）需要改为 roslaunch kuavo_nav_demo kuavo_build_map.launch robot_version:=52。
注意：这里后续定位是使用pcd地图， PCD 默认是 $(find fast_lio)/PCD/scans.pcd（通常为 research_navigation_demo/src/FAST_LIO/PCD/scans.pcd）。启动并关闭建图程序后该文件会被自动生成并覆盖。若要保留既有定位地图，请在建图前先备份这个 scans.pcd。

步骤2：观察建图效果

• 打开RViz可视化界面
• 观察点云地图的构建过程
• 确认激光雷达数据正常

步骤3：控制机器人移动

• 使用遥控器或键盘控制机器人
• 建议速度：0.1-0.4 m/s
• 避免快速转弯和急停

也可以挂着机器人建图（不启动机器人）， 让机器人处于使能状态，能确保头部角度保持水平不变 （推荐）

步骤4：完成建图

• 覆盖整个目标区域
• 确保地图质量正常
• 保存地图数据

效果如下图：

4.2 保存地图

自动保存

cd research_navigation_demo
source devel/setup.bash
# 启动自动保存
roslaunch kuavo_nav_demo save_pgm_map_auto_fix.launch

若未出现报错，现在可以关闭建图相关终端和程序

---

5. 地图编辑

5.1 为什么需要编辑地图？

建图过程中可能会包含：

• 🚶 动态障碍物（行人、车辆）
• 🗑️ 临时物体（垃圾桶、椅子）
• 🌫️ 环境干扰 (场景中包含玻璃等反光性强的物品)

5.2 编辑前准备（建议先做）

1. 确保地图文件成对存在：xxx.pgm 与 xxx.yaml 必须在同一路径下。
2. 先备份原图，避免误操作后无法恢复：

cd research_navigation_demo/src/kuavo_nav_demo/maps
cp test_map.pgm test_map_backup.pgm
cp test_map.yaml test_map_backup.yaml

3. 若使用 ros_map_editor，先安装依赖：

sudo apt install python3-pyqt5 python3-pil python3-yaml

5.3 使用图像编辑软件（推荐）

安装编辑工具：

# 安装KolourPaint（推荐）
sudo apt install kolourpaint

# 或安装GIMP
sudo apt install gimp

编辑地图：

# 打开地图文件
kolourpaint research_navigation_demo/src/kuavo_nav_demo/maps/test_map.pgm

编辑技巧：

• 🟦 白色区域：空闲空间（机器人可以通行）
• ⬛ 黑色区域：障碍物（机器人不能通过）
• ⬜ 灰色区域：未知区域（机器人谨慎通行）
• 🖌️ 橡皮：擦除错误障碍物
• 🖌️ 黑色画笔：添加障碍物
• 建议不要修改图像分辨率和画布尺寸，否则可能导致导航比例失真。

5.4 使用ROS地图编辑器（MapEditor.py）

启动ROS地图编辑器：

# 进入编辑器目录
cd research_navigation_demo/src/ros_map_editor

# 启动编辑器
python3 MapEditor.py research_navigation_demo/src/kuavo_nav_demo/maps/test_map.pgm
# 也可不带后缀
# python3 MapEditor.py research_navigation_demo/src/kuavo_nav_demo/maps/test_map

编辑器常用操作：

• Color=alternate：左键单击按 白 -> 黑 -> 灰 -> 白 循环切换
• Color=occupied：左键拖动涂黑（占据，像素值0）
• Color=unoccupied：左键拖动涂白（空闲，像素值255）
• Color=uncertain：左键拖动涂灰（未知，像素值200）
• 点击 Save 会覆盖当前 .pgm 文件；.yaml 不会自动修改
• Zoom 可调显示倍率，便于细节修补

5.5 地图质量检查与回归验证

编辑完成后，请检查：

• ✅ 地图边界是否完整
• ✅ 障碍物是否准确
• ✅ 是否有明显错误

编辑示例：

编辑前：

编辑后：

---

6. 定位导航

6.0 KUAVO实机启动前检查

• 机器人本体已上电并处于可控状态
• MID360网络可达（ping 192.168.1.191）
• 地图文件已生成并可被定位模块读取

6.1 启动实物导航系统

需要先启动机器人
方法1：一键启动（推荐）

cd research_navigation_demo
source devel/setup.bash
# 启动完整导航系统
roslaunch kuavo_nav_demo kuavo_nav_test.launch

仅 kuavo5（52安装方向）需要改为 roslaunch kuavo_nav_demo kuavo_nav_test.launch robot_version:=52。

6.2 设置初始位姿

在RViz中设置：

1. 打开RViz界面
2. 点击顶部工具栏的"2D Pose Estimate"按钮
3. 在地图上点击并拖动，设置机器人的初始位置和朝向
4. 确认位姿准确

是否可以生效判据（重要）：

• 初始化定位要看到终端输出 Waiting for initial pose.... 才可以初始化（该日志通常是黄色字体，比较显眼），大概等30s会自动弹出。
• 建议先确认有这条提示，再在RViz发送 2D Pose Estimate。

初始化前：

初始化后：

6.3 设置导航目标

在RViz中设置：

1. 点击顶部工具栏的"2D Nav Goal"按钮
2. 在地图上点击并拖动，设置目标位置和朝向
3. 机器人将自动规划路径并开始导航

6.4 监控导航状态

主要话题说明：

• /Odometry (nav_msgs/Odometry)
  • 由fastlio发布的里程计信息
• /localization (nav_msgs/Odometry)
  • 融合后的全局定位结果，由transform_fusion.py发布。其位姿信息为map→body的变换，供导航等模块使用。
• /move_base/result (move_base_msgs/MoveBaseActionResult)
  • 导航结果（成功/失败）。
• /move_base/status (actionlib_msgs/GoalStatusArray)
  • 当前所有导航目标的状态。
• /move_base_simple/goal (geometry_msgs/PoseStamped)
  • RViz中设置的导航目标点。

查看导航状态：

# 查看导航状态
rostopic echo /move_base/status


# 查看速度命令
rostopic echo /cmd_vel

RViz可视化：

• 🟢 绿色路径：全局规划路径
• 🔵 蓝色路径：局部规划路径

6.5 导航参数调整

调整速度参数：

# 编辑速度参数
vim research_navigation_demo/src/kuavo_nav_demo/param/base_local_planner_params.yaml

常用参数：

TrajectoryPlannerROS:
  max_vel_x: 0.5          # 最大前进速度 (m/s)
  min_vel_x: 0.1          # 最小前进速度 (m/s)
  max_vel_theta: 1.0      # 最大旋转速度 (rad/s)
  min_in_place_vel_theta: 0.4  # 最小原地旋转速度
  acc_lim_x: 0.5          # 前进加速度限制
  acc_lim_theta: 0.5      # 旋转加速度限制

调整避障参数：

# 编辑代价地图参数
vim research_navigation_demo/src/kuavo_nav_demo/param/costmap_common_params.yaml

常用参数：

obstacle_range: 2.5       # 障碍物检测范围 (m)
raytrace_range: 3.0       # 射线追踪范围 (m)
inflation_radius: 0.55    # 障碍物膨胀半径 (m)
cost_scaling_factor: 10.0 # 代价缩放因子

6.6 导航失败时清理代价地图（推荐先做）

当出现“明明可通行但不出路径”或“局部规划器卡住”的情况，可先清理一次代价地图：

rosservice call /move_base/clear_costmaps "{}"

如果需要自动周期清理并重发目标，可使用仓库中的脚本：

cd research_navigation_demo
source devel/setup.bash
python3 src/kuavo_nav_demo/scripts/clear_costmaps.py

6.7 仿真导航

如果想研究机器人的规划算法建议使用仿真，不涉及雷达和定位

# 先启动仿真
cd research_navigation_demo
source devel/setup.bash
roslaunch kuavo_nav_demo kuavo_nav_test_sim.launch

---

7. 常见问题

7.1 建图问题

Q: 建图时点云飘移怎么办？
A:

• 确保激光雷达安装稳固

Q: 地图中出现大量噪点，或者出现太低高度的障碍物没有扫到？
A:

修改 research_navigation_demo/src/FAST_LIO/launch/Pointcloud2Map.launch 中的 pointcloud_min_z 参数，将 -0.5 增大或减小。

• 启用高度滤波，设置合适的高度范围
• 使用地图编辑工具手动清除

Q: 将地面识别为障碍物？
A:

修改 research_navigation_demo/src/FAST_LIO/launch/Pointcloud2Map.launch 中的 max_range 参数，将 10 减小。

• 雷达处于非水平状态，确保雷达状态保持水平
• 也可以根据实机安装角度，调整雷达外参中的 pitch（单位：度）：

"extrinsic_parameter" : {
  "roll": 180.0,
  "pitch": 0.0,
  "yaw": 0.0,
  "x": 0,
  "y": 0,
  "z": 0
}

对应文件：
research_navigation_demo/src/livox_ros_driver2/config/MID360_config.json

• 例如 kuavo4 标准版本（头部不能转动）可将 pitch 设为实测俯仰角，如 15.0。
• 修改后需要重启雷达驱动与相关建图/定位节点；并确保建图和导航使用同一套外参配置，即需要重新建图才能去导航。

7.2 定位问题

Q: 初始定位不准确？
A:

• 确保提供了较为准确的初始位姿估计
• 在机器人静止状态下进行初始化
• 确保建图时头部角度和导航时角度一致

Q: 初始化定位一直不成功？
A:

• 先确认终端出现 Waiting for initial pose....，再在RViz发送 2D Pose Estimate。
• 检查定位用 PCD 是否被建图流程覆盖（默认文件：$(find fast_lio)/PCD/scans.pcd，即 research_navigation_demo/src/FAST_LIO/PCD/scans.pcd）。
• 若怀疑被覆盖，恢复备份 PCD 后重启定位与导航模块再试。

Q: 定位过程中出现偏移？
A:

• 确保地图质量良好，特征丰富
• 检查激光雷达是否被遮挡
• 周围环境和建图时变化不大

7.3 导航问题

Q: 机器人无法规划路径？
A:

• 查看雷达和地图是否对齐，俯视和侧视去看！！！！！
• 检查地图中是否存在障碍物阻断路径
• 确保起点和终点都在可达区域
• 调整全局规划器参数，如default_tolerance
• 先执行一次 rosservice call /move_base/clear_costmaps "{}" 后再重试目标点

Q: 机器人无法靠近目标点？
A:

• 增加xy_goal_tolerance和yaw_goal_tolerance
• 检查目标点周围是否有障碍物
• 调整局部代价地图参数
• 必要时清理代价地图：rosservice call /move_base/clear_costmaps "{}"

7.4 系统错误

Q: 找不到激光雷达设备？
A:

• 检查网络连接和IP设置
• 确保激光雷达供电正常

Q: 点云方向或导航朝向明显不对？
A:

• 优先检查 robot_version 是否与实机安装版本一致（45/49/52）
• 确认驱动加载配置是否正确（MID360_config.json 或 MID360_config_52.json）
• 使用 rosrun tf tf_echo camera_init livox_frame 检查姿态方向是否符合预期

---

🙏 致谢

本项目集成了来自各种开源项目的工作：

• FAST-LIO - 高性能LiDAR-惯性里程计
• FAST-LIO-LOCALIZATION - 基于FAST-LIO的定位框架
• Livox ROS Driver - Livox激光雷达驱动
• ROS Navigation Stack - ROS导航栈
• OctoMap - 八叉树地图库