Track planning method of CiLQR based on iLQR and electronic equipment

Abstract

The invention provides a CiLQR trajectory planning method based on iLQR and electronic equipment. The method comprises the following steps: acquiring a dynamic model of a vehicle; acquiring vehicle driving cost; obtaining an initial trajectory optimization model realized based on an iLQR algorithm, wherein the initial trajectory optimization model takes a vehicle initial moment state vector and an arbitrarily set initial control vector as input; performing updating iteration on the initial moment state vector in the track initial optimization model according to the dynamic model under the condition that the vehicle driving cost is minimum, and determining an optimal control vector in the track initial optimization model; and taking the optimal control vector as a final control vector of the trajectory initial optimization model to obtain a trajectory optimization model, and planning the driving trajectory of the vehicle by using the trajectory optimization model. According to the scheme, the advantage of high efficiency of the iLQR algorithm can be utilized, the dynamic constraint in the vehicle driving process can be considered, and the optimized driving path can be stably output under any condition.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous driving path planning, and in particular to a trajectory planning method and electronic equipment based on a CiLQR (Constrained Iterative LQR) of an iLQR (Iterative Linear Quadratic Regulator). Background Art

[0002] The local path planning algorithm of 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 sampling methods and lateral path planning algorithms based on osqp (Operator Splitting Quadratic Programming). 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, and the sampled plan is not necessarily the optimal trajectory; the lateral path planning algorithm based on osqp is fast, but it does not directly consider the vehicle dynamics constraints during 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 autonomous driving process.

[0004] Therefore, there is still room for improvement in the computational efficiency, accuracy and security of the local path planning algorithms in the prior art. Summary of the invention

[0005] The technical problem to be solved by the present invention is that the computational efficiency, accuracy and safety of the local path planning algorithm in the prior art are low. To this end, the present invention provides a trajectory planning method and electronic device based on iLQR and CiLQR.

[0006] In a first aspect, the present application provides a trajectory planning method of CiLQR based on iLQR, comprising:

[0007] Acquire a dynamic model of the vehicle, wherein the input of the dynamic model includes a state vector of the vehicle at a current moment and a control vector at a current moment, and the output of the dynamic model includes a state vector of the vehicle at a next moment;

[0008] Obtaining a vehicle driving cost, wherein the vehicle driving cost is determined according to a state vector change at two adjacent moments during vehicle driving, a difference between the vehicle driving state vector and a reference baseline vector, and the control vector;

[0009] Obtaining an initial trajectory optimization model based on the iLQR algorithm, wherein the initial trajectory optimization model takes the vehicle state vector at the initial moment and an arbitrarily set initial control vector as input;

[0010] Under the condition that the vehicle driving cost is minimized, the initial state vector in the trajectory initial optimization model is updated and iterated according to the dynamic model to determine the optimal control vector in the trajectory initial optimization model;

[0011] The optimal control vector is used as the final control vector of the trajectory initial optimization model to obtain a trajectory optimization model, and the trajectory optimization model is used to plan the driving trajectory of the vehicle.

[0012] In some schemes, the trajectory planning method based on iLQR of CiLQR, the vehicle driving cost is obtained, and the vehicle driving cost is determined according to the state vector change at two adjacent moments during the vehicle driving process, the difference between the vehicle driving state vector and the reference reference vector, and the control vector, and the vehicle driving cost includes:

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

[0014] The smoothness of the vehicle's driving path is within the set smoothness range;

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

[0016] The control vector corresponding to the vehicle's driving path tends to the set lower limit value;

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

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

[0019] In some schemes, the trajectory planning method based on CiLQR of iLQR, the comfort of the vehicle driving path is within the set comfort range and is represented by the following cost function:

[0020] Jc omfort =0.5w acc (v 2 κ) 2 +0.5w jerk (2vaκ+v 3 κ′) 2 ;

[0021] The smoothness of the vehicle driving path is within the set smoothness range and the control vector corresponding to the vehicle driving path tends to the set lower limit value, which is represented by the following cost function:

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

[0023] The degree of deviation between the vehicle driving path and the reference baseline is less than the set deviation, which is represented by the following cost function:

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

[0025] The change rate of the control vector corresponding to the vehicle driving path is within the set change rate range and is represented by the following cost function:

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

[0027] The distance between the vehicle's driving path and the road obstacle is greater than the safety distance, which is represented by the following cost function:

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

[0029] Among them, the meaning of each parameter in the above cost functions is:

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

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

[0032]

[0033] In some schemes, the total cost function of the vehicle driving cost of the CiLQR-based iLQR trajectory planning method is expressed as:

[0034] J=J comfort +J shape +J ref +J u +J safe ;

[0035] The vehicle driving cost is minimized, that is, the value of the total cost function is minimized.

[0036] In some schemes, the trajectory planning method based on CiLQR of iLQR, the dynamic model of the vehicle is obtained, the input of the dynamic model includes the state vector of the vehicle at the current moment and the control vector at the current moment, and the output of the dynamic model includes the state vector of the vehicle at the next moment:

[0037] The vehicle state vector at the current moment includes the abscissa, the ordinate, the heading angle, the curvature of the driving path, and the curvature change rate of the driving path; the control vector is represented by the second-order derivative of the curvature of the driving path; then the dynamic model is represented as:

[0038] f(g i+1 ,u i+1 )=f(g i ,u i )+δf(g i ,u i )=f(g i +δg i ,u i +δu i );

[0039] in:

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

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

[0042]

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

[0044] g=[x,y,θ,k,κ′] T ;

[0045] u=[k″];

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

[0047] In some schemes, the trajectory planning method based on CiLQR of iLQR, the dynamic model of the vehicle is obtained, the input of the dynamic model includes the state vector of the vehicle at the current moment and the control vector at the current moment, and the output of the dynamic model includes the state vector of the vehicle at the next moment:

[0048] The kinetic model is linearized to obtain:

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

[0050] A i is f(g i ,u i ) for g i The gradient matrix, B i is f(g i ,u i ) i The gradient matrix of .

[0051] In a second aspect, the technical solution of the present application provides a trajectory planning device based on iLQR and CiLQR, comprising:

[0052] A dynamic model acquisition module is configured to acquire a dynamic model of a vehicle, wherein the input of the dynamic model includes a state vector of the vehicle at a current moment and a control vector at a current moment, and the output of the dynamic model includes a state vector of the vehicle at a next moment;

[0053] A driving cost acquisition module is configured to acquire a vehicle driving cost, wherein the vehicle driving cost is determined according to a state vector change at two adjacent moments during vehicle driving, a difference between the vehicle driving state vector and a reference baseline vector, and the control vector;

[0054] An initial optimization model acquisition module is configured to acquire an initial trajectory optimization model based on an iLQR algorithm, wherein the initial trajectory optimization model takes a state vector of the vehicle at an initial moment and an arbitrarily set initial control vector as input;

[0055] An update and iteration module is configured to update and iterate the initial state vector in the trajectory initial optimization model according to the dynamic model under the condition that the vehicle driving cost is minimized, so as to determine the optimal control vector in the trajectory initial optimization model;

[0056] The trajectory optimization module is configured to use the optimal control vector as the final control vector of the trajectory initial optimization model to obtain a trajectory optimization model, and use the trajectory optimization model to plan the vehicle's driving trajectory.

[0057] 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 trajectory planning method of CiLQR based on iLQR as described in any one of the first aspects.

[0058] In a fourth aspect, 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 trajectory planning method of CiLQR based on iLQR as described in any one of the first aspects.

[0059] 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 trajectory planning method of CiLQR based on iLQR as described in any one of the first aspects are implemented.

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

[0061] The trajectory planning method and electronic device based on CiLQR of iLQR provided in this application establishes a final trajectory optimization model based on the dynamic model, the vehicle driving cost and the initial trajectory optimization model implemented based on the iLQR algorithm, and optimizes the vehicle driving trajectory using the trajectory optimization model. In this process, the high efficiency advantage of the iLQR algorithm can be utilized, and the dynamic constraints during vehicle driving can also be considered, so that the optimized driving path can be stably output under any circumstances. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 A flowchart of a trajectory planning method of CiLQR based on iLQR according to an embodiment of the present invention;

[0063] Figure 2 A schematic diagram of the optimization result of the vehicle driving trajectory using the application scheme;

[0064] Figure 3A schematic diagram showing a comparison between the heading angle optimization result after optimizing the vehicle driving trajectory using the present application scheme and the heading angle optimization result of the prior art scheme;

[0065] Figure 4 A schematic diagram comparing the trajectory curvature optimization result after optimizing the vehicle driving trajectory using the solution of the present application with the trajectory curvature optimization result of the prior art solution;

[0066] Figure 5 A structural block diagram of a trajectory planning device based on CiLQR of iLQR according to an embodiment of the present invention;

[0067] Figure 6 A schematic diagram of the hardware connection relationship of an electronic device for executing the trajectory planning method of CiLQR based on iLQR according to an embodiment of the present invention. DETAILED DESCRIPTION

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

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

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

[0071] This embodiment provides a trajectory planning method based on iLQR CiLQR, which can be applied to the vehicle head end or the cloud. Figure 1 As shown, the method comprises the following steps:

[0072] S100: Obtain a dynamic model of the vehicle, the input of the dynamic model includes the vehicle's current state vector and current control vector, and the output of the dynamic model includes the vehicle's next state vector. The vehicle dynamic model is usually expressed as follows:

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

[0074] Among them, g iIt 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, g can be obtained. i+1 Because the vehicle will not suddenly change its strategy during driving, there is a certain relationship between the state vectors of 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.

[0075] S200: Obtaining a vehicle driving cost, wherein the vehicle driving cost is determined according to a change in a state vector between two adjacent moments during vehicle driving, a difference between a vehicle driving state vector and a reference baseline vector, and the control vector.

[0076] The vehicle driving cost can impose certain constraints on the vehicle driving trajectory. For example, the driving speed should not exceed a certain range and must be within the speed limit value range, the driving trajectory cannot exceed the road boundary line, etc. On this basis, the state vector changes of the vehicle at adjacent moments are limited.

[0077] S300: Acquire an initial trajectory optimization model implemented based on the iLQR algorithm, wherein the initial trajectory optimization model takes the vehicle state vector at the initial moment and an arbitrarily set initial control vector as input.

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

[0079]

[0080] g0 represents the first state vector input to the iLQR algorithm model, U represents the control vector, J is the driving cost, l f is the terminal cost function, g N is 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, g0 is a known quantity, and U can be given an arbitrary value.

[0081] S400: Under the condition that the vehicle driving cost is minimized, the initial state vector in the trajectory initial optimization model is updated and iterated according to the dynamic model to determine the optimal control vector in the trajectory initial optimization model.

[0082] Since the initial control vector in step S300 is given arbitrarily, it is inaccurate to use it for trajectory optimization. Therefore, in this solution, by combining the limiting condition of the minimum vehicle driving cost and the motion constraint of the vehicle dynamics model on the trajectory, the initial state vector is used as a reference to obtain the optimal state vector corresponding to each step point in the optimized trajectory in turn, and then re-substitute it into the initial trajectory optimization model for calculation to obtain an optimized control vector. Repeat this step, and after multiple iterations, it can be determined that the control vector obtained after two adjacent iterations has basically not changed. At this time, it can be considered that the control vector can no longer be optimized, that is, it is the optimal control vector.

[0083] S50: Using the optimal control vector as the final control vector of the trajectory initial optimization model to obtain a trajectory optimization model, and using the trajectory optimization model to plan the driving trajectory of the vehicle.

[0084] The above solution provided by this application establishes a final trajectory optimization model based on the dynamic model, vehicle driving cost and the initial trajectory optimization model implemented based on the iLQR algorithm, and applies the trajectory optimization model to optimize the vehicle driving trajectory. In this process, it can not only take advantage of the high efficiency of the iLQR algorithm, but also consider the dynamic constraints during vehicle driving, and can stably output the optimized driving path under any circumstances.

[0085] Further, in the above scheme, in the step S200, the vehicle driving cost is obtained, and the vehicle driving cost is determined according to the state vector change at two adjacent moments during the vehicle driving process, the difference between the vehicle driving state vector and the reference baseline vector, and the control vector, and the vehicle driving cost includes:

[0086] S201: The comfort level of the vehicle's driving path is within a set comfort level range. Specifically, the vehicle's driving speed, acceleration, trajectory curvature, and other aspects may be limited.

[0087] S202: The smoothness of the vehicle's driving path is within a set smoothness range; that is, the smoother the vehicle's driving path is, the better.

[0088] S203: The deviation between the vehicle driving path and the reference baseline is less than the set deviation; that is, the vehicle driving trajectory should be as close as possible to the reference baseline. Usually, the reference baseline is the center line of the lane.

[0089] S204: The control vector corresponding to the vehicle driving path tends to the set lower limit value; that is, the control vector is as small as possible, because the control vector actually represents the degree of change of the vehicle driving state vector. If the control vector is too large, it will easily affect driving comfort and safety.

[0090] S205: The change rate of the control vector corresponding to the vehicle driving path is within a set change rate range; that is, the change of the control vector should maintain a certain stability.

[0091] S206: The distance between the vehicle's driving path and the road obstacle is greater than the safe distance. That is, the vehicle should be too close to other vehicles on the road to ensure driving safety.

[0092] Based on the above driving costs, this application proposes the following cost function:

[0093] The comfort level of the vehicle driving path is within the set comfort level range and is represented by the following cost function:

[0094] J comfort =0.5w acc (v 2 κ) 2 +0.5w jerk (2vak+v 3 k′) 2 ;

[0095] The smoothness of the vehicle driving path is within the set smoothness range and the control vector corresponding to the vehicle driving path tends to the set lower limit value, which is represented by the following cost function:

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

[0097] The degree of deviation between the vehicle driving path and the reference baseline is less than the set deviation, which is represented by the following cost function:

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

[0099] The change rate of the control vector corresponding to the vehicle driving path is within the set change rate range and is represented by the following cost function:

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

[0101] The distance between the vehicle's driving path and the road obstacle is greater than the safety distance, which is represented by the following cost function:

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

[0103] Among them, the meaning of each parameter in the above cost functions is:

[0104] w acc 、w jerk 、w k 、w k′ 、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 k 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;

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

[0106]

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

[0108] 4. The trajectory planning method of CiLQR based on iLQR according to claim 3, characterized in that:

[0109] The total cost function of the vehicle driving cost is expressed as:

[0110] J=J comfort +J shape +J ref +J u +J safe ;

[0111] The vehicle driving cost is minimized, that is, the value of the total cost function is minimized.

[0112] In the above scheme, the vehicle dynamics model is obtained in step S100, and the input of the dynamics model includes the vehicle state vector and the control vector at the current moment, and the output of the dynamics model includes the vehicle state vector at the next moment:

[0113] The vehicle state vector at the current moment includes the abscissa, the ordinate, the heading angle, the curvature of the driving path, and the curvature change rate of the driving path; the control vector is represented by the second-order derivative of the curvature of the driving path; then the dynamic model is expressed as:

[0114] f(g i+1 ,u i+1 )=f(g i ,u i )+δf(g i ,u i )=f(g i +δg i ,u i +δu i );

[0115] in:

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

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

[0118]

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

[0120] g=[x,y,θ,κ,κ′] T ;

[0121] u = [κ″];

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

[0123] Furthermore, the kinetic model is linearized, and the linearization can be achieved by using Taylor series expansion and retaining only the first-order derivative result.

[0124] f(gi+1,ui+1 )≡f(g i ,u i )+A i δx i +B i δu i ;

[0125] A i is f(g i ,u i ) for g i The gradient matrix, B i is f(g i ,u i ) i The gradient matrix of .

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

[0127]

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

[0129]

[0130] The following equation relationship is obtained:

[0131]

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

[0133] like Figures 2 to 4 The results of optimizing the vehicle driving trajectory using the above solution of this application are given, such as Figure 2 As shown in , the deviation between the optimized trajectory (orange line) obtained by this scheme and the reference baseline (blue line) is very small, and the curvature is basically consistent. Figure 3 As shown in FIG. 1 , the heading angle (orange line) of the optimized trajectory obtained by the present solution changes more slowly than the heading angle (blue line) of the optimized trajectory of the prior art. Figure 4As shown, the smoothness of the optimized trajectory obtained by this solution (blue line) is better than the heading angle (orange line) of the optimized trajectory of the prior art. In addition, the application solution saves 50% or even more time when performing driving path optimization, and the solution speed of the prior art osqp has been improved several times. The algorithm is solved on an open road, without the need for frenet and Cartesian coordinate system conversion. All constraints can be perfectly expressed in the open space, and there is no situation where the solution cannot be solved, which greatly ensures the safety of the autonomous driving vehicle during driving.

[0134] An embodiment of the present application provides a trajectory planning device based on iLQR CiLQR, such as Figure 4 As shown, including:

[0135] The dynamic model acquisition module 100 is configured to acquire a dynamic model of a vehicle, wherein the input of the dynamic model includes a state vector and a control vector of the vehicle at a current moment, and the output of the dynamic model includes a state vector of the vehicle at a next moment.

[0136] The driving cost acquisition module 200 is configured to acquire the vehicle driving cost, which is determined according to the state vector change at two adjacent moments during the vehicle driving process, the difference between the vehicle driving state vector and the reference baseline vector, and the control vector.

[0137] The initial optimization model acquisition module 300 is configured to acquire an initial trajectory optimization model based on the iLQR algorithm, wherein the initial trajectory optimization model takes the vehicle state vector at the initial moment and an arbitrarily set initial control vector as input.

[0138] The update iteration module 400 is configured to update and iterate the initial state vector in the trajectory initial optimization model according to the dynamic model under the condition that the vehicle driving cost is minimized, and determine the optimal control vector in the trajectory initial optimization model.

[0139] The trajectory optimization module 500 is configured to use the optimal control vector as the final control vector of the trajectory initial optimization model to obtain a trajectory optimization model, and use the trajectory optimization model to plan the driving trajectory of the vehicle.

[0140] The above-mentioned solution of the present application has a trajectory planning solution speed faster than the prior art osqp, satisfies the vehicle dynamics constraints, and can stably output the solution results under any circumstances.

[0141] 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 trajectory planning method of CiLQR based on iLQR described in any of the aforementioned embodiments are implemented.

[0142] An embodiment of the present application also provides a computer program product, including a computer program / instruction, which, when executed by a processor, implements the steps of the trajectory planning method of CiLQR based on iLQR as described in any of the aforementioned embodiments.

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

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

[0145] 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 trajectory planning method based on CiLQR of iLQR, characterized in that: include: Acquire a dynamic model of the vehicle, wherein the input of the dynamic model includes a state vector of the vehicle at a current moment and a control vector at a current moment, and the output of the dynamic model includes a state vector of the vehicle at a next moment; Obtaining a vehicle driving cost, wherein the vehicle driving cost is determined according to a state vector change at two adjacent moments during vehicle driving, a difference between the vehicle driving state vector and a reference baseline vector, and the control vector; Obtaining an initial trajectory optimization model based on the iLQR algorithm, wherein the initial trajectory optimization model takes the vehicle state vector at the initial moment and an arbitrarily set initial control vector as input; Under the condition that the vehicle driving cost is minimized, the initial state vector in the trajectory initial optimization model is updated and iterated according to the dynamic model to determine the optimal control vector in the trajectory initial optimization model; The optimal control vector is used as the final control vector of the trajectory initial optimization model to obtain a trajectory optimization model, and the trajectory optimization model is used to plan the driving trajectory of the vehicle.

2. The trajectory planning method of CiLQR based on iLQR according to claim 1, characterized in that: The vehicle driving cost is obtained, and the vehicle driving cost is determined according to the state vector change at two adjacent moments during the vehicle driving process, the difference between the vehicle driving state vector and the reference baseline vector, and the control vector. The vehicle driving cost 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 the set smoothness range; The deviation between the vehicle's driving path and the reference baseline is less than the set deviation; The control vector corresponding to the vehicle's driving path tends to the set lower limit value; 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.

3. The trajectory planning method of CiLQR based on iLQR according to claim 2, characterized in that: The comfort level of the vehicle driving path is within the set comfort level range and is represented by the following cost function: J comfort =0.5w acc (v 2 k) 2 +0.5w jerk (2vaκ+v 3 k′) 2 ; The smoothness of the vehicle driving path is within the set smoothness range and the control vector corresponding to the vehicle driving path tends to the set lower limit value, which is represented by the following cost function: J shape =0.5(w k k 2 +w κ′ k′ 2 +w u k″ 2 ); The degree of deviation between the vehicle driving path and the reference baseline is less than the set deviation, which is represented by the following cost function: J ref =0.5w ref ((xx) ref ) 2 +(yy ref ) 2 ) 2 ; The change rate of the control vector corresponding to the vehicle driving path is within the set change rate range and is represented by the following cost function: J u =0.5(w clb c 2 (k″ min -k″)+w cub c 2 (k″-k″ max )); The distance between the vehicle's driving path and the road obstacle is greater than the safety distance, which is represented by the following 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); Among them, the meaning of each parameter in the above cost functions is: w acc 、w jerk 、w k 、w κ′ 、w u 、w ref 、w clb 、w cub 、w safe represent weight values, v represents vehicle speed, κ represents curvature of the driving path, K′ represents the first-order derivative of curvature of the driving path, κ″ represents the second-order derivative of curvature of the driving path, a represents 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(*) represents a piecewise function, and its value result is:

4. The trajectory planning method of CiLQR based on iLQR according to claim 3, characterized in that: The total cost function of the vehicle driving cost is expressed as: J=J comfort +J shape +J ref +J u +J safe ; The vehicle driving cost is minimized, that is, the value of the total cost function is minimized.

5. The trajectory planning method of CiLQR based on iLQR according to any one of claims 1 to 4, characterized in that: The dynamic model of the vehicle is obtained, the input of the dynamic model includes the vehicle state vector and the control vector at the current moment, and the output of the dynamic model includes the vehicle state vector at the next moment: The vehicle state vector at the current moment includes the abscissa, the ordinate, the heading angle, the curvature of the driving path, and the curvature change rate of the driving path; the control vector is represented by the second-order derivative of the curvature of the driving path; then the dynamic model is expressed as: f(g i+1 ,u i+1 )=f(g i ,u i )+δf(g i ,u i )=f(g i +δg i ,u i +δu i ); in: x i+1 =x i +cos(θ i )·ds; and i+1 =and i +sin(θ i )·ds; k′ i+1 =κ′ i +k′ i ·ds 2 ; g=[x, y, θ, κ, κ′] T ; u=[k ″ ]; 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.

6. The trajectory planning method based on iLQR of CiLQR according to claim 5, characterized in that: The dynamic model of the vehicle is obtained, the input of the dynamic model includes the vehicle state vector and the control vector at the current moment, and the output of the dynamic model includes the vehicle state vector at the next moment: The kinetic model is linearized to obtain: f(g i+1 ,u i+1 )≡f(g i ,u i )+A i δx i +B i δu i ; A i is f(g i ,u i ) for g i The gradient matrix, B i is f(g i ,u i ) i The gradient matrix of .

7. A trajectory planning device based on CiLQR of iLQR, characterized in that: include: A dynamic model acquisition module is configured to acquire a dynamic model of a vehicle, wherein the input of the dynamic model includes a state vector of the vehicle at a current moment and a control vector at a current moment, and the output of the dynamic model includes a state vector of the vehicle at a next moment; A driving cost acquisition module is configured to acquire a vehicle driving cost, wherein the vehicle driving cost is determined according to a state vector change at two adjacent moments during vehicle driving, a difference between the vehicle driving state vector and a reference baseline vector, and the control vector; An initial optimization model acquisition module is configured to acquire an initial trajectory optimization model implemented based on an iLQR algorithm, wherein the initial trajectory optimization model takes a state vector of the vehicle at an initial moment and an arbitrarily set initial control vector as input; An update and iteration module is configured to update and iterate the initial state vector in the trajectory initial optimization model according to the dynamic model under the condition that the vehicle driving cost is minimized, so as to determine the optimal control vector in the trajectory initial optimization model; The trajectory optimization module is configured to use the optimal control vector as the final control vector of the trajectory initial optimization model to obtain a trajectory optimization model, and use the trajectory optimization model to plan the vehicle's driving trajectory.

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 trajectory planning method of CiLQR based on iLQR as described in any one of claims 1 to 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 trajectory planning method of CiLQR based on iLQR according to any one of claims 1-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 trajectory planning method of CiLQR based on iLQR as described in any one of claims 1 to 6 are implemented.

Images