【FAST-LIO2】45°雷达坐标系对齐——输入端旋转方案的完整踩坑与修复

前言

• 【10天速通ROS2-PX4无人机】(二) FAST-LIO2 仿真部署，200HZ IMU前推的无限魅力
• 【FAST-LIO2 源码解读】从 Mid-360 到 ESEKF — 公式与代码逐行对照
• 刚好前两期我们都完成了FAST-LIO2的仿真部署与源码解读
• 最近在机器狗项目中遇到一个问题：Mid-360雷达不是水平安装的，而是以45° 倾斜固定在机器狗前方——这样做是为了让雷达能看到更多地面和前方障碍物
• 但直接打开FAST-LIO2后出现了三个诡异现象：里程计一直在飞（Z 轴不断升高）、Rviz 里狗和路径都歪了 45°、直走还凑合，一转弯就 “升天” 或 “入地”
• 本文记录这个问题的根因分析、方案对比、以及最终落地的输入端旋转方案

1 问题描述

• 机器狗前面放置的雷达是倾斜 45 度放置的，这样是为了能够看到更多的地面特征
• 在URDF中，laser_livox（雷达）通过joint挂在base上：

<!-- robot.xacro --><jointname="laser_livox_joint"type="fixed"><originxyz="0.2 0 0.08"rpy="0 0.785 0"/><parentlink="base"/><childlink="laser_livox"/></joint><jointname="livox_imu_joint"type="fixed"><parentlink="laser_livox"/><childlink="livox_imu_link"/><originxyz="-0.011 -0.02329 0.04412"rpy="0 0 0"/></joint>

• 坐标系关系：

base (水平, Z朝上) └── laser_livox (rpy=0, 0.785, 0 → +45° pitch) └── livox_imu_link (rpy=0, 0, 0 → 同姿态)

• 说人话：base是水平的，雷达和 IMU 都固定在同一块板子上，整块板子前倾 45°，所以雷达和 IMU 的坐标系都是斜的

• 如果这个时候直接使用默认配置打开FAST-LIO2，会出现严重问题：
  • 狗往前走，里程计 Z 轴持续升高——走几米就 “飞” 到几米高
  • 狗和路径都倾斜 45°，世界坐标系却是正的
  • 直走勉强能看，转弯就 “升天” 或 “入地”

2 根因分析

2-1 坐标系关系

• FAST-LIO2的body帧 = IMU 所在的坐标系
• 我们的body被设成base（水平），但 IMU 数据实际在livox_imu_link（倾斜 45°）
• echo一下 IMU 数据就能看到问题：

rostopicecho/livox/imu|greplinear_acceleration# 水平安装应该是 (0, 0, -9.8)，但实际是：# x: -6.8, y: 0.0, z: 7.1 ← 重力被劈成了 X 和 Z 两个分量！

• FAST-LIO2启动时读 IMU 重力方向来初始化姿态。它读到(-6.8, 0, 7.1)，以为 “body 是倾斜的”，于是把base姿态转了 ~114° 去 “对齐” 重力
• 说人话：FAST-LIO2以为狗是歪着站的，所以把整个odom → base的 TF 转歪了

2-2 为什么extrinsic_R救不了

• extrinsic_R是 LiDAR 相对于 IMU 的旋转。在Mid-360内部，IMU 和 LiDAR 是同一个物理封装，它们之间没有相对旋转
• 45° 是整个传感器包相对于机器人base的倾斜，不是 IMU 和 LiDAR 之间的倾斜
• > extrinsic_R只管 IMU↔LiDAR，管不了传感器包↔base。这个问题超过了它的管辖范围

2-3 方案对比

• 网上搜索了倾斜安装的标准做法，三种候选方案对比如下：

• 核心原理一句话：FAST-LIO2的body帧 = IMU 坐标系。只要 IMU 数据在水平系，body就是水平的
• 我们选择输入端旋转方案——在 IMU 和点云进入FAST-LIO2之前，都把数据转到base坐标系下：

/livox/imu (倾斜 livox_imu_link) → imu_transform → /livox/imu_base (水平 base) /scan (倾斜 laser_livox) → scan_to_pointcloud2 → /livox/lidar (水平 base)

• 转换后，FAST-LIO2保持完全默认配置：extrinsic_R = identity,extrinsic_T = [0,0,0],body = base

3 解决方法

3-1 IMU 坐标转换节点

• 订阅/livox/imu，查询TF: base → livox_imu_link的旋转矩阵，用R_y(+45°)旋转加速度和角速度，发布到/livox/imu_base（frame_id = "base"）

// imu_transform.cpp#include<ros/ros.h>#include<sensor_msgs/Imu.h>#include<tf/transform_listener.h>#include<tf/transform_datatypes.h>classImuTransform{public:ImuTransform(){ros::NodeHandle nh;sub_=nh.subscribe("/livox/imu",200,&ImuTransform::callback,this);pub_=nh.advertise<sensor_msgs::Imu>("/livox/imu_base",200);}private:voidcallback(constsensor_msgs::Imu::ConstPtr&msg){tf::StampedTransform transform;try{listener_.lookupTransform("base","livox_imu_link",msg->header.stamp,transform);}catch(tf::TransformException&e){ROS_WARN_THROTTLE(1.0,"imu_transform TF error: %s",e.what());return;}sensor_msgs::Imu out=*msg;out.header.frame_id="base";// R_base→imu = R_y(45°), 直接用 getBasis() 旋转到 base 系tf::Matrix3x3 R_imu_to_base=transform.getBasis();tf::Vector3acc_in(msg->linear_acceleration.x,msg->linear_acceleration.y,msg->linear_acceleration.z);tf::Vector3 acc_out=R_imu_to_base*acc_in;out.linear_acceleration.x=acc_out.x();out.linear_acceleration.y=acc_out.y();out.linear_acceleration.z=acc_out.z();tf::Vector3ang_in(msg->angular_velocity.x,msg->angular_velocity.y,msg->angular_velocity.z);tf::Vector3 ang_out=R_imu_to_base*ang_in;out.angular_velocity.x=ang_out.x();out.angular_velocity.y=ang_out.y();out.angular_velocity.z=ang_out.z();pub_.publish(out);}ros::Subscriber sub_;ros::Publisher pub_;tf::TransformListener listener_;};

3-2 点云坐标转换节点

• 订阅/scan（sensor_msgs/PointCloud），查询TF: base → laser_livox，对每个点做p_base = R * p_lidar + T，加入intensity字段（FAST-LIO2内部走pcl::PointXYZI），发布为sensor_msgs/PointCloud2，frame_id = "base"

// scan_to_pointcloud2.cpp#include<ros/ros.h>#include<sensor_msgs/PointCloud.h>#include<sensor_msgs/PointCloud2.h>#include<sensor_msgs/point_cloud2_iterator.h>#include<tf/transform_listener.h>classScanToPointCloud2{public:ScanToPointCloud2(){ros::NodeHandle nh;sub_=nh.subscribe("/scan",10,&ScanToPointCloud2::callback,this);pub_=nh.advertise<sensor_msgs::PointCloud2>("/livox/lidar",10);}private:voidcallback(constsensor_msgs::PointCloud::ConstPtr&msg){tf::StampedTransform T_base_lidar;try{listener_.lookupTransform("base","laser_livox",msg->header.stamp,T_base_lidar);}catch(tf::TransformException&e){ROS_WARN_THROTTLE(1.0,"scan_to_pointcloud2 TF: %s",e.what());return;}sensor_msgs::PointCloud2 cloud2;cloud2.header=msg->header;cloud2.header.frame_id="base";cloud2.height=1;cloud2.width=msg->points.size();cloud2.is_bigendian=false;cloud2.is_dense=true;sensor_msgs::PointCloud2Modifiermodifier(cloud2);modifier.setPointCloud2Fields(4,"x",1,sensor_msgs::PointField::FLOAT32,"y",1,sensor_msgs::PointField::FLOAT32,"z",1,sensor_msgs::PointField::FLOAT32,"intensity",1,sensor_msgs::PointField::FLOAT32);modifier.resize(msg->points.size());sensor_msgs::PointCloud2Iterator<float>iter_x(cloud2,"x");sensor_msgs::PointCloud2Iterator<float>iter_y(cloud2,"y");sensor_msgs::PointCloud2Iterator<float>iter_z(cloud2,"z");sensor_msgs::PointCloud2Iterator<float>iter_i(cloud2,"intensity");tf::Matrix3x3 R=T_base_lidar.getBasis();tf::Vector3 T=T_base_lidar.getOrigin();for(constauto&pt:msg->points){tf::Vector3p(pt.x,pt.y,pt.z);tf::Vector3 q=R*p+T;*iter_x=q.x();++iter_x;*iter_y=q.y();++iter_y;*iter_z=q.z();++iter_z;*iter_i=0.0;++iter_i;}pub_.publish(cloud2);}ros::Subscriber sub_;ros::Publisher pub_;tf::TransformListener listener_;};

3-3 踩坑：旋转方向

• 这是我们花费时间最多的一个坑
• lookupTransform("base", "livox_imu_link", ...)返回T_base→imu，其getBasis()=R_base→imu = R_y(+45°)
• 最开始用了transform.getBasis().inverse()（=R_y(-45°)），加速度从(-6.8, 0, 7.1)转成了(0.1, 0, 9.8)——看起来 Z 分量接近 9.8，似乎对了，但角速度方向是反的
• 改用transform.getBasis()直接乘（=R_y(+45°)），角速度才转对。验证方法：原地转弯，只有 Z 轴角速度变化——这才是正确的
• > 核心教训：旋转方向必须用纯 Z 轴旋转来验证。直走测试不够——直走只考验平移分量，转弯才考验角速度方向

4 配置与验证

• 因为输入数据已经在base坐标系下，FAST-LIO2的外参保持默认即可：

# config/sim.yamlcommon:lid_topic:"/livox/lidar"imu_topic:"/livox/imu_base"# 注意：走转换后的 topicpreprocess:lidar_type:4# MARSIM（仿真 PointCloud2）scan_line:4blind:0.1mapping:acc_cov:0.1gyr_cov:0.1b_acc_cov:0.0001b_gyr_cov:0.0001fov_degree:360det_range:100.0extrinsic_est_en:false# 输入端已对齐，关闭在线估计extrinsic_T:[0.0,0.0,0.0]extrinsic_R:[1,0,0,0,1,0,0,0,1]

• launch 文件结构：

<!-- launch/mapping_sim.launch --><launch><nodepkg="fast_lio"type="scan_to_pointcloud2"name="scan_to_pointcloud2"output="screen"/><nodepkg="fast_lio"type="imu_transform"name="imu_transform"output="screen"/><nodepkg="fast_lio"type="odom_2d"name="odom_2d"output="screen"/><rosparamfile="$(find fast_lio)/config/sim.yaml"command="load"/><nodepkg="fast_lio"type="fastlio_mapping"name="laserMapping"output="screen"/></launch>

• 启动后验证：

# 确认 IMU 重力只在 Z 轴rostopicecho/livox/imu_base|greplinear_acceleration# 预期：x≈0, y≈0, z≈9.8

总结

• 本文从 Mid-360 倾斜安装的实际问题出发，分析了坐标系关系、对比了三种方案，最终落地为输入端双旋转方案
• 核心要点回顾：
  • extrinsic_R只管 LiDAR↔IMU，管不了整个传感器包的倾斜
  • 输入端旋转（IMU + 点云都转到 base 系）是从根源解决倾斜问题的正确方式
  • getBasis()的方向必须用纯 Z 轴旋转来验证——直走测试不够
  • FAST-LIO2的body帧 = IMU 所在坐标系。只要 IMU 数据在水平系，body就是水平的
• 如有错误，欢迎指出！
• 感谢观看！