[发明专利]一种基于双向高程模型的局部环境特征描述方法

权利要求书

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_β。