Parking path planning method and system

Abstract

The invention provides a parking path planning method and system. The parking path planning method comprises the following steps: acquiring vehicle parameter information of a target vehicle, and determining a parking final position; establishing a vehicle kinematics equation set based on the vehicle parameter information, and obtaining a first parking path according to the parking end point position based on the vehicle kinematics equation set by adopting a fourth-order Runge-Kutta method; and adopting an A * algorithm to obtain a second parking path based on the current position of the target vehicle and the parking space parking starting point, so as to merge the first parking path and the second parking path, and obtain a final parking path. According to the invention, the path search process of a hybrid A * algorithm in a non-convex environment with long time consumption is avoided, the kinematic characteristics of the vehicle are fully considered, and rapid path planning for parallel parking spaces is realized.

Description

Technical Field

[0001] The present application relates to a planning method, and in particular to a parking path planning method and system, belonging to the field of driving assistance technology. Background Art

[0002] Smart parking technology is an innovative solution based on artificial intelligence and autonomous driving. Its core technology relies on real-time perception of the vehicle's surroundings through various sensors onboard the vehicle, such as cameras, radar, and ultrasonic sensors. Intelligent parking is achieved through algorithmic analysis and processing of this sensory data. These sensors enable vehicles to obtain real-time information about parking spaces, such as the location, size, and availability of parking spaces. Based on this perception information, the vehicle calculates a parking path and automatically maneuvers the vehicle into an appropriate parking space. As a key component of smart parking technology, the results of the path planning algorithm directly impact parking safety, parking space utilization, control execution efficiency, and user experience.

[0003] Currently, the mainstream path planning algorithms for intelligent parking technology generally fall into two categories. The first category is geometric methods, including parametric curve methods such as the Dubins curve, Reeds-Shepp curve, spline curve, and Bezier curve. The geometric method has a fast solution speed, but it requires solving the starting area that meets the parking requirements based on the parking space. The algorithm cannot cover many actual scenarios, and it is more difficult to achieve obstacle avoidance in the presence of obstacles.

[0004] The second category is planning algorithms based on graph search (such as the Dijkstra algorithm and the A* algorithm). These algorithms implement path planning by constructing a grid map of the vehicle's environment and calculating the shortest path or the optimal path. Among them, the improved form of the A* algorithm, the hybrid A* algorithm combined with the Reeds-Shepp curve planning method, is widely used. It combines some of the vehicle's kinematic characteristics, and the planning results are more reasonable. The characteristics of grid map planning also make it easier to achieve obstacle avoidance. However, in narrow scenarios, the path planning is long and prone to failure. At the same time, its planning effect and speed are greatly affected by the cost function. For example, in the parallel parking space planning process, the vehicle needs to circumvent the garage multiple times. The hybrid A* algorithm is prone to calculation failure in this scenario and takes a long time. Summary of the Invention

[0005] In view of this, the present application provides a parking path planning method and system to solve or alleviate the technical problems existing in the prior art and at least provide a beneficial option.

[0006] The technical solution of the embodiment of the present application is implemented as follows:

[0007] A parking path planning method is provided, comprising the following steps: obtaining vehicle parameter information of a target vehicle and determining a parking endpoint position; establishing a vehicle kinematic equation system based on the vehicle parameter information, and using a fourth-order Runge-Kutta method to obtain a first parking path based on the vehicle kinematic equation system and the parking endpoint position; and using an A* algorithm to obtain a second parking path based on the current position of the target vehicle and a parking start point, and merging the first parking path and the second parking path to obtain a final parking path.

[0008] Further preferably, the vehicle parameter information includes at least the position coordinates of the center point of the vehicle's rear axle, the vehicle's heading angle, the vehicle's equivalent front wheel turning angle, the vehicle's wheelbase and speed.

[0009] Further preferably, the obtaining of the first parking path includes: using the parking endpoint position and vehicle parameter information as initial calculation values ​​and initial calculation parameters of a vehicle kinematic equation group, and using a fourth-order Runge-Kutta method to solve the vehicle kinematic equation group to solve the vehicle forward trajectory.

[0010] Further preferably, in the process of obtaining the first parking path, the method includes: determining whether a first calculation termination condition is satisfied based on the first vehicle preset position and the parking space preset position; if so, obtaining the first parking path based on the current solution result; otherwise, continuing to solve the vehicle forward trajectory.

[0011] Further preferably, in the process of obtaining the first parking path, the method further includes: judging whether a second calculation termination condition is satisfied based on the first vehicle preset position and the first parking space line position; if satisfied, starting to solve the vehicle backward trajectory; otherwise, continuing to solve the vehicle forward trajectory.

[0012] Further preferably, the obtaining and solving the backward trajectory of the vehicle includes: based on the first vehicle preset position that meets the second calculation termination condition, using the fourth-order Runge-Kutta method to solve the vehicle kinematic equations to solve the backward trajectory of the vehicle.

[0013] Further preferably, in the process of solving the backward trajectory of the vehicle, the method includes: judging whether a third calculation termination condition is satisfied based on the second vehicle preset position and the second parking line position, or judging whether a fourth calculation termination condition is satisfied based on the third vehicle preset position and the third parking line position; when either the third calculation termination condition or the fourth calculation termination condition is satisfied, the solution of the backward trajectory of the vehicle is terminated, and the first vehicle preset position that satisfies the second calculation termination condition is used as the initial calculation value to re-solve the forward trajectory of the vehicle; otherwise, the solution of the backward trajectory of the vehicle continues.

[0014] Further preferably, obtaining the second parking path includes: discretizing the parking space environment to obtain a grid map, and obtaining a vehicle driving direction based on the grid map and vehicle kinematic characteristics; determining a path search starting point and a path search end point based on the current position of the target vehicle and a parking start point, searching for a shortest path on the grid map based on the vehicle driving direction, the path search starting point, and the path search end point using an A* algorithm, and generating a motion path based on a current search node; and performing collision detection and path evaluation based on the motion path to update the vehicle driving direction according to the collision detection and path evaluation results.

[0015] Further preferably, in the process of obtaining the second parking path, the method includes: determining whether a fifth calculation termination condition is met based on the current search node; if so, using the shortest path currently searched as the second parking path; otherwise, continuing to search for the shortest path.

[0016] Based on the same concept, the present application also provides a parking path planning system, which includes: an acquisition module for acquiring vehicle parameter information of a target vehicle; a first planning module for determining a parking end position and establishing a vehicle kinematic equation set based on the vehicle parameter information, so as to obtain a first parking path based on the parking end position using a fourth-order Runge-Kutta method based on the vehicle kinematic equation set; a second planning module for obtaining a second parking path based on the current position of the target vehicle and a parking start point using an A* algorithm; and a merging module for merging the first parking path and the second parking path to obtain a final parking path.

[0017] The embodiment of the present application adopts the above technical solution, which has the following advantages:

[0018] This application utilizes a combined path planning algorithm. In the garage parking path calculation, which is difficult and time-consuming to calculate using the hybrid A* algorithm, a fourth-order Runge-Kutta method is used to directly solve the vehicle's kinematic equations to obtain the garage parking path. The hybrid A* algorithm is then used to directly solve the equation based on the vehicle's current position and the starting point for parking in the garage. The two paths are then combined to obtain the complete parking path. This algorithm avoids the time-consuming path search process of the hybrid A* algorithm in non-convex environments (such as within a garage), while fully considering the vehicle's kinematic characteristics to achieve rapid path planning for parallel parking spaces.

[0019] The above summary is for illustrative purposes only and is not intended to be limiting in any way. In addition to the illustrative aspects, embodiments and features described above, further aspects, embodiments and features of the present application will be readily apparent by reference to the accompanying drawings and the following detailed description. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or technical descriptions. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0021] Figure 1 This is a flowchart of the parking path planning method in an embodiment of the present application.

[0022] Figure 2 This is a schematic diagram of parallel parking spaces and vehicle positions in an embodiment of the present application.

[0023] Figure 3 This is a simplified schematic diagram of a single vehicle model in an embodiment of the present application.

[0024] Figure 4 This is a schematic diagram of the final state after parking is completed in an embodiment of the present application.

[0025] Figure 5 This is a schematic diagram of the completion point of parking in the garage in the embodiment of the present application.

[0026] Figure 6 This is a schematic diagram of the stop point of forward movement in the garage in an embodiment of the present application.

[0027] Figure 7 This is a schematic diagram of the end point of backward movement in the garage in the embodiment of the present application.

[0028] Figure 8 This is a framework diagram of the parking path planning system in an embodiment of the present application. DETAILED DESCRIPTION

[0029] Hereinafter, only certain exemplary embodiments are briefly described. As will be appreciated by those skilled in the art, the described embodiments may be modified in various ways without departing from the spirit or scope of the present application. Therefore, the drawings and description are to be regarded as illustrative in nature and not restrictive.

[0030] The embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0031] like Figure 1 As shown, an embodiment of the present application provides a parking path planning method, including the following steps S100-S300.

[0032] S100: Obtain vehicle parameter information of the target vehicle and determine the parking end position.

[0033] Further preferably, the vehicle parameter information includes at least the position coordinates of the center point of the rear axle of the vehicle (cx ,c y ), vehicle heading angle, vehicle equivalent front wheel turning angle, vehicle wheelbase and speed.

[0034] In some embodiments, in addition to obtaining the vehicle parameter information of the target vehicle, it is also necessary to obtain the coordinates of the four corner points of the parking space (x1, y1), (x2, y2), (x3, y3), and (x4, y4).

[0035] It should be noted that this application is mainly used for parallel parking spaces, and the coordinates of the corner points and the position coordinates of the center point of the vehicle's rear axle are as follows: Figure 2 shown.

[0036] S200: Establishing a vehicle kinematics equation group based on the vehicle parameter information, and using a fourth-order Runge-Kutta method to obtain a first parking path based on the vehicle kinematics equation group and the parking end position.

[0037] In some embodiments, the target vehicle is simplified into a bicycle model moving on a two-dimensional plane. The bicycle model diagram is as follows: Figure 3 As shown in , the model assumes that the vehicle has no lateral movement and the vehicle's motion is described by the vehicle's position and posture. Figure 3 C is the center of mass of the vehicle, l f , l r are the positions from the center of mass of the vehicle to the center of the front and rear axles, respectively, and R is the turning radius of the vehicle under the equivalent front wheel turning angle δ. The following three-degree-of-freedom vehicle kinematic equations are established:

[0038]

[0039] Where x, y are the coordinates of the center point of the rear axle of the vehicle (i.e. (c x ,c y )), ψ is the vehicle heading angle, δ is the vehicle equivalent front wheel turning angle, L is the vehicle wheelbase, and v is the speed.

[0040] Specifically, obtaining the first parking path includes: using the parking end position and vehicle parameter information as initial calculation values ​​and initial calculation parameters of the vehicle kinematic equations, and using a fourth-order Runge-Kutta method to solve the vehicle kinematic equations to solve the vehicle forward trajectory.

[0041] In some embodiments, a parking path for the vehicle in the garage is solved in combination with the vehicle kinematic equations. During the solution process, the parking end position is first determined, and then the parking exit path is solved in reverse order from the end point. Finally, the solved paths are arranged in reverse order to obtain a final parking path, which serves as the first parking path.

[0042] Step 1: Determine the parking end position (P1x end ,P1yend ),like Figure 4 As shown, where d rsafe It is the safe distance between the rear of the vehicle and the left sideline of the garage after parking is completed.

[0043] Step 2: Parallel parking end position (P1x end ,P1y end ), the vehicle equivalent front wheel turning angle is: -δ, speed: v (positive value), and vehicle heading angle: φ = 0 are the initial calculation values ​​and initial calculation parameters of the vehicle kinematic equations. The fourth-order Runge-Kutta method is used to solve the vehicle kinematic equations and solve the vehicle forward trajectory.

[0044] Further preferably, in the process of obtaining the first parking path, the method includes: determining whether a first calculation termination condition is satisfied based on the first vehicle preset position and the parking space preset position; if so, obtaining the first parking path based on the current solution result; otherwise, continuing to solve the vehicle forward trajectory.

[0045] In some embodiments, the termination condition in the second step includes the following two cases:

[0046] Case 1: If Figure 5 As shown, when the right front vertex position of the vehicle (ie, the first vehicle preset position) and the upper right vertex coordinate y4 of the garage (ie, the parking space preset position) are parallel, that is, the first calculation termination condition c3 y =y4, the first parking path calculation is completed, and jump to step 4.

[0047] Further preferably, in the process of obtaining the first parking path, the method further includes: judging whether a second calculation termination condition is satisfied based on the first vehicle preset position and the first parking space line position; if satisfied, starting to solve the vehicle backward trajectory; otherwise, continuing to solve the vehicle forward trajectory.

[0048] In some embodiments, case 2: Figure 6 As shown, when the right front vertex position of the vehicle (i.e., the first vehicle preset position) and the right side line of the garage (i.e., the first parking space line position) reach the upper limit of the safety distance d fsafe When, that is: the second calculation termination condition |c3 x -x4|≤d fsafe ; The solution is terminated and jump to the third step.

[0049] Further preferably, the obtaining and solving the backward trajectory of the vehicle includes: based on the first vehicle preset position that meets the second calculation termination condition, using the fourth-order Runge-Kutta method to solve the vehicle kinematic equations to solve the backward trajectory of the vehicle.

[0050] In some embodiments, the third step includes:

[0051] The vehicle position and heading angle calculated in case 2 of the second step are used as the initial calculation values. The initial calculation parameters are: the vehicle equivalent front wheel angle is δ, the speed is -v, and the fourth-order Runge-Kutta method is used to continue solving the vehicle kinematic equations to solve the vehicle's backward trajectory.

[0052] Further preferably, in the process of solving the backward trajectory of the vehicle, the method includes: judging whether a third calculation termination condition is satisfied based on the second vehicle preset position and the second parking line position, or judging whether a fourth calculation termination condition is satisfied based on the third vehicle preset position and the third parking line position; when either the third calculation termination condition or the fourth calculation termination condition is satisfied, the solution of the backward trajectory of the vehicle is terminated, and the first vehicle preset position that satisfies the second calculation termination condition is used as the initial calculation value to re-solve the forward trajectory of the vehicle; otherwise, the solution of the backward trajectory of the vehicle continues.

[0053] In some embodiments, the termination condition for solving the vehicle's backward trajectory is as follows: Figure 7 As shown, when the left rear vertex position of the vehicle (i.e., the second vehicle preset position) and the left side line of the garage (i.e., the second parking space line position) reach a safe distance d bsafe Or the right rear vertex position of the vehicle (i.e. the third vehicle preset position) and the lower side line of the garage (i.e. the third parking space line position) reach a safe distance d lsafe When, that is:

[0054] The third calculation termination condition: |c1 x -x1|≤d bsafe

[0055] The fourth calculation termination condition: |c2 y -y2|≤d lsafe

[0056] When any of the above conditions is met, the solution is terminated and the process jumps to the second step, in which the vehicle forward trajectory is solved using the termination position parameters as the initial calculation values ​​for the vehicle forward trajectory calculation.

[0057] The fourth step includes:

[0058] The path points calculated in the second and third steps are linked and arranged in reverse order to obtain the parking path of the target vehicle in the garage, which is used as the first parking path.

[0059] In the second step, when solving the forward trajectory of the vehicle, a new vehicle position is obtained each time (including information such as the vehicle's coordinates on the plane and the vehicle's heading angle). This position information is used as a path point and is stored in a list in the order of calculation. The order of the path points in this list is arranged in the chronological order of the vehicle's forward movement. In the third step, when solving the backward trajectory of the vehicle, a new vehicle position is also obtained each time. This is used as a path point and is stored in another list in the order of calculation. The order of the path points in this list is arranged in the chronological order of the vehicle's backward movement. The list storing the backward trajectory path points and the list storing the forward trajectory path points are directly spliced ​​together to obtain a new complete path point list.

[0060] In order to obtain the parking path of the target vehicle from its current position to the parking destination, the concatenated complete list of path points needs to be sorted in reverse order. The reverse order function of the list can be used to reverse the order of the elements in the list. In this way, the order of the path points in the list becomes the order from the parking destination to the current position of the vehicle.

[0061] The list of path points obtained after the above steps and arranged in reverse order is the parking path of the target vehicle in the garage, which is used as the first parking path.

[0062] S300: Using an A* algorithm to obtain a second parking path based on the current position of the target vehicle and the parking starting point, so as to merge the first parking path and the second parking path to obtain a final parking path.

[0063] Specifically, obtaining the second parking path includes: discretizing the parking space environment to obtain a grid map, and obtaining a vehicle driving direction based on the grid map and the vehicle kinematic characteristics; determining a path search starting point and a path search end point based on the current position of the target vehicle and the parking start point, using an A* algorithm to search for the shortest path on the grid map based on the vehicle driving direction, the path search starting point, and the path search end point, and generating a motion path based on the current search node; and performing collision detection and path evaluation based on the motion path to update the vehicle driving direction according to the collision detection and path evaluation results.

[0064] In some embodiments, the parking environment is discretized into a grid map, and the vehicle kinematic characteristics (maximum

[0065]

[0066] where δ max is the maximum equivalent front wheel turning angle, and L is the vehicle wheelbase.

[0067] The path search starting point and ending point are determined based on the target vehicle's current location and the starting point for parking. The A* algorithm is then used to search for the shortest path on the discretized map, serving as the initial path. The A* algorithm uses a heuristic function to evaluate the cost of each discrete state and selects the path with the lowest cost. The search cost function considers the path's distance cost, direction switching costs, and forward and backward movement costs.

[0068] Based on the kinematic state of the current search node, the possible motion path for the next step is generated. Within a fixed time interval, the Reeds-Shepp curve is used to directly connect the path end points. Collision detection is performed based on the generated motion path, and the path of the vehicle's rear axle center point is converted into the vehicle body contour line for collision detection, eliminating paths that collide with obstacles.

[0069] By evaluating the cost function of each motion path, the best motion option is selected as the next motion path; the searched path points are added to a path list and the kinematic state of the vehicle is updated.

[0070] Further preferably, in the process of obtaining the second parking path, the method includes: determining whether a fifth calculation termination condition is met based on the current search node; if so, using the shortest path currently searched as the second parking path; otherwise, continuing to search for the shortest path.

[0071] The fifth calculation termination condition includes the following situations:

[0072] Arrival at the end of the path search: The current search node has reached the predetermined parking space parking starting point, which is the end of the path search.

[0073] Reaching the maximum number of search steps: To prevent the search process from going indefinitely, a maximum number of search steps is set. When the number of search steps reaches this maximum value, the search process is considered to be stopped.

[0074] Path cost exceeds threshold: When searching for a path using the A* algorithm, a cost is calculated for each path. If the cost of the currently searched path exceeds a pre-set threshold, further search may not yield a better path, and the search can be stopped.

[0075] Furthermore, if the current search node satisfies any of the fifth calculation termination conditions during the determination process, the search process is terminated. At this point, all path points from the path search starting point to the current search node are connected, and the resulting shortest path is the second parking path.

[0076] Based on the same concept, the present application also provides a parking path planning system, which includes: an acquisition module for acquiring vehicle parameter information of a target vehicle; a first planning module for determining a parking end position and establishing a vehicle kinematic equation set based on the vehicle parameter information, so as to obtain a first parking path based on the parking end position using a fourth-order Runge-Kutta method based on the vehicle kinematic equation set; a second planning module for obtaining a second parking path based on the current position of the target vehicle and a parking start point using an A* algorithm; and a merging module for merging the first parking path and the second parking path to obtain a final parking path.

[0077] The system and the parking path planning method are implemented based on the same concept and have the same specific implementation methods, which will not be described in detail here.

[0078] The above description is merely a specific embodiment of the present application, but the scope of protection of the present application is not limited thereto. Any person skilled in the art can easily conceive of various modifications or substitutions within the technical scope disclosed in this application, and such modifications or substitutions should be included in the scope of protection of the present application. Therefore, the scope of protection of the present application should be based on the scope of protection of the claims.

Claims

1. A parking path planning method, characterized in that: The following steps are involved: Obtain vehicle parameter information of the target vehicle and determine the parking end position; Establishing a vehicle kinematics equation group based on the vehicle parameter information, and obtaining a first parking path according to the parking end position based on the vehicle kinematics equation group using a fourth-order Runge-Kutta method; Furthermore, an A* algorithm is used to obtain a second parking path based on the current position of the target vehicle and the parking starting point of the parking space, so as to merge the first parking path and the second parking path to obtain a final parking path.

2. The parking path planning method according to claim 1, characterized in that: The vehicle parameter information includes at least the position coordinates of the center point of the vehicle's rear axle, the vehicle's heading angle, the vehicle's equivalent front wheel turning angle, the vehicle's wheelbase and speed.

3. The parking path planning method according to claim 2, characterized in that: Obtaining the first parking path includes: using the parking endpoint position and vehicle parameter information as initial calculation values ​​and initial calculation parameters of a vehicle kinematic equation group, and using a fourth-order Runge-Kutta method to solve the vehicle kinematic equation group to solve the vehicle forward trajectory.

4. The parking path planning method according to claim 3, characterized in that: The process of obtaining the first parking path includes: determining whether a first calculation termination condition is satisfied based on the first vehicle preset position and the parking space preset position; if so, obtaining the first parking path based on the current solution result; otherwise, continuing to solve the vehicle forward trajectory.

5. The parking path planning method according to claim 3, characterized in that: The process of obtaining the first parking path also includes: determining whether a second calculation termination condition is satisfied based on the first vehicle preset position and the first parking space line position; if so, starting to solve the vehicle backward trajectory; otherwise, continuing to solve the vehicle forward trajectory.

6. The parking path planning method according to claim 5, characterized in that: Obtaining and solving the vehicle backward trajectory includes: solving the vehicle kinematic equations using a fourth-order Runge-Kutta method based on a first vehicle preset position that meets a second calculation termination condition to solve the vehicle backward trajectory.

7. The parking path planning method according to claim 6, characterized in that: In the process of solving the backward trajectory of the vehicle, the method includes: determining whether a third calculation termination condition is satisfied based on the second vehicle preset position and the second parking line position, or determining whether a fourth calculation termination condition is satisfied based on the third vehicle preset position and the third parking line position; When either the third calculation termination condition or the fourth calculation termination condition is met, the vehicle backward trajectory solution is terminated, and the first vehicle preset position that meets the second calculation termination condition is used as the initial calculation value to re-solve the vehicle forward trajectory; otherwise, the vehicle backward trajectory solution is continued.

8. The parking path planning method according to claim 1, characterized in that: Obtaining the second parking path includes: discretizing the parking environment to obtain a grid map, and obtaining a vehicle driving direction based on the grid map according to the vehicle kinematic characteristics; Determining a path search starting point and a path search end point based on the current position of the target vehicle and the parking start point, searching for the shortest path on the grid map based on the vehicle's driving direction, the path search starting point, and the path search end point using an A* algorithm, and generating a motion path based on the current search node; Furthermore, collision detection and path evaluation are performed based on the motion path, so as to update the vehicle driving direction according to the collision detection and path evaluation results.

9. The parking path planning method according to claim 8, characterized in that: The process of obtaining the second parking path includes: determining whether a fifth calculation termination condition is met based on the current search node; if so, using the shortest path obtained by the current search as the second parking path; otherwise, continuing to search for the shortest path.

10. A system using the parking path planning method according to any one of claims 1 to 9, characterized in that: The system comprises: An acquisition module, used to obtain vehicle parameter information of a target vehicle; a first planning module, configured to determine a parking end position and establish a vehicle kinematic equation system based on the vehicle parameter information, so as to obtain a first parking path based on the vehicle kinematic equation system and the parking end position using a fourth-order Runge-Kutta method; A second planning module is used to obtain a second parking path based on the current position of the target vehicle and the parking starting point using an A* algorithm; and a merging module, configured to merge the first parking path and the second parking path to obtain a final parking path.

Images