一种基于激光雷达和IMU的定位与建图方法及系统与流程

一种基于激光雷达和imu的定位与建图方法及系统
技术领域
1.本发明涉及机器人感知与导航技术领域，具体涉及一种基于激光雷达和imu的定位与建图方法及系统。


背景技术：

2.在进行定位与建图时，通常会利用激光雷达。激光雷达是一种主动式环境信息探测的传感器，它通过向外发射激光束获得环境点云信息。通常与激光雷达同步使用的还有imu，即惯性测量单元，此时imu是测量物体三轴姿态角(或角速率)以及加速度的装置，用以检测激光雷达的加速度、角速度等数据，一般将这些数据统称为imu数据。假设在某个移动装置上安装激光雷达，然后使得该装置在某未知环境中进行范围移动，通过一段时间的点云数据收集，接下来利用数据处理算法便可以构建出环境地图，同时也能够生成移动装置在环境中从出发位置开始的移动轨迹。该数据处理算法通常称为slam技术，即同时定位与建图。在slam技术中除激光雷达外，还常用到以下的传感器，如相机、轮速计、gnss、惯性导航等。
3.slam算法可以分为前端与后端，前端可以得到带有累计误差的位姿信息，后端利用回环检测或者gnss等对前端得到的位姿进一步优化。在前端，通常是将当前帧点云信息与前一个时间周期内点云进行关联匹配，进而得到当前帧点云与目标点云的相对位姿关系，这个相对位姿再加上已知的目标点云的位姿就得到了当前帧点云的位姿。如果这个关联匹配的过程执行不好，则得到的相对位姿就会有较大的误差甚至在一些比较困难的场景下得到完全错误的结果，也就是匹配失败。因此，关联匹配的过程很大程度上影响前端输出的结果精度，相对的，提高关联匹配正确性对提升前端精度起到关键作用。尤其，在某些场景下常规算法难以有效关联匹配，或者不能利用gnss或者有效回环进行后端优化，这时候前端关联匹配的正确程度就成为了问题的关键。
4.在进行关联匹配时，有两大类方法：1、不基于特征的方法；2、基于特征的方法。
5.不基于特征的方法不去提取特征，而是利用当前帧的所有点云数据进行计算的一类方法。以icp点云匹配技术为例，其是一种经典的点云配准算法。首先使用一个初始的旋转矩阵r以及平移变换t，将源点云旋转到目标点云所在坐标系，然后根据距离在目标点云中寻找源点云中每个点的对应点，接着利用点的对应关系求解一个更好的变换使对应点之间的距离变短，然后不断重复上述过程，直到满足终止条件为止。对于icp算法而言具有较大的计算量，一方面它使用一帧点云的所有数据点，另一方面icp反复执行对应点搜索以及最小二乘优化，它们都是计算量较大的操作，因此icp算法处理速度较慢，无法获得实时性能，且对初始的预测位姿的要求也叫高，如果初始位姿偏差较大则算法无法收敛。另外icp算法对于一些具有挑战性的场景下，比如长的隧道、比较开阔的场景很容易失败，原因在于这种场景下几何特征相似、开阔场景捕获的点少，那么icp算法在寻找前后两帧激光点的对应关系时很容易产生错误。
6.基于特征的方法就是对当前点云提取某类特征，然后利用特征信息进行关联的一
类方法，目前来说对于多线机械式激光雷达主要还是采用此类方法，以loam系列的特征提取算法为例：loam算法前端的配准是用当前帧点云与上一帧点云进行配准，然后获取到当前帧到上一帧的相对变换关系。loam的特征提取以及匹配算法具有很好的性能，但是与icp算法一样在具有挑战性的场景下，其特征提取的能力依然有提升的空间。因在前后帧进行匹配时依赖于特征点的提取，但是对于loam而言对于相同的特征点，在不同位置计算其曲率时它的曲率值发生变化，造成特征提取不稳定，尤其在挑战性场景下更加明显，这种不稳定性导致了算法鲁棒性受到很大影响。


技术实现要素：

7.为了克服上述现有技术中存在的缺陷，本发明的目的是提供一种基于激光雷达和imu的定位与建图方法及系统。
8.为了实现本发明的上述目的，本发明提供了一种基于激光雷达和imu的定位与建图方法，包括以下步骤：
9.获取激光雷达采集的原始点云数据，获取imu采集的imu数据；
10.对所述原始点云数据及imu数据进行数据预处理；
11.根据预处理后的原始点云数据提取当前激光帧的点云数据的特征点；
12.所述特征点的提取步骤为：
13.找出当前激光帧中的当前计算点以及当前点周围的若干点，所述周围的若干点包括当前计算点扫描线前后的若干点和/或与当前计算点相邻的若干条扫描线上位置相同的若干点；
14.将这些点作为一个点集s，计算该点集s的协方差矩阵m；
15.根据所述协方差矩阵m计算该矩阵的特征值；
16.根据特征值计算当前计算点的曲率，如当前计算点的曲率达到设定值，则当前计算点为特征点；
17.构建滑窗，该滑窗对应激光帧的点云数据及imu数据构成一个局部地图；
18.构建优化函数，在所述局部地图中将提取的当前激光帧的特征点用于该优化函数中进行优化，得到当前激光帧激光雷达的位姿输出；
19.根据所有激光帧激光雷达的位姿输出拼接输出3d点云地图。
20.该基于激光雷达和imu的定位与建图方法增强了特征点提取的稳定性，提升了匹配鲁棒性。
21.该基于激光雷达和imu的定位与建图方法的优选方案：在找出当前激光帧中的当前计算点以及当前点周围的若干点时，假设当前计算的点为i点，则在i点所在扫描线前后各选若干个点，将这些点作为点集s1；选与当前计算点i相邻的若干条扫描线上位置相同的若干点，得到点集s2；
22.然后分别计算点集s1内所有点的协方差矩阵m1、点集s2内所有点的协方差矩阵m2；
23.分别计算协方差矩阵m1、协方差矩阵m2对应的特征值，并分别将各矩阵的特征值由大到小排序；
24.根据协方差矩阵m1对应的特征值计算当前计算点的水平曲率，根据协方差矩阵m2
对应的特征值计算当前计算点的垂直曲率，当水平曲率与垂直曲率均达到对应的设定值时，当前计算点为平面特征点，当仅有水平曲率达到对应的设定值时，该计算点为边缘特征点。
25.该优选方案中所采用的水平曲率和垂直曲率计算方法增强了特征提取的稳定性。
26.优选的，将协方差矩阵m的最大特征值与第二大特征值的比值作为曲率。
27.该基于激光雷达和imu的定位与建图方法的优选方案：协方差矩阵m的计算步骤为：
28.令点集s为s＝[p
i-n
,p
i-n+1
,...,pi,...,p
i+n-1
,p
i+n
]，pi为点集s内第i个点，n为正数，2n+1为点集s内点的数量；
[0029]
计算点集s内所有点的中心点，即中心点
[0030]
将点集s中每个点减去该点集s的中心点，得到s'，
[0031][0032]
点集s对应的协方差矩阵为m：
[0033]
该基于激光雷达和imu的定位与建图方法的优选方案：所述优化函数的构建步骤为：
[0034]
在局部地图中找每个边缘特征点对应的线，找每个平面特征点对应的面；
[0035]
计算每个边缘特征点到与其对应的直线的距离，点到直线的距离的计算公式为：其中，为第i个边缘特征点到局部地图中与之对应的直线的距离，为该直线上的两个点的坐标,指第i个边缘特征点的坐标，为当前激光帧的待估计位姿；
[0036]
计算每个平面特征点到与其对应的面的距离，点到平面的距离的计算公式为：计算每个平面特征点到与其对应的面的距离，点到平面的距离的计算公式为：为当前激光帧内第j个平面特征点到局部地图中与之对应的平面的距离表达式，指第i个平面特征点的坐标，指第i个平面特征点的坐标，为局部地图中平面上的三个点的坐标；
[0037]
优化函数的公式为：其中α和β分别为边缘特征点和平面特征点的个数。
[0038]
该基于激光雷达和imu的定位与建图方法的优选方案：所述预处理包括：
[0039]
imu与激光雷达时间同步：根据激光雷达时间戳找到对应时刻的imu数据，以及两帧激光雷达之间的所有imu数据；如果在激光雷达时间戳时间没有对应的imu数据，则利用线性插值计算出对应时刻imu数据，以实现imu与激光雷达时间同步；
[0040]
点云去畸变：将一帧激光雷达所有点统一到该帧激光雷达的第一个点所在坐标
系；
[0041]
imu预积分：利用imu数据计算出两帧激光雷达之间相对运动信息，一方面作为位姿匹配的初始位姿，另一方面作为滑窗优化中的预积分因子参与滑窗优化。
[0042]
该基于激光雷达和imu的定位与建图方法的优选方案：提取特征点时，将360度的角度方向按角度均分为若干个区域，在每个区域里提取相同或相近数量的特征点。
[0043]
该基于激光雷达和imu的定位与建图方法的优选方案：利用所述滑窗对预处理后的imu数据进行滑窗优化；
[0044]
滑窗优化时，如果当前激光帧符合关键激光帧条件，则将当前激光帧加入到滑窗中进行联合优化，得到经滑窗优化后的关键激光帧位姿，同时将关键激光帧构成局部地图用以里程计的匹配。
[0045]
该基于激光雷达和imu的定位与建图方法的优选方案：还包括以下步骤：
[0046]
回环检测：随着激光雷达和imu的工作重复执行上述步骤，当当前激光帧与之前的某个激光帧k的位姿差小于阈值时，则认为检测到回环；
[0047]
全局优化：构建一个全局优化函数，将历史关键激光帧对应时刻的位姿作为优化变量，滑窗优化后的关键激光帧位姿作为全局优化的初值，回环检测到的回环作为约束条件，然后执行最小二乘优化，得到最终优化后的各个时刻激光雷达的位姿，将各个时刻激光雷达的位姿拼接起来构成全局地图。
[0048]
本发明还提出了一种基于激光雷达和imu的定位与建图系统，其特征在于，包括激光雷达、imu、处理模块和存储模块，所述激光雷达获取原始点云数据，imu获取imu数据，所述激光雷达与处理模块连接，将其获取的原始点云数据发送给处理模块，所述imu与处理模块连接，将其获取的imu数据发送给处理模块，所述处理模块与存储模块通信连接，所述存储模块用于存放至少一可执行指令，所述可执行指令使所述处理模块根据所述原始点云数据和imu数据执行如上述的基于激光雷达和imu的定位与建图方法对应的操作。
[0049]
本发明的有益效果是：基于本发明能更准确利用激光雷达和imu进行定位与建图，特别是本发明中的水平曲率和垂直曲率计算，它和观察者所在的位置无关，能够稳定的反应环境几何特性，因此在挑战性环境中往往能够取得更鲁邦的结果，也能够提取到更多的有效特征，使算法在挑战环境中更具有效性，更容易收敛，不至发散。
[0050]
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
[0051]
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
[0052]
图1是基于激光雷达和imu的定位与建图方法的流程示意图。
具体实施方式
[0053]
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
[0054]
在本发明的描述中，除非另有规定和限定，需要说明的是，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是机械连接或电连接，也可以是两个元件内部的连通，可以是直接相连，也可以通过中间媒介间接相连，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
[0055]
如图1所示，本发明提供了一种基于激光雷达和i mu的定位与建图方法的实施例，该实施例中采用多线激光雷达为主要使用的传感器，多线激光雷达可以获取准确的环境信息。该方法具体包括以下步骤：
[0056]
获取激光雷达采集的原始点云数据，获取i mu采集的i mu数据，i mu数据包括加速度及角速度数据。
[0057]
然后对所述原始点云数据及i mu数据进行数据预处理。
[0058]
本实施例中，这里的预处理主要包括三个部分：i mu与激光雷达时间同步处理、点云去畸变处理和i mu预积分处理。
[0059]
i mu与激光雷达时间同步：根据激光雷达时间戳找到对应时刻的i mu数据，以及两帧激光雷达之间的所有i mu数据；如果在激光雷达时间戳时间没有对应的i mu数据，则利用线性插值计算出对应时刻i mu数据，以实现i mu与激光雷达时间同步。
[0060]
点云去畸变：因为激光雷达在得到一帧数据的时间段内的点因为激光雷达本身的运动，所以需要执行去畸变操作，具体操作为：将一帧激光雷达所有点统一到该帧激光雷达的第一个点所在坐标系。
[0061]
i mu预积分：利用i mu数据计算出两帧激光雷达之间相对运动信息，一方面作为位姿匹配的初始位姿，另一方面作为滑窗优化中的预积分因子参与滑窗优化，具体如何参与滑窗优化的，可参考现有公知的技术，如lio-mapping、lio-sam等方案中都有应用。
[0062]
特征点提取：根据预处理后的原始点云数据提取当前激光帧的点云数据的特征点。
[0063]
本实施例中，提取特征点的具体步骤为：
[0064]
找出当前激光帧中的当前计算点以及当前计算点周围的若干点；然后将这些点作为一个点集s，计算该点集s的协方差矩阵m；根据所述协方差矩阵m计算该矩阵的特征值与特征向量，并根据特征值由大到小排序；根据特征值计算当前计算点的曲率，如当前计算点的曲率达到设定值，则当前计算点为特征点。
[0065]
具体地，在找出当前激光帧中的当前计算点以及当前点周围的若干点时，假设当前计算的点为i点，则在i点所在扫描线前后各选若干个点，将这些点作为点集s1；选与当前计算点i相邻的若干条扫描线上位置相同的若干点，得到点集s2，这里的若干条扫描线优选但不限于为当前计算点上下各三条扫描线，若干点优选但不限于为6个点，优选在上述六条扫描线中每条扫描线上各取一个点，若不能取到6个点则在当前点上方或下方至少取3个点。
[0066]
协方差矩阵m的计算步骤为：令点集s为s＝[p
λ-n
,p
λ-n+1
,...,p
λ
,...,p
λ+n-1
,p
λ+n
]，p
λ
为点集s内第λ个点，n为正数，2n+1为点集s内点的数量；首先计算点集s内所有点的中心点，即中心点再将点集s中每个点减去该点集s的中心点，得到s'，
最后，点集s对应的协方差矩阵为m：由于这里是有点集s1和点集s2，因此分别计算点集s1内所有点的协方差矩阵m1、点集s2内所有点的协方差矩阵m2。当是计算点集s1内所有点的协方差矩阵m1时，这里的点集就采用点集s1，当是计算点集s2内所有点的协方差矩阵m2时，这里的点集就采用点集s2。
[0067]
分别计算协方差矩阵m1、协方差矩阵m2对应的特征值与特征向量，并分别将各矩阵的特征值由大到小排序。这里具体计算特征值和特征向量的方法为svd矩阵分解，在此不做详细说明，协方差矩阵m1、协方差矩阵m2将各自得到的自己的三个特征值[γ1、γ2、γ3]和对应的特征向量为[υ1、υ2、υ3]。
[0068]
特征值的大小反应了点集的局部区域内在特征向量的方向上点的分布情况，如果特征值较大则说明在该方向上点分布比较分散，相反如果特征值较小，则在对应方向上点的分布较集中，因此特征值的大小反应了点集局部的几何特征。
[0069]
因此根据协方差矩阵m1对应的特征值计算当前计算点的水平曲率，当前计算点的水平曲率根据协方差矩阵m2对应的特征值计算当前计算点的垂直曲率，当前计算点的垂直曲率
[0070]
当水平曲率与垂直曲率均达到对应的设定值时，当前计算点为平面特征点，当仅有水平曲率达到对应的设定值时，该计算点为边缘特征点。
[0071]
在提取特征点时，为保证选取的特征点在360度的方向上尽量平局分布，将360度的角度方向按角度均分为若干个区域，在每个区域里提取相同或相近数量的特征点。例如将360度均分为6个区域，每60度为一个区域，在一个区域中最多选30个边缘特征点和100个平面特征点。
[0072]
在实际计算中，一帧点云可以认为是顺序排列的，从第一个点到最后一个点，特征点提取时，不需所有点都计算协方差矩阵，比如计算每个点左右各5个点的点集，那么对于最开始的5个点它的左边不够5个点，因此前5个点不计算协方差矩阵，从第6个点开始计算，类似地最后的5个点同样不计算协方差矩阵。
[0073]
滑窗
[0074]
与lio-mapping项目处理方式类似，构建宽度为q激光帧的滑窗，该滑窗对应的q激光帧的激光数据构成一个局部地图,供位姿匹配使用，这里的激光数据指预处理后的点云数据以及滑窗内每帧点云数据对应的imu数据，知道位姿就可以将点云统一在世界坐标系下表示，这里生成局部地图的方法为现有方法，此处不再赘述。滑窗构建了一个非线性优化问题，优化因子有边缘化因子、激光里程计因子、预积分因子，每一次当新的激光帧进入到窗口中，则窗口边缘化掉最老数据帧，然后进行一次优化，得到q个激光数据帧对应的优化后的位姿，因此这里利用该滑窗对预处理后的imu数据进行滑窗优化。
[0075]
本实施例中，在滑窗优化时，如果当前激光帧符合关键激光帧条件，则将当前激光帧加入到滑窗中进行联合优化，滑窗优化后的关键激光帧位姿输出给全局优化模块，同时滑窗激光帧构成局部地图用以里程计的匹配。判断当前激光帧是否符合关键激光帧的条件
是若当前激光帧的位姿比最近的一个关键激光帧的位姿差异超过设定的范围即认为当前激光帧为关键激光帧，第一个关键激光帧直接由人为或系统指定，比如第一个激光帧。
[0076]
位姿估计
[0077]
本实施例中通过构建优化函数进行位姿估计，优化函数的构建步骤为：
[0078]
在局部地图中找每个边缘特征点对应的线，找每个平面特征点对应的面。
[0079]
这里找地图中对应的线和对应的面的方法与loam所述方法相同，其大概过程为将局部地图构建一个kd树，然后找到与当前边缘特征点距离最近的若干个点，或与当前平面特征点最近的若干个点，然后再判断地图中的这些点是否可以拟合成一条线或者一个面，如果可以拟合，则认为拟合出的直线或面就是当前边缘特征点或平面特征点所对应的直线或平面。
[0080]
计算每个边缘特征点到与其对应的直线的距离，点到直线的距离的计算公式为：其中，为第i个边缘特征点到局部地图中与之对应的直线的距离，为该直线上的两个点的坐标,指第i个边缘特征点的坐标，为当前激光帧的待估计位姿。当前激光帧的待估计位姿是一个变量，正是优化函数要求解的值，经过计算便可得到该值，计算初始时imu数据为这个值提供一个优化前的初始值以供计算。
[0081]
计算每个平面特征点到与其对应的面的距离，点到平面的距离的计算公式为：计算每个平面特征点到与其对应的面的距离，点到平面的距离的计算公式为：为当前激光帧内第i个平面特征点到局部地图中与之对应的平面的距离表达式，指第i个平面特征点的坐标，指第i个平面特征点的坐标，为局部地图中平面上的三个点的坐标，
·
表示向量之间的点乘。
[0082]
优化函数的公式为：其中α和β分别为边缘特征点和平面特征点的个数，该优化函数的含义为将所有的边缘特征点到直线和平面特征点的平面的距离残差累加到一起进行优化。
[0083]
在所述局部地图中将提取的当前激光帧的特征点用于该优化函数中进行优化，得到当前激光帧激光雷达的位姿输出，即实现定位。
[0084]
根据所有激光帧激光雷达的位姿输出拼接输出3d点云地图。
[0085]
为减小误差，提高3d点云地图的准确性，在上述实施例的基础上还可增加以下两个步骤，得到更精准的全局地图。
[0086]
回环检测
[0087]
回环检测就是随着激光雷达的运动检测到回到了以前已经到达过的位置。随着激光雷达和i mu的工作重复执行上述步骤，当当前激光帧与之前的某个激光帧k的位姿差小于阈值时，则认为检测到回环。
[0088]
具体的，回环检测是使用一个kd-tree保存滑窗边缘后的位姿节点，这个kd-tree
够成一个位姿图，当新的一帧到来，利用kd-tree寻找与当前帧位姿差异小于阈值的过去的某一帧，如果找到则找到一个回环约束。当检测到回环，将激光帧k至当前激光帧对应的激光雷达位姿构成回环约束输出给全局优化模块。
[0089]
全局优化
[0090]
全局优化并不实时运行，系统在前述几个步骤中，会保存滑窗优化后的关键激光帧位姿以及对应的点云数据，以及检测的回环信息，然后如果有需要，用户可以离线触发全局优化，此步骤的主要考虑是利用回环约束来降低系统的累计误差，因全局优化涉及的数据量较大故不进行实时的运算，而是离线触发，保证系统能够实时运行同时还能得到最后经过全局优化的地图。
[0091]
全局优化约束由两个部分组成，第一部分是相邻关键激光帧相对位姿约束，这些关键激光帧由滑窗模块优化后的历史关键激光帧构成；第二部分即经过回环检测找到的回环约束。
[0092]
全局优化模块接受关键激光帧位姿以及回环约束，进行全局一致性优化，最终拼接输出全局地图。
[0093]
全局优化的步骤为：构建一个全局优化函数优化变量为历史关键激光帧对应时刻的位姿，滑窗优化后输出的关键激光帧位姿作为全局优化的初值，回环检测到的回环作为约束条件，然后执行最小二乘优化，得到最终优化后的各个时刻激光雷达的位姿，将各个时刻激光雷达的位姿拼接起来构成全局地图，其中，r
ρ，σ
为相邻激光帧残差约束，表示相邻两帧相对位姿，h是所有激光帧的集合，ρ和σ是相邻的两个关键激光帧。为回环检测残差，表示构成回环的两个帧之间的相对位姿，l是所有回环帧的集合，和τ是构成回环的两个帧。
[0094]
本技术还提出了一种基于激光雷达和imu的定位与建图系统的实施例，该实施例中的系统包括激光雷达、imu、处理模块和存储模块，所述激光雷达获取原始点云数据，imu获取imu数据，所述激光雷达与处理模块连接，将其获取的原始点云数据发送给处理模块，所述imu与处理模块连接，将其获取的imu数据发送给处理模块，所述处理模块与存储模块通信连接，所述存储模块用于存放至少一可执行指令，所述可执行指令使所述处理模块根据所述原始点云数据和imu数据执行如上述的基于激光雷达和imu的定位与建图方法对应的操作。
[0095]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0096]
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

技术特征：
1.一种基于激光雷达和imu的定位与建图方法，其特征在于，包括以下步骤：获取激光雷达采集的原始点云数据，获取imu采集的imu数据；对所述原始点云数据及imu数据进行数据预处理；根据预处理后的原始点云数据提取当前激光帧的点云数据的特征点；所述特征点的提取步骤为：找出当前激光帧中的当前计算点以及当前点周围的若干点，所述周围的若干点包括当前计算点扫描线前后的若干点和/或与当前计算点相邻的若干条扫描线上位置相同的若干点；将这些点作为一个点集s，计算该点集s的协方差矩阵m；根据所述协方差矩阵m计算该矩阵的特征值；根据特征值计算当前计算点的曲率，如当前计算点的曲率达到设定值，则当前计算点为特征点；构建滑窗，该滑窗对应激光帧的点云数据及imu数据构成一个局部地图；构建优化函数，在所述局部地图中将提取的当前激光帧的特征点用于该优化函数中进行优化，得到当前激光帧激光雷达的位姿输出；根据所有激光帧激光雷达的位姿输出拼接输出3d点云地图。2.根据权利要求1所述的基于激光雷达和imu的定位与建图方法，其特征在于，在找出当前激光帧中的当前计算点以及当前点周围的若干点时，假设当前计算的点为i点，则在i点所在扫描线前后各选若干个点，将这些点作为点集s1；选与当前计算点i相邻的若干条扫描线上位置相同的若干点，得到点集s2；然后分别计算点集s1内所有点的协方差矩阵m1、点集s2内所有点的协方差矩阵m2；分别计算协方差矩阵m1、协方差矩阵m2对应的特征值；根据协方差矩阵m1对应的特征值计算当前计算点的水平曲率，根据协方差矩阵m2对应的特征值计算当前计算点的垂直曲率，当水平曲率与垂直曲率均达到对应的设定值时，当前计算点为平面特征点，当仅有水平曲率达到对应的设定值时，该计算点为边缘特征点。3.根据权利要求1或2所述的基于激光雷达和imu的定位与建图方法，其特征在于，协方差矩阵m的计算步骤为：令点集s为s＝[p
i-n
,p
i-n+1
,...,p
i
,...,p
i+n-1
,p
i+n
]，p
i
为点集s内第i个点，n为正数，2n+1为点集s内点的数量；计算点集s内所有点的中心点，即中心点将点集s中每个点减去该点集s的中心点，得到s'，点集s对应的协方差矩阵为m：4.根据权利要求1所述的基于激光雷达和imu的定位与建图方法，其特征在于，将协方差矩阵m的最大特征值与第二大特征值的比值作为曲率。5.根据权利要求1所述的基于激光雷达和imu的定位与建图方法，其特征在于，所述优
化函数的构建步骤为：在局部地图中找每个边缘特征点对应的线，找每个平面特征点对应的面；计算每个边缘特征点到与其对应的直线的距离，点到直线的距离的计算公式为：其中，为第i个边缘特征点到局部地图中与之对应的直线的距离，为该直线上的两个点的坐标,指第i个边缘特征点的坐标，为当前激光帧的待估计位姿；计算每个平面特征点到与其对应的面的距离，点到平面的距离的计算公式为：计算每个平面特征点到与其对应的面的距离，点到平面的距离的计算公式为：为当前激光帧内第j个平面特征点到局部地图中与之对应的平面的距离表达式，指第i个平面特征点的坐标，指第i个平面特征点的坐标，为局部地图中平面上的三个点的坐标；优化函数的公式为：其中α和β分别为边缘特征点和平面特征点的个数。6.根据权利要求1所述的基于激光雷达和imu的定位与建图方法，其特征在于，所述预处理包括：imu与激光雷达时间同步：根据激光雷达时间戳找到对应时刻的imu数据，以及两帧激光雷达之间的所有imu数据；如果在激光雷达时间戳时间没有对应的imu数据，则利用线性插值计算出对应时刻imu数据，以实现imu与激光雷达时间同步；点云去畸变：将一帧激光雷达所有点统一到该帧激光雷达的第一个点所在坐标系；imu预积分：利用imu数据计算出两帧激光雷达之间相对运动信息，一方面作为位姿匹配的初始位姿，另一方面作为滑窗优化中的预积分因子参与滑窗优化。7.根据权利要求1所述的基于激光雷达和imu的定位与建图方法，其特征在于，提取特征点时，将360度的角度方向按角度均分为若干个区域，在每个区域里提取相同或相近数量的特征点。8.根据权利要求1所述的基于激光雷达和imu的定位与建图方法，其特征在于：利用所述滑窗对预处理后的imu数据进行滑窗优化；滑窗优化时，如果当前激光帧符合关键激光帧条件，则将当前激光帧加入到滑窗中进行联合优化，得到经滑窗优化后的关键激光帧位姿，同时将关键激光帧构成局部地图用以里程计的匹配。9.根据权利要求8所述的基于激光雷达和imu的定位与建图方法，其特征在于，还包括以下步骤：回环检测：随着激光雷达和imu的工作重复执行上述步骤，当当前激光帧与之前的某个激光帧k的位姿差小于阈值时，则认为检测到回环；全局优化：构建一个全局优化函数，将历史关键激光帧对应时刻的位姿作为优化变量，
滑窗优化后的关键激光帧位姿作为全局优化的初值，回环检测到的回环作为约束条件，然后执行最小二乘优化，得到最终优化后的各个时刻激光雷达的位姿，将各个时刻激光雷达的位姿拼接起来构成全局地图。10.一种基于激光雷达和imu的定位与建图系统，其特征在于，包括激光雷达、imu、处理模块和存储模块，所述激光雷达获取原始点云数据，imu获取imu数据，所述激光雷达与处理模块连接，将其获取的原始点云数据发送给处理模块，所述imu与处理模块连接，将其获取的imu数据发送给处理模块，所述处理模块与存储模块通信连接，所述存储模块用于存放至少一可执行指令，所述可执行指令使所述处理模块根据所述原始点云数据和imu数据执行如权利要求1-9任一项所述的基于激光雷达和imu的定位与建图方法对应的操作。

技术总结
本发明提出了一种基于激光雷达和IMU的定位与建图方法及系统。该方法为：获取激光雷达采集的原始点云数据，获取IMU采集的IMU数据；对所述原始点云数据及IMU数据进行数据预处理；根据预处理后的原始点云数据提取当前激光帧的点云数据的特征点；构建滑窗，该滑窗对应激光帧的点云数据及IMU数据构成一个局部地图；构建优化函数，在所述局部地图中将提取的当前激光帧的特征点用于该优化函数中进行优化，得到当前激光帧激光雷达的位姿输出；根据所有激光帧激光雷达的位姿输出拼接输出3D点云地图。该基于激光雷达和IMU的定位与建图方法增强了特征点提取的稳定性，提升了匹配鲁棒性。性。性。


技术研发人员：王刚
受保护的技术使用者：新驱动重庆智能汽车有限公司
技术研发日：2023.05.10
技术公布日：2023/8/14