CN117705145A - 一种局部几何信息融合的激光惯性里程计方法 - Google Patents

Abstract

本发明公开了一种局部几何信息融合的激光惯性里程计方法，涉及计算机视觉技术领域，包括如下步骤：获取预设时间段内，目标车辆的点云数据和惯性测量数据；并通过惯性测量数据对点云数据进行畸变矫正；对畸变矫正后的点云数据进行特征提取，获得边缘点和平面点；分别对边缘点和平面点进行匹配处理；基于匹配处理后的边缘点和平面点，同时结合IMU预积分约束，获得目标车辆的里程计。该方法通过融合局部相似几何信息，可以有效减少了系统计算冗余性以及提高定位准确性。

Description

一种局部几何信息融合的激光惯性里程计方法

技术领域

本发明涉及计算机视觉技术领域，更具体的说是涉及一种局部几何信息融合的激光惯性里程计方法。

背景技术

随着科技的迅猛发展，无人驾驶技术逐渐成为汽车行业的研究热点。传统的驾驶方式存在诸多问题，例如交通事故、交通拥堵和能源浪费等，这些问题给人们的生活和社会带来了许多负面影响。而无人驾驶技术的出现为解决这些问题提供了新的可能性。无人驾驶技术利用激光雷达、摄像头和惯性传感器等感知设备，同时结合人工智能和自动控制算法，使汽车具备自主感知、决策和操作能力，能够在没有人类驾驶员的情况下进行安全、高效的驾驶。

随着应用需求的不断增加和细分，无人系统面临着在一些复杂、随机和多变的环境下进行自主作业的挑战，例如室内、地下、隧道和对抗干扰环境等。在这些环境中，全球导航卫星系统(Global Navigation Satellite System,GNSS)的信号可能被遮挡或屏蔽。因此，激光雷达和惯性传感器等自主传感器成为了常用的感知和导航手段。激光雷达能够收集自身到周围物体之间的距离信息，确定自身在环境中的相对位置，具有误差恒定、测量频率高等优点，是同步定位与建图(Simultaneous Localization and Mapping，SLAM)技术应用最为广泛的传感器之一。文献“LOAM:Lidar odometry and mapping in real-time”，(J.Zhang and S.Singh,In Proc.of Robotics:Science and Systems，2014)提出了激光里程计和建图(Lidar Odometry and Mapping，LOAM)框架，其根据曲率从激光扫描点云中提取位于尖锐边缘和平面表面块的特征点，并且分别配准到边缘线段和平面特征上。文献“Lego-loam:Lightweight and ground optimized lidar odometry andmapping onvariable terrain”，(T.Shan and B.Englot，In IEEE/RSJ International Conferenceon Intelligent Robots and Systems，2018)在LOAM的基础上继续改进，提出了LeGO-LOAM(Lightweight and Groud-Optimized LOAM)框架。相较于LOAM框架，LeGO-LOAM框架增加了点云地面分割模块，通过提取地面点和分割点，对点云进行聚类，并基于平滑度二次选取特征点，提高了鲁棒性。

激光雷达SLAM在处理平面光滑特征时能够体现出优势，但它依赖于简单的扫描匹配方法，稳定性不高。此外，激光SLAM的环境特征不明显，数据缺乏色彩、纹理信息，易产生运动畸变，对于导航来说并不可靠，因此往往需要与惯性测量单元(Inertial MeasurementUnit，IMU)进行融合。文献“LIOSAM:Tightly-coupled LidarInertial Odometry viaSmoothing and Mapping”，(T.Shan and B.Englot，In IEEE/RSJ InternationalConference on Intelligent Robots and Systems，2020)利用ISAM平滑优化工具箱，将惯性、激光雷达和GPS进行融合，并且采用一种局部地图匹配策略替换LOAM中的全局地图匹配策略，LIO-SAM具有更高的精度和更短的计算耗时。文献“Fast-lio2:Fast direct lidar-inertial odometry”，(Xu W,Cai Y,He D,et al.,IEEE Transactions On Robotics，2022)直接将激光雷达获得的原始点云与地图进行匹配，使用Ikd-tree来保障对地图的快速搜索，利用迭代误差卡尔曼滤波器将激光里程计和IMU进行紧融合。然而，目前激光惯性里程计方法依然存在一个主要的问题：在点云配准过程中，通常仅使用局部的点云地图信息进行拟合，而当处于一个结构化的环境中，许多局部直线和平面表示的是一个平面，这会带来信息的冗余。此外，如果点在同一个平面或直线上，用于提取平面或直线的点越多，其拟合的结果越准确。因此，通常尝试将粗配准的点线(面)对进行融合，可以有效提高系统和准确性。

因此，如何有效提高里程计算法在具有结构化的环境中的稳定性，有效减少里程计算法耗时，提高里程计算法的准确性和鲁棒性，是本领域技术人员亟需解决的问题。

发明内容

鉴于上述问题，本发明提供一种局部几何信息融合的激光惯性里程计方法，以至少解决上述背景技术中提到的部分技术问题。

为了实现上述目的，本发明采用如下技术方案：

本发明实施例提供了一种局部几何信息融合的激光惯性里程计方法，包括如下步骤：

S1、获取预设时间段内，目标车辆的点云数据和惯性测量数据；并通过所述惯性测量数据对所述点云数据进行畸变矫正；

S2、对畸变矫正后的所述点云数据进行特征提取，获得边缘点和平面点；

S3、分别对所述边缘点和所述平面点进行匹配处理；

S4、基于匹配处理后的边缘点和平面点，同时结合IMU预积分约束，获得所述目标车辆的里程计。

进一步地，所述步骤S1具体包括：

S11、获取预设时间段内目标车辆的点云数据，并同步获得所述目标车辆的惯性测量数据；

S12、通过对所述惯性测量数据进行插值处理，分别获取所述预设时间段内，第任意个时刻的点云位姿和最后个时刻的点云位姿；

S13、根据所述第任意个时刻的点云位姿和所述最后个时刻的点云位姿，获得所述点云数据在第任意个时刻至最后个时刻的相对运动；

S14、根据所述点云数据在第任意个时刻至最后个时刻的相对运动，并结合惯性测量的外参数，获得所述点云数据在第任意个时刻至最后个时刻的相对变换；

S15、基于所述点云数据在第任意个时刻至最后个时刻的相对变换，将第任意个时刻的点云数据投影到最后个时刻的坐标系下，得到最后个时刻的点云数据，实现对所述点云数据进行畸变矫正。

进一步地，所述步骤S2具体包括：获取每个点云数据的曲率；所述曲率大于第一预设阈值的点云数据属于边缘点；所述曲率小于第一预设阈值的点云数据属于平面点。

进一步地，所述步骤S3中具体包括：

S31、分别对所述边缘点和所述平面点进行初始匹配；

S32、对初始匹配后的边缘点和平面点，分别进行融合匹配。

进一步地，所述步骤S31具体包括：

S311、使用IMU积分估计的姿态作为初值，将当前时刻下激光雷达坐标系下的边缘点或平面点变换到世界坐标系下，获得世界坐标系下的边缘点或平面点；

S312、查询世界坐标系下当前局部地图中边缘点或平面点的多个邻近点，获得邻近点集合；

S313、获取所述邻近点集合的协方差矩阵；

S314、对所述协方差矩阵进行特征值分解处理，获得多个特征值；

S315、基于所述多个特征值，分别对所述边缘点和平面点进行初始匹配。

进一步地，所述步骤S315具体包括：

(1)对于所述边缘点：将首个特征值和最后一个特征值的比值作为矩阵条件；判断所述矩阵条件是否大于第二预设阈值；若所述矩阵条件大于第二预设阈值，则获得所述边缘点的初始匹配直线对(p^l,v^l)；其中，p^l等于邻近点集合中所有邻近点的均值；v^l表示首个特征值对应的特征向量；

(2)对于所述平面点：判断最后一个特征值是否小于第三预设阈值；若最后一个特征值小于第三预设阈值，则获得所述平面点的初始匹配平面对(p^p,n^p)；其中，p^p等于邻近点集合中所有邻近点的均值；n^p表示最后一个特征值对应的特征向量。

进一步地，在所述步骤S32中，对初始匹配后的边缘点进行融合匹配，具体包括：

根据所述初始匹配直线对，获得初始匹配直线集合；对于所述初始匹配直线集合中的每条直线对，筛选出最邻近的直线对；将每条直线对记作第一直线对，将所述第一直线对最邻近的直线对记作第二直线对；

获取所述第一直线对和所述第二直线对之间的距离向量r_l；

若所述距离向量r_l的二范数小于第四预设阈值，则将所述第一直线对和所述第二直线对相融合，并获取融合后的直线对邻近点集合；

获取所述直线对邻近点集合对应的直线对协方差矩阵；

对所述直线对协方差矩阵进行特征值分解处理，获得多个特征值，基于其中最大特征值对应的特征向量获得融合匹配直线对/>其中/>等于所述直线对邻近点集合中所有邻近点的均值。

进一步地，在所述步骤S32中，对初始匹配后的平面点进行融合匹配，具体包括：

根据所述初始匹配平面对，获得初始匹配平面集合；对于所述初始匹配平面集合中的每条平面对，筛选出最邻近的平面对；将每条平面对记作第一平面对，将所述第一平面对最邻近的平面对记作第二平面对；

获取所述第一平面对和所述第二平面对之间的距离向量r_p；

若所述距离向量r_p的二范数小于第五预设阈值，则将所述第一平面对和所述第二平面对相融合，并获取融合后的平面对邻近点集合；

获取所述平面对邻近点集合对应的平面对协方差矩阵；

对所述平面对协方差矩阵进行特征值分解处理，获得多个特征值，基于其中最小特征值对应的特征向量获得融合匹配平面对/>其中/>等于所述平面对邻近点集合中所有邻近点的均值。

进一步地，所述步骤S4具体包括：

S41、针对融合匹配后的边缘点和平面点，分别计算残差，获得边缘点残差和平面点残差；

S42、获取IMU预积分约束；所述IMU预积分约束包括速度预积分值、位置预积分值和旋转预积分值；

S43、根据所述获得边缘点残差和所述平面点残差，同时结合所述速度预积分值、所述位置预积分值和所述旋转预积分值，建一个最小二乘问题，以获得目标车辆的里程计。

经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种局部几何信息融合的激光惯性里程计方法，包括如下有益效果：

本发明对点云数据进行特征提取，获得边缘点和计算点，基于此可以有效降低数据的存储和算法处理时间，使得里程计算法能实时的运行。

通常来说，点云匹配仅考虑了点云局部的信息，许多匹配直线和平面其实是重复冗余的；本发明中通过融合这些冗余信息，可以有效提高平面或直线的拟合的有效性，减少算法耗时。

本发明通过高频率的IMU信息可以对点云进行畸变矫正，融合IMU和激光雷达的信息可以联合优化位姿，提高算法的鲁棒性。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明实施例提供的局部几何信息融合的激光惯性里程计方法流程示意图。

图2为本发明实施例提供的激光点云畸变矫正示意图。

图3为本发明实施例提供的匹配直线或平面融合的示意图。

图4为本发明实施例提供的基于因子图的激光惯性紧融合框架图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参见图1所示，本发明实施例公开了一种局部几何信息融合的激光惯性里程计方法，包括如下步骤：

S1、获取预设时间段内，目标车辆的点云数据和惯性测量数据；并通过惯性测量数据对点云数据进行畸变矫正；

S2、对畸变矫正后的点云数据进行特征提取，获得边缘点和平面点；

S3、分别对边缘点和平面点进行匹配处理；

S4、基于匹配处理后的边缘点和平面点，同时结合IMU预积分约束，获得所述目标车辆的里程计。

接下来分别对上述各个步骤进行详细说明。

在上述步骤S1中，通过激光雷达获得预设时间段内目标车辆的点云数据，并同步通过IMU测量获得目标车辆的惯性测量数据(即IMU数据)；通过对惯性测量数据进行插值处理，分别获取预设时间段内，第任意个时刻t_任意的点云位姿和最后个时刻t_末的点云位姿；基于这两个时刻下的点云位姿，获得点云数据在t_任意时刻至t_末时刻的相对运动，进而获得点云数据在t_任意时刻至t_末时刻的相对变换；最后，将t_任意时刻的点云数据投影到t_末时刻的坐标系下，得到t_末时刻的点云数据，实现对点云数据进行畸变矫正；

如图2所示，通过IMU测量和激光雷达同步获取数据，其中IMU测量获取的时刻点为t₁、t₃、t₄和t₆，激光雷达获取的时刻点为t₂和t₅；由于IMU具有较高的频率，对于t₂时刻的点云数据，假设在一个IMU采样周期t₁-t₃中载体保持匀速运动，则通过对t₁和t₃时刻的性测量数据进行插值处理，可以获得t₂时刻的点云位姿，具体表示为：

其中，表示t₂时刻下IMU坐标系(表示为I系)到世界坐标系(表示为W系)的位姿矩阵；/>表示t₁时刻的旋转矩阵；/>表示t₁时刻的平移矩阵；/>表示t₁时刻下IMU测量的角速度；/>表示t₁时刻下IMU测量的线速度；0表示3×1阶矩阵；T表示转置操作；

同理，t₅时刻图像的点云位姿可以被获得；利用/>和/>可以得到点云数据在t₂时刻和t₅时刻之间的相对运动，表示为：

利用预先获得的激光雷达和IMU的外参数表示IMU坐标系到激光雷达坐标系的位姿矩阵，得到激光雷达坐标系(表示为L系)下，点云数据在t₂时刻和t₅时刻的相对变换，表示为：

最后，可以将在t₂时刻的激光点云数据投影到t₅时刻激光雷达坐标系下，从而得到t₅时刻的激光点云数据，表示为：

在上述步骤S2中，激光点云通常含有每个点相对于激光原点的三维坐标信息，每个点采集时对应的时间信息，反射强度信息以及每个点所在的线束(环)信息；因此进行激光特征提取时，更多的是考虑激光点云的几何信息；对于单个激光点，竖直方向的角度分辨率过大导致临近点的距离也相对过大，而水平方向的角度分辨率较小，通常都是同一物体上的临近点，因此可以使用水平方向近邻点寻找激光点云中的几何特征；

基于上述内容，本发明实施例考虑使用曲率提取点云数据的边缘点和平面点；具体地，对于每个激光点云数据，考虑其水平方向近邻点，使用曲率公式计算曲率；并通过判断曲率的大小，将点云数据划分为边缘点和平面点；其中，曲率公式表示如下：

其中，C表示当前激光点p_i在水平方向的临近点集合；c_i表示第i个激光点p_i的曲率；p_j表示p_i的第j个邻近点的位置；

通过设置第一预设阈值，可以将点云数据划分为边缘点和平面点，例如若激光点云数据的曲率大于第一预设阈值，则该点云数据属于边缘点，反之，则为平面点。

在上述步骤S3中，具体包括：

S31、分别对边缘点和平面点进行初始匹配；具体包括：

对于第k时刻下激光雷达坐标系下每一个边缘点或平面点使用IMU积分估计的姿态/>作为初值，/>表示第k时刻下激光雷达坐标系L_k到世界坐标系W之间的变换，将当前时刻下的边缘点或平面点变换到世界坐标系下，获得世界坐标系下的边缘点或平面点/>该变换过程可以表示为：

之后使用KDTree查询当前世界坐标系下局部地图Map^W中周围的K个近邻点，获得邻近点集合S，并基于邻近点集合S，计算集合S的协方差矩阵M；具体表示为：

其中，表示在W系中邻近点集合S中第s个邻近点，x_s、y_s、z_s表示/>的三维坐标；表示集合S中所有邻近点的均值；具体地，/>表示所有邻近点在x轴上的均值；表示所有邻近点在y轴上的均值；/>表示所有邻近点在z轴上的均值；

对协方差矩阵M进行特征值分解，获得多个特征值；基于该多个特征值，分别对边缘点和平面点进行初始匹配，该初始匹配具体如下：

(1)对于边缘点将首个特征值和最后一个特征值的比值作为矩阵条件；判断矩阵条件是否大于第二预设阈值；若矩阵条件大于第二预设阈值，则获得边缘点的初始匹配直线对(p^l,v^l)；其中，p^l等于邻近点集合中所有邻近点的均值；v^l表示首个特征值对应的特征向量；

例如，假设对协方差矩阵M进行特征值分解后，获得3个特征值，记作λ₁＞λ₂＞λ₃，那么对于边缘点则判断矩阵条件数/>是否大于第二预设阈值δ_l，若大于第二预设阈值δ_l，则可以获得边缘点/>的初始匹配直线对(p^l,v^l)，其中/>v^l是λ₁对应的特征向量；

(2)对于平面点判断最后一个特征值是否小于第三预设阈值；若最后一个特征值小于第三预设阈值，则获得平面点的初始匹配平面对(p^p,n^p)；其中，p^p等于邻近点集合中所有邻近点的均值；n^p表示最后一个特征值对应的特征向量；

例如，假设对协方差矩阵M进行特征值分解后，获得3个特征值，记作λ₁＞λ₂＞λ₃，那么对于平面点则判断特征值λ₃是否小于第三预设阈值δ_p；若小于第三预设阈值δ_p；则可以获得平面点/>的初始匹配平面对(p^p,n^p)，其中/>n^p是λ₃对应的特征向量。

S32、对初始匹配后的边缘点和平面点，分别进行融合匹配；如图3所示：

(1)对初始匹配后的边缘点进行融合匹配：

根据边缘点的初始匹配直线对(p^l,v^l)，可以获得初始匹配直线集合对于每一条直线对/>(为了方便说明，将每条直线对记作第一直线对)，使用KDTree寻找其最近邻的直线对/>(为了方便说明，将第一直线对对应的最邻近的直线对记作第二直线对)，通过估计第一直线对与第二直线对之间的距离来判断是否融合，即：

其中，r_l表示初始匹配直线集合中，第一直线对与第二直线对之间的距离向量；表示第一直线对对应的特征向量；/>表示第二直线对对应的特征向量；/>表示第一直线对对应的邻近点集合E_m中所有邻近点的均值；/>表示第二直线对对应的邻近点集合E_n中所有邻近点的均值；

若距离向量r_l的二范数小于第四预设阈值，则将第一直线对和第二直线对相融合，并获取融合后的新邻近点集合，记作直线对邻近点集合E_merge，表示为：

E_merge＝E_m+E_n

之后，计算直线对邻近点集合E_merge的直线对协方差矩阵，对该直线对协方差矩阵进行特征值分解处理，获得多个特征值，基于其中最大特征值对应的特征向量获得新的匹配直线对，记作融合匹配直线对/>其中，/>等于所述直线对邻近点集合中所有邻近点的均值。

(2)对初始匹配后的平面点进行融合匹配：

根据平面点的初始匹配平面对(p^p,n^p)，获得初始匹配平面集合对于每一条平面对/>(为了方便说明，将每条平面对记作第一平面对)，使用KDTree寻找其最近邻的平面对/>(为了方便说明，将第一平面对对应的最邻近的平面对记作第二平面对)，通过估计第一平面对与第二平面对之间的距离来判断是否融合，即：

其中，r_p表示初始匹配平面集合中，第一平面对与第二平面对之间的距离向量；表示第一平面对对应的特征向量；/>表示第二平面对对应的特征向量；/>表示第一平面对对应的邻近点集合P_u中所有邻近点的均值；/>表示第二平面对对应的邻近点集合P_v中所有邻近点的均值；T表示转置操作；

若距离向量r_p的二范数小于第五预设阈值，则将第一平面对和第二平面对相融合，并获取融合后的新邻近点集合，记作平面对邻近点集合P_merge，表示为：

P_merge＝P_u+P_v

之后，计算平面对邻近点集合P_merge的平面对协方差矩阵，对该平面对协方差矩阵进行特征值分解处理，获得多个特征值，基于其中最小特征值对应的特征向量获得新的匹配平面对，记作融合匹配平面对/>其中，/>等于平面对邻近点集合中所有邻近点的均值。

在上述步骤S4中，基于匹配处理后的边缘点和平面点，同时结合IMU预积分约束，获得目标车辆的里程计，具体包括：

S41、针对融合匹配后的边缘点和平面点，分别计算残差；具体包括：

在上述内容中，采用来表示第k时刻下激光坐标系下的边缘点或平面点，接下来，为了对边缘点或平面点进行区分，采用/>来表示边缘点；采用/>来表示平面点；

(1)对融合匹配后的边缘点，计算残差：

针对每个边缘点及其对应的融合匹配直线对/>使用点到线的距离定义残差，具体表示为：

其中，表示边缘点/>的残差；×表示叉乘；

(2)对融合匹配后的平面点，计算残差：

针对每个平面点及其对应的融合匹配平面对/>使用点到面的距离定义残差，具体表示为：

其中，表示平面点/>的残差；·表示点乘；

且在上述内容中，未加粗T表示转置操作；加粗表示从局部坐标系转换到地图坐标系之间的状态变量；

S42、获取IMU预积分约束：

(1)对于IMU测量，可以表示为：

其中，和/>分别表示在IMU坐标系下中t时刻下IMU测量的角速度和线加速度值，且/>和都受到偏置/>和白噪声/>的影响；/>表示第t时刻下从世界坐标系W到IMU坐标系I的旋转矩阵；g^W表示世界坐标系下的重力向量；

现在可以使用IMU的测量结果来推断目标车辆的运动；目标车辆在t+Δt时刻的速度，位置和旋转的计算公式表示为：

其中，表示第t+Δt时刻世界坐标系下目标车辆的速度矩阵；/>表示第t时刻世界坐标系下目标车辆的的速度矩阵；/>表示第t+Δt时刻世界坐标系下目标车辆的位置矩阵；/>表示第t时刻世界坐标系下目标车辆的位置矩阵；；/>表示第t+Δt时刻下目标车辆到世界坐标系的旋转矩阵；/>表示第t时刻下目标车辆到世界坐标系的旋转矩阵；然后应用IMU预积分方法获得两个时间步之间目标车辆的相对运动，则t时刻和t+Δt时刻之间的预积分值Δv_Δt，ΔQ_Δt和ΔR_Δt可以被计算为

其中，Δv_Δt表示t时刻和t+Δt时刻之间目标车辆的速度预积分值；ΔQ_Δt表示t t时刻和t+Δt时刻之间目标车辆的位置预积分值；ΔR_Δt表示时刻和t+Δt时刻之间目标车辆的旋转预积分值；

S43、基于上述所获得的边缘点残差和平面点残差，结合IMU预积分约束(即速度预积分值、位置预积分值和旋转预积分值)，可以构建一个最小二乘问题来最小化获得状态变量的最大似然估计，即获得里程计估计值；表示为：

式中，||f_p(T)||²表示边缘化的先验误差；表示IMU的预积分误差；该具体表示为：

如图4所示，通过使用因子图，使用Bayes树的增量平滑来优化代价函数可以获得最优解。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

综上，本发明提供了一种局部几何信息融合的激光惯性里程计方法，通过融合点云地图的局部几何信息，可以有效的提高算法在具有结构化的环境中的稳定性，有效减少算法耗时，提高算法精度；此外，通过合理融合IMU的信息，融合两个传感器的优势，提高了里程计算法的准确性和鲁棒性。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (9)

1.一种局部几何信息融合的激光惯性里程计方法，其特征在于，包括如下步骤：

S1、获取预设时间段内，目标车辆的点云数据和惯性测量数据；并通过所述惯性测量数据对所述点云数据进行畸变矫正；

S2、对畸变矫正后的所述点云数据进行特征提取，获得边缘点和平面点；

S3、分别对所述边缘点和所述平面点进行匹配处理；

S4、基于匹配处理后的边缘点和平面点，同时结合IMU预积分约束，获得所述目标车辆的里程计。

2.根据权利要求1所述的一种局部几何信息融合的激光惯性里程计方法，其特征在于，所述步骤S1具体包括：

S11、获取预设时间段内目标车辆的点云数据，并同步获得所述目标车辆的惯性测量数据；

S12、通过对所述惯性测量数据进行插值处理，分别获取所述预设时间段内，第任意个时刻的点云位姿和最后个时刻的点云位姿；

S13、根据所述第任意个时刻的点云位姿和所述最后个时刻的点云位姿，获得所述点云数据在第任意个时刻至最后个时刻的相对运动；

S14、根据所述点云数据在第任意个时刻至最后个时刻的相对运动，并结合惯性测量的外参数，获得所述点云数据在第任意个时刻至最后个时刻的相对变换；

S15、基于所述点云数据在第任意个时刻至最后个时刻的相对变换，将第任意个时刻的点云数据投影到最后个时刻的坐标系下，得到最后个时刻的点云数据，实现对所述点云数据进行畸变矫正。

3.根据权利要求1所述的一种局部几何信息融合的激光惯性里程计方法，其特征在于，所述步骤S2具体包括：获取每个点云数据的曲率；所述曲率大于第一预设阈值的点云数据属于边缘点；所述曲率小于第一预设阈值的点云数据属于平面点。

4.根据权利要求1所述的一种局部几何信息融合的激光惯性里程计方法，其特征在于，所述步骤S3中具体包括：

S31、分别对所述边缘点和所述平面点进行初始匹配；

S32、对初始匹配后的边缘点和平面点，分别进行融合匹配。

5.根据权利要求4所述的一种局部几何信息融合的激光惯性里程计方法，其特征在于，所述步骤S31具体包括：

S311、使用IMU积分估计的姿态作为初值，将当前时刻下激光雷达坐标系下的边缘点或平面点变换到世界坐标系下，获得世界坐标系下的边缘点或平面点；

S312、查询世界坐标系下当前局部地图中边缘点或平面点的多个邻近点，获得邻近点集合；

S313、获取所述邻近点集合的协方差矩阵；

S314、对所述协方差矩阵进行特征值分解处理，获得多个特征值；

S315、基于所述多个特征值，分别对所述边缘点和平面点进行初始匹配。

6.根据权利要求5所述的一种局部几何信息融合的激光惯性里程计方法，其特征在于，所述步骤S315具体包括：

(1)对于所述边缘点：将首个特征值和最后一个特征值的比值作为矩阵条件；判断所述矩阵条件是否大于第二预设阈值；若所述矩阵条件大于第二预设阈值，则获得所述边缘点的初始匹配直线对(p^l,v^l)；其中，p^l等于邻近点集合中所有邻近点的均值；v^l表示首个特征值对应的特征向量；

(2)对于所述平面点：判断最后一个特征值是否小于第三预设阈值；若最后一个特征值小于第三预设阈值，则获得所述平面点的初始匹配平面对(p^p,n^p)；其中，p^p等于邻近点集合中所有邻近点的均值；n^p表示最后一个特征值对应的特征向量。

7.根据权利要求6所述的一种局部几何信息融合的激光惯性里程计方法，其特征在于，在所述步骤S32中，对初始匹配后的边缘点进行融合匹配，具体包括：

根据所述初始匹配直线对，获得初始匹配直线集合；对于所述初始匹配直线集合中的每条直线对，筛选出最邻近的直线对；将每条直线对记作第一直线对，将所述第一直线对最邻近的直线对记作第二直线对；

获取所述第一直线对和所述第二直线对之间的距离向量rl；

若所述距离向量rl的二范数小于第四预设阈值，则将所述第一直线对和所述第二直线对相融合，并获取融合后的直线对邻近点集合；

获取所述直线对邻近点集合对应的直线对协方差矩阵；

对所述直线对协方差矩阵进行特征值分解处理，获得多个特征值，基于其中最大特征值对应的特征向量获得融合匹配直线对/>其中/>等于所述直线对邻近点集合中所有邻近点的均值。

8.根据权利要求6所述的一种局部几何信息融合的激光惯性里程计方法，其特征在于，在所述步骤S32中，对初始匹配后的平面点进行融合匹配，具体包括：

根据所述初始匹配平面对，获得初始匹配平面集合；对于所述初始匹配平面集合中的每条平面对，筛选出最邻近的平面对；将每条平面对记作第一平面对，将所述第一平面对最邻近的平面对记作第二平面对；

获取所述第一平面对和所述第二平面对之间的距离向量r_p；

若所述距离向量r_p的二范数小于第五预设阈值，则将所述第一平面对和所述第二平面对相融合，并获取融合后的平面对邻近点集合；

获取所述平面对邻近点集合对应的平面对协方差矩阵；

对所述平面对协方差矩阵进行特征值分解处理，获得多个特征值，基于其中最小特征值对应的特征向量获得融合匹配平面对/>其中/>等于所述平面对邻近点集合中所有邻近点的均值。

9.根据权利要求6所述的一种局部几何信息融合的激光惯性里程计方法，其特征在于，所述步骤S4具体包括：

S41、针对融合匹配后的边缘点和平面点，分别计算残差，获得边缘点残差和平面点残差；

S42、获取IMU预积分约束；所述IMU预积分约束包括速度预积分值、位置预积分值和旋转预积分值；

S43、根据所述获得边缘点残差和所述平面点残差，同时结合所述速度预积分值、所述位置预积分值和所述旋转预积分值，建一个最小二乘问题，以获得目标车辆的里程计。