一种面向重定位的稀疏化三维点云地图生成方法与流程

本发明属于三维点云地图构建及定位技术领域，具体涉及一种基于时间和空间约束的曲率计算算法进行点云稀疏化处理的三维点云地图生成方法。

背景技术：

随着机器人技术的不断发展和应用场景的不断丰富，激光测距传感器凭借其测量精度高、不受光照等环境因素影响的优势得到了广泛的使用。基于激光点云的定位就是基于点云场景的配准来确定机器人当前位姿。目前常用的方法是icp(iterativeclosestpoint)配准和ndt(normaldistributiontransform)配准，其中icp算法基于点集之间的匹配关系来进行配准，而ndt算法基于点云统计模型来进行配准。但在实时应用中，这些方法会受到点云地图中点云数据量的制约。以velodynevlp-16三维激光传感器为例，其每秒采集约30万个激光测距点，这样在连续作业中会产生数据量巨大的稠密点云。如果直接存储稠密点云，则随着时间的不断推移，点云数据的不断累积会给后续计算带来沉重负担。

基于激光点云的重定位，就是要确定机器人在已有地图中的位姿，从而提高机器人的工作效率。重定位时由于机器人不知道其当前所处位置，因此需要在已有地图中进行全局搜索。如果已有地图存储的是稠密点云，那么全局搜索的计算量非常大且容易出现误匹配；如果存储的是栅格地图，则由于栅格模型会将每个栅格中点云的几何中心或重心作为新的点云位置保存，而这并不是传感器获得的真实数据，因此会导致匹配精度的下降；如果存储的是拓扑地图，则由于拓扑边并不包含任何可用于定位的观测信息，因此只能在有限的拓扑节点上实现机器人定位。

通过对机器人在实际应用中所获得点云数据的分析，可知机器人运行时，如果机器人沿着直线运行会出现大量的冗余数据。这是因为激光测距范围大，在直线运动时会针对同一局部场景生成大量重叠数据，从而造成了数据冗余。如果对这部分数据进行合理的稀疏化处理，不仅不会影响到重定位的精度和准确性，而且还会大大降低全局搜索的计算负担，因此需要对稠密的三维点云地图进行合理的稀疏化处理。

文献(zhangj,singhs.loam:lidarodometryandmappinginreal-time[c]//robotics:scienceandsystemsconference.2014.)中提出了一种实时的用于六自由度运动的三维激光扫描仪低漂移里程计和地图构建的方法。该算法基于改进的icp算法得到高精度的机器人实时运行轨迹和三维激光点云。该方法的优点是可实时生成高精度的稠密三维激光点云地图，同时还可给出与点云具有时间同步关系的轨迹。该方法的不足是所生成稠密点云的数据量过大，无法保证在机器人重定位时点云配准的实时性。

八叉树是一种能够分割三维空间的层次数据结构。通过建立树状结构，其将要表述的三维空间分割成八个大小相同的子三维栅格，然后对每个子栅格也进行分割，如此递归直到子三维栅格的尺寸与给定尺寸相同为止。文献(hornunga,wurmkm,bennewitzm,etal.octomap:anefficientprobabilistic3dmappingframeworkbasedonoctrees[j].autonomousrobots,2013,34(3):189-206.)中提出了一种基于八叉树的地图，该方法通过在栅格中存储有点云分布的栅格的中心坐标来达到稀疏化三维点云的目的。该方法的不足是如果栅格分辨率太大会导致配准精度的下降，而分辨率太小则会增加计算负担，导致点云配准的实时性下降。此外八叉树地图一开始就要限定整个空间的范围，这会导致地图的扩展性不足。

为了保证所构建地图的可扩展性，文献(dubér,dugasd,stumme,etal.segmatch:segmentbasedplacerecognitionin3dpointclouds[c]//ieeeinternationalconferenceonroboticsandautomation.ieee,2017:5266-5272.)中提出了一种基于局部三维空间分割的动态栅格地图，其通过动态存储某一栅格内点云重心的坐标来存储三维点云。其优点是栅格表示的空间允许动态更新，因此全局搜索的效率较高。该方法的缺点是由于其最小栅格尺寸很小，因此地图更新的计算量比较大，且配准精度比稠密点云有所降低。

点云算法库pcl(pointcloudlibarary，http://pointclouds.org/)是一个大型跨平台针对二维、三维图像和点云处理的开源项目。该算法库中提供了大量与点云相关的通用算法和高效数据结构。该算法库中的voxelgrid滤波器通过栅格化方法来对点云进行降采样，其原理是用栅格中点云的重心来近似表示这些点，因此可以对三维点云进行稀疏化。该算法的缺点是其直接将输入点云整体进行稀疏化，因此比八叉树中存储栅格中心和动态栅格地图中更新点云重心的速度慢，点云配准精度也低于稠密点云。

综上所述，如何找到一种针对稠密点云的稀疏化方法，使得生成的点云地图既能充分的表征三维环境，又能有效降低三维点云地图的数据存储量，同时还需保证定位的精度，这对机器人基于三维点云地图进行重定位是非常重要的。

技术实现要素：

针对现有技术和方法存在的各种问题，本发明提出了一种基于时间和空间约束的曲率计算算法进行点云稀疏化处理的三维点云地图生成方法，该方法充分考虑了机器人运行时沿直道运行会导致点云地图中存在大量冗余数据的问题，并能够在保证定位精度的前提下，对点云数据进行稀疏化处理。机器人在实时构建三维点云地图的同时，也会同步保存机器人的运行轨迹数据。虽然机器人运行轨迹本身是连续的，但是机器人实际上是以离散点的形式来保存轨迹数据，因此这种表述机器人运行轨迹的离散点被称为轨迹点，而由轨迹点累积成的数据被定义为轨迹点数据。因为轨迹数据和点云数据之间存在着时间同步关系，也就是说轨迹中的每一个位置点都与机器人在该点所采集的三维点云相对应，因此通过对轨迹冗余数据的删除就可以达到对点云进行有针对性的稀疏化的目的。

基于位置约束和角度约束可得到预处理后的轨迹点数据，进而通过计算轨迹点的曲率来剔除机器人沿着直线运行时所生成的冗余轨迹点，这样也就同时删除了与轨迹点相对应的三维点云数据，从而达到了三维点云稀疏化的目的。由于机器人实际运行轨迹通常不会是绝对的直线，因此当机器人沿着轨迹点曲率小于一定阈值的轨迹运行时，就可以认为机器人此时是沿近似直线运动。

如何准确选择所要删除的冗余点是本算法的核心。这种选择需要基于两方面因素，一个是轨迹点的曲率，另一个是距离间隔约束。完成冗余轨迹点数据剔除后，便可根据剔除后保留的轨迹点与三维点云之间的时间同步关系构建出稀疏化三维点云地图。本发明提出的点云地图稀疏化方法克服了传统方法重定位时点云配准实时性差、计算量大、动态性能差的局限性，同时保留了稠密点云配准精度高的优势，可以满足机器人在三维空间内快速重定位的应用需求。

本发明的技术方案：

一种面向重定位的稀疏化三维点云地图生成方法，步骤如下：

(1)机器人三维点云地图及对应轨迹的生成

通过基于三维激光点云特征提取和连续点云配准的实时低漂移机器人slam算法，获得机器人三维点云地图及对应轨迹；

(2)对轨迹点数据进行距离约束和角度约束预处理

2.1)距离约束预处理

设距离阈值为δd，计算相邻两个轨迹点之间的空间距离d，当d大于距离阈值δd时，则进入步骤2.2)，反之不作处理。

2.2)角度约束预处理

设角度阈值为δθ，计算相邻两个轨迹点的偏航角θyaw、横滚角θroll和俯仰角θpitch之间差的绝对值δθyaw、δθroll和δθpitch，并将之与角度阈值δθ进行比较，当δθyaw、δθroll和δθpitch均小于角度阈值δθ时，则删除这两个轨迹点中前一个轨迹点，反之不作处理。

(3)同时基于曲率约束和距离间隔约束进行轨迹点数据的筛选

3.1)基于曲率约束的筛选

a)设曲率阈值为δthreshold，轨迹点数据共包含s个轨迹点，且每个轨迹点由其三维坐标表示，对每个轨迹点按照生成的时间次序进行编号，得到轨迹点序列p1,p2,p3,…,pi,…,ps。

b)轨迹点集合的选取

在步骤a)获得的轨迹点序列中，除序列开始的n个点和序列结束部分的n个点外，剩余的轨迹点均计算曲率；当计算第i(i∈[n+1,s-n])个轨迹点pi的曲率时，则以pi为中心，取轨迹点pi及其前后连续的n个点(共2n+1个轨迹点)构成集合si；

c)计算轨迹点集合si的协方差矩阵

在集合si中，重新对轨迹点进行编号，得到序列p1′,p2′,p3′,…,pn′；轨迹点pi′(i∈[1,n])的坐标为pi′＝[xi1,xi2,…,xim]^t，则集合si表示成矩阵

其中，n为集合si中轨迹点的数目，m表示轨迹点数据的维度，m＝3。

计算矩阵xi每列的均值则矩阵列向量减去均值得到根据[x1,...,xl,...,xm]^t[x1,...,xl,...,xm]得到轨迹点集合si的协方差矩阵：

d)对协方差矩阵进行奇异值分解

m＝3时，通过对协方差矩阵进行奇异值分解，得到三个特征值λ0、λ1和λ2，其中λ0≤λ1≤λ2。三个特征值λ0、λ1和λ2对应的特征向量分别表示数据分布的三个方向，则第i个轨迹点的曲率δi为：

e)重复步骤b)到步骤d)，计算得到所有轨迹点的曲率。

f)将步骤e)得到的每个轨迹点的曲率与曲率阈值δthreshold进行比较，当该轨迹点的曲率大于该曲率则保留该轨迹点，反之则删除。

3.2)基于距离间隔约束的筛选

在考虑曲率约束的同时，根据距离间隔约束，在轨迹点数据中每隔固定间隔保留一个轨迹点，避免机器人沿直线运行所生成的轨迹点被删除过多。

(4)构建稀疏化的三维点云地图

根据步骤(3)筛选后的所需轨迹点数据，按照时间同步关系在场景的稠密三维点云中提取对应的三维点云，构成新的三维点云地图。

为了保证构建地图能够用于重定位，需要在各步骤设置合理的阈值。在步骤(2)由于机器人沿直线运行时三个旋转角变化很小，同时考虑到机器人的运行速度对轨迹点数据数目的影响，距离阈值为δd可为0.1-0.3米，角度阈值为δθ的取值范围可为5°-30°，具体可视机器人运行速度和应用场景作调整。在步骤(3)中，由于机器人沿直线运动时轨迹点的曲率会很小，而在转弯时曲率较大，因此可将曲率阈值δthreshold设置为0.001-0.005。同时，为了保证轨迹点不至于被剔除过多，同时也要保证不会距离间隔约束不会过多干涉曲率约束，固定间隔的取值范围为5-10米，具体可视三维激光传感器的测距范围作调整。

本发明的有益效果：本发明生成了稀疏化的三维点云地图，其避免了基于稠密点云地图定位实时性差，基于栅格地图定位精度不足，以及基于拓扑地图定位连贯性差的问题。本发明在构建稀疏化三维点云地图时充分利用了轨迹点数据与三维点云之间的时间同步关系，从而保证了移动机器人基于稀疏化后的点云仍可实现可靠重定位。该地图在充分表述三维环境的同时，降低了三维点云地图中的点云数据量，保证了后续机器人重定位的实时性和可靠性，并且由于地图中包含的是原始数据，因此重定位时该稀疏点云地图的点云配准的精度和稠密点云地图基本相同。

附图说明

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

图2为稠密点云地图。

图3为稠密点云地图对应的轨迹点。

图4为包含各种结构的三维场景示意图。

图5为轨迹点数据预处理示意图。

图6为根据曲率和距离间隔约束生成的轨迹点数据示意图。

图7为稀疏化的三维点云地图。

具体实施方式

下面将结合具体实施例和附图对本发明的技术方案进行进一步的说明，本发明技术方案的流程图如图1所示。

本发明具体实施过程中使用的是基于16线三维激光传感器来生成稀疏化三维点云地图，32线、64线等三维激光传感器同样适用于该方法。

本发明使用的16线三维激光传感器，其生成三维点云的频率为10hz，每秒能输出300000个三维激光点，测量范围可达100米。

一种面向重定位的稀疏化三维点云地图生成方法，步骤如下：

(1)机器人三维点云地图及对应轨迹的生成

利用前述文献(zhangj,singhs.loam:lidarodometryandmappinginreal-time[c]//robotics:scienceandsystemsconference.2014.)中所提方法通过将slam(simutaneouslocalizationandmapping)问题分解成两个问题，首先，loam算法对每次三维激光传感器扫描的激光点进行特征提取得到特征点集合。然后，基于连续两次扫描得到的特征点集合进行配准，得到一个高频低失真的里程计来估计激光扫描仪的速度。接下来，基于上一步配准后的当前扫描特征点集合与之前所有次扫描的特征点累积数据进行配准，得到高精度低漂移的里程计和旋转平移关系。最终，基于该旋转平移关系生成稠密三维点云地图和与之有良好时间同步关系的轨迹点数据。如图2所示为某一场景的稠密点云，图3所示为图2所对应的轨迹点数据。

选取一个完整场景的原始数据，包含全局坐标系下的轨迹点数据和三维点云数据，两种数据存储时保留每次扫描获得数据的时间。为了更直观的验证本发明的有效性，该场景可以包含任意数目的直道、弯道等结构，如图4。

(2)对轨迹点数据进行距离约束和角度约束预处理

轨迹点数据预处理是为了降低后续轨迹点数据剔除的计算量，其包含两个方面：距离约束和角度约束。

2.1)距离约束预处理

由于轨迹点数据生成频率很高，因此相邻轨迹点之间的距离比较小。设距离阈值δd为0.2米，计算相邻两个轨迹点之间的空间距离d，当d大于距离阈值δd时，则进入步骤2.2)，反之不作处理。

2.2)角度约束预处理

由于轨迹点数据生成频率很高，因此机器人沿直线运动时相邻轨迹点之间的姿态不会有太大的突变。设角度阈值δθ为30°，计算相邻两个轨迹点的偏航角θyaw、横滚角θroll和俯仰角θpitch之间差的绝对值δθyaw、δθroll和δθpitch，并将之与角度阈值δθ进行比较，当δθyaw、δθroll和δθpitch均小于角度阈值δθ时，则删除这两个轨迹点中前一个轨迹点，反之不作处理。处理结果如图5。

(3)同时基于曲率约束和距离间隔约束进行轨迹点数据的筛选

3.1)基于曲率约束的筛选

根据轨迹点坐标可以计算该点处的离散曲率，而该离散曲率可以反映出轨迹点所处轨迹的离散程度，因此可以基于曲率约束对轨迹点数据进行进一步剔除。因为转弯处轨迹点的曲率与直道上轨迹点的曲率相差很大，因此设置合适的曲率阈值就可以将两种轨迹点进行区分。

a)设曲率阈值δthreshold为0.005，轨迹点数据共包含s个轨迹点，且每个轨迹点由其三维坐标表示，对每个轨迹点按照生成的时间次序进行编号，得到轨迹点序列p1,p2,p3,…,pi,…,ps。

b)轨迹点集合的选取

在步骤a)获得的轨迹点序列中，除序列开始的5个点和序列结束部分的5个点外，剩余的轨迹点均计算曲率，由于纳入计算的轨迹点的数目会影响到曲率的大小以及计算的实时性，当计算第i(i∈[n+1,s-n])个轨迹点pi的曲率时，则以pi为中心，取轨迹点pi及其前后连续的5个点(共11个轨迹点)构成集合si；

c)计算轨迹点集合si的协方差矩阵

在集合si中，重新对轨迹点进行编号，得到序列p1′,p2′,p3′,…,pn′；轨迹点pi′(i∈[1,n])的坐标为pi′＝[xi1,xi2,…,xim]^t，则集合si表示成矩阵

其中，n为集合si中轨迹点的数目，m表示轨迹点数据的维度，m＝3。

计算矩阵xi每列的均值则矩阵列向量减去均值得到根据[x1,...,xl,...,xm]^t[x1,...,xl,...,xm]得到轨迹点集合si的协方差矩阵：

d)对协方差矩阵进行奇异值分解

由步骤c)得到的协方差矩阵是实对称阵。m＝3时，通过对协方差矩阵进行奇异值分解，得到三个特征值λ0、λ1和λ2，其中λ0≤λ1≤λ2。三个特征值λ0、λ1和λ2对应的特征向量分别表示数据分布的三个方向，则第i个轨迹点的曲率δi为：

e)重复步骤b)到步骤d)，计算得到所有轨迹点的曲率。

f)将步骤e)得到的每个轨迹点的曲率与曲率阈值δthreshold进行比较，当该轨迹点的曲率大于该曲率则保留该轨迹点，反之则删除。

3.2)基于距离间隔约束的筛选

由于机器人沿直线运行所生成的轨迹点的曲率小于沿曲线运行所生成的轨迹点的曲率，因此为了实现稀疏化，可将曲率较小的轨迹点剔除。然而，当机器人沿直线运行时，仅基于曲率进行剔除会导致所保留的轨迹点数目过少，这也意味着与轨迹点对应的三维点云数据会过于稀疏，这将无法保证机器人进行可靠的重定位。为了克服该问题，在应用曲率约束的时候需要同时设置距离间隔约束，在轨迹点数据中每隔5米保留一个轨迹点，避免机器人沿直线运行所生成的轨迹点被删除过多。如图6。

(4)构建稀疏化的三维点云地图

根据步骤(3)筛选后的所需轨迹点数据，按照时间同步关系在场景的稠密三维点云中提取对应的三维点云，构成新的三维点云地图。至此，面向重定位问题的稀疏化三维点云地图生成结束。最终生成的三维点云地图如图7所示。