A drivable area analysis method based on laser radar

Abstract

The present invention provides a method for analyzing drivable areas based on laser radar. The terrain is accurately modeled by adopting the normal distribution transformation mapping method and fusing the information of continuous frames of laser radar; based on the terrain modeling results, the geometric connectivity analysis between adjacent terrain areas is performed to obtain the traversability of each area, thereby obtaining the drivable area. The present invention can output stable, complete and accurate terrain models and drivability analysis results.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent robot autonomous navigation, and in particular relates to a drivable area analysis method based on laser radar. Background Art

[0002] For autonomous driving, determining the drivable area in the environment is a prerequisite for autonomous navigation, which directly affects the safety of the vehicle's autonomous navigation. At present, most methods treat environmental information as a binary classification task, that is, classifying the area observed in a single frame of data as a ground area or an obstacle area. However, this solution may have three problems: (1) The detection results generated by processing single-frame data are usually unstable and cannot provide sufficient coverage of the local environment; (2) Passability is closely related to the vehicle's kinematic model, and the binary classification method is too absolute and cannot be generalized; (3) The feasible area surrounded by obstacles should also be regarded as an inaccessible area. The determination of the driving area should not only use the current frame information, but also combine the information of several previous and subsequent frames. Summary of the invention

[0003] In order to overcome the shortcomings of the existing laser radar single frame data information is insufficient and connectivity information cannot be obtained, the present invention provides a drivable area analysis method based on laser radar. The terrain is accurately modeled by fusing the information of continuous laser radar frames using the normal distribution transformation mapping method; based on the terrain modeling results, the geometric connectivity analysis between adjacent terrain areas is performed to obtain the traversability of each area, thereby obtaining the drivable area. The present invention can output a stable, complete and accurate terrain model and drivability analysis results.

[0004] A method for analyzing a drivable area based on laser radar, characterized by the following steps:

[0005] Step 1, data preprocessing: perform posture correction on the point cloud data obtained by the vehicle-mounted laser radar, and project the corrected point cloud data into a two-dimensional grid map, and then use the minimum-maximum height difference method to divide the grid cells into terrain cells and obstacle cells;

[0006] Step 2, global grid map update: the lidar data from the current moment to the previous 5 seconds are stored and maintained in the global grid, and the elevation distribution of all data points in each grid is counted; if the vehicle travels a distance greater than the grid resolution, the data coordinates in the grid are translated by one position in the opposite direction of the vehicle's travel, and each data point in the current frame is traversed in this way to update its corresponding grid position; data points in each grid whose distance is outside the threshold and whose release time is later than 5 seconds are eliminated; the threshold value range is [-30,30] meters;

[0007] Step 3, multi-frame data fusion: Fusion of the joint elevation distribution of the previous time data and the elevation distribution of the current observation data according to the following formula:

[0008]

[0009] Among them, t represents the current time t, represents the grid cell x up to time t i The total number of projection points, x i represents the i-th grid cell, represents the grid cell x up to time t-1 i The total number of projection points in represents the number of points collected by the lidar at time t, represents the mean of the observed data at the current time t, Represents the fused grid cell x i The estimated mean of the data in represents the mean of the data before time t, represents the variance of the observed data at the current time t, Represents the fused grid cell x i The estimated variance of the data in Represents the variance of the data before time t;

[0010] If the estimated variance of a grid cell is If the variance threshold is greater than the grid cell, the grid cell is considered to be an obstacle cell; the variance threshold is 0.1v, where v represents the current speed of the vehicle;

[0011] Step 4, driving cost calculation: First, calculate the normal vector of each grid as follows:

[0012]

[0013] in, represents the normal vector of the grid cell (r, c), are the three-dimensional coordinates of the four neighborhoods of the grid unit (r, c). For a grid unit (r, c), its three-dimensional coordinates are (x r ,y c ) represents the plane coordinates of the grid unit (r, c), h (r，c) represents the height of the grid cell (r, c);

[0014] For adjacent grid cells x i and x j , the path between two grid cells is considered passable if the following equation is satisfied:

[0015]

[0016] in, represents the i-th grid cell x i The normal vector of represents the jth grid cell x j The normal vector of Represents the grid cell x i To grid cell x j The direction vector, Represents the grid cell x j To grid cell x i The direction vector, T θ represents the minimum concavity angle;

[0017] Crossing grid cell x i The driving cost is calculated as follows:

[0018]

[0019] Among them, C i represents the crossing of the i-th grid cell x i The travel cost, m represents the total number of traversable adjacent units, T α represents the maximum similarity angle; the T θ and T α Determined by the vehicle model;

[0020] Step 5, drivability analysis: select seed grid cells based on the center position of the vehicle, take the center of the vehicle as the center, and take all grid cells on the circumference of a circle with a radius of 5 meters as seed grid cells. Starting from the seed grid cell, add the adjacent grid cells that meet the drivability requirements to the seed grid cell set until the seed grid cell set no longer expands. At this time, the areas corresponding to all grid cells in the obtained seed grid cell set constitute the final drivable area.

[0021] Specifically, the posture correction of the point cloud data acquired by the laser radar includes first performing intra-frame motion compensation on the posture estimation information, and then unifying the point cloud data into a map coordinate system based on the vehicle positioning information.

[0022] Specifically, the minimum-maximum height difference method refers to setting a height difference threshold Th. If the minimum-maximum height difference of a grid unit is greater than Th, the grid unit is considered to be an obstacle unit, otherwise, it is considered to be a terrain unit; the value of the height difference threshold Th is the maximum obstacle-crossing height of the vehicle.

[0023] Specifically, for each grid cell x i , whose elevation distribution refers to the observed height modeled as a normal distribution N(μ i ,∑i ), where μ i represents the mean of the distribution N, ∑ i represents the variance of the distribution N.

[0024] The beneficial effects of the present invention are as follows: due to the use of the normal distribution transformation mapping method, the terrain is accurately modeled by fusing the information of continuous laser radar frames, and the traversability of the terrain is estimated by geometric connectivity analysis between adjacent terrain units, thereby distinguishing terrains with different drivability costs, and being able to generate stable, complete and accurate drivability analysis results in real time. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 It is a flow chart of a drivable area analysis method based on laser radar of the present invention;

[0026] Figure 2 It is a schematic diagram of updating the global grid graph of the present invention;

[0027] Figure 3 It is a schematic diagram of the region growing algorithm of the present invention. DETAILED DESCRIPTION

[0028] The present invention is further described below in conjunction with the accompanying drawings and embodiments. The present invention includes but is not limited to the following embodiments.

[0029] At present, most drivability analysis methods have certain limitations, such as the detection results generated by single-frame data are usually unstable, the drivability analysis is too absolute, and the separation scheme is not intelligent enough. The present invention provides a drivable area analysis method based on laser radar, which can overcome the shortcomings of existing methods. Figure 1 As shown, the specific implementation process is as follows:

[0030] 1. Data preprocessing

[0031] The point cloud data acquired by the vehicle-mounted laser radar is subjected to posture correction, and the corrected point cloud data is projected into a two-dimensional grid map, and then the grid cells are divided into terrain cells and obstacle cells using the minimum-maximum height difference method;

[0032] During the laser radar scanning process, as the vehicle moves, the laser radar point cloud will be distorted. In order to correct the distorted point cloud, the posture information output by the posture estimation module is used to perform intra-frame motion compensation. Subsequently, the point cloud data is unified into the map coordinate system based on the vehicle positioning information.

[0033] The corrected point cloud is then projected onto a two-dimensional grid map, and the minimum-maximum height difference method is used to roughly divide the grid cells into terrain cells and obstacle cells. This method requires setting a height difference threshold Th. If the minimum-maximum height difference of a grid cell is greater than Th, the grid cell is considered to be an obstacle cell, otherwise, it is considered to be a terrain cell; the value of the height difference threshold Th is the maximum obstacle crossing height of the vehicle.

[0034] 2. Global grid map update

[0035] For each grid cell x i , the observed height can be modeled as a normal distribution N(μ i ,∑ i ), where μ i represents the mean of the distribution N, ∑ i represents the variance of the distribution N.

[0036] The lidar data from the current moment to the previous 5 seconds is stored and maintained in the global grid, and the elevation distribution of all data points in each grid is counted as above;

[0037] If the vehicle travels a greater distance than the grid resolution, the data coordinates in the grid are shifted one position in the opposite direction of the vehicle's travel (e.g. Figure 2 As shown), traverse each data point of the current frame and update its corresponding grid position;

[0038] Data points in each grid whose distance is outside the threshold and whose release time is later than 5 seconds are removed, while other new data points that meet the conditions are retained to complete the global grid map update. The threshold value range is [-30, 30] meters.

[0039] 3. Multi-frame data fusion

[0040] After the latest sensor data is obtained in step 2, it needs to be fused with the existing data distribution in the global grid. The present invention proposes another fusion strategy based on NDT, which directly fuses the joint elevation distribution of the previous moment data with the elevation distribution of the current observation data, as follows:

[0041]

[0042] Among them, t represents the current time t, represents the grid cell x up to time t i The total number of projection points, x i represents the i-th grid cell, represents the grid cell x up to time t-1 i The total number of projection points in represents the number of points collected by the lidar at time t, represents the mean of the observed data at the current time t, Represents the fused grid cell x i The estimated mean of the data in represents the mean of the data before time t, represents the variance of the observed data at the current time t, Represents the fused grid cell x i The estimated variance of the data in Represents the variance of the data before time t.

[0043] Compared with the Kalman filter-based method, the above fusion method makes full use of the mean and variance information contained in each frame of radar point cloud data without setting parameters. The elevation distribution generated by each frame of radar point cloud data is fused together in the global grid map, and the result is more stable and complete.

[0044] In addition, the segmentation of the potential terrain area can be refined by the stable distribution variance information, that is, if the estimated variance of a grid cell If it is greater than the variance threshold, the grid unit is considered to be an obstacle unit; the variance threshold is 0.1v, where v represents the current speed of the vehicle.

[0045] 4. Driving cost calculation

[0046] After the rough classification mentioned above, the global grid is divided into two categories: obstacle grid cells and terrain grid cells. However, the terrain grid cells obtained at this stage may contain misjudgments. For example, the roof of a nearby vehicle may be mistakenly judged as potential terrain. This misjudgment is generally difficult to correct by processing single-frame data, but it can be avoided by multi-frame information fusion strategies.

[0047] After obtaining the global elevation map, the terrain accessibility can be further analyzed based on the geometric connectivity between adjacent terrain units. That is, first generate a normal vector map from the elevation map, and then estimate the traversability and driving cost of each grid unit by combining terrain information and local convexity criteria. Starting from the grid cells near the vehicle, regional growth calculations are performed to determine the terrain accessibility, which forms a hard constraint on the final analysis results. The specific method process is as follows:

[0048] First, given the global elevation map, the normal vector of the current grid cell can be calculated by referring to the elevation of the current grid cell and the elevation of its adjacent grid cells, that is, the normal vector of each grid is calculated as follows:

[0049]

[0050] in, represents the normal vector of the grid cell (r, c), are the three-dimensional coordinates of the four neighborhoods of the grid unit (r, c). For a grid unit (r, c), its three-dimensional coordinates are (x r ,y c ) represents the plane coordinates of the grid unit (r, c), h (r，c) Represents the height of the grid cell (r, c).

[0051] Using the estimated global elevation map and normal vector map, the traversability of each grid cell can be analyzed by the local convexity criterion. If the vehicle can be located between two adjacent grid cells x i and x j Pass between them, then their corresponding normal vectors and The angle α between ij It should be small, and the convexity between two grid cells cannot be too high. The maximum similarity angle T under the given vehicle motion model constraints α and minimum concavity angle T θ , if the following inequality is satisfied, then the grid cell x is considered i The path between and x is traversable:

[0052]

[0053] in, represents the i-th grid cell x i The normal vector of represents the jth grid cell x j The normal vector of Represents the grid cell x i To grid cell x j The direction vector, Represents the grid cell x j To grid cell x i The direction vector of .

[0054] Crossing grid cell x i The driving cost is calculated as follows:

[0055]

[0056] Among them, C i represents the crossing of the i-th grid cell x i The travel cost of a cell is m, and m represents the total number of traversable adjacent cells.

[0057] 5. Feasibility analysis

[0058] Finally, the region growing algorithm is used to analyze the passability of the terrain. First, seed grid cells are selected based on the center position of the vehicle. With the center of the vehicle as the center, all grid cells on a circle with a radius of 5 meters are used as seed grid cells. Starting from these seed grids, any adjacent grid cells that satisfy equation (7) can be added to the seed grid set. If the seed grid set does not expand any more, the algorithm terminates, and the area corresponding to all grid cells in the seed grid set constitutes the final drivable area. Figure 3 A schematic diagram of the region growing algorithm is given.

[0059] In this way, except for those absolutely untraversable and unreachable areas, the remaining terrain areas are associated with travel cost values ​​stored in the cost map. The cost map can distinguish the traversability of different terrains, provide richer information for the path planning module, and help the vehicle autonomously choose a reasonable and safe driving path in a complex outdoor environment.

Claims

1. A method for analyzing drivable areas based on laser radar, characterized in that Here are the steps: Step 1, data preprocessing: perform posture correction on the point cloud data obtained by the vehicle-mounted laser radar, and project the corrected point cloud data into a two-dimensional grid map, and then use the minimum-maximum height difference method to divide the grid cells into terrain cells and obstacle cells; Step 2, global grid map update: the lidar data from the current moment to the previous 5 seconds is stored and maintained in the global grid, and the elevation distribution of all data points in each grid is counted; If the vehicle travels a distance greater than the grid resolution, the data coordinates in the grid are translated one position in the opposite direction of the vehicle's travel, and each data point in the current frame is traversed in this way to update its corresponding grid position; data points in each grid whose distance is outside the threshold and whose release time is later than 5 seconds are eliminated; the threshold value range is [-30, 30] meters; Step 3, multi-frame data fusion: Fusion of the joint elevation distribution of the previous time data and the elevation distribution of the current observation data according to the following formula: Among them, t represents the current time t, represents the grid cell x up to time t i The total number of projection points, x i represents the i-th grid cell, represents the grid cell x up to time t-1 i The total number of projection points in represents the number of points collected by the lidar at time t, represents the mean of the observed data at the current time t, Represents the fused grid cell x i The estimated mean of the data in represents the mean of the data before time t, represents the variance of the observed data at the current time t, Represents the fused grid cell x i The estimated variance of the data in Represents the variance of the data before time t; If the estimated variance of a grid cell is If the variance threshold is greater than the grid cell, the grid cell is considered to be an obstacle cell; the variance threshold is 0.1v, where v represents the current speed of the vehicle; Step 4, driving cost calculation: First, calculate the normal vector of each grid as follows: in, represents the normal vector of the grid cell (r, c), are the three-dimensional coordinates of the four neighborhoods of the grid unit (r, c). For a grid unit (r, c), its three-dimensional coordinates are (x r ,y c ) represents the plane coordinates of the grid unit (r, c), h (r，c) represents the height of the grid cell (r, c); For adjacent grid cells x i and x j , the path between two grid cells is considered passable if the following equation is satisfied: in, represents the i-th grid cell x i The normal vector of represents the jth grid cell x j The normal vector of Represents the grid cell x i To grid cell x j The direction vector of Represents the grid cell x j To grid cell x i The direction vector, T θ represents the minimum concavity angle; Crossing grid cell x i The driving cost is calculated as follows: Among them, C i represents the crossing of the i-th grid cell x i The travel cost, m represents the total number of traversable adjacent units, T α represents the maximum similarity angle; the T θ and T α Determined by the vehicle model; Step 5, drivability analysis: select seed grid cells based on the center position of the vehicle, take the center of the vehicle as the center, and take all grid cells on the circumference of a circle with a radius of 5 meters as seed grid cells. Starting from the seed grid cell, add the adjacent grid cells that meet the drivability requirements to the seed grid cell set until the seed grid cell set no longer expands. At this time, the areas corresponding to all grid cells in the obtained seed grid cell set constitute the final drivable area.

2. The method for analyzing a drivable area based on laser radar according to claim 1, characterized in that: The posture correction of the point cloud data acquired by the laser radar includes first performing intra-frame motion compensation on the posture estimation information, and then unifying the point cloud data into a map coordinate system based on the vehicle positioning information.

3. The method for analyzing a drivable area based on laser radar according to claim 1, characterized in that: The minimum-maximum height difference method refers to setting a height difference threshold Th. If the minimum-maximum height difference of a grid unit is greater than Th, the grid unit is considered to be an obstacle unit, otherwise, it is considered to be a terrain unit; the value of the height difference threshold Th is the maximum obstacle-crossing height of the vehicle.

4. The method for analyzing a drivable area based on laser radar according to claim 1, characterized in that: For each grid cell x i , whose elevation distribution refers to the observed height modeled as a normal distribution N(μ i ,∑ i ), where μ i represents the mean of the distribution N, ∑ i represents the variance of the distribution N.

Images