Vehicle driving track planning method and system, vehicle and equipment
By directly using the vehicle coordinate system in vehicle trajectory planning, and optimizing it with iterative linear secondary regulator, the high cost problems of degradation of model accuracy and dependence on external solvers in the prior art are solved, and higher trajectory planning accuracy and stability are achieved.
Patent Information
- Application Number
- CN202510008000.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-01-02
- Publication Date
- 2025-06-06
AI Technical Summary
The existing vehicle trajectory planning algorithm is based on the reference line coordinate system, resulting in a decrease in model accuracy; at the same time, relying on external solvers has edge problems and high costs, and the development cost of self-developed solvers is high and difficult.
Model construction and problem solving are directly based on the vehicle coordinate system, and optimization solutions are used using an iterative linear quadratic regulator to avoid relying on external solvers.
Improves the model accuracy and solution stability of trajectory planning, reducing development costs and the risk of relying on external solvers.
Smart Images
Figure CN120096593A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of vehicle technology, and in particular to a vehicle driving trajectory planning method, system, vehicle and equipment. Background Art
[0002] Computer technology, information technology, automatic control technology and artificial intelligence technology are gradually being applied to the field of autonomous driving technology. Vehicle trajectory planning technology is one of the key technologies for autonomous driving, and it is also a basic problem and necessary condition for studying intelligent vehicles. Vehicle trajectory planning technology is to calculate a driving trajectory that meets safety, comfort and feasibility based on the state and environmental information of the vehicle. At present, the commonly used trajectory planning algorithms are optimization-based methods, among which model predictive control (MPC) and its improved algorithms are mostly used. This type of algorithm builds a model based on the vehicle model, then constructs an optimization objective function, uses an optimization solver to solve it, and obtains the optimal state quantity, which is the trajectory planning result.
[0003] The following technical defects exist: (1) Many algorithms are based on the reference line coordinate system (sl) for model construction and problem solving. Due to the coordinate transformation involved, the model accuracy will be reduced to a certain extent compared with the actual model; (2) Trajectory planning based on optimization solutions usually rely on external solvers (osqp, qpOASES, ipopt, etc.). Open source solvers have some marginal solution problems that are difficult to solve. Commercial solvers are expensive, and self-developed solvers are expensive and difficult to develop. Summary of the invention
[0004] Based on this, it is necessary to provide a vehicle driving trajectory planning method, system, vehicle and equipment to address the above-mentioned technical problems, directly build models and solve problems based on the vehicle coordinate system, which can effectively improve the model accuracy of trajectory planning. In addition, an iterative linear quadratic regulator is used to solve the optimization problem, which does not rely on an external solver. The solution process is controllable and can effectively improve the accuracy and solution stability of trajectory planning.
[0005] In a first aspect, a vehicle driving trajectory planning method is provided, comprising:
[0006] The lane line is fitted based on the vehicle status and the perceived lane line information.
[0007] Based on the lateral motion information of the vehicle in the vehicle coordinate system and the lane line obtained by fitting, a discrete-time vehicle kinematic model is obtained;
[0008] Based on the vehicle kinematic model, defining an objective function;
[0009] The objective function is optimized and solved by using an iterative linear quadratic regulator to obtain the driving trajectory of the vehicle.
[0010] In some examples, the discrete-time vehicle kinematic model is obtained based on the lateral motion information of the vehicle in the vehicle coordinate system and the lane line obtained by fitting, including:
[0011] Based on the lateral motion information of the vehicle in the vehicle coordinate system, the lateral motion of the vehicle is modeled in continuous time to obtain a lateral motion model;
[0012] According to the perceived lane line information, the lateral motion reference model of the vehicle is obtained;
[0013] Obtaining an error model of lateral motion according to the lateral motion model and the lateral motion reference model;
[0014] Obtaining an initial state error of the error model according to the kinematic relationship and the lane line;
[0015] Based on the initial state error of the error model, a standard state space model is obtained;
[0016] The standard state space model is discretized to obtain a discrete-time vehicle kinematic model.
[0017] In some examples, optimizing and solving the objective function using an iterative linear quadratic regulator to obtain a driving trajectory of the vehicle includes:
[0018] Using an obstacle function to replace the inequality constraint in the objective function;
[0019] Define the gradient matrix of the objective function;
[0020] Define the Hessian matrix of the objective function;
[0021] Using abstract functions to express the vehicle kinematic model;
[0022] defining the Jacobian matrix of the vehicle kinematic model;
[0023] Define the optimal state value function;
[0024] Define the first and second partial derivatives of the optimal state value function;
[0025] The objective function is solved by using an iterative linear quadratic regulator to obtain the driving trajectory.
[0026] In some examples, solving the objective function using an iterative linear quadratic regulator to obtain the driving trajectory includes:
[0027] Calculating a state quantity sequence according to the initial state quantity, the initial control quantity sequence and the vehicle kinematic model;
[0028] Calculating an initial objective function according to the state quantity sequence and the control quantity sequence;
[0029] The following steps are repeated until the convergence condition is met:
[0030] Perform reverse recursion and forward recursion;
[0031] Update the objective function according to the new state quantity sequence and control quantity sequence.
[0032] In some examples, this also includes:
[0033] Determine whether the absolute error converges and whether the relative error converges;
[0034] If the absolute error converges and the relative error converges, it is determined that the convergence condition is satisfied.
[0035] In some examples, after obtaining the driving trajectory of the vehicle, the method further includes:
[0036] The vehicle is controlled to travel according to the travel trajectory.
[0037] In a second aspect, a vehicle driving trajectory planning system is provided, comprising:
[0038] A fitting module is used to fit the lane line according to the vehicle state and the perceived lane line information;
[0039] A model building module, used to obtain a discrete-time vehicle kinematic model based on the vehicle lateral motion information in the vehicle coordinate system and the lane line obtained by fitting;
[0040] The planning module is used to define an objective function based on the vehicle kinematic model, and optimize and solve the objective function using an iterative linear quadratic regulator to obtain a driving trajectory of the vehicle.
[0041] In a third aspect, a vehicle is provided, comprising: a vehicle driving trajectory planning system according to the second aspect described above.
[0042] In a fourth aspect, a computer device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the steps of the vehicle driving trajectory planning method of the above-mentioned first aspect and any possible implementation method of the first aspect are implemented.
[0043] In a fifth aspect, a computer-readable storage medium is provided, on which a computer program is stored. When the program is executed by a processor, the steps of the vehicle driving trajectory planning method of the above-mentioned first aspect and any possible implementation method of the first aspect are implemented.
[0044] In a sixth aspect, a computer program product is provided, on which a computer program is stored. When the program is executed by a processor, the steps of the vehicle driving trajectory planning method of the above-mentioned first aspect and any possible implementation method of the first aspect are implemented.
[0045] Using the embodiment of the present application, firstly, based on the lateral motion information of the vehicle in the vehicle coordinate system and the fitted lane lines, a discrete-time vehicle kinematic model is obtained, then based on the vehicle kinematic model, an objective function is defined, and finally an iterative linear quadratic regulator is used to optimize and solve the objective function, thereby obtaining the vehicle's driving trajectory. Model construction and problem solving directly based on the vehicle coordinate system can effectively improve the model accuracy of trajectory planning. In addition, the use of an iterative linear quadratic regulator to solve the optimization problem does not rely on an external solver, and the solution process is controllable, which can effectively improve the accuracy of trajectory planning and the stability of the solution. BRIEF DESCRIPTION OF THE DRAWINGS
[0046] Other features, objects and advantages of the present application will become more apparent by reading the detailed description of non-limiting embodiments made with reference to the following drawings:
[0047] Figure 1 A flow chart of a vehicle driving trajectory planning method provided in an embodiment of the present application;
[0048] Figure 2 A structural block diagram of a vehicle driving trajectory planning system provided in an embodiment of the present application;
[0049] Figure 3 A structural block diagram of a computer device provided in an embodiment of the present application. DETAILED DESCRIPTION
[0050] The present application is further described in detail below in conjunction with the embodiments and drawings. It is to be understood that the specific embodiments described herein are only used to explain the relevant application, rather than to limit the application. It is also necessary to explain that, for ease of description, only the parts related to the application are shown in the drawings.
[0051] It should be noted that, in the absence of conflict, the embodiments of the present application, that is, the features of the embodiments, can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.
[0052] The following describes in detail the vehicle driving trajectory planning method, system, device and medium according to the embodiments of the present application in conjunction with the accompanying drawings.
[0053] Figure 1 FIG. 1 is a flow chart of a vehicle driving trajectory planning method according to an embodiment of the present application. Figure 1 As shown, the vehicle driving trajectory planning method according to the embodiment of the present application includes the following steps:
[0054] S101: fitting a lane line according to the vehicle state and the perceived lane line information.
[0055] That is, according to the lane line provided by perception, and according to the vehicle speed and lateral position provided by positioning, the lane line is fitted into a fifth-order polynomial y(t), where the lane line is fitted into a fifth-order polynomial y(t) as follows:
[0056] y(t)=c 5 t 5 +c 4 t 4 +c 3 t 3 +c 2 t 2 +c 1 t+c 0
[0057] S102: Obtaining a discrete-time vehicle kinematic model based on the lateral motion information of the vehicle in the vehicle coordinate system and the lane line obtained by fitting.
[0058] In one embodiment of the present application, a discrete-time vehicle kinematic model is obtained based on the vehicle lateral motion information in a vehicle coordinate system and the lane line obtained by fitting, including: based on the vehicle lateral motion information in the vehicle coordinate system, modeling the vehicle's lateral motion in continuous time to obtain a lateral motion model; obtaining a vehicle lateral motion reference model based on the perceived lane line information; obtaining a lateral motion error model based on the lateral motion model and the lateral motion reference model; obtaining an initial state error of the error model based on the kinematic relationship and the lane line; obtaining a standard state space model based on the initial state error of the error model; and discretizing the standard state space model to obtain a discrete-time vehicle kinematic model.
[0059] Specifically, trajectory planning takes into account the lateral motion of the vehicle. Therefore, the lateral motion can be modeled in continuous time to obtain a lateral motion model, where the lateral motion model is expressed as follows:
[0060]
[0061] Among them, py (t) is the lateral position of the vehicle, v y (t) is the lateral velocity of the vehicle, a y (t) is the lateral acceleration of the vehicle, j y (t) is the lateral acceleration of the vehicle.
[0062] From the lane line information obtained by perception, a reference model of lateral motion can be obtained as follows
[0063]
[0064] Among them, p y-ref (t) is the reference lateral position of the vehicle, v y-ref (t) is the reference lateral velocity of the vehicle, a y-ref (t) is the vehicle reference lateral acceleration, j y-ref (t) is the vehicle reference lateral acceleration.
[0065] Let the lateral position error be p e (t) = p y (t)-p y-ref (t), the lateral velocity error is v e (t) = v y (t)-v y-ref (t), the lateral acceleration error is a e (t) = a y (t)-a y-ref (t), the error model of lateral motion can be obtained. The error model of lateral motion is expressed as follows:
[0066]
[0067] According to the kinematic relationship and the lane line y(t), the initial state error can be obtained as follows:
[0068]
[0069] Assume the state quantity is x(t)=[p e (t),v e (t),a e (t)] T , the control quantity is u(t)=j y (t), the interference amount is z(t)=j y-ref (t), the above equation can be transformed into a standard state space model, as shown below:
[0070]
[0071] in:
[0072]
[0073] For the above model, use forward difference change to discretize and get:
[0074] x(k+1)=A d x(k)+B d u(k)+E d z(k)
[0075] Where T is the discrete step length,
[0076]
[0077] Thus, the discrete-time vehicle kinematic model is obtained, which is expressed as follows:
[0078]
[0079] S103: Based on the vehicle kinematics model, define an objective function.
[0080] In one embodiment of the present application, the objective function is expressed as follows:
[0081]
[0082] stx(k+1)=A d x(k)+B d u(k)+E d z(k),k=0,1,K,N-1
[0083] x(k) min ≤x(k)≤x(k) max
[0084] u(k) min ≤u(k)≤u(k) max
[0085] Among them, J is the objective function, W xf is the weight matrix of the terminal state quantity, W x is the weight matrix of the state quantity, W u is the weight matrix of the control quantity, x(k) min and x(k) max is the minimum and maximum value of the state quantity x(k) at time k, u(k) min and u(k) max is the minimum and maximum value of the control quantity u(k) at time k.
[0086] In addition, the state quantity x(k) min and x(k) maxThe lateral position error limit in can be obtained by processing obstacles and lane lines to obtain the upper and lower boundaries of the lateral position of the vehicle at each moment.
[0087] S104: Optimizing and solving the objective function using an iterative linear quadratic regulator to obtain a driving trajectory of the vehicle.
[0088] In one embodiment of the present application, the objective function is optimized and solved by an iterative linear quadratic regulator to obtain a driving trajectory of the vehicle, including: replacing the inequality constraint in the objective function with an obstacle function; defining a gradient matrix of the objective function; defining a Hessian matrix of the objective function;
[0089] The vehicle kinematic model is expressed by using an abstract function; the Jacobian matrix of the vehicle kinematic model is defined; the optimal state value function is defined; the first-order partial derivative and the second-order partial derivative of the optimal state value function are defined; and the objective function is solved by using an iterative linear quadratic regulator to obtain the driving trajectory.
[0090] In this example, an iterative linear quadratic regulator is used to solve the objective function to obtain the driving trajectory, including: calculating a state quantity sequence according to an initial state quantity, an initial control quantity sequence and the vehicle kinematic model; calculating an initial objective function according to the state quantity sequence and the control quantity sequence; looping through the following steps until convergence conditions are met: performing reverse recursion and forward recursion; and updating the objective function according to a new state quantity sequence and a control quantity sequence.
[0091] In this example, the method further includes: judging whether the absolute error converges and whether the relative error converges; if the absolute error converges and the relative error converges, determining that the convergence condition is satisfied.
[0092] Specifically, since the above objective function has inequality constraints, it is necessary to use the iterative linear quadratic regulator (ILQR) method to solve it. It is necessary to transform it and put the inequality constraints into the objective function. The barrier function is usually used to replace it, as follows:
[0093]
[0094] stx(k+1)=A d x(k)+B d u(k)+E d z(k),k=0,1,K,N-1
[0095] The barrier function is B(x(k),u(k))=-b[(x(k)max -x(k)) 2 +(x(k)-x(k) min ) 2 ], b is the barrier function gain coefficient.
[0096] The gradient matrix of the objective function is defined as:
[0097]
[0098] The Hessian matrix of the objective function is defined as:
[0099]
[0100] The model is expressed using an abstract function as follows:
[0101] x(k+1)=f(x(k),u(k))+E d z(k)→x(k+1)=A d x(k)+B d u(k)+E d z(k)
[0102] The Jacobian matrix of the model is defined as:
[0103]
[0104] Define the optimal state value function V k (x(k)) is:
[0105]
[0106] The optimal state value function V in the above formula k (x(k)) represents the minimum objective value function J(x,u) when taking the optimal u(k).
[0107] Define the first-order partial derivative of the optimal state value function V x and the second-order partial derivative V xx for:
[0108]
[0109] The main process of the Iterative Linear Quadratic Regulator (ILQR) method is as follows:
[0110] According to the initial state x(0) and the initial control sequence [u(0),u(1),K,u(N-1)] T (usually initialized to zero vector), according to the system model x(k+1)=A dx(k)+B d u(k)+E d z(k), calculate the state sequence [x(1), x(2), K, x(N)] T ;
[0111] According to the state sequence [x(1),x(2),K,x(N)] T And the control quantity sequence [u(0),u(1),K,u(N-1)] T , calculate the initial objective function:
[0112]
[0113] Perform N opt Cycles, N opt is the maximum number of optimizations until the convergence condition is met;
[0114] Backward pass, from k=N to k=0, the operation at each moment is as follows:
[0115] Calculate the Jacobian matrix f of the system model x and f u ;
[0116] Calculate the gradient matrix J of the objective function x and J u ;
[0117] Calculate the Hessian matrix J of the objective function xx , J uu and J ux ;
[0118] Q x =J x +(f x ) T V x
[0119] Q u =J u +(f u ) T V u
[0120] Q xx =J xx +(f x ) T V xx f x
[0121] Q uu =J uu +(f u )T V uu f u
[0122] Calculate the coefficient matrix, Q ux =J ux +(f u ) T V xx f x
[0123] Calculate the gain factor and feedforward
[0124] Update value function V x and V xx .
[0125] Forward pass, from k=1 to k=N, the operation at each moment is as follows:
[0126] At time k, let the old state be The old control quantity is
[0127] According to the state quantity x(k-1), gain coefficient K and feedforward F at the previous moment, calculate the new control quantity α = 0.5 is the line search coefficient;
[0128] According to the new control quantity u new (k), input system model x(k+1)=A d x(k)+B d u(k)+E d z(k), get the new state x new (k+1);
[0129] Until x new (N)Calculation completed.
[0130] According to the new state sequence [x new (1),x new (2),K,x new (N)] T and the control quantity sequence [u new (0),u new (1),K,u new (N-1)] T , update the objective function J(x,u);
[0131] To judge the optimization convergence conditions, the following two conditions need to be met at the same time:
[0132] Absolute error threshold judgment: If J(k) is less than the absolute error threshold (the absolute error threshold is related to the size of the weight matrix), the absolute error converges;
[0133] Relative error threshold judgment: Calculate the interpolation value J of the objective function between two adjacent times diff =J(k)-J(k-1), if J diff If it is smaller than the relative error threshold (usually 1e-3), the relative error converges.
[0134] If the optimization result meets the convergence condition, then take the state sequence [x(0), x(1), K, x(N)] T As the optimal result output. That is: take the state sequence [x(0), x(1), K, x(N)] T , converted to the driving trajectory [y, θ, κ], where y is the lateral position, θ is the heading angle, and κ is the curvature. Finally, the driving trajectory is output to the control module for use.
[0135] That is to say, after obtaining the driving trajectory of the vehicle, the method further includes: controlling the vehicle to travel according to the driving trajectory.
[0136] According to the vehicle driving trajectory planning method of the embodiment of the present application, firstly, based on the lateral motion information of the vehicle in the vehicle coordinate system and the fitted lane line, a discrete-time vehicle kinematic model is obtained, then based on the vehicle kinematic model, an objective function is defined, and finally an iterative linear quadratic regulator is used to optimize and solve the objective function, thereby obtaining the vehicle's driving trajectory. Model construction and problem solving directly based on the vehicle coordinate system can effectively improve the model accuracy of trajectory planning. In addition, the use of an iterative linear quadratic regulator to solve the optimization problem does not rely on an external solver, and the solution process is controllable, which can effectively improve the accuracy of trajectory planning and the stability of the solution.
[0137] Figure 2 FIG. 1 is a structural block diagram of a vehicle driving trajectory planning system according to an embodiment of the present application. Figure 2 As shown, a vehicle driving trajectory planning system according to an embodiment of the present application includes: a fitting module 210, a model building module 220 and a planning module 230, wherein:
[0138] A fitting module 210, configured to fit a lane line according to a vehicle state and the perceived lane line information;
[0139] A model building module 220, configured to obtain a discrete-time vehicle kinematic model based on the lateral motion information of the vehicle in the vehicle coordinate system and the lane line obtained by fitting;
[0140] The planning module 230 is used to define an objective function based on the vehicle kinematic model, and optimize and solve the objective function using an iterative linear quadratic regulator to obtain a driving trajectory of the vehicle.
[0141] According to the vehicle driving trajectory planning system of the embodiment of the present application, the vehicle kinematic model of discrete time is first obtained based on the lateral motion information of the vehicle in the vehicle coordinate system and the lane line obtained by fitting, and then the objective function is defined based on the vehicle kinematic model, and finally the objective function is optimized and solved using an iterative linear quadratic regulator to obtain the vehicle's driving trajectory. Model construction and problem solving directly based on the vehicle coordinate system can effectively improve the model accuracy of trajectory planning. In addition, the use of an iterative linear quadratic regulator to solve the optimization problem does not rely on an external solver, and the solution process is controllable, which can effectively improve the accuracy of trajectory planning and the stability of the solution.
[0142] For the specific limitations of the vehicle's driving trajectory planning system, please refer to the limitations of the vehicle's driving trajectory planning method above, which will not be repeated here. The various modules of the above-mentioned vehicle's driving trajectory planning system can be implemented in whole or in part through software, hardware and a combination thereof. The above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory of the computer device in the form of software, so that the processor can call and execute the operations corresponding to the above modules.
[0143] In one embodiment, a vehicle is provided, comprising: a vehicle driving trajectory planning system according to any one of the above embodiments. The vehicle first obtains a discrete-time vehicle kinematic model based on the vehicle lateral motion information in the vehicle coordinate system and the fitted lane lines, and then defines an objective function based on the vehicle kinematic model, and finally optimizes and solves the objective function using an iterative linear quadratic regulator to obtain the vehicle's driving trajectory. Model construction and problem solving directly based on the vehicle coordinate system can effectively improve the model accuracy of trajectory planning. In addition, the use of an iterative linear quadratic regulator to solve the optimization problem does not rely on an external solver, and the solution process is controllable, which can effectively improve the accuracy of trajectory planning and solution stability.
[0144] In addition, other structures and functions of the vehicle according to the embodiment of the present application are known to ordinary technicians in the field and will not be elaborated here.
[0145] In one embodiment, a computer device is provided. Figure 3 This is a block diagram of the computer device provided in the embodiment of the present application, refer to Figure 3The computer device includes a memory and a processor. The memory stores a computer program. When the processor executes the computer program, the aforementioned vehicle driving trajectory planning method embodiment is implemented. For example, the following is performed: fitting a lane line according to the vehicle state and the perceived lane line information;
[0146] Based on the lateral motion information of the vehicle in the vehicle coordinate system and the lane line obtained by fitting, a discrete-time vehicle kinematic model is obtained;
[0147] Based on the vehicle kinematic model, defining an objective function;
[0148] The objective function is optimized and solved by using an iterative linear quadratic regulator to obtain the driving trajectory of the vehicle.
[0149] The embodiment of the present application further provides a computer-readable storage medium, which stores a computer program, and the processor implements the above-mentioned vehicle driving trajectory planning method embodiment when executing the computer program. For example, the following is performed: fitting a lane line according to the vehicle state and the perceived lane line information;
[0150] Based on the lateral motion information of the vehicle in the vehicle coordinate system and the lane line obtained by fitting, a discrete-time vehicle kinematic model is obtained;
[0151] Based on the vehicle kinematic model, defining an objective function;
[0152] The objective function is optimized and solved by using an iterative linear quadratic regulator to obtain the driving trajectory of the vehicle.
[0153] The present application embodiment provides a computer program product, which includes instructions. When the instructions are executed, the method described in the embodiment of the present application is executed. For example, it can be executed Figure 1 The various steps of the vehicle driving trajectory planning method shown, for example, are performed: fitting a lane line according to the vehicle state and the perceived lane line information;
[0154] Based on the lateral motion information of the vehicle in the vehicle coordinate system and the lane line obtained by fitting, a discrete-time vehicle kinematic model is obtained;
[0155] Based on the vehicle kinematic model, defining an objective function;
[0156] The objective function is optimized and solved by using an iterative linear quadratic regulator to obtain the driving trajectory of the vehicle.
[0157] Those of ordinary skill in the art can understand that all or part of the processes in the methods for implementing the above-mentioned embodiments can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory or optical memory, etc. Volatile memory can include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).
[0158] The technical features of the above embodiments may be arbitrarily combined. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.
[0159] The above embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the patent application. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent application shall be subject to the attached claims.
Claims
1. A vehicle driving trajectory planning method, characterized in that: include: According to the vehicle status and the perceived lane line information, the lane line is fitted; Based on the lateral motion information of the vehicle in the vehicle coordinate system and the lane line obtained by fitting, a discrete-time vehicle kinematic model is obtained; Based on the vehicle kinematic model, defining an objective function; The objective function is optimized and solved by using an iterative linear quadratic regulator to obtain the driving trajectory of the vehicle.
2. The vehicle driving trajectory planning method according to claim 1, characterized in that: The vehicle kinematic model in discrete time is obtained based on the lateral motion information of the vehicle in the vehicle coordinate system and the lane line obtained by fitting, including: Based on the lateral motion information of the vehicle in the vehicle coordinate system, the lateral motion of the vehicle is modeled in continuous time to obtain a lateral motion model; According to the perceived lane line information, the lateral motion reference model of the vehicle is obtained; Obtaining an error model of lateral motion according to the lateral motion model and the lateral motion reference model; Obtaining an initial state error of the error model according to the kinematic relationship and the lane line; Based on the initial state error of the error model, a standard state space model is obtained; The standard state space model is discretized to obtain a discrete-time vehicle kinematic model.
3. The vehicle driving trajectory planning method according to claim 1, characterized in that: The objective function is optimized and solved by using an iterative linear quadratic regulator to obtain a driving trajectory of the vehicle, including: Using an obstacle function to replace the inequality constraint in the objective function; Define the gradient matrix of the objective function; Define the Hessian matrix of the objective function; Using abstract functions to express the vehicle kinematic model; defining the Jacobian matrix of the vehicle kinematic model; Define the optimal state value function; Define the first and second partial derivatives of the optimal state value function; The objective function is solved by using an iterative linear quadratic regulator to obtain the driving trajectory.
4. The vehicle driving trajectory planning method according to claim 3, characterized in that: The method of solving the objective function by using an iterative linear quadratic regulator to obtain the driving trajectory includes: Calculating a state quantity sequence according to the initial state quantity, the initial control quantity sequence and the vehicle kinematic model; Calculating an initial objective function according to the state quantity sequence and the control quantity sequence; The following steps are repeated until the convergence condition is met: Perform reverse recursion and forward recursion; Update the objective function according to the new state quantity sequence and control quantity sequence.
5. The vehicle driving trajectory planning method according to claim 4, characterized in that: Also includes: Determine whether the absolute error converges and whether the relative error converges; If the absolute error converges and the relative error converges, it is determined that the convergence condition is satisfied.
6. The vehicle driving trajectory planning method according to any one of claims 1 to 5, characterized in that: After obtaining the vehicle's driving trajectory, it also includes: The vehicle is controlled to travel according to the travel trajectory.
7. A vehicle driving trajectory planning system, characterized in that: include: A fitting module is used to fit the lane line according to the vehicle state and the perceived lane line information; A model building module, used to obtain a discrete-time vehicle kinematic model based on the vehicle lateral motion information in the vehicle coordinate system and the lane line obtained by fitting; The planning module is used to define an objective function based on the vehicle kinematic model, and optimize and solve the objective function using an iterative linear quadratic regulator to obtain a driving trajectory of the vehicle.
8. A vehicle, characterized in that: include: The vehicle driving trajectory planning system according to claim 7.
9. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the vehicle driving trajectory planning method according to any one of claims 1-6 is implemented.
10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the vehicle driving trajectory planning method according to any one of claims 1-6 is implemented.