CN114347982A - Path planning method and system for automatic parking - Google Patents

Path planning method and system for automatic parking Download PDF

Info

Publication number
CN114347982A
CN114347982A CN202111597267.XA CN202111597267A CN114347982A CN 114347982 A CN114347982 A CN 114347982A CN 202111597267 A CN202111597267 A CN 202111597267A CN 114347982 A CN114347982 A CN 114347982A
Authority
CN
China
Prior art keywords
vehicle
parking
search
intermediate point
point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202111597267.XA
Other languages
Chinese (zh)
Inventor
樊晓谦
盛愈欢
刘祥
万国强
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Jingwei Hirain Tech Co Ltd
Original Assignee
Beijing Jingwei Hirain Tech Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Jingwei Hirain Tech Co Ltd filed Critical Beijing Jingwei Hirain Tech Co Ltd
Priority to CN202111597267.XA priority Critical patent/CN114347982A/en
Publication of CN114347982A publication Critical patent/CN114347982A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units, or advanced driver assistance systems for ensuring comfort, stability and safety or drive control systems for propelling or retarding the vehicle
    • B60W30/06—Automatic manoeuvring for parking
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0019—Control system elements or transfer functions

Abstract

The invention discloses a path planning method and system for automatic parking. The method comprises the following steps: selecting a middle point on a path from the starting position to the parking target position in the parking lot area, wherein the middle point is close to an angular point of the parking target position, and the vehicle only moves through an arc from the middle point corresponding position to the parking target position; determining a path of the vehicle from the initial position to the middle point through a Reed shepp curve and/or a bidirectional hybrid A-search algorithm comprising forward search and reverse search, wherein the forward search and the reverse search are directions gradually approaching to and departing from a parking target position respectively; and calculating the angle of the vehicle, which needs to be adjusted at each moment from the intermediate point to the parking target position, by combining the vehicle pose of the corresponding position of the intermediate point through the vehicle kinematic model until the angle is equal to the target angle, and determining that the vehicle is parked at the parking target position. According to the technical scheme, the parking path planning flexibility can be improved, and the success rate of parking planning at any angle is improved.

Description

Path planning method and system for automatic parking
Technical Field
The invention relates to the field of automatic driving and the technical field of automatic parking, in particular to a path planning method and system for automatic parking.
Background
The current parking planning methods are roughly divided into two types from the academic and engineering aspects: one is searching for the optimal solution of the parking path based on a graph searching or convex optimization mode, such as a mixed A or QP algorithm; the other method is to calculate a parking path according with the vehicle kinematics according to the geometric relation based on the vehicle kinematics principle.
The map search algorithm has low requirement on the initial pose of the vehicle, and can well traverse the map to search for a feasible parking track, so that the parking flexibility is greatly improved. However, map searching needs to involve map construction and historical track storage, and the like, and although the searching algorithm is flexible, the implementation method is complicated, and the calculation amount is large during searching. Because parking scenes are mostly narrow spaces, vehicles need to be repeatedly moved in the same position, the efficiency of the graph search algorithm is further reduced, and therefore higher requirements are put forward on the performance and the calculation power of hardware.
The geometric method based on kinematic constraint greatly reduces the computational force requirement, and a feasible parking track is determined through the relative position relationship between the parking space and the vehicle. However, the method has strict requirements on the initial position of the vehicle, a feasible parking path cannot be planned if the posture of the vehicle exceeds the range, and the parking position of the vehicle is highly random and cannot be guaranteed to be within the expected range in the using process of a driver, so that certain defects exist in engineering application, and the using experience of a user is influenced.
In view of the above technical problems in the related art, no effective solution has been proposed at present.
Disclosure of Invention
Aiming at the problems in the related art, the invention provides a path planning method and system for automatic parking, which can improve the parking path planning flexibility and the success rate of parking planning at any angle.
The technical scheme of the invention is realized as follows:
according to one aspect of the present invention, a path planning method for automatic parking is provided. The path planning method for automatic parking comprises the following steps:
selecting an intermediate point on a path from the starting position to the parking target position in the parking space area, wherein the intermediate point is close to an angular point of the parking target position, and the vehicle only moves through an arc from the intermediate point corresponding position to the parking target position;
determining a path of the vehicle from the initial position to the intermediate point through a Reed shepp curve and/or a bidirectional hybrid A-search algorithm, wherein the bidirectional hybrid A-search algorithm comprises forward search and reverse search, the forward search is a direction gradually approaching the parking target position, and the reverse search is a direction gradually departing from the parking target position;
and calculating the angle of the vehicle, which needs to be adjusted at each moment from the intermediate point to the parking target position, by combining the vehicle pose at the corresponding position of the intermediate point through a vehicle kinematic model until the vehicle is determined to be parked at the parking target position when the angle is equal to the target angle.
In the path planning method, before selecting the intermediate point, the method further includes: and establishing a parking space coordinate system, wherein the front angular point of the near end of the parking space in the parking space area is selected as an original point, the straight line where the vehicle firstly reaches the boundary of the parking space area is selected as a transverse axis, and the straight line which is perpendicular to the transverse axis and is far away from the boundary of the initial position is selected as a longitudinal axis.
In the path planning method, the paths generated from the starting position to the intermediate point are connected through the Reed shepp curve, and the paths from the intermediate point to the parking target position are connected, so that a full-course planned path is obtained.
In the path planning method, when determining that there is no collision risk in the parking process, the path from the starting position to the intermediate point is planned through the ReedShepp curve, and when determining that there is a collision risk in the parking process, the path searched through the bidirectional hybrid A algorithm is searched through the bidirectional hybrid A algorithm, and the path searched through the bidirectional hybrid A algorithm is connected through the ReedShepp curve.
In the above path planning method, the searching is performed by a bidirectional hybrid a-x algorithm, including: carrying out forward search to find the current minimum cost node as the starting point of the next time search; and performing reverse search to find the current minimum cost node as the starting point of the search at the next moment.
In the path planning method, the searching in reverse direction to find the current minimum cost node includes:
by introducing the inverse search cost term from the following formula,
frev＝grev+hrev+wv·ρV(x,y)+q
q＝w1q1+w2q2+w3q3
wherein f isrevIs the sum of cost functions; grevFor the cost of motion, in some embodiments, grevAdopting the Euclidean distance from the current point to the target point; h isrevA heuristic cost for the current point; rhoV(x, y) is the voronoi diagram potential energy of the current point; w is avIs the weight of the Voronoi potential energy; q is a reverse search cost function, q1Distance between two points in forward and backward directions, q2Difference between course angles of two forward and backward points, q3The included angle between the reverse course angle and the joining direction of the two forward and reverse points is included; w is a1、w2And w3Are each q1、q2And q is3The cost weight of (2).
In the above path planning method, calculating, by using a vehicle kinematics model and in combination with a vehicle pose at a corresponding position of an intermediate point, an angle of the vehicle to be adjusted at each time from the intermediate point to a parking target position, includes:
by the position (X) of the vehicle at the intermediate point GG,YG,θG) The vehicle is finally stopped in the middle of the parking space area for the initial posture, the final heading angle of the vehicle is 90 degrees, and the angle theta of the vehicle, which needs to be adjusted forwards when the vehicle is stopped at the parking target position, is calculated by a vehicle kinematics model by using the following formula:
2*sin(θ+θG)-sin(θG)+XG,＝Pw/2
wherein, thetaGThe angle of the vehicle at the intermediate point G, Pw is the width of the parking space region, and Pw/2 represents the final expected position of the vehicle.
In the path planning method, selecting the intermediate point includes: the position where one wheel of the vehicle reaches the boundary of the parking space area is selected as a middle point, wherein the one wheel is the wheel which firstly reaches the parking space area among the plurality of wheels of the vehicle.
According to another aspect of the present invention, a path planning system for automated parking is provided. The path planning system for automatic parking includes: the intermediate point selection module is used for selecting an intermediate point on a path from the starting position to the parking target position in the parking space area, wherein the intermediate point is close to the angular point of the parking target position, and the vehicle only moves through an arc from the position corresponding to the intermediate point to the parking target position; the map searching module is used for determining a path of the vehicle from the initial position to the middle point through a ReedShepp curve and/or a bidirectional hybrid A-search algorithm, wherein the bidirectional hybrid A-search algorithm comprises forward search and reverse search, the forward search is a direction gradually approaching the parking target position, and the reverse search is a direction gradually departing from the parking target position; and the kinematic calculation module is used for calculating the angle of the vehicle, which needs to be adjusted at each moment from the intermediate point to the parking target position, by combining the vehicle pose at the corresponding position of the intermediate point through the vehicle kinematic model until the angle is equal to the target angle, and determining that the vehicle stops at the parking target position.
According to still another aspect of the present invention, there is provided a storage device including a storage medium storing a program executed to implement the above-described path planning method for automatic parking.
According to the technical scheme, the parking process is divided into two stages by using the selected intermediate point and different planning modes at present are combined, the process that the vehicle needs to be adjusted repeatedly after entering the parking space area is planned by using the vehicle kinematics model, the path of the vehicle is planned from the initial position to the stage of parking and warehousing, and the planning method is obtained by combining the graph search algorithm and the geometric planning algorithm, so that the hardware calculation requirement is reduced, the planning activity is improved, and the success rate of parking planning at any angle is improved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings needed in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings without creative efforts.
Fig. 1 is a flowchart of a path planning method for automatic parking according to an embodiment of the present invention.
Fig. 2 is a flowchart of a path planning method for automatic parking according to another embodiment of the present invention.
Fig. 3 is a schematic diagram of a parking space coordinate system and names of corner points according to an embodiment of the present invention.
Fig. 4 is a diagram of paths before searching for an obtained bin according to an embodiment of the present invention.
Fig. 5 is a two-way search effect diagram.
Fig. 6 is a schematic diagram of a bi-directional search flow.
FIG. 7 is a schematic view of a start point location heading angle in reverse cost for a two-way search.
FIG. 8 is a schematic diagram of an in-library adjustment path obtained based on geometric relationships.
Fig. 9 is a block diagram of a path planning system for automatic parking according to an embodiment of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments that can be derived by one of ordinary skill in the art from the embodiments given herein are intended to be within the scope of the present invention.
According to an embodiment of the present invention, a path planning method for automatic parking is provided. Fig. 1 is a flowchart of a path planning method for automatic parking according to an embodiment of the present invention. As shown in fig. 1, the path planning method for automatic parking of the present invention includes steps S102, S104, and S106.
First, at step S102, an intermediate point is selected on a path of the vehicle from the start position to before the parking target position in the parking space area. In some embodiments, the selected intermediate point is adjacent to the corner of the parking target location, and the process after the intermediate point may be referred to as a parking-in stage. The principle of selecting the intermediate point needs to ensure that the vehicle moves only through an arc from the position corresponding to the intermediate point to the parking target position.
At step S104, the path of the vehicle from the starting position to the intermediate point is determined by the reedsshepp curve and/or the bi-directional hybrid a-search algorithm. The bidirectional mixed A-star search algorithm comprises forward search and reverse search, wherein the forward search is a direction gradually approaching the parking target position, and the reverse search is a direction gradually departing from the parking target position.
At step S106, the vehicle is determined to be parked at the parking target position when the angle, which needs to be adjusted at each moment from the intermediate point to the final parking target position, is calculated by the vehicle kinematics model in combination with the vehicle pose at the intermediate point corresponding position until the angle is equal to the target angle (e.g., 90 °).
According to the method, the parking process is divided into two stages by using the selected intermediate point and different planning modes at present are combined, the process that the vehicle needs to be adjusted repeatedly after entering the parking space area is planned by using the vehicle kinematics model, the path planning is carried out by using a Reed shepp curve and/or a bidirectional hybrid A search algorithm when the vehicle enters the parking space area from the initial position to the stage that the vehicle is to be parked and put in storage, and the planning method obtained by combining the graph search algorithm and the geometric planning algorithm reduces the hardware computation requirement, improves the planning flexibility and improves the parking planning success rate at any angle.
Fig. 2 is a flowchart of a path planning method for automatic parking according to another embodiment of the present invention. As shown in fig. 2, for the convenience of the subsequent algorithm development, the relative coordinate system origin position must be determined. Therefore, in step S102, a parking space coordinate system may be established in advance before selecting the intermediate point based on the parking space.
Referring to fig. 3, a parking space near-end front corner point O of the parking space area OABC is selected as an origin of the coordinate system. The parking space near-end front corner G is located on a boundary OC which a vehicle in a parking space area OABC firstly passes through, and is one end of the boundary OC which is far away from the vehicle. Considering that the calculation of the subsequent pose is facilitated when the coordinate system is consistent with the traditional Cartesian coordinate system, the front angular point G of the near end of the parking space is selected as the origin. The straight line on the boundary OC is selected as the X axis (horizontal axis), and the straight line on the boundary OA, which is perpendicular to the X axis and is away from the vehicle start position, is selected as the Y axis (vertical axis).
Referring to fig. 2, after the coordinate system is established at step S102, an intermediate point in the parking process, that is, a target position at which the vehicle is to be parked, is selected. Referring to fig. 3, to ensure that the vehicle can park successfully in the parking space area OABC, a position where a rear wheel of the vehicle, which first reaches the parking space area OABC, reaches the boundary OC is selected as the middle point G. The choice of the position of the intermediate point G can be predetermined based on experience. Referring to fig. 4, an intermediate point taken on a path between a vehicle arriving at a parking space area from a starting location is shown. When the vehicle reaches the middle point position, the rear wheel of the vehicle reaches the boundary of the parking space area. In some cases, the intermediate point G is selected to ensure that the vehicle only needs to travel through a simple arc from the location to the parking target location. The planning mode of different parking stalls can be adapted by adjusting the position and the angle of the middle point G, and the complexity of path planning is reduced.
Referring to fig. 2, after the intermediate point is selected at step S102, steps S1041 to S1045 (corresponding to step S104 in fig. 1) are performed to determine a path of the vehicle from the start position to the intermediate point through a redshepp curve and/or a bidirectional hybrid a × search algorithm.
Specifically, a path is planned through the reedsshepp curve at step S1041. It is necessary to determine whether to consider the collision risk at step S1042. In the case where it is determined at step S1042 that the collision risk is not considered, proceeding to step S1045, a path from the start position to the intermediate point is planned by the reedsshepp curve. The characteristics of the Reed shepp curve can be used to ensure that nine modes such as CCC (circular arc), C | C | C (circular arc reversing circular arc), C | CC (circular arc reversing circular arc), CSC (circular arc straight line circular arc), CC | CC (circular arc reversing circular arc), C | CC | C (circular arc reversing circular arc), C | CSC (circular arc reversing circular arc straight line circular arc), CSC | C (circular arc straight line circular arc reversing circular arc), and C | CSC | C (circular arc reversing circular arc straight line circular arc reversing circular arc) reach the intermediate point position, so that the search calculation amount is simplified.
Based on the vehicle body signal, when the vehicle body signal is far from the target parking space, the search needs to be performed in consideration of the surrounding environment obstacle, and in the case that the collision risk is determined to be considered at step S1042, the search is performed in step S1043 by a bidirectional hybrid a-star algorithm based on the positioning information. As shown in fig. 2 and 5, when it is determined at step S1044 that the next node is not the end point, step S1043 is repeated, and the search is performed by the bidirectional hybrid a-algorithm. When it is determined at step S1044 that the next node is the end point, it proceeds to step S1045 to generate a path by connecting the paths searched by the bidirectional hybrid a-x algorithm through the reedsrep curve based on the map information. Wherein, for the forward searching process, the direction gradually approaches to the parking target position, and the terminal point is the parking target position of the vehicle; for the reverse search process, the end point is the starting position of the vehicle in a direction gradually away from the parking target position.
More specifically, the bidirectional hybrid a-x algorithm searches from the forward direction and the reverse direction respectively each time to find the current minimum cost value, and searches as the starting point of the search at the next moment. As shown in fig. 6, at step S601, a forward search is performed in a direction from the Start position (Start) to the parking target position (End). At step S602, the first minimum cost Node1 in the forward closed set is found. In step S603, a reverse search is performed in a direction away from the parking target position (End) and closer to the Start position (Start). At step S604, the second minimum cost Node2 in the inverse closed set is found. Referring to FIG. 7, Node1 is a forward search starting point and Node2 is a reverse search starting point.
At step S605, it is determined whether there is a collision-free reedshapp path from the first minimum cost Node1 to the second minimum cost Node 2. If the judgment result is negative, returning to the step S601 to repeatedly execute the bidirectional search; if the judgment result is yes, the step S606 is performed, and the procedure of the forward and reverse combination path is returned.
According to an embodiment of the invention, the bidirectional hybrid a-search algorithm introduces an inverse search cost term:
frev＝grev+hrev+wv·ρV(x,y)+q
q＝w1q1+w2q2+w3q3
wherein f isrevIs the sum of cost functions; grevFor the cost of motion, in some embodiments, grevAdopting the Euclidean distance from the current point to the target point; h isrevFor the heuristic cost of the current point, i.e. the cost of reaching the target point from the current point in the obstructed map without motion constraint, hrevObtaining the map by an A-algorithm when the map is preprocessed; rhoV(x, y) is the voronoi diagram potential energy of the current point, i.e. the distance of the current point from surrounding obstacles; w is avIs the weight of the Voronoi potential energy; q is a reverse search cost function, q1Distance between two points in forward and backward directions, q2Difference between course angles of two forward and backward points, q3The included angle between the reverse course angle and the joining direction of the two forward and reverse points is included; w is a1、w2And w3Are each q1、q2And q is3The cost weight of (2).
In step S1042, under the condition that it is determined that the obstacle avoidance is considered, a bidirectional hybrid a manner is adopted to perform search, and finally, a reedsleep curve is used to perform connection of search paths, so as to achieve the purpose of fast search. The search is respectively carried out from the starting point and the end point, so that the problem of low search speed in one-way search is solved.
Referring to fig. 2, after a route between the vehicle from the start position to the intermediate point is generated, the route is output to step S106 (corresponding to step S106 in fig. 2). Specifically, in step S1061, based on the end point position of the path, i.e., the final parking target position, an angle that needs to be adjusted at each time from the intermediate point to the parking target position is calculated through the vehicle kinematic model in combination with the vehicle pose at the intermediate point corresponding to the position, and it is determined that the vehicle is parked at the parking target position until the angle is equal to the target angle, so as to complete the geometric parking planning from the intermediate point to the final parking target position.
Specifically, referring to fig. 8, the horizontal direction X-axis direction is specified, the horizontal angle being 0 °, in order to the position of the vehicle at the intermediate point G (X)G,YG,θG) For the initial attitude, an angle θ, at which the vehicle wants to stop at the final parking target position and needs to be adjusted forward, is calculated by the vehicle kinematics model. Setting the width of the parking space area to be Pw, if the vehicle is desired to stop in the middle of the parking space area, the final expected position of the vehicle should be Pw/2, and the desired vehicle heading angle (target angle) should be 90 °, the following formula can be obtained:
2*sin(θ+θG)-sin(θG)+XG,＝Pw/2
wherein, thetaGThe angle θ can be obtained from the above formula for the angle of the vehicle at the intermediate point G, and the subsequent path can be calculated, as shown in fig. 8.
At step S1062, the paths generated between the starting location and the intermediate point and between the intermediate point and the parking target location are connected by the reedsshepp curve, so as to form a complete parking planning trajectory. Furthermore, after the connection to form a complete parking trajectory, collision detection can be carried out again. The complete parking path is then output.
According to an embodiment of the invention, a path planning system for automatic parking is also provided. As shown in fig. 9, the route planning system 900 for automatic parking includes an intermediate point selection module 910, a graph search module 920, and a kinematics calculation module 930. The intermediate point selecting module 910 is configured to select an intermediate point on a route from the start location to a parking target location in the parking space area. Wherein, the selection principle of the intermediate point should satisfy: the middle point is close to the corner point of the parking target position, and the vehicle moves only through the circular arc from the corresponding position of the middle point to the parking target position. The graph search module 920 is used to determine the path of the vehicle from the starting location to the intermediate point via a redshepp curve and/or a bi-directional hybrid a-search algorithm. The bidirectional hybrid A-search algorithm comprises forward search and reverse search, wherein the forward search is a direction gradually approaching the parking target position, and the reverse search is a direction gradually departing from the parking target position. The kinematics calculation module 930 is configured to calculate, through the vehicle kinematics model, an angle that needs to be adjusted at each time from the intermediate point to the parking target location in combination with the vehicle pose at the intermediate point corresponding to the location, until the vehicle is determined to be parked at the parking target location when the angle is equal to the target angle.
The path planning system 900 for automatic parking may further include a coordinate system establishing module 905 for establishing a parking space coordinate system. The coordinate system establishing module 905 selects the front corner point of the parking space near end of the parking space area as an origin, selects a straight line where the vehicle firstly reaches the boundary of the parking space area as a horizontal axis, and selects a straight line which is perpendicular to the horizontal axis and is far away from the boundary of the initial position as a longitudinal axis.
In some embodiments, the middle point selection module 910 selects a position where one wheel of the vehicle reaches a boundary of the parking space area as the middle point, where the one wheel is a wheel of the plurality of wheels of the vehicle that first reaches the parking space area, for example, a rear wheel of the parking space area.
In some embodiments, graph search module 920 includes a first search submodule 922 and a second search submodule 924. The first search submodule 922 is used to plan a path from the starting position to the intermediate point through a reedsshepp curve when it is determined that there is no risk of collision during parking. The second search submodule 924 is configured to perform a search through the bidirectional hybrid a algorithm and connect the paths searched through the bidirectional hybrid a algorithm by using a reedsshepp curve, in case it is determined that there is a risk of collision during parking. Wherein searching by the two-way hybrid a-algorithm comprises: and carrying out forward search to find the current minimum cost node as the starting point of the next-time search, and carrying out reverse search to find the current minimum cost node as the starting point of the next-time search.
According to the embodiment of the invention, the reverse search is carried out to find the current minimum cost node by introducing the reverse search cost item according to the following formula,
frev＝grev+hrev+wv·ρV(x,y)+q
q＝w1q1+w2q2+w3q3
wherein f isrevIs the sum of cost functions; grevFor the cost of motion, in some embodiments, grevAdopting the Euclidean distance from the current point to the target point; h isrevA heuristic cost for the current point; rhoV(x, y) is the voronoi diagram potential energy of the current point; w is avIs the weight of the Voronoi potential energy; q is a reverse search cost function, q1Distance between two points in forward and backward directions, q2Difference between course angles of two forward and backward points, q3The included angle between the reverse course angle and the joining direction of the two forward and reverse points is included; w is a1、w2And w3Are each q1、q2And q is3The cost weight of (2).
The kinematics calculation module 930 calculates the angle θ that needs to be adjusted at each moment from the middle point to the parking target position of the vehicle, including: assuming a horizontal direction X-axis direction, the horizontal angle is 0 DEG to the position of the vehicle at the intermediate point G (X)G,YG,θG) The vehicle is finally stopped in the middle of the parking space area for the initial posture, the final heading angle of the vehicle is 90 degrees, and the angle theta of the vehicle, which needs to be adjusted forwards when the vehicle is stopped at the parking target position, is calculated by a vehicle kinematics model by using the following formula:
2*sin(θ+θG)-sin(θG)+XG,＝Pw/2
wherein, thetaGThe angle of the vehicle at the intermediate point G, Pw is the width of the parking space region, and Pw/2 represents the final expected position of the vehicle.
According to the system, the parking process is divided into two stages by using the selected intermediate point and different planning modes at present are combined, the process that the vehicle needs to be adjusted repeatedly after entering the parking space area is planned by using the vehicle kinematics model, and the path planning is carried out by using a Reed shepp curve and/or a bidirectional hybrid A star search algorithm from the initial position of the vehicle to the stage of parking and warehousing, so that the requirement on hardware calculation is reduced, the planning flexibility is improved and the success rate of parking planning at any angle is improved by using a mode of fusing a graph search algorithm and a geometric planning algorithm.
According to an embodiment of the present invention, there is also provided an electronic device, which may include the automatic parking route planning system (e.g., route planning system 900 in fig. 9).
According to an embodiment of the present invention, there is also provided a storage device including a storage medium storing a program executed to implement the above-described path planning method for automatic parking.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.

Claims (10)

1. A path planning method for automatic parking, characterized by comprising:
selecting an intermediate point on a path from an initial position to a parking target position in a parking space area, wherein the intermediate point is close to an angular point of the parking target position, and the vehicle only moves through an arc from the intermediate point corresponding position to the parking target position;
determining a path of the vehicle from the initial position to the intermediate point through a Reed shepp curve and/or a bidirectional hybrid A-search algorithm, wherein the bidirectional hybrid A-search algorithm comprises forward search and reverse search, the forward search is a direction gradually approaching the parking target position, and the reverse search is a direction gradually departing from the parking target position;
and calculating the angle of the vehicle, which needs to be adjusted at each moment from the intermediate point to the parking target position, by combining the vehicle pose at the corresponding position of the intermediate point through a vehicle kinematic model until the vehicle is determined to be parked at the parking target position when the angle is equal to the target angle.
2. The method for path planning for automatic parking according to claim 1, further comprising, before selecting the intermediate point:
and establishing a parking space coordinate system, wherein a front corner point of a near end of a parking space in the parking space area is selected as an original point, a straight line where the vehicle firstly reaches the boundary of the parking space area is selected as a transverse axis, and a straight line which is perpendicular to the transverse axis and is far away from the boundary of the initial position is selected as a longitudinal axis.
3. The path planning method for automatic parking according to claim 2,
and connecting the path generated from the starting position to the intermediate point through a ReedShepp curve, and connecting the path from the starting position to the intermediate point and the path from the intermediate point to the parking target position to obtain a full-range planned path.
4. The path planning method for automatic parking according to claim 3,
planning the path from the starting position to the intermediate point through a Reed shepp curve under the condition that no collision risk is determined during parking,
and under the condition that the collision risk is determined to exist in the parking process, searching is carried out through a bidirectional hybrid A algorithm, and paths searched by the bidirectional hybrid A algorithm are connected through a Reed shepp curve.
5. The path planning method for automatic parking according to claim 4, wherein the search is performed by a two-way hybrid a-algorithm, comprising:
carrying out forward search to find the current minimum cost node as the starting point of the next time search;
and performing reverse search to find the current minimum cost node as the starting point of the search at the next moment.
6. The method of claim 5, wherein performing a reverse search to find a current minimum cost node comprises:
by introducing the inverse search cost term from the following formula,
frev＝grev+hrev+wv·ρV(x,y)+q
q＝w1q1+w2q2
wherein f isrevIs the sum of cost functions; grevFor the cost of motion, in some embodiments, grevAdopting the Euclidean distance from the current point to the target point; h isrevA heuristic cost for the current point; rhoV(x, y) is the voronoi diagram potential energy of the current point; w is avIs the weight of the Voronoi potential energy; q is a reverse search cost function, q1Distance between two points in forward and backward directions, q2Difference between course angles of two forward and backward points, q3The included angle between the reverse course angle and the joining direction of the two forward and reverse points is included; w is a1、w2And w3Are each q1、q2And q is3The cost weight of (2).
7. The path planning method for automatic parking according to claim 4, wherein the calculating of the angle of the vehicle to be adjusted at each time from the intermediate point to the parking target position by the vehicle kinematics model in combination with the vehicle pose at the intermediate point corresponding position comprises:
horizontal direction X-axis direction, horizontal angle 0 DEG, with the position (X) of the vehicle at the intermediate point GG,YG,θG) The vehicle is in an initial posture, the vehicle is finally stopped in the middle of the parking space area, the final course angle of the vehicle is 90 degrees, and the vehicle movesThe learning model calculates the angle theta of the vehicle to be parked at the parking target position, which needs to be adjusted forward, by using the following formula:
2*sin(θ+θG)-sin(θG)+XG,＝Pw/2
wherein, thetaGThe angle of the vehicle at said intermediate point G, Pw is the width of the parking space region, and Pw/2 represents the final expected position of the vehicle.
8. The path planning method for automatic parking according to claim 1, wherein selecting the intermediate point includes:
selecting a position where one wheel of the vehicle reaches a boundary of the parking space area as the intermediate point, wherein the one wheel is a wheel which reaches the parking space area first among the plurality of wheels of the vehicle.
9. A path planning system for automatic parking, comprising:
the intermediate point selection module is used for selecting an intermediate point on a path from an initial position to a parking target position in a parking space area, wherein the intermediate point is close to an angular point of the parking target position, and the vehicle only moves through an arc from the intermediate point corresponding position to the parking target position;
the graph searching module is used for determining a path of the vehicle from the initial position to the intermediate point through a ReedShepp curve and/or a bidirectional hybrid A-search algorithm, wherein the bidirectional hybrid A-search algorithm comprises forward search and reverse search, the forward search is a direction gradually approaching the parking target position, and the reverse search is a direction gradually departing from the parking target position;
and the kinematic calculation module is used for calculating an angle which needs to be adjusted at each moment from the middle point to the parking target position by combining the vehicle pose at the corresponding position of the middle point through a vehicle kinematic model until the vehicle is determined to be parked at the parking target position when the angle is equal to the target angle.
10. A storage device characterized by comprising a storage medium storing a program executed to implement the path planning method for automatic parking according to any one of claims 1 to 8.
CN202111597267.XA 2021-12-24 2021-12-24 Path planning method and system for automatic parking Pending CN114347982A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111597267.XA CN114347982A (en) 2021-12-24 2021-12-24 Path planning method and system for automatic parking

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111597267.XA CN114347982A (en) 2021-12-24 2021-12-24 Path planning method and system for automatic parking

Publications (1)

Publication Number Publication Date
CN114347982A true CN114347982A (en) 2022-04-15

Family

ID=81101948

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111597267.XA Pending CN114347982A (en) 2021-12-24 2021-12-24 Path planning method and system for automatic parking

Country Status (1)

Country Link
CN (1) CN114347982A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2024001215A1 (en) * 2022-06-30 2024-01-04 重庆长安汽车股份有限公司 Path planning method and device for autonomous parking of vertical and diagonal parking spaces

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110562245A (en) * 2019-08-15 2019-12-13 重庆长安汽车股份有限公司 Automatic parking space searching path following control method and system, storage medium and vehicle
CN112082567A (en) * 2020-09-05 2020-12-15 上海智驾汽车科技有限公司 Map path planning method based on combination of improved Astar and Grey wolf algorithm
CN112414422A (en) * 2020-11-01 2021-02-26 北京航空航天大学 Automatic parking path planning method and device and storage medium
CN112714729A (en) * 2019-11-29 2021-04-27 华为技术有限公司 Vehicle path planning method and vehicle path planning device
CN113503886A (en) * 2021-06-09 2021-10-15 中国国家铁路集团有限公司 Rapid path search optimization method under condition of large-scale complex road network

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110562245A (en) * 2019-08-15 2019-12-13 重庆长安汽车股份有限公司 Automatic parking space searching path following control method and system, storage medium and vehicle
CN112714729A (en) * 2019-11-29 2021-04-27 华为技术有限公司 Vehicle path planning method and vehicle path planning device
CN112082567A (en) * 2020-09-05 2020-12-15 上海智驾汽车科技有限公司 Map path planning method based on combination of improved Astar and Grey wolf algorithm
CN112414422A (en) * 2020-11-01 2021-02-26 北京航空航天大学 Automatic parking path planning method and device and storage medium
CN113503886A (en) * 2021-06-09 2021-10-15 中国国家铁路集团有限公司 Rapid path search optimization method under condition of large-scale complex road network

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2024001215A1 (en) * 2022-06-30 2024-01-04 重庆长安汽车股份有限公司 Path planning method and device for autonomous parking of vertical and diagonal parking spaces

Similar Documents

Publication Publication Date Title
WO2021180035A1 (en) Parking path planning method and apparatus, vehicle, and storage medium
US11460311B2 (en) Path planning method, system and device for autonomous driving
KR102593224B1 (en) Automatic parking method and apparatus, electronic device, and storage medium
CN109927716B (en) Autonomous vertical parking method based on high-precision map
JP6750365B2 (en) Parking assistance method and parking assistance device
CN108196536A (en) A kind of improved unmanned vehicle rapidly-exploring random tree paths planning method
CN110026979B (en) Mobile robot and return charging method
JP4670807B2 (en) Travel route creation method, autonomous mobile body, and autonomous mobile body control system
CN112256034B (en) Autonomous obstacle avoidance planning method and device
EP4208691A1 (en) Autonomous parking with hybrid exploration of parking space
CN111474928A (en) Robot control method, robot, electronic device, and readable storage medium
CN114347982A (en) Path planning method and system for automatic parking
Feng et al. Model-based decision making with imagination for autonomous parking
CN114527761A (en) Intelligent automobile local path planning method based on fusion algorithm
CN112612266A (en) Unstructured road global path planning method and system
CN115167398A (en) Unmanned ship path planning method based on improved A star algorithm
CN112706760B (en) Unmanned parking path planning method for special road scene
Meng et al. Improved hybrid A-star algorithm for path planning in autonomous parking system based on multi-stage dynamic optimization
CN115027453A (en) Head-first-out vertical parking-out path planning method
CN115202364A (en) Path planning method and device, robot and computer readable storage medium
CN115562290A (en) Robot path planning method based on A-star penalty control optimization algorithm
TWI747651B (en) Autonomous driving assistant system
CN115056765A (en) Path planning method and device for autonomous parking in vertical and inclined parking spaces
CN113885525A (en) Path planning method and system for automatically driving vehicle to get rid of trouble, vehicle and storage medium
US20210001837A1 (en) Parking assist device

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination