A Mean Field Game Integrated MPC-QP Framework for Collision-Free Multi-Vehicle Control

1. Introduction

In recent years, human-robot interactions (HRI) has raised high attention from public [1,2]. Among the HRIs, the field of autonomous driving has witnessed remarkable progress driven by advancements in sensing technologies [3,4,5,6], planning [7,8,9] and control systems [10,11], and machine learning techniques [12,13,14]. Despite these breakthroughs, ensuring safe and efficient interactions among vehicles in complex traffic scenarios remains a formidable challenge. As autonomous vehicles (AVs) are increasingly deployed in mixed traffic environments alongside human-driven vehicles (HDVs) [15], the ability to predict and respond to the actions of surrounding agents becomes crucial for collision avoidance and smooth operation.

Traditional methods that combine artificial potential fields (APF) with game-theoretic models have demonstrated promise in modeling vehicle interactions and preventing collisions [16,17,18]. However, these approaches often result in overly conservative control decisions or fail to accurately capture the nonlinear dynamics inherent in real-world driving. For instance, such methods may lead to unnecessarily cautious maneuvers that compromise traffic efficiency or may not adapt adequately to rapidly changing traffic conditions.

Learning-based methods have gained considerable attention [19,20,21,22,23,24,25] due to their ability to capture complex vehicle behaviors. However, these techniques typically require vast amounts of training data, which demands significant computational resources. Furthermore, since they often have an unseen decision-making process [26], thus their decision-makings lack transparency and interpretability. Recently, large language models (LLMs) also becomes an attracting field, which enables agent to recognize the textual information [27,28]. However, LLMs sometimes are without precision in low-level decision-making, even they can reasonably give a proper high-level commands.

To address these challenges, we propose a novel framework that integrates mean field game (MFG) theory [29] with model predictive control (MPC) integrated with quadratic programming (QP) [30]. In our approach, MFG theory is used to capture the aggregate behavior of surrounding vehicles. Instead of modeling each vehicle individually, MFG approximates the interactions of a large population of agents by a mean field term, which represents the average influence of all vehicles. This not only reduces the complexity of the multi-agent system but also preserves essential interactive dynamics, which are critical for safety in dense traffic environments. On the other hand, MPC provides a receding-horizon framework that continuously updates the control inputs based on real-time state measurements and predicted future behavior. By linearizing the vehicle dynamics around a nominal trajectory and formulating the control problem as a QP, we can efficiently solve the resulting optimization problem even in the presence of constraints and nonlinearities. By doing so, the framework dynamically adjusts the AV’s control inputs in real time, thereby balancing safety and efficiency across various driving tasks. Our approach offers several significant contributions:

• The proposed framework utilizes mean field approximations to capture the aggregate behavior of surrounding vehicles, providing a more accurate and computationally efficient representation of interactive driving scenarios.
• By integrating these mean field predictions into a receding-horizon MPC-QP framework, our framework effectively handles the nonlinear dynamics of vehicles while ensuring real-time computation of optimal control actions.
• Extensive simulation results demonstrate that our framework not only guarantees collision-free operation but also produces smoother trajectories and enhanced overall traffic performance compared to existing game-based approaches.

The remainder of this paper is organized as follows. Section 2 reviews related works and their limitations. Section 3 describes the system model and presents our vehicle dynamics. Section 4 details the proposed MFG integrated MPC-QP framework, including the formulation of the optimization problem and the solution approach. Finally, Section 5 presents simulation results demonstrating the efficacy of the proposed method, and Section 6 concludes the paper.

2. Related Works

2.1. Challenges in Interactive Decision-Making

Interactive decision-making in autonomous driving is particularly challenging due to the highly dynamic and uncertain nature of traffic environments [31,32,33]. Vehicles must continually predict and respond to the behavior of surrounding agents while dealing with nonlinear vehicle dynamics and measurement uncertainties [34,35,36,37]. As the number of interacting vehicles increases, the computational burden associated with modeling each individual interaction becomes prohibitive. Consequently, ensuring safe, efficient, and smooth vehicle maneuvers in dense traffic remains a formidable problem.

2.2. Shortcomings of Current Game-Based Approaches

Game-based approaches have been widely employed to model the strategic interactions among vehicles, providing a structured framework to predict the actions of other drivers. In the case of Stackelberg games [38,39], the AV is modeled as the leader that anticipates the optimal responses of the followers. However, this formulation typically relies on the assumption of perfect rationality, which is not always valid in real-world scenarios, and tends to yield overly conservative decisions that sacrifice traffic efficiency. Consequently, Stackelberg games heavily depend on accurate predictions of other participants’ decisions. Moreover, the hierarchical structure of Stackelberg models may not scale well when multiple vehicles are involved; for instance, an additional game-ordering mechanism is required when using Stackelberg games for multi-vehicle interactions [40].

On the other hand, Nash game formulations treat all vehicles symmetrically by seeking a simultaneous equilibrium [41]. Although Nash approaches capture the mutual influence among agents, they often suffer from the existence of multiple equilibria, complicating the selection of a unique solution in real time. For example, Nash approaches are typically applied only to maintain a game process for a single focused vehicle in a two-vehicle game [42,43], which neglects broader game-based interactions with other vehicles. In addition, the computational complexity associated with solving for a Nash equilibrium in high-dimensional and nonlinear settings can be significant. These limitations in both Stackelberg and Nash formulations have motivated our focus on developing more scalable game-based approaches that offer an interpretable framework for interactive decision-making despite their shortcomings.

MFG theory provides a promising alternative by approximating the aggregate behavior of a large population of vehicles through a mean field term. This approach alleviates the need to model each interaction individually and addresses the scalability issue inherent in traditional game formulations. However, previous works on MFG integrated with autonomous driving fail to conduct simulations among multiple driving scenarios, compare with other game-based approaches, and connect with the control optimization problems [29,44]. By integrating MFG with MPC-QP, our framework effectively captures the collective dynamics of surrounding vehicles while leveraging a receding-horizon optimization scheme that handles nonlinear vehicle dynamics in real time  [45,46,47]. This integration not only mitigates the overly conservative nature of Stackelberg games and the computational complexity of Nash games, but also yields a more flexible and robust solution for interactive driving.

3. System Overview

Framework Description:

Figure 1 provides a schematic illustration of our overall approach. In the left-hand portion, the multi-vehicle scenario is shown, where an AV navigates within a bounded environment alongside several surrounding vehicles (SVs). Potential collision zones are highlighted by overlapping or convergent trajectories. Moving to the central portion, our MFG module is depicted. In this module, the states of all vehicles (except the one being controlled) are aggregated to compute a mean field correction term, $\Delta {\mathbf{x}}_{i,\mathrm{MFG}}$. This correction approximates the collective influence of the SVs, and it is dynamically adjusted based on environmental constraints and historical proximity data, enabling the correction to evolve over time and accurately reflect real-time interactions among vehicles. In the right-hand portion, these MFG predictions are integrated into a MPC formulation, which is solved via QP. The MPC-QP optimization minimizes a cost function—expressed in compact matrix form—that accounts for trajectory tracking, control effort, and safety constraints such as collision and boundary penalties, all subject to the vehicle’s dynamic model. The resulting control commands ensure that the AV can safely and efficiently navigate through complex, multi-participant traffic scenarios.

4. Methodology

We consider a multi-vehicle system composed of one AV and $N-1$ HDVs operating in a bounded environment $\mathcal{D}\subset {\mathbb{R}}^2$, for example, a rectangular area with boundaries at $x=x_{min}$, $x=x_{max}$, $y=y_{min}$, and $y=y_{max}$. The environment constraints are integrated into our control design to ensure that vehicles remain within safe operational limits.

For each vehicle i, the state is defined as

$${\mathbf{x}}_i\left(k\right)=\left[\begin{array}{c}{x}_i\left(k\right)\\ y_i\left(k\right)\\ v_i\left(k\right)\\ {\theta }_i\left(k\right)\end{array}\right]\in {\mathbb{R}}^4,$$

(1)

and the control input is

$${\mathbf{u}}_i\left(k\right)=\left[\begin{array}{c}{a}_i\left(k\right)\\ {\omega }_i\left(k\right)\end{array}\right]\in {\mathbb{R}}^2.$$

(2)

The dynamics follow the bicycle model:

$$\begin{array}{cc}\hfill x_i(k+1)& =x_i\left(k\right)+\Delta t{v}_i\left(k\right)cos{\theta }_i\left(k\right),\hfill \\ \hfill y_i(k+1)& =y_i\left(k\right)+\Delta t{v}_i\left(k\right)sin{\theta }_i\left(k\right),\hfill \\ \hfill v_i(k+1)& =v_i\left(k\right)+\Delta t{a}_i\left(k\right),\hfill \\ \hfill {\theta }_i(k+1)& ={\theta }_i\left(k\right)+\Delta t{\omega }_i\left(k\right).\hfill \end{array}$$

(3)

The joint state vector for all vehicles is

$$\mathbf{X}\left(k\right)=\left[\begin{array}{c}{\mathbf{x}}_{\mathrm{AV}}\left(k\right)\\ {\mathbf{x}}_{{\mathrm{HDV}}_1}\left(k\right)\\ ⋮\\ {\mathbf{x}}_{{\mathrm{HDV}}_{N-1}}\left(k\right)\end{array}\right]\in {\mathbb{R}}^{4N}.$$

(4)

4.1. Interactive Prediction via MFG

To capture vehicle interactions, the instantaneous cost for vehicle i is defined as

$${\ell }_i({\mathbf{x}}_i\left(k\right),{\mathbf{u}}_i\left(k\right))={\left({\mathbf{x}}_i\left(k\right)-{\mathbf{x}}_{i,\mathrm{ref}}\left(k\right)\right)}^{\top }{\mathbf{Q}}_i\left({\mathbf{x}}_i\left(k\right)-{\mathbf{x}}_{i,\mathrm{ref}}\left(k\right)\right)+{\mathbf{u}}_i{\left(k\right)}^{\top }{\mathbf{R}}_i{\mathbf{u}}_i\left(k\right),$$

(5)

where ${\mathbf{Q}}_i\in {\mathbb{R}}^{4\times 4}$ and ${\mathbf{R}}_i\in {\mathbb{R}}^{2\times 2}$ are positive-definite matrices. In addition, to capture the influence of surrounding vehicles and potential collision risks, we define an interaction cost

$${\phi }_i\left({\mathbf{x}}_i\left(k\right),{\left\{{\mathbf{x}}_j\left(k\right)\right\}}_{j\ne i}\right)=\sum _{j\ne i}{\varphi }_{ij}\mathbb{I}\left\{\parallel {\mathbf{x}}_i\left(k\right)-{\mathbf{x}}_j{\left(k\right)\parallel }_2<d_{\mathrm{safe}}\right\},$$

(6)

and a collision penalty term

$${\phi }_{\mathrm{col},i}\left(k\right)=\sum _{j\ne i}{\psi }_{ij}\sigma \left(d_{\mathrm{safe}}-\parallel {\mathbf{x}}_i\left(k\right)-{\mathbf{x}}_j\left(k\right)\parallel \right),$$

(7)

where $\sigma (·)$ is a smooth penalty function. Thus, the total cost over the horizon T is

$$J_i=\sum _{k=0}^{T-1}\left[{\ell }_i({\mathbf{x}}_i\left(k\right),{\mathbf{u}}_i\left(k\right))+{\phi }_i\left({\mathbf{x}}_i\left(k\right),{\left\{{\mathbf{x}}_j\left(k\right)\right\}}_{j\ne i}\right)+{\phi }_{\mathrm{col},i}\left(k\right)\right].$$

(8)

To reduce the complexity of modeling individual pairwise interactions in multi-participant scenarios, our framework employs MFG theory. In our implementation, each vehicle contributes to a density field over the road surface. The contribution of vehicle i is modeled as a Gaussian function:

$${\rho }_i(x,y)=I_{i}exp\left(-{\displaystyle \frac{{(x-x_i)}^2}{2{\sigma }_{x,i}^2}}-{\displaystyle \frac{{(y-y_i)}^2}{2{\sigma }_{y,i}^2}}\right),$$

(9)

where $I_i$ is the intensity and ${\sigma }_{x,i}$, ${\sigma }_{y,i}$ denote the spatial spreads. The overall density field is computed as

$$\rho (x,y)=\sum _{i=1}^N{\rho }_i(x,y).$$

(10)

The MFG module computes an aggregate correction term by applying a mean field operator:

$$\Delta {\mathbf{x}}_{i,\mathrm{MFG}}\left(k\right)=\mathcal{F}\left({\left\{{\mathbf{x}}_j\left(k\right)\right\}}_{j\ne i}\right),$$

(11)

which is then used to update the reference state:

$${\mathbf{x}}_{i,\mathrm{ref}}\left(k\right)={\mathbf{x}}_{i,\mathrm{nom}}\left(k\right)+\Delta {\mathbf{x}}_{i,\mathrm{MFG}}\left(k\right).$$

(12)

This process enables the controller to dynamically adjust for interactive effects based on both the real-time density field and environmental constraints.

Figure 2 illustrates our multi-vehicle simulation results at four distinct key times. Each row of subplots corresponds to one of these times, capturing significant moments in the simulation, such as early maneuvers or congested intervals. In every row, the left subplot depicts the traffic scene, where each vehicle is rendered as a colored polygon, sized and oriented according to its real-world dimensions and heading. These polygons highlight potential collision zones by overlapping or converging.

Moving to the right, the next three columns show the mean field game (MFG) density fields at the current time and two future offsets (for example, $t+1.0\mathrm{s}$ and $t+1.5\mathrm{s}$). These density fields are visualized using a color map, with warmer regions indicating higher vehicle density and cooler regions indicating lower density. By comparing rows and columns, one can observe how congested regions emerge, shift, or dissipate based on each vehicle’s movement and interaction.

The integration of these density fields with the traffic scenes underscores how our MFG module predicts and tracks the evolving distribution of vehicles in real time. The AV leverages these density predictions within our MPC-QP control framework, thereby adapting its trajectory and control actions to avoid high-density or high-risk areas. This dynamic approach ensures that the AV can safely and efficiently navigate through complex, multi-participant driving environments.

4.2. MPC-QP Formulation with Environmental and Safety Constraints

The MPC problem for each vehicle is formulated as

$$\underset{\left\{{\mathbf{u}}_i\left(k\right)\right\}}{min}\sum _{k=0}^{T-1}\left[\parallel {\mathbf{x}}_i\left(k\right)-{\mathbf{x}}_{i,\mathrm{ref}}{\left(k\right)\parallel }_{{\mathbf{Q}}_i}^2+{\parallel {\mathbf{u}}_i\left(k\right)\parallel }_{{\mathbf{R}}_i}^2\right],$$

(13)

subject to the nonlinear dynamics (3) and the environmental constraint

$${\mathbf{x}}_i\left(k\right)\in \mathcal{D},\forall k.$$

(14)

Additionally, vehicle dynamic constraints such as acceleration and steering limits are enforced.

To solve (13) efficiently, we linearize the dynamics about a nominal trajectory

$\{{\mathbf{x}}_i^{*}\left(k\right),{\mathbf{u}}_i^{*}\left(k\right)\}$:

$$\delta {\mathbf{x}}_i(k+1)={\mathbf{A}}_i\left(k\right)\delta {\mathbf{x}}_i\left(k\right)+{\mathbf{B}}_i\left(k\right)\delta {\mathbf{u}}_i\left(k\right),$$

(15)

with

$${\mathbf{A}}_i\left(k\right)={\displaystyle \frac{\partial f}{\partial {\mathbf{x}}_i}}{|}_{({\mathbf{x}}_i^{*}\left(k\right),{\mathbf{u}}_i^{*}\left(k\right))},{\mathbf{B}}_i\left(k\right)={\displaystyle \frac{\partial f}{\partial {\mathbf{u}}_i}}{|}_{({\mathbf{x}}_i^{*}\left(k\right),{\mathbf{u}}_i^{*}\left(k\right))}.$$

(16)

The QP is then formulated as

$$\underset{\delta \mathbf{U}}{min}{\displaystyle \frac{1}{2}}\delta {\mathbf{U}}^{\top }\mathbf{H}\delta \mathbf{U}+{\mathbf{g}}^{\top }\delta \mathbf{U},$$

(17)

where the decision variable is

$$\delta \mathbf{U}={\left[\begin{array}{cccc}\delta {\mathbf{u}}_i{\left(0\right)}^{\top }& \delta {\mathbf{u}}_i{\left(1\right)}^{\top }& \cdots & \delta {\mathbf{u}}_i{(T-1)}^{\top }\end{array}\right]}^{\top }.$$

The Hessian matrix $\mathbf{H}$ is defined as

$$\mathbf{H}=\sum _{k=0}^{T-1}\left({\mathbf{B}}_i{\left(k\right)}^{\top }{\mathbf{Q}}_f{\mathbf{B}}_i\left(k\right)+{\mathbf{R}}_i\right),$$

(18)

where ${\mathbf{Q}}_f\in {\mathbb{R}}^{4\times 4}$ is the terminal cost weight. The gradient vector is

$$\mathbf{g}=\sum _{k=0}^{T-1}{\mathbf{B}}_i{\left(k\right)}^{\top }{\mathbf{Q}}_f\left({\mathbf{A}}_i\left(k\right)\delta {\mathbf{x}}_i\left(k\right)\right).$$

(19)

Each term in (18) reflects the propagation of control variations through the linearized dynamics, while (19) captures the deviation from the desired trajectory.

The optimal control correction is computed as

$$\Delta {\mathbf{u}}_i\left(k\right)=-{\mathbf{H}}^{-1}\mathbf{g},$$

(20)

and the final control input is updated via

$${\mathbf{u}}_i\left(k\right)={\mathbf{u}}_i^{*}\left(k\right)+\Delta {\mathbf{u}}_i\left(k\right).$$

(21)

Our integrated framework combines the MFG prediction, MPC-QP optimization, and safety and environmental constraints into a unified control process. In every receding-horizon cycle, the following steps are executed:

Algorithm 1: Extended MFG Integrated MPC-QP Algorithm with Environmental and Safety Constraints

Input: Initial joint state $\mathbf{X}\left(0\right)$, prediction horizon T, nominal trajectories $\{{\mathbf{x}}_i^{*}\left(k\right),{\mathbf{u}}_i^{*}\left(k\right)\}$ for $i=1,\dots ,N$, and environment region $\mathcal{D}$. for$k=0$ to $T-1$ do    for each vehicle $i\in \{1,\cdots ,N\}$ do      Step 1: MFG Prediction and Reference Update. Compute the mean field correction $\Delta {\mathbf{x}}_{i,\mathrm{MFG}}\left(k\right)$ using the operator $\mathcal{F}$ on the set of states ${\left\{{\mathbf{x}}_j\left(k\right)\right\}}_{j\ne i}$, and update the reference state as ${\mathbf{x}}_{i,\mathrm{ref}}\left(k\right)={\mathbf{x}}_{i,\mathrm{nom}}\left(k\right)+\Delta {\mathbf{x}}_{i,\mathrm{MFG}}\left(k\right)$.      Step 2: Boundary Penalty. Evaluate the boundary penalty ${\phi }_{\mathrm{bdry},i}\left(k\right)$ based on the proximity of ${\mathbf{x}}_i\left(k\right)$ to the boundaries of $\mathcal{D}$.      Step 3: Reference Update. Update the reference state as $${\mathbf{x}}_{i,\mathrm{ref}}\left(k\right)={\mathbf{x}}_{i,\mathrm{nom}}\left(k\right)+\Delta {\mathbf{x}}_{i,\mathrm{MFG}}\left(k\right)+{\phi }_{\mathrm{bdry},i}\left(k\right).$$      Step 4: Dynamics Linearization. Linearize the dynamics at the nominal point $({\mathbf{x}}_i^{*}\left(k\right),{\mathbf{u}}_i^{*}\left(k\right))$ to obtain the Jacobians: $${\mathbf{A}}_i\left(k\right)={\displaystyle \frac{\partial f}{\partial {\mathbf{x}}_i}}{|}_{({\mathbf{x}}_i^{*}\left(k\right),{\mathbf{u}}_i^{*}\left(k\right))},{\mathbf{B}}_i\left(k\right)={\displaystyle \frac{\partial f}{\partial {\mathbf{u}}_i}}{|}_{({\mathbf{x}}_i^{*}\left(k\right),{\mathbf{u}}_i^{*}\left(k\right))}.$$      Step 5: Compute State Deviation. Calculate the deviation $\delta {\mathbf{x}}_i\left(k\right)={\mathbf{x}}_i\left(k\right)-{\mathbf{x}}_i^{*}\left(k\right)$.      Step 6: QP Matrix Formulation. For the remaining horizon, form the Hessian matrix and gradient vector by accumulating: $$\mathbf{H}=\sum _{l=k}^{T-1}\left({\mathbf{B}}_i{\left(l\right)}^{\top }{\mathbf{Q}}_f{\mathbf{B}}_i\left(l\right)+{\mathbf{R}}_i\right)$$ and $$\mathbf{g}=\sum _{l=k}^{T-1}{\mathbf{B}}_i{\left(l\right)}^{\top }{\mathbf{Q}}_f\left({\mathbf{A}}_i\left(l\right)\delta {\mathbf{x}}_i\left(l\right)\right),$$ where ${\mathbf{Q}}_f$ denotes the terminal cost weight matrix.      Step 7: QP Solution. Solve the quadratic program to obtain the optimal control correction: $$\Delta {\mathbf{u}}_i\left(k\right)=-{\mathbf{H}}^{-1}\mathbf{g}.$$      Step 8: Control Update. Update the control input: $${\mathbf{u}}_i\left(k\right)={\mathbf{u}}_i^{*}\left(k\right)+\Delta {\mathbf{u}}_i\left(k\right).$$      Step 9: Safety Check. Optionally, if a collision is predicted (e.g., if ${min}_{j\ne i}\parallel {\mathbf{x}}_i\left(k\right)-{\mathbf{x}}_j\left(k\right)\parallel <d_{\mathrm{safe}}$), augment the control correction by applying an additional penalty term (embedded in the cost through ${\phi }_{\mathrm{col},i}\left(k\right)$).    end for    Step 10: State Propagation. Apply the computed control inputs $\left\{{\mathbf{u}}_i\left(k\right)\right\}$ and propagate the vehicle states using the dynamics (). Additionally, enforce the constraint that ${\mathbf{x}}_i(k+1)\in \mathcal{D}$ by projecting any state outside $\mathcal{D}$ back onto the feasible region. end for Step 11: Shift Horizon. Update the nominal trajectories $\{{\mathbf{x}}_i^{*}\left(k\right),{\mathbf{u}}_i^{*}\left(k\right)\}$ by shifting the horizon forward and repeat the process.

Figure 3. Flowchart of the integrated MFG-MPC-QP framework for two scenarios: lane change and intersection negotiation.

Figure 3. Flowchart of the integrated MFG-MPC-QP framework for two scenarios: lane change and intersection negotiation.

Algorithm Explanation: At each time step, every vehicle first computes a mean field correction $\Delta {\mathbf{x}}_{i,\mathrm{MFG}}\left(k\right)$ that aggregates the influence of all other vehicles. This term, along with a boundary penalty ${\phi }_{\mathrm{bdry},i}\left(k\right)$ enforcing environmental constraints, updates the reference state. The vehicle dynamics are then linearized about a nominal trajectory to obtain the Jacobian matrices ${\mathbf{A}}_i\left(k\right)$ and ${\mathbf{B}}_i\left(k\right)$. Using these matrices, the QP problem is constructed via the Hessian $\mathbf{H}$ and gradient $\mathbf{g}$, which encapsulate both tracking and control effort costs. The QP is solved to yield an optimal control correction $\Delta {\mathbf{u}}_i\left(k\right)$, and the final control input is updated. This process is executed in a receding-horizon fashion to continuously adapt to real-time interactions and ensure safety, including collision avoidance via the penalty ${\phi }_{\mathrm{col},i}\left(k\right)$.

The following flowchart summarizes our approach for two typical scenarios: lane change and intersection negotiation. The flowchart is drawn using TikZ with SCI-inspired colors.The flowchart (Figure ) illustrates the major steps of our integrated framework.

• Start: This is the initialization step where the system loads all sensor data, initial vehicle states, and environmental parameters. The controller is set up to begin the receding-horizon control loop.
• Sensing & State Estimation: In this step, all vehicles (both the AV and HDVs) gather sensor data (e.g., from LiDAR, cameras, radar) to estimate their current states (position, velocity, heading). Accurate state estimation is critical for reliable prediction and control.
• Compute MFG Correction: The MFG module processes the states of all vehicles (except the one under consideration) to compute an aggregate correction term, $\Delta {\mathbf{x}}_{i,\mathrm{MFG}}$. This term approximates the collective influence of surrounding vehicles, reducing the complexity of pairwise interaction modeling. It is then used to update the nominal reference state, so that the adjusted reference becomes

  $${\mathbf{x}}_{i,\mathrm{ref}}={\mathbf{x}}_{i,\mathrm{nom}}+\Delta {\mathbf{x}}_{i,\mathrm{MFG}},$$

  thereby ensuring that the control policy dynamically incorporates real-time interactive effects.
• Update Reference: The nominal reference state ${\mathbf{x}}_{i,\mathrm{nom}}$ is updated by incorporating the MFG correction along with a boundary penalty ${\phi }_{\mathrm{bdry}}$ that accounts for the proximity to the environment boundaries. This yields the new reference state:

  $${\mathbf{x}}_{i,\mathrm{ref}}={\mathbf{x}}_{i,\mathrm{ref}}+{\phi }_{\mathrm{bdry}}.$$
• Linearize Dynamics: The vehicle’s nonlinear dynamics (given by the bicycle model) are linearized around the nominal trajectory $({\mathbf{x}}_i^{*},{\mathbf{u}}_i^{*})$. The Jacobian matrices ${\mathbf{A}}_i$ and ${\mathbf{B}}_i$ capture the sensitivity of the state with respect to state and control inputs, respectively.
• Formulate QP Matrices: Using the linearized model, the Hessian matrix $\mathbf{H}$ and gradient vector $\mathbf{g}$ for the QP are constructed. Specifically, $\mathbf{H}$ is computed by accumulating the terms

  $$\mathbf{H}=\sum _{k=0}^{T-1}\left({\mathbf{B}}_i{\left(k\right)}^{\top }{\mathbf{Q}}_f{\mathbf{B}}_i\left(k\right)+{\mathbf{R}}_i\right),$$

  and the gradient is given by

  $$\mathbf{g}=\sum _{k=0}^{T-1}{\mathbf{B}}_i{\left(k\right)}^{\top }{\mathbf{Q}}_f\left({\mathbf{A}}_i\left(k\right)\delta {\mathbf{x}}_i\left(k\right)\right),$$

  where ${\mathbf{Q}}_f$ is the terminal cost weight matrix. These matrices encapsulate the tracking errors, control efforts, and indirectly include collision penalties from ${\phi }_{\mathrm{col}}$.
• Solve QP: The quadratic programming problem is solved to obtain the optimal control correction:

  $$\Delta {\mathbf{u}}_i=-{\mathbf{H}}^{-1}\mathbf{g}.$$
  This step yields the necessary adjustment to the nominal control inputs to reduce the overall cost.
• Update Control: The control input is updated by adding the correction term to the nominal control:

  $${\mathbf{u}}_i={\mathbf{u}}_i^{*}+\Delta {\mathbf{u}}_i.$$
  This new control command is what is applied to the vehicle.
• Propagate Dynamics: The updated control inputs are applied to the nonlinear dynamics model to propagate the vehicle states forward. Additionally, environmental constraints are enforced to ensure that all vehicle states remain within the designated region $\mathcal{D}$.
• Scenario Decision: A decision node determines the driving scenario (e.g., lane change or intersection negotiation) based on the current state and predicted vehicle interactions. This decision may trigger additional scenario-specific maneuvers.
• Execute Maneuver: Depending on the scenario decision, the system executes the corresponding maneuver—either a lane change or an intersection negotiation—adjusting the control inputs as necessary.
• Shift Horizon: After control actions are applied and states are updated, the prediction horizon is shifted forward. The nominal trajectories are updated, and the process repeats in a receding-horizon fashion.
• End: The control loop terminates when the driving task is complete or when the specified time horizon is reached.

5. Experimental Evaluation

The simulations were conducted to evaluate the safety, stability, and efficiency of the proposed method. They were executed on a computer running Ubuntu 18.04.6 LTS, equipped with a 12th-generation, 16-thread Intel® Core™ i5-12600KF CPU, an NVIDIA GeForce RTX 3070Ti GPU, and 16 GB of RAM. All simulation results were generated using MATLAB R2024b.

To verify the effectiveness of our proposed MFG-based MPC-QP, we designed four distinct simulation scenarios that reflect typical interactive driving involving both HDVs and AVs. In the first scenario, the AV initiates interactive driving with surrounding HDVs in an irregular environment. In the second scenario, the AV navigates a two-lane highway while two surrounding AVs are positioned around the host AV; here, the AV must perform a lane change while maintaining a safe distance from the surrounding AVs. In the third scenario, the number of surrounding AVs is increased to three, with all other conditions remaining the same as in the second scenario. In the fourth scenario, the AV is required to reach a target point across an intersection while the surrounding HDVs follow fixed routes. To underscore the superior performance of the proposed method, we compared it against other popular benchmark algorithms in the last three scenarios.

5.1. Verification for Scenario 1

In this first scenario, Figure 4 illustrates our first test scenario in a top-down view. The environment is bounded by fences in the upper and lower sections, with decorative trees placed near each corner. In the lower-left portion, the AV is shown traveling from left to right, indicated by the dashed road boundary. Several HDVs appear at different positions and orientations: one moves vertically down near the upper-left fence, another navigates horizontally from the right fence toward the center, and an additional HDV heads northward near the middle of the scene. This setup emphasizes potential crossing paths and showcases how the AV and HDVs share the same road space while maneuvering around obstacles and boundaries.

Figure 5(a) displays the position of each vehicle at discrete time steps, plotting their x-y coordinates and linking them to reveal their paths over time. Vehicles moving primarily in the vertical direction appear as nearly straight vertical lines, whereas those traveling horizontally show up as elongated horizontal tracks. The intersection of these paths highlights potential conflict points, yet collision avoidance ensures no overlapping positions occur at the same time.

Figure 5(b) shows the corresponding density field at several key instants. The color scale transitions from yellow/white to red . Each vehicle contributes a Gaussian-like distribution to the field, which is then summed and normalized. Over the progression of time frames, one can observe how the distribution evolves, reflecting changes in vehicle positions and velocities. This time-evolving density field supports collision-free navigation by allowing the decision-making module to anticipate and react to congested zones before they become hazardous.

Figure 6 presents the evolution of two key indicators over a six-second window: the average speed of all vehicles and the cumulative collision count. The horizontal axis (T) indicates the discrete time steps in seconds. As the simulation progresses, each second’s average speed is computed by taking the mean of the current speeds across all vehicles. Meanwhile, the collision count is accumulated whenever two vehicles come within a specified safety distance. This plot demonstrates the system can maintain stable average speeds while keeping the collision count at zero. A gradual decrease or fluctuation in speed may occur as vehicles maneuver to avoid potential conflicts, underscoring the trade-off between efficiency and safety.

Figure 7 shows a two-lane road scene with dotted lane markers. The host AV is positioned in the lower lane, while another AV and an additional vehicle occupy the upper lane. The gray rectangle represents the drivable region, and each lane is bounded by dashed lines. The host vehicle’s objective is to switch from its current lane to the target lane, coordinating with the other vehicles to avoid collisions or abrupt maneuvers. By adjusting speed and lateral motion, the host vehicle ensures that its lane change occurs safely, even in the presence of adjacent vehicles in the upper lane. This setup underscores how lane-changing decisions must account for the trajectories of surrounding vehicles, balancing efficiency and collision avoidance in real time.

Figure 8 shows the x-y trajectories of three autonomous vehicles (each color-coded: blue, green, and red) under three different approaches. In the left panel, our method produces smooth and collision-free paths without any abrupt or backward motion. Each vehicle converges toward its desired lane or target position in a balanced way, demonstrating how the MFG correction effectively captures multi-vehicle interactions, and the MPC framework ensures efficient, real-time trajectory adjustments. By contrast, the center panel APF+Stackelberg and right panel APF+Nash reveal that both methods sometimes force a vehicle to move backward to avoid collisions, which can be a poor choice in real traffic. In the Stackelberg scenario, the leading vehicle receives priority, causing the following vehicles to execute sudden lateral or even backward maneuvers to maintain safety. Meanwhile, in the Nash formulation, each vehicle optimizes its own objective simultaneously, leading to oscillatory negotiations and occasional drive-back motions for collision avoidance. Although neither approach results in actual collisions, such backward driving maneuvers are undesirable and highlight a lack of coordination.

These results emphasize that our method not only avoids collisions but also maintains forward progress and more natural trajectories for all vehicles. By modeling the collective effect of surrounding vehicles through the mean field, our method ensures smooth lane transitions without requiring any one vehicle to drive back to prevent collisions, thereby offering a more realistic and efficient solution.

Figure 9 illustrates a three-lane road environment, labeled from bottom to top as an irrelevant lane, a current lane, and a target lane. The host autonomous vehicle (Host AV) initially occupies the middle (current) lane. Meanwhile, two additional AVs appear: one in the irrelevant lane and another in the target lane. Each vehicle is represented by a colored polygon corresponding to its approximate size and heading. Dashed lane markers define the boundaries of each lane. The goal for the host AV is to transition from its current lane to the target lane, coordinating its lateral movement with respect to both the AV in the target lane and the one in the irrelevant lane. This setup examines how the host AV can safely execute a lane change in the presence of other autonomous vehicles moving at different speeds and positions, emphasizing multi-lane coordination and collision avoidance in real time.

Figure 10 shows the x-y trajectories of four AVs, each color-coded to represent a distinct vehicle, under three different approaches. The left panel shows our method, in which all vehicles quickly settle into collision-free paths, exhibiting smooth transitions with minimal lateral shifts or oscillations. The center panel, based on APF and a Stackelberg formulation, occasionally forces certain vehicles to make abrupt course corrections or yield heavily to a leader, leading to more noticeable lateral or even retrograde maneuvers. Meanwhile, the right panel, employing an APF+Nash approach, highlights simultaneous decision-making among all vehicles, which can result in repeated small deviations or slow convergence as vehicles iteratively react to one another.

Our method experiences no collisions, though the trajectory smoothness has slight fluctuations. This is because our method balances efficiency, safety, and comfort by accounting for collective interactions via the mean field, ensuring forward progress without drastic maneuvers. In contrast, the Stackelberg and Nash approaches, while maintaining smooth curves, may involve the host AV having conflicts with its follower AV. These outcomes emphasize the advantages of MFG+MPC in achieving safer and more stable multi-vehicle coordination.

Figure 11 presents six bar charts that summarize key metrics across ten runs for each scenario, where every AV’s initial longitudinal position is randomly shifted by up to $\pm 10\%$. In the upper-left bar chart, the average computation time is displayed for each method, indicating how quickly each approach computes control actions in real time. MFG+MPC typically achieves moderate or lower values, demonstrating efficient iterative updates, whereas the other two methods exhibit more efficient real-time computation.

For the collisions, our method consistently maintains near-zero or zero collisions in both scenarios, suggesting robust avoidance strategies when initial positions deviate from nominal. APF+Stackelberg and APF+Nash occasionally record non-zero collision counts or exhibit spikier bars, indicating that their handling of unexpected initial placements can be less stable. The comfort measure, derived from jerk-based calculations, highlights how smoothly each method handles accelerations and steering. While our method maintains moderate comfort values that do not drastically fluctuate, the other methods may produce slightly higher comfort scores, sometimes indicating more abrupt maneuvers or poorer collision avoidance.

Further metrics, such as lane-change duration or minimum inter-vehicle distance, also show our ability to balance progress and safety. APF+Stackelberg can favor the leader at the expense of follower vehicles, whereas APF+Nash, though collision-free in most runs, may exhibit less predictable lane-change times due to simultaneous multi-vehicle negotiation. The final bar chart illustrating average speed indicates that MFG+MPC generally preserves moderate forward velocity without resorting to excessive slowdowns. By contrast, the other methods occasionally reduce speed more aggressively when reacting to unexpected positions.

Figure 12 illustrates a multi-way intersection environment, where an AV approaches from the bottom-right lane, heading toward a target port. Several HDVs appear from different directions, including one traveling north-to-south along the upper boundary and another moving west-to-east in the left portion of the intersection. The gray, dashed lines indicate road boundaries, and fences or decorative greenery occupy the corners. Each vehicle is represented by a color-coded rectangle approximating its size and heading. This setup tests the AV’s ability to coordinate lane changes and intersection maneuvers in the presence of crossing HDVs, emphasizing collision avoidance, efficient navigation, and real-time adaptation to oncoming traffic.

Figure 13 shows an intersection scenario in which four vehicles including one AV and three HDVs. In the left panel, our method constrains all vehicles within the intersection bounds, achieving smooth turns or straight crossings without any vehicle drifting outside the designated road area. In contrast, the center and right panels depict the results of the APF+Stackelberg and APF+Nash methods, respectively. Although no collisions occur, one can observe instances where a vehicle’s path strays beyond the dashed boundary lines. This ramp-out behavior arises because neither of these methods strictly enforces road constraints; vehicles focus on potential field minimization or equilibrium-seeking rather than adhering to explicit lane boundaries. As a result, HDVs may execute large lateral maneuvers or fail to respect the intersection’s geometric limits. These comparisons emphasize that our method naturally incorporates control constraints and road boundaries, keeping vehicles within the intersection and producing more realistic maneuvers. Meanwhile, APF-based methods risk suboptimal or unrealistic trajectories when attempting to avoid collisions in tight spaces without explicit boundary constraints.

Figure 14 presents three subplots depicting the autonomous vehicle’s speed, acceleration, and its minimum distance to a follower human-driven vehicle over a five-second interval. The yellow curve represents our method, whereas the blue and orange curves correspond to the APF+Stackelberg and APF+Nash methods, respectively.

In the top subplot, our method shows a gradual increase in speed, resulting in smoother changes than the other two approaches. This reflects how the MFG scheme carefully balances forward progress and collision avoidance, avoiding abrupt accelerations. The middle subplot indicates that our acceleration profile remains comparatively stable, whereas the other methods may exhibit more sudden adjustments or even brief periods of zero or negative acceleration.

The bottom subplot depicts the smallest inter-vehicle distance between the autonomous vehicle and the follower HDV. While our method briefly attains a lower separation than the other approaches, this occurs at a point where the host vehicle is leading and the HDV behind it is assumed to maintain a fixed route rather than adapt for safety. In a real-world scenario, that HDV would naturally adjust its speed or lateral position to preserve a safer following distance. Consequently, the lower separation does not indicate a dangerous event but rather an artifact of our assumption that human-driven vehicles do not change their paths in response to the host vehicle’s maneuvers. These results highlight how our MFG-based control ensures a smooth speed profile while relying on follower vehicles to adjust their behavior in practical driving conditions.

6. Conclusion

In this paper, we have presented and evaluated an MFG framework integrated with an MPC-QP approach. Through multiple test scenarios and comparative studies against other potential field–based methods, we observed that our method maintains collision-free motion with smoother speed profiles and more stable lane-change or intersection maneuvers. In particular, the MFG component effectively captures the collective influence of surrounding vehicles, while the MPC formulation ensures real-time adaptability and efficient trajectory planning. These combined advantages enable our method to outperform benchmarks that lack explicit boundary constraints or rely on hierarchical or simultaneous equilibria without mean field modeling. Simulation results indicate that our method provides a promising, robust solution for multi-vehicle coordination, whether for AVs or HDVs in different driving scenarios. In future work, an MFG system could be designed for mixed traffic flows that include both HDVs and AVs, and various driving styles could be incorporated for further verification.