一种基于概率栅格移除动态对象的静态地图构建方法

1.本发明涉及无人驾驶领域，具体为一种基于概率栅格移除动态对象的静态地图构建方法。


背景技术：

2.随着科技的进步，无人驾驶技术快速发展，并且已经有相对成熟的落地产品，如自主泊车系统、智能辅助驾驶系统等。无人驾驶的关键技术包括高精度地图、定位、感知、规划、决策、控制等。如何在动态环境中构建高精度的地图是无人驾驶领域研究的一个关键问题。
3.无人驾驶车辆基于各种传感器获取周围环境中的三维结构信息，并在环境中进行定位、规划和决策。在完成高精度地图构建的基础上，无人驾驶车辆可以进行更准确的定位，也可以在运动过程中快速的做出规划和决策。通常的建图算法都是基于静态环境假设的，然而实际环境中并不是这样，往往存在着许多移动的物体如行人、车辆等。在高精度地图构建过程中，动态对象的存在会在三维地图中留下不需要的痕迹，使周围环境的空间结构多样化，而且这些痕迹也会被当成障碍物，从而导致三维点云地图的地图精度降低和定位精度降低，并且降低无人驾驶车辆的运动规划和决策的效率。因此，在动态环境中建立高精度的静态点云地图成为一个重要的研究分支。


技术实现要素：

4.针对动态环境中的动态对象导致三维点云地图的地图精度降低和定位精度降低的问题，本发明提供一种基于概率栅格移除动态对象的静态地图构建方法：以紧耦合的方式融合激光雷达数据、imu数据和gps数据，建立因子图模型，优化融合权重；采用激光雷达强度作为描述子，并极坐标的方式构建具有描述三维点云数据特征的栅格图；采用平面拟合的模型，将拟合平面以上的动态区域点云数据剔除，得到三维的静态点云地图。
5.以下给出一个或多个方面的主要概述以应对这些方面的基本理解。此概述不能将所有的构想进行详述，其唯一的目的是要以简化形式给出一个或多个方面的一些概念以为稍后给出的更加详细的描述之序。
6.本发明提供一种基于概率栅格移除动态对象的静态地图构建方法，包含以下步骤：
7.步骤1：采集激光雷达点云数据、imu数据和gps数据，采集数据过程中，对每个时刻的激光雷达点云数据、imu数据和gps数据同时进行紧耦合处理，从而得到关键帧的数据集合，该集合中包含关键帧的点云数据集合、关键帧的时间数据集合和关键帧的位姿数据集合，其中包括以下步骤：
8.步骤1.1：本发明中的激光雷达数据来自velodyne16线激光雷达，imu数据和gps数据都来自xsens-680g组合导航单元；本发明中的激光雷达点云数据是指激光雷达点云的位置信息和强度值，imu数据是指加速度信息和角速度信息，gps数据是指经度、纬度和海拔信
息；将激光雷达点云数据按时间前后顺序，依次拼接成一个三维点云地图，在拼接的过程中，采用关键帧和关键帧对应子地图进行scan to map匹配；
9.步骤1.2：将两个相邻关键帧的imu数据进行imu预积分，随着时间增长，动态更新imu预积分；
10.步骤1.3：采用因子图优化的模型，该模型中选取的因子有激光雷达里程计因子、imu因子、gps因子和回环检测因子，求位姿的最优解，从而得到关键帧的数据集合。
11.步骤2：依次读取关键帧的数据集合中的数据，构建关键帧的栅格图和关键帧对应子地图的栅格图，采用激光雷达强度作为栅格图中每一个格子的描述子，得到带有描述子的关键帧的栅格图和关键帧对应子地图的栅格图，其中包括以下步骤：
12.步骤2.1：依次读取关键帧的数据集合中关键帧的点云数据、时间数据和位姿数据，采用极坐标的方式构建关键帧的栅格图和关键帧对应子地图的栅格图；
13.步骤2.2：计算关键帧的栅格图中每一个格子中激光雷达强度密度最高的值，将该值作为关键帧的栅格图中每一个格子的描述子，同时计算关键帧对应子地图的栅格图中每一个格子中激光雷达强度密度最高的值，将该值作为关键帧对应子地图的栅格图中每一个格子的描述子，从而得到带有描述子的关键帧的栅格图和关键帧对应子地图的栅格图。
14.步骤3：对带有描述子的关键帧的栅格图和关键帧对应子地图的栅格图进行比较，找到子地图中的动态区域，然后对动态区域进行平面拟合，得到平面方程，之后剔除动态区域中平面方程上方的点云，最后得到剔除动态对象后的三维静态点云地图，其中包括以下步骤：
15.步骤3.1：计算关键帧的栅格图和关键帧对应子地图的栅格图中每一个格子的描述子的差值，如果差值小于e，则认为是存在动态对象的区域，该区域以下简称为动态区域；
16.步骤3.2：找到关键帧对应子地图的动态区域中高度最低的点，该点的z轴数值作为z0，选择高度大于z0并且小于(z0 0.5)的所有点作为数据集；
17.步骤3.3：采用ransac算法对数据集进行平面拟合，当到达最大迭代次数或者拟合的平面模型正确率大于阈值时，停止迭代，从而得到拟合平面的平面方程；
18.步骤3.4：将动态区域中的平面方程上方的点云认为是动态对象的点云，然后剔除动态区域中动态对象的点云，最后得到三维静态点云地图。
19.本发明的优点在于采用激光雷达强度作为描述子，构建栅格图，对比每个栅格的描述子来识别动态对象点云，提高了识别动态对象的效率，并具有较高的鲁棒性。本发明可以在动态环境下构建高精度的三维静态点云地图中，提高建图精度、定位精度、运动规划效率和决策效率。
附图说明
20.构成本技术的一部分的说明书附图用来提供对本技术的进一步理解，本技术的示意性实施及其说明用于解释本技术，并不构成对本技术的不当限定。
21.图1是本发明中的总体流程图。
22.图2是本发明中的关键帧示意图。
23.图3是本发明中的关键帧对应子地图示意图。
24.图4是本发明中的原始地图示意图。
25.图5是本发明中的三维静态点云地图示意图。
具体实施方式
26.为了更好地理解本发明的技术方案，以下结合附图和具体例子对本发明的实施方式作进一步的介绍。注意，以下结合附图和具体实施例描述的诸方面仅是示例性的，而不应被理解为对本发明的保护范围进行任何限制。
27.一种基于概率栅格移除动态对象的静态地图构建方法的总体流程图，如图1所示。
28.步骤1：采集来自velodyne16线激光雷达的点云数据、来自xsens-680g组合导航单元的imu的加速度信息、角速度信息和gps的经度、纬度和海拔信息；采集数据过程中，对每个时刻的激光雷达的点云数据、imu数据和gps数据同时进行紧耦合处理，其中包括以下步骤：
29.步骤1.1：计算t时刻激光雷达点云数据的特征点，并得到特征点集合，如图2所示，特征点集合表达式如下(∪为集合的并集)：
30.f
t
＝{p1,p2,
…
,pm}
31.pk＝{xk,yk,zk,rk}，k＝1,2,3,
…
,m
32.其中f
t
表示t时刻的特征点集合，p1,p2,
…
,pm表示t时刻的所有特征点，这个特征点集合就是关键帧；pk表示所有特征点中的一个点，该点包含x轴位置xk、y轴位置y、z轴位置zk和激光雷达强度信息rk；
33.t时刻的关键帧与t时刻之前的n个关键帧，这(n 1)个关键帧构成关键帧对应的子地图m
t
，如图3所示，采用关键帧和关键帧对应子地图进行scan to map匹配，匹配后得到的位姿就是激光雷达里程计因子，其中m
t
的表达式如下：
34.m
t
＝{f
t-n
∪f
t-(n-1)
∪
…
∪f
t
}
35.其中m
t
表示t时刻关键帧对应的子地图，f
t-n
，f
t-(n-1)
，
…
，f
t
表示t时刻之前的n个关键帧与t时刻的关键帧；
36.步骤1.2：将两个相邻关键帧的imu数据进行imu预积分，随着时间增长，动态更新imu预积分，imu预积分表达式如下：
[0037][0038][0039]
其中表示t时刻imu的角速度估计值，w
t
表示t时刻imu的角速度测量值，表示t时刻imu的角速度偏置，表示t时刻imu的角速度噪声，表示t时刻imu的加速度估计值，表示t时刻的转换矩阵，a
t
表示t时刻imu的加速度测量值，g表示重力加速度，表示t时刻imu的加速度噪声，表示t时刻imu的加速度噪声；由imu预积分得到imu因子；
[0040]
步骤1.3：将t时刻的gps位置协方差做为gps因子；搜索t时刻关键帧附近l米之内的所有关键帧，从中找到距离上最接近t时刻关键帧，并且间隔时间较长的一个关键帧作为候选关键帧(间隔时间较长指的是间隔时间超过50秒)，采用icp迭代的方法，求解修正位姿，并将该修正位姿作为回环检测因子；根据中心极限定理，绝大多数传感器的噪声分布是符合高斯分布的，采用因子图优化的方法，在高斯噪声模型下，将激光雷达里程计因子、imu
因子、gps因子和回环检测因子加入到约束中，通过因子图优化的方法融合激光雷达点云数据、imu数据和gps数据得到原始地图，如图4所示，同时也得到一个包含关键帧的点云数据集合、关键帧的时间数据集合和关键帧的位姿数据集合的关键帧数据集合。
[0041]
步骤2：依次读取关键帧的数据集合中的数据，构建关键帧的栅格图和关键帧对应子地图的栅格图，采用激光雷达强度作为栅格图中每一个格子的描述子，得到带有描述子的关键帧的栅格图和关键帧对应子地图的栅格图。
[0042]
步骤2.1：依次读取关键帧的数据集合中关键帧的点云数据、时间数据和位姿数据，对t时刻的关键帧，以t时刻的关键帧的位姿为中心，选择半径60米，高度为-0.5米到4米的范围，采用极坐标的方式将该范围划分为一个一个的格子，所有的格子组合在一起，则得到t时刻关键帧的栅格图记为对t时刻的关键帧对应的子地图，也是以t时刻关键帧的位姿为中心，选择半径60米，高度为-0.5米到4米的范围，采用极坐标的方式将该范围划分为一个一个的格子，所有的格子组合在一起，则得到t时刻的关键帧对应的子地图的栅格图记为
[0043]
步骤2.2：激光强度是激光雷达点云数据中非常关键的信息，尽管激光强度会受目标表面特征和表面反射率的影响，但不同的材料会反射不同强度的激光，如：金属等材料返回高强度值，混凝土等材料返回低强度值，因此激光在识别不同对象中，具有很高的鲁棒性，因此本发明选择激光强度作为栅格图中的描述子，该描述子可以有效的识别不同对象。计算关键帧的栅格图中每一个格子中激光雷达强度密度最高的值，将该值作为关键帧的栅格图中每一个格子的描述子，同时计算关键帧对应子地图的栅格图中每一个格子中激光雷达强度密度最高的值，记作rp，将该值作为关键帧对应子地图的栅格图中每一个格子的描述子，从而得到带有描述子的关键帧的栅格图和关键帧对应子地图的栅格图。
[0044]
步骤3：对带有描述子的关键帧的栅格图和关键帧对应子地图的栅格图进行比较，找到子地图中的动态区域，然后对动态区域进行平面拟合，得到平面方程，之后剔除动态区域中平面方程上方的点云，最后得到剔除动态对象后的三维静态点云地图。
[0045]
步骤3.1：对比和中的同一位置格子的数值，如果中格子的激光强度远小于中格子的激光强度(远小于的含义是：差值小于e，e的数值为0.7)，那么认为中的这个格子是动态对象存在的区域，记作动态区域；所有的格子进行对比之后，就找到了t时刻关键帧对应子地图中所有的动态区域；
[0046]
步骤3.2：找到关键帧对应子地图的动态区域中高度最低的点，该点的z轴数值作为z0，选择高度大于z0并且小于(z0 0.5)的所有点作为数据集；
[0047]
步骤3.3：采用ransac算法对数据集进行平面拟合，具体实现过程：
[0048]
将数据集中的所有点，记作n，表达式如下：
[0049]
n＝{x1,x2,
…
,xn}
[0050]
在n中的随机选取m个点作为拟合数据，估计平面模型；根据估计的平面模型，遍历n中的所有数据点，判断它们到这个平面模型的直线距离是否小于阈值(0.05m)；如果小于0.05m，则将该点加入集合ni，否则继续计算下一个点；
[0051]
遍历完n的所有点后，判断集合ni中元素的数量是否大于阈值t，如果大于t，则用
集合ni中的所有点重新估计平面模型并停止迭代；如果小于阈值t，则在n中重新选择m个点，进行下一次迭代；
[0052]
如果在最大迭代次数max_n之前没有停止迭代，则在达到最大迭代次数后，选择一个数量最大的ni，来估计平面模型，并结束。
[0053]
由上述的ransac算法得到平面方程；
[0054]
步骤3.4：将动态区域中的平面方程上方的点云认为是动态对象的点云，然后剔除动态区域中动态对象的点云，剔除了所有关键帧中的动态对象数据后，得到三维静态点云地图，如图5所示。
[0055]
为了使得本方法的解释更简单化，将上述的图文描述为一系列步骤，但是应该理解并理会，这些方法不受操作的次序所限制，因为按照一个或多个步骤进行实施，一些动作可按不同的顺序发生，但本领域技术人员可以理解其动作发生的原理。
[0056]
尽管已对本发明说明性的具体实施方式逐步进行了描述，以便于本技术领域的技术人员能够进行领会，但是本发明不仅限于具体实施方式的范围，本领域技术人员可以在权利要求的范围内做出各种变形或修改，只要各种变化只要在所附的权利要求限定和确定的本发明精神和范围内。