要说速腾16线雷达配FAST-LIO2这件事我前前后后折腾了两个礼拜中间踩过的坑比吃的盐还多。网上也不是没有教程但要么只讲驱动要么只讲算法能直接把“速腾16线雷达 FAST-LIO2 ROS Melodic”串起来跑通的保姆级文章我是真没找到。所以这篇我决定把自己从零开始的完整过程写下来包括驱动怎么配、源码怎么编译、launch和yaml怎么改、外参怎么设、建图效果漂移了怎么排查全部一次讲透。这篇文章适合什么读者就是你手里有一台速腾RS-LiDAR-16或者类似的多线机械雷达电脑上装好了Ubuntu 18.04和ROS Melodic想跑FAST-LIO2做实时定位建图但对着GitHub仓库不知道从哪下手的人。如果你已经能正常看到雷达点云但不清楚怎么把数据喂给FAST-LIO2这篇同样适合你。我默认你懂最基本的ROS命令比如roscore、roslaunch、rostopic不需要你是大佬但别完全没见过终端。先说结论速腾16线完全能跑FAST-LIO2但前提是搞清楚三个关键点。第一雷达驱动必须输出标准的sensor_msgs/PointCloud2话题并且话题格式要符合FAST-LIO2对机械雷达点云的预期第二除了雷达你还必须有一个IMU因为FAST-LIO2是激光惯性紧耦合里程计没有IMU它启动都起不来第三launch和yaml文件里的参数必须改对尤其是lidar_type、scan_line、topic名称和外参任何一个不对效果都会非常拉胯。下面我开始一步步说。1. 项目全貌与准备1.1 速腾16线雷达和FAST-LIO2是什么先捋一下这两个主角。速腾RS-LiDAR-16是RoboSense家非常经典的一款16线机械式激光雷达水平360度扫描垂直视角大概30度上下各15度测距范围在20厘米到150米左右点频约30万点每秒默认通过以太网UDP协议输出点云。它在户外机器人和低速无人车上用得很多因为性价比高、装车简单。FAST-LIO2是香港大学MaRS Lab开源的激光惯性里程计全称是Fast LiDAR-Inertial Odometry核心思路是用迭代扩展卡尔曼滤波器把激光雷达点云和IMU数据紧耦合在一起一边算状态估计一边更新ikd-Tree维护的地图。相比LOAM这类纯激光里程计它的优势是运动畸变处理更好、初始化快、地图局部精度高而且对计算资源要求比较友好。这两者搭配起来很自然速腾16线提供三维点云IMU提供角速度和加速度FAST-LIO2把两者融合输出雷达位姿和增量地图。实际测试下来在室内和园区场景只要参数调对、运动不过分剧烈效果能做到比较稳定的厘米到分米级精度。当然你要指望它像多线高精雷达那样做超大场景的全局地图那不太现实但做实时定位建图是完全够用的。1.2 软硬件环境清单我这次使用的环境是笔记本电脑一台CPU是i7-8750H内存16GB核显就够不需要独显Ubuntu 18.04.5ROS Melodic完整版ros-melodic-desktop-full速腾RS-LiDAR-16一台用的是在线网络连接模式不是离线pcap包IMU用的是自制的MPU6050模块通过Arduino转串口发ROS消息话题名/imu/data消息类型sensor_msgs/Imu频率200Hz雷达和电脑用网线直连雷达默认IP 192.168.1.200电脑手动设置IP为192.168.1.102需要注意FAST-LIO2的代码对ROS版本要求其实不高Melodic和Noetic都能编译。但是驱动部分如果你用RoboSense官方新版rslidar_sdk它对ROS版本有一些严格要求Melodic没问题。如果你的系统是Ubuntu 20.04 ROS Noetic驱动部分可能要换用更新的SDK版本这个后面细说。1.3 动手前必须搞懂的四个概念动手之前我先讲几个绕不开的概念搞懂它们能帮你少走很多弯路。第一是点云话题消息类型。驱动输出的点云话题消息类型通常是sensor_msgs/PointCloud2。FAST-LIO2接收的话题类型在官方代码里也兼容PointCloud2所以关键在于设置正确的topic名称比如/rslidar_points或者/velodyne_points只要两边对上就行。第二是lidar_type参数。FAST-LIO2预处理代码里用整数区分不同雷达的点云结构0代表Livox系列1代表Velodyne这类机械旋转雷达2代表Ouster。速腾16线本质上也是机械旋转雷达点云排列方式和Velodyne类似所以一般设置lidar_type为1。如果你的点云时间戳格式特殊可能还需要改preprocess代码但大多数情况下用1就行。第三是点云的时间戳类型。FAST-LIO2做运动畸变补偿时需要知道每个点是在扫描周期内的哪个时刻采到的。机械雷达消息里每个点的相对时间通常存在PointCloud2的time字段PointXYZIRT格式有time如果你的驱动输出的是不带每点时间的PointXYZI那FAST-LIO2只能用默认方式近似也就是把一次扫描里的点都安排到0到0.1秒的时间跨度里这个需要和驱动配置配合好。第四是外参。雷达和IMU之间的安装位置关系用旋转矩阵和平移向量表示定义在FAST-LIO2的yaml文件里。如果外参不准里程计会漂移严重时甚至崩掉。稍后我会仔细讲怎么设。2. 驱动安装把速腾16线点云跑起来2.1 驱动版本怎么选RoboSense官方的雷达驱动有好几个版本最常用的是rslidar_sdk和老的rslidar_pointcloud dDriver。我自己推荐用rslidar_sdk它是官方新出的统一SDK支持RS16、RS32、RS80、RS128这些型号同时支持在线和pcap离线模式编译方式简单ROS集成度也好。老版的rslidar_pointcloud是围绕rslidar_msgs和rslidar_driver拆分的比较旧在新系统上编译经常遇到protobuf或者Boost版本问题。所以如果你没有历史包袱直接用rslidar_sdk。到我现在写的时候rslidar_sdk的稳定版本是1.5.xGitHub地址是RoboSense-LiDAR/rslidar_sdk。2.2 编译rslidar_sdk先把驱动代码克隆到工作空间。我习惯建一个catkin_ws然后在src目录下放代码。cd ~/catkin_ws/src git clone https://github.com/RoboSense-LiDAR/rslidar_sdk.git cd rslidar_sdk git submodule update --init --recursiverslidar_sdk依赖YAML库。如果你不是用catkin直接编译而是单独cmake需要先装libyaml-cpp-dev。但我们用ROS catkin方式编译时也有可能会遇到找不到yaml-cpp的情况所以先装依赖sudo apt-get install libyaml-cpp-dev cd ~/catkin_ws catkin_make source devel/setup.bash编译过程中如果报找不到ROS头文件确认你已经source过/opt/ros/melodic/setup.bash。如果报找不到yaml-cpp确认libyaml-cpp-dev装好了并且在CMakeLists.txt里能看到对yaml-cpp的find_package。2.3 修改雷达驱动配置rslidar_sdk的配置文件在rslidar_sdk/config/rslidar_config.yaml。我的配置大概长这样common: frame_id: rslidar lidar_type: RS16 time_sync_en: false pcap_file: driver: lidar_type: RS16 msop_port: 6699 difop_port: 7788 lidar_ip: 192.168.1.200 host_ip: 192.168.1.102 pcap: false ros: ros_recv_port: 6699 pointcloud_topic: /rslidar_points pointcloud_type: 0 frame_id: rslidar需要解释几个关键项lidar_type填写RS16SDK会根据这个值选择对应的解析协议。msop_port和difop_port保持默认6699和7788不要随便改。重点是把lidar_ip改成雷达本机IPhost_ip改成你电脑的IP。如果找不到雷达多半就是这两个IP没配对。pointcloud_topic输出点云的话题名后面FAST-LIO2要用同一个名字。pointcloud_type0代表输出的点云包含XYZI字段1代表包含XYZIRT字段也就是每个点带时间戳和反射率。这里要注意如果你希望FAST-LIO2能利用逐点时间做畸变补偿建议把pointcloud_type设为1输出PointXYZIRT格式。但FAST-LIO2的通用机械雷达写法读取的是point.x,y,z和intensity以及它内部的time结构。实测中pointcloud_type0也能跑但我们后面会聊到时间戳问题。改好配置后用launch启动驱动roslaunch rslidar_sdk start.launch这里有个坑rslidar_sdk的launch文件名叫start.launch不是rslidar_sdk.launch。启动后用 rostopic hz /rslidar_points 查看如果正常应该看到10Hz左右的点云频率。再用rviz订阅/rslidar_points选择Fixed Frame为rslidar就能看到点云了。2.4 网络配置和常见启动失败问题雷达和电脑之间是用网线直连还是经过交换机配置思路不太一样。我建议直连简单可靠。首先要设置电脑的静态IP。Ubuntu 18.04上我用netplan配置如下# /etc/netplan/01-network-manager-all.yaml network: version: 2 renderer: NetworkManager ethernets: eth0: dhcp4: no addresses: - 192.168.1.102/24然后 sudo netplan apply 让配置生效。接着查看网卡状态确保IP地址已经变成192.168.1.102。这时候再ping雷达的IPping 192.168.1.200能ping通就说明链路没问题。如果ping不通可以先检查网线、网口再检查防火墙sudo ufw disable还有个小细节雷达的默认IP有时不是192.168.1.200而是192.168.1.201或者别的。如果你不确定可以用RoboSense官方的上位机软件查看或者在启动驱动后看终端日志它会打印雷达的IP地址。启动驱动后如果rviz里看不到点云先用rostopic info /rslidar_points确认话题有没有发出消息。如果话题存在但频率是0说明驱动没收到UDP数据大概率是IP或者端口没对。如果话题频率正常但rviz不显示多半是Frame ID设置问题在rviz里把Fixed Frame改成rslidar。3. FAST-LIO2源码编译与依赖安装3.1 依赖准备FAST-LIO2的编译依赖主要有ROS包、PCL、Eigen、Sophus还有livox_ros_driver。如果你用的是原版FAST-LIO2CMakeLists里默认会找livox_ros_driver就算你不用Livox雷达它也会强制要求这个包存在否则会报找不到livox_ros_driver。我第一次编译就被这个卡了很久。我的解决方法是先安装livox_ros_driver但不一定非要用它。安装方式sudo apt install ros-melodic-livox-ros-driver如果你的ROS版本没有这个包可以从GitHub拉源码编译。其他基础依赖sudo apt install ros-melodic-pcl-ros ros-melodic-cv-bridge libpcl-dev libeigen3-devEigen版本要注意Ubuntu 18.04系统自带的Eigen是3.3.x已经够用。别去折腾Eigen 3.4反而容易出问题。Sophus是李群李代数库FAST-LIO2的代码里直接include了sophus/so3.h。你可以从GitHub拉取并编译安装到系统里git clone https://github.com/strasdat/Sophus.git cd Sophus mkdir build cd build cmake .. make -j4 sudo make install如果你是通过git submodule方式拉到了FAST-LIO2自带的Sophus同样要进到Thirdparty/Sophus里编译一次。3.2 正式编译FAST-LIO2假设你已经建好了catkin_wscd ~/catkin_ws/src git clone https://github.com/hku-mars/FAST-LIO2.git cd .. catkin_make如果之前编译过rslidar_sdkcatkin_make会把所有功能包一起编译需要多等一会儿。如果只想编译FAST-LIO2可以用catkin_make --pkg fast_lio编译完成的标志是生成了devel/lib/fast_lio/fastlio_mapping这个可执行文件。3.3 编译错误实录下面几个错误是我实际踩过的写出来供你对照错误1找不到sophus/Sophus 解决方法确认Sophus已经sudo make install或者在CMakeLists里添加Sophus_DIR路径。有时还需要在~/.bashrc里加export Sophus_DIR/usr/local/lib/cmake/Sophus。错误2找不到livox_ros_driver 解决方法sudo apt install ros-melodic-livox-ros-driver装完重新catkin_make即可。错误3Eigen和PCL头文件冲突 报错信息类似“Eigen/Dense: No such file or directory”。Ubuntu 18.04上我遇到过一次原因是在include头文件顺序上出了问题。解决方法是确保CMakeLists里把Eigen的include目录放在PCL之前不过这个属于少数情况先不用管。错误4编译卡在opencv相关 报错找不到opencv2/xfeatures2d等。FAST-LIO2其实不完全依赖opencv的contrib模块但catkin自动解析会报。我直接sudo apt install libopencv-dev解决。如果你已经装了ROS desktop-full一般没问题。4. 配置与运行把速腾数据喂给FAST-LIO24.1 新建rslidar的launch和yamlFAST-LIO2的launch文件在src/launch目录下官方给了几个示例比如mapping_avia.launch、mapping_ouster.launch。我们需要新建一个mapping_rslidar.launch内容如下launch node pkgfast_lio typefastlio_mapping namefastlio_mapping outputscreen rosparam file$(find fast_lio)/config/rslidar.yaml / /node /launch然后复制一个yaml比如从config/velodyne.yaml复制改为rslidar.yaml。我的核心配置如下common: lid_topic: /rslidar_points imu_topic: /imu/data time_sync_en: false preprocess: lidar_type: 1 scan_line: 16 blind: 0.5 time_scale: 1e-3 mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 360 det_range: 100.0 extrinsic_est_en: false extrinsic_T: [0.0, 0.0, 0.0] extrinsic_R: [1, 0, 0, 0, 1, 0, 0, 0, 1]这些参数的含义lid_topic雷达点云话题和驱动输出一致。imu_topicIMU数据话题必须存在且频率不能太低建议100Hz以上。lidar_type为1对应机械旋转雷达。如果你的驱动输出每点带时间戳可以用1部分版本的FAST-LIO2对Livox点云做时间处理对机械雷达则直接读取点云消息里的header.stamp和每点时间如果有的话。scan_line设为16线数必须和雷达一致否则预处理时按行切分点云会出错。blind是盲区距离设置为0.5米分辨率内的点被剔除防止近处杂散点影响配准。time_scale用于把点云时间戳的单位换算成秒一般机械雷达是1e-3毫秒Livox是1e-6微秒。如果你发现建图时间轴错乱多半是这个参数不对。extrinsic_T和extrinsic_R分别是IMU在雷达坐标系下的平移和旋转。如果你把IMU放在雷达正上方且朝向一致平移项可能要改旋转为单位阵。4.2 话题映射和IMU准备启动FAST-LIO2前先确认驱动和IMU节点都已经运行并且三个话题都能正常收到数据/rslidar_pointssensor_msgs/PointCloud2/imu/datasensor_msgs/Imu用rostopic echo /imu/data/header.stamp看一眼时间戳是否正确。IMU时间戳乱跳会直接导致滤波器发散这是我们实际踩坑最重的一款。FAST-LIO2要求IMU数据频率足够高。官方README建议至少100Hz我的200Hz效果很好。如果你的IMU只有50Hz也能跑但抖动会明显一些。如果暂时没有IMU只靠雷达也能跑纯激光版本吗不行因为FAST-LIO2的传播方程依赖IMU激励。你可以用cv2_ros之类的仿真器生成虚拟IMU但那不是真实场景。所以请务必准备一个IMU。4.3 外参标定与配置心得外参是FAST-LIO2最容易翻车的点。官方参数中extrinsic_T表示IMU在雷达坐标系下的平移向量extrinsic_R是旋转矩阵表达的是从雷达到IMU的旋转或者说代码里定义了I_T_L [extrinsic_R, extrinsic_T]表示雷达在IMU坐标系下的位姿不同版本含义不完全一样这里我们不纠结数学只说怎么设。如果你的IMU和雷达安装在一个刚性支架上且两者大概平行可以先用近似值。比如IMU放在雷达正上方10cm则extrinsic_T大概就是[0, 0, 0.1]。旋转部分先设单位阵跑起来之后如果发现方向明显不对再翻转旋转矩阵的对应行。注意外参不准确会导致建图在起步阶段就迅速漂移。我第一次用外参[0, 0, 0]但我的IMU实际在y方向偏了5cm结果地图拐弯时出现了严重的s形漂移。后来用卷尺量了一下改成[0, 0.05, 0]之后效果立刻正常了。所以最省事的做法是先用尺子量一个大概的平移量旋转矩阵用单位阵先让系统能跑通再用标定工具优化。如果对旋转敏感可以试试把外参当作可优化量设置extrinsic_est_en为true让FAST-LIO2在后端自动估计。但我个人经验是外参初始误差过大时自动估计很难收敛甚至会让系统崩溃。建议先把大致外参估准再考虑开自动估计。4.4 完整运行流程一切准备就绪后按下面顺序启动终端1roslaunch rslidar_sdk start.launch终端2rosrun imu_node my_imu_node终端3roslaunch fast_lio mapping_rslidar.launchFAST-LIO2启动后在终端会不断打印update及其耗时。初始几秒内如果没有运动它不会输出位姿你需要拿着雷达或者把雷达装在机器人上小幅晃动几次让它完成初始化。如果一切正常会在控制台看到类似下面的输出[ INFO] [1700000000.123456789]: LiDAR motion start! [ INFO] [1700000000.456789012]: Update: 2.123 ms如果你是用rosbag离线测试那就直接播放bagrosbag play your_data.bag注意bag里的雷达话题和IMU话题必须和yaml里一致。如果不一致可以用rosbag play的remap功能比如rosbag play your_data.bag /rslidar_points:/lidar_points /imu_data:/imu/data不过我还是建议直接改yaml里的lid_topic和imu_topic一劳永逸。运行过程中FAST-LIO2会发布几个重要话题包括/Odometry里程计位姿类型nav_msgs/Odometry/path轨迹类型nav_msgs/Path/cloud_registered配准后的当前帧点云/cloud_registered_raw原始点云/map局部地图点云在rviz里添加/cloud_registered和/map就能看到动态建图过程。4.5 用rviz实时查看建图效果启动rviz固定坐标系选为camera_init或bodyFAST-LIO2的全局坐标系叫camera_init虽然是激光雷达里程计但它沿用了LOAM的命名。把Fixed Frame设为camera_init然后添加PointCloud2显示选择/cloud_registered保持模式为Map这样能看到点云一张一张拼接起来。接着添加Path或Odometry查看轨迹是否平滑。如果初始瞬间地图就糊成一团先不要慌可能是运动太快导致点云畸变或者外参没设对。可以试着把雷达固定不动几秒后再以缓慢速度绕一圈让初始化更稳定。实际测试中启动时用手拿着设备在原地小幅度晃动几次往往最有效。5. 建图效果优化与避坑实录5.1 建图漂移的排查方向我做了几组实验发现建图漂移大多源于这几类问题按出现频率排序如下最常见的是IMU外参不准通常表现为转弯时地图错位起步阶段轨迹漂移。解决方法是用尺子量准平移量旋转矩阵尽量用水平仪校准或者拿标定工具做一次标定。第二常见的是点云时间戳混乱。如果你的驱动输出的点云不带逐点时间戳FAST-LIO2会把一帧的所有点当作同时刻采集来处理这就会导致运动畸变补偿失效在快速旋转时地图出现撕裂。解决方法是把rslidar_sdk的pointcloud_type设为1输出PointXYZIRT格式然后在驱动配置里把time_sync_en设为true确保每点时间戳以毫秒为单位并把FAST-LIO2的preprocess/time_scale设为1e-3。如果驱动输出的是系统时间而非雷达扫描相对时间效果依然不好建议参考官方FAST-LIO2对Velodyne的处理方式。第三是扫描线数配置错误。scan_line必须与雷达线数完全一致有次我手滑设成32FAST-LIO2在预处理阶段把16线数据按32行切分点云索引直接越界程序崩溃。第四是盲区设置过小。如果blind设置成0近距离的尘埃和杂点会大量进入配准导致位姿抖动。5.2 点云类型与时间戳这里展开讲一下点云类型。fast_lio2的源码里预处理模块会判断lidar_type如果是1机械雷达它会把收到的PointCloud2转换成PCL的PointCloud 其中PointType定义通常包含x,y,z,intensity和time。如果你用PointXYZI的消息time字段是0但FAST-LIO2里对机械雷达的处理不是完全依赖time字段而是根据点的线号和时间戳的插值来补具体看代码里InputFeature函数。如果你不想深究代码最简单的办法是让驱动输出点云时带上每点的时间信息。rslidar_sdk的pointcloud_type参数0表示输出PointXYZI1表示输出PointXYZIRT。我们建议设1并在启动驱动后看看topic里的字段rostopic echo /rslidar_points -n1输出里应该有time字段才对。如果没有检查驱动配置。若用的是老驱动可能这个参数不生效需要自己改一下发布逻辑。5.3 关键参数调优心得我调试过程中尝试过很多组参数下面几个改动收益最大point_filter_num这个参数在FAST-LIO2的yaml里叫point_filter_num表示每隔多少点取一个点默认4我改成6之后建图速度明显加快精度几乎没有损失。如果你的CPU较弱可以改成10甚至16但太大会损失细节。fov_degree默认360如果你的雷达实际水平视场不是360度比如某些车载雷达需要按实际设置速腾16是360度保持默认即可。det_range默认100米但速腾16的有效距离也差不多这样。如果你在室内小场景可以减小到30米减少远点噪声影响。extrinsic_est_en我先设false等外参差不多准后再设true。在室外试过开自动估计大多数时候能收敛但室内特征少的时候偶尔会飘。acc_cov和gyr_cov这两个是IMU噪声方差。默认0.1对多数IMU都适用。如果你用的IMU噪声很大可以稍微加大到0.2但不要过大否则滤波更信任雷达容易初始化困难。5.4 实测避坑清单我把实际操作中最容易踩的坑整理成表格方便你对照现象大概率原因解决方法启动后无位姿输出IMU话题没匹配或运动不充分检查imu_topic正常发布/imu/data手持设备晃几下地图快速发散外参错误量准平移量旋转矩阵先设单位阵再微调转向时地图撕裂点云时间戳异常驱动输出PointXYZIRTtime_scale设为1e-3建图卡顿严重CPU过载点云太密调大point_filter_num减小det_range地图出现层叠错位IMU频率过低提高IMU发布频率到100Hz以上启动即崩溃报错out of rangescan_line与实际线数不符改为16并检查预处理代码中线数读取方式还有两个小技巧。第一个录制bag时尽量让机器人先静止3秒再缓慢启动这能给FAST-LIO2一个稳定的IMU初始化窗口。第二个在室外建图时地面点会影响配准可以在yaml里把blind调大一点比如1.0把近距离地面噪声剔除或者在预处理里加一个高度过滤但这个需要改代码新手可以先忽略。最后分享一个我的个人操作习惯每次改完参数后我会把原来的bag用同一个bag测试并分别保存生成的点云pcd文件用pcl_compare或者可视化工具对比效果。这样调参不会变成“玄学”每一次改动都有数据支撑。尤其是外参从[0,0,0]改成[0,0.05,0]这种微调整用同一份bag测试对比轨迹误差很快就能找到最佳值。这个项目后续我还在继续扩展比如用IMU预积分配合轮式里程计做室内定位以及把FAST-LIO2输出的地图导入到navigation栈里做路径规划。如果你也在折腾同样的方案希望这篇记录能帮你省下几天时间。有什么问题欢迎在评论区交流我看到都会回。