CN113721253B - 基于fmcw激光雷达的运动物体速度检测方法 - Google Patents

Abstract

本发明公开了一种基于FMCW激光雷达的运动物体速度检测方法，主要解决现有的常规激光雷达目标速度无法单帧检测、检测精度不高的问题，其实现步骤包括：根据FMCW激光雷达获取当前帧点云信息，包括点云的位置信息和径向速度信息；处理该帧点云信息，区分出该帧信息下的静止点和运动点，区分方式包括：通过自身速度过滤掉静止点从而对运动点做聚类，以及直接基于速度连续性对运动点和静止点做聚类；利用运动物体速度估计方法对运动点云做速度估计。本发明基于FMCW激光雷达并利用该类激光雷达可获取扫描点径向速度的特点，实现单帧对运动物体速度的高精度检测，可用于智慧安防、智慧城市、无人驾驶和辅助驾驶等领域。

Description

基于FMCW激光雷达的运动物体速度检测方法

技术领域

本发明属于激光雷达技术领域，尤其一种基于FMCW激光雷达的运动物体速度检测方法，可用于智慧安防、智慧城市、无人驾驶和辅助驾驶等领域。

背景技术

FMCW激光雷达是一种发射功率恒定、光载波频率(或相位)持续周期性变化的雷达，通过相干检测的方式，测量回波信号与发射信号之间由距离延时引入的调制频率差和相对速度引入的多普勒频率差，从而解调出目标的距离与速度。基于调频连续波(FMCW)技术的激光雷达可以有效地克服ToF激光雷达的抗干扰性差的问题，且连续波工作模式比脉冲工作模式具有更小的平均发射功率和整体功耗以及相干检测方式带来的更高的灵敏度。因此，未来的无人驾驶技术在激光雷达的选择会逐渐从ToF激光雷达转移到FMCW激光雷达。

在一个未知的环境中，对运动物体的感知尤为重要。在路径规划算法中，车辆需要知道周围哪些物体正在运动，并对运动物体进行速度的追踪，从而确定车辆周围的可行驶区域；在SLAM建图中，车辆通过当前帧的点云图，判断出哪些为运动的物体，从而剔除其相关的点云信息，从而建出没有杂点的地图。传统雷达扫描的信息特征不包含速度信息，因此传统雷达要获取速度信息需要两帧甚至多帧的数据估计速度信息。而且基于传统激光雷达的运动物体检测算法，通常是处理扫描点坐标信息进行运动物体的检测，使用该坐标信息进行运动物体检测，往往存在计算量大，估计目标运动物体速度不够精确等特点。FMCW激光雷达相较于常规的激光雷达，扫描到的点云数据，不仅含有扫描点坐标信息，还包含了该坐标点相对于激光雷达的径向速度信息。

发明内容

本发明的目的在于针对上述常规雷达做运动物体检测和速度估算时存在物体信息不够丰富、计算量大、速度估计不够精确的问题，提出一种基于FMCW激光雷达的运动物体速度检测方法，能够实现更高效的运动物体检测以及运动物体速度估计。

本发明的目的是通过以下技术方案来实现的：一种基于FMCW激光雷达的运动物体速度检测方法，该方法包括以下步骤：

(1)根据FMCW激光雷达获取当前帧点云信息，包括点云的位置信息和径向速度信息；

(2)处理该帧点云信息，区分出该帧信息下的静止点和运动点，区分方式包括：通过自身速度过滤掉静止点从而对运动点做聚类，以及直接基于速度连续性对运动点和静止点做聚类；

(3)利用运动物体速度估计方法对运动点云做速度估计。

进一步地，步骤(2)中所述的通过自身速度过滤掉静止点从而对运动点做聚类方式中，估计雷达自身速度的方法具体为：

首先使用地面分割算法，获得一个初步的静止点云集根据静止点的径向速度信息，利用最小二乘法推导雷达自身速度；点云集/>中，单个点信息表示为x_i，y_i，z_i为雷达坐标系下第i个点的三维坐标，v_i为该点相对于激光雷达的径向速度，n为点云集/>的点数，则有以下公式：

其中，V_A＝(v_x，v_y，v_z)，为FMCW激光雷达当前时刻的瞬时速度；为每个扫描点的单位方向向量；将静止点云集/>带入公式，即可得到：

其中，为/>中所有静止点径向速度的估计值集合，A为/>中所有静止点的单位方向向量的集合；由此，上述公式可转化为以下函数：

f(V_A)＝V-A·V_A ^T

其中，V为中所有静止点径向速度的测量值集合；

该函数可以转化为最小二乘问题：

解出该函数即可获得估计的雷达自身速度V_A。

进一步地，步骤(2)中所述的通过自身速度过滤掉静止点从而对运动点做聚类方式中，判定静止点的方法具体为：

由于单帧下每个点的径向速度估计值满足以下公式：

其中，表示第i个点的径向速度估计值，e_si表示第i个点的单位方向向量；

如果该帧点云集中任一点满足以下公式，则判断为静止点，否则为运动点：

其中，v_i表示第i个点的径向速度观测值，T_th为判断阈值，T_th的大小取决于V_A的估计精度以及雷达的测量精度。

进一步地，步骤(2)中所述的通过自身速度过滤掉静止点从而对运动点做聚类方式中，将过滤后的运动点云集做欧式距离聚类，聚类后的运动点云集表示为：

其中，是当前帧所有运动物体的点云集合，/>表示当前帧单个运动物体的点云集合。

进一步地，步骤(2)中所述的直接基于速度连续性对运动点和静止点做聚类，具体为：

同一运动物体上相邻点之间的径向速度差Δv_m为：

Δv_m＝|v_j-v_i|＝||v_model-v_lidar||·(cos(θ_j)-cos(θ_i))

其中，v_i表示当前帧点云集中第i个点的径向速度，v_j表示与第i个点相邻的周围某点的径向速度，v_model表示运动点所属的运动物体在FMCW激光雷达坐标系下的速度矢量，v_lidar表示当前激光雷达自身的速度矢量，θ_i表示i点相对速度与该点射线方向的夹角，θ_j表示j点相对速度与该点射线方向的夹角；由于i与j点的相邻关系，近似有以下公式成立：

|Δθ|＝|θ_i-θ_j|≈θ_res

其中，θ_res为FMCW激光雷达的角分辨率；故可将径向速度差公式简化为：

Δv_m≈||v_model-v_lidar||·θ_res

同理可得相邻静止点之间的径向速度差Δv_s为：

Δv_s≈||v_lidar||·θ_res

当相邻点一个为静止点，另一个为运动点时，其径向速度差Δv_diff为：

其中，表示静止点j的真实速度与该点射线方向的夹角，θ_j表示静止点j的相对速度与该点射线方向的夹角，θ_i表示运动点i的相对速度与该点射线方向的夹角，公式同样可化简为：

由于θ_res级数在1×10^-3，为一个极小值，故带有θ_res的式子可以近似看作一个极小值，因此可以采用Δv_diff做运动点和静止点的区分；

在通常情况下，通过设立阈值T_th，当有任意两点的径向速度差Δv满足Δv＞T_th时，便可认为这两点中一个为运动点，一个为静止点；通过相邻点的径向速度差可将运动、静止点分割开，分隔开后的点云集表示为：

其中，是当前帧某静止点集的点云集合，q为静止点集总数，/>表示当前帧单个运动物体的点云集合，k为运动物体的总数；默认当前帧点云集/>中，数量最多的点云集为静止点集/>其他点云集为运动物体的点云集，将运动物体的点云集/>分割出来，表示为：

根据静止点集径向速度信息，利用最小二乘法推导激光雷达自身速度V_A。

进一步地，所述步骤(3)具体为：

首先得到激光雷达自身速度V_A以及运动物体的点云集将/>中第k个运动物体的点云集/>取出，有/>则有以下公式：

其中，为目标运动物体的速度，/>为每个扫描点的单位方向向量；将/>带入上述公式，即可得到：

其中，V_mk为中所有点的径向速度的测量值集合，A_mk为该帧所有点的单位方向向量的集合；上述公式可转化为以下函数：

该函数转化为最小二乘问题，表示为：

解出该函数即可获得估计的目标运动物体的速度

本发明与现有技术相比具有以下优点：

(1)本发明提出了两种聚类运动物体的方法，通过该方法单帧便可检测出场景中的运动物体，大大简化了运动物体检测复杂度，提高了运动物体检测精度。

(2)本发明提出了一种获取雷达自身速度信息与运动物体速度信息的方法，利用速度信息，能够优化激光雷达追踪运动物体的准确性；相对于传统方法必须依靠两帧或多帧的数据才能估计速度信息，本发明可以根据单帧数据估计雷达自身速度和运动物体速度。

(3)本发明方法可用于智慧安防、智慧城市、无人驾驶和辅助驾驶等领域。

附图说明

图1是本发明方法的总体流程图。

具体实施方式

为了更好的理解本申请的技术方案，下面结合附图对本申请实施例进行详细描述。

应当明确，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本申请保护的范围。

在本申请实施例中使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请实施例和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。

为了充分利用FMCW激光雷达能够获取更为丰富的信息的特点，本发明提供一种基于FMCW激光雷达的运动物体速度检测方法，以便进行更加高效实时的运动物体检测以及更精确的运动物体速度估计。参照图1，本发明的实现步骤如下：

1.根据FMCW激光雷达获取当前帧点云信息，包括点云的位置信息和径向速度信息。

2.处理该帧点云信息，区分出该帧信息下的静止点和运动点，区分方式包括：通过自身速度过滤掉静止点从而对运动点做聚类，以及直接基于速度连续性对运动点和静止点做聚类；

方式一：通过自身速度过滤掉静止点从而对运动点做聚类，包括以下步骤：

(1)首先使用地面分割算法，获得一个初步的静止点云集根据静止点的径向速度信息，利用最小二乘方法推导雷达自身速度。点云集/>中，单个点信息表示为x_i，y_i，z_i为雷达坐标系下第i个点的三维坐标，v_i为该点相对于激光雷达的径向速度，n为点云集/>的点数，则有以下公式：

其中，V_A＝(v_x，v_y，v_z)，为FMCW激光雷达当前时刻的瞬时速度；为每个扫描点的单位方向向量。将静止点云集/>带入公式，即可得到：

其中，为/>中所有静止点径向速度的估计值集合，A为/>中所有静止点的单位方向向量的集合。由此，上述公式可转化为以下函数：

f(V_A)＝V-A·V_A ^T

其中，V为中所有静止点径向速度的测量值集合。

该函数可以转化为最小二乘问题：

解出该函数即可获得估计的雷达自身速度V_A。

(2)由于单帧下每个点的径向速度估计值满足以下公式：

其中，表示第i个点的径向速度估计值，e_si表示第i个点的单位方向向量。

如果该帧点云集中的任一点满足以下公式，则判断为静止点，否则为运动点：

其中，v_i表示第i个点的径向速度观测值，T_th为判断阈值，T_th的大小取决于V_A的估计精度以及雷达的测量精度。

(3)将过滤后的运动点云集做欧式距离聚类，聚类后的运动点云集表示为：

其中，是当前帧所有运动物体的点云集合，/>表示当前帧单个运动物体的点云集合。

该方式分割精度高，运动物体识别率高，但是因为地面分割步骤，会略延长速度检测时间。

方式二：直接基于速度连续性对运动点和静止点做聚类，具体为：

同一运动物体上相邻点之间的径向速度差Δv_m为：

Δv_m＝|v_j-v_i|＝||v_model-v_lidar||·(cos(θ_j)-cos(θ_i))

其中，v_i表示当前帧点云集中第i个点的径向速度，v_j表示与第i个点相邻的周围某点的径向速度，v_model表示运动点所属的运动物体在FMCW激光雷达坐标系下的速度矢量，v_lidar表示当前激光雷达自身的速度矢量，θ_i表示i点相对速度与该点射线方向的夹角，θ_j表示j点相对速度与该点射线方向的夹角。由于i与j点的相邻关系，近似有以下公式成立：

|Δθ|＝|θ_i-θ_j|≈θ_res

其中，θ_res为FMCW激光雷达角分辨率。故可以将径向速度差公式简化为：

Δv_m≈||v_model-v_lidar||·θ_res

同理可得相邻静止点之间的径向速度差Δv_s为：

Δv_s≈||v_lidar||·θ_res

当相邻点一个为静止点，另一个为运动点时，其径向速度差Δv_diff为：

其中，表示静止点j的真实速度与该点射线方向的夹角，θ_j表示静止点j的相对速度与该点射线方向的夹角，θ_i表示运动点i的相对速度与该点射线方向的夹角。公式同样可化简为：

由于θ_res级数在1×10^-3，为一个极小值，故带有θ_res的式子可以近似看作一个极小值，因此可以采用Δv_diff做运动点和静止点的区分。

在通常情况下，通过设立阈值T_th，当有任意两点的径向速度差Δv满足Δv＞T_th时，便可认为这两点中一个为运动点，一个为静止点。至此，通过相邻点的径向速度差，便可轻松将运动、静止点分割开，分隔开后的点云集表示为：

其中，是当前帧某静止点集的点云集合，q为静止点集总数，/>表示当前帧单个运动物体的点云集合，k为运动物体的总数。通常情况下，单帧中运动物体的点云数远少于静止点云数，故可默认当前帧点云集/>中，数量最多的点云集为静止点集/>其他点云集为运动物体的点云集。由此运动物体的点云集/>被分割出来，表示为：

根据静止点集径向速度信息，利用最小二乘法推导激光雷达自身速度V_A。

该方案时效性很强，可以达到毫秒级。

步骤三：利用运动物体速度估计方法对运动点云做速度估计，具体包括：

对运动物体速度估计，首先得到激光雷达自身速度V_A以及运动物体的点云集将/>中第k个运动物体的点云集/>取出，有/>则有以下公式：

其中，为目标运动物体的速度，/>为每个扫描点的单位方向向量。将/>带入上述公式，即可得到：

其中，V_mk为中所有点的径向速度的测量值集合，A_mk为该帧所有点的单位方向向量的集合。由此，上述公式可转化为以下函数：

该函数转化为最小二乘问题，表示为：

解出该函数即可获得估计的目标运动物体的速度

以上所述仅为本说明书一个或多个实施例的较佳实施例而已，并不用以限制本说明书一个或多个实施例，凡在本说明书一个或多个实施例的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本说明书一个或多个实施例保护的范围之内。

Claims (5)

1.一种基于FMCW激光雷达的运动物体速度检测方法，其特征在于，包括以下步骤：

(1)根据FMCW激光雷达获取当前帧点云信息，包括点云的位置信息和径向速度信息；

(2)处理该帧点云信息，区分出该帧信息下的静止点和运动点，区分方式包括：通过自身速度过滤掉静止点从而对运动点做聚类，以及直接基于速度连续性对运动点和静止点做聚类；所述的通过自身速度过滤掉静止点从而对运动点做聚类方式中，估计雷达自身速度的方法具体为：

首先使用地面分割算法，获得一个初步的静止点云集根据静止点的径向速度信息，利用最小二乘法推导雷达自身速度；点云集/>中，单个点信息表示为x_i，y_i，z_i为雷达坐标系下第i个点的三维坐标，v_i为该点相对于激光雷达的径向速度，n为点云集/>的点数，则有以下公式：

其中，V_A＝(v_x，v_y，v_z)，为FMCW激光雷达当前时刻的瞬时速度；为每个扫描点的单位方向向量；将静止点云集/>带入公式，得到：

其中，为/>中所有静止点径向速度的估计值集合，A为/>中所有静止点的单位方向向量的集合；由此，上述公式转化为以下函数：

f(V_A)＝V-A·V_A ^T

其中，V为中所有静止点径向速度的测量值集合；

该函数转化为最小二乘问题：

解出该函数获得估计的雷达自身速度V_A；

(3)利用运动物体速度估计方法对运动点云做速度估计。

2.根据权利要求1所述的一种基于FMCW激光雷达的运动物体速度检测方法，其特征在于，步骤(2)中所述的通过自身速度过滤掉静止点从而对运动点做聚类方式中，判定静止点的方法具体为：

由于单帧下每个点的径向速度估计值满足以下公式：

其中，表示第i个点的径向速度估计值，e_si表示第i个点的单位方向向量；

如果该帧点云集中任一点满足以下公式，则判断为静止点，否则为运动点：

其中，v_i表示第i个点的径向速度观测值，T_th为判断阈值，T_th的大小取决于V_A的估计精度以及雷达的测量精度。

3.根据权利要求2所述的一种基于FMCW激光雷达的运动物体速度检测方法，其特征在于，步骤(2)中所述的通过自身速度过滤掉静止点从而对运动点做聚类方式中，将过滤后的运动点云集做欧式距离聚类，聚类后的运动点云集表示为：

其中，是当前帧所有运动物体的点云集合，/>表示当前帧单个运动物体的点云集合。

4.根据权利要求1所述的一种基于FMCW激光雷达的运动物体速度检测方法，其特征在于，步骤(2)中所述的直接基于速度连续性对运动点和静止点做聚类，具体为：

同一运动物体上相邻点之间的径向速度差Δv_m为：

Δv_m＝|v_j-v_i|＝||v_model-v_lidar||·(cos(θ_j)-cos(θ_i))

其中，v_i表示当前帧点云集中第i个点的径向速度，v_j表示与第i个点相邻的周围某点的径向速度，v_model表示运动点所属的运动物体在FMCW激光雷达坐标系下的速度矢量，v_lidar表示当前激光雷达自身的速度矢量，θ_i表示i点相对速度与该点射线方向的夹角，θ_j表示j点相对速度与该点射线方向的夹角；由于i与j点的相邻关系，近似有以下公式成立：

|Δθ|＝|θ_i-θ_j|≈θ_res

其中，θ_res为FMCW激光雷达的角分辨率；将径向速度差公式简化为：

Δv_m≈||v_model-v_lidar||·θ_res

同理得到相邻静止点之间的径向速度差Δv_s为：

Δv_s≈||v_lidar||·θ_res

当相邻点一个为静止点，另一个为运动点时，其径向速度差Δv_diff为：

其中，表示静止点j的真实速度与该点射线方向的夹角，θ_j表示静止点j的相对速度与该点射线方向的夹角，θ_i表示运动点i的相对速度与该点射线方向的夹角，公式化简为：

由于θ_res级数在1×10^-3，为一个极小值，故带有θ_res的式子近似看作一个极小值，因此采用Δv_diff做运动点和静止点的区分；

在通常情况下，通过设立阈值T_th，当有任意两点的径向速度差Δv满足Δv＞T_th时，认为这两点中一个为运动点，一个为静止点；通过相邻点的径向速度差将运动、静止点分割开，分隔开后的点云集表示为：

其中，是当前帧某静止点集的点云集合，q为静止点集总数，/>表示当前帧单个运动物体的点云集合，k为运动物体的总数；默认当前帧点云集/>中，数量最多的点云集为静止点集/>其他点云集为运动物体的点云集，将运动物体的点云集/>分割出来，表示为：

根据静止点集径向速度信息，利用最小二乘法推导激光雷达自身速度V_A。

5.根据权利要求1所述的一种基于FMCW激光雷达的运动物体速度检测方法，其特征在于，所述步骤(3)具体为：

首先得到激光雷达自身速度V_A以及运动物体的点云集将/>中第k个运动物体的点云集/>取出，有/>则有以下公式：

其中，为目标运动物体的速度，/>为每个扫描点的单位方向向量；将/>带入上述公式，得到：

其中，V_mk为中所有点的径向速度的测量值集合，A_mk为该帧所有点的单位方向向量的集合；上述公式转化为以下函数：

该函数转化为最小二乘问题，表示为：

解出该函数获得估计的目标运动物体的速度