Dual Process Motion Planning

Model Predictive Control (MPC) is a widely used optimal control framework for motion planning under dynamic and environmental constraints (Mayne et al. 2000). At each time step, MPC solves a finite-horizon optimization problem to compute a control sequence that minimizes a cost function while satisfying system dynamics and constraints.

Consider nonlinear dynamics $\dot{x}=f(x,u),$ or, after discretization, $x_{k+1}=F_{d}(x_{k},u_{k}),$ where $x_{k}\in\mathbb{R}^{n}$ is the state and $u_{k}\in\mathbb{R}^{m}$ is the control. Given the current state $x_{0}$ and goal $x_{g}$, nonlinear MPC solves $\min_{u_{0:N_{h}-1}}\sum_{k=0}^{N_{h}-1}\ell(x_{k},u_{k};x_{g})+V_{f}(x_{N_{h}};x_{g})$ subject to $x_{k+1}=F_{d}(x_{k},u_{k}),\qquad x_{k}\in\mathcal{X}_{\mathrm{free}},\qquad u_{k}\in\mathcal{U}.$ The stage cost is typically chosen as $\ell(x_{k},u_{k};x_{g})=\|x_{k}-x_{g}\|_{Q}^{2}+\|u_{k}\|_{R}^{2},$ where $Q\succeq 0$ and $R\succ 0$. Obstacle avoidance is encoded through nonlinear state constraints defining the collision-free set $\mathcal{X}_{\mathrm{free}}$ MPC is accurate because it optimizes over future trajectories, but repeatedly solving the resulting nonlinear program can be computationally expensive.

Control Barrier Functions (CBFs) provide a complementary approach for enforcing safety constraints in control systems (Ames et al. 2019). Unlike MPC, the CBF-QP does not optimize an entire future trajectory; instead, it acts as a safety filter that enforces local obstacle-avoidance constraints.

For a nonlinear control-affine system $\dot{x}=f(x)+g(x)u,$ let the safe set for obstacle $i$ be $\mathcal{C}_{i}=\{x:h_{i}(x)\geq 0\},$ where $h_{i}(x)$ is positive outside the obstacle and negative inside it. Forward invariance of $\mathcal{C}_{i}$ can be encouraged by enforcing $L_{f}h_{i}(x)+L_{g}h_{i}(x)u+\gamma h_{i}(x)\geq 0,$ where $L_{f}h_{i}$ and $L_{g}h_{i}$ are Lie derivatives and $\gamma>0$ controls how aggressively the controller moves away from the safety boundary.

At each timestep, the CBF controller solves the quadratic program $\min_{u}\|u-u_{\mathrm{ref}}\|^{2}$ subject to $L_{f}h_{i}(x)+L_{g}h_{i}(x)u+\gamma h_{i}(x)\geq 0,\quad\forall i,\qquad u\in\mathcal{U}.$ Here, $u_{\mathrm{ref}}$ is a nominal control input, such as a goal-directed command or a command proposed by the neural System-1 policy.

Kahneman (2011) describe human decision making as the interaction between two complementary processes: a fast, intuitive, experience-driven System-1 and a slower, deliberative, reasoning-based System-2. Recent AI architectures, including SOFAI (Pallagani et al. 2025; Fabiano et al. 2025), adapt this dual-process principle to machine decision-making by combining fast and slow solvers with a metacognitive agent that arbitrates them (Ganapini et al. 2021). These architectures have proven successful in tackling settings closely related to motion planning, such as classical planning and constrained grid navigation, as shown in Fabiano et al. (2025).

Formally, SOFAI defines a decision architecture composed of three components: (i) a set of fast S1 solvers, typically data-driven and experience-based; (ii) a set of slow S2 solvers, based on explicit symbolic reasoning; and (iii) a centralized metacognitive (MC) controller. Incoming problem instances automatically trigger one or more S1 solvers, which produce candidate solutions together with confidence estimates. The MC controller then decides whether to accept the S1 proposal or invoke an S2 solver. This decision is performed in two stages: a lightweight assessment that evaluates whether the expected solution quality satisfies a task-dependent threshold under resource constraints, followed, when necessary, by a cost-benefit comparison between S1 and S2 execution. S2 reasoning is activated only if its expected improvement compensates for the additional computational cost. A key aspect of this framework is that S1 behavior is not static. Through metacognition, solutions produced or validated by S2 can be used to improve the fast solver over time, effectively distilling deliberative reasoning into reactive policies. In this sense, the architecture supports an iterative refinement process in which expensive symbolic reasoning is gradually amortized into efficient inference, enabling the system to adapt to recurring problem distributions while reducing reliance on S2 computation.

Our System-1 is implemented as a neural reactive policy. This follows the common approach of imitation learning to predict low-level controls from local environment observations and goal information (Ichter et al. 2017).

Given the current rollout context, local obstacle information, nonlinear dynamics features, and the goal direction, the policy predicts a control input directly: $u_{t}^{\mathrm{S1}}=\pi_{\theta}(c_{t},s,d_{t},g_{t}),$ where $c_{t}$ is a fixed-length window of recent states in a local coordinate frame, $s$ encodes the local obstacle situation, $d_{t}$ encodes the nonlinear dynamics at the current state, and $g_{t}$ is the local goal vector.

The policy uses a lightweight convolutional architecture. A one-dimensional convolutional encoder processes the recent state context $c_{t}$, while separate multilayer encoders process the situation vector, dynamics features, and goal vector. The resulting features are concatenated and passed through a feedforward control head:

This design keeps S1 inexpensive at runtime while still conditioning the control prediction on both geometry and nonlinear dynamics.

As mentioned, S1 is trained by imitation from successful System-2 trajectories. In particular, the bootstrap demonstrations are solved offline by System-2, and the System-2 recoveries are collected online during continual learning. For each state-control pair, the supervised target is the expert control $u_{t}^{\star}$, with auxiliary single-step losses that encourage accurate one-step motion, correct direction, and progress toward the goal, and a multi-step rollout term that penalizes the drift accumulated when the policy is run in closed loop:

Here, $\ell_{u}$ is a Huber control-imitation loss, $\ell_{x}$ penalizes one-step prediction error under the nonlinear dynamics, $\ell_{\mathrm{dir}}$ encourages alignment with the expert displacement, $\ell_{\mathrm{spd}}$ matches step magnitude, and $\ell_{\mathrm{prog}}$ penalizes insufficient progress toward the goal. The remaining three terms are evaluated on a differentiable $H$-step rollout of the policy under the nonlinear dynamics, run once every $N$ supervised batches ($H=N=8$): $\ell_{\mathrm{roll}}$ penalizes deviation of the rolled-out positions from the expert positions, $\ell_{\mathrm{smo}}$ penalizes large control changes, and $\ell_{\mathrm{obs}}$ penalizes proximity to obstacles along the rollout.

At execution time, S1 rolls out the learned policy under the nonlinear dynamics. A lightweight one-step safety filter selects among the predicted control, goal-directed controls, scaled controls, and zero control, keeping only candidates whose next segment is collision-free. The resulting trajectory is then verified for collision freedom and goal reach. In the SOFAI setting, S2 is invoked only when the S1 rollout fails this verification.

Dual-MP can be instantiated with two state-of-the-art S2 solvers:

• •

  safe_control: a control barrier function (CBF)-based safety filter  (Kim et al. 2025)

• •

  acados: a nonlinear model predictive control (MPC) planner  (Verschueren et al. 2021)

Let us note that both solvers are implemented using their standard formulations from the literature.

Dual-MP follows the SOFAI principle of coordinating fast S1 solvers and slower deliberative S2 solvers through a metacognitive arbitration module (Pallagani et al. 2025). Depending on the Dual-MP configuration, the S1 and S2 components are instantiated differently; in our experiments, the two configurations are obtained by pairing the S1 solver with two S2 solvers introduced above.

For each new scenario, the metacognitive module first invokes the configured S1 solver. The proposed rollout is accepted only if it passes verification (collision freedom and goal reach - cf. above). If the S1 proposal is rejected, Dual-MP invokes the configured S2 fallback, provided that the available time budget is sufficient relative to the estimated problem difficulty. If S2 does not return a satisfactory solution within the budget, Dual-MP falls back to the best available S1 proposal. Successful S2 rollouts are stored in memory and periodically used to improve S1.

The metacognitive module relies on three quantities to properly arbitrate the two systems: problem difficulty, solution correctness, and solver confidence. Difficulty is a scenario-level quantity used to characterize the planning instance; correctness measures the quality of a proposed rollout; and confidence estimates whether an S1 proposal is worth verifying. Following the SOFAI principle, S2 solvers are treated as deliberative solvers and assigned confidence $1$, while S1 confidence is computed as described above.

A key advantage of SOFAI-inspired architectures is that S1 solvers can improve over time. Dual-MP supports this by storing successfully generated S2 trajectories and using them to enrich the corresponding S1 solver. Specifically, when S2 successfully solves a query, the resulting trajectory is added to the online dataset. For Neural S1, accumulated S2 trajectories are distilled periodically after a fixed number of episodes by fine-tuning the policy on both the original offline dataset and the newly collected S2 successes.

We now present the experimental results of our paper and use these to answer the four research questions we introduced above. The main results are presented in Table 2, which reports results macro-averaged over the six families for the six configurations, and in Table 3 that breaks the same runs down per family.

Although Dual-MP is only an initial step toward dual-process motion planning, it delivers promising results; it outperforms the standalone baselines in the reported success and efficiency metrics. This suggests that verified arbitration is not just a conceptual framework, but a practical way to combine learned policies with optimization-based solvers.

The results show that Dual-MP is most effective when the two systems have distinct costs and complementary strengths. A fast S1 policy is most valuable when it avoids expensive MPC calls, but it offers little benefit when the fallback is already cheap, as with CBF. This means that success rate alone is not sufficient to evaluate a hybrid planner: the runtime balance between S1 and S2 largely determines whether arbitration is worthwhile. Exact rollout verification is central to this behavior. It allows S1 to produce low-latency feasible solutions without letting unsafe or incomplete predictions replace the fallback solver. When S1 fails, S2 takes over, and successful recoveries can then be reused for continual learning. This makes the architecture practical without requiring the neural policy to be perfectly safe in isolation, as long as it is paired with a correct fallback and a verification gate.

Similarly, we note that continual learning is most effective when the teacher’s behavior is compatible with the student’s policy class. CBF provides short, local corrections that improve S1 coverage, whereas MPC often produces longer, route-dependent behaviors that are harder for a reactive policy to reproduce reliably in closed loop. This suggests several promising directions for future work: (i) training S1 on MPC trajectories and pairing it with CBF, (ii) exploring other combinations of fast and slow components, and (iii) using a portfolio metacontroller to select among multiple S2 planners rather than relying on a single fallback. More generally, future work could optimize different objectives depending on the application, including planning time, trajectory quality, success rate, or even accepting slightly suboptimal solutions when that yields a better overall trade-off. In the same spirit, better knowledge transfer across environment families could reduce bootstrap cost, either by sharing an S1 policy across tasks or by making continual learning more selective when offline retraining budget is limited. Rejected S1 rollouts should likewise be used more selectively as MPC initializations; a stronger confidence or feasibility estimate could help determine when a proposal is close enough to serve as a useful warm start and when MPC should instead solve the problem from scratch.

Overall, this work provides an initial but promising step toward optimizing not only the individual tools, but also their composition. It shows that the interaction between learned policies, symbolic solvers, verification, and retraining can itself be designed and improved.