ROS2 Humble Livox MID360+FAST-LIO2 动态水平补偿避障节点完整实现和安装角度偏移补偿计算脚本

将 Livox MID-360 激光雷达点云转化为 MAVLink 避障距离数据（LaserScan），支持逐点动态水平补偿，利用 FAST-LIO2 的实时姿态估计将点云从倾斜的机体坐标系旋转到水平世界坐标系，确保飞行中避障的准确性。目录工作原理坐标系说明步骤 1：静态安装偏移补偿步骤 2：动态水平补偿完整数据流环境要求安装与编译运行全部源代码参数参考调试与可视化常见问题工作原理核心问题无人机飞行时，机身会因加速、转弯、风扰等产生俯仰和横滚。如果 LiDAR 点云不随姿态补偿，会出现：地面被识别为"前方障碍物"障碍物方位角计算错误避障系统误判，产生不必要的规避动作解决方案本节点在收到每一帧 LiDAR 点云时，对每个点独立执行以下流程：对于每个LiDAR点 (x, y, z): 1. 用该点的精确时间戳，从FAST-LIO2中插值出对应时刻的飞机姿态四元数 2. 补偿LiDAR的静态安装偏移 (LiDAR → 机体) 3. 用四元数将点从机体坐标系旋转到水平世界坐标系 4. 将水平点按角度分桶，取每个角度方向最近的距离，发布为LaserScan坐标系说明坐标系标识说明LiDAR 坐标系livox_frameMID-360 自身：X 向前，Y 向左，Z 向上。由于 LiDAR 通常倾斜安装，该系不水平机体坐标系body飞行器本体：X 向前（机头方向），Y 向右，Z 向下（右手系）。随飞机倾斜世界坐标系camera_initFAST-LIO2 建立的重力对齐世界系。Z 轴垂直向上，不随飞机旋转变换目标P_lidar ──步骤1──→ P_body ──步骤2──→ P_camera_init (水平世界) 安装偏移补偿 动态水平补偿步骤 1：静态安装偏移补偿为什么需要？MID-360 为了获得更大的有效视场角，通常倾斜安装（探头朝下）。如本案例中绕 Y 轴旋转-15.6°。如果不补偿，LiDAR 感知的"前方"与飞机实际前方偏差 15.6°，会导致：近距离地面被错误地映射到前向避障 bin 中远距离障碍物的实际方位角计算错误参数设置在 launch 文件中定义三个安装偏移角度（单位：度）：# 文件: launch/livox_distance_launch.py parameters=[{ 'roll_offset': 0.2, # 绕X轴（横滚），正=右侧抬高 'pitch_offset': -15.6, # 绕Y轴（俯仰），正=连接器端抬高（探头下俯） 'yaw_offset': 0.0, # 绕Z轴（偏航），正=顺时针 }]标定安装角：livox_calibrate.py安装雷达后，如何确定roll_offset和pitch_offset？Livox 官方提供了一个自动标定脚本livox_calibrate.py，它通过 RANSAC 拟合地面平面来计算安装偏移角。⚠️ 确保飞机水平放置在平整地面上再执行标定。使用方法# 在一个终端启动 LiDAR 驱动 ros2 launch livox_ros_driver2 livox_lidar_msg.launch.py # 另一个终端运行标定脚本 ros2 run livox_ros_driver2 livox_calibrate.py脚本会自动采集约 3 秒点云，输出类似：========================= 标定结果 ========================= 采集点数: 65420 地面内点: 52340 (80.0%) 地面法向量: (0.0217, -0.2698, 0.9636) 👉 roll_offset = -0.2 (正=右侧抬高，绕X轴) 👉 pitch_offset = -15.6 (正=机头下俯，绕Y轴) 将以上值填入 launch 文件或通过以下命令设置: ros2 param set /livox_distance_node roll_offset -0.2 ros2 param set /livox_distance_node pitch_offset -15.6 ===========================================================将输出的roll_offset和pitch_offset填入 launch 文件即可。标定原理：通过 RANSAC 从点云中拟合地面平面，提取地面法向量，反推出 LiDAR 相对于水平面的安装倾斜角。详细源码见 全部源代码 章节。变换矩阵推导定义：roll_rad = roll_offset × π / 180pitch_rad = pitch_offset × π / 180先绕 X 轴补偿横滚（即旋转-roll_rad）：rx = x ry = y × cos(roll_rad) - z × sin(roll_rad) rz = y × sin(roll_rad) + z × cos(roll_rad)再绕 Y 轴补偿俯仰（即旋转-pitch_rad）：bx = rx × cos(pitch_rad) - rz × sin(pitch_rad) by = ry bz = rx × sin(pitch_rad) + rz × cos(pitch_rad)得到P_body = (bx, by, bz)。步骤 2：动态水平补偿数据来源：FAST-LIO2FAST-LIO2 是一个 LiDAR-IMU 紧耦合里程计，以 ~100Hz 发布/Odometry话题：header.frame_id: "camera_init" ← 世界固定系（Z向上） child_frame_id: "body" ← 机体坐标系其中的pose.orientation是一个四元数q = (w, x, y, z)，表示body坐标系相对于camera_init的旋转。逐点时间戳插值LiDAR（MID-360）以 ~10Hz 发布一帧点云，每帧包含数千个点。每个点携带offset_time（纳秒级偏移）：// 该点的精确时刻 = 帧时间戳 + 该点的时间偏移 rclcpp::Time point_time = frame_time + rclcpp::Duration::from_nanoseconds(point.offset_time);由于 FAST-LIO2 的频率（~100Hz）远高于 LiDAR（~10Hz），一帧 LiDAR 数据内包含多个 Odometry 样本。我们在内存中维护一个双端队列缓冲区，存储最近 0.5 秒的 Odometry 样本。对于每个 LiDAR 点，通过二分查找找到前后两个 Odometry 样本，用Slerp（球面线性插值）计算出该点精确时刻的四元数：// 二分查找: 找到最后一个时间戳 = point_time 的样本 auto it = std::upper_bound(imu_buffer_.begin(), imu_buffer_.end(), point_time, ...); // 插值系数 ratio = (point_time - t0) / (t1 - t0) // 线性插值（含最短路径处理 + 归一化） qw = q0.w + ratio × (q1.w - q0.w) // 若内积0则取反 qx = q0.x + ratio × (q1.x - q0.x) qy = q0.y + ratio × (q1.y - q0.y) qz = q0.z + ratio × (q1.z - q0.z) norm = sqrt(qw² + qx² + qy² + qz²) qw /= norm; qx /= norm; qy /= norm; qz /= norm;四元数旋转用插值得到的四元数q = (w, x, y, z)，将机体坐标点旋转到camera_init世界坐标系：p' = q × p_body × q_conjugate展开为矩阵形式（避免了欧拉角的万向锁和旋转顺序歧义）：level_x = (w² + x² - y² - z²) × bx + 2×(x×y - w×z) × by + 2×(x×z + w×y) × bz level_y = 2×(x×y + w×z) × bx + (w² - x² + y² - z²) × by + 2×(y×z - w×x) × bz level_z = 2×(x×z - w×y) × bx + 2×(y×z + w×x) × by + (w² - x² - y² + z²) × bz得到P_level = (level_x, level_y, level_z)，即该点在世界水平坐标系下的坐标。完整数据流┌─────────────────────────────────────────────────────┐ │ MID-360 LiDAR │ │ 发布 /livox/lidar (10Hz) │ └──────────────────────┬──────────────────────────────┘ │ ▼ ┌─────────────────────────────────────────────────────┐ │ FAST-LIO2 │ │ 发布 /Odometry (100Hz) │ │ frame_id: camera_init → child_frame_id: body │ └──────────┬────────────────────────────────┬──────────┘ │ │ ▼ ▼ ┌──────────────────┐ ┌──────────────────┐ │ odom_callback │ │ lidar_callback │ │ 存储姿态到缓冲区 │ │ 处理每一帧点云 │ └──────────────────┘ └──────────────────┘ │ ┌──────────────────────┴──────────┐ │ 逐点处理 │ │ │ ▼ ▼ ┌─────────────────┐ ┌─────────────────┐ │ 1. 插值四元数 │ │ 默认q=(1,0,0,0) │ │ (有FAST-LIO2 ｜ │ (无姿态数据) │ │ 姿态时) │ │ │ └────────┬────────┘ └────────┬────────┘ │ │ └──────────┬──────────────────────┘ │ ▼ ┌─────────────────────┐ │ 2