Safe, Optimal, Real-Time Trajectory Planning With a Parallel Constrained Bernstein Algorithm

To move while using new sensor information, mobile robots use receding-horizon planning, executing a short plan while computing a new one. A plan should have dynamic feasibility (obeying a robot's dynamics and avoiding obstacles), liveness (planning frequently enough to complete tasks), and optimality (minimizing, e.g., distance to a goal). Reachability-based trajectory design (RTD) is a method to generate provably dynamically feasible plans in real time by solving a polynomial optimization program (POP) in each planning iteration. However, RTD uses a derivative-based solver, which may converge to local minima that impact liveness and optimality. This article proposes a parallel constrained Bernstein algorithm (PCBA) branch-and-bound method to optimally solve RTD's POP at runtime; the resulting optimal planner is called RTD*. The specific contributions of this article are the PCBA implementation, proofs of PCBA's bounded time and memory usage, a comparison of PCBA with state-of-the-art solvers, and a demonstration of PCBA/RTD* on hardware. RTD* shows better optimality and liveness than RTD in dozens of environments with random obstacles.