导航功能包

概述#

本功能包采用 3D 定位与 2D 导航混合架构。系统集成了 FAST-LIO2 算法，利用误差状态卡尔曼滤波与动态 ikd-Tree 结构，在长走廊、空旷大厅等特征退化场景下实现极低漂移的绝对定位。通过引入 Nav2 适配器，利用 3D 位姿在 2D 平面上进行路径规划和避障。

该功能包提供了一套完整的工程化落地工具箱。工具箱基于法向量估计与坡度分析，将 3D 点云转换为 2D 栅格地图，生成高质量的 Nav2 Costmap。同时，系统支持“不停车连续巡逻”与“精确定点操作”两种模式的热切换，为构建工业级自主移动机器人提供从建图到导航的全流程解决方案。

开源地址：https://github.com/Roboparty/roboparty_navigation

使用指南（详见仓库中README.md）#

前置要求#

系统要求#

操作系统：Ubuntu 22.04
ROS 版本：ROS 2 Humble

硬件要求#

激光雷达：Livox MID-360 或兼容型号
IMU：集成式或外置 IMU（重力单位应为 'g'）
UWB 模块：NLink LinkTrack（可选）

依赖项#

请参考各个功能包的 README 安装详细依赖。

通用依赖

Bash

sudo apt update
sudo apt install -y \
    ros-humble-navigation2 \
    ros-humble-nav2-* \
    ros-humble-pcl-ros \
    ros-humble-pcl-conversions \
    ros-humble-tf2-ros \
    ros-humble-tf2-geometry-msgs \
    libceres-dev \
    ccache

Python 依赖

Bash

pip install open3d numpy pyyaml Pillow scipy

第三方库编译

Sophus (推荐 1.22.10)

Bash

git clone https://github.com/strasdat/Sophus.git
cd Sophus/
git checkout 1.22.10
mkdir build && cd build
cmake ..
make -j$(nproc)
sudo make install

fmt (需开启 -fPIC)

Bash

git clone https://github.com/fmtlib/fmt
cd fmt
mkdir build && cd build
cmake .. -DCMAKE_POSITION_INDEPENDENT_CODE=ON
make -j$(nproc)
sudo make install

注意：必须在 CMakeLists.txt 中添加 add_compile_options(-fPIC) ！！

Livox SDK2 & Driver

Livox 驱动推荐安装在工作空间之外，参考链接：Livox-SDK2 和 livox_ros_driver2

Bash

# Livox SDK2
git clone https://github.com/Livox-SDK/Livox-SDK2.git
cd Livox-SDK2
mkdir build && cd build
cmake ..
make -j$(nproc)
sudo make install

# Livox ROS Driver2 (建议放在 src 目录外)
git clone https://github.com/Livox-SDK/livox_ros_driver2.git
cd livox_ros_driver2
./build.sh humble

安装#

1. 克隆仓库#

使用 --recursive 参数自动克隆子模块：

Bash

cd ~/
mkdir -p atom01_ws/src
cd atom01_ws/src
git clone --recursive https://github.com/Roboparty/atom01_navigation.git

2. 编译工作空间#

Bash

cd ~/atom01_ws
colcon build --symlink-install

使用方法#

在使用任何命令前，请确保已加载工作空间环境变量：

Bash

cd ~/atom01_ws
source install/setup.bash

1. 建图与定位#

详细操作请参考 robots_localization_ros2/README.md。

简要步骤：

启动雷达驱动。
修改 config/robots_localization.yaml 中的 mapping_en 参数：
- true: 建图模式
- false: 定位模式
启动节点：

Bash

# 建图模式
# 注：建图和定位使用同一个启动文件，区别在于配置文件中的 mapping_en 参数
ros2 launch robots_localization_ros2 localization.launch.py

# 或 定位模式
ros2 launch robots_localization_ros2 localization.launch.py

2. 导航与地图处理#

详细操作请参考 nav2_localization_adapter/README_CN.md。

简要步骤：

PCD 转 2D 地图：使用 pcd2pgm.py 工具转换地图。
启动导航：

Bash

ros2 launch nav2_localization_adapter navigation.launch.py map:=<地图路径>

配置说明#

robots_localization_ros2#

主配置文件：config/robots_localization.yaml

关键参数：

common.lid_topic：激光雷达点云话题
common.imu_topic：IMU 数据话题
preprocess.lidar_type：激光雷达类型（Livox 为 1）
mapping_en：建图使能 (true: 建图, false: 纯定位)
mapping.extrinsic_T：LiDAR 到 IMU 的平移
mapping.extrinsic_R：LiDAR 到 IMU 的旋转

nav2_localization_adapter#

详细配置请参考包内文档：nav2_localization_adapter

已知问题与解决方案#

1. 雷达连接问题#

检查 IP 配置（机器人不应使用代理）
验证雷达 IP 地址与配置匹配
确保网络接口设置正确

重要说明#

Livox IMU：重力单位为 'g'，确保代码中的归一化与此匹配
MID-360 IMU 外参：在 LiDAR 坐标系中的位置为 [0.011, 0.0234, -0.044]
点云时间戳：Header 时间是最早点的时间；单个点的时间是相对于 header 时间的偏移量（单位：毫秒）
PCD 地图：在启动定位模式前，将 PCD 文件放置在指定的地图目录中

概述#

开源地址：https://github.com/Roboparty/roboparty_navigation