Trajectory planning method, device, equipment and storage medium based on incremental iterative linear quadratic regulator solution

Abstract

This application discloses a trajectory planning method, apparatus, device, and storage medium based on an incremental iterative linear quadratic regulator solution, relating to the field of intelligent driving technology. The method comprises: constructing an initial model based on a Cartesian coordinate system; performing objective function design and multi-circle model-based constraint processing on the initial model to obtain a target model; and performing incremental iterative linear quadratic regulator solution and trajectory post-processing on the target model to obtain a target trajectory. Based on the concept of spatiotemporal joint planning, the application improves the accuracy and applicability of trajectory planning and reduces the impact of curvature fluctuations by constructing a vehicle model and performing incremental iLQR solution.

Description

Technical Field

[0001] The present application relates to the field of intelligent driving technology, and in particular to a trajectory planning method, apparatus, device and storage medium based on incremental iterative linear quadratic regulator solution. Background Art

[0002] As intelligent driving technology becomes increasingly widespread and its application scenarios become increasingly complex, the requirements for intelligent driving decision-making and planning modules are becoming increasingly demanding. Problems with time-space separation schemes based on the Frenet coordinate system are becoming increasingly apparent. These schemes typically perform lateral planning before longitudinal planning. From a solution space perspective, this artificially narrows the solution space to facilitate implementation. This artificial narrowing of the solution space can exclude the optimal solution from the list of feasible solutions. In complex scenarios, the algorithm is likely to produce a suboptimal solution, resulting in suboptimal intelligent driving performance. The objective function for path planning typically uses metrics describing lateral motion as cost terms. When planning based on the Frenet coordinate system, these metrics primarily include l′, l″, and l″′. When the Frenet curvature fluctuates significantly, even if these metrics are optimized to zero (the optimal result in the Frenet coordinate system), when the Frenet result is converted to a Cartesian coordinate system, the curvature fluctuations of the Frenet will still be reflected in the Cartesian result, leading to comfort and even safety issues. At the same time, the path planning mathematical model generally treats the vehicle as a point mass, and the influence of vehicle width is taken into account when setting the QP boundary. Since the boundary problem of intelligent driving vehicles is handled approximately, the processing capability for large-angle or small-space scenes is limited. Summary of the Invention

[0003] The main purpose of this application is to provide a trajectory planning method, device, equipment and storage medium based on incremental iterative linear quadratic regulator solution, aiming to solve the technical problem of how to improve the accuracy and applicability of trajectory planning and reduce the impact of curvature fluctuations.

[0004] To achieve the above objectives, the present application proposes a trajectory planning method based on incremental iterative linear quadratic regulator solution, the method comprising:

[0005] Construct an initial model based on a Cartesian coordinate system;

[0006] Performing objective function design and constraint processing based on a multi-circle model on the initial model to obtain a target model;

[0007] An incremental iterative linear quadratic regulator solution and trajectory post-processing are performed on the target model to obtain a target trajectory.

[0008] In one embodiment, the step of performing objective function design and constraint processing based on a multi-circle model on the initial model to obtain a target model includes:

[0009] Obtaining horizontal comfort index, longitudinal comfort index and constraint conditions;

[0010] Calculating an initial objective function cost term by combining the lateral comfort index and the longitudinal comfort index;

[0011] Based on the constraints, a constraint cost term is obtained by approximating the vehicle bounding box through a multi-circle model;

[0012] Performing a weighted summation on the initial objective function cost term and the constraint condition cost term to obtain a state transition objective function;

[0013] A target model is obtained according to the state transition target function.

[0014] In one embodiment, the step of calculating the cost term of the initial objective function by combining the lateral comfort index and the longitudinal comfort index includes:

[0015] Calculating the index costs of the lateral comfort index and the longitudinal comfort index in a quadratic form;

[0016] An initial objective function cost term is obtained according to the indicator cost.

[0017] In one embodiment, the step of obtaining a constraint cost term by approximating the vehicle frame using a multi-circle model based on the constraint condition includes:

[0018] Obtaining the weight corresponding to the constraint condition;

[0019] Calculating the degree of non-satisfaction of the constraint condition by approximating the vehicle boundary using a multi-circle model;

[0020] A constraint cost item is obtained according to the weight and the degree of non-satisfaction.

[0021] In one embodiment, the step of performing incremental iterative linear quadratic regulator solving and trajectory post-processing on the target model to obtain the target trajectory includes:

[0022] Obtaining a state value increment function and an action state value increment function according to the target model;

[0023] Set the initial state of the iteration;

[0024] Based on the initial state, reverse iterative optimization is performed according to the state value increment function and the action state value increment function to obtain a feedforward gain and a feedback gain;

[0025] Perform forward iterative optimization according to the feedforward gain and the feedback gain to obtain a reference trajectory;

[0026] The reference trajectory is processed for trajectory validity and trajectory safety to obtain a target trajectory.

[0027] In one embodiment, the step of performing forward iterative optimization according to the feedforward gain and the feedback gain to obtain a reference trajectory includes:

[0028] Get the preset step size through line search;

[0029] Based on the preset step size, performing forward iterative optimization through the feedforward gain and the feedback gain to update the state variables and action variables of the target model;

[0030] Return to the step of performing reverse iterative optimization based on the initial state and according to the state value increment function and the action state value increment function to obtain the feedforward gain and the feedback gain, until the result of the iterative optimization converges or reaches a preset number of iteration cycles to obtain a reference trajectory.

[0031] In one embodiment, the step of constructing the initial model based on the Cartesian coordinate system includes:

[0032] Based on the Cartesian coordinate system, the state variables and action variables of the vehicle dynamics model are obtained according to the vehicle parameters;

[0033] According to the state variables and the action variables, the vehicle dynamics model is linearized and expressed in incremental form to obtain an initial model.

[0034] In addition, to achieve the above-mentioned purpose, the present application also proposes a trajectory planning device based on an incremental iterative linear quadratic regulator solution, the device comprising:

[0035] A model building module, used to build an initial model based on a Cartesian coordinate system;

[0036] A model processing module, configured to perform objective function design and constraint processing based on a multi-circle model on the initial model to obtain a target model;

[0037] The incremental solution module is used to perform incremental iterative linear quadratic regulator solution and trajectory post-processing on the target model to obtain the target trajectory.

[0038] In addition, to achieve the above-mentioned purpose, the present application also proposes a trajectory planning device based on an incremental iterative linear quadratic regulator solution, the device comprising: a memory, a processor, and a computer program stored on the memory and executable on the processor, the computer program being configured to implement the steps of the trajectory planning method based on an incremental iterative linear quadratic regulator solution as described above.

[0039] In addition, to achieve the above-mentioned purpose, the present application also proposes a storage medium, which is a computer-readable storage medium, and a computer program is stored on the storage medium. When the computer program is executed by the processor, the steps of the trajectory planning method based on the incremental iterative linear quadratic regulator solution as described above are implemented.

[0040] In addition, to achieve the above-mentioned purpose, the present application also provides a computer program product, which includes a computer program, and when the computer program is executed by a processor, it implements the steps of the trajectory planning method based on incremental iterative linear quadratic regulator solution as described above.

[0041] One or more technical solutions proposed in this application have at least the following technical effects:

[0042] By adopting the idea of ​​joint spatiotemporal planning, building a vehicle model and performing incremental iLQR solution, we can solve the technical problems that the optimal solution may be excluded from the feasible solution, the curvature fluctuation of Frenet itself affects the trajectory solution, and the processing ability is limited for large-angle or small space scenes. Compared with existing technologies, this method improves the accuracy and applicability of trajectory planning and reduces the impact of curvature fluctuations. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0044] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0045] Figure 1 A flowchart of the first embodiment of the trajectory planning method based on incremental iterative linear quadratic regulator solution provided in this application;

[0046] Figure 2 A flow chart of the second embodiment of the trajectory planning method based on incremental iterative linear quadratic regulator solution provided in this application;

[0047] Figure 3 A schematic diagram of a multi-circle model of a trajectory planning method based on an incremental iterative linear quadratic regulator solution provided in Example 2 of the present application;

[0048] Figure 4 A flowchart of the third embodiment of the trajectory planning method based on incremental iterative linear quadratic regulator solution provided in this application;

[0049] Figure 5 A flowchart diagram of a fourth embodiment of a trajectory planning method based on an incremental iterative linear quadratic regulator solution is provided for this application;

[0050] Figure 6 Schematic diagram of the module structure of the trajectory planning device based on the incremental iterative linear quadratic regulator solution according to an embodiment of the present application;

[0051] Figure 7 Schematic diagram of the device structure of the hardware operating environment involved in the trajectory planning method based on incremental iterative linear quadratic regulator solution in the embodiment of the present application.

[0052] The purpose, features and advantages of this application will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0053] It should be understood that the specific embodiments described herein are merely used to explain the technical solutions of the present application and are not intended to limit the present application.

[0054] In order to better understand the technical solution of the present application, a detailed description will be given below in conjunction with the accompanying drawings and specific implementation methods.

[0055] The main solution of the embodiment of the present application is: constructing an initial model based on a Cartesian coordinate system; performing objective function design and constraint processing based on a multi-circle model on the initial model to obtain a target model; performing incremental iterative linear quadratic regulator solution and trajectory post-processing on the target model to obtain a target trajectory.

[0056] In this embodiment, for ease of description, the following description is made by taking the internal actuator of the trajectory planning system based on the incremental iterative linear quadratic regulator as the execution body.

[0057] The existing trajectory planning methods have the problem that the optimal solution may be excluded from the feasible solution, the curvature fluctuation of Frenet itself affects the trajectory solution, and the processing ability of large-angle or small space scenes is limited.

[0058] This application provides a solution based on the idea of ​​joint space-time planning. By constructing a vehicle model and performing incremental iLQR solution, the accuracy and applicability of trajectory planning are improved and the impact of curvature fluctuations is reduced.

[0059] It can be seen from the above embodiments that the present application adopts the idea of ​​joint space-time planning to construct a vehicle model and perform incremental iLQR solution, which solves the technical problems that the optimal solution may be excluded from the feasible solution, the curvature fluctuation of Frenet itself affects the trajectory solution, and the processing capability of large-angle or small space scenes is limited, thereby improving the accuracy and applicability of trajectory planning and reducing the impact of curvature fluctuations.

[0060] It should be noted that the execution subject of this embodiment can be a computing service device with data processing, network communication, and program execution capabilities, such as a tablet computer, personal computer, mobile phone, etc., or an electronic device capable of performing the aforementioned functions. The following describes this embodiment and the following embodiments using the internal actuator of a trajectory planning system based on an incremental iterative linear quadratic regulator as an example.

[0061] Based on this, the embodiment of the present application provides a trajectory planning method based on incremental iterative linear quadratic regulator solution, referring to Figure 1 , Figure 1 This is a flow chart of the first embodiment of the trajectory planning method based on incremental iterative linear quadratic regulator solution of this application.

[0062] In this embodiment, the trajectory planning method based on incremental iterative linear quadratic regulator solution includes steps S10 to S30:

[0063] Step S10: constructing an initial model based on a Cartesian coordinate system.

[0064] It should be noted that Cartesian coordinates are a coordinate system used to describe the position of points in a plane or space. The initial model is a linearized incremental kinematic model of the vehicle. Linear systems are simpler to analyze and calculate than nonlinear systems. To improve algorithm stability and adaptability, this linearized incremental kinematic model is used for trajectory planning.

[0065] When constructing the initial model, it is necessary to combine the vehicle parameters. The vehicle parameters include the vehicle's x-coordinate in the Cartesian coordinate system, the vehicle's y-coordinate in the Cartesian coordinate system, the vehicle's heading angle, the vehicle's wheel angle, the vehicle's yaw rate, the distance to the rear axle center, the vehicle's speed, the vehicle's acceleration, the angular acceleration, and the jerk.

[0066] The objective function of path planning generally uses indicators that describe lateral motion as cost terms. When planning based on the Frenet coordinate system, the indicators that describe lateral motion mainly include l′, l″, and l″′. When the curvature of the Frenet coordinate system fluctuates greatly, even if the aforementioned indicators are optimized to 0 (the optimal result in the Frenet coordinate system), when the results from the Frenet coordinate system are converted to the Cartesian coordinate system, the curvature fluctuations of the Frenet coordinate system itself will still be reflected in the Cartesian coordinate system results, leading to comfort and even safety issues. This solution plans based on the Cartesian coordinate system, avoiding the process of converting the planning results from the Frenet coordinate system to the Cartesian coordinate system, reducing the module's computational complexity, and solving the problem of synchronous changes in trajectory curvature caused by changes in the curvature of the Frenet coordinate system.

[0067] Furthermore, path planning mathematical models typically treat vehicles as point masses, taking into account the impact of vehicle width when setting the QP boundary. Because these approaches approximate the boundaries of intelligent driving vehicles, their ability to handle large corners or small spaces is limited. The vehicle model used in this solution significantly improves planning accuracy compared to point mass models.

[0068] Step S20 , performing objective function design and multi-circle model-based constraint processing on the initial model to obtain a target model.

[0069] It should be noted that the objective function is a function that minimizes the deviation between the actual motion trajectory of the vehicle and the expected motion trajectory. Constraint processing is the process of properly managing and operating limiting conditions (constraints). When we build a model, such as a vehicle kinematics incremental model, there will be various physical conditions, performance requirements and other constraints. Constraint processing is to find a way to enable the solution process to be carried out while satisfying these constraints to ensure the rationality and feasibility of the results. When performing constraint processing, in order to accurately describe the distance from the intelligent driving vehicle to the boundary, a multi-circle model consisting of three circles is used to approximate the impact of the vehicle boundary on the planning results. Constraints generally include static constraints and dynamic constraints. This embodiment and other embodiments are described using static boundary constraints as an example.

[0070] In addition, it should be noted that the target model is a linearized vehicle kinematics incremental model that combines the pursuit of the ideal vehicle motion state with consideration of actual physical limitations. By solving the target model, a trajectory planning result that meets the expectations can be obtained.

[0071] Conventional time-space separation trajectory planning schemes consider lateral comfort indicators first, followed by longitudinal comfort indicators. This involves performing lateral planning before longitudinal planning. From a solution space perspective, this artificially narrows the solution space. However, this artificial narrowing of the solution space can exclude the optimal solution from the list of feasible solutions. In complex scenarios, the algorithm is likely to produce a suboptimal solution, resulting in suboptimal intelligent driving performance. This solution, based on the principle of joint time-space planning, incorporates both lateral and longitudinal comfort indicators into the objective function calculation process, expanding the solution space and raising the upper limit of intelligent driving capabilities.

[0072] Step S30 , performing incremental iterative linear quadratic regulator solving and trajectory post-processing on the target model to obtain a target trajectory.

[0073] It should be noted that the Incremental Iterative Linear Quadratic Regulator (iLQR) is a powerful model-based optimization control algorithm primarily used to address the control problems of dynamic systems, particularly nonlinear systems. It iteratively optimizes the control input sequence based on the system's dynamic model and a quadratic cost function. In each iteration, iLQR linearizes the system, expanding the nonlinear system into a linear approximation around the current state and control input. It then uses the theory of the Linear Quadratic Regulator (LQR) to solve the optimal control problem. Specifically, it considers the system's state equation and cost function, which typically consists of the state error and the cost of the control input and is in quadratic form, making it easier to solve the optimality condition. Starting from an initial control trajectory, iLQR continuously linearizes the system and solves the LQR problem, updating the control trajectory based on the solution results until convergence is achieved. Through this iterative update method, it can find the control sequence that minimizes the cost function, allowing the system to transfer from the initial state to the desired state in the best way, while taking into account the dynamic characteristics and control cost of the system. In each iteration, since the system is linearized, the calculation is relatively efficient. It is suitable for many complex control problems such as robot motion planning and vehicle autonomous driving, and can effectively improve the performance and stability of the system.

[0074] This solution uses an incremental iLQR model, applicable to both global and local Cartesian coordinate systems. This approach is applicable regardless of whether the decision module outputs results in the global or local coordinate system, improving its applicability and avoiding the computational overhead of coordinate conversion. Furthermore, using the incremental iLQR algorithm for solving the problem not only improves solution efficiency but also allows the use of a multi-circle model to approximate the impact of the vehicle boundary on the planning results.

[0075] In addition, it should be noted that trajectory post-processing includes trajectory validity processing and safety processing. Trajectory validity processing is mainly used to ensure that the sign of the velocity is consistent with the vehicle's forward direction and the curvature of the trajectory point does not exceed the vehicle's steering limit. Trajectory safety processing mainly uses collision detection to determine whether there is a collision risk between the vehicle's trajectory and other obstacle trajectories. In cases where there is a collision risk, the fallback trajectory is used to replace the original solution trajectory.

[0076] Additionally, it should be noted that the target trajectory is the trajectory that the system is expected to follow in a control system or motion planning, and the vehicle can be controlled to travel according to the target trajectory to meet vehicle driving requirements.

[0077] This embodiment provides a trajectory planning method based on an incremental iterative linear quadratic regulator solution. Based on the concept of spatiotemporal joint planning, by constructing a vehicle model and performing an incremental iLQR solution, this method addresses the technical issues of the optimal solution being excluded from feasible solutions, the impact of Frenet's own curvature fluctuations on the trajectory solution, and the limited processing capability for large-angle or small-space scenarios. This method improves the accuracy and applicability of trajectory planning and reduces the impact of curvature fluctuations.

[0078] Based on the first embodiment of the present application, in the second embodiment of the present application, the same or similar contents as those in the above embodiment 1 can be referred to the above introduction and will not be described in detail later. Figure 2 , step S20 includes steps S21 to S25:

[0079] Step S21: Acquire a lateral comfort index, a longitudinal comfort index, and constraint conditions.

[0080] It should be noted that the lateral comfort index mainly includes the front-to-back frame consistency target, the rough trajectory target, the wheel angle target, the vehicle yaw rate target, and the angular acceleration target. The longitudinal comfort index mainly includes the front-to-back consistency target, the acceleration target, and the jerk target.

[0081] In addition, it should be noted that the constraints include static boundary constraints. Static boundary constraints are spatial point string constraints in a Cartesian coordinate system generated based on lane line information and static obstacle information, including the left static boundary constraint left_static_boundary_constraint and the right static boundary constraint right_static_boundary_constraint. The left and right static boundary ranges are safe areas where intelligent driving vehicles can pass through, and the boundaries cannot exceed the constraints during vehicle driving.

[0082] Step S22 , calculating an initial objective function cost term by combining the lateral comfort index and the longitudinal comfort index.

[0083] It should be noted that the cost term of the initial objective function is the cost term obtained by weighted summation of the costs of the horizontal and vertical comfort indices.

[0084] The design of the objective function needs to take into account the lateral and longitudinal comfort, and the costs of both the lateral comfort index and the longitudinal comfort index are calculated in quadratic form.

[0085] In a feasible implementation, step S22 includes steps S221 to S222:

[0086] Step S221 , calculating the index costs of the lateral comfort index and the longitudinal comfort index in a quadratic form.

[0087] Calculating the cost of a system's performance metrics in a quadratic form is a method for quantitatively evaluating system performance metrics. This method uses a set of system variables and expresses the cost as a quadratic function of these variables. This quadratic function includes coefficients for the squared terms of the variables themselves and the product terms of the variables themselves. These coefficients serve as weights, measuring the contribution of different variables and their interactions to the cost. For example, in vehicle motion control, if variables such as velocity, acceleration, and jerk are involved, we can assign different weights based on their impact on system performance, forming a quadratic function to calculate the cost. This provides a simple mathematical model that facilitates optimization algorithms, such as minimizing this cost function, to find the optimal system parameters. Furthermore, by adjusting the weights, we can balance the impact of different factors on the cost based on actual needs. For example, when considering comfort, assigning appropriate weights to acceleration and jerk can ensure that the system optimization results better meet our comfort expectations.

[0088] The target costs of the front-to-back frame consistency target, coarse trajectory target, wheel angle target, vehicle yaw rate target, angular acceleration target, front-to-back consistency target, acceleration target and jerk target are calculated in quadratic form, and the resulting set of target costs is the index cost.

[0089] Step S222: obtaining an initial objective function cost term according to the indicator cost.

[0090] The initial objective function cost term is calculated based on the indicator cost. The expression formula is as follows:

[0091] cost target =cost consistency +cost warmstart +cost δ +cost ω +cost α +cost a +cost jerk

[0092] Among them, cost target is the cost term of the initial objective function, cost consistency Cost is the target cost of consistency between the previous and next frames. warmstart The coarse trajectory target cost provided to the decision module, cost δ is the target cost of the wheel angle, cost ω is the target cost of yaw rate, cost α is the angular acceleration target cost, cost a is the longitudinal acceleration target cost, cost jerk is the target cost of longitudinal acceleration.

[0093] By calculating the target cost and designing the objective function, it is possible to achieve spatiotemporal joint planning of the trajectory.

[0094] Step S23 : Based on the constraint conditions, the vehicle bounding box is approximated by a multi-circle model to obtain a constraint condition cost term.

[0095] It should be noted that the constraint cost term is calculated by weighting the cost of the constraint condition. The multi-circle model uses three circles to approximate the vehicle boundary to accurately describe the distance between the intelligent driving vehicle and the boundary.

[0096] The standard iLQR algorithm cannot handle complex constraints, so the constraints need to be converted into cost terms.

[0097] In a feasible implementation, step S23 includes steps S231 to S233:

[0098] Step S231: Obtain the weight corresponding to the constraint condition.

[0099] It should be noted that the weights corresponding to the constraints are used to balance the importance of different constraints in the overall optimization problem. Constraints can be assigned corresponding weights based on understanding and experience with the system. Alternatively, initial weights can be set, and the optimization algorithm can be run to observe the results, then the weights can be adjusted and rerun, repeating this process until the desired results are achieved. This embodiment does not limit the method for obtaining the weights corresponding to the constraints.

[0100] Step S232 , calculating the degree of non-satisfaction of the constraint condition by approximating the vehicle frame using a multi-circle model.

[0101] like Figure 3 The figure shows a schematic diagram of a multi-circle model, in which three circles are used to approximate the vehicle boundary, and the degree of non-satisfaction of the constraint conditions is calculated based on the shortest distance from the centers of the three circles to the boundary.

[0102] Step S233: Obtain a constraint cost item according to the weight and the degree of non-satisfaction.

[0103] The constraint cost is calculated based on the weight and the degree of non-satisfaction. The expression formula is as follows:

[0104] cost constraint =0.5*ω constraint *val 2

[0105] Among them, cost constraint is the constraint cost term, ω constraint is the weight corresponding to the constraint, and val is the degree of non-satisfaction of the constraint.

[0106] By calculating the cost of constraints and designing the objective function, we can provide a basis for trajectory planning.

[0107] Step S24 , performing weighted summation on the initial objective function cost term and the constraint condition cost term to obtain a state transition objective function.

[0108] After the constraints are converted into cost terms, they are weighted and summed together with the cost terms of the initial objective function to form the state transition objective function cost of iLQR transfer , the expression formula is as follows:

[0109] cost transfer =cost target +cost constraint

[0110] Among them, cost transfer is the state transition objective function, cost target is the cost term of the initial objective function, cost constraint is the constraint cost term.

[0111] Step S25: obtaining a target model according to the state transition target function.

[0112] The state transition objective function is used as the objective function of the linearized vehicle dynamics incremental model to obtain the target model for subsequent trajectory solution.

[0113] This embodiment provides a trajectory planning method based on an incremental iterative linear quadratic regulator solution, which obtains a lateral comfort index, a longitudinal comfort index, and constraints; calculates an initial objective function cost term in combination with the lateral comfort index and the longitudinal comfort index; based on the constraints, obtains a constraint cost term by approximating the vehicle frame through a multi-circle model; performs a weighted summation of the initial objective function cost term and the constraint cost term to obtain a state transition objective function; and obtains a target model based on the state transition objective function, thereby improving the accuracy and applicability of trajectory planning.

[0114] Based on the first embodiment of the present application, in the third embodiment of the present application, the same or similar contents as those in the above embodiment 1 can be referred to the above introduction and will not be described in detail later. Figure 4 , step S30 includes steps S31 to S35:

[0115] Step S31: obtaining a state value increment function and an action state value increment function according to the target model.

[0116] It's important to note that state value increment functions are typically used to describe the amount of change in a system between different states. In dynamic systems, the system's state changes over time or due to other factors. State value increment functions are tools for quantifying these changes. Action state value increment functions are primarily used to describe the change in a system's state value before and after executing an action. Assume that a system has a set of state variables that represent various system properties or performance indicators. The system's state is represented by a vector S. When the system executes an action a, state S changes.

[0117] The incremental iLQR solution includes a reverse iteration process, which includes two important functions: the state value increment function and the action state value increment function. The state value increment function can be expressed as follows:

[0118]

[0119] Among them, X t is the state variable, U t is the action variable, ΔV(ΔX t ) is the incremental function of the state value of the t-th trajectory point, Δcost t is the incremental cost of state transition of the t-th trajectory point, V t is the quadratic coefficient matrix of the incremental function of the state value of the t-th trajectory point, v t is the linear term coefficient matrix of the incremental function of the state value of the t-th trajectory point.

[0120] The action state value increment function can be expressed as follows:

[0121]

[0122] Among them, X t is the state variable, U t is the action variable, ΔQ(ΔX t ,ΔU t ) is the incremental function of the action state value of the t-th trajectory point, Q t is the quadratic coefficient matrix of the incremental function of the action state value of the t-th trajectory point, Q ΔXt,ΔXt is the quadratic term of the incremental function of the action state value of the t-th trajectory point ΔXt The second-order partial derivative matrix, Q ΔXt,ΔUt is the quadratic term of the incremental function of the action state value of the t-th trajectory point ΔX t Then ΔU t The matrix of partial derivatives, Q ΔUt,ΔXt is the quadratic term of the incremental function of the action state value of the t-th trajectory point ΔU t Then ΔX t The matrix of partial derivatives, Q ΔUt,ΔUt is the quadratic term of the incremental function of the action state value of the t-th trajectory point ΔU t The second-order partial derivative matrix, q t is the linear coefficient matrix of the action state value function of the t-th trajectory point, q ΔXt is the linear term of the action state value function of the t-th trajectory point ΔX t Partial derivative, q ΔUt is the linear term of the action state value function of the t-th trajectory point ΔU t Partial derivative.

[0123] Step S32: setting the initial state of the iteration.

[0124] In the iLQR solution process, reverse iteration is performed first, and then forward iteration is performed. Since the reverse iteration is performed first, the initial state actually corresponds to the last state, so the initial state reset is to reset the information related to the last state, which is expressed by the following formula:

[0125] V T+1 =0

[0126] v T+1 =0

[0127] Among them, V T+1 is the coefficient matrix of the quadratic term of the incremental function of the state value of the last trajectory point, v T+1 It is the coefficient matrix of the linear term of the incremental function of the state value of the last trajectory point.

[0128] Step S33 : Based on the initial state, reverse iterative optimization is performed according to the state value increment function and the action state value increment function to obtain a feedforward gain and a feedback gain.

[0129] It's important to note that feedforward gain is a key parameter in the feedforward control phase of a control system. Feedforward control is an open-loop control method that directly generates a control action based on changes in the disturbance or input to compensate for changes in the system output. Feedforward gain is used to adjust the intensity of this feedforward control action. Feedback gain is a key parameter in feedback control systems. In a feedback control system, a portion of the system's output signal is extracted (via a feedback loop) and compared with a reference input signal. The resulting deviation signal is then passed through a controller to adjust the system's output. Feedback gain is a coefficient that determines the degree to which the feedback signal influences the control action.

[0130] The reverse iteration starts from the end point and ends at the starting point. Each time step is an optimization solution process, which can be expressed by the following formula:

[0131]

[0132] The above optimization problem has no constraints and can be solved directly using the KKT condition. It can be expressed as follows:

[0133] ΔU t =K t ΔX t +k t

[0134]

[0135] Among them, K t is the feedback gain, k t is the feedforward gain.

[0136] The state value increment function is actually in ΔX t Take ΔU in the state t The cumulative cost increment generated by the action can be expressed as follows:

[0137]

[0138] Step S34 , performing forward iterative optimization according to the feedforward gain and the feedback gain to obtain a reference trajectory.

[0139] It should be noted that forward iteration is a process of gradually improving system performance. Starting from the initial state, according to certain rules and algorithms, the system parameters or control strategies are updated through multiple iterations to achieve the optimization goal.

[0140] In a feasible implementation, step S34 includes steps S341 to S343:

[0141] Step S341: Obtain a preset step length through line search.

[0142] It should be noted that line search is an important technique in optimization algorithms, used to find the optimal step size for the objective function in a given search direction. The preset step size is mainly used to update the parameters in the optimization algorithm.

[0143] The step size obtained through line search can help control the convergence speed and stability of the optimization process. If the step size is too large, the objective function value may skip the optimal value, or even cause the optimization process to diverge. On the other hand, if the step size is too small, while convergence may be guaranteed, the convergence speed will be too slow. Determining the optimal step size through line search is to find a balance between convergence speed and stability.

[0144] The k solved by the above reverse iteration is t It is actually the Newton direction of the optimization problem. The default step size in the forward iteration process is 1. When the actual model is significantly different from the simplified model, the default step size corresponding to the Newton direction may cause the solution to diverge. Therefore, the Armijo line search algorithm is used to solve a step size γ that ensures that the iterative result decreases.

[0145] Step S342 : Based on the preset step size, forward iterative optimization is performed through the feedforward gain and the feedback gain to update the state variables and action variables of the target model.

[0146] Based on the step size γ, the forward iteration is based on the feedforward gain k optimized by the reverse iteration. t and feedback gain K t Update the state variable X of the trajectory point sequence t and action variable U t , which can be expressed by the following formula:

[0147] ΔU t =K t ΔX t +γk t

[0148]

[0149] ΔX t+1 =A t ΔX t +B t ΔU t

[0150]

[0151] in, is the action variable of the last loop trajectory point, is the state variable of the last loop trajectory point.

[0152] Step S343, returning to the step of performing reverse iterative optimization based on the initial state and according to the state value increment function and the action state value increment function to obtain the feedforward gain and the feedback gain, until the result of the iterative optimization converges or reaches the preset number of iteration cycles to obtain the reference trajectory.

[0153] It should be noted that the reference trajectory is the trajectory result obtained by iLQR solution. It may not meet all the constraints. The reference trajectory needs to be post-processed later.

[0154] Trajectory planning is to plan the trajectory within a certain time domain range T and time step dt in the future. The number of trajectory points contained in the trajectory sequence is n = T / dt+1. The goal of the incremental iLQR solution is to calculate the Δcost of all trajectory points in the trajectory sequence. total Optimize to the minimum, which can be expressed by the following formula:

[0155] Δcost total =Δcost1+Δcost2…+Δcost n

[0156]

[0157] Among them, Δcost total is the total cost increment of all trajectory points in the trajectory sequence, Δcost t The incremental cost of the state transition cost for each trajectory point transder , C t is the quadratic coefficient matrix of the incremental cost of the state transition of the t-th trajectory point, C ΔXt,ΔXt is the quadratic term of the incremental cost of the state transition of the t-th trajectory point ΔX t The second-order partial derivative matrix, C ΔXt,ΔUt For the t-th trajectory point state transfer cost increment quadratic term, first calculate ΔX t Then ΔU t The matrix of partial derivatives, C ΔUt,ΔXt For the state transfer cost increment quadratic term of the t-th trajectory point, first calculate ΔU t Then ΔX t The matrix of partial derivatives, C ΔUt,ΔUt For the state transfer cost increment quadratic term of the t-th trajectory point, first calculate ΔU t The second-order partial derivative matrix, c t is the linear term coefficient matrix of the incremental cost of the state transition of the t-th trajectory point, c ΔXt is the incremental linear term of the state transition cost of the t-th trajectory point ΔX t Partial derivative, c ΔUt is the incremental linear term of the state transition cost of the t-th trajectory point ΔU t Partial derivative.

[0158] Incremental iLQR is performed through continuous iterations. Each loop begins with a reverse iteration followed by a forward iteration, and the trajectory is outputted only after the results converge or the maximum number of iterations is reached. Therefore, after completing a forward iteration, if the conditions for convergence or the maximum number of iterations are not met, the reverse and forward iterations must be repeated to obtain the reference trajectory.

[0159] Through loop iterative optimization, the accuracy of trajectory planning can be improved.

[0160] Step S35 , performing trajectory validity processing and trajectory safety processing on the reference trajectory to obtain a target trajectory.

[0161] It should be noted that the target trajectory is the trajectory finally obtained by trajectory planning that can be used for actual vehicle driving.

[0162] The incremental iLQR solution method means that its solution may not fully satisfy the constraints in all cases. Therefore, the trajectory requires post-processing, which mainly includes trajectory validity and safety processing. Trajectory validity processing mainly ensures that the velocity sign is consistent with the vehicle's forward direction and that the trajectory point curvature does not exceed the vehicle's steering limit. Trajectory safety processing mainly uses collision detection to determine whether the ego vehicle trajectory has a collision risk with other obstacle trajectories. In such cases, a fallback trajectory is used to replace the original solution trajectory. After processing is completed, the resulting trajectory is the target trajectory.

[0163] This embodiment provides a trajectory planning method based on incremental iterative linear quadratic regulator solution, which obtains a state value increment function and an action state value increment function according to the target model; sets an initial state of the iteration; based on the initial state, performs reverse iterative optimization according to the state value increment function and the action state value increment function to obtain a feedforward gain and a feedback gain; performs forward iterative optimization according to the feedforward gain and the feedback gain to obtain a reference trajectory; performs trajectory validity processing and trajectory safety processing on the reference trajectory to obtain a target trajectory, thereby improving the accuracy and applicability of trajectory planning.

[0164] Based on the first embodiment of the present application, in the fourth embodiment of the present application, the same or similar contents as those in the above-mentioned embodiment 1 can be referred to the above introduction and will not be described in detail later. Figure 5 , step S10 includes steps S11 to S12:

[0165] In step S11 , based on the Cartesian coordinate system, the state variables and action variables of the vehicle dynamics model are obtained according to the vehicle parameters.

[0166] It should be noted that vehicle parameters include the vehicle's x- and y-coordinates in the Cartesian coordinate system, the vehicle's heading angle, the vehicle's wheel angle, the vehicle's yaw rate, the distance to the rear axle center, the vehicle's speed, the vehicle's acceleration, the angular acceleration, and the jerk. A vehicle dynamics model is a mathematical model used to describe the temporal evolution of a vehicle's motion. It comprehensively considers various forces and torques acting on the vehicle, including tire forces, air resistance, gravity, engine power (or motor power), and other factors, accurately simulating the vehicle's driving characteristics under different operating conditions, such as acceleration, deceleration, and cornering.

[0167] Furthermore, it should be noted that state variables are a set of variables used to describe the state of a vehicle's dynamics system at a given moment. These variables fully represent the vehicle's current state of motion and, combined with the system's dynamics equations, can predict future changes in the vehicle's state.

[0168] Additionally, it should be noted that action variables are variables that can be controlled or changed, which directly affect the motion state of the vehicle and are usually operational inputs to the vehicle control system (such as the driver or the autonomous driving system).

[0169] Using the vehicle kinematic model instead of the ordinary mass model, the state variable X can be expressed as follows:

[0170] X=[x yθ δ ω sva] T

[0171] Where x is the x-coordinate of the ego vehicle in the Cartesian coordinate system, y is the y-coordinate of the ego vehicle in the Cartesian coordinate system, θ is the heading angle of the ego vehicle, δ is the wheel angle of the ego vehicle, ω is the yaw velocity of the ego vehicle, s is the distance to the center of the rear axle, v is the velocity of the ego vehicle, and a is the acceleration of the ego vehicle.

[0172] The action variable U can be expressed as follows:

[0173] U=[αjerk] T

[0174] Where α is the angular acceleration and jerk is the jerk.

[0175] Step S12: linearizing and incrementally expressing the vehicle dynamics model according to the state variables and the action variables to obtain an initial model.

[0176] In order to improve the stability and adaptability of the algorithm, the vehicle dynamics model is linearized and expressed in incremental form according to the state variables and action variables. After linearization, the incremental kinematic model of the vehicle can be expressed as follows:

[0177] Δf=A·ΔX+B·ΔU

[0178]

[0179] Where Δf is the state change at the next moment (relative to the linearization reference point), A is the state matrix, B is the input matrix, ΔX is the state change at the current moment, and ΔU is the action change at the current moment.

[0180] This embodiment provides a trajectory planning method based on an incremental iterative linear quadratic regulator solution. Based on a Cartesian coordinate system, the state variables and action variables of the vehicle dynamics model are obtained according to the vehicle parameters. Based on the state variables and the action variables, the vehicle dynamics model is linearized and expressed in incremental form to obtain an initial model, thereby improving the accuracy and applicability of trajectory planning.

[0181] It should be noted that the above examples are only used to understand the present application and do not constitute a limitation on the trajectory planning method of the present application based on the incremental iterative linear quadratic regulator solution. More forms of simple transformations based on this technical concept are all within the scope of protection of the present application.

[0182] This application also provides a trajectory planning device based on incremental iterative linear quadratic regulator solution, please refer to Figure 6 , the device comprises:

[0183] The model building module 10 is used to build an initial model based on a Cartesian coordinate system.

[0184] The model processing module 20 is used to perform objective function design and multi-circle model-based constraint processing on the initial model to obtain a target model.

[0185] The incremental solution module 30 is used to perform incremental iterative linear quadratic regulator solution and trajectory post-processing on the target model to obtain a target trajectory.

[0186] In one embodiment, the model processing module 20 is also used to obtain a lateral comfort index, a longitudinal comfort index and a constraint condition; calculate the initial objective function cost term in combination with the lateral comfort index and the longitudinal comfort index; based on the constraint condition, obtain the constraint condition cost term by approximating the vehicle frame through a multi-circle model; perform a weighted summation of the initial objective function cost term and the constraint condition cost term to obtain a state transition objective function; and obtain a target model based on the state transition objective function.

[0187] In one embodiment, the model processing module 20 is further configured to calculate the index costs of the lateral comfort index and the longitudinal comfort index in a quadratic form; and obtain the initial objective function cost term according to the index costs.

[0188] In one embodiment, the model processing module 20 is further used to obtain the weight corresponding to the constraint condition; calculate the degree of non-satisfaction of the constraint condition by approximating the vehicle frame through a multi-circle model; and obtain the constraint condition cost item based on the weight and the degree of non-satisfaction.

[0189] In one embodiment, the incremental solution module 30 is further used to obtain a state value incremental function and an action state value incremental function according to the target model; set an initial state of the iteration; based on the initial state, perform reverse iterative optimization according to the state value incremental function and the action state value incremental function to obtain a feedforward gain and a feedback gain; perform forward iterative optimization according to the feedforward gain and the feedback gain to obtain a reference trajectory; perform trajectory validity processing and trajectory safety processing on the reference trajectory to obtain a target trajectory.

[0190] In one embodiment, the incremental solution module 30 is further used to obtain a preset step size through line search; based on the preset step size, forward iterative optimization is performed through the feedforward gain and the feedback gain to update the state variables and action variables of the target model; return to the step of performing reverse iterative optimization based on the initial state and according to the state value incremental function and the action state value incremental function to obtain the feedforward gain and feedback gain, until the result of the iterative optimization converges or reaches a preset number of iteration cycles to obtain a reference trajectory.

[0191] In one embodiment, the model construction module 10 is further used to obtain state variables and action variables of the vehicle dynamics model based on the vehicle parameters based on a Cartesian coordinate system; and to express the vehicle dynamics model in linear and incremental form based on the state variables and the action variables to obtain an initial model.

[0192] The trajectory planning device based on an incremental iterative linear quadratic regulator solution provided in this application adopts the trajectory planning method based on an incremental iterative linear quadratic regulator solution in the above-mentioned embodiment, which can solve the technical problems of how to improve the accuracy and applicability of trajectory planning and reduce the impact of curvature fluctuations. Compared with the prior art, the beneficial effects of the trajectory planning device based on an incremental iterative linear quadratic regulator solution provided in this application are the same as the beneficial effects of the trajectory planning method based on an incremental iterative linear quadratic regulator solution provided in the above-mentioned embodiment, and the other technical features of the trajectory planning device based on an incremental iterative linear quadratic regulator solution are the same as the features disclosed in the above-mentioned embodiment method, and are not further described here.

[0193] The present application provides a trajectory planning device based on an incremental iterative linear quadratic regulator solution. The trajectory planning device based on an incremental iterative linear quadratic regulator solution includes: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions that can be executed by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the trajectory planning method based on the incremental iterative linear quadratic regulator solution in the above-mentioned embodiment one.

[0194] Reference below Figure 7 , which shows a schematic structural diagram of a trajectory planning device based on an incremental iterative linear quadratic regulator solution suitable for implementing an embodiment of the present application. The trajectory planning device based on an incremental iterative linear quadratic regulator solution in the embodiment of the present application may include, but is not limited to, mobile terminals such as mobile phones, laptop computers, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Portable Application Descriptions), PMPs (Portable Media Players), in-vehicle terminals (e.g., in-vehicle navigation terminals), and fixed terminals such as digital TVs and desktop computers. Figure 7 The trajectory planning device based on incremental iterative linear quadratic regulator solution shown is only an example and should not bring any limitation to the functions and scope of use of the embodiments of the present application.

[0195] like Figure 7As shown, the trajectory planning device based on the incremental iterative linear quadratic regulator solution may include a processing device 1001 (such as a central processing unit, a graphics processing unit, etc.), which can perform various appropriate actions and processes according to the program stored in the read-only memory (ROM: Read Only Memory) 1002 or the program loaded from the storage device 1003 to the random access memory (RAM: Random Access Memory) 1004. In RAM1004, various programs and data required for the operation of the trajectory planning device based on the incremental iterative linear quadratic regulator solution are also stored. The processing device 1001, ROM1002 and RAM1004 are connected to each other via a bus 1005. The input / output (I / O) interface 1006 is also connected to the bus. Typically, the following systems can be connected to the I / O interface 1006: an input device 1007 including, for example, a touch screen, a touchpad, a keyboard, a mouse, an image sensor, a microphone, an accelerometer, a gyroscope, etc.; an output device 1008 including, for example, a liquid crystal display (LCD), a speaker, a vibrator, etc.; a storage device 1003 including, for example, a magnetic tape, a hard disk, etc.; and a communication device 1009. The communication device 1009 can allow the trajectory planning device based on the incremental iterative linear quadratic regulator solution to communicate wirelessly or wired with other devices to exchange data. Although the figure shows a trajectory planning device based on the incremental iterative linear quadratic regulator solution with various systems, it should be understood that it is not required to implement or have all the systems shown. More or fewer systems can be implemented or have instead.

[0196] In particular, according to the embodiments disclosed in the present application, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, the embodiments disclosed in the present application include a computer program product comprising a computer program carried on a computer-readable medium, the computer program comprising program code for executing the method shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network via a communication device, or installed from a storage device 1003, or installed from a ROM 1002. When the computer program is executed by the processing device 1001, the above-mentioned functions defined in the method of the embodiment disclosed in the present application are executed.

[0197] The trajectory planning device based on an incremental iterative linear quadratic regulator solution provided in this application, which employs the trajectory planning method based on an incremental iterative linear quadratic regulator solution in the above-mentioned embodiment, can solve the technical problem of how to improve the accuracy and applicability of trajectory planning and reduce the impact of curvature fluctuations. Compared with the prior art, the beneficial effects of the trajectory planning device based on an incremental iterative linear quadratic regulator solution provided in this application are the same as the beneficial effects of the trajectory planning method based on an incremental iterative linear quadratic regulator solution provided in the above-mentioned embodiment. The other technical features of the trajectory planning device based on an incremental iterative linear quadratic regulator solution are the same as those disclosed in the method of the previous embodiment and are not further described here.

[0198] It should be understood that the various parts disclosed in this application can be implemented using hardware, software, firmware, or a combination thereof. In the description of the above embodiments, specific features, structures, materials, or characteristics can be combined in any one or more embodiments or examples in a suitable manner.

[0199] The above description is merely a specific embodiment of the present application, but the scope of protection of the present application is not limited thereto. Any changes or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in this application should be included in the scope of protection of this application. Therefore, the scope of protection of this application should be based on the scope of protection of the claims.

[0200] The present application provides a computer-readable storage medium having computer-readable program instructions (i.e., a computer program) stored thereon, and the computer-readable program instructions are used to execute the trajectory planning method based on incremental iterative linear quadratic regulator solution in the above-mentioned embodiment.

[0201] The computer-readable storage medium provided in this application may be, for example, a USB flash drive, but is not limited to electrical, magnetic, optical, electromagnetic, infrared, or semiconductor systems, systems or devices, or any combination thereof. More specific examples of computer-readable storage media may include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination thereof. In this embodiment, the computer-readable storage medium may be any tangible medium that contains or stores a program that can be used by or in conjunction with an instruction execution system, system or device. The program code contained on the computer-readable storage medium may be transmitted using any appropriate medium, including but not limited to: wires, optical cables, RF (Radio Frequency), etc., or any suitable combination thereof.

[0202] The above-mentioned computer-readable storage medium can be included in the trajectory planning device based on the incremental iterative linear quadratic regulator solution; or it can exist independently without being assembled into the trajectory planning device based on the incremental iterative linear quadratic regulator solution.

[0203] The computer-readable storage medium carries one or more programs. When the one or more programs are executed by a trajectory planning device based on an incremental iterative linear quadratic regulator solution, the trajectory planning device based on the incremental iterative linear quadratic regulator solution: constructs an initial model based on vehicle parameters; performs objective function design and constraint processing on the initial model to obtain a target model; and performs an incremental iterative linear quadratic regulator solution on the target model to obtain a target trajectory.

[0204] Computer program code for performing the operations of the present application may be written in one or more programming languages, or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, C++, and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user's computer, partially on the user's computer, as a stand-alone software package, partially on the user's computer and partially on a remote computer, or entirely on the remote computer or server. In cases involving a remote computer, the remote computer may be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., through the Internet using an Internet service provider).

[0205] The flow charts and block diagrams in the accompanying drawings illustrate the possible architecture, functions and operations of the systems, methods and computer program products according to various embodiments of the present application. In this regard, each box in the flow chart or block diagram can represent a module, program segment or a part of code, and the module, program segment or a part of code contains one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order than that marked in the accompanying drawings. For example, two boxes represented in succession can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or flow chart, and the combination of the boxes in the block diagram and / or flow chart can be implemented by a dedicated hardware-based system that performs the specified function or operation, or can be implemented by a combination of dedicated hardware and computer instructions.

[0206] The modules described in the embodiments of the present application may be implemented in software or hardware, wherein the name of a module does not necessarily limit the unit itself.

[0207] The computer-readable storage medium provided herein stores computer-readable program instructions (i.e., a computer program) for executing the aforementioned trajectory planning method based on an incremental iterative linear quadratic regulator solution. This computer-readable storage medium addresses the technical issues of improving the accuracy and applicability of trajectory planning and reducing the impact of curvature fluctuations. Compared to the prior art, the beneficial effects of the computer-readable storage medium provided herein are the same as those of the trajectory planning method based on an incremental iterative linear quadratic regulator solution provided in the aforementioned embodiments, and are not further elaborated upon herein.

[0208] The present application also provides a computer program product, comprising a computer program, which, when executed by a processor, implements the steps of the trajectory planning method based on incremental iterative linear quadratic regulator solution as described above.

[0209] The computer program product provided in this application can solve the technical problem of improving the accuracy and applicability of trajectory planning and reducing the impact of curvature fluctuations. Compared with the prior art, the beneficial effects of the computer program product provided in this application are the same as those of the trajectory planning method based on incremental iterative linear quadratic regulator solution provided in the above embodiment, and will not be elaborated here.

[0210] The above description is only part of the embodiments of the present application and does not limit the patent scope of the present application. All equivalent structural transformations made by using the contents of the present application specification and drawings under the technical concept of the present application, or direct / indirect application in other related technical fields are included in the patent protection scope of the present application.

Claims

1. A trajectory planning method based on incremental iterative linear quadratic regulator solution, characterized in that: The method comprises: Construct an initial model based on a Cartesian coordinate system; Performing objective function design and constraint processing based on a multi-circle model on the initial model to obtain a target model; performing incremental iterative linear quadratic regulator solving and trajectory post-processing on the target model to obtain a target trajectory; The step of performing objective function design and constraint processing based on a multi-circle model on the initial model to obtain a target model comprises: Obtaining horizontal comfort index, longitudinal comfort index and constraint conditions; Calculating an initial objective function cost term by combining the lateral comfort index and the longitudinal comfort index; Based on the constraints, a constraint cost term is obtained by approximating the vehicle bounding box through a multi-circle model; Performing a weighted summation on the initial objective function cost term and the constraint condition cost term to obtain a state transition objective function; Obtaining a target model according to the state transition target function; The step of obtaining a constraint cost term by approximating the vehicle bounding box by a multi-circle model based on the constraint condition includes: Obtaining the weight corresponding to the constraint condition; Calculating the degree of non-satisfaction of the constraint condition by approximating the vehicle boundary using a multi-circle model; A constraint cost item is obtained according to the weight and the degree of non-satisfaction.

2. The method according to claim 1, wherein The step of calculating the cost term of the initial objective function by combining the lateral comfort index and the longitudinal comfort index comprises: Calculating the index costs of the lateral comfort index and the longitudinal comfort index in a quadratic form; An initial objective function cost term is obtained according to the indicator cost.

3. The method according to claim 1, wherein The step of performing incremental iterative linear quadratic regulator solving and trajectory post-processing on the target model to obtain the target trajectory includes: Obtaining a state value increment function and an action state value increment function according to the target model; Set the initial state of the iteration; Based on the initial state, reverse iterative optimization is performed according to the state value increment function and the action state value increment function to obtain a feedforward gain and a feedback gain; Perform forward iterative optimization according to the feedforward gain and the feedback gain to obtain a reference trajectory; The reference trajectory is processed for trajectory validity and trajectory safety to obtain a target trajectory.

4. The method according to claim 3, wherein The step of performing forward iterative optimization according to the feedforward gain and the feedback gain to obtain a reference trajectory includes: Get the preset step size through line search; Based on the preset step size, performing forward iterative optimization through the feedforward gain and the feedback gain to update the state variables and action variables of the target model; Return to the step of performing reverse iterative optimization based on the initial state and according to the state value increment function and the action state value increment function to obtain the feedforward gain and the feedback gain, until the result of the iterative optimization converges or reaches a preset number of iteration cycles to obtain a reference trajectory.

5. The method according to claim 1, wherein The step of constructing the initial model based on the Cartesian coordinate system includes: Based on the Cartesian coordinate system, the state variables and action variables of the vehicle dynamics model are obtained according to the vehicle parameters; According to the state variables and the action variables, the vehicle dynamics model is linearized and expressed in incremental form to obtain an initial model.

6. A trajectory planning device based on incremental iterative linear quadratic regulator solution, characterized in that: The device comprises: A model building module, used to build an initial model based on a Cartesian coordinate system; A model processing module, configured to perform objective function design and constraint processing based on a multi-circle model on the initial model to obtain a target model; An incremental solution module, configured to perform incremental iterative linear quadratic regulator solution and trajectory post-processing on the target model to obtain a target trajectory; The model processing module is further used to obtain a lateral comfort index, a longitudinal comfort index and constraint conditions; Calculating an initial objective function cost term by combining the lateral comfort index and the longitudinal comfort index; Based on the constraints, a constraint cost term is obtained by approximating the vehicle bounding box through a multi-circle model; Performing a weighted summation on the initial objective function cost term and the constraint condition cost term to obtain a state transition objective function; Obtaining a target model according to the state transition target function; The model processing module is further used to obtain the weight corresponding to the constraint condition; Calculating the degree of non-satisfaction of the constraint condition by approximating the vehicle boundary using a multi-circle model; A constraint cost item is obtained according to the weight and the degree of non-satisfaction.

7. A trajectory planning device based on incremental iterative linear quadratic regulator solution, characterized in that: The device includes: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the computer program is configured to implement the steps of the trajectory planning method based on incremental iterative linear quadratic regulator solution according to any one of claims 1 to 5.

8. A storage medium, characterized in that: The storage medium is a computer-readable storage medium, and a computer program is stored on the storage medium. When the computer program is executed by a processor, the steps of the trajectory planning method based on incremental iterative linear quadratic regulator solution as described in any one of claims 1 to 5 are implemented.

Images