A method for planning and controlling large-curvature curves for autonomous vehicles

Abstract

The present application provides a method for planning and controlling a large-curvature curve for an autonomous driving vehicle, comprising the following steps: obtaining path information in a current scene, collecting a reference path, and planning a target speed; obtaining vehicle status information; obtaining a reference path point in the reference path based on the position of the vehicle's preview point, calculating a lateral position error and a heading angle error from the horizontal and vertical coordinates and heading angle of the reference path point and the horizontal and vertical coordinates and heading angle of the preview point, and inputting the calculated lateral position error and heading angle error into a lateral controller; the lateral controller is established based on a variable parameter state feedback control algorithm using a table lookup method; the difference between the reference speed generated by a tracking differentiator for the target speed and the longitudinal speed of the vehicle in the inertial coordinate system XOY is input into the longitudinal controller; the longitudinal controller is established based on a terminalless model predictive control algorithm; TD is added to the model predictive control algorithm to arrange a transition process for a specified speed signal, so that the longitudinal speed tracking is more stable and smooth.

Description

Technical field:

[0001] The present invention belongs to the field of autonomous driving vehicle control, and specifically relates to a method for planning and controlling a large-curvature curve for an autonomous driving vehicle. Background technology:

[0002] Autonomous driving, as a cutting-edge technology, promises to reduce traffic accidents and free drivers' hands. However, tracking steep curves presents a significant challenge. When navigating steep curves at high speeds, passengers experience discomfort due to the outward lateral forces exerted by the seats. Furthermore, excessive speed or sharp turns can lead to safety issues such as skidding and rollover. To reduce accidents on curves and improve passenger comfort, many highway curves are designed with superelevation. However, even with these features, drivers struggle to accurately estimate the optimal speed for the turn and maintain smooth lateral and longitudinal control. This results in poor tracking accuracy, compromises passenger comfort, and can even lead to accidents such as skidding and rollover. Therefore, research on speed planning and lateral and longitudinal control of autonomous vehicles on steep curves with superelevation is crucial.

[0003] Model Predictive Control (MPC) is a powerful algorithm for longitudinal control. It addresses hard constraints and predicts future states by solving optimization problems within a finite time domain. Due to the computational complexity required to solve these optimization problems, MPC was initially used in the power generation and oil industries, which have long sampling periods. With the increasing computing power of onboard computers, MPC has also gained application in autonomous vehicle control. However, the endpoint set of standard MPC algorithms is sometimes difficult to solve, and conservative endpoint sets significantly reduce the feasible range of MPC algorithms. Existing research has applied MPC algorithms to the longitudinal velocity tracking problem of autonomous vehicles. However, the endpoint set of standard MPC algorithms is sometimes difficult to solve, and conservative endpoint sets significantly reduce the feasible range of MPC algorithms. Furthermore, using a large step signal as the target velocity can result in large overshoot and a lack of smoothness in velocity tracking.

[0004] For the lateral control of an autonomous vehicle, when using traditional PID control and state feedback control, due to the nonlinearity of the model, when the longitudinal speed changes, the same control parameters may produce very different control effects and may also result in large steady-state errors.

[0005] When it comes to speed planning, traditional tracking algorithms often only focus on tracking accuracy and ignore passenger comfort. However, when the vehicle's longitudinal speed is adjusted to the point where passengers are not subjected to lateral forces from their seats parallel to the slope, there is no relative displacement between the passengers and the vehicle itself, which is the optimal state of comfort. Summary of the invention:

[0006] The purpose of the present invention is to propose a method for planning and controlling large-curvature curves for an autonomous driving vehicle. By using a variable parameter state feedback control based on a lookup table method, the problem that when using traditional PID control and state feedback control, the same control parameters may produce very different control effects and may have large steady-state errors due to the nonlinearity of the model when the longitudinal speed changes is overcome.

[0007] The technical solution of the present invention is: a method for planning and controlling a large-curvature curve for an autonomous driving vehicle, comprising the following steps:

[0008] S1. Obtain path information in the current scene, collect a reference path, and plan a target speed; the path information includes the coordinate information of each point on the path in the X-axis and Y-axis directions in the inertial coordinate system XOY, the curvature information of the path, the road friction coefficient, and the superelevation slope angle of the curve;

[0009] S2. Acquire vehicle status information; the status information includes the X coordinate, Y coordinate, heading angle, speed, and heading angle change rate of the vehicle's preview point in the inertial coordinate system XOY;

[0010] S3. Acquire a reference path point in the reference path based on the position of the vehicle preview point, calculate the lateral position error and the heading angle error based on the horizontal and vertical coordinates and heading angle of the reference path point and the horizontal and vertical coordinates and heading angle of the preview point, and input them into a lateral controller; the lateral controller is established based on a variable parameter state feedback control algorithm using a table lookup method;

[0011] The difference between the reference speed generated by the tracking differentiator of the target speed and the longitudinal speed of the vehicle in the inertial coordinate system XOY is input into the longitudinal controller; the longitudinal controller is established based on the terminalless model predictive control algorithm;

[0012] S4. The lateral controller and the longitudinal controller determine control outputs based on the received information, wherein the control output of the lateral controller is the front wheel deflection angle of the vehicle, and the control output of the longitudinal controller is the desired acceleration of the vehicle; the control outputs of the lateral controller and the longitudinal controller are simultaneously given to the vehicle;

[0013] S5. The vehicle performs corresponding operations according to the control signals output by the lateral controller and the longitudinal controller. Preferably, the lateral controller is established based on a variable parameter state feedback control algorithm using a table lookup method, comprising the steps of:

[0014] Sa-1, establish a vehicle lateral path tracking model;

[0015] Sa-2, discretize the vehicle lateral path tracking model;

[0016] Sa-3. Design a variable parameter state feedback algorithm based on table lookup method.

[0017] Preferably, the lateral controller is established based on a variable parameter state feedback control algorithm using a table lookup method, specifically comprising the steps of:

[0018] Sa-1. Establishing a vehicle lateral path tracking model:

[0019] Lateral position error e y (t) and heading angle error Defined as:

[0020]

[0021]

[0022] Among them, X r (t) is the X coordinate of the reference path point in the inertial coordinate system XOY, Y r (t) is the Y coordinate of the reference path point in the inertial coordinate system XOY, is the desired heading angle determined by the reference path point, X(t) is the X coordinate of the vehicle preview point in the inertial coordinate system XOY, and Y(t) is the Y coordinate of the vehicle preview point in the inertial coordinate system XOY. The heading angle of the vehicle preview point in the inertial coordinate system XOY;

[0023] The counterclockwise direction is defined as the positive direction. Given the curvature information ρ(t) of the reference path point, the first-order derivative of the heading angle with respect to time is expected to be satisfy:

[0024]

[0025] Among them, v x is the speed of the vehicle along the x-axis in the vehicle body coordinate system xoy;

[0026] First and second derivatives of heading angle error and lateral position error with respect to time There are the following relations:

[0027]

[0028]

[0029]

[0030]

[0031] in, and is the first and second derivative of the vehicle heading angle with respect to time, v y (t) is the speed of the vehicle along the y-axis in the vehicle body coordinate system xoy, T d It is the preview time;

[0032] Perform force analysis on the vehicle's center of mass along the y-axis:

[0033]

[0034] Among them, F yf (t) is the force on the front wheel of the vehicle along the y-axis in the vehicle body coordinate system xoy; F yr (t) is the force on the rear wheel of the vehicle along the y-axis in the vehicle body coordinate system xoy; m is the mass of the vehicle;

[0035] When the sideslip angle and front wheel slip angle are small:

[0036] F yf (t) = 2C cf α f (t)

[0037] F yr (t) = 2C cr α r (t)

[0038]

[0039]

[0040] Among them, C cf is the cornering stiffness of the front wheel; C cr is the cornering stiffness of the rear wheel; α f (t) is the sideslip angle of the front wheel; α r (t) is the sideslip angle of the rear wheel; l f is the distance from the center of the front wheel axle to the center of mass of the vehicle, l r is the distance from the rear axle center to the vehicle's center of mass; δ f (t) is the front wheel deflection angle;

[0041] Perform force analysis on the vehicle's center of mass along the z-axis:

[0042]

[0043] in, is the second-order derivative of the heading angle with respect to time, I zis the moment of inertia, which is obtained from the above formula:

[0044]

[0045]

[0046] Sa-2, discretize the vehicle lateral path tracking model;

[0047]

[0048] Among them, the state quantity Control input u y (t) = δ f (t), A yl (v x ), B yl (v x ),d yl (t) are:

[0049]

[0050]

[0051] Where η1=2(C cf +C cr ),η2=-2(l f C cf -l r C cr ), The model is divided into two groups with a sampling period of T. s Discretization:

[0052] ξ y (k+1)=A y (v x )ξ y (k)+B y (v x )u y (k)+d y (k)

[0053] Among them, A y (v x )=I+T s A yl (v x ),B y (v x )=T s B yl (v x ),d y (k) = T s d yl (k), I is the identity matrix;

[0054] Sa-3. Design a variable parameter state feedback algorithm based on table lookup method;

[0055]

[0056] Among them, K b,i With K d,i is the state feedback matrix, i=1,2,3,...,N v ,N v is the total number of speed intervals is the endpoint speed of each speed interval; K b,i With K d,i Satisfies a set of linear matrix inequalities obtained by the following method:

[0057] Define the state-dependent Lyapunov function:

[0058]

[0059] The Lyapunov function increment is expressed as:

[0060]

[0061] in,

[0062]

[0063]

[0064] κ b,i (v x )=A y (v x )+B y (v x )K b,i

[0065] I4 is the fourth-order identity matrix;

[0066] To make the closed loop system stable, P i -1 、 and should be a positive definite matrix, should be a negative definite matrix;

[0067] According to Schur's complement lemma, K b,i With K d,i The following linear matrix inequalities must be satisfied:

[0068]

[0069] Where, “>” indicates that the matrix is ​​positive definite, Wb,i =K b,i P i , W d,i =K d,i P i ; Obviously, this is an infinite number of linear matrix inequalities, which are transformed into a finite number of solvable linear matrix inequalities below; in each speed range, A y (v x ) is the h-th boundary matrix in the ith interval, For B y (v x ) The lth boundary matrix in the ith interval, the boundary matrix is ​​all the v x All matrices obtained by taking the maximum value of the changing matrix elements and combining them can be obtained from this linear matrix inequality:

[0070]

[0071] When the linear matrix inequality is satisfied,

[0072]

[0073] is also satisfied, where N h With N l A y (v x ) and B y (v x ) The number of boundary matrices; K that satisfy this linear matrix inequality b,i With K d,i Make the closed-loop system stable in the i-th speed range; K b,i With K d,i Obtained through experimental tuning; when it is detected that the longitudinal speed of the autonomous vehicle is in the i-th speed range, the i-th group of state feedback matrix is ​​obtained by looking up the table, that is, K b,i With K d,i .

[0074] Preferably, the longitudinal controller is established based on a terminalless model predictive control algorithm, comprising the steps of:

[0075] Sb-1. Establish a vehicle longitudinal velocity tracking model;

[0076] Sb-2, discretize the vehicle longitudinal velocity tracking model;

[0077] Sb-3. Design state constraints, input constraints, and input increment constraints for the longitudinal controller;

[0078] Sb-4. Design a terminalless model predictive control algorithm.

[0079] Preferably, the longitudinal controller is established based on a terminalless model predictive control algorithm, specifically comprising the steps of:

[0080] Sb-1. Establish a vehicle longitudinal velocity tracking model;

[0081] The entire longitudinal model is divided into two layers. The upper-layer system calculates the desired longitudinal acceleration through the designed control law and sends it to the lower-layer system. The lower-layer system drives the accelerator and brake to accelerate or decelerate based on the desired acceleration obtained by the upper-layer system. The relationship between the actual acceleration of the vehicle and the desired acceleration is as follows:

[0082]

[0083] Among them, a x (t) is the actual longitudinal acceleration of the vehicle, is the first-order derivative of the vehicle's actual longitudinal acceleration with respect to time, a xd (t) is the expected longitudinal acceleration of the vehicle, K l and τ are two parameters obtained through experimental tuning to describe the characteristics of the underlying system;

[0084] The longitudinal acceleration of the vehicle a x (t) and longitudinal velocity v x (t) is:

[0085]

[0086] in, is the first-order derivative of the longitudinal velocity with respect to time. The state space method representation of the vehicle longitudinal velocity tracking model is obtained from the above two formulas:

[0087]

[0088] Among them, ξ x (t)=[v x (t),a x (t)] T is the state quantity, u x (t) = a xd (t) is the control quantity, y(t) is the output quantity; A xl 、B xl and C are:

[0089] C=[1 0]

[0090] Sb-2, discretize the vehicle longitudinal velocity tracking model;

[0091]

[0092] in,

[0093] C=[1 0]

[0094] Error vector ξ xe (k)=[v xe (k),a xe (k)] T ;

[0095] v xe (k)=v xr (k)-v x (k),a xe (k) = a xr (k)-a x (k)

[0096] Among them, v xr (k) and a xr (k) is the reference velocity and reference acceleration; define u xe (k)=u xr (k)-u x (k), where u xr (k) is the reference control quantity, and the error system is expressed as:

[0097]

[0098] Among them, y e (k) is the output error;

[0099] Sb-3. Design state constraints, input constraints, and input increment constraints for the longitudinal controller:

[0100] Design input constraints: The input constraints in longitudinal control are: The input increment constraint in longitudinal control is: Where Δu xe (k)=u xe (k)-u xe (k-1) is the input increment;

[0101] Design state constraints: State constraints are expressed as:

[0102]

[0103] Sb-4. Design of terminalless model predictive control algorithm:

[0104] To add the input increment constraint, the error system is rewritten as:

[0105]

[0106] in, Δuxe (k)=u xe (k)-u xe (k-1), C x,u =[1 0 0]; the state constraint is rewritten as Set the prediction step size to N and solve the following MPC longitudinal controller optimization algorithm in each sampling period:

[0107]

[0108] stξ xe,u (k|k)=ξ xe,u (k),

[0109] ξ xe,u (k+i+1|k)=A x,u ξ xe,u (k+i|k)+B x,u Δu xe (k+i|k),

[0110]

[0111]

[0112] Where i = k, k+1, ..., k+N-1, and the cost function is:

[0113]

[0114] Q, R are positive definite weight matrices, N is the prediction step length; the stability coefficient ε = λ max (PQ -1 ), that is, ε is PQ -1 The maximum eigenvalue of ; N must satisfy the following inequality:

[0115]

[0116] To ensure the closed-loop system is asymptotically stable; the optimal control sequence obtained is expressed as The optimal control quantity is During the current sampling period, the vehicle's current state information is obtained through the inertial navigation system, and the optimal control quantity is applied after solving the optimization problem.

[0117] Preferably, the planning of the target speed comprises the steps of:

[0118] If the passenger in the self-driving car is regarded as a point mass, when the passenger moves in a uniform circular motion along the curve with the car, the force formula is as follows:

[0119]

[0120] Among them, F N (k) is the upward support force exerted on the passenger by the seat perpendicular to the slope, F S (k) is the lateral force parallel to the slope surface exerted on the passenger from the seat, β(k) is the superelevation angle of the curve, and ρ(k) is the curvature of the path; m p is the mass of the passenger; in order to maximize the comfort of the passenger in the car, the passenger must not be subjected to the lateral force from the seat, that is, F S (k)=0, then the target speed is:

[0121]

[0122] Preferably, the specific form of the tracking differentiator is:

[0123]

[0124] Where s is the parameter to be adjusted, and s satisfies 0≤s≤1 / T s , v xr (k) and a xr (k) is the reference speed and reference acceleration generated by the tracking differentiator; on this basis, the longitudinal controller calculates the optimal expected acceleration based on the reference speed, reference acceleration and real-time feedback information output by the tracking differentiator.

[0125] Beneficial effects

[0126] Compared with the prior art, the present invention has the following advantages:

[0127] 1. When used by autonomous vehicles on steep curves, traditional path-tracking control algorithms often focus only on tracking accuracy and ignore passenger comfort. Designing longitudinal target speed planning based on road conditions and designing lateral and longitudinal control algorithms for autonomous vehicles on curves can not only enable smooth and rapid cornering but also improve passenger comfort.

[0128] 2. Standard model predictive control algorithms require terminal constraints to ensure stability, but terminal constraints for complex systems are difficult to obtain, and more conservative terminal constraints will reduce the feasible space of the optimization problem to varying degrees. Terminal-free model predictive control algorithms can avoid these problems.

[0129] 3. Reference speed step signals often result in unstable longitudinal control and excessive overshoot, impacting passenger comfort. By incorporating TD into the model predictive control algorithm to schedule a transition for a specified speed signal, a reference speed curve more closely aligned with actual speed variations can be generated and reference acceleration information extracted, resulting in more stable and smooth longitudinal speed tracking.

[0130] 4. The present invention overcomes the problem that when using traditional PID control and state feedback control, the same control parameters may produce very different control effects and may have large steady-state errors due to the nonlinearity of the model when the longitudinal speed changes, by using a variable parameter state feedback control based on a lookup table method. Description of the drawings:

[0131] Figure 1 This is a diagram of the path tracking model of an autonomous driving vehicle according to an embodiment of the present invention.

[0132] Figure 2 This is a structural block diagram of the control algorithm according to an embodiment of the present invention.

[0133] Figure 3 This is a cornering trajectory diagram according to an embodiment of the present invention.

[0134] Figure 4 This is a force analysis diagram of passengers in a car according to an embodiment of the present invention.

[0135] Figure 5 This is a speed tracking curve of an autonomous driving vehicle according to an embodiment of the present invention.

[0136] Figure 6 This is a graph showing the error between the actual trajectory and the reference route of the autonomous driving vehicle according to an embodiment of the present invention.

[0137] Figure 7 This is a lateral position error curve of the autonomous driving vehicle according to an embodiment of the present invention.

[0138] Figure 8 This is a heading angle error curve of the autonomous driving vehicle according to an embodiment of the present invention. Specific implementation method:

[0139] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments.

[0140] The present invention divides the process of an autonomous driving car passing through a high curvature and ultra-high curve under ideal conditions into three stages.

[0141] Phase 1: The autonomous car starts and reaches the optimal speed before entering the curve.

[0142] Phase 2: The autonomous vehicle enters the curve. The autonomous vehicle performs approximately uniform circular motion at the optimal speed and then exits the curve.

[0143] Phase 3: The autonomous car continues driving after exiting the curve and eventually stops at the end.

[0144] The present invention relies on an autonomous driving platform as an experimental platform. The main components of the platform include: a Nuvo industrial computer equipped with a Core i5 processor, an inertial navigation system from Qixun Positioning Company, etc.

[0145] Reference: The reference path is a path collected by the inertial navigation system in the scene, and the reference speed is the set value.

[0146] The present invention provides a method for planning and controlling a large curvature curve for an autonomous driving vehicle. Figure 1-4 , including the following steps:

[0147] S1. Obtain path information in the current scene, collect reference paths, and plan target speeds. Path information includes the X-axis and Y-axis coordinates of each point on the path in the inertial coordinate system XOY, path curvature, road friction coefficient, and superelevation slope angle of the curve.

[0148] S2. Obtain vehicle status information; the status information includes the X coordinate, Y coordinate, heading angle, speed, and heading angle change rate of the vehicle's preview point in the inertial coordinate system XOY;

[0149] S3. Obtain a reference path point in the reference path based on the vehicle preview point position, calculate the lateral position error and heading angle error based on the horizontal and vertical coordinates and heading angle of the reference path point and the horizontal and vertical coordinates and heading angle of the preview point, and input them into the lateral controller; the lateral controller is established based on a variable parameter state feedback control algorithm using a lookup table method;

[0150] The difference between the reference speed generated by the tracking differentiator (TD) and the longitudinal speed of the vehicle in the inertial coordinate system XOY is input into the longitudinal controller; the longitudinal controller is established based on the terminalless model predictive control algorithm;

[0151] S4. The lateral controller and the longitudinal controller determine control outputs based on the received information, wherein the control output of the lateral controller is the front wheel deflection angle of the vehicle, and the control output of the longitudinal controller is the desired acceleration of the vehicle; the control outputs of the lateral controller and the longitudinal controller are simultaneously given to the vehicle;

[0152] S5. The vehicle performs corresponding operations according to the control signals output by the lateral controller and the longitudinal controller.

[0153] Furthermore, the lateral controller is established based on a variable parameter state feedback control algorithm using a table lookup method, including the following steps:

[0154] Sa-1. Establish a vehicle lateral path tracking model;

[0155] Sa-2, discretize the vehicle lateral path tracking model;

[0156] Sa-3. Design a lateral controller using a variable parameter state feedback algorithm based on the table lookup method.

[0157] Furthermore, the lateral controller is established based on a variable parameter state feedback control algorithm using a table lookup method, specifically comprising the following steps:

[0158] Sa-1. Establishing a vehicle lateral path tracking model:

[0159] First, establish the vehicle lateral path tracking model. Because the longitudinal velocity of the vehicle does not change much in this scenario, the longitudinal velocity of the vehicle can be assumed to be constant when designing the lateral controller. The lateral position error e y (t) and heading angle error Defined as:

[0160]

[0161]

[0162] Among them, X r (t) is the X coordinate of the reference path point in the inertial coordinate system XOY, Y r (t) is the Y coordinate of the reference path point in the inertial coordinate system XOY, is the desired heading angle determined by the reference path point, X(t) is the X coordinate of the vehicle preview point in the inertial coordinate system XOY, and Y(t) is the Y coordinate of the vehicle preview point in the inertial coordinate system XOY. The heading angle of the vehicle preview point in the inertial coordinate system XOY;

[0163] The counterclockwise direction is defined as the positive direction. Given the curvature information ρ(t) of the reference path point, the first-order derivative of the heading angle with respect to time is expected to be satisfy:

[0164]

[0165] Among them, v x is the speed of the vehicle along the x-axis in the vehicle body coordinate system xoy;

[0166] First and second derivatives of heading angle error and lateral position error with respect to time There are the following relations:

[0167]

[0168]

[0169]

[0170]

[0171] in, and is the first and second derivative of the vehicle heading angle with respect to time, vy (t) is the speed of the vehicle along the y-axis in the vehicle body coordinate system xoy, T d It is the preview time;

[0172] Perform force analysis on the vehicle's center of mass along the y-axis:

[0173]

[0174] Among them, F yf (t) is the force on the front wheel of the vehicle along the y-axis in the vehicle body coordinate system xoy; F yr (t) is the force on the rear wheel of the vehicle along the y-axis in the vehicle body coordinate system xoy; m is the mass of the vehicle;

[0175] When the sideslip angle and front wheel slip angle are small:

[0176] F yf (t) = 2C cf α f (t)

[0177] F yr (t) = 2C cr α r (t)

[0178]

[0179]

[0180] Among them, C cf is the cornering stiffness of the front wheel; C cr is the cornering stiffness of the rear wheel; α f (t) is the sideslip angle of the front wheel; α r (t) is the sideslip angle of the rear wheel; l f is the distance from the center of the front wheel axle to the center of mass of the vehicle, l r is the distance from the rear axle center to the vehicle mass center, δ f (t) is the front wheel deflection angle.

[0181] Perform force analysis on the vehicle's center of mass along the z-axis:

[0182]

[0183] in, is the second-order derivative of the heading angle with respect to time, I z is the moment of inertia, which is obtained from the above formula (for convenience of writing, "(t)" is omitted):

[0184] From the above formula we can get:

[0185]

[0186]

[0187] Sa-2, discretize the vehicle lateral path tracking model;

[0188]

[0189] The state quantity Control input u y (t) = δ f (t); A yl (v x ), B yl (v x ),d yl (t) are:

[0190]

[0191]

[0192] Where η1=2(C cf +C cr ),η2=-2(l f C cf -l r C cr ), The model is divided into two groups with a sampling period of T. s Discretization:

[0193] ξ y (k+1)=A y (v x )ξ y (k)+B y (v x )u y (k)+d y (k)

[0194] Among them A y (v x )=I+T s A yl (v x ),B y (v x )=T s B yl (v x ),d y (k) = T s d yl (k), I is the identity matrix;

[0195] Sa-3. Design of lateral controller using variable parameter state feedback algorithm based on table lookup method;

[0196]

[0197] Among them, K b,i With K d,i is the state feedback matrix, i=1,2,3,...,N v ,N v is the total number of speed intervals is the endpoint speed of each speed interval; K b,i With K d,i Satisfies a set of linear matrix inequalities obtained by the following method:

[0198] Define the state-dependent Lyapunov function:

[0199]

[0200] The Lyapunov function increment is expressed as:

[0201]

[0202] in,

[0203]

[0204]

[0205] κ b,i (v x )=A y (v x )+B y (v x )K b,i

[0206] I4 is the fourth-order identity matrix;

[0207] To make the closed loop system stable, P i -1 、 and should be a positive definite matrix, Should be a negative definite matrix.

[0208] According to Schur's complement lemma, K b,i With K d,i The following linear matrix inequalities must be satisfied:

[0209]

[0210] Where, “>” indicates that the matrix is ​​positive definite, W b,i =K b,i P i , W d,i =K d,iP i ; Obviously, this is an infinite number of linear matrix inequalities, which are transformed into a finite number of solvable linear matrix inequalities. In each speed range, A y (v x ) is the h-th boundary matrix in the ith interval, For B y (v x ) The lth boundary matrix in the ith interval, the boundary matrix is ​​all the v x The changing matrix elements take the maximum value and combine all the matrices obtained; thus, we can obtain that when the following linear matrix inequality:

[0211]

[0212] When the linear matrix inequality is satisfied,

[0213]

[0214] is also satisfied, where N h With N l A y (v x ) and B y (v x ) The number of boundary matrices; K that satisfy this linear matrix inequality b,i With K d,i The closed-loop system is input to a stable state when it is in the i-th speed interval. When the longitudinal speed of the autonomous vehicle is detected to be in the i-th speed interval, the i-th group of state feedback matrix is ​​obtained by the table lookup method, that is, K b,i With K d,i At this point, the lateral control algorithm design is completed.

[0215] The parameter table of the lookup method is as follows:

[0216] <![CDATA[v x (m / s)]]> (1.5,2] (2,3] (3,4] (4,5] i 1 2 3 4 <![CDATA[K b,i ]]> [0.2,0,1.2,0] [0.2,0.1,1.3,0] [0.2,0.2,1.4,0] [0.2,0.3,1.5,0] <![CDATA[K d,i ]]> [0,0.01,0,0] [0,-0.004,0,0] [0,-0.004,0,0.003] [0,0.008,0,0.001]

[0217] Furthermore, the longitudinal controller is established based on a terminalless model predictive control algorithm, including the steps of:

[0218] Sb-1. Establish a vehicle longitudinal velocity tracking model;

[0219] Sb-2, discretize the vehicle longitudinal velocity tracking model;

[0220] Sb-3. Design state constraints, input constraints, and input increment constraints for the longitudinal controller;

[0221] Sb-4. Design a terminalless model predictive control algorithm.

[0222] Furthermore, the longitudinal controller is established based on the terminalless model predictive control algorithm, which specifically includes the following steps:

[0223] Sb-1. Establish a vehicle longitudinal velocity tracking model;

[0224] A vehicle longitudinal velocity tracking model was established. The entire longitudinal model is divided into two layers. The upper-layer system calculates the desired longitudinal acceleration using the designed control law and transmits it to the lower-layer system. The lower-layer system activates the throttle and brakes to accelerate or decelerate based on the desired acceleration obtained by the upper-layer system. Since the relevant physical parameters of the lower-layer system, such as the motor, are unavailable, parameter tuning is performed based on experiments to obtain an approximate relationship between the actual and desired acceleration of the vehicle:

[0225]

[0226] Among them, a x (t) is the actual longitudinal acceleration of the vehicle, is the first-order derivative of the vehicle's actual longitudinal acceleration with respect to time, a xd (t) is the expected longitudinal acceleration of the vehicle, K l and τ are two parameters obtained through experimental tuning to describe the characteristics of the underlying system;

[0227] The longitudinal acceleration of the vehicle a x (t) and longitudinal velocity v x (t) is:

[0228]

[0229] in, is the first-order derivative of the longitudinal velocity with respect to time; the state space method representation of the vehicle longitudinal velocity tracking model is obtained from the above two formulas:

[0230]

[0231] Among them, ξ x (t)=[v x (t),a x (t)] T is the state quantity, u x (t) = a xd (t) is the control quantity, y(t) is the output quantity; A xl 、B xl and C are:

[0232] C=[1 0]

[0233] Sb-2, discretize the vehicle longitudinal velocity tracking model;

[0234]

[0235] in,

[0236] C=[1 0]

[0237] Error vector ξ xe (k)=[v xe (k),a xe (k)] T ;

[0238] v xe (k)=v xr (k)-v x (k),a xe (k) = a xr (k)-a x (k)

[0239] Among them, v xr (k) and a xr (k) is the reference velocity and reference acceleration; define u xe (k)=u xr (k)-u x (k), where u xr (k) is the reference control variable, which is set to 0 in the present invention. The error system is expressed as:

[0240]

[0241] Among them, y e (k) is the output error.

[0242] Sb-3. Design state constraints, input constraints, and input increment constraints for the longitudinal controller:

[0243] First, design the input constraints. In longitudinal control, the vehicle throttle has physical saturation limitations, and the throttle opening cannot change too much in a short time. Design the input constraints: The input constraints in longitudinal control are: The input increment constraint in longitudinal control is: Where Δu xe (k)=u xe (k)-u xe (k-1) is the input increment;

[0244] Then, design the state constraints. In the scenario of a large curvature curve, in order to prevent the vehicle from skidding at high speed and causing the risk of rollover, it is necessary to impose constraints on the vehicle's longitudinal speed. Design state constraints: The state constraints are expressed as:

[0245]

[0246] Among them, u xmin ,u xmax , Δu xmin , Δu xmax , v xe , These are constants obtained through experimental tuning based on vehicle conditions and experimental scenarios;

[0247] Sb-4. Design of terminalless model predictive control algorithm:

[0248] To add the input increment constraint, the error system is rewritten as:

[0249]

[0250] in, Δu xe (k)=u xe (k)-u xe (k-1), C x,u =[1 0 0]; the state constraint is rewritten as Set the prediction step size to N. In each sampling period, solve the following MPC longitudinal controller optimization algorithm:

[0251]

[0252] stξ xe,u (k|k)=ξ xe,u (k),

[0253] ξ xe,u (k+i+1|k)=A x,u ξ xe,u (k+i|k)+B x,u Δu xe (k+i|k),

[0254]

[0255]

[0256] Where i = k, k+1, ..., k+N-1, and the cost function is:

[0257]

[0258] Q, R are positive definite weight matrices, N is the prediction step length; the stability coefficient ε = λ max (PQ -1 ), that is, ε is PQ -1 The maximum eigenvalue of ; N must satisfy the following inequality:

[0259]

[0260] To ensure the closed-loop system is asymptotically stable. The optimal control sequence obtained is expressed as The optimal control quantity is During the current sampling period, the vehicle's current state information is obtained through the inertial navigation system, and the optimal control quantity is applied after solving the optimization problem.

[0261] Furthermore, the target speed is planned, including the following steps:

[0262] In the second stage, the passengers in the self-driving car are treated as point masses. When they move in a uniform circular motion along the curve, the force formula is as follows:

[0263]

[0264] Among them, F N (k) is the upward support force exerted on the passenger by the seat perpendicular to the slope, F S (k) is the lateral force parallel to the slope surface exerted on the passenger from the seat, β(k) is the superelevation angle of the curve, and ρ(k) is the curvature of the path; m p is the mass of the passenger; in order to maximize the comfort of the passenger in the car, the passenger must not be subjected to the lateral force from the seat, that is, F S (k)=0, then the target speed is:

[0265]

[0266] Furthermore, the specific form of the tracking differentiator (TD) is:

[0267]

[0268] Where s is the parameter to be adjusted, and s satisfies 0≤s≤1 / T s , v xr (k) and a xr (k) is the reference velocity and reference acceleration generated by TD; on this basis, the MPC longitudinal controller can calculate the optimal expected acceleration based on the reference velocity, reference acceleration and real-time feedback information output by TD.

[0269] The experimental site of the present invention is in an automobile testing ground, which is relatively open and has the conditions for safe experiments. After the experiment starts, the reference speed of the autonomous driving vehicle is set to 3.5m / s, and it starts from point O1 and drives along a straight road. When the vehicle reaches point S, the optimal speed is calculated based on the superelevation angle and curvature of the curve and is designed as the reference speed so that the speed reaches the optimal speed and is maintained when the vehicle reaches point S1. After arriving at point S1, the vehicle enters the curve and performs approximately uniform circular motion. When the vehicle reaches point S2, the vehicle exits the curve and resets the reference speed to 3.5m / s. Finally, the vehicle stops at point O2. The target longitudinal speed that changes with time, the reference speed generated by TD and the actual longitudinal speed of the vehicle are as follows Figure 5 As shown, it can be seen that the vehicles eventually reach the target speed. Figure 6 is the actual trajectory and reference path diagram of the autonomous vehicle in the inertial coordinate system; the lateral position error and heading angle error that change with time are as follows: Figure 7 and 8 As shown, they all converge to 0 in the end. The average tracking error is shown in the following table:

[0270] Average lateral position error Average heading angle error Longitudinal velocity average error 0.0732m 0.0739rad 0.1643m / s

Claims

1. A method for planning and controlling a large curvature curve for an autonomous vehicle, characterized in that: The following steps are involved: S1. Obtain path information in the current scene, collect a reference path, and plan a target speed; the path information includes the coordinate information of each point on the path in the X-axis and Y-axis directions in the inertial coordinate system XOY, the curvature information of the path, the road friction coefficient, and the superelevation slope angle of the curve; S2. Acquire vehicle status information; the status information includes the X coordinate, Y coordinate, heading angle, speed, and heading angle change rate of the vehicle's preview point in the inertial coordinate system XOY; S3. Acquire a reference path point in the reference path based on the position of the vehicle preview point, calculate the lateral position error and the heading angle error based on the horizontal and vertical coordinates and heading angle of the reference path point and the horizontal and vertical coordinates and heading angle of the preview point, and input them into a lateral controller; the lateral controller is established based on a variable parameter state feedback control algorithm using a table lookup method; The difference between the reference speed generated by the tracking differentiator of the target speed and the longitudinal speed of the vehicle in the inertial coordinate system XOY is input into the longitudinal controller; the longitudinal controller is established based on the terminalless model predictive control algorithm; S4. The lateral controller and the longitudinal controller determine control outputs based on the received information, wherein the control output of the lateral controller is the front wheel deflection angle of the vehicle, and the control output of the longitudinal controller is the desired acceleration of the vehicle; the control outputs of the lateral controller and the longitudinal controller are simultaneously given to the vehicle; S5. The vehicle performs corresponding operations according to the control signals output by the lateral controller and the longitudinal controller; In S3, the lateral controller is established based on a variable parameter state feedback control algorithm using a table lookup method, specifically comprising the following steps: Sa-1. Establishing a vehicle lateral path tracking model: Lateral position error Heading angle error Defined as: ； in, is the X coordinate of the reference path point in the inertial coordinate system XOY, is the Y coordinate of the reference path point in the inertial coordinate system XOY, is the desired heading angle determined by the reference waypoint, is the X coordinate of the vehicle preview point in the inertial coordinate system XOY, is the Y coordinate of the vehicle preview point in the inertial coordinate system XOY, The heading angle of the vehicle preview point in the inertial coordinate system XOY; Define the counterclockwise direction as the positive direction, given the curvature information of the reference path point , the first-order derivative of the desired heading angle with respect to time satisfy: ； in, is the speed of the vehicle along the x-axis in the vehicle body coordinate system xoy; First and second derivatives of heading angle error and lateral position error with respect to time There are the following relations: ； in, and are the first and second derivatives of the vehicle heading angle with respect to time, is the speed of the vehicle along the y-axis in the body coordinate system xoy, It is the preview time; Perform force analysis on the vehicle's center of mass along the y-axis: ； in, The force on the front wheel of the vehicle along the y-axis in the vehicle body coordinate system xoy; The force on the rear wheel of the vehicle along the y-axis in the vehicle body coordinate system xoy; is the mass of the vehicle; When the sideslip angle and front wheel slip angle are small: ； in, is the cornering stiffness of the front wheel; is the cornering stiffness of the rear wheel; is the sideslip angle of the front wheel; is the sideslip angle of the rear wheel; is the distance from the center of the front wheel axle to the center of mass of the vehicle, is the distance from the center of the rear axle to the center of mass of the vehicle; is the front wheel deflection angle; Perform force analysis on the vehicle's center of mass along the z-axis: ； in, is the second-order derivative of the heading angle with respect to time, is the moment of inertia, which is obtained from the above formula: ； Sa-2. Discretize the vehicle lateral path tracking model: ； Among them, the state quantity , control input , 、 、 They are: ； in, , , ; The model is based on the sampling period Discretization: ； in, , , , is the identity matrix; Sa-3. Design a variable parameter state feedback algorithm based on table lookup method: ； in, and is the state feedback matrix, , is the total number of speed intervals, is the endpoint speed of each speed interval; and Satisfies a set of linear matrix inequalities obtained by the following method: Define the state-dependent Lyapunov function: ； The Lyapunov function increment is expressed as: ； in, ； ； is the fourth-order identity matrix; To make the closed loop system stable, 、 and should be a positive definite matrix, should be a negative definite matrix; According to Schur's complement lemma, and The following linear matrix inequalities must be satisfied: ； in," " indicates that the matrix is ​​positive definite, , , , ; Obviously, this is an infinite number of linear matrix inequalities, which are transformed into a finite number of solvable linear matrix inequalities below; in each speed range, for In the The first interval A boundary matrix, for In the The first interval The boundary matrix is ​​the matrix of all All matrices obtained by taking the maximum value of the changing matrix elements and combining them can be obtained from this linear matrix inequality: ； When , the linear matrix inequality is satisfied: ； Also satisfied, among them, and They are and The number of boundary matrices that satisfy this linear matrix inequality and Make the closed loop system The input is stable within the speed range; and Obtained through experimental tuning; when it is detected that the longitudinal speed of the autonomous vehicle is in the When the speed range is 0, the first The group state feedback matrix is and .

2. The method for planning and controlling a large curvature curve for an autonomous driving vehicle according to claim 1, characterized in that: The longitudinal controller is established based on the terminalless model predictive control algorithm, including the steps of: Sb-1. Establish a vehicle longitudinal velocity tracking model; Sb-2, discretize the vehicle longitudinal velocity tracking model; Sb-3. Design state constraints, input constraints, and input increment constraints for the longitudinal controller; Sb-4. Design a terminalless model predictive control algorithm.

3. The method for planning and controlling a steep curve on an autonomous driving vehicle according to claim 1 or 2, wherein: The longitudinal controller is established based on the terminalless model predictive control algorithm, and specifically includes the following steps: Sb-1. Establish a vehicle longitudinal velocity tracking model; The entire longitudinal model is divided into two layers. The upper-layer system calculates the desired longitudinal acceleration through the designed control law and sends it to the lower-layer system. The lower-layer system drives the accelerator and brake to accelerate or decelerate based on the desired acceleration obtained by the upper-layer system. The relationship between the actual acceleration of the vehicle and the desired acceleration is as follows: ； in, is the actual longitudinal acceleration of the vehicle, is the first-order derivative of the vehicle's actual longitudinal acceleration with respect to time, is the desired longitudinal acceleration of the vehicle, and are two parameters obtained through experimental tuning to describe the characteristics of the underlying system; Longitudinal acceleration of the vehicle and longitudinal speed The relationship is: ； in, is the first-order derivative of the longitudinal velocity with respect to time. The state space method representation of the vehicle longitudinal velocity tracking model is obtained from the above two formulas: ； in, is the state quantity, To control the amount, is the output; 、 and They are: ； Sb-2, discretize the vehicle longitudinal velocity tracking model; ； in, ; Error vector ; ； in, and are the reference velocity and reference acceleration; define ,in, is the reference control quantity, and the error system is expressed as: ； in, is the output error; Sb-3. Design state constraints, input constraints, and input increment constraints for the longitudinal controller: Design input constraints: The input constraints in the longitudinal control are: , the input increment constraint in longitudinal control is: ,in, is the input increment; Design state constraints: State constraints are expressed as: ； Sb-4. Design of terminalless model predictive control algorithm: To add the input increment constraint, the error system is rewritten as: ； in, , , , ; The state constraint is rewritten as ; Set the prediction step size to ; In each sampling period, solve the following MPC longitudinal controller optimization algorithm: ； in, , the cost function is: ； is a positive definite weight matrix, is the prediction step length; stability coefficient ,Right now for The maximum eigenvalue of To satisfy the following inequality: ； To ensure the closed-loop system is asymptotically stable; the optimal control sequence obtained is expressed as The optimal control quantity is ; During the current sampling period, the vehicle's current state information is obtained through the inertial navigation system, and the optimal control quantity is applied after solving the optimization problem.

4. The method for planning and controlling a steep curve on an autonomous driving vehicle according to claim 1, wherein: The target speed planning includes the following steps: If the passenger in the self-driving car is regarded as a point mass, when the passenger moves in a uniform circular motion along the curve with the car, the force formula is as follows: ； in, The upward support force perpendicular to the slope surface exerted on the passenger by the seat. is the lateral force on the passenger from the seat parallel to the slope, is the super-high angle of the curve, is the curvature of the path; is the mass of the passenger; in order to maximize the comfort of the passenger in the car, the passenger must not be subjected to lateral forces from the seat, that is, , the target speed is: 。 5. The method for planning and controlling a large curvature curve for an autonomous driving vehicle according to claim 4, characterized in that: The specific form of the tracking differentiator is: ； in, is the parameter to be tuned, and satisfy , and It is the reference speed and reference acceleration generated by the tracking differentiator; on this basis, the longitudinal controller calculates the optimal expected acceleration according to the reference speed, reference acceleration and real-time feedback information output by the tracking differentiator.

Images