BPOMP: A Bilevel Path Optimization Formulation for Motion Planning.

Balancing computation efficiency and success rate is challenging for path optimization. To obtain a collision-free path, each waypoint along the path should be collision-free, and the waypoints should be dense. As a consequence, the solutions are computationally expensive to obtain. This paper introduces a bilevel path optimization formulation for motion planning (BPOMP). Different from standard formulations that only consider the collision on each waypoint, BPOMP additionally considers the collision that may happen in the middle by computing the closest position to the obstacle along the continuous path. Intuitively, if the closest position is out of collision, the entire path should be collision-free. The problem is formulated as a bilevel optimization and then relaxed to canonical nonlinear programming (NLP), which can be solved classically. Comparison results in both simulations and experiments are provided to show the effeteness of the proposed formulation. Videos are available at https://changhaowang.github.io/BPOMP/BPOMP.html.