基于 FAST-LIO2 的已有地图定位导航
基于 FAST-LIO2 的已有地图定位导航
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. 已有地图定位导航整体方案
整套流程可拆分为三步:
- 建图阶段:运行 FAST-LIO2,在目标环境中完整扫描一遍,输出全局一致的点云地图并保存为 PCD 文件。
- 重定位阶段:机器人启动后,加载已有 PCD 地图,将当前激光帧通过点云配准(NDT/ICP)对齐到全局地图,得到
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 对噪声和局部点分布差异更鲁棒,也适合大规模点云的快速配准。其原理可拆为以下几步:
- 划分体素网格:将目标点云(这里的已有地图)所在空间按固定分辨率划分为三维体素。
setResolution即决定该体素边长。 - 构建正态分布:对每个体素内落入的点计算均值 μ \mu μ 与协方差矩阵 Σ \Sigma Σ。若某体素内点过少,则丢弃该体素,避免协方差矩阵奇异。这样地图就从“离散点集合”变成了“局部高斯分布集合”。
- 变换源点云:用当前位姿估计 T T T 把当前激光帧中的每个源点 p i \mathbf{p}_i pi 变换到地图坐标系,得到 T p i T\mathbf{p}_i Tpi。
- 计算概率密度:在变换后的每个源点处,查询其所在体素的高斯分布,计算概率密度:
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∣Σ∣1exp(−21(Tpi−μ)TΣ−1(Tpi−μ))
- 构建目标函数: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)
- 迭代优化位姿:使用牛顿法对
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 压缩三维地图体积。对于算力受限的平台,可以将定位频率、体素分辨率和地图规模作为主要优化杠杆。掌握这套流程后,可以将其迁移到仓储、巡检、配送等多种轮式/履带平台中。
DAMO开发者矩阵,由阿里巴巴达摩院和中国互联网协会联合发起,致力于探讨最前沿的技术趋势与应用成果,搭建高质量的交流与分享平台,推动技术创新与产业应用链接,围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐


所有评论(0)