CN114419573A - 动态占据栅格估计方法及装置 - Google Patents

动态占据栅格估计方法及装置 Download PDF

Info

Publication number
CN114419573A
CN114419573A CN202011085533.6A CN202011085533A CN114419573A CN 114419573 A CN114419573 A CN 114419573A CN 202011085533 A CN202011085533 A CN 202011085533A CN 114419573 A CN114419573 A CN 114419573A
Authority
CN
China
Prior art keywords
grid
point cloud
particles
moment
unit
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202011085533.6A
Other languages
English (en)
Inventor
张蓉
熊祺
张放
李晓飞
张德兆
王肖
霍舒豪
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Idriverplus Technologies Co Ltd
Original Assignee
Beijing Idriverplus Technologies Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Idriverplus Technologies Co Ltd filed Critical Beijing Idriverplus Technologies Co Ltd
Priority to CN202011085533.6A priority Critical patent/CN114419573A/zh
Priority to PCT/CN2021/123335 priority patent/WO2022078342A1/zh
Publication of CN114419573A publication Critical patent/CN114419573A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G06F18/232—Non-hierarchical techniques
    • G06F18/2321—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/20—Image preprocessing
    • G06V10/25—Determination of region of interest [ROI] or a volume of interest [VOI]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/762—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using clustering, e.g. of similar faces in social networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
    • G06V20/58—Recognition of moving objects or obstacles, e.g. vehicles or pedestrians; Recognition of traffic objects, e.g. traffic signs, traffic lights or roads

Abstract

本发明提供了一种动态占据栅格估计方法及装置，方法包括：对K时刻的激光点云数据进行处理，得到前景点云并将其映射至障碍物占据栅格地图中，得到前景点云在K时刻对应的占据栅格单元；在K+1时刻，根据K时刻的第一数量个粒子的状态值，进行预测，得到K+1时刻第二数量个粒子的预测状态值；第一数量个粒子包括第K‑1时刻的第三数量个粒子以及第四数量个初始化的新粒子；根据K+1时刻第二数量个粒子的预测状态值和前景点云在K+1时刻对应的占据栅格单元，更新K+1时刻前景点云对应的占据栅格单元，得到K+1时刻的目标占据栅格单元。由此，通过目标占据栅格单元的速度信息和位置信息，得到障碍物的位置信息和速度信息。

Description

动态占据栅格估计方法及装置
技术领域
本发明涉及数据处理领域，尤其涉及一种动态占据栅格估计方法及装置。
背景技术
近年来，自动驾驶车辆在智能交通出行、物流配送、清洁作业等多场景中，构建了智慧城市生活的应用示范。自动驾驶感知方案最重要的障碍物就是正确检测出环境中的障碍物并得到其运动状态。激光雷达是自动驾驶汽车感知环境的重要传感器，具有精度高、抗干扰等特点。激光雷达主要能够提供障碍物的位置信息和反射率信息，但无法直接输出障碍物的速度信息。
目前从激光雷达点云中获取障碍物的速度信息采用的方法是：将激光点云投影到二维平面中进行聚类，获得聚类后的障碍物位置信息，利用经典的卡尔曼滤波等算法获取障碍物的运动状态。
现有的激光点云障碍物速度估计算法强依赖于对于激光点云的聚类效果。如果聚类出现较多的过分割或者欠分割的现象，将会导致障碍物的匹配出现问题，从而导致对障碍物的速度估计出现偏差。
发明内容
本发明实施例的目的是提供一种动态占据栅格估计方法及装置，以解决现有技术中的无法直接输出障碍物的速度信息或者对障碍物速度估计存在偏差的问题。
为解决上述问题，第一方面，本发明提供了一种动态占据栅格估计方法，所述方法包括：
对K时刻的激光点云数据进行处理，得到前景点云；所述前景点云为非地面点云；
将所述前景点云映射至障碍物占据栅格地图中，得到前景点云在K时刻对应的占据栅格单元；
根据前景点云在K时刻对应的占据栅格单元，确定前景点云在K+1时刻对应的占据栅格单元；
在K+1时刻，根据K时刻的第一数量个粒子的状态值，进行预测，得到K+1时刻第二数量个粒子的预测状态值；所述第一数量个粒子包括第K-1时刻的第三数量个粒子以及第四数量个初始化的新粒子；所述第三数量和所述第四数量之和为第一数量；所述状态值包括粒子位置和粒子速度；
根据K+1时刻第二数量个粒子的预测状态值和前景点云在K+1时刻对应的占据栅格单元，更新K+1时刻前景点云对应的占据栅格单元，得到K+1时刻的目标占据栅格单元；所述目标占据栅格单元包括速度信息和位置信息；所述第二数量个粒子为处于K+1时刻对应的占据栅格单元中的粒子。
在一种可能的实现方式中，所述方法之前还包括：
以地面平面为基准，将激光点云数据划分至预设的栅格中；
计算所述栅格地图中的任一栅格中，高度最大点和高度最小点的高度差值，得到第一高度差值；
当所述第一高度差值小于预设的第一高度阈值时，确定所述任一栅格为第一栅格，并确定所述第一栅格内的点云为疑似地面点；
根据全部所述第一栅格的疑似地面点，拟合得到地面模型；
计算所述地面模型中的每个点至所述地面平面的高度差值，得到第二高度差值；
确定所述第二高度差值不小于预设的第二高度阈值时的点构成前景点云。
在一种可能的实现方式中，所述方法之前还包括：
对第一时刻的激光点云数据进行处理，得到第一时刻的前景点云；
将所述第一时刻的前景点云映射至障碍物占据栅格地图中，得到第一时刻的前景点云对应的占据栅格单元；
根据前景点云在第一时刻对应的占据栅格单元，确定前景点云在占据栅格单元的占据概率；
初始化第一数量个粒子；
根据所述占据概率，将所述第一数量个粒子设置在所述第一时刻对应的占据栅格单元中。
在一种可能的实现方式中，所述根据K+1时刻第四数量个粒子的预测状态值和前景点云在K+1时刻对应的占据栅格单元，更新K+1时刻前景点云对应的占据栅格单元，得到K+1时刻的目标占据栅格单元，具体包括：
确定K+1时刻第一数量个粒子所在的占据栅格单元与前景点云在K+1时刻对应的占据栅格单元的重合占据栅格单元；
确定所述重合占据栅格单元中粒子的数量为第二数量；
初始化第五数量个粒子；所述第二数量与所述第五数量之和为第一数量；
根据所述重合占据栅格单元的数量，将所述第三数量个粒子设置在所述重合占据栅格单元中；所述重合占据栅格单元为所述目标占据栅格。
在一种可能的实现方式中，所述方法之后还包括：
为所述第三数量个粒子设置随机状态值；
根据所述第二数量个粒子的状态值和所述第三数量个粒子的随机状态值，得到所述目标占据栅格中的各栅格的位置信息和速度信息。
第二方面，本发明提供了一种动态占据栅格估计装置，所述装置包括：
处理单元，所述处理单元用于对K时刻的激光点云数据进行处理，得到前景点云；所述前景点云为非地面点云；
映射单元，所述映射单元用于将所述前景点云映射至障碍物占据栅格地图中，得到前景点云在K时刻对应的占据栅格单元；
确定单元，所述确定单元用于根据前景点云在K时刻对应的占据栅格单元，确定前景点云在K+1时刻对应的占据栅格单元；
预测单元，所述预测单元用于在K+1时刻，根据K时刻的第一数量个粒子的状态值，进行预测，得到K+1时刻第二数量个粒子的预测状态值；所述第一数量个粒子包括第K-1时刻的第三数量个粒子以及第四数量个初始化的新粒子；所述第三数量和所述第四数量之和为第一数量；所述状态值包括粒子位置和粒子速度；
更新单元，所述更新单元用于根据K+1时刻第二数量个粒子的预测状态值和前景点云在K+1时刻对应的占据栅格单元，更新K+1时刻前景点云对应的占据栅格单元，得到K+1时刻的目标占据栅格单元；所述目标占据栅格单元包括速度信息和位置信息；所述第二数量个粒子为处于K+1时刻对应的占据栅格单元中的粒子。
在一种可能的实现方式中，所述装置还包括：
划分单元，所述划分单元用于以地面平面为基准，将激光点云数据划分至预设的栅格中；
计算单元，所述计算单元用于计算所述栅格地图中的任一栅格中，高度最大点和高度最小点的高度差值，得到第一高度差值；
所述确定单元还用于当所述第一高度差值小于预设的第一高度阈值时，确定所述任一栅格为第一栅格，并确定所述第一栅格内的点云为疑似地面点；
拟合单元，所述拟合单元用于根据全部所述第一栅格的疑似地面点，拟合得到地面模型；
所述计算单元还用于计算所述地面模型中的每个点至所述地面平面的高度差值，得到第二高度差值；
所述确定单元还用于确定所述第二高度差值不小于预设的第二高度阈值时的点构成前景点云。
在一种可能的实现方式中，所述处理单元还用于，对第一时刻的激光点云数据进行处理，得到第一时刻的前景点云；
所述映射单元还用于，将所述第一时刻的前景点云映射至障碍物占据栅格地图中，得到第一时刻的前景点云对应的占据栅格单元；
所述确定单元还用于，根据前景点云在第一时刻对应的占据栅格单元，确定前景点云在占据栅格单元的占据概率；
初始化单元，所述初始化单元用于初始化第一数量个粒子；
设置单元，所述设置单元用于根据所述占据概率，将所述第一数量个粒子设置在所述第一时刻对应的占据栅格单元中。
在一种可能的实现方式中，所述更新单元具体用于：
确定K+1时刻第一数量个粒子所在的占据栅格单元与前景点云在K+1时刻对应的占据栅格单元的重合占据栅格单元；
确定所述重合占据栅格单元中粒子的数量为第二数量；
初始化第五数量个粒子；所述第二数量与所述第五数量之和为第一数量；
根据所述重合占据栅格单元的数量，将所述第三数量个粒子设置在所述重合占据栅格单元中；所述重合占据栅格单元为所述目标占据栅格。
在一种可能的实现方式中，所述设置单元还用于，为所述第三数量个粒子设置随机状态值；
所述确定单元还用于，根据所述第二数量个粒子的状态值和所述第三数量个粒子的随机状态值，得到所述目标占据栅格中的各栅格的位置信息和速度信息。
第三方面，本发明提供了一种设备，包括存储器和处理器，存储器用于存储程序，处理器用于执行第一方面任一的方法。
第四方面，本发明提供了一种包含指令的计算机程序产品，当计算机程序产品在计算机上运行时，使得计算机执行如第一方面任一的方法。
第五方面，本发明提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现如第一方面任一的方法。
通过应用本申请提供的动态占据栅格估计方法及装置，不依赖于对于激光点云的聚类效果，而是通过将激光点云进行二维栅格映射，然后采用基于粒子滤波的方法，输出动态占据栅格信息，包括自车周围一定范围内的障碍物所占据的栅格信息，以及每个占据栅格单元估计出来的速度信息，该速度信息可以直接用于决策规划模块进行障碍物避障计算，也便于后续聚类时，直接利用占据栅格的速度信息，从而提高聚类效果。
附图说明
图1为本发明实施例一提供的动态占据栅格估计方法流程示意图；
图2为本发明实施例一提供的前景点云对应的占据栅格单元示意图；
图3为本发明实施例一提供的前景点云映射至障碍物占据栅格地图的示意图；
图4为本发明实施例一提供的根据k时刻的粒子状态得到K+1时刻的粒子预测状态后，根据K+1时刻的粒子预测状态及前景点云对应的占据栅格单元结合得到目标占据栅格单元的过程的示意图；
图5为本发明实施例二提供的动态占据栅格估计装置的一个结构示意图；
图6为本发明实施例二提供的动态占据栅格估计装置的又一个结构示意图。
具体实施方式
下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为便于描述，附图中仅示出了与有关发明相关的部分。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
图1为本发明实施例一提供的动态估计栅格估计方法流程示意图，该方法应用在安装有激光雷达的终端上，比如安装有激光雷达的无人驾驶车辆，本申请的执行主体为具有计算功能的终端、服务器或者处理器。本申请以将该方法应用在无人驾驶车辆为例进行说明，当将该方法应用在无人驾驶车辆时，该方法的执行主体为自动驾驶车辆控制单元(Automated Vehicle Control Unit，AVCU)，即无人驾驶车辆的中央处理器相当于无人驾驶车辆的“大脑”。如图1所示，本申请包括以下步骤：
步骤101，对K时刻的激光点云数据进行处理，得到前景点云。
其中，前景点云为去除了地面点云后，障碍物所对应的点云。下面对如何得到前景点云进行说明。
在一个示例中，首先，获取原始激光点云数据和定位数据。然后，当定位数据有效时，根据定位数据，获取地图数据。接着，根据地图数据，确定地图中的第一感兴趣区域(region of interest，ROI)。最后，确定原始激光点云中，处于第一ROI内的点云为待分割点云。
具体的，车辆上安装有激光雷达、全球定位系统(Global Positioning System,GPS)、惯性测量单元(Inertial measurement unit，IMU)、视觉模块等传感器，可以通过单线或多线激光雷达获取原始激光点云数据。
其中，随着车辆的位置变化，通过GPS获取定位数据，获取到的GPS发送的多个定位数据包后，对定位数据包进行解析后，会得到每个定位数据包的端口号、数据包长度、标志位等，通过数据包长度或者标志位，可以判断定位数据是否有效，比如，数据包的长度、标志位等都符合要求，则判定定位数据有效。
服务器中预存有预先构建的高精地图，高精地图可以根据经纬度，划分为多个地图数据文件并存储，在每个地图数据文件中，可以将边界进行划分，比如一个文件中包括多个边界，可以将每个边界确定为一个ROI区域。当定位数据有效时，车辆可以向服务器发送包括定位数据的请求消息，以获取定位数据对应的地图数据文件，比如，定位数据中包括车辆当前所处的经纬度，可以向服务器发送包括车辆当前经纬度的请求消息，以获取到当前经纬度对应的地图数据文件。车辆接收到地图数据文件后，解析得到地图数据，并根据地图数据，确定车辆当前经纬度对应的地图中的第一ROI区域，并将处于第一ROI区域内的原始激光点云确定为待分割点云。
其中，示例而非限定，对于在封闭园区内进行作业的无人车辆，可以预先从服务器中下载该封闭园区的高精地图，并根据定位数据，查询得到对应的地图数据文件，并解析地图数据文件后，得到地图数据。
在另一个示例中，当定位数据无效时，或者定位数据有效，地图数据无效时，根据原始激光点云，确定车辆的位置；根据车辆的位置，确定第二ROI；确定原始激光点云中，处于第二ROI内的点云为待分割点云。
具体的，接上例，当数据包的长度不符合预设的长度阈值，则判定数据包无效，或者数据包的标志位为无效标志位，则判定数据包无效。或者定位数据有效，但是根据定位数据向服务器发送查询请求，并未查找到对应的地图数据文件时，则说明地图数据无效。此时，根据原始激光点云，确定车辆的位置。随后，根据车辆坐标系，在车辆坐标系中设定第二ROI。比如，可以在车辆坐标系的右侧，将5m*10m的矩形，确定为第二ROI。并将原始激光点云中处于第二ROI中的点云，确定为待分割点云。
其中，上述的原始激光点云是处于车辆坐标系的原始激光点云，可以根据车辆与激光雷达的安装位置关系，确定激光雷达在车辆坐标系的位置，并将得到的激光点云转换为车辆坐标系的原始激光点云。定位数据是进行坐标转换后，车辆坐标系的定位数据。其中，可以以车辆的质心作为车辆坐标系的原点，自车面向前方时，指向车辆右侧作为x轴，将自车面向前方时，在行驶的方向上为y轴。
进一步的，可以根据下述步骤得到前景点云。
首先，以地面平面为基准，对于当前时刻的单帧原始激光点云，在得到待分割点云后，可以将待分割点云划分至预设的栅格地图中。然后，计算栅格地图中的任一栅格中，高度最大点和高度最小点的高度差值，得到第一高度差值。接着，当第一高度差值小于预设的第一高度阈值时，确定任一栅格为第一栅格，并确定第一栅格内的点云为疑似地面点。接着，根据全部第一栅格的疑似地面点，根据平面拟合算法，拟合得到地面模型。接着，计算地面模型中的每个点至地面平面的高度差值，得到第二高度差值。最后，确定第二高度差值不小于预设的第二高度阈值时的点为前景点云。同时，还可以确定第二高度差值小于预设的第二高度阈值时的点为背景点云。
其中，栅格的大小可以根据多次计算的经验值得到，待分割点云是多个点的集合，每个点具有位置信息，栅格也具有位置，栅格的位置可以以栅格的左上方的角的位置表示，在坐标系统一的前提下，可以将待分割点云映射至栅格中，从而得到分布在栅格中的待分割点云。可以根据车辆的通行能力，设定第一高度阈值。可以根据车辆的类型，设定第二高度阈值，比如，车辆为小型清扫车和车辆为物流车或者为街道上行驶的小汽车时，第二高度阈值是不同的。平面拟合算法包括但不限于最小二乘法，随机采样一致性(Random SampleConsensus，RANSAC)算法。
步骤102，将前景点云映射至障碍物占据栅格地图中，得到前景点云在K时刻对应的占据栅格单元。
具体的，得到三维的前景点云后，需要将其降维至二维的前景点云并映射至障碍物占据栅格地图，参见图2，其中，可以以车辆的质心作为车辆坐标系的原点，自车面向前方时，指向车辆右侧作为x轴，将自车面向前方时，在行驶的方向上为y轴，建立障碍物占据栅格地图，并可以按照0.1m*0.1m的间隔将障碍物占据栅格地图进行划分，将前景点云投影至障碍物占据栅格地图后，得到的障碍物占据栅格单元，即前景点云占据栅格单元，或者也可以称为障碍物占据栅格单元。参见图2，假如障碍物为车辆，则图2中的①～⑧即为障碍物车辆所占据的栅格单元。
进一步的，继续接图2，可以根据传感器模型，对前景点云映射至障碍物占据栅格地图后的障碍物占据栅格地图中的栅格单元，进行状态估计，并以不同的灰度值进行标记。
在一个示例中，参见图3，可以将障碍物占据概率最高的部分赋占据概率0.9。示例而非限定，为了进行更形象的显示，可以在图3中通过灰度进行不同概率的区分，比如，可以将占据概率0.9对应的栅格单元标记为灰度值255，可以将占据概率最低的部分赋予占据概率0.1，并标记为灰度值0，将占据概率不确定的部分赋予占据概率0.5，并标记为灰度值122。
在另一个示例中，可以对占据概率不确定的部分继续进行划分，并以不同的灰度值进行标记。
其中，此处的占据概率是根据反传感器距离模型(inverse sensor model)计算得到的，可以是0-1之间的某一个值。
步骤103，根据前景点云在K时刻对应的占据栅格单元，确定前景点云在K+1时刻对应的占据栅格单元。
具体的，可以根据步骤101-步骤102的方法，得到K+1时刻前景点云对应的占据栅格单元。
步骤104，在K+1时刻，根据K时刻的第一数量个粒子的状态值，进行预测，得到K+1时刻第二数量个粒子的预测状态值；第一数量个粒子包括第K-1时刻的第三数量个粒子以及第四数量个初始化的新粒子；第三数量和第四数量之和为第一数量；状态值包括粒子位置和粒子速度。
具体的，粒子滤波是指通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程。这里的一组随机样本即称为粒子。
在完成了激光点云数据的预处理，得到前景点云所对应的占据栅格单元后，即可以获得当前时刻最新的障碍物占据栅格状态，用于粒子滤波的状态值更新。
参见图4，在k时刻，前景点云对应的占据栅格单元占拥有一定数量的粒子，可以称为第一数量个粒子，此处与图3一样，可以以占据概率表示不同的占据栅格单元，并为了进行区分，可以以灰度值对占据概率进行不同占据概率的区分，示例而非限定，可以为255表示前景点云对应的占据栅格单元。每个粒子的状态值包括位置估计和速度估计(箭头表示)两种。
具体的，继续接上述，参见图4，在k+1时刻，滤波器可以根据k时刻的第一数量个粒子的状态值，进行预测，得到K+1时刻第一数量个粒子的预测状态值。预测状态值包括每个粒子的预测速度和预测位置。
其中，粒子的状态进行预测之后，如果粒子存在速度，则位置会变化，速度超过一定值的粒子，预测之后会落入周围其它的占据栅格单元之中，这样，原来的占据栅格单元所持有的粒子数量会变小，原来的栅格被占据的概率也会相应降低，比如，如以为灰度图表示的话，会从灰度值255的黑色，变为较低灰度值的灰色。
其中，在初始化状态，对第一数量个粒子如何设置在占据栅格单元进行描述。
具体的，对第一时刻的激光点云数据进行处理，得到第一时刻的前景点云；将第一时刻的前景点云映射至障碍物占据栅格地图中，得到第一时刻的前景点云对应的占据栅格单元；根据前景点云在第一时刻对应的占据栅格单元，确定前景点云在占据栅格单元的占据概率；初始化第一数量个粒子；根据占据概率，将第一数量个粒子设置在第一时刻对应的占据栅格单元中。
其中，此处的第一时刻，可以是初始时刻。
步骤105，根据K+1时刻第二数量个粒子的预测状态值和前景点云在K+1时刻对应的占据栅格单元，更新K+1时刻前景点云对应的占据栅格单元，得到K+1时刻的目标占据栅格单元；目标占据栅格单元包括速度信息和位置信息；第二数量个粒子为处于K+1时刻对应的占据栅格单元中的粒子。
具体的，目标占据栅格单元包括速度信息和位置信息。
下面对步骤105进行具体的说明。
参见图4，首先，确定K+1时刻第一数量个粒子所在的占据栅格单元与前景点云在K+1时刻对应的占据栅格单元的重合占据栅格单元。然后，确定重合占据栅格单元中粒子的数量为第二数量。接着，初始化第三数量个粒子；第三数量与第二数量之和为第一数量。最后，根据重合占据栅格单元的数量，将第五数量个粒子设置在重合占据栅格单元中；重合占据栅格单元为目标占据栅格。参见图4，可以将K+1时刻的第一数量个粒子所在的占据栅格单元与前景点云在K+1时刻对应的占据栅格单元的概率进行叠加，从而得到重合占据栅格单元。
进一步的，在步骤105之后还包括：
为第三数量个粒子设置随机状态值；
根据第二数量个粒子的状态值和所述第三数量个粒子的随机状态值，得到所述目标占据栅格中的各栅格的位置信息和速度信息。
具体的，对于重合占据栅格单元，可以统计每个栅格中的粒子的状态值，得到该栅格的粒子的速度信息，并根据每个栅格的粒子的速度信息，统计障碍物的速度信息，并将障碍物的速度信息，作为目标占据栅格的速度信息。此处的速度信息，可以是x方向的速度和/或y方向的速度和/或合速度。其中，此处的和速度，是根据x方向的速度和y方向的速度所确定。
进一步的，也可以统计障碍物的位置信息，即为重合占据栅格单元的位置。
K和K+1仅仅是为了对不同时刻进行区分，比如，可以以K表示当前时刻，则K+1为当前时刻的下一时刻，也可以以其他字母进行标记，本申请对此并不限定。
由此，通过应用本申请提供的动态占据栅格估计方法，不依赖于对于激光点云的聚类效果，而是通过将激光点云进行二维栅格映射，然后采用基于粒子滤波的方法，输出动态占据栅格信息，包括自车周围一定范围内的障碍物所占据的栅格信息，以及每个占据栅格单元估计出来的速度信息，该速度信息可以直接用于决策规划模块进行障碍物避障计算，也便于后续聚类时，直接利用占据栅格的速度信息，从而提高聚类效果。
图5为本发明实施例二提供的动态占据栅格估计装置的一个结构示意图，如图5所示，该动态占据栅格估计装置500包括：处理单元501、映射单元502、确定单元503、预测单元504和更新单元505。
处理单元501用于对K时刻的激光点云数据进行处理，得到前景点云；所述前景点云为非地面点云；
映射单元502用于将前景点云映射至障碍物占据栅格地图中，得到前景点云在K时刻对应的占据栅格单元；
确定单元503用于根据前景点云在K时刻对应的占据栅格单元，确定前景点云在K+1时刻对应的占据栅格单元。
预测单元504用于在K+1时刻，根据K时刻的第一数量个粒子的状态值，进行预测，得到K+1时刻第二数量个粒子的预测状态值；第一数量个粒子包括第K-1时刻的第三数量个粒子以及第四数量个初始化的新粒子；第三数量和第四数量之和为第一数量；状态值包括粒子位置和粒子速度。
更新单元505用于根据K+1时刻第二数量个粒子的预测状态值和前景点云在K+1时刻对应的占据栅格单元，更新K+1时刻前景点云对应的占据栅格单元，得到K+1时刻的目标占据栅格单元；目标占据栅格单元包括速度信息和位置信息；第二数量个粒子为处于K+1时刻对应的占据栅格单元中的粒子。
进一步的，参见图6，图6为本发明实施例二提供的动态占据栅格估计装置的又一个结构示意图，该动态占据栅格估计装置600还包括：划分单元601、计算单元602和拟合单元603。
划分单元601用于以地面平面为基准，将激光点云数据划分至预设的栅格中。
计算单元602用于计算栅格地图中的任一栅格中，高度最大点和高度最小点的高度差值，得到第一高度差值。
确定单元503还用于当第一高度差值小于预设的第一高度阈值时，确定任一栅格为第一栅格，并确定第一栅格内的点云为疑似地面点。
拟合单元603用于根据全部第一栅格的疑似地面点，拟合得到地面模型。
计算单元602还用于计算地面模型中的每个点至地面平面的高度差值，得到第二高度差值。
确定单元503还用于确定第二高度差值不小于预设的第二高度阈值时的点构成前景点云。
进一步的，参见图6，该装置还包括初始化单元604和设置单元605。
处理单元501还用于，对第一时刻的激光点云数据进行处理，得到第一时刻的前景点云。
映射单元502还用于，将第一时刻的前景点云映射至障碍物占据栅格地图中，得到第一时刻的前景点云对应的占据栅格单元。
确定单元503还用于，根据前景点云在第一时刻对应的占据栅格单元，确定前景点云在占据栅格单元的占据概率。
初始化单元604用于初始化第一数量个粒子。
设置单元605用于根据占据概率，将第一数量个粒子设置在第一时刻对应的占据栅格单元中。
进一步的，更新单元505具体用于：
确定K+1时刻第一数量个粒子所在的占据栅格单元与前景点云在K+1时刻对应的占据栅格单元的重合占据栅格单元；
确定重合占据栅格单元中粒子的数量为第二数量；
初始化第五数量个粒子；第二数量与第五数量之和为第一数量；
根据重合占据栅格单元的数量，将第三数量个粒子设置在重合占据栅格单元中；重合占据栅格单元为目标占据栅格。
进一步的，设置单元604还用于，为第三数量个粒子设置随机状态值。
确定单元503还用于，根据第二数量个粒子的状态值和第三数量个粒子的随机状态值，得到目标占据栅格中的各栅格的位置信息和速度信息。
由此，通过应用本申请提供的动态占据栅格估计装置，不依赖于对于激光点云的聚类效果，而是通过将激光点云进行二维栅格映射，然后采用基于粒子滤波的方法，输出动态占据栅格信息，包括自车周围一定范围内的障碍物所占据的栅格信息，以及每个占据栅格单元估计出来的速度信息，该速度信息可以直接用于决策规划模块进行障碍物避障计算，也便于后续聚类时，直接利用占据栅格的速度信息，从而提高聚类效果。
发明实施例三提供了一种设备，包括存储器和处理器，存储器用于存储程序，存储器可通过总线与处理器连接。存储器可以是非易失存储器，例如硬盘驱动器和闪存，存储器中存储有软件程序和设备驱动程序。软件程序能够执行本发明实施例提供的上述方法的各种功能；设备驱动程序可以是网络和接口驱动程序。处理器用于执行软件程序，该软件程序被执行时，能够实现本发明实施例一提供的方法。
本发明实施例四提供了一种包含指令的计算机程序产品，当计算机程序产品在计算机上运行时，使得计算机执行本发明实施例一提供的方法。
本发明实施例五提供了一种计算机可读存储介质，计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现本发明实施例一提供的方法。
专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种动态占据栅格估计方法，其特征在于，所述方法包括：
对K时刻的激光点云数据进行处理，得到前景点云；所述前景点云为非地面点云；
将所述前景点云映射至障碍物占据栅格地图中，得到前景点云在K时刻对应的占据栅格单元；
根据前景点云在K时刻对应的占据栅格单元，确定前景点云在K+1时刻对应的占据栅格单元；
在K+1时刻，根据K时刻的第一数量个粒子的状态值，进行预测，得到K+1时刻第二数量个粒子的预测状态值；所述第一数量个粒子包括第K-1时刻的第三数量个粒子以及第四数量个初始化的新粒子；所述第三数量和所述第四数量之和为第一数量；所述状态值包括粒子位置和粒子速度；
根据K+1时刻第二数量个粒子的预测状态值和前景点云在K+1时刻对应的占据栅格单元，更新K+1时刻前景点云对应的占据栅格单元，得到K+1时刻的目标占据栅格单元；所述目标占据栅格单元包括速度信息和位置信息；所述第二数量个粒子为处于K+1时刻对应的占据栅格单元中的粒子。
2.根据权利要求1所述的方法，其特征在于，所述方法之前还包括：
以地面平面为基准，将激光点云数据划分至预设的栅格中；
计算所述栅格地图中的任一栅格中，高度最大点和高度最小点的高度差值，得到第一高度差值；
当所述第一高度差值小于预设的第一高度阈值时，确定所述任一栅格为第一栅格，并确定所述第一栅格内的点云为疑似地面点；
根据全部所述第一栅格的疑似地面点，拟合得到地面模型；
计算所述地面模型中的每个点至所述地面平面的高度差值，得到第二高度差值；
确定所述第二高度差值不小于预设的第二高度阈值时的点构成前景点云。
3.根据权利要求1所述的方法，其特征在于，所述方法之前还包括：
对第一时刻的激光点云数据进行处理，得到第一时刻的前景点云；
将所述第一时刻的前景点云映射至障碍物占据栅格地图中，得到第一时刻的前景点云对应的占据栅格单元；
根据前景点云在第一时刻对应的占据栅格单元，确定前景点云在占据栅格单元的占据概率；
初始化第一数量个粒子；
根据所述占据概率，将所述第一数量个粒子设置在所述第一时刻对应的占据栅格单元中。
4.根据权利要求1所述的方法，其特征在于，所述根据K+1时刻第四数量个粒子的预测状态值和前景点云在K+1时刻对应的占据栅格单元，更新K+1时刻前景点云对应的占据栅格单元，得到K+1时刻的目标占据栅格单元，具体包括：
确定K+1时刻第一数量个粒子所在的占据栅格单元与前景点云在K+1时刻对应的占据栅格单元的重合占据栅格单元；
确定所述重合占据栅格单元中粒子的数量为第二数量；
初始化第五数量个粒子；所述第二数量与所述第五数量之和为第一数量；
根据所述重合占据栅格单元的数量，将所述第三数量个粒子设置在所述重合占据栅格单元中；所述重合占据栅格单元为所述目标占据栅格。
5.根据权利要求4所述的方法，其特征在于，所述方法之后还包括：
为所述第三数量个粒子设置随机状态值；
根据所述第二数量个粒子的状态值和所述第三数量个粒子的随机状态值，得到所述目标占据栅格中的各栅格的位置信息和速度信息。
6.一种动态占据栅格估计装置，其特征在于，所述装置包括：
处理单元，所述处理单元用于对K时刻的激光点云数据进行处理，得到前景点云；所述前景点云为非地面点云；
映射单元，所述映射单元用于将所述前景点云映射至障碍物占据栅格地图中，得到前景点云在K时刻对应的占据栅格单元；
确定单元，所述确定单元用于根据前景点云在K时刻对应的占据栅格单元，确定前景点云在K+1时刻对应的占据栅格单元；
预测单元，所述预测单元用于在K+1时刻，根据K时刻的第一数量个粒子的状态值，进行预测，得到K+1时刻第二数量个粒子的预测状态值；所述第一数量个粒子包括第K-1时刻的第三数量个粒子以及第四数量个初始化的新粒子；所述第三数量和所述第四数量之和为第一数量；所述状态值包括粒子位置和粒子速度；
更新单元，所述更新单元用于根据K+1时刻第二数量个粒子的预测状态值和前景点云在K+1时刻对应的占据栅格单元，更新K+1时刻前景点云对应的占据栅格单元，得到K+1时刻的目标占据栅格单元；所述目标占据栅格单元包括速度信息和位置信息；所述第二数量个粒子为处于K+1时刻对应的占据栅格单元中的粒子。
7.根据权利要求6所述的装置，其特征在于，所述装置还包括：
划分单元，所述划分单元用于以地面平面为基准，将激光点云数据划分至预设的栅格中；
计算单元，所述计算单元用于计算所述栅格地图中的任一栅格中，高度最大点和高度最小点的高度差值，得到第一高度差值；
所述确定单元还用于当所述第一高度差值小于预设的第一高度阈值时，确定所述任一栅格为第一栅格，并确定所述第一栅格内的点云为疑似地面点；
拟合单元，所述拟合单元用于根据全部所述第一栅格的疑似地面点，拟合得到地面模型；
所述计算单元还用于计算所述地面模型中的每个点至所述地面平面的高度差值，得到第二高度差值；
所述确定单元还用于确定所述第二高度差值不小于预设的第二高度阈值时的点构成前景点云。
8.根据权利要求6所述的装置，其特征在于，
所述处理单元还用于，对第一时刻的激光点云数据进行处理，得到第一时刻的前景点云；
所述映射单元还用于，将所述第一时刻的前景点云映射至障碍物占据栅格地图中，得到第一时刻的前景点云对应的占据栅格单元；
所述确定单元还用于，根据前景点云在第一时刻对应的占据栅格单元，确定前景点云在占据栅格单元的占据概率；
初始化单元，所述初始化单元用于初始化第一数量个粒子；
设置单元，所述设置单元用于根据所述占据概率，将所述第一数量个粒子设置在所述第一时刻对应的占据栅格单元中。
9.根据权利要求6所述的装置，其特征在于，所述更新单元具体用于：
确定K+1时刻第一数量个粒子所在的占据栅格单元与前景点云在K+1时刻对应的占据栅格单元的重合占据栅格单元；
确定所述重合占据栅格单元中粒子的数量为第二数量；
初始化第五数量个粒子；所述第二数量与所述第五数量之和为第一数量；
根据所述重合占据栅格单元的数量，将所述第三数量个粒子设置在所述重合占据栅格单元中；所述重合占据栅格单元为所述目标占据栅格。
10.根据权利要求9所述的装置，其特征在于，所述设置单元还用于，为所述第三数量个粒子设置随机状态值；
所述确定单元还用于，根据所述第二数量个粒子的状态值和所述第三数量个粒子的随机状态值，得到所述目标占据栅格中的各栅格的位置信息和速度信息。
CN202011085533.6A 2020-10-12 2020-10-12 动态占据栅格估计方法及装置 Pending CN114419573A (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202011085533.6A CN114419573A (zh) 2020-10-12 2020-10-12 动态占据栅格估计方法及装置
PCT/CN2021/123335 WO2022078342A1 (zh) 2020-10-12 2021-10-12 动态占据栅格估计方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011085533.6A CN114419573A (zh) 2020-10-12 2020-10-12 动态占据栅格估计方法及装置

Publications (1)

Publication Number Publication Date
CN114419573A true CN114419573A (zh) 2022-04-29

Family

ID=81207719

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011085533.6A Pending CN114419573A (zh) 2020-10-12 2020-10-12 动态占据栅格估计方法及装置

Country Status (2)

Country Link
CN (1) CN114419573A (zh)
WO (1) WO2022078342A1 (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116147637B (zh) * 2023-04-24 2023-06-30 福勤智能科技（昆山）有限公司 占用栅格地图的生成方法、装置、设备及存储介质
CN116661468B (zh) * 2023-08-01 2024-04-12 深圳市普渡科技有限公司 障碍物检测方法、机器人及计算机可读存储介质

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
IN201641039332A (zh) * 2016-11-18 2018-05-25
US10838067B2 (en) * 2017-01-17 2020-11-17 Aptiv Technologies Limited Object detection system
US11555927B2 (en) * 2019-02-05 2023-01-17 Honda Motor Co., Ltd. System and method for providing online multi-LiDAR dynamic occupancy mapping
CN110705458B (zh) * 2019-09-29 2022-06-28 北京智行者科技有限公司 边界检测方法及装置
CN111591288B (zh) * 2020-03-31 2021-09-10 北京智行者科技有限公司 基于距离变换图的碰撞检测方法及装置

Also Published As

Publication number Publication date
WO2022078342A1 (zh) 2022-04-21

Similar Documents

Publication Publication Date Title
CN109143207B (zh) 激光雷达内参精度验证方法、装置、设备及介质
CN110988912B (zh) 自动驾驶车辆的道路目标与距离检测方法、系统、装置
CN110687549B (zh) 障碍物检测方法和装置
CN112101092A (zh) 自动驾驶环境感知方法及系统
CN110705458B (zh) 边界检测方法及装置
CN112380317B (zh) 高精地图更新方法、装置、电子设备和存储介质
WO2019007263A1 (zh) 车载传感器的外部参数标定的方法和设备
CN110674705B (zh) 基于多线激光雷达的小型障碍物检测方法及装置
CN109435955B (zh) 一种自动驾驶系统性能评估方法、装置、设备及存储介质
GB2558716A (en) Road sign recognition
US20150233720A1 (en) Geographic feature-based localization with feature weighting
CN110632617B (zh) 一种激光雷达点云数据处理的方法及装置
US20210389133A1 (en) Systems and methods for deriving path-prior data using collected trajectories
CN112740225B (zh) 一种路面要素确定方法及装置
CN110673107A (zh) 基于多线激光雷达的路沿检测方法及装置
WO2022078342A1 (zh) 动态占据栅格估计方法及装置
WO2024012211A1 (zh) 自动驾驶环境感知方法、介质及车辆
CN114485698B (zh) 一种交叉路口引导线生成方法及系统
CN114241448A (zh) 障碍物航向角的获取方法、装置、电子设备及车辆
CN113822944A (zh) 一种外参标定方法、装置、电子设备及存储介质
US11747454B2 (en) Granularity-flexible existence-based object detection
US11315417B2 (en) Method, device and system for wrong-way driver detection
US10532750B2 (en) Method, device and system for wrong-way driver detection
CN115359332A (zh) 基于车路协同的数据融合方法、装置、电子设备及系统
CN115309773A (zh) 定位方法、装置、设备、介质和车辆

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB02 Change of applicant information

Address after: B4-006, maker Plaza, 338 East Street, Huilongguan town, Changping District, Beijing 100096

Applicant after: Beijing Idriverplus Technology Co.,Ltd.

Address before: B4-006, maker Plaza, 338 East Street, Huilongguan town, Changping District, Beijing 100096

Applicant before: Beijing Idriverplus Technology Co.,Ltd.

CB02 Change of applicant information