  In this paper, we present a bilevel optimal motion planning (BOMP) model for
autonomous parking. The BOMP model treats motion planning as an optimal control
problem, in which the upper level is designed for vehicle nonlinear dynamics,
and the lower level is for geometry collision-free constraints. The significant
feature of the BOMP model is that the lower level is a linear programming
problem that serves as a constraint for the upper-level problem. That is, an
optimal control problem contains an embedded optimization problem as
constraints. Traditional optimal control methods cannot solve the BOMP problem
directly. Therefore, the modified approximate Karush-Kuhn-Tucker theory is
applied to generate a general nonlinear optimal control problem. Then the
pseudospectral optimal control method solves the converted problem.
Particularly, the lower level is the $J_2$-function that acts as a distance
function between convex polyhedron objects. Polyhedrons can approximate
vehicles in higher precision than spheres or ellipsoids. Besides, the modified
$J_2$-function (MJ) and the active-points based modified $J_2$-function (APMJ)
are proposed to reduce the variables number and time complexity. As a result,
an iteirative two-stage BOMP algorithm for autonomous parking concerning
dynamical feasibility and collision-free property is proposed. The MJ function
is used in the initial stage to find an initial collision-free approximate
optimal trajectory and the active points, then the APMJ function in the final
stage finds out the optimal trajectory. Simulation results and experiment on
Turtlebot3 validate the BOMP model, and demonstrate that the computation speed
increases almost two orders of magnitude compared with the area criterion based
collision avoidance method.
