AL-iLQR path planning method based on iLQR, storage medium and electronic equipment

Abstract

The invention provides an AL-iLQR path planning method based on iLQR, a storage medium and electronic equipment. The AL-iLQR path planning method comprises the following steps: constructing a trajectory optimization model based on an iLQR algorithm; constructing a kinematic model in the vehicle driving process; an augmented Lagrange trajectory optimization model is constructed; constraint conditions are obtained, the constraint conditions are divided into a first constraint condition and a second constraint condition according to whether the constraint conditions are equality constraints or not, the first constraint condition is an equality constraint condition, and the second constraint condition is an inequality constraint condition; performing loop iteration on the augmented Lagrangian trajectory optimization model by combining a state vector in a vehicle driving process according to a constraint condition until a control vector in the trajectory optimization model is optimal and a Lagrangian multiplication operator in the Lagrangian operator model is optimal, and obtaining a final path planning model; and planning a vehicle driving path by using the path planning model. According to the method, rapid calculation can be carried out aiming at the equality constraint condition and the inequality constraint condition.

Description

Technical Field

[0001] The present application relates to the technical field of path planning during vehicle automatic driving, and specifically to an AL-iLQR (Augmented Lagrangian Iterative Linear Quadratic Regulator) path planning method, storage medium and electronic device based on iLQR (Iterative Linear Quadratic Regulator). 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 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, the vehicle 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. Through the design of smooth paths, the situations of sudden acceleration, sudden braking and sharp turns are reduced, and the driving stability and efficiency are improved; 5. Fusion of multiple perception 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 perception data, thereby enhancing the vehicle's navigation ability in complex environments. The local path planning algorithm is one of the core technologies in the autonomous driving system to ensure the safety and smooth driving of the vehicle.

[0003] Common local path planning algorithms include lattic planner (sampling method), polynomial interpolation, lateral path planning algorithm based on osqp (Operator Splitting Quadratic Programming, quadratic programming solver), and planning algorithm based on iLQR. The sampling method generates multiple candidate paths in front of the vehicle and selects the best path based on safety and comfort. The sampling method has a large amount of calculation and is slow. The sampled plan is not necessarily the optimal trajectory; polynomial interpolation, such as the quintic polynomial interpolation algorithm, is applicable to limited scenarios and has poor physical perception; the lateral path planning algorithm based on osqp is fast, but it does not directly consider the vehicle dynamics constraints in the solution process. The calculated path may be uncomfortable or the result cannot be output when the solution fails, which is very dangerous during the driving process of autonomous driving; although the iLQR algorithm has a fast calculation speed and can handle nonlinear vehicle dynamics constraints, iLQR can only handle equality constraints and has poor processing effect on inequality constraints. Summary of the invention

[0004] The present application aims to solve the problem that there is a lack of a path planning solution in the prior art that can both quickly calculate and handle inequality constraints, and further provides an AL-iLQR path planning method, storage medium and electronic device based on iLQR.

[0005] In a first aspect, the technical solution of the present application provides an AL-iLQR path planning method based on iLQR, comprising:

[0006] Constructing a trajectory optimization model based on the iLQR algorithm, wherein input data of the trajectory optimization model includes a driving state vector and a control vector;

[0007] 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;

[0008] Constructing an augmented Lagrangian trajectory optimization model, wherein the augmented Lagrangian trajectory optimization model adds a Lagrangian operator model on the basis of the trajectory optimization model;

[0009] Obtaining a constraint condition of the driving state vector, and dividing the constraint condition into a first constraint condition and a second constraint condition according to whether the constraint condition is an equality constraint, wherein the first constraint condition is an equality constraint condition, and the second constraint condition is an inequality constraint condition;

[0010] The first constraint condition is used as the constraint condition of the trajectory optimization model, the second constraint condition is used as the constraint condition of the Lagrangian operator model, and the augmented Lagrangian trajectory optimization model is iterated cyclically in combination with the state vector of the vehicle during driving, until the control vector in the trajectory optimization model is optimal and the Lagrangian multiplication operator in the Lagrangian operator model is optimal, thereby obtaining a final path planning model;

[0011] The path planning model is used to plan the vehicle's driving path.

[0012] In some schemes, the AL-iLQR path planning method based on iLQR algorithm is described, wherein the trajectory optimization model based on the iLQR algorithm is constructed, and the input data of the trajectory optimization model includes a driving state vector and a control vector, and the trajectory optimization model is expressed as:

[0013]

[0014] The kinematic model of the vehicle during driving is constructed. The kinematic model is used to define the relationship between the driving state vectors at two adjacent moments. The kinematic model is expressed as:

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

[0016] The augmented Lagrangian trajectory optimization model is constructed. The augmented Lagrangian trajectory optimization model adds a Lagrangian operator model on the basis of the trajectory optimization model. The augmented Lagrangian trajectory optimization model is expressed as:

[0017]

[0018] 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 a constraint.

[0019] The iLQR-based AL-iLQR path planning method described in some schemes, wherein the first constraint condition is used as the constraint condition of the trajectory optimization model, the second constraint condition is used as the constraint condition of the Lagrangian operator model, and the augmented Lagrangian trajectory optimization model is iterated cyclically in combination with the state vector of the vehicle during driving, until the control vector in the trajectory optimization model is optimal, and the Lagrangian multiplication operator in the Lagrangian operator model is optimal, thereby obtaining the final path planning model, including:

[0020] Using KKT conditions to determine the conditional function of the augmented Lagrangian trajectory optimization model output path planning results;

[0021] Substituting the conditional function into the augmented Lagrangian trajectory optimization model for iteration until the Lagrangian multiplication operator in the augmented Lagrangian trajectory optimization model converges to an optimum.

[0022] 4. The AL-iLQR path planning method based on iLQR according to claim 3 is characterized in that, in the conditional function of using KKT conditions to determine the output path planning result of the augmented Lagrangian trajectory optimization model, the conditional function includes:

[0023]

[0024] Among them, the constraints are c(x)≤0, λ≥0.

[0025] In some embodiments, the AL-iLQR path planning method based on iLQR, in the conditional function, the constraint condition c(x i ) takes an extreme value, that is, c(x i )=0, the conditional function is expressed as:

[0026]

[0027] The Lagrange multiplication operator λ and the penalty coefficient μ in the conditional function are updated as follows:

[0028]

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

[0030] In some schemes, the AL-iLQR path planning method based on iLQR is described, wherein the first constraint condition is used as the constraint condition of the trajectory optimization model, the second constraint condition is used as the constraint condition of the Lagrangian operator model, and the augmented Lagrangian trajectory optimization model is iterated cyclically in combination with the state vector of the vehicle during driving, until the control vector in the trajectory optimization model is optimal, and the Lagrangian multiplication operator in the Lagrangian operator model is optimal, thereby obtaining the final path planning model:

[0031] Performing Taylor series expansion on the trajectory optimization model for linearization;

[0032] Regularization is performed on the gradient matrix Q in the operation function obtained after the Taylor series expansion.

[0033] In a second aspect, the technical solution of the present application provides an AL-iLQR path planning device based on iLQR, comprising:

[0034] An iLQR algorithm model building module is configured to build a trajectory optimization model based on the iLQR algorithm, wherein input data of the trajectory optimization model includes a driving state vector and a control vector;

[0035] A kinematic model building module is configured to build 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;

[0036] An AL-iLQR model building module is configured to build an augmented Lagrangian trajectory optimization model, wherein the augmented Lagrangian trajectory optimization model adds a Lagrangian operator model on the basis of the trajectory optimization model;

[0037] a constraint condition acquisition module, configured to acquire the constraint condition of the driving state vector, and divide the constraint condition into a first constraint condition and a second constraint condition according to whether the constraint condition is an equality constraint, the first constraint condition being an equality constraint condition, and the second constraint condition being an inequality constraint condition;

[0038] a data iteration module, configured to use the first constraint condition as a constraint condition of the trajectory optimization model, use the second constraint condition as a constraint condition of the Lagrangian operator model, and perform cyclic iteration on the augmented Lagrangian trajectory optimization model in combination with the state vector of the vehicle during driving, until the control vector in the trajectory optimization model reaches an optimal value, the Lagrangian multiplication operator in the Lagrangian operator model reaches an optimal value, and a final path planning model is obtained;

[0039] The trajectory optimization module is configured to plan the vehicle driving path using the path planning model.

[0040] In a third 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 iLQR-based AL-iLQR path planning method described in any one of the first aspects.

[0041] In a fourth 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 iLQR-based AL-iLQR path planning method described in any one of the first aspects.

[0042] In a fifth aspect, the present application provides a computer program product, comprising a computer program / instruction, characterized in that when the computer program / instruction is executed by a processor, the steps of the iLQR-based AL-iLQR path planning method described in any one of the first aspects are implemented.

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

[0044] The iLQR-based AL-iLQR path planning method, storage medium and electronic device provided in the present application obtain an augmented Lagrangian trajectory optimization model by introducing a Lagrangian operator model on the basis of a trajectory optimization model based on the iLQR algorithm, thereby enabling the augmented Lagrangian trajectory optimization model to quickly calculate both equality constraints and inequality constraints. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 This is a flow chart of an AL-iLQR path planning method based on iLQR according to an embodiment of the present invention;

[0046] Figure 2 A schematic diagram of the positional relationship between the optimized trajectory output by the iLQR-based AL-iLQR path planning method according to an embodiment of the present invention and the road centerline;

[0047] Figure 3 A schematic diagram showing a comparison between the heading angle of the optimized trajectory output by the AL-iLQR path planning method based on iLQR according to an embodiment of the present invention and the heading angle of the optimized trajectory in the prior art;

[0048] Figure 4 A schematic diagram showing a comparison between the heading angle of the optimized trajectory output by the AL-iLQR path planning method based on iLQR according to an embodiment of the present invention and the curvature of the optimized trajectory in the prior art;

[0049] Figure 5A framework diagram of an AL-iLQR path planning method based on iLQR according to an embodiment of the present application;

[0050] Figure 6 A structural block diagram of an AL-iLQR path planning device based on iLQR according to an embodiment of the present invention;

[0051] Figure 7 A schematic diagram of the hardware connection relationship of an electronic device that executes the iLQR-based AL-iLQR path planning method according to an embodiment of the present invention. DETAILED DESCRIPTION

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

[0053] 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.

[0054] 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.

[0055] This embodiment provides an AL-iLQR path planning method based on iLQR, such as Figure 1 As shown, the following steps are included:

[0056] S100: constructing a trajectory optimization model based on the iLQR algorithm, wherein input data of the trajectory optimization model includes a driving state vector and a control vector.

[0057] The basic trajectory initial optimization model based on the iLQR algorithm is expressed as follows:

[0058]

[0059] x0 represents the first state vector input by the iLQR algorithm, U represents the control vector, J is the driving cost, l f is the terminal cost function, g Nis the state vector of the terminal input of the system, l is the cost function of the kth step, and the summation formula represents the sum of the cost functions of the N-1 steps corresponding to the step length corresponding to the initial moment to the step length corresponding to the final moment. In the present application scheme, at the initial stage, x0 is a known quantity, and U can be given an arbitrary value.

[0060] S200: 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.

[0061] The vehicle kinematic model is usually expressed as follows:

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

[0063] 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 system's initial state x0 to control the vector u0, and a series of state vectors x1 to x2 can be derived through the state transfer function f. n-1 Because 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.

[0064] S300: constructing an augmented Lagrangian trajectory optimization model, wherein the augmented Lagrangian trajectory optimization model adds a Lagrangian operator model on the basis of the trajectory optimization model.

[0065] Specifically, the Lagrangian operator model can be expressed as λ represents the Lagrange multiplication operator, μ is the penalty coefficient, c(x i ) represents a constraint.

[0066] S400: Obtaining a constraint condition of a driving state vector, and dividing the constraint condition into a first constraint condition and a second constraint condition according to whether the constraint condition is an equality constraint, wherein the first constraint condition is an equality constraint condition and the second constraint condition is an inequality constraint condition.

[0067] The first constraint can be expressed as h(x)=0, and the second constraint can be expressed as g(x)≤0. The combination of the two is c(x i ).

[0068] S500: Using the first constraint as the constraint of the trajectory optimization model, using the second constraint as the constraint of the Lagrangian operator model, and combining the state vector of the vehicle during driving to iterate the augmented Lagrangian trajectory optimization model in a loop until the control vector in the trajectory optimization model reaches an optimal state, and the Lagrangian multiplication operator in the Lagrangian operator model reaches an optimal state, thereby obtaining a final path planning model.

[0069] In the specific implementation, the Lagrangian multiplication operator λ and the penalty coefficient μ in the Lagrangian operator model are initialized and added; according to the current Lagrangian multiplication operator λ and the penalty coefficient μ, as well as the initialized state vector and any given control vector, the initial optimization trajectory is calculated using the trajectory initial optimization model based on the iLQR algorithm. The state vector of each point on the initial optimization trajectory is used to iteratively update the trajectory initial optimization model based on the iLQR algorithm again, and the Lagrangian multiplication operator λ and the penalty coefficient μ in the Lagrangian operator model are also added for update. After each update iteration, it is judged whether the state vector of each point on the output optimization 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 λ, the penalty coefficient μ, and the control vector at the last update iteration are used as the optimal data, thereby obtaining the final path planning model.

[0070] S600: Plan a vehicle travel path using the path planning model.

[0071] After obtaining the path planning model, the initial state vector is input to plan the driving trajectory. The trajectory planned by the above method of the present application has good smoothness, such as Figure 2 As shown, the deviation between the planned trajectory of the embodiment of the present application and the center line of the road is very small (the blue line in the figure is the center line of the road, and the orange line is the optimized trajectory line). Figure 3 As shown, compared with the trajectory optimized by the prior art, the trajectory optimized by the method of the present application has a smoother change in heading angle and better driver comfort. Figure 4 As shown, the trajectory line after optimization in the present application has a smaller curvature variation range, and sharp turns will not occur during driving.

[0072] The above scheme of this embodiment obtains an augmented Lagrangian trajectory optimization model by introducing a Lagrangian operator model on the basis of a trajectory optimization model based on the iLQR algorithm, thereby enabling the augmented Lagrangian trajectory optimization model to quickly calculate both equality constraints and inequality constraints.

[0073] Furthermore, in the AL-iLQR path planning method based on iLQR described in the above scheme, in step S100, the trajectory optimization model based on the iLQR algorithm is constructed, and the input data of the trajectory optimization model includes the driving state vector and the control vector. The trajectory optimization model is expressed as:

[0074]

[0075] In step S200, the kinematic model of the vehicle during driving is constructed. The kinematic model is used to define the relationship between the driving state vectors at two adjacent moments. The kinematic model is expressed as:

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

[0077] The augmented Lagrangian trajectory optimization model is constructed. The augmented Lagrangian trajectory optimization model adds a Lagrangian operator model on the basis of the trajectory optimization model. The augmented Lagrangian trajectory optimization model is expressed as:

[0078]

[0079] 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 a constraint.

[0080] Furthermore, in the method, the first constraint condition is used as the constraint condition of the trajectory optimization model, the second constraint condition is used as the constraint condition of the Lagrangian operator model, and the augmented Lagrangian trajectory optimization model is iterated in a loop in combination with the state vector of the vehicle during driving, until the control vector in the trajectory optimization model reaches the optimal value, and the Lagrangian multiplication operator in the Lagrangian operator model reaches the optimal value, so as to obtain the final path planning model, including: using the KKT condition to determine the conditional function of the augmented Lagrangian trajectory optimization model to output the path planning result; substituting the conditional function into the augmented Lagrangian trajectory optimization model for iteration until the Lagrangian multiplication operator in the augmented Lagrangian trajectory optimization model converges to the optimal value.

[0081] 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:

[0082]

[0083] The above equations 3 and 4 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. However, from equation 3, it can be seen that 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:

[0084]

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

[0086]

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

[0088] For the trajectory optimization model based on the iLQR algorithm, the iterative goal is to minimize the cost function and obtain the control vector. The kinematic model can be linearized by using the Taylor series expansion method to retain only the first-order derivative result.

[0089] f(x i+1 ,u i+1 )≡f(x i ,u i )+A i δx i +B i δu i ;

[0090] A 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 .

[0091] Furthermore, the function J (essentially the cost function) of the trajectory optimization model based on the iLQR algorithm is Taylor expanded. Assuming that the total cost function J takes the minimum value, it can be expressed as right Also perform Taylor expansion:

[0092]

[0093] 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:

[0094]

[0095] The following equation relationship is obtained:

[0096]

[0097]

[0098] After the augmented Lagrangian trajectory optimization model is linearized, the calculation method of the obtained Q matrix becomes:

[0099]

[0100] Since the KKT condition is only a necessary condition for the optimization problem to obtain an extreme value, the matrix Q must be positive definite during the iterative solution process. Therefore, when solving: When , the matrix Q is regularized:

[0101]

[0102] To ensure that the matrix Q is a positive definite matrix during calculation, the entire optimization problem is iterated in the direction of minimization. Figure 5 The framework shown is carried out.

[0103] The present application also provides an AL-iLQR path planning device based on iLQR, such as Figure 6 As shown, including:

[0104] An iLQR algorithm model building module 100 is configured to build a trajectory optimization model based on the iLQR algorithm, wherein input data of the trajectory optimization model includes a driving state vector and a control vector;

[0105] A kinematic model building module 200 is configured to build 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;

[0106] The AL-iLQR model construction module 300 is configured to construct an augmented Lagrangian trajectory optimization model, wherein the augmented Lagrangian trajectory optimization model adds a Lagrangian operator model on the basis of the trajectory optimization model;

[0107] The constraint condition acquisition module 400 is configured to acquire the constraint condition of the driving state vector, and divide the constraint condition into a first constraint condition and a second constraint condition according to whether the constraint condition is an equality constraint, the first constraint condition is an equality constraint condition, and the second constraint condition is an inequality constraint condition;

[0108] The data iteration module 500 is configured to use the first constraint condition as the constraint condition of the trajectory optimization model, use the second constraint condition as the constraint condition of the Lagrangian operator model, and perform cyclic iteration on the augmented Lagrangian trajectory optimization model in combination with the state vector of the vehicle during driving, until the control vector in the trajectory optimization model reaches an optimal value, and the Lagrangian multiplication operator in the Lagrangian operator model reaches an optimal value, thereby obtaining a final path planning model;

[0109] The trajectory optimization module 600 is configured to plan the vehicle driving path using the path planning model.

[0110] The above solution provided by the present application has a fast calculation speed. For the same trajectory optimization situation, when the calculation process of the present application is within 1 millisecond, osqp needs to calculate for 5-8 milliseconds. Moreover, since the present application can process inequality constraints, the optimization effect of the present application is better than that of the iLQR algorithm.

[0111] 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 iLQR-based AL-iLQR path planning method described in any one of the above items are implemented.

[0112] The present application also provides an electronic device, such as Figure 7As shown, the electronic device includes at least one processor 71 and at least one memory 72, at least one of the memories 72 stores program information, and at least one of the processors 71 reads the program information and executes the AL-iLQR path planning method based on iLQR described in any one of the above method embodiments. The device may also include: an input device 73 and an output device 74. The processor 71, the memory 72, the input device 73 and the output device 74 can be connected in communication. The memory 72, 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 71 executes various functional applications and data processing by running the non-volatile software programs, instructions and modules stored in the memory 72, that is, the AL-iLQR path planning method based on iLQR provided in any of the above schemes is implemented. The memory 72 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 AL-iLQR path planning method based on iLQR, etc. In addition, the memory 72 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 device. In some embodiments, the memory 72 may optionally include a memory remotely arranged relative to the processor 71, and these remote memories may be connected to a device for executing the AL-iLQR path planning method based on iLQR through 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 73 may receive input user clicks, and generate signal inputs related to user settings and function controls of the AL-iLQR path planning method based on iLQR. The output device 74 may include a display device such as a display screen. When the one or more modules are stored in the memory 72, when they are run by the one or more processors 71, the AL-iLQR path planning method based on iLQR in any of the above-mentioned method embodiments is executed.

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

[0114] 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. An AL-iLQR path planning method based on iLQR, characterized in that: include: Constructing a trajectory optimization model based on the iLQR algorithm, wherein input data of the trajectory optimization model includes a driving state vector and a control vector; 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 optimization model, wherein the augmented Lagrangian trajectory optimization model adds a Lagrangian operator model on the basis of the trajectory optimization model; Obtaining a constraint condition of the driving state vector, and dividing the constraint condition into a first constraint condition and a second constraint condition according to whether the constraint condition is an equality constraint, wherein the first constraint condition is an equality constraint condition, and the second constraint condition is an inequality constraint condition; The first constraint condition is used as the constraint condition of the trajectory optimization model, the second constraint condition is used as the constraint condition of the Lagrangian operator model, and the augmented Lagrangian trajectory optimization model is iterated cyclically in combination with the state vector of the vehicle during driving, until the control vector in the trajectory optimization model is optimal and the Lagrangian multiplication operator in the Lagrangian operator model is optimal, thereby obtaining a final path planning model; The path planning model is used to plan the vehicle's driving path.

2. The AL-iLQR path planning method based on iLQR according to claim 1, characterized in that: The trajectory optimization model based on the iLQR algorithm is constructed, and the input data of the trajectory optimization model includes the driving state vector and the control vector. The trajectory optimization model is expressed as: The kinematic model of the vehicle during driving is constructed. The kinematic model is used to define the relationship between the driving state vectors at two adjacent moments. The kinematic model is expressed as: x i+1 =f(x i ,u i ); The augmented Lagrangian trajectory optimization model is constructed. The augmented Lagrangian trajectory optimization model adds a Lagrangian operator model on the basis of the trajectory optimization 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 a constraint.

3. The AL-iLQR path planning method based on iLQR according to claim 1, characterized in that: The first constraint condition is used as the constraint condition of the trajectory optimization model, the second constraint condition is used as the constraint condition of the Lagrangian operator model, and the augmented Lagrangian trajectory optimization model is iterated cyclically in combination with the state vector of the vehicle during driving, until the control vector in the trajectory optimization model is optimal, and the Lagrangian multiplication operator in the Lagrangian operator model is optimal, so as to obtain the final path planning model, including: Using KKT conditions to determine the conditional function of the augmented Lagrangian trajectory optimization model output path planning results; Substituting the conditional function into the augmented Lagrangian trajectory optimization model for iteration until the Lagrangian multiplication operator in the augmented Lagrangian trajectory optimization model converges to an optimum.

4. The AL-iLQR path planning method based on iLQR according to claim 3, characterized in that: In the conditional function of using the KKT condition to determine the output path planning result of the augmented Lagrangian trajectory optimization model, the conditional function includes: Among them, the constraints are c(x)≤0, λ≥0.

5. The AL-iLQR path planning method based on iLQR according to claim 4, characterized in that: In the conditional function, the constraint condition c(x i ) takes an extreme value, that is, c(x i )=0, the conditional function is expressed as: λ ≥ 0; c(x)≤0; λc=0; The Lagrange multiplication operator λ and the penalty coefficient μ in the conditional function are updated as follows: m + =φμ,φ>1 Among them, λ + and μ + represents the updated Lagrange multiplication operator and penalty coefficient of the i-th constraint.

6. The AL-iLQR path planning method based on iLQR according to claim 5, characterized in that: The first constraint condition is used as the constraint condition of the trajectory optimization model, the second constraint condition is used as the constraint condition of the Lagrangian operator model, and the augmented Lagrangian trajectory optimization model is iterated cyclically in combination with the state vector of the vehicle during driving, until the control vector in the trajectory optimization model is optimal, and the Lagrangian multiplication operator in the Lagrangian operator model is optimal, so as to obtain the final path planning model: Performing Taylor series expansion on the trajectory optimization model for linearization; Regularization is performed on the gradient matrix Q in the operation function obtained after the Taylor series expansion.

7. An AL-iLQR path planning device based on iLQR, characterized in that: include: An iLQR algorithm model building module is configured to build a trajectory optimization model based on the iLQR algorithm, wherein input data of the trajectory optimization model includes a driving state vector and a control vector; A kinematic model building module is configured to build 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; An AL-iLQR model building module is configured to build an augmented Lagrangian trajectory optimization model, wherein the augmented Lagrangian trajectory optimization model adds a Lagrangian operator model on the basis of the trajectory optimization model; a constraint condition acquisition module, configured to acquire the constraint condition of the driving state vector, and divide the constraint condition into a first constraint condition and a second constraint condition according to whether the constraint condition is an equality constraint, the first constraint condition being an equality constraint condition, and the second constraint condition being an inequality constraint condition; a data iteration module, configured to use the first constraint condition as a constraint condition of the trajectory optimization model, use the second constraint condition as a constraint condition of the Lagrangian operator model, and perform cyclic iteration on the augmented Lagrangian trajectory optimization model in combination with the state vector of the vehicle during driving, until the control vector in the trajectory optimization model reaches an optimal value, the Lagrangian multiplication operator in the Lagrangian operator model reaches an optimal value, and a final path planning model is obtained; The trajectory optimization module is configured to plan the vehicle driving path using the path planning model.

8. A computer-readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instruction is executed by a processor, the steps of the iLQR-based AL-iLQR path planning method described in any one of claims 1-6 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 iLQR-based AL-iLQR path planning method according to any one of claims 1 to 6.

10. A computer program product comprising a computer program / instructions, characterized in that When the computer program / instruction is executed by a processor, the steps of the iLQR-based AL-iLQR path planning method described in any one of claims 1-6 are implemented.

Images