【没想到我国的科技开发已经发展到这个地步】Super-LIO 项目分析与 Mid-360 无人机移植指南

Super-LIO 项目分析与 Mid-360 无人机移植指南一、项目概述Super-LIO 是一个鲁棒高效的激光惯性里程计LiDAR-Inertial Odometry, LIO系统论文发表于 IEEE Robotics and Automation Letters (RA-L 2026)。项目采用紧凑的结构化建图策略实现了可预测的对应点搜索和稳定的状态估计在保证精度的同时资源消耗更低实时处理速度比同类方法快 1.2–4 倍。代码仓库https://github.com/Liansheng-Wang/Super-LIO支持平台ROS1 Noetic / ROS2 Humble/Iron/JazzyX86 ARM64核心依赖Eigen、PCL、glog、TBB、livox_ros_driver2二、系统架构2.1 整体结构项目采用 ROS2 工作空间结构包含两个核心包Super-LIO/src/ ├── basic/ # 基础数学库李代数、类型别名、缓冲区 │ ├── include/basic/ │ │ ├── alias.h # Eigen/PCL 类型别名 │ │ ├── Manifold.h # SO3/SE3/S2 流形数学 │ │ ├── ds.h # 数据结构 │ │ ├── math.h # 数学工具 │ │ ├── logs.h # 日志封装 │ │ └── buffer/ # 多种缓冲区实现 │ └── src/ └── super_lio/ # 主算法包 ├── include/ │ ├── lio/ # LIO 核心算法 │ ├── OctVoxMap/ # 八叉体素地图 │ ├── common/ # 通用数据结构 │ └── ros/ # ROS 封装层 ├── src/ │ ├── lio/ # 算法实现 │ ├── ros/ # ROS 节点实现 │ └── apps/ # 可执行文件入口 ├── config/ # 传感器配置文件 ├── launch/ # 启动文件 └── msg/ # 自定义消息2.2 核心类层次SuperLIO (基类) ├── 状态机stateWaitKFInit → stateWaitMapInit → stateProcess ├── ESKF误差状态卡尔曼滤波器 ├── OctVoxMap八叉体素地图 ├── VoxelGridClosest体素降采样 └── ROSWrapperROS 接口封装 SuperLIOReLoc (继承自 SuperLIO) └── 重定位模式加载预建地图 NDTICP 粗配准三、核心功能实现分析3.1 状态机驱动的处理流程系统采用三阶段状态机设计在 [super_lio.h](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/include/lio/super_lio.h#L54-L56) 中定义usingStateFnvoid(SuperLIO::*)();StateFn state_fn_;三个状态依次为stateWaitKFInit— 等待卡尔曼滤波初始化收集约 50 帧 IMU 数据约 0.5 秒假设 100Hz计算平均角速度和加速度利用重力向量对齐初始姿态初始化 ESKF 的零偏、协方差矩阵stateWaitMapInit— 等待地图初始化用初始姿态将前几帧点云变换到世界系插入 OctVoxMap 构建初始地图超过 3 帧后认为地图初始化完成stateProcess— 正常处理状态每帧依次执行IMU 传播与去畸变 → 降采样 → 观测更新 → 地图更新 → 输出状态切换在 [super_lio.cpp](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/src/lio/super_lio.cpp#L90-L112) 中实现。3.2 ESKF 误差状态卡尔曼滤波ESKFError-State Kalman Filter是系统的状态估计核心定义在 [ESKF.h](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/include/lio/ESKF.h#L12-L123) 中。状态向量定义系统维护 18 维的名义状态nominal state和 18 维的误差状态error state索引含义维度0–2旋转误差 δθSO3 左扰动33–5位置误差 δp36–8速度误差 δv39–11陀螺仪零偏误差 δbg312–14加速度计零偏误差 δba315–17重力向量误差 δg3名义状态变量直接存储为R_, p_, v_, bg_, ba_, g_误差状态存储在dx_中。预测步骤Predict预测函数有两个重载版本ESKF::Predict(imu)— 用于 LIO 主循环的点云去畸变传播计算状态转移矩阵 Fx18×18和噪声矩阵 Fw18×12协方差传播P Fx · P · Fxᵀ Fw · Q · Fwᵀ名义状态积分旋转用 SO3 指数映射位置速度用中值积分ESKF::Predict(imu, state_imu, state_robot)— 用于 IMU 高频里程计输出正向传播 IMU 状态同时输出 IMU 系和机器人系的状态不更新协方差只做运动学积分关键实现见 [ESKF.cpp](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/src/lio/ESKF.cpp#L187-L247) 中的预测函数。状态转移矩阵中旋转对陀螺仪零偏的雅可比使用右雅可比 Jr(dt·ω)速度对旋转的雅可比-R·[a]×·dt速度对加速度计零偏的雅可比-R·dt速度对重力的雅可比I·dt更新步骤UpdateObserve更新采用迭代扩展卡尔曼滤波IEKF的形式在 [ESKF.cpp](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/src/lio/ESKF.cpp#L250-L336) 中实现boolUpdateObserve(ObsFunc obs);使用函数回调ObsFunc传入观测残差和雅可比实现了算法与观测模型的解耦。迭代流程构造先验误差 dx_prior当前名义状态与预测名义状态之差计算先验协方差的雅可比 G_prior用左雅可比近似调用观测函数计算 HᵀR⁻¹H 和 HᵀR⁻¹r用信息矩阵形式求解A P⁻¹ HTRHQk A⁻¹计算增益 Kx Qk · HTRH更新误差状态dx Qk·b (Kx - I)·dx_prior误差注入名义状态R ← R·Exp(δθ)p ← pδp依此类推检查收敛条件||dx||∞ quit_eps迭代次数由g_kf_max_iterations控制默认 4 次。3.3 OctVoxMap 八叉体素地图OctVoxMap 是 Super-LIO 的核心创新之一定义在 [OctVoxMap.hpp](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/include/OctVoxMap/OctVoxMap.hpp) 中。它将空间划分为两层体素结构数据结构OctVoxMap ├── grids_: robin_hash_mapKEY, DATA_ITER // 哈希索引 ├── data_: listpairKEY, OctVox // 实际存储LRU 链表 ├── resolution_: 0.5m // 大体素尺寸 └── sub_resolution_: 0.25m // 子体素尺寸 resolution/2 OctVox (每个大体素含 8 个子体素) ├── points_[8]: Point // 8 个子体素的代表点 └── counts_[8]: uint8_t // 每个子体素的点数最多 20每个大体素如 0.5m被均匀划分为 2×2×2 8 个小体素如 0.25m每个小体素只存储一个平均点running average。这是一种紧凑的地图表示既保留了一定的空间分辨率又控制了内存占用。插入机制insert插入流程见 [OctVoxMap.hpp#L265-L303](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/include/OctVoxMap/OctVoxMap.hpp#L265-L303)计算点的子体素索引fine_key floor(pt / sub_resolution)大体素索引为子体素索引右移一位子体素局部索引local_idx (dz2) | (dy1) | dx在哈希表中查找大体素不存在创建新的 OctVox插到链表头部超过容量时淘汰链表尾部已存在调用AddPoint更新对应子体素AddPoint的策略子体素未初始化直接存入已超过 20 个点不再更新新点与存储点距离 0.1m不更新保持平面结构的稳定性否则累积平均running averageKNN 最近邻搜索getTopK搜索 5 个最近邻点使用预计算的分层搜索顺序flat_search_order从近到远依次检查体素。关键优化镜像对称加速利用mirror_axis将搜索模式对齐到点所在子体素的位置减少分支判断提前终止找到 5 个点且当前最远距离小于当前层的最小距离阈值时停止搜索更远的层预取体素指针提前加载 8 个最近体素的指针避免重复哈希查找LRU 淘汰策略地图使用std::list存储体素配合tsl::robin_map做哈希索引。每次访问或插入体素时将其移到链表头部超过capacity_时淘汰链表尾部最久未使用的体素。这保证了地图在有限内存下的稳定运行适合大规模场景。3.4 点到平面观测模型观测更新在 [super_lio.cpp](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/src/lio/super_lio.cpp#L442-L537) 的Observe()函数中实现。平面拟合对每个点在地图中查找 5 个最近邻然后用最小二乘法拟合平面Ax -1 → nᵀp d 0其中 n 是平面法向量d 是平面到原点的距离。使用colPivHouseholderQr求解。拟合完成后做平面度校验所有邻点到平面的距离必须小于 0.1m。残差与雅可比点到平面残差residual nᵀ·(R·p_body t) d对 SE3 状态的雅可比在观测函数中计算J_rot (R·p_body) × n J_trans n对应 6 维雅可比向量J [ (p_body × n_body)ᵀ , n_worldᵀ ]ᵀ其中n_body Rᵀ·n_world。残差权重设置为 1000相当于信息矩阵 Λ 1000·I。迭代中的有效点筛选第一次迭代时所有点都参与 KNN 搜索和平面拟合。后续迭代利用上一次的平面参数只需要重新计算残差残差过大的点被标记为无效effect_mask_有效点索引存入effect_knn_idxs_下一轮迭代只遍历这些点这是一种渐进式的外点剔除策略与 FAST-LIO2 的 ikd-tree 方案类似但因为 OctVoxMap 的结构更规整实现更简洁。TBB 并行加速观测计算使用 Intel TBB 的parallel_for做并行化tbb::enumerable_thread_specificThreadACCtls_acc;每个线程维护独立的 HᵀR⁻¹H 和 HᵀR⁻¹r 累加器最后合并。这是系统高性能的重要原因之一。3.5 IMU 去畸变点云去畸变在Propagation_Undistort()函数中实现见 [super_lio.cpp#L361-L426](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/src/lio/super_lio.cpp#L361-L426)。方法前向传播 线性插值先将上一帧末尾的状态作为起点依次用本帧的 IMU 数据前向传播得到每个 IMU 时刻的状态将所有状态存入propagate_states_向量对每个点根据其offset_time在状态序列中定位前后两个 IMU 时刻用 SLERP 插值旋转用匀加速模型插值位置R_i slerp(R_h, R_t, s) p_i p_h v_h·τ 0.5·a_t·τ²将点从当前时刻帧变换到点时刻帧再变换到IMU 系完成去畸变注意最终输出的去畸变点云是相对于帧末尾时刻的 IMU 坐标系的。3.6 数据同步与 ROS 接口ROSWrapper 类负责 ROS 通信和数据同步定义在 [ROSWrapper.h](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/include/ros/ROSWrapper.h) 中。传感器数据接收支持三种激光雷达输入模式由g_lidar_type控制类型值传感器消息类型1LIVOX (Mid-360 等)livox_ros_driver2::msg::CustomMsg2Hesai 16hesai_ros::Point (PointCloud2)3–4Velodyne 16/32velodyne_ros::Point (PointCloud2)5Velodyne NCLTNCLT::Point (PointCloud2)7Ousterouster_ros::Point (PointCloud2)IMU 统一使用sensor_msgs::msg::Imu。时间同步机制sync_measure()函数实现了激光雷达和 IMU 的时间同步见 [ROSWrapper.cpp#L532-L565](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/src/ros/ROSWrapper.cpp#L532-L565)维护两个双端队列lidar_buffer_和imu_buffer_取出一帧激光数据作为基准等待 IMU 数据时间戳超过激光帧末尾时间提取所有时间戳小于激光帧末尾的 IMU 数据组成MeasureGroup返回给算法层这种同步策略是松耦合的假设激光和 IMU 的时间戳都来自同一时钟源。3.7 重定位功能SuperLIOReLoc 继承自 SuperLIO实现了基于预建地图的重定位定义在 [super_lio_reloc.h](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/include/lio/super_lio_reloc.h) 中。重定位流程加载地图从 PCD 文件加载预建地图插入 OctVoxMap接收初始位姿通过 ROS 话题/initialpose或配置文件获取初始猜测收集观测数据积累 10 帧点云作为初始观测粗配准先用 NDT 做初始对齐再用 ICP 精细对齐NDT 分辨率 1.0m最大迭代 25 次ICP 最大对应距离 4.0m最大迭代 40 次收敛阈值fitness score 1.5初始化 ESKF用 ICP 结果作为初始位姿进入正常 LIO 流程重定位模式下地图更新可选g_update_map控制默认关闭以保持地图一致性。四、Livox Mid-360 原生支持分析Super-LIO 对 Livox Mid-360 提供了开箱即用的支持。4.1 相关文件文件作用[config/livox_360.yaml](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/config/livox_360.yaml)Mid-360 配置文件[launch/Livox_mid360.py](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/launch/Livox_mid360.py)Mid-360 启动文件ROSWrapper::livoxHandler()Livox 点云回调处理livox2pcl()Livox 消息转 PCL 点云4.2 关键配置参数从 [livox_360.yaml](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/config/livox_360.yaml) 中提取的 Mid-360 默认参数lio.sensor.lidar_type:1# 1 LIVOXlio.sensor.blind:2.0# 盲区 2mlio.sensor.maxrange:60.0# 最大测距 60mlio.sensor.filter_rate:3# 点云采样率每 3 个点取 1 个lio.sensor.voxel_fliter_size:0.5# 降采样体素大小lio.sensor.gravity_norm:9.7946# 当地重力加速度lio.ros.lidar_topic:/livox/lidar# 激光话题lio.ros.imu_topic:/livox/imu# IMU 话题lio.extrinsic.lidar_imu:[-0.011,-0.02329,0.04412,# 平移1.0,0.0,0.0,# 旋转矩阵0.0,1.0,0.0,0.0,0.0,1.0]4.3 Livox 数据处理流程livoxHandler()函数[ROSWrapper.cpp#L419-L441](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/src/ros/ROSWrapper.cpp#L419-L441)接收livox_ros_driver2::msg::CustomMsg遍历点检查 tag 字段只保留正常回波点tag 0x30 0x10 或 0x00过滤盲区和最大距离外的点将offset_time纳秒转换为秒存入点云记录帧起始时间和结束时间推入lidar_buffer_等待同步五、Mid-360 无人机移植指南5.1 硬件准备必备硬件Livox Mid-360 激光雷达飞行控制器如 Pixhawk 系列、DJI 飞控等机载计算机建议 ARM64 架构如 Jetson Xavier NX / Orin NX至少 4 核 CPU、8GB 内存局域网路由器或数传电台用于 ROS2 通信合理的电源方案Mid-360 约 10W机载计算机另计安装注意事项Mid-360 应安装在无人机顶部或前部视野尽量开阔避免螺旋桨进入激光视场可通过 blind 参数或点云裁剪解决安装方向默认 z 轴向上x 轴朝前如果安装方向不同需调整外参IMU 尽量靠近飞控或使用 Mid-360 内置 IMU5.2 软件环境搭建系统要求Ubuntu 22.04 / 24.04ROS2 Humble / JazzyC20 编译器依赖安装# 基础依赖sudoaptinstalllibgoogle-glog-dev libtbb-dev libeigen3-dev libpcl-dev# Livox ROS2 驱动cd~/ros2_ws/srcgitclone https://github.com/Livox-SDK/livox_ros_driver2.gitcdlivox_ros_driver2 ./build.sh humble# 根据 ROS2 版本选择cd../..colcon build --packages-select livox_ros_driver2编译 Super-LIOcd~/ros2_ws/srcgitclone https://github.com/Liansheng-Wang/Super-LIO.gitcd..colcon build --packages-select basic super_lio5.3 配置文件修改复制 [livox_360.yaml](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/config/livox_360.yaml) 并根据实际情况修改1. 话题名称lio.ros.lidar_topic:/livox/lidar# 检查实际话题名lio.ros.imu_topic:/livox/imu# 如用飞控 IMU 需修改使用ros2 topic list确认实际话题名称。2. 外参标定最关键lio.extrinsic.lidar_imu是激光雷达在 IMU 坐标系下的位姿即 IMU → 激光的变换。格式为[tx, ty, tz, r11, r12, r13, r21, r22, r23, r31, r32, r33]标定方法方法一使用 Kalibr 标定录制 IMU 和激光数据用 Kalibr 做相机-IMU 标定得到相机-IMU 外参用 hand-eye 标定方法得到激光-相机外参联立得到激光-IMU 外参方法二使用 livox_calibration 工具# 参考 Livox 官方标定工具gitclone https://github.com/Livox-SDK/livox_calibration.git方法三粗略估计 算法自动微调如果安装结构简单可以先手动测量得到近似值系统运行时 ESKF 会估计 IMU 零偏但外参偏差较大时可能无法初始化平移误差 5cm、旋转误差 5° 时通常可以正常工作3. 机器人坐标系配置lio.extrinsic.odom_robo是 IMU → 机器人坐标系的变换6 维xyz rpy角度单位度。对于无人机通常 IMU 就在机体中心附近平移设为 0如果 IMU 安装方向与机体一致旋转也设为 0lio.extrinsic.odom_robo:[0.0,0.0,0.0,0.0,0.0,0.0]4. 性能参数调整机载计算资源有限时lio.sensor.filter_rate:3# 增大此值可减少点数建议 2-5lio.sensor.voxel_fliter_size:0.5# 增大可减少观测点lio.hash_map.hash_capacity:500000# 减小地图容量默认 200 万lio.hash_map.vox_resolution:0.5# 可增大到 0.75 减少体素数lio.kf.kf_max_iterations:3# 减少迭代次数lio.output.pub_step:2# 降低可视化发布频率5.4 飞控集成方案一使用 Mid-360 内置 IMU推荐简单Mid-360 内置 IMU可以直接作为 LIO 的 IMU 输入。此时LIO 输出的里程计坐标系是 IMU 系即 Mid-360 内部 IMU需要将 LIO 输出的位姿转换到飞控坐标系通过lio.extrinsic.odom_robo配置 IMU 与机体的关系飞控端接收 LIO 位姿作为视觉/激光里程计辅助话题/mavros/vision_pose/pose需开启g_2_robot: true飞控配置EKF3 或 LPE 融合外部视觉方案二使用飞控 IMU精度更高将飞控的 IMU 数据通过 ROS2 话题发布出来飞控端如 Pixhawk PX4配置 MicroRTPS 或 DDS 客户端启用 IMU 数据发布机载端运行micro_ros_agent或px4_msgs将飞控 IMU 话题重映射到 Super-LIO 的 IMU 话题这种方案的好处是 IMU 与飞控的加速度计/陀螺仪完全一致避免了两个 IMU 之间的时间同步和外参问题。时间同步无论哪种方案都需要保证激光和 IMU 的时间同步优先使用硬件触发PPS软件方面可使用 NTP/PTP 同步Mid-360 驱动支持硬件时间戳5.5 无人机集成注意事项1. 螺旋桨遮挡无人机的螺旋桨可能进入 Mid-360 的下视场导致大量噪声点。解决方案调整 blind 参数将盲区适当增大如从 2.0 调到 3.0安装位置将雷达尽量抬高远离螺旋桨平面点云裁剪在驱动或算法中裁剪下方一定角度的点2. 振动问题无人机振动较大可能影响 IMU 测量质量给 Mid-360 或机载计算机加减震垫确保 IMU 低通滤波设置合理如果振动过大考虑使用飞控的 IMU通常有更好的减震设计3. 计算资源Jetson Xavier NX 建议开启最大性能模式sudo nvpmodel -m 0 sudo jetson_clocks监控 CPU 和内存使用率htop、jtop如果实时性不够优先降低点云采样率增大filter_rate4. 建图模式 vs 纯里程计模式纯定位已有地图使用重定位模式g_save_map: false建图模式g_save_map: true设置合理的保存间隔无人机高速运动时建议关闭可视化输出以节省资源5.6 飞行测试步骤第一阶段地面静态测试连接所有硬件开机检查 ROS2 话题是否正常发布启动 Super-LIO观察初始化是否成功手持 Mid-360 缓慢移动检查里程计是否稳定用 RViz2 查看点云和路径第二阶段地面动态测试将无人机放置在地面不启动电机人工推动无人机观察 LIO 输出检查 IMU 零偏是否收敛记录一段时间的数据检查漂移情况第三阶段悬停测试确保 GPS 正常飞控可正常定点悬停启动 LIO等待初始化完成起飞悬停 1–2 分钟观察 LIO 输出与 GPS 的偏差降落检查数据记录第四阶段航线飞行规划简单矩形航线起飞执行航线对比 LIO 轨迹与 GPS/RTK 轨迹根据结果调整参数5.7 常见问题排查问题可能原因解决方案初始化失败IMU 数据异常或外参错误检查 IMU 话题和方向核对外参漂移很大点云质量差或振动过大检查 blind/maxrange检查减震运行卡顿计算资源不足降低 filter_rate减小地图容量频繁丢失场景特征少如空旷场地尽量在有建筑/树木的环境测试Z 轴漂移IMU 加速度计零偏大或重力参数错检查 gravity_norm 配置校准 IMU位置跳变外参不准或时间不同步重新标定外参检查时间戳六、与 FAST-LIO2 的对比Super-LIO 在设计上借鉴了 FAST-LIO2 的 IEKF 增量式地图框架但有以下区别特性FAST-LIO2Super-LIO地图结构ikd-tree动态 kd 树OctVoxMap八叉体素哈希表搜索方式kd-tree 范围搜索分层有序体素搜索内存占用较高每个点独立存储较低子体素平均化并行化主要是观测阶段去畸变、观测、插入全流程 TBB 并行外点处理基于卡尔曼增益的离群点剔除基于平面残差阈值的渐进式剔除重定位不支持需结合其他模块原生支持代码结构算法与 ROS 耦合算法层与 ROS 层解耦Super-LIO 的核心优势在于 OctVoxMap 的紧凑表示和有序搜索使得最近邻搜索更快、内存更省特别适合资源有限的嵌入式平台如无人机机载计算机。七、参考资料论文Super-LIO: A Robust and Efficient LiDAR-Inertial Odometry System with a Compact Mapping Strategy (RA-L 2026)arXivhttps://arxiv.org/abs/2509.05723代码仓库https://github.com/Liansheng-Wang/Super-LIOLivox Mid-360 官网https://www.livoxtech.com/mid-360livox_ros_driver2https://github.com/Livox-SDK/livox_ros_driver2