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.
翻译:我们提出了一种在复杂动态环境中使用模型预测平衡点控制的安全机器人导航算法的优化形式。我们在每个时间步上通过优化轨迹成本函数来优雅地导航机器人在动态环境中,我们提出了一种新颖的轨迹成本公式,显著降低保守和死锁行为,并生成平滑的轨迹。特别地,我们提出了一种新的碰撞概率函数,有效捕捉给定配置和速度方向的碰撞风险和避免碰撞的时间。此外,我们提出了一个基于预期到达时间和避免进入死锁状态的末态成本,可以帮助避免导致死锁的轨迹。我们在多个模拟和真实场景下评估了我们的成本公式,包括带动态障碍物的狭窄走廊,并观察到与先前方法相比,显著提高了导航行为并减少了死锁。