Probability route map path planning method and device

Abstract

The invention discloses a probabilistic route map path planning method and device. The method comprises the following steps: S1, acquiring global position information and obstacle information of an automatic driving vehicle; s2, sampling to obtain candidate path nodes; s3, constructing a covariance matrix; s4, calculating the mahalanobis distance between each candidate path node and the barrier distribution; s5, discarding sampling points whose mahalanobis distances are smaller than a set safety threshold, and performing preliminary screening on candidate path nodes; s6, constructing a probability route map based on the screened candidate path nodes; and S7, completing path planning. According to the method, useless and low-efficiency sampling nodes can be accurately eliminated, so that the construction of the probabilistic route map is simpler and more efficient, and the construction quality and effect of the probabilistic route map are greatly improved; meanwhile, interference and calculation burden caused by invalid nodes are reduced, and the efficiency of path planning is remarkably improved; in addition, waste of computer resources is reduced, and resource utilization efficiency is improved.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and specifically to a probabilistic route planning method and apparatus. Background Technology

[0002] With the rapid development of artificial intelligence, sensor technology, and computing power, autonomous driving technology has become a current research hotspot, demonstrating enormous application potential in logistics and transportation, intelligent transportation, and unmanned operations in specific areas (such as industrial parks, smart communities, ports, and airports). Path planning, as one of the core modules of an autonomous driving system, aims to generate a feasible, safe, and optimal driving path given a starting point and destination, and considering obstacles in the environment, traffic rules, and the vehicle's own constraints.

[0003] In the field of autonomous driving, path planning is one of the key technologies. The Probabilistic Roadmap (PRM) method is widely used in path planning because of its advantages in finding feasible paths in high-dimensional spaces and complex environments. PRM generates nodes by randomly sampling in free space, connects these nodes to form a graph, and then uses a graph search algorithm to find paths on the graph.

[0004] However, in park environments, roads are typically narrow and winding, with irregular obstacles such as pedestrians, non-motorized vehicles, and temporarily parked vehicles, posing numerous challenges to traditional PRM systems. To ensure sufficient connectivity and complete environmental coverage, traditional PRM systems in complex or narrow park environments often require large-scale random sampling. This means the system must generate and process a large number of sampled nodes to construct a route map that fully represents the traversable space. Collision detection and nearest neighbor search must be performed independently for each sampled node. Due to the large number of nodes in complex park environments and the complexity of the detection and search operations themselves, this process consumes significant computational resources and time. Especially in high-dimensional or large-scale planning spaces, the number of nodes increases dramatically, causing a sharp decline in planning efficiency.

[0005] Furthermore, in a park environment, many randomly generated sampling points may unfortunately fall inside obstacles, in extremely dangerous areas at the edges of obstacles, or in isolated small patches of free space. These points are invalid or inefficient; they cannot be used to construct safe and feasible paths, yet they still require collision detection, nearest neighbor search, and other processing, undoubtedly wasting computational resources. Therefore, traditional probabilistic route graph methods have significant shortcomings in path planning efficiency and resource utilization in park environments and urgently need improvement. Summary of the Invention

[0006] The purpose of this invention is to address the shortcomings of existing technologies by providing a probabilistic route map path planning method to avoid the technical problems of wasted computer resources and low planning efficiency caused by generating and processing a large number of invalid or inefficient sampling nodes in complex park environments, especially when facing unevenly distributed obstacles.

[0007] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0008] A probabilistic route planning method, comprising:

[0009] S1, obtain global position information and obstacle information of autonomous vehicles;

[0010] S2, sample and obtain candidate path nodes;

[0011] S3, constructing the covariance matrix based on obstacle information;

[0012] S4, based on the covariance matrix, calculates the Mahalanobis distance between each candidate path node and the obstacle distribution;

[0013] S5. Based on the calculated Mahalanobis distance, discard sampling points whose Mahalanobis distance is less than the set safety threshold to perform preliminary screening of candidate path nodes;

[0014] S6, Construct a probabilistic route map based on the filtered candidate path nodes, including:

[0015] S6-1, the sampling points that have been filtered by Mahalanobis distance and retained are used as the node set of the probability roadmap;

[0016] S6-2, for each node in the node set, find the K nearest neighbor nodes as potential connection objects by performing K nearest neighbor search;

[0017] S6-3, Perform geometric collision detection for each pair of potential connected nodes:

[0018] If a geometric collision exists, the connection is discarded;

[0019] If geometric collision detection is passed, multiple sub-sampling points are generated along the path segment, and the Mahalanobis distance between these sub-sampling points and the obstacle distribution is calculated. The path segment is assigned a risk cost based on the sum of all Mahalanobis distances of the path segment.

[0020] S6-4 Calculate the possible rate of change of heading angle of autonomous vehicles on the path segment connecting two nodes. If it exceeds the preset maximum steering capability or comfort threshold of the vehicle, then exclude the connection.

[0021] Return to step S6-2 and iteratively construct the probability roadmap;

[0022] S7, complete path planning.

[0023] In some embodiments, the method for calculating the covariance matrix ∑ in step S3 is as follows:

[0024]

[0025] Where, x i The coordinate vector representing the i-th obstacle point The total number of obstacle points is N. It is the mean vector of the coordinates of all obstacle points.

[0026] In some embodiments, the method for calculating the Mahalanobis distance in step S4 is as follows:

[0027]

[0028] Where, ∑ -1 It is the inverse of the covariance matrix ∑ constructed in step S3, D M Represents each sampling point P s Mahalanobis distance between the location of obstacles and the distribution of obstacles.

[0029] In some embodiments, the method for calculating the Mahalanobis distance in step S4 is as follows:

[0030]

[0031] Among them, D' M Represents each sampling point P s Mahalanobis distance between the location of obstacles and the distribution of obstacles; w pose (θ i ,θ ego ) is a scaling function whose input includes obstacle pose information θ i and the heading angle θ of autonomous vehicles ego obstacle attitude θ i With the current heading angle θ of the autonomous vehicle ego The smaller the angular difference between them, the greater the scaling function w pose (θ i ,θ ego The smaller the value of ), the lower the potential collision risk indicated by the Mahalanobis distance.

[0032] In some embodiments, the scaling function w pose (θ i ,θ ego The calculation method for ) is as follows:

[0033]

[0034] In some embodiments, in step S2, the starting point and ending point of the planned path are set according to the current location and destination of the autonomous vehicle and the park map information, a park planning range is determined, random sampling is performed within the park planning range, a large number of candidate path nodes are obtained by uniform random sampling, and sampling points that fall inside known obstacle entities are excluded by geometric collision detection method.

[0035] In some embodiments, in step S6-3, the smaller the sum of Mahalanobis distances, the closer the path segment is to the dangerous area where obstacles are distributed, and the higher the corresponding risk cost.

[0036] In some embodiments, in step S6-4, the preset maximum steering capability or comfort threshold of the vehicle is determined based on the vehicle wheelbase and minimum turning radius.

[0037] In some embodiments, in step S1, the global position information includes the horizontal and vertical coordinates of the autonomous vehicle in the global coordinate system, the current heading angle of the autonomous vehicle, and the destination information, and the obstacle information includes the horizontal and vertical coordinates of the obstacle and its heading angle at the current moment.

[0038] The present invention also provides a probabilistic route map path planning device, comprising:

[0039] The information acquisition module is used to acquire the global position information and obstacle information of the autonomous vehicle;

[0040] The candidate path node generation module is used to set the start and end points of the planned path based on the current location, destination, and park map information of the autonomous vehicle, determine the park planning range, and perform random sampling within this range to generate the coordinates of candidate path nodes, while initially excluding sampling points that fall inside obstacles.

[0041] The covariance matrix construction module is used to construct a covariance matrix to characterize the statistical properties of obstacle distribution;

[0042] The Mahalanobis distance calculation module is used to calculate the Mahalanobis distance between each candidate path node and the obstacle distribution based on the covariance matrix.

[0043] The preliminary screening module for candidate path nodes is used to perform preliminary screening of candidate path nodes based on the calculated Mahalanobis distance.

[0044] The probabilistic route map construction module uses the sampled points filtered by Mahalanobis distance and retained as the node set of the probabilistic route map. For each node in the node set, it performs a K nearest neighbor search to find the K nearest other nodes as potential connection objects. For each pair of potential connection nodes, it performs geometric collision detection: if a geometric collision exists, the connection is discarded; if the geometric collision detection passes, multiple sub-sampled points are generated along the path segment, and the Mahalanobis distance between these sub-sampled points and the obstacle distribution is calculated. The risk cost of the path segment is assigned based on the sum of all Mahalanobis distances of the path segment. The module calculates the possible rate of change of heading angle of the vehicle on the path segment connecting the two nodes. If it exceeds the preset maximum steering capability or comfort threshold of the vehicle, the connection is excluded. The probabilistic route map is constructed iteratively.

[0045] The optimal path generation module is used to search for the optimal path from the starting point to the destination on a probabilistic route map by using the A* algorithm and combining the risk cost of the path segments.

[0046] Due to the application of the above technical solution, the present invention has the following advantages compared with the prior art: The probabilistic route map path planning method of the present invention has the following advantages:

[0047] (1) By constructing a covariance matrix using obstacle locations and combining random sampling to calculate the Mahalanobis distance between each sampling point and the obstacle distribution, this invention can accurately eliminate useless and inefficient sampling nodes. These nodes would occupy a lot of computing resources in traditional methods, but this invention effectively removes them, making the construction of probability roadmaps more concise and efficient, and greatly improving the construction quality and effect of probability roadmaps.

[0048] (2) After selecting reasonable and efficient safe sampling nodes through Mahalanobis distance and vehicle dynamics constraints, the probabilistic route map constructed by this invention is more optimized. Path planning in such a probabilistic route map reduces the interference and computational burden caused by invalid nodes, significantly improves the efficiency of path planning, and can plan safe and feasible paths in a shorter time, especially in large-scale park planning spaces;

[0049] (3) This invention avoids collision detection and nearest neighbor search for a large number of invalid or inefficient sampling nodes, thereby reducing the waste of computer resources and improving resource utilization efficiency. While meeting the needs of autonomous driving path planning in the park, it reduces the requirements for hardware computing resources, enabling the system to operate more efficiently. Attached Figure Description

[0050] Figure 1 This is the overall flowchart of the probabilistic route map path planning method in this embodiment.

[0051] Figure 2 A traditional probability roadmap generated for simulation.

[0052] Figure 3 This is a probability route map generated by simulation in this embodiment, incorporating Mahalanobis distance.

[0053] Figure 4 This is the path planning result of the probability route map combined with Mahalanobis distance generated in the simulation of this embodiment. Detailed Implementation

[0054] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0055] In the description of this invention, it should be noted that the terms "center," "upper," "lower," "left," "right," "vertical," "horizontal," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, or the orientation or positional relationship commonly used when the product of this invention is in use. They are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this invention. In addition, the terms "first," "second," and "third," etc., are only used to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0056] The flowchart of the probabilistic route map path planning method in this embodiment is as follows: Figure 1 As shown, the specific steps include the following:

[0057] S1 acquires the global location information and obstacle information of the autonomous vehicle.

[0058] Global position information can be obtained by inertial navigation units, which provide the starting point for the entire path planning process and aim to obtain the current state of the autonomous vehicle itself.

[0059] When an autonomous driving system initiates a path planning task, it first needs to determine the starting point and the desired destination of the autonomous vehicle. Global location information typically includes the autonomous vehicle's x and y coordinates in the global coordinate system (e.g., east coordinate E and north coordinate N), the vehicle's current heading angle (e.g., the angle between the vehicle's longitudinal axis and true north), and destination information. This information can be obtained through various fusion methods, such as map data, GPS / RTK positioning systems, inertial navigation units (IMUs), and the vehicle's own attitude sensors, to ensure the accuracy and real-time nature of the position and attitude information.

[0060] To ensure safe path planning, the system needs to perceive obstacles around the vehicle in real time. Obstacle information includes the obstacle's horizontal and vertical coordinates and its heading angle at the current moment. In this embodiment, obstacle information in the environment can be acquired using LiDAR (Light Detection and Ranging) to provide the necessary data foundation for subsequent planning. LiDAR can scan the environment with high precision, generating dense point cloud data. After processing and clustering, these point clouds can identify individual obstacle entities and extract the horizontal and vertical coordinates and heading angle of each obstacle at the current moment. If the obstacle is a moving object or has a clear direction, such as other vehicles or pedestrians, this obstacle information will serve as the basis for constructing the covariance matrix.

[0061] Step S2: Generate the coordinates of candidate path nodes.

[0062] Based on environmental characteristics, a series of candidate path node coordinates are efficiently generated. This aims to avoid generating nodes in invalid or high-risk areas, thereby improving the efficiency and safety of subsequent route map construction. Specifically, this includes:

[0063] Based on the current location and destination of the autonomous vehicle, as well as the park map information, the starting point (e.g., the vehicle's current location) and ending point of the planned path are set, defining a park planning area. This area can be the maximum perception range that the vehicle can traverse, or a pre-defined task area. Random sampling is performed within the park planning area, using a uniform random sampling method to obtain a large number of candidate path nodes. Uniform sampling ensures broad coverage throughout the entire planning space, providing a sufficient number of potential intermediate points for subsequent path searching.

[0064] During random sampling, the total number of sampling points is determined based on the size and complexity of the park's planned area. For example, for more complex or narrow areas, the number of sampling points can be appropriately increased to improve connectivity.

[0065] Meanwhile, to improve efficiency, after generating initial candidate path nodes through random sampling, sampling points that clearly fall inside known obstacle entities are initially excluded. This exclusion can be achieved through geometric collision detection, such as collision detection based on the intersection of a point and the bounding box of an obstacle, to avoid generating nodes that will inevitably become invalid later, thus reducing unnecessary computational burden.

[0066] Each retained sampling point, as a candidate path node, is denoted below as the s-th sampling point P. s Except for P s =[x s ,y s ] T In addition to the horizontal and vertical coordinates, T represents the matrix transpose, and can also be randomly assigned a heading angle to construct a multi-dimensional path planning state.

[0067] Step S3: Construct the covariance matrix ∑.

[0068] Based on the obstacle information obtained in step S1, a covariance matrix ∑ is constructed. The covariance matrix ∑ represents the statistical characteristics of the obstacle distribution. The calculation formula of the covariance matrix ∑ is shown in equation (1).

[0069]

[0070] Where, x i The coordinate vector representing the i-th obstacle point The total number of obstacle points is N. It is the mean vector of the coordinates of all obstacle points.

[0071] By constructing a covariance matrix ∑, we can quantify the dispersion of obstacle data in the park environment and the correlation between various coordinate dimensions.

[0072] By constructing the covariance matrix, a statistical model describing the distribution of obstacles can be obtained, rather than simply treating them as discrete points.

[0073] Step S4: Calculate the Mahalanobis distance between each candidate path node and the obstacle distribution.

[0074] In one embodiment, the s-th candidate path node P is calculated based on equation (2) according to the covariance matrix ∑. s Mahalanobis distance between the obstacle distribution and the obstacle distribution:

[0075]

[0076] Where, ∑ -1 It is the inverse of the covariance matrix ∑ constructed in step S3, D M Represents each sampling point P s Mahalanobis distance.

[0077] Mahalanobis distance D M The smaller the value, the closer the sampling point is to the center of the obstacle distribution or a high-density area, and the higher the potential collision risk; Mahalanobis distance D M The larger the value, the farther the sampling point is from the edge of the obstacle distribution, and the lower the collision risk.

[0078] In another embodiment, when calculating the Mahalanobis distance between candidate path nodes and obstacle distribution, the obstacle pose information θ i It is used to scale the calculation of Mahalanobis distance, which can more accurately reflect the danger zone of the obstacle. Specifically, the formula for calculating Mahalanobis distance can be adjusted to Equation (3).

[0079]

[0080] Among them, D' M Represents each sampling point P s Mahalanobis distance; w pose (θ i ,θ ego ) is a scaling function whose input includes obstacle pose information θ i and the vehicle's heading angle θ ego .

[0081] When the obstacle's attitude θ i With the current heading angle θ of the autonomous vehicle ego The smaller the angular difference between them, the greater the scaling function w pose (θ i ,θ ego The smaller the value of ), the lower the potential collision risk indicated by the Mahalanobis distance. Based on this principle, the scaling function can be, but is not limited to, described by equation (4).

[0082]

[0083] Step S5: Initial screening of candidate path nodes.

[0084] Based on the Mahalanobis distance calculated in step S4, candidate path nodes are initially screened, discarding sampling points whose Mahalanobis distance is less than a set safety threshold. This safety threshold can be set according to actual conditions, such as the width of a vehicle, or less than 1 meter wider than the vehicle; for example, the safety threshold is equal to the vehicle width plus 0.5 meters. This means that sampling points considered to be located inside obstacles, tightly surrounded areas, or highly dangerous areas will be excluded, thus avoiding placing nodes inside obstacles or in overly dangerous areas, significantly improving the quality and safety of subsequent probabilistic route map nodes. In this way, this embodiment can more intelligently utilize the sampling space, generate more effective nodes, and improve the construction efficiency of the probabilistic route map.

[0085] Step S6: Construct a probabilistic route map of the passable space.

[0086] A probabilistic route map representing the passable space is constructed using the selected candidate path nodes. This construction process considers node connectivity and safety, and incorporates risk assessment based on Mahalanobis distance and vehicle dynamics constraints. Specifically, it includes the following steps:

[0087] S6-1, Determine the set of nodes (Nodes).

[0088] The sampling points in S5 that have been filtered and retained using Mahalanobis distance are used as the node set for constructing the probabilistic route map. These nodes represent safe and usable discrete locations within the campus environment.

[0089] S6-2, proximity search and generation of potential connections.

[0090] For each node in the node set, a K nearest neighbor search is performed to find the K nearest other nodes as potential connection objects. KNN (K-Nearest Neighbors) is an efficient proximity search method that helps to find enough connection possibilities for each node with reasonable computational complexity, so as to form a connected route map later.

[0091] S6-3 performs geometric collision detection for each pair of potential connected nodes and assigns a risk cost.

[0092] For each pair of potential connected nodes found by KNN, it is necessary to verify whether the direct path segment between them is traversable.

[0093] First, perform basic geometric collision detection to ensure that the path segment does not physically overlap with or penetrate any known obstacle entities:

[0094] If a geometric collision exists, the potential connection is discarded.

[0095] If geometric collision detection is successful, a more detailed risk assessment is then performed on the path segments that pass the geometric collision detection using Mahalanobis distance. The specific assessment method is as follows:

[0096] (1) Generate multiple sub-sampling points along the path segment formed between each pair of connecting nodes;

[0097] (2) Using the covariance matrix constructed in step S3, calculate the Mahalanobis distance between each sub-sampling point in each path segment and the distribution of obstacles in step S4.

[0098] (3) Assign a risk cost to each path segment based on the sum of all Mahalanobis distances. The smaller the sum of Mahalanobis distances, the closer the path segment is to the dangerous area where obstacles are distributed, and the higher its corresponding risk cost. This risk cost will serve as an important basis for selecting the optimal path in subsequent path searches, guiding vehicles to avoid high-risk areas.

[0099] S6-4, limits the rate of change of heading angle.

[0100] In addition to considering the connection between two nodes and performing collision detection, this embodiment also introduces autonomous vehicle dynamics constraints. The rate of change of heading angle that the autonomous vehicle might experience on the path segment connecting the two nodes is calculated. If this rate of change exceeds a preset maximum vehicle steering capability or comfort threshold, the connection is considered infeasible, even if it is geometrically collision-free and the Mahalanobis distance risk is acceptable, and is therefore excluded. This ensures that the generated path is not only safe but also smooth and conforms to the actual driving characteristics of autonomous vehicles.

[0101] The preset maximum steering capability or comfort threshold δ of the vehicle can be described by equation (5).

[0102]

[0103] Where L is the vehicle wheelbase, R min This is the minimum turning radius.

[0104] Return to step S6-2 and repeat steps S6-3 and S6-4 above to iteratively construct the probability roadmap until the starting point and the ending point are connected.

[0105] Each iteration attempts to increase the probability of a route being identified as a safe node and connection, in order to improve its coverage and connectivity. For example, for each node in the node set, the K nearest neighbors of each sample point are found through proximity search.

[0106] Step S7: Complete path planning.

[0107] On the probabilistic roadmap, the A* algorithm is used, combined with the risk cost of the path segments, to search for and obtain an optimal path from the starting point to the destination.

[0108] The A* algorithm is a highly efficient heuristic search algorithm that evaluates the priority of each node by combining the actual cost from the starting point to the current node and the estimated cost from the current node to the target node, thereby efficiently finding the optimal path. The risk cost is the magnitude of the Mahalanobis distance; the closer to an area with dense obstacles, the smaller the Mahalanobis distance and the greater the cost.

[0109] like Figure 2 It is a simulation probability roadmap constructed using traditional methods, such as Figure 3 This is a simulation probability roadmap constructed using the method described in this embodiment, such as... Figure 4 This is the planning result of the simulation probability roadmap constructed using the method of this embodiment.

[0110] The probabilistic route planning device in this embodiment includes an information acquisition module, a candidate path node generation module, a covariance matrix construction module, a Mahalanobis distance calculation module, a preliminary candidate path node screening module, a probabilistic route map construction module, and an optimal path generation module.

[0111] The information acquisition module is used to acquire global location information and obstacle information of autonomous vehicles. The information acquisition module includes one or more of the following: map data, GPS / RTK positioning system, inertial navigation unit (IMU) and vehicle attitude sensor, etc., for acquiring global location information of autonomous vehicles. The information acquisition module also includes LiDAR for acquiring obstacle information.

[0112] The candidate path node generation module is used to set the start and end points of the planned path based on the current location, destination, and park map information of the autonomous vehicle, determine the park planning range, and perform random sampling within this range to generate the coordinates of candidate path nodes, while initially excluding sampling points that fall inside obstacles.

[0113] The covariance matrix construction module is used to construct the covariance matrix, thereby characterizing the statistical properties of obstacle distribution.

[0114] The Mahalanobis distance calculation module is used to calculate the Mahalanobis distance between each candidate path node and the obstacle distribution based on the covariance matrix.

[0115] The preliminary screening module for candidate path nodes is used to perform preliminary screening of candidate path nodes based on the calculated Mahalanobis distance.

[0116] The probabilistic route map construction module uses the sampled points filtered and retained by Mahalanobis distance as the node set of the probabilistic route map. For each node in the node set, a K nearest neighbor search is performed to find the K nearest other nodes as potential connection objects. For each pair of potential connection nodes, geometric collision detection is performed: if a geometric collision exists, the connection is discarded; if the geometric collision detection is successful, multiple sub-sampled points are generated along the path segment, and the Mahalanobis distance between these sub-sampled points and the obstacle distribution is calculated. The risk cost of the path segment is assigned based on the sum of all Mahalanobis distances of the path segment. The possible rate of change of heading angle of the vehicle on the path segment connecting the two nodes is calculated. If it exceeds the preset maximum steering capability or comfort threshold of the vehicle, the connection is excluded. The probabilistic route map is constructed iteratively.

[0117] The optimal path generation module is used to search for the optimal path from the starting point to the destination on a probabilistic route map, using the A* algorithm and combining the risk cost of the path segments.

[0118] The above embodiments are only for illustrating the technical concept and features of the present invention, and are intended to enable those skilled in the art to understand the content of the present invention and implement it accordingly. They should not be construed as limiting the scope of protection of the present invention. All equivalent changes or modifications made in accordance with the spirit and essence of the present invention should be covered within the scope of protection of the present invention.

Claims

1. A probabilistic route map path planning method, characterized in that: include: S1, obtain global position information and obstacle information of autonomous vehicles; S2, sample and obtain candidate path nodes; S3, constructing the covariance matrix based on obstacle information; S4, based on the covariance matrix, calculates the Mahalanobis distance between each candidate path node and the obstacle distribution; S5. Based on the calculated Mahalanobis distance, discard sampling points whose Mahalanobis distance is less than the set safety threshold to perform preliminary screening of candidate path nodes; S6, Construct a probabilistic route map based on the filtered candidate path nodes, including: S6-1, the sampling points that have been filtered by Mahalanobis distance and retained are used as the node set of the probability roadmap; S6-2, for each node in the node set, find the K nearest neighbor nodes as potential connection objects by performing K nearest neighbor search; S6-3, Perform geometric collision detection for each pair of potential connected nodes: If a geometric collision exists, the connection is discarded; If geometric collision detection is passed, multiple sub-sampling points are generated along the path segment, and the Mahalanobis distance between these sub-sampling points and the obstacle distribution is calculated. The path segment is assigned a risk cost based on the sum of all Mahalanobis distances of the path segment. S6-4 Calculate the possible rate of change of heading angle of autonomous vehicles on the path segment connecting two nodes. If it exceeds the preset maximum steering capability or comfort threshold of the vehicle, then exclude the connection. Return to step S6-2 and iteratively construct the probability roadmap; S7, complete path planning.

2. The probabilistic route planning method according to claim 1, characterized in that: The method for calculating the covariance matrix ∑ in step S3 is as follows: Where, x i The coordinate vector representing the i-th obstacle point The total number of obstacle points is N. It is the mean vector of the coordinates of all obstacle points.

3. The probabilistic route planning method according to claim 2, characterized in that: The method for calculating the Mahalanobis distance in step S4 is as follows: Where, ∑ -1 It is the inverse of the covariance matrix ∑ constructed in step S3, D M Represents each sampling point P s Mahalanobis distance between the location of obstacles and the distribution of obstacles.

4. The probabilistic route planning method according to claim 2, characterized in that: The method for calculating the Mahalanobis distance in step S4 is as follows: Among them, D' M Represents each sampling point P s Mahalanobis distance between the location of obstacles and the distribution of obstacles; w pose (θ i ,θ ego ) is a scaling function whose input includes obstacle pose information θ i and the heading angle θ of autonomous vehicles ego obstacle attitude θ i With the current heading angle θ of the autonomous vehicle ego The smaller the angular difference between them, the greater the scaling function w pose (θ i ,θ ego The smaller the value of ), the lower the potential collision risk indicated by the Mahalanobis distance.

5. The probabilistic route planning method according to claim 4, characterized in that: Scaling function w pose (θ i ,θ ego The calculation method for ) is as follows:

6. The probabilistic route planning method according to claim 1, characterized in that: In step S2, based on the current location and destination of the autonomous vehicle and the park map information, the start and end points of the planned path are set, a park planning area is determined, random sampling is performed within the park planning area, a large number of candidate path nodes are obtained by uniform random sampling, and sampling points that fall inside known obstacle entities are excluded by geometric collision detection method.

7. The probabilistic route planning method according to claim 1, characterized in that: In step S6-3, the smaller the sum of the Mahalanobis distances, the closer the path segment is to the dangerous area where obstacles are distributed, and the higher the corresponding risk cost.

8. The probabilistic route planning method according to claim 1, characterized in that: In step S6-4, the preset maximum steering capability or comfort threshold of the vehicle is determined based on the vehicle wheelbase and minimum turning radius.

9. The probabilistic route planning method according to claim 1, characterized in that: In step S1, the global position information includes the horizontal and vertical coordinates of the autonomous vehicle in the global coordinate system, the current heading angle of the autonomous vehicle, and the destination information. The obstacle information includes the horizontal and vertical coordinates of the obstacle and its heading angle at the current moment.

10. A probabilistic route planning device, characterized in that: include: The information acquisition module is used to acquire the global position information and obstacle information of the autonomous vehicle; The candidate path node generation module is used to set the start and end points of the planned path based on the current location, destination, and park map information of the autonomous vehicle, determine the park planning range, and perform random sampling within this range to generate the coordinates of candidate path nodes, while initially excluding sampling points that fall inside obstacles. The covariance matrix construction module is used to construct a covariance matrix to characterize the statistical properties of obstacle distribution; The Mahalanobis distance calculation module is used to calculate the Mahalanobis distance between each candidate path node and the obstacle distribution based on the covariance matrix. The preliminary screening module for candidate path nodes is used to perform preliminary screening of candidate path nodes based on the calculated Mahalanobis distance. The probabilistic route map construction module is used to select the sampled points that have been filtered and retained by Mahalanobis distance as the node set of the probabilistic route map. For each node in the node set, the K nearest neighbor search is performed to find the K other nodes that are closest to it as potential connection objects. For each pair of potential connection nodes, geometric collision detection is performed: if a geometric collision exists, the connection is discarded; if the geometric collision detection is successful, multiple sub-sampled points are generated along the path segment, and the Mahalanobis distance between these sub-sampled points and the distribution of obstacles is calculated. The risk cost is assigned to the path segment based on the sum of all Mahalanobis distances of the path segment. Calculate the possible rate of change of heading angle of vehicles on the path segment connecting two nodes. If it exceeds the preset maximum steering capability or comfort threshold of the vehicle, the connection is excluded. A probabilistic route map is constructed iteratively. The optimal path generation module is used to search for the optimal path from the starting point to the destination on a probabilistic route map by using the A* algorithm and combining the risk cost of the path segments.

Images