Path planning algorithm, storage medium and electronic equipment

Abstract

The invention provides a path planning algorithm, a storage medium and electronic equipment, and the algorithm comprises the steps: constructing a kinematic model in a vehicle driving process, and the kinematic model is used for limiting the relation of driving state vectors at two adjacent moments; constructing an augmented Lagrange trajectory planning model, wherein the augmented Lagrange trajectory planning model comprises a driving cost function sub-model and a Lagrange sub-model; obtaining an initial smoothing result of a driving path according to the kinematic model and the augmented Lagrange trajectory planning model; and performing secondary smoothing processing on the initial smoothing result to obtain a final smoothing path. In the scheme of the invention, the smooth path after the secondary smoothing processing can ensure that the track points on the smooth path basically meet the path constraint conditions in the vehicle driving process.

Description

Technical Field

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

[0002] The local path planning algorithm for autonomous driving is a key technology for generating short-term driving paths based on the real-time environmental information around the vehicle during driving. The algorithm needs to consider factors such as dynamic obstacles, road curvature, speed limits, etc. to ensure that the vehicle can drive safely and smoothly. The local path planning algorithm is one of the core technologies to ensure safe and smooth driving of vehicles in autonomous driving systems. Although the iLQR (Iterative Linear Quadratic Regulator) algorithm has a fast calculation speed and can handle nonlinear vehicle dynamics constraints, iLQR can only handle equality constraints and is not good enough for inequality constraints; although the AL-iLQR (Augmented Lagrangian Iterative Linear Quadratic Regulator) algorithm adds the processing of inequality constraints on the basis of the iLQR algorithm, at the end of the solution, it often causes an ill-conditioned solution due to the extremely large penalty parameters, resulting in the constraints of the driving trajectory sometimes not being met. Summary of the invention

[0003] The technical problem to be solved by the present invention is that the AL-iLQR local path planning algorithm in the prior art generates an ill-conditioned solution in certain circumstances, and further provides a path planning algorithm, a storage medium and an electronic device.

[0004] In a first aspect, the technical solution of the present application provides a path planning algorithm, including:

[0005] Constructing a kinematic model of the vehicle during driving, wherein the kinematic model is used to define the relationship between driving state vectors at two adjacent moments;

[0006] Constructing an augmented Lagrangian trajectory planning model, wherein the augmented Lagrangian trajectory planning model includes a driving cost function sub-model and a Lagrangian sub-model;

[0007] Obtaining an initial smoothing result of the driving path according to the kinematic model and the augmented Lagrangian trajectory planning model;

[0008] The initial smoothing result is subjected to secondary smoothing processing to obtain a final smoothing path.

[0009] In some embodiments of the path planning algorithm, the step of performing secondary smoothing on the initial smoothing result to obtain a final smoothed path includes:

[0010] Dividing the initial smoothing result into a number of trajectory segments according to the step length;

[0011] Obtain path constraints during vehicle driving;

[0012] Get the disturbance amount of the trajectory segment corresponding to each step length;

[0013] Correcting the disturbance amount of each of the trajectory segments according to the path constraint condition;

[0014] Each corrected trajectory segment is reconnected to obtain the final smooth path.

[0015] In some embodiments of the path planning algorithm, the step of obtaining path constraints during vehicle driving includes:

[0016] The comfort level of the vehicle's driving path is within the set comfort level range;

[0017] The smoothness of the vehicle's driving path is within a set smoothness range and the control vector corresponding to the vehicle's driving path tends to a set lower limit value;

[0018] The deviation between the vehicle's driving path and the reference baseline is less than the set deviation;

[0019] The change rate of the control vector corresponding to the vehicle's driving path is within the set change rate range;

[0020] The distance between the vehicle's driving path and the road obstacle is greater than the safe distance.

[0021] In some embodiments, the path planning algorithm is constructed by constructing an augmented Lagrangian trajectory planning model, wherein the augmented Lagrangian trajectory planning model includes a driving cost function sub-model and a Lagrangian sub-model:

[0022] The augmented Lagrangian trajectory optimization model is expressed as:

[0023]

[0024] Where J represents the cost function, represents the driving state vector, represents the control vector, x i represents the driving state sequence, u i represents the control sequence; λ represents the Lagrange multiplication operator, μ is the penalty coefficient, c(x i ) represents constraint conditions; the constraint conditions include equality constraints and inequality constraints.

[0025] The path planning algorithm described in some solutions, the augmented Lagrangian trajectory planning model is constructed, and the augmented Lagrangian trajectory planning model includes a driving cost function sub-model and a Lagrangian sub-model, including:

[0026] Construct the comfort cost function:

[0027] J comfort =0.5w acc (v 2 κ) 2 +0.5w jerk (2vaκ+v 3 κ′) 2 ;

[0028] Construct a smoothness cost function:

[0029] J shape =0.5(w κ κ 2 +w κ′ κ′ 2 +w u κ″ 2 );0

[0030] Construct a path offset cost function:

[0031] J ref =0.5w ref ((xx ref ) 2 +(yy ref ) 2 ) 2 ;

[0032] Construct the control vector deviation cost function:

[0033] J u =0.5(w clb c 2 (κ″ min -κ″)+w cub c 2 (κ″-κ″ max ));

[0034] Construct a safe distance cost function:

[0035] Js afe =0.5w safe c 2 (l safe -l);

[0036] Construct the total cost function as a sub-model of the driving cost function:

[0037] J=Jcomfort +J shape +J ref +J u +J safe ;

[0038] in:

[0039] w acc 、w jek 、w κ 、w κ′ 、w u 、w ref 、w clb 、w cub 、w safe represent weight values, v represents the vehicle speed, κ represents the curvature of the driving path, κ′ represents the first-order derivative of the curvature of the driving path, κ″ represents the second-order derivative of the curvature of the driving path, a represents the acceleration, and x ref and ref represents the coordinates of the reference trajectory point on the reference baseline, x and y represent the coordinates of the trajectory point on the vehicle's driving path, and κ min represents the minimum value of the curvature of the driving path, κ max Indicates the maximum curvature of the driving path, l safe Indicates safe distance;

[0040] c(x) represents a piecewise function, and its value result is:

[0041]

[0042] In some embodiments of the path planning algorithm, obtaining the disturbance amount of the trajectory segment corresponding to each step length includes:

[0043] The total disturbance of the initial smoothing result is obtained, which is expressed as a matrix δz and satisfies the following relationship:

[0044]

[0045] Wherein, Z represents the matrix obtained by concatenating the driving state vector and the control vector corresponding to each trajectory point in the initial smoothing result, T represents transpose, H matrix is ​​the Hessian matrix of the driving cost, D matrix is ​​the Jacobian matrix of the initial smoothing result that has not yet satisfied the path constraint condition, and d represents the specific value of the path constraint condition;

[0046] The relationship corresponding to the total disturbance is split into the disturbance function relationship corresponding to each step length of the trajectory segment:

[0047]

[0048] Where i represents the step number;

[0049] The step of correcting the disturbance amount of each trajectory segment according to the path constraint condition comprises:

[0050] The perturbation function of each step-length trajectory segment is converted into an augmented Lagrangian equation, and the optimal solution of the KKT condition of the augmented Lagrangian equation is obtained as the optimal perturbation δz of the trajectory segment. i ;

[0051] Each corrected trajectory segment is reconnected to obtain a final smooth path, including:

[0052] The optimal matrix Z corresponding to each trajectory segment is calculated based on the optimal disturbance amount of each trajectory segment. i ; The optimal matrix Z for each of the trajectory segments i The optimal matrix corresponding to the smooth path is obtained by concatenation.

[0053] In some embodiments, the path planning algorithm is used to calculate the optimal matrix Z corresponding to each trajectory segment according to the optimal disturbance amount of each trajectory segment. i ,include:

[0054] Constructing the optimal matrix calculation model:

[0055] Z i =Z+αδz i , 0≤α≤1;

[0056] The optimal solution of α in the optimal matrix calculation model is determined using a linear search method.

[0057] In a second aspect, the technical solution of the present application provides a computer-readable storage medium having a computer program / instruction stored thereon, which, when executed by a processor, implements the steps of the path planning algorithm described in any one of the first aspects.

[0058] In a third aspect, the technical solution of the present application provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory, characterized in that the processor executes the computer program to implement the steps of the path planning algorithm described in any one of the first aspects.

[0059] In a fourth aspect, the technical solution of the present application provides a computer program product, including a computer program / instruction, which, when executed by a processor, implements the steps of the path planning algorithm described in any one of the first aspects.

[0060] Compared with the prior art, the above technical solution provided by this application has at least the following technical effects:

[0061] The path planning algorithm, storage medium and electronic device provided by the present application, after constructing the kinematic model and augmented Lagrangian trajectory planning model of the vehicle during driving, first optimizes the trajectory of the driving path according to the augmented Lagrangian trajectory planning model, outputs the result as the initial smoothing result, and then performs secondary smoothing on the initial smoothing result to obtain the final smoothed path. In the present application scheme, the smoothed path after the secondary smoothing process can ensure that the trajectory points on it basically meet the path constraints during the vehicle driving process. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 A flow chart of a path planning algorithm according to an embodiment of the present application;

[0063] Figure 2a This is a schematic diagram of the distance relationship between the initial smoothed result output by the augmented Lagrangian trajectory planning model and the obstacle boundary during the obstacle avoidance process;

[0064] Figure 2b A schematic diagram of the distance relationship between the smoothed path obtained after secondary smoothing using the solution of the present application and the obstacle boundary during the obstacle avoidance process;

[0065] Figure 3 A schematic diagram of the hardware connection relationship of an electronic device that executes a path planning algorithm according to an embodiment of the present application. DETAILED DESCRIPTION

[0066] The specific implementation of the present application is further described below with reference to the accompanying drawings.

[0067] It is easy to understand that according to the technical solution of the present application, without changing the essential spirit of the present application, a variety of structural modes and implementation modes that can be replaced by those skilled in the art can be replaced with each other. Therefore, the following specific implementation modes and drawings are only exemplary descriptions of the technical solution of the present application, and should not be regarded as the entirety of the present application or as a limitation or restriction on the technical solution of the application.

[0068] The directional terms such as up, down, left, right, front, back, front, back, top, bottom, etc. mentioned or may be mentioned in this specification are defined relative to the structures shown in the drawings. They are relative concepts and may change accordingly according to different positions and different usage conditions. Therefore, these or other directional terms should not be interpreted as restrictive terms.

[0069] The local path planning algorithm plays a vital role in the autonomous driving system, mainly including the following aspects: 1. Dynamic obstacle avoidance: The local path planning algorithm perceives and processes dynamic obstacles on the road (such as pedestrians, vehicles, etc.) in real time, and generates a safe path for the autonomous driving vehicle to avoid obstacles, thereby avoiding collisions; 2. Adapting to complex road conditions: In actual driving, vehicles may encounter complex and changeable road conditions, such as sharp turns, narrow roads, ramps, etc. The local path planning algorithm can dynamically adjust the driving path according to these complex road conditions to ensure that the vehicle can pass smoothly and safely; 3. Real-time response to environmental changes: The local path planning algorithm can quickly respond to changes in the environment (such as sudden obstacles or changes in road conditions), and provide the vehicle with instant path adjustments, thereby improving driving safety; 4. Optimizing driving experience: In addition to safety, the local path planning algorithm also considers riding comfort and driving efficiency. By designing a smooth path, we can reduce sudden acceleration, sudden braking and sharp turns, and improve driving stability and efficiency; 5. Integrate multiple sensory information: The local path planning algorithm integrates information from multiple sensors (such as cameras, radars, lidars, etc.) to generate path planning results based on real-time sensory data, thereby enhancing the vehicle's navigation capabilities in complex environments. The local path planning algorithm must be efficient and real-time to cope with complex and changing road conditions and emergencies to ensure the safe operation of autonomous vehicles.

[0070] This embodiment provides a path planning algorithm that can be applied to a processing system. The processing system can be set in a vehicle terminal or a cloud server. Figure 1 As shown, the method includes:

[0071] S100: constructing a kinematic model of the vehicle during driving, wherein the kinematic model is used to define the relationship between driving state vectors at two adjacent moments.

[0072] The kinematic model is usually expressed as follows:

[0073] x i+1 =f(x i ,u i );

[0074] Among them, x i It can represent the state vector at the current moment, u i It can represent the control vector at the current moment. The control vector represents the change of the state vector. According to the motion law of the vehicle during driving, x can be obtained. i+1 That is, the kinematic model uses the initial state x of the system 0 Control vector u 0 , a series of state vectors x can be derived through the state transfer function f 1 to x n-1Because the vehicle will not suddenly change its strategy during driving, there is a certain relationship between the state vectors at two adjacent moments. For example, if the driving speed, acceleration and heading angle are determined, the position coordinates at the next moment can be estimated based on the position coordinates at the current moment, thereby controlling the change of the control vector.

[0075] x i+1 =x i +cos(θ i )·ds;

[0076] y i+1 =y i +sin(θ i )·ds;

[0077]

[0078] κ′ i+1 =κ′ i +κ″ i ·ds 2 ;

[0079] x=[x,y,θ,κ,κ′] T ;

[0080] u = [κ″];

[0081] Among them, the subscript i represents the step size corresponding to the current moment, the subscript i+1 represents the step size corresponding to the next moment; θ represents the heading angle, s represents the driving distance between two adjacent moments; g represents the state vector, and u represents the control vector.

[0082] S200: constructing an augmented Lagrangian trajectory planning model, wherein the augmented Lagrangian trajectory planning model includes a driving cost function sub-model and a Lagrangian sub-model.

[0083] The augmented Lagrangian trajectory optimization model is expressed as:

[0084]

[0085] Where J represents the cost function, represents the driving state vector, represents the control vector, x i represents the driving state sequence, u i represents the control sequence; λ represents the Lagrange multiplication operator, μ is the penalty coefficient, c(x i ) represents constraint conditions; the constraint conditions include equality constraints and inequality constraints.

[0086] Above, the driving cost function sub-model is expressed as J:

[0087]

[0088] x 0 represents the first quantity of the system input, U represents the control sequence, l f is the terminal cost function, x N is the driving state vector of the terminal input of the system, and l is the cost function from step 0 to step N-1.

[0089] The equality constraint condition can be expressed as h(x)=0, and the inequality constraint condition can be expressed as g(x)≤0.

[0090] S300: Obtaining an initial smoothing result of the driving path according to the kinematic model and the augmented Lagrangian trajectory planning model.

[0091] In the specific implementation, the Lagrangian multiplication operator λ and penalty coefficient μ in the Lagrangian sub-model are initialized; according to the current Lagrangian multiplication operator λ and penalty coefficient μ, as well as the initialized state vector and any given control vector, the minimum cost principle in the driving cost function sub-model is used to perform cyclic iterations to obtain the output trajectory, and the Lagrangian multiplication operator λ and penalty coefficient μ in the Lagrangian sub-model are updated using the state vector of each point on the output trajectory. After each update iteration, it is judged whether the state vector of each point on the output optimized trajectory meets the constraint conditions. If the constraint conditions are not met, the update iteration continues. If the constraint conditions are met, the update iteration can be stopped, and the Lagrangian multiplication operator λ, penalty coefficient μ, and control vector at the last update iteration are used as the optimal data, and the optimized initial smoothing result is obtained by iterating again.

[0092] S400: performing secondary smoothing processing on the initial smoothing result to obtain a final smoothed path.

[0093] In the above embodiment, after constructing the kinematic model and augmented Lagrangian trajectory planning model of the vehicle during driving, the trajectory of the driving path is first optimized according to the augmented Lagrangian trajectory planning model, and the output result is used as the initial smoothing result, and then the initial smoothing result is subjected to secondary smoothing to obtain the final smoothed path. In the present application, the smoothed path after the secondary smoothing can ensure that the trajectory points thereon basically meet the path constraints during the vehicle driving process.

[0094] In the specific implementation, for the conventional driving cost function sub-model, it is implemented based on the iLQR algorithm. In the above scheme, the kinematic model is usually nonlinear and needs to be linearized. Therefore, the kinematic model can be transformed into:

[0095]

[0096] The linearization process can be achieved by using Taylor series expansion and retaining only the first-order derivative result. i is f(x i ,u i ) for x i The gradient matrix, B i is f(x i ,u i ) i The gradient matrix of .

[0097] Furthermore, the total cost function J is Taylor expanded, assuming that the total cost function J takes the minimum value and is expressed as right Also perform Taylor expansion:

[0098] J is right Taylor expansion is also performed

[0099]

[0100] Q Correspondence The expanded gradient matrix, T represents the matrix transpose. Assume that the value function of J when taking the minimum value is It is only related to g, so its Taylor expansion can be obtained:

[0101]

[0102] So we can get the equation relationship:

[0103]

[0104] According to the above equation, the transfer matrix and its corresponding transposed matrix when the cost function is minimized can be determined.

[0105] For the augmented Lagrangian trajectory optimization model, the KKT condition is used to determine the conditional function of the augmented Lagrangian trajectory optimization model output path planning results; the conditional function is substituted into the augmented Lagrangian trajectory optimization model for iteration until the Lagrangian multiplication operator in the augmented Lagrangian trajectory optimization model converges to the optimum.

[0106] Specifically, using the KKT (Karush-Kuhn-Tucker conditions) conditions, we can know that the conditions for the new optimization problem to take the extreme value are:

[0107]

[0108] The last two equations above can be combined, because the purpose is to minimize the value of f in the above equation, and the constraint condition c is less than or equal to 0, so the Lagrange multiplication operator λ must be greater than or equal to 0, and c = 0 when the extreme value is taken. Combining the above two conditions, it can be seen that when the constraint condition c is less than 0, the Lagrange multiplication operator λ = 0; when the constraint condition c is equal to 0, the Lagrange multiplication operator λ can be any integer. In summary, the KKT condition is expressed as:

[0109]

[0110] When iteratively solving the control optimization problem, λ and μ are updated as follows:

[0111]

[0112] Among them, λ + and μ + represents the updated Lagrange multiplication operator and penalty coefficient of the i-th constraint.

[0113] During the operation of the above iterative method, the penalty factor μ may become very large at the end, resulting in the constraint condition not being satisfied during the calculation. On this basis, the present application introduces a path constraint condition to perform secondary smoothing. In specific implementation, the initial smoothing result is subjected to secondary smoothing processing as described in the above step S400 to obtain the final smoothed path, including:

[0114] S401: Divide the initial smoothing result into a number of trajectory segments according to the step length.

[0115] S402: Obtain path constraint conditions during vehicle travel.

[0116] S403: Obtain the disturbance amount of the trajectory segment corresponding to each step length.

[0117] S404: Correcting the disturbance amount of each trajectory segment according to the path constraint condition.

[0118] S405: Each corrected trajectory segment is reconnected to obtain a final smooth path.

[0119] The above-mentioned secondary smoothing process is transformed into the following algorithm optimization problem:

[0120] The total disturbance of the initial smoothing result is obtained, which is expressed as a matrix δz and satisfies the following relationship:

[0121]

[0122] Among them, Z represents the matrix obtained by concatenating the driving state vector and the control vector corresponding to each trajectory point in the initial smoothing result, T represents the transpose, the H matrix is ​​the Hessian matrix of the driving cost, the D matrix is ​​the Jacobian matrix of the initial smoothing result that has not yet satisfied the path constraint, and d represents the specific value of the path constraint. If the matrix corresponding to the entire initial smoothing result is used to solve the disturbance at one time to make it all satisfy the constraints, the solved matrix will be too large. Therefore, this solution further splits the above optimization problem and performs operations according to the step size to reduce the amount of operations and speed up the solution process. That is, the relationship corresponding to the total disturbance is split into the disturbance function relationship corresponding to the trajectory segment of each step length:

[0123]

[0124] Here, i represents the step number.

[0125] That is, the solution of the AL-iLQR algorithm for the entire n steps is optimized and converted into solving a perturbation of one step each time, and correcting the trajectory segments of a single step with unsatisfied constraints little by little.

[0126] Since the optimization of a single step length has only one equality constraint, the augmented Lagrangian algorithm can be used to solve it, that is, the disturbance amount of each trajectory segment is corrected according to the path constraint condition, including: converting the disturbance amount function of the trajectory segment of each step length into an augmented Lagrangian equation, solving the augmented Lagrangian equation KKT (Karush-Kuhn-Tucker Conditions) condition optimal solution as the optimal disturbance amount δz of the trajectory segment i :

[0127] The KKT condition is expressed as:

[0128]

[0129] The present application solution only needs to solve the KKT equation to obtain the optimal disturbance δz i .

[0130] Each of the modified trajectory segments is reconnected to obtain a final smooth path, including: calculating the optimal matrix Z corresponding to each trajectory segment according to the optimal disturbance amount of each trajectory segment i ; The optimal matrix Z for each of the trajectory segments i The optimal matrix corresponding to the smooth path is obtained by splicing. Specifically:

[0131] Constructing the optimal matrix calculation model:

[0132] Z i =Z+αδzi , 0≤α≤1;

[0133] The optimal solution of α in the optimal matrix calculation model is determined using a linear search method.

[0134] Through the above-mentioned solution of the present application, it can be ensured that the trajectory segment corresponding to each step length on the smooth path can meet the constraint conditions.

[0135] Preferably, in the above solution, the step S402 of obtaining the path constraint conditions during vehicle driving includes:

[0136] The comfort level of the vehicle's driving path is within the set comfort level range;

[0137] The smoothness of the vehicle's driving path is within a set smoothness range and the control vector corresponding to the vehicle's driving path tends to a set lower limit value;

[0138] The deviation between the vehicle's driving path and the reference baseline is less than the set deviation;

[0139] The change rate of the control vector corresponding to the vehicle's driving path is within the set change rate range;

[0140] The distance between the vehicle's driving path and the road obstacle is greater than the safe distance.

[0141] Then, the augmented Lagrangian trajectory planning model is constructed in step S200, and the augmented Lagrangian trajectory planning model includes a driving cost function sub-model and a Lagrangian sub-model, including:

[0142] Construct the comfort cost function:

[0143] J comfort =0.5w acc (v 2 κ) 2 +0.5w jerk (2vaκ+v 3 κ′) 2 ;

[0144] Construct a smoothness cost function:

[0145] J shape =0.5(w κ κ 2 +w κ′ κ′ 2 +w u κ″ 2 );0

[0146] Construct a path offset cost function:

[0147] J ref =0.5w ref((xx ref ) 2 +(yy ref ) 2 ) 2 ;

[0148] Construct the control vector deviation cost function:

[0149] J u =0.5(w clb c 2 (κ″ min -κ″)+w cub c 2 (κ″-κ″ max ));

[0150] Construct a safe distance cost function:

[0151] J safe =0.5w safe c 2 (l safe -l);

[0152] Construct the total cost function as a sub-model of the driving cost function:

[0153] J=J comfort +J shope +J ref +J u +J safe ;

[0154] in:

[0155] w acc 、w jerk 、w κ 、w κ ′、w u 、w ref 、w clb 、w cub 、w safe represent weight values, v represents the vehicle speed, κ represents the curvature of the driving path, κ′ represents the first-order derivative of the curvature of the driving path, κ″ represents the second-order derivative of the curvature of the driving path, a represents the acceleration, and x ref and ref represents the coordinates of the reference trajectory point on the reference baseline, x and y represent the coordinates of the trajectory point on the vehicle's driving path, and κ min represents the minimum value of the curvature of the driving path, κ max Indicates the maximum curvature of the driving path, l safe Indicates safe distance;

[0156] c(x) represents a piecewise function, and its value result is:

[0157]

[0158] In the above scheme, each weight value can be determined based on empirical values.

[0159] By constraining the path planning results through the various constraints provided in this application, driving safety can be ensured. Figure 2a and Figure 2b As shown, Figure 2a The figure shows the initial smoothing result without secondary smoothing. It can be seen from the figure that the vehicle edge (blue frame edge) corresponding to the initial smoothing result has overlapped with the obstacle edge (indicated by the green line). Figure 2b After the second smoothing, there is a certain safety distance between the edge of the vehicle and the edge of the obstacle. Therefore, the present application scheme greatly improves the ill-conditioned solution in the original optimization result.

[0160] An embodiment of the present application also provides a computer-readable storage medium having a computer program / instruction stored thereon. When the computer program / instruction is executed by a processor, the steps of the path planning algorithm described in any one of the schemes in the method embodiment are implemented.

[0161] The present application also provides an electronic device, such as Figure 3As shown, the electronic device includes at least one processor 31 and at least one memory 32, at least one of the memories 32 stores program information, and at least one of the processors 31 reads the program information and executes the path planning algorithm described in any one of the above method embodiments. The device may also include: an input device 33 and an output device 34. The processor 31, the memory 32, the input device 33 and the output device 34 can be connected in communication. The memory 32, as a non-volatile computer-readable storage medium, can be used to store non-volatile software programs, non-volatile computer executable programs and modules. The processor 31 executes various functional applications and data processing by running the non-volatile software programs, instructions and modules stored in the memory 32, that is, the path planning algorithm provided by any of the above schemes is implemented. The memory 32 may include a program storage area and a data storage area, wherein the program storage area may store an operating system and an application required by at least one function; the data storage area may store data created according to the use of the path planning algorithm, etc. In addition, the memory 32 may include a high-speed random access memory, and may also include a non-volatile memory, such as at least one disk storage device, a flash memory device, or other non-volatile solid-state storage devices. In some embodiments, the memory 32 may optionally include a memory remotely arranged relative to the processor 31, and these remote memories may be connected to a device for executing the path planning algorithm via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and a combination thereof. The input device 33 may receive input user clicks, and generate signal inputs related to user settings and function controls of the path planning algorithm. The output device 34 may include a display device such as a display screen. When the one or more modules are stored in the memory 32, when they are run by the one or more processors 31, the path planning algorithm in any of the above-mentioned method embodiments is executed.

[0162] As needed, the above technical solutions can be combined to achieve the best technical effect.

[0163] The above are only the principles and preferred embodiments of the present application. It should be noted that, for ordinary technicians in this field, on the basis of the principles of the present application, several other modifications can be made, which should also be considered as the protection scope of the present application.

Claims

1. A path planning algorithm, characterized in that: include: Constructing a kinematic model of the vehicle during driving, wherein the kinematic model is used to define the relationship between driving state vectors at two adjacent moments; Constructing an augmented Lagrangian trajectory planning model, wherein the augmented Lagrangian trajectory planning model includes a driving cost function sub-model and a Lagrangian sub-model; Obtaining an initial smoothing result of the driving path according to the kinematic model and the augmented Lagrangian trajectory planning model; The initial smoothing result is subjected to secondary smoothing processing to obtain a final smoothing path.

2. The path planning algorithm according to claim 1, characterized in that: The performing secondary smoothing on the initial smoothing result to obtain a final smoothing path includes: Dividing the initial smoothing result into a number of trajectory segments according to the step length; Obtain path constraints during vehicle driving; Get the disturbance amount of the trajectory segment corresponding to each step length; Correcting the disturbance amount of each of the trajectory segments according to the path constraint condition; Each corrected trajectory segment is reconnected to obtain the final smooth path.

3. The path planning algorithm according to claim 2, characterized in that: The obtaining of the path constraint conditions during vehicle driving includes: The comfort level of the vehicle's driving path is within the set comfort level range; The smoothness of the vehicle's driving path is within a set smoothness range and the control vector corresponding to the vehicle's driving path tends to a set lower limit value; The deviation between the vehicle's driving path and the reference baseline is less than the set deviation; The change rate of the control vector corresponding to the vehicle's driving path is within the set change rate range; The distance between the vehicle's driving path and the road obstacle is greater than the safe distance.

4. The path planning algorithm according to claim 3, characterized in that: The augmented Lagrangian trajectory planning model is constructed, and the augmented Lagrangian trajectory planning model includes a driving cost function sub-model and a Lagrangian sub-model: The augmented Lagrangian trajectory optimization model is expressed as: Where J represents the cost function, represents the driving state vector, represents the control vector, x i represents the driving state sequence, u i represents the control sequence; λ represents the Lagrange multiplication operator, μ is the penalty coefficient, c(x i ) represents constraint conditions; the constraint conditions include equality constraints and inequality constraints.

5. The path planning algorithm according to claim 4, characterized in that: The augmented Lagrangian trajectory planning model is constructed, and the augmented Lagrangian trajectory planning model includes a driving cost function sub-model and a Lagrangian sub-model, including: Construct the comfort cost function: j comfort =0.5w acc (v 2 k) 2 +0.5w jerk (2vaκ+v 3 k′) 2 ; Construct a smoothness cost function: J shape =0.5(w κ k 2 +w κ′ k′ 2 +w u k″ 2 );0 Construct a path offset cost function: J ref =0.5w ref ((xx) ref ) 2 +(yy ref ) 2 ) 2 ; Construct the control vector deviation cost function: J u =0.5(W clb c 2 (K″ min -k″)+w cub c 2 (k″-k″ max )); Construct a safe distance cost function: <h2 style=";text-align:left;direction:ltr">J<h2 style=";text-align:left;direction:ltr"> safe <h2 style=";text-align:left;direction:ltr"> <0.5w<h2 style=";text-align:left;direction:ltr"> safe <h2 style=";text-align:left;direction:ltr"> c<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> (l<h2 style=";text-align:left;direction:ltr"> safe <h2 style=";text-align:left;direction:ltr"> -l); Construct the total cost function as a sub-model of the driving cost function: J=J comfort +J shape +J ref +J u +J safe ; in: w acc 、w jerk 、w κ 、w κ′ 、w u 、w ref 、w clb 、w cub 、w safe represent weight values, v represents the vehicle speed, κ represents the curvature of the driving path, κ′ represents the first-order derivative of the curvature of the driving path, κ″ represents the second-order derivative of the curvature of the driving path, a represents the acceleration, and x ref and ref represents the coordinates of the reference trajectory point on the reference baseline, x and y represent the coordinates of the trajectory point on the vehicle's driving path, and κ min represents the minimum value of the curvature of the driving path, κ max Indicates the maximum curvature of the driving path, l safe Indicates safe distance; c(x) represents a piecewise function, and its value result is:

6. The path planning algorithm according to claim 4 or 5, characterized in that: The step of obtaining the disturbance amount of the trajectory segment corresponding to each step length includes: The total disturbance of the initial smoothing result is obtained, which is expressed as a matrix δz and satisfies the following relationship: Wherein, Z represents the matrix obtained by concatenating the driving state vector and the control vector corresponding to each trajectory point in the initial smoothing result, T represents transpose, H matrix is ​​the Hessian matrix of the driving cost, D matrix is ​​the Jacobian matrix of the initial smoothing result that has not yet satisfied the path constraint condition, and d represents the specific value of the path constraint condition; The relationship corresponding to the total disturbance is split into the disturbance function relationship corresponding to each step length of the trajectory segment: Where i represents the step number; The step of correcting the disturbance amount of each trajectory segment according to the path constraint condition comprises: The perturbation function of each step-length trajectory segment is converted into an augmented Lagrangian equation, and the optimal solution of the KKT condition of the augmented Lagrangian equation is obtained as the optimal perturbation δz of the trajectory segment. i ; Each corrected trajectory segment is reconnected to obtain a final smooth path, including: The optimal matrix Z corresponding to each trajectory segment is calculated based on the optimal disturbance amount of each trajectory segment. i ; The optimal matrix Z for each of the trajectory segments i The optimal matrix corresponding to the smooth path is obtained by concatenation.

7. The path planning algorithm according to claim 6, characterized in that: The optimal matrix Z corresponding to each trajectory segment is calculated based on the optimal disturbance amount of each trajectory segment. i ,include: Constructing the optimal matrix calculation model: WITH i =Z+αδz i ,0≤α≤1; The optimal solution of α in the optimal matrix calculation model is determined using a linear search method.

8. A computer-readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instructions are executed by a processor, the steps of the path planning algorithm described in any one of claims 1-7 are implemented.

9. An electronic device comprising a memory, a processor and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the path planning algorithm according to any one of claims 1-7.

10. A computer program product comprising a computer program / instructions, characterized in that When the computer program / instructions are executed by a processor, the steps of the path planning algorithm described in any one of claims 1-7 are implemented.

Images