CN114638934A - 一种3D激光slam建图中动态障碍物的后处理方法 - Google Patents

Abstract

本发明公开了一种3D激光slam建图中动态障碍物的后处理方法，包括：利用3D激光slam方法得到先验地图和建图过程中激光雷达的行驶位姿；对所述得到的先验地图中的点云信息进行重定义；根据激光雷达的行驶位姿和重定义后的点云信息对先验地图进行网格划分，得到可能存在动态点的区域；根据可能存在动态点的区域筛选出动态障碍物点云；根据动态障碍物点云，区分动态点和地面点，将动态点移除同时对地面点进行拟合；逐步完成对整个地图的动态障碍物剔除，最终得到不含动态障碍物的静态地图。本发明通过对先验地图中点云信息的重定义和点云信息的编码提取，直接筛选对比编码提取后的点云信息，提升了动态障碍物识别的计算效率，减少了大量冗余计算。

Description

一种3D激光slam建图中动态障碍物的后处理方法

技术领域

本发明属于智能驾驶的控制技术领域，具体涉及一种3D激光slam建图中动态障碍物的后处理方法。

背景技术

随着科技的发展，自动驾驶技术开始成为当下的研究热点，自动驾驶技术的发展，有望解决汽车给社会带来的大气污染、城市拥堵等负面影响，使汽车更好地服务于社会。而实现自动驾驶的前提是具有高精度的地图作为定位、规划和导航的依据。高精度的三维地图可以有效地降低自动驾驶实现的难度。目前，大部分的高精度的地图都是利用3D激光slam技术进行构建的；激光slam(Simulation Location And Mapping)是一种利用激光雷达进行即时定位与地图构建的技术。

但是由于激光扫描的数据中存在动态物体如行人、车辆等，导致构建的地图存在错误的障碍点，这些错误的观测数据会影响地图的精度和使用，进而导致实现自动驾驶的难度提升。于是如何有效地剔除地图中的动态障碍物问题成为了一个重要研究课题。中国发明专利申请号CN201811029593.9，名称为“一种基于激光定位的动态障碍物剔除算法”中提出了通过激光定位实时剔除室外环境下，机器人构建的2D栅格地图中的动态障碍物。中国发明专利申请号CN202110601426.2，名称为“一种考虑轮式机器人位姿估计的激光雷达动态障碍物检测方法”中提出了通过考虑轮式机器人位姿，提取点云数据几何特征建立代价函数，在建图过程中实时匹配相邻两帧间点云特征，检测动态障碍物。在上述方法中都没有考虑如何剔除3D地图中的动态点剔除问题。同时，在实际的建图过程中实时建图往往不能做到及时剔除动态点，特别是对于体量较大的地图实时剔除动态点会产生巨大的计算量。因此，在剔除地图中动态点的研究中，需要考虑3D地图和体量较大的地图中的动态点剔除问题。

发明内容

针对于上述现有技术的不足，本发明的目的在于提供一种3D激光slam建图中动态障碍物的后处理方法，对3D激光slam构建的地图进行后处理，通过对传感器激光雷达扫描数据构建的地图中点云信息进行重定义和地图的网格划分，对比每个网格中定义的点云信息的差异，区分出三维地图中的动态点和地面点，并将动态点进行剔除；解决了现有技术中实时剔除动态点无法处理较大体量地图的问题，提升了地图精度，为实现自动驾驶的后续环节提供可靠的和有效的依据。

为达到上述目的，本发明采用的技术方案如下：

本发明的一种3D激光slam建图中动态障碍物的后处理方法，步骤如下：

1)利用3D激光slam方法得到先验地图和建图过程中激光雷达的行驶位姿；

2)对所述得到的先验地图中的点云信息进行重定义；

3)根据所述步骤1)中得到的激光雷达的行驶位姿和步骤2)中重定义后的点云信息对步骤1)中得到的先验地图进行网格划分，得到可能存在动态点的区域V_t；

4)根据所述步骤3)中网格划分得到的可能存在动态点的区域V_t筛选出动态障碍物点云；

5)根据所述步骤4)中的动态障碍物点云，区分动态点和地面点，将动态点移除同时对地面点进行拟合；

6)重复所述步骤5)逐步完成对整个地图的动态障碍物剔除，最终得到不含动态障碍物的静态地图。

进一步地，所述步骤1)中先验地图为激光雷达扫描的点云数据逐帧累积构建而成的地图；所述激光雷达的行驶位姿为行驶过程中激光雷达的位置和姿态。

进一步地，所述步骤2)中的先验地图中的点云信息为激光雷达扫描得到的周围环境的三维位置信息的集合；

所述步骤2)中的点云信息重定义方法为：设P_t是一组在时间t处包含n个点的点云，P_t ^M是先验地图中时间t处的点云，即全局坐标系下时间t处包含的点云信息，P_t ^Q是时间t处的查询帧上的查询点云，即激光雷达坐标下时间t处包含的点云信息，T_t ^Q是与P_t ^Q关联的SE(3)姿态变换矩阵；因此将先验地图中的点云信息进行重定义如下：

式中，T为总时间步长，[T]等于{1,2，...，T-1，T}，M为在世界框架上先验地图中所有被测量的点云信息；同时，P_t ^M为全局坐标系下时间t处包含的点云信息，意味着P_t ^M可通过T_t ^Q的逆变换得到P_t ^Q，即P_t ^Q和P_t ^M存在联系，P_t ^Q是查询，P_t ^M是映射。

进一步地，所述步骤3)中的先验地图网格划分的方法为：以激光雷达为中心，ρ为半径，角度为θ划分先验地图，将其划分为多个网格，将这些网格作为筛选动态障碍物的可能存在动态点的区域V_t；

V_t＝{P_k|P_k∈P_t,ρ_k-1＜ρ(P_k)＜ρ_k,θ_k-1＜θ(P_k)＜θ_k,ρ_k＜ρ_max}

式中，ρ_max为最大半径，ρk-₁，ρ_k分别为k-1和k处的半径，θ_k-1，θ_k分别为k-1和k处的角度，P_k是k处的一组点云，ρ(P_k)为P_k的半径，θ(P_k)为P_k的角度，P_t是一组在时间t处包含n个点的点云。

进一步地，所述步骤4)中的筛选动态障碍物点云的方法为：在所述步骤2)中定义了P_t ^Q和P_t ^M分别是同一地图下的查询与映射，即激光雷达坐标系下和全局下时间t处包含的点云信息；因此，通过对比P_t ^Q和P_t ^M中的点云信息筛选出动态障碍物点云，若某点云P_k在全局坐标系下的子图P_t ^M中存在，而在激光雷达坐标系下的查询帧点云信息P_t ^Q中不存在，则判断该点云为动态障碍物并进行剔除；对点云信息进行编码，提炼特征信息后对点云P_t ^Q和P_t ^M进行对比，缩短动态点筛选的时间提升筛选的效率；所述对点云信息的编码方法为：

式中，l(P)为可能存在动态点的区域V_t中包含的所有点云至网格划分中心的距离，

和

分别为在全局坐标系下和激光雷达坐标系下，可能存在动态点的区域V_t中包含的所有点云至网格划分中心的距离最小值；对点云信息进行重提取后，进行动态障碍物的筛选，筛选的结果为：

式中，

为

和

的差值，P_t ^D为动态障碍物点云，τ^D为阈值；当

时，可认为点云P_k在全局坐标系下的子图P_t ^M中存在，而在激光雷达坐标系下的查询帧点云信息P_t ^Q中不存在，即

为包含动态障碍物的点云集合。

进一步地，所述步骤5)中的地面点为动态障碍物和地面重合的部分(如行人，车辆与地面接触的位置)；动态点为地面点以上的动态障碍物点云。

进一步地，所述步骤5)中的具体的区分方法为：

式中，P_t ⁰为高度最低的若干点，z(P_k)为P_k的高度值，

为所选局部区域包含点云高度的平均值，τ0为高度阈值；其中，高度阈值可通过多次试验得到最优；通过若干个高度最低点P_t ⁰作为种子点在局部区域拟合平面，设为L，局部区域中属于拟合平面内的点为地面点，不属于拟合平面内的点为动态点；将动态点移除，地面点以平面L进行拟合。

本发明的有益效果：

本发明考虑了3D地图中动态障碍物的剔除问题，相较于2D栅格地图中的动态障碍物剔除方法具有更广的应用场景；

本发明通过对先验地图中点云信息的重定义和点云信息的编码提取，直接筛选对比编码提取后的点云信息，极大地提升了动态障碍物识别的计算效率，减少了大量冗余计算；

本发明在剔除动态点的过程中，考虑到了地面点的影响，提出了区分地面点和动态点的方法，有效地提高了剔除动态点的准确率，并且通过地面点的拟合降低了剔除动态点后对整个地图的破坏。

附图说明

图1为本发明方法的流程图。

具体实施方式

为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。

参照图1所示，本发明的一种3D激光slam建图中动态障碍物的后处理方法，步骤如下：

1)利用3D激光slam方法得到先验地图和建图过程中激光雷达的行驶位姿；

具体地，所述步骤1)中先验地图为激光雷达扫描的点云数据逐帧累积构建而成的地图；所述激光雷达的行驶位姿为行驶过程中激光雷达的位置和姿态。

2)对所述得到的先验地图中的点云信息进行重定义；

具体地，所述步骤2)中的先验地图中的点云信息为激光雷达扫描得到的周围环境的三维位置信息的集合；

所述步骤2)中的点云信息重定义方法为：设P_t是一组在时间t处包含n个点的点云，P_t ^M是先验地图中时间t处的点云，即全局坐标系下时间t处包含的点云信息，P_t ^Q是时间t处的查询帧上的查询点云，即激光雷达坐标下时间t处包含的点云信息，T_t ^Q是与P_t ^Q关联的SE(3)姿态变换矩阵；因此将先验地图中的点云信息进行重定义如下：

式中，T为总时间步长，[T]等于{1,2，...，T-1，T}，M为在世界框架上先验地图中所有被测量的点云信息；同时，P_t ^M为全局坐标系下时间t处包含的点云信息，意味着P_t ^M可通过T_t ^Q的逆变换得到P_t ^Q，即P_t ^Q和P_t ^M存在联系，P_t ^Q是查询，P_t ^M是映射。

3)根据所述步骤1)中得到的激光雷达的行驶位姿和步骤2)中重定义后的点云信息对步骤1)中得到的先验地图进行网格划分，得到可能存在动态点的区域V_t；

具体地，所述步骤3)中的先验地图网格划分的方法为：以激光雷达为中心，ρ为半径，角度为θ划分先验地图，将其划分为多个网格，将这些网格作为筛选动态障碍物的可能存在动态点的区域V_t；

V_t＝{P_k|P_k∈P_t,ρ_k-1＜ρ(P_k)＜ρ_k,θ_k-1＜θ(P_k)＜θ_k,ρ_k＜ρ_max}

式中，ρ_max为最大半径，ρk-₁，ρ_k分别为k-1和k处的半径，θ_k-1，θ_k分别为k-1和k处的角度，P_k是k处的一组点云，ρ(P_k)为P_k的半径，θ(P_k)为P_k的角度，P_t是一组在时间t处包含n个点的点云。

4)根据所述步骤3)中网格划分得到的可能存在动态点的区域V_t筛选出动态障碍物点云；

具体地，所述步骤4)中的筛选动态障碍物点云的方法为：在所述步骤2)中定义了P_t ^Q和P_t ^M分别是同一地图下的查询与映射，即激光雷达坐标系下和全局下时间t处包含的点云信息；因此，通过对比P_t ^Q和P_t ^M中的点云信息筛选出动态障碍物点云，若某点云P_k在全局坐标系下的子图P_t ^M中存在，而在激光雷达坐标系下的查询帧点云信息P_t ^Q中不存在，则判断该点云为动态障碍物并进行剔除；(然而点云中包含着位置和姿态多种信息，而一幅地图中往往包含大量点云，若直接进行点云信息的对比会导致动态点识别效率缓慢的问题，无法用于体量大的地图动态点处理问题)对点云信息进行编码，提炼特征信息后对点云P_t ^Q和P_t ^M进行对比，缩短动态点筛选的时间提升筛选的效率；所述对点云信息的编码方法为：

式中，l(P)为可能存在动态点的区域V_t中包含的所有点云至网格划分中心的距离，

和

分别为在全局坐标系下和激光雷达坐标系下，可能存在动态点的区域V_t中包含的所有点云至网格划分中心的距离最小值；对点云信息进行重提取后，进行动态障碍物的筛选，筛选的结果为：

式中，

为

和

的差值，P_t ^D为动态障碍物点云，τ^D为阈值；当

时，可认为点云P_k在全局坐标系下的子图P_t ^M中存在，而在激光雷达坐标系下的查询帧点云信息P_t ^Q中不存在，即P_k ^D为包含动态障碍物的点云集合。

5)根据所述步骤4)中的动态障碍物点云，区分动态点和地面点，将动态点移除同时对地面点进行拟合；

具体地，所述步骤5)中的地面点为动态障碍物和地面重合的部分(如行人，车辆与地面接触的位置)；动态点为地面点以上的动态障碍物点云。

所述步骤5)中的具体的区分方法为：

式中，P_t ⁰为高度最低的若干点，z(P_k)为P_k的高度值，

为所选局部区域包含点云高度的平均值，τ0为高度阈值；其中，高度阈值可通过多次试验得到最优；通过若干个高度最低点P_t ⁰作为种子点在局部区域拟合平面，设为L，局部区域中属于拟合平面内的点为地面点，不属于拟合平面内的点为动态点；将动态点移除，地面点以平面L进行拟合。

6)重复所述步骤5)逐步完成对整个地图的动态障碍物剔除，最终得到不含动态障碍物的静态地图。

本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。

Claims (7)

1.一种3D激光slam建图中动态障碍物的后处理方法，其特征在于，步骤如下：

1)利用3D激光slam方法得到先验地图和建图过程中激光雷达的行驶位姿；

2)对所述得到的先验地图中的点云信息进行重定义；

3)根据所述步骤1)中得到的激光雷达的行驶位姿和步骤2)中重定义后的点云信息对步骤1)中得到的先验地图进行网格划分，得到可能存在动态点的区域；

4)根据所述步骤3)中网格划分得到的可能存在动态点的区域筛选出动态障碍物点云；

5)根据所述步骤4)中的动态障碍物点云，区分动态点和地面点，将动态点移除同时对地面点进行拟合；

6)重复所述步骤5)逐步完成对整个地图的动态障碍物剔除，最终得到不含动态障碍物的静态地图。

2.根据权利要求1所述的3D激光slam建图中动态障碍物的后处理方法，其特征在于，所述步骤1)中先验地图为激光雷达扫描的点云数据逐帧累积构建而成的地图；所述激光雷达的行驶位姿为行驶过程中激光雷达的位置和姿态。

3.根据权利要求1所述的3D激光slam建图中动态障碍物的后处理方法，其特征在于，所述步骤2)中的先验地图中的点云信息为激光雷达扫描得到的周围环境的三维位置信息的集合；

所述步骤2)中的点云信息重定义方法为：设P_t是一组在时间t处包含n个点的点云，P_t ^M是先验地图中时间t处的点云，即全局坐标系下时间t处包含的点云信息，P_t ^Q是时间t处的查询帧上的查询点云，即激光雷达坐标下时间t处包含的点云信息，T_t ^Q是与P_t ^Q关联的SE(3)姿态变换矩阵；因此将先验地图中的点云信息进行重定义如下：

式中，T为总时间步长，[T]等于{1,2，...，T-1，T}，M为在世界框架上先验地图中所有被测量的点云信息；同时，P_t ^M为全局坐标系下时间t处包含的点云信息，意味着P_t ^M可通过T_t ^Q的逆变换得到P_t ^Q，即P_t ^Q和P_t ^M存在联系，P_t ^Q是查询，P_t ^M是映射。

4.根据权利要求3所述的3D激光slam建图中动态障碍物的后处理方法，其特征在于，所述步骤3)中的先验地图网格划分的方法为：以激光雷达为中心，ρ为半径，角度为θ划分先验地图，将其划分为多个网格，将这些网格作为筛选动态障碍物的可能存在动态点的区域V_t；

V_t＝{P_k|P_k∈P_t,ρ_k-1＜ρ(P_k)＜ρ_k,θ_k-1＜θ(P_k)＜θ_k,ρ_k＜ρ_max}

式中，ρ_max为最大半径，ρk-1，ρ_k分别为k-1和k处的半径，θ_k-1，θ_k分别为k-1和k处的角度，P_k是k处的一组点云，ρ(P_k)为P_k的半径，θ(P_k)为P_k的角度，P_t是一组在时间t处包含n个点的点云。

5.根据权利要求4所述的3D激光slam建图中动态障碍物的后处理方法，其特征在于，所述步骤4)中的筛选动态障碍物点云的方法为：在所述步骤2)中定义了P_t ^Q和P_t ^M分别是同一地图下的查询与映射，即激光雷达坐标系下和全局下时间t处包含的点云信息；通过对比P_t ^Q和P_t ^M中的点云信息筛选出动态障碍物点云，若点云P_k在全局坐标系下的子图P_t ^M中存在，而在激光雷达坐标系下的查询帧点云信息P_t ^Q中不存在，则判断该点云为动态障碍物并进行剔除；对点云信息进行编码，提炼特征信息后对点云P_t ^Q和P_t ^M进行对比，缩短动态点筛选的时间提升筛选的效率；所述对点云信息的编码方法为：

式中，l(P)为可能存在动态点的区域V_t中包含的所有点云至网格划分中心的距离，

和

分别为在全局坐标系下和激光雷达坐标系下，可能存在动态点的区域V_t中包含的所有点云至网格划分中心的距离最小值；对点云信息进行重提取后，进行动态障碍物的筛选，筛选的结果为：

式中，

为

和

的差值，P_t ^D为动态障碍物点云，τ^D为阈值；当

时，认为点云P_k在全局坐标系下的子图P_t ^M中存在，而在激光雷达坐标系下的查询帧点云信息P_t ^Q中不存在，即

为包含动态障碍物的点云集合。

6.根据权利要求1所述的3D激光slam建图中动态障碍物的后处理方法，其特征在于，所述步骤5)中的地面点为动态障碍物和地面重合的部分；动态点为地面点以上的动态障碍物点云。

7.根据权利要求6所述的3D激光slam建图中动态障碍物的后处理方法，其特征在于，所述步骤5)中的具体的区分方法为：

式中，P_t ⁰为高度最低的若干点，z(P_k)为P_k的高度值，

为所选局部区域包含点云高度的平均值，τ0为高度阈值；其中，高度阈值可通过多次试验得到最优；通过若干个高度最低点P_t ⁰作为种子点在局部区域拟合平面，设为L，局部区域中属于拟合平面内的点为地面点，不属于拟合平面内的点为动态点；将动态点移除，地面点以平面L进行拟合。

Images