Track planning method, device and equipment based on incremental iteration linear quadratic regulator solution and storage medium

Abstract

The invention discloses a trajectory planning method and device based on incremental iteration linear quadratic regulator solution, equipment and a storage medium, and relates to the technical field of intelligent driving, and the method comprises the steps: building an initial model based on a Cartesian coordinate system; performing target function design and multi-circle model-based constraint processing on the initial model to obtain a target model; and performing incremental iteration linear quadratic regulator solution and trajectory post-processing on the target model to obtain a target trajectory. According to the invention, based on the thought of space-time joint planning, by constructing the vehicle model and carrying out increment iLQR solution, the accuracy and applicability of trajectory planning can be improved, and the influence of curvature fluctuation can be reduced.

Description

Technical Field

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

[0002] As the application of intelligent driving technology becomes more and more extensive and the application scenarios become more and more complex, the functional requirements for intelligent driving decision-making and planning modules are becoming higher and higher. The problems of the time-space separation scheme based on the Frenet coordinate system are also increasingly exposed. The time-space separation scheme generally performs lateral planning first and then longitudinal planning. From the perspective of the solution space, the solution space is artificially reduced, which is convenient for the implementation of the scheme. Because the solution space is artificially reduced, the optimal solution may be excluded from the feasible solution. In complex scenarios, the algorithm is likely to solve the result that is not the optimal solution, resulting in the intelligent driving performance not being intelligent enough. The objective function of path planning generally uses the indicator describing lateral motion as the cost item. When planning based on the Frenet coordinate system, the indicators describing lateral motion mainly include l′, l″, and l″′. When the curvature fluctuation of Frenet itself is large, even if the aforementioned indicators are optimized to 0 (the optimal result in the Frenet coordinate system), when the result in the Frenet coordinate system is transferred to the Cartesian coordinate system, the curvature fluctuation of Frenet itself will still be reflected in the result of the Cartesian coordinate system, resulting in comfort and even safety problems. At the same time, the vehicle is generally treated as a particle in the path planning mathematical model, and the influence of the vehicle width is considered when setting the QP boundary. Since the boundary problem of the intelligent driving vehicle is handled approximately, the processing capability for large turns 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] Perform objective function design and constraint processing based on a multi-circle model on the initial model to obtain a target model;

[0007] The target model is subjected to incremental iterative linear quadratic regulator solving and trajectory post-processing 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 lateral 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 constraint conditions, the constraint condition cost term is obtained by approximating the vehicle boundary 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 transfer objective function;

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

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

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

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

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

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

[0019] The degree of non-satisfaction of the constraint condition is calculated 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] Performing 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, 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;

[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 a feedforward gain and a 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 an initial model based on a 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 expressed in linear and incremental form to obtain an initial model.

[0034] In addition, to achieve the above purpose, the present application also proposes a trajectory planning device based on 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, used for performing 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 a 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 space-time planning, building a vehicle model and performing incremental iLQR solution, the optimal solution may be excluded from the feasible solution, the curvature fluctuation of Frenet itself affects the trajectory solution, and the technical problems of limited processing ability for large-angle or small-space scenes are solved. Compared with the existing technology, the accuracy and applicability of trajectory planning are improved and the impact of curvature fluctuation is reduced. 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 drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0045] Figure 1 A flow chart of the first embodiment of the trajectory planning method based on incremental iterative linear quadratic regulator solution provided in the present 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 flow chart 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 flow chart of a fourth embodiment of a trajectory planning method based on incremental iterative linear quadratic regulator solution provided in this application;

[0050] Figure 6 A schematic diagram of the module structure of a trajectory planning device based on an 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 described 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 only used to explain the technical solutions of the present application and are not used 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 is explained using the internal actuator of the trajectory planning system based on the incremental iterative linear quadratic regulator as the execution body.

[0057] Since the optimal solution of the trajectory planning method in the prior art 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 can be improved and the impact of curvature fluctuations can be 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 achieving the improvement of 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 running functions, such as a tablet computer, a personal computer, a mobile phone, etc., or an electronic device capable of realizing the above functions, etc. The following takes the internal actuator of the trajectory planning system based on the incremental iterative linear quadratic regulator solution as an example to illustrate this embodiment and the following embodiments.

[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 the present 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 a point in a plane or space. The initial model is a linearized vehicle kinematics incremental model. The linear system is simpler in analysis and calculation than the nonlinear system. In order to improve the stability and adaptability of the algorithm, the linearized vehicle kinematics incremental model is used for trajectory planning.

[0065] When constructing the initial model, it is necessary to combine the vehicle parameters for construction. The vehicle parameters include the x-coordinate in the vehicle's Cartesian coordinate system, the y-coordinate in the vehicle's Cartesian coordinate system, the vehicle's heading angle, the vehicle's wheel angle, the vehicle's yaw angular velocity, 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 the indicator describing lateral motion as the cost item. When planning based on the Frenet coordinate system, the indicators describing lateral motion mainly include l′, l″, and l″′. When the curvature of Frenet itself fluctuates greatly, even if the aforementioned indicators are optimized to 0 (the optimal result in the Frenet coordinate system), when the result in the Frenet coordinate system is transferred to the Cartesian coordinate system, the curvature fluctuation of Frenet itself will still be reflected in the result of the Cartesian coordinate system, resulting in comfort and even safety issues. This solution is based on the Cartesian coordinate system for planning, avoiding the process of converting the planning results from the Frenet coordinate system to the Cartesian coordinate system, reducing the computational complexity of the module, and solving the problem of synchronous changes in trajectory curvature caused by changes in the curvature of Frenet itself.

[0067] At the same time, the path planning mathematical model generally treats the vehicle as a particle, and considers the influence of vehicle width when setting the QP boundary. This type of processing method is an approximation of the intelligent driving vehicle boundary problem, resulting in limited processing capabilities for large corners or small space scenes. The vehicle model used in this solution can effectively improve planning accuracy compared to the particle model.

[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 restriction 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 influence of the vehicle border 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 motion state of the vehicle 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 the lateral comfort index first, and then the longitudinal comfort index, that is, the lateral planning is performed first and then the longitudinal planning. From the perspective of the solution space, the solution space is artificially reduced. However, due to the artificial reduction of the solution space, the optimal solution may be excluded from the feasible solution. In complex scenarios, the algorithm is likely to solve a non-optimal solution, resulting in insufficient intelligent driving performance. This scheme is based on the idea of ​​joint time-space planning. When designing the objective function, the lateral and longitudinal comfort indicators are simultaneously included in the calculation process, which can increase the scope of the solution space and improve 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 LinearQuadratic Regulator (iLQR) is a powerful model-based optimization control algorithm that is mainly used to deal with control problems of dynamic systems, especially nonlinear systems. It optimizes the control input sequence in an iterative manner based on the system's dynamic model and a quadratic cost function. In each iteration, iLQR linearizes the system, expands the nonlinear system into a linear approximation around the current state and control input, and then uses the theory of the linear quadratic regulator (LQR) to solve the optimal control problem. Specifically, it considers the state equation and cost function of the system, where the cost function usually contains the cost of the state error and the control input, and is in quadratic form, which makes it easier to solve the optimality condition. iLQR starts from an initial control trajectory, continuously linearizes the system and solves the LQR problem, and updates the control trajectory according to the solution results until the convergence condition is reached. 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 costs of the system. In each iteration step, 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 the incremental iLQR model, which is applicable to both the global Cartesian coordinate system and the local Cartesian coordinate system. This solution is applicable regardless of whether the output of the decision module is in the global coordinate system or the local coordinate system, which improves the applicability of the solution and avoids the consumption of computing power in coordinate conversion. At the same time, the incremental iLQR algorithm is used for solving, which is not only efficient but also can use the multi-circle model to approximately consider the impact of the vehicle boundary on the planning result.

[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 speed is consistent with the vehicle's forward direction, and the trajectory point curvature does not exceed the vehicle's steering limit. Trajectory safety processing mainly determines whether there is a risk of collision between the vehicle's trajectory and other obstacle trajectories through collision detection. In cases where there is a risk of collision, the fallback trajectory is used to replace the original solution trajectory.

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

[0077] This embodiment provides a trajectory planning method based on incremental iterative linear quadratic regulator solution. Based on the idea of ​​joint space-time planning, by constructing a vehicle model and performing incremental iLQR solution, it 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 ability of large-angle or small-space scenes is limited, thereby achieving the goal of improving the accuracy and applicability of trajectory planning and reducing 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-mentioned embodiment 1 can be referred to the above introduction, and will not be repeated in the following. Figure 2 , step S20 includes steps S21 to S25:

[0079] Step S21, obtaining 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 and rear frame consistency target, the rough track target, the wheel angle target, the vehicle yaw rate target and the angular acceleration target. The longitudinal comfort index mainly includes the front and rear consistency target, the acceleration target and the jerk target.

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

[0082] Step S22, calculating the 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 a cost term obtained by weighted summation of the costs of the lateral and longitudinal comfort indices.

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

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

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

[0087] Calculating the cost of an index in quadratic form is a method for quantitatively evaluating system performance indicators. It is based on some variables of the system and expresses the cost of the index as a quadratic function of these variables. In this quadratic function, the coefficients of the square terms of the variables themselves and the coefficients of the product terms between different variables are included. These coefficients are weights used to measure the contribution of different variables and their interactions to the cost of the index. For example, in vehicle motion control, if variables such as speed, acceleration, and jerk are involved, we can assign different weights according to their degree of influence on system performance to form a quadratic function to calculate the cost. It provides us with a concise mathematical model, which facilitates us to use optimization algorithms, such as minimizing this cost function, to find the optimal system parameters. At the same time, by adjusting the weights, we can balance the impact of different factors on the cost of the index according to actual needs. For example, when considering comfort, we can assign appropriate weights to acceleration and jerk so that the optimization results of the system are more in line with our expectations for comfort.

[0088] The target costs of the front and back frame consistency target, rough trajectory target, wheel angle target, vehicle yaw rate target, angular acceleration target, front and back consistency target, acceleration target and jerk target are calculated in quadratic form, and the set of target costs obtained is the indicator 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 is the target cost of the consistency between the previous and next frames, cost warmstart The coarse trajectory target cost provided to the decision module, cost δ is the wheel angle target cost, cost ω is the yaw rate target cost, cost α is the angular acceleration target cost, cost a is the longitudinal acceleration target cost, cost jerk is the longitudinal acceleration target cost.

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

[0094] Step S23, based on the constraint condition, approximate the vehicle frame through a multi-circle model to obtain a constraint condition cost item.

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

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

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

[0098] Step S231, obtaining 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 entire optimization problem. Constraints can be assigned corresponding weights based on the understanding and experience of the system, or some initial weights can be set first, and the results can be observed by running the optimization algorithm, and then the weights can be adjusted and re-run, and this process can be repeated until the expected 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 through 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 border, and the degree of non-satisfaction of the constraint conditions is calculated using the shortest distance from the centers of the three circles to the border.

[0102] Step S233, obtaining 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, it is possible to 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 transfer 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 transfer 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, approximates the vehicle frame through a multi-circle model to obtain a constraint cost term; performs a weighted summation of the initial objective function cost term and the constraint cost term to obtain a state transfer objective function; obtains a target model based on the state transfer 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-mentioned embodiment 1 can be referred to the above introduction, and will not be repeated in the following. 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 should be noted that the state value increment function is usually used to describe the amount of change in the system between different states. In a dynamic system, the state of the system will change over time or other factors, and the state value increment function is a tool to quantify this state change. The action state value increment function is mainly used to describe the amount of change in the state value of the system before and after performing a certain action. Assume that the system has a set of state variables, which can represent different properties or performance indicators of the system. The state of the system is represented by a vector S. When the system performs an action a, it will cause a change in state S.

[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 by the following formula:

[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 transfer at the t-th trajectory point, V t is the coefficient matrix of the quadratic term of the incremental function of the state value of the t-th trajectory point, v t It 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 by the following formula:

[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 coefficient matrix of the quadratic term 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 term 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 should be noted that the feedforward gain is an important parameter of the feedforward control link in the control system. Feedforward control is an open-loop control method that directly generates a control action based on the change of the disturbance or input to compensate for the change of the system output. The feedforward gain is used to adjust the intensity of this feedforward control action. The feedback gain is a key parameter in the feedback control system. In the feedback control system, a part of the system's output signal is taken out (through the feedback link), compared with the reference input signal, and the resulting deviation signal is then passed through the controller to adjust the system's output. The feedback gain is a coefficient used to determine the degree of influence of the feedback signal on 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, which 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 feed-forward gain.

[0136] The state value increment function is actually in ΔX t Take ΔU 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 manner, step S34 includes steps S341 to S343:

[0141] Step S341, obtaining a preset step length through line search.

[0142] It should be noted that line search is an important technique in optimization algorithms, which is used to find the optimal step size of 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 by line search can help control the convergence speed and stability of the optimization process. If the step size is too large, it may cause the objective function value to skip the optimal value, or even cause the optimization process to diverge; while if the step size is too small, although it may guarantee convergence, it will lead to too slow convergence speed. The optimal step size is obtained through line search in order to find a balance between convergence speed and stability.

[0144] The k solved by the reverse iteration above 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 quite 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, It 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 a preset number of iteration cycles to obtain a 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 convert the Δcost of all trajectory points in the trajectory sequence into 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 coefficient matrix of the quadratic term of the state transfer cost increment of the t-th trajectory point, C ΔXt,ΔXt is the quadratic term of the state transition cost increment of the t-th trajectory point ΔX t The second-order partial derivative matrix, C ΔXt,ΔUt The quadratic term of the state transfer cost increment of the t-th trajectory point is first calculated for ΔX t Then ΔU t The matrix of partial derivatives, C ΔUt,ΔXt The quadratic term of the state transfer cost increment of the t-th trajectory point is first calculated for ΔU t Then ΔX t The matrix of partial derivatives, C ΔUt,ΔUt The quadratic term of the state transfer cost increment of the t-th trajectory point is first calculated for Δ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 through continuous loop iteration. Each loop process first performs a reverse iteration process and then a forward iteration process. It will not exit the loop until the result converges or the maximum number of loops is reached, and the trajectory result is output. Therefore, after completing a forward iteration, if the result converges or the maximum number of loops is not met, the reverse iteration and forward iteration process must be performed again to finally 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 and can be used for actual vehicle driving.

[0162] The incremental iLQR solution method determines that its solution result cannot fully meet the constraints in all cases, so the trajectory needs to be post-processed, mainly including 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 determines whether the vehicle's trajectory has a collision risk with other obstacle trajectories through collision detection. In the case of a collision risk, the fallback trajectory is used to replace the original solution trajectory. After the processing is completed, the trajectory result obtained 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 first embodiment can be referred to the above description, and will not be described in detail later. Figure 5 , step S10 includes steps S11 to S12:

[0165] Step S11, based on the Cartesian coordinate system, obtain the state variables and action variables of the vehicle dynamics model according to the vehicle parameters.

[0166] It should be noted that the vehicle parameters include the x-coordinate in the self-vehicle Cartesian coordinate system, the y-coordinate in the self-vehicle Cartesian coordinate system, the self-vehicle heading angle, the self-vehicle wheel angle, the self-vehicle yaw rate, the distance to the rear axle center, the self-vehicle speed, the self-vehicle acceleration, the angular acceleration and the jerk. The vehicle dynamics model is a mathematical model used to describe the change of the vehicle's motion state over time. It comprehensively considers the effects of various forces and torques of the vehicle, including tire force, air resistance, gravity, engine driving force (or motor driving force) and many other factors, so as to accurately simulate the driving characteristics of the vehicle under different working conditions, such as acceleration, deceleration, turning, etc.

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

[0168] In addition, 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 automatic 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] Among them, x is the x-coordinate of the vehicle in the Cartesian coordinate system, y is the y-coordinate of the vehicle in the Cartesian coordinate system, θ is the heading angle of the vehicle, δ is the wheel angle of the vehicle, ω is the yaw velocity of the vehicle, s is the distance to the center of the rear axle, v is the speed of the vehicle, and a is the acceleration of the vehicle.

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

[0173] U=[αjerk] T

[0174] Among them, α is the angular acceleration and jerk is the jerk.

[0175] Step S12: 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.

[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. The linearized vehicle kinematics incremental model can be expressed by the following formula:

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

[0178]

[0179] Among them, Δ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 incremental iterative linear quadratic regulator solution, which is based on the Cartesian coordinate system and obtains the state variables and action variables of the vehicle dynamics model 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] The present 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 transfer objective function; and obtain a target model based on the state transfer objective function.

[0187] In one embodiment, the model processing module 20 is further used to calculate the index cost 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 cost.

[0188] In one embodiment, the model processing module 20 is also 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 also 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 also used to obtain a preset step size through a 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 the 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 building module 10 is also used to obtain state variables and action variables of the vehicle dynamics model based on the vehicle parameters based on the Cartesian coordinate system; 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.

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

[0193] The present application provides a trajectory planning device based on an incremental iterative linear quadratic regulator solution, and 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 diagram of the structure of a trajectory planning device based on incremental iterative linear quadratic regulator solution suitable for implementing the embodiment of the present application. The trajectory planning device based on 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, laptops, digital broadcast receivers, PDAs (Personal Digital Assistants), PADs (Portable Application Descriptions: tablet computers), PMPs (Portable Media Players: portable multimedia players), vehicle-mounted terminals (such as vehicle-mounted navigation terminals), etc., and fixed terminals such as digital TVs, desktop computers, etc. Figure 7 The trajectory planning device based on incremental iterative linear quadratic regulator solution is shown as an example only 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 processor, etc.), which can perform various appropriate actions and processes according to the program stored in the read-only memory (ROM: ReadOnly Memory) 1002 or the program loaded from the storage device 1003 to the random access memory (RAM: RandomAccess 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: input devices 1007 including, for example, a touch screen, a touchpad, a keyboard, a mouse, an image sensor, a microphone, an accelerometer, a gyroscope, etc.; output devices 1008 including, for example, a liquid crystal display (LCD), a speaker, a vibrator, etc.; storage devices 1003 including, for example, a magnetic tape, a hard disk, etc.; and communication devices 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 may be implemented or have instead.

[0196] In particular, according to the embodiments disclosed in the present application, the process described above with reference to the flowchart can be implemented as a computer software program. For example, the embodiments disclosed in the present application include a computer program product, which includes a computer program carried on a computer-readable medium, and the computer program includes a 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 through 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 incremental iterative linear quadratic regulator solution provided by the present application adopts the trajectory planning method based on incremental iterative linear quadratic regulator solution in the above embodiment, which can solve the technical problem of how to improve the accuracy and applicability of trajectory planning and reduce the influence of curvature fluctuations. Compared with the prior art, the beneficial effects of the trajectory planning device based on incremental iterative linear quadratic regulator solution provided by the present application are the same as the beneficial effects of the trajectory planning method based on incremental iterative linear quadratic regulator solution provided by the above embodiment, and the other technical features of the trajectory planning device based on incremental iterative linear quadratic regulator solution are the same as the features disclosed in the method of the previous embodiment, which will not be repeated here.

[0198] It should be understood that the various parts disclosed in this application can be implemented by 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 is only a specific implementation of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

[0200] The present application provides a computer-readable storage medium having computer-readable program instructions (i.e., computer programs) 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 the present 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 of the above. 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 of the above. In this embodiment, the computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in combination 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 of the above.

[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 above-mentioned computer-readable storage medium carries one or more programs. When the above-mentioned 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 the 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 separate software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In the case of 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., via the Internet using an Internet service provider).

[0205] The flow chart and block diagram in the accompanying drawings illustrate the possible architecture, function and operation of the system, method and computer program product according to various embodiments of the present application. In this regard, each square box in the flow chart or block diagram can represent a module, a program segment or a part of a code, and the module, the program segment or a part of the 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 square box can also occur in a sequence different from that marked in the accompanying drawings. For example, two square 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 square box in the block diagram and / or flow chart, and the combination of the square boxes in the block diagram and / or flow chart can be implemented with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0206] The modules involved in the embodiments described in this application may be implemented by software or hardware, wherein the name of the module does not constitute a limitation on the unit itself in some cases.

[0207] The readable storage medium provided by the present application is a computer-readable storage medium, which stores computer-readable program instructions (i.e., computer programs) for executing the above-mentioned trajectory planning method based on incremental iterative linear quadratic regulator solution, and can solve the technical problems of how to improve the accuracy and applicability of trajectory planning and reduce the influence of curvature fluctuations. Compared with the prior art, the beneficial effects of the computer-readable storage medium provided by the present application are the same as the beneficial effects of the trajectory planning method based on incremental iterative linear quadratic regulator solution provided by the above-mentioned embodiment, and will not be repeated here.

[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 by the present application 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 computer program product provided by the present application are the same as the beneficial effects of the trajectory planning method based on incremental iterative linear quadratic regulator solution provided by the above embodiment, which will not be repeated here.

[0210] The above descriptions are only some embodiments of the present application, and are not intended to limit the patent scope of the present application. All equivalent structural changes made using the contents of the present application specification and drawings under the technical concept of the present application, or direct / indirect applications 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; Perform objective function design and constraint processing based on a multi-circle model on the initial model to obtain a target model; The target model is subjected to incremental iterative linear quadratic regulator solving and trajectory post-processing to obtain a target trajectory.

2. The method according to claim 1, characterized in that 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 lateral 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 constraint conditions, the constraint condition cost term is obtained by approximating the vehicle boundary 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 transfer objective function; A target model is obtained according to the state transfer target function.

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

4. The method according to claim 2, characterized in that The step of obtaining the constraint condition cost item by approximating the vehicle frame by a multi-circle model based on the constraint condition comprises: Obtaining a weight corresponding to the constraint condition; The degree of non-satisfaction of the constraint condition is calculated 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.

5. The method according to claim 1, characterized in that The step of performing incremental iterative linear quadratic regulator solving and trajectory post-processing on the target model to obtain the target trajectory comprises: 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; Performing 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.

6. The method according to claim 5, characterized in that The step of performing forward iterative optimization according to the feedforward gain and the feedback gain to obtain a reference trajectory comprises: Get the 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 increment function and the action state value increment function to obtain a feedforward gain and a feedback gain, until the result of the iterative optimization converges or reaches a preset number of iteration cycles to obtain a reference trajectory.

7. The method according to claim 1, characterized in that The step of constructing the initial model based on the Cartesian coordinate system comprises: 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 expressed in linear and incremental form to obtain an initial model.

8. 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, used for performing objective function design and constraint processing based on a multi-circle model on the initial model to obtain a target model; The incremental solution module is used to perform incremental iterative linear quadratic regulator solution and trajectory post-processing on the target model to obtain a target trajectory.

9. A trajectory planning device based on incremental iterative linear quadratic regulator solution, characterized in that: The device comprises: 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 as described in any one of claims 1 to 7.

10. 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 7 are implemented.

Images