Scalable lazy SMT-based motion planning

We present a scalable robot motion planning algorithm for reach-avoid problems. We assume a discrete-time, linear model of the robot dynamics and a workspace described by a set of obstacles and a target region, where both the obstacles and the region are polyhedra. Our goal is to construct a trajectory, and the associated control strategy, that steers the robot from its initial point to the target while avoiding obstacles. Differently from previous approaches, based on the discretization of the continuous state space or uniform discretization of the workspace, our approach, inspired by the lazy satisfiability modulo theory paradigm, decomposes the planning problem into smaller subproblems, which can be efficiently solved using specialized solvers. At each iteration, we use a coarse, obstacle-based discretization of the workspace to obtain candidate high-level, discrete plans that solve a set of Boolean constraints, while completely abstracting the low-level continuous dynamics. The feasibility of the proposed plans is then checked via a convex program, under constraints on both the system dynamics and the control inputs, and new candidate plans are generated until a feasible one is found. To achieve scalability, we show how to generate succinct explanations for the infeasibility of a discrete plan by exploiting a relaxation of the convex program that allows detecting the earliest possible occurrence of an infeasible transition between workspace regions. Simulation results show that our algorithm favorably compares with state-of-the-art techniques and scales well for complex systems, including robot dynamics with up to 50 continuous states.

我们提出了一种用于避障可达问题的可扩展机器人运动规划算法。我们假设机器人动力学具有离散时间线性模型，并且工作空间由一组障碍物和一个目标区域描述，其中障碍物和区域均为多面体。我们的目标是构建一条轨迹以及相关的控制策略，使机器人从初始点驶向目标同时避开障碍物。与先前基于连续状态空间离散化或工作空间均匀离散化的方法不同，我们的方法受惰性可满足性模理论范式的启发，将规划问题分解为更小的子问题，这些子问题可以使用专门的求解器高效求解。在每次迭代中，我们使用基于障碍物的工作空间粗离散化来获得候选的高级离散规划，这些规划解决了一组布尔约束，同时完全抽象了低级连续动力学。然后通过一个凸规划在系统动力学和控制输入的约束下检查所提出规划的可行性，并生成新的候选规划，直到找到一个可行的规划。为了实现可扩展性，我们展示了如何通过利用凸规划的松弛来为离散规划的不可行性生成简洁的解释，这种松弛允许检测工作空间区域之间不可行转换的最早可能发生情况。仿真结果表明，我们的算法与现有技术相比具有优势，并且对于复杂系统（包括具有多达50个连续状态的机器人动力学）具有良好的扩展性。