1. 已有地图定位导航的适用场景

基于激光雷达的自主导航通常分为两个阶段:建图与定位导航。建图阶段通过 SLAM 算法扫描未知环境,生成全局一致的点云地图或栅格地图;定位导航阶段则不再从零建图,而是加载这张已有地图,让机器人实时确定自己在全局坐标中的位置,并规划从起点到目标点的可行路径。

FAST-LIO2 是一款由香港大学 Mars 实验室开源的激光雷达-惯性里程计框架,核心论文为:

  • FAST-LIO:FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter(RA-L 2021)
  • FAST-LIO2:FAST-LIO2: Fast Direct LiDAR-inertial Odometry(T-RO 2022)

FAST-LIO 系列以**迭代误差状态卡尔曼滤波(IESKF)**实现 LiDAR 与 IMU 的紧耦合,具有计算效率高、鲁棒性强、支持多种主流雷达等优点,非常适合用来构建高质量点云地图。本文以 FAST-LIO2 为例,介绍如何利用已有地图完成重定位,并接入 ROS Navigation 实现全局路径规划与局部避障。

系统环境约定:Ubuntu 20.04 + ROS Noetic + PCL 1.10 + Eigen 3.3;若使用 Livox 雷达需额外安装 livox_ros_driver。

2. 已有地图定位导航整体方案

整套流程可拆分为三步:

  1. 建图阶段:运行 FAST-LIO2,在目标环境中完整扫描一遍,输出全局一致的点云地图并保存为 PCD 文件。
  2. 重定位阶段:机器人启动后,加载已有 PCD 地图,将当前激光帧通过点云配准(NDT/ICP)对齐到全局地图,得到 map -> base_link 变换。
  3. 导航阶段:把定位结果发布为 TF 与里程计,供 move_base 使用,并结合代价地图完成全局路径规划和局部避障。

整体数据流如下:

激光雷达点云

FAST-LIO2 建图

点云地图 PCD

NDT 重定位

IMU 数据

map -> base_link TF

move_base 导航栈

全局/局部规划器

轮速/底盘控制

其中,重定位阶段可以有两种实现思路:

  • 纯配准方案:每帧点云直接与全局地图做 NDT 配准,简单直接,适合地图规模中等、算力充足的场景。
  • 里程计 + 配准融合方案:用 FAST-LIO2 的高频输出作为运动预测初值,再用低频 NDT 做全局修正。该方案在退化环境(长廊、空旷区域)中鲁棒性更好,也是工程落地的常见做法。

3. 定位导航环境准备

3.1 安装基础依赖

sudo apt update
sudo apt install -y \
  ros-noetic-pcl-ros \
  ros-noetic-tf2-geometry-msgs \
  ros-noetic-navigation \
  ros-noetic-move-base \
  libpcl-dev \
  libeigen3-dev

3.2 创建工作空间并编译 FAST-LIO2

mkdir -p ~/fastlio_ws/src
cd ~/fastlio_ws/src
git clone https://github.com/hku-mars/FAST_LIO.git
cd ..
catkin_make -j$(nproc)
source devel/setup.bash

若使用 Livox 系列雷达,请先安装 livox_ros_driver,并在 FAST_LIO 的 CMakeLists.txt 中启用 Livox 支持(FAST-LIO2 建图框架)。Velodyne、Ouster、Hesai 等雷达通常可直接配置使用。

4. 已有地图准备:用 FAST-LIO2 建图

4.1 配置传感器参数

在 FAST_LIO/config 目录下找到对应雷达的 YAML 配置文件,需要重点核对如下参数:

common:
  lid_topic:  "/velodyne_points"     # 雷达点云话题
  imu_topic:  "/imu/data"            # IMU 话题
  time_sync_en: true                 # 时间同步
  time_offset_lidar_to_imu: 0.0

preprocess:
  lidar_type: 1                      # 1=Velodyne,2=Ouster,3=Livox 等
  scan_line: 16
  blind: 4

mapping:
  down_sample_size: 0.2              # 体素降采样尺寸
  max_iteration: 4                   # IESKF 迭代次数
  filter_size_map: 0.4               # 地图体素滤波尺寸

不同雷达型号的 lidar_type 取值请参考官方 README,配置错误会导致点云时间戳错乱或无法收敛。

4.2 启动建图

# 终端 1:启动雷达与 IMU 驱动,确认数据正常
roslaunch fast_lio mapping_velodyne.launch

启动后可在 Rviz 中订阅 /cloud_registered 话题查看聚合后的点云地图,同时订阅 /Odometry 查看轨迹是否平滑无漂移。建图时应尽量让机器人缓慢平稳移动,避免快速旋转和剧烈颠簸,以保证 IMU 前向传播的精度。

5. 已有点云地图的保存与预处理

部分 FAST-LIO2 版本支持在建图节点运行时按下 s 键保存全局地图。若你的版本未提供该功能,建议编写一个独立的保存节点,订阅聚合点云并落盘为 PCD:

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>

pcl::PointCloud<pcl::PointXYZI>::Ptr global_map(new pcl::PointCloud<pcl::PointXYZI>);

void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(*msg, *cloud);

    // 体素降采样,控制地图体量
    pcl::VoxelGrid<pcl::PointXYZI> vg;
    vg.setInputCloud(cloud);
    vg.setLeafSize(0.3f, 0.3f, 0.3f);
    pcl::PointCloud<pcl::PointXYZI>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZI>);
    vg.filter(*filtered);

    *global_map += *filtered;
    ROS_INFO_STREAM("Accumulated points: " << global_map->size());
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "map_saver");
    ros::NodeHandle nh;

    ros::Subscriber sub = nh.subscribe("/cloud_registered", 10, cloudCallback);

    ROS_INFO("Map saver started. Press Enter to save map...");
    std::string cmd;
    std::getline(std::cin, cmd);

    pcl::io::savePCDFileBinary("/home/user/map/global_map.pcd", *global_map);
    ROS_INFO("Map saved to /home/user/map/global_map.pcd");

    ros::shutdown();
    return 0;
}

地图保存后建议做一次离群点剔除,否则动态物体、镜面反射残留的稀疏点会影响后续配准质量。可以使用 PCL 的 StatisticalOutlierRemoval 完成:

pcl::StatisticalOutlierRemoval<pcl::PointXYZI> sor;
sor.setInputCloud(global_map);
sor.setMeanK(30);
sor.setStddevMulThresh(1.0);
sor.filter(*global_map_filtered);

6. 基于已有地图的重定位

6.1 使用 NDT 进行全局配准

NDT(Normal Distributions Transform)的核心思想是将点云空间划分为规则体素网格,用每个体素内的正态分布描述局部几何形状,从而把离散点云配准问题转化为连续概率密度函数的优化问题。相比直接匹配最近点的 ICP 方法,NDT 对噪声和局部点分布差异更鲁棒,也适合大规模点云的快速配准。其原理可拆为以下几步:

  1. 划分体素网格:将目标点云(这里的已有地图)所在空间按固定分辨率划分为三维体素。setResolution 即决定该体素边长。
  2. 构建正态分布:对每个体素内落入的点计算均值 μ \mu μ 与协方差矩阵 Σ \Sigma Σ。若某体素内点过少,则丢弃该体素,避免协方差矩阵奇异。这样地图就从“离散点集合”变成了“局部高斯分布集合”。
  3. 变换源点云:用当前位姿估计 T T T 把当前激光帧中的每个源点 p i \mathbf{p}_i pi​ 变换到地图坐标系,得到 T p i T\mathbf{p}_i Tpi​。
  4. 计算概率密度:在变换后的每个源点处,查询其所在体素的高斯分布,计算概率密度:

p ( T p i ) = 1 ( 2 π ) 3 / 2 ∣ Σ ∣ exp ⁡ ( − 1 2 ( T p i − μ ) T Σ − 1 ( T p i − μ ) ) p(T\mathbf{p}_i)=\frac{1}{(2\pi)^{3/2}\sqrt{|\Sigma|}}\exp\left(-\frac{1}{2}(T\mathbf{p}_i-\mu)^T\Sigma^{-1}(T\mathbf{p}_i-\mu)\right) p(Tpi​)=(2π)3/2∣Σ∣ ​1​exp(−21​(Tpi​−μ)TΣ−1(Tpi​−μ))

  1. 构建目标函数:NDT 的目标是找到使所有源点落在对应高斯分布中概率最大的位姿 T T T。为便于求解,通常取负对数似然并求和,形成得分函数:

J ( T ) = − ∑ i log ⁡ p ( T p i ) J(T)=-\sum_i \log p(T\mathbf{p}_i) J(T)=−i∑​logp(Tpi​)

  1. 迭代优化位姿:使用牛顿法对 J ( T ) J(T) J(T) 求导,不断更新位姿 T T T,直到位姿变化小于 setTransformationEpsilon 或达到 setMaximumIterations。每一步的更新步长由 setStepSize 控制。

由于 NDT 构建分布时只需要对地图体素做一次统计,后续每次配准仅需查询体素并计算梯度,因此即使地图规模较大,也能保持较高的定位频率。需要特别注意的是,NDT 本质仍是局部迭代优化方法,当初始位姿偏差过大时,源点可能落入错误体素,导致函数陷入局部极小,因此本方案中由 FAST-LIO2 里程计提供运动预测初值。核心实现流程如下:

#include <pcl/registration/ndt.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

// 加载全局地图
pcl::PointCloud<pcl::PointXYZI>::Ptr target(new pcl::PointCloud<pcl::PointXYZI>);
pcl::io::loadPCDFile("/home/user/map/global_map.pcd", *target);

pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> ndt;
ndt.setTransformationEpsilon(0.01);   // 收敛阈值
ndt.setStepSize(0.1);                 // 学习率
ndt.setResolution(1.0);               // 体素分辨率
ndt.setMaximumIterations(50);         // 最大迭代次数
ndt.setInputTarget(target);           // 目标:全局地图
ndt.setInputSource(current_cloud);    // 源:当前激光帧

// init_guess 为初始位姿估计,通常来自 FAST-LIO2 里程计或手动标定
pcl::PointCloud<pcl::PointXYZI> aligned;
ndt.align(aligned, init_guess);

if (ndt.hasConverged())
{
    Eigen::Matrix4f T = ndt.getFinalTransformation();
    // T 即 map -> base_link 的刚体变换
}
else
{
    ROS_WARN("NDT did not converge");
}

下面给出一个可直接落地的 ROS 重定位节点完整实现。该节点订阅 FAST-LIO2 输出的当前激光帧 /cloud_registered 与高频里程计 /Odometry,以里程计帧间增量持续维护 init_guess 运动预测,并以 1 Hz 低频执行 NDT 全局配准,再持续广播 map -> base_link 变换:

// relocalizer_node.cpp
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <Eigen/Geometry>
#include <mutex>

class FastLIO2Relocalizer
{
public:
    FastLIO2Relocalizer(ros::NodeHandle& nh)
    {
        // 1. 加载已有全局点云地图作为 NDT 配准目标
        pcl::PointCloud<pcl::PointXYZI>::Ptr global_map(new pcl::PointCloud<pcl::PointXYZI>());
        if (pcl::io::loadPCDFile("/home/user/map/global_map.pcd", *global_map) == -1)
        {
            ROS_ERROR("Failed to load global map PCD file");
            ros::shutdown();
            return;
        }
        ndt_.setInputTarget(global_map);

        // 2. NDT 参数:直接影响收敛性与配准精度
        ndt_.setTransformationEpsilon(0.01);   // 位姿变化收敛阈值,越小越精确但迭代次数更多
        ndt_.setStepSize(0.1);                 // 优化步长,过大会震荡、过小收敛慢
        ndt_.setResolution(1.0);               // 体素分辨率,建议取雷达点间距的 5-10 倍
        ndt_.setMaximumIterations(50);         // 最大迭代次数,防止不收敛时长时间占用

        // 3. 订阅 FAST-LIO2 的当前激光帧与高频里程计
        cloud_sub_ = nh.subscribe("/cloud_registered", 10,
                                  &FastLIO2Relocalizer::cloudCallback, this);
        odom_sub_ = nh.subscribe("/Odometry", 50,
                                 &FastLIO2Relocalizer::odomCallback, this);

        // 4. 低频定时器:1 Hz 执行一次 NDT 全局配准
        ndt_timer_ = nh.createTimer(ros::Duration(1.0),
                                    &FastLIO2Relocalizer::ndtTimerCallback, this);

        has_initial_guess_ = false;
        has_new_cloud_ = false;

        ROS_INFO("FastLIO2 relocalizer node started.");
    }

private:
    void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        pcl::fromROSMsg(*msg, *current_cloud_);
        has_new_cloud_ = true;
    }

    void odomCallback(const nav_msgs::OdometryConstPtr& msg)
    {
        std::lock_guard<std::mutex> lock(data_mutex_);

        Eigen::Quaternionf q(msg->pose.pose.orientation.w,
                             msg->pose.pose.orientation.x,
                             msg->pose.pose.orientation.y,
                             msg->pose.pose.orientation.z);
        Eigen::Vector3f t(msg->pose.pose.position.x,
                          msg->pose.pose.position.y,
                          msg->pose.pose.position.z);

        // 首次收到里程计:用它初始化全局位姿,等价于 Rviz 手动给定初始位姿
        if (!has_initial_guess_)
        {
            init_guess_ = Eigen::Isometry3f::Identity();
            init_guess_.linear() = q.toRotationMatrix();
            init_guess_.translation() = t;
            last_odom_ = init_guess_;
            has_initial_guess_ = true;
            return;
        }

        // 计算当前里程计相对上一时刻的帧间运动 ΔT
        Eigen::Isometry3f cur_odom = Eigen::Isometry3f::Identity();
        cur_odom.linear() = q.toRotationMatrix();
        cur_odom.translation() = t;
        Eigen::Isometry3f delta = last_odom_.inverse() * cur_odom;

        // 运动量太小时不累积,避免高频噪声引起漂移
        if (delta.translation().norm() < 0.01 &&
            Eigen::AngleAxisf(delta.linear()).angle() < 0.001)
            return;

        // 将运动增量叠加到 NDT 的位姿预测中,提供良好的配准初值
        init_guess_ = init_guess_ * delta;
        last_odom_ = cur_odom;
    }

    void ndtTimerCallback(const ros::TimerEvent&)
    {
        std::lock_guard<std::mutex> lock(data_mutex_);

        // 没有初始位姿或没有新点云时不配准
        if (!has_initial_guess_ || !has_new_cloud_)
            return;

        // 以运动模型给出的预测值作为 NDT 初值,执行低频全局配准
        ndt_.setInputSource(current_cloud_);
        pcl::PointCloud<pcl::PointXYZI> aligned;
        ndt_.align(aligned, init_guess_.matrix());

        if (ndt_.hasConverged())
        {
            Eigen::Matrix4f T = ndt_.getFinalTransformation();
            Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
            pose.matrix() = T;
            init_guess_ = pose;

            // 发布 map -> base_link TF
            geometry_msgs::TransformStamped ts;
            ts.header.stamp = ros::Time::now();
            ts.header.frame_id = "map";
            ts.child_frame_id = "base_link";
            ts.transform.translation.x = init_guess_.translation().x();
            ts.transform.translation.y = init_guess_.translation().y();
            ts.transform.translation.z = init_guess_.translation().z();

            Eigen::Quaternionf rot(init_guess_.linear());
            ts.transform.rotation.w = rot.w();
            ts.transform.rotation.x = rot.x();
            ts.transform.rotation.y = rot.y();
            ts.transform.rotation.z = rot.z();

            tf_broadcaster_.sendTransform(ts);
            has_new_cloud_ = false;
        }
        else
        {
            ROS_WARN("NDT did not converge, keep last pose.");
        }
    }

    pcl::NormalDistributionsTransform<pcl::PointXYZI, pcl::PointXYZI> ndt_;
    pcl::PointCloud<pcl::PointXYZI>::Ptr current_cloud_{new pcl::PointCloud<pcl::PointXYZI>()};

    ros::Subscriber cloud_sub_;
    ros::Subscriber odom_sub_;
    ros::Timer ndt_timer_;
    tf2_ros::TransformBroadcaster tf_broadcaster_;

    bool has_initial_guess_;
    bool has_new_cloud_;
    Eigen::Isometry3f init_guess_ = Eigen::Isometry3f::Identity();
    Eigen::Isometry3f last_odom_ = Eigen::Isometry3f::Identity();
    std::mutex data_mutex_;
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "fast_lio2_relocalizer");
    ros::NodeHandle nh;

    FastLIO2Relocalizer relocalizer(nh);

    ros::spin();
    return 0;
}

编译时需在 CMakeLists.txt 中链接 pcl_ros、tf2_ros、nav_msgs 等依赖。若 FAST-LIO2 节点已发布 odom -> base_link,为避免 TF 冲突,可在本节点中改为发布 map -> odom,由 FAST-LIO2 继续维护 odom -> base_link,二者共同组成 map -> odom -> base_link 树。

6.2 初始位姿的获取

NDT 属于局部优化方法,需要较好的初始位姿。常见获取方式:

  • 人工给定:在 Rviz 中使用 2D Pose Estimate 工具指定机器人在全局地图中的初始位置。
  • 固定起点:约定机器人每次从相同位置启动,复用上一次的终点位姿作为本次初值。
  • 场景识别:使用 Scan Context 等全局描述子检索历史帧,获取候选初始位姿。

本教程采用“Rviz 手动指定 + FAST-LIO2 里程计预测”的方式,既能保证演示效果,又便于在实际场景中快速验证。

6.3 里程计 + 配准融合

在工程实现中,FAST-LIO2 提供的高频里程计可以作为运动模型,为 NDT 配准提供良好的帧间初值预测;NDT 则以较低频率跑全局配准,抑制里程计的累积漂移。定位节点维护一个高频发布 TF 的线程和一个低频配准线程:

FAST-LIO2 里程计(高频,如 10Hz)
        │  提供帧间相对运动 ΔT
        ▼
运动预测得到粗略位姿
        │
        ▼
NDT 全局配准(低频,如 1-2Hz)
        │  输出全局位姿 T_map_base
        ▼
发布 map -> odom -> base_link TF

最终发布到 TF 树中的 map -> base_link 关系应保持连续、平滑,避免低频配准造成的跳变。可以在两次配准之间使用线性插值或滤波器平滑位姿。

7. 接入 ROS Navigation 实现路径规划

有了稳定的全局定位后,即可接入 move_base 实现导航。move_base 通过 TF 获取机器人位姿,结合代价地图规划路径并输出速度指令。

7.1 准备二维栅格地图

move_base 默认使用二维占据栅格地图。由于我们的地图是三维点云,需要将点云投影为 2D 占用信息。常用做法是先将三维点云降维为二维激光扫描:

rosrun pointcloud_to_laserscan pointcloud_to_laserscan_node \
  cloud_in:=/cloud_registered \
  scan:=/scan \
  angle_min:=-3.1416 \
  angle_max:=3.1416 \
  angle_increment:=0.007 \
  range_min:=0.3 \
  range_max:=30.0

随后使用 map_server 将预先构建的 2D 栅格地图加载为全局代价地图:

# 生成 2D 栅格地图后保存为 pgm/yaml
rosrun map_server map_saver -f ~/map/nav_map

7.2 配置 move_base

在 move_base 参数中指定全局与局部规划器、代价地图参数:

# move_base_params.yaml
base_global_planner: "global_planner/GlobalPlanner"
base_local_planner: "teb_local_planner/TebLocalPlannerROS"
controller_frequency: 10.0
planner_frequency: 2.0
recovery_behavior_enabled: true

# costmap_common_params.yaml
obstacle_range: 3.0
raytrace_range: 3.5
footprint: [[-0.25, -0.2], [-0.25, 0.2], [0.25, 0.2], [0.25, -0.2]]
robot_radius: 0.25

启动导航栈:

roslaunch my_robot_navigation navigation.launch

启动后,在 Rviz 中点击 2D Nav Goal 选择目标点,机器人应能规划出全局路径,并在局部规划器控制下避开动态障碍物到达目标。

8. 关键参数与调优建议

  • NDT 分辨率:setResolution 建议设置为激光雷达点间距的 5-10 倍。分辨率过大配准不够精确,过小则计算量显著增长,且容易陷入局部极小。
  • 配准频率:全局 NDT 配准不必每帧执行,1-2Hz 的修正频率配合 10Hz 的 FAST-LIO2 里程计即可保证平滑性。
  • 地图体素大小:filter_size_map 过小会导致地图文件庞大、加载缓慢;过大会丢失走廊、扶手等细结构。室内建议 0.2-0.4 米。
  • 退化检测:在长廊、隧道等几何退化环境中,NDT 可能在沿走廊方向出现漂移。可通过配准协方差矩阵的特征值判断退化方向,并结合轮式里程计约束该自由度。
  • 动态物体干扰:建图阶段尽量选择人流车流较少的时间窗口;若无法避免,可在保存地图前做离群点剔除和多次扫描叠加。

9. 常见问题排查

现象可能原因排查建议
NDT 无法收敛初始位姿偏差过大检查初始位姿是否接近真实位置,缩小体素分辨率
配准结果跳动严重地图存在离群点或动态残留对地图做统计滤波,提高配准平滑度
TF 树断裂定位节点未发布 map -> odom检查 TF 广播器是否持续发布,频率是否满足 10Hz
move_base 无路径代价地图与真实环境不一致核对栅格地图分辨率、机器人足迹半径配置
点云时间戳错乱time_sync_en 或雷达类型配置错误检查 YAML 中的 lidar_type 与时间同步开关

10. 总结

本文梳理了“FAST-LIO2 建图 ➜ 点云地图保存 ➜ NDT 重定位 ➜ move_base 导航”的完整技术链路。FAST-LIO 系列凭借 IESKF 紧耦合框架,在建图阶段能够输出高质量、全局一致的点云地图;定位阶段则以里程计运动模型配合 NDT 全局配准,实现稳定、连续的全局位姿估计;最后接入 ROS Navigation 完成路径规划与避障,形成一套可落地的激光导航方案。

在实际部署中,还可以进一步引入闭环检测(如 Scan Context + 位姿图优化)提升大场景地图一致性,或使用 OctoMap 压缩三维地图体积。对于算力受限的平台,可以将定位频率、体素分辨率和地图规模作为主要优化杠杆。掌握这套流程后,可以将其迁移到仓储、巡检、配送等多种轮式/履带平台中。

Logo

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

更多推荐