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 a simulation environment 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重构将双层问题转化为单层形式，并通过非线性求解器进行优化。该MPC方法可在单机器人-多人类环境中，既满足严格安全约束，又能影响行人运动。我们在仿真环境中分析SICNav的性能，验证机器人能够以影响周围人类的方式实现安全运动。同时通过人类轨迹数据集验证ORCA的轨迹预测性能。