FMCW激光雷达随机采样提取静止点云的速度估计方法与流程

fmcw激光雷达随机采样提取静止点云的速度估计方法
技术领域
1.本发明涉及了一种基于雷达的速度估计方法，具体涉及一种fmcw激光雷达随机采样提取静止点云的速度估计方法。


背景技术：

2.fmcw激光雷达是一种发射功率恒定、光载波频率(或相位)持续周期性变化的雷达，通过相干检测的方式，测量回波信号与发射信号之间由距离延时引入的调制频率差和相对速度引入的多普勒频率差，从而解调出目标的距离与速度。基于调频连续波(fmcw)技术的激光雷达可以有效地克服tof激光雷达的抗干扰性差的问题，且连续波工作模式比脉冲工作模式具有更小的平均发射功率和整体功耗以及相干检测方式带来的更高的灵敏度。因此，未来的无人驾驶技术在激光雷达的选择会逐渐从tof激光雷达转移到fmcw激光雷达。
3.在一个未知的环境中，对运动物体的感知尤为重要。在路径规划算法中，车辆需要知道周围哪些物体正在运动，并对运动物体进行速度的追踪，从而确定车辆周围的可行驶区域；在slam建图中，车辆通过当前帧的点云图，判断出哪些为运动的物体，从而剔除其相关的点云信息，从而建出没有杂点的地图。传统雷达扫描的信息特征不包含速度信息，因此传统雷达要获取速度信息需要两帧甚至多帧的数据估计速度信息。而且基于传统激光雷达的运动物体检测算法，通常是处理扫描点坐标信息进行运动物体的检测，使用该坐标信息进行运动物体检测，往往存在计算量大，估计目标运动物体速度不够精确等特点。fmcw激光雷达相较于常规的激光雷达，扫描到的点云数据，不仅含有扫描点坐标信息，还包含了该坐标点相对于激光雷达的径向速度信息。


技术实现要素：

4.为了解决背景技术中存在的问题，本发明所提供一种fmcw激光雷达随机采样提取静止点云的速度估计方法，可用于智慧安防、智慧城市、无人驾驶和辅助驾驶等领域。
5.本发明采用的技术方案是：
6.方法包括如下步骤：
7.步骤一：通过fmcw激光雷达获取物体的当前帧的点云集，点云集包括点云中的所有点以及各个点的位置和径向速度；所述的物体包含了静止物体和运动物体。
8.步骤二：遍历点云集中的每个点，以当前点作为扫描点，在扫描点周围的点中选取若干点作为周围点，所述的周围点的径向速度与扫描点的径向速度之间的差小于预设速度阈值，所述的扫描点周围的点为以扫描点为圆心，以预设半径阈值为半径的圆内的除扫描点外的所有点；通过扫描点及所有周围点的位置与径向速度计算扫描点的归一化速度；
9.步骤三：根据点云集中各点的归一化速度，通过ransac算法随机多次采样提取点云集中的静止点，由所有静止点构成静止点云集；
10.步骤四：根据步骤三得到的静止点云集估计fmcw激光雷达自身速度；
11.根据场景需求使用不同的估计方式，具体分别为：使用步骤三得到的静止点云集
中各点的归一化速度粗略估计fmcw激光雷达自身速度，使用步骤三得到的静止点云集中各点的位置和径向速度精确估计fmcw激光雷达自身速度；
12.步骤五：根据步骤二得到的点云集中各点的归一化速度聚类运动物体，并通过步骤四估计的fmcw激光雷达自身速度，估计运动物体的速度。
13.根据场景需求使用不同的估计方式，具体分别为：使用运动物体中各点的归一化速度和粗略估计的fmcw激光雷达自身速度粗略估计运动物体的速度，使用运动物体中各点的位置和径向速度以及精确估计的fmcw激光雷达自身速度精确估计运动物体的速度。
14.所述的步骤二中，具体为：
15.遍历点云集中的每个点，以当前点作为扫描点，针对每一个扫描点，通过设立速度阈值t
th1
对扫描点周围的点进行以下判断：
16.当扫描点周围的点和扫描点之间的径向速度差δv满足δv≥t
th1
时，则认为扫描点周围的该点和扫描点不属于同一类，即不同属于静止对象或同属于同一运动物体，将扫描点的归一化速度设置为空值，即设置为null；所述的静止对象是由所有静止物体构成。
17.当扫描点周围的点和扫描点之间的径向速度差δv满足δv＜t
th1
时，则将扫描点周围所有的该点均作为周围点，认为扫描点和所有周围点同属于同一类，即同属于静止对象或属于同一运动物体，将和扫描点属于同一类的所有周围点一起组成同类点集，并进行以下判断：
18.当同类点集中点的总数量小于3，则将扫描点的归一化速度设置为空值，即设置为null；
19.当同类点集中点的总数量不小于3，则通过扫描点和自身对应的所有周围点的位置与径向速度计算扫描点的归一化速度，具体为：
20.建立扫描点的归一化速度的目标函数
[0021][0022][0023]
其中，表示对目标函数进行求解获得的扫描点的归一化速度向量；表示二范数；v表示扫描点和所有周围点的径向速度的集合；a表示扫描点和所有周围点的单位方向向量的集合，通过扫描点和所有周围点的位置计算获得；
[0024]
通过最小二乘法计算出扫描点的归一化速度的目标函数进而计算出扫描点的归一化速度。
[0025]
所述的步骤三中，ransac算法如下：
[0026]
1)随机选取出点云集中的n个点，并将n个点全部作为静止点，组成点云子集，其中，n为预设值；
[0027]
2)根据点云子集的归一化速度，使用概率统计方法设定点云子集的静止点参考速度；
[0028]
3)计算点云集中除点云子集外的各个点的归一化速度与步骤2)得到的静止点参考速度之间的静止点速度差δvs，针对点云集中除点云子集外的每一个点，通过设立静
止点阈值t
th2
进行以下判断：
[0029]
当静止点速度差δvs满足δvs＜t
th2
时，则将该点加入点云子集中；
[0030]
当静止点速度差δvs满足δvs≥t
th2
时，则不将该点加入点云子集中；
[0031]
4)统计步骤3)所得当前点云子集中的点数量：
[0032]
若点数量大于预设数量阈值n时，则保留当前点云子集，继续进行下一步操作；
[0033]
若点数量小于等于数量阈值n时，则舍弃当前点云子集，返回步骤1)继续进行操作；
[0034]
5)重复执行步骤1)～4)，在迭代过程中，随机组成若干不同的点云子集，针对当前点云子集，计算当前点云子集的误差度量，并进行以下判断：
[0035]
若当前点云子集是第一次迭代的点云子集，则将当前点云子集的误差度量作为最优误差度量，将当前点云子集作为静止点云集进行保留；
[0036]
若当前点云子集不是第一次迭代的点云子集，则进行以下判断：
[0037]
若当前点云子集的误差度量不小于上一次迭代获得的最优误差度量，则舍弃当前点云子集，保留上一次迭代获得的静止点云集和最优误差度量；
[0038]
若当前点云子集的误差度量小于上一次迭代获得的最优误差度量，则保留当前点云子集，舍弃上一次迭代获得的静止点云集和最优误差度量，并将当前点云子集的误差度量作为最优误差度量，将当前点云子集作为静止点云集进行保留；
[0039]
最终的静止点云集中的每个点均为静止点。
[0040]
所述的步骤三中的步骤2)中，概率统计方法包括但不限于：a)对点云子集的归一化速度做高斯分布概率拟合，将拟合的高斯分布的期望作为静止点参考速度；b)对点云子集的归一化速度排序，将排序的归一化速度中的中位数作为静止点参考速度；c)对点云子集的归一化速度做算术平均，将算术平均值作为静止点参考速度。
[0041]
所述的步骤三中的步骤5)中，计算当前点云子集中每个点的归一化速度与静止点参考速度之差的平方值，将所有点所得的平方值取平均值作为误差度量。
[0042]
在迭代开始前，预设的最优误差度量的初始值为无穷大。
[0043]
所述的步骤四中：
[0044]
a)使用静止点云集的归一化速度粗略估计fmcw激光雷达自身速度，公式如下：
[0045][0046]
其中，表示粗略估计的fmcw激光雷达自身速度，表示静止点云集中静止点的归一化速度向量；
[0047]
b)使用静止点云集的位置和径向速度精确估计fmcw激光雷达自身速度，具体为：
[0048]
建立精确估计的fmcw激光雷达速度的目标函数
[0049]
[0050][0051]
其中，表示精确估计的fmcw激光雷达自身速度；表示二范数；v
′
表示静止点云集的径向速度的集合；a
′
表示静止点云集的单位方向向量的集合，通过静止点云集的位置计算获得。
[0052]
通过最小二乘法计算出精确估计fmcw激光雷达速度的目标函数进而计算出精确估计的fmcw激光雷达速度自身速度。
[0053]
所述的步骤五中：
[0054]
提取点云集中除静止点云集外的所有点组成运动点云集通过设立的速度阈值t
th1
，根据运动点云集中各点的归一化速度，针对运动点云集中的每两个点，进行以下判断：
[0055]
当两个点之间的归一化速度差δvm满足δvm＜t
th1
时，则认为这两点为同一个运动物体；
[0056]
当两个点之间的归一化速度差δvm满足δvm≥t
th1
时，则认为这两点不为同一个运动物体；
[0057]
将属于同一个运动物体的点组成一个运动点云子集，即将运动点云集区分并聚类出不同的运动物体的运动点云子集，表示为：
[0058][0059]
其中，表示为第一、第二
…
第q个运动点云子集；
[0060]
针对其中的每个运动点云子集均进行以下处理：
[0061]
a)通过运动点云子集的归一化速度和粗略估计的fmcw激光雷达自身速度粗略估计运动物体的速度，公式具体为：
[0062][0063]
其中，v
mq
表示粗略估计的第q个运动物体的速度，n表示第q个运动点云子集中的总点数，表示第q个运动点云子集的归一化速度向量；
[0064]
b)通过运动点云子集的位置和径向速度以及精确估计的fmcw激光雷达自身速度精确估计第q个运动物体的速度公式具体为：
[0065]
建立精确估计的运动物体的速度的目标函数
[0066][0067][0068]
其中，表示精确估计的第q个运动物体的速度；表示二范数；
[0069]vmq
表示第q个运动点云子集的径向速度的集合；a
mq
表示第q个运动点云子集的单位方向向量的集合，通过第q个运动点云子集的位置计算获得；
[0070]
通过最小二乘法计算出精确估计的运动物体的速度的目标函数进而计算出精确估计的运动物体的速度。
[0071]
本发明的有益效果是：
[0072]
1)本发明提出了扫描点归一化速度的概念。归一化速度可以快速地分割出静止点云，可以快速检测出场景中的运动物体并聚类，大大简化了运动物体检测复杂度，提高了运动物体检测精度。
[0073]
2)本发明提出了一种使用ransac算法随机多次提取出静止点云的方法，该方法能够在包含大量运动点云的点云数据集中精确地提取出静止点云，具有很强的鲁棒性。
[0074]
3)本发明提出了一种获取雷达自身速度信息与运动物体速度信息的方法，利用速度信息，能够优化激光雷达追踪运动物体的准确性，相对于传统方法必须依靠两帧或多帧的数据才能估计速度信息，本发明可以根据单帧数据估计雷达自身速度和运动物体速度。可以在精度要求不高的情况下，使用归一化速度粗略估计雷达自身速度与运动物体速度，大大减少了计算量；也可以在精度要求高的情况下，使用径向速度信息精确估计雷达自身速度与运动物体速度，符合不同场景的需求。
[0075]
4)本发明方法可用于智慧安防、智慧城市、无人驾驶和辅助驾驶等领域。
附图说明
[0076]
图1为本发明方法的总体流程图；
[0077]
图2为静止点云集示意图；
[0078]
图3为精确估计的fmcw激光雷达速度矢量示意图；
[0079]
图4为精确估计的各个运动物体的速度矢量示意图一；
[0080]
图5为精确估计的各个运动物体的速度矢量示意图二。
具体实施方式
[0081]
下面结合附图及具体实施例对本发明作进一步详细说明。
[0082]
应当明确，所描述的实施例仅仅是本技术一部分实施例，而不是全部的实施例。基于本技术中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本技术保护的范围。
[0083]
在本技术实施例中使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本技术。在本技术实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。
[0084]
如图1所示，本发明方法包括如下步骤：
[0085]
步骤一：通过fmcw激光雷达获取物体的当前帧的点云集，点云集包括点云中的所有点以及各个点的位置和径向速度；所述的物体包含了静止物体和运动物体。
[0086]
步骤二：遍历点云集中的每个点，以当前点作为扫描点，在扫描点周围的点中选取若干点作为周围点，所述的周围点的径向速度与扫描点的径向速度之间的差小于预设速度
阈值，所述的扫描点周围的点为以扫描点为圆心，以预设半径阈值为半径的圆内的除扫描点外的所有点；通过扫描点及所有周围点的位置与径向速度计算扫描点的归一化速度。
[0087]
遍历点云集中的每个点，以当前点作为扫描点，针对每一个扫描点，通过设立速度阈值t
th1
对扫描点周围的点进行以下判断：
[0088]
当扫描点周围的点和扫描点之间的径向速度差δv满足δv≥t
th1
时，则认为扫描点周围的该点和扫描点不属于同一类，即不同属于静止对象或同属于同一运动物体，将扫描点的归一化速度设置为空值，即设置为null；所述的静止对象是由所有静止物体构成。
[0089]
当扫描点周围的点和扫描点之间的径向速度差δv满足δv＜t
th1
时，则将扫描点周围所有的该点均作为周围点，认为扫描点和所有周围点同属于同一类，即同属于静止对象或属于同一运动物体，将和扫描点属于同一类的所有周围点一起组成同类点集，并进行以下判断：
[0090]
当同类点集中点的总数量小于3，则将扫描点的归一化速度设置为空值，即设置为null；
[0091]
当同类点集中点的总数量不小于3，则通过扫描点和自身对应的所有周围点的位置与径向速度计算扫描点的归一化速度，具体为：
[0092]
建立扫描点的归一化速度的目标函数
[0093][0094][0095]
其中，表示对目标函数进行求解获得的扫描点的归一化速度向量；表示二范数；v表示扫描点和所有周围点的径向速度的集合；a表示扫描点和所有周围点的单位方向向量的集合，通过扫描点和所有周围点的位置计算获得。
[0096]
通过最小二乘法计算出扫描点的归一化速度的目标函数进而计算出扫描点的归一化速度。
[0097]
步骤三：根据点云集中各点的归一化速度，通过ransac算法随机多次采样提取点云集中的静止点，由所有静止点构成静止点云集。
[0098]
ransac算法如下：
[0099]
1)随机选取出点云集中的n个点，并将n个点全部作为静止点，组成点云子集，其中，n为预设值。
[0100]
2)根据点云子集的归一化速度，使用概率统计方法设定点云子集的静止点参考速度；所述的概率统计方法包括但不限于：a)对点云子集的归一化速度做高斯分布概率拟合，将拟合的高斯分布的期望作为静止点参考速度；b)对点云子集的归一化速度排序，将排序的归一化速度中的中位数作为静止点参考速度；c)对点云子集的归一化速度做算术平均，将算术平均值作为静止点参考速度；
[0101]
概率统计方法包括但不限于：a)对点云子集的归一化速度做高斯分布概率拟合，将拟合的高斯分布的期望作为静止点参考速度；b)对点云子集的归一化速度排序，将排序的归一化速度中的中位数作为静止点参考速度；c)对点云子集的归一化速度做算术平均，
将算术平均值作为静止点参考速度。
[0102]
3)计算点云集中除点云子集外的各个点的归一化速度与步骤2)得到的静止点参考速度之间的静止点速度差δvs，针对点云集中除点云子集外的每一个点，通过设立静止点阈值t
th2
进行以下判断：
[0103]
当静止点速度差δvs满足δvs＜t
th2
时，则将该点加入点云子集中；
[0104]
当静止点速度差δvs满足δvs≥t
th2
时，则不将该点加入点云子集中；
[0105]
4)统计步骤3)所得当前点云子集中的点数量：
[0106]
若点数量大于预设数量阈值n时，则保留当前点云子集，继续进行下一步操作；
[0107]
若点数量小于等于数量阈值n时，则舍弃当前点云子集，返回步骤1)继续进行操作；
[0108]
5)在迭代开始前，预设的最优误差度量的初始值为无穷大；重复执行步骤1)～4)，在迭代过程中，随机组成若干不同的点云子集，针对当前点云子集，计算当前点云子集的误差度量：计算当前点云子集中每个点的归一化速度与静止点参考速度之差的平方值，将所有点所得的平方值取平均值作为误差度量；并进行以下判断：
[0109]
若当前点云子集是第一次迭代的点云子集，则将当前点云子集的误差度量作为最优误差度量，将当前点云子集作为静止点云集进行保留；
[0110]
若当前点云子集不是第一次迭代的点云子集，则进行以下判断：
[0111]
若当前点云子集的误差度量不小于上一次迭代获得的最优误差度量，则舍弃当前点云子集，保留上一次迭代获得的静止点云集和最优误差度量；
[0112]
若当前点云子集的误差度量小于上一次迭代获得的最优误差度量，则保留当前点云子集，舍弃上一次迭代获得的静止点云集和最优误差度量，并将当前点云子集的误差度量作为最优误差度量，将当前点云子集作为静止点云集进行保留；
[0113]
最终的静止点云集中的每个点均为静止点，如图2所示，最中心的白色部分为最终的静止点云集剩余的白色部分均为运动物体。
[0114]
步骤四：根据步骤三得到的静止点云集估计fmcw激光雷达自身速度。根据场景需求使用不同的估计方式，具体分别为：使用步骤三得到的静止点云集中各点的归一化速度粗略估计fmcw激光雷达自身速度，使用步骤三得到的静止点云集中各点的位置和径向速度精确估计fmcw激光雷达自身速度。
[0115]
步骤五：根据步骤二得到的点云集中各点的归一化速度聚类运动物体，并通过步骤四估计的fmcw激光雷达自身速度，估计运动物体的速度。根据场景需求使用不同的估计方式，具体分别为：使用运动物体中各点的归一化速度和粗略估计的fmcw激光雷达自身速度粗略估计运动物体的速度，使用运动物体中各点的位置和径向速度以及精确估计的fmcw激光雷达自身速度精确估计运动物体的速度。
[0116]
步骤四和步骤五具体如下：
[0117]
提取点云集中除静止点云集外的所有点组成运动点云集通过设立的速度阈值t
th1
，根据运动点云集中各点的归一化速度，针对运动点云集中的每两个点，进行以下判断：
[0118]
当两个点之间的归一化速度差δvm满足δvm＜t
th1
时，则认为这两点为同一个运动
物体；
[0119]
当两个点之间的归一化速度差δvm满足δvm≥t
th1
时，则认为这两点不为同一个运动物体；
[0120]
将属于同一个运动物体的点组成一个运动点云子集，即将运动点云集区分并聚类出不同的运动物体的运动点云子集，表示为：
[0121][0122]
其中，表示为第一、第二
…
第q个运动点云子集。
[0123]
针对其中的每个运动点云子集均进行以下处理：
[0124]
a)使用静止点云集的归一化速度粗略估计fmcw激光雷达自身速度，公式如下：
[0125][0126]
其中，表示粗略估计的fmcw激光雷达自身速度，表示静止点云集中静止点的归一化速度向量。
[0127]
通过运动点云子集的归一化速度和粗略估计的fmcw激光雷达自身速度粗略估计运动物体的速度，公式具体为：
[0128][0129]
其中，v
mq
表示粗略估计的第q个运动物体的速度，n表示第q个运动点云子集中的总点数，表示第q个运动点云子集的归一化速度向量。
[0130]
b)如图3所示，以fmcw激光雷达自身作为坐标参考系，显示在图3的右上角；
[0131]
使用静止点云集的位置和径向速度精确估计fmcw激光雷达自身速度，具体为：
[0132]
建立精确估计的fmcw激光雷达速度的目标函数
[0133][0134][0135]
其中，表示精确估计的fmcw激光雷达自身速度；表示二范数；v
′
表示静止点云集的径向速度的集合；a
′
表示静止点云集的单位方向向量的集合，通过静止点云集的位置计算获得。
[0136]
通过最小二乘法计算出精确估计fmcw激光雷达速度的目标函数进而计算出精确估计的fmcw激光雷达速度自身速度。
[0137]
如图4和图5所示，为精确估计各个运动物体的速度矢量，以fmcw激光雷达自身作为坐标参考系，显示在各个运动物体的上方。
[0138]
通过运动点云子集的位置和径向速度以及精确估计的fmcw激光雷达自身速度
精确估计第q个运动物体的速度公式具体为：
[0139]
建立精确估计的运动物体的速度的目标函数
[0140][0141][0142]
其中，表示精确估计的第q个运动物体的速度；表示二范数；
[0143]vmq
表示第q个运动点云子集的径向速度的集合；a
mq
表示第q个运动点云子集的单位方向向量的集合，通过第q个运动点云子集的位置计算获得；
[0144]
通过最小二乘法计算出精确估计的运动物体的速度的目标函数进而计算出精确估计的运动物体的速度。
[0145]
本发明的方法中，步骤一至步骤三的平均运算速度0.068s，步骤是至步骤五的平均运算速度为0.0084s，显著提高了算法的平均运算速度。