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.

翻译：机器人需要预测并响应人类运动，以在人群中实现无碰撞导航。现有方法大多将预测与规划解耦，未能考虑机器人运动与人类运动之间的相互作用，可能导致机器人陷入停滞。本文提出SICNav——一种模型预测控制（MPC）方法，通过闭环系统联合求解机器人运动与预测人群运动。我们将人群中每个个体建模为遵循最优互惠避碰（ORCA）策略，并将该模型作为约束嵌入机器人局部规划器，从而构建双层非线性MPC优化问题。通过KKT重构将双层问题转化为单层形式，并采用非线性求解器进行优化。该方法能在单机器人-多人类环境中主动影响行人运动，同时严格满足安全约束。我们在两种仿真环境及真实机器人室内实验中分析了SICNav的性能，验证了其能生成安全机器人轨迹并影响周围人群。此外，我们在人类轨迹数据集上验证了ORCA的轨迹预测性能。