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

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

Info

Publication number
WO2022078342A1
WO2022078342A1 PCT/CN2021/123335 CN2021123335W WO2022078342A1 WO 2022078342 A1 WO2022078342 A1 WO 2022078342A1 CN 2021123335 W CN2021123335 W CN 2021123335W WO 2022078342 A1 WO2022078342 A1 WO 2022078342A1
Authority
WO
WIPO (PCT)
Prior art keywords
grid
point cloud
occupied
particles
unit
Prior art date
Application number
PCT/CN2021/123335
Other languages
English (en)
French (fr)
Inventor
张蓉
熊祺
张放
李晓飞
张德兆
王肖
霍舒豪
Original Assignee
北京智行者科技有限公司
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 北京智行者科技有限公司 filed Critical 北京智行者科技有限公司
Publication of WO2022078342A1 publication Critical patent/WO2022078342A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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
    • 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

Definitions

  • the present application relates to the field of data processing, and in particular, to a method and device for dynamic occupancy grid estimation.
  • Lidar is an important sensor for autonomous vehicles to perceive the environment, and has the characteristics of high precision and anti-interference. Lidar can mainly provide the position information and reflectivity information of obstacles, but cannot directly output the speed information of obstacles.
  • the method used to obtain the speed information of obstacles from the lidar point cloud is: project the laser point cloud into a two-dimensional plane for clustering, obtain the position information of the obstacles after the clustering, and use the classical Kalman filter and other algorithms Get the motion state of the obstacle.
  • the existing laser point cloud obstacle velocity estimation algorithms strongly depend on the clustering effect of the laser point cloud. If there are many over-segmentation or under-segmentation phenomena in the cluster, it will cause problems in the matching of obstacles, which will lead to deviations in the speed estimation of obstacles.
  • the purpose of the embodiments of the present application is to provide a method and device for dynamic occupancy grid estimation, so as to solve the problem in the prior art that the speed information of obstacles cannot be directly output or there is a deviation in the speed estimation of obstacles.
  • the present application provides a dynamic occupancy grid estimation method, the method includes:
  • the laser point cloud data at time K is processed to obtain a foreground point cloud;
  • the foreground point cloud is a non-ground point cloud;
  • the foreground point cloud is mapped to the obstacle occupied grid map, and the corresponding occupied grid unit of the foreground point cloud at time K is obtained;
  • the first number of particles includes time K-1
  • the third number of particles and the fourth number of initialized new particles the sum of the third number and the fourth number is the first number
  • the state value includes particle position and particle velocity
  • the target occupies a grid unit; the target occupied grid unit includes speed information and position information; the second quantity of particles is the particle in the occupied grid unit corresponding to time K+1.
  • the method further includes:
  • the first height difference is less than a preset first height threshold, determining that any grid is the first grid, and determining that the point cloud in the first grid is a suspected ground point;
  • a ground model is obtained by fitting
  • Points when it is determined that the second height difference is not less than a preset second height threshold constitutes a foreground point cloud.
  • the method further includes:
  • the foreground point cloud at the first moment is mapped to the obstacle occupied grid map, and the occupied grid unit corresponding to the foreground point cloud at the first moment is obtained;
  • the first number of particles is set in the occupancy grid unit corresponding to the first moment.
  • the third number of particles is set in the coincident occupied grid cells; the coincident occupied grid cells are the target occupied grids.
  • the method further includes:
  • the position information and speed information of each grid in the grid occupied by the target are obtained.
  • the present application provides a dynamic occupancy grid estimation device, the device comprising:
  • a processing unit which is used for processing the laser point cloud data at time K to obtain a foreground point cloud;
  • the foreground point cloud is a non-ground point cloud;
  • mapping unit is used to map the foreground point cloud to the obstacle occupied grid map, and obtain the occupied grid unit corresponding to the foreground point cloud at time K;
  • a determination unit which is used to determine the occupied grid unit corresponding to the foreground point cloud at time K+1 according to the occupied grid unit corresponding to the foreground point cloud at time K;
  • the prediction unit is used to make predictions at time K+1 according to the state values of the first number of particles at time K to obtain the predicted state values of the second number of particles at time K+1;
  • the first The number of particles includes the third number of particles at the K-1th moment and the fourth number of initialized new particles; the sum of the third number and the fourth number is the first number;
  • the state value includes the particle position and particle velocity;
  • the updating unit the updating unit is used to update the occupancy corresponding to the foreground point cloud at the K+1 moment according to the predicted state value of the second number of particles at the K+1 moment and the occupied grid unit corresponding to the foreground point cloud at the K+1 moment.
  • grid unit, the grid unit occupied by the target at time K+1 is obtained; the grid unit occupied by the target includes speed information and position information; the second quantity of particles is located in the grid unit occupied by the target at time K+1 particle of.
  • the apparatus further includes:
  • a dividing unit which is used to divide the laser point cloud data into a preset grid based on the ground plane
  • the calculation unit is configured to calculate the height difference between the maximum height point and the minimum height point in any grid in the grid map, to obtain a first height difference value
  • the determining unit is further configured to, when the first height difference value is smaller than a preset first height threshold, determine that any grid is the first grid, and determine the point cloud in the first grid is a suspected ground point;
  • a fitting unit configured to obtain a ground model by fitting according to all the suspected ground points of the first grid
  • the computing unit is further configured to calculate the height difference between each point in the ground model and the ground plane to obtain a second height difference
  • the determining unit is further configured to determine that points when the second height difference is not less than a preset second height threshold constitute a foreground point cloud.
  • the processing unit is further configured to process the laser point cloud data at the first moment to obtain the foreground point cloud at the first moment;
  • the mapping unit is further configured to map the foreground point cloud at the first moment to the obstacle occupied grid map to obtain the occupied grid unit corresponding to the foreground point cloud at the first moment;
  • the determining unit is further configured to, according to the grid unit occupied by the foreground point cloud at the first moment, determine the occupancy probability of the foreground point cloud occupying the grid unit;
  • the initialization unit is used to initialize the first number of particles
  • a setting unit configured to set the first number of particles in the occupancy grid unit corresponding to the first moment according to the occupancy probability.
  • the updating unit is specifically used for:
  • the third number of particles is set in the coincident occupied grid cells; the coincident occupied grid cells are the target occupied grids.
  • the setting unit is further configured to set a random state value for the third number of particles
  • the determining unit is further configured to, according to the state values of the second quantity of particles and the random state values of the third quantity of particles, obtain position information and velocity information of each grid in the grid occupied by the target .
  • the present application provides a device including a memory and a processor, where the memory is used for storing a program, and the processor is used for executing any one of the dynamic occupancy grid estimation methods provided in the first aspect.
  • the present application provides a computer program product that, when the computer program product runs on a computer, causes the computer to execute any one of the dynamic occupancy grid estimation methods provided in the first aspect.
  • the present application provides a computer-readable storage medium, where a computer program is stored on the computer-readable storage medium, and when the computer program is executed by a processor, any one of the dynamic occupancy grid estimation methods provided in the first aspect is implemented .
  • the present application provides a computer server, memory, processor and transceiver;
  • the processor is configured to be coupled with the memory to read and execute instructions in the memory, so as to implement any one of the dynamic occupancy grid estimation methods provided in the first aspect;
  • the transceiver is coupled to the processor, and the processor controls the transceiver to send and receive messages.
  • the present application provides a mobile tool, the mobile tool including the computer server provided in the sixth aspect.
  • the laser point cloud is not dependent on the clustering effect of the laser point cloud, but the laser point cloud is mapped to a two-dimensional grid, and then the particle filter-based method is used to output the output.
  • Dynamic occupancy grid information including grid information occupied by obstacles within a certain range around the vehicle, and speed information estimated by each occupied grid unit, which can be directly used by the decision planning module for obstacle avoidance It is also convenient for subsequent clustering to directly utilize the speed information of the occupied grid, thereby improving the clustering effect.
  • FIG. 1 is a schematic flowchart of a dynamic occupancy grid estimation method provided in Embodiment 1 of the present application;
  • FIG. 2 is a schematic diagram of an occupied grid unit corresponding to a foreground point cloud provided in Embodiment 1 of the present application;
  • FIG. 3 is a schematic diagram of mapping a foreground point cloud to a grid map occupied by an obstacle according to Embodiment 1 of the present application;
  • FIG. 5 is a schematic structural diagram of an apparatus for dynamic occupancy grid estimation provided in Embodiment 2 of the present application.
  • FIG. 6 is another schematic structural diagram of the apparatus for dynamic occupancy grid estimation provided in Embodiment 2 of the present application.
  • FIG. 1 is a schematic flowchart of a dynamic estimation grid estimation method provided in Embodiment 1 of the present application.
  • the method is applied to a terminal equipped with a lidar, such as a mobile tool equipped with a lidar, and the mobile tool may be an unmanned vehicle. , robot, etc.
  • the mobile tool is an unmanned vehicle
  • the execution subject of the present application is a terminal, server or processor with computing functions.
  • This application takes the application of the method to an unmanned vehicle as an example for description.
  • the execution subject of the method is the Automatic Vehicle Control Unit (AVCU), that is,
  • AVCU Automatic Vehicle Control Unit
  • the central processing unit of a driverless vehicle is equivalent to the “brain” of the driverless vehicle.
  • this application includes the following steps:
  • Step 101 Process the laser point cloud data at time K to obtain a foreground point cloud.
  • the foreground point cloud is the point cloud corresponding to the obstacle after removing the ground point cloud.
  • the following describes how to obtain the foreground point cloud.
  • raw laser point cloud data and localization data are acquired.
  • map data is acquired according to the positioning data.
  • a first region of interest (region of interest, ROI) in the map is determined.
  • the point cloud in the first ROI is the point cloud to be segmented.
  • sensors such as lidar, global positioning system (GPS), inertial measurement unit (IMU), and vision modules are installed on the vehicle, and the original laser point cloud can be obtained through single-line or multi-line lidar data.
  • GPS global positioning system
  • IMU inertial measurement unit
  • vision modules are installed on the vehicle, and the original laser point cloud can be obtained through single-line or multi-line lidar data.
  • the positioning data is obtained through GPS, and after the obtained multiple positioning data packets sent by the GPS, after analyzing the positioning data packets, the port number and data packet length of each positioning data packet will be obtained. , flag bits, etc., through the data packet length or flag bit, you can determine whether the positioning data is valid.
  • a pre-built high-precision map is pre-stored in the server.
  • the high-precision map can be divided into multiple map data files and stored according to the latitude and longitude.
  • the boundaries can be divided, for example, a file includes multiple boundaries.
  • each boundary can be determined as a ROI region.
  • the vehicle can send a request message including the positioning data to the server to obtain a map data file corresponding to the positioning data.
  • the positioning data includes the current latitude and longitude of the vehicle, and can send to the server a message including the current longitude and latitude of the vehicle. Request message to obtain the map data file corresponding to the current latitude and longitude.
  • the vehicle After the vehicle receives the map data file, it parses and obtains the map data, and according to the map data, determines the first ROI area in the map corresponding to the current longitude and latitude of the vehicle, and determines the original laser point cloud in the first ROI area as the point to be divided cloud.
  • the high-precision map of the closed park can be downloaded from the server in advance, and the corresponding map data file can be obtained by querying according to the positioning data, and the map data can be parsed. After file, get map data.
  • the position of the vehicle is determined according to the original laser point cloud; the second ROI is determined according to the position of the vehicle; it is determined that in the original laser point cloud, The point cloud in the second ROI is the point cloud to be segmented.
  • the data packet when the length of the data packet does not meet the preset length threshold, it is determined that the data packet is invalid, or the flag bit of the data packet is an invalid flag bit, and the data packet is determined to be invalid. Or the positioning data is valid, but when a query request is sent to the server according to the positioning data and the corresponding map data file is not found, the map data is invalid.
  • the position of the vehicle is determined.
  • a second ROI is set in the vehicle coordinate system. For example, a rectangle of 5m*10m may be determined as the second ROI on the right side of the vehicle coordinate system.
  • the point cloud in the second ROI in the original laser point cloud is determined as the point cloud to be segmented.
  • the above-mentioned original laser point cloud is the original laser point cloud in the vehicle coordinate system.
  • the position of the laser radar in the vehicle coordinate system can be determined, and the obtained laser point cloud can be converted into the vehicle.
  • the positioning data is the positioning data of the vehicle coordinate system after coordinate transformation.
  • the center of mass of the vehicle can be used as the origin of the vehicle coordinate system.
  • the foreground point cloud can be obtained according to the following steps.
  • the point cloud to be segmented can be divided into a preset grid map. Then, in any grid in the grid map, the height difference value of the point with the highest height and the point with the lowest height is calculated to obtain the first height difference value. Next, when the first height difference is smaller than the preset first height threshold, any grid is determined as the first grid, and the point cloud in the first grid is determined as a suspected ground point. Next, according to the suspected ground points of all the first grids, according to a plane fitting algorithm, a ground model is obtained by fitting.
  • the height difference between each point in the ground model and the ground plane is calculated to obtain a second height difference. Finally, it is determined that the point when the second height difference is not less than the preset second height threshold is the foreground point cloud. At the same time, it can also be determined that the point when the second height difference is smaller than the preset second height threshold is the background point cloud.
  • the size of the grid can be obtained according to the empirical value of multiple calculations.
  • the point cloud to be divided is a collection of multiple points, each point has position information, and the grid also has a position.
  • the position of the grid can be the upper left of the grid.
  • the position of the corner of the square indicates that, under the premise of coordinate system 1, the point cloud to be segmented can be mapped to the grid, so as to obtain the point cloud to be segmented distributed in the grid.
  • the first height threshold may be set according to the traffic capacity of the vehicle.
  • the second height threshold may be set according to the type of the vehicle. For example, when the vehicle is a small sweeper and the vehicle is a logistics vehicle or a car driving on the street, the second height threshold is different.
  • the plane fitting algorithm includes, but is not limited to, the least squares method and the Random Sample Consensus (RANSAC) algorithm.
  • Step 102 Map the foreground point cloud to the obstacle occupied grid map to obtain the occupied grid cell corresponding to the foreground point cloud at time K.
  • the part with the highest occupation probability of the obstacle may be assigned an occupation probability of 0.9.
  • different probabilities can be distinguished in Figure 3 by grayscale.
  • the grid cell corresponding to an occupation probability of 0.9 can be marked with a grayscale value of 255, and the lowest occupation probability can be marked.
  • the part with an occupancy probability of 0.1 is assigned an occupancy probability of 0.1, and is marked with a gray value of 0;
  • the portion with uncertain occupancy probability can be further divided and marked with different gray values.
  • the occupation probability here is calculated according to the inverse sensor model, which can be a certain value between 0 and 1.
  • Step 103 according to the grid unit occupied by the foreground point cloud at time K, determine the grid unit occupied by the foreground point cloud at time K+1.
  • the occupied grid unit corresponding to the foreground point cloud at time K+1 can be obtained according to the method of step 101 to step 102 .
  • Step 104 at time K+1, perform prediction according to the state values of the first number of particles at time K, and obtain the predicted state value of the second number of particles at time K+1;
  • the first number of particles includes the K-1th particle
  • the third number of particles at the moment and the fourth number of initialized new particles; the sum of the third number and the fourth number is the first number;
  • the state value includes particle position and particle velocity.
  • particle filtering refers to the process of approximately representing the probability density function by finding a set of random samples propagating in the state space, replacing the integral operation with the sample mean, and then obtaining the minimum variance estimation of the system state.
  • a set of random samples here is called a particle.
  • the latest obstacle occupied grid state at the current moment can be obtained, which is used for updating the state value of the particle filter.
  • the occupied grid cell corresponding to the foreground point cloud occupies a certain number of particles, which can be called the first number of particles.
  • different occupied grids can be represented by occupancy probabilities.
  • the occupancy probabilities can be distinguished by different occupancy probabilities by using gray values. For example, but not limitation, 255 can be used to represent the occupancy grid cell corresponding to the foreground point cloud.
  • the state value of each particle includes position estimation and velocity estimation (indicated by arrows).
  • the filter can make predictions according to the state values of the first number of particles at time k, and obtain the predicted state value of the first number of particles at time K+1 .
  • the predicted state value includes the predicted velocity and predicted position of each particle.
  • the position will change, and the particle whose speed exceeds a certain value will fall into other surrounding occupied grid cells after prediction.
  • the number of particles held will become smaller, and the probability of the original grid being occupied will also decrease accordingly. For example, if it is represented by a grayscale image, it will change from black with a grayscale value of 255 to gray with a lower grayscale value. .
  • the laser point cloud data at the first moment is processed to obtain the foreground point cloud at the first moment; the foreground point cloud at the first moment is mapped to the obstacle occupied grid map to obtain the foreground point cloud at the first moment
  • the corresponding occupied grid unit according to the occupied grid unit corresponding to the foreground point cloud at the first moment, determine the occupancy probability of the foreground point cloud in the occupied grid unit; initialize the first number of particles; Each particle is set in the occupied grid cell corresponding to the first moment.
  • the first moment here may be the initial moment.
  • Step 105 according to the predicted state value of the second number of particles at time K+1 and the occupied grid cell corresponding to the foreground point cloud at time K+1, update the occupied grid cell corresponding to the foreground point cloud at time K+1 to obtain K.
  • the target at time +1 occupies a grid unit; the grid unit occupied by the target includes velocity information and position information; the second number of particles is the particle in the occupied grid unit corresponding to time K+1.
  • the grid unit occupied by the target includes speed information and position information.
  • Step 105 will be described in detail below.
  • the occupied grid cells where the first number of particles at time K+1 are located can be superimposed with the probability of occupied grid cells corresponding to the foreground point cloud at time K+1, so as to obtain overlapping occupied grid cells.
  • step 105 also includes:
  • the state values of the particles in each grid can be counted, the speed information of the particles in the grid can be obtained, and the speed of the obstacles can be counted according to the speed information of the particles in each grid.
  • information and take the speed information of the obstacle as the speed information of the grid occupied by the target.
  • the velocity information here may be the velocity in the x-direction and/or the velocity in the y-direction and/or the resultant velocity.
  • the sum speed is determined according to the speed in the x direction and the speed in the y direction.
  • the position information of the obstacles can also be counted, that is, the positions of the grid cells occupied by the coincidence.
  • K and K+1 are only to distinguish different moments.
  • K can be used to represent the current moment, then K+1 is the next moment of the current moment, or it can be marked with other letters, which is not limited in this application. .
  • the dynamic occupancy grid estimation method provided in this application, it does not rely on the clustering effect on the laser point cloud, but by performing a two-dimensional grid mapping on the laser point cloud, and then using a method based on particle filtering, Output dynamic occupancy grid information, including grid information occupied by obstacles within a certain range around the vehicle, and speed information estimated by each occupied grid unit, which can be directly used by the decision planning module for obstacle detection.
  • the obstacle avoidance calculation is also convenient for subsequent clustering, and the speed information of the occupied grid can be directly used, thereby improving the clustering effect.
  • FIG. 5 is a schematic structural diagram of an apparatus for dynamic occupancy grid estimation provided in Embodiment 2 of the present application.
  • the dynamic occupancy grid estimation apparatus 500 includes: a processing unit 501 , a mapping unit 502 , a determining unit 503 , and a predicting unit 501 . unit 504 and update unit 505.
  • the processing unit 501 is used to process the laser point cloud data at time K to obtain a foreground point cloud; the foreground point cloud is a non-ground point cloud;
  • the mapping unit 502 is used to map the foreground point cloud to the obstacle occupied grid map to obtain the occupied grid unit corresponding to the foreground point cloud at time K;
  • the determining unit 503 is configured to determine the occupied grid unit corresponding to the foreground point cloud at time K+1 according to the occupied grid unit corresponding to the foreground point cloud at time K.
  • the prediction unit 504 is configured to perform prediction at time K+1 according to the state values of the first number of particles at time K, and obtain the predicted state value of the second number of particles at time K+1; the first number of particles includes the Kth particle The third number of particles at -1 time and the fourth number of initialized new particles; the sum of the third number and the fourth number is the first number; the state value includes particle position and particle velocity.
  • the updating unit 505 is configured to update the occupied grid unit corresponding to the foreground point cloud at the K+1 moment according to the predicted state value of the second number of particles at the K+1 moment and the occupied grid unit corresponding to the foreground point cloud at the K+1 moment,
  • the grid cell occupied by the target at time K+1 is obtained;
  • the grid cell occupied by the target includes velocity information and position information;
  • the second quantity of particles is the particle in the occupied grid cell corresponding to time K+1.
  • FIG. 6 is another schematic structural diagram of the dynamic occupancy grid estimation apparatus provided in Embodiment 2 of the present application.
  • the dynamic occupancy grid estimation apparatus 600 further includes: a dividing unit 601 , a calculating unit 602 and a fitting unit 601 . unit 603.
  • the dividing unit 601 is used to divide the laser point cloud data into preset grids based on the ground plane.
  • the calculating unit 602 is configured to calculate the height difference value between the point with the maximum height and the point with the minimum height in any grid in the grid map, to obtain the first height difference value.
  • the determining unit 503 is further configured to determine any grid as the first grid when the first height difference value is smaller than the preset first height threshold, and determine that the point cloud in the first grid is a suspected ground point.
  • the fitting unit 603 is configured to obtain a ground model by fitting according to all the suspected ground points of the first grid.
  • the calculating unit 602 is further configured to calculate the height difference between each point in the ground model and the ground plane to obtain a second height difference.
  • the determining unit 503 is further configured to determine that the points when the second height difference is not less than the preset second height threshold constitute a foreground point cloud.
  • the apparatus further includes an initialization unit 604 and a setting unit 605 .
  • the processing unit 501 is further configured to process the laser point cloud data at the first moment to obtain the foreground point cloud at the first moment.
  • the mapping unit 502 is further configured to map the foreground point cloud at the first moment to the obstacle occupied grid map to obtain the occupied grid unit corresponding to the foreground point cloud at the first moment.
  • the determining unit 503 is further configured to determine the occupancy probability of the foreground point cloud occupying the grid cell according to the grid cell occupied by the foreground point cloud at the first moment.
  • the initialization unit 604 is used to initialize the first number of particles.
  • the setting unit 605 is configured to set the first number of particles in the occupancy grid unit corresponding to the first moment according to the occupancy probability.
  • update unit 505 is specifically used for:
  • a third number of particles are set in the coincidentally occupied grid cells; the coincidentally occupied grid cells are the target occupied grids.
  • the setting unit 604 is further configured to set a random state value for the third quantity of particles.
  • the determining unit 503 is further configured to, according to the state values of the second quantity of particles and the random state values of the third quantity of particles, obtain the position information and speed information of each grid in the grid occupied by the target.
  • the third embodiment of the invention provides a device including a memory and a processor, the memory is used to store a program, and the memory can be connected to the processor through a bus.
  • the memory may be non-volatile memory, such as hard drives and flash memory, in which software programs and device drivers are stored.
  • the software program can perform various functions of the foregoing methods provided by the embodiments of the present application; the device driver may be a network and interface driver.
  • the processor is configured to execute a software program, and when the software program is executed, the method provided in Embodiment 1 of the present application can be implemented.
  • the fourth embodiment of the present application provides a computer program product, which enables the computer to execute the method provided by the first embodiment of the present application when the computer program product runs on a computer.
  • the fifth embodiment of the present application provides a computer-readable storage medium, where a computer program is stored on the computer-readable storage medium, and when the computer program is executed by a processor, the method provided by the first embodiment of the present application is implemented.
  • Embodiment 5 of the present application provides a computer server, where the computer server includes: a memory, a processor, and a transceiver; the processor is configured to be coupled with the memory, and read and execute instructions in the memory to implement implementation The method of any one of the examples;
  • the transceiver is coupled to the processor, and the processor controls the transceiver to send and receive messages.
  • the sixth embodiment of the present application provides a mobile tool, including the computer server provided in the fifth embodiment.
  • Embodiment 7 of the present application provides a device, where the device includes a memory and a processor, where the memory is used to store a program, and the processor is used to execute any one of the methods provided in Embodiment 1.
  • a software module can be placed in random access memory (RAM), internal memory, read only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, removable disk, CD-ROM, or any other in the technical field. in any other known form of storage medium.
  • RAM random access memory
  • ROM read only memory
  • electrically programmable ROM electrically erasable programmable ROM
  • registers hard disk, removable disk, CD-ROM, or any other in the technical field. in any other known form of storage medium.

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Databases & Information Systems (AREA)
  • Remote Sensing (AREA)
  • Multimedia (AREA)
  • General Engineering & Computer Science (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Medical Informatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Health & Medical Sciences (AREA)
  • Computing Systems (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Evolutionary Biology (AREA)
  • Software Systems (AREA)
  • Probability & Statistics with Applications (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Automation & Control Theory (AREA)
  • Traffic Control Systems (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

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

Description

动态占据栅格估计方法及装置
本申请要求于2020年10月12日提交的、申请号为202011085533.6、标题为“动态占据栅格估计方法及装置”的中国专利申请的优先权，该中国专利申请的公开内容以引用的方式并入本文。
技术领域
本申请涉及数据处理领域，尤其涉及一种动态占据栅格估计方法及装置。
背景技术
近年来，自动驾驶车辆在智能交通出行、物流配送、清洁作业等多场景中，构建了智慧城市生活的应用示范。自动驾驶感知方案最重要的障碍物就是正确检测出环境中的障碍物并得到其运动状态。激光雷达是自动驾驶汽车感知环境的重要传感器，具有精度高、抗干扰等特点。激光雷达主要能够提供障碍物的位置信息和反射率信息，但无法直接输出障碍物的速度信息。
目前从激光雷达点云中获取障碍物的速度信息采用的方法是：将激光点云投影到二维平面中进行聚类，获得聚类后的障碍物位置信息，利用经典的卡尔曼滤波等算法获取障碍物的运动状态。
现有的激光点云障碍物速度估计算法强依赖于对于激光点云的聚类效果。如果聚类出现较多的过分割或者欠分割的现象，将会导致 障碍物的匹配出现问题，从而导致对障碍物的速度估计出现偏差。
发明内容
本申请实施例的目的是提供一种动态占据栅格估计方法及装置，以解决现有技术中的无法直接输出障碍物的速度信息或者对障碍物速度估计存在偏差的问题。
为解决上述问题，第一方面，本申请提供了一种动态占据栅格估计方法，所述方法包括：
对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 Sample Consensus，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 (15)

  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所述的装置，其特征在于，所述设置单元还用于，为所述第三数量个粒子设置随机状态值；
    所述确定单元还用于，根据所述第二数量个粒子的状态值和所述第三数量个粒子的随机状态值，得到所述目标占据栅格中的各栅格的位置信息和速度信息。
  11. 一种设备，其特征在于，所述设备包括存储器和处理器，存储器用于存储程序，处理器用于执行如权利要求1～5任意一项所述的动态占据栅格 估计方法。
  12. 一种计算机程序产品，其特征在于，当计算机程序产品在计算机上运行时，使得计算机执行如权利要求1～5任意一项所述的动态占据栅格估计方法。
  13. 一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，计算机程序被处理器执行时实现如权利要求1～5任意一项所述的动态占据栅格估计方法。
  14. 一种计算机服务器，其特征在于，所述计算机服务器包括：存储器、处理器和收发器；
    所述处理器用于与所述存储器耦合，读取并执行所述存储器中的指令，以实现权利要求1～5任一项所述的动态占据栅格估计方法；
    所述收发器与所述处理器耦合，由所述处理器控制所述收发器进行消息收发。
  15. 一种移动工具，其特征在于，包括上述权利要求14所述的计算机服务器。
PCT/CN2021/123335 2020-10-12 2021-10-12 动态占据栅格估计方法及装置 WO2022078342A1 (zh)

Applications Claiming Priority (2)

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

Publications (1)

Publication Number Publication Date
WO2022078342A1 true WO2022078342A1 (zh) 2022-04-21

Family

ID=81207719

Family Applications (1)

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

Country Status (2)

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

Cited By (2)

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

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
IN201641039332A (zh) * 2016-11-18 2018-05-25
US20180203124A1 (en) * 2017-01-17 2018-07-19 Delphi Technologies, Inc. Object detection system
CN110705458A (zh) * 2019-09-29 2020-01-17 北京智行者科技有限公司 边界检测方法及装置
US20200249356A1 (en) * 2019-02-05 2020-08-06 Honda Motor Co., Ltd. SYSTEM AND METHOD FOR PROVIDING ONLINE MULTI-LiDAR DYNAMIC OCCUPANCY MAPPING
CN111591288A (zh) * 2020-03-31 2020-08-28 北京智行者科技有限公司 基于距离变换图的碰撞检测方法及装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
IN201641039332A (zh) * 2016-11-18 2018-05-25
US20180203124A1 (en) * 2017-01-17 2018-07-19 Delphi Technologies, Inc. Object detection system
US20200249356A1 (en) * 2019-02-05 2020-08-06 Honda Motor Co., Ltd. SYSTEM AND METHOD FOR PROVIDING ONLINE MULTI-LiDAR DYNAMIC OCCUPANCY MAPPING
CN110705458A (zh) * 2019-09-29 2020-01-17 北京智行者科技有限公司 边界检测方法及装置
CN111591288A (zh) * 2020-03-31 2020-08-28 北京智行者科技有限公司 基于距离变换图的碰撞检测方法及装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
NUSS DOMINIK, REUTER STEPHAN, THOM MARKUS, YUAN TING, KREHL GUNTHER, MAILE MICHAEL, GERN AXEL, DIETMAYER KLAUS: "A random finite set approach for dynamic occupancy grid maps with real-time application", INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH., SAGE SCIENCE PRESS, THOUSAND OAKS., US, vol. 37, no. 8, 1 July 2018 (2018-07-01), US , pages 841 - 866, XP055921798, ISSN: 0278-3649, DOI: 10.1177/0278364918775523 *

Cited By (3)

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

Also Published As

Publication number Publication date
CN114419573A (zh) 2022-04-29

Similar Documents

Publication Publication Date Title
WO2022022694A1 (zh) 自动驾驶环境感知方法及系统
CN108319655B (zh) 用于生成栅格地图的方法和装置
EP3875985B1 (en) Method, apparatus, computing device and computer-readable storage medium for positioning
EP3714290B1 (en) Lidar localization using 3d cnn network for solution inference in autonomous driving vehicles
CN109143207B (zh) 激光雷达内参精度验证方法、装置、设备及介质
KR102628778B1 (ko) 위치결정을 위한 방법, 컴퓨팅 기기, 컴퓨터 판독가능 저장 매체 및 컴퓨터 프로그램
WO2020154970A1 (en) Deep learning–based feature extraction for lidar localization of autonomous driving vehicles
CN109435955B (zh) 一种自动驾驶系统性能评估方法、装置、设备及存储介质
WO2020154973A1 (en) Lidar localization using rnn and lstm for temporal smoothness in autonomous driving vehicles
CN110286389B (zh) 一种用于障碍物识别的栅格管理方法
US10325163B2 (en) Vehicle vision
WO2022078342A1 (zh) 动态占据栅格估计方法及装置
EP4089659A1 (en) Map updating method, apparatus and device
CN111563450B (zh) 数据处理方法、装置、设备及存储介质
CN109849930B (zh) 自动驾驶汽车的相邻车辆的速度计算方法和装置
JP7413543B2 (ja) データ伝送方法および装置
CN110705458A (zh) 边界检测方法及装置
EP3875905B1 (en) Method, device and medium for detecting environmental change
WO2024012211A1 (zh) 自动驾驶环境感知方法、介质及车辆
CN114485698B (zh) 一种交叉路口引导线生成方法及系统
CN114694111A (zh) 车辆定位
EP3798665A1 (en) Method and system for aligning radar detection data
CN115359332A (zh) 基于车路协同的数据融合方法、装置、电子设备及系统
CN116022163A (zh) 基于超局部子图的自动驾驶车辆扫描匹配和雷达姿态估计器
CN115496782A (zh) Lidar对lidar对准和lidar对车辆对准的在线验证

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 21879386

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 21879386

Country of ref document: EP

Kind code of ref document: A1