一种基于地平面拟合和地面概率估计的三阶段点云地面分割方法
本发明涉及智能驾驶感知，具体涉及一种基于地平面拟合和地面概率估计的三阶段点云地面分割方法。
背景技术：
1、近年来，得益于人工智能、传感器等技术的飞速发展，智能驾驶车辆等设备逐渐走进了人们的日常生活中。这些设备通常配备有摄像头、激光雷达、毫米波雷达和惯导等传感器，其中激光雷达能够输出高精度的点云数据，可以为环境感知等任务提供准确的信息，是智能驾驶车辆的重要传感器单元。通过对点云数据进行准确的分析和处理，可以帮助智能驾驶车辆更好地理解周围环境。点云地面分割是指从激光雷达等传感器获取的3d点云数据中，将地面点云和非地面点云进行分离的过程。它是智能驾驶的基本任务之一，能够为可行驶区域检测、目标检测、激光 slam等感知任务提供准确的先验信息。因此，高效而又准确的地面分割有助于提升智能驾驶车辆的环境感知能力，从而提高车辆的自主导航能力和安全性。
2、目前，基于激光雷达点云的地面分割方法已经取得了一定的研究进展，但仍存在一些问题和挑战。例如，传统的地面分割方法主要基于高度阈值、随机样本一致性（ransac）等算法，这些方法在简单场景中能够取得较好的效果，但在复杂多变的环境中，如存在坡度、台阶、草丛等地形时，其分割效果往往不尽如人意，泛化能力较差。随着技术的不断进步，基于主成分分析（pca）的地平面拟合方法凭借其高效的迭代方式和良好的分割性能，在传统的点云地面分割方法中取得了显著的进步，并且已渐渐成为主流方法。然而，这类方法在提高地平面拟合的准确性、提升精度和召回率等方面所做的工作仍有许多不完善的地方，其分割不足和分割过度的问题仍然存在，且鲁棒性仍有待加强。与此同时，在处理大规模复杂场景的激光雷达点云数据时，其分割速度仍有待提高。
技术实现思路
1、针对现有技术的上述不足，本发明提供了一种基于地平面拟合和地面概率估计的三阶段点云地面分割方法，该方法能够显著提高目前基于主成分分析（pca）的地平面拟合方法的分割准确性、鲁棒性以及实时性，对于提升智能驾驶系统的自主导航能力和安全性具有重要意义。
2、为实现上述目的，本发明提供了如下技术方案：
3、一种基于地平面拟合和地面概率估计的三阶段点云地面分割方法，包括以下步骤：
4、s1：基于地平面拟合和绝对地面判断，采用分段极坐标栅格模型和动态距离阈值，提取出原始点云中绝大部分地面点；
5、s2：进一步结合体素预处理，对剩余点云中的地面点进行二次提取，并合并前后两次提取的地面点，得到候选地面点云；
6、s3：基于地平面拟合和地面概率估计，采用分段极坐标栅格模型和距离阈值自适应，分离出候选地面点云中的非地面点，并合并所有的非地面点，从而得到最终的地面点云和非地面点云。
7、进一步地，步骤s1的具体实施方式为：1)以激光雷达坐标系的原点为中心，采用分段极坐标栅格模型划分原始点云，具体做法为：在极径为的感兴趣区域进行栅格划分，其中在极径为的区域按照径向分辨率和角度分辨率进行均匀极坐标栅格划分，而在<mstyle displaystyle="true" mathcolor="#000000"><mi>(</mi><msubsup><mi>r</mi><mrow><mstyle displaystyle="true" mathcolor="#000000"><mi>i</mi></mstyle><mstyle displaystyle="true" mathcolor="#000000"><mi>n</mi></mstyle><mstyle displaystyle="true" mathcolor="#000000"><mi>n</mi></mstyle><mstyle displaystyle="true" mathcolor="#000000"><mi>e</mi></mstyle><mstyle displaystyle="true" mathcolor="#000000"><mi>r</mi></mstyle></mrow><mi>'</mi></msubsup><mi>,</mi><msub><mi>r</mi><mi>roi</mi></msub><mi>]</mi></mstyle>的区域仅按照角度分辨率进行极坐标栅格划分；其中为预设值，有；
8、进一步地，2) 根据每个栅格在xoy平面的面积计算与之对应的距离阈值：当时，，其中；当时，，其中，；当时，，其中，；上述、分别为第二阶段均匀划分极坐标栅格的最小栅格面积、最大栅格面积，为感兴趣区域的面积，有；、和则分别为预设的与面积、和相对应的距离阈值，有；
9、进一步地，3) 处理每个栅格中的点云：若栅格中点的数量为0，则跳过该栅格，继续处理下一个栅格中的点云；若栅格中点的数量大于0小于3，则根据高度信息和距离阈值来分割点云，首先计算栅格中个高度最低点的平均高度值，特别地，若实际参与计算的点数量为0，取；若，取高度上限值为，否则取值为，将栅格中高度值小于的点归类为候选地面点，其它点归类为剩余点，其中为预设的点数量阈值，为根据实际地面高度计算的高度阈值；若栅格中点的数量大于等于3，则继续执行及后续的处理步骤；
10、进一步地，4) 计算栅格点云的质心高度、表面法向量以及平坦程度，其中为点云协方差矩阵的最小特征值对应的特征向量，，式中、、为点云协方差矩阵的特征值，假设；分别根据、和计算栅格点云的质地高差、表面法向量与轴正方向夹角余弦值的绝对值以及平坦程度与理想平坦程度边界0的差值；当、以及均满足时，将栅格中的所有点直接归类为候选地面点，并继续处理下一个栅格中的点云，否则继续执行及后续的处理步骤，其中、和分别为预设的质心高度差常数、倾斜程度差常数以及平坦程度差常数；
11、进一步地，5) 对栅格点云进行降采样，并使用降采样后的点云执行后续的处理步骤；
12、进一步地，6) 剔除点云中的反射噪声；
13、进一步地，7) 将所有点打上地面属性的默认标签；
14、进一步地，8) 计算点云的值；
15、进一步地，9) 基于距离阈值执行迭代地平面拟合；
16、进一步地，10) 对平面拟合结果进行判断并分割栅格中的原有点云：若平面的倾斜程度符合预设的阈值要求，将原有点云中到该平面的有向距离小于的点归类为候选地面点，其它点归类为剩余点；若平面的倾斜程度不符合预设的阈值要求，则丢弃该平面，根据高度信息和距离阈值将栅格中的原有点云分割为候选地面点云和剩余点云。
17、进一步地，步骤s2的具体实施方式为：
18、进一步地，1) 以激光雷达坐标系的原点为中心，采用分段极坐标栅格模型划分剩余点云，具体做法为：在极径为的感兴趣区域进行栅格划分，其中在极径为的区域按照径向分辨率和角度分辨率进行均匀极坐标栅格划分，而在<mstyle displaystyle="true" mathcolor="#000000"><mi>(</mi><msubsup><mi>r</mi><mrow><mstyle displaystyle="true" mathcolor="#000000"><mi>i</mi></mstyle><mstyle displaystyle="true" mathcolor="#000000"><mi>n</mi></mstyle><mstyle displaystyle="true" mathcolor="#000000"><mi>n</mi></mstyle><mstyle displaystyle="true" mathcolor="#000000"><mi>e</mi></mstyle><mstyle displaystyle="true" mathcolor="#000000"><mi>r</mi></mstyle></mrow><mi>''</mi></msubsup><mi>,</mi><msub><mi>r</mi><mi>roi</mi></msub><mi>]</mi></mstyle>的区域仅按照角度分辨率进行极坐标栅格划分；其中为预设值，有；
19、进一步地，2) 根据每个栅格在xoy平面的面积计算与之对应的距离阈值：当时，；当 时，；当时，；
20、进一步地，3) 处理每个栅格中的点云：若栅格中点的数量为0，则跳过该栅格，继续处理下一个栅格中的点云；若栅格中点的数量大于0小于3，则根据高度信息和距离阈值将栅格中的点云分割为候选地面点和非地面点；若栅格中点的数量大于等于3，则继续执行后续的处理步骤
21、进一步地，4) 分别计算栅格点云的、和值，当、以及均满足时，将栅格中的所有点直接归类为候选地面点，并继续处理下一个栅格中的点云，否则继续执行2.5)及后续的处理步骤；
22、进一步地，5) 复制一份相同的点云，并使用该点云执行后续的处理步骤；
23、进一步地，6) 将所有点打上地面属性的默认标签；
24、进一步地，7) 计算点云的值；
25、进一步地，8) 对点云进行体素预处理，具体做法为：使用预处理距离阈值，分别按照点到平面距离小于和大于，向两个方向执行迭代平面拟合，在得到每次迭代的平面后，判断平面的倾斜程度是否符合预设的阈值要求，若不符合，使用到该平面的无向距离小于且高度值大于等于的点更新和的边界值，在迭代结束后，得到、、、，将点云中满足，且的点标记为障碍物点，最后提取出点云中的非障碍物点，用于继续执行后续的处理步骤；其中为该栅格的保留地面点高度范围，当时，；当时，；当时，；其中，，，，，、和分别为预设的与面积、和相对应的保留地面点高度范围，有；
26、进一步地，9) 基于距离阈值执行迭代地平面拟合；
27、进一步地，10) 对平面拟合结果进行判断并分割栅格中的原有点云：若平面的倾斜程度符合预设的阈值要求，进一步计算原有点云中点到该平面有向距离的最小值以及点到该平面有向距离小于的点个数，若或者但，将原有点云中到该平面的有向距离小于的点归类为候选地面点，其它点归类为非地面点，若且，则丢弃该平面，根据高度信息和距离阈值将栅格中的原有点云分割为候选地面点云和非地面点云，其中为预设的缓冲区点数量阈值；若平面的倾斜程度不符合预设的阈值要求，则丢弃该平面，根据高度信息和距离阈值将栅格中的原有点云分割为候选地面点云和非地面点云。
28、进一步地，步骤s3的具体实施方式为：
29、进一步地，1) 以激光雷达坐标系的原点为中心，采用分段极坐标栅格模型划分候选地面点云，具体做法为：在极径为的感兴趣区域进行栅格划分，其中在极径为的区域按照径向分辨率和角度分辨率进行均匀极坐标栅格划分，而在<mstyle displaystyle="true" mathcolor="#000000"><mi>(</mi><msubsup><mi>r</mi><mrow><mstyle displaystyle="true" mathcolor="#000000"><mi>i</mi></mstyle><mstyle displaystyle="true" mathcolor="#000000"><mi>n</mi></mstyle><mstyle displaystyle="true" mathcolor="#000000"><mi>n</mi></mstyle><mstyle displaystyle="true" mathcolor="#000000"><mi>e</mi></mstyle><mstyle displaystyle="true" mathcolor="#000000"><mi>r</mi></mstyle></mrow><mi>'''</mi></msubsup><mi>,</mi><msub><mi>r</mi><mi>roi</mi></msub><mi>]</mi></mstyle>的区域仅按照角度分辨率进行极坐标栅格划分；其中为预设值，有；
30、进一步地，2) 处理每个栅格中的点云：若栅格中点的数量为0，则跳过该栅格，继续处理下一个栅格中的点云；若栅格中点的数量大于0小于3，则根据高度信息和来分割点云，具体做法为：首先计算栅格点云的值，再基于函数 计算栅格点云属于地面的概率，其中为放大系数，有，式中为期望的接近于1的概率值；当时，地面概率；当时，地面概率，其中为与当前栅格直接相邻的栅格的数量，为当前栅格点云质心高度与相邻栅格点云质心高度的差值；当时，地面概率；若当前栅格点云的地面概率大于0.5，将栅格中的所有点直接归类为地面点，并继续处理下一个栅格中的点云；若当前栅格点云的地面概率小于等于0.5，计算当前栅格点云和所有直接相邻的栅格点云的质心高度之和，并求取平均值，取高度上限值为，将栅格中高度值小于的点归类为地面点，其它点归类为非地面点；若栅格中点的数量大于等于3，则继续执行后续的处理步骤；
31、进一步地，3) 分别计算当前栅格点云在质心高度、倾斜程度以及平坦程度上的地面概率：在质心高度上，计算地面概率为；在倾斜程度上，首先计算栅格点云的值，再基于函数计算栅格点云属于地面的概率，其中为放大系数，有；当时，地面概率；当时，地面概率，其中为与当前栅格直接相邻的栅格的数量，为当前栅格点云表面法向量与相邻栅格点云表面法向量夹角余弦值的绝对值；当时，地面概率；在平坦程度上，首先计算栅格点云的值，再基于函数计算栅格点云属于地面的概率，其中为放大系数，有；当时，地面概率；当时，地面概率，其中为与当前栅格直接相邻的栅格的数量，为当前栅格点云平坦程度与相邻栅格点云平坦程度的差值；当时，地面概率；
32、进一步地，4) 判断函数为，当自变量大于0.5时，取值为1，否则取值为0；若 ，将栅格中的所有点直接归类为地面点，并继续处理下一个栅格中的点云，否则继续执行后续的处理步骤；
33、进一步地，5) 距离阈值自适应：首先根据计算当前栅格点云在xoy平面的分布面积，其中、和、分别为栅格中点云的最大、最小水平视场角和最大、最小径向距离，水平视场角以弧度表示；然后计算与之对应的距离阈值，当时，，其中；当时，，其中，；当时，，其中，；上述、分别为第三阶段均匀划分极坐标栅格的最小栅格面积、最大栅格面积，为感兴趣区域的面积，有；、和则分别为预设的与面积、和相对应的距离阈值，有；
34、进一步地，6) 合并当前栅格点云和与之直接相邻的所有栅格点云，并使用合并后的点云执行后续的处理步骤；
35、进一步地，7) 剔除点云中的反射噪声；
36、进一步地，8) 将所有点打上地面属性的默认标签；
37、进一步地，9) 计算点云的值；
38、进一步地，10) 基于距离阈值执行迭代地平面拟合；
39、进一步地，11) 对平面拟合结果进行判断并分割栅格中的原有点云：若平面的倾斜程度符合预设的阈值要求，将原有点云中到该平面的有向距离小于的点归类为地面点，其它点归类为非地面点；若平面的倾斜程度不符合预设的阈值要求，则丢弃该平面，根据高度信息和距离阈值将栅格中的原有点云分割为地面点云和非地面点云。
40、进一步地，
技术实现要素：
所述步骤s1中9)、s2中9)、s3中10)的具体实施方式为：
41、(1) 从点云中选取初始迭代点集；(2) 检查初始迭代点集的点数量，如果小于3，则根据方程手动构建地平面，若，取系数<mstyle displaystyle="true" mathcolor="#000000"><mi>[</mi><mi>a</mi><mi>,</mi><mi>b</mi><mi>,</mi><mi>c</mi><mi>,</mi><mi>d</mi><mi>]</mi><mi>=</mi><mi>[</mi><mn>0</mn><mi>,</mi><mn>0</mn><mi>,</mi><mn>1</mn><mi>,</mi><mi>−</mi><msub><mover accent="true"><mi>h</mi><mo>¯</mo></mover><mi>ave-lpr</mi></msub><mi>]</mi></mstyle>，否则<mstyle displaystyle="true" mathcolor="#000000"><mi>[</mi><mi>a</mi><mi>,</mi><mi>b</mi><mi>,</mi><mi>c</mi><mi>,</mi><mi>d</mi><mi>]</mi><mi>=</mi><mi>[</mi><mn>0</mn><mi>,</mi><mn>0</mn><mi>,</mi><mn>1</mn><mi>,</mi><mi>−</mi><msub><mi>h</mi><mi>thr</mi></msub><mi>]</mi></mstyle>；如果点数量大于等于3，则继续执行后续的处理步骤；(3) 进入迭代循环，并在每次使用迭代点集拟合平面前，检查该点集的点数量，如果小于3，进一步判断最近一次拟合得到的平面的倾斜程度是否符合预设的阈值要求，如果符合，则直接退出迭代循环，如果不符合，则丢弃该平面，并手动构建地平面，然后退出迭代循环；如果该点集的点数量大于等于3，则继续执行后续的处理步骤；(4)使用迭代点集拟合平面，并将点云中到该平面的有向距离小于的点更新到迭代点集中，其中为传入的距离阈值；(5) 标记障碍物点：检查每次迭代得到的平面的倾斜程度是否符合预设的阈值要求，如果不符合，则将点云中到该平面的无向距离小于的点标记为障碍物点；(6) 判断迭代是否收敛，若收敛，则退出迭代循环；(7) 迭代循环最多执行次，为预设的迭代次数；(8) 检查迭代结束后的平面拟合结果并对错误的结果进行修正：如果平面的倾斜程度符合预设的阈值要求，则退出修正循环；如果不符合，则丢弃该平面，提取出点云中没有被标记为障碍物类型的点，并使用这些点重新执行拟合地平面的过程，该过程最多重复次，为预设的重复次数。
42、相比现有技术，本发明具有如下有益效果：
43、1）、本发明所述的一种基于地平面拟合和地面概率估计的三阶段点云地面分割方法，提供了一种距离阈值动态调整方法，该方法在第一阶段和第二阶段通过采用基于直线方程的距离阈值分段函数，并结合分段极坐标栅格模型均匀极坐标栅格划分区域的最大最小栅格面积以及该模型覆盖的感兴趣区域面积，实现了距离阈值的动态调整，从而提升了算法的准确性。
44、2）、本发明所述的一种基于地平面拟合和地面概率估计的三阶段点云地面分割方法，提供了一种距离阈值自适应方法，该方法在第三阶段通过计算每个栅格中点云的实际分布面积，并结合距离阈值动态调整方法，实现了根据点云的实际分布情况进行距离阈值的自适应，进一步提升了算法的准确性。
45、3）、本发明所述的一种基于地平面拟合和地面概率估计的三阶段点云地面分割方法，提供了一种基于体素空间的点云预处理方法，该方法在第二阶段通过采用二方向平面迭代拟合的方法，计算出每个栅格中障碍物点的分布边界，从而形成一个包裹障碍物点的体素空间，在根据该体素空间剔除障碍物点后，平面可以更准确的拟合到地面上，从而进一步提升了算法的准确性。
46、4）、本发明所述的一种基于地平面拟合和地面概率估计的三阶段点云地面分割方法，提供了一种基于绝对地面概率和相对地面概率的地面属性判断方法，该方法在第一阶段和第二阶段通过计算栅格点云的质心高度、倾斜程度以及平坦程度，并与相应的预设阈值进行比较，最后综合这三个维度上的比较结果判断栅格点云是否全为地面点云；该方法在第三阶段通过计算栅格点云在质心高度、倾斜程度以及平坦程度上的地面概率，并综合在这三个维度上的地面概率判断栅格点云是否全为地面点云；在三个阶段中，该方法会根据判断的结果决定是否拟合地平面来分割点云；该方法显著的增强了算法的准确性、鲁棒性和实时性。
47、5）、本发明所述的一种基于地平面拟合和地面概率估计的三阶段点云地面分割方法，提供了一种对错误拟合结果的修正方法，该方法通过在迭代过程中对障碍物点进行标记，使得算法在检查出拟合结果不正确时能够提取出点云中非障碍物类型的点重新拟合地平面，增强了算法的准确性、鲁棒性和实时性。
技术特征：
1.一种基于地平面拟合和地面概率估计的三阶段点云地面分割方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的基于地平面拟合和地面概率估计的三阶段点云地面分割方法，其特征在于，步骤s1 具体包括：
3.根据权利要求1所述的基于地平面拟合和地面概率估计的三阶段点云地面分割方法，其特征在于，步骤s2具体包括：
4.根据权利要求1所述的基于地平面拟合和地面概率估计的三阶段点云地面分割方法，其特征在于，步骤s3具体包括：
5.根据权利要求1所述的基于地平面拟合和地面概率估计的三阶段点云地面分割方法，其特征在于，步骤s1-s3所述的地平面拟合的实现过程为：
技术总结
本发明公开了一种基于地平面拟合和地面概率估计的三阶段点云地面分割方法，属于智能驾驶感知技术领域，包括以下步骤：S1基于地平面拟合和绝对地面判断，采用分段极坐标栅格模型和动态距离阈值，提取出原始点云中绝大部分地面点；S2进一步结合体素预处理，对剩余点云中的地面点进行二次提取，并合并前后两次提取的地面点，得到候选地面点云；S3基于地平面拟合和地面概率估计，采用分段极坐标栅格模型和距离阈值自适应，分离出候选地面点云中的非地面点，并合并所有的非地面点，从而得到最终的地面点云和非地面点云。本发明所述方法精准高效、稳定可靠，能有效提升目前传统点云地面分割方法的准确性、稳健性和实时性，可用于智能驾驶的建图和导航。
技术研发人员：肖德旭,郭亮,刘锐军,刘君
受保护的技术使用者：南昌航空大学
技术研发日：
技术公布日：2024/9/23