Path planning method, device and storage medium

Abstract

The application discloses a path planning method, device and storage medium, the method comprises the following steps: constructing a target function based on an initial planning path, each penalty term affecting the quality of the path to be optimized and the weight parameters of each penalty term; optimizing the target function to obtain an optimized path corresponding to the initial planning path; based on the detection result of the optimized path, determining whether to adjust the weight parameters of each penalty term to re-execute the preceding steps. In the foregoing manner, the application can improve the success rate of path planning.

Description

Technical Field

[0001] This application relates to the field of autonomous driving, and in particular to a path planning method, device, and storage medium. Background Technology

[0002] Path planning is a crucial component of autonomous driving control algorithms, aiming to provide a smooth, collision-free trajectory that closely follows the navigation route for the control module to track. However, road conditions are complex and varied, especially in challenging scenarios such as narrow roads or road obstructions, which increases the difficulty of path planning and leads to a lower success rate for real-time path planning. Therefore, improving the success rate of path planning in various road scenarios is of great significance. Summary of the Invention

[0003] The main technical problem addressed by this application is to provide a path planning method, device, and storage medium that can improve the success rate of path planning.

[0004] To address the aforementioned technical problems, this application provides a path planning method, which includes: constructing an objective function based on an initial planned path, various penalty terms affecting the quality of the path to be optimized, and the weight parameters of each penalty term; optimizing the objective function to obtain an optimized path corresponding to the initial planned path; and determining whether to adjust the weight parameters of each penalty term based on the detection results of the optimized path, so as to re-execute the aforementioned steps.

[0005] The objective function is constructed based on the initial planned path, the penalty terms affecting the quality of the path to be optimized, and the weight parameters of each penalty term. This includes: constructing the penalty function corresponding to each penalty term based on the initial planned path, the weight parameters of each penalty term, and the constraints of each penalty term; and combining the penalty functions corresponding to each penalty term to construct the objective function.

[0006] The penalty terms affecting the quality of the path to be optimized include at least two of the following: a first penalty term regarding the distance between the path to be optimized and associated obstacles, a second penalty term regarding the offset distance between the path to be optimized and the initial planned path, a third penalty term regarding the vehicle kinematic constraints of the path to be optimized, a fourth penalty term regarding the minimum turning radius constraint of the path to be optimized, and a fifth penalty term regarding the curvature change rate constraint of the path to be optimized.

[0007] The initial planned path is the initial state of the path to be optimized, and the path to be optimized is the intermediate optimization state in the optimization of the initial planned path. The initial planned path includes several initial state points, and the path to be optimized includes several state points to be optimized. Based on the initial planned path, the weight parameters of each penalty term, and the constraints of each penalty term, a penalty function corresponding to each penalty term is constructed, including: in response to each penalty term including a first penalty term, a first penalty function for the first penalty term is constructed based on the first distance between each state point to be optimized and its corresponding associated obstacles, the first weight parameter of the first penalty term, and the first distance constraint; in response to each penalty term including a second penalty term, matching points for each initial state point are selected from each state point to be optimized, and a second penalty function is constructed based on the second distance between each initial state point and its corresponding matching point, the second weight parameter of the second penalty term, and the second distance constraint. The second penalty function; in response to each penalty term including the third penalty term, a third penalty function for the third penalty term is constructed based on the difference between the first state to be optimized of each two adjacent state points, the third weight parameter of the third penalty term, and the kinematic constraints; wherein, the first state to be optimized includes the vehicle heading angle and position, and the kinematic constraints include at least one of the velocity constraints and the front wheel steering angle constraints; in response to each penalty term including the fourth penalty term, a fourth penalty function for the third penalty term is constructed based on the difference between the second state to be optimized of each two adjacent state points, the fourth weight parameter of the fourth penalty term, and the maximum curvature constraint; wherein, the second state to be optimized includes the vehicle heading angle and position; in response to each penalty term including the fifth penalty term, a fifth penalty function for the fifth penalty term is constructed based on the rate of curvature change between three adjacent state points and the rate of curvature change constraint.

[0008] Specifically, for each state point to be optimized, the first distance includes sub-distances between the state point to be optimized and each associated obstacle. Based on the first distance between each state point to be optimized and its corresponding associated obstacles, the first weight parameter of the first penalty term, and the first distance constraint, a first penalty function of the first penalty term is constructed, including: for each state point to be optimized, obtaining the first comparison results between each sub-distance of the state point to be optimized and the cutoff distance and the expected minimum distance; based on the first comparison results and the first distance constraint, determining the first sub-penalty function between the state point to be optimized and each associated obstacle; weighting the first sub-penalty function between the state point to be optimized and each associated obstacle based on the first weight parameter and the first sub-penalty function between the state point to be optimized to obtain the first weighted penalty function of the state point to be optimized; and combining the first weighted penalty functions of each state point to be optimized to obtain the first penalty function of the first penalty term.

[0009] The first sub-penalty function is determined based on the first comparison result and the first distance constraint, between the state point to be optimized and each associated obstacle. This includes: in response to the first comparison result being that the sub-distance is greater than the cutoff distance, using a first constant as the first sub-penalty function; and in response to the first comparison result being that the sub-distance is not less than the expected minimum distance and not greater than the cutoff distance, using the first product of the α power of the first ratio and the β power of the expected minimum distance as the first sub-penalty function. The first ratio is the second product of the second ratio and the second constant, and the second ratio is the ratio of the first difference and the second difference. The first difference is the difference between the sub-distance and the cutoff distance, and the second difference is the difference between the expected minimum distance and the cutoff distance. In response to the first comparison result that the sub-distance is less than the expected minimum distance, the second product of the third difference raised to the power of α and the expected minimum distance raised to the power of β is used as the first sub-penalty function. The third difference is the difference between the third constant and the third ratio, the third ratio is the ratio of the third product to the expected minimum distance, and the third product is the product of the fourth constant and the sub-distance. The expected minimum distance is less than the cutoff distance. The first constant is less than the first product, and the first product is less than the second product.

[0010] Wherein, each associated obstacle of the state point to be optimized includes all first obstacles within a first distance range of the state point to be optimized, and the second obstacle closest to the state point to be optimized, wherein the second obstacle is an obstacle located between the first distance range and the second distance range of the state point to be optimized; and / or, the first weight parameter includes a first sub-weight set for each type of associated obstacle; the first weighted penalty function of the state point to be optimized is obtained by weighting based on the first weight parameter and the first sub-penalty function between the state point to be optimized and each associated obstacle, including: weighting based on the first sub-penalty function between the state point to be optimized and each associated obstacle and the first sub-weight of each associated obstacle to obtain the first weighted penalty function of the state point to be optimized.

[0011] The process of selecting matching points for each initial state point from among the various state points to be optimized includes: for each initial state point, selecting the state point closest to the initial state point as the matching point; and / or, constructing a second penalty function for the second penalty term based on the second distance between each initial state point and its corresponding matching point, the second weight parameter of the second penalty term, and the second distance constraint, including: for each initial state point, obtaining a second comparison result between the second distance and the cutoff distance between the initial state point and its corresponding matching point; determining a second sub-penalty function between the initial state point and its corresponding matching point based on the second comparison result and the second distance constraint; weighting the second sub-penalty function between each initial state point and its corresponding matching point based on the second weight parameter of the second penalty term and the second sub-penalty function between the initial state point and its corresponding matching point to obtain a second weighted penalty function for each matching point; and combining the second weighted penalty functions of each matching point to obtain the second penalty function for the second penalty term.

[0012] The second sub-penalty function is determined based on the second comparison result and the second distance constraint, including: in response to the second comparison result that the second distance is less than the cutoff distance, a first constant is determined as the second sub-penalty function; in response to the second comparison result that the second distance is not less than the cutoff distance, a fourth product is obtained between the fourth difference, the second distance, and the target result, and the fourth product is used as the second sub-penalty function; wherein, the fourth difference is the difference between the fifth constant and the fourth ratio, the fourth ratio is the ratio of the fifth product to the length of the initial planned path, the fifth product is the product of the fifth constant and the third distance, and the third distance is the distance between the initial state point and the starting point of the initial planned path; the target result is the ratio of the sixth constant to e raised to the power of n, where e represents the base of the natural logarithm, n is the sixth product of the seventh constant and the fifth difference, and the fifth difference is the difference between the largest of the cutoff curvature and the absolute value of the curvature of the initial state point, and the cutoff curvature; wherein, the first constant is less than the fourth product.

[0013] The process includes determining whether to adjust the weight parameters of each penalty item based on the detection results of the optimized path, in order to re-execute the aforementioned steps. This includes: responding to the detection result of the optimized path as a first detection result, using the optimized path as the target planning path; wherein, the first detection result indicates that each penalty item of the optimized path meets the corresponding preset requirements; responding to the detection result of the optimized path as a second detection result, adjusting the weight parameters of each penalty item based on the second detection result, using the adjusted weight parameters of each penalty item as the new weight parameters of each penalty item, and re-executing the objective function and subsequent steps based on the initial planning path, each penalty item affecting the quality of the path to be optimized, and the weight parameters of each penalty item, until the latest detection result of the optimized path is the first detection result, using the latest optimized path as the target planning path, or the number of times the first detection result is detected reaches a preset number, stopping path planning; wherein, the second detection result indicates that there are penalty items among the penalty items of the optimized path that do not meet the corresponding preset requirements.

[0014] The adjustment of the weight parameters of each penalty item includes increasing the weight parameter by a first preset coefficient or decreasing the weight parameter by a second preset coefficient, wherein the first preset coefficient is greater than the second preset coefficient.

[0015] Each penalty term includes at least two of the following: a first penalty term regarding the distance between the path to be optimized and associated obstacles; a second penalty term regarding the offset distance between the path to be optimized and the initially planned path; a third penalty term regarding the vehicle kinematic constraints of the path to be optimized; a fourth penalty term regarding the minimum turning radius constraint of the path to be optimized; and a fifth penalty term regarding the curvature change rate constraint of the path to be optimized. The weight parameters of each penalty term are adjusted based on the second detection result, including: in response to the second detection result indicating that the first penalty term does not meet the corresponding preset requirement, increasing the weight parameter of the first penalty term, and / or decreasing the weight parameter of the associated penalty term corresponding to the first penalty term. Weight parameters; the preset requirement for the first penalty item is that the optimized path does not collide with obstacles; in response to the second detection result that the fourth penalty item does not meet the corresponding preset requirement, the weight parameter of the fourth penalty item is increased, and / or the weight parameter of the associated penalty item corresponding to the fourth penalty item is decreased; the preset requirement for the fourth penalty item is that the maximum curvature of the optimized path is not greater than the curvature threshold; in response to the second detection result that the fifth penalty item does not meet the corresponding preset requirement, the weight parameter of the fifth penalty item is increased, and / or the weight parameter of the associated penalty item corresponding to the fifth penalty item is decreased; the preset requirement for the fifth penalty item is that the rate of change of curvature of the optimized path is not greater than the change threshold.

[0016] Wherein, the associated penalty item corresponding to the first penalty item includes the second penalty item; and / or, the associated penalty item corresponding to the fourth penalty item includes at least one of the second penalty item and the fifth penalty item; and / or, the associated penalty item corresponding to the fifth penalty item includes at least one of the first penalty item and the second penalty item.

[0017] The initial planning path is multiple. Before constructing the objective function based on the initial planning path, the penalty terms affecting the quality of the path to be optimized, and the weight parameters of each penalty term, the process includes: obtaining the navigation path line planned based on the current starting point and the target endpoint; dividing the navigation path line into multiple navigation path segments; for each navigation path segment, determining whether to use the navigation path segment as the corresponding initial planning path or the path obtained by searching based on the navigation path segment as the corresponding initial planning path based on the fourth distance between the navigation path segment and each obstacle; each initial planning path is the initial state of the corresponding path to be optimized, and the optimized path corresponding to each initial planning path is the final optimized state of the corresponding path to be optimized.

[0018] Specifically, based on the fourth distance between the navigation path segment and each obstacle, determining whether to use the navigation path segment as the corresponding initial planning path or the path obtained by searching based on the navigation path segment as the corresponding initial planning path includes: in response to each fourth distance being greater than a preset distance threshold, using the navigation path segment as the initial planning path; in response to the existence of a fourth distance among the fourth distances that is not greater than a preset distance threshold, using the path obtained by searching based on the navigation path segment as the initial planning path.

[0019] The process of obtaining the corresponding initial planned path based on the navigation path segment search includes: obtaining the target drivable boundary of the road where the navigation path segment is located; sampling within the target drivable area corresponding to the target drivable boundary based on the navigation path segment to obtain several sampling points; and performing path search based on each sampling point to obtain the corresponding search path, which is then used as the initial planned path.

[0020] The width of the drivable area is negatively correlated with the density of the sampling point distribution; and / or, based on the navigation path segment, sampling is performed within the drivable area corresponding to the target drivable boundary to obtain several sampling points, including: selecting several reference path points from the navigation path segment; for each reference path point, sampling is performed in the forward direction of the reference path point, and it is determined whether there are any failed sampling points in the forward direction of the reference path point; in response to the existence of failed sampling points in the forward direction of the reference path point, and based on the determination that passage is possible based on the forward drivable area, sampling is performed in the backward and forward directions of the reference path point respectively to obtain each sampling point.

[0021] The process of obtaining the target drivable boundary of the road where the navigation path segment is located includes: generating each initial drivable boundary based on the navigation path line; determining whether there is a first intersection between the normal of each target path point on the navigation path line and each target obstacle in the initial drivable area; the initial drivable area is the area between different initial drivable boundaries; determining the target boundary point corresponding to each target path point based on the determination result of the first intersection between the normal of each target path point and each target obstacle; and obtaining the target drivable boundary by combining the target boundary points of each target path point.

[0022] Specifically, based on the determination of the first intersection point between the normal of each target path point and each target obstacle, the target boundary point corresponding to each target path point is determined, including: for each target path point, in response to the absence of a first intersection point between the normal of the target path point and each target obstacle, the second intersection point between the normal and each initial drivable boundary is taken as the target boundary point of the target path point; in response to the presence of a first intersection point between the normal of the target path point and each target obstacle, a third comparison result is determined between the drivable space to the left and right of the target obstacle where the first intersection point is located and the preset drivable space, and the target boundary point of the target path point is determined based on the third comparison result.

[0023] Wherein, the target obstacle is a static obstacle; and / or, based on the third comparison result, the target boundary point of the target path point is determined, including: in response to the third comparison result that the passable space on each side is less than the preset passable space, determining that the target path point does not have a target boundary point; in response to the third comparison result that the passable space on each side is not less than the preset passable space, taking the second intersection point of the normal of the target path point and each initial drivable boundary as the target boundary point of the target path point; in response to the third comparison result that the passable space on one side is not less than the preset passable space, and the passable space on the other side is less than the preset passable space, taking the side on both sides that is not less than the preset passable space as the target side, and taking the first intersection point of the normal of the target path point and the target side as a target boundary point of the target path point.

[0024] To solve the above-mentioned technical problems, another technical solution adopted in this application is: to provide an electronic device, including a memory and a processor coupled to each other, wherein the memory stores program instructions; and the processor is used to execute the program instructions stored in the memory to implement the above-mentioned method.

[0025] To solve the above-mentioned technical problems, another technical solution adopted in this application is to provide a computer-readable storage medium for storing program instructions that can be executed to implement the above-mentioned method.

[0026] The above scheme, after optimizing the constructed objective function and obtaining the optimized path, further determines whether to adjust the weight parameters of each penalty term based on the detection results of the optimized path. A new objective function and a new optimized path are then constructed based on the adjusted weight parameters. Compared to not adjusting the weight parameters of each penalty term, this application's method of adjusting the weight parameters of each penalty term and re-planning the path when the optimized path detection result fails increases the chances of successful path planning, thereby improving the success rate of path planning. Attached Figure Description

[0027] Figure 1 This is a flowchart illustrating an embodiment of the path planning method provided in this application;

[0028] Figure 2 yes Figure 1 The flowchart of step S11 shown is a schematic diagram of one embodiment.

[0029] Figure 3 This is a flowchart illustrating an embodiment of constructing the first penalty function provided in this application;

[0030] Figure 4 This is a flowchart illustrating an embodiment of constructing the second penalty function provided in this application;

[0031] Figure 5 This is a flowchart illustrating an embodiment of obtaining each initial planned path provided in this application;

[0032] Figure 6 yes Figure 5 The flowchart of step S53 shown is a schematic diagram of an embodiment.

[0033] Figure 7 This is a schematic diagram of an embodiment of forward sampling provided in this application;

[0034] Figure 8 This is a schematic diagram of an embodiment of backward plus forward sampling provided in this application;

[0035] Figure 9 This is a schematic diagram of the framework of an embodiment of the path planning device provided in this application;

[0036] Figure 10 This is a schematic diagram of the framework of an embodiment of the electronic device provided in this application;

[0037] Figure 11 This is a schematic diagram of the framework of the computer-readable storage medium provided in this application. Detailed Implementation

[0038] To make the purpose, technical solution and effects of this application clearer and more explicit, the following describes this application in further detail with reference to the accompanying drawings and embodiments.

[0039] Furthermore, if the embodiments of this application involve descriptions such as "first" or "second," these descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, features defined with "first" or "second" may explicitly or implicitly include at least one of those features. Additionally, the technical solutions of various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. If the combination of technical solutions is contradictory or impossible to implement, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed in this application.

[0040] It should be noted that the path planning method of this application is applicable to any road scenario requiring path planning, which can be an indoor or outdoor scenario. It is applicable to the path planning of any mobile device, and even electronic devices carried by pedestrians (such as mobile phones, smartwatches, etc.). Mobile devices can be, but are not limited to, vehicles, and can also be robots, etc. For ease of description, the path planning method of this application will be illustrated using a vehicle as an example below.

[0041] In the path planning process of this application, the weight parameters of each penalty term involved in the construction of the objective function are adaptively adjustable. Specifically, after the optimized path is obtained, the weight parameters can be adaptively adjusted based on the quality detection results of the optimized path.

[0042] Please see Figure 1 , Figure 1 This is a flowchart illustrating an embodiment of the path planning method provided in this application. It should be noted that if substantially the same result is obtained, this embodiment does not necessarily reflect that result. Figure 1 The illustrated process sequence is limited. For example... Figure 1 As shown, this embodiment includes:

[0043] S11: Construct the objective function based on the initial planned path, the penalty terms that affect the quality of the path to be optimized, and the weight parameters of each penalty term.

[0044] In this embodiment, path optimization is performed based on the initial planned path. That is, the initial planned path is the initial state of path optimization (path to be optimized), while the path to be optimized is the intermediate optimization state of the initial planned path during optimization, and the optimized path corresponding to the initial planned path is the final optimization state of the corresponding path to be optimized.

[0045] In one embodiment, there are multiple initial planned paths, each of which is a segment of the entire path from the starting point to the target endpoint; that is, the entire path includes multiple initial planned paths. In this embodiment, the planning (including optimization) of each initial planned path segment is performed segment by segment, and the planning method for each initial planned path segment is the same. The specific method for obtaining each initial planned path segment can be found below. Figure 5 Description of the illustrated embodiment.

[0046] In one embodiment, step S11 includes: constructing a penalty function corresponding to each penalty term based on the initial planned path and the constraints of each penalty term; and weighting the penalty function corresponding to each penalty term based on the weight parameters of each penalty term to construct the objective function.

[0047] In another embodiment, considering that the weight parameters of each penalty term are the weight parameters of the entire path to be optimized with respect to each penalty term, and that the entire path contains multiple state points, in some cases, the degree of influence of state points in different states on the path quality is different. Therefore, in order to construct the penalty function of each penalty term based on the degree of influence of each state point on the path quality, the influence of the weight of each state point on the corresponding penalty term can be added during the construction of the penalty function corresponding to each penalty term. Specifically:

[0048] Please see Figure 2 , Figure 2 yes Figure 1 The flowchart of one embodiment of step S11 is shown. In this embodiment, step S11 further includes:

[0049] S21: Based on the initial planned path, the weight parameters of each penalty term, and the constraints of each penalty term, construct the penalty function corresponding to each penalty term.

[0050] In this embodiment, the initial planned path includes several initial state points, and the path to be optimized includes several state points to be optimized. Each state point is a path point containing some state information, such as position, heading angle or velocity, curvature, etc.

[0051] In some embodiments, the penalties affecting the quality of the path to be optimized include at least two of the following: a first penalty regarding the distance between the path to be optimized and associated obstacles, a second penalty regarding the offset distance between the path to be optimized and the initial planned path, a third penalty regarding the vehicle kinematic constraints of the path to be optimized, a fourth penalty regarding the minimum turning radius constraint of the path to be optimized, and a fifth penalty regarding the rate of curvature change constraint of the path to be optimized.

[0052] In one implementation scenario, to maintain a safe distance between the optimized path and obstacles, penalty terms can be set, including a first penalty term related to the distance between the path to be optimized and associated obstacles. In this implementation scenario, a first penalty function for the first penalty term can be constructed based on the first distance between each state point to be optimized and its corresponding associated obstacles, the first weight parameter of the first penalty term, and the first distance constraint. The specific method for constructing the first penalty function is detailed below. Figure 3 The relevant description of the illustrated embodiment.

[0053] In this implementation scenario, for each state point to be optimized, the first distance between the state point to be optimized and the corresponding associated obstacles includes the sub-distance between the state point to be optimized and each associated obstacle.

[0054] In one embodiment, the associated obstacles of the state point to be optimized include all first obstacles within a first distance range of the state point to be optimized, and the second obstacle closest to the state point to be optimized. The second obstacle is an obstacle located between the first distance range and the second distance range of the state point to be optimized. The first distance range is smaller than the second distance range. Specifically, the distance between the state point to be optimized and each obstacle can be determined. All obstacles within the first distance range are considered as associated obstacles of the state point to be optimized, and the obstacles located between the first and second distance ranges that are closest to the left and right sides of the state point to be optimized are considered as associated obstacles of the state point to be optimized.

[0055] In another embodiment, obstacles within the second distance range of the state point to be optimized can also be considered as associated obstacles of the state point to be optimized.

[0056] Please see Figure 3 , Figure 3 This is a flowchart illustrating an embodiment of constructing a first penalty function provided in this application. In this embodiment, constructing the first penalty function includes:

[0057] S31: For each state point to be optimized, obtain the first comparison results between each sub-distance of the state point to be optimized and the cutoff distance and the expected minimum distance.

[0058] S32: Based on the first comparison result and the first distance constraint, determine the first sub-penalty function between the state point to be optimized and each associated obstacle.

[0059] The first sub-penalty function between each state point to be optimized and each associated obstacle is expressed as follows:

[0060]

[0061] In the formula, e obs_ijLet d represent the first sub-penalty function between the state point i to be optimized and the associated obstacle j, and let d represent the sub-distance between the state point i to be optimized and the associated obstacle j. cut_off d represents the cutoff distance. min Let c1 and c2 represent the expected minimum distance, where c2 > c1, and α is an exponent used to control the rate of increase of the penalty, where α > 1. The expected minimum distance is less than the cutoff distance.

[0062] The first comparison result is d. ij With d cut_off and d min The comparison results between them.

[0063] Specifically, combining the above formula, based on the first comparison result and the first distance constraint, the first sub-penalty function between the state point to be optimized and each associated obstacle is determined, including the following three cases:

[0064] Case 1: If d ij >d cut_off If the distance between the state point to be optimized and the associated obstacle (the sub-distance mentioned above) is greater than the cutoff distance, then the first constant is used as the first sub-penalty function between the state point i to be optimized and the associated obstacle j. The first constant can be, but is not limited to, 0 in the above formula; it can be any constant representing a very small penalty value, such as 0.5 or 1.

[0065] Case 2: If d ij ≥d min If the distance between the state point to be optimized and the associated obstacle is not less than the expected minimum distance, then the first product of the first power of the first ratio and the first power of the expected minimum distance is used as the first sub-penalty function.

[0066] Wherein, the first ratio is the second product between the second ratio and the second constant, the second ratio is the ratio of the first difference and the second difference, the first difference is the difference between the first distance and the cutoff distance, and the second difference is the difference between the expected minimum distance and the cutoff distance.

[0067] Based on the above formula, the second ratio is The second constant is c1.

[0068] Case 3: If d ij <d minIf the distance between the state point to be optimized and the associated obstacle is less than the expected minimum distance, then the product of the third difference raised to the power of α and the expected minimum distance raised to the power of β is used as the first sub-penalty function. Here, the third difference is the difference between the third constant and the third ratio, the third ratio is the ratio of the third product to the expected minimum distance, and the third product is the product of the distance between the state point to be optimized and the associated obstacle, and the fourth constant, i.e., (c2-c1)*d in the above formula. ij .

[0069] Based on the above formula, the third difference is The third constant is c2.

[0070] Where α is greater than β, in a specific embodiment, as in the above formula, β is α-1.

[0071] Among them, the first constant is less than the first product, and the first product is less than the second product.

[0072] It should be noted that the first penalty term is set to ensure path safety. The ultimate goal is to maintain a moderate distance between the optimized path and obstacles. A larger penalty is applied when the path is very close to an obstacle, while a penalty of 0 is applied when the path is far from an obstacle, resulting in a very low penalty value. Therefore, in this embodiment, when d... ij >d cut_off When the distance between the path and the associated obstacle is relatively large, it is considered safe. Therefore, in this case, no penalty may be imposed on the associated obstacle, or a lower penalty may be imposed. min For a critical proximity, when d ij ≥d min At that time, with d ij As d increases, the safety factor increases, and the penalty value can be set to decrease exponentially; when d ij <d min The safety factor decreases, allowing the penalty value to increase rapidly. Here, c1 and c2 are two constant parameters used to control the penalty value at the critical distance, with c2 > c1; α is an exponent used to control the rate of increase of the penalty.

[0073] S33: The first weighted penalty function of the state point to be optimized is obtained by weighting the first weight parameter and the first sub-penalty function between the state point to be optimized and each associated obstacle.

[0074] In one embodiment, the weight parameter of the first penalty term (first weight parameter) includes a first sub-weight set for each type of associated obstacle, that is, each type of associated obstacle has a corresponding first sub-weight, wherein the first sub-weights of different types of associated obstacles may be the same or different.

[0075] To better adapt to road conditions and obtain a safer optimized trajectory, different first sub-weights can be set for different types of associated obstacles. For example, to ensure the safety of pedestrians on the road, a higher first sub-weight can be set for associated obstacles of the pedestrian type, while other types of associated obstacles can be set with a lower first sub-weight. The specific weights can be preset according to the actual possible road conditions.

[0076] In one embodiment, the first weighted penalty function for the state point to be optimized is expressed as follows:

[0077]

[0078] In the formula, f obs_i Let w represent the first weighted penalty function for the state point to be optimized. obs_j The first sub-weight of the associated obstacle j is represented by m in the formula, which represents the number of associated obstacles at the state point to be optimized.

[0079] In this embodiment, the first weighted penalty function of the state point to be optimized is obtained by weighting the first sub-penalty function between the state point to be optimized and each associated obstacle and the first sub-weight of each associated obstacle.

[0080] It should be noted that since there are at least two penalty terms, the goal is to make the penalty value of the objective function constructed based on each penalty term as close to 0 as possible. To avoid some penalty terms being negative and others being positive, which would cause errors in the objective function obtained by combining the penalty functions corresponding to each penalty term due to the interference of the positive and negative values ​​of different penalty terms, the squares of the above first sub-penalty functions can be used as new first sub-penalty functions. Then, based on the first weight parameter and the first sub-penalty functions between the state point to be optimized and each associated obstacle, a weighted penalty function is obtained for the state point to be optimized.

[0081] S34: Combine the first weighted penalty functions of each state point to be optimized to obtain the first penalty function of the first penalty term.

[0082] The first penalty function for the first penalty term is expressed as follows:

[0083]

[0084] In the formula, f obs Let f represent the first penalty function of the first penalty term. obs_i Let n represent the first weighted penalty function for the state point to be optimized, and n represent the number of state points to be optimized.

[0085] In one implementation scenario, to ensure the optimized path closely approximates the initial planned path, a second penalty term can be set for each penalty term, including a second penalty term related to the offset distance between the path to be optimized and the initial planned path. In this implementation scenario, matching points for each initial state point can be selected from the state points to be optimized. Then, based on the second distance between each initial state point and its corresponding matching point, the second weight parameter of the second penalty term, and the second distance constraint, a second penalty function for the second penalty term is constructed.

[0086] One common method for optimizing the offset distance between the path to be optimized and the initial planned path is to optimize all path points on the initial planned path. This optimization method increases the computational load and restricts the path's degrees of freedom. To solve this problem, a number of path points (initial state points) can be selected at certain intervals on the initial planned path. These selected initial state points and their corresponding matching points are then used to construct a second penalty function for subsequent optimization. The interval between points can be the same distance between any two points or different distances; the specific interval can be preset according to actual needs.

[0087] In one embodiment, after selecting a number of initial state points, for each initial state point, the state point to be optimized that is closest to the initial state point can be used as the matching point of the initial state point, so as to optimize the offset distance between each state point to be optimized and the corresponding initial state point in the subsequent optimization.

[0088] In this embodiment, please refer to Figure 4 , Figure 4 This is a flowchart illustrating an embodiment of constructing a second penalty function provided in this application. In this embodiment, constructing the second penalty function includes:

[0089] S41: For each initial state point, obtain the second comparison result between the second distance between the initial state point and the corresponding matching point and the cutoff distance.

[0090] S42: Based on the second comparison result and the second distance constraint, determine the second sub-penalty function between the initial state point and the corresponding matching point.

[0091] The second sub-penalty function between the initial state point and the corresponding matching point is expressed as follows:

[0092]

[0093] In the formula, e via_i The second sub-penalty function represents the distance between the initial state point and the corresponding matching point, ε represents the cutoff distance (which can be set according to actual needs, for example, ε < 0.1), and ρ represents the distance between the initial state point and the corresponding matching point (the second distance). cut_off To cut off curvature, ρvia_i Let be the curvature of the i-th initial state point, and s represent the length of the initial planned path. i c represents the distance between the i-th initial state point and the starting point of the initial planned path, where c is a constant.

[0094] Wherein, the curvature is less than the cutoff curvature ρ cut_off The initial state point can be considered as a straight line, without any correction.

[0095] Specifically, combining the above formula, and based on the second comparison result and the second distance constraint, the second sub-penalty function between the initial state point and the corresponding matching point is determined, including the following two cases:

[0096] Case 1: If d i If the distance between the initial state point i and its corresponding matching point (the second distance) is less than the cutoff distance, then the first constant is determined as the second sub-penalty function between the initial state point i and its corresponding matching point. The purpose of setting this penalty term is to optimize the final path to be as close as possible to the initial planned path. Therefore, when the distance between the initial state point i and its corresponding matching point is less than the cutoff distance, it indicates that the initial state point and its corresponding matching point are very close, which meets the expectation of setting this penalty term. In this case, the second penalty sub-function can be set to a small penalty value (the first constant). For example, the first constant can be, but is not limited to, 0 in the above formula, and can also be, for example, 0.5 or 1.

[0097] Scenario 2: If d i ≥ε, meaning the distance between the initial state point i and the corresponding matching point is not less than the truncation distance. The fourth product is obtained by combining the fourth difference, the second distance, and the target result, and is used as the second sub-penalty function. Here, the fourth difference is the difference between the fifth constant and the fourth ratio; the fourth ratio is the ratio of the fifth product to the initial planned path length; the fifth product is the product of the fifth constant and the third distance; and the third distance is the distance between the initial state point and the starting point of the initial planned path. The target result is the ratio of the sixth constant to e raised to the power of n, where e represents the base of the natural logarithm, and n is the sixth product of the seventh constant and the fifth difference. The fifth difference is the difference between the maximum of the truncation curvature and the absolute value of the curvature of the initial state point, and the truncation curvature itself.

[0098] Combining the above formula, the fourth product is:

[0099]

[0100] The fourth difference is The fifth constant is c, and the fourth ratio is The target result is Where n is -α(max(|ρ via_i |,ρ cut_off )-ρcut_off The seventh constant is α, and the sixth constant is 1.

[0101] The first constant is less than the fourth product, so that when the distance between the initial state point i and the corresponding matching point is greater than the cutoff distance, the penalty value of that point is increased.

[0102] It should be noted that the concept behind the second sub-penalty function is to ensure that the final optimized path closely approximates the initial planned path. However, the requirements for closeness to the initial planned path vary depending on the road environment. For example, on routes with high curvature, such as U-turns or right-angle bends, the presence of obstacles and the need to meet constraints such as the minimum turning radius of the vehicle mean that consistent penalty parameters may lead to optimization failure (e.g., curvature not meeting constraints, large curvature jumps, etc.). Furthermore, since path optimization is performed in real-time with a high update frequency, even significant deviations from the initial planned path at the far end of the optimized path have little impact on control. Therefore, the penalty function design must incorporate considerations of the curvature of the initial state point and the distance between the initial state point and the starting point of the initial planned path. This means allowing for larger deviations at high curvature points and at the far end in exchange for greater flexibility. For specific considerations, please refer to scenario two above.

[0103] S43: Based on the second weight parameter of the second penalty term and the second sub-penalty function between each initial state point and the corresponding matching point, the second weighted penalty function of each matching point is obtained by weighting.

[0104] In one embodiment, the second weighted penalty function for the matching point (the state point to be optimized) corresponding to each initial state point is expressed as follows:

[0105]

[0106] In the formula, f via_i ω represents the second weighted penalty function for the matching point (the state point to be optimized) corresponding to the initial state point i. via This represents the second weight parameter of the second penalty term.

[0107] It should be noted that since there are at least two penalty terms, the goal is to make the penalty value of the objective function constructed based on each penalty term as close to 0 as possible. To avoid some penalty terms being negative and others being positive, which could lead to errors in the objective function obtained by combining the penalty functions corresponding to each penalty term due to the interference of the positive and negative values ​​of different penalty terms, the above-mentioned method, when constructing the second weighted penalty function, uses the square of the second sub-penalty function between the initial state point and the corresponding matching point as the new first sub-penalty function. Then, based on the second weight parameter of the second penalty term and the second sub-penalty function between each initial state point and the corresponding matching point, a weighted average is performed to obtain the second weighted penalty function for each matching point.

[0108] S44: Combine the second weighted penalty function of each matching point to obtain the second penalty function of the second penalty term.

[0109] The second penalty function for the second penalty term is expressed as follows:

[0110]

[0111] In the formula, f via Let f represent the second penalty function of the second penalty term. via_i The second weighted penalty function represents the matching point (the state point to be optimized) corresponding to the initial state point i, and m represents the number of initial state points selected.

[0112] In one implementation scenario, to ensure the optimized path meets vehicle kinematic requirements, a third penalty term can be set for each penalty term, including a third penalty term relating to the vehicle kinematic constraints of the path to be optimized. In this implementation scenario, a third penalty function for the third penalty term can be constructed based on the difference between the first state to be optimized of each pair of adjacent state points, the third weight parameter of the third penalty term, and the kinematic constraints. The first state to be optimized includes the vehicle's heading angle and position, and the kinematic constraints include at least one of velocity constraints and front wheel steering angle constraints.

[0113] For example, this embodiment is based on a vehicle geometric kinematic model:

[0114]

[0115] Where v is the vehicle speed, L is the vehicle wheelbase, θ is the front wheel steering angle, and β is the vehicle heading angle. Discretization is performed: assuming the vehicle maintains a constant speed at each time interval and the steering angle between adjacent positions remains unchanged (corresponding to the constraint condition), this translates to the trajectory (path) being composed only of arcs and straight line segments between adjacent points. Correspondingly, in this implementation scenario, constructing the third penalty function for the third penalty term includes the following steps:

[0116] First, based on the difference between the first states of two adjacent states to be optimized (state i and state i+1) and the kinematic constraints, a third penalty function for state i is constructed, as follows:

[0117] e nh_i =(cos(β) i )+cos(β i+1 ))*(y i+1 -y i )-(sin(β i )+sin(β i+1 ))*(x i+1 -x i )

[0118] Among them, (x i y i Let ) represent the position of state point i to be optimized. Let β be the position of the state point i+1 to be optimized. i and β i+1 Let i and i+1 be the vehicle heading angles of the state points to be optimized, respectively.

[0119] Then, based on the third weight parameter of the third penalty term and the third penalty sub-function of the state point to be optimized, the third weighted penalty function of the non-complete kinematics of the state point to be optimized is obtained as follows:

[0120]

[0121] The weight parameters of the third penalty term include the sub-weights of each state point to be optimized with respect to the third penalty term, ω. nh_i This represents the sub-weight of the state point i to be optimized with respect to the third penalty term. The sub-weights of different state points i to be optimized can be the same or different, depending on the actual needs. Let represent the third penalty sub-function for the state point i to be optimized. Since there are at least two penalty terms, the goal is to make the penalty value of the objective function constructed based on each penalty term as close to 0 as possible. To avoid some penalty terms being negative and others positive, which could lead to errors in the final objective function obtained by combining the penalty functions corresponding to each penalty term due to interference from the positive and negative values ​​of different penalty terms, here we will... Update the third penalty function to the state point i to be optimized.

[0122] Finally, by combining the third weighted penalty functions of each state point to be optimized, the third penalty function of the third penalty term is obtained, as follows:

[0123]

[0124] In the formula, f nh Let f represent the third penalty function. ah_i Let represent the third weighted penalty function for the non-holistic kinematics of the state point i to be optimized, and n represent the number of state points to be optimized.

[0125] In one implementation scenario, to ensure the optimized path meets the minimum turning radius constraint, a fourth penalty term can be set for each penalty term, including one related to the minimum turning radius constraint of the path to be optimized. In this implementation scenario, the fourth penalty function of the third penalty term can be constructed based on the difference between the second state to be optimized of each pair of adjacent state points, the fourth weight parameter of the fourth penalty term, and the maximum curvature constraint; wherein, the second state to be optimized includes the vehicle heading angle and position.

[0126] There is a direct reciprocal relationship between the minimum turning radius and the maximum curvature; the minimum turning radius can be regarded as the maximum limit of curvature under certain conditions, that is, when the curvature reaches the maximum, the turning radius reaches the minimum.

[0127] Specifically, based on the difference between the second state to be optimized between each pair of adjacent state points, the fourth weight parameter of the fourth penalty term, and the maximum curvature constraint, the fourth penalty function of the fourth penalty term is constructed, including:

[0128] First, calculate the curvature of two consecutive state points in the trajectory to be optimized, as follows:

[0129]

[0130] Where, Δβ i =β i+1 -β i , (x i y i Let ) represent the position of state point i to be optimized. Let β be the position of the state point i+1 to be optimized. i and β i+1 Let i and i+1 be the vehicle heading angles of the state points to be optimized, respectively.

[0131] Then, the fourth penalty sub-function for the minimum turning radius (maximum curvature) of the state point i to be optimized is constructed, as follows:

[0132]

[0133] Where: ρ cut_off This represents the minimum permissible curvature. The curvature of the state point i to be optimized is allowed to be less than this minimum curvature. If it exceeds this minimum curvature, it is considered an unacceptable large curvature. Correspondingly, when ρ i <ρ cut_off When ρ is at this time, the function will maintain a small penalty to minimize the curvature of the optimized path, while preventing the curvature penalty term from becoming too large on curves, thus avoiding a significant impact on other optimization terms. i >ρ cut_off When the penalty value increases rapidly, the fourth penalty sub-function can keep the curvature of the optimized path within a reasonable range as much as possible; γ is a constant used to control the rate of increase of the entire penalty term, where 0 < γ < 1, or γ is greater than 1, and the specific value can be selected according to actual needs.

[0134] Furthermore, a fourth weighted penalty function is constructed for the state point i to be optimized, as follows:

[0135]

[0136] In the formula, the weight parameters of the fourth penalty term include the sub-weights of each state point to be optimized with respect to the fourth penalty term, ω. kappa_i This represents the sub-weight of the optimized state point i with respect to the fourth penalty term. The sub-weights of different optimized state points i can be the same or different, depending on the actual needs. This represents the square of the fourth penalty sub-function, and is the updated fourth penalty sub-function. For details on the reasons for the update, please refer to the descriptions of other penalty terms.

[0137] Finally, the fourth penalty function for the fourth penalty term is constructed as follows:

[0138]

[0139] In the formula, f kappa Let n represent the fourth penalty function, and n represent the number of state points to be optimized.

[0140] In one implementation scenario, to ensure the optimized path satisfies the curvature change rate constraint, a fifth penalty term can be set for each penalty term, including one related to the curvature change rate constraint of the path to be optimized. In this implementation scenario, the fifth penalty function of the fifth penalty term can be constructed based on the curvature change rate between three adjacent state points to be optimized, and the curvature change rate constraint.

[0141] First, the rate of change of curvature is calculated by taking three consecutive state points (i, i+1, and i+2) in the trajectory to be optimized. The calculation method is as follows:

[0142]

[0143] The purpose of constructing this penalty term is to make the optimized path as smooth as possible, correspondingly, to minimize the rate of curvature change of the entire path. Therefore, the fifth penalty sub-function for the rate of curvature change of the state point i to be optimized can be constructed as follows:

[0144] e kppa_diff_i =Δρ i

[0145] Furthermore, based on the weight parameters of the fifth penalty term, the fifth penalty sub-function is weighted to construct the fifth weighted penalty function for the rate of change of curvature of the state point i to be optimized, as follows:

[0146]

[0147] Where, ω kappa_diff_i The sub-weights represent the rate of change of curvature of the state point i to be optimized.

[0148] In this embodiment, the weight parameters of the fifth penalty term include the sub-weights corresponding to each state point i to be optimized, where ω kappa_diff_iThe location of the state point i to be optimized is related to the fact that, in general, there is a higher requirement for the smoothness of the path closer to the starting point of the path to be optimized. Therefore, the sub-weights corresponding to the state points to be optimized at the near end can be set to be greater than the sub-weights corresponding to the state points to be optimized at the far end.

[0149] Finally, the fifth penalty function for the fifth penalty term is constructed as follows:

[0150]

[0151] In the formula, f kappa_diff Let n represent the fifth penalty function, and n represent the number of state points to be optimized.

[0152] It should be noted that, in one implementation scenario, each of the aforementioned state points to be optimized can be considered a vertex, and each sub-penalty function corresponding to each state point can be considered an edge of that vertex, thus constructing a hypergraph. Subsequent optimization of the objective function is then performed based on this created hypergraph.

[0153] S22: Combine the penalty functions corresponding to each penalty term to construct the objective function.

[0154] In this embodiment, each penalty term includes at least two of the above five penalty terms. The objective function is constructed by combining the corresponding penalty functions. The specific penalty terms can be set according to actual needs.

[0155] In one specific embodiment, each penalty term includes each of the above five penalty terms, and the corresponding constructed objective function is represented as follows:

[0156] f(Q)=f obs +f via +f nh +f kappa +f kappa_diff

[0157] S12: Optimize the objective function to obtain the optimized path corresponding to the initial planned path.

[0158] The goal of optimization is to obtain the state corresponding to each state point to be optimized when the objective function value is minimized.

[0159] Existing open-source algorithms, such as the g2o optimizer, gradient descent, GM (Gauss-Newton) iterative method, or LM (Levenberg-Marquardt) algorithm, can be used to optimize the objective function and obtain the optimized path corresponding to the initial planned path.

[0160] S13: Based on the detection results of the optimized path, determine whether to adjust the weight parameters of each penalty item in order to re-execute the aforementioned steps.

[0161] In this embodiment, after obtaining the optimized path in step S12, the optimized path is subjected to validity (quality) detection to obtain the corresponding detection results. Specifically, the detection results include two possible results: a first detection result indicating that each penalty item in the optimized path meets the corresponding preset requirements, and a second detection result indicating that some penalty items in the optimized path do not meet the corresponding preset requirements.

[0162] If the detection result of the optimized path is the first detection result, it means that each penalty item meets the corresponding preset requirements, the quality of the optimized path is high, and the optimized path can be used as the target planning path.

[0163] For example, if the first penalty item meets the corresponding preset requirement, it means that the vehicle will not collide with other obstacles when traveling along the current optimized path; if the second penalty item meets the corresponding preset requirement, it means that the proximity between the optimized path and the initial planned path meets the requirement; if the third penalty item meets the corresponding preset requirement, it means that the optimized path meets the vehicle's kinematic constraints; if the fourth penalty item meets the corresponding preset requirement, it means that the optimized path meets the vehicle's minimum turning radius constraint; if the fifth penalty item meets the corresponding preset requirement, it means that the optimized path meets the corresponding rate of curvature change constraint.

[0164] If the detection result of the optimized path is the second detection result, it means that there are penalty items that do not meet the preset requirements. In this case, the weight parameters of each penalty item can be adjusted based on the specific second detection result. The adjusted weight parameters of each penalty item are used as the new weight parameters of each penalty item. The objective function and subsequent steps are then re-executed based on the initial planned path, the penalty items that affect the quality of the path to be optimized, and the weight parameters of each penalty item, until the detection result of the latest optimized path is the first detection result. The latest optimized path is then used as the target planned path, or the number of times the first detection result is detected reaches a preset number, at which point path planning stops.

[0165] The adjustment of the weight parameters for each penalty item includes either increasing the weight parameter by a first preset coefficient or decreasing the weight parameter by a second preset coefficient, where the first preset coefficient is greater than the second preset coefficient. In other words, the weight parameter of the penalty item to be adjusted is based on the previous weight parameter multiplied by the first preset coefficient to increase the weight, or multiplied by the second preset coefficient to decrease the weight. The first preset coefficient is greater than 1, and the second preset coefficient is less than 1.

[0166] In some embodiments, the weight parameters of each penalty term are adjusted based on the second detection result, including at least the following cases:

[0167] Case 1: If the second detection result indicates that the first penalty term regarding the distance between the path to be optimized and the associated obstacle does not meet the corresponding preset requirement (the optimized path collides with the associated obstacle, or the distance is less than the threshold), then the weight parameter of the first penalty term is increased, and / or the weight parameter of the associated penalty term corresponding to the first penalty term is decreased; the preset requirement of the first penalty term is that the optimized path does not collide with the obstacle, or the distance between the optimized path and the obstacle is greater than the corresponding distance threshold.

[0168] In summary, when the optimized path collides with associated obstacles or the distance is less than a distance threshold, the weight parameter of the first penalty term can be increased. This allows for greater attention to the first penalty term when optimizing the objective function constructed based on the new weight parameter, thus ensuring that the optimized path meets the preset requirements corresponding to the first penalty term. Alternatively, the weight parameter of the first penalty term can be indirectly increased by decreasing the weight parameter of its associated penalty terms, or simultaneously increasing the weight parameter of the first penalty term and decreasing the weight parameter of its associated penalty terms to further enhance the weight parameter of the first penalty term. This also improves the optimization effect of the optimized path with respect to the first penalty term.

[0169] Case 2: If the second detection result indicates that the fourth penalty term regarding the minimum turning radius constraint of the path to be optimized does not meet the corresponding preset requirement, then the weight parameter of the fourth penalty term is increased, and / or the weight parameter of the associated penalty term corresponding to the fourth penalty term is decreased; the preset requirement corresponding to the fourth penalty term is that the maximum curvature of the optimized path is not greater than the curvature threshold.

[0170] Similar to Case 1 above, the purpose of increasing the weight parameter of the fourth penalty term and / or decreasing the weight parameter of the corresponding associated penalty term is to pay more attention to the fourth penalty term when optimizing the objective function constructed based on the new weight parameters, so that the maximum curvature of the optimized path is not greater than the curvature threshold.

[0171] Case 3: If the second detection result indicates that the fifth penalty term regarding the curvature change rate constraint of the path to be optimized does not meet the corresponding preset requirements, then the weight parameter of the fifth penalty term is increased, and / or the weight parameter of the associated penalty term corresponding to the fifth penalty term is decreased; the preset requirement of the fifth penalty term is that the curvature change rate of the optimized path is not greater than the change threshold.

[0172] Similar to situations one and two above, the purpose of increasing the weight parameter of the fifth penalty term and / or decreasing the weight parameter of the corresponding associated penalty term is to: pay more attention to the fifth penalty term when optimizing the objective function constructed based on the new weight parameters, so that the rate of curvature change of the optimized path is not greater than the change threshold.

[0173] The associated penalty term corresponding to the first penalty term may include any one of the remaining penalty terms; preferably, the associated penalty term includes the second penalty term. That is, if the optimized path collides with an associated obstacle, or the distance is less than a threshold, the weight parameter of any one of the remaining penalty terms may be reduced, or only the weight parameter of the second penalty term regarding the offset distance between the path to be optimized and the initial planned path may be reduced.

[0174] The associated penalty term corresponding to the fourth penalty term may include any of the remaining penalty terms, preferably at least one of the second and fifth penalty terms. That is, when the maximum curvature of the optimized path is greater than the curvature threshold, the weight parameter of any of the remaining penalty terms may be reduced, or only the weight parameters of the second and / or fifth penalty terms may be reduced.

[0175] The associated penalty term corresponding to the fifth penalty term can include any of the remaining penalty terms, preferably at least one of the first and second penalty terms. That is, when the rate of change of curvature of the optimized path is greater than the change threshold, the weight parameter of any of the remaining penalty terms can be reduced, or only the weight parameters of the first and / or second penalty terms can be reduced.

[0176] The three scenarios described above are merely examples. In reality, regardless of whether each penalty item meets the corresponding preset requirements, the weight parameters of the penalty items that do not meet the preset requirements can be increased, and the weight parameters of other penalty items that meet the requirements can be decreased, so that the final optimized path meets the preset requirements of each penalty item.

[0177] The above scheme, after optimizing the constructed objective function and obtaining the optimized path, further determines whether to adjust the weight parameters of each penalty term based on the detection results of the optimized path. A new objective function and a new optimized path are then constructed based on the adjusted weight parameters. Compared to methods that do not adjust the weight parameters of each penalty term based on the detection results, this application's method of adjusting the weight parameters of each penalty term and re-planning the path when the optimized path detection result fails increases the chances of path planning and thus improves the success rate of path planning.

[0178] Please see Figure 5 , Figure 5 This is a flowchart illustrating an embodiment of obtaining initial planned paths provided in this application. In this embodiment, obtaining initial planned paths includes:

[0179] S51: Obtain the navigation path planned based on the current starting point and the target destination.

[0180] S52: Divide the navigation path into multiple navigation path segments.

[0181] In summary, after dividing the navigation path into multiple navigation path segments, this embodiment determines the initial planned path for each navigation path segment based on the distance between the navigation path segment and each obstacle, or the path obtained by searching based on the navigation path segment.

[0182] The navigation route is a route planned based on the vehicle's current starting point, destination, and map information. The starting point is the vehicle's current location, and the destination is the location the vehicle hopes to reach.

[0183] Considering the long path between the current starting point and the target destination, and that the path at the far end will not affect the vehicle's current movement, the navigation path is first divided into multiple navigation path segments before path optimization, and then path planning is performed segment by segment. The specific number of segments and the length of each navigation path segment can be determined based on practical experience.

[0184] S53: For each navigation path segment, based on the fourth distance between the navigation path segment and each obstacle, determine whether to use the navigation path segment as the corresponding initial planning path, or use the path obtained by searching based on the navigation path segment as the corresponding initial planning path.

[0185] In this embodiment, for each navigation path segment, if the fourth distance between the navigation path segment and each obstacle is greater than a preset distance threshold, then the navigation path segment is directly used as the initial planned path for that segment; conversely, if there is a fourth distance between the navigation path segment and each obstacle that is not greater than a preset distance threshold, then a path search is performed based on the navigation path segment, and the searched path is used as the initial planned path for that segment for downstream path optimization.

[0186] The preset distance threshold is a set distance used to determine whether there is a high risk of collision. If the fourth distance between the navigation path segment and each obstacle is not greater than the preset distance threshold, it is considered that the vehicle is highly likely to collide with the obstacle if it travels along the navigation path segment. If the fourth distance is greater than the preset distance threshold, it is considered that the probability of the vehicle colliding with the obstacle is low if it travels along the navigation path segment. In this case, the navigation path segment can be used as the initial planned path for further path optimization to obtain an optimized path.

[0187] In one embodiment, the initial planned path obtained by path search based on the navigation path segment is obtained within the drivable boundary of the road where the navigation path segment is located. Specifically:

[0188] Please see Figure 6 , Figure 6 yes Figure 5The flowchart shown is a schematic diagram of an embodiment of step S53. In this embodiment, the initial planned path is obtained based on navigation path segment search, including:

[0189] S61: Obtain the target drivable boundary of the road where the navigation path segment is located.

[0190] In this embodiment, each initial drivable boundary can be generated based on the navigation path line first. Then, it is determined whether there is a first intersection between the normal of each target path point on the navigation path line and each target obstacle in the initial drivable area. Based on the determination result of the first intersection between the normal of each target path point and each target obstacle, the target boundary point corresponding to each target path point is determined. Finally, the target drivable boundary is obtained by combining the target boundary points of each target path point.

[0191] Each initial drivable boundary is a boundary generated by offsetting a preset distance to the left and right sides of the navigation path. The initial drivable area is the region between different initial drivable boundaries.

[0192] For example, let the path points on the navigation path be represented as (x ref_i ,y ref_i ,β ref_i ), i = 0, 1, 2...n, left offset is L left The right offset is L right For each path point on the navigation path, a line segment is generated along the normal direction of the point based on the left and right offset distances (offsets). The two endpoints of the line segment are the boundary points on the initial drivable boundaries. The endpoints of the line segment are calculated as follows:

[0193] (x left_i ,y left_i )=(x ref_i -L left *sin(β ref_i ),y ref_i +L left *cos(β ref_i ))

[0194] (x right_i ,y right_i )=(x ref_i +L left *sin(β ref_i ),y ref_i -L left *cos(β ref_i ))

[0195] It should be noted that in this embodiment, the coordinates of each point (including path points, boundary points, intersection points, etc.) and each obstacle are expressed in a pre-established Cartesian coordinate system, so as to accurately describe the above-mentioned positions using the Cartesian coordinate system.

[0196] Furthermore, based on the determination of the first intersection point between the normal of each target path point and each target obstacle, the target boundary point corresponding to each target path point is determined, including the following cases:

[0197] Scenario 1: For each target path point, if there is no first intersection between the normal of the target path point and each target obstacle, the second intersection between the normal and each initial drivable boundary is taken as the target boundary point of the target path point. In practice, Scenario 1 means that if there is no first intersection, the boundary point on the initial drivable boundary corresponding to the target path point is taken as the target boundary point.

[0198] Case 2: If there is a first intersection between the normal of the target path point and each target obstacle, determine the third comparison result between the passable space on the left and right sides of the target obstacle where the first intersection point is located and the preset passable space. Based on the third comparison result, determine the target boundary point of the target path point.

[0199] If the third comparison result shows that the passable space to the left and right of the target obstacle where the first intersection point is located is smaller than the preset passable space, then it is determined that the target path point does not have a target boundary point. Alternatively, if the passable space to the left and right of the target obstacle where the first intersection point is located is smaller than the preset passable space, it means that the path ahead is impassable. In this case, it is determined that the target path point does not have a target boundary point, and the previously constructed boundary can be returned to.

[0200] If the third comparison result shows that the passable space on each side is not less than the preset passable space, the second intersection point of the normal of the target path point and each initial drivable boundary is taken as the target boundary point of the target path point. Correspondingly, this situation occurs when there is a first intersection point with the target obstacle, but the left and right sides of the target obstacle are still passable. In this case, the boundary point (second intersection point) corresponding to the target path point on each initial drivable boundary can be directly taken as the target boundary point of the target path point.

[0201] If the third comparison result shows that the passable space on one side is not less than the preset passable space, and the passable space on the other side is less than the preset passable space, the side with the passable space not less than the preset passable space is taken as the target side, and the first intersection point of the normal of the target path point and the target side is taken as a target boundary point of the target path point. Correspondingly, this situation occurs when there is a first intersection point with the target obstacle, but one side of the target obstacle (left or right) is passable while the other side is not. In this case, the passable side is taken as the target side, and the first intersection point of the normal of the target path point and the target side is taken as a target boundary point of the target path point. For example, if the right side of the target obstacle with the intersection point is passable, then the first intersection point with the right boundary (obstacle boundary) of the target obstacle is taken as a target boundary point (left boundary of the drivable area).

[0202] Among them, another target boundary point of the target path point can be further determined based on other target obstacles, but the specific determination logic is the same as the processing logic of the above-mentioned cases.

[0203] In some embodiments, considering that dynamic obstacles are in motion, have spatiotemporal misalignment, and can be avoided through optimization, only static obstacles are considered when constructing the target drivable boundary. That is, the target obstacles mentioned above are static obstacles, such as walls, pillars, railings on the road, or stationary vehicles.

[0204] S62: Based on the navigation path segment, sample within the target drivable area corresponding to the target drivable boundary to obtain several sampling points.

[0205] In one embodiment, random sampling can be performed within the target drivable area to obtain each sampling point.

[0206] In another embodiment, in order to increase the probability of vehicle passage, sampling can be performed based on the width of the drivable area. Specifically, the width of the drivable area is set to be negatively correlated with the density of the sampling point distribution. That is, if the drivable area is wider, the corresponding sampling points are sparser; if the drivable area is narrower, the corresponding sampling points are denser.

[0207] In one embodiment, forward sampling is preferred, and the search is performed based on the forward sampling points to obtain the corresponding search path.

[0208] In another implementation, considering that in some scenarios, such as large curvature or narrow passages, searching for forward sampling points may not find the corresponding path, i.e., the search fails, but in fact the forward space is passable, to avoid this situation, sampling can be performed in both the forward and backward directions to obtain each sampling point, so that a path that moves backward and forward can be found based on each sampling point.

[0209] Please refer to the following: Figure 7-8 , Figure 7 This is a schematic diagram of an embodiment of forward sampling provided in this application. Figure 8 This is a schematic diagram of an embodiment of backward plus forward sampling provided in this application. In one specific embodiment, sampling is performed within the drivable area corresponding to the target drivable boundary based on the navigation path segment to obtain several sampling points, including the following steps:

[0210] First, select several reference path points from the navigation path segment.

[0211] In one embodiment, several reference path points may be randomly selected from the navigation path segment.

[0212] In another embodiment, points can be taken at intervals along the navigation path segment to obtain several reference path points. The intervals between points can be the same distance between any two points, or they can be different distances; the specific intervals can be preset according to actual needs.

[0213] Second, for each reference path point, sample in the forward direction of the reference path point and determine whether there are any failed sampling points in the forward direction of the reference path point.

[0214] Third, in response to the existence of sampling points where the search failed in the forward direction of the reference path point, and based on the determination that the drivable area in the forward direction is passable, sampling is performed in the backward and forward directions of the reference path point to obtain each sampling point.

[0215] In this embodiment, forward sampling is prioritized, and the search is performed based on the sampled points. For each reference point, if the forward search of the reference path point is successful, the corresponding forward path is searched based on the forward sampled points. If the forward search fails, that is, if it is determined that there are sampled points with failed searches in the forward direction of the reference path point, and it is determined that the path is passable based on the drivable area in the forward direction, then sampling is performed in both the backward and forward directions of the reference path point to obtain each sampled point. For example, ... Figure 8 As shown, forward sampling is performed first, followed by backward sampling, to search for a path that combines backward and forward movement. This can be done using random sampling or sampling according to pre-defined rules. Figure 8 As shown, backward sampling uses a random sampling method, while forward sampling is performed according to a pre-set rule, so that the longitudinal difference of the forward sampling points is relatively small.

[0216] S63: Perform path search based on each sampling point to obtain the corresponding search path, and use it as the initial planned path.

[0217] In one embodiment, a directed graph can be established sequentially between each sampling layer. The nodes of the directed graph are connected by polynomial curves, such as Bézier curves, or cubic or quintic polynomials. After the directed graph is established, algorithms such as A* and dynamic programming can be called in the directed graph to search for the corresponding search path, which is then used as the initial planning path.

[0218] In one implementation, considering the validity of the polynomial curve degrees of freedom, minor collisions are permissible during path search, and these collisions are corrected by setting a penalty term for collisions during subsequent path optimization.

[0219] It should be noted that, based on actual road conditions, the above-mentioned sampling, directed graph construction and search methods can effectively balance the flexibility and accessibility of path generation. When the forward search is successful, a forward driving path can be planned. When the forward search fails (e.g., in congested road conditions), a path can be obtained by combining backward and forward sampling and search to reverse and then move forward, thus achieving the goal of getting out of trouble in congested road conditions.

[0220] It should also be noted that the above-mentioned method of using navigation path segments as the corresponding initial planning paths, or using paths obtained by searching based on the navigation path segments as the corresponding initial planning paths, may result in different initial planning path segments having some path segments that are navigation path segments and others that are search path segments obtained by searching based on the navigation path segments. In this case, there may be breakpoints between different initial planning paths. In this case, two adjacent initial planning path segments can be spliced ​​together.

[0221] Please see Figure 9 , Figure 9 This is a schematic diagram of a framework of an embodiment of the path planning device provided in this application. In this embodiment, the path planning device 90 includes a function construction module 91, an optimization module 92, and an adjustment determination module 93. The function construction module 91 is used to construct an objective function based on the initial planned path, each penalty term affecting the quality of the path to be optimized, and the weight parameters of each penalty term; the optimization module 92 is used to optimize the objective function to obtain the optimized path corresponding to the initial planned path; the adjustment determination module 93 is used to determine whether to adjust the weight parameters of each penalty term based on the detection results of the optimized path, so as to re-execute the aforementioned steps.

[0222] In some embodiments, the function construction module 91 constructs an objective function based on the initial planned path, each penalty term affecting the quality of the path to be optimized, and the weight parameters of each penalty term, including: constructing a penalty function corresponding to each penalty term based on the initial planned path, the weight parameters of each penalty term, and the constraints of each penalty term; and combining the penalty functions corresponding to each penalty term to construct the objective function.

[0223] In some embodiments, the penalties affecting the quality of the path to be optimized include at least two of the following: a first penalty regarding the distance between the path to be optimized and associated obstacles, a second penalty regarding the offset distance between the path to be optimized and the initial planned path, a third penalty regarding the vehicle kinematic constraints of the path to be optimized, a fourth penalty regarding the minimum turning radius constraint of the path to be optimized, and a fifth penalty regarding the rate of curvature change constraint of the path to be optimized.

[0224] In some embodiments, the initial planned path is the initial state of the path to be optimized, and the path to be optimized is an intermediate optimized state in the optimization of the initial planned path. The initial planned path includes several initial state points, and the path to be optimized includes several state points to be optimized. Based on the initial planned path, the weight parameters of each penalty term, and the constraints of each penalty term, a penalty function corresponding to each penalty term is constructed, including: in response to each penalty term including a first penalty term, constructing a first penalty function for the first penalty term based on a first distance between each state point to be optimized and its corresponding associated obstacles, a first weight parameter of the first penalty term, and a first distance constraint; in response to each penalty term including a second penalty term, selecting matching points for each initial state point from each state point to be optimized, and constructing a second penalty function based on a second distance between each initial state point and its corresponding matching point, a second weight parameter of the second penalty term, and a second distance constraint. The second penalty function for each penalty term; in response to each penalty term including the third penalty term, a third penalty function for the third penalty term is constructed based on the difference between the first state to be optimized of each two adjacent state points, the third weight parameter of the third penalty term, and kinematic constraints; wherein, the first state to be optimized includes the vehicle heading angle and position, and the kinematic constraints include at least one of speed constraints and front wheel steering angle constraints; in response to each penalty term including the fourth penalty term, a fourth penalty function for the third penalty term is constructed based on the difference between the second state to be optimized of each two adjacent state points, the fourth weight parameter of the fourth penalty term, and the maximum curvature constraint; wherein, the second state to be optimized includes the vehicle heading angle and position; in response to each penalty term including the fifth penalty term, a fifth penalty function for the fifth penalty term is constructed based on the rate of curvature change between three adjacent state points and the rate of curvature change constraint.

[0225] In some embodiments, for each state point to be optimized, the first distance includes sub-distances between the state point to be optimized and each associated obstacle; based on the first distance between each state point to be optimized and the corresponding associated obstacles, the first weight parameter of the first penalty term, and the first distance constraint, a first penalty function of the first penalty term is constructed, including: for each state point to be optimized, obtaining a first comparison result between each sub-distance of the state point to be optimized and the cutoff distance and the expected minimum distance; based on the first comparison result and the first distance constraint, determining the first sub-penalty function between the state point to be optimized and each associated obstacle; weighting the first sub-penalty function between the state point to be optimized and each associated obstacle based on the first weight parameter and the first sub-penalty function between the state point to be optimized and obtaining a first weighted penalty function for the state point to be optimized; and combining the first weighted penalty functions of each state point to be optimized to obtain the first penalty function of the first penalty term.

[0226] In some embodiments, based on a first comparison result and a first distance constraint, determining a first sub-penalty function between the state point to be optimized and each associated obstacle includes: in response to the first comparison result being that the sub-distance is greater than the cutoff distance, using a first constant as the first sub-penalty function; in response to the first comparison result being that the sub-distance is not less than the expected minimum distance and not greater than the cutoff distance, using the first product of the α power of the first ratio and the β power of the expected minimum distance as the first sub-penalty function; wherein the first ratio is the second product between the second ratio and the second constant, and the second ratio is the first difference and the second difference. The ratios are as follows: the first difference is the difference between the sub-distance and the cutoff distance; the second difference is the difference between the expected minimum distance and the cutoff distance; in response to the first comparison result being that the sub-distance is less than the expected minimum distance, the second product of the third difference raised to the power of α and the expected minimum distance raised to the power of β is used as the first sub-penalty function; where the third difference is the difference between the third constant and the third ratio, the third ratio is the ratio of the third product to the expected minimum distance, and the third product is the product of the fourth constant and the sub-distance; where the expected minimum distance is less than the cutoff distance; the first constant is less than the first product, and the first product is less than the second product.

[0227] In some embodiments, each associated obstacle of the state point to be optimized includes all first obstacles within a first distance range of the state point to be optimized, and the second obstacle closest to the state point to be optimized, wherein the second obstacle is an obstacle located between the first distance range and the second distance range of the state point to be optimized; and / or, the first weight parameter includes a first sub-weight set for each type of associated obstacle; and a first weighted penalty function for the state point to be optimized is obtained by weighting based on the first weight parameter and the first sub-penalty function between the state point to be optimized and each associated obstacle, including: weighting based on the first sub-penalty function between the state point to be optimized and each associated obstacle and the first sub-weight of each associated obstacle to obtain the first weighted penalty function for the state point to be optimized.

[0228] In some embodiments, selecting a matching point for each initial state point from each state point to be optimized includes: for each initial state point, selecting the state point to be optimized closest to the initial state point as the matching point of the initial state point; and / or, constructing a second penalty function for the second penalty term based on the second distance between each initial state point and the corresponding matching point, the second weight parameter of the second penalty term, and the second distance constraint, including: for each initial state point, obtaining a second comparison result between the second distance between the initial state point and the corresponding matching point and the cutoff distance; determining a second sub-penalty function between the initial state point and the corresponding matching point based on the second comparison result and the second distance constraint; weighting the second sub-penalty function between each initial state point and the corresponding matching point based on the second weight parameter of the second penalty term and the second sub-penalty function between the initial state point and the corresponding matching point to obtain a second weighted penalty function for each matching point; and combining the second weighted penalty functions of each matching point to obtain the second penalty function for the second penalty term.

[0229] In some embodiments, determining a second sub-penalty function between the initial state point and the corresponding matching point based on the second comparison result and the second distance constraint includes: determining a first constant as the second sub-penalty function in response to the second comparison result being that the second distance is less than the truncation distance; obtaining a fourth product between the fourth difference, the second distance, and the target result in response to the second comparison result being that the second distance is not less than the truncation distance, and using the fourth product as the second sub-penalty function; wherein the fourth difference is the difference between the fifth constant and the fourth ratio, the fourth ratio is the ratio of the fifth product to the length of the initial planned path, the fifth product is the product of the fifth constant and the third distance, and the third distance is the distance between the initial state point and the starting point of the initial planned path; the target result is the ratio of the sixth constant to e raised to the power of n, where e represents the base of the natural logarithm, n is the sixth product of the seventh constant and the fifth difference, and the fifth difference is the difference between the largest of the truncation curvature and the absolute value of the curvature of the initial state point, and the truncation curvature; wherein the first constant is less than the fourth product.

[0230] In some embodiments, the adjustment determining module 93 determines whether to adjust the weight parameters of each penalty item based on the detection result of the optimized path to re-execute the aforementioned steps, including: responding to the detection result of the optimized path being a first detection result, using the optimized path as the target planning path; wherein, the first detection result indicates that each penalty item of the optimized path meets the corresponding preset requirements; responding to the detection result of the optimized path being a second detection result, adjusting the weight parameters of each penalty item based on the second detection result, using the adjusted weight parameters of each penalty item as the new weight parameters of each penalty item, and re-executing the objective function based on the initial planning path, each penalty item affecting the quality of the path to be optimized, and the weight parameters of each penalty item, and subsequent steps, until the detection result of the latest optimized path is the first detection result, using the latest optimized path as the target planning path, or the number of times the first detection result is detected reaches a preset number, stopping path planning; wherein, the second detection result indicates that there are penalty items among the penalty items of the optimized path that do not meet the corresponding preset requirements.

[0231] In some embodiments, adjusting the weight parameters of each penalty item includes increasing the weight parameter by a first preset coefficient or decreasing the weight parameter by a second preset coefficient, wherein the first preset coefficient is greater than the second preset coefficient.

[0232] In some embodiments, each penalty term includes at least two of the following: a first penalty term regarding the distance between the path to be optimized and associated obstacles; a second penalty term regarding the offset distance between the path to be optimized and the initial planned path; a third penalty term regarding vehicle kinematic constraints of the path to be optimized; a fourth penalty term regarding the minimum turning radius constraint of the path to be optimized; and a fifth penalty term regarding the rate of curvature change constraint of the path to be optimized. Adjusting the weight parameters of each penalty term based on the second detection result includes: in response to the second detection result indicating that the first penalty term does not meet the corresponding preset requirement, increasing the weight parameter of the first penalty term, and / or decreasing the associated penalty term corresponding to the first penalty term. The weight parameters of the penalty items; the preset requirement for the first penalty item is that the optimized path does not collide with obstacles; in response to the second detection result that the fourth penalty item does not meet the corresponding preset requirement, the weight parameter of the fourth penalty item is increased, and / or the weight parameter of the associated penalty item corresponding to the fourth penalty item is decreased; the preset requirement for the fourth penalty item is that the maximum curvature of the optimized path is not greater than the curvature threshold; in response to the second detection result that the fifth penalty item does not meet the corresponding preset requirement, the weight parameter of the fifth penalty item is increased, and / or the weight parameter of the associated penalty item corresponding to the fifth penalty item is decreased; the preset requirement for the fifth penalty item is that the rate of change of curvature of the optimized path is not greater than the change threshold.

[0233] In some embodiments, the associated penalty item corresponding to the first penalty item includes the second penalty item; and / or, the associated penalty item corresponding to the fourth penalty item includes at least one of the second penalty item and the fifth penalty item; and / or, the associated penalty item corresponding to the fifth penalty item includes at least one of the first penalty item and the second penalty item.

[0234] In some embodiments, there are multiple initial planned paths. Before the function construction module 91 constructs the objective function based on the initial planned paths, the penalty terms affecting the quality of the path to be optimized, and the weight parameters of each penalty term, the method further includes: obtaining a navigation path line planned based on the current starting point and the target endpoint; dividing the navigation path line into multiple navigation path segments; for each navigation path segment, determining whether to use the navigation path segment as the corresponding initial planned path or the path obtained by searching based on the navigation path segment as the corresponding initial planned path based on the fourth distance between the navigation path segment and each obstacle; each initial planned path is the initial state of the corresponding path to be optimized, and the optimized path corresponding to each initial planned path is the final optimized state of the corresponding path to be optimized.

[0235] In some embodiments, determining the navigation path segment as the corresponding initial planning path, or the path obtained by searching based on the navigation path segment, based on the fourth distance between the navigation path segment and each obstacle, includes: in response to each fourth distance being greater than a preset distance threshold, using the navigation path segment as the initial planning path; in response to the existence of a fourth distance among the fourth distances that is not greater than the preset distance threshold, using the path obtained by searching based on the navigation path segment as the initial planning path.

[0236] In some embodiments, obtaining the corresponding initial planned path based on navigation path segment search includes: obtaining the target drivable boundary of the road where the navigation path segment is located; sampling within the target drivable area corresponding to the target drivable boundary based on the navigation path segment to obtain a number of sampling points; and performing path search based on each sampling point to obtain the corresponding search path, which is then used as the initial planned path.

[0237] In some embodiments, the width of the drivable area is negatively correlated with the density of the sampling point distribution; and / or, sampling is performed within the drivable area corresponding to the target drivable boundary based on the navigation path segment to obtain several sampling points, including: selecting several reference path points from the navigation path segment; for each reference path point, sampling is performed in the forward direction of the reference path point, and it is determined whether there are any failed sampling points in the forward direction of the reference path point; in response to the existence of failed sampling points in the forward direction of the reference path point, and based on the determination that passage is possible based on the forward drivable area, sampling is performed in the backward direction and forward direction of the reference path point respectively to obtain each sampling point.

[0238] In some embodiments, obtaining the target drivable boundary of the road where the navigation path segment is located includes: generating each initial drivable boundary based on the navigation path line; determining whether there is a first intersection between the normal of each target path point on the navigation path line and each target obstacle in the initial drivable area; the initial drivable area is the area between different initial drivable boundaries; determining the target boundary point corresponding to each target path point based on the determination result of the first intersection between the normal of each target path point and each target obstacle; and combining the target boundary points of each target path point to obtain the target drivable boundary.

[0239] In some embodiments, determining the target boundary point corresponding to each target path point based on the determination result of the first intersection point between the normal of each target path point and each target obstacle includes: for each target path point, in response to the absence of a first intersection point between the normal of the target path point and each target obstacle, taking the second intersection point between the normal and each initial drivable boundary as the target boundary point of the target path point; in response to the presence of a first intersection point between the normal of the target path point and each target obstacle, determining the third comparison result between the drivable space to the left and right of the target obstacle where the first intersection point is located and the preset drivable space, and determining the target boundary point of the target path point based on the third comparison result.

[0240] In some embodiments, the target obstacle is a static obstacle; and / or, based on a third comparison result, determining the target boundary point of the target path point includes: in response to the third comparison result being that the passable space on each side is less than a preset passable space, determining that the target path point does not have a target boundary point; in response to the third comparison result being that the passable space on each side is not less than the preset passable space, taking the second intersection point of the normal of the target path point and each initial drivable boundary as the target boundary point of the target path point; in response to the third comparison result being that the passable space on one side is not less than the preset passable space, and the passable space on the other side is less than the preset passable space, taking the side on both sides that is not less than the preset passable space as the target side, and taking the first intersection point of the normal of the target path point and the target side as a target boundary point of the target path point.

[0241] Please see Figure 10 , Figure 10 This is a schematic diagram of a framework of an embodiment of the electronic device provided in this application. In this embodiment, the electronic device 100 includes a memory 101 and a processor 102 coupled to each other.

[0242] The memory 101 stores program instructions, and the processor 102 executes the program instructions stored in the memory 101 to implement the steps of any of the above-described method implementations. In a specific implementation scenario, the electronic device 100 may include, but is not limited to, a microcomputer or a server. In addition, the electronic device 100 may also include mobile devices such as laptops and tablets, which are not limited here.

[0243] Specifically, processor 102 controls itself and memory 101 to implement the steps of any of the above embodiments. Processor 102 may also be referred to as a CPU (Central Processing Unit). Processor 102 may be an integrated circuit chip with signal processing capabilities. Processor 102 may also be a general-purpose processor, digital signal processor (DSP), application-specific integrated circuit (ASIC), field-programmable gate array (FPGA), or other programmable logic device, discrete gate or transistor logic device, or discrete hardware component. A general-purpose processor may be a microprocessor or any conventional processor. Furthermore, processor 102 may be implemented using integrated circuit chips.

[0244] Please see Figure 11 , Figure 11 This is a schematic diagram of the framework of the computer-readable storage medium provided in this application. The computer-readable storage medium 110 of this application embodiment stores program instructions 111, which, when executed, implement the methods provided in any embodiment or any non-conflicting combination of the above-described methods. The program instructions 111 can form a program file and be stored in the computer-readable storage medium 110 in the form of a software product, so that a computer device (which may be a personal computer, server, or network device, etc.) can execute all or part of the steps of the methods of various embodiments of this application. The aforementioned computer-readable storage medium 110 includes various media capable of storing program code, such as a USB flash drive, portable hard drive, read-only memory (ROM), random access memory (RAM), magnetic disk, or optical disk, or terminal devices such as computers, servers, mobile phones, and tablets.

[0245] The above scheme, after optimizing the constructed objective function and obtaining the optimized path, further determines whether to adjust the weight parameters of each penalty term based on the detection results of the optimized path. A new objective function and a new optimized path are then constructed based on the adjusted weight parameters. Compared to methods that do not adjust the weight parameters of each penalty term based on the detection results, this application's method of adjusting the weight parameters of each penalty term and re-planning the path when the optimized path detection result fails increases the chances of path planning and thus improves the success rate of path planning.

[0246] In some embodiments, the functions or modules of the apparatus provided in this disclosure can be used to perform the methods described in the above method embodiments. The specific implementation can be referred to the description of the above method embodiments, and for the sake of brevity, it will not be repeated here.

[0247] The description of the various embodiments above tends to emphasize the differences between the various embodiments. The similarities or similarities between them can be referred to, and for the sake of brevity, they will not be repeated here.

[0248] In the several embodiments provided in this application, it should be understood that the disclosed methods and apparatus can be implemented in other ways. For example, the apparatus implementations described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

[0249] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment, depending on actual needs.

[0250] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0251] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) or processor to execute all or part of the steps of the methods of various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0252] The above description is merely an embodiment of this application and does not limit the patent scope of this application. Any equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A path planning method characterized by, The method comprises: constructing a target function based on the initial planning path, each penalty term affecting the quality of the path to be optimized, and a weight parameter of each penalty term; each penalty term comprises at least two of the following: a first penalty term related to the distance between the path to be optimized and the associated obstacle, a second penalty term related to the offset distance between the path to be optimized and the initial planning path, a third penalty term related to the vehicle kinematics constraint of the path to be optimized, a fourth penalty term related to the minimum turning radius constraint of the path to be optimized, and a fifth penalty term related to the curvature rate constraint of the path to be optimized; optimizing the target function to obtain an optimized path corresponding to the initial planning path; based on the detection result of the optimized path, determining whether to adjust the weight parameter of each penalty term to re-execute the foregoing steps; the initial planning path is the initial state of the path to be optimized, the path to be optimized is the intermediate optimization state in the initial planning path optimization, the initial planning path comprises a plurality of initial state points, and the path to be optimized comprises a plurality of state points to be optimized; wherein the construction of the target function comprises: constructing a penalty function of each penalty term based on the initial planning path, the weight parameter of each penalty term, and the constraint condition of each penalty term; wherein, in response to each penalty term comprising the first penalty term, constructing a first penalty function of the first penalty term based on the first distance between each state point to be optimized and the corresponding plurality of associated obstacles, the first weight parameter of the first penalty term, and the first distance constraint condition; in response to each penalty term comprising the second penalty term, selecting a matching point of each initial state point from each state point to be optimized, constructing a second penalty function of the second penalty term based on the second distance between each initial state point and the corresponding matching point, the second weight parameter of the second penalty term, and the second distance constraint condition; in response to each penalty term comprising the third penalty term, constructing a third penalty function of the third penalty term based on the difference between the first state to be optimized of each two adjacent state points to be optimized, the third weight parameter of the third penalty term, and the kinematics constraint condition; wherein the first state to be optimized comprises at least one of the vehicle heading angle and the position, and the kinematics constraint comprises at least one of the speed constraint and the front wheel steering angle constraint; in response to each penalty term comprising the fourth penalty term, constructing a fourth penalty function of the third penalty term based on the difference between the second state to be optimized of each two adjacent state points to be optimized, the fourth weight parameter of the fourth penalty term, and the maximum curvature constraint condition; wherein the second state to be optimized comprises the vehicle heading angle and the position; in response to each penalty term comprising the fifth penalty term, constructing a fifth penalty function of the fifth penalty term based on the curvature rate between the three adjacent state points to be optimized, and the curvature rate constraint condition; comprehensively constructing the target function based on the penalty function corresponding to each penalty term.

2. The method of claim 1, wherein, For each state point to be optimized, the first distance comprises a sub-distance between the state point to be optimized and each associated obstacle; constructing the first penalty function of the first penalty term based on the first distance between each state point to be optimized and the corresponding plurality of associated obstacles, the first weight parameter of the first penalty term, and the first distance constraint condition, comprises: For each to-be-optimized state point, a first comparison result between each sub-distance of the to-be-optimized state point and a cutoff distance and an expected minimum distance is obtained; based on the first comparison result and the first distance constraint condition, a first sub-penalty function between the to-be-optimized state point and each associated obstacle is determined; based on the first weight parameter and the first sub-penalty function between the to-be-optimized state point and each associated obstacle, a first weighted penalty function of the to-be-optimized state point is obtained; comprehensive first weighted penalty function of each to-be-optimized state point, the first penalty function of the first penalty term is obtained.

3. The method of claim 2, wherein, The first penalty function of the first penalty term is obtained by combining the first weighted penalty function of each to-be-optimized state point. The first penalty function of the first penalty term is obtained by combining the first weighted penalty function of each to-be-optimized state point. In response to the first comparison result being that the sub-distance is greater than the cutoff distance, a first constant is taken as the first sub-penalty function; In response to the first comparison result being that the sub-distance is not less than the expected minimum distance and not greater than the cutoff distance, a first product of a first ratio raised to the power of a and the expected minimum distance raised to the power of b is taken as the first sub-penalty function; wherein the first ratio is a second product between a second ratio and a second constant, the second ratio is a ratio of a first difference and a second difference, the first difference is a difference between the sub-distance and the cutoff distance, and the second difference is a difference between the expected minimum distance and the cutoff distance; In response to the first comparison result being that the sub-distance is less than the expected minimum distance, a second product of a third difference raised to the power of a and the expected minimum distance raised to the power of b is taken as the first sub-penalty function; wherein the third difference is a difference between a third constant and a third ratio, the third ratio is a ratio of a third product and the expected minimum distance, and the third product is a product of a fourth constant and the sub-distance.

4. The method of claim 2, wherein, The expected minimum distance is less than the cutoff distance; the first constant is less than the first product, and the first product is less than the second product. The associated obstacles of the to-be-optimized state point include all first obstacles within a first distance range of the to-be-optimized state point, and a second obstacle closest to the to-be-optimized state point, the second obstacle being an obstacle located between the first distance range and a second distance range of the to-be-optimized state point; And / or, the first weight parameter includes a first sub-weight respectively set for each type of associated obstacle; and the weighting based on the first weight parameter and the first sub-penalty function between the to-be-optimized state point and each associated obstacle to obtain the first weighted penalty function of the to-be-optimized state point includes:

5. The method of claim 1, wherein, The first weighted penalty function of the to-be-optimized state point is obtained by weighting the first sub-penalty function between the to-be-optimized state point and each associated obstacle and the first sub-weight of each associated obstacle. The matching point of each initial state point is selected from each to-be-optimized state point, including: For each initial state point, a to-be-optimized state point closest to the initial state point among each to-be-optimized state point is taken as the matching point of the initial state point. And / or, based on the second distance between each initial state point and the corresponding matching point, the second weight parameter of the second penalty term, and the second distance constraint condition, a second penalty function of the second penalty term is constructed, including: For each initial state point, a second comparison result between the second distance between the initial state point and the corresponding matching point and the truncation distance is obtained; Based on the second comparison result and the second distance constraint condition, a second sub-penalty function between the initial state point and the corresponding matching point is determined; Based on the second weight parameter of the second penalty term and the second sub-penalty function between each initial state point and the corresponding matching point, a second weighted penalty function of each matching point is obtained; The second weighted penalty function of each matching point is integrated to obtain the second penalty function of the second penalty term.

6. The method of claim 5, wherein, The second sub-penalty function between the initial state point and the corresponding matching point is determined based on the second comparison result and the second distance constraint condition, including: In response to the second comparison result being that the second distance is less than the truncation distance, a first constant is determined as the second sub-penalty function; In response to the second comparison result being that the second distance is not less than the truncation distance, a fourth difference value, a fourth product between the second distance and a target result are obtained, and the fourth product is taken as the second sub-penalty function; Wherein, the fourth difference value is the difference between a fifth constant and a fourth ratio value, the fourth ratio value is the ratio of a fifth product and the initial planning path length, the fifth product is the product of the fifth constant and the third distance, and the third distance is the distance between the initial state point and the initial planning path start point; The target result is the ratio of a sixth constant and e to the power of n, e represents the base of natural logarithm, n is the sixth product of a seventh constant and a fifth difference value, and the fifth difference value is the difference between the maximum of the truncation curvature and the absolute value of the curvature of the initial state point. ; Wherein, the first constant is less than the fourth product.

7. The method of claim 1, wherein, Based on the detection result of the optimized path, it is determined whether to adjust the weight parameter of each penalty term to re-execute the preceding steps, including: In response to the detection result of the optimized path being a first detection result, the optimized path is taken as a target planning path; wherein the first detection result indicates that each penalty term of the optimized path meets the corresponding preset requirement; In response to the detection result of the optimized path being a second detection result, adjusting the weight parameters of the respective penalty terms based on the second detection result, taking the adjusted weight parameters of the respective penalty terms as new weight parameters of the respective penalty terms, and re-executing the steps of constructing a target function based on the initial planning path, the respective penalty terms affecting the quality of the path to be optimized, and the weight parameters of the respective penalty terms, and subsequent steps, until the detection result of the latest optimized path is a first detection result, taking the latest optimized path as the target planning path, or the number of times of detection of the first detection result reaches a preset number, stopping the path planning; the second detection result indicates that there is a penalty term in the optimized path that does not meet the corresponding preset requirement.

8. The method of claim 7, wherein, The adjustment of the weight parameters of the respective penalty terms includes increasing the weight parameters by a first preset coefficient, or decreasing the weight parameters by a second preset coefficient, the first preset coefficient being greater than the second preset coefficient.

9. The method of claim 7, wherein, The adjustment of the weight parameters of the respective penalty terms based on the second detection result includes: in response to the second detection result being that the first penalty term does not meet the corresponding preset requirement, increasing the weight parameter of the first penalty term, and / or decreasing the weight parameter of the associated penalty term corresponding to the first penalty term; the preset requirement of the first penalty term is that the optimized path does not collide with an obstacle; in response to the second detection result being that the fourth penalty term does not meet the corresponding preset requirement, increasing the weight parameter of the fourth penalty term, and / or decreasing the weight parameter of the associated penalty term corresponding to the fourth penalty term; the preset requirement corresponding to the fourth penalty term is that the maximum curvature of the optimized path is not greater than a curvature threshold; in response to the second detection result being that the fifth penalty term does not meet the corresponding preset requirement, increasing the weight parameter of the fifth penalty term, and / or decreasing the weight parameter of the associated penalty term corresponding to the fifth penalty term; the preset requirement of the fifth penalty term is that the rate of change of curvature of the optimized path is not greater than a change threshold.

10. The method of claim 9, wherein: the associated penalty term corresponding to the first penalty term includes the second penalty term; and / or, the associated penalty term corresponding to the fourth penalty term includes at least one of the second penalty term and the fifth penalty term; and / or, the associated penalty term corresponding to the fifth penalty term includes at least one of the first penalty term and the second penalty term.

11. The method of claim 1, wherein, The number of initial planning paths is a plurality, before the step of constructing a target function based on the initial planning path, the respective penalty terms affecting the quality of the path to be optimized, and the weight parameters of the respective penalty terms, the method further includes: obtaining a navigation path line planned based on a current starting point and a target ending point; segmenting the navigation path line into a plurality of navigation path segments; For each of the navigation path segments, a fourth distance between the navigation path segment and each of the obstacles is determined, and based on the fourth distance, the navigation path segment is determined as a corresponding initial planning path, or a path searched based on the navigation path segment is determined as the corresponding initial planning path; each of the initial planning paths is an initial state corresponding to the to-be-optimized path, and a corresponding optimized path of each of the initial planning paths is a final optimized state corresponding to the to-be-optimized path.

12. The method of claim 11, wherein, The determining, based on the fourth distance between the navigation path segment and each of the obstacles, the navigation path segment as the corresponding initial planning path, or the path searched based on the navigation path segment as the corresponding initial planning path, includes: in response to each of the fourth distances being greater than a preset distance threshold, determining the navigation path segment as the initial planning path; in response to there being a fourth distance that is not greater than the preset distance threshold among the fourth distances, determining the path searched based on the navigation path segment as the initial planning path.

13. The method of claim 11, wherein, The searching, based on the navigation path segment, for the corresponding initial planning path includes: obtaining a target drivable boundary of a road on which the navigation path segment is located; based on sampling of the navigation path segment in a target drivable region corresponding to the target drivable boundary, obtaining a plurality of sampling points; based on each of the sampling points, performing path searching to obtain a corresponding search path, and taking the search path as the initial planning path.

14. The method of claim 13, wherein, The width of the drivable region is negatively correlated with the density of the sampling point distribution. And / or, the obtaining, based on the navigation path segment, of the plurality of sampling points by sampling in the drivable region corresponding to the target drivable boundary includes: selecting a plurality of reference path points from the navigation path segment; for each of the reference path points, sampling in a forward direction of the reference path point, and determining whether there is a sampling point that fails in searching in the forward direction of the reference path point; in response to there being a sampling point that fails in searching in the forward direction of the reference path point, and based on the drivable region in the forward direction being determined to be passable, sampling in a backward direction and the forward direction of the reference path point respectively to obtain each of the sampling points.

15. The method of claim 13, wherein, The obtaining of the target drivable boundary of the road on which the navigation path segment is located includes: generating each initial drivable boundary based on the navigation path line; determining whether there is a first intersection point between a normal line of each target path point on the navigation path line and each target obstacle in an initial drivable region; the initial drivable region is a region between different initial drivable boundaries; based on a determination result of the first intersection point between the normal line of each target path point and each target obstacle, determining a target boundary point corresponding to each of the target path points; integrating the target boundary points of each of the target path points to obtain the target drivable boundary.

16. The method of claim 15, wherein, The determining, based on the determination result of the first intersection point between the normal line of each target path point and each target obstacle, of the target boundary point corresponding to each of the target path points includes: for each of the target path points, in response to there being no first intersection point between the normal line of the target path point and each of the target obstacles, taking a second intersection point between the normal line and each of the initial drivable boundaries as the target boundary point of the target path point; In response to the first intersection point existing between the normal line of the target path point and each target obstacle, a third comparison result of passable space on the left and right sides of the target obstacle where the first intersection point is located and a preset passable space is determined, and a target boundary point of the target path point is determined based on the third comparison result.

17. The method of claim 16, wherein, The target obstacle is a static obstacle. The target boundary point of the target path point is determined based on the third comparison result, and the target boundary point of the target path point is determined based on the third comparison result. In response to the third comparison result being that the passable space on each side is less than the preset passable space, it is determined that the target path point does not have the target boundary point. In response to the third comparison result being that the passable space on each side is not less than the preset passable space, a second intersection point of the normal line of the target path point and each initial drivable boundary is determined as the target boundary point of the target path point. In response to the third comparison result being that the passable space on one side of the two sides is not less than the preset passable space and the passable space on the other side is less than the preset passable space, the side not less than the preset passable space is determined as a target side, and a first intersection point of the normal line of the target path point and the target side is determined as a target boundary point of the target path point.

18. An electronic device, comprising: The memory and the processor are coupled to each other, The memory stores program instructions. The processor is configured to execute the program instructions stored in the memory to implement the method in any one of claims 1-17.

19. A computer-readable storage medium, characterized in that, The computer readable storage medium stores program instructions executable by the processor, and the program instructions are executable by the processor to implement the method in any one of claims 1-17.

Images