基于 2D 激光雷达提高快速激光雷达惯性里程计定位精度的方法
doi: 10.19328/j.cnki.2096‐8655.2026.01.011
赵威 ， 严怀成 ， 高生 ， 吕云凯
华东理工大学 信息科学与工程学院，上海 200237
基金项目: 国家自然科学基金资助项目（62333005）
A Method for Improving FAST-LIO Positioning Accuracy Based on 2D LiDAR
ZHAO Wei ， YAN Huaicheng ， GAO Sheng ， LÜ Yunkai
School of Information Science and Engineering, East China University of Science and Technology, Shanghai 200237, China
摘要
为解决传统快速激光雷达惯性里程计（FAST-LIO）在全球定位系统（GPS）拒止环境中，因初始高度默认全局坐标系原点、Z 轴观测约束单一，导致无人机定位精度，尤其是高度方向精度退化，进而制约其整体定位性能进一步提升的问题，提出低成本 2D 激光雷达与 FAST-LIO 集成的融合方案。方法上，先通过 2D 激光雷达完成极坐标转三维点云、随机采样一致性直线拟合、多重验证滤波及坐标转换，获取厘米级初始高度；然后将 2D 激光雷达与 FAST-LIO 自身的惯性测量单元、3D 激光雷达结合，构建三重紧耦合系统；再将 2D 激光雷达观测融入迭代误差状态卡尔曼滤波（IESKF）观测矩阵，补充 Z 轴约束。该方法低成本易集成，有效提升无人机定位及位姿精度，支撑 GPS 拒止场景自主导航，未来将探索三维平面拟合优化适应性。
Abstract
To address the issue of degraded positioning accuracy，particularly along the vertical axis，in traditional fast LiDAR-inertial odometry systems operating in GPS-denied environments—a problem stemming from the default initialization of height at the global coordinate origin and insufficient observational constraints in the Z-axis，which limits further improvement of overall localization performance—this paper proposes a fusion scheme that integrates a low-cost 2D LiDAR with the FAST-LIO framework.Methodologically，the approach begins by converting polar coordinate data from the 2D LiDAR into a 3D point cloud，followed by Random Sample Consensus line fitting，multi-stage validation filtering，and coordinate transformation to obtain a centimeter-level initial height estimate. Subsequently，a tightlycoupled system is constructed by combining the 2D Light detection and ranging （LiDAR）with the inherent inertial measurement unit （IMU）and 3D LiDAR of FAST-LIO.Observations from the 2D LiDAR are incorporated into the observation matrix of the iterated error state kalman filter （IESKF），thereby enhancing constraints in the Z-axis.The proposed method is low-cost，easy to integrate，and effectively improves the positioning and pose estimation accuracy of unmanned aerial vehicles （UAVs），supporting reliable autonomous navigation in GPS-denied scenarios.Future work will explore the use of 3D plane fitting to further optimize adaptability.
0 引言
四旋翼无人机在全球定位系统（Global Positioning System，GPS）拒止或信号不佳的复杂环境（如室内仓库、地下矿井、城市峡谷及茂密林区）中实现自主运行[1]，其核心前提在于获取精确的初始定位[2]与持续稳定的位姿估计结果[3]，这 2 大环节直接决定了后续飞行控制[4]、路径规划[5]及任务执行的可靠性[6]。当前，激光惯性里程计（LiDAR Inertial Odometry，LIO）[7]技术凭借对复杂环境的强适应性[8]，已成为该场景下定位方案的主流选择。其中，以高效性著称的快速紧耦合激光雷达惯性里程计（Fast LiDAR-Inertial Odometry，FAST-LIO）[9]系统表现尤为突出，该系统通过采用紧耦合迭代误差状态卡尔曼滤波[10]（Iterated Error State Kalman Filter，IESKF）算法，将惯性测量单元（Inertial Measurement Unit，IMU）的实时运动感知数据与激光雷达（Light Detection and Ranging，LiDAR）的环境点云数据深度融合，有效平衡了定位精度与计算效率，为无人机提供了极具竞争力的位姿估计能力。
然而，FAST-LIO 及同类 LIO 系统在实际应用中仍存在 2 大固有且关键的局限[11]，这些局限对定位精度的影响贯穿系统运行全程：1）其状态估计模型中，初始位置被默认设定为全局坐标系原点，这意味着系统启动瞬间，无人机真实的三维空间位置（尤其是对飞行安全与高度控制至关重要的 Z 轴高度值）对状态估计而言完全处于未知状态；2）原有FAST-LIO 的观测矩阵仅依赖 IMU 与 3D LiDAR 2类传感器的观测信息，状态估计过程中的约束维度相对单一[12]，尤其在 Z 轴方向，仅依靠 3D LiDAR 的稀疏垂直分辨率[13]与 IMU 积分数据，难以提供持续且充足的观测约束，易在复杂运动或环境纹理稀疏场景下出现位姿估计漂移[14]。
针对高度估计与位姿优化问题，学界与工程界已开展大量研究并取得系列进展，但尚未能同时解决上述两大局限。在基于气压计的高度测量方向[15]，SIMONETTI 等[16]针对气压计易受温度、气流干扰的问题[17]，提出结合气象数据的鲁棒建模方法，可在动态气象条件下提升高度估计稳定性；RANĐELOVIĆ 等[18]进一步将气压计与其他低成本传感器融合，降低长时间飞行中的漂移效应[19]，但此类方法需已知基准点气压才能换算为绝对高度，且启动瞬间易受气流扰动，无法满足弱信息环境下初始高度的厘米级精度需求。在基于超声波的方案中，DAVIES 等[20]设计面向无人机的超声波测距系统，在低空场景下展现出良好的精度与实时性；HENTSCHKE 等[21]在植保无人机场景中验证超声波对气压计漂移的补偿作用。但超声波量程有限，且在草地、软质地表等复杂地形易发生回波失真[22]，难以覆盖多场景下的持续高度观测需求。在视觉与多传感器融合领域[23]，QIN 等[24]提出的单目视觉惯性导航系统（Visual-Inertial Navigation System-Mono，VINS-Mono）通过紧耦合单目视觉与 IMU，结合非线性优化[25]实现亚米级高度精度；MUR-ARTAL 等[26]和 CAMPOS 等[27]研发的基于方向加速分割测试与旋转二进制鲁棒独立基本特征的同步定位与地图构建（Simultaneous Localization and Mapping based on Oriented FAST and Rotated BRIEF，ORB-SLAM）系列扩展了双目、IMU 融合的适用性，在无 GPS 环境下提供稳定高度估计。但这类方法对环境纹理与光照条件依赖极强[28]，在弱纹理或动态光照环境中初始化困难[29]，且计算复杂度较高[30]，难以适配资源受限的无人机平台。即便是激光雷达相关研究，XU 等[31]提出的快速紧耦合激光雷达惯性里程计第 2 版（Fast LiDAR-Inertial Odometry 2，FAST-LIO2），SHAN 等[32]的基于平滑与建图的激光雷达惯性里程计（LiDAR-Inertial Odometry-Smoothing and Mapping，LIO-SAM）通过优化紧耦合策略提升了位姿估计精度，ZHU 等[33]和 HEO 等[34]也针对激光惯性系统初始化提出了稳健的实时方法，但现有LiDAR 系统仍普遍默认初始高度为零，且未通过扩展观测矩阵[35]补充 Z 轴方向的约束，无法从根本上解决初始高度误差与持续观测不足的问题。
在弱信息条件下（即无已知起飞起点、无可用GPS 信号、无预先构建的环境先验地图），为规避FAST-LIO 的上述局限，实践中常需依赖操作员手动输入起飞高度、使用气压计读数或等待 GPS 信号短暂恢复等外部方式设定初始 Z值，这些方式不仅增加操作流程的复杂性、降低无人机自主运行能力，且所提供的初始值精度往往不足，易引入系统性误差；同时，单一的 IMU-3D LiDAR 观测约束，也使得系统在Z 轴方向的位姿估计易受运动失真或点云匹配误差影响，进一步降低定位可靠性。因此，如何在弱信息条件下为 FAST-LIO 提供精确的初始高度参考，并通过扩展观测约束提升全程位姿估计精度，成为提升四旋翼无人机复杂环境定位性能的关键问题。
针对上述初始高度未知、Z 轴观测约束单一两大难点，本文提出一种基于低成本 2D 激光雷达的改进方案，主要贡献总结如下：
1）提出基于低成本 2D激光雷达的初始高度精确估计方法，通过其高频测距数据，结合安装外参与机体姿态，在系统初始化阶段直接计算全局绝对高度，取代默认的零值假设，从根源上消除了初始 Z轴误差；
2）构建 IMU、3D 激光雷达与 2D 激光雷达的三重紧耦合系统，将 2D 激光雷达的实时观测信息融入 IESKF 的观测矩阵，显著增强 Z 轴方向的观测约束，有效抑制因 IMU 积分漂移与 3D 激光雷达垂直分辨率不足导致的位姿估计漂移。
本文提出的方案是在 FAST-LIO 的基础上改进的，由于 FAST-LIO 框架本身是为实时处理而设计的，结合 2D 激光雷达可实现快速高度测量。2D 激光雷达具有高频次的距离测量能力，可提供快速且相对精确的垂直高度信息，尤其在动态环境中相比气压计等更能实时反映高度变化。此外，与气压计方案相比，激光雷达受天气、温度等环境因素的影响较小，特别是在高温、低温等极端环境下，气压计的精度可能出现较大波动，而激光雷达测量的稳定性较好。与视觉方案相比，2D 激光雷达不依赖于外部环境的特征识别，不受光照、天气变化等因素干扰，能在低光照、夜间或其他恶劣环境下稳定工作。
由于本文提出的方案仍是基于激光雷达，因此存在相应的劣势。例如，2D 激光雷达的作用距离相对较短，通常在几十米以内，这限制了在较高飞行或较远距离下的高度测量能力。本方案的激光雷达是安装在无人机的旋翼平台上，因此会受旋翼震动、风速变化等因素的影响，可能导致激光束的指向发生微小偏移，从而影响测量的精度。
1 系统框架
本文提出的 FAST-LIO-singleline 系统在原有FAST-LIO 架构基础上引入 2D 激光雷达，以在初始化阶段提供高精度的绝对高度测量值。整体框架如图 1 所示。
图 1 系统框架
Fig. 1 System framework
传感器层包括 3D LiDAR、IMU，以及作为高度测量的 2D LiDAR。数据处理层在系统启动时，优先利用 2D LiDAR 提供的实时测距值估计无人机的初始高度。状态估计层在 IESKF 框架下，将初始高度注入初始状态向量，替代传统 FAST-LIO 默认的零高度假设，从而消除初始偏差并加速收敛，然后将 2D LiDAR 和 3D LiDAR 组合成整体观测约束加入预测更新模块。
2 方法论
2.1 无人机初始高度计算
本步骤是获取精准初始高度值的关键，需通过多环节降噪与优化，确保高度测量精度达厘米级。
1）极坐标数据转三维点云
2D 激光雷达的原始数据为其单激光束旋转扫描所得的极坐标点 p(ρ，φ)，ρ 为测量距离，φ 为测量角度。数据处理流程的第 1 步是进行数据预处理，即滤除超出传感器有效量程的测量点。继而执行从极坐标系到空间直角坐标系的转换，根据下式将每个有效的极坐标点 p(ρ，φ) 映射为点 P（x，y，z）。鉴于扫描平面为二维，故所有点的 Z 坐标被初始化为零。最终，通过此转换，构建出对应的三维点云集合{U}，为后续的直线拟合提供数据基础。
{x=ρ⋅cos⁡ϕy=ρ⋅sin⁡ϕz=0
（1）
2）RANSAC 直线模型拟合
在获取到 2D 激光雷达点云数据基础上，利用随机采样一致性（Random Sample Consensus，RANSAC）算法对所述点云进行鲁棒直线拟合。通过多次随机采样与模型验证，从包含噪声的数据中识别出代表地面的最优直线模型。随后，通过点到直线距离公式计算雷达坐标系原点到该直线的距离。
原理如图 2 所示。
图 2 基于 2D 激光雷达的高度测量原理
Fig. 2 Principle of height calculation using 2D LiDAR
这里选择用 RANSAC 算法来拟合直线的原因是，相较于传统最小二乘法对离群点敏感，一个错误点可导致拟合完全错误；而 RANSAC 算法对离群点更鲁棒，能识别并排除异常点。
下面开始构建直线模型。从获取的 2D 激光雷达点云数据{U} 中随机取 2 个点 P1(x1，y 1，z 1)，P2(x2，y 2，z 2)。用向量来表示P1 P2 为[ x2-x1，y 2-y 1，z 2-z 1 ]T，将 P1 P2 归一化即可得到单位方向向量如下：
d=[dx,dy,dz]T=[x2−x1,y2−y1,z2−z1]T[(x2−x1)2+(y2−y1)2+(z2−z1)2]
（2）
由于点云 z 轴分量为零，将点云数据看作分布在 2D 激光雷达 z=0 平面上，因此忽略 z 轴，则 d=[ dx，dy ]T。因为法向量垂直于方向向量，由 d 可以推出直线 P1 P2 的单位法向量如下，记n=[ a，b ]T。
n=[−dy,dx]T
（3）
由直线上一点 P1(x1，y 1) 和对应法向量 n=[ a，b]T，可通过点法式求得直线方程如下：
a(x−x1)+b(y−y1)=0
（4）
将（4）式展开为一般式，可得：
ax+by−(ax1+by1)=0
（5）
式（5）是表示P1 P2 的直线方程。得到直线方程后，遍历点云数据{U}中其他点，计算这些点到直线P1 P2的距离。例如取点 Q(q 1，q 2)，计算 Q 到 P1 P2的距离：
dQ=|aq1+bq2−(ax1+by1)|a2+b2
（6）
上面在求 P1 P2 的法向量 n 时已经归一化过，即a2+ b2= 1，所以这里的点到直线距离可进一步简化为
dQ=|aq1+bq2−(ax1+by1)|
（7）
此时，需设置一个自适应距离阈值 dthreshold来表示直线附近的范围。具体设置方法是根据点云的空间分布密度，动态计算并设置随机采样一致性算法的距离阈值，实现自适应参数配置，从而在不同观测距离和点云密度下均能获得最优的拟合效果。
然后，将 dthreshold 跟 d Q 比较。当 d Q 小于 dthreshold时，认为 Q 点是直线 P1 P2 的内点，将 Q 归为直线P1 P2 的点集中；当 d Q大于dthreshold 时，认为 Q 点是直线 P1 P2 的外点，则被剔除。遍历完所有点后，统计内点数量。
接下来，重复上述步骤，再次随机选取 2 个点，求解方程，计算所有点到直线的距离，得到内点数量。重复这一过程直到内点数量满足要求，或者达到下式计算出的迭代次数 Kiter，此时得到的直线方程就是所需要的直线方程，即 2D 激光雷达扫描平面与地面相交的直线，记为 Ax + By + C= 0，A、B、C 为直线方程的系数。
Kiter=log⁡(1−R)/log⁡(1−wY)
（8）
式中：R 为点集的置信度；w 为内点占据总点集的比例；Y 为拟合出直线的最小样本数。
2D 激光雷达自身的坐标为原点(0，0)，因此可计算出 2D 激光雷达距离与地面交线的距离 d2D如下：
d2D=|C|A2+B2
（9）
3）多重验证与滤波
为确保测量结果 d2D 的可靠性与稳定性，对计算得到的原始距离值进行多重验证与滤波处理。首先进行距离合理性检查，将原始距离与预设的最大合理距离阈值（2D 激光雷达能够探测的最大有效距离）进行比较，若超出则判定为异常值并启动回退处理；其次进行帧间连续性检查，在系统已初始化的状态下，计算当前帧距离与上一帧有效距离的差值，若该差值超过最大允许突变阈值，则判定为异常跳变并使用上一帧的有效距离值进行替代。
此外，系统会维护一个固定尺寸的滑动窗口来保存最近几帧的数据。将经过验证处理的距离值存入固定长度的历史数据缓冲区（即滑动窗口），并应用移动平均滤波，当缓冲区中的数据样本达到预设数量时，计算其算术平均值作为当前帧的最终输出，实现数据平滑与噪声抑制；最后，更新系统状态，将本次有效距离结果记录为下一帧计算的参考基准，并标记系统初始化完成。由 d2D 经过上面处理计算得到的用于更新系统状态的高度值为 d~2D。设向量 D~2D=[0,0,d~2D]T。
通过上述多重验证、滤波与状态维护机制的协同作用，有效提升了系统在复杂工况下的输出连续性与鲁棒性。
4）坐标系转换
输出的 2D 激光雷达测量值 h~2D 是 2D 激光雷达到地面的高度距离，并不是无人机到地面的高度距离（系统把 IMU 坐标系作为无人机的机体坐标系）。由于传感器设备安装原因，2D 激光雷达坐标系和无人机上 IMU 的坐标系之间存在位姿偏差（即 2D 激光雷达相对于 IMU 的外参），设 2D 激光雷达相对于IMU 的安装外参为旋转矩阵 RLI 和平移向量 pLI。因此，无人机相对于地面的高度值计算如下：
Z2D=pLI+RLI⋅D~2D
（10）
式中：向量 Z2D 第三维的值即为所需的无人机的高度值，记为 z 2D。
2.2 利用 z 2D 正确初始化 FAST-LIO 算法
FAST-LIO 的数学模型主要围绕状态估计展开，核心是基于流形的紧耦合迭代卡尔曼滤波器，包含运动学模型（状态转移模型）和测量模型，并通过迭代更新实现状态优化。
系统将 IMU 坐标系记作 I，3D 激光雷达坐标系记作 L，将第 1 帧 IMU 数据帧记作世界坐标系 G。公式中变量上的点表示对该变量求导，符号表示求向量的反对称叉乘矩阵，例如三维向量v=[ v 1，v 2，v 3 ]T，则 ⌊v⌋∧=[0−v3v2v30−v1−v2v10]。运动学模型如下：
{GR˙I=GRI⌊ωm−bω−nω⌋,Gp˙I=GvIGv˙I=GRI(am−ba−na)+Gg,b˙ω=nbω,b˙a=nba,Gg=0,IR˙L=0,Ip˙L=0
（11）
式中：G RI、G pI 分别为 IMU 在全局坐标系中的姿态旋转矩阵和位置向量；G g为全局坐标系中的重力向量；G vI 为 IMU 坐标系相对于世界坐标系的速度向量；am、ω m 分别为 IMU 的测量值线性加速度和角速度；na、nω 分别为 am、ω m 的测量噪声；ba、bω 分别为IMU 线性加速度偏差和角速度偏差（建模为由 nba、nbω 驱动的随机游走过程）；LiDAR-IMU 外参 I RL 和I pL 分别为 3D 激光雷达坐标系相对于 IMU 坐标系的姿态和位置。具体状态向量如下：
xFAST−LIO=[GRIT,GpIT,GvIT,bωT,baT,GgT,IRLT,IpLT]T
（12）
原始 FAST-LIO 初始化是将状态向量（位置 p、速度 v、姿态 R）初始化为零值。其中位置初始化G pI=[ 0，0，0]T在无人机真实离地高度非零时会导致状态估计从错误起点开始，引发收敛延迟与轨迹漂移。因此将 2D 激光雷达在初始化时测量的无人机距离地面的垂直高度 z 2D 记作 zinit，并用来正确初始化无人机位姿即 G pI=[ 0，0，zinit ]T。
2.3 2D 激光雷达与 FAST-LIO 系统紧耦合
FAST-LIO 是一种基于 IESKF 的激光雷达与IMU 融合的同步定位与地图构建（Simultaneous Localization and Mapping，SLAM）算法，其核心是通过激光点云观测约束 IMU 估计的状态。
FAST-LIO 观测数据来自 3D 激光雷达的点云{U