CN112415518B - 基于车载毫米波雷达的可通行空间检测方法 - Google Patents

Abstract

本发明公开了一种基于车载毫米波雷达的可通行空间检测方法，包括：通过以太网获取多部毫米波雷达点云数据，对这些数据进行空间融合；获取组合惯导和车身底盘数据，对多个传感器数据进行时间融合；设定FreeSpace算法的预估执行周期，并计算实际执行周期，若实际周期大于预估周期，则根据实际周期重新设定预估周期，否则执行下一步；运行FreeSpace算法，得到栅格地图结果，其中栅格类型被判定为OCCUPY的栅格代表不可通行空间；在PC端运行的上位机软件中显示最终可通行空间的输出结果。本发明更大限度地保证了检测的准确性，同时还具有全天时、全天候的工作能力，极大地扩展了该技术在汽车雷达应用技术领域的实用性。

Description

基于车载毫米波雷达的可通行空间检测方法

技术领域

本发明属于汽车雷达应用技术领域，特别是一种基于车载毫米波雷达的可通行空间检测方法。

背景技术

在智能驾驶领域，多传感器融合技术是实现智能驾驶的必要条件，其中，车载毫米波雷达以其全天时、全天候的特点，已经成为不可或缺的检测传感器。相较于视觉传感器，车载毫米波雷达具有高精度、高分辨力、可测速可测距、探测距离远、高频率低功耗、高实时性等优点。

目前，在智能驾驶领域中，可通行空间检测技术主要依赖于激光雷达和视觉传感器，但是它们有着自身的缺点，其一，激光雷达成本过高，虽然其具有良好的检测能力，但不适用于量产车型；其二，视觉传感器具有较大的局限性，在恶劣天气和光线强度较弱的场景，很难完成对可通行空间的检测，受环境因素的影响较大。

发明内容

本发明的目的在于针对上述现有技术存在的问题，提供一种基于车载高分辨率毫米波雷达，实时输出车身周围无障碍物的可通行空间检测方法。

实现本发明目的的技术解决方案为：一种基于车载毫米波雷达的可通行空间检测方法，所述方法包括以下步骤：

步骤1，通过以太网获取多部毫米波雷达点云数据，对这些数据进行空间融合；

步骤2，获取组合惯导和车身底盘数据，对毫米波雷达、组合惯导和车身底盘这些传感器数据进行时间融合；

步骤3，设定FreeSpace算法的预估执行周期T_e，并计算FreeSpace算法的实际执行周期T_a，若实际执行周期大于预估执行周期，则根据实际执行周期重新设定预估执行周期，否则执行下一步；

步骤4，运行FreeSpace算法，得到栅格地图结果，其中栅格类型被判定为OCCUPY的栅格代表不可通行空间；

步骤5，在PC端运行的上位机软件中显示最终可通行空间的输出结果。

本发明与现有技术相比，其显著优点为：1)该方法通过构建栅格地图，制定栅格分值计算规则，结合Bresenham算法，对求得的栅格地图进行高斯滤波后，将可通行空间检测结果，按照离散栅格的形式输出，极大的提高了可通行空间判别的准确性和实用性；2)该方法结合组合惯导与车身底盘信息，引入多时间点平滑算法，可以更加准确地估计自车与其他目标的运动状态；3)该方法采用车载毫米波雷达作为主传感器，在大桥、公共道路、特定园区内具有良好的检测能力，能适应全天候、全天时工作。

下面结合附图对本发明作进一步详细描述。

附图说明

图1为一个实施例中基于车载毫米波雷达的可通行空间检测方法的流程图。

图2为一个实施例中构建的栅格灰度图。

图3为一个实施例中检测目标的效果图，其中图(a)为检测出行人的效果图；图(b)为检测出车辆的效果图；图(c)为检测出路交通锥的效果图；图(d)为检测出护栏的效果图。

图4为一个实施例中最终可通行空间离散栅格结果输出图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

在一个实施例中，结合图1，提供了一种基于车载毫米波雷达的可通行空间检测方法，所述方法包括以下步骤：

步骤1，通过以太网获取多部毫米波雷达点云数据，对这些数据进行空间融合；

步骤2，获取组合惯导和车身底盘数据，对毫米波雷达、组合惯导和车身底盘这些传感器数据进行时间融合；

步骤3，设定FreeSpace算法的预估执行周期T_e，并计算FreeSpace算法的实际执行周期T_a，若实际执行周期大于预估执行周期，则根据实际执行周期重新设定预估执行周期，否则执行下一步；

步骤4，运行FreeSpace算法，得到栅格地图结果，其中栅格类型被判定为OCCUPY的栅格代表不可通行空间；

步骤5，在PC端运行的上位机软件中显示最终可通行空间的输出结果。

进一步地，在其中一个实施例中，步骤1所述通过以太网获取毫米波雷达点云数据，对多部毫米波雷达数据进行空间融合，具体过程包括：

步骤1-1，从毫米波雷达点云数据中，获取每部毫米波雷达的时间戳，基于时间戳对多部毫米波雷达进行时间对齐；

步骤1-2，获取多部毫米波雷达的实际安装位置，之后对多部毫米波雷达的点云数据进行坐标转换，统一到以车辆后轴中心为原点的坐标系下，实现对多部毫米波雷达数据进行空间对齐。

进一步地，在其中一个实施例中，步骤1-1的具体过程包括：选定某一部毫米波雷达某一帧的时间戳，之后在该时间戳之后的Δt ms时间间隔内选定其他几部毫米波雷达数据，并统一所有毫米波雷达数据当前帧的时间戳，从而实现对多部毫米波雷达数据进行时间对齐。

这里优选地，Δt＝100。

进一步地，在其中一个实施例中，步骤2所述获取组合惯导和车身底盘数据，对毫米波雷达、组合惯导和车身底盘这些传感器数据进行时间融合，具体过程包括：

步骤2-1，引入多时间点平滑算法，对处于不同时间点的多个传感器数据进行平滑处理(保留了历史数据的趋势信息，使得预测模型能自动识别数据模式的变化而进行短期预测，以减少历史数据对平滑值的影响)；

步骤2-2，获取各传感器每一帧数据内的时间戳信息，之后对处理各传感器数据所消耗的时间进行补偿后，统一各传感器数据当前帧的时间戳，实现对多个传感器数据进行时间对齐。

这里优选地，步骤2-1所述多时间点平滑算法具体采用二次指数平滑法，该方法原理如下：

s_i＝αx_i+(1-α)(s_i-1+t_i-1)

t_i＝β(s_i-s_i-1)+(1+β)t_i-1

x_i+h＝s_i+ht_i

式中，α和β为平滑参数，i为当前最后一个数据记录的坐标，s_i表示之前i个数据的平滑值，t_i表示前i个数据平滑后的趋势，x_i表示前i个数据二次指数平滑后的预测结果，x_i+h表示前i+h个数据二次指数平滑后的预测结果。

进一步地，在其中一个实施例中，步骤3中所述计算FreeSpace算法的实际执行周期T_a，计算公式为：

T_a＝n*T_r+g*T_g+T_s

式中，n为当前一帧内所有毫米波雷达的点云个数，T_r为遍历每个毫米波雷达点云数据所需要的时间，g为预设的FreeSpace算法检测范围内栅格的总数，T_g为更新每个栅格状态所需要的时间，T_s为输出最终栅格结果所需要的时间。

进一步地，在其中一个实施例中，步骤4所述运行FreeSpace算法，得到栅格地图结果，具体过程包括：

步骤4-1，根据FreeSpace算法的检测范围，初始化栅格地图，包括栅格总数、每个栅格的栅格状态，包含栅格类型、栅格分值、动静状态；

这里优选地，预设的FreeSpace算法的检测范围为自身车辆前方50m、左右方向各20m的50m*40m的检测区域，对应栅格地图参数为width＝160格，height＝200格，分辨率为resolution＝0.25m/格，故预设的FreeSpace算法检测范围内栅格总数为g＝32000个；

步骤4-2，制定栅格分值计算规则，设定以下变量：栅格分值的最小值grid_{min_grade}和最大值grid_{max_grade}，该栅格存在障碍物概率的加分值grid_{add_grade}和时间衰减值grid_{dec_grade}，该栅格类型为OCUPPY和FREE的阈值grid_{ocuppy_thres}和grid_{free_thres}；

这里优选地，

根据连续q帧(优选地，取3)内栅格地图上点云分布情况，针对某个栅格而言，栅格分值初始为0，栅格类型初始化为FREE，当前帧时，该栅格所处位置每出现n个单位点云时，则该栅格分值增加n*grid_{add_grade}，下一帧时，该栅格分值优先减去grid_{dec_grade}后，再根据实际情况选择是否增加grid_{add_grade}，若该栅格所处位置的点云数量小于一个单位点云，则不增加，通过以上过程，计算出该栅格连续q帧的栅格分值结果；

之后将栅格分值与阈值grid_{ocuppy_thres}和grid_{free_thres}进行对比，确定该栅格的栅格类型，并将栅格类型和栅格分值更新到该栅格的栅格状态中；其中与阈值grid_{ocuppy_thres}和grid_{free_thres}进行对比，确定该栅格的栅格类型，具体包括：若栅格的栅格分值大于阈值grid_{ocuppy_thres}，则栅格的栅格类型为OCUPPY；若栅格的栅格分值小于阈值grid_{free_thres}，则栅格的栅格类型为FREE；

步骤4-3，获取多部毫米波雷达的点云数据，以及存有组合惯导和车身底盘数据的里程计信息，根据栅格分值计算规则，更新当前帧的栅格地图状态，栅格分值不为初始值0的栅格记为有效栅格，在更新下一帧时，优先对上一帧的有效栅格进行时间衰减，如此往复，计算出连续q帧的栅格分值结果，并判定该栅格的栅格类型，标记出栅格类型为OCCUPY的栅格，同时结合Bresenham算法，计算每个有效栅格的位置，并获取栅格灰度图；

步骤4-4，遍历当前栅格灰度图，检索出栅格类型为OCCUPY的栅格，对其进行高斯滤波，获得平滑处理后的栅格地图，根据栅格索引填充待返回的栅格地图数据，之后输出栅格地图数据。

进一步地，在其中一个实施例中，步骤4-3中所述计算每个有效栅格的位置，具体计算如下：

步骤4-3-1，记测试车辆状态为(x,y,θ)，其中，θ表示测试车辆的车头朝向，该值由里程计信息中车身底盘的偏航角决定，x和y表示测试车辆的坐标，默认为原点(0,0)；毫米波雷达波束与车辆的车头朝向的夹角为ω，有效栅格即目标障碍物的距离为L，计算有效栅格即目标障碍物所在点的实际位置(x_t,y_t)为：

x_t＝L cos(θ+ω)+x

y_t＝L sin(θ+ω)+y

步骤4-3-2，根据x_t,y_t计算有效栅格即目标障碍物所在点对应的栅格索引(idx,idy)，将实际障碍物映射到栅格地图中：

idx＝floor(x_t/resolution)

idy＝-ceil((y-width*resolution/2.0)/resolution)

式中，resolution为栅格的分辨率，width为栅格地图的宽；

步骤4-3-3，对于处于边界位置的障碍物点进行上述处理后，得到对应的栅格索引，之后根据位于当前栅格内毫米波雷达点云数据的径向速度信息，更新该栅格的动静状态，若径向速度超过预设速度阈值，则栅格为动态，否则为静态；

步骤4-3-4，将上述每个栅格的栅格类型、栅格分值、动静状态更新到每个栅格的栅格状态中，得到栅格灰度图如图2所示，若栅格的灰度值越小(越偏黑色)，则说明该栅格所处位置存在障碍物的概率越大。

进一步地，在其中一个实施例中，步骤5所述在PC端运行的上位机软件中显示最终可通行空间的输出结果，具体包括：

步骤5-1，遍历步骤4输出的栅格地图，检索出栅格类型为OCCUPY和FREE的栅格，将栅格类型为FREE的栅格信息输出，得到FreeSpace结果；

步骤5-2，构建包含FreeSpace结果和毫米波雷达点云信息的UDP数据包，并向PC端广播发送这些UDP数据包，PC端上位机软件接收并解析UDP数据包后，即可将最终可通行空间即FreeSpace结果实时显示出来，如图3和图4所示，其中，点状结果表示毫米波雷达和激光雷达点云，方格结果表示不可通行空间，由图可以看出，圆圈中行人、车辆、交通锥和护栏等物体所处位置被判定为方格结果，表示该处存在障碍物，属于不可通行空间，其他位置为可通行空间。

由上可知，本发明的方法，结合毫米波雷达目标信息、组合惯导与车身底盘信息，在全天时、全天候对可通行空间进行检测。本发明可以有效地识别护栏、车辆、行人等目标，更加精确地检测出可通行空间，增强智能驾驶的安全可靠性。

综上，本发明充分利用车载毫米波雷达提供的目标信噪比等特征，通过融合多个传感器数据，引入多时间点平滑算法，综合考虑不同时间点的障碍物位置与速度等特征信息，结合Bresenham算法，对求得的栅格地图进行高斯滤波后，更新栅格地图状态，将可通行空间的检测结果实时输出，从而更大限度地保证了检测的准确性。本发明基于车载毫米波雷达对车身周围障碍物进行了较为准确的边界提取，检测出的可通行空间与激光雷达检测出的基本一致。本发明FreeSpace算法处理频率约为20Hz，边界平均距离误差小于10％，检测率大于90％，错误率小于1％。另外，本发明还具有全天时、全天候的工作能力，极大地扩展了该技术在汽车雷达应用技术领域的实用性。

以上显示和描述了本发明的基本原理、主要特征及优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (8)

1.一种基于车载毫米波雷达的可通行空间检测方法，其特征在于，所述方法包括以下步骤：

步骤1，通过以太网获取多部毫米波雷达点云数据，对这些数据进行空间融合；

步骤2，获取组合惯导和车身底盘数据，对毫米波雷达、组合惯导和车身底盘这些传感器数据进行时间融合；

步骤3，设定FreeSpace算法的预估执行周期T_e，并计算FreeSpace算法的实际执行周期T_a，若实际执行周期大于预估执行周期，则根据实际执行周期重新设定预估执行周期，否则执行下一步；所述计算FreeSpace算法的实际执行周期T_a，计算公式为：

T_a＝n*T_r+g*T_g+T_s

式中，n为当前一帧内所有毫米波雷达的点云个数，T_r为遍历每个毫米波雷达点云数据所需要的时间，g为预设的FreeSpace算法检测范围内栅格的总数，T_g为更新每个栅格状态所需要的时间，T_s为输出最终栅格结果所需要的时间；

步骤4，运行FreeSpace算法，得到栅格地图结果，其中栅格类型被判定为OCCUPY的栅格代表不可通行空间；具体过程包括：

步骤4-1，根据FreeSpace算法的检测范围，初始化栅格地图，包括栅格总数、每个栅格的栅格状态，包含栅格类型、栅格分值、动静状态；

步骤4-2，制定栅格分值计算规则，设定以下变量：栅格分值的最小值grid_{min_grade}和最大值grid_{max_grade}，该栅格存在障碍物概率的加分值grid_{add_grade}和时间衰减值grid_{dec_grade}，该栅格类型为OCUPPY和FREE的阈值grid_{ocuppy_thres}和grid_{free_thres}；

根据连续q帧内栅格地图上点云分布情况，针对某个栅格而言，栅格分值初始为0，栅格类型初始化为FREE，当前帧时，该栅格所处位置每出现n个单位点云时，则该栅格分值增加n*grid_{add_grade}，下一帧时，该栅格分值优先减去grid_{dec_grade}后，再根据实际情况选择是否增加grid_{add_grade}，若该栅格所处位置的点云数量小于一个单位点云，则不增加，通过以上过程，计算出该栅格连续q帧的栅格分值结果；

之后将栅格分值与阈值grid_{ocuppy_thres}和grid_{free_thres}进行对比，确定该栅格的栅格类型，并将栅格类型和栅格分值更新到该栅格的栅格状态中；其中与阈值grid_{ocuppy_thres}和grid_{free_thres}进行对比，确定该栅格的栅格类型，具体包括：若栅格的栅格分值大于阈值grid_{ocuppy_thres}，则栅格的栅格类型为OCUPPY；若栅格的栅格分值小于阈值grid_{free_thres}，则栅格的栅格类型为FREE；

步骤4-3，获取多部毫米波雷达的点云数据，以及存有组合惯导和车身底盘数据的里程计信息，根据栅格分值计算规则，更新当前帧的栅格地图状态，栅格分值不为初始值0的栅格记为有效栅格，在更新下一帧时，优先对上一帧的有效栅格进行时间衰减，如此往复，计算出连续q帧的栅格分值结果，并判定该栅格的栅格类型，标记出栅格类型为OCCUPY的栅格，同时结合Bresenham算法，计算每个有效栅格的位置，并获取栅格灰度图；

步骤4-4，遍历当前栅格灰度图，检索出栅格类型为OCCUPY的栅格，对其进行滤波，获得平滑处理后的栅格地图，根据栅格索引填充待返回的栅格地图数据，之后输出栅格地图数据；

步骤5，在PC端运行的上位机软件中显示最终可通行空间的输出结果。

2.根据权利要求1所述的基于车载毫米波雷达的可通行空间检测方法，其特征在于，步骤1所述通过以太网获取毫米波雷达点云数据，对多部毫米波雷达数据进行空间融合，具体过程包括：

步骤1-1，从毫米波雷达点云数据中，获取每部毫米波雷达的时间戳，基于时间戳对多部毫米波雷达进行时间对齐；

步骤1-2，获取多部毫米波雷达的实际安装位置，之后对多部毫米波雷达的点云数据进行坐标转换，统一到以车辆后轴中心为原点的坐标系下，实现对多部毫米波雷达数据进行空间对齐。

3.根据权利要求2所述的基于车载毫米波雷达的可通行空间检测方法，其特征在于，步骤1-1的具体过程包括：选定某一部毫米波雷达某一帧的时间戳，之后在该时间戳之后的Δtms时间间隔内选定其他几部毫米波雷达数据，并统一所有毫米波雷达数据当前帧的时间戳，从而实现对多部毫米波雷达数据进行时间对齐。

4.根据权利要求3所述的基于车载毫米波雷达的可通行空间检测方法，其特征在于，步骤2所述获取组合惯导和车身底盘数据，对毫米波雷达、组合惯导和车身底盘这些传感器数据进行时间融合，具体过程包括：

步骤2-1，引入多时间点平滑算法，对处于不同时间点的多个传感器数据进行平滑处理；

步骤2-2，获取各传感器每一帧数据内的时间戳信息，之后对处理各传感器数据所消耗的时间进行补偿后，统一各传感器数据当前帧的时间戳，实现对多个传感器数据进行时间对齐。

5.根据权利要求4所述的基于车载毫米波雷达的可通行空间检测方法，其特征在于，步骤2-1所述多时间点平滑算法具体采用二次指数平滑法，该方法原理如下：

s_i＝αx_i+(1-α)(s_i-1+t_i-1)

t_i＝β(s_i-s_i-1)+(1+β)t_i-1

x_i+h＝s_i+ht_i

式中，α和β为平滑参数，i为当前最后一个数据记录的坐标，s_i表示之前i个数据的平滑值，t_i表示前i个数据平滑后的趋势，x_i表示前i个数据二次指数平滑后的预测结果，x_i+h表示前i+h个数据二次指数平滑后的预测结果。

6.根据权利要求1所述的基于车载毫米波雷达的可通行空间检测方法，其特征在于，步骤4-3中所述计算每个有效栅格的位置，具体计算如下：

步骤4-3-1，记测试车辆状态为(x,y,θ)，其中，θ表示测试车辆的车头朝向，该值由里程计信息中车身底盘的偏航角决定，x和y表示测试车辆的坐标，默认为原点(0,0)；毫米波雷达波束与车辆的车头朝向的夹角为ω，有效栅格即目标障碍物的距离为L，计算有效栅格即目标障碍物所在点的实际位置(x_t,y_t)为：

x_t＝Lcos(θ+ω)+x

y_t＝Lsin(θ+ω)+y

步骤4-3-2，根据x_t,y_t计算有效栅格即目标障碍物所在点对应的栅格索引(idx,idy)，将实际障碍物映射到栅格地图中：

idx＝floor(x_t/resolution)

idy＝-ceil((y-width*resolution/2.0)/resolution)

式中，resolution为栅格的分辨率，width为栅格地图的宽；

步骤4-3-3，对于处于边界位置的障碍物点进行上述处理后，得到对应的栅格索引，之后根据位于当前栅格内毫米波雷达点云数据的径向速度信息，更新该栅格的动静状态，若径向速度超过预设速度阈值，则栅格为动态，否则为静态；

步骤4-3-4，将上述每个栅格的栅格类型、栅格分值、动静状态更新到每个栅格的栅格状态中，得到栅格灰度图，若栅格的灰度值越小，则说明该栅格所处位置存在障碍物的概率越大。

7.根据权利要求1所述的基于车载毫米波雷达的可通行空间检测方法，其特征在于，步骤4-4中滤波具体采用高斯滤波。

8.根据权利要求1所述的基于车载毫米波雷达的可通行空间检测方法，其特征在于，步骤5所述在PC端运行的上位机软件中显示最终可通行空间的输出结果，具体包括：

步骤5-1，遍历步骤4输出的栅格地图，检索出栅格类型为OCCUPY和FREE的栅格，将栅格类型为FREE的栅格信息输出，得到FreeSpace结果；

步骤5-2，构建包含FreeSpace结果和毫米波雷达点云信息的UDP数据包，并向PC端广播发送这些UDP数据包，PC端上位机软件接收并解析UDP数据包后，即可将最终可通行空间即FreeSpace结果实时显示出来。