Model predictive control vehicle obstacle avoidance method based on road and obstacle modeling
By embedding road and obstacle models and penalty terms into the MPC, the problems of insufficient environmental modeling and rigid hard constraints in traditional MPC during vehicle obstacle avoidance are solved, achieving higher safety, smoothness and environmental adaptability, and improving the control efficiency of autonomous driving.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- JILIN NORMAL UNIV
- Filing Date
- 2026-03-06
- Publication Date
- 2026-05-22
AI Technical Summary
Traditional model predictive control methods suffer from insufficient environmental modeling during vehicle obstacle avoidance, rigid handling of hard constraints, poor adaptability to perceived scenarios, and difficulty in maintaining high robustness and smoothness in complex dynamic scenarios.
By constructing a model based on roads and obstacles, embedding the MPC optimization objective in the form of differentiable penalty terms, designing a third-order vehicle path tracking model and an Euclidean distance model, and using the Sigmoid function to construct continuous penalty terms, the problem is transformed into a standard quadratic programming problem.
It significantly improves the safety and trajectory smoothness of vehicle obstacle avoidance, enhances environmental adaptability, ensures real-time performance and robustness, and improves control efficiency, especially under limited perception conditions.
Smart Images
Figure CN121806974B_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of autonomous driving and intelligent vehicle trajectory tracking control technology. Background Technology
[0002] The realization of autonomous driving technology relies heavily on the collaboration of environmental perception, planning, and control modules. Among these, the motion planning and control module is responsible for generating safe, comfortable, and executable trajectories based on environmental information and achieving accurate tracking. Model predictive control, due to its ability to explicitly handle system constraints, perform multi-step forward optimization, and adapt to nonlinear systems, has been widely used in trajectory tracking and dynamic obstacle avoidance.
[0003] However, traditional MPC methods still have the following limitations when applied to vehicle obstacle avoidance: 1) Insufficient environmental modeling: Most methods are based on simplified vehicle dynamics models and do not fully incorporate the precise geometric and motion constraints of structured road boundaries and dynamic obstacles; 2) Rigid handling of hard constraints: When road boundaries and obstacle distances are used as hard constraints, optimization problems may become infeasible in complex dynamic scenarios, leading to control failure; 3) Poor adaptability to limited perception scenarios: In specific scenarios such as industrial parks, campuses, and ports, perception systems are often limited by low-cost sensors (such as low-beam lidar), with limited perception range and lack of semantic information, which places higher demands on the algorithm's environmental understanding and adaptive capabilities.
[0004] While existing research has attempted to combine methods such as Artificial Potential Fields (APF) with MPC, or to introduce obstacle distance constraints into MPC, the environmental models used are typically quite simple, making it difficult to fully utilize prior map information. Furthermore, they still fall short in balancing smoothness, safety, and real-time performance in dynamic, dense obstacle scenarios. Therefore, a more robust and smooth MPC obstacle avoidance control method is needed that can deeply integrate environmental models, enhance the controller's environmental perception and adaptability, and maintain high robustness and smoothness even under limited perception conditions. Summary of the Invention
[0005] The purpose of this invention is to develop a model prediction control method for vehicle obstacle avoidance based on road and obstacle modeling by embedding a pre-built road and obstacle model as a differentiable penalty term into the optimization objective of MPC.
[0006] The steps of this invention are:
[0007] S1. Establish vehicle path tracking and road and obstacle models:
[0008] Based on Ackermann steering geometry and path curvature feedforward compensation, a third-order vehicle path tracking model was constructed with lateral error, heading error, and actual steering angle as state variables; the lateral error e was selected as... v and heading error As the system output, the desired steering angle is selected simultaneously. As a control variable of the system Then the state vector , where T is the transpose of the matrix, describes the vehicle path tracking model; establish the minimum Euclidean distance model between the vehicle and the lane boundary to ensure that the vehicle's current position is always within the boundary of the road; for static obstacles, dynamic pedestrians and vehicles in the scene, establish an Euclidean distance model based on the real-time position to ensure that the vehicle and obstacles always maintain a safe distance.
[0009] S2. Construct a model-based predictive control method for vehicle obstacle avoidance based on road and obstacle modeling: Discretize the vehicle path tracking model and predict in the time domain. By analyzing the state trajectory within a given range, a prediction vector is obtained. After defining the prediction sequence of the system state, the prediction sequence of the system output, and the control input sequence, the prediction equations for the system state and the system output are derived. An optimization objective function is then designed. To achieve global path tracking of vehicles;
[0010] S3. To avoid treating the road boundary as a hard constraint, a continuously differentiable penalty term based on the Sigmoid function is designed.
[0011] The third-order vehicle path tracking model described in this invention is as follows:
[0012] (1)
[0013] (2)
[0014] In the formula, This refers to the wheelbase; For vehicle speed; e v The lateral deviation is the vertical distance from the vehicle's current position to the desired path. The rate of change of the vehicle's lateral error; For heading error; The rate of change of the heading error; This is the actual steering angle; The rate of change of the steering angle; For reference steering angle, For the desired steering angle, The time constant of the steering system;
[0015] Vehicle path tracking model:
[0016] (3)
[0017] In the formula, For disturbance terms; For the output vector, This is the output matrix; The instantaneous rate of change of the state variable. The state matrix, To control the input matrix;
[0018] The matrices in equation (3) are as follows:
[0019]
[0020] The minimum Euclidean distance model between the vehicle and the lane boundary:
[0021] (4)
[0022] In the formula, The lateral position of the vehicle; The left and right boundaries of the road. This is the minimum distance between a vehicle and a lane. Preset safe distance between vehicles and the road;
[0023] Euclidean distance model of the real-time positions of vehicles and obstacles:
[0024] (5)
[0025] In the formula, This refers to the longitudinal position of the vehicle. The longitudinal and lateral positions of the obstacle; The distance between the vehicle and the obstacle; Preset safe distance between vehicles and obstacles.
[0026] The discretized vehicle path tracking model of this invention is as follows:
[0027] (6)
[0028] In the formula, for State quantity at any given time. for State quantity at any given time. for The amount of control at any given moment for The output state variables of the system at any given time. The discrete state transition matrix, For discrete control input matrix, Here is the perturbation matrix;
[0029] The discrete state transition matrix and the discrete control input matrix are as follows:
[0030]
[0031] In the formula, It is the identity matrix. The sampling period;
[0032] Prediction Time Domain The state trajectory within is,
[0033] (7)
[0034] In the formula, for The state quantity at any given time. for The amount of control at any given moment For prediction of the time domain back The state quantity at any given time. When the prediction time domain is Discrete state transition matrix at time, When the prediction time domain is Discrete state transition matrix at time, for The amount of control at any given moment For the summation term;
[0035] The prediction vector is obtained based on the state trajectory:
[0036] (8)
[0037] In the formula, Let be the discrete state transition matrix when the prediction time domain is 2. When the prediction time domain is Discrete state transition matrix at time, for The amount of control at any given moment;
[0038] Define the system state prediction sequence, system prediction output sequence, and control input sequence as follows:
[0039] (9)
[0040] In the formula, Sampling time For the next moment The predicted value of the system state vector, To be at the sampling time For the next moment The prediction matrix of the system output vector. for The predicted output value at time 10:00. for The predicted output value at time 10:00. For prediction of the time domain back The predicted output value at time 10:00. To control the input sequence;
[0041] The prediction equations for the system state and the system output are as follows:
[0042] (10)
[0043] (11)
[0044] In the formula, , , and Prediction time domain The state transition matrix, control input matrix, disturbance matrix, and output matrix within the system;
[0045] The state transition matrix, control input matrix, disturbance matrix, and output matrix are as follows:
[0046] ;
[0047] Optimize objective function for:
[0048] (12)
[0049] In the formula, This is the state weight matrix; It is a sequence of feedforward steering angles calculated based on the original path of A* programming after smoothing. To control the weight matrix; for The weight, For speed The reciprocal, for matrix transpose, The system output sequence; where, , for Reference steering angle at any moment for Reference steering angle at any moment for Reference steering angle at any given time.
[0050] The design process of the continuously differentiable penalty term described in this invention is as follows:
[0051] When the vehicle's lateral position approaches the boundary, the penalty term is defined as follows:
[0052] (13)
[0053] (14)
[0054] In the formula, The input value is The Sigmoid function at that time This is the road penalty coefficient. For the road boundary penalty function, A function to determine whether the distance between a vehicle and the road is within a safe range. This is the road penalty term; the final function formula is:
[0055] (15)
[0056] In the formula, The linear combination of the exponential part is Euler number;
[0057] When a vehicle is too close to an obstacle, the penalty is defined as follows:
[0058] (16)
[0059] (17)
[0060] In the formula, This is the obstacle penalty coefficient; The obstacle distance penalty function; A function to determine whether the distance between a vehicle and an obstacle is within a safe range. The preset safe distance between the vehicle and the obstacle. For obstacle penalty items, The input value is The Sigmoid function at that time;
[0061] The final function for obstacle distance penalty is:
[0062] (18)
[0063] In the formula, The linear combination of the exponential part is Euler number;
[0064] In optimizing the objective function The cost objective function is obtained by adding road boundary penalties and obstacle penalties. :
[0065] (19);
[0066] Cost objective function Convert to standard quadratic programming form:
[0067] (20)
[0068] In the formula, It is the coefficient matrix of the quadratic terms. This is the vector of coefficients for the first-order terms;
[0069] The quadratic coefficient matrix and the linear coefficient vector matrix in equation (20) are as follows:
[0070]
[0071] In the formula, For prediction of the time domain The transpose of the matrix obtained by multiplying the output matrix and the control input matrix. and They are respectively The result matrix and matrix transpose, and These are the reference steering angle matrix and matrix transpose, This is the matrix transpose of the road boundary penalty function. This is the matrix transpose of the obstacle distance penalty function;
[0072] Known state quantity at time 1 Control quantity at the previous moment The control time domain is obtained by solving the QP problem of formula (20). Optimal control sequence within :
[0073] (twenty one)
[0074] In the formula, To be at the current sampling time The calculated optimal control quantity. The current sampling time Calculated next moment The optimal control quantity. The current sampling time Calculated The optimal control quantity at any given time.
[0075] The advantages of this invention compared to the prior art are:
[0076] 1. Significantly Enhanced Safety: By embedding road and obstacle models into the optimization target as soft constraints, vehicles can actively maintain a greater safe distance from obstacles. Compared to traditional MPC, the average obstacle avoidance distance can be increased by up to 87.3%, effectively reducing the risk of collision;
[0077] 2. Trajectory smoothness optimization: The introduction of environmental penalty terms makes the controller's optimization objectives more comprehensive, which can significantly reduce the fluctuation of steering angle and path curvature while ensuring safety, thereby improving ride comfort and handling stability.
[0078] 3. Strong environmental adaptability: It makes full use of prior high-precision map information, enhances the ability to follow structured roads, makes up for the shortcomings of online perception, and is particularly suitable for specific closed or semi-closed scenarios with limited perception.
[0079] 4. Good real-time performance and feasibility: The penalty term is constructed using a continuously differentiable Sigmoid function, which avoids the infeasibility of optimization problems that may be caused by hard constraints, and transforms the problem into a standard QP problem to be solved, ensuring the real-time running capability of the algorithm on the vehicle computing platform. Attached Figure Description
[0080] Figure 1 This is a schematic diagram of the RO-MPC control principle;
[0081] Figure 2 This is a comparison diagram of vehicle movement paths using two methods in a static obstacle scenario;
[0082] Figure 3 Comparison of vehicle movement paths in pedestrian avoidance scenarios;
[0083] Figure 4 Comparison of overtaking trajectories of actual vehicles at medium speeds on campus. Detailed Implementation
[0084] Specifically, this invention relates to a real-time obstacle avoidance and trajectory tracking control method based on road and obstacle modeling and integrated into the optimization objective of model predictive control (MPC).
[0085] This invention proposes a model predictive control (RO-MPC) method for vehicle obstacle avoidance based on road and obstacle modeling (RO-MPC). The invention aims to significantly improve vehicle trajectory tracking accuracy, obstacle avoidance safety distance, driving smoothness, and overall control efficiency in specific structured scenarios by embedding a pre-built road and obstacle model as a differentiable penalty term into the optimization objective of MPC.
[0086] The technical solution adopted in this invention includes the following steps:
[0087] I. Establish vehicle path tracking and road and obstacle models:
[0088] 1) Based on Ackermann steering geometry and path curvature feedforward compensation, a third-order vehicle path tracking model was constructed with lateral error, heading error, and actual steering angle as state variables:
[0089] (1)
[0090] (2)
[0091] In the formula, This refers to the wheelbase; For vehicle speed; e v The lateral deviation is the vertical distance from the vehicle's current position to the desired path. For e v The derivative of represents the rate of change of the vehicle's lateral error; For heading error; for The derivative of represents the rate of change of the heading error; This is the actual steering angle; for The derivative of represents the rate of change of the steering angle; The reference steering angle is used to compensate for the effects of path curvature and is obtained from formula (2); The desired steering angle is calculated using the MPC algorithm. is the time constant of the steering system.
[0092] Selecting lateral error and heading error As the system output, the desired steering angle is selected simultaneously. As a control variable of the system Select the state vector , where T is the transpose of the matrix.
[0093] The vehicle path tracking model can be described as:
[0094] (3)
[0095] In the formula, For disturbance terms; The output vector is composed of the first two elements of the state vector. This is the output matrix; The instantaneous rate of change of the state variable. The state matrix, To control the input matrix.
[0096] The matrices in equation (3) (state matrix, control input matrix, disturbance term, and output matrix) are as follows:
[0097] .
[0098] 2) Based on the road boundary constraints of the prior map, establish a minimum Euclidean distance model between the vehicle and the lane boundary to ensure that the vehicle's current position is always within the road boundary range:
[0099] (4)
[0100] In the formula, The lateral position of the vehicle; The left and right boundaries of the road. This is the minimum distance between a vehicle and a lane. Preset safe distance between vehicles and roads.
[0101] 3) For static obstacles, dynamic pedestrians, and vehicles that may appear in the scene, establish an Euclidean distance model based on real-time location to ensure that vehicles and obstacles always maintain a safe distance:
[0102] (5)
[0103] In the formula, This refers to the longitudinal position of the vehicle. The longitudinal and lateral positions of the obstacle; The distance between the vehicle and the obstacle; Preset safe distance between vehicles and obstacles.
[0104] II. This invention is based on the traditional MPC method, as detailed below:
[0105] Discretize the vehicle path tracking model established by formula (3):
[0106] (6)
[0107] In the formula, for State quantity at any given time. for State quantity at any given time. for The amount of control at any given moment for The output state variables of the system at any given time. The discrete state transition matrix is calculated using a bilinear transformation. The discrete control input matrix is calculated using the Euler approximation method. Let be the perturbation matrix.
[0108] The discrete state transition matrix and the discrete control input matrix are as follows:
[0109]
[0110] In the formula, It is the identity matrix. The sampling period.
[0111] Prediction Time Domain The state trajectory within the range is shown in formula (7):
[0112] (7)
[0113] In the formula, for State quantity at any given time. for The amount of control at any given moment For prediction of the time domain back State quantity at any given time. When the prediction time domain is Discrete state transition matrix at time, When the prediction time domain is Discrete state transition matrix at time, for The amount of control at any given moment For the summation term.
[0114] The prediction vector is obtained based on the state trajectory:
[0115] (8)
[0116] In the formula, Let be the discrete state transition matrix when the prediction time domain is 2. When the prediction time domain is Discrete state transition matrix at time, for The amount of control at any given moment.
[0117] Define the system state prediction sequence, system prediction output sequence, and control input sequence as follows:
[0118] (9)
[0119] In the formula, Sampling time For the next moment The predicted value of the system state vector, To be at the sampling time For the next moment The prediction matrix of the system output vector. for The predicted output value at time 10:00. for The predicted output value at time 10:00. For prediction of the time domain back The predicted output value at time 10:00. To control the input sequence.
[0120] The prediction equations for the system state and the system output are obtained as follows:
[0121] (10)
[0122] (11)
[0123] In the formula, , , and Prediction time domain The state transition matrix, control input matrix, disturbance matrix, and output matrix are included.
[0124] The state transition matrix, control input matrix, disturbance matrix, and output matrix are as follows:
[0125] .
[0126] Design and optimize the objective function. To achieve global path tracking for the vehicle, the first part has the function of tracking the global path and ensuring a relatively smooth steering angle; the second part completes the global tracking task; the third part makes the vehicle as large as possible to reach the destination as quickly as possible.
[0127] (12)
[0128] In the formula, This is the state weight matrix; It is a sequence of feedforward steering angles calculated based on the original path of A* programming after smoothing. To control the weight matrix; for The weight, For speed The reciprocal, for matrix transpose, The system output sequence; where, , for Reference steering angle at any moment for Reference steering angle at any moment for Reference steering angle at any given time.
[0129] III. The present invention constructs an RO-MPC fusion optimization strategy, as detailed below:
[0130] To avoid the optimization problem becoming infeasible due to treating road boundaries as hard constraints, a method based on the Sigmoid function is designed. The penalty term is continuously differentiable. The penalty value increases rapidly as the vehicle's lateral position approaches the boundary.
[0131] The penalty item is defined as:
[0132] (13)
[0133] (14)
[0134] In the formula, The input value is The Sigmoid function at that time This is the road penalty coefficient. For the road boundary penalty function, This function determines whether the distance between a vehicle and the road is within a safe range. When the distance between the vehicle and the road boundary is greater than the safe distance, the function value is 0, representing safety; otherwise, the penalty is increased. As a road penalty item, when When the value is 0, the Sigmoid function value is 0.5. At this time, the distance between the vehicle and the road is a safe distance, so 0.5 is subtracted.
[0135] The final function formula is:
[0136] (15)
[0137] In the formula, The linear combination of the exponential part is Euler number.
[0138] The penalty increases rapidly when the vehicle gets too close to an obstacle. The penalty is defined as follows:
[0139] (16)
[0140] (17)
[0141] In the formula, This is the obstacle penalty coefficient; The obstacle distance penalty function; This function determines whether the distance between a vehicle and an obstacle is within a safe range. When the distance between the vehicle and the obstacle is greater than the safe distance, the function value is 0, representing safety; otherwise, the penalty is increased. The preset safe distance between the vehicle and the obstacle. For obstacle penalty items, The input value is The Sigmoid function at that time.
[0142] The final function for obstacle distance penalty is:
[0143] (18)
[0144] In the formula, The linear combination of the exponential part is Euler number.
[0145] In the optimization objective function of equation (12) After adding road boundary penalties and obstacle penalties, a new cost objective function is obtained. :
[0146] (19).
[0147] Cost objective function Convert to standard quadratic programming (QP) form:
[0148] (20)
[0149] In the formula, It is the coefficient matrix of the quadratic terms. This is the vector of coefficients for the first-order terms.
[0150] The quadratic coefficient matrix and the linear coefficient vector matrix in equation (20) are as follows:
[0151]
[0152] In the formula, For prediction of the time domain The transpose of the matrix obtained by multiplying the output matrix and the control input matrix. and They are respectively The result matrix and matrix transpose, and These are the reference steering angle matrix and matrix transpose, This is the matrix transpose of the road boundary penalty function. This is the matrix transpose of the obstacle distance penalty function.
[0153] Known state quantity at time 1 Control quantity at the previous moment The control time domain is obtained by solving the QP problem of formula (20). Optimal control sequence within :
[0154] (twenty one)
[0155] In the formula, To be at the current sampling time The calculated optimal control quantity. The current sampling time Calculated next moment The optimal control quantity. The current sampling time Calculated The optimal control quantity at any given time.
[0156] The first element in the control sequence is used as the actual control input to the system. This process is repeated in the next control cycle to achieve trajectory tracking and obstacle avoidance control of the vehicle.
[0157] The specific design steps for realizing the integrated "sensing-modeling-prediction-control" framework are as follows:
[0158] 1) Global path planning stage: The A* global planning algorithm is used to generate the optimal reference path from the starting point to the destination based on prior environmental information.
[0159] 2) RO-MPC Online Rolling Optimization Phase: Using the globally planned path as a reference, the current vehicle state, along with road boundary information and dynamic obstacle position and speed information obtained from perception and the map, are input into the RO-MPC controller. The controller performs multi-step predictive optimization based on an objective function that incorporates environmental penalties, solving for the optimal front wheel steering angle command in real time.
[0160] 3) Control Execution and Feedback Phase: The optimized control commands are sent to the vehicle actuators via the drive-by-wire system to achieve trajectory tracking and obstacle avoidance. The system then enters the next control cycle, repeating steps 2) to 3) until the final target is reached.
[0161] To verify the effectiveness of the RO-MPC method proposed in this invention, system tests were conducted on a high-fidelity virtual simulation platform and in a real campus road environment. The software platform adopts a certain communication framework, integrates a certain autonomous driving software stack, and constructs a three-dimensional simulation environment containing static obstacles and dynamic pedestrians in a certain simulation environment; the hardware platform is based on a real vehicle equipped with a 16-line LiDAR, a combined navigation system, and an on-board industrial control computer, and is tested on structured campus roads.
[0162] 1) In a simulation environment, static obstacle scenarios and pedestrian avoidance scenarios were selected for verification. Experimental results show that in static obstacle scenarios, compared with traditional MPC, RO-MPC reduces the average steering angle by 13.1%, the average curvature by 5.1%, the maximum steering angle by 6.7%, and the minimum distance to obstacles by 21.85%; in pedestrian avoidance scenarios, compared with traditional MPC, RO-MPC reduces the average steering angle by 17.4%, the average curvature by 4.1%, the maximum steering angle by 9.3%, and the minimum distance to obstacles by 3.54%, with a significant improvement in trajectory smoothness.
[0163] 2) In the campus real-vehicle verification, a medium-speed three-vehicle overtaking scenario test was conducted. Real-vehicle data shows that the RO-MPC trajectory is significantly smoother, the average steering angle is reduced by 19.4%, the average curvature is reduced by 34.2%, the minimum safe distance from slow obstacle vehicles is increased by an average of 87.3%, and the task completion time is shortened by 39.2%, significantly improving traffic efficiency while ensuring safety.
[0164] In summary, by incorporating road and obstacle models into the MPC framework as penalty terms, this invention effectively improves the obstacle avoidance safety, trajectory smoothness, and control efficiency of vehicles in specific scenarios, providing an effective solution for solving the autonomous driving control problem under limited perception conditions.
Claims
1. A model predictive control method for vehicle obstacle avoidance based on road and obstacle modeling, characterized in that: The steps are as follows: S1. Establish vehicle path tracking and road and obstacle models: Based on Ackermann steering geometry and path curvature feedforward compensation, a third-order vehicle path tracking model was constructed with lateral error, heading error, and actual steering angle as state variables; the lateral error e was selected as... v and heading error As the system output, the desired steering angle is selected simultaneously. As a control variable of the system Then the state vector , where T is the transpose of the matrix, describes the vehicle path tracking model; establish the minimum Euclidean distance model between the vehicle and the lane boundary to ensure that the vehicle's current position is always within the boundary of the road; for static obstacles, dynamic pedestrians and vehicles in the scene, establish an Euclidean distance model based on the real-time position to ensure that the vehicle and obstacles always maintain a safe distance. S2. Construct a model-based predictive control method for vehicle obstacle avoidance based on road and obstacle modeling: Discretize the vehicle path tracking model and predict in the time domain. By analyzing the state trajectory within a given range, a prediction vector is obtained. After defining the prediction sequence of the system state, the prediction sequence of the system output, and the control input sequence, the prediction equations for the system state and the system output are derived. An optimization objective function is then designed. To achieve global path tracking of vehicles; The objective function is optimized. for: （12） In the formula, This is the state weight matrix; It is a sequence of feedforward steering angles calculated based on the original path of A* programming after smoothing. To control the weight matrix; for The weight, For speed The reciprocal, for matrix transpose, The system output sequence; where, , for Reference steering angle at any moment for Reference steering angle at any moment for Reference steering angle at any given time; S3. To avoid treating road boundaries as hard constraints, a continuously differentiable penalty term based on the Sigmoid function is designed: The continuously differentiable penalty term contains two penalty terms: (1) Penalty for a vehicle's lateral position being close to the boundary: （15） In the formula, The linear combination of the exponential part is Euler number, This is the road penalty coefficient. The road penalty coefficient is a function within the safe range between the vehicle and the road. (2) Penalty for vehicles being too close to obstacles: （18） In the formula, The linear combination of the exponential part is Euler number, The obstacle penalty coefficient. It is the obstacle penalty coefficient, which is a function within the safe range between the vehicle and the obstacle.
2. The model predictive control vehicle obstacle avoidance method based on road and obstacle modeling according to claim 1, characterized in that: The third-order vehicle path tracking model is as follows: （1） （2） In the formula, Wheelbase; For vehicle speed; e v The lateral deviation is the vertical distance from the vehicle's current position to the desired path. The rate of change of the vehicle's lateral error; For heading error; The rate of change of the heading error; This is the actual steering angle; The rate of change of the steering angle; For reference steering angle, For the desired steering angle, The time constant of the steering system; Vehicle path tracking model: （3） In the formula, For disturbance terms; For the output vector, This is the output matrix; The instantaneous rate of change of the state variable. The state matrix, To control the input matrix; The matrices in equation (3) are as follows: The minimum Euclidean distance model between the vehicle and the lane boundary: （4） In the formula, The lateral position of the vehicle; The left and right boundaries of the road. This is the minimum distance between a vehicle and a lane. Preset safe distance between vehicles and the road; Euclidean distance model of the real-time positions of vehicles and obstacles: （5） In the formula, This refers to the longitudinal position of the vehicle. The longitudinal and lateral positions of the obstacle; The distance between the vehicle and the obstacle; Preset safe distance between vehicles and obstacles.
3. The model predictive control vehicle obstacle avoidance method based on road and obstacle modeling according to claim 1, characterized in that: The discretized vehicle path tracking model is as follows: （6） In the formula, for State quantity at any given time. for State quantity at any given time. for The amount of control at any given moment for The output state variables of the system at any given time. The discrete state transition matrix, For discrete control input matrix, Here is the perturbation matrix; The discrete state transition matrix and the discrete control input matrix are as follows: ； In the formula, It is the identity matrix. The sampling period; Prediction Time Domain The state trajectory within is, （7） In the formula, for State quantity at any given time. for The amount of control at any given moment For prediction of the time domain back State quantity at any given time. When the prediction time domain is Discrete state transition matrix at time, When the prediction time domain is Discrete state transition matrix at time, for The amount of control at any given moment For the summation term; The prediction vector is obtained from the state trajectory: （8） In the formula, Let be the discrete state transition matrix when the prediction time domain is 2. When the prediction time domain is Discrete state transition matrix at time, for The amount of control at any given moment; Define the system state prediction sequence, system prediction output sequence, and control input sequence as follows: （9） In the formula, Sampling time For the next moment The predicted value of the system state vector, To be at the sampling time For the next moment The prediction matrix of the system output vector. for The predicted output value at time 10:
00. for The predicted output value at time 10:
00. For prediction of the time domain back The predicted output value at time 10:
00. To control the input sequence; The prediction equations for the system state and the system output are as follows: （10） （11） In the formula, , , and Prediction time domain The state transition matrix, control input matrix, disturbance matrix, and output matrix within the system; The state transition matrix, control input matrix, disturbance matrix, and output matrix are as follows: ； Thus, the optimization objective function is obtained. .
4. The model predictive control vehicle obstacle avoidance method based on road and obstacle modeling according to claim 1, characterized in that: The design process for the continuously differentiable penalty term is as follows: When the vehicle's lateral position approaches the boundary, the penalty term is defined as follows: （13） （14） In the formula, The input value is The Sigmoid function at that time For the road boundary penalty function, A function to determine whether the distance between a vehicle and the road is within a safe range. Road penalty items ; Substitute the function within the safe range between the vehicle and the road into the road penalty term to obtain the penalty term for the vehicle's lateral position approaching the boundary. When a vehicle is too close to an obstacle, the penalty is defined as follows: （16） （17） In the formula, The obstacle distance penalty function; A function to determine whether the distance between a vehicle and an obstacle is within a safe range. The preset safe distance between the vehicle and the obstacle. For obstacle penalty items, The input value is The Sigmoid function at that time; Substituting the function representing the safe distance between the vehicle and the obstacle into the obstacle penalty term yields the penalty term for the vehicle being too close to the obstacle. ; In optimizing the objective function The cost objective function is obtained by adding road boundary penalties and obstacle penalties. : （19） ； Cost objective function Convert to standard quadratic programming form: （20） In the formula, It is the coefficient matrix of the quadratic terms. This is the vector of coefficients for the first-order terms; The quadratic coefficient matrix and the linear coefficient vector matrix in equation (20) are as follows: ； In the formula, For prediction of the time domain The transpose of the matrix obtained by multiplying the output matrix and the control input matrix. and They are respectively The result matrix and matrix transpose, and These are the reference steering angle matrix and matrix transpose, This is the matrix transpose of the road boundary penalty function. This is the matrix transpose of the obstacle distance penalty function; Known state quantity at time 1 Control quantity at the previous moment The control time domain is obtained by solving the QP problem of formula (20). Optimal control sequence within : （21） In the formula, To be at the current sampling time The calculated optimal control quantity. The current sampling time Calculated next moment The optimal control quantity. The current sampling time Calculated The optimal control quantity at any given time.