Livox 点云动态水平补偿方案

1. 问题背景无人机搭载 Livox MID-360 激光雷达用于避障时，激光雷达随无人机一起运动。当无人机产生横滚（roll）或俯仰（pitch）姿态变化时，点云也会随之倾斜，导致pointcloud_to_laserscan生成的 LaserScan 数据中包含地面或天空的干扰点，无法正确反映水平方向的障碍物分布。核心需求：无论无人机如何倾斜，始终获得一个保持在水平面内的 360° 激光扫描数据。2. 方案概述通过一个动态 TF 广播节点（horizontal_frame_broadcaster），实时监听无人机姿态数据，广播base_link → horizontal_frame的变换关系：horizontal_frame与base_link原点相同horizontal_frame保持水平（roll = 0, pitch = 0）horizontal_frame跟随无人机偏航（yaw）然后配置pointcloud_to_laserscan以horizontal_frame为目标帧，点云在变换到这个水平参考系后再生成 LaserScan，从而排除机身倾斜的干扰。数据流plaintextLivox MID-360 │ ├─ /livox/lidar ──→ livox_distance_node ──→ /livox/cloud (PointCloud2 in livox_frame) │ ├─ /livox/imu ──→ horizontal_frame_broadcaster ──→ TF: base_link → horizontal_frame │ (200Hz 直接广播) └─ [静态 TF] ──→ TF: base_link → livox_frame (mounting calibration) │ ┌────────────────────────────┐ │ pointcloud_to_laserscan │ │ input: /livox/cloud │ │ target: horizontal_frame │ │ output: /horizontal_scan │ └────────────────────────────┘ │ livox_obstacle_bridge /horizontal_scan → /mavros/obstacle/send3. 姿态数据来源方案支持多种姿态数据源，通过参数配置切换：表格模式Topic说明优点缺点livox_imu（默认）/livox/imuMID360 内置 BMI088 加速度计独立于飞控，200Hz 高频，硬件时间戳急加速时有偏差odometry/OdometryFAST-LIO2 VIO 里程计平滑高频参考系非重力对齐，需偏置校正imu/mavros/imu/dataMAVROS PX4 EKF 融合姿态重力对齐频率较低，需定时器缓存pose用户指定通用 PoseStamped灵活取决于数据源3.1 推荐方案：livox_imu（加速度计法）利用 MID360 内置 BMI088 加速度计，根据重力矢量方向计算横滚和俯仰角：plaintextroll_imu = atan2(ay, az) pitch_imu = -atan2(ax, sqrt(ay² + az²))然后通过已知的 MID360 安装角度（mount_roll,mount_pitch）将 IMU 姿态转换到机身坐标系：plaintextq_base = q_imu * q_mount⁻¹其中q_mount为base_link → livox_frame的安装旋转。由于 MID360 的 IMU 和 LiDAR 硬件时间同步，TF 在 IMU 回调中直接广播，使用 IMU 的硬件时间戳，无需定时器缓存，延迟最低，实时性最高。3.2 备选方案：VIO + 初始偏置校正当使用 FAST-LIO2 VIO 的/Odometry时，其camera_init参考系可能不与重力对齐。方案在节点启动时记录第一个姿态作为初始偏置，后续每帧减去该偏置：plaintextq_corrected = q_bias⁻¹ * q_current这样得到的是相对起飞的姿态变化，近似与重力对齐。4. TF 变换树plaintextbase_link (无人机机体) ├── static → livox_frame (MID360 安装校准: roll=-0.7°, pitch=-16°) └── dynamic → horizontal_frame (水平参考系: roll=0, pitch=0, yaw=机头方向)通过这套 TF 树，pointcloud_to_laserscan可以将点云从livox_frame变换到base_link，再变换到horizontal_frame，最终在水平参考系中生成 LaserScan。5. 核心数学原理5.1 加速度计求姿态当无人机悬停或缓慢飞行时，加速度计主要测量重力矢量。在 IMU 坐标系中：ax = -g × sin(pitch)ay = g × sin(roll) × cos(pitch)az = g × cos(roll) × cos(pitch)解得：plaintextroll = atan2(ay, az) pitch = -atan2(ax, √(ay² + az²))5.2 旋转顺序与补偿水平补偿的关键是旋转顺序。必须先用 R_x 补偿横滚，再用 R_y 补偿俯仰：plaintextlevel_q = R_x(-roll) × R_y(-pitch)使用setRPY(-roll, -pitch, 0)会得到R_y(-pitch) × R_x(-roll)，这在横滚俯仰同时非零时是错误的。5.3 安装角度补偿MID360 以mount_roll=-0.7°,mount_pitch=-16°安装。静态 TF 广播：cppq_mount.setRPY(roll_rad, -pitch_rad, 0.0); // = R_y(+16°) × R_x(-0.7°)加速度计求得的 IMU 姿态需要补偿这个安装角度才能得到机身姿态：cppq_base = q_imu × q_mount⁻¹5.4 TF 时间戳同步对于默认的livox_imu模式：IMU 回调（200Hz）中直接计算姿态并广播 TF使用 IMU 硬件时间戳（msg-header.stamp），与 LiDAR 点云时间戳同源无需定时器缓存，延迟最低对于低频率的imu模式（MAVROS 10~20Hz）：回调只缓存姿态数据定时器以 50Hz 固定频率广播 TF，使用this-now()保证时间戳始终最新6. 代码清单6.1 horizontal_frame_broadcaster.cppcpp#include memory #include cmath #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/imu.hpp" #include "tf2_ros/transform_broadcaster.h" #include "tf2/LinearMath/Quaternion.h" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" class HorizontalFrameBroadcaster : public rclcpp::Node { public: HorizontalFrameBroadcaster() : Node("horizontal_frame_broadcaster"), use_vio_bias_(false), bias_initiali