Path planning method and device and storage medium

Abstract

The invention discloses a path planning method and device and a storage medium, and the method comprises the steps: constructing a target function based on an initial planning path, penalty terms affecting the quality of a to-be-optimized path, and weight parameters of the penalty terms; optimizing the target function to obtain an optimized path corresponding to the initial planning path; and based on the detection result of the optimized path, determining whether to adjust the weight parameter of each penalty term so as to re-execute the previous steps. In this way, the success rate of path planning can be improved.

Description

Technical Field

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

[0002] Path planning is an important part of the autonomous driving control algorithm, which aims to provide a smooth, collision-free trajectory that fits the navigation route as closely as possible so that the control module can track it. However, road conditions are complex and diverse, especially in complex road scenes, such as narrow roads and blockages ahead, which increases the difficulty of path planning and leads to a low success rate of real-time planned paths. Therefore, how to improve the success rate of path planning in various road scenes is of great significance. Summary of the invention

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

[0004] In order to solve the above technical problems, a technical solution adopted in the present application is: to provide a path planning method, the method comprising: constructing an objective function based on the initial planned path, various penalty items affecting the quality of the path to be optimized and the weight parameters of each penalty item; optimizing the objective function to obtain an optimized path corresponding to the initial planned path; based on the detection results of the optimized path, determining whether to adjust the weight parameters of each penalty item to re-execute the above steps.

[0005] Among them, based on the initial planned path, each penalty item affecting the quality of the path to be optimized and the weight parameter of each penalty item, an objective function is constructed, including: based on the initial planned path, the weight parameter of each penalty item and the constraint condition of each penalty item, a penalty function corresponding to each penalty item is constructed; and the penalty function corresponding to each penalty item is integrated to construct the objective function.

[0006] Among them, the penalty items that affect the quality of the path to be optimized include at least two of the following: a first penalty item regarding the distance between the path to be optimized and the associated obstacle, a second penalty item regarding the offset distance between the path to be optimized and the initial planned path, a third penalty item regarding the vehicle kinematic constraint of the path to be optimized, a fourth penalty item regarding the minimum turning radius constraint of the path to be optimized, and a fifth penalty item regarding the curvature change rate constraint of the path to be optimized.

[0007] Among them, the initial planned path is the initial state of the path to be optimized, 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 item and the constraints of each penalty item, a penalty function corresponding to each penalty item is constructed, including: in response to each penalty item including a first penalty item, based on the first distance between each state point to be optimized and the corresponding several associated obstacles, the first weight parameter of the first penalty item, and the first distance constraint condition, a first penalty function of the first penalty item is constructed; in response to each penalty item including a second penalty item, a matching point of each initial state point is selected from each state point to be optimized, and based on the second distance between each initial state point and the corresponding matching point, the second weight parameter of the second penalty item, and the second distance constraint condition, a second penalty function of the second penalty item is constructed. a second penalty function; in response to each penalty item including a third penalty item, 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 item, and the kinematic constraint, a third penalty function of the third penalty item is constructed; wherein, the first state to be optimized includes the vehicle heading angle and position, and the kinematic constraint includes at least one of the speed constraint and the front wheel turning angle constraint; in response to each penalty item including a fourth penalty item, 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 item, and the maximum curvature constraint, a fourth penalty function of the third penalty item is constructed; wherein, the second state to be optimized includes the vehicle heading angle and position; in response to each penalty item including a fifth penalty item, based on the curvature change rate between three adjacent state points to be optimized, and the curvature change rate constraint, a fifth penalty function of the fifth penalty item is constructed.

[0008] Among them, for each state point to be optimized, the first distance includes a sub-distance 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 several associated obstacles, the first weight parameter of the first penalty item, and the first distance constraint condition, a first penalty function of the first penalty item 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 respectively; based on the first comparison result and the first distance constraint condition, determining the first sub-penalty function between the state point to be optimized and each associated obstacle; weighting based on the first weight parameter and the first sub-penalty function between the state point to be optimized and each associated obstacle to obtain a 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 item.

[0009] Wherein, based on the first comparison result and the first distance constraint condition, a first sub-penalty function between the state point to be optimized and each associated obstacle is determined, including: in response to the first comparison result that the sub-distance is greater than the cutoff distance, a first constant is used as the first sub-penalty function; in response to the first comparison result that the sub-distance is not less than the expected minimum distance and not greater than the cutoff distance, a first product of the α power of the first ratio and the β power of the expected minimum distance is used 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 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 αth power of the third difference and the βth power of the expected minimum distance is used as the first sub-penalty function; wherein 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; 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.

[0010] 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 a second obstacle closest to the state point to be optimized, and 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 first sub-weights set for each type of associated obstacle respectively; weighting based on the first weight parameter and a first sub-penalty function between the state point to be optimized and each associated obstacle to obtain a first weighted penalty function of the state point to be optimized, 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] Among them, the matching point of each initial state point is selected from each state point to be optimized, including: for each initial state point, a state point to be optimized that is closest to the initial state point among each state point to be optimized is used 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 item, and the second distance constraint, a second penalty function of the second penalty item 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, 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 item 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 by weighting; the second weighted penalty function of each matching point is obtained by combining the second weighted penalty function of each matching point.

[0012] Wherein, based on the second comparison result and the second distance constraint, a second sub-penalty function between the initial state point and the corresponding matching point is determined, including: in response to the second comparison result that the second distance is less than the cutoff distance, determining the first constant as the second sub-penalty function; in response to the second comparison result that the second distance is not less than the cutoff distance, obtaining the fourth difference, the fourth product between the second distance and the target result, 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 the nth power of e, e represents the base of the natural logarithm, n is the sixth product of the seventh constant and the fifth difference, the fifth difference is the maximum of the absolute value of the curvature of the cutoff curvature and the initial state point, and the difference with the cutoff curvature; wherein the first constant is less than the fourth product.

[0013] Among them, based on the detection result of the optimized path, it is determined whether to adjust the weight parameters of each penalty item to re-execute the aforementioned steps, including: in response to the detection result of the optimized path being the first detection result, the optimized path is used as the target planning path; wherein, the first detection result indicates that the penalty items of the optimized path meet the corresponding preset requirements; in response to the detection result of the optimized path being the second detection result, the weight parameters of each penalty item are adjusted based on the second detection result, the adjusted weight parameters of each penalty item are used as the new weight parameters of each penalty item, and the initial planning path, each penalty item affecting the quality of the path to be optimized and the weight parameters of each penalty item are re-executed, the objective function is constructed, and subsequent steps are performed until the latest detection result of the optimized path is the first detection result, the latest optimized path is used as the target planning path, or the number of detections as the first detection result reaches the preset number, and the path planning is stopped; wherein, the second detection result indicates that there are penalty items in the penalty items of the optimized path that do not meet the corresponding preset requirements.

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

[0015] Among them, each penalty item includes a first penalty item regarding the distance between the path to be optimized and the associated obstacle, a second penalty item regarding the offset distance between the path to be optimized and the initial planned path, a third penalty item regarding the vehicle kinematic constraint of the path to be optimized, a fourth penalty item regarding the minimum turning radius constraint of the path to be optimized, and at least two of the fifth penalty item regarding the curvature change rate constraint of the path to be optimized; adjusting the weight parameters of each penalty item based on the second detection result, including: in response to the second detection result that the first penalty item does not meet the corresponding preset requirements, increasing the weight parameter of the first penalty item, and / or lowering the associated penalty item corresponding to the first penalty item. Weight parameter; the preset requirement of 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 lowered; the preset requirement corresponding to 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 lowered; the preset requirement of the fifth penalty item is that the curvature change rate of the optimized path is not greater than the change threshold.

[0016] Among them, 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] Among them, the number of initial planned paths is multiple. Before constructing the objective function based on the initial planned paths, each penalty item affecting the quality of the path to be optimized and the weight parameters of each penalty item, it also includes: obtaining a navigation path line planned based on the current starting point and the target end point; dividing the navigation path line into multiple navigation path segments; for each navigation path segment, based on the fourth distance between the navigation path segment and each obstacle, determining to use the navigation path segment as the corresponding initial planned path, or to use the path obtained by searching based on the navigation path segment as the corresponding initial planned path; 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.

[0018] Among them, based on the fourth distance between the navigation path segment and each obstacle, determining to use the navigation path segment as the corresponding initial planned path, or to use the path obtained by searching based on the navigation path segment as the corresponding initial planned path, including: in response to each fourth distance being greater than a preset distance threshold, using the navigation path segment as the initial planned path; in response to there being 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 planned path.

[0019] The corresponding initial planned path is obtained based on the search of the navigation path segment, including: obtaining a target drivable boundary of the road where the navigation path segment is located; sampling within a target drivable area corresponding to the target drivable boundary based on the navigation path segment to obtain a number of sampling points; performing a path search based on each sampling point to obtain a corresponding search path, and using it as the initial planned path.

[0020] Among them, the width of the drivable area is negatively correlated with the density of the distribution of the sampling points; and / or, sampling is performed in the drivable area corresponding to the target drivable boundary based on the navigation path segment to obtain a number of sampling points, including: selecting a number of reference path points from the navigation path segment; for each reference path point, sampling in the forward direction of the reference path point, and determining whether there is a sampling point in the forward direction of the reference path point where the search fails; in response to the presence of a sampling point in the forward direction of the reference path point where the search fails, and it is determined that the drivable area is passable based on the forward direction, sampling is performed in the backward direction and forward direction of the reference path point respectively to obtain each sampling point.

[0021] Among them, 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 line 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; based on the determination result of the first intersection between the normal line of each target path point and each target obstacle, determining the target boundary point corresponding to each target path point; and obtaining the target drivable boundary by integrating the target boundary points of each target path point.

[0022] Among them, 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, including: for each target path point, in response to the absence of the first intersection between the normal of the target path point and each target obstacle, the second intersection of the normal and each initial drivable boundary is used as the target boundary point of the target path point; in response to the existence of the first intersection between the normal of the target path point and each target obstacle, a third comparison result of the passable space on the left and right sides of the target obstacle where the first intersection is located and the preset passable space is determined, and based on the third comparison result, the target boundary point of the target path point is determined.

[0023] Among them, 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 smaller than the preset passable space, determining that there is no target boundary point at the target path 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 normal of the target path point and the second intersection of 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 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, taking the side of the two sides that is not less than the preset passable space as the target side, and taking the first intersection 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 technical problems, another technical solution adopted in the present application is: to provide an electronic device, comprising a memory and a processor coupled to each other, the memory storing program instructions; the processor is used to execute the program instructions stored in the memory to implement the above method.

[0025] In order to solve the above technical problem, another technical solution adopted by the present application is: providing a computer-readable storage medium for storing program instructions, which can be executed to implement the above method.

[0026] In the above scheme, after optimizing the constructed objective function and obtaining the optimized path, it is further determined whether to adjust the weight parameters of each penalty item based on the detection result of the optimized path, so as to construct a new objective function and optimize a new optimized path based on the adjusted weight parameters. Compared with the method of not adjusting the weight parameters of each penalty item, the method of adjusting the weight parameters of each penalty item and re-planning the path when the optimization path detection result fails can increase the chance of path planning, thereby improving the success rate of path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 It is a flowchart of an embodiment of a path planning method provided by the present application;

[0028] Figure 2 yes Figure 1 The flowchart of step S11 is shown as an embodiment;

[0029] Figure 3 It is a flowchart of an embodiment of constructing a first penalty function provided by the present application;

[0030] Figure 4 It is a flowchart of an embodiment of constructing a second penalty function provided by the present application;

[0031] Figure 5 This is a flowchart of an embodiment of obtaining each initial planning path provided by the present application;

[0032] Figure 6 yes Figure 5 The flowchart of step S53 is shown as an embodiment;

[0033] Figure 7 is a schematic diagram of an embodiment of forward sampling provided by the present application;

[0034] Figure 8 It is a schematic diagram of an embodiment of backward plus forward sampling provided by the present application;

[0035] Fig. 9 It is a schematic diagram of a framework of an embodiment of a path planning device provided by the present application;

[0036] Fig.10 It is a schematic diagram of a framework of an embodiment of an electronic device provided by the present application;

[0037] Fig.11 It is a schematic diagram of the framework of the computer-readable storage medium provided by this application. DETAILED DESCRIPTION

[0038] In order to make the purpose, technical solution and effect of the present application clearer and more specific, the present application is further described in detail below with reference to the accompanying drawings and examples.

[0039] In addition, if there are descriptions involving "first", "second", etc. in the embodiments of the present application, the descriptions of "first", "second", etc. are only used for descriptive purposes and cannot be understood as indicating or suggesting their relative importance or implicitly indicating the number of technical features indicated. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In addition, the technical solutions between the various embodiments can be combined with each other, but they must be based on the ability of ordinary technicians in the field to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by this application.

[0040] It should be noted that the path planning method of the present application can be applied to any road scene that requires path planning, which can be an indoor scene or an outdoor scene, and can be applied to the driving path planning of any movable device or even an electronic device carried by pedestrians (such as a mobile phone, an electronic watch, etc.). The movable device can be, but is not limited to, a vehicle, or a robot, etc. For the convenience of description, the path planning method of the present application is illustrated below using a vehicle as an example.

[0041] In the path planning process of the present application, the weight parameters of each penalty item involved in the construction of the objective function are weight parameters that can be adaptively adjusted. Specifically, after the optimized path is obtained through optimization, the adaptive adjustment can be performed based on the quality detection results of the optimized path.

[0042] See also Figure 1 , Figure 1 is a flow chart of an embodiment of the path planning method provided by the present application. It should be noted that if there are substantially the same results, this embodiment does not necessarily Figure 1 The process sequence shown is limited. Figure 1 As shown, this embodiment includes:

[0043] S11: constructing an objective function based on the initial planned path, each penalty item affecting the quality of the path to be optimized, and a weight parameter of each penalty item.

[0044] In this embodiment, path optimization is performed on the basis of the initial planned path, that is, the initial planned path is the initial state of the path optimization (path to be optimized), and the path to be optimized is the intermediate optimization state of the initial planned path during the 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 planning paths, each of which is a segment of the entire path from the starting point to the target end point, that is, the entire path includes multiple segments of initial planning paths. In this embodiment, the planning (including optimization) of each segment of the initial planning path is performed segment by segment, and the planning method of each segment of the initial planning path is the same. The specific method for obtaining each segment of the initial planning path can be referred to below. Figure 5 Description of the illustrated embodiment.

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

[0047] In another embodiment, considering that the weight parameter of each penalty item is the weight parameter of the entire path to be optimized with respect to each penalty item, and 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 item based on the degree of influence of each state point on the path quality, the influence of the weight of the influence of each state point on the corresponding penalty item can be added in the process of constructing the penalty function corresponding to each penalty item. Specifically:

[0048] See also Figure 2 , Figure 2 yes Figure 1 The flowchart of an embodiment of step S11 is shown. In this embodiment, step S11 further includes:

[0049] S21: Constructing a penalty function corresponding to each penalty item based on the initial planned path, the weight parameters of each penalty item and the constraints of each penalty item.

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

[0051] In some embodiments, the penalty items that affect the quality of the path to be optimized include at least two of the following: a first penalty item regarding the distance between the path to be optimized and an associated obstacle, a second penalty item regarding the offset distance between the path to be optimized and an initial planned path, a third penalty item regarding the vehicle kinematics constraint of the path to be optimized, a fourth penalty item regarding the minimum turning radius constraint of the path to be optimized, and a fifth penalty item regarding the curvature change rate constraint of the path to be optimized.

[0052] In one implementation scenario, in order to keep the optimized path and obstacles at a safe distance, each penalty term may include a first penalty term regarding the distance between the path to be optimized and the associated obstacle. In this implementation scenario, a first penalty function of the first penalty term may be constructed 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. The specific method of constructing the first penalty function may refer to the following Figure 3 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 several associated obstacles includes sub-distances between the state point to be optimized and each associated obstacle.

[0054] In one embodiment, each associated obstacle of the state point to be optimized includes all first obstacles within the first distance range of the state point to be optimized, and the second obstacle closest to the state point to be optimized, and the second obstacle is an obstacle located between the first distance range and the second distance range of the state point to be optimized. Among them, 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, and all obstacles within the first distance range of each obstacle are used as the associated obstacles of the state point to be optimized, and the obstacles located between the first distance range and the second distance range and the obstacles closest to the state point to be optimized are used as the associated obstacles of the state point to be optimized.

[0055] In another embodiment, each obstacle within the second distance range of the state point to be optimized may also be used as an associated obstacle of the state point to be optimized.

[0056] See also Figure 3 , Figure 3 : is a flow chart of an embodiment of constructing a first penalty function provided by the present application. In this embodiment, constructing the first penalty function includes:

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

[0058] S32: Determine a first sub-penalty function between the state point to be optimized and each associated obstacle based on the first comparison result and the first distance constraint condition.

[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_ijrepresents the first sub-penalty function between the state point i to be optimized and the associated obstacle j, represents the sub-distance between the state point i to be optimized and the associated obstacle j, d cut_off represents the cut-off distance, d min represents the expected minimum distance, c1 and c2 are two constants, and c2>c1, α is an exponent used to control the rate of increase of the penalty, α>1. Among them, the expected minimum distance is less than the cutoff distance.

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

[0063] Specifically, in combination with the above formula, based on the first comparison result and the first distance constraint condition, 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 , that is, the distance between the state point to be optimized and the associated obstacle (the sub-distance above) is greater than the cutoff distance, and the first constant is used as the first sub-penalty function between the state point to be optimized i and the associated obstacle j. The first constant can be, but is not limited to, 0 in the above formula, and can be any constant representing a very small penalty value, such as 0.5 or 1.

[0065] Case 2: If d ij ≥d min , that is, 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 αth power of the first ratio and the βth power of the expected minimum distance is used as the first sub-penalty function.

[0066] The first ratio is the second product of the second ratio and the second constant, the second ratio is the ratio of the first difference to 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] Combining the above formula, the second ratio is The second constant is c1.

[0068] Case 3: If d ij <d min, that is, the distance between the state point to be optimized and the associated obstacle is less than the expected minimum distance, then the second product of the αth power of the third difference and the βth power of the expected minimum distance is used as the first sub-penalty function; wherein 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 distance between the state point to be optimized and the associated obstacle, multiplied by the fourth constant, that is, (c2-c1)*d in the above formula ij .

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

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

[0071] The first constant is smaller than the first product, and the first product is smaller than the second product.

[0072] It should be noted that the first penalty item is set to ensure the safety of the path. The ultimate goal is to keep the distance between the optimized path and the obstacle moderate. When the path is very close to the obstacle, a larger penalty is given. When it is far away, the penalty is set to 0, which will set a very low penalty value. Therefore, in this embodiment, when d ij >d cut_off When , it means that the distance between the path and the associated obstacle is far and it is safe. Therefore, in this case, no penalty or a lower penalty may be imposed on the associated obstacle; d min is a critical close distance, when d ij ≥d min When d ij As d increases, the safety factor increases, and the penalty value can be set to decrease exponentially; ij <d min , the safety factor becomes smaller, and the penalty value can be set to rise rapidly. Among them, c1 and c2 are two constant parameters used to control the penalty value at the critical distance, c2>c1; α is an exponent used to control the rate of increase of the penalty.

[0073] S33: performing weighting based on the first weight parameter and the first sub-penalty function between the state point to be optimized and each associated obstacle to obtain a first weighted penalty function of the state point to be optimized.

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

[0075] In order to better adapt to road conditions and obtain safer optimized trajectories, different first sub-weights of different types of associated obstacles may be set. For example, for pedestrians on the road, in order to ensure their safety, a higher first sub-weight may be set for associated obstacles of the pedestrian type, while other types of associated obstacles may be set with a lower first sub-weight than that of pedestrians. The specific weight setting may be pre-set according to the actual possible road conditions.

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

[0077]

[0078] In the formula, f obs_i represents the first weighted penalty function of the state point to be optimized, w obs_j represents the first sub-weight of the associated obstacle j, and m in the formula represents the number of associated obstacles of the state point to be optimized.

[0079] In this implementation manner, the first weighted penalty function of the state point to be optimized is obtained based on the first sub-penalty function between the state point to be optimized and each associated obstacle and the first sub-weight term of each associated obstacle.

[0080] It should be noted that, since there are at least two penalty terms, it is ultimately expected that the penalty value of the objective function constructed based on the penalty terms will be as close to 0 as possible. In order to avoid some penalty terms being negative and some penalty terms being positive, resulting in errors in the objective function obtained by integrating the penalty functions corresponding to the penalty terms due to interference from the positive and negative values ​​of different penalty terms, the squares of the above-mentioned first sub-penalty functions can be used as new first sub-penalty functions, and then weighted based on the first weight parameter and the first sub-penalty function between the state point to be optimized and the associated obstacles to obtain the first weighted penalty function of the state point to be optimized.

[0081] S34: synthesizing the first weighted penalty functions of the state points to be optimized to obtain a first penalty function of the first penalty item.

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

[0083]

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

[0085] In one implementation scenario, in order to make the optimized path as close as possible to the initial planned path, each penalty item may be set to include a second penalty item regarding the offset distance between the path to be optimized and the initial planned path. In this implementation scenario, the matching points of each initial state point may be selected from each state point to be optimized, and then based on the second distance between each initial state point and the corresponding matching point, the second weight parameter of the second penalty item, and the second distance constraint, a second penalty function of the second penalty item may be constructed.

[0086] Among them, the common method for optimizing the offset distance between the path to be optimized and the initial planned path is: optimize all path points on the initial planned path. This optimization method will increase the amount of calculation and limit the degree of freedom of the path. To solve this problem, a number of path points (initial state points) can be selected at a certain distance on the initial planned path, so as to use a small number of selected initial state points and corresponding matching points to construct a second penalty function of the second penalty term for subsequent optimization. Among them, the interval distance points can be the same distance between every two points, or different distances. The specific interval distance can be preset according to actual needs.

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

[0088] In this embodiment, please refer to Figure 4 , Figure 4 : is a flow chart of an embodiment of constructing a second penalty function provided by the present application. In this embodiment, constructing the second penalty function includes:

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

[0090] S42: Determine 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 condition.

[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 represents the second sub-penalty function 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), represents the distance between the initial state point and the corresponding matching point (second distance), ρ cut_off is the cutoff curvature, ρvia_i is the curvature of the i-th initial state point, s represents the length of the initial planning path, s i Represents the distance between the i-th initial state point and the starting point of the initial planning path, and c is a constant.

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

[0095] Specifically, in combination with the above formula, 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, including the following two situations:

[0096] Case 1: If d i <ε, that is, the distance between the initial state point i and the corresponding matching point (the second distance) is less than the cutoff distance, and the first constant is determined to be the second sub-penalty function between the initial state point i and the corresponding matching point. Among them, the purpose of setting this penalty term is to make the final optimized path as close to the initial planned path as possible through optimization. Therefore, when the distance between the initial state point i and the corresponding matching point is less than the cutoff distance, it indicates that the initial state point and the corresponding matching point are very close, which meets the expectation of setting the penalty term. At this time, the second penalty sub-function can be set to a smaller penalty value (the first constant). Exemplarily, 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] Case 2: If d i ≥ε, that is, the distance between the initial state point i and the corresponding matching point is not less than the cutoff distance, obtain the fourth difference, the fourth product between the second distance and the target result, and use 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 the nth power of e, e represents the base of the natural logarithm, n is the sixth product of the seventh constant and the fifth difference, the fifth difference is the maximum of the absolute value of the cutoff curvature and the curvature of the initial state point, and the difference with the cutoff curvature.

[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 smaller than the fourth product, so as to increase the penalty value of the point when the distance between the initial state point i and the corresponding matching point is greater than the cutoff distance.

[0102] It should be noted that the construction concept of the second sub-penalty function is: it is hoped that the final optimized path is as close to the initial planned path as possible, among which the requirements for being close to the initial planned path are different under different road environments. For example, on large curvature routes such as U-shaped bends or right-angle bends, due to the presence of obstacles and the need to meet constraints such as the minimum turning radius of the vehicle, consistent penalty parameters may lead to optimization failure (such as curvature not meeting constraints, large curvature jumps, etc.). At the same time, since path optimization is performed in real time and the update frequency is fast, for the far end of the optimized path, even if it deviates greatly from the initial planned path, it has almost no effect on the control. Therefore, when designing the penalty function, it is necessary to 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, that is, to allow for larger deviations at large curvatures and at the far end in exchange for greater flexibility. For specific considerations, please refer to the above situation 2.

[0103] S43: performing weighting based on the second weight parameter of the second penalty item and the second sub-penalty function between each initial state point and the corresponding matching point to obtain a second weighted penalty function for each matching point.

[0104] In one embodiment, the second weighted penalty function of the matching point (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 of the matching point (state point to be optimized) corresponding to the initial state point i, ω via Represents the second weight parameter of the second penalty term.

[0107] It should be noted that, since there are at least two penalty terms, it is ultimately expected that the penalty value of the objective function constructed based on the penalty terms will be as close to 0 as possible. In order to avoid some penalty terms being negative and some penalty terms being positive, which results in the objective function being obtained by combining the penalty functions corresponding to the penalty terms, errors are generated due to the interference of positive and negative values ​​of different penalty terms. When constructing the second weighted penalty function, the square of the second sub-penalty function between the initial state point and the corresponding matching point is used as the new first sub-penalty function, and then weighted 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 to obtain the second weighted penalty function of each matching point.

[0108] S44: synthesizing the second weighted penalty functions of the matching points to obtain a second penalty function for the second penalty item.

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

[0110]

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

[0112] In one implementation scenario, in order to make the optimized path meet the vehicle kinematic requirements, each penalty item may be set to include a third penalty item regarding the vehicle kinematic constraints of the path to be optimized. In this implementation scenario, a third penalty function of the third penalty item may be constructed based on the difference between the first state to be optimized of each adjacent two state points to be optimized, the third weight parameter of the third penalty item, and the kinematic constraints. The first state to be optimized includes the vehicle heading angle and position, and the kinematic constraints include at least one of a speed constraint and a front wheel turning angle constraint.

[0113] Exemplarily, this embodiment is based on the vehicle geometric kinematics model:

[0114]

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

[0116] First, based on the difference between the first states to be optimized of two adjacent state points to be optimized (state point to be optimized i and state point to be optimized i+1) and the kinematic constraints, the third penalty subfunction of the state point to be optimized i is constructed, which is specifically expressed 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 ) is the position of the state point i to be optimized, is the position of the state point i+1 to be optimized, β i and β i+1 are the vehicle heading angles of state points i and i+1 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-holonomic kinematics of the state point to be optimized is obtained and expressed as follows:

[0120]

[0121] The weight parameter of the third penalty term includes the sub-weights of each state point to be optimized with respect to the third penalty term, ω nh_i It 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, and are set according to actual needs. Represents the third penalty subfunction of the state point i to be optimized. Among them, since there are at least two penalty terms, it is ultimately expected that the penalty value of the objective function constructed based on each penalty term should be as close to 0 as possible. In order to avoid some penalty terms being negative and some penalty terms being positive, resulting in errors in the objective function obtained by combining the penalty functions corresponding to the penalty terms due to the interference of different positive and negative values ​​of the penalty terms, here Update to the third penalty sub-function of the state point i to be optimized.

[0122] Finally, the third weighted penalty function of each state point to be optimized is combined to obtain the third penalty function of the third penalty term, which is expressed as follows:

[0123]

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

[0125] In one implementation scenario, in order to make the optimized path satisfy the minimum turning radius constraint, each penalty item may be set to include a fourth penalty item regarding the minimum turning radius constraint of the path to be optimized. In this implementation scenario, a fourth penalty function of the third penalty item may be constructed 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 item, and the maximum curvature constraint; wherein the second state to be optimized includes the vehicle heading angle and position.

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

[0127] Wherein, based on the difference between the second states 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, a fourth penalty function of the fourth penalty term is constructed, including:

[0128] First, take two consecutive state points to be optimized in the trajectory to be optimized to calculate the curvature. The calculation method is as follows:

[0129]

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

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

[0132]

[0133] Where: cut_off Represents the minimum curvature allowed. The curvature of the state point i to be optimized is allowed to be less than the minimum curvature. If it exceeds the minimum curvature, it is considered to be an unallowed large curvature. Correspondingly, when ρ i < cut_off When ρ is , the function will maintain a small penalty to make the curvature of the optimized path as small as possible, while the curvature penalty term will not be too large in the curve, resulting in a greater impact on other optimization items. i >ρ cut_off When , the penalty value will rise rapidly. The above fourth penalty sub-function can make the curvature of the optimization path as close to a reasonable range as possible; γ is a constant used to control the rising speed of the entire penalty term, where 0<γ<1, or γ is greater than 1, and the specific value can be taken according to actual needs.

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

[0135]

[0136] Wherein, the weight parameter of the fourth penalty term includes the sub-weights of each state point to be optimized with respect to the fourth penalty term, ω kappa_i It represents the sub-weight of the optimization state point i with respect to the fourth penalty term. The sub-weights of different state points i to be optimized can be the same or different, and are set according to actual needs. is the square of the fourth penalty sub-function, is the updated fourth penalty sub-function. For the specific update reasons, please refer to the relevant descriptions of other penalty items.

[0137] Finally, the fourth penalty function of the fourth penalty term is constructed, which is expressed as follows:

[0138]

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

[0140] In one implementation scenario, in order to make the optimized path satisfy the curvature change rate constraint, each penalty item may be set to include a fifth penalty item regarding the curvature change rate constraint of the path to be optimized. In this implementation scenario, a fifth penalty function of the fifth penalty item may be constructed based on the curvature change rate between three adjacent state points to be optimized and the curvature change rate constraint.

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

[0142]

[0143] Among them, the purpose of constructing this penalty term is to expect the entire path after optimization to be as smooth as possible. Correspondingly, the curvature change rate of the entire path is as small as possible. Therefore, the fifth penalty subfunction of the curvature change rate of the state point i to be optimized can be constructed as follows:

[0144] e kppa_diff_i =Δρ i

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

[0146]

[0147] Among them, ω kappa_diff_i The sub-weight representing the curvature change rate of the state point i to be optimized.

[0148] In this embodiment, the weight parameter of the fifth penalty term includes sub-weights corresponding to each state point i to be optimized, where ω kappa_diff_iIt is related to the position of the state point i to be optimized. Generally, there are higher requirements for the path smoothness near the starting point of the path to be optimized. Therefore, the sub-weight corresponding to the state point to be optimized at the near end can be set greater than the sub-weight corresponding to the state point to be optimized at the far end.

[0149] Finally, the fifth penalty function of the fifth penalty term is constructed, which is expressed as follows:

[0150]

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

[0152] It should be noted that, in one implementation scenario, the above-mentioned state points to be optimized can be used as vertices, and the sub-penalty functions corresponding to the state points to be optimized can be used as edges of the vertices to construct a hypergraph. In the subsequent process of optimizing the objective function, optimization is performed based on the created hypergraph.

[0153] S22: The penalty functions corresponding to each penalty item are integrated to construct the objective function.

[0154] In this embodiment, each penalty item includes at least two of the above five penalty items, and the corresponding penalty function is integrated to construct the objective function. The corresponding penalty item can be set according to actual needs.

[0155] In a specific embodiment, each penalty item includes each penalty item in the above five penalty items, and the corresponding constructed objective function is expressed as follows:

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

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

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

[0159] Among them, existing open source algorithms such as g2o optimizer, gradient descent, GM (Gauss-Newton) iteration 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 result of the optimization path, determine whether to adjust the weight parameters of each penalty item to re-execute the above steps.

[0161] In this embodiment, after the optimized path is obtained in step S12, the optimized path is tested for validity (quality) to obtain a corresponding test result. The specific test result includes two possible test results, namely: a first test result that characterizes that each penalty item of the optimized path meets the corresponding preset requirements, and a second test result that characterizes that there is a penalty item in each penalty item of the optimized path that does 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] Exemplarily, if the first penalty item meets the corresponding preset requirements, 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 requirements, it means that the degree of proximity between the optimized path and the initial planned path meets the requirements; if the third penalty item meets the corresponding preset requirements, it means that the optimized path meets the kinematic constraints of the vehicle; if the fourth penalty item meets the corresponding preset requirements, it means that the optimized path meets the minimum turning radius constraints of the vehicle; if the fifth penalty item meets the corresponding preset requirements, it means that the optimized path meets the corresponding curvature change rate constraints.

[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, and the adjusted weight parameters of each penalty item are used as the new weight parameters of each penalty item. 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 are re-executed to construct the objective function and subsequent steps until the latest detection result of the optimized path is the first detection result, the latest optimized path is used as the target planning path, or the number of detections as the first detection result reaches the preset number, and the path planning is stopped.

[0165] The adjustment of the weight parameter of each penalty item includes increasing the weight parameter by a first preset coefficient, or decreasing the weight parameter by a second preset coefficient, and the first preset coefficient is greater than the second preset coefficient. That is, the weight parameter of the penalty item to be adjusted is multiplied by the first preset coefficient on the basis of the previous weight parameter 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, adjusting the weight parameters of each penalty item based on the second detection result includes at least the following situations:

[0167] Case 1: If the second detection result is that the first penalty item 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 a threshold), then the weight parameter of the first penalty item is increased, and / or the weight parameter of the associated penalty item corresponding to the first penalty item is lowered; the preset requirement of the first penalty item 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 optimization path collides with the associated obstacle or the distance is less than the distance threshold, the weight parameter of the first penalty item can be increased so that when optimizing the objective function constructed based on the new weight parameter, the first penalty item can be paid more attention to, so that the optimized path obtained by optimization meets the preset requirements corresponding to the first penalty item. Of course, the weight parameter of the first penalty item can also be indirectly increased by lowering the weight parameter of the associated penalty item of the first penalty item, or the weight parameter of the first penalty item can be increased at the same time, and the weight parameter of the associated penalty item corresponding to the first penalty item can be lowered to further increase the weight parameter of the first penalty item, which can also improve the optimization effect of the optimization path with respect to the first penalty item.

[0169] Case 2: If the second detection result is that the fourth penalty item regarding the minimum turning radius constraint of the path to be optimized does not meet the corresponding preset requirements, then 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 lowered; the preset requirement corresponding to the fourth penalty item is that the maximum curvature of the optimized path is not greater than the curvature threshold.

[0170] Similar to the above situation, the purpose of increasing the weight parameter of the fourth penalty item and / or lowering the weight parameter of the associated penalty item corresponding to the fourth penalty item is to pay more attention to the fourth penalty item when optimizing the objective function constructed based on the new weight parameter, so that the maximum curvature of the optimized path obtained by optimization is not greater than the curvature threshold.

[0171] Case three: if the second detection result is that the fifth penalty item 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 item is increased, and / or the weight parameter of the associated penalty item corresponding to the fifth penalty item is lowered; the preset requirement of the fifth penalty item is that the curvature change rate of the optimized path is not greater than the change threshold.

[0172] Similar to the above situations one and two, the purpose of increasing the weight parameter of the fifth penalty item and / or lowering the weight parameter of the associated penalty item corresponding to the fifth penalty item is to: when optimizing the objective function constructed based on the new weight parameter, more attention can be paid to the fifth penalty item, so that the curvature change rate of the optimized path obtained by optimization is not greater than the change threshold.

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

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

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

[0176] The above three situations are only illustrative examples. In fact, no matter whether each penalty item does not meet 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 reduced, so that the optimization path finally obtained by optimization meets the preset requirements corresponding to each penalty item.

[0177] In the above scheme, after optimizing the constructed objective function and obtaining the optimized path, it is further determined whether to adjust the weight parameters of each penalty item based on the detection result of the optimized path, so as to construct a new objective function and optimize a new optimized path based on the adjusted weight parameters. Compared with the method of not adjusting the weight parameters of each penalty item according to the detection result, the method of adjusting the weight parameters of each penalty item and re-planning the path when the optimization path detection result fails can increase the chance of path planning, thereby improving the success rate of path planning.

[0178] See also Figure 5 , Figure 5 1 is a flow chart of an embodiment of obtaining each initial planning path provided by the present application. In this embodiment, obtaining each initial planning path includes:

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

[0180] S52: Divide the navigation path line into a plurality of navigation path segments.

[0181] In summary, after the navigation path line is divided into multiple navigation path segments, for each navigation path segment, the present embodiment determines, based on the distance between the navigation path segment and each obstacle, to use the navigation path segment, or the path searched based on the navigation path segment, as the initial planned path corresponding to the navigation path segment.

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

[0183] Considering that the entire path between the current starting point and the target end point is very long, and for the vehicle, the path at the far end will not affect the current driving of the vehicle, the navigation path line is divided into multiple navigation path segments before path optimization, and then the path is planned segment by segment. The specific number of segmentation segments and the length of each navigation path segment can be determined based on actual experience.

[0184] S53: For each navigation path segment, based on a fourth distance between the navigation path segment and each obstacle, determine to use the navigation path segment as the corresponding initial planned path, or to use a path searched based on the navigation path segment as the corresponding initial planned path.

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

[0186] The preset distance threshold is a distance set to determine whether there is a high risk of collision. When 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 very likely to collide with the obstacle when traveling along the navigation path segment. When each fourth distance is greater than the preset distance threshold, it is considered that the probability of the vehicle colliding with the obstacle when traveling along the navigation path segment is small, and the navigation path segment can be used as the initial planning path of the segment for further path optimization to obtain an optimized path.

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

[0188] See also Figure 6 , Figure 6 yes Figure 5The flowchart of step S53 of an embodiment is shown. In this embodiment, the initial planning path is obtained based on the navigation path segment search, including:

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

[0190] In this embodiment, each initial drivable boundary can be first generated based on the navigation path line, and then it is determined whether there is a first intersection between the normal line of each target path point on the navigation path line and each target obstacle in the initial drivable area, and then based on the determination result of the first intersection between the normal line of each target path point and each target obstacle, the target boundary point corresponding to each target path point is determined; finally, the target boundary points of each target path point are combined to obtain the target drivable boundary.

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

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

[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 the coordinates of the positions of various points (including path points, boundary points, intersection points, etc.) and obstacles in this embodiment are expressed in a pre-established Cartesian coordinate system to accurately describe the above-mentioned positions using the Cartesian coordinate system.

[0196] Further, based on the determination result of the first intersection point between the normal line 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] Case 1: For each target path point, if there is no first intersection between the normal line of the target path point and each target obstacle, the second intersection point between the normal line and each initial drivable boundary is used as the target boundary point of the target path point. In fact, case 1 is that when there is no first intersection point, the boundary point on the initial drivable boundary corresponding to the target path point is used 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 of the passable space on the left and right sides of the target obstacle where the first intersection is located and the preset passable space, and determine the target boundary point of the target path point based on the third comparison result.

[0199] If the third comparison result is that the passable space on the left and right sides of the target obstacle where the first intersection is located is smaller than the preset passable space, it is determined that the target path point does not have a target boundary point. If the passable space on the left and right sides of the target obstacle where the first intersection is located is smaller than the preset passable space, it means that the road ahead is no longer passable. 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.

[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 used as the target boundary point of the target path point. Correspondingly, in this case, although there is a first intersection point with the target obstacle, 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 used as the target boundary point of the target path point.

[0201] If the third comparison result is that the passable space on one 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 that is not less than the preset passable space on the two sides is taken as the target side, and the first intersection point between the normal of the target path point and the target side is taken as a target boundary point of the target path point. Correspondingly, in this case, although there is a first intersection point with the target obstacle, one of the left and right sides of the target obstacle is passable, and the other side is not passable. In this case, the passable side can be taken as the target side, and the first intersection point between 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 an intersection is passable, the first intersection point with the right boundary of the target obstacle (obstacle boundary) 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 situations.

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

[0204] S62: Sampling in the target drivable area corresponding to the target drivable boundary based on the navigation path segment to obtain a plurality of sampling points.

[0205] In one embodiment, random sampling may be performed within the target drivable area to obtain sampling points.

[0206] In another embodiment, in order to increase the probability that a vehicle can pass, sampling may be performed according to the width of the drivable area. Specifically, the width of the drivable area is set to be negatively correlated with the density of the distribution of sampling points. That is, if the drivable area is wider, the corresponding sampling points are relatively sparse; if the drivable area is narrower, the corresponding sampling points are relatively dense.

[0207] In one implementation, forward sampling may be preferred, and a search is performed based on forward sampling points to obtain a corresponding search path.

[0208] In another embodiment, considering that in some scenarios, such as large curvature or narrow passages, searching for forward sampling points may not result in finding a corresponding path, i.e., a search failure occurs, but in fact the forward space is passable. To avoid this situation, sampling may be performed in the forward direction and the backward direction respectively to obtain sampling points, so that subsequent searches can be performed based on the sampling points to find a path that moves backward and forward.

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

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

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

[0212] In another embodiment, points may be taken at certain intervals along the navigation path segment to obtain a number of reference path points. The intervals may be taken at the same distance between any two points or at different distances. The specific intervals may be preset according to actual needs.

[0213] Second, for each reference path point, sampling is performed in the forward direction of the reference path point, and it is determined whether there is a sampling point in the forward direction of the reference path point where the search fails.

[0214] Third, in response to the presence of a sampling point where the search fails in the forward direction of the reference path point, and based on the forward drivable area, it is determined that the road is passable, sampling is performed in the backward direction and the forward direction 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 sampling points. For each reference point, if the forward direction search of the reference path point is determined to be successful, the corresponding forward path is searched based on the forward sampling point. If the forward search fails, that is, it is determined that there is a sampling point in the forward direction of the reference path point where the search fails, and it is determined that the road ahead is passable based on the forward drivable area, then sampling is performed in the backward direction and forward direction of the reference path point to obtain each sampling point, for example, Figure 8 As shown, sampling is performed backwards first and then forwards to search for a path that goes backwards and forwards. Sampling can be performed in a random sampling manner or in accordance with a pre-set rule. Figure 8 As shown, the backward sampling adopts a random sampling method, and the 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 a corresponding search path and use it as an initial planning path.

[0217] In one embodiment, a directed graph can be established in sequence between each sampling layer, and the nodes of the directed graph are connected by polynomial curves, such as Bezier 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 and obtain the corresponding search path, which is used as the initial planning path.

[0218] In one embodiment, considering that the polynomial curve has effective degrees of freedom, a slight collision behavior may be allowed during the path search process, and the collision behavior may be corrected by setting a penalty item related to the collision during subsequent path optimization.

[0219] It should be noted that, according to the actual road conditions, the above-mentioned sampling, directed graph establishment and search methods can effectively take into account the flexibility and passability of path generation, and plan the forward driving path when the forward search is successful. When the forward search fails (for example, blocked road conditions), backward plus forward sampling and search can be used to obtain a path that first reverses and then moves forward, so as to achieve escape from blocked road conditions.

[0220] It should also be noted that, in the above-mentioned method of using the navigation path segment as the corresponding initial planned path, or using the path obtained by searching based on the navigation path segment as the corresponding initial planned path, there will be different segments of initial planned paths, some of which are navigation path segments, and some of which are search path segments searched based on the navigation path segments, wherein different initial planned paths may have breakpoints. In this case, the two adjacent initial planned paths can be spliced.

[0221] See also Fig. 9 , Fig. 9 It is a schematic diagram of the framework of an embodiment of the path planning device provided by the present 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 item affecting the quality of the path to be optimized and the weight parameters of each penalty item; 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 item based on the detection result of the optimized path 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, the penalty items that affect the quality of the path to be optimized, and the weight parameters of each penalty item, including: constructing a penalty function corresponding to each penalty item based on the initial planned path, the weight parameters of each penalty item and the constraints of each penalty item; and constructing an objective function by combining the penalty functions corresponding to each penalty item.

[0223] In some embodiments, the penalty items that affect the quality of the path to be optimized include at least two of the following: a first penalty item regarding the distance between the path to be optimized and an associated obstacle, a second penalty item regarding the offset distance between the path to be optimized and an initial planned path, a third penalty item regarding the vehicle kinematics constraint of the path to be optimized, a fourth penalty item regarding the minimum turning radius constraint of the path to be optimized, and a fifth penalty item regarding the curvature change rate 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, the path to be optimized is the intermediate optimization state in the optimization of the initial planned path, the initial planned path includes a number of initial state points, and the path to be optimized includes a number of state points to be optimized; based on the initial planned path, the weight parameters of each penalty item and the constraints of each penalty item, a penalty function corresponding to each penalty item is constructed, including: in response to each penalty item including a first penalty item, based on the first distance between each state point to be optimized and the corresponding number of associated obstacles, the first weight parameter of the first penalty item, and the first distance constraint, a first penalty function of the first penalty item is constructed; in response to each penalty item including a second penalty item, a matching point of each initial state point is 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 the corresponding matching point, the second weight parameter of the second penalty item, and the second distance constraint. a second penalty function of the penalty item; in response to each penalty item including a third penalty item, based on the difference between the first state to be optimized of each adjacent two state points to be optimized, the third weight parameter of the third penalty item, and the kinematic constraint, a third penalty function of the third penalty item is constructed; wherein, the first state to be optimized includes the vehicle heading angle and position, and the kinematic constraint includes at least one of the speed constraint and the front wheel turning angle constraint; in response to each penalty item including a fourth penalty item, based on the difference between the second state to be optimized of each adjacent two state points to be optimized, the fourth weight parameter of the fourth penalty item and the maximum curvature constraint, a fourth penalty function of the third penalty item is constructed; wherein, the second state to be optimized includes the vehicle heading angle and position; in response to each penalty item including a fifth penalty item, based on the curvature change rate between three adjacent state points to be optimized, and the curvature change rate constraint, a fifth penalty function of the fifth penalty item is constructed.

[0225] In some embodiments, for each state point to be optimized, the first distance includes a sub-distance 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 number of associated obstacles, the first weight parameter of the first penalty item, and the first distance constraint, a first penalty function of the first penalty item 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 respectively; 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 based on the first weight parameter and the first sub-penalty function between the state point to be optimized and each associated obstacle to obtain a 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 item.

[0226] In some embodiments, based on the first comparison result and the first distance constraint condition, determining the first sub-penalty function between the state point to be optimized and each associated obstacle includes: in response to the first comparison result that the sub-distance is greater than the cutoff distance, using the first constant as the first sub-penalty function; in response to the first comparison result 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 sum of the first difference and the second difference. ratio, the first difference is the difference between the sub-distance and the cut-off distance, and the second difference is the difference between the expected minimum distance and the cut-off distance; in response to the first comparison result that the sub-distance is less than the expected minimum distance, the second product of the αth power of the third difference and the βth power of the expected minimum distance is used as the first sub-penalty function; wherein 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; wherein the expected minimum distance is less than the cut-off 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 a second obstacle closest to the state point to be optimized, and 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 respectively; weighting is performed based on the first weight parameter and a first sub-penalty function between the state point to be optimized and each associated obstacle to obtain a first weighted penalty function of the state point to be optimized, including: weighting is performed 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.

[0228] In some embodiments, a matching point of each initial state point is selected from each state point to be optimized, including: for each initial state point, a state point to be optimized that is closest to the initial state point among each state point to be optimized is used 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 item, and the second distance constraint, a second penalty function of the second penalty item 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 cutoff distance is obtained; based on the second comparison result and the second distance constraint, 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 item 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 by weighting; and the second weighted penalty function of each matching point is obtained by combining the second weighted penalty function of each matching point.

[0229] In some embodiments, based on the second comparison result and the second distance constraint, a second sub-penalty function between the initial state point and the corresponding matching point is determined, including: in response to the second comparison result being that the second distance is less than the cutoff distance, determining the first constant as the second sub-penalty function; in response to the second comparison result being that the second distance is not less than the cutoff distance, obtaining a fourth difference, a fourth product between the second distance and the target result, 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 to the nth power, 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 maximum of the absolute value of the curvature of the cutoff curvature and the initial state point, and the difference with the cutoff curvature; wherein the first constant is less than the fourth product.

[0230] In some embodiments, the adjustment determination 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: in response to the detection result of the optimized path being a first detection result, taking the optimized path as the target planning path; wherein, the first detection result indicates that the penalty items of the optimized path meet the corresponding preset requirements; in response 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 initial planned path, each penalty item affecting the quality of the path to be optimized and the weight parameters of each penalty item, constructing the objective function, and subsequent steps, until the latest detection result of the optimized path is the first detection result, taking the latest optimized path as the target planning path, or the number of detections as the first detection result reaches a preset number, stopping path planning; wherein, the second detection result indicates that there are penalty items in the penalty items of the optimized path that do not meet the corresponding preset requirements.

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

[0232] In some embodiments, each penalty item includes at least two of a first penalty item regarding the distance between the path to be optimized and the associated obstacle, a second penalty item regarding the offset distance between the path to be optimized and the initial planned path, a third penalty item regarding the vehicle kinematic constraint of the path to be optimized, a fourth penalty item regarding the minimum turning radius constraint of the path to be optimized, and a fifth penalty item regarding the curvature change rate constraint of the path to be optimized; adjusting the weight parameters of each penalty item based on the second detection result includes: in response to the second detection result that the first penalty item does not meet the corresponding preset requirement, increasing the weight parameter of the first penalty item, and / or lowering the associated penalty item corresponding to the first penalty item; The weight parameter of the penalty item; the preset requirement of 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 penalty item corresponding to the fourth penalty item is decreased; the preset requirement corresponding to 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 penalty item corresponding to the fifth penalty item is decreased; the preset requirement of the fifth penalty item is that the curvature change rate 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 items affecting the quality of the paths to be optimized and the weight parameters of the penalty items, it also includes: obtaining a navigation path line planned based on the current starting point and the target end point; dividing the navigation path line into multiple navigation path segments; for each navigation path segment, based on the fourth distance between the navigation path segment and each obstacle, determining to use the navigation path segment as the corresponding initial planned path, or to use the path obtained based on the search of the navigation path segment as the corresponding initial planned path; 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, based on the fourth distance between the navigation path segment and each obstacle, it is determined to use the navigation path segment as the corresponding initial planned path, or to use the path obtained by searching based on the navigation path segment as the corresponding initial planned path, including: in response to each fourth distance being greater than a preset distance threshold, using the navigation path segment as the initial planned path; in response to there being 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 planned path.

[0236] In some embodiments, the corresponding initial planned path is obtained based on the search of the navigation path segment, including: 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; performing a path search based on each sampling point to obtain a corresponding search path, and using it as the initial planned path.

[0237] In some embodiments, the width of the drivable area is negatively correlated with the density of the distribution of the sampling points; and / or, sampling is performed within the drivable area corresponding to the target drivable boundary based on the navigation path segment to obtain a number of sampling points, including: selecting a number of reference path points from the navigation path segment; for each reference path point, sampling in the forward direction of the reference path point, and determining whether there is a sampling point in the forward direction of the reference path point where the search failed; in response to the presence of a sampling point in the forward direction of the reference path point where the search failed, and determining that the drivable area is passable based on the forward direction, sampling in the backward direction and forward direction of the reference path point respectively to obtain each sampling point.

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

[0239] In some embodiments, 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, including: for each target path point, in response to the absence of a first intersection between the normal of the target path point and each target obstacle, the second intersection of the normal and each initial drivable boundary is used as the target boundary point of the target path point; in response to the existence of a first intersection between the normal of the target path point and each target obstacle, a third comparison result of the passable space on the left and right sides of the target obstacle where the first intersection is located and the preset passable space is determined, and based on the third comparison result, the target boundary point of the target path point is determined.

[0240] In some embodiments, 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 smaller than the preset passable space, determining that there is no target boundary point at the target path point; in response to the third comparison result that the passable space on each side is not smaller than the preset passable space, taking the normal of the target path point and the second intersection of 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 of the two sides is not smaller than the preset passable space, and the passable space on the other side is smaller than the preset passable space, taking the side of the two sides that is not smaller than the preset passable space as the target side, and taking the first intersection of the normal of the target path point and the target side as a target boundary point of the target path point.

[0241] See also Fig.10 , Fig.10 1 is a schematic diagram of a framework of an electronic device according to an embodiment of the present 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 is used to execute the program instructions stored in the memory 101 to implement the steps of any of the above method implementations. In a specific implementation scenario, the electronic device 100 may include but is not limited to: a microcomputer, a server, and in addition, the electronic device 100 may also include a mobile device such as a laptop computer and a tablet computer, which is not limited here.

[0243] Specifically, the processor 102 is used to control itself and the memory 101 to implement the steps of any of the above-mentioned embodiments. The processor 102 can also be called a CPU (Central Processing Unit). The processor 102 may be an integrated circuit chip with signal processing capabilities. The processor 102 can also be a general-purpose processor, a digital signal processor (Digital Signal Processor, DSP), an application-specific integrated circuit (Application Specific Integrated Circuit, ASIC), a field-programmable gate array (Field-Programmable Gate Array, FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor, etc. In addition, the processor 102 can be implemented by an integrated circuit chip.

[0244] See also Fig.11 , Fig.11 It is a schematic diagram of the framework of the computer-readable storage medium provided by the present application. The computer-readable storage medium 110 of the embodiment of the present application stores a program instruction 111, and the program instruction 111 is executed to implement the method provided by any embodiment of the above method and any non-conflicting combination. Among them, the program instruction 111 can form a program file and be stored in the above-mentioned computer-readable storage medium 110 in the form of a software product, so that a computer device (which can be a personal computer, a server, or a network device, etc.) executes all or part of the steps of each implementation method of the present application. The aforementioned computer-readable storage medium 110 includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a disk or an optical disk, or a terminal device such as a computer, a server, a mobile phone, and a tablet.

[0245] In the above scheme, after optimizing the constructed objective function and obtaining the optimized path, it is further determined whether to adjust the weight parameters of each penalty item based on the detection result of the optimized path, so as to construct a new objective function and optimize a new optimized path based on the adjusted weight parameters. Compared with the method of not adjusting the weight parameters of each penalty item according to the detection result, the method of adjusting the weight parameters of each penalty item and re-planning the path when the optimization path detection result fails can increase the chance of path planning, thereby improving the success rate of path planning.

[0246] In some embodiments, the functions or modules included in the device provided by the embodiments of the present disclosure can be used to execute the method described in the above method embodiments. The specific implementation can refer to the description of the above method embodiments, and for the sake of brevity, it will not be repeated here.

[0247] The above description of various embodiments tends to emphasize the differences between the various embodiments. The same or similar aspects can be referenced to each other, and for the sake of brevity, they will not be repeated herein.

[0248] In the several embodiments provided in the present application, it should be understood that the disclosed methods and devices can be implemented in other ways. For example, the device implementation described above is only schematic. For example, the division of modules or units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, and the indirect coupling or communication connection of devices or units can be electrical, mechanical or other forms.

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

[0250] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0251] If the integrated unit is implemented in the form of 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 the present application is essentially 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, and the computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, server, or network device, etc.) or a processor (processor) to perform all or part of the steps of each implementation method of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program code.

[0252] The above description is only an implementation method of the present application, and does not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly used in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A path planning method, characterized in that: The method comprises: Constructing an objective function based on the initial planned path, each penalty item affecting the quality of the path to be optimized, and a weight parameter of each penalty item; Optimizing the objective function to obtain an optimized path corresponding to the initial planned path; Based on the detection result of the optimization path, it is determined whether to adjust the weight parameters of each penalty item to re-execute the above steps.

2. The method according to claim 1, characterized in that The objective function is constructed based on the initial planned path, each penalty item affecting the quality of the path to be optimized and the weight parameter of each penalty item, including: Based on the initial planned path, the weight parameters of each penalty item and the constraint conditions of each penalty item, constructing a penalty function corresponding to each penalty item; The penalty functions corresponding to the penalty items are combined to construct the objective function.

3. The method according to claim 1 or 2, characterized in that: The penalty items that affect the quality of the path to be optimized include at least two of the following: a first penalty item regarding the distance between the path to be optimized and the associated obstacle, a second penalty item regarding the offset distance between the path to be optimized and the initial planned path, a third penalty item regarding the vehicle kinematics constraint of the path to be optimized, a fourth penalty item regarding the minimum turning radius constraint of the path to be optimized, and a fifth penalty item regarding the curvature change rate constraint of the path to be optimized.

4. The method according to claim 3, characterized in that The initial planned path is the initial state of the path to be optimized, the path to be optimized is the intermediate optimization state in the optimization of the initial planned path, the initial planned path includes a number of initial state points, and the path to be optimized includes a number of state points to be optimized; The constructing a penalty function corresponding to each penalty item based on the initial planned path, the weight parameter of each penalty item and the constraint condition of each penalty item comprises: In response to each of the penalty items including the first penalty item, constructing a first penalty function of the first penalty item based on a first distance between each state point to be optimized and a corresponding number of associated obstacles, a first weight parameter of the first penalty item, and a first distance constraint condition; In response to each of the penalty items including the second penalty item, selecting a matching point of each initial state point from each state point to be optimized, and constructing a second penalty function of the second penalty item based on a second distance between each initial state point and the corresponding matching point, a second weight parameter of the second penalty item, and a second distance constraint condition; In response to each of the penalty items including the third penalty item, a third penalty function of the third penalty item is constructed based on the difference between the first states to be optimized of each two adjacent state points to be optimized, a third weight parameter of the third penalty item, and a kinematic constraint; wherein the first state to be optimized includes a vehicle heading angle and position, and the kinematic constraint includes at least one of a speed constraint and a front wheel turning angle constraint; In response to each of the penalty items including the fourth penalty item, constructing a fourth penalty function of the third penalty item based on the difference between the second states to be optimized of each two adjacent state points to be optimized, a fourth weight parameter of the fourth penalty item and a maximum curvature constraint; wherein the second state to be optimized includes a vehicle heading angle and a position; In response to the penalty items including the fifth penalty item, a fifth penalty function of the fifth penalty item is constructed based on the curvature change rate between three adjacent state points to be optimized and a curvature change rate constraint condition.

5. The method according to claim 4, characterized in that For each state point to be optimized, the first distance includes a sub-distance between the state point to be optimized and each associated obstacle; The constructing a first penalty function of the first penalty item 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 item, and the first distance constraint condition includes: For each state point to be optimized, obtaining a first comparison result between each sub-distance of the state point to be optimized and a cutoff distance and a desired minimum distance; Determining a first sub-penalty function between the state point to be optimized and each associated obstacle based on the first comparison result and the first distance constraint condition; Performing weighting based on the first weight parameter and a first sub-penalty function between the state point to be optimized and each associated obstacle to obtain a first weighted penalty function of the state point to be optimized; The first weighted penalty function of each of the state points to be optimized is combined to obtain the first penalty function of the first penalty item.

6. The method according to claim 5, characterized in that The determining, based on the first comparison result and the first distance constraint condition, 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, a first product of the αth power of the first ratio and the βth power of the expected minimum distance is used 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 to 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, taking a second product of the αth power of the third difference and the βth power of the expected minimum distance as the first sub-penalty function; wherein the third difference is the difference between a third constant and a 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 a fourth constant and the sub-distance; Among them, the expected minimum distance is smaller than the cut-off distance; the first constant is smaller than the first product, and the first product is smaller than the second product.

7. The method according to claim 5, characterized in that Each associated obstacle of the state point to be optimized includes all first obstacles within the first distance range of the state point to be optimized and the second obstacle closest to the state point to be optimized, and 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 weighting based on the first weight parameter and the first sub-penalty function between the state point to be optimized and each associated obstacle to obtain the first weighted penalty function of the state point to be optimized includes: 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, a first weighted penalty function of the state point to be optimized is obtained.

8. The method according to claim 4, characterized in that The step of selecting matching points of each initial state point from each state point to be optimized includes: For each initial state point, a state point to be optimized that is closest to the initial state point among the state points to be optimized is used as a matching point of the initial state point; And / or, constructing a second penalty function of the second penalty item based on the second distance between each initial state point and the corresponding matching point, the second weight parameter of the second penalty item, and the second distance constraint condition, comprises: For each initial state point, obtaining a second comparison result between a second distance between the initial state point and the corresponding matching point and a cutoff distance; Determine 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 condition; Performing weighting based on a second weight parameter of the second penalty item and a second sub-penalty function between each of the initial state points and the corresponding matching point to obtain a second weighted penalty function for each of the matching points; The second weighted penalty function of each of the matching points is combined to obtain a second penalty function of the second penalty item.

9. The method according to claim 8, characterized in that The determining, based on the second comparison result and the second distance constraint, a second sub-penalty function between the initial state point and the corresponding matching point comprises: In response to the second comparison result being that the second distance is smaller than the cutoff distance, determining a first constant as the second sub-penalty function; In response to the second comparison result being that the second distance is not less than the cutoff distance, obtaining a fourth difference, a fourth product between the second distance and a target result, and using the fourth product as the second sub-penalty function; Among them, the fourth difference is the difference between the fifth constant and the fourth ratio, the fourth ratio is the ratio of the fifth product and 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 and e to the nth power, where e represents the base of the natural logarithm, and n is the sixth product of the seventh constant and the fifth difference, and the fifth difference is the maximum of the absolute value of the curvature of the truncation curvature and the initial state point, and the difference between the truncation curvature; The first constant is smaller than the fourth product.

10. The method according to claim 1, characterized in that The step of determining whether to adjust the weight parameters of each penalty item based on the detection result of the optimization path to re-execute the aforementioned steps comprises: In response to the detection result of the optimized path being a first detection result, the optimized path is used as a target planning path; wherein the first detection result indicates that each penalty item of the optimized path meets a corresponding preset requirement; In response to the detection result of the optimized path being the second detection result, the weight parameters of each penalty item are adjusted based on the second detection result, the adjusted weight parameters of each penalty item are used as new weight parameters of each penalty item, and the penalty items affecting the quality of the path to be optimized and the weight parameters of each penalty item based on the initial planned path and re-executed to construct an objective function and subsequent steps until the latest detection result of the optimized path is the first detection result, the latest optimized path is used as the target planned path, or the number of detections as the first detection result reaches a preset number, and the path planning is stopped; wherein, the second detection result indicates that there are penalty items in the penalty items of the optimized path that do not meet the corresponding preset requirements.

11. The method according to claim 10, characterized in that The adjustment of the weight parameter of each penalty item includes increasing the weight parameter by a first preset coefficient, or decreasing the weight parameter by a second preset coefficient, and the first preset coefficient is greater than the second preset coefficient.

12. The method according to claim 10, characterized in that Each of the penalty items includes at least two of a first penalty item regarding a distance between the path to be optimized and an associated obstacle, a second penalty item regarding an offset distance between the path to be optimized and the initial planned path, a third penalty item regarding a vehicle kinematic constraint of the path to be optimized, a fourth penalty item regarding a minimum turning radius constraint of the path to be optimized, and a fifth penalty item regarding a curvature change rate constraint of the path to be optimized; The adjusting the weight parameters of each penalty item based on the second detection result includes: In response to the second detection result that the first penalty item does not meet the corresponding preset requirement, increasing the weight parameter of the first penalty item, and / or reducing the weight parameter of the associated penalty item corresponding to the first penalty item; the preset requirement of the first penalty item is that the optimized path does not collide with an obstacle; In response to the second detection result that the fourth penalty item does not meet the corresponding preset requirement, increasing the weight parameter of the fourth penalty item, and / or lowering the weight parameter of the associated penalty item corresponding to the fourth penalty item; the preset requirement corresponding to 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 lowered; the preset requirement of the fifth penalty item is that the curvature change rate of the optimized path is not greater than the change threshold.

13. The method according to claim 12, characterized in that 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.

14. The method according to claim 1, characterized in that The number of the initial planned paths is multiple, Before constructing the objective function based on the initial planned path, each penalty item affecting the quality of the path to be optimized and the weight parameter of each penalty item, the method further includes: Get the navigation path line planned based on the current starting point and target end point; dividing the navigation path line into a plurality of navigation path segments; For each of the navigation path segments, based on the fourth distance between the navigation path segment and each obstacle, determine to use the navigation path segment as the corresponding initial planned path, or to use the path obtained by searching based on the navigation path segment as the corresponding initial planned path; each of the initial planned paths is an initial state corresponding to the path to be optimized, and the optimized path corresponding to each of the initial planned paths is a final optimized state corresponding to the path to be optimized.

15. The method according to claim 14, characterized in that The determining, based on a fourth distance between the navigation path segment and each obstacle, to use the navigation path segment as a corresponding initial planned path, or to use a path searched based on the navigation path segment as a corresponding initial planned path, includes: In response to each of the fourth distances being greater than a preset distance threshold, taking the navigation path segment as an initial planned path; In response to a fourth distance not greater than the preset distance threshold among the fourth distances, a path searched based on the navigation path segment is used as an initial planned path.

16. The method according to claim 14, characterized in that The corresponding initial planned path is obtained based on the navigation path segment search, including: Obtaining a 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 plurality of sampling points; A path search is performed based on each of the sampling points to obtain a corresponding search path, which is used as the initial planned path.

17. The method according to claim 16, characterized in that The width of the drivable area is negatively correlated with the density of the sampling point distribution; And / or, the sampling in the drivable area corresponding to the target drivable boundary based on the navigation path segment to obtain a plurality of sampling points includes: Selecting a number of reference path points from the navigation path segment; For each of the reference path points, sampling is performed in the forward direction of the reference path point, and determining whether there is a sampling point in the forward direction of the reference path point where the search fails; In response to the presence of a sampling point where the search fails in the forward direction of the reference path point, and based on the forward drivable area, it is determined that the area is passable, sampling is performed in the backward direction and the forward direction of the reference path point to obtain the sampling points.

18. The method according to claim 16, characterized in that The step of obtaining a 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; Determine whether there is a first intersection between the normal of each target path point on the navigation path and each target obstacle in the initial drivable area; the initial drivable area is the area between different initial drivable boundaries; Determining a target boundary point corresponding to each target path point based on a determination result of a first intersection point between a normal line of each target path point and each target obstacle; The target boundary points of each target path point are integrated to obtain the target drivable boundary.

19. The method according to claim 18, characterized in that The determining of the target boundary points corresponding to the target path points based on the determination results of the first intersections between the normal lines of the target path points and the target obstacles comprises: For each of the target path points, in response to the absence of a first intersection point between the normal line of the target path point and each target obstacle, taking a second intersection point of the normal line with each of the initial drivable boundaries as a target boundary point of the target path point; In response to the existence of a first intersection between the normal of the target path point and each target obstacle, a third comparison result between the passable space on the left and right sides of the target obstacle where the first intersection is located and the preset passable space is determined, and based on the third comparison result, the target boundary point of the target path point is determined.

20. The method according to claim 19, characterized in that The target obstacle is a static obstacle; And / or, determining the target boundary point of the target path point based on the third comparison result includes: In response to the third comparison result that the passable spaces on each side are smaller than the preset passable space, determining that the target path point does not have the 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 line of the target path point and each of the initial drivable boundaries as the target boundary point of the target path point; In response to the third comparison result that the passable space on one 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 of the two sides that is 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.

21. An electronic device, characterized in that: comprising a memory and a processor coupled to each other, The memory stores program instructions; The processor is used to execute the program instructions stored in the memory to implement the method according to any one of claims 1 to 20.

22. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores program instructions that can be run by a processor, and the program instructions can be executed by the processor to implement the method according to any one of claims 1 to 20.

Images