Efficient Optimal Path Planning in Dynamic Environments Using Koopman MPC

TL;DR

The paper addresses real-time path planning for mobile robots in dynamic environments by learning a bilinear Koopman representation that jointly captures nonlinear unicycle dynamics and collision-avoidance constraints. Using bilinear EDMD, the lifted predictor $Z_{k+1}=A Z_k + B u_k + \sum_i H_i [u_k^i \cdot Z_k]$ faithfully represents input–state couplings, enabling a convex QP-MPC formulation in the lifted space. A comprehensive dictionary including $X,Y,v,\theta,X^2,Y^2,\sin\theta,\cos\theta,v\sin\theta,v\cos\theta$ yields $N_\ell=65$ observables, with $100{,}000$ trajectories supporting strong bilinear couplings $H_i$. The resulting BK-MPC achieves real-time performance (average solve times around $3.6$–$8.8$ ms) and up to a 320x speedup over nonlinear MPC while maintaining comparable safety and target-tracking accuracy in the presence of moving obstacles. This demonstrates the practical potential of bilinear Koopman models for efficient, safe navigation in dynamic scenarios with nonlinear constraints. Key equations showing the lifted dynamics, the linearized control input, and the collision-avoidance constraints are embedded in the QP that governs the receding-horizon optimization, facilitating deployment on embedded hardware without sacrificing safety guarantees.

Abstract

This paper presents a data-driven model predictive control framework for mobile robots navigating in dynamic environments, leveraging Koopman operator theory. Unlike the conventional Koopman-based approaches that focus on the linearization of system dynamics only, our work focuses on finding a global linear representation for the optimal path planning problem that includes both the nonlinear robot dynamics and collision-avoidance constraints. We deploy extended dynamic mode decomposition to identify linear and bilinear Koopman realizations from input-state data. Our open-loop analysis demonstrates that only the bilinear Koopman model can accurately capture nonlinear state-input couplings and quadratic terms essential for collision avoidance, whereas linear realizations fail to do so. We formulate a quadratic program for the robot path planning in the presence of moving obstacles in the lifted space and determine the optimal robot action in an MPC framework. Our approach is capable of finding the safe optimal action 320 times faster than a nonlinear MPC counterpart that solves the path planning problem in the original state space. Our work highlights the potential of bilinear Koopman realizations for linearization of highly nonlinear optimal control problems subject to nonlinear state and input constraints to achieve computational efficiency similar to linear problems.

Figures (6)

• Figure 1: Unicycle robot kinematics and ellipsoidal obstacle representation with safety margin for collision avoidance.
• Figure 2: Open-loop prediction of the nonlinear unicycle model (black) compared with Bilinear EDMD (red) and Linear EDMD (blue) over a 4-second horizon. The bilinear realization accurately reproduces both the primary states $(X,Y,v,\theta)$ and the quadratic observables $(X^2,Y^2)$.
• Figure 3: Identified bilinear coupling matrices $H_1$ (acceleration channel) and $H_2$ (angular velocity channel). The highlighted entries show the learned input–state interactions responsible for the bilinear terms in \ref{['eq:taylor1']} and \ref{['eq:taylor2']}.
• Figure 4: Closed-loop trajectories of BK-MPC and NMPC in the presence of a moving obstacle, shown at time snapshots $t=1.0\,\text{s}$, $t=2.5\,\text{s}$, $t=4.0\,\text{s}$, and $t=5.5\,\text{s}$. Predicted trajectories are indicated by dashed lines, while executed trajectories are shown as solid lines. Both controllers successfully reach the target while avoiding the moving obstacle. The proposed BK-MPC achieves real-time operation and is approximately $320$ times faster than NMPC, which does not meet real-time feasibility.
• Figure 5: Closed-loop state and input trajectories of NMPC and BK-MPC. Both controllers exhibit nearly identical state trajectories and apply comparable control efforts while satisfying constraints.