Research on Dynamic Trajectory Planning Based on Model Predictive Theory for Complex Driving Scenarios Authors: Hongluo Li, Hai Pang, Hongyang Xia, Yongxian Huang, Xiangkun Zeng Published: 2025 ABSTRACT Autonomous driving, a transformative automotive technology, is currently a major research focus. Trajectory planning, one of the three core technologies for realizing autonomous driving, plays a decisive role in the performance of autonomous driving systems. The key challenge lies in planning an optimal trajectory based on real-time environmental information, yet significant research gaps remain, particularly for dynamic driving scenarios. To address this, our study investigates lane-changing trajectory planning in dynamic scenarios based on model predictive control (MPC) theory and proposes a novel dynamic lane-changing trajectory planning method. First, kinematic models for both the host vehicle and surrounding vehicles are established. Then, following the core components of MPC theory, we construct a prediction model, define an objective function, and formulate constraints for the rolling optimization step. Finally, the optimal control sequence derived from the optimization is processed using a least-squares fitting method to generate a lane-changing trajectory that demonstrates real-time adaptability in dynamic environments. The proposed method is validated through simulation studies of three typical driving conditions on a co-simulation platform. The results confirm that the planned trajectory exhibits excellent dynamic real-time adaptability, thereby contributing a foundation for achieving full-scenario autonomous driving. 1. Introduction With the vigorous development of new-generation information technologies such as artificial intelligence, autonomous driving technology based on pure electric vehicles as carriers has become a new strategic direction in the global automotive industry layout. The key technologies of autonomous driving mainly include positioning and perception, decision-making and planning, control and execution, etc. Among these, planning a reasonable and ideal trajectory that meets the requirements of real driving scenarios has become a research difficulty in realizing full-scenario autonomous driving. With the continuous deepening of research on this technology, the trajectory planning technology for autonomous vehicles under simple constraints has gradually matured; however, its development still faces severe challenges. Particularly, trajectory planning in complex dynamic driving scenarios, which requires dynamic randomness and high real-time performance, has become a challenging problem under more complex constraints that urgently needs to be addressed in the trajectory planning of autonomous driving technology. At present, research on this problem adopts various methods with different characteristics. Among them, curve fitting methods [1,2], artificial potential field methods [3,4,5,6,7,8], sampling-based methods [9,10], and methods based on deep learning and reinforcement learning [11,12,13,14] are widely used in current autonomous driving trajectory planning. However, these methods still suffer from limitations such as high computational complexity and proneness to deadlock. The B-spline curve method is a commonly used approach in curve fitting. Y. Zhang et al. [1] proposed the use of B-spline curves to generate smooth trajectories that satisfy centripetal acceleration constraints, which is suitable for in-lane driving scenarios. B.S. Oldaç et al. [2] attempted to solve the trajectory generation and tracking problem of intelligent vehicles by performing linear interpolation on discrete points generated by B-spline curves. However, the B-spline curve method often has problems such as easy deformation of the overall curve caused by single-point adjustment. As a result, control points need to be recalculated frequently in dynamic scenarios, which affects the real-time performance of trajectory planning. The artificial potential field method guides the robot to avoid obstacles and move toward the target by setting target attractive forces and obstacle repulsive forces. Z. Zhang et al. [3] proposed an algorithm integrating global and local path planning based on the artificial potential field method. By optimizing the gravitational and repulsive force functions acting on the vehicle, this algorithm overcomes the failure problem of the traditional artificial potential field method in specific scenarios. G. Du et al. [6] first conducted systematic modeling and mathematical description of the planning and control problems of autonomous vehicles, and then used the global heuristic planning algorithm based on the artificial potential field method to generate real-time optimal motion sequences. H. Zhang et al. [8] optimized the problems of unreachable target and low success rate in the artificial potential field method by improving the repulsion field function to solve the unreachable target issue and selecting a virtual target point to make the system escape the trap when it falls into one. Although such methods can effectively construct models, they generally have problems of low path search efficiency and an inability to ensure path optimality. Sampling-based methods construct paths by randomly generating samples in the environment. Z. Chen et al. [9] adopted a longitudinal sampling strategy based on preset driving modes, which improved the probabilistic completeness of existing parameterized curve sampling technologies, thereby realizing fast and effective obstacle avoidance. G. Li et al. [10] proposed an improved sampling-based hybrid A* algorithm. First, a hybrid potential field model that characterizes the vehicle’s workspace and integrates traffic rules and obstacle distribution was proposed as a heuristic function, and environmental topology prior information was introduced to generate a set of directional motion primitives, which further improved the feasibility of the planned path. Although such methods can find the optimal path, the speed of approaching the optimal solution is relatively slow. In addition, learning-based methods have gradually attracted attention, especially the application of deep learning and reinforcement learning. Z. Yu et al. [11] proposed a learning-based Frenet planning network, which samples and selects the most human-like terminal states through learning strategies to generate safe trajectories. Z. Bai et al. [12] proposed an improved deep reinforcement learning path planning algorithm. First, the Automated Mobile Robot (AMR) path planning was modeled as a Markov Decision Process (MDP), and a Double Deep Q-Network (DDQN) was used to obtain the optimal adaptive solution. Finally, the B-spline curve theory was applied to realize path smoothness optimization. J. Nan et al. [13] proposed a longitudinal car-following trajectory planning method based on offline sampling maximum entropy Deep Inverse Reinforcement Learning (DIRL). This method collects natural driving data and uses a deep neural network to replace the linear function of traditional inverse reinforcement learning, which greatly improves the fitting ability to human reward functions. R. Chai et al. [14] specifically focused on the parking maneuver problem of autonomous ground vehicles. In the motion planning stage, a Recurrent Deep Neural Network (RDNN) was innovatively used to approximate the optimal parking trajectory, and the internal correlation between vehicle states was fully explored through the recurrent network structure. In terms of tracking control, a controller based on Adaptive Neural Networks (ALNN) was designed. By dynamically adjusting network parameters, the system stability and error convergence were theoretically guaranteed. Although these methods perform well in handling complex environments and nonlinear problems, they still face challenges such as long training time and large data demand. In recent years, the Model Predictive Control (MPC) algorithm has garnered significant attention and been widely adopted due to its advantages of good control effect, strong robustness, and convenient handling of various constraints in controlled variables and manipulated variables of the process. Among relevant studies, D. J. Kim et al. [15] proposed a lateral vehicle trajectory planning and control algorithm based on MPC for automatic vertical parking systems. In the research of Bassam Alrifaee [16], vehicle collision avoidance control was studied based on the MPC method, and the effectiveness of this method was verified through simulations and experiments. Zhao et al. [17] designed a two-layer controller for trajectory planning and tracking based on the MPC algorithm and a fuzzy adaptive PID trajectory tracking controller to improve the driving safety of intelligent vehicles. Q. Zeng et al. [18] addressed the problems of insufficient emergency obstacle avoidance capability and vulnerability to noise interference in vehicle collision avoidance systems, and constructed a Cooperative Adaptive Cruise Control (CACC) model integrating multi-constraint MPC. Q. Dai et al. [19] proposed a Model Predictive Decision-Making (MPDM) method integrating lane-changing time constraints based on the MPC method. I. Askari et al. [20] transformed the MPC problem of motion planning into a Bayesian state estimation problem, and then developed a new type of implicit particle filtering/smoothing algorithm. This algorithm is implemented through multiple sets of Unscented Kalman Filters/Smoothers and has the characteristics of high sampling efficiency, fast calculation speed, and excellent estimation accuracy. D. Chu et al. [21] proposed a trajectory planning and tracking framework, which obtains the target trajectory through the artificial potential field method and uses an MPC controller combined with PID feedback to achieve accurate tracking. Experimental and simulation results show that compared with traditional MPC, this framework has significant improvements in tracking accuracy and steering smoothness, especially under curve driving conditions, where its steady-state error can approach zero. G. Yu et al. [22] proposed a bidirectional multi-vehicle collaborative collision avoidance system based on the MPC framework to solve the collision risk prevention and control problem in multi-vehicle head-on driving scenarios. H. Yang et al. [23] proposed a dynamic obstacle avoidance strategy for autonomous vehicles based on MPC by establishing new collision constraints considering road width and vehicle geometric dimensions, ensuring a safe distance between the host vehicle and obstacle vehicles. J. Wang et al. [24] proposed a new obstacle avoidance method based on event-triggered MPC. By setting the trajectory tracking error as the trigger condition, the frequent collaborative operation between the planning layer and the control layer was realized. Y. Wang et al. [25] innovatively considered the motion prediction of other traffic participants and used MPC to optimize the trajectory according to the current state of the vehicle. Through the collaborative optimization strategy of differentiated prediction horizons and coordinate transformation, it not only simplified the integration process of constraints, but also made the planning results more intuitive. However, the aforementioned methods generally have problems such as excessively high computational complexity or insufficient adaptability when facing dynamic and complex working conditions. Therefore, in view of the characteristic that constraint conditions and planning objectives in trajectory planning often change with different scenarios, researching trajectory planning technologies for different scenarios under complex constraints based on control theory with predictive capabilities will effectively solve the key problem of trajectory planning in complex dynamic driving scenarios, thereby accelerating the application and industrialization of autonomous driving technology in engineering. Based on the current research landscape, this paper conducts relevant research on the challenging issue of trajectory planning in complex dynamic scenarios using the MPC method and proposes a novel trajectory planning approach tailored for complex dynamic driving scenarios. The organization of this paper is as follows. In Section 2, a vehicle kinematic model including the host vehicle and surrounding vehicles is established. In Section 3, the implementation process of dynamic trajectory planning based on model prediction is deduced and analyzed in detail. In Section 4, simulation analyses are carried out under different working conditions based on the built co-simulation platform to verify the effectiveness of the proposed method. Finally, the conclusion and future work are summarized in Section 5. 2. Establishment of a Vehicle Kinematic Model Autonomous driving needs to be realized through the control of the vehicle’s kinematic or dynamic system. Establishing a reasonable vehicle system model is not only a prerequisite for designing a model predictive controller, but also the foundation for achieving vehicle motion planning and control. 2.1. Vehicle Kinematic Modeling (Kinematic Model) Kinematics examines the laws of object motion from a geometric standpoint, encompassing changes in spatial position, velocity, and other parameters over time. Applying kinematic models in vehicle path planning algorithms ensures that the planned paths are practically feasible and satisfy the kinematic geometric constraints during driving; meanwhile, kinematic models can also reflect vehicle motion characteristics with relatively high accuracy. The vehicle steering motion model is shown in Figure 1 and Figure 2. In the inertial coordinate system XOY, (Xf, Yf) and (Xr, Yr) represent the coordinates of the centers of the vehicle’s front axle and rear axle, respectively. φ denotes the vehicle’s yaw angle (heading angle). The research object is assumed to be a front-wheel-steering vehicle, where δf is the front-wheel steering angle, vf is the velocity at the center of the front axle, vr is the velocity at the center of the rear axle, and l is the wheelbase. In the variables, the subscript “f” stands for “front”, and “r” stands for “rear”; the same notation applies hereinafter. Since the research object is a front-wheel-steering vehicle, during the vehicle’s steering movement, the instantaneous center of rotation P lies on the extension line of the rear axle, where R represents the steering radius of the rear wheels. Let M be the center of the vehicle’s rear axle and N be the center of the front axle. It is assumed that the vehicle’s sideslip angle at the center of mass remains constant during steering, meaning the vehicle’s instantaneous steering radius is the same as the road’s curvature radius. At the rear axle’s travel center (Xr, Yr), the velocity can be expressed as(1)vr=X˙rcosφ+Y˙rsinφ Then, the kinematic constraint at the rear axle center M can be expressed as(2)X˙rsinφ−Y˙rcosφ=0 By combining the two equations above, the following can be obtained:(3)X˙r=vrcosφY˙r=vrsinφ According to the geometric relationship between the front wheels and rear wheels, the following can be obtained:(4)Xf=Xr+lcosφYf=Yr+lsinφ At the center M of the vehicle’s rear axle, the yaw rate can be expressed as(5)ω=vrltanδf In the formula, ω represents the vehicle yaw rate; meanwhile, the steering radius R and the front-wheel steering angle δf can be derived from the yaw rate ω and the vehicle’s rear-axle speed vr:(6)R=vr/ωδf=arctanl/R By synthesizing the above equations, the kinematic model of a single vehicle can be obtained as follows:(7)X˙rY˙rφ˙=cosφsinφtanδf/lvr This model can be further expressed in a more general form:(8)ξ˙kin=fkinξkin,ukin In this system, the state variables are selected as the longitudinal position, lateral position, and heading angle of the host vehicle in the inertial coordinate system. ξkin=XrYrφT, The control variables are selected as the vehicle speed and the front-wheel steering angle of the vehicle. ukin=vrδfT; In the formula, fkin·,· is the state transition function of the system. 2.2. Kinematic Model of Surrounding Vehicles Considering that there are surrounding vehicles during the vehicle’s driving process, and the motion state of each vehicle is also dynamically updated. The kinematic model for surrounding vehicles is established using the same method as described above:(9)X˙siY˙siφ˙si=cosφsisinφsitanδf_si/lvsi In the formula, the subscript si represents surrounding vehicles, and when i takes values p,lf,lr,rf,rr, they represent the preceding vehicle in the current lane, the preceding vehicle in the left lane, the following vehicle in the left lane, the preceding vehicle in the right lane, and the following vehicle in the right lane, respectively. Similarly, it can be expressed in a more general form:(10)ξ˙kin_si=fkin_siξkin_si,ukin_si In this system, the state variables are selected as the longitudinal-lateral positions and heading angles of other surrounding vehicles in the inertial coordinate system, ξkin_si=XsiYsiφ_siT, and the control variables are selected as the vehicle speeds and front-wheel steering angles of these vehicles ukin_si=vsiδf_siT. 3. Implementation of Dynamic Trajectory Planning Based on Model Predictive Control The implementation process of the model predictive theory mainly consists of three key steps [26]: predictive model, rolling optimization, and feedback correction (Figure 3). The predictive model is the foundation of model predictive control, and its main function is to predict the future output of the system based on the historical information of the object and future inputs. Rolling optimization refers to the process where model predictive control determines the control action through the optimization of a certain performance index. However, this optimization is not carried out offline once, but repeatedly online, which is the fundamental difference between model predictive control and traditional optimal control. Feedback correction is intended to prevent the deviation of the control from the ideal state caused by model mismatch or environmental interference. At a new sampling moment, the actual output of the object is first detected, and this real-time information is used to correct the prediction result based on the model, followed by the execution of a new round of optimization. 3.1. Vehicle Model Construction—Predictive Model In the prediction horizon, both the system’s future state variables and output variables can be calculated using the system’s current state variables and the control increments within the control horizon. This corresponds to the “prediction” function in the model predictive algorithm.Xk+1=Xk+ΔhvkcosφkYk+1=Yk+Δhvksinφkφk+1=φk+ΔhvktanδfkL In the formula, k represents a certain moment during the lane-changing process. Xk, Yk, φk, vk, δfk are the values of the vehicle’s driving state variables and control variables at moment k, respectively, Δh is the simulation time step. Expressed in matrix form:xk+1=Akxk+Bkuk In the formula,Ak=100010001,Bk=Δhcosφk0Δhsinφk0ΔhtanδfkL0,uk=vkδfk. 3.2. Establishment of Objective Function with Constraints—Rolling Optimization At each moment in MPC, optimization must be performed to obtain the optimal control quantity for the next moment (Figure 4). Objective Function (Performance Function) Objective Function (Performance Function) Minimize the lateral acceleration of the vehicle during the lane-changing process to ensure riding comfort: Jc=ay. In the formula, ay represents the lateral acceleration value during the lane-changing process. Minimize the lateral acceleration of the vehicle during the lane-changing process to ensure riding comfort: Jc=ay. In the formula, ay represents the lateral acceleration value during the lane-changing process. Enable the lane change to be completed as quickly as possible, with the shortest possible lane-changing time: Jt=yd−ye. In the formula, yd and ye represent the desired lateral displacement and the actual lateral displacement during the lane-changing process, respectively. Enable the lane change to be completed as quickly as possible, with the shortest possible lane-changing time: Jt=yd−ye. In the formula, yd and ye represent the desired lateral displacement and the actual lateral displacement during the lane-changing process, respectively. Convert each index into a quadratic form to obtain the total objective function (performance index function):minJ=Jc+Jt=ω1∫t0tfyd−ye2dt+ω2ay2 In the formula, ω1 and ω2 are the weight coefficients of each index in the objective function; yd is the desired lateral displacement during the lane-changing process, which is set as the lane width in this paper, taking 3.5 m; ye is the actual lateral displacement during the lane-changing process; ye=y0+∫t0tfvsinφdt and y0 are the vehicle lateral position information at the start moment of lane change; ay is the vehicle lateral acceleration value during the lane-changing process, which can be expressed as ay=v2L1+Kv2δf according to the stable yaw rate gain relationship of the two-degree-of-freedom vehicle and the motion relationship of the vehicle during instantaneous curve driving. From this, it can be concluded that the expression of the objective function is only related to the vehicle driving state x=φ and the control quantity u=v,δf. In the above relational expression, v is the vehicle’s driving speed; φ is the vehicle’s heading angle; δf is the vehicle’s front-wheel steering angle; K is the vehicle’s stability factor, K=mL2aCr−bCf, with the unit of s2/m2; where Cf and Cr are the cornering stiffness of the front and rear tires, respectively; and L is the vehicle’s wheelbase. 2.Constraints Constraints Ensure that the relative position between the Ego vehicle and surrounding vehicles remains appropriate during the lane-changing process—position constraint: Ensure that the relative position between the Ego vehicle and surrounding vehicles remains appropriate during the lane-changing process—position constraint: In the formula, i and n represent the ID number of surrounding vehicles and the number of surrounding vehicles, respectively; xi,yi is the position coordinate information of the i-th surrounding vehicle, which can be obtained in real time through on-board sensors; Jd represents the real-time distance between the Ego vehicle and surrounding vehicles; Ssafe represents the minimum safe distance between the Ego vehicle and surrounding vehicles during driving. Satisfy the equality constraint equation for changes in vehicle driving states—equation constraint: Satisfy the equality constraint equation for changes in vehicle driving states—equation constraint: The position coordinate information of the future predicted output must fall within the planned feasible region—output constraint: The position coordinate information of the future predicted output must fall within the planned feasible region—output constraint: The vehicle speed and front-wheel steering angle must be controlled within the executable range of the vehicle—control constraint: The vehicle speed and front-wheel steering angle must be controlled within the executable range of the vehicle—control constraint: Through the above analysis, the problem is converted into an optimization solution problem for the objective function with constraints: (11)minutJ=ω1∫t0tfyd−ye2dt+ω2ay2s.t.∑i=1sixe−xi2+ye−yi2≥Ssafext+k+1=Axt+k+But+kxt+k∈X, k=0,…,Nut+k∈U, k=0,…,N−1 By solving this optimization objective with constraints, the control sequence u∗k=v∗kδf∗kT for a future period can be obtained. However, since the system model changes in real time, it cannot be guaranteed that a feasible solution for this optimization objective can be obtained at every moment. Therefore, the optimization objective needs to be processed accordingly. A common and proven effective method is to add a slack variable to the optimization objective:(12)minutJ=ω1∫t0tfyd−ye2dt+ω2ay2+ρε2 In the formula, ρ is the weight coefficient, and ε is the slack variable. 3.Coordinate Conversion of Control Quantities Coordinate Conversion of Control Quantities To achieve the optimal solution for the established multi-objective function and constraint conditions, and in consideration of the vehicle’s driving characteristics, the area that the vehicle can reach at the next moment—predictable based on the vehicle’s speed and heading angle information—can be represented by a sector. For standardized representation and to facilitate the direct solution of control quantities, a conversion method is proposed to convert the 2D coordinate system into a polar coordinate form. This method converts the speed and heading angle (which are the corresponding control quantities) into the radius and rotation angle forms of polar coordinates (Figure 5). Correspondingly, the vehicle speed v is represented as the radius R in polar coordinates, and the front-wheel steering angle δf is represented as the rotation angle θ in polar coordinates. The coordinate relational expressions corresponding to the vehicle’s driving process are as follows:(13)xe=Rcosθye=Rsinθ In this way, the optimal solution of the multi-objective function with constraints is converted into the solution of R and θ in the polar coordinate system. 3.3. Return of the Optimal Control Sequence—Feedback Adjustment After completing the optimal solution of the above formula in each control cycle, a series of control input increments within the control horizon are obtained.ΔUkin,t∗=Δukin,t∗,Δukin,t+1∗,…,Δukin,t+Nc−1∗T According to the basic principle of model predictive control, the first element in the control sequence is used as the actual control input increment acting on the system, that is, ukint=ukint−1+Δukin,t∗. The system executes this control quantity until the next moment. At the new moment, the system re-predicts the output of the next time domain based on the state information, and obtains a new sequence of control increments through the optimization process. This cycle repeats until the system completes the entire control process. 3.4. Curve Fitting of Optimal Discrete Points The discrete points corresponding to the optimal solution obtained from the rolling optimization process are subjected to curve fitting to plan a smooth and feasible trajectory line. To fully ensure the smoothness of the curve and that the curve curvature at the start and end points can meet the consistency of the vehicle’s forward direction, a quintic polynomial is used for least squares fitting. The least squares fitting polynomial (also known as the method of least squares) is a mathematical optimization technique. It finds the best function fit for the data by minimizing the sum of the squares of the errors. Given the function value y1,…,yn of function y=fx at point x1,…,xn, find a polynomial px=a0+a1x+…+amxm∈∏mm+1