We present an algorithm for safe robot navigation in complex dynamic environments using a variant of model predictive equilibrium point control. We use an optimization formulation to navigate robots gracefully in dynamic environments by optimizing over a trajectory cost function at each timestep. We present a novel trajectory cost formulation that significantly reduces the conservative and deadlock behaviors and generates smooth trajectories. In particular, we propose a new collision probability function that effectively captures the risk associated with a given configuration and the time to avoid collisions based on the velocity direction. Moreover, we propose a terminal state cost based on the expected time-to-goal and time-to-collision values that helps in avoiding trajectories that could result in deadlock. We evaluate our cost formulation in multiple simulated and real-world scenarios, including narrow corridors with dynamic obstacles, and observe significantly improved navigation behavior and reduced deadlocks as compared to prior methods.

翻译：我们提出了一种算法，用于在复杂动态环境中实现安全的机器人导航，该算法采用了模型预测平衡点控制的变体。我们通过在每个时间步优化轨迹代价函数，利用优化公式使机器人在动态环境中优雅地导航。我们提出了一种新颖的轨迹代价公式，该公式显著减少了保守行为和死锁现象，并生成了平滑的轨迹。具体而言，我们提出了一种新的碰撞概率函数，该函数基于速度方向有效捕捉给定构型相关的风险以及避免碰撞的时间。此外，我们提出了一种基于预期到达目标时间和碰撞时间的终端状态代价，这有助于避免可能导致死锁的轨迹。我们在多个模拟和真实场景（包括包含动态障碍物的狭窄走廊）中评估了我们的代价公式，并观察到相比于先前方法，导航行为显著改善，死锁情况减少。