CN112365592B - 一种基于双向高程模型的局部环境特征描述方法 - Google Patents

Abstract

一种基于双向高程模型的局部环境特征描述方法，属于无人系统及自主机器人的激光雷达环境感知技术领域。该方法首先基于点云在XY平面上的投影进行主分布方向对齐。通过构建曲面体素栅格，解决三维激光点云数据密度不均匀的问题，同时滤除噪声以及无效点云，构建有序的点云索引。计算每个栅格对应的正向和逆向高程图，根据分隔平面处的连通性将逆向高程图与正向高程值进行融合更新，得到最终的双通道高程描述子，最后进行冗余处理，得到场景的鲁棒描述符。本发明可以用在移动机器人的场景识别、大范围环境中定位与建图和闭环检测等领域。

Description

一种基于双向高程模型的局部环境特征描述方法

技术领域

本发明属于无人系统及自主机器人的激光雷达环境感知技术领域，涉及到激光雷达数据的高效场景描述方法。

背景技术

随着无人系统的快速发展，同时定位与地图构建技术的焦点已转移到长期自主性能，而场景识别是设备在静态和受控环境之外的真实场景中运行的关键，其中，场景特征描述方法对闭环检测速度、场景匹配性能都有重要的影响。在实际场景中，光照和动态环境变化导致基于视觉的描述方式存在大量感知混淆，而激光雷达因其对光照和感知方位的不变性而得到了重视。最直接的描述方式就是选定局部关键点并结合邻域计算局部描述符，使用词袋方法将局部描述符矢量化并对应到离线构造的字典单词，单词的直方图作为描述符。但是在处理三维点云时，具有高重复性的独特关键点的检测是一个低效的过程，且结果对噪声敏感，而基于视点特征的全局描述符能够更好地描述三维点云的整体结构特征，而且建模效率高，在实际场景中的应用更为广泛。

文献(Salti S,Tombari F,Stefano L D.SHOT:unique signatures ofhistograms for surface and texture description[J].Computer Vision and ImageUnderstanding.2014,125(8):251-265.)提出了一种用于外观匹配的局部三维描述符。该方法结合了特征签名和直方图这两类主流方案，前者对关键点建立局部参考系和局部三维描述符，后者以特征和直方图之间的混合结构为特征，在描述能力和鲁棒性之间取得了较好的平衡。但是，该方法仅利用了点云的部分几何特征，很难编码局部空间分布信息，存在对数据分辨率变化、非均匀采样以及噪声敏感等缺陷，从而影响场景匹配的准确率。

文献(L.He,X.Wang,and H.Zhang.M2DP:a novel 3d point cloud descriptorand its application in loop closuredetection[C].Proceedings of the 2016IEEE/RSJ International Conference on Intelligent Robots and Systems.2016,231-237.)提出一种三维点云的全局描述子，并将其应用于闭环检测问题。该方法将三维点云投影到多个二维平面，并为每个平面的点生成特征标签，然后将这些标签的左右奇异矢量作为三维点云的描述符。该方法通过特征向量对齐实现了描述子的旋转不变性，而求解奇异矢量实现了特征降维，从而得以实现快速匹配。但是，该论文所采用的三维点云投影到二维平面的方法会导致点云一个维度空间信息的缺失，从而造成数据分布的区分度降低，在场景识别过程中无法保证良好的召回率。

文献(X.Meng,Z.Cao,S.Liang,etal.A terrain descriptionmethod fortraversability analysis based on elevation gridmap[J].International Journalof Advanced Robotic Systems.2018,15(1):1-12)提出了一种基于高程栅格图的地形描述方法，用于可通行性分析。该方法利用高差核分割地面，再通过高程值聚类得到地形特征，包括高度指数、粗糙度和坡度角，从而可以对边缘进行划分。但是该方法直接采用高程值作为结构特征会导致高处悬浮物体的高程值屏蔽低处物体特征，丢失了部分垂直结构信息。此外，将高程值聚类所得到的精细地形描述应用于场景匹配时，计算效率会显著降低，从而导致场景匹配性能下降。

发明内容

本发明针对利用三维激光点云描述环境特征所存在的问题，提出了一种基于双向高程描述的场景建模方法。为了实现描述子的旋转不变性，同时保证匹配过程的计算效率，该方法首先基于点云在XY平面上的投影进行主分布方向对齐。通过构建曲面体素栅格，解决三维激光点云数据密度不均匀的问题，同时滤除噪声以及无效点云，构建有序的点云索引。计算每个栅格对应的正向和逆向高程图，根据分隔平面处的连通性将逆向高程图与正向高程值进行融合更新，得到最终的双通道高程描述子，最后进行冗余处理，得到场景的鲁棒描述符。

本发明的技术方案：

一种基于双向高程模型的局部环境特征描述方法，步骤如下：

步骤一、激光点云数据预处理

为了保证描述符在3D空间中的旋转不变性。对输入激光点云进行主成分分析，对点云进行旋转，将主特征向量在XY平面上的投影与X轴对齐。详细步骤如下：

(1)每帧激光扫描P包含p₁，p₂，…，p_i，…，p_n共n个激光点，其中p_i＝[x_i,y_i,z_i]，则通过公式(1)计算该点云集合的质心，其坐标为

(2)对点云集合P进行主成分分析，首先计算该集合的协方差矩阵

运算过程如公式(2)所示，

然后根据det(∑-λI)＝0求得协方差矩阵的特征值λ₀、λ₁、λ₂，由∑ξ＝λξ求得特征值对应的特征向量ξ₀、ξ₁、ξ₂，代表了点云集合P的三个主要分布方向。取λ₀为最大特征值，即主特征值，则对应的特征向量ξ₀为主特征向量，表示点云在空间上离散程度最大的方向。

(3)进行点云对齐时并不关心Z轴方向的分布，所以将主特征向量ξ₀投影到XY平面，得到投影后的主特征向量ξ’₀＝[ξ_0-x,ξ_0-y,0]，ξ_0-x和ξ_0-y分别是ξ₀在X和Y方向的分量。计算ξ’₀与X轴正方向的夹角θ＝arctan(ξ’₀-_y/ξ’₀-_x),ξ’₀-_x和ξ’₀-_y分别为ξ’₀在X和Y方向的分量。通过公式(3)对点云集合P进行旋转，将ξ’₀与X轴对齐，得到空间方向归一化的点云集合P’＝{p’₁,p’₂,...,p’_i,...,p’_n}，

其中，θ需要进行规范化，限定在[0,2π)范围内。

(4)在实际场景中，视角变化、动态物体以及噪声这些不确定因素会导致点云数据分布上的差异，但通过对齐后，这些不确定因素只会对整体点云分布产生微小的旋转扰动。此处设定旋转扰动β，根据公式(3)，令θ＝θ+β，分别计算经过旋转变换后的点云P’_-β、P’_β，作为当前场景的冗余扩展。

步骤二、构建曲面体素索引

由于三维扫描式激光雷达获取的数据具有如下属性：距离激光传感器越远点云越稀疏；三维激光雷达的垂直分辨率远小于水平分辨率。此外，受三维激光雷达本身原理、精度以及噪声影响，所测得的点云可能偏离实际位置或完全不同，其中包含随机噪声、孤立噪声点、光滑斜面或多路反射导致的不准确测量等。我们通过构建曲面体素索引解决上述问题，详细过程如下。

(1)以激光传感器为中心，对当前激光点云场景进行划分。在XY的平行平面上，以激光传感器为中心构建h个嵌套的同心环，环的宽度为Δr＝r_max/h，r_max为环的最大半径，在r_max半径范围外的点云过于稀疏，对场景描述没有贡献，然后将r_max对应的最大圆形区域等分为w个相邻的扇形，划分角度为Δθ＝2π/w。扇形与环相交的单元称为曲面栅格。在Z轴方向上对点进行等间距划分，Δz为划分间隔，得到

个高度区间，其中z_max和z_min分别为激光点在Z轴方向上的最大值和最小值。至此，每个曲面栅格上又划分了l个高度区间，w×h×l维度的曲面体素VG构建完成。

(2)利用点云的空间坐标xyz计算得到其在曲面体素结构中的索引uvc。u为扇区索引，以X负方向为起始，沿顺时针方向递增；v为同心环索引，由中心到外缘递增；c为通道索引，沿Z轴正方向递增。坐标映射公式(5)如下：

其中，r为点到激光雷达传感器中心的距离，对于点p_i＝{x_i,y_i,z_i}，其计算式为r_i＝||p_i||₂，只保留r_i≤r_max的激光点云。至此，三维点云数据被分配到了对应的曲面体素中。

(3)曲面体素的去噪

可以通过邻域点的数量判断指定数据点是否为噪声。为了避免有效占用体素被错误地判定为噪声，设某个曲面体素的索引值为u_qv_qc_q，其中，u_q为扇区索引，v_q为同心环索引，c_q为通道索引，该体素中包含n_q个激光点。在计算该体素的点云密度时，取其周围(2δ+1)³个曲面体素作为邻域，该邻域的索引集合为S＝{i,j,k∈[-δ,δ]}，其中，δ为体素邻域区间半径。用当前体素与邻域体素中点云数量的加权和

来表示点的空间密度，n(u_q+i,v_q+j,c_q+k)表示索引u_q+i,v_q+j,c_q+k对应的体素所包含的激光点的数量。最后，通过密度阈值ρ_th区分有效点与噪声点。由于在构建曲面体素索引的过程中在垂直方向上进行了均匀分割，所以根据公式(6)，结合激光点距r_i对空间密度阈值

进行自适应调整，

其中，ρ_max为密度最大值，α为密度自适应调整系数。利用该阈值对曲面体素进行筛选：

的曲面体素为噪声，应予以剔除；

的曲面体素为有效数据点，应当保留。

步骤三、双向高程模型构建

曲面体素映射可以实现针对激光点云数据固有特性的密度自适应建模，但要实现高效匹配就需要对三维数据结构进行压缩。高程图是一种高效的2.5维场景描述方式，但当场景中存在大面积的悬空物体时，局部高程描述相似度较高，悬空物体的高程值屏蔽了近地物体的高程值，使得描述符的辨识度较低。所以我们需要快速提取悬空物体，计算近地物体的高程值，同时保留垂直物体作为有效特征。

(1)将整个曲面体素栅格分割为上下两部分，生成正向和逆向高程图。

三维激光雷达所采集的数据以雷达中心为原点，则由步骤二可知，激光点云数据的坐标原点对应曲面体素坐标系下的点(0,0,c₀)，其中c₀＝-z_min/Δz。取c＝c₀作为分割平面，将曲面体素栅格分为下部VG_d和上部VG_u。对于体素栅格的下部VG_d，将c＝0平面作为基准平面，根据公式(7)沿Z轴正方计算每个栅格的高程值G_d(i,j)，称G_d(i,j)的集合为正向高程图G_d。而对于体素栅格的上部VG_u，将c＝c_max平面作为基准平面，其中

是通道索引的最大值，根据公式(8)沿Z轴负方向计算每个栅格的高程值G_u(i,j)，称G_u(i,j)的集合为逆向高程图G_u。

其中，VG(i,j,k)为体素索引ijk对应的Z轴坐标；G_d(i,j)为曲面栅格索引ij对应的正向高程值；G_u(i,j)为曲面栅格索引ij对应的逆向高程值。

(2)利用分割平面的邻域特性判断上下区域的连通性，如下所示分为两步：

第一步，根据G_d(i,j)+G_u(i,j)判断上下区域是否接触。如果G_d(i,j)+G_u(i,j)≤c_max-ε，则上下区域无接触，故上部属于悬空物体，不需要进一步处理。如果G_d(i,j)+G_u(i,j)＞c_max-ε，则上下区域可能有接触，但由于存在动态、部分遮挡等情况，需要进一步验证。其中，ε为分隔平面的邻域半径。

第二步，如果上下区域可能接触，进行二次验证。取c₀的邻域通道C_S＝{c₀-ε,...,c₀,...,c₀+ε}，计算VG的各个栅格上邻域C_S中曲面体素的占用比例β＝n_occupied/n_total，其中n_occupied为占用体素数量，n_total为体素总数量2ε+1。当β≥β_th时，VG_d和VG_u是连通的，即当前栅格上存在连续的垂直物体，此时根据公式

来更新G_d，获取更加显著的环境特征；当β＜β_th时，VG_d和VG_u是非连通的，此时上部VG_u中存在悬空物体，应该保留G_d(i,j)的高程值来进行场景描述。其中，β_th为体素占用比例阈值。

(3)在分别得到上下两部分的有效高程描述后，由于两次所采用的基准平面不同，导致在不同位置构建的场景描述不具备通用性，所以取c＝c₀平面作为基准平面，对正向和逆向的高程图进行融合，近地部分的高程图EG_d＝G_d-c₀Δz，悬空部分的高程图EG_u＝c_maxΔz-G_u，二者结合构成了双通道的高程描述EG＝{EG_d,EG_u}。最终，沿V轴正方向将双通道的高程描述EG展开，

根据步骤二和步骤三，对步骤一中的冗余扩展P’_-β和P’_β进行相同的运算，得到场景的冗余高程描述EG_-β和EG_β。

本发明的有益效果：该方法实现了对三维激光数据实现了快速索引，针对其数据特性，采用了特化的体素结构，解决了点云密度不均匀的问题。基于该体素结构，提出双向高程模型构建方法，考虑上下两部分点云的连通性，对地面高程图进行更新，可以充分考虑到存在大量悬浮物体的情况，在保证计算效率的同时提高了描述子的辨识度。本发明可以用在移动机器人的场景识别、大范围环境中定位与建图和闭环检测等领域。

附图说明

图1方法流程图。

图2曲面体素栅格俯视图。

图3曲面体素栅格正视图。

图4单扇区对应的双向高程图。

图5连通性判断示意图。

图6场景双向高程图上部。

图7场景双向高程图下部。

图8悬浮物体高程描述。

图9去除悬空物体后的场景高程描述。

具体实施方式

以下结合技术方案和附图详细叙述本发明的具体实施方式。

本发明具体实施过程选择新松移动机器人作为实验平台，使用Velodyne公司的16线激光雷达VLP-16作为主要感知传感器，采集环境点云数据，其水平扫描角度为360度，分辨率为0.2度，垂直扫描角度为30度，分辨率为2度，每次扫描约采集3万个点云数据，扫描频率为10Hz。然后使用GIGABYTEBKi5HA对点云进行建模。本发明所提的方法同样适于32、64线或者多激光雷达传感器构成的组合系统，这些设备可以采集更加密集的激光数据点，场景描述更加精确。通过近似组合系统的中心位置，就可以对数据进行密度自适应的调整。整体方法流程如图1所示，实施方法如下：

第一步、使用移动机器人平台在校园环境中采集激光数据，数据涵盖了室内大厅、低矮通道、大范围室外道路、非结构化环境等多类场景。

第二步、对每帧激光数据进行预处理，计算点云的主特征向量在XY平面上的投影，进而计算该向量投影与X轴的夹角θ，将点云绕Z轴顺时针旋转θ，实现点云分布方向与X轴对齐，其中β取值-10度、10度。

第三步，根据步骤二计算每个点在曲面体素栅格中的索引，记录体素中点云数量以及体素质心，此处w取值40，h取值20，Δz取值0.2m，r_max取值30m。基于曲面体素结构，滤除噪声和无效点，其中，δ取值1，ρ_max取值40，α取值150。

第四步、在w×h的曲面栅格上计算其正向高程值和逆向高程值，根据分隔平面邻域体素的占用情况，判断每个栅格上下两部分体素的连通性，分隔平面的邻域半径ε取值5，体素占用比例阈值β_th取值0.5。根据连通性更新高程值，得到双通道高程描述子，分别对应场景中的垂直结构高程图和悬浮物体高程图。然后，对点云进行微小的旋转扰动调整，分别计算其对应的高程描述子。

Claims (1)

1.一种基于双向高程模型的局部环境特征描述方法，其特征在于，步骤如下：

步骤一、激光点云数据预处理

对输入激光点云进行主成分分析，对点云进行旋转，将主特征向量在XY平面上的投影与X轴对齐；详细步骤如下：

(1)每帧激光扫描P包含p₁，p₂，…，p_i，…，p_n共n个激光点，其中p_i＝[x_i,y_i,z_i]，则通过公式(1)计算该点云集合的质心，其坐标为

(2)对点云集合P进行主成分分析，首先计算该集合的协方差矩阵

运算过程如公式(2)所示，

然后根据det(∑-λI)＝0求得协方差矩阵的特征值λ₀、λ₁、λ₂，由∑ξ＝λξ求得特征值对应的特征向量ξ₀、ξ₁、ξ₂，代表了点云集合P的三个主要分布方向；取λ₀为最大特征值，即主特征值，则对应的特征向量ξ₀为主特征向量，表示点云在空间上离散程度最大的方向；

(3)进行点云对齐时并不关心Z轴方向的分布，所以将主特征向量ξ₀投影到XY平面，得到投影后的主特征向量ξ'₀＝[ξ_0-x,ξ_0-y,0]，ξ_0-x和ξ_0-y分别是ξ₀在X和Y方向的分量；计算ξ'₀与X轴正方向的夹角θ＝arctan(ξ'_0-y/ξ'_0-x),ξ'_0-x和ξ'_0-y分别为ξ'₀在X和Y方向的分量；通过公式(3)对点云集合P进行旋转，将ξ₀'与X轴对齐，得到空间方向归一化的点云集合P'＝{p'₁,p'₂,...,p'_i,...,p'_n}，

其中，θ需要进行规范化，限定在[0,2π)范围内；

(4)在实际场景中，视角变化、动态物体以及噪声这些不确定因素会导致点云数据分布上的差异，但通过对齐后，这些不确定因素只会对整体点云分布产生微小的旋转扰动；此处设定旋转扰动β，根据公式(3)，令θ＝θ+β，分别计算经过旋转变换后的点云P’_-β、P’_β，作为当前场景的冗余扩展；

步骤二、构建曲面体素索引

(1)以激光传感器为中心，对当前激光点云场景进行划分；在XY的平行平面上，以激光传感器为中心构建h个嵌套的同心环，环的宽度为Δr＝r_max/h，r_max为环的最大半径，在r_max半径范围外的点云过于稀疏，对场景描述没有贡献，然后将r_max对应的最大圆形区域等分为w个相邻的扇形，划分角度为Δθ＝2π/w；扇形与环相交的单元称为曲面栅格；在Z轴方向上对点进行等间距划分，Δz为划分间隔，得到

个高度区间，其中z_max和z_min分别为激光点在Z轴方向上的最大值和最小值；至此，每个曲面栅格上又划分了l个高度区间，w×h×l维度的曲面体素VG构建完成；

(2)利用点云的空间坐标xyz计算得到其在曲面体素结构中的索引uvc；u为扇区索引，以X负方向为起始，沿顺时针方向递增；v为同心环索引，由中心到外缘递增；c为通道索引，沿Z轴正方向递增；坐标映射公式(5)如下：

其中，r为点到激光雷达传感器中心的距离，对于点p_i＝{x_i,y_i,z_i}，其计算式为r_i＝||p_i||₂，只保留r_i≤r_max的激光点云；至此，三维点云数据被分配到了对应的曲面体素中；

(3)曲面体素的去噪

设某个曲面体素的索引值为u_qv_qc_q，其中，u_q为扇区索引，v_q为同心环索引，c_q为通道索引，该体素中包含n_q个激光点；在计算该体素的点云密度时，取其周围(2δ+1)³个曲面体素作为邻域，该邻域的索引集合为S＝{i,j,k∈[-δ,δ]}，其中，δ为体素邻域区间半径；用当前体素与邻域体素中点云数量的加权和

来表示点的空间密度，n(u_q+i,v_q+j,c_q+k)表示索引u_q+i,v_q+j,c_q+k对应的体素所包含的激光点的数量；最后，通过密度阈值ρ_th区分有效点与噪声点；由于在构建曲面体素索引的过程中在垂直方向上进行了均匀分割，所以根据公式(6)，结合激光点距r_i对空间密度阈值

进行自适应调整，

其中，ρ_max为密度最大值，α为密度自适应调整系数；利用该阈值对曲面体素进行筛选：

的曲面体素为噪声，应予以剔除；

的曲面体素为有效数据点，应当保留；

步骤三、双向高程模型构建

(1)将整个曲面体素栅格分割为上下两部分，生成正向和逆向高程图；

三维激光雷达所采集的数据以雷达中心为原点，则由步骤二可知，激光点云数据的坐标原点对应曲面体素坐标系下的点(0,0,c₀)，其中c₀＝-z_min/Δz；取c＝c₀作为分割平面，将曲面体素栅格分为下部VG_d和上部VG_u；对于体素栅格的下部VG_d，将c＝0平面作为基准平面，根据公式(7)沿Z轴正方计算每个栅格的高程值G_d(i,j)，称G_d(i,j)的集合为正向高程图G_d；而对于体素栅格的上部VG_u，将c＝c_max平面作为基准平面，其中

是通道索引的最大值，根据公式(8)沿Z轴负方向计算每个栅格的高程值G_u(i,j)，称G_u(i,j)的集合为逆向高程图G_u；

其中，VG(i,j,k)为体素索引ijk对应的Z轴坐标；G_d(i,j)为曲面栅格索引ij对应的正向高程值；G_u(i,j)为曲面栅格索引ij对应的逆向高程值；

(2)利用分割平面的邻域特性判断上下区域的连通性，如下所示分为两步：

第一步，根据G_d(i,j)+G_u(i,j)判断上下区域是否接触；如果G_d(i,j)+G_u(i,j)≤c_max-ε，则上下区域无接触，故上部属于悬空物体，不需要进一步处理；如果G_d(i,j)+G_u(i,j)＞c_max-ε，则上下区域可能有接触，但由于存在动态、部分遮挡等情况，需要进一步验证；其中，ε为分隔平面的邻域半径；

第二步，如果上下区域可能接触，进行二次验证；取c₀的邻域通道C_S＝{c₀-ε,...,c₀,...,c₀+ε}，计算VG的各个栅格上邻域C_S中曲面体素的占用比例β＝n_occupied/n_total，其中n_occupied为占用体素数量，n_total为体素总数量2ε+1；当β≥β_th时，VG_d和VG_u是连通的，即当前栅格上存在连续的垂直物体，此时根据公式

来更新G_d，获取更加显著的环境特征；当β＜β_th时，VG_d和VG_u是非连通的，此时上部VG_u中存在悬空物体，应该保留G_d(i,j)的高程值来进行场景描述；其中，β_th为体素占用比例阈值；

(3)在分别得到上下两部分的有效高程描述后，由于两次所采用的基准平面不同，导致在不同位置构建的场景描述不具备通用性，所以取c＝c₀平面作为基准平面，对正向和逆向的高程图进行融合，近地部分的高程图EG_d＝G_d-c₀Δz，悬空部分的高程图EG_u＝c_maxΔz-G_u，二者结合构成了双通道的高程描述EG＝{EG_d,EG_u}；最终，沿V轴正方向将双通道的高程描述EG展开，

根据步骤二和步骤三，对步骤一中的冗余扩展P’_-β和P’_β进行相同的运算，得到场景的冗余高程描述EG_-β和EG_β。

Images