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.

翻译：本文提出一种面向自主泊车的双层最优运动规划（BOMP）模型。该模型将运动规划视为最优控制问题：上层处理车辆非线性动力学，下层处理几何无碰撞约束。BOMP模型的核心特征在于下层为线性规划问题，其作为上层问题的约束条件——即最优控制问题中嵌入了优化问题作为约束。传统最优控制方法无法直接求解BOMP问题，因此采用修正近似Karush-Kuhn-Tucker理论将其转化为一般非线性最优控制问题，继而通过伪谱最优控制方法求解转化后的问题。特别地，下层采用$J_2$函数作为凸多面体对象间的距离函数。相比球体或椭球体，多面体可更高精度地近似车辆形状。此外，为减少变量数量与时间复杂度，本文提出修正$J_2$函数（MJ）和基于活动点的修正$J_2$函数（APMJ）。据此，提出一种面向自主泊车的迭代两阶段BOMP算法，同时兼顾动力学可行性及无碰撞特性：初始阶段使用MJ函数求解初始无碰撞近似最优轨迹及活动点，最终阶段使用APMJ函数求解最优轨迹。仿真结果与Turtlebot3实验验证了BOMP模型的有效性，且表明其计算速度较基于面积准则的避碰方法提升近两个数量级。