Path planning method and device applied to automatic parking, equipment and storage medium

Abstract

The invention provides a path planning method and device applied to automatic parking, equipment and a storage medium. Relates to the technical field of parking control. The method comprises the steps of performing iteration by taking a parking starting point as a starting point based on scene information and coordinate information of a vehicle, and establishing a random tree; according to the index points, the obstacle information and the target offset, the sampling probability of each sampling area is calculated, and a probability map is formed; sampling on the probability map to obtain a random point, traversing nodes on the random tree, respectively calculating the distance between each node and the random point, and taking the node closest to the random point on the random tree as a near point; expanding a new point from the near point in the direction of the random point in an expansion step length, and adding the new point into the random tree for node reconnection; and calculating the distance between the new point and the target point, judging whether iteration is terminated or not, performing path smooth optimization on an obtained optimal path after iteration is terminated, and outputting a parking path, thereby realizing automatic parking path planning in a complex environment.

Description

Technical Field

[0001] The present application relates to the field of parking control technology, and in particular to a path planning method, device, equipment and storage medium for automatic parking. Background Art

[0002] With the surge in the number of motor vehicles and the scarcity of parking space resources, we are inevitably faced with problems such as traffic congestion and parking difficulties. On the one hand, the number of parking spaces planned by the municipal government and the existing parking lots cannot meet the parking needs of many motor vehicles for the time being, which aggravates the contradiction between the supply and demand of urban cars and parking spaces, further leading to narrow parking spaces and complex parking environments; on the other hand, there are certain technical difficulties in parking operations. When parking, the driver's vision is blocked and there are blind spots in the field of vision. He cannot fully grasp the situation around the parking space. Drivers who are not proficient in parking skills need more parking time and road space, which may cause road traffic congestion and easily cause parking safety accidents. Therefore, the study of automatic parking path planning in complex parking environments has important theoretical significance and practical value.

[0003] Currently, the automatic parking path planning algorithms are as follows:

[0004] Graph search algorithms represented by the A* algorithm are based on grid maps and use a breadth-first or depth-first strategy to search for the node with the lowest target cost. The current node serves as the parent node of the next expanded node, and is eventually connected into a complete parking path.

[0005] The random sampling algorithm represented by the RRT algorithm gradually iterates and generates a search tree through random sampling increments and checks the feasibility of the nodes. When the node on the tree approaches the target point, the parking path is determined by backtracking to the parent node.

[0006] The path planning of the A* algorithm can generally obtain the shortest or suboptimal parking path and can achieve obstacle avoidance, but the solution accuracy and real-time performance are limited by the grid resolution. Due to its heuristic greed, it is easy to fall into the local optimum. The RRT algorithm has the property of probabilistic completeness and can also achieve obstacle avoidance, but its search is blind and has greater limitations in narrow scenes.

[0007] During the automatic parking process, vehicles often face various complex parking environments, such as disorderly parking of roadside vehicles, incorrect parking within the parking space lines, and static obstacles such as stone piers and traffic cones around parking spaces, as well as dynamic obstacles such as pedestrians that may appear, which together constitute a complex and changeable parking environment. In addition, narrow parking spaces, unconventional parking space designs, and spatial limitations of parking lanes further increase the complexity of the parking environment. Faced with such a complex parking environment, existing automatic parking systems often find it difficult to cope, resulting in the failure of parking path planning, and the driver must intervene to take over. Therefore, how to achieve automatic parking path planning in complex parking environments is a technical problem that urgently needs to be solved. Summary of the invention

[0008] The present application provides a path planning method, device, equipment and storage medium for automatic parking. By improving the path cost function of the RS curve, different penalty coefficients are applied according to the quality of the path, and the curvature penalty term is added to reduce the frequency of reversing, shifting and turning of the RS curve; a probability map is constructed as a sampling method of the RRT* algorithm, and the random tree generation situation is characterized by multiple indicators, and the various cost data of the search space grid points and the indicator points are calculated, and converted into sampling probabilities based on Gaussian distribution, the utilization rate of the sampling points is optimized, and the search efficiency and convergence speed of the algorithm are improved; a forgetting mechanism measured by fault tolerance is used to avoid path planning from entering a local dead zone (local optimum) and causing planning failure, thereby improving adaptability.

[0009] In a first aspect, the present application provides a path planning method for automatic parking, comprising:

[0010] Acquire scene information, the scene information including target parking space, obstacle information, parking starting point and target point posture, establish a kinematic equation of the vehicle, and determine the coordinate information of the vehicle based on the kinematic equation;

[0011] Based on the scene information and the coordinate information of the vehicle, iterate with the parking starting point as the starting point to establish a random tree; the random tree includes a plurality of nodes, each node stores node information, and the node information includes a node sequence number, a node coordinate, a parent node sequence number, a node heading angle, a node cost, and a node total cost; wherein the node cost is the RS cost between the node and the parent node, and the total cost is the RS cost from the node to the starting point;

[0012] Based on the random tree, the index points are obtained, the entire sampling space is rasterized and divided into multiple independent sampling areas, and the sampling probability of each sampling area is calculated according to the index points, obstacle information and target bias to form a probability map;

[0013] Sampling is performed on the probability map to obtain a random point, traversing the nodes on the random tree, respectively calculating the distance between each node and the random point, and taking the node on the random tree closest to the random point as the near point;

[0014] Expand the near point to a new point in the direction of the random point with an expansion step, and add the new point to the random tree;

[0015] For new points, search for nodes within the set range and calculate the RS cost to each selected node. Determine the path formed by the new point as the parent node of each node to minimize the cost from the starting point to each node. Update the parent node with a smaller cost for each node, expand it with the RS curve, perform the shortest path optimization, and obtain the optimal path.

[0016] The distance between the new point and the target point is calculated. If the distance is greater than the termination distance threshold, re-sampling is performed on the probability map, and iteration is performed to plan the path. If the distance is less than or equal to the termination distance threshold, the target point is added to the random tree, and the iteration is terminated. The optimal path is smoothed and optimized through multiple B-spline curves, and the parking path is output.

[0017] In one possible design, the kinematic equations of the vehicle are formulated as follows:

[0018] The vehicle is simplified into a two-wheel model using the Ackerman steering principle, and kinematic analysis is performed to establish the vehicle's kinematic equations.

[0019] The established kinematic equation is expressed as:

[0020]

[0021] In the formula, and are the speed in the x-axis direction, the speed in the y-axis direction and the angular velocity of the heading angle, θ is the vehicle heading angle, δ is the equivalent turning angle of the front wheel, l is the vehicle wheelbase, and v is the speed of the center point of the rear axle of the vehicle.

[0022] In a possible design, the index point includes a first index point, a second index point, a third index point and a fourth index point, and obtaining the index point based on the random tree includes:

[0023] The node closest to the target point on the random tree is taken as the first index point, and the node closest to the target point on the random tree is determined by the following formula:

[0024]

[0025] In the formula, (x last ,y last) is the node closest to the target point on the random tree, ω is the angle correction coefficient, x goal ,y goal ,θ goal is the target point pose, x i ,y i ,θ i is the tree node pose, i is the node number, m is the number of nodes on the random tree, θ is the vehicle heading angle, δ is the front wheel equivalent turning angle, and y is the ordinate of the center point of the rear axle of the vehicle;

[0026] The child nodes of the longest and second longest branches in the random tree are used as the second index points, and the child nodes of the longest and second longest branches in the random tree are determined by the following formula:

[0027]

[0028] In the formula, (x s1 ,y s1 ) and (x s2 ,y s2 ) are the child nodes of the longest and second longest branches in the random tree, P i (x i ,y i ) is the RS cost between the parent and child nodes, n1 is the index of all child nodes that are not parent nodes, and n2 is the index of all parent nodes connected to the child nodes.

[0029] The average value of the latest n points on the random tree is used as the third index point, and the average value of the latest n points on the random tree is determined by the following formula:

[0030]

[0031] Where n is the number of nodes recently added to the random tree, (x n ,y n ) is the average value of the latest n points on the random tree;

[0032] The average value of all random tree nodes is used as the fourth index point, and the average value of all random tree nodes is determined by the following formula:

[0033]

[0034] In the formula, (x n ,y n ) is the average value of all random tree nodes, and N is the total number of nodes in the random tree.

[0035] In one possible design, the entire sampling space is rasterized and divided into multiple independent sampling areas. The sampling probability of each sampling area is calculated based on the index point, obstacle information, and target offset to form a probability map, including:

[0036] According to the distance between the grid point in the search space and the target point and the cost of the random tree, the path cost function is determined, which is expressed as:

[0037]

[0038] In the formula, g p (x i ,y i ) is the grid point path cost, i is the grid point (x i ,y i ), j is the index point (x j ,y j ), P(i,j) is the RS cost between two points, C(j) is the total cost from the index point to the starting point, and goal is the target point (x goal ,y goal ), ||i-goal|| is the Euclidean distance between two points;

[0039] According to the path cost of each grid point calculated by the path cost function, the results are arranged according to Gaussian distribution to construct a probability map based on the path cost;

[0040] Based on the obstacle information, the obstacle bias effect is calculated by the following formula:

[0041]

[0042] In the formula, η is the obstacle bias coefficient, d is the distance from the index point to the obstacle, and the distance direction is direction, w b is the obstacle threshold;

[0043] The target bias effect is calculated by the following formula:

[0044]

[0045] Where κ is the target bias coefficient, The direction is the direction from the index point to the target point;

[0046] Based on the obstacle bias and target bias, the common action direction is determined by vector addition, including the bias action direction of the index point and the direction from the grid point in the search space to the index point. The bias action direction of the index point is The direction from the grid point in the search space to the index point is

[0047] According to the joint action direction, the obstacle and target offset function is determined as:

[0048]

[0049] In the formula, θ(x i ,y i ) is the angle between the grid point and the bias action direction;

[0050] Based on the angle between the grid point and the bias direction, select angles greater than π and arrange them according to Gaussian distribution to construct a probability map based on the bias.

[0051] The probability map based on path cost and the probability map based on bias effect are fused to obtain a probability map.

[0052] In a possible design, the near point is extended to a new point in the direction of the random point with an extension step, and the new point is added to the random tree, including:

[0053] The extended length is determined by the following formula:

[0054]

[0055] In the formula, step min , step max are the minimum and maximum allowed step sizes, s is the distance from the new point to the obstacle, and w b is the obstacle threshold;

[0056] Expand the near point to a new point in the direction of the random point with the expansion step length;

[0057] The angle threshold θ is determined by the following formula m :

[0058]

[0059] In the formula, R is the minimum turning radius of the vehicle, and λ is the control coefficient;

[0060] If the node heading angle of the new point is greater than the angle threshold, the new point will be eliminated and will not be added to the random tree;

[0061] If the node heading angle of the new point is less than or equal to the angle threshold, search for nodes within the range of a*step, where a is a multiple of the range, and calculate the RS cost of each selected node to the new point. Determine the path formed by each node as the parent node of the new point to minimize the cost from the starting point to the new point. Select the parent node with the minimum cost for the new point, expand it with the RS curve, and optimize the shortest path.

[0062] Based on the expanded RS curve, the rectangular outline of the vehicle is calculated according to the geometric relationship, and collision detection with surrounding obstacles is performed based on vector cross multiplication. If a collision is detected, the new point is eliminated and will not be added to the random tree;

[0063] The error tolerance of the eliminated new point and its corresponding parent node is reduced by 1. When the error tolerance of the node drops to 0, the node no longer participates in the expansion of the nodes on the tree.

[0064] In one possible design, the RS representative is calculated based on a cost function of an improved RS curve, where the cost function of the improved RS curve is expressed as:

[0065]

[0066] Where P is the total path cost, L forward is the forward path length, L back is the length of the reversing path, L reverse is the shift path length, L circular is the arc path length, k l+1 and k l are the curvatures of the l+1th and lth segments of the path, c is the number of path segments, α, β, γ, and K are the penalty coefficients of the reversing path length, the gear shift path length, the arc path length, and the path curvature, respectively.

[0067] In one possible design, the vehicle is simplified into a rectangle according to the kinematic model, and each obstacle is simplified into a polygon according to its morphology;

[0068] When performing collision detection, the four line segments constituting the rectangle of the vehicle body are sequentially subjected to collision detection conditions with the parking space boundary line and the line segments of the polygon. When the collision detection conditions are met, it is determined that a collision will occur; wherein the collision detection conditions are expressed as:

[0069]

[0070] Wherein, M, N, P, Q are the endpoints of line segment MN and line segment PQ respectively, line segment MN is any line segment constituting the rectangle of the vehicle body, and line segment PQ is any line segment constituting the boundary line or polygon of the parking space.

[0071] In a second aspect, the present application provides a path planning device for automatic parking, the device comprising:

[0072] A data acquisition module is configured to acquire scene information, the scene information including target parking space, obstacle information, parking start point and target point posture, establish a kinematic equation of the vehicle, and determine the coordinate information of the vehicle based on the kinematic equation;

[0073] The random tree generation module is configured to iterate based on the scene information and the coordinate information of the vehicle, taking the parking starting point as the starting point to establish a random tree; the random tree includes a plurality of nodes, each node stores node information, and the node information includes a node sequence number, a node coordinate, a parent node sequence number, a node heading angle, a node cost, and a node total cost; wherein the node cost is the RS cost between the node and the parent node, and the total cost is the RS cost from the node to the starting point;

[0074] A probability map construction module is configured to obtain index points based on the random tree, rasterize the entire sampling space, divide it into multiple independent sampling areas, and calculate the sampling probability of each sampling area according to the index points, obstacle information and target bias to form a probability map;

[0075] A near point sampling module is configured to perform sampling on the probability map, obtain a random point, traverse the nodes on the random tree, calculate the distance between each node and the random point, and take the node on the random tree closest to the random point as the near point;

[0076] A new point expansion module is configured to expand the near point to a new point in the direction of the random point with an expansion step, and add the new point to the random tree;

[0077] The node reconnection module is configured to search for nodes within a set range for new points, calculate the RS cost to each selected node, determine the path formed by the new point as the parent node of each node, minimize the cost from the starting point to each node, update the parent node with a smaller cost for each node, expand it with the RS curve, perform the shortest path optimization, and obtain the optimal path;

[0078] The path output module is configured to calculate the distance between the new point and the target point. If the distance is greater than the termination distance threshold, re-sampling is performed on the probability map, and iteration is performed to plan the path. If the distance is less than or equal to the termination distance threshold, the target point is added to the random tree, and the iteration is terminated. The optimal path obtained is smoothed and optimized through multiple B-spline curves, and the parking path is output.

[0079] In a third aspect, an embodiment of the present application provides an electronic device, comprising: at least one processor and a memory; the memory stores computer-executable instructions; the at least one processor executes the computer-executable instructions stored in the memory, so that the at least one processor executes the path planning method for automatic parking as described in the first aspect and various possible designs of the first aspect.

[0080] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, in which computer execution instructions are stored. When a processor executes the computer execution instructions, the path planning method for automatic parking as described in the first aspect and various possible designs of the first aspect is implemented.

[0081] In a fifth aspect, an embodiment of the present application provides a computer program product, including a computer program. When the computer program is executed by a processor, it implements the path planning method for automatic parking as described in the first aspect and various possible designs of the first aspect.

[0082] The path planning method, device, equipment and storage medium for automatic parking provided in the present application have at least the following beneficial effects:

[0083] This application proposes probabilistic map sampling, which integrates the growth of random numbers and the search space grid points to improve node utilization and accelerate algorithm convergence; improves the RS curve cost, imposes penalty coefficients on the path and curvature, and reduces the frequency of path reversing and turning; increases the angle limit, eliminates large angle sampling points, and makes the path more consistent with vehicle kinematics; variable expansion step size is used to optimize the planning effect of the path near obstacles; proposes node tolerance to make path planning more flexible, able to escape from local dead zones and reverse search; proposes a collision detection model to ensure that the vehicle can achieve accurate obstacle avoidance during the planning process; uses cubic B-spline curves to ensure the continuity of the curvature of the parking path while ensuring the general shape of the planned path curve. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0085] Figure 1 A flow chart of a path planning method for automatic parking provided in an embodiment of the present application;

[0086] Figure 2 A specific implementation flow chart of a path planning method for automatic parking provided in an embodiment of the present application;

[0087] Figure 3 A schematic diagram of the structure of a path planning device for automatic parking provided in an embodiment of the present application.

[0088] The above drawings have shown clear embodiments of the present application, which will be described in more detail later. These drawings and text descriptions are not intended to limit the scope of the present application in any way, but to illustrate the concept of the present application to those skilled in the art by referring to specific embodiments. DETAILED DESCRIPTION

[0089] Exemplary embodiments will be described in detail herein, examples of which are shown in the accompanying drawings. When the following description refers to the drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The implementations described in the following exemplary embodiments do not represent all implementations consistent with the present application. Instead, they are merely examples of devices and methods consistent with some aspects of the present application as detailed in the appended claims.

[0090] In the technical solution of this application, the collection, storage, use, processing, transmission, provision and disclosure of information such as financial data or user data involved shall comply with the provisions of relevant laws and regulations and shall not violate public order and good morals.

[0091] It should be noted that in the embodiments of the present application, certain software, components, models and other existing solutions in the industry may be mentioned, and they should be regarded as exemplary. Their purpose is only to illustrate the feasibility of implementing the technical solution of the present application, but it does not mean that the applicant has or will necessarily use the solution.

[0092] The technical solution of the present application and how the technical solution of the present application solves the above-mentioned technical problems are described in detail below with specific embodiments. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be repeated in some embodiments. The embodiments of the present application will be described below in conjunction with the accompanying drawings.

[0093] The Rapidly-exploring Random Tree (RRT) algorithm and its variants gradually iterate and generate a search tree by random sampling increments. When the nodes on the tree are close to the target point, the parking path is determined by backtracking the parent node. The RRT* algorithm adds two processes: reselecting the parent node and rewiring the random tree. The main steps of RRT* are as follows:

[0094] Generate a random point in the entire search space, called the random point x_rand, traverse the current tree structure, find the node closest to the random point x_rand, called the nearest node x_near. Start from the nearest node x_near, move a fixed step length step towards the random point x_rand, and generate a new node x_new.

[0095] Perform collision detection on the newly generated path (from x_near to x_new) to determine whether the path intersects with the obstacle. If it intersects, discard the new node and re-randomly sample; otherwise, continue with the subsequent steps.

[0096] In the tree, find all the nodes within a certain radius as candidate parent nodes, calculate the path cost from each candidate parent node to x_new, including the cost from the starting point to the candidate parent node plus the cost from the candidate parent node to x_new, and select the candidate parent node with the minimum cost as the actual parent node of x_new.

[0097] For all the nodes in the tree within a certain radius, check whether their path costs can be reduced by changing their parent nodes to x_new. If so, update their parent nodes and costs to optimize the structure of the entire tree.

[0098] Check whether the new node x_new is close enough to the end point, that is, the distance between them is less than the set threshold. If so, add the end point to the tree and end the loop.

[0099] The embodiments of the present application provide a path planning method applied to automatic parking. Combining the above description, this path planning method improves the RRT* algorithm to achieve path planning for automatic parking in complex environments. As Figure 1 shown, it is a flowchart of a path planning method applied to automatic parking provided by the embodiments of the present application. This path planning method applied to automatic parking includes the following steps S100 - S700.

[0100] S100: Obtain scene information, where the scene information includes the target parking space, obstacle information, the starting point of parking, and the target point pose, establish the kinematic equation of the vehicle, and determine the coordinate information of the vehicle based on the kinematic equation.

[0101] It should be noted that the scene information can be obtained through the sensing devices of the vehicle. For example, the image information collected by the camera is used to automatically identify the parking space or the parking space selected by the driver from the image information as the target parking space, the obstacle information is obtained by identifying from the image information, the starting point of parking is the vehicle coordinate when starting automatic parking, and the target point pose is determined according to the target parking space and vehicle parameters (such as size parameters of the vehicle length, width, etc.).

[0102] In some embodiments, the kinematic equation of the vehicle is established in the following manner:

[0103] During the parking process of the vehicle, its speed is generally low. It can be assumed that the wheels cannot deform and are in pure rolling friction without skidding and other behaviors, and the effects of wheel camber, side slip, and tires are ignored. The vehicle is simplified to a two - wheel model using the Ackermann steering principle and kinematic analysis is performed.

[0104] The established kinematic equation is as follows:

[0105]

[0106] in, and are the speed in the x-axis direction, the speed in the y-axis direction and the angular velocity of the heading angle, θ is the vehicle heading angle, δ is the equivalent turning angle of the front wheel, l is the vehicle wheelbase, and v is the speed of the center point of the rear axle of the vehicle.

[0107] S200: Based on the scene information and the coordinate information of the vehicle, iterate with the parking starting point as the starting point to establish a random tree; the random tree includes multiple nodes, each node stores node information, the node information includes node sequence number, node coordinates, parent node sequence number, node heading angle, node cost and node total cost; wherein the node cost is the RS cost of the node and the parent node, and the total cost is the RS cost from the node to the starting point.

[0108] In this embodiment, the purpose of step S200 is to construct an initial random tree, wherein the RS cost involved refers to the cost of the RS curve, which is the Reeds-Shepp curve, a curve of discontinuous curvature composed of multiple straight lines and arcs. On the basis of the Dubins curve, it adds a backward movement method, i.e., reversing. The RS curve solves the optimal path between the starting point and the end point by calculating the path cost. The obtained path has a continuous change in heading angle and meets the kinematic requirements such as the minimum turning radius of the vehicle. Therefore, selecting the RS curve as the expansion method of the random tree node can more reasonably describe the change of the parking path.

[0109] In some embodiments, the traditional RS curve is improved to obtain an improved RS curve, and the RS cost is calculated using the cost function of the improved RS curve. In this embodiment, all RS costs can be calculated using the cost function of the improved RS curve.

[0110] Specifically, the starting and ending positions of the rear axle center of the vehicle are used as the input of the RS curve. The basic motion of the vehicle includes: forward motion (S + ), backward movement (S - ), turn left (L + ), turn right (R + ), turn left (L - ), turn right (R - ), these 6 movement modes constitute the following 9 categories and 48 basic RS curve types.

[0111] {C|C|C,CC|C,C|CC,CSC,CC β |C β C,CC β C β |C,C|C π / 2 SC,CSC π / 2|C,C|C π / 2 SC π / 2 |C}

[0112] Where C represents L or R, “|” represents the transition of the vehicle forward or backward, and π / 2 and β represent the arc value of the vehicle turning.

[0113] In order to ensure the comfort during parking and reduce the difficulty of tracking control, and to reduce the number of steering, shifting, reversing and other operations during parking, a penalty term is designed for the cost function of the RS curve to increase the cost of steering, shifting and reversing, thereby reducing its frequency.

[0114] The cost function of the improved RS curve is:

[0115]

[0116] Where P is the total path cost, L forward is the forward path length, L back is the length of the reversing path, L reverse is the shift path length, L circular is the arc path length, k is the path curvature, and c is the number of path segments.

[0117] α, β, γ, K are the penalty coefficients for each item, and the forward path L forward is the expected path, and no penalty coefficient is required. α and β are the penalty coefficients of the reversing path and the gear shift path, which are paths that need to be avoided and whose penalty coefficients should be larger. γ is the penalty coefficient of the arc path. Penalty is imposed on the arc path to increase the proportion of the straight path, which helps to smooth the path. K is the curvature penalty to reduce the number and degree of curvature changes of the path.

[0118] S300: Based on the random tree, obtain the index points, rasterize the entire sampling space, divide it into multiple independent sampling areas, calculate the sampling probability of each sampling area according to the index points, obstacle information and target bias, and form a probability map.

[0119] In the traditional RRT / RRT* algorithm, since random sampling is directly used in the process, the pure random sampling method consumes more computing resources, resulting in full convergence of the algorithm and low search efficiency. In addition, when facing complex parking processes, the planning complexity will be further increased due to the influence of too many obstacles. This pure random sampling method is more likely to fall into the local optimum and cause parking failure.

[0120] To address the deficiencies of pure random sampling, a sampling method based on a probability map (i.e., step S300) is proposed. The entire sampling space is rasterized and divided into multiple independent sampling areas. Based on heuristic thinking, considering the influence of indicators such as the growth of the random tree, obstacle information, and target bias, the sampling probability of each sampling area is calculated respectively to form a probability map. In subsequent steps, sampling points will be selected based on the constructed probability map to improve the search efficiency.

[0121] When implementing this path planning method, a random tree will be gradually generated. The quality of the parking path depends on the growth status of the random tree. Reasonably select the nodes on the random tree as index points to characterize the growth of the random tree. In some embodiments, four index points are selected, namely the first index point, the second index point, the third index point, and the fourth index point.

[0122] The first index point: the point closest to the target point (x last , y last ).

[0123] The RRT* algorithm expands the random tree through new nodes. For each node on the random tree, calculate its distance to the target point. At the same time, considering the requirements of the target point heading angle, introduce angle correction, and select the node with the smallest actual distance to reach the target point. The expression is as follows:

[0124]

[0125] In the formula, ω is the angle correction coefficient, x goal , y goal , θ goal is the pose of the target point, x i , y i , θ i is the pose of the tree node, and m is the serial number of each node on the tree.

[0126] The point closest to the target point characterizes the expansion range of the random tree.

[0127] The second index point: the child nodes of the longest / second-longest branch (x s1 , y s1 ), (x s2 , y s2 ).

[0128] During the path planning process of the RRT* algorithm, the random tree will expand multiple branches, which are connected in the form of child / parent nodes. The longest branch is the farthest path planned by the algorithm. For the end node of each branch, it is a child node that does not act as a parent node. To reduce randomness, select the child nodes of the longest and second-longest branches. The expression is as follows:

[0129]

[0130] Where P is the RS curve cost between the parent and child nodes, n1 is the index of all child nodes that are not parent nodes, and n2 is the index of all parent nodes connected to the starting point.

[0131] The child nodes of the longest / second longest branch represent the longest path currently planned, which is the actual direction of the random tree. By optimizing the longest / second longest branch, the parking path can be as short as possible.

[0132] The third index point: the average value of the latest n points on the random tree (x n ,y n ).

[0133] The RRT* algorithm adds new nodes to the tree through iteration. In this process, n nodes that have been added to the random tree are selected. Since these new nodes are affected by the random tree itself, there may be large distribution differences. The average coordinate position of these nodes is calculated to eliminate the difference as much as possible. The expression is as follows:

[0134]

[0135] Where n is the number of nodes most recently added to the random tree.

[0136] The average value of the latest n points on the random tree represents the expansion position of the random tree in the search space and is the general planning trend of the parking path.

[0137] Fourth indicator point: the average value of all random tree nodes (x N ,y N ).

[0138] Similar to the third indicator point, each new node added to the random tree has an impact on the planned path. For all nodes of the random tree, the average coordinates can also be obtained by calculation. The expression is as follows:

[0139]

[0140] Where N is the total number of nodes in the random tree.

[0141] The average value of all random tree nodes represents the approximate center position of the random tree in the search space.

[0142] In the above, the first indicator point selects 1 node, and the second indicator point selects 2 nodes. These 3 nodes are all fixed points. Based on the properties of the random tree generated by the RRT* algorithm, the cost, heading angle and other fixed information of these nodes can be obtained. The third and fourth indicator points are the average points calculated from multiple points. They are virtual points and only their coordinate information is used.

[0143] Next, for the above four index points, considering the optimal path, effective obstacle avoidance, vehicle kinematics and other constraints, a heuristic function is established based on reasonable performance indicators to calculate the sampling probability of each sampling area.

[0144] When performing automatic parking, we hope to obtain the shortest possible parking path, so the impact of path cost on the random tree must be considered.

[0145] RS curve is used as an extension method of random tree. In the process of path planning, the cost between the newly extended child node and its parent node is P, which is the cost of the current RS curve. The total cost of each child node in the algorithm is the total cost of connecting its parent node to the starting point. The cost expression from any node on the tree to the starting point is:

[0146]

[0147] For the grid points in the search space, the following path cost function is designed based on their distance from the target point and the cost of the random tree:

[0148]

[0149] In the formula, g p (x i ,y i ) is the grid point path cost, i is the grid point (x i ,y i ), j is the index point (x j ,y j ), P(i,j) is the RS cost between two points, C(j) is the total cost from the index point to the starting point, and goal is the target point (x goal ,y goal ), ||i-goal|| is the Euclidean distance between two points.

[0150] The path cost of each grid point is calculated and the results are arranged according to Gaussian distribution. The cost corresponds to the sampling probability. The smaller the cost, the higher the sampling probability. A probability map based on the path cost is constructed.

[0151] There are various obstacles in the parking environment, which will increase the complexity of the search space and affect the efficiency of the algorithm planning. Reasonable use of obstacle information to optimize the random tree expansion direction will improve the search efficiency of the method to a certain extent.

[0152] For obstacle bias, the random tree is not affected by it in the whole process. The obstacle action threshold is set to prevent the planning from falling into the local optimum and causing failure. The threshold is one vehicle width w from the obstacle to the decision point. b When the indicator point is within the action threshold, obstacle offset is performed.

[0153] There are two types of obstacles in the parking space. For the parking space boundary line, the distance from the four vertices around the vehicle body corresponding to the index point to the edge line should be calculated to accurately describe the actual situation. The distance is d l , and for each type of obstacle, calculate the distance d from its geometric center to the index point o Obstacle bias effect The expression is:

[0154]

[0155] In the formula, η is the obstacle bias coefficient, d is the distance from the index point to the obstacle, and the distance direction is direction.

[0156] In particular, for certain points such as indicators 1 and 2, which can accurately describe the node position of the vehicle, a larger weight obstacle bias should be set, so the corresponding bias coefficient is also larger.

[0157] Similarly, the target bias can further accelerate the algorithm's convergence to the target point by guiding the random tree through the target point. The expression is:

[0158]

[0159] Where κ is the target bias coefficient, The direction is the direction from the index point to the target point.

[0160] Based on the action formula of obstacle bias and target bias, the common action direction of the two is determined by the vector addition method. The bias action direction of the index point is: The direction from the grid point in the search space to the index point is The obstacle and target offset functions are designed as:

[0161]

[0162] In the formula, θ(x i ,y i ) is the angle between the grid point and the offset direction.

[0163] Only angles where θ is greater than π are selected and arranged according to a Gaussian distribution. The angles correspond to sampling probabilities. The smaller the angle, the higher the sampling probability. This constructs a probability map based on bias.

[0164] Finally, the two types of probability maps are fused to obtain the final probability map.

[0165] It should be noted that the method of fusing the probability map based on path cost with the probability map based on bias to obtain the final probability map can adopt existing methods, including but not limited to the following four methods:

[0166] 1) Averaging method: simple average or weighted average.

[0167] 2) Multiplicative / Bayesian fusion: Assume independence, normalize by multiplication or add log-odds.

[0168] 3) Maximum / minimum method: take the maximum or minimum probability of each point.

[0169] 4) Machine learning model fusion: Use models such as logistic regression to learn the optimal fusion method.

[0170] S400: Sampling is performed on the probability map to obtain a random point, traversing the nodes on the random tree, respectively calculating the distance between each node and the random point, and taking the node on the random tree closest to the random point as the near point.

[0171] In this embodiment, sampling is performed on the probability map, and based on the constructed probability map, sampling is performed in the search space to obtain a random point x_rand. The nodes on the random tree are traversed, and the Euclidean distance between them and x_rand is calculated respectively, and the node x_near closest to x_rand on the random tree is determined.

[0172] S500: Expand the near point to a new point in the direction of the random point with an expansion step, and add the new point to the random tree.

[0173] In this embodiment, the following steps are performed to extend the new point x_new from the near point x_near in a certain step length in the direction of x_rand:

[0174] S501: Determine the extension step size. Determine whether x_near is within the obstacle threshold range wb. The extension step size step is expressed as:

[0175]

[0176] In the formula, step min , step max are the minimum and maximum allowed step sizes, and s is the distance from x_near to the obstacle.

[0177] In the process of path planning, when the random tree grows near obstacles, its planning efficiency often decreases, and the fixed step size is difficult to meet the kinematic requirements. Therefore, the obstacle threshold w is used to calculate the path planning efficiency. bIn the planning process, when the x_near point is within the threshold range of the obstacle, its expansion step can be flexibly changed. The closer the distance to the obstacle, the shorter the expansion step, so as to optimize the path around the obstacle. At the same time, when the x_near point is far away from the obstacle, appropriately increasing the step can improve the search efficiency.

[0178] S502: Determine the turning angle limit. The node heading angle of the new point x_new is the angle between the line connecting it and the corresponding parent node x_near and the x-axis. If the angle is greater than the angle threshold θ m , then the new point x_new will be eliminated and will not be added to the random tree. The corner limit threshold is closely related to the expansion step size, and its expression is:

[0179]

[0180] Where R is the minimum turning radius of the vehicle and λ is the control coefficient.

[0181] In step S502, in order to ensure that the parking path planned by the vehicle meets the vehicle kinematic requirements, the nodes on the random tree will carry heading angle information. During the expansion of the x_near point, if the heading angle between the x_new point and the x_near point is greater than the set angle threshold, x_new will not be added to the random tree.

[0182] S503: Reselect the parent node. If the new point x_new is within the corner limit, search the tree nodes within the range of a*step, where a is the range multiple, and calculate the RS cost of each selected node to x_new. Determine the path formed by each node as the parent node of x_new, so that the cost from the starting point to the new point x_new is minimized, select the parent node with the minimum cost for x_new, expand it with the RS curve, and optimize the shortest path.

[0183] S504: Perform collision detection. For the extended curve, which is the trajectory of the center point of the rear axle of the vehicle, the rectangular outline of the vehicle is calculated based on the geometric relationship, and collision detection with surrounding obstacles is performed based on vector cross multiplication. If a collision is detected, the new point x_new is eliminated and will not be added to the random tree.

[0184] Specifically, when a vehicle is parking, there may be static obstacles such as piers and cones, or dynamic obstacles such as pedestrians around the parking space. At the same time, the vehicle cannot cross the boundary line of the target parking space, and it must be ensured that it does not collide with these obstacles. Therefore, it is necessary to perform collision detection during the path planning process to achieve parking obstacle avoidance.

[0185] The vector cross multiplication method is used to detect vehicle collision. It determines whether two line segments intersect by calculating the cross product of two vectors. The cross product of two vectors can be used to determine the spatial relationship between the two vectors. There are two two-dimensional vectors. The formula for the two-dimensional vector cross product is:

[0186]

[0187] In the formula, if but exist counterclockwise, if but exist clockwise direction.

[0188] Based on the above geometric meaning, two straddling experiments can be used to test whether two line segments intersect. If there are two intersecting line segments MN and PQ, two vector cross multiplication calculations are performed to detect whether points P and Q are on both sides of MN, and whether points M and N are on both sides of PQ, that is, the following conditions are met:

[0189]

[0190] This means that the two line segments intersect.

[0191] Furthermore, the vehicles and obstacles in the parking process are reasonably simplified. The vehicle is simplified into a rectangle based on the kinematic model, the parking space boundary line itself is a standard rectangle, and each obstacle is simplified into a polygon based on its shape. When performing collision detection, the four line segments that constitute the body rectangle and the line segments of the parking space boundary line and obstacle polygon are sequentially subjected to the above collision detection conditions to accurately detect whether the vehicle collides with an obstacle or parking space boundary.

[0192] S505: Forgetting mechanism: For the new point x_new eliminated by the above corner restriction and collision detection, the error tolerance of its corresponding parent node is reduced by 1. When the error tolerance drops to 0, the node no longer participates in the expansion of nodes on the tree.

[0193] For parking environments with narrow parking spaces or many obstacles, during the planning process of the RRT* algorithm, there are many x_new that are difficult to meet due to corner restrictions and collision detection. Some random tree nodes will fail to expand multiple times. Therefore, a fault tolerance M is set for each node on the random tree. When the node is selected as the x_near point, if the x_new point cannot be successfully expanded due to corner restrictions or collision detection, its fault tolerance is reduced by 1. When the fault tolerance M is 0, the node no longer participates in the expansion of the nodes on the tree.

[0194] In particular, to ensure the completeness of the algorithm planning, the error tolerance of the starting point is set to infinity inf.

[0195] After completing the above steps, the new point x_new will be added to the random tree, and its information will also be stored in the random tree node.

[0196] S600: For new points, search for nodes within the set range and calculate the RS cost of each selected node. Determine the path formed by the new point as the parent node of each node to minimize the cost from the starting point to each node. Update the parent node with a smaller cost for each node, expand it with the RS curve, perform shortest path optimization, and obtain the optimal path.

[0197] In this embodiment, the purpose of step S600 is to reconnect the newly added nodes, which is similar to reselecting the parent node. For the new point x_new, the tree nodes are searched within the range of a*step, where a is a multiple of the range, and the RS cost of each selected node is calculated. The path formed by x_new as the parent node of each node is determined to minimize the cost from the starting point to each node, and the parent node with a smaller cost is updated for each node. The RS curve is used for expansion to perform the shortest path optimization.

[0198] S700: Calculate the distance between the new point and the target point. If the distance is greater than the termination distance threshold, resample on the probability map and iterate to plan the path. If the distance is less than or equal to the termination distance threshold, add the target point to the random tree and terminate the iteration. Perform path smoothing optimization on the obtained optimal path through multiple B-spline curves and output the parking path.

[0199] In this embodiment, step S700 is used to determine the termination of the iteration. After the new point x_new is added to the random tree, the Euclidean distance between x_new and the target point is calculated. If the distance is greater than the termination distance threshold D min , then return to Step 2 and iterate to plan the path; if its distance is less than the termination distance threshold D min , then directly add the target point to the random tree, terminate the iteration, and output the parking path.

[0200] Finally, the planned path is optimized by cubic B-spline curve. The parking path is composed of RS curve, and each point on the curve can be used as the control point of B-spline curve. The curve expression is:

[0201]

[0202] Where N i,3 (t) is the cubic B-spline basis function, and V is the control point.

[0203] RS curves are used to extend and connect random tree nodes. Although RS curves can ensure the continuity and smoothness of the heading angle of the path, the curvature of the path is discontinuous. At the intersection of straight line-arc and arc-arc, the curvature will change suddenly, which will make parking difficult to track and control and increase tire wear. The cubic B-spline curve is composed of cubic polynomial curve segments controlled by a series of points called control points. These curve segments smoothly transition between adjacent control points to form a continuous curve. Using the points on the RS path as control points to construct a cubic B-spline curve can make the curvature of the parking path continuous and smooth while ensuring the approximate shape of the RS curve.

[0204] In some embodiments, Figure 2 FIG. 1 is a flow chart showing a specific implementation of a path planning method for automatic parking, wherein the path planning method comprises the following steps:

[0205] S201: Obtain scene information and load vehicle displacement information.

[0206] Among them, the parking link information is obtained and the vehicle posture information is loaded, including the target parking space, obstacle information, the parking starting point and target point posture, vehicle parameters, etc. The vehicle model is simplified to a two-wheel model through the Ackerman steering principle, and the kinematic equation describing the center point of the rear axle of the vehicle is established. According to the geometric relationship, the coordinates of the four vertices around the vehicle body can also be solved for subsequent use of the algorithm.

[0207] S202: Establish an initial random tree.

[0208] S203: Obtaining indicator points based on random tree information to calculate path costs and bias effect indicators to construct and update a probability map.

[0209] The construction of probability map has been explained above and will not be repeated here. In the initial stage of random tree growth, there are too few nodes on the tree, and multiple indicator points may correspond to the same node. Different indicators of the same node are merged to avoid repeated calculations. The probability map needs to be updated in real time as the number of nodes on the tree increases to keep the method heuristic.

[0210] S204: Obtain a random point x_rand based on probability map sampling.

[0211] S205: traverse the random tree to find the tree node with the closest Euclidean distance to x_rand as the near point x_near.

[0212] Sampling is performed on the probability map. Based on the constructed probability map, sampling is performed in the search space to obtain a random point x_rand. Traversing the nodes on the random tree, calculating the Euclidean distance between each node and x_rand, and determining the node x_near on the random tree that is closest to x_rand.

[0213] S206: Determine whether the heading angle between x_rand and x_near is greater than the angle threshold. If so, execute S207; if not, execute S208.

[0214] S207: Subtract 1 from the tolerance corresponding to x_near, and return to S203.

[0215] S208: Determine the expansion step size based on the distance from x_near to the obstacle.

[0216] S209: Take x_near as the parent node and RS curve as the expansion method to obtain x_new.

[0217] S210: Reselect the parent node of x_new within a radius of 3 steps from x_new (RS cost).

[0218] S211: Determine whether the path collides with an obstacle, if so, execute S207, if not, execute S211.

[0219] S212: Add x_new to the random tree.

[0220] S213: Within the range of 3step radius of x_new, reselect the parent node of the node within the range (RS cost).

[0221] S214: Determine whether the path collides with an obstacle, if not, execute S215.

[0222] S215: Determine whether the target point is near x_new, if not, execute S203, if yes, execute S216.

[0223] S216: Add the target point to the random tree.

[0224] S217: Path smoothing.

[0225] The present application also provides a path planning device for automatic parking. Figure 3 As shown, the path planning device applied to automatic parking includes:

[0226] The data acquisition module 301 is configured to acquire scene information, including target parking space, obstacle information, parking start point and target point posture, establish a kinematic equation of the vehicle, and determine the coordinate information of the vehicle based on the kinematic equation;

[0227] The random tree generation module 302 is configured to iterate based on the scene information and the coordinate information of the vehicle, taking the parking starting point as the starting point, and establish a random tree; the random tree includes a plurality of nodes, each node stores node information, and the node information includes a node sequence number, a node coordinate, a parent node sequence number, a node heading angle, a node cost, and a node total cost; wherein the node cost is the RS cost between the node and the parent node, and the total cost is the RS cost from the node to the starting point;

[0228] The probability map construction module 303 is configured to obtain the index point based on the random tree, rasterize the entire sampling space, divide it into multiple independent sampling areas, and calculate the sampling probability of each sampling area according to the index point, obstacle information and target bias to form a probability map;

[0229] The near point sampling module 304 is configured to perform sampling on the probability map, obtain a random point, traverse the nodes on the random tree, calculate the distance between each node and the random point, and take the node on the random tree closest to the random point as the near point;

[0230] A new point expansion module 305 is configured to expand the near point to a new point in the direction of the random point with an expansion step, and add the new point to the random tree;

[0231] The node reconnection module 306 is configured to search for nodes within a set range for the new point, calculate the RS cost to each selected node, determine the path formed by the new point as the parent node of each node, minimize the cost from the starting point to each node, update the parent node with a smaller cost for each node, expand it with the RS curve, perform the shortest path optimization, and obtain the optimal path;

[0232] The path output module 307 is configured to calculate the distance between the new point and the target point. If the distance is greater than the termination distance threshold, re-sampling is performed on the probability map, and iteration is performed to plan the path. If the distance is less than or equal to the termination distance threshold, the target point is added to the random tree, and the iteration is terminated. The optimal path obtained is smoothed and optimized through multiple B-spline curves, and the parking path is output.

[0233] An embodiment of the present application provides an electronic device, which may include: a processor and a memory, wherein the processor and the memory may communicate with each other; illustratively, the processor and the memory communicate with each other via a communication bus.

[0234] The processor executes the computer execution instructions stored in the memory, so that the processor executes the scheme in the above embodiment. The processor can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be a digital signal processor (DSP), an application-specific integrated circuit ASIC, a field programmable gate array FPGA or other programmable logic devices, discrete gates or transistor logic devices, and discrete hardware components.

[0235] The communication bus can be a peripheral component interconnect (PCI) bus or an extended industry standard architecture (EISA) bus. The system bus can be divided into an address bus, a data bus, a control bus, etc. The transceiver is used to realize the communication between the database access device and other computers (such as clients, read-write libraries, and read-only libraries). The memory may include random access memory (RAM) and may also include non-volatile memory.

[0236] The electronic device provided in the embodiment of the present application may be the terminal device of the above embodiment.

[0237] An embodiment of the present application further provides a computer-readable storage medium, in which computer instructions are stored. When the computer instructions are executed on a computer, the computer executes the technical solution of the path planning method for automatic parking applied in the above embodiment.

[0238] An embodiment of the present application also provides a computer program product, which includes a computer program stored in a computer-readable storage medium. At least one processor can read the computer program from the computer-readable storage medium. When at least one processor executes the computer program, it can implement the technical solution of the path planning method applied to automatic parking in the above embodiment.

[0239] In the several embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic, for example, the division of modules is only a logical function division, and there may be other division methods in actual implementation, such as multiple modules can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, devices or modules, which can be electrical, mechanical or other forms.

[0240] The modules described as separate components may or may not be physically separated, and the components shown as modules may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the modules may be selected according to actual needs to implement the solution of this embodiment.

[0241] In addition, each functional module in each embodiment of the present application can be integrated into one processing unit, or each module can exist physically separately, or two or more modules can be integrated into one unit. The above-mentioned module-composed unit can be implemented in the form of hardware or in the form of hardware plus software functional units.

[0242] The above-mentioned integrated module implemented in the form of a software function module can be stored in a computer-readable storage medium. The above-mentioned software function module is stored in a storage medium, including a number of instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) or a processor to perform some steps of the methods of various embodiments of the present application.

[0243] It should be understood that the processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), etc. A general-purpose processor may be a microprocessor or any conventional processor. The steps of the method disclosed in the invention may be directly implemented as being executed by a hardware processor, or may be implemented by a combination of hardware and software modules in the processor.

[0244] The memory may include a high-speed RAM memory, and may also include a non-volatile storage NVM, such as at least one disk memory, and may also be a USB flash drive, a mobile hard disk, a read-only memory, a magnetic disk or an optical disk, etc.

[0245] The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Architecture (EISA) bus, etc. The bus can be divided into an address bus, a data bus, a control bus, etc.

[0246] The above storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic memory, flash memory, magnetic disk or optical disk. The storage medium can be any available medium that can be accessed by a general or special purpose computer.

[0247] An exemplary storage medium is coupled to a processor so that the processor can read information from the storage medium and write information to the storage medium. Of course, the storage medium can also be a component of the processor. The processor and the storage medium can be located in an application specific integrated circuit (ASIC). Of course, the processor and the storage medium can also exist as discrete components in an electronic control unit or a main control device.

[0248] Those skilled in the art can understand that all or part of the steps of implementing the above-mentioned method embodiments can be completed by hardware related to program instructions. The aforementioned program can be stored in a computer-readable storage medium. When the program is executed, the steps of the above-mentioned method embodiments are executed; and the aforementioned storage medium includes: ROM, RAM, disk or optical disk and other media that can store program codes.

[0249] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit it. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A path planning method for automatic parking, characterized in that: The method comprises: Acquire scene information, the scene information including target parking space, obstacle information, parking starting point and target point posture, establish a kinematic equation of the vehicle, and determine the coordinate information of the vehicle based on the kinematic equation; Based on the scene information and the coordinate information of the vehicle, iterate with the parking starting point as the starting point to establish a random tree; the random tree includes a plurality of nodes, each node stores node information, and the node information includes a node sequence number, a node coordinate, a parent node sequence number, a node heading angle, a node cost, and a node total cost; wherein the node cost is the RS cost between the node and the parent node, and the total cost is the RS cost from the node to the starting point; Based on the random tree, the index points are obtained, the entire sampling space is rasterized and divided into multiple independent sampling areas, and the sampling probability of each sampling area is calculated according to the index points, obstacle information and target bias to form a probability map; Sampling is performed on the probability map to obtain a random point, traversing the nodes on the random tree, respectively calculating the distance between each node and the random point, and taking the node on the random tree closest to the random point as the near point; Expand the near point to a new point in the direction of the random point with an expansion step, and add the new point to the random tree; For new points, search for nodes within the set range and calculate the RS cost to each selected node. Determine the path formed by the new point as the parent node of each node to minimize the cost from the starting point to each node. Update the parent node with a smaller cost for each node, expand it with the RS curve, perform the shortest path optimization, and obtain the optimal path. The distance between the new point and the target point is calculated. If the distance is greater than the termination distance threshold, re-sampling is performed on the probability map, and iteration is performed to plan the path. If the distance is less than or equal to the termination distance threshold, the target point is added to the random tree, and the iteration is terminated. The optimal path is smoothed and optimized through multiple B-spline curves, and the parking path is output.

2. The path planning method for automatic parking according to claim 1, characterized in that: The kinematic equations of the vehicle are established as follows: The vehicle is simplified into a two-wheel model using the Ackerman steering principle, and kinematic analysis is performed to establish the vehicle's kinematic equations. The established kinematic equation is expressed as: In the formula, and are the speed in the x-axis direction, the speed in the y-axis direction and the angular velocity of the heading angle, θ is the vehicle heading angle, δ is the equivalent turning angle of the front wheel, l is the vehicle wheelbase, and v is the speed of the center point of the rear axle of the vehicle.

3. The path planning method for automatic parking according to claim 1, characterized in that: The index points include a first index point, a second index point, a third index point and a fourth index point. Based on the random tree, obtaining the index points includes: The node closest to the target point on the random tree is taken as the first index point, and the node closest to the target point on the random tree is determined by the following formula: In the formula, (x last ,y last ) is the node closest to the target point on the random tree, ω is the angle correction coefficient, x goal ,y goal ,θ goal is the target point pose, x i ,y i ,θ i is the tree node pose, i is the node number, m is the number of nodes on the random tree, θ is the vehicle heading angle, δ is the front wheel equivalent turning angle, and y is the ordinate of the center point of the rear axle of the vehicle; The child nodes of the longest and second longest branches in the random tree are used as the second index points, and the child nodes of the longest and second longest branches in the random tree are determined by the following formula: In the formula, (x s1 ,y s1 ) and (x s2 ,y s2 ) are the child nodes of the longest and second longest branches in the random tree, P i (x i ,y i ) is the RS cost between the parent and child nodes, n1 is the index of all child nodes that are not parent nodes, and n2 is the index of all parent nodes connected to the child nodes. The average value of the latest n points on the random tree is used as the third index point, and the average value of the latest n points on the random tree is determined by the following formula: Where n is the number of nodes recently added to the random tree, (x n ,y n ) is the average value of the latest n points on the random tree; The average value of all random tree nodes is used as the fourth index point, and the average value of all random tree nodes is determined by the following formula: In the formula, (x n ,y n ) is the average value of all random tree nodes, and N is the total number of nodes in the random tree.

4. The path planning method for automatic parking according to claim 3, characterized in that: The entire sampling space is rasterized and divided into multiple independent sampling areas. The sampling probability of each sampling area is calculated based on the index points, obstacle information, and target offset to form a probability map, including: According to the distance between the grid point in the search space and the target point and the cost of the random tree, the path cost function is determined, which is expressed as: In the formula, g p (x i ,y i ) is the grid point path cost, i is the grid point (x i ,y i ), j is the index point (x j ,y j ), P(i,j) is the RS cost between two points, C(j) is the total cost from the index point to the starting point, and goal is the target point (x goal ,y goal ), ||i-goal|| is the Euclidean distance between two points; According to the path cost of each grid point calculated by the path cost function, the results are arranged according to Gaussian distribution to construct a probability map based on the path cost; Based on the obstacle information, the obstacle bias effect is calculated by the following formula: In the formula, η is the obstacle bias coefficient, d is the distance from the index point to the obstacle, and the distance direction is direction, w b is the obstacle threshold; The target bias effect is calculated by the following formula: Where κ is the target bias coefficient, The direction is the direction from the index point to the target point; Based on the obstacle bias and target bias, the common action direction is determined by vector addition, including the bias action direction of the index point and the direction from the grid point in the search space to the index point. The bias action direction of the index point is The direction from the grid point in the search space to the index point is According to the joint action direction, the obstacle and target offset function is determined as: In the formula, θ(x i ,y i ) is the angle between the grid point and the bias action direction; Based on the angle between the grid point and the bias direction, select angles greater than π and arrange them according to Gaussian distribution to construct a probability map based on the bias. The probability map based on path cost and the probability map based on bias effect are fused to obtain a probability map.

5. The path planning method for automatic parking according to claim 1, characterized in that: Extending the near point to a new point in the direction of the random point with an extension step, and adding the new point to the random tree, including: The extended length is determined by the following formula: In the formula, step min , step max are the minimum and maximum allowed step sizes, s is the distance from the new point to the obstacle, and w b is the obstacle threshold; Expand the near point to a new point in the direction of the random point with the expansion step length; The angle threshold θ is determined by the following formula m : In the formula, R is the minimum turning radius of the vehicle, and λ is the control coefficient; If the node heading angle of the new point is greater than the angle threshold, the new point will be eliminated and will not be added to the random tree; If the node heading angle of the new point is less than or equal to the angle threshold, search for nodes within the range of a*step, where a is a multiple of the range, and calculate the RS cost of each selected node to the new point. Determine the path formed by each node as the parent node of the new point to minimize the cost from the starting point to the new point. Select the parent node with the minimum cost for the new point, expand it with the RS curve, and optimize the shortest path. Based on the expanded RS curve, the rectangular outline of the vehicle is calculated according to the geometric relationship, and collision detection with surrounding obstacles is performed based on vector cross multiplication. If a collision is detected, the new point is eliminated and will not be added to the random tree; The error tolerance of the eliminated new point and its corresponding parent node is reduced by 1. When the error tolerance of the node drops to 0, the node no longer participates in the expansion of the nodes on the tree.

6. The path planning method for automatic parking according to claim 5, characterized in that: Based on the extended RS curve, the vehicle rectangular outline is calculated according to the geometric relationship, and collision detection with surrounding obstacles is performed based on vector cross multiplication, including: The vehicle is simplified into a rectangle according to the kinematic model, and each obstacle is simplified into a polygon according to its shape; When performing collision detection, the four line segments constituting the rectangle of the vehicle body are sequentially subjected to collision detection conditions with the parking space boundary line and the line segments of the polygon. When the collision detection conditions are met, it is determined that a collision will occur; wherein the collision detection conditions are expressed as: Wherein, M, N, P, Q are the endpoints of line segment MN and line segment PQ respectively, line segment MN is any line segment constituting the rectangle of the vehicle body, and line segment PQ is any line segment constituting the boundary line or polygon of the parking space.

7. The path planning method for automatic parking according to any one of claims 1 to 6, characterized in that: The RS representative is calculated based on the cost function of the improved RS curve, and the cost function of the improved RS curve is expressed as: Where P is the total path cost, L forward is the forward path length, L back is the length of the reversing path, L reverse is the shift path length, L circular is the arc path length, k l+1 and k l are the curvatures of the l+1th and lth segments of the path, c is the number of path segments, α, β, γ, and K are the penalty coefficients of the reversing path length, the gear shift path length, the arc path length, and the path curvature, respectively.

8. A path planning device for automatic parking, characterized in that: The device comprises: A data acquisition module is configured to acquire scene information, the scene information including target parking space, obstacle information, parking start point and target point posture, establish a kinematic equation of the vehicle, and determine the coordinate information of the vehicle based on the kinematic equation; The random tree generation module is configured to iterate based on the scene information and the coordinate information of the vehicle, taking the parking starting point as the starting point to establish a random tree; the random tree includes a plurality of nodes, each node stores node information, and the node information includes a node sequence number, a node coordinate, a parent node sequence number, a node heading angle, a node cost, and a node total cost; wherein the node cost is the RS cost between the node and the parent node, and the total cost is the RS cost from the node to the starting point; A probability map construction module is configured to obtain index points based on the random tree, rasterize the entire sampling space, divide it into multiple independent sampling areas, and calculate the sampling probability of each sampling area according to the index points, obstacle information and target bias to form a probability map; A near point sampling module is configured to perform sampling on the probability map, obtain a random point, traverse the nodes on the random tree, calculate the distance between each node and the random point, and take the node on the random tree closest to the random point as the near point; A new point expansion module is configured to expand the near point to a new point in the direction of the random point with an expansion step, and add the new point to the random tree; The node reconnection module is configured to search for nodes within a set range for new points, calculate the RS cost to each selected node, determine the path formed by the new point as the parent node of each node, minimize the cost from the starting point to each node, update the parent node with a smaller cost for each node, expand it with the RS curve, perform the shortest path optimization, and obtain the optimal path; The path output module is configured to calculate the distance between the new point and the target point. If the distance is greater than the termination distance threshold, re-sampling is performed on the probability map, and iteration is performed to plan the path. If the distance is less than or equal to the termination distance threshold, the target point is added to the random tree, and the iteration is terminated. The optimal path obtained is smoothed and optimized through multiple B-spline curves, and the parking path is output.

9. An electronic device, characterized in that: include: A processor, and a memory communicatively connected to the processor; The memory stores computer-executable instructions; The processor executes the computer-executable instructions stored in the memory to implement the path planning method for automatic parking as described in any one of claims 1-7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer-executable instructions, which, when executed by a processor, are used to implement the path planning method for automatic parking according to any one of claims 1 to 7.

Images