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

Abstract

The application provides a path planning method and device applied to automatic parking, equipment and a storage medium. It relates to the technical field of parking control. The method comprises the following steps: based on scene information and coordinate information of a vehicle, taking a parking starting point as a starting point to perform iteration, and establishing a random tree; according to index points, obstacle information and a target bias, the sampling probability of each sampling area is calculated respectively to form a probability map; sampling is performed on the probability map to obtain random points, nodes on the random tree are traversed, the distance between each node and the random points is calculated, and the node closest to the random points on the random tree is taken as a near point; the near point is expanded to a new point in the direction of the random point at an expansion step, and the new point is added to the random tree for node reconnection; the distance between the new point and a target point is calculated, and it is judged whether to terminate iteration; after the termination of iteration, the optimal path obtained is subjected to path smoothing optimization, and a parking path is output, so as to realize automatic parking path planning in a complex environment.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of parking control, and particularly relates to a path planning method and device applied to automatic parking, equipment and a storage medium. BACKGROUND

[0002] With the rapid increase of the number of motor vehicles and the scarcity of parking resources, it is inevitable to face traffic congestion and parking difficulties. On the one hand, the number of parking spaces and existing parking lots is difficult to meet the parking demand of numerous motor vehicles at the moment, which aggravates the contradiction between the supply and demand of urban vehicles and parking spaces, and further leads to narrow parking spaces and complex parking environment. On the other hand, there is a certain technical difficulty in parking operation. The driver's line of sight is blocked when parking, and there is a blind area in the field of vision, which cannot completely master the situation around the parking space. The drivers who are not skilled in parking technology need more parking time and road space, which may cause traffic congestion on the road and easily cause parking safety accidents. Therefore, it has important theoretical significance and practical value to study the automatic parking path planning in complex parking environment.

[0003] At present, the automatic parking path planning algorithm has the following:

[0004] The graph search algorithm represented by the A* algorithm is based on the grid map, searches the node with the minimum target cost according to the breadth-first or depth-first strategy, and the current node is used as the parent node of the next expansion node, and finally connected into a complete parking path.

[0005] The random sampling algorithm represented by the RRT algorithm generates a search tree by random sampling increment iteration, checks the feasibility of the node, and then determines the parking path by backtracking the parent node when the node on the tree is close to the target point.

[0006] The path planning of the A* algorithm can generally obtain the shortest or suboptimal parking path, and can realize obstacle avoidance, but the solving accuracy and real-time performance are limited by the grid resolution. Due to its greedy nature, it is easy to fall into local optimization. The RRT algorithm has the property of probability completeness, and can also realize obstacle avoidance, but its search is blind and is greatly limited in narrow scenes.

[0007] In the process of automatic parking of a vehicle, various complex parking environments are often faced, for example, disordered parking of roadside vehicles, incorrect parking in a parking space line, and static obstacles such as stone pillars and traffic cones around the parking space, combined with possible dynamic obstacles such as pedestrians, which together constitute a complex and variable parking environment. In addition, narrow parking spaces, unconventional parking space designs, and space limitations of parking channels further increase the complexity of the parking environment. In the face of such complex parking environments, existing automatic parking systems often fail to cope, resulting in failure of parking path planning, which must be taken over by the driver. Therefore, how to realize automatic parking path planning in a complex parking environment is a technical problem that needs to be solved urgently. SUMMARY

[0008] The application provides a path planning method, device and equipment applied to automatic parking and a storage medium. By improving the path cost function of the RS curve, different penalty coefficients are applied according to the advantages and disadvantages of the path, and a 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, the generation of a random tree is represented by multiple indexes, and each cost data of a grid point in a search space and an index point is calculated, which is converted into a sampling probability based on a Gaussian distribution, thereby optimizing the utilization rate of a sampling point and improving the search efficiency and convergence speed of the algorithm. A forgetting mechanism measured by fault tolerance avoids the failure of path planning caused by entering a local dead zone (local optimum), thereby improving adaptability.

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

[0010] Obtaining scene information, the scene information including target parking space, obstacle information, parking starting point and target point pose, establishing a kinematics equation of the vehicle, and determining coordinate information of the vehicle based on the kinematics equation;

[0011] Based on the scene information and the coordinate information of the vehicle, a random tree is established by iteration with the parking starting point as a starting point; the random tree includes multiple nodes, and node information is stored in each node, the node information including a node serial number, a node coordinate, a parent node serial number, a node heading angle, a node cost and a 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 of the node to the starting point;

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

[0013] Sampling is performed on the probability map to obtain random points. The nodes on the random tree are traversed, and the distance between each node and the random point is calculated. The node on the random tree that is closest to the random point is taken as the nearest point.

[0014] The nearest point is expanded to a new point in the direction of the random point by an expansion step size, and the new point is added to the random tree;

[0015] For a new point, search for nodes within a 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 the lower cost for each node, expand with the RS curve, perform path shortest optimization, and obtain the optimal path.

[0016] 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. Optimal path is then smoothed using multiple B-spline curves to output the parking path.

[0017] In one possible design, the vehicle's kinematic equations are established as follows:

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

[0019] The established kinematic equations are expressed as follows:

[0020]

[0021] In the formula, and These represent the velocity along the x-axis, the velocity along the y-axis, and the angular velocity along the heading angle, respectively. θ is the vehicle heading angle, δ is the equivalent steering angle of the front wheels, l is the vehicle wheelbase, and v is the vehicle speed at the center point of the rear axle.

[0022] In one possible design, the indicator points include a first indicator point, a second indicator point, a third indicator point, and a fourth indicator point. Based on the random tree, the indicator points are obtained, including:

[0023] Using the node in the random tree closest to the target point as the first index point, the node closest to the target point in 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 equivalent front wheel steering angle, and y is the longitudinal coordinate of the vehicle rear axle center point.

[0026] The child nodes of the longest and second longest branches in the random tree are taken 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 node and the child node, n1 is the index of all child nodes other than the parent node, and n2 is the index of all parent nodes to which the child node is connected to the starting point.

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

[0030]

[0031] In the formula, n is the number of nodes newly added to the random tree, and (x n ,y n ) is the average of the latest n points on the random tree.

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

[0033]

[0034] In the formula, (x n ,y n ) is the average 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 into multiple independent sampling regions, and the sampling probability of each sampling region is calculated according to the index points, obstacle information and target bias 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, a path cost function is determined, which is expressed as:

[0037]

[0038] In the formula, g p (x i ,y i ) is the path cost of the grid point, 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 the two points, C(j) is the total cost of the index point to the starting point, goal is the target point (x goal ,y goal ), and ||i-goal|| is the Euclidean distance between the two points.

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

[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, the distance direction is b the direction of w , and w is the obstacle threshold value.

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

[0044]

[0045] In the formula, κ is the target bias coefficient, the direction of is the direction of the index point pointing to the target point.

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

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

[0048]

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

[0050] Based on the angle between the grid point and the bias direction, an angle greater than π is selected and arranged according to a Gaussian distribution to construct a probability map based on bias action;

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

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

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

[0054]

[0055] wherein step min , step max are the minimum and maximum step lengths allowed, s is the distance from the new point to the obstacle, and w b is an obstacle threshold value;

[0056] The near point is expanded to a new point in the direction of a random point with the extended step length;

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

[0058]

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

[0060] If the node heading angle of the new point is greater than the angle threshold value, the new point is 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 value, nodes within a*step are searched, a is a range multiple, the RS cost of each selected node to the new point is calculated, the path formed by each node as the parent node of the new point is determined to minimize the cost from the starting point to the new point, the parent node with the minimum cost is selected for the new point, and the RS curve is expanded to optimize the shortest path;

[0062] Based on the RS curve after expansion, the vehicle rectangular contour is calculated according to geometric relations, collision detection is performed between the vehicle rectangular contour and the surrounding obstacles based on the vector cross product method, and if collision is detected, the new point is eliminated and will not be added to the random tree;

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

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

[0065]

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

[0067] In a possible design, the vehicle is simplified as a rectangle according to a kinematic model, and each obstacle is simplified as a polygon according to a shape;

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

[0069]

[0070] In the formula, M, N, P and Q are end points of the line segments MN and PQ, the line segment MN is any line segment constituting the rectangle of the vehicle body, and the line segment PQ is any line segment constituting the parking space boundary line or the polygon.

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

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

[0073] The random tree generation module is configured to establish a random tree by iteration with a parking starting point as a starting point based on the scene information and coordinate information of the vehicle; the random tree comprises a plurality of nodes, and node information is stored in each node, the node information comprising a node serial number, a node coordinate, a parent node serial number, a node heading angle, a node cost and a node total cost; wherein the node cost is a RS cost of the node and the parent node, and the total cost is a RS cost of the node to the starting point;

[0074] The probability map construction module is configured to obtain an index point, perform rasterization processing on the entire sampling space, divide the entire sampling space into a plurality of independent sampling regions, calculate a sampling probability of each sampling region according to the index point, obstacle information and a target bias, and construct a probability map.

[0075] The near point sampling module is configured to sample on the probability map, obtain a random point, traverse nodes on the random tree, calculate distances between the nodes and the random point respectively, and take a node closest to the random point on the random tree as a near point.

[0076] The new point expansion module is configured to expand a new point from the near point to the random point in an expansion step, and add the new point to the random tree.

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

[0078] The path output module is configured to calculate a distance between the new point and a target point, if the distance is greater than a termination distance threshold, resample on the probability map, iterate to plan a path, if the distance is less than or equal to the termination distance threshold, add the target point to the random tree, and terminate iteration, perform path smoothing optimization on the optimal path through a plurality of B-spline curves, and output a parking path.

[0079] In a third aspect, an electronic device is provided, comprising at least one processor and a memory; the memory stores computer execution instructions; the at least one processor executes the computer execution instructions stored in the memory, so that the at least one processor executes the path planning method applied to automatic parking as described in the first aspect and various possible designs of the first aspect.

[0080] In a fourth aspect, the embodiments of the present application provide a computer readable storage medium, which stores computer execution instructions, and when a processor executes the computer execution instructions, the application of the path planning method for automatic parking is realized.

[0081] In a fifth aspect, the embodiments of the present application provide a computer program product, which comprises a computer program, and when a processor executes the computer program, the application of the path planning method for automatic parking is realized.

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

[0083] The present application proposes a probability map sampling, which comprehensively considers the growth of random numbers and the search space grid points, improves the node utilization rate, and accelerates the convergence of the algorithm; the RS curve cost is improved, a penalty coefficient is applied to the path and the curvature, and the frequency of path reversing and turning is reduced; the turning angle limit is increased, the sampling points with large turning angles are removed, the path is more consistent with the vehicle kinematics; the variable expansion step is used to optimize the planning effect of the path near the obstacle; the node fault tolerance is proposed, the path planning is more flexible, can escape from the local dead zone, and change the search direction; the collision detection model is proposed to ensure the precise obstacle avoidance of the vehicle in the planning process; the cubic B-spline curve is used to ensure the general shape of the planning path curve, and the continuity of the parking path curvature. BRIEF DESCRIPTION OF DRAWINGS

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

[0085] Figure 1 A flowchart of the path planning method for automatic parking provided by the embodiments of the present application is shown in the accompanying drawings;

[0086] Figure 2 A specific implementation flowchart of the path planning method for automatic parking provided by the embodiments of the present application is shown in the accompanying drawings;

[0087] Figure 3 A structure diagram of the path planning device for automatic parking provided by the embodiments of the present application is shown in the accompanying drawings.

[0088] Through the above-mentioned drawings, the specific embodiments of the present application have been shown, and more detailed descriptions will be given in the following. These drawings and textual descriptions are not intended to limit the scope of the concept of the present application by any means, but to illustrate the concept of the present application to those skilled in the art by referring to specific embodiments. DETAILED DESCRIPTION

[0089] The exemplary embodiments will be described in detail herein with reference to the attached drawings. In the following description, like reference numerals refer to like elements throughout the description. The following exemplary embodiments are not representative of all embodiments consistent with the present application. Rather, they are merely examples of apparatuses and methods consistent with some aspects of the present application as detailed in the appended claims.

[0090] In the technical solutions of the present application, the collection, storage, use, processing, transmission, provision and disclosure of the financial data or user data and other information involved in the technical solutions comply with the relevant legal regulations and do not violate public order and good customs.

[0091] It should be noted that in the embodiments of the present application, some existing industry solutions such as software, components, models, etc. may be mentioned, which should be considered as exemplary, and the purpose is only to illustrate the feasibility of the implementation of the technical solutions of the present application, but it does not mean that the applicant has or will necessarily use the solution.

[0092] The technical solutions of the present application and how the technical solutions of the present application solve the above technical problems will be 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 described again in some embodiments. The embodiments of the present application will be described below with reference to the accompanying drawings.

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

[0094] A point is randomly generated in the entire search space, which is a random point x_rand. The current tree structure is traversed to find the node closest to the random point x_rand, which is the nearest node x_near. From the nearest node x_near, a new node x_new is generated by moving a fixed step length step in the direction of the random point x_rand.

[0095] Collision detection is performed on the newly generated path (from x_near to x_new) to determine whether the path intersects with the obstacle. If it intersects, the new node is discarded and random sampling is performed again; otherwise, the subsequent steps are continued.

[0096] In the tree, find all 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, select the candidate parent node with the minimum cost as the actual parent node of x_new.

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

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

[0099] Embodiments of the present application provide a path planning method applied to automatic parking. In combination with the above description, the path planning method improves the RRT* algorithm to realize path planning for automatic parking in a complex environment. As shown in Figure 1 The path planning method applied to automatic parking provided by embodiments of the present application includes the following steps S100-S700.

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

[0101] It should be noted that the scene information can be obtained by a sensing device of the vehicle, for example, image information collected by a camera, a target parking space automatically recognized from the image information, or a target parking space selected by a driver from the image information as the target parking space, obstacle information obtained by recognizing from the image information, a parking starting point being a vehicle coordinate when automatic parking is determined to start, and a target point pose being determined according to the target parking space and vehicle parameters such as size parameters of the vehicle.

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

[0103] During the parking process, the vehicle speed is generally low, it can be assumed that the wheel cannot be deformed and is pure rolling friction, no sliding behavior occurs, and the effects of camber, camber, and tire are ignored. The vehicle is simplified to a two-wheel model using the Ackerman steering principle, and kinematic analysis is performed.

[0104] The established kinematic equation is as follows:

[0105]

[0106] wherein, and are the velocity in x-axis direction, the velocity in y-axis direction and the yaw rate respectively, θ is the vehicle yaw angle, δ is the front wheel equivalent steering angle, l is the vehicle wheel base, and v is the vehicle rear axle center point speed.

[0107] S200: based on the scene information and the coordinate information of the vehicle, a random tree is established by taking the parking starting point as the starting point; the random tree includes a plurality of nodes, and node information is stored in each node, the node information including node serial number, node coordinates, parent node serial number, node yaw 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 of 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, and the RS curve is a non-continuous curvature curve composed of a plurality of straight lines and circular arcs, which is based on the Dubins curve and adds a backward movement mode, i.e. reversing. The RS curve solves the optimal path between the starting point and the ending point by calculating the path cost, and the obtained path has continuous changes in yaw angle and meets the kinematic requirements such as minimum turning radius of the vehicle, so that selecting the RS curve as the expansion mode 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 by improving the cost function of the RS curve. In this embodiment, all RS costs can be calculated by using the cost function of the improved RS curve.

[0110] Specifically, the starting and ending positions of the rear axle center point of the vehicle are taken as the input of the RS curve, and the basic movements of the vehicle include: forward movement (S + ), backward movement (S - ), forward left turn (L + ), forward right turn (R + ), backward left turn (L - ), and backward right turn (R - ), which are combined with each other to form 9 types of 48 basic RS curves.{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}

[0111] In the formula, C represents L or R, " | " represents the transition of the vehicle advancing or retreating, π / 2 and β represent the value of the arc through which the vehicle turns.

[0112] To ensure the comfort in the parking process, reduce the difficulty of tracking control, and reduce the number of operations such as turning, gear shifting and reversing in the parking process, a penalty term is designed for the cost function of the RS curve to increase the cost of turning, gear shifting and reversing, and thus reduce the frequency thereof.

[0113] The improved cost function of the RS curve is as follows:

[0114]

[0115] In the formula, P is the total path cost, L forward is the forward path length, L back is the reversing path length, L reverse is the gear shifting path length, L circular is the circular arc path length, k is the path curvature, and c is the number of path segments.

[0116] α, β, γ, K are respectively the penalty coefficients of each term, L forward is the desired path, and no penalty coefficient is required, α, β are the penalty coefficients of the reversing path and the gear shifting path, and is the path to be avoided, and the penalty coefficient thereof should be larger, γ is the penalty coefficient of the circular arc path, and the circular arc path is penalized to increase the proportion of the straight path, which is helpful to the path smoothing, and K is the curvature penalty to reduce the number of times and the degree of change of the path curvature.

[0117] S300: Based on the random tree, an index point is obtained, the entire sampling space is rasterized and divided into a plurality of independent sampling regions, the sampling probability of each sampling region is calculated according to the index point, obstacle information and target bias, and a probability map is constructed.

[0118] In the traditional RRT / RRT* algorithm, since the random sampling is directly selected, the pure random sampling method needs to consume more computing resources, which leads to slow convergence of the algorithm and low search efficiency. Moreover, when facing complex parking links, the planning complexity will be further improved due to the influence of too many obstacles, and this pure random sampling method is more likely to fall into local optimum and cause parking failure.

[0119] To address the shortcomings 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 regions. Based on the heuristic idea, the sampling probabilities of each sampling region are calculated based on the influences of the random tree growth, obstacle information, and target bias, and a probability map is constructed. The selection of sampling points in subsequent steps is based on the constructed probability map, thereby improving the search efficiency.

[0120] The path planning method gradually generates a random tree during implementation, and the parking path quality depends on the growth of the random tree. Reasonable selection of nodes on the random tree as index points can represent the growth of the random tree. In some embodiments, four index points are selected, namely a first index point, a second index point, a third index point, and a fourth index point.

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

[0122] The RRT* algorithm extends the random tree through new nodes. For each node on the random tree, the distance to the target point is calculated, and the requirement for the target point heading angle is considered. An angle correction is introduced, and the node that actually reaches the target point the least is selected. The expression is as follows:

[0123]

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

[0125] The nearest point to the target point represents the extension range of the random tree.

[0126] The second index point is the child nodes of the longest / second longest branch (x s1 ,y s1 ) and (x s2 ,y s2 ).

[0127] In the process of path planning, the RRT* algorithm extends multiple branches of the random tree, which are connected through child / parent nodes. The longest branch is the farthest path planned by the algorithm. For the end nodes of each branch, they are child nodes without parent nodes. To reduce randomness, the child nodes of the longest and second longest branches are selected, and the expression is as follows:

[0128]

[0129] In the formula, P is the RS curve cost between 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 to which the child nodes are connected to the starting point.

[0130] The child nodes of the longest / shortest branch represent the farthest path currently planned, and the actual direction of the random tree. The longest / shortest branch can be optimized to obtain the shortest parking path.

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

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

[0133]

[0134] In the formula, n is the number of nodes recently added to the random tree.

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

[0136] The fourth index point is the average of all random tree nodes (x N ,y N ).

[0137] Similar to the third index 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 coordinate can also be obtained by calculation. The expression is as follows:

[0138]

[0139] In the formula, N is the total number of nodes of the random tree.

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

[0141] The first index point selects 1 node, the second index point selects 2 nodes, and the three nodes are determined points. Based on the properties of the random tree generated by the RRT* algorithm, the cost, heading angle, and other determined information of these nodes can be obtained. The third index point and the fourth index point are average points calculated from multiple points, which are virtual points, and only the coordinate information is used.

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

[0143] When automatic parking, we hope to get as short as possible parking path, so the influence of path cost on random tree must be considered.

[0144] RS curve as an extension of the random tree, in the process of path planning, the cost between the newly expanded child node and its parent node is P, that is, the cost of the current RS curve, and the total cost of each child node in the algorithm is the total cost of its parent node connection to the starting point. The cost expression of any node on the tree to the starting point is:

[0145]

[0146] For the grid points in the search space, according to their distance to the target point and the cost of the random tree, the following path cost function is designed:

[0147]

[0148] In the formula, g p (x i ,y i ) is the path cost of the grid point, 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 the two points, C(j) is the total cost of the index point to the starting point, goal is the target point (x goal ,y goal ), and ||i-goal|| is the Euclidean distance between the two points.

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

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

[0151] For obstacle bias, the random tree is not affected by the obstacle throughout the process. Set the obstacle threshold to prevent falling into local optimum and causing planning failure. The threshold is the distance between the obstacle and the decision point 1 car body width w b When the index point is within the action threshold, the obstacle bias is performed.

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

[0153]

[0154] In the formula, η is the obstacle bias coefficient, d is the distance between the index point and the obstacle, and the distance direction is The direction of the index point.

[0155] In particular, for the determined points such as index 1 and 2, which can accurately describe the node pose of the vehicle, the obstacle bias with larger weight should be set, so the corresponding bias coefficient is also larger.

[0156] Similarly, the target bias can further accelerate the convergence of the algorithm to the target point through the guiding effect of the target point on the random tree. Target bias effect The expression is:

[0157]

[0158] In the formula, κ is the target bias coefficient, The direction of the index point is the direction of the index point to the target point.

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

[0160]

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

[0162] Only angles greater than π are selected, which are arranged according to the Gaussian distribution, and the angle corresponds to the sampling probability. The smaller the angle, the higher the sampling probability, and the probability map based on the bias effect is constructed.

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

[0164] It should be noted that the way of fusing the path cost based probability map and the bias effect based probability map to obtain the final probability map can adopt the existing way, including but not limited to the following four ways:

[0165] 1) Average method: simple average or weighted average.

[0166] 2) Multiplication / Bayesian fusion: assuming independence, normalized by multiplication or log-likelihood addition.

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

[0168] 4) Machine learning model fusion: using a model such as logistic regression to learn the optimal fusion method.

[0169] S400: Sampling 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 a near point.

[0170] In this embodiment, sampling is performed on the probability map, and a random point x_rand is obtained by sampling in the search space based on the constructed probability map. The nodes on the random tree are traversed, and the Euclidean distance between each node and x_rand is calculated to determine the node x_near on the random tree closest to x_rand.

[0171] S500: Expanding a new point from the near point to the random point in the direction of the random point with an expanded step, and adding the new point to the random tree.

[0172] In this embodiment, expanding a new point x_new from the near point x_near to x_rand in a certain step includes the following steps:

[0173] S501: Determine the expansion step. Determine whether x_near is within the obstacle threshold range wb, and the expression of the expansion step step is:

[0174]

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

[0176] In the process of path planning, when the random tree grows near the obstacle, its planning efficiency is often reduced, and the fixed step is difficult to meet the kinematic requirements, so the obstacle threshold w bIn the planning process, the expansion step of the x_near point can be flexibly changed when the distance between the x_near point and the obstacle is within the threshold range. The closer the distance between the x_near point and 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 away from the obstacle, the step is appropriately increased, which can improve the search efficiency.

[0177] S502: Determine the corner limit. The node heading angle of the new point x_new is the angle between the line connecting the new point x_new and the corresponding parent node x_near and the x-axis. If the angle is greater than the angle threshold θ m , 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, and its expression is:

[0178]

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

[0180] In step S502, in order to ensure that the parking path planned by the vehicle meets the kinematic requirements of the vehicle, the nodes on the random tree will carry heading angle information. In the expansion process 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, the x_new point will not be added to the random tree.

[0181] S503: Re-select the parent node. If the new point x_new is within the corner limit, search for 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 the RS curve, and optimize the shortest path.

[0182] S504: Perform collision detection. For the expanded curve, which is the trajectory of the center point of the rear axle of the vehicle, calculate the rectangular contour of the vehicle based on the geometric relationship, and perform collision detection with the surrounding obstacles based on the vector cross product method. If a collision is detected, the new point x_new is eliminated and will not be added to the random tree.

[0183] Specifically, during the parking process of the vehicle, there may be static obstacles such as stone pillars and cone barrels around the parking space, or dynamic obstacles such as pedestrians. At the same time, the vehicle cannot cross the boundary line of the target parking space and must ensure 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.

[0184] The vector cross product method is used for collision detection of vehicles, which judges whether two line segments intersect by calculating the cross product of two vectors twice. The cross product of two vectors can be used to determine the spatial relationship of the two vectors, and there are two two-dimensional vectors The two-dimensional vector cross product formula is:

[0185]

[0186] In the formula, if then In the counterclockwise direction of , if then In the clockwise direction of

[0187] Based on the above geometric meaning, it can be verified whether two line segments intersect by two cross experiments. There are two intersecting line segments MN and PQ, and two vector cross product calculations are performed to detect whether points P and Q are on the two sides of MN and whether points M and N are on the two sides of PQ, that is, the following conditions are met:

[0188]

[0189] That is, the two line segments intersect.

[0190] Further, the vehicle and the obstacle in the parking process are reasonably simplified. The vehicle is simplified as a rectangle according to the kinematic model, and the parking space boundary line itself is a standard rectangle. Each obstacle is simplified as a polygon according to the shape. When performing collision detection, the four line segments of the vehicle body rectangle are sequentially executed with the line segments of the parking space boundary line and the obstacle polygon to perform the above collision detection conditions, so as to accurately detect whether the vehicle collides with the obstacle or the parking space boundary.

[0191] S505: Forgetting mechanism. The new point x_new eliminated by the above corner limit and collision detection, the fault tolerance degree of the corresponding parent node is reduced by 1, and when the fault tolerance degree drops to 0, the node no longer participates in the expansion of the nodes on the tree.

[0192] For a parking environment with a narrow parking space or many obstacles, the RRT* algorithm has many x_new that cannot meet the corner limit and collision detection during planning, and some random tree nodes may not be successfully expanded several times. Therefore, the fault tolerance M of each node on the random tree is set. When the node is selected as the x_near point, the fault tolerance of the x_new point is reduced by 1 when it cannot be successfully expanded due to the corner limit or collision detection. When the fault tolerance M is 0, the node no longer participates in the expansion of the nodes on the tree.

[0193] In particular, in order to ensure the completeness of the algorithm planning, the fault tolerance of the starting point is set to infinity inf.

[0194] ​The new point x_new completing the above steps will be added to the random tree, and each information thereof will also be stored into the random tree node.

[0195] S600: For the new point, search nodes within a set range, and calculate RS cost to each selected node, judge the path connected by the new point as the parent node of each node to make the cost from the starting point to each node minimum, update the parent node of each node with smaller cost, expand with the RS curve, perform path shortest optimization, and obtain an optimal path.

[0196] In this embodiment, the purpose of step S600 is to perform node reconnection for the newly added node, which is similar to reselecting a parent node. For the new point x_new, search tree nodes within a*step range, a is a range multiple, and calculate RS cost to each selected node, judge the path connected by x_new as the parent node of each node to make the cost from the starting point to each node minimum, update the parent node of each node with smaller cost, and expand with the RS curve to perform path shortest optimization.

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

[0198] In this embodiment, the function of step S700 is used for iteration termination judgment. 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 a termination distance threshold D min , return to Step 2, perform iteration to plan a path; if the distance is less than the termination distance threshold D min , directly add the target point to the random tree, and terminate iteration, and output a parking path.

[0199] Finally, perform path smoothing optimization on the planned path through a cubic B-spline curve. The parking path is composed of RS curves, and each point on the curve can be a control point of the B-spline curve, and the curve expression is:

[0200]

[0201] In the formula, N i,3 (t) is a cubic B-spline base function, and V is a control point.

[0202] RS curve is used to expand and connect between random tree nodes. Although RS curve can ensure the continuity and smoothness of the heading angle of the path, the curvature of the path is discontinuous. At the junction of straight line-arc and arc-arc, the curvature will change suddenly, which will lead to difficult tracking control of parking and aggravate tire wear. The cubic B-spline curve is a cubic polynomial curve segment controlled by a series of points called control points. These curve segments are smoothly connected between adjacent control points to form a continuous curve. By constructing a cubic B-spline curve with the points on the RS path as control points, the curvature of the parking path can be made continuous and smooth while the general shape of the RS curve is ensured.

[0203] In some embodiments, as shown in FIG. 1, a specific implementation flowchart of a path planning method applied to automatic parking is provided, which includes the following steps: Figure 2

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

[0205] Among them, the parking link information and the vehicle pose information are loaded, including target parking space, obstacle information, parking starting point and target point pose, vehicle parameters, etc. The vehicle model is simplified to a two-wheel model by Ackerman steering principle, and the kinematics equation describing the motion of 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 by the algorithm.

[0206] S202: Establish an initial random tree.

[0207] S203: Calculate path cost and bias effect index based on index point of random tree information, construct and update probability map.

[0208] The construction of the probability map has been described above and will not be repeated here. For the initial stage of random tree growth, there are too few nodes on the tree, and multiple index points may correspond to the same node. Combining different indexes of the same node can avoid repeated calculation. The probability map needs to be updated in real time as the number of nodes on the tree increases to maintain the heuristic of the method.

[0209] S204: Sample to obtain a random point x_rand based on the probability map.

[0210] S205: Traverse the random tree to find the nearest tree node to x_rand as the near point x_near.

[0211] Sampling on the probability map, based on the constructed probability map, sampling in the search space to obtain a random point x_rand. Traverse the nodes on the random tree, calculate the Euclidean distance between each node and x_rand respectively, and determine the node x_near on the random tree closest to x_rand. ​

[0212] S206: Determine whether the included angle between x_rand and x_near is greater than the included angle threshold, if yes, execute S207, if no, execute S208.

[0213] S207: Corresponding to the fault tolerance of x_near, return to S203.

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

[0215] S209: Take x_near as the parent node, and RS curve as the expansion mode to obtain x_new.

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

[0217] S211: Determine whether the path collides with the obstacle, if yes, execute S207, if no, execute S211.

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

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

[0220] S214: Determine whether the path collides with the obstacle, if no, execute S215.

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

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

[0223] S217: Path smoothing.

[0224] The embodiment of the application also provides a path planning device applied to automatic parking, as shown in the figure, the path planning device applied to automatic parking comprises: Figure 3

[0225] A data acquisition module 301 is configured to acquire scene information, the scene information comprising target parking space, obstacle information, parking starting point and target point pose, establish kinematics equation of the vehicle, and determine coordinate information of the vehicle based on the kinematics equation.

[0226] ​The random tree generation module 302 is configured to establish a random tree by iteration with a parking starting point as a starting point based on the scene information and coordinate information of the vehicle; the random tree comprises a plurality of nodes, and node information is stored in each node, the node information comprising a node serial number, a node coordinate, a parent node serial number, a node heading angle, a node cost and a node total cost; wherein the node cost is a RS cost of the node and the parent node, and the total cost is a RS cost of the node to the starting point;

[0227] The probability map construction module 303 is configured to obtain an index point based on the random tree, perform rasterization processing on the entire sampling space, divide the entire sampling space into a plurality of independent sampling regions, calculate a sampling probability of each sampling region according to the index point, obstacle information and a target bias, and construct a probability map.

[0228] The near point sampling module 304 is configured to sample on the probability map, obtain a random point, traverse nodes on the random tree, calculate distances between each node and the random point, and take a node closest to the random point on the random tree as a near point.

[0229] The new point expansion module 305 is configured to expand a new point from the near point to the random point in an expansion step, and add the new point to the random tree.

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

[0231] The path output module 307 is configured to calculate a distance between the new point and a target point, if the distance is greater than a termination distance threshold, resample on the probability map, iterate to plan a path, if the distance is less than or equal to the termination distance threshold, add the target point to the random tree and terminate iteration, perform path smoothing optimization by a plurality of B-spline curves on the optimal path, and output a parking path.

[0232] An electronic device is provided in an embodiment of the present application. The electronic device can include a processor, a memory, wherein the processor and the memory can communicate; for example, the processor and the memory communicate through a communication bus.

[0233] The processor executes computer-executed instructions stored in the memory, so that the processor executes the solutions in the above embodiments. The processor can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; can also be a digital signal processor (DSP), an application-specific integrated circuit ASIC, a field programmable gate array FPGA or other programmable logic device, a discrete gate or transistor logic device, a discrete hardware component.

[0234] The communication bus can be a peripheral component interconnect (PCI) bus or an extended industry standard architecture (EISA) bus, etc. 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 can include random access memory (RAM), and can also include non-volatile memory.

[0235] The electronic device provided by the embodiments of the present application can be the terminal device of the above embodiments.

[0236] The embodiments of the present application further provide a computer readable storage medium, which stores computer instructions, and when the computer instructions run on a computer, the computer executes the technical solutions of the path planning method for automatic parking applied by the above embodiments.

[0237] The embodiments of the present application further provide a computer program product, which includes a computer program stored in a computer readable storage medium, and at least one processor can read the computer program from the computer readable storage medium, and when the at least one processor executes the computer program, the technical solutions of the path planning method for automatic parking applied in the above embodiments can be realized.

[0238] In several embodiments provided in the present application, it should be understood that the disclosed devices and methods can be implemented in other manners. For example, the embodiments of the device described above are merely schematic. For example, the division of the modules is merely logical function division. There can be another division manner for the actual implementation. For example, a plurality of modules or a component can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the displayed or discussed mutual couplings or direct couplings or communication connections can be indirect couplings or communication connections through some interfaces, devices or modules, and can be electrical, mechanical or in other forms.

[0239] The modules illustrated as separated components can or can not be physically separated, and the components illustrated as modules can or can not be physical units, i.e., can be located in one place, or can be distributed on a plurality of network units. Some or all of the modules can be selected according to actual needs to implement the embodiments of the present application.

[0240] In addition, each function module in each embodiment of the present application can be integrated in one processing unit, or each module can be physically present separately, or two or more modules can be integrated in one unit. The unit formed by the above modules can be realized in the form of hardware, or in the form of hardware plus software function unit.

[0241] The integrated module realized in the form of software function module can be stored in a computer readable storage medium. The software function module stored in the storage medium includes a plurality of instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) or a processor to execute part of the steps of the method of each embodiment of the present application.

[0242] It should be understood that the processor can be a central processing unit (CPU), and can also be other general-purpose processors, digital signal processors (DSP), application specific integrated circuits (ASIC), etc. The general-purpose processor can be a microprocessor, or the processor can also be any conventional processor. The steps of the method disclosed in the present application can be directly embodied as a hardware processor to execute, or be executed by a combination of hardware and software modules in the processor.

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

[0244] 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.

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

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

[0247] Those skilled in the art can understand that all or part of the steps of the above-mentioned method embodiments can be completed by program instruction-related hardware. The foregoing 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 foregoing storage medium includes a ROM, a RAM, a magnetic disk, or an optical disk, and various media that can store program codes.

[0248] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present application, and not to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacements to some or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A path planning method applied to automatic parking, characterized in that, The method comprises: acquiring scene information, the scene information comprising a target parking space, obstacle information, a parking starting point and a target point pose, establishing a kinematic equation of the vehicle, and determining coordinate information of the vehicle based on the kinematic equation; based on the scene information and the coordinate information of the vehicle, performing iteration with the parking starting point as a starting point to establish a random tree; the random tree comprises a plurality of nodes, and node information is stored in each node, the node information comprising a node serial number, a node coordinate, a parent node serial number, a node heading angle, a node cost and a node total cost; wherein the node cost is a RS cost of the node and the parent node, and the total cost is a RS cost from the node to the starting point; based on the random tree, acquiring an index point, performing rasterization processing on the entire sampling space to divide the entire sampling space into a plurality of independent sampling regions, calculating a sampling probability of each sampling region according to the index point, the obstacle information and a target bias to form a probability map; sampling on the probability map to acquire a random point, traversing the nodes on the random tree, respectively calculating distances between each node and the random point, and taking a node closest to the random point on the random tree as a near point; extending a new point from the near point to the random point in an extended step length, and adding the new point to the random tree; for the new point, searching for nodes within a set range, calculating a RS cost to each selected node, judging a path formed by taking the new point as a parent node of each node to minimize the cost from the starting point to each node, updating a parent node with a smaller cost for each node, extending by a RS curve, performing shortest path optimization to obtain an optimal path; calculating a distance between the new point and the target point, if the distance is greater than a termination distance threshold, resampling on the probability map to iteratively plan a path, if the distance is less than or equal to the termination distance threshold, adding the target point to the random tree and terminating iteration, and performing path smoothing optimization on the obtained optimal path by a plurality of B-spline curves to output a parking path; calculating a RS cost based on an improved RS curve cost function, the improved RS curve cost function being expressed as: where P is the total cost of the path, L forward is the forward path length, L back is the reverse path length, L reverse is the shift path length, L circular is the circular path length, k l+1 and k l are the curvatures of the (l+1)th and lth path segments, c is the number of path segments, and a, b, g, K are the penalty coefficients of the reverse path length, shift path length, circular path length, and path curvature, respectively.

2. The path planning method for automatic parking according to claim 1, wherein, the kinematic equation of the vehicle is established in the following manner: simplifying the vehicle into a two-wheel model by using Ackerman steering principle and performing kinematic analysis to establish the kinematic equation of the vehicle; the established kinematic equation is expressed as: wherein and are the x-axis velocity, y-axis velocity and yaw rate, respectively, θ is the vehicle yaw angle, δ is the front wheel equivalent steering angle, l is the vehicle wheel base, and v is the vehicle rear axle center point speed.

3. The path planning method for automatic parking according to claim 1, wherein, the index point comprises a first index point, a second index point, a third index point and a fourth index point, and the index point is acquired based on the random tree in the following manner: taking a node closest to the target point on the random tree as the first index point, and determining the node closest to the target point on the random tree by the following formula: wherein (x last ,y last ) is the node closest to the target point on the random tree, ω is an 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, i is the node number, m is the number of nodes on the random tree, θ is the vehicle heading angle, δ is the equivalent steering angle of the front wheel, and y is the longitudinal coordinate of the center point of the rear axle of the vehicle. taking child nodes of the longest and second longest branches in the random tree as the second index point, and determining the child nodes of the longest and second longest branches in the random tree by the following formula: where (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 the parent node, and n2 is the index of all parent nodes to which the child node is connected to the starting point; taking an average of the latest n points on the random tree as the third index point, and determining the average of the latest n points on the random tree by the following formula: where n is the number of nodes recently added to the random tree, (x n , y n ) is the average of the last n points on the random tree; taking an average of all nodes of the random tree as the fourth index point, and determining the average of all nodes of the random tree by the following formula: where (x n ,y n ) is the average of all random tree nodes, and N is the total number of nodes of the random tree.

4. The path planning method for automatic parking according to claim 3, wherein, The whole sampling space is rasterized to divide into multiple independent sampling regions, and the sampling probability of each sampling region is calculated according to the index point, obstacle information and target bias to form a probability map, including: According to the distance between the grid points in the search space and the target point and the cost of the random tree, the path cost function is determined, which is represented as: where g p (x i ,y i ) is the path cost of the grid point, 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 the two points, C(j) is the total cost of the index point to the starting point, goal is the target point (x goal ,y goal ), and ||i-goal|| is the Euclidean distance between the two points. According to the path cost of each grid point calculated by the path cost function, the results are arranged according to the Gaussian distribution to construct the probability map based on the path cost; Based on the obstacle information, the obstacle biasing effect is calculated by the following formula In the formula, η is an obstacle bias coefficient, d is the distance from the index point to the obstacle, the distance direction is the direction of w b is the obstacle threshold; The target biasing effect is calculated by the following equation In the formula, k is a target bias coefficient, The direction of the index point to the target point. Based on the obstacle biasing action and the target biasing action, a common action direction is determined by vector addition, including a biasing action direction of the index point and a direction of a grid point in the search space to the index point, wherein the biasing action direction of the index point is The direction of the grid point in the search space to the index point is According to the common action direction, the obstacle and target bias function is determined as: where θ(x i ,y i ) is the angle between the grid point and the direction of the biasing action. Based on the angle between the grid point and the bias action direction, the angle greater than π is selected and arranged according to the Gaussian distribution to construct the probability map based on the bias action; The probability map based on the path cost and the probability map based on the bias action are fused to obtain the probability map.

5. The path planning method for automatic parking according to claim 1, wherein, The near point is expanded to a new point in the direction of the random point with an expansion step, and the new point is added to the random tree, including: The expansion step is determined by the following formula: where step min , step max are the minimum and maximum allowed steps, s is the distance of the new point to the obstacle, w b is the obstacle threshold; The near point is expanded to a new point in the direction of the random point with the expansion step; The angle threshold θ is determined by the following equation 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 is 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, the nodes within the range of a*step are searched, a is the range multiple, the RS cost of each selected node to the new point is calculated, the path formed by each node as the parent node of the new point is determined to minimize the cost from the starting point to the new point, the parent node with the minimum cost is selected for the new point, and the RS curve is expanded to optimize the shortest path; Based on the expanded RS curve, the vehicle rectangular contour is calculated according to the geometric relationship, and the collision detection is performed with the surrounding obstacles based on the vector cross product method, and if a collision is detected, the new point is eliminated and will not be added to the random tree; The fault tolerance of the eliminated new point and the corresponding parent node is reduced by 1, and when the fault tolerance of the node decreases to 0, the node will no longer participate in the expansion of the nodes on the tree.

6. The path planning method for automatic parking according to claim 5, wherein, Based on the expanded RS curve, the vehicle rectangular contour is calculated according to the geometric relationship, and the collision detection is performed with the surrounding obstacles based on the vector cross product method, including: According to the kinematic model, the vehicle is simplified as a rectangle, and each obstacle is simplified as a polygon according to the shape; When performing collision detection, the four line segments constituting the vehicle body rectangle are sequentially subjected to collision detection conditions with the parking space boundary line and the polygon line segment, and when the collision detection condition is met, it is determined that a collision will occur; wherein the collision detection condition is represented as: In the formula, M, N, P and Q are the end points of line segments MN and PQ, MN is any line segment constituting the vehicle body rectangle, and PQ is any line segment constituting the parking space boundary line or polygon.

7. A path planning device for automatic parking, characterized by comprising: The device comprises: A data acquisition module configured to acquire scene information including target parking space, obstacle information, parking starting point and target point pose, establish a kinematic equation of the vehicle, and determine coordinate information of the vehicle based on the kinematic equation; The random tree generation module is configured to establish a random tree by iteration with a parking starting point as a starting point based on the scene information and coordinate information of the vehicle; the random tree comprises a plurality of nodes, and node information is stored in each node, the node information comprising a node serial number, node coordinates, a parent node serial number, a node heading angle, a node cost and a node total cost; wherein the node cost is a RS cost of the node and the parent node, and the total cost is a RS cost of the node to the starting point; The probability map construction module is configured to obtain index points, perform rasterization processing on the entire sampling space, divide the sampling space into a plurality of independent sampling regions, and calculate a sampling probability of each sampling region according to the index points, obstacle information and target bias to construct a probability map based on the random tree; The near point sampling module is configured to sample on the probability map to obtain a random point, traverse nodes on the random tree, calculate distances between each node and the random point, and take a node closest to the random point on the random tree as a near point; The new point expansion module is configured to expand a new point from the near point to the random point in an expansion step, and add the new point to the random tree; The node reconnection module is configured to, for the new point, search for nodes within a set range, calculate RS costs to each selected node, determine a path formed by the new point as a parent node of each node to minimize costs from the starting point to each node, update a parent node with a smaller cost for each node, expand in an RS curve, optimize a shortest path, and obtain an optimal path; The path output module is configured to calculate a distance between the new point and a target point, sample on the probability map again to plan a path if the distance is greater than a termination distance threshold, add the target point to the random tree and terminate iteration if the distance is less than or equal to the termination distance threshold, perform path smoothing optimization on the optimal path through a plurality of B-spline curves, and output a parking path. A RS cost is calculated based on an improved RS curve cost function, and the improved RS curve cost function is represented as: where P is the total cost of the path, L forward is the forward path length, L back is the reverse path length, L reverse is the shift path length, L circular is the circular path length, k l+1 and k l are the curvatures of the (l+1)th and lth path segments, respectively, c is the number of path segments, and a, b, g, K are the penalty coefficients of the reverse path length, shift path length, circular path length, and path curvature, respectively.

8. An electronic device, comprising: The application comprises: a processor and a memory connected in communication with the processor; the memory stores computer execution instructions; the processor executes the computer execution instructions stored in the memory to implement the path planning method for automatic parking according to any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, The computer readable storage medium stores computer execution instructions, and the computer execution instructions are executed by the processor to implement the path planning method for automatic parking according to any one of claims 1-6.

Images