BPOMP: Bilevel Path Optimization for Motion Planning
Changhao Wang, Hsien-Chung Lin, Shiyu Jin, Xinghao Zhu, Liting Sun, and Masayoshi Tomizuka
Welcome! This website supplements our ACC 2022 submission, a novel path optimization formulation that is able to consider collision more efficiently.
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).
With fewer waypoints, BPOMP is able to outperform selecting dense waypoints with standard formulations. Different from standard formulations that only consider the collision on each waypoint, BPOMP additionally constraints 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.
Simulation and Experiment Results
Simulation
The proposed formulation was first implemented on a mobile robot in a 2D space for validation. The robot has threedegrees of freedom x,y,rotation.
The proposed algorithm was also tested on a 6-DOF robot. A FANUC LR Mate 200iD7Lrobot with 6 DOFs was used. For collision checking, each link was modeled by several spheres with different radius that can cover the whole link.
 |
Experiments
The proposed BPOMP algorithm was experimentally validated with a FANUC M20iA robot. In the workspace, a cage with thin frames was placed in front of the robot on a table. The robot was set to put a bottle inside the cage with several different poses. Due to the geometric structure of the cage, the robot has to go through different faces of the cage in order to reach the particular target pose.