An automatic parking speed planning method and system

Abstract

The application discloses an automatic parking speed planning method and system, and the method comprises the following steps: acquiring a parking planning path, a self-vehicle pose, a current speed of the self-vehicle and dangerous obstacle information; determining a nearest point on the path from the self-vehicle and obtaining the pose of the nearest point on the path from the self-vehicle according to the self-vehicle pose and the parking planning path; determining a starting constraint speed, a longitudinal distance constraint speed, an angle deviation constraint speed, a lateral deviation constraint speed, a path curvature constraint speed and a dangerous obstacle constraint speed at a next moment, and taking the minimum value among the constraint speeds as a target speed at the next moment. The application can improve the parking safety and stability under the premise of ensuring the parking accuracy.

Description

Technical Field

[0001] This invention belongs to the field of automatic parking control, specifically relating to an automatic parking speed planning method and system. Background Technology

[0002] Automated parking refers to the process where a vehicle autonomously parks itself into a parking space without driver intervention. Currently, automated parking primarily uses ultrasonic sensors. Due to limitations in sensor sensing capabilities, the vehicle speed is generally quite low during automated parking. Furthermore, the space around the parking space is often confined, and multiple forward and backward adjustments are required during parking, necessitating high control precision.

[0003] CN110347167A discloses a speed planning method and speed planning system, which can guarantee parking accuracy and improve the efficiency and user experience of automatic parking to a certain extent. However, it does not consider path tracking, obstacle constraints, and starting constraints, resulting in low safety and stability of automatic parking. Summary of the Invention

[0004] The purpose of this invention is to provide an automatic parking speed planning method and system to improve parking safety and smoothness while ensuring parking accuracy.

[0005] In a first aspect, the automatic parking speed planning method of the present invention includes: The system acquires parking planning path, vehicle pose, current speed, and information on dangerous obstacles. The parking planning path includes the total path length, each path point, and the pose and curvature of each path point.

[0006] Based on the vehicle's pose and the parking planning path, determine the closest point on the path to the vehicle (i.e., the point on the path that is closest to the vehicle), and obtain the pose of the closest point on the path to the vehicle.

[0007] Based on the parking planning path, the pose of the nearest point on the path to the vehicle, the vehicle's pose, the vehicle's current speed, information on dangerous obstacles, and preset upper acceleration limit, upper speed limit, lower speed limit, first speed transformation parameters, second speed transformation parameters, third speed transformation parameters, fourth speed transformation parameters, and preset cycle time, the starting constraint speed, longitudinal distance constraint speed, angle deviation constraint speed, lateral deviation constraint speed, path curvature constraint speed, and dangerous obstacle constraint speed for the next moment are determined.

[0008] Take the minimum value among the starting constraint speed, longitudinal distance constraint speed, angle deviation constraint speed, lateral deviation constraint speed, path curvature constraint speed, and dangerous obstacle constraint speed at the next moment as the target speed (i.e., the planned speed) at the next moment.

[0009] Preferably, the method for determining the starting constraint speed at the next moment includes: Calculate the speed at the next moment based on the vehicle's current speed, the preset acceleration limit, and the preset cycle time.

[0010] By using a preset speed limit to restrict the speed at the next moment, the starting constraint speed at the next moment can be obtained.

[0011] Preferably, the method for determining the longitudinal distance constraint velocity at the next moment includes: Calculate the traversed path segment and its length based on the coordinates of the nearest point on the path. Since the pose of the nearest point on the path includes both its coordinates and its angle (attitude), knowing the pose of the nearest point on the path also reveals its coordinates.

[0012] The remaining path distance to the destination is obtained by subtracting the length of the path segment already traversed from the total path length.

[0013] Using the remaining path distance to the destination, the preset speed limit, and the preset first speed transformation parameters, calculate the longitudinal distance constraint speed at the next moment.

[0014] Preferably, the method for determining the angle deviation constraint velocity at the next moment includes: The angle deviation is obtained by subtracting the angle of the nearest point on the path to the vehicle from the vehicle's angle. Since the vehicle's pose includes both its position coordinates and its angle (attitude), knowing the vehicle's pose means knowing its angle. Furthermore, knowing the pose of the nearest point on the path to the vehicle also means knowing its angle (attitude).

[0015] Dividing the angular deviation by π (pi) yields the normalized deviation.

[0016] The angle deviation constraint speed at the next moment is calculated using the normalized deviation, the preset upper speed limit, and the preset lower speed limit.

[0017] Preferably, the method for determining the lateral deviation constraint velocity at the next moment includes: Calculate the distance from the vehicle's position coordinates to the position coordinates of the nearest point on the path to the vehicle, and use this distance as the lateral deviation.

[0018] The lateral deviation constraint speed at the next moment is calculated using the lateral deviation, the preset upper speed limit, the preset lower speed limit, and the preset second speed transformation parameters.

[0019] Preferably, the method for determining the path curvature constraint velocity at the next moment includes: Find the two path points that correspond to the closest point on the path to the vehicle.

[0020] The larger of the curvatures of the two path points is taken as the curvature of the point on the path closest to the vehicle.

[0021] The path curvature constraint speed at the next moment is calculated using the curvature of the point closest to the vehicle on the path, the preset upper speed limit, the preset lower speed limit, and the preset third speed transformation parameters.

[0022] Preferably, the method for determining the constraint speed of the dangerous obstacle at the next moment is as follows: calculate the constraint speed of the dangerous obstacle at the next moment based on the dangerous obstacle information, the preset speed limit, and the preset fourth speed transformation parameters; wherein, the dangerous obstacle information is the nearest distance to the dangerous obstacle.

[0023] Preferably, the method for obtaining the closest distance to the dangerous obstacle includes: The detected obstacles near the vehicle are divided into m obstacle points.

[0024] Determine the velocity V of the i-th obstacle point relative to the vehicle. i ; where i takes all integers from 1 to m in sequence.

[0025] Based on the speed V of the i-th obstacle point relative to the vehicle's motion i Estimate the trajectory of the i-th obstacle point.

[0026] For the i-th obstacle point, construct the i-th obstacle coordinate system with its coordinates in the vehicle coordinate system as the origin and the velocity direction as the positive direction.

[0027] Determine the point on the outer contour of the vehicle that is closest to the i-th obstacle point, and record this point as the first candidate collision point A corresponding to the i-th obstacle point. i_1 .

[0028] Determine the path of the i-th obstacle point and the first candidate collision point A. i_1 Find the point closest to the i-th obstacle and record this point as the first trajectory prediction point B corresponding to the i-th obstacle point. i_1 .

[0029] If the first trajectory prediction point B i_1 If the negative half-axis of the coordinate system of the i-th obstacle is located, then the danger coefficient x of the i-th obstacle point is... i It is 0.

[0030] If the first trajectory prediction point B i_1 If the obstacle is not located on the negative half-axis of the i-th obstacle coordinate system, iterative calculation is performed to find the predicted collision point C corresponding to the i-th obstacle point.i .

[0031] Calculate the i-th obstacle point and the predicted collision point C i The distance between them is used as the collision distance d corresponding to the i-th obstacle point. i .

[0032] Using the formula: x i =R i *V i / d i The danger coefficient x of the i-th obstacle point is calculated. i Among them, R i This represents the risk coefficient corresponding to the type of the i-th obstacle point.

[0033] Identify the obstacle with the highest risk among m obstacle points, and take the collision distance corresponding to the obstacle with the highest risk as the nearest distance to the dangerous obstacle.

[0034] Preferably, iterative calculations are performed to find the predicted collision point C corresponding to the i-th obstacle point. i The methods include: First, set the iteration count k=1, and then execute the second step.

[0035] The second step is to determine the k-th trajectory prediction point B on the outer contour of the vehicle corresponding to the i-th obstacle point. i_k Find the point closest to the i-th obstacle and denote this point as the (k+1)-th candidate collision point A corresponding to the i-th obstacle. i_k+1 Then proceed to step three.

[0036] Step 3: Determine the path of the i-th obstacle point and the (k+1)-th candidate collision point A. i_k+1 Find the point closest to the i-th obstacle and record this point as the (k+1)-th trajectory prediction point B corresponding to the i-th obstacle point. i_k+1 Then proceed to step four.

[0037] Step 4: Determine if the predicted trajectory point B is the (k+1)th time. i_k+1 With the k-th trajectory prediction point B i_k If the distance between them is less than or equal to a preset first distance threshold, then proceed to step six; otherwise, proceed to step five.

[0038] Step 5: Increment the iteration count k by 1 (i.e., k = k + 1), then return to step 2.

[0039] Step 6: Calculate the (k+1)th trajectory prediction point B. i_k+1 With the (k+1)th candidate collision point A i_k+1The distance between them is used as the closest distance S to the i-th obstacle point. i_dist Then proceed to step seven.

[0040] Step 7: Determine if the closest distance S is specified. i_dist If the distance is less than or equal to the preset second distance threshold, proceed to step nine; otherwise, proceed to step eight.

[0041] Step 8: Increase the danger coefficient x of the i-th obstacle point. i Set the value to 0, then end.

[0042] Step 9: Predict the trajectory point B for the (k+1)th time. i_k+1 As the predicted collision point C corresponding to the i-th obstacle point i Then it ends.

[0043] Preferably, the method for determining the closest point to the vehicle on the path includes: Construct a path segment coordinate system based on each pair of adjacent path points in the parking planning path.

[0044] Transform the vehicle's position coordinates to the coordinate system of all path segments, calculate their lateral distances, and compare to find the closest path segment.

[0045] Project the vehicle's position coordinates onto the coordinate system of the nearest path segment to obtain the projection point.

[0046] By transforming the projection points back to the original coordinate system, the closest point to the vehicle on the path is obtained.

[0047] Secondly, the automatic parking speed planning system of the present invention includes a processor and a memory connected to the processor; the memory stores a computer-readable program, which, when called by the processor, can execute the above-described automatic parking speed planning method.

[0048] The present invention has the following effects: The target speed (i.e., the planned speed) is limited by starting constraint speed and longitudinal distance constraint speed, taking into account the smoothness of parking; the target speed is limited by angle deviation constraint speed, lateral deviation constraint speed, and path curvature constraint speed, taking into account parking accuracy; and the target speed is limited by hazard obstacle constraint speed, taking into account parking safety. The minimum value among the starting constraint speed, longitudinal distance constraint speed, angle deviation constraint speed, lateral deviation constraint speed, path curvature constraint speed, and hazard obstacle constraint speed at the next moment is taken as the target speed for the next moment. This achieves the goal of improving parking safety and smoothness while ensuring parking accuracy, thereby enhancing the user experience. Attached Figure Description

[0049] Figure 1 This is a flowchart of the automatic parking speed planning method in a real-time example of the present invention.

[0050] Figure 2 This is a flowchart of a method for determining the starting constraint speed at the next moment in some embodiments.

[0051] Figure 3 This is a flowchart of a method for determining the longitudinal distance constraint velocity at the next moment in some embodiments.

[0052] Figure 4 This is a flowchart of a method for determining the angle deviation constraint velocity at the next moment in some embodiments.

[0053] Figure 5 This is a flowchart of a method for determining the lateral deviation constraint velocity at the next moment in some embodiments.

[0054] Figure 6 This is a flowchart of a method for determining the path curvature constraint velocity at the next moment in some embodiments.

[0055] Figure 7 This is a flowchart illustrating a method for obtaining the closest distance to a dangerous obstacle in some embodiments.

[0056] Figure 8 In some embodiments, the predicted collision point C corresponding to the i-th obstacle point is determined. i The method flowchart. Detailed Implementation

[0057] To gain a more detailed understanding of the features and technical content of the embodiments of the present invention, the implementation of the embodiments of the present invention will be described in detail below with reference to the accompanying drawings. The accompanying drawings are for reference and illustration only and are not intended to limit the embodiments of the present invention.

[0058] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein is for the purpose of describing embodiments of the invention only and is not intended to limit the invention.

[0059] In the following description, references are made to “some embodiments,” which describe a subset of all possible embodiments. However, it is understood that “some embodiments” may be the same subset or different subsets of all possible embodiments and may be combined with each other without conflict.

[0060] It should also be noted that the terms "first, second, third, and fourth" used in the embodiments of the present invention are used to distinguish similar objects.

[0061] like Figure 1 As shown, the automatic parking speed planning method in this embodiment of the invention includes: Step 1: Obtain the parking planning path, vehicle position, current speed, and information on dangerous obstacles.

[0062] The parking planning path includes the total path length, each path point, and the pose and curvature of each path point. It should be noted that the parking planning path consists of multiple connected curves from the rear axle center point of the vehicle to the parking space. The parking planning path is calculated by the path planning module, and the calculation method is existing technology. The vehicle pose includes the vehicle's position coordinates and angle (attitude); therefore, obtaining the vehicle pose allows us to know the vehicle's position coordinates and angle. The vehicle pose and current speed can be calculated using wheel speed sensors and gyroscopes, but this is not limited in this embodiment of the invention.

[0063] Dangerous obstacles typically refer to obstacles near the vehicle that may collide with it, detected by sensors such as cameras, ultrasonic waves, and lidar. In some embodiments, dangerous obstacle information refers to the closest distance to the dangerous obstacle. Methods for obtaining the closest distance to a dangerous obstacle (see...) Figure 7 )include: Step S101: Divide the detected obstacles near the vehicle into m obstacle points, and then execute step S102.

[0064] Step S102: Determine the velocity V of the i-th obstacle point relative to the vehicle. i Then, step S103 is executed; where i takes all integers from 1 to m. In some embodiments, the velocity V of the i-th obstacle point relative to the vehicle's motion is determined. i The method is as follows: transform the i-th obstacle point into the vehicle's coordinate system, and transform the vehicle's velocity and the velocity of the i-th obstacle point in the global coordinate system into the velocity V of the i-th obstacle point relative to the vehicle. i .

[0065] Step S103: Based on the speed V of the i-th obstacle point relative to the vehicle's motion. i Estimate the trajectory of the i-th obstacle point, and then execute step S104.

[0066] Step S104: For the i-th obstacle point, construct the i-th obstacle coordinate system with its coordinates in the vehicle coordinate system as the origin and the velocity direction as the positive direction, and then execute step S105.

[0067] Step S105: Determine the point on the outer contour of the vehicle that is closest to the i-th obstacle point, and record this point as the first candidate collision point A corresponding to the i-th obstacle point. i_1 Then proceed to step S106.

[0068] Step S106: Determine the first candidate collision point A on the trajectory of the i-th obstacle point. i_1 Find the point closest to the i-th obstacle and record this point as the first trajectory prediction point B corresponding to the i-th obstacle point. i_1 Then proceed to step S107.

[0069] Step S107: Determine if this is the first time trajectory prediction point B is used. i_1 If the negative half-axis of the i-th obstacle coordinate system is positive (indicating no collision risk), then proceed to step S108; otherwise, proceed to step S109.

[0070] Step S108: Set the danger coefficient x of the i-th obstacle point. i Set the value to 0, then end.

[0071] Step S109: Perform iterative calculations to find the predicted collision point C corresponding to the i-th obstacle point. i Then proceed to step S110.

[0072] In some embodiments, iterative calculations are performed to find the predicted collision point C corresponding to the i-th obstacle point. i The method (see) Figure 8 )include: First, set the iteration count k=1, and then execute the second step.

[0073] The second step is to determine the k-th trajectory prediction point B on the outer contour of the vehicle corresponding to the i-th obstacle point. i_k Find the point closest to the i-th obstacle and denote this point as the (k+1)-th candidate collision point A corresponding to the i-th obstacle. i_k+1 Then proceed to step three.

[0074] Step 3: Determine the path of the i-th obstacle point and the (k+1)-th candidate collision point A. i_k+1 Find the point closest to the i-th obstacle and record this point as the (k+1)-th trajectory prediction point B corresponding to the i-th obstacle point. i_k+1 Then proceed to step four.

[0075] Step 4: Determine if point B is the predicted point for the (k+1)th trajectory. i_k+1 With the k-th trajectory prediction point B i_k If the distance between them is less than or equal to a preset first distance threshold, then proceed to step six; otherwise, proceed to step five.

[0076] Step 5: Increment the iteration count k by 1 (i.e., k = k + 1), then return to step 2.

[0077] Step 6: Calculate the predicted trajectory point B for the (k+1)th time. i_k+1 With the (k+1)th candidate collision point Ai_k+1 The distance between them is used as the closest distance S to the i-th obstacle point. i_dist Then proceed to step seven.

[0078] Step 7: Determine if it is the closest distance S i_dist If the distance is less than or equal to the preset second distance threshold, proceed to step nine; otherwise, proceed to step eight. It should be noted that the closest distance S... i_dist If the distance is less than or equal to a preset second distance threshold, it indicates that the i-th obstacle point has a collision risk, therefore the corresponding predicted collision point needs to be found; the closest distance S i_dist If the distance is greater than the preset second distance threshold, it means that there is no risk of collision with the i-th obstacle point, so there is no need to find the corresponding predicted collision point.

[0079] Step 8: Increase the danger coefficient x of the i-th obstacle point. i Set the value to 0, then end.

[0080] Step 9: Predict the trajectory point B for the (k+1)th time. i_k+1 As the predicted collision point C corresponding to the i-th obstacle point i Then it ends.

[0081] Step S110: Calculate the collision point C between the i-th obstacle point and the predicted collision point. i The distance between them is used as the collision distance d corresponding to the i-th obstacle point. i Then proceed to step S111.

[0082] Step S111: Using the formula: x i =R i *V i / d i The danger coefficient x of the i-th obstacle point is calculated. i Then, proceed to step S112. Where R... i This represents the risk coefficient corresponding to the type of the i-th obstacle point. As an example, R can be obtained by looking up the table based on the type of the i-th obstacle point. i The value of .

[0083] Step S112: Determine the obstacle with the highest risk coefficient among the m obstacle points, and take the collision distance corresponding to the obstacle with the highest risk coefficient as the nearest distance S_risk_dist(n) of the dangerous obstacle, and then end.

[0084] Step 2: Based on the vehicle's pose and the parking planning path, determine the closest point on the path to the vehicle (i.e., the point on the path that is closest to the vehicle), and obtain the pose of that closest point.

[0085] In some embodiments, the method for determining the closest point on the path to the vehicle includes: First, a coordinate system for the path segment is constructed based on each pair of adjacent path points in the parking planning path.

[0086] Secondly, the vehicle's position coordinates are transformed to the coordinate system of all path segments, their lateral distances are calculated, and the nearest path segment is obtained by comparison.

[0087] Then, the vehicle's position coordinates are projected onto the coordinate system of the nearest path segment to obtain the projection point.

[0088] Finally, the projected points are transformed back to the original coordinate system to obtain the closest point on the path to the vehicle.

[0089] Step 3: Determine the following constraints for the next time step: starting speed Vr_boot(n+1), longitudinal distance constraint speed Vr_lon(n+1), angle deviation constraint speed Vr_angle(n+1), lateral deviation constraint speed Vr_lat(n+1), path curvature constraint speed Vr_curv(n+1), and obstacle constraint speed Vr_risk(n+1).

[0090] In some embodiments, the method for determining the start-up constraint speed Vr_boot(n+1) at the next moment (see [reference]). Figure 2 )include: First, based on the vehicle's current speed Vs(n), the preset acceleration limit Amax, and the preset cycle time dt, calculate the speed Vs(n+1) at the next moment. As an example, the calculation formula is: Vs(n+1) = Vs(n) + Amax * dt.

[0091] Then, the speed Vs(n+1) at the next moment is limited by the preset speed upper limit Vmax to obtain the starting constraint speed Vr_boot(n+1) at the next moment. As an example, the calculation formula is: Vr_boot(n+1) = min(Vmax,Vs(n+1) ); where min(Vmax,Vs(n+1)) means taking the minimum value between Vmax and Vs(n+1).

[0092] In some embodiments, the method for determining the longitudinal distance constraint velocity Vr_lon(n+1) at the next moment (see [reference]). Figure 3 )include: First, calculate the path segments already traversed and their lengths based on the coordinates of the nearest point on the path to the vehicle.

[0093] It should be noted that the pose of the closest point to the vehicle on the path includes the position coordinates of the closest point to the vehicle on the path and the angle (attitude) of the closest point to the vehicle on the path; therefore, knowing the pose of the closest point to the vehicle on the path also means knowing the position coordinates and the angle of the closest point to the vehicle on the path.

[0094] Then, by subtracting the length of the path segments already traversed from the total path length, the remaining path distance S_long_dist(n) from the destination is obtained.

[0095] Finally, using the remaining path distance S_long_dist(n) from the destination, the preset speed limit Vmax, and the preset first speed transformation parameter Kslop1, the longitudinal distance constraint speed Vr_lon(n+1) for the next moment is calculated. As an example, the calculation formula is: Vr_lon(n+1) = Vmax * S_long_dist(n) / (Kslop1+S_long_dist(n)).

[0096] In some implementation examples, the method for determining the angle deviation constraint velocity Vr_angle(n+1) at the next moment (see...) Figure 4 )include: First, subtract the angle of the nearest point on the path to the vehicle from the vehicle's angle to obtain the angle deviation S_angle_error(n).

[0097] Then, divide the angle deviation S_angle_error(n) by π to obtain the normalized deviation S_angle_error(n) / π.

[0098] Finally, using the normalized deviation S_angle_error(n) / π, the preset upper speed limit Vmax, and the preset lower speed limit Vmin, the angle deviation constraint speed Vr_angle(n+1) at the next moment is calculated. As an example, the calculation formula is: Vr_angle(n+1) = Vmin + (Vmax-Vmin)*(1-S_angle_error(n) / π).

[0099] In some embodiments, the method for determining the lateral deviation constraint velocity Vr_lat(n+1) at the next time step (see [reference]). Figure 5 )include: First, calculate the distance from the vehicle's position coordinates to the position coordinates of the nearest point on the path to the vehicle, and use it as the lateral deviation S_lat_dist(n).

[0100] Then, using the lateral deviation S_lat_dist(n), the preset upper speed limit Vmax, the preset lower speed limit Vmin, and the preset second speed transformation parameter Kslop2, the lateral deviation constraint speed Vr_lat(n+1) at the next moment is calculated. As an example, the calculation formula is: Vr_lat(n+1)= Vmin + (Vmax-Vmin)*( Kslop2 / (Kslop2+S_lat_dist(n))) .

[0101] In some embodiments, the method for determining the path curvature constraint velocity Vr_curv(n+1) at the next time step (see [link to documentation]). Figure 6 )include: First, find the two path points that correspond to the closest point on the path to the vehicle.

[0102] Then, the larger of the curvatures of the two path points is taken as the curvature C(n) of the point on the path closest to the vehicle.

[0103] Finally, using the curvature C(n) of the closest point on the path to the vehicle, the preset upper speed limit Vmax, the preset lower speed limit Vmin, and the preset third speed transformation parameter Kslop3, the path curvature constraint speed Vr_curv(n+1) at the next moment is calculated. As an example, the calculation formula is: Vr_curv(n+1) = Vmin + (Vmax-Vmin)*( Kslop3 / ( Kslop3+C(n))) .

[0104] In some embodiments, the method for determining the hazard constraint velocity Vr_risk(n+1) at the next moment is as follows: Based on the nearest distance to the dangerous obstacle S_risk_dist(n), the preset speed limit Vmax, and the preset fourth speed transformation parameter Kslop4, calculate the constraint speed Vr_risk(n+1) of the dangerous obstacle at the next moment. As an example, the calculation formula is: Vr_risk(n+1) = Vmax * S_risk_dist(n) / (Kslop4+S_risk_dist(n)).

[0105] Step 4: Take the minimum value among the following constraints for the next time step: starting speed Vr_boot(n+1), longitudinal distance constraint speed Vr_lon(n+1), angular deviation constraint speed Vr_angle(n+1), lateral deviation constraint speed Vr_lat(n+1), path curvature constraint speed Vr_curv(n+1), and obstacle constraint speed Vr_risk(n+1). Use this minimum value as the target speed Vr (i.e., the planned speed) for the next time step. Vr = min(Vr_boot(n+1),Vr_lon(n+1),Vr_angle(n+1),Vr_lat(n+1),Vr_curv(n+1),Vr_risk(n+1)). Here, min() represents the minimum value operation.

[0106] This invention also provides an automatic parking speed planning system, which includes a processor and a memory connected to the processor; the memory stores a computer-readable program, which, when called by the processor, can execute the above-described automatic parking speed planning method.

[0107] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. An automatic parking speed planning method, characterized in that, include: The system acquires parking planning path, vehicle pose, current speed, and information on dangerous obstacles. The parking planning path includes the total path length, each path point, and the pose and curvature of each path point. Based on the vehicle's pose and the parking planning path, determine the closest point on the path to the vehicle and obtain the pose of that closest point; Based on the parking planning path, the pose of the nearest point, the vehicle's pose, the vehicle's current speed, information on dangerous obstacles, and preset upper limits for acceleration, speed, and speed, as well as preset first speed transformation parameters, second speed transformation parameters, third speed transformation parameters, fourth speed transformation parameters, and preset cycle time, the starting constraint speed, longitudinal distance constraint speed, angle deviation constraint speed, lateral deviation constraint speed, path curvature constraint speed, and dangerous obstacle constraint speed for the next moment are determined. Take the minimum value among the starting constraint speed, longitudinal distance constraint speed, angle deviation constraint speed, lateral deviation constraint speed, path curvature constraint speed, and dangerous obstacle constraint speed at the next moment as the target speed at the next moment.

2. The automatic parking speed planning method according to claim 1, characterized in that: Methods for determining the starting constraint speed at the next moment include: Calculate the speed at the next moment based on the vehicle's current speed, preset acceleration limit, and preset cycle time; By using a preset speed limit to restrict the speed at the next moment, the starting constraint speed at the next moment can be obtained.

3. The automatic parking speed planning method according to claim 1, characterized in that: Methods for determining the longitudinal distance constraint velocity at the next moment include: Based on the coordinates of the nearest point, calculate the path segment that has been traversed and its length; Subtract the length of the path segments already traversed from the total path length to obtain the remaining path distance to the destination; Using the remaining path distance to the destination, the preset speed limit, and the preset first speed transformation parameters, calculate the longitudinal distance constraint speed at the next moment.

4. The automatic parking speed planning method according to claim 1, characterized in that, Methods for determining the angular deviation constraint velocity at the next moment include: The angle deviation is obtained by subtracting the angle of the nearest point from the angle of the vehicle itself. Divide the angular deviation by π to obtain the normalized deviation; The angle deviation constraint speed at the next moment is calculated using the normalized deviation, the preset upper speed limit, and the preset lower speed limit.

5. The automatic parking speed planning method according to claim 1, characterized in that, Methods for determining the lateral deviation constraint velocity at the next moment include: Calculate the distance from the vehicle's position coordinates to the position coordinates of the nearest point, as the lateral deviation; The lateral deviation constraint speed at the next moment is calculated using the lateral deviation, the preset upper speed limit, the preset lower speed limit, and the preset second speed transformation parameters.

6. The automatic parking speed planning method according to claim 1, characterized in that, Methods for determining the path curvature constraint velocity at the next moment include: Find the two path points corresponding to the nearest point; The larger of the curvatures of the two path points is taken as the curvature of the nearest point; Using the curvature of the nearest point, the preset upper speed limit, the preset lower speed limit, and the preset third speed transformation parameters, the path curvature constraint speed at the next moment is calculated.

7. The automatic parking speed planning method according to claim 1, characterized in that, The method for determining the constraint speed of the dangerous obstacle at the next moment is as follows: based on the dangerous obstacle information, the preset speed limit, and the preset fourth speed transformation parameters, calculate the constraint speed of the dangerous obstacle at the next moment; where the dangerous obstacle information is the nearest distance to the dangerous obstacle.

8. The automatic parking speed planning method according to claim 7, characterized in that, The method for obtaining the closest distance to the dangerous obstacle includes: The detected obstacles near the vehicle are divided into m obstacle points; Determine the velocity V of the i-th obstacle point relative to the vehicle. i Where i takes any integer from 1 to m; Based on the speed V of the i-th obstacle point relative to the vehicle's motion i Estimate the trajectory of the i-th obstacle point; For the i-th obstacle point, construct the i-th obstacle coordinate system with its coordinates in the vehicle coordinate system as the origin and the velocity direction as the positive direction; Determine the point on the outer contour of the vehicle that is closest to the i-th obstacle point, and record this point as the first candidate collision point A corresponding to the i-th obstacle point. i_1 ; Determine the path of the i-th obstacle point and the first candidate collision point A. i_1 Find the point closest to the i-th obstacle and record this point as the first trajectory prediction point B corresponding to the i-th obstacle point. i_1 ; If the first trajectory prediction point B i_1 If the negative half-axis of the coordinate system of the i-th obstacle is located, then the danger coefficient x of the i-th obstacle point is... i =0; If the first trajectory prediction point B i_1 If the obstacle is not located on the negative half-axis of the i-th obstacle coordinate system, iterative calculation is performed to find the predicted collision point C corresponding to the i-th obstacle point. i ; Calculate the i-th obstacle point and the predicted collision point C i The distance between them is used as the collision distance d corresponding to the i-th obstacle point. i ; Using the formula: x i =R i *V i / d i The danger coefficient x of the i-th obstacle point is calculated. i Among them, R i This represents the risk coefficient corresponding to the type of the i-th obstacle point; Identify the obstacle with the highest risk among m obstacle points, and take the collision distance corresponding to the obstacle with the highest risk as the nearest distance to the dangerous obstacle.

9. The automatic parking speed planning method according to claim 8, characterized in that: Perform iterative calculations to find the predicted collision point C corresponding to the i-th obstacle point. i The methods include: First, set the iteration count k=1, then execute the second step; The second step is to determine the k-th trajectory prediction point B on the outer contour of the vehicle corresponding to the i-th obstacle point. i_k Find the point closest to the i-th obstacle and denote this point as the (k+1)-th candidate collision point A corresponding to the i-th obstacle. i_k+1 Then proceed to step three; Step 3: Determine the path of the i-th obstacle point and the (k+1)-th candidate collision point A. i_k+1 Find the point closest to the i-th obstacle and record this point as the (k+1)-th trajectory prediction point B corresponding to the i-th obstacle point. i_k+1 Then proceed to step four; Step 4: Determine if the predicted trajectory point B is the (k+1)th time. i_k+1 With the k-th trajectory prediction point B i_k If the distance between them is less than or equal to a preset first distance threshold, then proceed to step six; otherwise, proceed to step five. Fifth step: Increment the iteration count k by 1, then return to execute the second step; Step 6: Calculate the (k+1)th trajectory prediction point B. i_k+1 With the (k+1)th candidate collision point A i_k+1 The distance between them is used as the closest distance S to the i-th obstacle point. i_dist Then proceed to step seven; Step 7: Determine if the closest distance S is specified. i_dist If the distance is less than or equal to the preset second distance threshold, proceed to step nine; otherwise, proceed to step eight. Step 8: Increase the danger coefficient x of the i-th obstacle point. i Set the value to 0, then end; Step 9: Predict the trajectory point B for the (k+1)th time. i_k+1 As the predicted collision point C corresponding to the i-th obstacle point i Then it ends.

10. The automatic parking speed planning method according to any one of claims 1 to 9, characterized in that, Methods for determining the closest point to the vehicle on the path include: Construct a path segment coordinate system based on each pair of adjacent path points in the parking planning path; Transform the vehicle's position coordinates to the coordinate system of all path segments, calculate their lateral distances, and compare to obtain the closest path segment; Project the vehicle's position coordinates onto the coordinate system of the nearest path segment to obtain the projection point; By transforming the projection points back to the original coordinate system, the closest point to the vehicle on the path is obtained.

11. An automatic parking speed planning system, comprising a processor and a memory connected to the processor, characterized in that: The memory stores a computer-readable program, which, when invoked by a processor, can execute the automatic parking speed planning method as described in any one of claims 1 to 10.

Images