A three-dimensional path planning method based on point cloud map interactive passability analysis

Abstract

The application discloses a kind of three-dimensional path planning methods based on point cloud map interactive passability analysis, including the following steps: based on point cloud data, establish the grid map with multilayer structure;Based on multilayer grid map, generate three-dimensional terrain and mobile robot interactive passability map and interlayer communicability map;Add passability value as cost factor, improve A* algorithm, generate three-dimensional planning path based on point cloud map interactive passability analysis.The application can combine the passability of mobile robot and terrain information, consider the influence of cliff, steep slope, slope, chassis collision, unobserved terrain on the passability of mobile robot, and plan the shortest safe travel path of mobile robot suitable for complex three-dimensional terrain.

Description

Technical Field

[0001] The embodiments of the present invention relate to the field of robotics technology and are a three-dimensional path planning method based on interactive accessibility analysis of point cloud maps. Background Technology

[0002] With the development of computer and control technologies, mobile robots are finding increasingly wider applications, extending their working environments from structured indoor environments to various complex outdoor environments. Traditional two-dimensional obstacle avoidance path planning typically divides the map into passable and impassable zones, providing a safe path for most obstacle avoidance scenarios. Traditional path planning algorithms include the A* algorithm, D* algorithm, and artificial potential field method. However, the A* algorithm suffers from poor real-time performance; the D* algorithm is unsuitable for changes occurring on long-distance shortest paths; and the artificial potential field method is prone to getting trapped in local optima. None of these two-dimensional path planning algorithms integrate the complex three-dimensional environment with the mobile robot's mobility, making them unsuitable for path planning in complex three-dimensional terrain.

[0003] Patent publication number CN112800925A describes a deep learning-based path planning method that uses a semantic segmentation network pre-trained on an open-source dataset to perform accessibility analysis on current map information. Patent publication number CN113419539A improves the A* algorithm's cost function by introducing slope information, thereby using distance cost to find the optimal path, resulting in a smoother path. Both patents only consider terrain obstacles and slope information, neglecting the interaction between terrain and vehicle kinematics. They cannot handle common real-world scenarios in complex 3D terrain, such as vehicle rollover, chassis collisions, and wheel dangling. Summary of the Invention:

[0004] To address the problems of existing technologies, the present invention aims to overcome the shortcomings of existing technologies and provide a three-dimensional path planning method based on interactive accessibility analysis of point cloud maps. This method combines the mobility of mobile robots with the physical information of the terrain, considering the impact of cliffs, steep slopes, inclines, chassis collisions, and unobserved terrain on the accessibility of mobile robots, and plans the shortest safe travel path for mobile robots suitable for complex three-dimensional terrain.

[0005] To achieve the above-mentioned objectives, the present invention adopts the following technical solution:

[0006] A 3D path planning method based on interactive accessibility analysis of point cloud maps includes the following three steps:

[0007] The first step is to divide the XYZ value range of the point cloud data into segments based on the point cloud data and construct a raster map with a multi-layer structure.

[0008] The second step involves comprehensively analyzing the kinematics of the mobile robot and its interaction with the 3D terrain based on a multi-layered grid map. This process calculates potential hazards such as overturning, chassis collisions, and wheel suspension that may occur when the mobile robot passes through the area. The result is a passability map F-map and an inter-layer connectivity map C-map based on the interaction between the 3D terrain and the mobile robot.

[0009] The third step involves modifying the evaluation function of the A* algorithm based on the accessibility map F-map and the inter-layer connectivity map C-map, adding accessibility values ​​as a cost factor, and generating an improved A* algorithm based on interactive accessibility analysis of point cloud maps. This algorithm is then used to calculate the shortest 3D obstacle avoidance path.

[0010] Preferably, the 3D path planning method based on interactive accessibility analysis of point cloud maps of the present invention includes the following steps:

[0011] (1) Based on point cloud data, a multi-layered raster map is constructed; the XY plane is defined as a horizontal plane and the Z direction is vertically upward, dividing the three-dimensional space containing the point cloud data into uniform grids, with i and j being the indices of the grid positions in the X and Y directions, respectively; after removing grids without point clouds, at each i, j position on the XY plane, when the distance between two adjacent grids containing point clouds is greater than a set threshold h, these two grids belong to different layers, thus constructing a multi-layered raster map from the grids containing point clouds in the three-dimensional space. Each grid layer is represented by... Representation; where k is the index of the level in the Z direction; raster layer The upper surface grid is defined as The topmost uniform grid cell in its class;

[0012] (2) Based on the multi-layer grid map, the kinematics of the mobile robot and the interaction of the three-dimensional terrain are comprehensively analyzed to calculate the dangerous situations that may occur when the mobile robot passes through, such as overturning, chassis collision, and wheel suspension. A passability map F-map and an inter-layer connectivity map C-map based on the interaction of the three-dimensional terrain and the mobile robot are generated.

[0013] (2-1) The C-map, which represents the inter-layer connectivity, can be obtained through the following steps:

[0014] Define a i,j Given the point cloud data, the map grid at position i,j in the 3D space; traverse all raster layers in the multi-layer raster map. For raster layers without point clouds Do not assign a connectivity value to it; when the raster layer When there is point cloud data, use Represents grid a i,j The k-th raster layer to adjacent grid a m,n The connectivity value, where {(m,n)|m=i+1,i,i-1;n=j+1,j,j-1;m,n cannot be i and j at the same time}; It is a m,n Any raster layer at that location, and The coordinates of the upper surface grid in the Z direction are respectively and If the difference in the Z-direction between the upper surface grids of two adjacent horizontal grid layers is... Greater than the vehicle chassis height threshold c max Then it is believed and The surfaces are not connected and belong to different terrains; if a m,n No raster layer is associated with Connect C k i,j→m,n Set to 0; if a m,n There is a difference at this point Less than c max In this case, it is considered that two adjacent raster layers are connected, and Set to 1; all points in the 3D space where the point cloud data resides. That is, the interlayer connectivity map C-map that constitutes this three-dimensional space;

[0015] (2-2) Calculate the raster layers based on the multi-layer raster map. Accessibility to adjacent grid layers in the horizontal direction Represents grid a i,j The k-th raster layer to adjacent grid a m,n The connectivity value, where {(m,n)|m=i+1,i,i-1;n=j+1,j,j-1;m,n cannot be i,j at the same time}; for each There are 8 Used to evaluate the safety of a mobile robot moving in eight directions from its current location, the point cloud data contains all the data in the three-dimensional space. That is, the passability map F-map that constitutes this three-dimensional space; analyze the current raster layer. Passability value At this time, the robot is placed on the terrain, with its geometric center positioned at the grid layer. Above the upper surface grid, the robot's forward direction follows the directions of eight adjacent grids. The upper surface grid where the wheel is located is within the grid projected along the XY direction from the center of the wheel. The top surface grid is the closest top surface grid in the Z direction;

[0016] calculate The steps for obtaining the passability value are as follows:

[0017] There are four scenarios: First, if the point cloud of the projected grid in the XY direction where the robot's wheels are located is empty, the robot's accessibility at that location cannot be measured. This location and its direction of travel are considered dangerous, and the accessibility of that grid layer is marked. The maximum value η is given; in the second case, a collision occurs between the chassis and ground obstacles, and the height difference between the point cloud at the robot chassis and the fitted plane of the wheels is given. Z c This represents the normal vector of the fitted plane of the grid cells on the upper surface of all wheels of the current mobile robot. Let p be the point cloud vector of the grid cells on the upper surface where all wheels are located, and p be the point cloud vector of the upper surface grid cell whose height is closest to the geometric center of the robot within the grid range of the chassis projection in the XY direction. If the height difference between the point cloud at the robot chassis and the fitted plane of the wheels is greater than or equal to the actual chassis height H, then... Then it is considered that a collision has occurred, and the passability value will be... The value is marked as the maximum η; in the third case, the normal vector Z of the aforementioned wheel fitting plane is calculated. c The angle α between the vertical normal vector Z and the vertical normal vector Z is greater than the angle threshold. It is believed that a rollover and slippage occurred, which can be addressed through performance values. This is marked as a maximum value η; the fourth case is when none of the first three cases are satisfied. The passability value is |tanα|;

[0018] (3) Based on the accessibility map F-map and the inter-layer connectivity map C-map, this method uses an improved A* algorithm to generate the robot's planned path in three-dimensional space.

[0019] Given a starting raster and a target raster, an improved A* algorithm heuristically searches for neighboring raster layers in eight directions using inter-layer connectivity maps to expand its path; the improved A* algorithm searches the current raster layer in 3D space. When considering adjacent raster layers, only those in the C-map are considered. Value 1 Adjacent raster layers, labeled as Improved travel costs in the A* algorithm For cumulative path cost Heuristic target distance Accessibility values ​​in the accessibility map F-map The combination of these, i.e., the total travel cost. Where λ is a constant used to balance distance cost and accessibility cost, and can be adjusted according to the resolution of the accessibility map.

[0020] Compared with the prior art, the present invention has the following obvious and prominent substantive features and significant advantages:

[0021] 1. This invention constructs a passability map and an inter-layer connectivity map based on point cloud maps. These two maps comprehensively analyze the interaction between vehicle kinematics and complex terrain, and can handle the impact of cliffs, steep slopes, inclines, chassis collisions, and unobserved terrain on the passability of mobile robots. They can also conduct safety assessments that conform to the engineering practice of mobile robots for actual complex terrain.

[0022] 2. This invention improves the A* algorithm based on inter-layer connectivity information and passability values, which not only takes into account computational efficiency and path cost, but also makes it more suitable for path planning in complex 3D environments compared with traditional path planning methods, and better meets the requirements of path planning methods for mobile robot field application scenarios. Attached Figure Description

[0023] Figure 1 This is a flowchart of a 3D path planning method based on interactive accessibility analysis using point cloud maps, according to the present invention.

[0024] Figure 2 This is a passability map definition diagram for the present invention.

[0025] Figure 3 This is a simplified geometric model of the mobile robot of the present invention.

[0026] Figure 4 This is a schematic diagram of the collision of the mobile robot chassis according to the present invention.

[0027] Figure 5 This is a schematic diagram showing the tilt angle of the mobile robot of the present invention. Detailed Implementation

[0028] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0029] See Figure 1 , Figure 1 This is the main framework of the invention. First, a multi-layered grid map is generated based on point cloud data. Next, potential hazards such as overturning, chassis collision, and wheel suspension that may occur when the mobile robot passes through are calculated, generating an interactive accessibility map based on 3D terrain and the mobile robot, as well as an inter-layer connectivity map. Finally, the evaluation function of the A* algorithm is modified to generate a 3D planned path based on interactive accessibility analysis of the point cloud map. The specific steps are as follows:

[0030] The first step, based on point cloud data, defines the XY plane as a horizontal plane and the Z direction as vertically upward. The 3D space containing the point cloud data is divided into uniform grids, with i and j representing the grid positions in the X and Y directions, respectively. After removing grids without point clouds, at each i, j position on the XY plane, if the distance between two adjacent grids containing point clouds is greater than a set threshold h, these two grids belong to different layers. This constructs a multi-layered grid map from the grids containing point clouds in the 3D space. Each grid layer is... Represented by k, where k is the index of the level in the Z direction. Raster layer. The upper surface grid is defined as The topmost uniform grid cell in its class.

[0031] The second step is to construct the inter-layer connectivity map, C-map. C-map can be obtained through the following steps:

[0032] Define a i,j Let i be the map grid at position i,j in the 3D space containing the point cloud data. Traverse all raster layers in the multi-layered raster map. For raster layers without point clouds Do not assign a connectivity value to it. When the raster layer... When there is point cloud data, use Represents grid a i,j The k-th raster layer to adjacent grid a m,n The connectivity value is given by {(m,n)|m=i+1,i,i-1;n=j+1,j,j-1;m,n cannot be i and j at the same time}. It is a m,n Any raster layer at that location, and The coordinates of the upper surface grid in the Z direction are respectively and If the difference in the Z-direction between the upper surface grids of two adjacent horizontal grid layers is... Greater than the vehicle chassis height threshold c max Then it is believed and The surfaces are not connected and belong to different terrains. If a m,n No raster layer is associated with Connect C k i,j→m,n Set to 0; if a m,n There is a difference at this point Less than c max In this case, it is considered that two adjacent raster layers are connected, and C is... k i,j→m,n Set to 1. All points in the 3D space where the point cloud data resides. This refers to the interlayer connectivity map (C-map) that constitutes this three-dimensional space.

[0033] Calculate the raster layers based on a multi-layered raster map. Accessibility to adjacent grid layers in the horizontal direction Represents grid a i,j The k-th raster layer to adjacent grid a m,n The connectivity value, where {(m,n)|m=i+1,i,i-1;n=j+1,j,j-1;m,n cannot be i and j simultaneously}. For each There are 8 Used to evaluate the safety of a mobile robot moving in eight directions from its current location, the point cloud data contains all the data in the three-dimensional space. This refers to the accessibility map (F-map) that constitutes this three-dimensional space. Analyze the current raster layer. Passability value At this time, the robot is placed on the terrain, with its geometric center positioned at the grid layer. Above the upper surface grid, the robot's forward direction follows the directions of eight adjacent grids. The upper surface grid where the wheel is located is within the grid projected along the XY direction from the center of the wheel. The top surface grid is the closest top surface grid in the Z direction.

[0034] See Figure 2 , Figure 2 This is a schematic diagram of a accessibility map on the xy-plane, with the central grid in the diagram. Located in grid a i,j Place, Represents grid a i,j The k-th layer raster to adjacent grid a m,n The connectivity value is calculated, with 8 surrounding grid cells as candidate grid cells, and their angle values ​​relative to the current grid cell being φ = 0°, 45°, 90°, 135°, 180°, -45°, -90°, and -135° respectively. A simple mobile robot model is provided for reference. Figure 3 Where L is the side length of the vehicle body, W is the side width of the mobile robot, H is the height of the mobile robot chassis, and R is the wheel radius.

[0035] calculate The steps for obtaining the passability value are as follows:

[0036] There are four scenarios. In the first scenario, if the point cloud of the projected grid in the XY direction where the robot's wheels are located is empty, the robot's accessibility at that location cannot be measured. This location and its direction of travel are considered dangerous, and the accessibility of that grid layer is marked. The maximum value is η. The second case considers a collision between the chassis and ground obstacles; the height difference between the point cloud at the robot chassis and the fitted plane of the wheels is... Z c This represents the normal vector of the fitted plane of the grid cells on the upper surface of all wheels of the current mobile robot. Let be the point cloud vector of the grid cells on the upper surface where all wheels are located, and p be the point cloud vector of the upper surface grid cell whose height is closest to the robot's geometric center within the grid range projected onto the chassis in the XY directions. If the height difference between the point cloud at the robot chassis and the wheel fitting plane is greater than or equal to the actual chassis height H, i.e. Then it is considered that a collision has occurred, and the passability value will be... This is marked as the maximum value η. In the third case, the normal vector Z of the aforementioned wheel fitting plane is calculated. c The angle α between the vector and the vertical normal vector Z is referenced. Figure 5 , Figure 5 This is a schematic diagram of the tilt angle α of the mobile robot. When α is greater than the included angle threshold... It is believed that a rollover and slippage occurred, which can be addressed through performance values. This is marked as the maximum value η. The fourth case occurs when none of the first three cases are satisfied. The passability value is |tanα|.

[0037] The third step, based on the obtained F-map and C-map, employs an improved A* algorithm to generate the robot's planned path in 3D space. Given a starting grid and a target grid, the improved A* algorithm heuristically searches for adjacent grid layers in eight directions using inter-layer connectivity maps to expand the path. The improved A* algorithm searches the current grid layer in 3D space. When considering adjacent raster layers, only those in the C-map are considered. Value 1 Adjacent raster layers, labeled as Improved travel costs in the A* algorithm It is the cumulative path cost Heuristic target distance and the current travel cost in the accessibility map F-map The combination of these, i.e., the total travel cost. Here, λ is a constant used to balance distance cost and accessibility cost, and it needs to be adjusted according to the resolution of the accessibility map.

[0038] The improvements to the A* algorithm compared to the traditional A* algorithm are mainly: 1) using an inter-layer connectivity map to analyze the connectivity of adjacent grid layers; 2) using a passability map to assess the passability risk of each grid layer and its surrounding grid layers; and 3) improving the travel cost function of the A* algorithm by adding a passability value as a cost.

[0039] The above embodiments of the present invention provide a 3D path planning method based on interactive accessibility analysis of point cloud maps, comprising the following steps: establishing a multi-layered raster map based on point cloud data; generating a 3D terrain and mobile robot interactive accessibility map and an inter-layer connectivity map based on the multi-layered raster map; adding accessibility values ​​as a cost factor, improving the A* algorithm, and generating a 3D planned path based on interactive accessibility analysis of point cloud maps. The method of the above embodiments of the present invention can combine the mobile robot's mobility with terrain information, considering the impact of cliffs, steep slopes, inclines, chassis collisions, and unobserved terrain on the mobile robot's accessibility, and plan the shortest safe travel path for mobile robots suitable for complex 3D terrain.

[0040] The embodiments of the present invention have been described above in conjunction with the accompanying drawings. However, the present invention is not limited to the above embodiments. Various changes can be made according to the purpose of the invention. Any changes, modifications, substitutions, combinations or simplifications made based on the spirit and principle of the technical solution of the present invention shall be equivalent substitutions. As long as they meet the purpose of the invention and do not deviate from the technical principle and inventive concept of the present invention, they shall fall within the protection scope of the present invention.

Claims

1. A 3D path planning method based on interactive accessibility analysis of point cloud maps, characterized in that, It includes the following three steps: The first step is to divide the XYZ value range of the point cloud data into segments based on the point cloud data and construct a raster map with a multi-layer structure. The second step involves comprehensively analyzing the kinematics of the mobile robot and its interaction with the 3D terrain based on a multi-layered grid map. This process calculates potential hazards such as overturning, chassis collision, and wheel suspension that may occur when the mobile robot passes through the area. The result is a passability map F-map and an inter-layer connectivity map C-map based on the interaction between the 3D terrain and the mobile robot. The steps for constructing the inter-layer connectivity map C-map are as follows: Define a i,j Given the point cloud data, the map grid at position i,j in the 3D space; traverse all raster layers in the multi-layer raster map. For raster layers without point clouds Do not assign a connectivity value to it; when the raster layer When there is point cloud data, use Represents grid a i,j The k-th raster layer to adjacent grid a m,n The connectivity value, where {(m,n)|m=i+1,i,i-1;n=j+1,j,j-1;m,n cannot be i and j at the same time}; It is a m,n Any raster layer at that location, and The coordinates of the upper surface grid in the Z direction are respectively and If the difference in the Z-direction between the upper surface grids of two adjacent horizontal grid layers is... Greater than the vehicle chassis height threshold c max Then it is believed and The surfaces are not connected and belong to different terrains; if a m,n No raster layer is associated with Connect C k i,j→m,n Set to 0; if a m,n There is a difference at this point Less than c max In this case, it is considered that two adjacent raster layers are connected, and C is... k i,j→m,n Set to 1; all points in the 3D space where the point cloud data resides. That is, the interlayer connectivity map C-map that constitutes this three-dimensional space; The third step involves modifying the evaluation function of the A* algorithm based on the accessibility map F-map and the inter-layer connectivity map C-map, adding accessibility values ​​as a cost factor, and generating an improved A* algorithm based on interactive accessibility analysis of point cloud maps. This algorithm is then used to calculate the shortest 3D obstacle avoidance path.

2. The 3D path planning method based on interactive accessibility analysis of point cloud maps according to claim 1, characterized in that: The first step involves generating a multi-layered raster map using point cloud data, and the steps are as follows: Define the XY plane as a horizontal plane and the Z direction as vertically upward. Divide the 3D space containing the point cloud data into uniform grids, where i and j are the indices of the grid positions in the X and Y directions, respectively. After removing grids without point clouds, at each i, j position on the XY plane, if the distance between two adjacent grids containing point clouds is greater than a set threshold h, these two grids belong to different layers. This constructs a multi-layered grid map from the grids containing point clouds in the 3D space. Each grid layer is represented by... Representation; where k is the index of the level in the Z direction; raster layer The upper surface grid is defined as The topmost uniform grid cell in its class.

3. The 3D path planning method based on interactive accessibility analysis of point cloud maps according to claim 1, characterized in that: In the second step, the interactive accessibility map F-map based on point cloud data and mobile robots includes the following steps: Calculate the raster layers based on a multi-layered raster map. Accessibility to adjacent grid layers in the horizontal direction Represents grid a i,j The k-th raster layer to adjacent grid a m,n The connectivity value, where {(m,n)|m=i+1,i,i-1;n=j+1,j,j-1;m,n cannot be i,j at the same time}; for each There are 8 Used to evaluate the safety of a mobile robot moving in eight directions from its current location, the point cloud data contains all the data in the three-dimensional space. That is, the passability map F-map that constitutes this three-dimensional space; analyze the current raster layer. Passability value At this time, the robot is placed on the terrain, with its geometric center positioned at the grid layer. Above the upper surface grid, the robot's forward direction follows the directions of eight adjacent grids. The upper surface grid where the wheel is located is within the grid projected along the XY direction from the center of the wheel. The top surface grid is the closest top surface grid in the Z direction; Calculate the passability value The steps are as follows: There are four possible scenarios: First scenario: If the point cloud of the projected grid in the XY direction where the current mobile robot's wheel is located is empty, the mobility of the mobile robot at that location cannot be measured. This location and its direction of travel are considered dangerous, and the mobility of that grid layer is marked. The maximum value η; The second scenario: Considering a collision between the chassis and ground obstacles, the height difference between the point cloud at the robot chassis and the fitted plane of the wheels is... Z c This represents the normal vector of the fitted plane of the grid cells on the upper surface of all wheels of the current mobile robot. Let p be the point cloud vector of the grid cells on the upper surface where all wheels are located, and p be the point cloud vector of the upper surface grid cell whose height is closest to the geometric center of the robot within the grid range of the chassis projection in the XY direction. If the height difference between the point cloud at the robot chassis and the fitted plane of the wheels is greater than or equal to the actual chassis height H, then... Then it is considered that a collision has occurred, and the passability value will be... Marked as the maximum value η; The third scenario: Calculate the normal vector Z of the aforementioned wheel fitting plane. c The angle α between the vertical normal vector Z and the vertical normal vector Z is greater than the angle threshold. It is believed that a rollover and slippage occurred, which can be addressed through performance values. Marked as the maximum value η; The fourth scenario: when none of the first three scenarios are met. The passability value is |tan α|.

4. The 3D path planning method based on interactive accessibility analysis of point cloud maps according to claim 1, characterized in that: In the third step, the 3D path planning steps based on interactive accessibility analysis of point cloud maps are as follows: First, the improved A* algorithm searches for the current raster layer in three-dimensional space. When considering adjacent raster layers, only those in the C-map are considered. Value 1 Adjacent raster layers, labeled as At the same time, improve the travel cost in the A* algorithm For cumulative path cost Heuristic target distance Accessibility values ​​in the accessibility map F-map The combination of these, i.e., the total travel cost. Where λ is a constant used to balance distance cost and accessibility cost, and is adjusted according to the resolution of the accessibility map.

Images