A method and system for automatically parking a vehicle

Abstract

The present invention provides a path planning method and system for automatic parking, comprising: S1, obtaining the starting node and the ending node of the vehicle parking; S2, if there is an HC curve that successfully connects the current node and the ending node between the current node and the ending node, and the HC curve between the current node and the ending node is valid; then the HC curve with the lowest HC curve cost value is selected as the path of the vehicle parking, and the path planning is completed; if it does not exist or is invalid, then enter step S3 to perform node expansion; S3, expand multiple expansion nodes based on the current node, so that the multiple expansion nodes are connected to the current node through a clothoid curve; S4, perform validity detection on multiple expansion nodes; if a certain expansion node is valid, the expansion node is used as a valid expansion node; S5, the valid expansion node with the lowest expansion node cost value is selected as the current node, and steps S2 to S5 are repeated. The present invention improves the success rate of node connection.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a path planning method and system for automatic parking. Background Art

[0002] With the rapid development of autonomous driving technology, the APA system (Automated Parking Assistance, APA) has become an important part of the smart car field. The APA system uses the vehicle's own sensors and computing units to achieve automatic parking, greatly facilitating the driver's parking operation. However, achieving efficient and accurate path planning remains one of the main challenges facing the APA system.

[0003] Path planning is one of the core functions of the APA system. Its purpose is to plan a safe, smooth and efficient parking path in a complex parking environment. Currently, there are many types of path planning algorithms. Among them, the Hybrid A algorithm combines the advantages of the A algorithm and the RS curve (Reeds-Shepp curve) to generate a smooth and feasible path, and is widely used in the APA system.

[0004] However, the existing Hybrid A* algorithm still has some shortcomings in practical applications:

[0005] 1) In the existing Hybrid A* algorithm node expansion stage, only the position and heading angle information are considered, and the curvature information is not combined for expansion. However, the continuity of curvature plays a key role in the underlying control.

[0006] 2) The existing Hybrid A* algorithm uses the RS curve (Reeds-Shepp curve) for connection in the analytical expansion stage. The RS curve (Reeds-Shepp curve) is composed of straight lines and arcs, and has the problem of discontinuous curvature mutation points, which affects the control accuracy of the lower-level controller.

[0007] 3) The paths generated by the existing Hybrid A* algorithm using RS curves are usually poor in anthropomorphism and do not conform to the driving habits of normal drivers.

[0008] 4) The existing Hybrid A* algorithm uses RS curves to generate paths that are not smooth enough and usually needs to be smoothed using path optimization before it can be sent to the lower-level controller. This increases the total time of the entire path planning and consumes more of the algorithm. Moreover, if the smoothing is not chosen properly, it will cause other problems for the entire path.

[0009] In order to solve the problem of insufficient smoothness of the path generated by the RS curve, the prior art uses a CC circular curve (Continuous Curvature circle) to generate the path. Although the CC circular curve can solve the problem of uneven path to a certain extent, when the CC circular curve is used to connect the parking nodes, there is a situation where the connection from the parking start node to the end node is unsuccessful. Moreover, as the turning angle of the CC circular curve increases, the length of the CC circular curve is longer. When the vehicle travels along the path generated by the CC circular curve, the vehicle travels on a longer path. Summary of the invention

[0010] The present invention proposes a path planning method and system for automatic parking to solve the technical problems in the prior art that the RS curve of path planning has a discontinuous curvature mutation point, the CC circular curve has an unsuccessful connection from the parking start point to the parking end point, and the path length is long.

[0011] One aspect of the present invention is to provide a path planning method for automatic parking, the path planning method comprising the following method steps:

[0012] S1. Obtain the starting node and the ending node of the vehicle parking, and use the starting node as the current node; construct a planning area according to the starting node and the ending node of the vehicle parking, and grid the planning area;

[0013] S2, searching for an HC curve connecting the current node and the end node, if there is at least one HC curve between the current node and the end node that successfully connects the current node and the end node, and the HC curve between the current node and the end node is valid;

[0014] Then the HC curve with the lowest HC curve cost is selected as the path for the vehicle to park, and the path planning is completed;

[0015] If there is no HC curve that successfully connects the current node and the terminating node between the current node and the terminating node, or the HC curve between the current node and the terminating node is invalid, proceed to step S3 to perform node expansion;

[0016] S3, dividing the forward area and the backward area based on the current node; calling the clothoid curve, extending multiple extended nodes in the forward area of ​​the current node, and extending multiple extended nodes in the backward area of ​​the current node, so that the multiple extended nodes are connected to the current node through the clothoid curve;

[0017] S4, performing validity check on multiple extension nodes; if an extension node is invalid, the extension node is removed;

[0018] If an extension node is valid, the extension node is used as a valid extension node and is placed in the valid extension node set;

[0019] S5. Select the valid extension node with the lowest extension node cost in the valid extension node set as the current node, repeat steps S2 to S5, and cyclically search the HC curve until the current node and the terminal node are successfully connected.

[0020] In a preferred embodiment, the status information of the starting node, the ending node, and the extended node of the vehicle parking includes the location information, the heading angle, and the path curvature information of the node.

[0021] In a preferred embodiment, in step S2, the cost value of each feasible HC curve is calculated by a cost function;

[0022] The cost function includes at least: a straight line penalty term, an arc penalty term, a gear shift penalty term, a short track penalty term and a long track penalty term.

[0023] In a preferred embodiment, in step S2, the HC curve between the current node and the end node is effectively detected by the following method:

[0024] S201, determining whether there is an obstacle between the current node and the end node that may collide with the vehicle;

[0025] S202, determining whether the current node and the end node, and all path points of the HC curve between the current node and the end node are located in the constructed planning area;

[0026] S203: If there is no obstacle between the current node and the end node that collides with the vehicle, and the HC curve between the current node and the end node does not collide with the obstacle;

[0027] If the current node and the terminal node, as well as all the path points of the HC curve between the current node and the terminal node, are located in the planning area, then the HC curve between the current node and the terminal node is valid, otherwise the HC curve between the current node and the terminal node is invalid.

[0028] In a preferred embodiment, in step S201, it is determined whether there is an obstacle between the current node and the end node that collides with the vehicle by the following method:

[0029] S2011. At each path point of the HC curve that successfully connects the current node and the terminal node, a rectangular frame matching the size of the vehicle is constructed, and three circles are used to enclose the rectangular frame, wherein the radius of the three circles is R;

[0030] S2012. Establish an ESDF map, and use the ESDF map to obtain the distances from the centers of the three circles to the nearest obstacles;

[0031] S2013. When a path point of the HC curve that successfully connects the current node and the terminal node, and the distances from the centers of the three circles to the nearest obstacle are all greater than the radius R, the path point is valid;

[0032] S2014: When a path point of the HC curve that successfully connects the current node and the terminal node has at least one circle whose center to the nearest obstacle is less than the radius R, the process proceeds to step S2015 to determine the validity of the path point;

[0033] S2015, using the ESDF map to obtain the nearest obstacle in step S2014 whose distance from the center of a circle is less than the radius R;

[0034] Determine whether the obstacle is within a rectangular frame that matches the size of the vehicle;

[0035] When the obstacle is within a rectangular box that matches the size of the vehicle, the waypoint is invalid;

[0036] When the obstacle is outside the rectangular box that matches the size of the vehicle, the waypoint is valid;

[0037] S2016. When all path points of the HC curve that successfully connects the current node and the terminal node are all valid, there is no obstacle between the current node and the terminal node that may collide with the vehicle.

[0038] In a preferred embodiment, in step S3, calling the clothoid curve to expand multiple expansion nodes in the forward area of ​​the current node and to expand multiple expansion nodes in the backward area of ​​the current node includes:

[0039] S301, dividing a forward area of ​​the current node into multiple sharpness values; for each sharpness value, setting a step length of node expansion, calling a clothoid curve, calculating and obtaining multiple expansion nodes corresponding to the multiple sharpness values, and connecting the multiple expansion nodes with the current node through the clothoid curve;

[0040] S302, dividing the retreat area of ​​the current node into multiple sharpness values; for each sharpness value, setting the step size of the node expansion, calling the clothoid curve, calculating and obtaining multiple expansion nodes corresponding to the multiple sharpness values, and connecting the multiple expansion nodes with the current node through the clothoid curve.

[0041] In a preferred embodiment, in step S4, the validity of multiple extension nodes is checked by the following method:

[0042] S401, determining whether there is an obstacle between the current node and the extended node that collides with the vehicle;

[0043] S402, determining whether the current node and the extended node are located in the constructed planning area;

[0044] S403: If there are no obstacles between the current node and the extended node that may collide with the vehicle, and the current node and the extended node are located within the planning area, then the extended node is valid; otherwise, the extended node is invalid.

[0045] In a preferred embodiment, in step S4, the path planning method further includes, if a certain extended node is valid, performing a pruning operation on the extended node:

[0046] If a certain extended node is valid, determine whether the curvature information of the extended node is greater than a preset threshold;

[0047] If the curvature information of the extended node is greater than a preset threshold, the extended node is removed; if the curvature information of the extended node is less than the preset threshold, the extended node is taken as a valid extended node.

[0048] In a preferred embodiment, in step S5, the expansion node cost value of the effective expansion node is calculated by using the expansion node cost function;

[0049] The extended node cost function includes at least a forward penalty term, a backward penalty term, a gear shift penalty term, a path angle change penalty term, a path angle penalty term, and a path distance to the nearest obstacle penalty term.

[0050] Another aspect of the present invention is to provide a path planning system for automatic parking, wherein the path planning system is used to execute an automatic parking path planning method provided by the present invention.

[0051] Compared with the prior art, the present invention has the following beneficial effects:

[0052] The present invention proposes an automatic parking path planning method and system, which adopts HC curve to connect nodes during path planning, effectively solving the curvature discontinuity problem existing in RS curve to connect nodes. At the same time, the use of HC curve to connect nodes improves the success rate of connecting the starting node to the ending node of parking, and shortens the planned path length.

[0053] The present invention proposes an automatic parking path planning method and system, in which a parking start point, a parking end point, and an extended node adopt a four-dimensional vector including position information (horizontal coordinate value and vertical coordinate value), a heading angle, and path curvature information. The path curvature information is added to the nodes represented by the original three-dimensional vectors, thereby improving the efficiency of path planning and the quality of the path, and making the path more certain.

[0054] The present invention proposes an automatic parking path planning method and system, wherein a set of targeted HC curve cost functions are designed to greatly improve the anthropomorphism of the searched HC curve, thereby improving the anthropomorphism of the path and making the path more in line with human driving behavior habits.

[0055] The present invention proposes a path planning method and system for automatic parking. In the node expansion stage, invalid expansion nodes are eliminated to avoid collision between the planned path and obstacles. Meanwhile, expansion nodes whose path curvature information exceeds a preset threshold are eliminated, which effectively reduces the calculation amount of path planning.

[0056] The present invention provides an automatic parking path planning method and system, which adopts a set of targeted expansion node cost functions and heuristic functions in the node expansion stage, so that the expanded expansion nodes always expand in the direction of the terminal node according to the shortest path. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] In order to more clearly illustrate the specific implementation of the present invention or the technical solution in the prior art, the following briefly introduces the drawings required for use in the specific implementation or the prior art description. Obviously, the drawings described below are some implementations of the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0058] Figure 1 It is a schematic diagram of the existing RS curve.

[0059] Figure 2 It is a schematic diagram of the existing CC circular curve.

[0060] Figure 3 It is a schematic diagram showing that the HC circle adopted in the present invention is composed of a clothoid curve and a circular arc.

[0061] Figure 4 It is a schematic diagram showing that the HC circle adopted in the present invention is composed of a clothoid curve and a circular arc and allows for reversal.

[0062] Figure 5 3 is a C|C|C linear schematic diagram of the HC curve in one embodiment of the present invention.

[0063] Figure 6 It is a flow chart of a path planning method for automatic parking of the present invention.

[0064] Figure 7 It is a schematic diagram of a planning area constructed by a starting node and an ending node of vehicle parking in one embodiment of the present invention.

[0065] Figure 8 It is a schematic diagram of path planning in one embodiment of the present invention.

[0066] Fig. 9 It is a schematic diagram of determining whether there is an obstacle that collides with the vehicle at a certain path point of the HC curve that successfully connects the current node and the terminal node Jg in an embodiment of the present invention.

[0067] Fig.10 It is a schematic diagram of determining whether there is an obstacle that collides with the vehicle at a certain path point of the HC curve that successfully connects the current node and the terminal node Jg in another embodiment of the present invention.

[0068] Fig.11 It is a schematic diagram of determining whether there is an obstacle that collides with the vehicle at a certain path point of the HC curve that successfully connects the current node and the terminal node Jg in another embodiment of the present invention.

[0069] Fig.12 It is a schematic diagram of the ESDF map of the present invention.

[0070] Fig.13 It is a schematic diagram of dividing the forward area and the backward area based on the starting node of the vehicle parking, and dividing the sharpness value in the forward area and the backward area in one embodiment of the present invention.

[0071] Fig.14 It is a schematic diagram of a plurality of expansion nodes in which a start node is used as an expansion of a current node in one embodiment of the present invention.

[0072] Fig.15 is an extended node J in one embodiment of the present invention 1 1 is a schematic diagram of multiple expansion nodes as expansions of the current node. DETAILED DESCRIPTION

[0073] In order to make the above and other features and advantages of the present invention more clear, the present invention is further described below in conjunction with the accompanying drawings. It should be understood that the specific embodiments given herein are for the purpose of explaining to those skilled in the art and are only exemplary and not restrictive.

[0074] In order to explain the present invention more clearly, it is necessary to explain the path primitives used in path planning.

[0075] There are two types of path primitives used in path planning: S (straight line) and C (curve). The traditional hybrid A* algorithm has only one C (curve) path primitive, namely the RS Circle (Reeds-Shepp Circle, RS Circle).

[0076] like Figure 1 As shown, the starting point q of the RS circle s and the end point q gWhen RS circles are used to connect nodes on a section of arc S1, the planned path generated has the problem of curvature discontinuity.

[0077] The existing RS curve (Reeds-Shepp curve) is an inspiration curve constructed by the S (straight line) path primitive and the C (curve) path primitive. There are only 14 line shapes, such as CSC, CS|C, etc. The first C represents the RS circle, S represents the straight line, the vertical line represents the gear shift, and the last C represents the RS circle.

[0078] The RS circle effect is not good, the curvature is not continuous, and it makes the entire RS curve (Reeds-Shepp curve) less linear.

[0079] like Figure 2 As shown, the prior art also improves the RS curve (Reeds-Shepp curve) by adding a CC circle (Continuous Curvature circle) as a C (curve) path primitive.

[0080] CC circle starting point q s and the end point q g On the two clothoid curves S2, the two clothoid curves S2 are connected by an arc S1. When the CC circle is used to connect nodes, there is a situation where the connection from the current node to the terminal node is unsuccessful, and as the rotation angle of the CC circle increases, the length of the CC circle is larger, and the planned path is longer.

[0081] Clothoid is a curve with continuous curvature and linear curvature variation along the arc length, expressed as:

[0082] ,

[0083] in, k is the curvature of the curve, s is the arc length of the curve, σ For sharpness.

[0084] In order to solve the problem in the prior art of using RS circle and CC circle as C (curve) path primitives of RS curve (Reeds-Shepp curve) to connect nodes, the present invention uses HC curve (Hybrid-Curvature curve) to connect nodes during path planning.

[0085] The HC curve of the present invention adds an HC circle (Hybrid-Curvature circle) on the basis of the C (curve) path primitive of the RS curve, that is, the HC circle is added on the basis of the RS circle and CC circle of the existing RS curve (Reeds-Shepp curve), so that the C (curve) path primitive of the HC curve provided by the present invention has three types of RS circle, CC circle and HC circle.

[0086] like Figure 3 As shown, in one embodiment, the starting point q of the HC circle s On a clothoid curve S2, the end point q g On an arc S1.

[0087] like Figure 4 As shown, in another embodiment, the starting point q of the HC circle s On a clothoid curve S2, the end point q g On an arc S1, with end point q g Reversal is allowed.

[0088] After the HC circle is added to the C (curve) path primitive of the HC curve (Hybrid-Curvature curve) of the present invention, the line shapes of the traditional RS curve are increased from 14 to 18.

[0089] like Figure 5 As shown, in one embodiment of the present invention, the line shape of the HC curve is: C|C|C. The first C is the HC circle, the second C is the RS circle, and the third C is also the HC circle.

[0090] After the C (curve) path primitive of the HC curve (Hybrid-Curvature curve) of the present invention is added with the HC circle, the HC curve increases the types of line shapes and can reduce the length of the curve. During the path planning process, the HC curve with the lowest replacement value is selected from the 18 types of line shapes of the HC curve to connect the nodes.

[0091] The present invention provides a method for automatic parking path planning, which performs path planning by searching for different HC curve combinations to connect nodes. The present invention provides a method for automatic parking path planning in detail.

[0092] like Figure 6 As shown, according to an embodiment of the present invention, a path planning method for automatic parking is provided, comprising the following method steps:

[0093] Step S1, obtaining the starting node and the ending node of the vehicle parking, and taking the starting node as the current node; constructing a planning area according to the starting node and the ending node of the vehicle parking, and gridding the planning area.

[0094] like Figure 7 As shown, in this embodiment, the vehicle's starting position information is acquired through the vehicle sensor to obtain the starting node Js of the vehicle parking.

[0095] The parking space information of the vehicle is obtained through the vehicle sensor, and the end node Jg of the vehicle parking is calculated. Specifically, the point where the geometric center of the vehicle itself coincides with the geometric center of the parking space is used as the end node Jg of the vehicle. It should be understood that according to the geometric relationship, those skilled in the art can easily calculate the end node Jg.

[0096] like Figure 7 and Figure 8 As shown, the present invention constructs a planning area according to the starting node Js and the ending node Jg of the vehicle parking, and grids the planning area, each grid representing a location information ( x Coordinate values ​​and y coordinate values).

[0097] The starting node Js and the ending node Jg of the vehicle parking are vectorized, so that the state information of the starting node Js and the ending node Jg of the vehicle parking includes the node position information, heading angle and path curvature information. Specifically, the state information of the starting node Js and the ending node Jg is represented by a four-dimensional vector, that is, the state vector of the starting node Js and the ending node includes x Coordinate value, y Coordinate value, heading angle θ and path curvature k Initialize the path curvature of the starting node Js and the ending node Jg k is 0.

[0098] Step S2, search for the HC curve connecting the current node and the terminal node Jg. If there is at least one HC curve successfully connecting the current node and the terminal node Jg, and the HC curve between the current node and the terminal node Jg is valid, then select the HC curve with the lowest HC curve cost as the path for the vehicle parking, and the path planning is completed.

[0099] In order to make the searched HC curve more in line with human driving behavior habits, the present invention uses a set of HC curve cost functions to screen the searched HC curves. Specifically, according to an embodiment of the present invention, the cost value of each feasible HC curve is calculated by the cost function.

[0100] The cost function of the HC curve includes at least: a straight line penalty term, an arc penalty term, a gear shift penalty term, a short track penalty term and a long track penalty term.

[0101] The cost function of the HC curve is expressed as:

[0102] Cost hc =W s *L s +Wc *L c +W short *L short +W long *L long +W geachange *Count geachange ,

[0103] in, Cost hc represents the cost function value of the HC curve, W s represents the weight of the straight-line penalty term, L s represents the length of the straight line, W c represents the weight of the arc penalty term, L c represents the length of the arc, W short represents the weight of the short trajectory penalty term, L short represents the length of the short trajectory, W long represents the weight of the long trajectory penalty term, L long represents the length of the long trajectory, W geachange represents the weight of the gear shift penalty term, Count geachange Indicates the number of gear shifts.

[0104] If there is no HC curve that successfully connects the current node and the terminating node Jg, or the HC curve between the current node and the terminating node Jg is invalid, then the process proceeds to step S3 to perform node expansion.

[0105] Furthermore, the present invention performs validity detection on the HC curve between the current node and the termination node Jg by the following method:

[0106] Step S201: Determine whether there is an obstacle between the current node and the end node Jg that may collide with the vehicle.

[0107] Specifically, the following method is used to determine whether there is an obstacle between the current node and the end node Jg that collides with the vehicle:

[0108] Step S2011: At each path point of the HC curve that successfully connects the current node and the terminal node, a rectangular frame matching the size of the vehicle is constructed, and three circles are used to enclose the rectangular frame, where the radius of the three circles is R.

[0109] Combination Figures 9 to 11 , construct a rectangular box K that matches the size of the vehicle, using three circles (the three circles are circle O1, circle O2 and circle O3). At each path point of the HC curve that successfully connects the current node and the terminal node Jg, use three circles to envelop the rectangular box K. The radii of the three circles are equal, all R.

[0110] Step S2012: Create an ESDF map, and use the ESDF map to obtain the distances from the centers of the three circles to the nearest obstacles.

[0111] SDF (Euclid Signed Distance Field) map is a map that can obtain the distance from each grid to the nearest obstacle in a grid map. In one implementation, such as Fig.12 The ESDF map created as shown, Fig.12 In the figure, the black cross is the obstacle point, and the red number is the distance from the grid to the nearest obstacle.

[0112] Step S2013: When a path point of the HC curve that successfully connects the current node and the terminal node, and the distances from the centers of the three circles to the nearest obstacle are all greater than the radius R, the path point is valid.

[0113] like Fig. 9 As shown, there are three obstacle points, namely obstacle A, obstacle B and obstacle C. At this path point of the HC curve that successfully connects the current node and the terminal node Jg, the obstacle closest to the center of circle O1 is obstacle C, the obstacle closest to the center of circle O2 is obstacle A, and the obstacle closest to the center of circle O3 is obstacle B.

[0114] A distance D1 between the center of circle O1 and the nearest obstacle C is greater than the radius R, a distance D2 between the center of circle O2 and the nearest obstacle A is greater than the radius R, and a distance D3 between the center of circle O3 and the nearest obstacle B is greater than the radius R.

[0115] Since the distances from the centers of the three circles to the nearest obstacle are all greater than the radius R, the path point of the HC curve that successfully connects the current node and the terminal node Jg is valid.

[0116] Step S2014: When a path point of the HC curve that successfully connects the current node and the terminal node has a distance from the center of at least one circle to the nearest obstacle that is less than the radius R, proceed to step S2015 to determine the validity of the path point.

[0117] like Fig.10 As shown, there are three obstacle points, namely obstacle A, obstacle B and obstacle C. At this path point of the HC curve that successfully connects the current node and the terminal node Jg, the obstacle closest to the center of circle O1 is obstacle C, the obstacle closest to the center of circle O2 is obstacle A, and the obstacle closest to the center of circle O3 is obstacle B.

[0118] The distance D1 between the center of circle O1 and the nearest obstacle C is less than the radius R, the distance D2 between the center of circle O2 and the nearest obstacle A is greater than the radius R, and the distance D3 between the center of circle O3 and the nearest obstacle B is greater than the radius R. Therefore, step S2015 is entered to determine the validity of the path point of the HC curve that successfully connects the current node and the end node Jg.

[0119] like Fig.11 As shown, there are three obstacle points, namely obstacle A, obstacle B and obstacle C. At this path point of the HC curve that successfully connects the current node and the terminal node Jg, the obstacle closest to the center of circle O1 is obstacle C, the obstacle closest to the center of circle O2 is obstacle B, and the obstacle closest to the center of circle O3 is obstacle B.

[0120] The distance D1 between the center of circle O1 and the nearest obstacle C is less than the radius R, the distance D2 between the center of circle O2 and the nearest obstacle B is greater than the radius R, and the distance D3 between the center of circle O3 and the nearest obstacle B is less than the radius R. Therefore, the process proceeds to step S2015 to determine the validity of the path point of the HC curve that successfully connects the current node and the terminal node Jg.

[0121] Step S2015, using the ESDF map to obtain the nearest obstacle in step S2014, the distance from the center of a circle being less than the radius R;

[0122] Determine whether the obstacle is within a rectangular frame that matches the size of the vehicle;

[0123] When the obstacle is within a rectangular box that matches the size of the vehicle, the waypoint is invalid;

[0124] When the obstacle is outside the rectangular box that matches the size of the vehicle, the waypoint is valid.

[0125] like Fig.10 As shown, the nearest obstacle C whose distance D1 from the center of circle O1 is less than the radius R is obtained. Since obstacle C is outside the rectangular box K that matches the size of the vehicle, the path point that successfully connects the current node and the terminal node Jg is valid.

[0126] like Fig.11As shown, the nearest obstacle C whose distance D1 to the center of circle O1 is less than the radius R, and the nearest obstacle B whose distance D3 to the center of circle O3 is less than the radius R are obtained. Since obstacle B is within a rectangular box K that matches the size of the vehicle, the path point that successfully connects the current node and the terminal node Jg is invalid.

[0127] Step S2016: When all path points of the HC curve that successfully connects the current node and the terminal node are all valid, there is no obstacle between the current node and the terminal node that may collide with the vehicle.

[0128] All path points of the HC curve that successfully connects the current node and the terminal node are traversed. When all path points of the HC curve that successfully connects the current node and the terminal node are all valid, there is no obstacle between the current node and the terminal node that collides with the vehicle.

[0129] Step S202: Determine whether the current node and the end node Jg, and all path points of the HC curve between the current node and the end node Jg are located within the constructed planning area.

[0130] Step S203: If there is no obstacle between the current node and the end node Jg that collides with the vehicle, and the HC curve between the current node and the end node does not collide with the obstacle;

[0131] And if the current node and the terminal node Jg, and all the path points of the HC curve between the current node and the terminal node are located in the planning area, then the HC curve between the current node and the terminal node Jg is valid, otherwise the HC curve between the current node and the terminal node Jg is invalid.

[0132] Step S3, dividing the forward area and the backward area based on the current node; calling the clothoid curve, expanding multiple expansion nodes in the forward area of ​​the current node, and expanding multiple expansion nodes in the backward area of ​​the current node, so that the multiple expansion nodes are connected to the current node through the clothoid curve.

[0133] The present invention starts node expansion from step S3. Specifically, calling the clothoid curve, expanding multiple expansion nodes in the forward area of ​​the current node, and expanding multiple expansion nodes in the backward area of ​​the current node, including:

[0134] Step S301, divide the forward area of ​​the current node into multiple sharpness values; for each sharpness value, set the step size of node expansion, call the clothoid curve, calculate and obtain multiple expansion nodes corresponding to the multiple sharpness values, and connect the multiple expansion nodes with the current node through the clothoid curve.

[0135] Step S302, divide the retreat area of ​​the current node into multiple sharpness values; for each sharpness value, set the step size of node expansion, call the clothoid curve, calculate and obtain multiple expansion nodes corresponding to the multiple sharpness values, and connect the multiple expansion nodes with the current node through the clothoid curve.

[0136] If the current node is the starting node Js, the forward area and the backward area are divided based on the starting node Js, and the spiral curve is called to expand multiple expansion nodes in the forward area of ​​the starting node Js and expand multiple expansion nodes in the backward area of ​​the starting node Js.

[0137] In the forward area of ​​the starting node Js, multiple sharpness values ​​are divided; for each sharpness value, the step size of the node expansion is set, the spiral curve is called, and multiple expansion nodes corresponding to the multiple sharpness values ​​are calculated, so that the multiple expansion nodes are connected to the starting node Js through the spiral curve.

[0138] In this embodiment, according to the maximum sharpness σ of the vehicle motion max , the forward area of ​​the starting node Js is divided into five sharpness values: σ max , σ max / 2, 0, -σ max / 2, -σ max For each sharpness value, the clothoid curve calculation function is called to calculate the five expansion nodes corresponding to the five sharpness values.

[0139] The clothoid curve calculation function of the present invention adopts the existing clothoid curve calculation function, and x Coordinate value, y Coordinate value, heading angle θ and path curvature k The starting node Js of the four-dimensional state vector is given by the step size of node expansion and a fixed sharpness value. The expansion node corresponding to a specific sharpness value can be calculated by the clothoid curve calculation function.

[0140] Combination Fig.13 and Fig.14 In this embodiment, the sharpness value σ is calculated by calling the clothoid curve calculation function max The corresponding expansion node J 1 1, sharpness value σ max / 2 corresponding to the expansion node J 2 1, the expansion node J corresponding to the sharpness value 0 3 1. Sharpness value - σ max / 2 corresponding to the expansion node J 4 1. Sharpness value - σ max The corresponding expansion node J 5 1. Five extended nodes after expansion (J 1 1. J 2 1. J3 1. J 4 1. J 5 1) It is connected to the starting node Js through a clothoid curve.

[0141] Similarly, the state information of the extended node obtained by the present invention includes the node's location information, heading angle, and path curvature information. That is, the state vector of the extended node is: x Coordinate value, y Coordinate value, heading angle θ and path curvature k A four-dimensional vector.

[0142] In the receding area of ​​the starting node Js, multiple sharpness values ​​are divided; for each sharpness value, the step size of the node expansion is set, and the spiral curve calculation function is called to calculate multiple expansion nodes corresponding to the multiple sharpness values, so that the multiple expansion nodes are connected to the starting node Js through the spiral curve.

[0143] The method of expanding multiple expansion nodes in the retreat area of ​​the starting node Js is the same as the method of expanding multiple expansion nodes in the forward area of ​​the starting node Js. Five expansion nodes (J 1 1', J 2 1', J 3 1', J 4 1', J 5 1'). The five extended nodes after expansion (J 1 1', J 2 1', J 3 1', J 4 1', J 5 1') is connected to the starting node Js through a clothoid curve.

[0144] If the current node is an extended node after expansion, the forward area and the backward area are divided based on the current node; the spiral curve is called to expand multiple extended nodes in the forward area of ​​the current node and expand multiple extended nodes in the backward area of ​​the current node, so that the multiple extended nodes are connected to the current node through the spiral curve.

[0145] Specifically, a plurality of sharpness values ​​are divided in the forward area of ​​the current node; for each sharpness value, the step size of the node expansion is set, and the clothoid curve calculation function is called to calculate and obtain a plurality of expansion nodes corresponding to the plurality of sharpness values, so that the plurality of expansion nodes are connected to the current node through the clothoid curve.

[0146] In the retreat area of ​​the current node, multiple sharpness values ​​are divided; for each sharpness value, the step size of the node expansion is set, and the clothoid curve calculation function is called to calculate multiple expansion nodes corresponding to the multiple sharpness values, so that the multiple expansion nodes are connected to the current node through the clothoid curve.

[0147] like Fig.15 As shown, in this embodiment, the current node is used as the expanded node J after expansion. 1 1 as an example, call the clothoid curve calculation function, at the current node (expand node J 1 1) The forward area is expanded by five expansion nodes (J 1 2. J 2 2. J 3 2. J 4 2. J 5 2) Expand five nodes in the back area of ​​the current node (J 1 2', J 2 2', J 3 2', J 4 2', J 5 2'), ten extension nodes (J 1 2. J 2 2. J 3 2. J 4 2. J 5 2. J 1 2', J 2 2', J 3 2', J 4 2', J 5 2') with the current node (expanded node J 1 1) are connected by clothoid curves.

[0148] Step S4: Perform validity check on multiple extension nodes. If an extension node is invalid, remove the extension node.

[0149] If an extension node is valid, the extension node is used as a valid extension node and is placed in a valid extension node set.

[0150] Furthermore, the validity of multiple extension nodes is checked by the following method:

[0151] Step S401: Determine whether there is an obstacle between the current node and the extended node that may collide with the vehicle.

[0152] The method for determining whether there is an obstacle that collides with the vehicle between the current node and the extended node is the same as the method for determining whether there is an obstacle that collides with the vehicle between the current node and the terminal node Jg in step S201, and will not be repeated here.

[0153] Step S402: Determine whether the current node and the extended node are located in the constructed planning area.

[0154] Step S403: If there are no obstacles between the current node and the extended node that may collide with the vehicle, and the current node and the extended node are located within the planning area, then the extended node is valid; otherwise, the extended node is invalid.

[0155] Combination Figure 8 , taking the current node as the starting node Js as an example, the current node (starting node Js) and the extended node J 2 1, there is an obstacle that collides with the vehicle. When the expansion node J 2 1 Invalid, remove the expansion node J 2 1.

[0156] For another example, expand node J 4 1 is outside the constructed planning area, then expand node J 4 1 Invalid, remove the expansion node J 4 1.

[0157] For another example, the current node (starting node Js) and the extended node J 1 1 There is no obstacle that may collide with the vehicle, and the current node (starting node Js) and the extended node J 1 1 is in the planning area, then expand node J 1 1 is valid, expand node J 1 1 as a valid expansion node.

[0158] Further, according to an embodiment of the present invention, if a certain extended node is valid, a pruning operation is performed on the extended node:

[0159] If a certain extended node is valid, determine whether the curvature information of the extended node is greater than a preset threshold;

[0160] If the curvature information of the extended node is greater than a preset threshold, the extended node is removed; if the curvature information of the extended node is less than the preset threshold, the extended node is taken as a valid extended node.

[0161] Take the starting node Js as the current node and the expanded node J after expansion 1 1 and expansion node J 5 1' is valid as an example.

[0162] Expand Node J 1 1 is valid, judging the expansion node J 1 1 is greater than the preset threshold. Expand node J 5 1' is valid, judging the extension node J 5 1' is greater than a preset threshold.

[0163] Since the expansion node J 1 1 and expansion node J5 1' status information usage includes x Coordinate value, y Coordinate value, heading angle θ and path curvature k The four-dimensional vector representation of the expansion node J in this embodiment 1 1 Curvature information (path curvature k ) is less than the preset threshold, then expand node J 1 1 is a valid expansion node, expansion node J 5 1' curvature information (path curvature k ) is greater than the preset threshold, then the expansion node J is removed 5 1'.

[0164] In this embodiment, ten expansion nodes J 1 1. J 2 1. J 3 1. J 4 1. J 5 1. J 1 1', J 2 1', J 3 1', J 4 1', J 5 1' After screening, the valid expansion nodes are: J 1 1. J 3 1. J 5 1. J 1 1', J 2 1', J 3 1', J 4 1'.

[0165] To expand node J 1 1 is the current node, and the expanded node J is 1 2 and expansion node J 5 2' is effective as an example.

[0166] Expand Node J 1 2 is valid, determine the extension node J 1 2 is greater than the preset threshold. Expand node J 5 2' is valid, determine the extension node J 5 2' is greater than a preset threshold.

[0167] In this embodiment, the expansion node J 1 2 Curvature information (path curvature k ) is less than the preset threshold, then expand node J 1 2 is a valid expansion node, expansion node J 5 2' curvature information (path curvature k) is greater than the preset threshold, then the expansion node J is removed 5 2'.

[0168] To expand node J 1 1 is the current node, and the ten expanded nodes J after expansion 1 2. J 2 2. J 3 2. J 4 2. J 5 2. J 1 2', J 2 2', J 3 2', J 4 2', J 5 2' After screening, the valid expansion nodes are: J 1 2. J 3 2. J 5 2. J 1 2', J 2 2', J 3 2', J 4 2'.

[0169] Step S5, select the valid extension node with the lowest extension node cost in the valid extension node set as the current node, repeat steps S2 to S5, and cyclically search the HC curve until the current node and the terminal node Jg are successfully connected.

[0170] In order to select more suitable expansion nodes for the next round of node expansion, the present invention adopts a set of expansion node cost functions to screen multiple valid expansion nodes, and calculates the expansion node cost values ​​of the valid expansion nodes through the expansion node cost functions.

[0171] Furthermore, the extended node cost function includes at least a forward penalty term, a backward penalty term, a gear shift penalty term, a path angle change penalty term, a path angle penalty term, and a path distance to the nearest obstacle penalty term.

[0172] The expansion node cost value between the current node and multiple valid expansion nodes is calculated through the expansion node cost function, and the valid expansion node with the lowest expansion node cost value is selected as the current node.

[0173] To expand node J 1 1 is used as the current node as an example. The valid extended nodes after expansion are: J 3 1. J 5 1. J 1 1', J 2 1', J 3 1', J 4 1', J 1 2. J 3 2. J 5 2. J1 2', J 2 2', J 3 2', J 4 2'.

[0174] Calculate the current node (expanded node J) by expanding the node cost function 1 1) With the expansion node J 3 1 (valid expansion node) between the expansion node cost value, the current node (expansion node J 1 1) With the expansion node J 5 1 (valid extended node), the extended node cost value between, ..., the current node (extended node J 1 1) With the expansion node J 4 2' (valid extension nodes).

[0175] Current node (expanded node J 1 1) With the expansion node J 1 2 (valid expansion nodes) has the lowest expansion node cost, then expand node J 1 2 (valid expansion node) is used as the current node for the next round of node expansion, and steps S2 to S5 are repeated to cyclically search the HC curve until the current node and the terminal node Jg are successfully connected.

[0176] It should be noted that, in the process of path planning, the present invention will select the node with the lowest cost of the extended node from all valid extended nodes as the current node for the next round of expansion. To achieve this selection process, for example, a priority queue can be set during the implementation process to store all valid extended nodes. However, as the space of the priority queue continues to increase, the valid extended node that was not selected in the previous round may be selected as the node with the lowest cost of the extended node in the subsequent round of node expansion, and then be selected as the current node for the next round of node expansion.

[0177] like Figure 8 As shown, the final planned path of this embodiment is: starting node Js and extension node J 1 1 between the clothoid curve, expansion node J 1 1 and expansion node J 1 Clothoid curve between 2, expansion node J 1 2 and expansion node J 1 3, and the extended node J 1 3 and the path formed by the HC curve between the terminal node Jg.

[0178] In the node expansion stage of the present invention, in order to make the expansion node always expand in the direction of the terminal node Jg, the heuristic function of the expansion node includes two parts:

[0179] The first part is: if there is an HC curve that successfully connects the current node and the terminal node Jg, then the cost function value of the HC curve is Cost hc As the first part of the heuristic function for expanding nodes;

[0180] If there is no HC curve that successfully connects the current node and the terminal node Jg, the Euclidean distance between the current node and the terminal node Jg is calculated, and the Euclidean distance is used as the first part of the heuristic function of the expanded node.

[0181] The second part: Using the DP-MAP graph is to start from the termination node Jg, calculate the distance from the termination node Jg to each grid in the planning area, and use this distance as the second part of the heuristic function of the expansion node.

[0182] Compared with the existing method of simply using the Euclidean distance between two points as the heuristic function value, the two parts of the heuristic function of the node expansion of the present invention improve the accuracy of searching the HC curve and the accuracy of the expansion direction, while avoiding the problem of entering a dead end during the search of the HC curve, thereby expanding many unused nodes and consuming a lot of time.

[0183] According to an embodiment of the present invention, there is provided an automatic parking path planning system for executing an automatic parking path planning method provided by the present invention.

[0184] Although the embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and are not to be construed as limitations of the present invention. A person skilled in the art may change, modify, replace and vary the above embodiments within the scope of the present invention.

Claims

1. A path planning method for automatic parking, characterized in that: The path planning method comprises the following steps: S1. Obtain the starting node and the ending node of the vehicle parking, and use the starting node as the current node; construct a planning area according to the starting node and the ending node of the vehicle parking, and grid the planning area; S2, searching for an HC curve connecting the current node and the end node, if there is at least one HC curve between the current node and the end node that successfully connects the current node and the end node, and the HC curve between the current node and the end node is valid; Then the HC curve with the lowest HC curve cost is selected as the path for the vehicle to park, and the path planning is completed; If there is no HC curve between the current node and the end node that successfully connects the current node and the end node, or the HC curve between the current node and the end node is invalid, proceed to step S3 to perform node expansion; S3, dividing the forward area and the backward area based on the current node; Calling the clothoid curve, extending multiple extended nodes in the forward area of ​​the current node, and extending multiple extended nodes in the backward area of ​​the current node, so that the multiple extended nodes are connected to the current node through the clothoid curve; S4. Perform validity detection on multiple extension nodes; If an extension node is invalid, remove the extension node; If an extension node is valid, the extension node is used as a valid extension node and is placed in the valid extension node set; S5. Select the valid extension node with the lowest extension node cost in the valid extension node set as the current node, repeat steps S2 to S5, and cyclically search the HC curve until the current node and the terminal node are successfully connected.

2. The path planning method according to claim 1, characterized in that: The status information of the starting node, the ending node, and the extended node of the vehicle parking includes the location information, the heading angle, and the path curvature information of the node.

3. The path planning method according to claim 1, characterized in that: In step S2, the cost value of each feasible HC curve is calculated by the cost function; The cost function includes at least: a straight line penalty term, an arc penalty term, a gear shift penalty term, a short track penalty term and a long track penalty term.

4. The path planning method according to claim 1, characterized in that: In step S2, the HC curve between the current node and the end node is effectively detected by the following method: S201, determining whether there is an obstacle between the current node and the end node that may collide with the vehicle; S202, determining whether the current node and the end node, and all path points of the HC curve between the current node and the end node are located in the constructed planning area; S203: If there is no obstacle between the current node and the end node that collides with the vehicle, and the HC curve between the current node and the end node does not collide with the obstacle; If the current node and the terminal node, as well as all the path points of the HC curve between the current node and the terminal node, are located in the planning area, then the HC curve between the current node and the terminal node is valid, otherwise the HC curve between the current node and the terminal node is invalid.

5. The path planning method according to claim 4, characterized in that: In step S201, it is determined whether there is an obstacle between the current node and the end node that may collide with the vehicle by the following method: S2011. At each path point of the HC curve that successfully connects the current node and the terminal node, a rectangular frame matching the size of the vehicle is constructed, and three circles are used to enclose the rectangular frame, wherein the radius of the three circles is R; S2012. Establish an ESDF map, and use the ESDF map to obtain the distances from the centers of the three circles to the nearest obstacles; S2013. When a path point of the HC curve that successfully connects the current node and the terminal node, and the distances from the centers of the three circles to the nearest obstacle are all greater than the radius R, the path point is valid; S2014: When a path point of the HC curve that successfully connects the current node and the terminal node has at least one circle whose center to the nearest obstacle is less than the radius R, the process proceeds to step S2015 to determine the validity of the path point; S2015, using the ESDF map to obtain the nearest obstacle in step S2014 whose distance from the center of a circle is less than the radius R; Determine whether the obstacle is within a rectangular frame that matches the size of the vehicle; When the obstacle is within a rectangular box that matches the size of the vehicle, the waypoint is invalid; When the obstacle is outside the rectangular box that matches the size of the vehicle, the waypoint is valid; S2016. When all path points of the HC curve that successfully connects the current node and the terminal node are all valid, there is no obstacle between the current node and the terminal node that may collide with the vehicle.

6. The path planning method according to claim 1, characterized in that: In step S3, the clothoid curve is called to expand multiple expansion nodes in the forward area of ​​the current node and expand multiple expansion nodes in the backward area of ​​the current node, including: S301, dividing a forward area of ​​the current node into multiple sharpness values; for each sharpness value, setting a step length of node expansion, calling a clothoid curve, calculating and obtaining multiple expansion nodes corresponding to the multiple sharpness values, and connecting the multiple expansion nodes with the current node through the clothoid curve; S302, dividing the retreat area of ​​the current node into multiple sharpness values; for each sharpness value, setting the step size of the node expansion, calling the clothoid curve, calculating and obtaining multiple expansion nodes corresponding to the multiple sharpness values, and connecting the multiple expansion nodes with the current node through the clothoid curve.

7. The path planning method according to claim 1, characterized in that: In step S4, the validity of multiple extension nodes is checked by the following method: S401, determining whether there is an obstacle between the current node and the extended node that collides with the vehicle; S402, determining whether the current node and the extended node are located in the constructed planning area; S403: If there are no obstacles between the current node and the extended node that may collide with the vehicle, and the current node and the extended node are located within the planning area, then the extended node is valid; otherwise, the extended node is invalid.

8. The path planning method according to claim 1, characterized in that: In step S4, the path planning method further includes, if a certain extended node is valid, performing a pruning operation on the extended node: If a certain extended node is valid, determine whether the curvature information of the extended node is greater than a preset threshold; If the curvature information of the extended node is greater than a preset threshold, the extended node is removed; If the curvature information of the extended node is less than a preset threshold, the extended node is regarded as a valid extended node.

9. The path planning method according to claim 1, characterized in that: In step S5, the expansion node cost value of the effective expansion node is calculated by the expansion node cost function; The extended node cost function includes at least a forward penalty term, a backward penalty term, a gear shift penalty term, a path angle change penalty term, a path angle penalty term, and a path distance to the nearest obstacle penalty term.

10. A path planning system for automatic parking, characterized in that: The path planning system is used to execute the path planning method described in any one of claims 1 to 9.

Images