[发明专利]一种面向重定位的稀疏化三维点云地图生成方法

摘要

本发明提供一种面向重定位的稀疏化三维点云地图生成方法，属于三维点云地图构建及定位技术领域。该方法首先生成机器人三维点云地图及对应轨迹，然后对轨迹点数据进行距离约束和角度约束预处理；之后同时基于曲率约束和距离间隔约束进行轨迹点数据的筛选；最后构建稀疏化的三维点云地图。本发明的方法同时基于轨迹点的曲率约束和距离间隔约束，能够准确选择所要删除的冗余点，可以满足机器人在三维空间内快速重定位的应用需求。

主权项

1.一种面向重定位的稀疏化三维点云地图生成方法，其特征在于，步骤如下：(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；计算矩阵X_i每列的均值则矩阵列向量减去均值得到根据[X₁,...,X_l,...,X_m]^T[X₁,...,X_l,...,X_m]得到轨迹点集合S_i的协方差矩阵：D)对协方差矩阵进行奇异值分解m＝3时，通过对协方差矩阵进行奇异值分解，得到三个特征值λ₀、λ₁和λ₂，其中λ₀≤λ₁≤λ₂；三个特征值λ₀、λ₁和λ₂对应的特征向量分别表示数据分布的三个方向，则第i个轨迹点的曲率δ_i为：E)重复步骤B)到步骤D)，计算得到所有轨迹点的曲率；F)将步骤E)得到的每个轨迹点的曲率与曲率阈值δthreshold进行比较，当该轨迹点的曲率大于该曲率则保留该轨迹点，反之则删除；3.2)基于距离间隔约束的筛选在考虑曲率约束的同时，根据距离间隔约束，在轨迹点数据中每隔固定间隔保留一个轨迹点，避免机器人沿直线运行所生成的轨迹点被删除过多；(4)构建稀疏化的三维点云地图根据步骤(3)筛选后的所需轨迹点数据，按照时间同步关系在场景的稠密三维点云中提取对应的三维点云，构成新的三维点云地图。