SICNav: Safe and Interactive Crowd Navigation using Model Predictive Control and Bilevel Optimization

TL;DR

The paper tackles safe navigation in crowds by addressing the interaction between a robot and humans through a bilevel MPC framework called SICNav, which jointly plans robot trajectories and ORCA-based human predictions in closed-loop. It derives a KKT-based reformulation to convert the bilevel problem into a single-level MPCC, analyzes global and local optimality conditions, and introduces a feasible warm-start strategy to ensure reliable convergence. The authors validate ORCA as a trajectory predictor on ETH/UCY and demonstrate SICNav's solid performance in both simulations (ORCA and SFM agents) and real indoor robot experiments, showing improvements over baselines in success rate, navigation time, and safety. The work advances interactive, safety-guaranteed crowd navigation by integrating a principled, optimization-based planner with a realistic human-motion model, paving the way for real-world deployment with explicit safety guarantees.

Abstract

Robots need to predict and react to human motions to navigate through a crowd without collisions. Many existing methods decouple prediction from planning, which does not account for the interaction between robot and human motions and can lead to the robot getting stuck. We propose SICNav, a Model Predictive Control (MPC) method that jointly solves for robot motion and predicted crowd motion in closed-loop. We model each human in the crowd to be following an Optimal Reciprocal Collision Avoidance (ORCA) scheme and embed that model as a constraint in the robot's local planner, resulting in a bilevel nonlinear MPC optimization problem. We use a KKT-reformulation to cast the bilevel problem as a single level and use a nonlinear solver to optimize. Our MPC method can influence pedestrian motion while explicitly satisfying safety constraints in a single-robot multi-human environment. We analyze the performance of SICNav in two simulation environments and indoor experiments with a real robot to demonstrate safe robot motion that can influence the surrounding humans. We also validate the trajectory forecasting performance of ORCA on a human trajectory dataset.

Figures (10)

• Figure 1: Our MPC algorithm, SICNav, jointly optimizes robot trajectories (blue) and associated predicted human trajectories (green) for multiple humans while satisfying kino-dynamic and collision-avoidance safety constraints. Video of our work deployed on the pictured robot: https://tiny.cc/sicnav_overview.
• Figure 2: (a) Exemplary crowd navigation environment state ${\mathbf{x}}_{t}$ with one robot (yellow) and its goal position (green star), $N=2$ simulated humans (gray circles) with respective goals (gray stars), agent velocities (red arrows), and $M = 2$ static obstacles (blue lines). (b) Graphical representation of the associated ORCA velocity space optimization \ref{['eq:relaxed_orca_argmin']} solved by each simulated human (Human 1 left and Human 2 right). The human's preferred velocity (gray circle) points toward their goal, and induces the minimum of the cost (gray contours). The constraints are illustrated as lines (for non-collision constraints) or a circle (for the maximum velocity constraint) with the feasible sides shaded. The optimum velocity, $\mathbf{v}^*$, i.e. the closest velocity to $\mathbf{v}_{\text{pref}}$ that is feasible with respect to every constraint, is illustrated as a black star, with the associated optimal value of the slack variable, $\zeta^*$, written on the graph.
• Figure 3: Overview of our method. In section \ref{['sec:our_soln']}, we formulate a Bilevel MPC problem \ref{['eq:combmpc0_prob']} that solves for robot actions in the upper level (blue) and predicted human actions via a lower level ORCA problems (green). In section \ref{['sec:orca_properties']}, we use Lagrangian duality analysis to derive the KKT optimality criteria for the lower level problems, and in section \ref{['sec:kkt_reform']}, we reformulate the bilevel problem by replacing the lower level problems with their KKT optimality criteria to get a single level KKT Reformulation MPC problem that we can solve. In section \ref{['sec:kkt_reform_properties']}, we analyze the equivalence between optima of the reformulation and optima of the original bilevel problem.
• Figure 4: Illustration of the optimization problem used to solve for robot actions for each step of the warm start for the same environment state as Fig. \ref{['fig:env_setup_posns']}. The robot's kino-dynamic constraints are underestimated by the red linear constraints. The extra constraints are identically formulated as the static obstacle constraints in \ref{['eq:relaxed_orca_argmin']}.
• Figure 5: Snapshots through time of a robot (yellow) using our SICNav approach (a) and the MPC-CVMM baseline approach (b), near a goal location (green star) obstructed by simulated human agents (gray circles). At time 0.25, the robot and the humans are moving downwards, and both the SICNav and MPC-CVMM robot plan to turn to the right towards their goal (MPC plan in blue). Our SICNav robot arrives at the goal by time 6.25 by predicting that human 1 will move out of its way (predictions in green), then proceed to its own goal, arriving at time 11.5. Meanwhile, the robot using the baseline MPC-CVMM method is unable to coordinate with human 1. Animation: https://tiny.cc/sicnav_fig5.

Theorems & Definitions (5)

• Definition 1: Relaxed ORCA
• Definition 2: Lagrangian Dual
• Definition 3: SCQ
• Definition 4: KKT Conditions
• Definition 5: LICQ