实时定位与地图构建(SLAM)技术是机器人导航领域的核心技术之一^[1]. SLAM技术作为实时定位和高精度三维点云地图构建的核心技术^[2]，其为机器人运动控制和轨迹规划提供了先验信息^[3]. SLAM主要分为视觉SLAM^[4-5]和激光SLAM.

在激光SLAM方面，激光惯性里程计的发展由LiDAR和IMU松耦合的融合方式逐渐发展为紧耦合^[6]. 松耦合方式将IMU的积分值作为当前帧与历史帧或局部地图进行配准的初值，未将IMU积分值加入到后端优化中，所以其在特征退化和旋转抖动的环境下鲁棒性较差. Besl等^[7]提出ICP. Chen等^[8]提出点到面源配准的改进ICP方法，Low^[9]提出基于线性最小二乘法的改进ICP方法，Segal等^[10]提出广义迭代最近邻G-ICP方法. Zhang等^[11]提出LOAM. LOAM-Livox^[12]、Lego-LOAM^[13]、F-LOAM^[14]、M-LOAM^[15]等衍生方法. 这些都是松耦合融合的激光惯性里程计方法. 紧耦合方法将IMU积分值加入后端优化中，将IMU积分值与点云配准结果进行联合优化. 利用紧耦合的方法，不仅可以抑制快速移动和旋转抖动造成的漂移，而且在半结构化或非结构化环境中相较于松耦合方式，利用紧耦合的方法能够显著提升系统的鲁棒性. Ye等^[16]提出第一个紧耦合的激光惯性里程计框架LIO-Mapping. Shan等^[17]提出LIO-SAM，通过因子图优化的方法进行多传感器的紧耦合融合. Qin等^[18]提出Lins，利用卡尔曼滤波的方法，实现了LiDAR和IMU的紧耦合融合. Lv等^[19]提出Clins，Xu等^[20]提出FAST-LIO. FAST-LIO通过提出的IMU反向传播方法和降维卡尔曼增益公式，提升了里程计的精度和效率. 在此基础上，FAST-LIO2^[21]和FASTER-LIO^[22]相继被提出.

利用高效率和高精度的里程计，可以保证机器人运动控制和轨迹规划模块获取先验位姿的实时性和精度^[23-24]. 当今，激光里程计和效率的精度的优化面临如下挑战：1)利用当前的点云特征提取方法，无法精确地提取特征点并对平面和地面点进行针对性的提取和分割. 2)高效的卡尔曼滤波的方法无法同时融合LiDAR、IMU和GPS信息. 为了解决上述问题，本文提出LCG-LIO. 提出双向降维曲率滤波的高质量平面与地面点云提取和分割的方法. 提出点云伪占用的后端优化方法，平衡了平面点和地面点在后端优化中的占比. 对后端优化函数中的观测误差方程和观测误差雅可比矩阵的构建方法进行改进，在观测误差方程中加入了GPS 约束.

本系统首先积累激光雷达采集的点云数据和IMU采集的状态数据，选择离最后一个雷达点时间戳最近的GPS信息，以时间长度为单位进行统一处理. 将积累后的数据传递给数据预处理模块，通过双向降维曲率滤波的方式进行平面和地面点的提取和分割，通过点云伪占用的方式平衡里程计处理的平面点和地面点数量. 以上一帧经卡尔曼滤波后的最优状态为前向传播的初始状态，通过IMU数据的前向传播和反向传播进行当前帧的状态估计和点云畸变补偿. 将IMU前向传播获得的当前帧状态、GPS信息和畸变补偿后的点云数据送入改进后的卡尔曼滤波位姿状态优化模块，得到当前帧的最优位姿. 将当前帧点云插入ikd-tree中，便于后续状态优化模块的近邻点搜索. 系统的总体流程如图1所示.

同一条LiDAR线束点云的x和y值组成的l维度易于识别点云的纵向平面和边角特征，点云的z值和点云间夹角θ组成的h维度易于识别地面和间隔点跳变特征. 同一条LiDAR线束点云中的纵向平面点在l维度上的投影呈现为一条近似直线，2个平面相交的边线点在l维度上的投影呈现为2条直线的交点，地面点在l维度上的投影呈现为圆弧形曲线. LiDAR的每条线束是以同一角度进行扫描，所以同一LiDAR线束在纵向平面上的z是不同的，且同一LiDAR线束上激光束长度和点的z值关系密切. 同一LiDAR线束点云中的地面点在h维度上的投影呈现为一条严格的水平直线，纵向平面或边线点呈现为一条斜曲线. 由于跳变远点的两点激光束长度相差较大，同一LiDAR线束的跳变远点在h维度上的投影呈现为一条斜率很大的直线. 由此可知，通过降维的方式将双向滤波与曲率滤波相结合，可以更有针对性地识别点云的各个特征，开展更高精度的特征点提取. 将当前LiDAR帧的每个LiDAR线束分别在点云的l维度和h维度上进行曲率滤波. 通过结合2个维度上的降维曲率，联合计算局部平面的光滑度，从而提取高质量的边线点、平面点和地面点，将平面点与地面点分割.

LiDAR点在l维度上的曲率越小，说明当前LiDAR点在l维度上在一条越直的线上. LiDAR点在h维度上的曲率越小，说明当前LiDAR点在h维度上在一条越直的线上. 由此可知，当l维度的曲率较小，且h维度的曲率较小，但不接近0时，则当前点为高质量的平面点；当l维度的曲率较小，且h维度的曲率较大时，则当前点的激光束长度发生跳变，为激光束与平面夹角较小的不稳定平面点，需要将其滤除. 当l维度的曲率较大，且h维度的曲率较小，但不接近0时，则该点为角点. 地面点在l维度上呈弧状，所以当l维度的曲率较大，且h维度的曲率严格接近0时，当前点为稳定的地面点. 其他情况的点为需要滤除的不稳定平面点、不稳定地面点、非结构化点和噪点. 分别定义l和h 2个维度的曲率公式如下：

(1)$ {c_{xy}} = \left(\sum\limits_{j = i - 3}^{i+3} ({x_i^{}} - x_j^{})\right)_{}^2+\left(\sum\limits_{j = i - 3}^{i+3} ({y_i^{}} - y_j^{})\right)_{}^2 ,$

(2)$ {c_z} = \frac{\beta}{{(x_i^{}+y_i^{})}}\left(\sum\limits_{j = i - 3}^{i+3} ({z_i^{}} - z_j^{})\right)_{}^2 .$

式中：$ {c_{xy}} $和$ {c_z} $分别为点i在l和h 2个维度上的曲率，$ x_i^{} $、$ y_i^{} $和$ z_i^{} $分别为当前点i在LiDAR坐标系下的x、y和z轴坐标. $ {c_{xy}} $和$ {c_z} $的计算只考虑当前点i与其相同雷达线束上的前后近邻3个点的关系. 雷达低线束的相邻地面点间隔较近，雷达高线束的相邻地面点间隔较远. 由此可见，判断z是否发生跳变以及是否符合稳定的地面点特征，需要加入自适应阈值$ {\beta}/{{(x_i^{}+y_i^{})}} $. 对于距离LiDAR中心较远的点，其近邻点间的$z $跳变阈值较大；对于距离LiDAR中心较近的点，其近邻点间的$z $跳变阈值较小.

如图2所示为双向降维曲率滤波的示意图. 图中，S1区域为地平面，S2区域为纵向平面，S3和S4区域为2个相交的纵向平面. 图2(a)中的线表示LiDAR一条负角度线束的扫描线. 图2(b)、(c)中，虚线表示平面点的集合，实线表示地面点的集合，三角表示跳变远点，星号表示边线点. 由图2可知，结合l维度和h维度的点云信息进行双向降维曲率滤波，可以精确地进行特征点提取以及平面和地面点分割.

当今最流行的三维曲率特征提取方法主要计算LiDAR点与当前线束相邻点间三维空间中的局部光滑度. 同一LiDAR线束地面点在三维空间中呈现圆弧状且局部光滑度较低，所以三维曲率提取地面点的精准度较低. 利用提出的双向降维曲率滤波的方法，可以对地面点进行更高精度的提取，且对纵向平面的约束较三维曲率滤波更加严格，具有提取精度更高且特征点更少的优势. 地面和平面点的分割是当今点云数据处理的关键问题，当今的分割算法均具有计算量大、无法与特征提取相结合的劣势. 通过降维曲率滤波的方法，可以基于地面和平面的特征对地面点和平面点分别进行提取，在特征提取的同时分割地面和平面点，弥补了当今地面和平面点分割算法的劣势，具有较高的分割精度. 利用降维曲率滤波的方法，可以增强对竖直方向平面点和水平方向地面点的约束，抑制平面或地面点特征不稳定造成的里程计的横向或纵向漂移. 特征点提取和分割效果如图3所示. 如图3（a）、（b）所示为在原始点云中提取并分割出的高质量平面和地面点组成的点云图.

以上一帧经卡尔曼滤波后的最优状态为前向传播的先验状态，以IMU帧间的时间差为间隔，前向传播IMU测量值，获得当前帧的初始估计状态.

IMU测量的前向传播公式为

(3)$ {{\boldsymbol{v}}_{{{k}}+1}} = {{\boldsymbol{v}}_{{k}}}+({\boldsymbol{R}}_k^{}{{\boldsymbol{a}}_{{k}}}+{\boldsymbol{g}}){{\Delta}} {{{t}}_{\mathrm{f}}} ,$

(4)$ {{\boldsymbol{p}}_{{{k}}+1}} = {{\boldsymbol{p}}_k}+{{\boldsymbol{v}}_{{k}}}\Delta {{{t}}_{\mathrm{f}}}, $

(5)$ {{\boldsymbol{R}}_{k+1}} = {{\boldsymbol{R}}_k}{\mathrm{exp}}\;({{\boldsymbol{\omega}} _{{k}}}{{\Delta}} {{{t_{\mathrm{f}}}}}) .$

式中：$ \Delta {t_{\mathrm{f}}} $为IMU帧间时间差，$ {{\boldsymbol{v}}_{{k}}} $为第k帧IMU在世界系下测量的速度，$ {{\boldsymbol{p}}}_k $为第k帧IMU在世界系下测量的位置，$ {{\boldsymbol{R}}}_k $为第k帧IMU在世界系下测量的姿态，$ {{\boldsymbol{a}}}_k $为第k帧IMU系下测量的加速度，$ {{\boldsymbol{\omega}}} _k $为第k帧IMU系下测量的角速度，$ {\boldsymbol{g}} $为IMU在世界坐标系下的重力矢量.

以当前帧经IMU预积分后的初始估计状态为反向传播的先验，以当前LiDAR点的时间戳左侧的IMU测量为输入，以当前LiDAR帧点间的时间差为间隔进行反向传播，对当前帧LiDAR点进行畸变补偿. 当前帧LiDAR点的反向传播公式为

(6)$ {{\boldsymbol{v}}_n} = {{\boldsymbol{v_{{{{n}}}+1}}}} - ({\boldsymbol{R_{{{{n}}}+1}}}^{}{{\boldsymbol{a_{{{{k}}}+1}}}}+{\boldsymbol{g}})\Delta {t_{\mathrm{l}}}, $

(7)$ {{\boldsymbol{p}}_n} = {{\boldsymbol{p}}_{{{n}}+1}} - {{\boldsymbol{v}}_{{{k}}+1}}\Delta {t_{\mathrm{l}}} ,$

(8)$ {{\boldsymbol{R}}_n} = {\mathrm{log}}\;(({{\boldsymbol{\omega}} _{{{k}}+1}}\Delta {t_{\mathrm{l}}})^{\mathrm{T}}{{\boldsymbol{R}}_{{{n}}+1}}) . $

式中：$ \Delta {t_{\mathrm{l}}} $为LiDAR点间的时间差，$ {{\boldsymbol{v}}_n} $、$ {{\boldsymbol{p}}_n} $和$ {\boldsymbol{R}}_n $为经反向传播后获得的第n个LiDAR点在世界系下的速度、位置和姿态.

在前向传播IMU测量的同时，传播IMU测量的协方差. 以上一帧经卡尔曼滤波后的状态协方差为先验协方差，以IMU帧间的时间差为间隔，前向传播IMU测量的协方差，获得当前帧的初始估计协方差. IMU测量的协方差的前向传播公式为

(9)$ {{\boldsymbol{P}}_{{{i}}+1}} = {{\boldsymbol{F}}_{{{\boldsymbol{\varepsilon}} _{\boldsymbol{i}}}}}{{\boldsymbol{P_i}}}{\boldsymbol{F}}_{{{\boldsymbol{\varepsilon _i}}}}^{\mathrm{T}}+{{\boldsymbol{F}}_{{{\boldsymbol{\omega _i}}}}}{\boldsymbol{QF}}_{{{\boldsymbol{\omega _i}}}}^{{{\mathrm{T}}}} . $

式中：$ {{\boldsymbol{\varepsilon }}}_i $为第i帧IMU测量的误差，$ {{\boldsymbol{F_{{{\boldsymbol{\varepsilon}} _i}}}}} $为第i帧IMU测量误差的雅可比矩阵，$ {{\boldsymbol{F}}_{{{\boldsymbol{\omega }}_i}}} $为第i帧IMU测量的高斯白噪声的雅可比矩阵，$ {{\boldsymbol{P}}_{{i}}} $和Q分别为第i帧IMU测量和高斯白噪声的协方差.

将前向传播得到的当前帧初始估计状态和初始估计协方差、反向传播得到的畸变补偿后的当前帧点云和离最后一个雷达点时间戳最近的GPS信息送入卡尔曼滤波器中，构建观测误差方程和预测误差方程. 里程计的状态、误差、状态与误差的广义加和状态与误差的广义减可以定义为

(10)$ {\boldsymbol{x}} = [{\boldsymbol{R}}^{\mathrm{T}},{\boldsymbol{p}}^{\mathrm{T}},{\boldsymbol{v}}^{\mathrm{T}},{\boldsymbol{b}}_{\boldsymbol{\omega}} ^{\mathrm{T}},{\boldsymbol{b}}_{\boldsymbol{a}}^{\mathrm{T}},{\boldsymbol{g}}^{\mathrm{T}}]^{\mathrm{T}} $

(11)$ {\boldsymbol{\varepsilon}} = [({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{R}})_{}^{\mathrm{T}}},({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{p}})_{}^{\mathrm{T}}},({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{v}})_{}^{\mathrm{T}}},({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{b}}_{\boldsymbol{\omega}}})^{\mathrm{T}},({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{b_a}}})^{\mathrm{T}},({\boldsymbol{\varepsilon}} ^{{\boldsymbol{g}}})^{\mathrm{T}}]_{}^{\mathrm{T}}, $

(12)$ \begin{gathered} {\boldsymbol{x}}\otimes {\boldsymbol{\varepsilon}} = [{\boldsymbol{R}}_{}^{_{}^{\mathrm{T}}}{\mathrm{exp}}\;[({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{R}}})_{}^{\mathrm{T}}],{\boldsymbol{p}}^{{\mathrm{T}}}+({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{p}}})_{}^{\mathrm{T}}, \\ {\boldsymbol{v}}^{{\mathrm{T}}}+({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{v}}})_{}^{\mathrm{T}},{\boldsymbol{b}}_{\boldsymbol{\omega}} ^{\mathrm{T}}+({\boldsymbol{\varepsilon}} _{}^{\boldsymbol{b_\omega}}) _{}^{\mathrm{T}},{\boldsymbol{g}}^{\mathrm{T}}+({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{g}}})^{\mathrm{T}}]_{}^{\mathrm{T}}, \\ \end{gathered} $

(13)$ \begin{gathered} {\boldsymbol{x}} \odot {\boldsymbol{\varepsilon}} = [\log\;[{\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{R}}_{}}{\boldsymbol{R}}^{{\mathrm{T}}}]_{}^{\mathrm{T}},{\boldsymbol{p}}^{{\mathrm{T}}} - ({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{p}}})_{}^{ {\mathrm{T}}},{\boldsymbol{v}}^{{\mathrm{T}}} - \\ ({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{v}}})_{}^{\mathrm{T}},({\boldsymbol{b}}_{\boldsymbol{\omega}}) ^{ {\mathrm{T}}} - ({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{b_\omega}}}) _{}^{\mathrm{T}},({\boldsymbol{b}}_{\boldsymbol{a}})^{\mathrm{T}} - ({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{b_a}}})_{}^{\mathrm{T}},{\boldsymbol{g}}^{\mathrm{T}} - ({\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{g}}})_{}^{\mathrm{T}}]_{}^{\mathrm{T}}. \\ \end{gathered} $

式中：x为里程计的状态，$ {\boldsymbol{\varepsilon }}$为里程计的整体状态误差，ε^R为里程计的姿态误差，ε^p为里程计的位置误差，ε^v为里程计的速度误差，${\boldsymbol{b_\omega}} $、$ {\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{b_\omega}}} $分别为里程计的角速度偏置及其误差，${\boldsymbol{b_a}} 、{\boldsymbol{\varepsilon}} _{}^{{\boldsymbol{b_a}}}$分别为里程计的加速度偏置及其误差，ε^g为重力矢量的误差，$ \otimes $为广义加符号，$ \odot $为广义减符号. 里程计第k帧的预测误差方程可以定义为

(14)$ \tilde{{\boldsymbol{\varepsilon}} }={{\boldsymbol{x}}}_{{\boldsymbol{k}}}^{}\odot \mathop {{{{\boldsymbol{x}}}}_{{\boldsymbol{k}}}^{}}\limits^{\frown}=({{\mathop{{\boldsymbol{x}}}\limits^{\frown}}}_{{\boldsymbol{k}}}^{{\boldsymbol{i}}}\otimes {\tilde{{\boldsymbol{x}}}}_{{\boldsymbol{k}}}^{{\boldsymbol{i}}})\odot {{\mathop{{\boldsymbol{x}}}\limits^{\frown}}}_{{\boldsymbol{k}}}^{}=({{\mathop{{\boldsymbol{x}}}\limits^{\frown}}}_{{\boldsymbol{k}}}^{{\boldsymbol{i}}}\odot {\mathop{{\boldsymbol{x}}}\limits^{\frown}}_{{\boldsymbol{k}}}^{})+{{\boldsymbol{h}}}_{{\boldsymbol{i}}}^{}{\tilde{{\boldsymbol{x}}}}_{{\boldsymbol{k}}}^{{\boldsymbol{i}}} .$

式中：$ \widetilde {\boldsymbol{\varepsilon}} $为第k帧的真实状态$ {\boldsymbol{x}}_k $与传播状态$ {\mathop{{\boldsymbol{x}}}\limits^{\frown}}_{{{k}}}^{} $的误差，$ {\mathop{{\boldsymbol{x}}}\limits^{\frown}}_{{{k}}}^{{{i}}} $和$ \widetilde {\boldsymbol{x}}_{{k}}^i $为当前帧卡尔曼滤波第i次迭代更新的状态值和其与真实状态之间的误差，$ {\boldsymbol{h}}_{{i}}^{} $为$ \widetilde {\boldsymbol{x}}_{{k}}^{{i}} $在预测误差方程中的雅可比矩阵.

将当前帧含有m个点的LiDAR点云L中的第h个点$ {\boldsymbol{p}}_h^L $以状态x映射到全局坐标系下与历史地图中的近邻点拟合成的局部平面的距离残差$ {{{\boldsymbol{z}}({\boldsymbol{x}},{\boldsymbol{p}}_h^L)}} $定义为点$ {\boldsymbol{p}}_h^L $的观测状态误差，则当前帧的观测状态误差为$ \displaystyle\sum\nolimits_{{{h}} = 0}^{{m}} {{\boldsymbol{z(x,p}}_h^L)} $. 第k帧第i次迭代的观测误差方程可以表示为

(15)$ 0\approx {\displaystyle \sum _{{{h}}=0}^{{{m}}}{\boldsymbol{z}}({\mathop{x}\limits^{\frown}}_{{{k}}}^{{{i}}},{{\boldsymbol{p}}}_{{{h}}}^{{{L}}})}+{{\boldsymbol{H}}}_{{{k}}}^{{{h}}}{\tilde{{\boldsymbol{x}}}}_{{{k}}}^{{{i}}} . $

式中：$ {\boldsymbol{H}}_k^h $为$ \widetilde {\boldsymbol{x}}_{{k}}^{{i}} $在观测误差中的雅可比矩阵. 由此可知，观测误差方程定义当前帧点云与历史帧点云中的近邻点云拟合成的局部平面的距离为观测误差，后端优化通过迭代更新位姿状态的方式来缩小当前帧所有点到相应局部平面的距离，所以观测误差方程在卡尔曼滤波过程中对误差的校正完全依赖于历史帧点云信息的准确性，容易造成累计误差的不断增加. 里程计计算的轨迹具有连续性，当里程计轨迹产生累计漂移时，漂移里程计轨迹附近的局部历史帧点云地图是不准确的，所以基于历史帧点云地图构建的当前帧点云观测误差方程是错误的，带有无法校正的基础误差. GPS信息的引入可以为我们提供先验位姿，对当前帧点云到世界系下的位姿映射进行伪加权，利用观测误差方程通过点云配准校正位姿状态的方法，纠正累计误差. 如图4所示，S2区域点云为带有累计轨迹误差的历史帧组成的局部点云地图，S1区域点云为历史帧点云在真实轨迹位姿下的局部点云地图. 由此可知，当里程计移动到S2区域部分并产生了累计误差，里程计的局部点云地图中插入S2区域部分带有累计误差的点云后，若通过S2区域的局部点云地图构建观测误差方程，则经过后端优化后的里程计误差会越来越大. 为了解决上述问题，提出伪轨迹加权的方法，在卡尔曼滤波的观测误差方程中加入GPS信息，将带有累计误差的当前帧位姿向当前帧位姿与GPS信息相反的误差更大的方向进行伪加权. 如图4所示，将当前帧点云在加权后的伪轨迹下与带有累计误差的S2区域历史帧点云进行残差拟合，里程计位姿将向箭头指向的方向进行校正. 将校正的状态改变量与带有累计误差的当前帧位姿进行融合，里程计将会向GPS轨迹方向校正累计误差，实现基于卡尔曼滤波的LiDAR、IMU和GPS信息的融合. GPS的优势是全局轨迹的鲁棒性，而里程计的局部精度很高，所以仅当里程计轨迹与GPS轨迹误差达到一定阈值时，通过伪轨迹加权对里程计轨迹进行全局性约束. 加入GPS约束和伪轨迹加权的观测误差方程为

(16)$ 0\approx {\displaystyle \sum _{{{h}}=0}^{{{m}}}[{\boldsymbol{z}}({\mathop{{\boldsymbol{x}}}\limits^{\frown}}_{{{k}}}^{{{i}}}\otimes {\boldsymbol{Q}}({\mathop{{\boldsymbol{x}}}\limits^{\frown}}_{{{k}}}^{{{i}}}\odot {{\boldsymbol{x}}}_{{{k}}}^{{\rm{gps}}}),{{\boldsymbol{p}}}_{{{h}}}^{{{L}}})}+_{}^{{\rm{G}}}{{\boldsymbol{H}}}_{{{k}}}^{{{h}}}{\tilde{{\boldsymbol{x}}}}_{{{k}}}^{{{i}}}] .$

式中：$ {\boldsymbol{x}}_k^{{\mathrm{gps}}} $为当前帧的GPS信息状态，Q为伪轨迹加权的权重，$ _{}^{\rm{G}}{\boldsymbol{H}} _k^h$为加入GPS约束后的新的$ \widetilde {\boldsymbol{x}}_{{k}}^{{i}} $的雅可比矩阵.

地面点云构建的点面残差方程对里程计的纵向位姿约束较强，平面点云构建的点面残差方程对里程计的横向位姿约束较强. 当LiDAR当前帧缺少高质量的地面点或平面点时，观测误差方程构建所使用的平面和地面点云比例会失衡，导致里程计纵向或横向位姿解算的欠拟合或过拟合. 为了解决上述问题，改进了里程计后端优化的最小化函数，提出的最小化函数将平面和地面点分离构建观测误差方程，对平面或地面点构建的观测误差方程进行加权，平衡平面和地面点云对里程计位姿的约束. 对前端通过双向降维曲率滤波提取并分割后的平面和地面点云分别构建观测误差方程，利用提出的点云伪占用方法对平面和地面点构建的观测误差方程进行加权. 当平面点的占比较大时，重复使用地面点构建观测误差方程中的点面残差；当地面点的占比较大时，重复使用平面点构建观测误差方程中的点面残差. 通过点云伪占用的方法，可以平衡地面和平面点在观测状态误差中的占比，抑制高质量平面或地面点缺失造成的横向或纵向漂移. 通过式(14)、(16)共同构建后端优化的最小化函数，形成最大后验估计(MAP)如下：

(17)$ \begin{array}{l}{\mathrm{mi{n}}}_{{\tilde{\boldsymbol{x}}}_{{k}}^{{{i}}}}\left({\left\| ({\mathop{\boldsymbol{x}}\limits^{\frown}}_{{k}}^{{i}}\odot {\mathop{\boldsymbol{x}}\limits^{\frown}}_{{k}}^{})+{{\boldsymbol{h}}}_{{{i}}}^{}{\tilde{\boldsymbol{x}}}_{{k}}^{{i}}\right\| }_{({{\boldsymbol{P}}}_{{k}}^{{{i}}})_{}^{-1}}^{2}+\right.\\\left. A{\displaystyle \sum _{h=0}^{l}{\left\| {\boldsymbol{z}}({\mathop{\boldsymbol{x}}\limits^{\frown}}_{{k}}^{{i}}\otimes {\boldsymbol{Q}}({\mathop{\boldsymbol{x}}\limits^{\frown}}_{{k}}^{{i}}\odot{\boldsymbol{x}}_{{k}}^{{\rm{gps}}}),{{\boldsymbol{p}}}_{{{h}}}^{{{L}}})+_{}^{{\rm{G}}}{{\boldsymbol{H}}}_{{k}}^{{{h}}}{\tilde{\boldsymbol{x}}}_{{k}}^{{i}}\right\| }_{({{\boldsymbol{U}}}_{{k}}^{i})_{}^{-1}}^{2}}\right. +\\\left. B{\displaystyle \sum _{h=l}^{l+n}{\left\| {\boldsymbol{z}}({\mathop{\boldsymbol{x}}\limits^{\frown}}_{{k}}^{{i}}\otimes {\boldsymbol{Q}}({\mathop{\boldsymbol{x}}\limits^{\frown}}_{{k}}^{{i}}\odot{\boldsymbol{x}}_{{k}}^{{\rm{gps}}}),{{\boldsymbol{p}}}_{{{h}}}^{{{L}}})+_{}^{{\rm{G}}}{{\boldsymbol{H}}}_{{k}}^{{{h}}}{\tilde{\boldsymbol{x}}}_{{k}}^{{i}}\right\| }_{({{\boldsymbol{U}}}_{{k}}^{{{i}}})_{}^{-1}}^{2}}\right).\end{array} $

式中：$ A+B = 1 $; $ \left\| {\boldsymbol{x}} \right\|_P^2 = {\boldsymbol{x}}_{}^{\rm T}{\boldsymbol{Px}} $，$ {\boldsymbol{P}}_k^i $为第k帧第i次状态更新的预测协方差，$ {\boldsymbol{U}}_k^i $为第k帧第i次状态更新的观测的协方差，l为当前帧的点云经降维曲率滤波分割后的平面点数量，n为当前帧的点云经降维曲率滤波分割后的地面点数量，A为当前帧平面点伪占用的自适应阈值，B为当前帧地面点伪占用的自适应阈值. 当平面点数量较大时，通过高质量地面点重新使用的方式，提高自适应阈值B；当地面点数量较大时，通过高质量平面点重新使用的方式，提高自适应阈值A. 第k帧第i次迭代的卡尔曼增益$ {\boldsymbol{K}}_k^i $和第i次迭代的更新误差状态$ \overline{\overline {\boldsymbol{\varepsilon}} } _i^{i+1} $可以分别表示为

(18)$ {\boldsymbol{K}}_k^i = ({\boldsymbol{H}}_{{\boldsymbol{ki}}}^{\mathrm{T}}({\boldsymbol{U}}_{\boldsymbol{k}}^{{\boldsymbol{i}}})^{ - 1}{\boldsymbol{H}}_{{{ki}}}^{}+{\boldsymbol{P}}_{{{ki}}}^{_{}^{ - 1}})_{}^{ - 1}{\boldsymbol{H}}_{{{ki}}}^{\mathrm{T}}({\boldsymbol{U}}_{{k}}^{{i}})_{}^{ - 1}, $

(19)$ {\overline{\overline{{\boldsymbol{\varepsilon}} }}}_{i}^{i+1}=-{{\boldsymbol{K}}}_{{{k}}}^{{{i}}}{{\boldsymbol{z}}}_{{{k}}}^{{{i}}}-({\boldsymbol{I}}-{{\boldsymbol{K}}}_{{{k}}}^{{{i}}}{{\boldsymbol{H}}}_{{{ki}}}^{}){{\boldsymbol{h}}}_{{{i}}}^{-1}({\mathop{{\boldsymbol{x}}}\limits^{\frown}}_{{{k}}}^{{{i}}}\odot {\mathop{{\boldsymbol{x}}}\limits^{\frown}}_{{{k}}}^{}) .$

式中：$ {\mathop{{\boldsymbol{x}}}\limits^{\frown}}_{{{k}}}^{{{i}}+1}={\mathop{{\boldsymbol{x}}}\limits^{\frown}}_{{{k}}}^{{{i}}}\otimes {\overline{\overline{{\boldsymbol{\varepsilon}} }}}_{{{i}}}^{{{i}}+1} $，当$ {\mathop{{\boldsymbol{x}}}\limits^{\frown}}_{{{k}}}^{{{i}}+1}\odot {\mathop{{\boldsymbol{x}}}\limits^{\frown}}_{{{k}}}^{{{i}}} $收敛时，结束迭代；$ {\boldsymbol{P_{ki}}}^{_{}^{}} $为第k帧第i次状态更新构建卡尔曼增益的预测协方差，$ {\boldsymbol{P_{ki}}}^{_{}^{}} = ({\boldsymbol{h}}_{{i}}^{ - 1}){\boldsymbol{P}}_k^i({\boldsymbol{h}}_i^{ - 1})_{}^{\mathrm{T}} $；$ {\boldsymbol{H_{ki}}}^{} $为第k帧第i次迭代的所有点的雅可比矩阵的集合；$ {\boldsymbol{z}}_k^i $为第k帧第i次迭代的所有点的观测误差的集合. 经过卡尔曼滤波后的第k帧状态$ \overline {\boldsymbol{x}} _{{k}}^{} $和协方差矩阵$ \overline {\boldsymbol{P}} _{{k}}^{} $为

(20)$ {\overline{{\boldsymbol{x}}}}_{{{k}}}^{}={\mathop{{\boldsymbol{x}}}\limits^{\frown}}_{{{k}}}^{{\boldsymbol{i}}+1} , $

(21)$ \overline {\boldsymbol{P}} _{{k}}^{} = ({\boldsymbol{I}} - {\boldsymbol{K}}_{{k}}^{{i}}{\boldsymbol{H_{ki}}}^{}){\boldsymbol{P}}_{{{ki}}}^{} . $

使用公开数据集KITTI^[25]和自采室外园区数据集，对提出的方法进行定量和定性评估. 公开数据集KITTI^[25]采集平台搭载了Velodyne Lidar HDL-64E 激光雷达和OXTS RT 3003定位系统，该系统结合了GPS、GLONASS、IMU和RTK校正信号. 自己采集的数据集采集平台搭载了Velodyne VLP-16激光雷达、Xsens MTI-100 IMU和GPS系统. 分别将提出的LCG-LIO不添加GPS约束、LCG-LIO添加GPS约束和FAST-LIO2在KITTI数据集上进行纵向对比，对LCG-LIO的精度和效率进行评估. 将通过提出的伪轨迹加权方法融合GPS信息的LCG-LIO与通过因子图优化融合GPS信息的LIO-SAM在自采数据集上进行横向对比. 为了公平起见，在实验过程中关闭了LIO-SAM中的回环检测模块，分别对LCG-LIO的精度和GPS信息融合方法的可行性进行评估. 为了保证实验结果的准确性，所有实验均在Intel(R) Core(TM) i7-12700H处理器、主频为2.3 GHZ、内存为16 GB的计算机上开展.

分别将LCG-LIO添加GPS约束、LCG-LIO不添加GPS约束和FAST-LIO2在KITTI数据集上进行测试. 使用EVO^[26]工具绘制里程计轨迹，计算不同方法的均方根误差(RMSE). 运行结果、轨迹和轨迹局部细节如图5~7所示. 图中，N为KITTI数据集的帧数. 不同方法的绝对位姿误差如表1所示.

表1中，$ {\partial _{{\mathrm{max}}}} $为绝对位姿误差的最大值，$ {\partial _{{\mathrm{mean}}}} $为绝对位姿误差的均值，$ {\partial _{{\mathrm{median}}}} $为绝对位姿误差的中值，$ {\partial _{{\mathrm{min}}}} $为绝对位姿误差的最小值，$ {\partial _{{\mathrm{rmse}}}} $为均方根误差. 从图5、6可知，FAST-LIO2的建图效果最差，整体结构产生了很大的畸变. FAST-LIO2的轨迹产生了很明显的轨迹漂移，转弯位置和特征退化位置是轨迹漂移的起始位置，随后累计漂移越来越大，没有得到修正. 不添加GPS约束的LCG-LIO的建图效果明显优于FAST-LIO2,可见整体结构比较成型，但存在一定的畸变，与真实尺寸有一定的差异. 不添加GPS约束的LCG-LIO的轨迹较FAST-LIO2明显更接近真实值，转弯位置的漂移明显减小. 由此可见，利用提出的双向降维曲率滤波的特征提取方法，结合伪占用的平面点和地面点平衡方法，提高了整体点云的质量和双向面点的约束. 不添加GPS的LCG-LIO在点云整体质量差且不稳定的转弯和特征退化位置，提取出了可靠的平面点和地平面点，平衡了平面点和地面点的约束，明显抑制了漂移. 添加GPS约束的LCG-LIO的建图效果优于不添加GPS约束的LCG-LIO，整体的畸变明显减小，建图尺寸与真实尺寸更加一致. 添加了GPS约束后，虽然转弯位置产生了漂移，但由于GPS约束的校正作用，没有产生之后的累计误差. 在发生轨迹漂移后，添加GPS约束的LCG-LIO会通过里程计轨迹与GPS轨迹之间的误差进行伪轨迹加权，实时对里程计的误差进行纠正，抑制里程计的累计漂移. 从图7可知，添加GPS约束的LCG-LIO的轨迹明显更接近于真实值，具有明显的累计误差校正作用. 从表1可知，FAST-LIO2的均方根误差为20.417，不添加GPS约束的LCG-LIO的均方根误差为11.264，添加GPS约束的LCG-LIO的均方根误差为9.162. 可见，不添加GPS约束的LCG-LIO的精度较FAST-LIO2提高了44.83%，添加GPS约束的LCG-LIO的精度较FAST-LIO2和不添加GPS约束的LCG-LIO分别提高了55.13%和18.66%.

分别将LCG-LIO添加GPS约束与LIO-SAM添加GPS约束在自采室外园区数据集上进行测试. 将当今最流行的基于因子图优化方法融合GPS信息的紧耦合激光里程计LIO-SAM与LCG-LIO进行对比，验证通过伪轨迹加权的方法将GPS信息融入卡尔曼滤波中的可行性. 数据集的采集过程严格按照相同位置起始和终止，所以通过计算起始和终止位置的闭环距离来评估里程计精度. 运行结果如图8所示. 不同方法的闭环误差如表2所示.

由图8可知，LCG-LIO的整体建图效果明显优于LIO-SAM，轮廓更加清晰. LCG-LIO的末端里程计坐标系明显重合，而LIO-SAM的末端里程计坐标系产生了明显的回环误差，局部建图效果产生了明显的漂移. 由此可见，由于GPS信息的局部精度较差，使用实时GPS信息进行因子图优化的LIO-SAM方法的精度较差. 由于GPS信息的全局鲁棒性，通过使用与里程计轨迹误差较大的部分GPS信息进行伪轨迹加权的LCG-LIO方法的精度较高. 由表2可知，LIO-SAM的回环误差达到0.920 m，LCG-LIO的回环误差明显减小，仅为0.052 m，实现了cm级定位. 由此可见，LCG-LIO通过伪轨迹加权的方法融合GPS信息，具有较高的可行性.

将FAST-LIO2和LCG-LIO在KITTI数据集上的每一帧整体运行时间绘制成曲线图进行直观的对比，计算2种方法的各部分运行时间均值和平均每一帧使用的特征点数，并进行对比.

表3中，N_p为点云数量，t₁为特征提取模块的耗时，t₂为里程计部分耗时，t为总体耗时. 由表3可知，LCG-LIO使用的特征点数较FAST-LIO2少82.72%，但精度更高. LCG-LIO不添加GPS约束和LCG-LIO的里程计部分耗时大致相同，可见GPS约束的加入对LCG-LIO的里程计效率没有明显影响. 由于处理的点云数变少，里程计部分的计算量明显降低，里程计部分耗时较FAST-LIO2减少了72.20%，总体处理时间较FAST-LIO2减少了53.01%. 可见，LCG-LIO在提升精度的同时，减少了里程计的计算量，提高了里程计的效率.

本文提出基于改进卡尔曼滤波的轻量级LiDAR-IMU里程计LCG-LIO. LCG-LIO前端通过提出的双向降维曲率滤波的方法，分别对平面点和地面点进行有针对性的提取和分割. 通过点云伪占用的方法平衡了平面和地面点的数量，减少了特征点数，提高了待处理点云的整体质量. LCG-LIO改进了后端卡尔曼滤波中的观测误差方程和观测误差的雅可比矩阵，融入了GPS约束，通过伪轨迹加权的方法实现了累计误差的校正. 将提出的方法在具有挑战性的公开数据集KITTI和自采数据集上进行实验. 实验结果表明，LCG-LIO较FAST-LIO2具有更高的精度和效率，且具有更强的自适应性；LCG-LIO中的GPS信息融合方法较LIO-SAM中的方法具有更高的可行性.