Trajectory tracking control method and device based on neural network and feedback MPC and storage medium
Through the trajectory tracking control method based on neural network and feedback MPC, the accuracy problem of vehicle dynamics model at high speed is solved, and accurate trajectory tracking and safe handling under different operating conditions are achieved.
Patent Information
- Application Number
- CN202410988314.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-23
- Publication Date
- 2024-11-05
AI Technical Summary
The existing vehicle dynamics model has drastically deteriorated under high speed or large lateral acceleration, making it difficult to accurately describe the dynamic behavior of the vehicle, resulting in a decrease in trajectory tracking accuracy, and high model complexity affects the control response speed.
Using a trajectory tracking control method based on neural network and feedback MPC, the neural network model and feedforward controller are constructed to track road curvature, and combined with the MPC feedback controller to eliminate pre-sight errors, and use road information to optimize steering wheel operation to realize the vehicle in different ways. Accurate tracking of trajectory under operating conditions.
The tracking accuracy and response speed are improved, the prediction error caused by changes in vehicle parameters is reduced, and the adaptability to different working conditions is enhanced, ensuring the safety and handling stability of the vehicle driving along the reference trajectory.
Smart Images

Figure CN118897470A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of trajectory tracking control of driving robots, and in particular to a trajectory tracking control method, device and storage medium based on neural network and feedback MPC. Background Art
[0002] At present, in order to evaluate the performance and improve the design of vehicles, professional testers are required to drive vehicles for a large number of repeated performance tests. Human testers are prone to driving fatigue and other phenomena during the test, which leads to low accuracy of test results and even dangerous situations. Driving robots provide a solution to overcome the shortcomings of human drivers and strengthen the execution of vehicle driving tasks. By installing the driving robot in the cab and connecting the robot's end actuator with the vehicle's operating mechanism, the vehicle can be driven automatically. In addition, the actuators of the driving robot have strong adjustment performance, and can drive a variety of models after self-learning of vehicle performance. Due to the accuracy and reliability of the robot control system and actuators, the driving robot has the advantages of high efficiency, good repeatability, accurate and reliable testing, etc., which can improve the efficiency of product design and development, shorten the test time, improve the accuracy of the test, and ensure the life safety of test personnel. It is of great significance to the unmanned field of vehicle performance testing.
[0003] Model predictive control is a type of model-based trajectory tracking control algorithm that can make full use of the information provided by the reference trajectory and consider the constraints at the current moment and within the prediction time. It has become the mainstream solution for lateral control of autonomous driving systems currently deployed. The design of the model predictive control algorithm relies on the system model of the controlled object, so establishing an accurate vehicle dynamics model is the basis of trajectory tracking control.
[0004] Among the vehicle dynamics modeling methods, the most commonly used is the two-degree-of-freedom vehicle dynamics model, which is simple and practical under medium and low speeds and small curvature conditions. However, under high speed or large lateral acceleration, due to the complex internal mechanisms of the vehicle system and related subsystems, with highly time-varying nonlinear and strong coupling characteristics, its model accuracy will deteriorate sharply, resulting in obvious model mismatch. In addition, although the nonlinear seven-degree-of-freedom vehicle dynamics model can better describe the vehicle dynamics characteristics and provide more accurate tracking effects, it is highly complex and involves many parameters, which affects the control response speed. In addition, in the face of extreme road conditions such as high-speed driving, violent steering, and low road adhesion coefficient, these models are not enough to accurately describe the dynamic behavior of the vehicle, resulting in a decrease in tracking accuracy. In order to improve the tracking accuracy, it is necessary to solve the problem of model deviation between the vehicle model and the real vehicle and the poor adaptability to road conditions, so as to achieve the vehicle to accurately track the established reference trajectory during the test. Summary of the invention
[0005] The present invention provides a trajectory tracking control method, device and storage medium based on neural network and feedback MPC to solve the technical problems existing in the known technology.
[0006] The technical solution adopted by the present invention to solve the technical problems existing in the known technology is:
[0007] A trajectory tracking control method based on neural network and feedback MPC includes the following steps:
[0008] Step 1: construct a neural network model, so that the input of the neural network model includes the lateral speed, yaw rate, longitudinal speed and front wheel turning angle of the vehicle at the current moment; the output of the neural network model includes the lateral speed and yaw rate of the vehicle at the next moment; collect and compile vehicle driving trajectory samples under various working conditions, and train the neural network model;
[0009] Step 2: construct a feedforward controller to control the vehicle to follow the road curvature; the input of the feedforward controller includes the longitudinal velocity of the vehicle at time k and the steady-state yaw angular velocity of the vehicle at time k, and the output includes the steady-state lateral velocity of the vehicle at time k and the feedforward control quantity at time k; in the feedforward controller, a trained neural network model is used to establish a nonlinear optimization problem, and the feedforward control quantity at time k is obtained according to the interior point method;
[0010] Step 3: Establish a vehicle yaw dynamics model based on path tracking, and establish a system state space equation based on the vehicle yaw dynamics model; linearize and discretize the system state space equation of the vehicle;
[0011] Step 4: construct an MPC feedback controller to eliminate preview errors; the MPC feedback controller, whose input includes the lateral velocity of the vehicle at time k, the yaw velocity of the vehicle at time k, the lateral error of the vehicle at time k, the yaw error of the vehicle at time k and the feedback control amount at time k-1, and whose output includes the feedback control amount at time k; in the MPC feedback controller, a prediction model is constructed based on the linearized and discretized system state space equation, and the prediction model predicts the vehicle system state at multiple times, and an objective function is constructed based on minimizing the preview error, and constraints are set, and the objective function and constraints are converted into a quadratic programming problem, and the quadratic programming problem is solved using an optimization tool to obtain the feedback control amount at time k;
[0012] Step 5: Add the feedforward control quantity and the feedback control quantity at time k to obtain the expected front wheel angle at time k, convert the expected front wheel angle at time k into the torque required for the steering motor of the driving robot, and the driving robot controls the steering wheel to complete trajectory tracking.
[0013] Furthermore, in step one, the method for collecting and compiling vehicle driving trajectory samples under various working conditions includes the following method steps: collecting actual trajectory sample data of the vehicle under high adhesion coefficient and low adhesion coefficient conditions respectively, and when collecting trajectory sample data, making the vehicle drive under various working conditions; setting the vehicle to drive on 50 to 100 random paths, the sampling time is 0.05s, and collecting 150,000 to 300,000 sets of vehicle status data.
[0014] Furthermore, in step 1, the neural network model adopts the BP neural network model: the number of hidden layers of the BP neural network model is H, and the number of neurons in each layer is [n1, n2, ..., n H ]; The neuron activation function uses the tansig activation function.
[0015] Furthermore, in step 1, when training the neural network model, the learning rate is set to 0.001, the training process uses the Levenberg-Marquardt optimization algorithm, the loss function selects MSE, and the maximum training error allowed is ε = 10 -6 .
[0016] Furthermore, step 2 includes the following method steps:
[0017] Assume: v x (k) is the longitudinal velocity of the vehicle at time k; v ys (k) is the steady-state lateral velocity of the vehicle at time k; w s (k) is the steady-state yaw rate of the vehicle at time k; v ys (k+1) is the steady-state lateral velocity of the vehicle at time k+1; w s (k+1) is the steady-state yaw rate of the vehicle at time k+1; u ff (k) is the feedforward control quantity at time k;
[0018] The trained neural network model is as follows:
[0019]
[0020] Where: x k =[v ys (k) w s (k) v x (k)] T ;
[0021] f NN (x k ,u ff (k)) represents the output of the neural network model at time k;
[0022] When the longitudinal velocity of the vehicle is constant, the lateral acceleration = 0, and the yaw angular acceleration = 0, the vehicle is in a steady state; let κ p is the curvature of the projection point on the reference trajectory; the steady-state yaw rate of the vehicle is calculated as follows:
[0023] w s (k) = v x (k)*κ p (2)
[0024] During the control process, measure v x (k), and calculate κ using an online map matching algorithm on the vehicle p ; In order to solve u ff (k) and v ys (k), based on the physical constraints of lateral velocity and front wheel steering angle, the nonlinear optimization problem is defined as follows:
[0025]
[0026] In the above formula:
[0027] v ys,min is the minimum value of the steady-state lateral velocity constraint;
[0028] v ys,max is the maximum value of the steady-state lateral velocity constraint;
[0029] δ min is the minimum value of the front wheel steering angle constraint;
[0030] δ max is the maximum value of the front wheel steering angle constraint;
[0031] The interior point method is used to solve the above nonlinear optimization problem and the feedforward control quantity u at time k is obtained. ff (k).
[0032] Furthermore, step three includes the following method steps:
[0033] set up is the yaw angle of the vehicle, which is the angle between the x-axis of the vehicle coordinate system and the x-axis of the earth coordinate system;
[0034] The vehicle yaw dynamics model is established using the two-degree-of-freedom vehicle dynamics equation, assuming that the front wheel turning angle is a small angle; the two-degree-of-freedom vehicle dynamics equation is as follows:
[0035]
[0036] Where:
[0037] v x is the longitudinal velocity of the vehicle;
[0038] v y is the lateral velocity of the vehicle;
[0039] is the lateral acceleration of the vehicle;
[0040] is the yaw rate of the vehicle;
[0041] is the vehicle’s yaw angular acceleration;
[0042] a is the distance from the vehicle's center of mass to the front axle;
[0043] b is the distance from the vehicle's center of mass to the rear axle;
[0044] C f is the cornering stiffness of the front tire;
[0045] C r is the cornering stiffness of the rear tire;
[0046] m is the mass of the car;
[0047] δ fb is the front wheel turning angle;
[0048] I z is the moment of inertia of the vehicle around the z-axis;
[0049] set up: is the error between the actual yaw angle of the vehicle and the desired yaw angle of the vehicle; is the error rate between the actual yaw angle of the vehicle and the desired yaw angle of the vehicle; e d is the lateral error between the vehicle center of mass and the projection point; is the lateral error change rate between the vehicle center of mass and the projection point; κ p is the curvature of the projection point on the reference trajectory; u fb is the feedback control quantity;
[0050] According to the kinematic relationship, and The calculation formula is as follows:
[0051]
[0052] Combining equations (4) and (6), the following vehicle yaw dynamics model based on path tracking is established:
[0053]
[0054] Where W is the vehicle state, u fb =δ fb, matrices A, B1, and B2 are expressed as:
[0055]
[0056] Let η be the output of the vehicle yaw dynamics model, and establish the following system state space equation based on the vehicle yaw dynamics model:
[0057]
[0058] in
[0059] Where:
[0060] A is the system matrix of the system state space equation;
[0061] B1 is the input matrix of the system state space equation;
[0062] B2 is an additional input matrix for the system state space equations.
[0063] The system state space equation is discretized and linearized by Taylor level expansion and forward Euler method, and the state quantity of the discrete system state space equation is adjusted as follows:
[0064]
[0065] The new discrete system state space equation is obtained as follows:
[0066]
[0067] in,
[0068] Where:
[0069] W(k) is the state quantity of the discrete system state space equation at time k before adjustment;
[0070] u fb (k-1) is the feedback control quantity at time k-1;
[0071] Δu fb (k) is the increment of feedback control quantity at time k;
[0072] κ p (k) is the curvature of the projection point on the reference trajectory corresponding to time k;
[0073] η(k) is the output of the vehicle yaw dynamics model at time k;
[0074] ξ(k) is the state quantity of the state space equation of the discrete system at time k after adjustment;
[0075] ξ(k+1) is the state quantity of the state space equation of the discrete system at time k+1 after adjustment;
[0076] is the system matrix of the state space equation of the adjusted discrete system at time k;
[0077] is the input matrix of the state space equation of the adjusted discrete system at time k;
[0078] is the additional input matrix of the state space equation of the adjusted discrete system at time k;
[0079] Output matrix for the adjusted discrete system state space equations.
[0080] Furthermore, step 4 includes the following method steps:
[0081] Assume that the number of prediction steps in the MPC feedback controller is N p , the control time domain steps of the MPC feedback controller are N c , where N p >N c , construct the following prediction model to predict the output at multiple moments as follows:
[0082]
[0083] Where:
[0084] η(k+1) is the predicted output of the vehicle yaw dynamics model at time k+1;
[0085] η(k+2) is the predicted output of the vehicle yaw dynamics model at time k+2;
[0086] η(k+N p ) is the vehicle yaw dynamics model k+N p The predicted output at the moment;
[0087] Δu fb (k+1) is the increment of feedback control quantity at time k+1;
[0088] Δu fb (k+N c -1) is k+N c -1 The increment of feedback control amount at time;
[0089] κ p (k+1) is the curvature of the projection point on the reference trajectory corresponding to time k+1;
[0090] κ p (k+N p -1) is k+Np -1 The curvature of the projection point on the reference trajectory corresponding to the moment;
[0091] Let L d is the projection distance in front of the vehicle; The projection distance in front of the vehicle is L d The vehicle tracking error is is the error between the actual yaw angle of the vehicle and the desired yaw angle of the vehicle; let e d is the lateral error between the vehicle center of mass and the projection point;
[0092] The control objective of the MPC feedback controller is to Minimum, The calculation formula is as follows:
[0093]
[0094] In the formula, is the preview distance weight matrix;
[0095] The objective function of the MPC feedback controller is set as follows:
[0096]
[0097] Where:
[0098] J is the objective function;
[0099] Q is the weight matrix of the error term;
[0100] R is the incremental weight matrix of feedback control quantity;
[0101] ρ is the relaxation factor weight;
[0102] ε is the relaxation factor;
[0103] η(k+i,k) is the predicted output of the vehicle yaw dynamics model at time k+i;
[0104] Δu fb (k+i,k) is the increment of feedback control quantity at time k+i;
[0105] Based on the physical limit constraints of the state and control quantities of the actual vehicle system, the following constraints are set for the control quantities and system output quantities:
[0106]
[0107] Where:
[0108] Δu min (k+i,k) is the minimum value of the feedback control increment constraint at time k+i;
[0109] Δu max (k+i,k) is the maximum value of the feedback control increment constraint at time k+i;
[0110] y fb (k+i,k) is the feedback control quantity at time k+i;
[0111] u min (k+i,k) is the minimum value of the feedback control constraint at time k+i;
[0112] u max (k+i,k) is the maximum value of the feedback control constraint at time k+i;
[0113] η(k+i,k) is the predicted output of the vehicle yaw dynamics model at time k+i;
[0114] η min (k+i,k) is the minimum value of the output constraint of the vehicle yaw dynamics model at time k+i;
[0115] η max (k+i, k) is the maximum value of the output constraint of the vehicle yaw dynamics model at time k+i;
[0116] The objective function and optimization conditions are transformed into a quadratic programming problem to obtain an optimized control sequence, and then the first control parameter is taken from the control sequence as the feedback control increment; let Δu fb (k) is the first control parameter, which is used as the front wheel steering angle increment; the feedback control quantity is obtained by the following formula:
[0117] u fb (k) = u fb (k-1)+Δu fb (k) (13)
[0118] Where:
[0119] u fb (k) is the feedback control variable solved by the MPC feedback controller at time k;
[0120] u fb (k-1) is the feedback control variable solved by the MPC feedback controller at time k-1;
[0121] Δu fb (k) is the increment of the feedback control quantity solved by the MPC feedback controller at time k.
[0122] Furthermore, the CasADi / IPOPT optimization tool is used to solve the quadratic programming problem.
[0123] The present invention also provides a device for a trajectory tracking control method based on a neural network and feedback MPC, comprising a memory and a processor, wherein the memory is used to store a computer program; the processor is used to execute the computer program and implement the method steps of the trajectory tracking control method based on a neural network and feedback MPC as described above when executing the computer program.
[0124] The present invention also provides a storage medium, wherein the storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the trajectory tracking control method based on a neural network and feedback MPC as described above are implemented.
[0125] The advantages and positive effects of the present invention are:
[0126] 1. The neural network model proposed in the present invention is trained based on real vehicle driving data under various working conditions; it has higher model accuracy than a two-degree-of-freedom vehicle model and faster response speed than a high-degree-of-freedom vehicle model; it reduces the prediction error caused by changes in vehicle parameters and can complete trajectory tracking tasks under various working conditions.
[0127] 2. The feedback MPC controller proposed in the present invention simulates the behavior of a human driver who constantly observes the road conditions ahead and pre-operates the steering wheel, making full use of road information. Its goal is to make the lateral tracking error of a certain preview point in front of the vehicle zero, which can not only make the vehicle travel along the reference trajectory, but also ensure the safety and handling stability of the vehicle, thereby improving the adaptability to different working conditions. BRIEF DESCRIPTION OF THE DRAWINGS
[0128] Figure 1 The present invention is a flowchart of a trajectory tracking control method based on neural network and feedback MPC.
[0129] Figure 2 Schematic diagram of the neural network model structure.
[0130] Figure 3 This is a schematic diagram of the feedforward controller structure.
[0131] Figure 4 Schematic diagram of the MPC feedback controller structure.
[0132] Figure 5 Schematic diagram of the vehicle dynamics model taking into account road curvature.
[0133] In the figure:
[0134] v x (k) is the longitudinal velocity of the vehicle at time k; v y (k) is the lateral velocity of the vehicle at time k; w(k) is the yaw angular velocity of the vehicle at time k; v ys(k) is the steady-state lateral velocity of the vehicle at time k; w s (k) is the steady-state yaw rate of the vehicle at time k; v ys (k+1) is the steady-state lateral velocity of the vehicle at time k+1; w s (k+1) is the steady-state yaw rate of the vehicle at time k+1; u ff (k) is the feedforward control quantity at time k; X is the longitudinal coordinate of the vehicle's center of mass in the geodetic coordinate system; Y is the lateral coordinate of the vehicle's center of mass in the geodetic coordinate system; is the yaw angle of the vehicle, which is the angle between the x-axis of the vehicle coordinate system and the x-axis of the earth coordinate system; p Y is the vertical coordinate of the projection point in the geodetic coordinate system; p is the lateral coordinate of the projection point in the geodetic coordinate system; is the angle between the tangent line of the projection point and the X-axis of the earth coordinate system; x is the longitudinal axis of the vehicle coordinate system; y is the horizontal axis of the vehicle coordinate system; v x is the longitudinal velocity of the vehicle; v y is the lateral speed of the vehicle; a is the distance from the center of mass of the vehicle to the front axle; b is the distance from the center of mass of the vehicle to the rear axle; δ f is the front wheel turning angle; is the error between the actual yaw angle of the vehicle and the desired yaw angle of the vehicle; e d is the lateral error between the vehicle center of mass and the projection point; κ p is the curvature of the projection point on the reference trajectory; u fb (k) is the feedback control quantity at time k; u fb (k-1) is the feedback control quantity at time k-1; u ff (k) is the feedforward control quantity at time k; δ ref (k) is the front wheel turning angle at the expected time k; L d is the projection distance in front of the vehicle; The projection distance in front of the vehicle is L d The vehicle tracking error. DETAILED DESCRIPTION
[0135] The present invention will be described in detail below with reference to the accompanying drawings and in combination with embodiments. It should be understood that the preferred embodiments described herein are only used to illustrate and explain the present invention, and are not used to limit the present invention.
[0136] The Chinese meanings of the following English words, abbreviations and phrases in this application are as follows:
[0137] MPC: Model Predictive Control.
[0138] BP neural network: back propagation neural network.
[0139] tansig activation function: hyperbolic tangent activation function.
[0140] MSE: mean square error.
[0141] Levenberg-Marquardt: Levenberg-Marquardt method.
[0142] See also Figures 1 to 5 , a trajectory tracking control method based on neural network and feedback MPC, comprising the following steps:
[0143] Step 1: construct a neural network model, so that the input of the neural network model includes the lateral speed, yaw rate, longitudinal speed and front wheel turning angle of the vehicle at the current moment; the output of the neural network model includes the lateral speed and yaw rate of the vehicle at the next moment; collect and compile vehicle driving trajectory samples under various working conditions, and train the neural network model;
[0144] Step 2: construct a feedforward controller to control the vehicle to follow the road curvature; the input of the feedforward controller includes the longitudinal velocity of the vehicle at time k and the steady-state yaw angular velocity of the vehicle at time k, and the output includes the steady-state lateral velocity of the vehicle at time k and the feedforward control quantity at time k; in the feedforward controller, a trained neural network model is used to establish a nonlinear optimization problem, and the feedforward control quantity at time k is obtained according to the interior point method;
[0145] Step 3: Establish a vehicle yaw dynamics model based on path tracking, and establish a system state space equation based on the vehicle yaw dynamics model; linearize and discretize the system state space equation of the vehicle;
[0146] Step 4: construct an MPC feedback controller to eliminate preview errors; the MPC feedback controller, whose input includes the lateral velocity of the vehicle at time k, the yaw velocity of the vehicle at time k, the lateral error of the vehicle at time k, the yaw error of the vehicle at time k and the feedback control amount at time k-1, and whose output includes the feedback control amount at time k; in the MPC feedback controller, a prediction model is constructed based on the linearized and discretized system state space equation, and the prediction model predicts the vehicle system state at multiple times, and an objective function is constructed based on minimizing the preview error, and constraints are set, and the objective function and constraints are converted into a quadratic programming problem, and the quadratic programming problem is solved using an optimization tool to obtain the feedback control amount at time k;
[0147] Step 5: Add the feedforward control quantity and the feedback control quantity at time k to obtain the expected front wheel angle at time k, convert the expected front wheel angle at time k into the torque required for the steering motor of the driving robot, and the driving robot controls the steering wheel to complete trajectory tracking.
[0148] Preferably, in step one, the method for collecting and compiling vehicle driving trajectory samples under various working conditions may include the following method steps: collecting actual trajectory sample data of the vehicle under high adhesion coefficient and low adhesion coefficient conditions respectively, and when collecting trajectory sample data, making the vehicle drive under various working conditions; setting the vehicle to drive on 50 to 100 random paths, the sampling time is 0.05s, and collecting 150,000 to 300,000 sets of vehicle status data.
[0149] Adhesion coefficient refers to the adhesion ability of tires on different road surfaces, which is roughly equivalent to the friction coefficient. On roads with high adhesion coefficients, cars are not easy to slip and drive safely; on roads with low adhesion coefficients, cars are easy to slip, such as snow.
[0150] Vehicle driving conditions, that is, automobile operating conditions, refer to the working conditions of the automobile during transportation and driving. According to the movement forms of the automobile, there are mainly: starting, acceleration, constant speed, deceleration, turning, uphill and downhill, parking, etc.
[0151] Preferably, in step 1, the neural network model can adopt a BP neural network model: the number of hidden layers of the BP neural network model is H, and the number of neurons in each layer is [n1, n2, ..., n H ]; The activation function of the neuron uses the tansig activation function. The activation function of the neuron is set to the tansig activation function, which can output a value between -1 and 1, which helps to generate smooth output in the model and improve gradient transfer, thereby reducing the problem of gradient disappearance.
[0152] Preferably, in step 1, when training the neural network model, the learning rate can be set to 0.001, the training process can use the Levenberg-Marquardt optimization algorithm, the loss function can select MSE, and the maximum training error allowable value can be ε=10 -6 .
[0153] Preferably, step 2 may include the following method steps:
[0154] Can be set: v x (k) is the longitudinal velocity of the vehicle at time k; v ys (k) is the steady-state lateral velocity of the vehicle at time k; w s (k) is the steady-state yaw rate of the vehicle at time k; v ys (k+1) is the steady-state lateral velocity of the vehicle at time k+1; w s (k+1) is the steady-state yaw rate of the vehicle at time k+1; u ff (k) is the feedforward control quantity at time k;
[0155] The trained neural network model can be as follows:
[0156]
[0157] Where: x k =[v ys (k) w s (k) v x (k)] T ;
[0158] f NN (x k ,u ff (k)) represents the output of the neural network model at time k;
[0159] When the longitudinal velocity of the vehicle is constant, the lateral acceleration = 0, and the yaw angular acceleration = 0, the vehicle is in a steady state; let κ p is the curvature of the projection point on the reference trajectory; the steady-state yaw rate calculation formula of the vehicle can be as follows:
[0160] w s (k) = v x (k)*κ p (2)
[0161] A feedforward controller is constructed to control the vehicle to track the road curvature. The feedforward controller calculates the feedforward control quantity and the steady-state lateral velocity of the vehicle so that the vehicle tracks the curvature of the reference trajectory in a stable state.
[0162] During the control process, the longitudinal velocity v of the vehicle at time k is measured. x (k), and the curvature κ of the projection point on the reference trajectory can be calculated using an online map matching algorithm on the vehicle p ; In order to solve u ff (k) and v ys (k), based on the physical constraints of lateral velocity and front wheel steering angle, the nonlinear optimization problem can be defined as follows:
[0163]
[0164] In the above formula:
[0165] v ys,min is the minimum value of the steady-state lateral velocity constraint;
[0166] v ys,max is the maximum value of the steady-state lateral velocity constraint;
[0167] δ min is the minimum value of the front wheel steering angle constraint;
[0168] δ max is the maximum value of the front wheel steering angle constraint;
[0169] The optimization problem aims to track the curvature of the reference trajectory and find the front wheel steering angle control value closest to the steady state under the physical constraints. In the controller, the interior point method is used to solve the nonlinear optimization problem shown in formula (3) to obtain the feedforward control value u at time k ff (k).
[0170] Preferably, step three may include the following method steps:
[0171] Establish a vehicle coordinate system, taking the center of mass of the vehicle as the origin, the longitudinal axis of the vehicle as the x-axis, and the transverse axis of the vehicle as the y-axis; is the yaw angle of the vehicle, which is the angle between the x-axis of the vehicle coordinate system and the x-axis of the earth coordinate system;
[0172] The vehicle yaw dynamics model is established using the two-degree-of-freedom vehicle dynamics equation, assuming that the front wheel turning angle is a small angle; the two-degree-of-freedom vehicle dynamics equation is as follows:
[0173]
[0174] Where:
[0175] v x is the longitudinal velocity of the vehicle;
[0176] v y is the lateral velocity of the vehicle;
[0177] is the lateral acceleration of the vehicle;
[0178] is the yaw rate of the vehicle;
[0179] is the vehicle’s yaw angular acceleration;
[0180] a is the distance from the vehicle's center of mass to the front axle;
[0181] b is the distance from the vehicle's center of mass to the rear axle;
[0182] C f is the cornering stiffness of the front tire;
[0183] C r is the cornering stiffness of the rear tire;
[0184] m is the mass of the car;
[0185] δ fb is the front wheel turning angle;
[0186] I z is the moment of inertia of the vehicle around the z-axis;
[0187] Can be set: is the error between the actual yaw angle of the vehicle and the desired yaw angle of the vehicle; is the error rate between the actual yaw angle of the vehicle and the desired yaw angle of the vehicle; e d is the lateral error between the vehicle center of mass and the projection point; is the lateral error change rate between the vehicle center of mass and the projection point; κ p is the curvature of the projection point on the reference trajectory; u fb is the feedback control quantity.
[0188] The expected yaw angle is equal to the angle between the tangent at the projection point and the X-axis of the geodetic coordinate system. The yaw angle error is equal to the angle between the tangent at the projection point and the X-axis of the geodetic coordinate system minus the yaw angle of the vehicle, which is equal to the angle between the tangent at the projection point and the x-axis of the vehicle coordinate system.
[0189] Autonomous driving projection points refer to the process of path planning and navigation of autonomous driving vehicles, in which the target path or position of the vehicle is projected into the actual environment through certain algorithms and technical means, so that the vehicle can accurately identify and track these paths or positions.
[0190] In order to better track the reference trajectory, a vehicle yaw dynamics model considering path tracking is established. The schematic diagram of the vehicle yaw dynamics model is shown in Figure 5 shown. is the position information of the vehicle's center of mass in the geodetic coordinate system, is the position information of the projection point in the geodetic coordinate system; X is the longitudinal coordinate of the vehicle's center of mass in the geodetic coordinate system; Y is the lateral coordinate of the vehicle's center of mass in the geodetic coordinate system; is the yaw angle of the vehicle, which is the angle between the x-axis of the vehicle coordinate system and the x-axis of the earth coordinate system; p Y is the vertical coordinate of the projection point in the geodetic coordinate system; p is the lateral coordinate of the projection point in the geodetic coordinate system; is the angle between the tangent line of the projection point and the X-axis of the geodetic coordinate system; Figure 5 The geometric relationship in is:
[0191]
[0192] According to the kinematic relationship, and The calculation formula is as follows:
[0193]
[0194] By combining equation (4) and equation (6), the following vehicle yaw dynamics model based on path tracking can be established:
[0195]
[0196] Where W is the vehicle state, u fb =δ fb , matrices A, B1, and B2 are expressed as:
[0197]
[0198]
[0199] η can be set as the output of the vehicle yaw dynamics model. Based on the vehicle yaw dynamics model, the following system state space equation can be established:
[0200]
[0201] in
[0202] Where:
[0203] A is the system matrix of the system state space equation;
[0204] B1 is the input matrix of the system state space equation;
[0205] B2 is an additional input matrix for the system state space equations.
[0206] The system state space equation is discretized and linearized by Taylor level expansion and forward Euler method, and the discretized system state space equation is adjusted to the following new discrete system state space equation:
[0207]
[0208] The new discrete system state space equation is obtained as follows:
[0209]
[0210] in,
[0211] Where:
[0212] W(k) is the state quantity of the discrete system state space equation at time k before adjustment;
[0213] u fb (k-1) is the feedback control quantity at time k-1;
[0214] Δu fb (k) is the increment of feedback control quantity at time k;
[0215] κ p (k) is the curvature of the projection point on the reference trajectory corresponding to time k;
[0216] η(k) is the output of the vehicle yaw dynamics model at time k;
[0217] ξ(k) is the state quantity of the state space equation of the discrete system at time k after adjustment;
[0218] ξ(k+1) is the state quantity of the state space equation of the discrete system at time k+1 after adjustment;
[0219] is the system matrix of the state space equation of the adjusted discrete system at time k;
[0220] is the input matrix of the state space equation of the adjusted discrete system at time k;
[0221] is the additional input matrix of the state space equation of the adjusted discrete system at time k;
[0222] Output matrix for the adjusted discrete system state space equations.
[0223] Preferably, step 4 may include the following method steps:
[0224] The prediction time domain step number of the MPC feedback controller can be set as N p , the control time domain step number of the MPC feedback controller is N c , where N p >N c , construct the following prediction model to predict the output at multiple moments as follows:
[0225]
[0226] Where:
[0227] η(k+1) is the predicted output of the vehicle yaw dynamics model at time k+1;
[0228] η(k+2) is the predicted output of the vehicle yaw dynamics model at time k+2;
[0229] v(k+N p ) is the vehicle yaw dynamics model k+N p The predicted output at the moment;
[0230] Δu fb (k+1) is the increment of feedback control quantity at time k+1;
[0231] Δu fb (k+N c -1) is k+N c -1 The increment of feedback control amount at time;
[0232] κ p (k+1) is the curvature of the projection point on the reference trajectory corresponding to time k+1;
[0233] κ p (k+N p -1) is k+N p -1 The curvature of the projection point on the reference trajectory corresponding to the moment;
[0234] Let L d is the projection distance in front of the vehicle; The projection distance in front of the vehicle is L d The vehicle tracking error is is the error between the actual yaw angle of the vehicle and the desired yaw angle of the vehicle; let e d is the lateral error between the vehicle center of mass and the projection point;
[0235] The goal of the feedback controller is to make full use of road information and minimize the preview error. The error is the projection distance L in front of the vehicle. d The vehicle tracking error is Figure 5 shown.
[0236] The control objective of the MPC feedback controller is to Minimum, The calculation formula can be as follows:
[0237]
[0238] In the formula, is the preview distance weight matrix;
[0239] The objective function of the MPC feedback controller can be set as follows:
[0240]
[0241] Where:
[0242] J is the objective function;
[0243] Q is the weight matrix of the error term;
[0244] R is the incremental weight matrix of feedback control quantity;
[0245] ρ is the relaxation factor weight;
[0246] ε is the relaxation factor;
[0247] η(k+i,k) is the predicted output of the vehicle yaw dynamics model at time k+i;
[0248] Δu fb(k+i,k) is the increment of feedback control quantity at time k+i;
[0249] Based on the physical limit constraints of the state and control quantities of the actual vehicle system, the following constraints can be set for the control quantity and system output:
[0250]
[0251] Where:
[0252] Δu min (k+i,k) is the minimum value of the feedback control increment constraint at time k+i;
[0253] Δu max (k+i,k) is the maximum value of the feedback control increment constraint at time k+i;
[0254] u fb (k+i,k) is the feedback control quantity at time k+i;
[0255] u min (k+i,k) is the minimum value of the feedback control constraint at time k+i;
[0256] u max (k+i,k) is the maximum value of the feedback control constraint at time k+i;
[0257] η(k+i,k) is the predicted output of the vehicle yaw dynamics model at time k+i;
[0258] η min (k+i,k) is the minimum value of the output constraint of the vehicle yaw dynamics model at time k+i;
[0259] η max (k+i, k) is the maximum value of the output constraint of the vehicle yaw dynamics model at time k+i;
[0260] The objective function and optimization conditions are transformed into a quadratic programming problem. The CasADi / IPOPT optimization tool is used to solve the quadratic programming problem, and an optimized control sequence is obtained. The first control parameter is taken from the control sequence as the feedback control increment. Let Δu fb (k) is the first control parameter, which is used as the front wheel steering angle increment; the feedback control quantity can be obtained by the following formula:
[0261] u fb (k) = u fb (k-1)+Δu fb (k) (13)
[0262] Where:
[0263] u fb (k) is the feedback control variable solved by the MPC feedback controller at time k;
[0264] u fb (k-1) is the feedback control variable solved by the MPC feedback controller at time k-1;
[0265] Δu fb (k) is the increment of the feedback control quantity solved by the MPC feedback controller at time k.
[0266] The input includes the feedback control quantity u at time k-1 fb (k-1), the output includes the feedback control quantity u at time k fb (k);
[0267] Preferably, the CasADi / IPOPT optimization tool can be used to solve the quadratic programming problem.
[0268] Step five may include the following method steps:
[0269] The calculation formula of the expected front wheel turning angle at time k is as follows:
[0270] δ ref (k) = u ff (k)+u fb (k) (14)
[0271] Where:
[0272] u fb (k) is the feedback control quantity at time k;
[0273] u ff (k) is the feedforward control quantity at time k;
[0274] δ ref (k) is the front wheel turning angle expected at time k;
[0275] The desired front wheel turning angle can be converted into the torque required by the steering motor of the driving robot, and the driving robot controls the steering wheel to complete trajectory tracking; the driving robot clamps the steering wheel through the chuck, and the steering motor drives the gear transmission to rotate the steering wheel of the vehicle.
[0276] The present invention also provides a device for a trajectory tracking control method based on a neural network and feedback MPC, comprising a memory and a processor, wherein the memory is used to store a computer program; the processor is used to execute the computer program and implement the method steps of the trajectory tracking control method based on a neural network and feedback MPC as described above when executing the computer program.
[0277] The present invention also provides a storage medium, wherein the storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the trajectory tracking control method based on a neural network and feedback MPC as described above are implemented.
[0278] The above-mentioned MPC feedback controller, feedforward controller, neural network model, tansig activation function, interior point method, BP neural network model, online map matching algorithm, Levenberg-Marquardt optimization algorithm, MSE loss function and other functional modules and algorithms can all adopt applicable functional modules and algorithms in the prior art, or be constructed using applicable functional modules and algorithms in the prior art.
[0279] The embodiments described above are only used to illustrate the technical ideas and features of the present invention, and their purpose is to enable those skilled in the art to understand the contents of the present invention and implement them accordingly. The patent scope of the present invention cannot be limited only by this embodiment, that is, any equivalent changes or modifications made to the spirit disclosed by the present invention still fall within the patent scope of the present invention.
Claims
1. A trajectory tracking control method based on neural network and feedback MPC, It is characterized in that The steps include: Step 1: construct a neural network model, so that the input of the neural network model includes the lateral speed, yaw rate, longitudinal speed and front wheel turning angle of the vehicle at the current moment; the output of the neural network model includes the lateral speed and yaw rate of the vehicle at the next moment; collect and compile vehicle driving trajectory samples under various working conditions, and train the neural network model; Step 2: construct a feedforward controller to control the vehicle to follow the road curvature; the input of the feedforward controller includes the longitudinal velocity of the vehicle at time k and the steady-state yaw angular velocity of the vehicle at time k, and the output includes the steady-state lateral velocity of the vehicle at time k and the feedforward control quantity at time k; in the feedforward controller, a trained neural network model is used to establish a nonlinear optimization problem, and the feedforward control quantity at time k is obtained according to the interior point method; Step 3: Establish a vehicle yaw dynamics model based on path tracking, and establish a system state space equation based on the vehicle yaw dynamics model; linearize and discretize the system state space equation of the vehicle; Step 4: construct an MPC feedback controller to eliminate preview errors; the MPC feedback controller, whose input includes the lateral velocity of the vehicle at time k, the yaw velocity of the vehicle at time k, the lateral error of the vehicle at time k, the yaw error of the vehicle at time k and the feedback control amount at time k-1, and whose output includes the feedback control amount at time k; in the MPC feedback controller, a prediction model is constructed based on the linearized and discretized system state space equation, and the prediction model predicts the vehicle system state at multiple times, and an objective function is constructed based on minimizing the preview error, and constraints are set, and the objective function and constraints are converted into a quadratic programming problem, and the quadratic programming problem is solved using an optimization tool to obtain the feedback control amount at time k; Step 5: Add the feedforward control quantity and the feedback control quantity at time k to obtain the expected front wheel angle at time k, convert the expected front wheel angle at time k into the torque required for the steering motor of the driving robot, and the driving robot controls the steering wheel to complete trajectory tracking.
2. The trajectory tracking control method based on neural network and feedback MPC according to claim 1, It is characterized in that In step 1, the method for collecting and compiling vehicle driving trajectory samples under various working conditions includes the following method steps: collecting actual vehicle trajectory sample data under high adhesion coefficient and low adhesion coefficient conditions respectively, and when collecting trajectory sample data, driving the vehicle under various working conditions; The vehicles are set to travel on 50 to 100 random paths, the sampling time is 0.05s, and 150,000 to 300,000 sets of vehicle status data are collected.
3. The trajectory tracking control method based on neural network and feedback MPC according to claim 1, It is characterized in that In step 1, the neural network model adopts the BP neural network model: the number of hidden layers of the BP neural network model is H, and the number of neurons in each layer is [n 1 ,n 2 ,…,n H ]; The neuron activation function uses the tansig activation function.
4. The trajectory tracking control method based on neural network and feedback MPC according to claim 1, It is characterized in that In step 1, when training the neural network model, the learning rate is set to 0.001, the training process uses the Levenberg-Marquardt optimization algorithm, the loss function selects MSE, and the maximum training error allowed is ε = 10 -6 .
5. The trajectory tracking control method based on neural network and feedback MPC according to claim 1, It is characterized in that Step 2 includes the following method steps: Assume: v x (k) is the longitudinal velocity of the vehicle at time k; v ys (k) is the steady-state lateral velocity of the vehicle at time k; w s (k) is the steady-state yaw rate of the vehicle at time k; v ys (k+1) is the steady-state lateral velocity of the vehicle at time k+1; w s (k+1) is the steady-state yaw rate of the vehicle at time k+1; u ff (k) is the feedforward control quantity at time k; The trained neural network model is as follows: Where: x k =[v ys (k) w s (k) v x (k)] T ; f NN (x k ,u ff (k)) represents the output of the neural network model at time k; When the longitudinal velocity of the vehicle is constant, the lateral acceleration = 0, and the yaw angular acceleration = 0, the vehicle is in a steady state; let κ p is the curvature of the projection point on the reference trajectory; the steady-state yaw rate of the vehicle is calculated as follows: w s (k) = v x (k)*κ p (2) During the control process, measure v x (k), and calculate κ using an online map matching algorithm on the vehicle p ; In order to solve u ff (k) and v ys (k), based on the physical constraints of lateral velocity and front wheel steering angle, the nonlinear optimization problem is defined as follows: In the above formula: v ys,min is the minimum value of the steady-state lateral velocity constraint; v ys,max is the maximum value of the steady-state lateral velocity constraint; δ min is the minimum value of the front wheel steering angle constraint; δ max is the maximum value of the front wheel steering angle constraint; The interior point method is used to solve the above nonlinear optimization problem and the feedforward control quantity u at time k is obtained. ff (k).
6. The trajectory tracking control method based on neural network and feedback MPC according to claim 1, It is characterized in that Step three includes the following method steps: set up is the yaw angle of the vehicle, which is the angle between the x-axis of the vehicle coordinate system and the x-axis of the earth coordinate system; The vehicle yaw dynamics model is established using the two-degree-of-freedom vehicle dynamics equation, assuming that the front wheel turning angle is a small angle; the two-degree-of-freedom vehicle dynamics equation is as follows: Where: v x is the longitudinal velocity of the vehicle; v y is the lateral velocity of the vehicle; is the lateral acceleration of the vehicle; is the yaw rate of the vehicle; is the vehicle’s yaw angular acceleration; a is the distance from the vehicle's center of mass to the front axle; b is the distance from the vehicle's center of mass to the rear axle; C f is the cornering stiffness of the front tire; C r is the cornering stiffness of the rear tire; m is the mass of the car; δ fb is the front wheel turning angle; I z is the moment of inertia of the vehicle around the z-axis; set up: is the error between the actual yaw angle of the vehicle and the desired yaw angle of the vehicle; is the error rate between the actual yaw angle of the vehicle and the desired yaw angle of the vehicle; e d is the lateral error between the vehicle center of mass and the projection point; is the lateral error change rate between the vehicle center of mass and the projection point; κ p is the curvature of the projection point on the reference trajectory; u fb is the feedback control quantity; According to the kinematic relationship, and The calculation formula is as follows: Combining equations (4) and (6), the following vehicle yaw dynamics model based on path tracking is established: Where W is the vehicle state, u fb =δ fb , matrix A, B 1 , B 2 Respectively expressed as: Assume η is the output of the vehicle yaw dynamics model, and establish the following system state space equation based on the vehicle yaw dynamics model: in Where: A is the system matrix of the system state space equation; B 1 is the input matrix of the system state space equation; B 2 is an additional input matrix for the system state-space equations. The system state space equation is discretized and linearized by Taylor level expansion and forward Euler method, and the state quantity of the discrete system state space equation is adjusted as follows: The new discrete system state space equation is obtained as follows: in, Where: W(k) is the state quantity of the discrete system state space equation at time k before adjustment; u fb (k-1) is the feedback control quantity at time k-1; Δu fb (k) is the increment of feedback control quantity at time k; κ p (k) is the curvature of the projection point on the reference trajectory corresponding to time k; η(k) is the output of the vehicle yaw dynamics model at time k; ξ(k) is the state quantity of the state space equation of the discrete system at time k after adjustment; ξ(k+1) is the state quantity of the state space equation of the discrete system at time k+1 after adjustment; is the system matrix of the state space equation of the adjusted discrete system at time k; is the input matrix of the state space equation of the adjusted discrete system at time k; is the additional input matrix of the state space equation of the adjusted discrete system at time k; Output matrix for the adjusted discrete system state space equations.
7. The trajectory tracking control method based on neural network and feedback MPC according to claim 6, It is characterized in that Step 4 includes the following method steps: set up: The number of prediction steps in the MPC feedback controller is N. p , the control time domain steps of the MPC feedback controller are N c , where N p >N c , construct the following prediction model to predict the output at multiple moments as follows: … Where: η(k+1) is the predicted output of the vehicle yaw dynamics model at time k+1; η(k+2) is the predicted output of the vehicle yaw dynamics model at time k+2; η(k+N p ) is the vehicle yaw dynamics model k+N p The predicted output at the moment; Δu fb (k+1) is the increment of feedback control quantity at time k+1; Δu fb (k+N c -1) is k+N c -1 The increment of feedback control amount at time; κ p (k+1) is the curvature of the projection point on the reference trajectory corresponding to time k+1; κ p (k+N p -1) is k+N p -1 The curvature of the projection point on the reference trajectory corresponding to the moment; Let L d is the projection distance in front of the vehicle; The projection distance in front of the vehicle is L d The vehicle tracking error is is the error between the actual yaw angle of the vehicle and the desired yaw angle of the vehicle; let e d is the lateral error between the vehicle center of mass and the projection point; The control objective of the MPC feedback controller is to Minimum, The calculation formula is as follows: In the formula, is the preview distance weight matrix; The objective function of the MPC feedback controller is set as follows: Where: J is the objective function; Q is the weight matrix of the error term; R is the incremental weight matrix of feedback control quantity; ρ is the relaxation factor weight; ε is the relaxation factor; η(k+i,k) is the predicted output of the vehicle yaw dynamics model at time k+i; Δu fb (k+i,k) is the increment of feedback control quantity at time k+i; Based on the physical limit constraints of the state and control quantities of the actual vehicle system, the following constraints are set for the control quantities and system output quantities: Where: Δu min (k+i,k) is the minimum value of the feedback control increment constraint at time k+i; Δu max (k+i,k) is the maximum value of the feedback control increment constraint at time k+i; u fb (k+i,k) is the feedback control quantity at time k+i; u min (k+i,k) is the minimum value of the feedback control constraint at time k+i; u max (k+i,k) is the maximum value of the feedback control constraint at time k+i; η(k+i,k) is the predicted output of the vehicle yaw dynamics model at time k+i; η min (k+i, k) is the minimum value of the output constraint of the vehicle yaw dynamics model at time k+i; η max (k+i, k) is the maximum value of the output constraint of the vehicle yaw dynamics model at time k+i; The objective function and optimization conditions are transformed into a quadratic programming problem to obtain an optimized control sequence, and then the first control parameter is taken from the control sequence as the feedback control increment; let Δu fb (k) is the first control parameter, which is used as the front wheel steering angle increment; the feedback control quantity is obtained by the following formula: u fb (k) = u fb (k-1)+Δu fb (k) (13)Where: u fb (k) is the feedback control variable solved by the MPC feedback controller at time k; u fb (k-1) is the feedback control variable solved by the MPC feedback controller at time k-1; Δu fb (k) is the increment of the feedback control quantity solved by the MPC feedback controller at time k.
8. The trajectory tracking control method based on neural network and feedback MPC according to claim 7, It is characterized in that Use the CasADi / IPOPT optimization tool to solve quadratic programming problems.
9. A device for trajectory tracking control method based on neural network and feedback MPC, comprising a memory and a processor, It is characterized in that The memory is used to store a computer program; the processor is used to execute the computer program and, when executing the computer program, implement the method steps of the trajectory tracking control method based on a neural network and feedback MPC as described in any one of claims 1 to 8.
10. A storage medium storing a computer program. It is characterized in that When the computer program is executed by a processor, the steps of the trajectory tracking control method based on a neural network and feedback MPC as described in any one of claims 1 to 8 are implemented.
Citation Information
Cited By
Trajectory tracking control method for electric vehicle
CN119370127A
Self-adaptive vehicle trajectory tracking control method and system suitable for curved surface terrain
CN119440013A
A curved surface terrain adaptive vehicle trajectory tracking control method and system
CN119440013B
Self-adaptive RNN vehicle trajectory tracking controller based on online learning and control method
CN119644710A
Flying car take-off path tracking method based on model predictive control
CN119717521A
