前言【10天速通ROS2-PX4无人机】(二) FAST-LIO2 仿真部署200HZ IMU前推的无限魅力【FAST-LIO2 源码解读】从 Mid-360 到 ESEKF — 公式与代码逐行对照刚好前两期我们都完成了FAST-LIO2的仿真部署与源码解读最近在机器狗项目中遇到一个问题Mid-360雷达不是水平安装的而是以45° 倾斜固定在机器狗前方——这样做是为了让雷达能看到更多地面和前方障碍物但直接打开FAST-LIO2后出现了三个诡异现象里程计一直在飞Z 轴不断升高、Rviz 里狗和路径都歪了 45°、直走还凑合一转弯就 “升天” 或 “入地”本文记录这个问题的根因分析、方案对比、以及最终落地的输入端旋转方案文章目录前言1 问题描述2 根因分析2-1 坐标系关系2-2 为什么 extrinsic_R 救不了2-3 方案对比3 解决方法3-1 IMU 坐标转换节点3-2 点云坐标转换节点3-3 踩坑旋转方向4 配置与验证总结1 问题描述机器狗前面放置的雷达是倾斜 45 度放置的这样是为了能够看到更多的地面特征在URDF中laser_livox雷达通过joint挂在base上!-- robot.xacro --jointnamelaser_livox_jointtypefixedoriginxyz0.2 0 0.08rpy0 0.785 0/parentlinkbase/childlinklaser_livox//jointjointnamelivox_imu_jointtypefixedparentlinklaser_livox/childlinklivox_imu_link/originxyz-0.011 -0.02329 0.04412rpy0 0 0//joint坐标系关系base (水平, Z朝上) └── laser_livox (rpy0, 0.785, 0 → 45° pitch) └── livox_imu_link (rpy0, 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 方案对比网上搜索了倾斜安装的标准做法三种候选方案对比如下方案做法效果适用场景改 extrinsic_R把倾斜角写入 LiDAR↔IMU 外参只适用于 LiDAR 单独倾斜、IMU 水平IMU 外置输出端静态 TF在 FAST-LIO2 输出后加 TF “掰正”直走还行转弯飞临时凑合输入端旋转IMU 和点云进 FAST-LIO2 之前就转到 base 系里外都对一体式传感器倾斜核心原理一句话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 base3 解决方法3-1 IMU 坐标转换节点订阅/livox/imu查询TF: base → livox_imu_link的旋转矩阵用R_y(45°)旋转加速度和角速度发布到/livox/imu_baseframe_id base// imu_transform.cpp#includeros/ros.h#includesensor_msgs/Imu.h#includetf/transform_listener.h#includetf/transform_datatypes.hclassImuTransform{public:ImuTransform(){ros::NodeHandle nh;sub_nh.subscribe(/livox/imu,200,ImuTransform::callback,this);pub_nh.advertisesensor_msgs::Imu(/livox/imu_base,200);}private:voidcallback(constsensor_msgs::Imu::ConstPtrmsg){tf::StampedTransform transform;try{listener_.lookupTransform(base,livox_imu_link,msg-header.stamp,transform);}catch(tf::TransformExceptione){ROS_WARN_THROTTLE(1.0,imu_transform TF error: %s,e.what());return;}sensor_msgs::Imu out*msg;out.header.frame_idbase;// R_base→imu R_y(45°), 直接用 getBasis() 旋转到 base 系tf::Matrix3x3 R_imu_to_basetransform.getBasis();tf::Vector3acc_in(msg-linear_acceleration.x,msg-linear_acceleration.y,msg-linear_acceleration.z);tf::Vector3 acc_outR_imu_to_base*acc_in;out.linear_acceleration.xacc_out.x();out.linear_acceleration.yacc_out.y();out.linear_acceleration.zacc_out.z();tf::Vector3ang_in(msg-angular_velocity.x,msg-angular_velocity.y,msg-angular_velocity.z);tf::Vector3 ang_outR_imu_to_base*ang_in;out.angular_velocity.xang_out.x();out.angular_velocity.yang_out.y();out.angular_velocity.zang_out.z();pub_.publish(out);}ros::Subscriber sub_;ros::Publisher pub_;tf::TransformListener listener_;};3-2 点云坐标转换节点订阅/scansensor_msgs/PointCloud查询TF: base → laser_livox对每个点做p_base R * p_lidar T加入intensity字段FAST-LIO2内部走pcl::PointXYZI发布为sensor_msgs/PointCloud2frame_id base// scan_to_pointcloud2.cpp#includeros/ros.h#includesensor_msgs/PointCloud.h#includesensor_msgs/PointCloud2.h#includesensor_msgs/point_cloud2_iterator.h#includetf/transform_listener.hclassScanToPointCloud2{public:ScanToPointCloud2(){ros::NodeHandle nh;sub_nh.subscribe(/scan,10,ScanToPointCloud2::callback,this);pub_nh.advertisesensor_msgs::PointCloud2(/livox/lidar,10);}private:voidcallback(constsensor_msgs::PointCloud::ConstPtrmsg){tf::StampedTransform T_base_lidar;try{listener_.lookupTransform(base,laser_livox,msg-header.stamp,T_base_lidar);}catch(tf::TransformExceptione){ROS_WARN_THROTTLE(1.0,scan_to_pointcloud2 TF: %s,e.what());return;}sensor_msgs::PointCloud2 cloud2;cloud2.headermsg-header;cloud2.header.frame_idbase;cloud2.height1;cloud2.widthmsg-points.size();cloud2.is_bigendianfalse;cloud2.is_densetrue;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::PointCloud2Iteratorfloatiter_x(cloud2,x);sensor_msgs::PointCloud2Iteratorfloatiter_y(cloud2,y);sensor_msgs::PointCloud2Iteratorfloatiter_z(cloud2,z);sensor_msgs::PointCloud2Iteratorfloatiter_i(cloud2,intensity);tf::Matrix3x3 RT_base_lidar.getBasis();tf::Vector3 TT_base_lidar.getOrigin();for(constautopt:msg-points){tf::Vector3p(pt.x,pt.y,pt.z);tf::Vector3 qR*pT;*iter_xq.x();iter_x;*iter_yq.y();iter_y;*iter_zq.z();iter_z;*iter_i0.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/lidarimu_topic:/livox/imu_base# 注意走转换后的 topicpreprocess:lidar_type:4# MARSIM仿真 PointCloud2scan_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]参数值含义lidar_type4MARSIM 模式走标准PointCloud2imu_topic/livox/imu_base指向 IMU 转换后的水平坐标系blind0.1仿真环境特征稀疏减小盲区保留更多点extrinsic_est_enfalse输入端已对齐关闭在线估计extrinsic_T/R零/单位阵外参保持 Mid-360 默认launch 文件结构!-- launch/mapping_sim.launch --launchnodepkgfast_liotypescan_to_pointcloud2namescan_to_pointcloud2outputscreen/nodepkgfast_liotypeimu_transformnameimu_transformoutputscreen/nodepkgfast_liotypeodom_2dnameodom_2doutputscreen/rosparamfile$(find fast_lio)/config/sim.yamlcommandload/nodepkgfast_liotypefastlio_mappingnamelaserMappingoutputscreen//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就是水平的如有错误欢迎指出感谢观看
前言【10天速通ROS2-PX4无人机】(二) FAST-LIO2 仿真部署200HZ IMU前推的无限魅力【FAST-LIO2 源码解读】从 Mid-360 到 ESEKF — 公式与代码逐行对照刚好前两期我们都完成了FAST-LIO2的仿真部署与源码解读最近在机器狗项目中遇到一个问题Mid-360雷达不是水平安装的而是以45° 倾斜固定在机器狗前方——这样做是为了让雷达能看到更多地面和前方障碍物但直接打开FAST-LIO2后出现了三个诡异现象里程计一直在飞Z 轴不断升高、Rviz 里狗和路径都歪了 45°、直走还凑合一转弯就 “升天” 或 “入地”本文记录这个问题的根因分析、方案对比、以及最终落地的输入端旋转方案文章目录前言1 问题描述2 根因分析2-1 坐标系关系2-2 为什么 extrinsic_R 救不了2-3 方案对比3 解决方法3-1 IMU 坐标转换节点3-2 点云坐标转换节点3-3 踩坑旋转方向4 配置与验证总结1 问题描述机器狗前面放置的雷达是倾斜 45 度放置的这样是为了能够看到更多的地面特征在URDF中laser_livox雷达通过joint挂在base上!-- robot.xacro --jointnamelaser_livox_jointtypefixedoriginxyz0.2 0 0.08rpy0 0.785 0/parentlinkbase/childlinklaser_livox//jointjointnamelivox_imu_jointtypefixedparentlinklaser_livox/childlinklivox_imu_link/originxyz-0.011 -0.02329 0.04412rpy0 0 0//joint坐标系关系base (水平, Z朝上) └── laser_livox (rpy0, 0.785, 0 → 45° pitch) └── livox_imu_link (rpy0, 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 方案对比网上搜索了倾斜安装的标准做法三种候选方案对比如下方案做法效果适用场景改 extrinsic_R把倾斜角写入 LiDAR↔IMU 外参只适用于 LiDAR 单独倾斜、IMU 水平IMU 外置输出端静态 TF在 FAST-LIO2 输出后加 TF “掰正”直走还行转弯飞临时凑合输入端旋转IMU 和点云进 FAST-LIO2 之前就转到 base 系里外都对一体式传感器倾斜核心原理一句话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 base3 解决方法3-1 IMU 坐标转换节点订阅/livox/imu查询TF: base → livox_imu_link的旋转矩阵用R_y(45°)旋转加速度和角速度发布到/livox/imu_baseframe_id base// imu_transform.cpp#includeros/ros.h#includesensor_msgs/Imu.h#includetf/transform_listener.h#includetf/transform_datatypes.hclassImuTransform{public:ImuTransform(){ros::NodeHandle nh;sub_nh.subscribe(/livox/imu,200,ImuTransform::callback,this);pub_nh.advertisesensor_msgs::Imu(/livox/imu_base,200);}private:voidcallback(constsensor_msgs::Imu::ConstPtrmsg){tf::StampedTransform transform;try{listener_.lookupTransform(base,livox_imu_link,msg-header.stamp,transform);}catch(tf::TransformExceptione){ROS_WARN_THROTTLE(1.0,imu_transform TF error: %s,e.what());return;}sensor_msgs::Imu out*msg;out.header.frame_idbase;// R_base→imu R_y(45°), 直接用 getBasis() 旋转到 base 系tf::Matrix3x3 R_imu_to_basetransform.getBasis();tf::Vector3acc_in(msg-linear_acceleration.x,msg-linear_acceleration.y,msg-linear_acceleration.z);tf::Vector3 acc_outR_imu_to_base*acc_in;out.linear_acceleration.xacc_out.x();out.linear_acceleration.yacc_out.y();out.linear_acceleration.zacc_out.z();tf::Vector3ang_in(msg-angular_velocity.x,msg-angular_velocity.y,msg-angular_velocity.z);tf::Vector3 ang_outR_imu_to_base*ang_in;out.angular_velocity.xang_out.x();out.angular_velocity.yang_out.y();out.angular_velocity.zang_out.z();pub_.publish(out);}ros::Subscriber sub_;ros::Publisher pub_;tf::TransformListener listener_;};3-2 点云坐标转换节点订阅/scansensor_msgs/PointCloud查询TF: base → laser_livox对每个点做p_base R * p_lidar T加入intensity字段FAST-LIO2内部走pcl::PointXYZI发布为sensor_msgs/PointCloud2frame_id base// scan_to_pointcloud2.cpp#includeros/ros.h#includesensor_msgs/PointCloud.h#includesensor_msgs/PointCloud2.h#includesensor_msgs/point_cloud2_iterator.h#includetf/transform_listener.hclassScanToPointCloud2{public:ScanToPointCloud2(){ros::NodeHandle nh;sub_nh.subscribe(/scan,10,ScanToPointCloud2::callback,this);pub_nh.advertisesensor_msgs::PointCloud2(/livox/lidar,10);}private:voidcallback(constsensor_msgs::PointCloud::ConstPtrmsg){tf::StampedTransform T_base_lidar;try{listener_.lookupTransform(base,laser_livox,msg-header.stamp,T_base_lidar);}catch(tf::TransformExceptione){ROS_WARN_THROTTLE(1.0,scan_to_pointcloud2 TF: %s,e.what());return;}sensor_msgs::PointCloud2 cloud2;cloud2.headermsg-header;cloud2.header.frame_idbase;cloud2.height1;cloud2.widthmsg-points.size();cloud2.is_bigendianfalse;cloud2.is_densetrue;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::PointCloud2Iteratorfloatiter_x(cloud2,x);sensor_msgs::PointCloud2Iteratorfloatiter_y(cloud2,y);sensor_msgs::PointCloud2Iteratorfloatiter_z(cloud2,z);sensor_msgs::PointCloud2Iteratorfloatiter_i(cloud2,intensity);tf::Matrix3x3 RT_base_lidar.getBasis();tf::Vector3 TT_base_lidar.getOrigin();for(constautopt:msg-points){tf::Vector3p(pt.x,pt.y,pt.z);tf::Vector3 qR*pT;*iter_xq.x();iter_x;*iter_yq.y();iter_y;*iter_zq.z();iter_z;*iter_i0.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/lidarimu_topic:/livox/imu_base# 注意走转换后的 topicpreprocess:lidar_type:4# MARSIM仿真 PointCloud2scan_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]参数值含义lidar_type4MARSIM 模式走标准PointCloud2imu_topic/livox/imu_base指向 IMU 转换后的水平坐标系blind0.1仿真环境特征稀疏减小盲区保留更多点extrinsic_est_enfalse输入端已对齐关闭在线估计extrinsic_T/R零/单位阵外参保持 Mid-360 默认launch 文件结构!-- launch/mapping_sim.launch --launchnodepkgfast_liotypescan_to_pointcloud2namescan_to_pointcloud2outputscreen/nodepkgfast_liotypeimu_transformnameimu_transformoutputscreen/nodepkgfast_liotypeodom_2dnameodom_2doutputscreen/rosparamfile$(find fast_lio)/config/sim.yamlcommandload/nodepkgfast_liotypefastlio_mappingnamelaserMappingoutputscreen//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就是水平的如有错误欢迎指出感谢观看
