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

We will present this work at ICRA 2025, Atlanta, USA

Wednesday 21 May 2025, 17:05 EDT, Room 402, Session Social Navigation 2, Paper WeET14.6

Our MPC algorithm, SICNav, jointly optimizes robot trajectories (blue) and associated predicted human trajectories (green) for multiple humans. Our approach explicitly models interactions between agents while satisfying safety constraints.

To achieve this, we formulate a Bilevel MPC problem that solves for robot actions in the upper level (blue) and predicted human actions via a lower level ORCA problems. We use Lagrangian duality analysis to derive the KKT optimality criteria for the lower level problems, and 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 solve in receding horizon fashion.

Video

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.

BibTex

@misc{samavi2024sicnav,
    author={Samavi, Sepehr and Han, James R. and Shkurti, Florian and Schoellig, Angela P.},
    journal={IEEE Transactions on Robotics},
    title={SICNav: Safe and Interactive Crowd Navigation Using Model Predictive Control and Bilevel Optimization},
    year={2025},
    volume={41},
    number={},
    pages={801-818},
    doi={10.1109/TRO.2024.3484634}
}