【没想到我国的科技开发已经发展到这个地步】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/Jazzy，X86 + 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：体素降采样 └── ROSWrapper：ROS 接口封装 SuperLIOReLoc (继承自 SuperLIO) └── 重定位模式：加载预建地图 + NDT+ICP 粗配准

三、核心功能实现分析

3.1 状态机驱动的处理流程

系统采用三阶段状态机设计，在 [super_lio.h](file:///e:/项目/写点博客/Super-LIO分析/Super-LIO/src/super_lio/include/lio/super_lio.h#L54-L56) 中定义：

usingStateFn=void(SuperLIO::*)();StateFn state_fn_;

三个状态依次为：

1. stateWaitKFInit— 等待卡尔曼滤波初始化

   • 收集约 50 帧 IMU 数据（约 0.5 秒，假设 100Hz）
   • 计算平均角速度和加速度
   • 利用重力向量对齐初始姿态
   • 初始化 ESKF 的零偏、协方差矩阵

2. stateWaitMapInit— 等待地图初始化

   • 用初始姿态将前几帧点云变换到世界系
   • 插入 OctVoxMap 构建初始地图
   • 超过 3 帧后认为地图初始化完成

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 误差状态卡尔曼滤波

ESKF（Error-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）：

名义状态变量直接存储为R_, p_, v_, bg_, ba_, g_，误差状态存储在dx_中。

预测步骤（Predict）

预测函数有两个重载版本：

1. ESKF::Predict(imu)— 用于 LIO 主循环的点云去畸变传播

   • 计算状态转移矩阵 Fx（18×18）和噪声矩阵 Fw（18×12）
   • 协方差传播：P = Fx · P · Fxᵀ + Fw · Q · Fwᵀ
   • 名义状态积分：旋转用 SO3 指数映射，位置速度用中值积分

2. 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传入观测残差和雅可比，实现了算法与观测模型的解耦。

迭代流程：

1. 构造先验误差 dx_prior（当前名义状态与预测名义状态之差）
2. 计算先验协方差的雅可比 G_prior（用左雅可比近似）
3. 调用观测函数计算 HᵀR⁻¹H 和 HᵀR⁻¹r
4. 用信息矩阵形式求解：A = P⁻¹ + HTRH，Qk = A⁻¹
5. 计算增益 Kx = Qk · HTRH
6. 更新误差状态：dx = Qk·b + (Kx - I)·dx_prior
7. 误差注入名义状态：R ← R·Exp(δθ)，p ← p+δp，依此类推
8. 检查收敛条件：||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_map<KEY, DATA_ITER> // 哈希索引 ├── data_: list<pair<KEY, 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)：

1. 计算点的子体素索引fine_key = floor(pt / sub_resolution)
2. 大体素索引为子体素索引右移一位
3. 子体素局部索引：local_idx = (dz<<2) | (dy<<1) | dx
4. 在哈希表中查找大体素：
   • 不存在：创建新的 OctVox，插到链表头部；超过容量时淘汰链表尾部
   • 已存在：调用AddPoint更新对应子体素

AddPoint的策略：

• 子体素未初始化：直接存入
• 已超过 20 个点：不再更新
• 新点与存储点距离 > 0.1m：不更新（保持平面结构的稳定性）
• 否则：累积平均（running average）

KNN 最近邻搜索（getTopK）

搜索 5 个最近邻点，使用预计算的分层搜索顺序（flat_search_order），从近到远依次检查体素。关键优化：

1. 镜像对称加速：利用mirror_axis将搜索模式对齐到点所在子体素的位置，减少分支判断
2. 提前终止：找到 5 个点且当前最远距离小于当前层的最小距离阈值时，停止搜索更远的层
3. 预取体素指针：提前加载 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_specific<ThreadACC>tls_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)。

方法：前向传播 + 线性插值

1. 先将上一帧末尾的状态作为起点
2. 依次用本帧的 IMU 数据前向传播，得到每个 IMU 时刻的状态
3. 将所有状态存入propagate_states_向量
4. 对每个点，根据其offset_time在状态序列中定位前后两个 IMU 时刻
5. 用 SLERP 插值旋转，用匀加速模型插值位置：

   R_i = slerp(R_h, R_t, s) p_i = p_h + v_h·τ + 0.5·a_t·τ²

6. 将点从"当前时刻帧"变换到"点时刻帧"再变换到"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控制：

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)：

1. 维护两个双端队列：lidar_buffer_和imu_buffer_
2. 取出一帧激光数据作为基准
3. 等待 IMU 数据时间戳超过激光帧末尾时间
4. 提取所有时间戳小于激光帧末尾的 IMU 数据
5. 组成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) 中。

重定位流程

1. 加载地图：从 PCD 文件加载预建地图，插入 OctVoxMap
2. 接收初始位姿：通过 ROS 话题/initialpose或配置文件获取初始猜测
3. 收集观测数据：积累 10 帧点云作为初始观测
4. 粗配准：先用 NDT 做初始对齐，再用 ICP 精细对齐
   • NDT 分辨率 1.0m，最大迭代 25 次
   • ICP 最大对应距离 4.0m，最大迭代 40 次
   • 收敛阈值：fitness score < 1.5
5. 初始化 ESKF：用 ICP 结果作为初始位姿
6. 进入正常 LIO 流程

重定位模式下地图更新可选（g_update_map控制），默认关闭以保持地图一致性。

四、Livox Mid-360 原生支持分析

Super-LIO 对 Livox Mid-360 提供了开箱即用的支持。

4.1 相关文件

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)）：

1. 接收livox_ros_driver2::msg::CustomMsg
2. 遍历点，检查 tag 字段（只保留正常回波点：tag & 0x30 == 0x10 或 0x00）
3. 过滤盲区和最大距离外的点
4. 将offset_time（纳秒）转换为秒，存入点云
5. 记录帧起始时间和结束时间
6. 推入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 内置 IMU

5.2 软件环境搭建

系统要求

• Ubuntu 22.04 / 24.04
• ROS2 Humble / Jazzy
• C++20 编译器

依赖安装

# 基础依赖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-LIO

cd~/ros2_ws/srcgitclone https://github.com/Liansheng-Wang/Super-LIO.gitcd..colcon build --packages-select basic super_lio

5.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 标定

1. 录制 IMU 和激光数据
2. 用 Kalibr 做相机-IMU 标定得到相机-IMU 外参
3. 用 hand-eye 标定方法得到激光-相机外参
4. 联立得到激光-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 安装方向与机体一致，旋转也设为 0

lio.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 话题发布出来：

1. 飞控端（如 Pixhawk + PX4）：

   • 配置 MicroRTPS 或 DDS 客户端
   • 启用 IMU 数据发布

2. 机载端：

   • 运行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_rate）

4. 建图模式 vs 纯里程计模式

• 纯定位（已有地图）：使用重定位模式，g_save_map: false
• 建图模式：g_save_map: true，设置合理的保存间隔
• 无人机高速运动时，建议关闭可视化输出以节省资源

5.6 飞行测试步骤

第一阶段：地面静态测试

1. 连接所有硬件，开机
2. 检查 ROS2 话题是否正常发布
3. 启动 Super-LIO，观察初始化是否成功
4. 手持 Mid-360 缓慢移动，检查里程计是否稳定
5. 用 RViz2 查看点云和路径

第二阶段：地面动态测试

1. 将无人机放置在地面（不启动电机）
2. 人工推动无人机，观察 LIO 输出
3. 检查 IMU 零偏是否收敛
4. 记录一段时间的数据，检查漂移情况

第三阶段：悬停测试

1. 确保 GPS 正常，飞控可正常定点悬停
2. 启动 LIO，等待初始化完成
3. 起飞，悬停 1–2 分钟
4. 观察 LIO 输出与 GPS 的偏差
5. 降落，检查数据记录

第四阶段：航线飞行

1. 规划简单矩形航线
2. 起飞执行航线
3. 对比 LIO 轨迹与 GPS/RTK 轨迹
4. 根据结果调整参数

5.7 常见问题排查

六、与 FAST-LIO2 的对比

Super-LIO 在设计上借鉴了 FAST-LIO2 的 IEKF + 增量式地图框架，但有以下区别：

Super-LIO 的核心优势在于 OctVoxMap 的紧凑表示和有序搜索，使得最近邻搜索更快、内存更省，特别适合资源有限的嵌入式平台（如无人机机载计算机）。

七、参考资料

• 论文：Super-LIO: A Robust and Efficient LiDAR-Inertial Odometry System with a Compact Mapping Strategy (RA-L 2026)
• arXiv：https://arxiv.org/abs/2509.05723
• 代码仓库：https://github.com/Liansheng-Wang/Super-LIO
• Livox Mid-360 官网：https://www.livoxtech.com/mid-360
• livox_ros_driver2：https://github.com/Livox-SDK/livox_ros_driver2