Integrating Flow Matching and Model Predictive Control for Safe Robot Navigation

Introduction

Recent advances in robot control for complex tasks have been driven by increasingly capable machine learning algorithms and the availability of high-performance computing resources [1, 2, 3]. Machine learning enables expressive control policies that often outperform classical methods in high-dimensional and multimodal settings, particularly when trained via imitation or reinforcement learning.

To handle multimodal data distributions, recent work has applied deep generative models based on diffusion [4] and flow matching [5, 6], originally developed for image synthesis, to robotics domains including imitation learning [7, 8], offline RL [9], grasping [10, 11], and motion planning [12, 13, 14, 15, 16, 17, 18]. These models are particularly attractive for their ability to represent complex, multimodal trajectory distributions and to generate diverse candidate motions from demonstration data, including different paths from the same initial point. In this article, we use a variant of flow matching the shortcut model [19], which offers fast sampling [20].

One central challenge in learning policies from demonstrations is the lack of guarantees for collision avoidance during deployment. For example, a generative model trained on human-demonstrated trajectories may produce paths that become unsafe when new obstacles are introduced or when dynamics or constraints (e.g., velocity limits) change. Nevertheless, the data remains valuable as a reference for a lower-level controller that can safely adapt to it.

To this end, we combine the deep generative planner with a model predictive path-following controller (MPPFC) [21, 22, 23], which tracks and locally reshapes the generated path to ensure safety, cf. Figure 1. Unlike tracking model predictive control (MPC) [24, 25], MPPFC treats path progression and velocity as optimization variables, enabling adaptive motion profiles and reactive behavior in dynamic environments. By enforcing input and state constraints, including collision avoidance and actuator limits, MPPFC ensures constraint satisfaction even if the reference path must be temporarily deviated from.

Image not extracted

Previous works have used simpler generative models, focused on unimodal settings and control-theoretic guarantees [26, 27, 28, 29, 30]. Since combining diffusion and flow matching generative models with predictive control offers a large potential [31], this has been explored by a series of works: [32] uses a diffusion model to learn multi-step predictions and warm-start MPC optimizations; [33] parameterizes MPC feedback using diffusion; [34] applies diffusion models to solve nonlinear MPC problems; and [35, 36] explore diffusion rollouts for model predictive path integral control (MPPIC). Relatedly, [31] uses diffusion to generate subgoals for long-horizon control with MPPIC. However, because of the underlying MPC methods used, these works do not guarantee safety or path-following. In contrast, our approach combines flow-matching for reference generation with MPPFC for local safety and reference tracking, overcoming the limitations of the current methods. By using MPPFC to track and refine paths generated via flow matching, we ensure constraint satisfaction — particularly collision avoidance — and enable real-time adaptation. In this sense, the MPPFC shows similarities to predictive safety filters [37, 38, 39]. However, in contrast to standard safety filters that modify unsafe inputs to ensure safety, we directly apply MPPFC as the main controller, using its structure to dynamically shape both path execution and progression.

The main contributions of this work are: (1) a framework that combines a flow-matching-based path generator, learned from human demonstrations, with MPPFC to enable constraint satisfaction and collision avoidance during execution; (2) a bidirectional coupling in which flow-generated paths guide MPPFC and its predictions bias the flow planner during eventtriggered re-planning; (3) a systematic evaluation demonstrating collision-free path-following in environments with multiple obstacle configurations not observed during training. The remainder of the paper is structured as follows: Section II outlines the problem setting. Section III introduces the generative model used to learn path distributions from demonstrations. Section IV details the MPPFC formulation and its integration with the planner. § 5 presents simulation results, and Section VI concludes the paper.

Problem Formulation

Consider a dynamical system governed by known continuous-time dynamics:

Here, $x \in\mathcal{X} \subseteq\mathbb{R}^{n_x}$ represents the system state, $u \in\mathcal{U} \subseteq\mathbb{R}^{n_u}$ is the control input, and $y \in\mathcal{Y} \subseteq\mathbb{R}^{n_y}$ denotes the system output. The function $f : \mathbb{R}^{n_x} \times\mathbb{R}^{n_u} \to\mathbb{R}^{n_x}$ defines the system dynamics, while $h : \mathbb{R}^{n_x} \to\mathbb{R}^{n_y}$ defines the output map. The initial state at time $t_0$ is given by $x_0 \in\mathbb{R}^{n_x}$.

Given a global reference path, the control objective is to navigate the system through a cluttered environment while strictly satisfying system constraints and avoiding collisions. Rather than relying on classical sampling-based planners (e.g., RRT [40]), we assume access to expert task demonstrations that encode nuanced human preferences for spatial navigation. To model and reproduce these demonstrations, we learn a flow-matching-based generative planner.

Given start and goal configurations, candidate paths are generated by sampling from this learned flow-matching model. Formally, a path is defined as a geometric curve $\rho: \mathcal{C}_{\theta} \to\mathcal{Y}$, $\theta\mapsto y = \rho(\theta)$ in the system’s output space, parametrized by a scalar path parameter $\theta$. We represent each path as a linear combination of a finite set of fixed-order B-splines, training the flow-matching model directly on the governing coefficients.

While the generative planner provides a strong, obstacle-aware prior based on demonstrations, it is not strictly tasked with enforcing real-time constraints or active collision avoidance using all current local information. Instead, this responsibility is systematically delegated to a lower-level controller that incorporates this information.

Specifically, the generated paths serve as reference priors for parallel instances of an MPPFC, each configured with a different prediction horizon. The predictive controller seamlessly enforces state and input constraints, adapting the trajectory online to navigate environmental changes and previously unseen obstacles. During closed-loop operation, the generative model is selectively resampled whenever predefined criteria are met (e.g., a significant spatial deviation from the reference path). Crucially, these newly generated paths are biased by the predictive controller’s solution trajectory. This bidirectional coupling steers the generative model toward inherently collision-free regions of the task space, ensuring tight consistency between global planning and local execution while minimizing the corrective effort demanded of the controller.

Generative Modeling for Path Planning

We begin by introducing the path parametrization used to represent output paths. We then describe how demonstration data are converted into this parametrized representation for use in learning. Finally, we provide an overview of the generative model of expert behaviors, formulated as a probabilistic model over paths inferred from demonstrations.

From Trajectory Data to Path Parametrization

We parameterize the output path $\rho(\theta) \in\mathbb{R}^{n_y}$ as a linear combination of $M \in\mathbb{N}^{*}$ clamped B-splines of order $r \in\mathbb{N}$. For each dimension $j$, the path is given by

where $\xi_{m,j}$ are the governing coefficients and $B_{m,r}(\theta)$ is defined via the Cox–de Boor recursion formula [41]. The full path $\rho(\theta)$ is thus characterized by $M n_y$ coefficients.

Given $N \in\mathbb{N}^{*}$ time-indexed expert demonstrations $\tau^{(i)} = \{(t_k,y(t_k))\}_{k=0}^{n_i}$, we convert these into time-independent geometric paths. This representation is agnostic to variations in trajectory lengths, sampling rates, and temporal profiles. We extract the corresponding coefficient dataset $\mathcal{D}_{\xi} = \{\xi^{(i)}\}_{i=1}^{N}$ from the trajectory data $\mathcal{D}_{\tau} = \{\tau^{(i)}\}_{i=1}^{N}$ by solving a linear least-squares regression for each demonstration as

using evaluation points $\theta_j = (t_j-t_0)/(t_{n_i}-t_0)$. This minimization has a standard closed-form solution [42].

Modeling and Learning Expert Behaviors

To model expert behavior, we learn a shortcut model [19], a variant of flow matching, on the dataset $\mathcal{D}_{\xi}$. This model encodes the distribution of path coefficients $p(\xi\mid c)$, conditioned on a context $c \in\mathbb{R}^{n_c}$, representing the start and goal states, and captures the (potentially multi-modal) distribution of expert behaviors in the environment. This distribution is modeled with flow-matching using a time-varying vector field whose solutions map samples from an initial noise distribution to the data distribution. The shortcut model is trained to match the displacement between noise and demonstration samples while enforcing consistency across integration steps, using the loss function

Here, $\xi_0 \sim\mathcal{N}(0,\mathbb{I})$, $\xi_1 \sim\mathcal{D}_{\xi}$, and $(\lambda,\delta) \sim\mathcal{U}(0,1) \times\mathcal{U}(0,1)$. Furthermore, $\xi_{\lambda} = (1-\lambda)\xi_0+\lambda\xi_1$, and $\bar{v} = 0.5[v_{\zeta}(\lambda,\xi_{\lambda},0.5\delta)+v_{\zeta}(\lambda+\delta,\xi_{\lambda+\delta},0.5\delta)]$. The resulting model can efficiently generate expert-like path coefficients, and we refer the reader to [19] for full details.

Model Predictive Control for Path Following and Safety Filtering

Once a path is generated using the expert model, we use it as a reference in a MPPFC. We first review the basics of MPPFC and then discuss its application as an integrated local safety filter and tracking controller, ensuring collision-free closed-loop behavior. Finally, we detail the proposed integration of the learned path planner with the MPPFC.

Model Predictive Path-Following Control

We use a MPPFC to steer the system (1) along a given path, following established approaches [21, 22, 23]. To enable parallel evaluation of multiple horizon lengths, we consider $K \in\mathbb{N}$ controllers, indexed by $i \in\{0,1,\ldots,K-1\}$. Each controller employs an extended prediction horizon of $(T+i\delta_t)$, where $T \in\mathbb{R}_{>0}$ denotes a base horizon length and $\delta_t \in\mathbb{R}_{>0}$ is a fixed increment that enables systematical exploration of longer horizons. The corresponding optimization problem is formulated as

The controller $\pi_i$ predicts the system’s future behavior using (5b) and (5d) to optimize the future input trajectory $\bar{u}$, typically parameterized as piecewise constant. Physical limits and collision-avoidance requirements are captured by the system state and input constraint sets $\mathcal{X}_{\mathrm{safe}}$ and $\mathcal{U}$ in (5g). Concurrently, the virtual dynamics (5c) and virtual input $\bar{w}$ control the progress along the reference path, effectively coupling planning and control. The corresponding constraints in (5f) act as tuning parameters. The terminal cost (5h) ensures consistency between the true and the virtual states at the end of the horizon, promoting convergence to the reference path while respecting system and path constraints.

The cost function (5a) is defined as

where the stage cost $L$ penalizes path deviations (5e), large control inputs, and the distance of $\theta$ to its final value, encouraging forward progress. The terminal cost $E$ and constraint (5h) jointly ensure convergence to the path when chosen appropriately [22]. Problem (5) can be solved with gradient-based (gMPPFC) or sampling-based (sMPPFC) [43, 44] methods, though the latter do not guarantee strict constraint satisfaction.

MPPFC for Safety Filtering

The flow matching model generates reference paths that provide a strong, obstacle-aware prior based on the demonstrations. To systematically address dynamic environmental changes, such as new or unforeseen obstacles, we integrate this generative planning with MPPFC using up-to-date local sensor information.

In principle, one could adapt the generative model directly to new obstacles using an energy-based guidance term: an energy function $J_g : \mathbb{R}^{n_\xi} \to\mathbb{R}_{\geq0}$ assigns higher values to unsafe regions, and its gradient $\nabla_\xi J_g(\xi)$ biases the sampled path away from obstacles. This yields a modified vector field $\tilde{v}_\zeta(\lambda,\xi) = v_\zeta(\lambda,\xi) - \gamma_g \nabla_\xi J_g(\xi)$, where $\gamma_g > 0$ controls the guidance strength. However, evaluating the flow-matching model is computationally expensive, and real-time systems can typically only afford a small number of integration steps [14]. With a limited number integration steps, a large $\gamma_g$ risks pushing the generative process out of distribution, while a small $\gamma_g$ has negligible effect (see Figure 2). Furthermore, updating the entire global plan for purely local environmental changes is computationally inefficient. To overcome these limitations, we leverage the MPPFC to locally re-plan the system’s behavior, enforcing system dynamics and collision-avoidance constraints exclusively where needed (see Figure 1).

Image not extracted

At the beginning of task execution, we generate a batch of candidate paths from the generative model and select a collision-free path with respect to known obstacles. Rather than strictly requiring the flow-matching planner to output directly executable trajectories, our framework systematically delegates dynamic feasibility and real-time safety to the predictive controller. The selected path is incorporated into the MPPFC formulation in two complementary ways: it acts as a reference objective within the stage cost and defines the terminal region constraint (12). Specifically, the MPPFC constrains the system to return to and align with the reference path at the end of the prediction horizon. The exact realignment point is dynamically determined by the controller via the path dynamics and virtual input.

This hierarchical integration explicitly accounts for previously unknown static obstacles at runtime.¹ Assuming the system is equipped with sensors (e.g., LiDAR), newly detected obstacles are convexified and incorporated into the safe set $\mathcal{X}_{\mathrm{safe}}$ as state constraints. Once the system safely bypasses an obstacle and it exits the sensing range, the associated constraints are removed to minimize the computational burden.

Due to the terminal alignment structure of the MPPFC, feasibility dictates that obstacle avoidance and path re-alignment must be achieved within a single prediction horizon. If an obstacle forces a significant deviation, strict terminal alignment may render the optimal control problem infeasible. To ensure robust collision-free operation, we conceptually decouple short-term collision avoidance from strict terminal alignment. While this can be achieved by softening the terminal constraint or employing a generative-guided terminal cost, we adopt a parallelized horizon-extension strategy. We solve multiple MPPFC instances simultaneously, each with an incrementally extended prediction horizon but a fixed number of collocation points. We then execute the solution from the instance with the shortest feasible horizon (see Figure 1). Because a shorter horizon with fixed collocation points implies a smaller integration step size, it naturally yields higher control fidelity. The controller autonomously reverts to the shortest horizon once local feasibility is re-established.

For substantial deviations, event-triggered re-planning using the expert model is employed to improve adherence to the demonstrated global behavior. Re-planning is triggered when the system exits a user-defined neighborhood around an evaded obstacle — after the MPPFC has successfully ensured local safety. This allows the global planner to realign the overall trajectory with the expert distribution. Crucially, switching the reference path within the MPPFC operates seamlessly without requiring modifications to the underlying algorithmic procedure.

Ultimately, this systematic integration of generative planning and predictive control prioritizes real-time adaptability and robust empirical performance over conservative formal constraints. By combining the global geometric awareness of flow-matching with the strict local safety filtering of the MPPFC, the proposed strategy consistently produces collision-free behavior in practice, assuming the existence of a topologically viable alternative path, a standard and practical assumption for agile robotic platforms operating in task-oriented environments.

Biasing the Planner Toward the MPPFC Solution

A key component of the proposed method is re-planning the reference path during evasive maneuvers to improve adherence to demonstrated behaviors. We achieve this by using the predictive controller’s collision-free solution, which incorporates updated local sensor and constraint information, to bias the generative planner toward locally safe trajectories, ensuring candidate paths remain consistent with the current system state.

Introducing this sampling bias in classical generative models, such as VAEs [48], is not straightforward. In denoising-diffusion-like methods, such as flow matching, this is easily integrated by applying noise steps to the previous MPC solution, followed by denoising steps. This approach ensures the generated trajectories remain closer to the MPC prediction than sampling from standard Gaussian noise followed by multiple denoising steps. A similar approach has been used in diffusion models for autonomous racing [49].

Specifically, instead of starting the generative process from pure noise $\xi_0 \sim\mathcal{N}(0,I)$, we propose initializing it from an intermediate step $\lambda= \bar{\lambda}$, with $0 < \bar{\lambda} < 1$. This requires a partially noisy sample $\xi_{\bar{\lambda}} = (1-\bar{\lambda})\xi_0 + \bar{\lambda}\xi_1$, which depends on a clean sample $\xi_1$. Since the true $\xi_1$ is generally unavailable, we use the MPC’s latest prediction to obtain a local approximation $\hat{\xi}_1$. This estimate is obtained by fitting a B-spline using (3), providing a fast-to-compute, biased initialization. The generative process is then started with $(\xi_{\bar{\lambda}},\bar{\lambda})$ based on $\hat{\xi}_1$, producing candidate paths that are locally aligned with the controller’s current plan and likely maintain its collision-avoidance guarantees locally.

In addition to aligning the generative process with the MPC plan, the biasing provides notable computational advantages. By initializing the flow-matching model from an intermediate state informed by the MPC prediction, sampling typically requires fewer refinement steps and can therefore be performed more efficiently. Furthermore, by increasing the likelihood of obtaining a collision-free alternative reference path, the bias often reduces the magnitude of corrective actions required by the MPC. This, in turn, generally decreases the computational effort associated with solving the MPC problem.

Simulation Experiments

In this section, we evaluate our framework in simulated tasks. The experiments are constructed to answer the following questions:

1. What is the system’s behavior in a navigation task with unknown obstacles?

2. How do the MPC horizon expansion and re-planning strategies influence the success rate?

Environment, Data Collection, and Shortcut Model

To train the shortcut model effectively, we require a diverse and informative dataset of collision-free paths. Such a dataset is constructed in a physically realistic MuJoCo simulation [50], where a human operator navigates a mobile robot through a cluttered environment populated with various static obstacles (see Figure 4). The operator guides the robot between multiple random start and goal positions, producing paths that, due to the non-deterministic nature of human behavior, exhibit a multimodal distribution. The resulting dataset consists of approximately 1800 demonstrations. Our shortcut model is based on a diffusion transformer architecture as in [51]. The model is trained on an NVIDIA A100 GPU, and evaluated on an NVIDIA GeForce RTX 3090 GPU.

Image not extracted

Dynamics, Environments, and MPPFC Design

We consider a mobile robot with dynamics $\dot{x} = [u_1 \cos x_3, u_1 \sin x_3, u_2]^\mathsf{T}$, where $x = [x_1,x_2,x_3]^\mathsf{T}$ denotes the state (planar position $(x_1,x_2)$ in m and heading $x_3$ in rad), and the inputs are the translational velocity $u_1 \in[-1.5,1.5]$ (in $\mathrm{m\,s}^{-1}$) and the angular velocity $u_2 \in[-1.5,1.5]$ (in $\mathrm{rad\,s}^{-1}$). The output is $y = (x_1,x_2)$, and simulations run with a 50 ms sampling time.

We consider four environments (C1–C4) (Figure 3) containing identical static obstacles (blue) but varying configurations and sizes of previously unknown obstacles (red).

Image not extracted

The MPPFC instances are designed according to (5), with obstacles within the system’s sensing range defining the state constraint set $\mathcal{X}_{\mathrm{safe}}$ to enforce obstacle avoidance and ensure local reactivity. We initially tested the MPC framework using a sampling-based approach, but found that the resulting inputs lacked smoothness, which negatively impacted path-following accuracy and traversal time. Consequently, we focus on gradient-based implementations using CasADi [52], which provide smoother control inputs and more reliable path-following performance.

We test three re-planning conditions: (P1) resampling every 10 closed-loop steps; (P2) resampling when a newly detected obstacle causes a collision in the MPC prediction²; and (P3) resampling upon exiting a safety margin around an evaded obstacle.

Closed-Loop Performance

We first consider the environment C4 and the setting shown in Figure 4. We employ $K = 15$ parallel controller instances with a base horizon length $T = 1.0$ s, an increment $\delta_t = 0.2$, and 20 equidistant collocation points along each horizon. This results in a maximum horizon length of 3.8 s. We employ the re-planning strategy P3, as proposed earlier.

We sample a batch of paths from the shortcut model, conditioned on a given start and desired goal position (Figure 4a). All sampled paths are collision-free with respect to the obstacles encountered during demonstrations. From this batch, a path is randomly selected, and the parallel MPC instances compute the corresponding optimal inputs. The solution from the instance with the shortest horizon that returns a feasible solution is then executed.

After some time, a previously unknown obstacle appears within the system’s sensing range, causing the current reference path to be in collision (4b). To initiate an evasive maneuver, we include collision avoidance with the detected obstacle as a hard constraint in the MPC. However, adding this constraints can make the optimal control problem infeasible. Specifically, the MPPFC must re-align with the path at the end of each prediction horizon, and depending on the horizon length, input limits, and other state constraints, this may no longer be achievable for any controller instance.

In such cases, the controller with the shortest horizon among the feasible instances is still selected and executed. Moreover, whenever a new obstacle is added as a collision-avoidance constraint, we initialize the path parameter for the next horizon with $\theta(T)=1$, corresponding to the end of the reference path. This has been found beneficial, as it biases the optimizer toward solutions that prioritize significant path progress, increasing the likelihood that one controller instance remains feasible while planning to return to the reference path at the horizon’s end (Figure 4c). If necessary, the prediction horizons of each controller instance can be iteratively extended until feasibility is recovered, depending on the available computation budget.

When the robot exits a user-defined neighborhood around an evaded obstacle (illustrated as a circle in Figure 4d, gray dashed), re-planning is triggered and a new batch of paths (light green) is sampled from the current position to the desired goal. As described in Section IV-C, we bias the planner by initializing it with a noisy version of the MPPFC output at $\bar{\lambda}=0.75$, rather than from standard Gaussian noise. As a result, the planner generates high-quality, locally collision-free candidate paths with only two integration steps.

From the newly generated paths, we select one that is collision-free with respect to both the obstacles in the current sensing range and those encountered during demonstration (yellow line in Figure 4d) and deploy it in all controller instances. If no such path exists, the framework continues without interruption, relying on the parallel MPC instances to handle local feasibility. This procedure is repeated whenever a new, previously unknown obstacle appears (Figure 4e,Figure 4f), until the mobile robot reaches its destination.

Validation using Ablation Studies

We conduct an ablation study to assess MPC horizon expansion and the re-planning strategies. The obstacle configurations and re-planning conditions are evaluated across $K=\{5,7,15\}$ controller instances, corresponding to maximum prediction horizons $T_{\max}=\{1.8\,\mathrm{s},2.2\,\mathrm{s},3.8\,\mathrm{s}\}$, respectively. Due to the stochasticity of the generative model, each scenario is simulated 10 times. Results for all tested combinations are shown in Figure 5 in terms of success rates (successful trials over total trials). The number of re-planning events using the generative planner is reported in Figure 6, considering only successful trials.

Image not extracted

Image not extracted

For $T_{\max}=1.8\,\mathrm{s}$ ($K=5$), the controller is not always able to find a feasible solution across all scenarios, indicating that the prediction horizon is too short to provide consistently reliable performance. Increasing the horizon to $T_{\max}=2.2\,\mathrm{s}$ ($K=7$) improves robustness, although obstacle configuration C3 remains challenging. With the longest horizon, $T_{\max}=3.8\,\mathrm{s}$ ($K=15$), all tested combinations achieve a 100% success rate, except for P2 in C4, which achieves 90%. Notably, P3 achieves a 100% success rate across all configurations for both $T_{\max}=2.2\,\mathrm{s}$ and $T_{\max}=3.8\,\mathrm{s}$, while requiring fewer evaluations of the generative planner and thus improving computational efficiency without compromising performance. Overall, the results indicate that the approach enables effective and efficient path planning and tracking in cluttered environments, maintaining collision-free behavior while outperforming naive baseline implementations.

Conclusions

We proposed a framework that combines flow-matching-based generative planning, trained on expert demonstrations, with model predictive path-following control for adaptive navigation in cluttered environments. The generative planner provides diverse candidate paths, while the predictive controller incorporates onboard sensor information to adapt these paths online and ensure constraint satisfaction and collision-free execution, including in the presence of previously unseen obstacles. Event-triggered re-planning further couples path generation and control throughout task execution. Future work will focus on incorporating richer contextual information, such as perception data, into path generation and extending the framework to dynamic environments through motion prediction.

Acknowledgments

During the preparation of this work, the authors used ChatGPT 5 and 5-Thinking by OpenAI to improve language and wording. The authors reviewed and edited the generated content and take full responsibility for the final publication.