A bilevel optimal motion planning (BOMP) model with application to autonomous parking

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 J2\documentclass[12pt]{minimal} \usepackage{amsmath} \usepackage{wasysym} \usepackage{amsfonts} \usepackage{amssymb} \usepackage{amsbsy} \usepackage{mathrsfs} \usepackage{upgreek} \setlength{\oddsidemargin}{-69pt} \begin{document}$$J_2$$\end{document}-function that acts as a distance function between convex polyhedron objects. Polyhedrons can approximate objects in higher precision than spheres or ellipsoids. As a result, a fast high-precision BOMP algorithm for autonomous parking concerning dynamical feasibility and collision-free property is proposed. 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.


I. Introduction
Real-time collision-free motion planning and control for autonomous vehicles have received a considerable amount of attentions, and share many research methods with robotics literature [1]- [26].Typically, autonomous parking is a critical maneuver especially in big narrow cities. Separated methods [3]- [6] are common approaches for parking problems, that vehicle path planning and path tracking are handled separately.Direct learning [7] is a novel and simple approach that learns the mapping relation between control inputs and parking trajectories.However, for complex high-precision parking problems, combined approaches [22], [23] are more effective, that vehicle motion planning and control are treated as a unified optimal control problem.In this paper, we also treat the parking problem as a combined optimal control problem.
Motion planning approaches can be categorized into graph searching methods [8], [9], artificial potential field methods [10], [11], sampling-based methods [12]- [16] and nonlinear optimization methods [17]- [26].In graph searching methods, systematic discretization of the environment or robot state space is applied to construct a graph first, then highly informative heuristics are used to guide the search to generate a connected feasible path or trajectory.Their practical applications are limited to low dimensional space since the discretization suffers from a large number of discrete cells.Artificial potential field method has made significant achievements in static or dynamic environments.Moreover, the application in automatic parking demonstrates its capability for the nonholonomic system.However, the artificial potential field method is likely to get stuck in a locally optimal path and cannot handle the multi-maneuver parking problem because of local planning characteristic.Sampling-based method randomly generates samples in robot configuration space or robot state space and attempts to connect the new samples to the generated graph or tree using local planning methods.Since sampling-based methods do not represent the obstacles free configuration space explicitly, they are computation effective and suitable for high-dimensional space.However, the smoothness issue and solution quality concerning some constraints and objectives need to be considered seriously [20].On account of the solution quality problem, nonlinear optimization methods can treat robot dynamics correctly, and some concerned objectives and constraints can be constructed to satisfy the task specification.Nevertheless, due to the presence of obstacles, the feasible set is discontinuous, and the nonlinear optimization problem is nonconvex, which increase the difficulty of the problem and the computation complexity.
Optimal control [27]- [29] is a remarkable method to generate high-quality trajectories for robots and has achieved great success in practical applications.It considers robot dynamics and other trajectory constraints in a compact and unified form, and it can deal with any predefined optimization objectives.The application of the optimal control method in robot motion planning involves two essential constraints, robot dynamics constraint, and geometry collision-free constraint.However, the applications are limited to elementary geometries like circles in [21] and rectangles in [22].In this work, we make the optimal control method applicable to complex scenarios with rectangles assumption and solve this problem at high speed.

A. Problem Analysis
The challenge of making optimal control method applicable in complex motion planning problems arises from how to represent the geometry collision-free constraints effectively.The point-point distance of circles in [21] and area criterion of rectangles in [22] are both straightforward collision avoidance methods.However, circle bounding volume approximation is too conservative to realize motion in complex and highprecision scenarios.And area criterion of rectangles is nonlinear and redundant, such that the optimal control problem is difficult to solve.In addition, the collision-free constraints for high-precision motion planning problems in 3D environment are still difficult to be built.We will show in this paper the  2 -function [30], [31] settles these challenges, and achieves fast high-precision motion planning for autonomous parking.The  2 -function is linear programming for collision checking between convex polyhedrons in any dimensional space, and it behaves as a distance function that  2 > 0 indicates collision free.Treating  2 ≥  > 0 ( is a safety distance, and  2 is a function of robot trajectory) as a constraint for the overall optimal control problem, a special  2 -function based bilevel optimal motion planning (BOMP) model is obtained.
The BOMP model contains an optimization problem within the constraints of the upper optimal control problem.Moreover, the lower optimization problem depends on the continuous robot state trajectory such that it is also infinite dimensional.To our knowledge, this particular model has not appeared in the literature, but the complexity of this model can be seen indirectly from some other problems.In [32], a relative simple bilevel optimal control problem is considered.The upper level is an optimal control problem concerning ordinary differential equations, control constraints, initial and final state constraints, and the lower level problem depends only on the final state of the physical system and is finite dimensional.In this case, nonsmooth analysis, optimization in Banach spaces and bilevel optimization [33], [34] are used to derive the necessary linearized Pontryagin-type optimality conditions.By nonconvex, non-differentiable or possibly disconnected, the hierarchical bilevel optimization problem is intrinsically difficult.Even for the most straightforward linear-linear bilevel optimization problem, it was proven to be strong NP-hard [35] and that merely evaluating the optimality is also NP-hard [36].
Two significant challenges are classified from the BOMP model, the bilevel problem, and the optimal control problem.Optimal control has experienced considerable development in mathematics and engineering.In particular, pseudospectral optimal control (PSOC) method [29] satisfies the differential equations globally and treats integral by an implicit Runge-Kutta method, such that this method achieves high order accuracy, high order stability, and exponential convergence speed.Therefore, the PSOC method is selected.For the bilevel optimization problem, the Karush-Kuhn-Tucker (KKT) reformulation of the lower-level optimization problem is always used to construct a traditional single level optimization problem [37].The KKT reformulation can not be applied to the BOMP model seemingly since the BOMP problem is an optimal control problem rather than an optimization problem.However, the PSOC algorithm discretizes the whole variables and approximates them as polynomials, such that it actually solves a large scale sparse nonlinear optimization problem (NLP).In this case, the KKT reformulation can be directly used to the discretized NLP.Furthermore, differential equations do not occur in the lower level of the BOMP model, the continuous KKT reformulation (the Lagrange multipliers are also trajectory functions of time) concept can be utilized unambiguously.
However, due to the nonconvexities in the KKT conditions, even the upper-level problem is also convex, the converted problem is still hard to solve.Besides, the complementary constraint is intrinsically combinatorial and makes the extent of violation of KKT conditions in a small neighborhood of the KKT point nonsmooth [38].Hence, the convergence peoperty in the neighborhood of the optimal value is poor, and it cannot provide efficient information to determine whether current value is close enough to the optimal value.Dutta [38] proposed a modified approximate KKT (MAKKT) theory that the KKT conditions are relaxed and the violation in the neighborhood is smooth enough.Since the KKT conditions are relaxed, an iteirative strategy is used to decrease the relaxation factor to approach the optimal value.Therefore, the MAKKT theory and iteirative convergence strategy are selected in this paper.

B. Contributions and Organization
In this paper, we present a general BOMP model in which the upper level is an optimal control problem designed for robot nonlinear dynamics, while the lower level is the  2 -function linear programming for geometry collision-free constraint (in Section II).The MAKKT theory is used to simplify the model to a traditional optimal control problem, then the PSOC method is used to solve the converted problem (in Section IV).Besides, a modified  2 -function (MJ) and an active points based modified  2 -function (APMJ) are derived in Section III to reduce time complexity.An iteirative convergence strategy combines the MJ function and APMJ function naturally, and a two-stage BOMP algorithm is proposed.The highlights of this paper are: 1) The BOMP model combines both advantages of the flexibility of optimal control and the simplicity of linear programming, which makes optimal control a fast and high-precision method for complex motion planning problems.2) For the two-stage algorithm, the MJ function is used in the initial stage to generate an initial collision-free approximate optimal trajectory and to pick out the active points, while the APMJ function is used in the final stage to find out the exact optimal trajectory.3) The BOMP algorithm benefits by the iterative convergence strategy and the simplicities of the MJ function and the APMJ function.Simulations in autonomous parking demonstrate that the computation speed increases almost two orders of magnitude compared with the area criterion based method (in Section V).
Finally, concluding remarks are given in Section VI and the issues to be researched in this field are also pointed out.

II. Bilevel Optimal Motion Planning
Optimization problem finds an optimal point, while optimal control problem finds an optimal trajectory.So it is a natural question: how to use the optimal control method to solve vehicle trajectory generation and optimization problem in complex and high-precision scenarios?As stated in Section I, the geometry collision-free constraint is the crucial matter.In this section, we will present the BOMP model with an embedded standard linear programming collision avoidance constraint, and the exact  2function linear programming which acts as a distance function will be presented in section III.
Vehicle is a typical nonholonomic constrained system, and its motion planning is very complicated.Without loss of generality, the front steering wheels vehicle is considered and the following kinematics model is used: where  = (, , ) ∈ ℜ ) describes some objective along the trajectory such as the energy consumption and 0 ≤  ≤ 1 is the weight.A particular performance index is to minimize the weight sum of completion time and energy consumption: The problem (2) is a traditional optimal control problem that can handle any nonlinear dynamics and (piecewise) continuous trajectory constraints.Adding point-point distance constraints between circles [21] or area criterion constraints between rectangles [22], a motion planning model can be obtained.However, neither collision avoidance methods can be used to general fast high-precision motion planning problems.Adding a standard linear programming collision avoidance problem as an embedded constraint to (2), the BOMP model is proposed: where  ∈ ℜ  and  ∈ ℜ  are constant vectors,  ∈ ℜ  is an extra added time dependent variable, and  ∈ ℜ × depends on vehicle state variable .Besides,  is the optimal value of the linear programming problem, it's a time-dependent scalar variable and  is a constant scalar.Therefore, the linear programming (4e) represents constraints on vehicle state trajectory.Suppose that the linear programming (4e) represents a distance function between vehicle and the surrounding obstacles, and  > 0 denotes collision free.Then, the constraints (4d) and (4e) denote that the vehicle configuration  is in the -interior of collision-free space [12], that is: As we can see, the optimal control model (the upper-level problem) (4) contains an embedded optimization problem (the lower-level problem) as its constraint, and to our knowledge this model has never appeared in the literature before.Though this is a bilevel optimal control problem, the complexity and convergence analysises can be seen indirectly from the bilevel optimization problems as stated in section I-A.In section III, we present the  2 -function collision avoidance method which checks collision between convex polyhedrons in any dimensional space, and can be applied to many domains.However, this paper focuses on fast high-precision autonomous parking problems, the general applicability of the BOMP model will be discussed in future works.

III. Collision Avoidance
The performances of robot collision-free motion planning algorithms depend highly on the geometry collision avoidance methods used between robot and the environment.In real-world, robot and environment geometries are always sophisticated such that the approximation techniques are always utilized to simplify the collision avoidance problem.Polyhedron or sphere approximation in 3D environment and polygon or circle approximation in 2D environment are the conventional methods.In this section, point-point distance constraints between spheres (or circles), area criterion constraints between convex polygons and the  2 -function are summarized first.The  2 -function represents one collision avoidance method between convex polyhedrons in any dimensional spaces.The in-depth geometric interpretation of the artificial variables in the  2 -function is presented.Then the MJ function and the APMJ function are proposed according to the geometric properties.

A. Basic Collision Avoidance Constraints
Suppose robot  is approximated by a sphere or circle of radius   that covers it, and obstacle  is approximated by a sphere or circle of radius   .The centers of the approximate objects are   and   .Then the geometry collision-free constraint between  and  is approximated as: where  (   ,   ) denotes the Euclidean distance between the two points.Usually, the robot and the obstacles are approximated by several overlapping spheres or circles to increase approximate precision, in this case, the geometry collision-free constraints are still straightforward.Suppose plane robot  and plane obstacle  are approximated by convex polygons with   vertexes and   vertexes, respectively.The vertexes coordinates of the polygons are Then the area criterion can describes the geometry collision-free constraints between  and : where  △ is the area of a triangle,   and   are the areas of the two approximate polygons.Moreover, for polyhedrons in 3D environment, the volume criterion can derive the collision avoidance constraints.
As we can see, for a pair of polygons, there are   +   nonlinear constraints in (7), and this number is usually larger than the circle approximation method.However, in theory, a complex geometry can be incorporated by several polyhedrons or polygons in any precision and low overlapping rate with fewer approximate objects than the sphere or circle approximation method.And to achieve planning and control in scenarios with long and thin obstacles, the polyhedron or polygon approximation is more efficient.Therefore, the convex polyhedron or polygon approximation method will play an important role in complex and high-precision robot motion planning problems.

B. Definition and Basic Properties of the 𝐽 2 -function
Given two convex polyhedron objects that are represented by the convex hulls of two point sets respectively, i.e., conv and conv.Where   ∈ ℜ  and   ∈ ℜ  are point coordinates,  is the dimensionality,  1 and  2 are the point numbers.Then the  2 -function checks whether the two objects conv and conv are colliding or not, and its form is: where The vector  and scalars  +1 ,  +2 are called artificial variables.In constraint (8a), the symbols  and  are actually matrixes composed of the points coordinates, i.e., , so in this paper, the notations of point set and matrix are not distinguished explicitly, and the true meaning can be seen directly and easily through context.Besides, when referring to a point set of one object, it is actually the convex hull of it is used to characterize that object.Throughout this paper, vector inequality is to be understood element-wise.
The  2 -function has the following basic properties: (a) the feasible region (8a)-(8d) is nonempty; (b) the range of the optimal value is 0 ≤  2 ≤ 2; (c)  2 > 0 is equivalent to conv ∩ conv = ∅, i.e., collision free between the objects; (d) it provides a concept of pseudodistance between convex polyhedrons and is pseudomonotonic with respect to the Euclidean distance.

C. Geometric Interpretation of the 𝐽 2 -function
Suppose   =  1 =1   and   =  2 =1   , then 0 ≤   ,   ≤ 1 and they denote the fractional shrinks of objects  and  with respect to the origin respectively, as shown in Fig. 2(a).In this way, the meaning of the artificial variables  +1 and  +2 can be seen directly: From the definition of   , we can get  =     where   ∈ conv.So in order to simplify the understanding of the artificial vector , a point set  and a point  are considered in the  2 -function.We first make the assumption that   =   = 1 (or  +1 =  +2 = 0) and concentrate on the meaning of vector .Since  ≥ 0, from the constraint (8a), the basic feasible domain is  ∈ conv + ℜ  + where ℜ  + denotes the nonnegative orthant.We then let conv + = conv + ℜ  + and make the assumptions that  ∈ conv + and  ∩ conv = ∅ as the case shown in Fig. 2(b) (the exception cases will be explained later).Combined with the objective of the  2 -function, it can be seen that the  1 norm of the optimal solution  * equals to the optimal value  2 ( , ), that is, the artificial vector  measures the  1 distance between two convex polyhedrons.And the following two geometric problems ( 10) and ( 11) can be obtained.
Consider a point set , a point  and suppose that   =   = 1 and  ∈ conv + , then the  2 -function is equivalent to the following geometric problem: where   denotes the level set of the  1 distance function with respect to point .
Consider point sets  and , and suppose that   =   = 1 and conv ∩ conv + ≠ ∅, then the  2 -function is equivalent to the following geometric problem: where   denotes the level set of the  1 distance function with respect to object .
As the geometric meaning of ,  +1 and  +2 are clear, we now discuss how the  2 -function works when  ∉ conv + .This case can be categorized into three subcases as shown in Figs.3(a)-(c): (a) 0 ∈ int conv + and  ∉ conv + ; (b) 0 ∈ bd conv + and  ∉ conv + ; and (c) 0 ∉ conv + and  ∉ conv + .Where int conv + and bd conv + denote the interior and the boundary of conv + , respectively.In subcase (a), ∃  > 0,    ∈ conv + such that the linear programming can be solved.In subcase (b),   must equals zero to make the constraint feasible.While in subcase (c), the optimal solution can be obtained intuitively:  *  = 0,  *  = 0 and  2 = 2.In the end, we point out that the optimal value  2 = 0 only occurs when Fig. 3. Some special relationships of a point  with respect to a convex polygon  in the plane.

D. Modified 𝐽 2 -function
The geometric meaning of the artificial variables states that the  2 -function achieves to maximize the shrinks   ,   as well as minimize the  1 distance between   conv and   conv with constraint   conv ∩ (  conv + ℜ  + ) ≠ ∅.In this subsection, we will show the  2 -function can be realized by only maximizing   and get the geometric problem (12).
Given point sets  and , and suppose that 0 ∈ int conv , then the  2 -function is equivalent to the geometric problem: When 0 ∈ int conv  and  ∉ conv + , the correctness of problem (12) can be seen directly from the geometric analysis in Section III-C.Therefore, we suppose 0 ∈ int conv  and  ∈ conv + , and a rectangle  and a point  as shown in Fig. 4(a) are used to illustrate intuitively of the proof.Since And if  ∉ conv, then as   approaches zero,    intersects the boundary of conv at first.Besides, if   gets small, then   will become smaller to make    intersect the boundary of   conv.As a result, the optimal value  *  equals one (the same result can be obtained from the view of the  1 distance between point  and object ).Now, only the shrink   and the  1 distance influence the objective value of the  2 -function.Suppose  ∉ conv, then the  1 distance gets small as   approaches zero, so the  2 -function achieves a balance between them.To compare the significance of these two factors in the  2 -function, they are considered independently.Let  ′  denotes the optimal solution of max   ∩conv≠∅   and let  1 =  ′  .Let  ′ denote the optimal value of (10),  2 =   ′ ∩ conv,  denote the normal of the surface from conv  that intersects with   ′ , and  3 denote the intersection point of the line   2 and the subspace whose normal is .The notations  1 ,  2 ,  3 and  are shown in Fig. 4(a), then: It can be seen that  ′ is much bigger than 1 −  ′  , such that  ′ According to the geometric problem (12), if 0 ∈ int conv , the MJ linear programming is: And its compact form: where The MJ function reduces  + 2 variables and increases computation speed.The optimal value is 0 ≤  2 ≤ 1, while  2 = 1 corresponds to the case that  is at infinity.

E. Active Points based Modified 𝐽 2 -function
In fact, the collision between two relatively moving polyhedrons always arises between the closest components, face/vertex, vertex/face or edge/edge [39], so the collision avoidance is equivalent to the closest components are collision free.According to this fact, if the closest components can be determined, the instantaneous geometric representation of each object can be simplified for collision avoidance problem.In this subsection, we make the assumption that the given two objects are not colliding such that the concept of closest component is meaningful.Fig. 4(b) shows two relatively moving plane polygons  and  that statisfy the assumption.Then at the instant, each polygon can be characterized by the red points to avoid collision.In this paper, these points are called active points and they correspond to the nonzero elements of the optimal solution  * ,  * in the  2 -function: Using   and   , an APMJ function can be obtained: where  1 and  2 are the active points numbers.Note that the APMJ function is not an optimization problem, it is just a constrained system of linear equations, besides, the cardinalities of point sets   ,   are very low.Therefore, the APMJ function reduces the computation time dramatically.However, given two objects arbitrarily, the active points between them are not transparent.Moreover, in robot motion planning problem, the robot motion trajectory is not known first, let alone the active points.As a consequence, the APMJ function can not be used straightway, some techniques have to be utilized to pick out the active points first.In Section IV, a two-stage BOMP algorithm is proposed.The MJ function at the initial stage finds an approximate optimal trajectory, and then the active points along the trajectory are picked out to feed into the APMJ function to construct the final stage algorithm.The related theory, and the two-stage algorithm details will be discussed in the following sections.

IV. Solution of the BOMP Model
where  and  are point sets described in the obstacle coordinate system.Therefore,  is constant, and  is a function of robot state .More precisely, , ,  are trajectory functions of time .Therefore, the constraint  2 ( , ) ≥  > 0 shows robot motion is collision-free.

B. Model Solution
Given the problematic nature of the BOMP model, it is helpful to reduce the overall bilevel optimal control problem to a traditional single level optimal control problem.According to the optimality conditions of linear programming [40] and noticing the constraint (18d), the constraint 1 +  T  ≥ 0 is not active.So in the lower level optimization problem (18e), there are Lagrange multipliers  ∈ ℜ  1 + 2 ≥ 0 corresponding to constraint  ≥ 0 and multipliers  ∈ ℜ +1 corresponding to constraint   = . and  are trajectory functions of time , then the continuous MAKKT form of the lower problem is: where (19a) are feasible conditions, (19b) are equilibrium and complementary conditions and  > 0 is the approximate relaxation.A point  statisfing these constraints is called an -MAKKT point (or  approximate optimal point), and note that when  = 0 it's actually the KKT point (or optimal point).
Since in the BOMP model the optimal value  2 ( , ) of the lower level optimization problem is also restricted, the relation between the optimal value and the approximate optimal value also needs to be considered.The Theorem 3.5 in [38] helps the analysis, that is, at an -MAKKT point, the objective value 1 +  T  is at least  larger than  2 ( , ).Consequently, the converted single level optimal motion planning problem for ( 18) can be formulated as follow: min (  ,   ,   ) + (1 − ) The relationship between the MAKKT and the exact KKT optimality conditions is as follow: if a sequence of points {   } converge to a point p where the Mangasarian Fromovitz constraint qualification is also satisfied, each point   is an   -MAKKT point, and   → 0 as  → ∞, then the point p is a KKT point.A robot state satisfying constraints (18d) and (18e) means that the configuration is in the -interior of free space, i.e.,  ∈ int  (C free ) [12].Similarly, we define a state  ∈  satisfying the constraints (20d)-(20f) means that the configuration is an --interior configuration.And the -interior of C, denoted as -int  ( C), is defined as the collection of all --interior configurations.According to the relationship between the MAKKT and the exact KKT optimality conditions, -int  ( C) converges to int  ( C free ) as  converges to zero (seen in Fig. 5).
Therefore, in order to solve problem (18), a decreasing sequence  should be applied to problem (20) and a sequence of approximate optimal trajectory is solved to converge to the exact optimal trajectory.When  is large, problem (20) is easy to solve an  optimal trajectory.Then decrease  and choose the previous trajectory as an initial guess to solve a new trajectory until  converges zero.However, as  approaches zero, the nonconvexities and combinatorial property in the MAKKT conditions become highlighted, then the problem becomes hard to solve or nonconvergent.Under this circumstance, a new Fig. 5.The square  is free to translate with fixed orientation, thus its configuration can be described by the coordinate of its center point and the configuration space C is the plane.The square  is the obstacle and the coordinate system is origined at its center.The label "" denotes the obstacle, the label "free" denotes the boundary of obstacle free configuration space C free , the label " = 0.00,  = 0.1" denotes the int  (X free ) and the other labels denote -int  (C).As we can see, -int  (C) converges to int  (C free ) as  converges to zero.
Fig. 6.The reason why the APMJ function works.The solid line is an approximate optimal trajectory for object  moving from   to   , and a small variation does not change the active points between the two objects.convergence technique needs to be sought.The APMJ function settles this issue, and why it works is as follows.
As  approaches zero, it will solve a collision-free approximate optimal trajectory first, and the exact optimal solution is a small variation of it as shown in Fig. 6, where the solid line and the dashed box are the approximate trajectory and an instant optimal location, respectively.As we can see, the small variation does not change the active points between the objects, so the active-points from the approximate trajectory can characterize the collision behavior precisely.Pick out the active points along this trajectory to construct the APMJ function (17), then the following motion planning model can be obtained to solve the exact optimal trajectory: min (  ,   ,   ) + (1 − ) where the notations , ,  need to be distinguished from those in (20).In this model, , ,  correspond to the active points whose cardinality is very low.Besides, the multipliers ,  and the complementary and equilibrium constraints disappear, so this model is much more easy to solve.

C. The BOMP Algorithm
Combining the above theories and techniques, a two-stage BOMP algorithm framework is formulated for robot trajectory generation and optimization mission.The algorithm details are shown in Algorithm 1, where steps 1-10 correspond to the initial stage, and step 11 is the final stage.In the initial stage, the MJ function is used to construct the motion planning problem (20), to generate a collision-free approximate optimal trajectory and to pick out the active-points along the trajectory.As the relationship between the -MAKKT point and the exact KKT point, a decreasing sequence  is adopted in the initial stage to solve a series of approximate optimal trajectories.The final stage figures out an optimal trajectory using the APMJ function based model (21).The PSOC method solves both problems ( 20) and ( 21), and the discrete nodes number in the two stages are 15 and 30, respectively.In order to limit the length of this paper, the PSOC method details are omitted, and readers can consult the reference paper [29].The IPOPT [41] algorithm of version 3.12.9 is used to solve the discrete large scale NLP.The IPOPT algorithm is designed with special options, such as convergence tolerance 1 −12 , MA86 linear solver and the acceptable termination conditions are more strict than the desired ones to avoid algorithm early termination.The IPOPT algorithm options are listed in Table I.In the end, we point out that the little trick, step 9, used in the BOMP algorithm reduces the collision avoidance constraint numbers dramatically.That is, if the robot is very far away from an obstacle, then the collision avoidance constraint does not need to be considered at that moment.The little trick picks out the right critical collision avoidance constraints, and these constraints are called active constraints in this paper.

7:
Let  ←  + 1. 8: Let  = 30 and get the interpolated trajectory .9: Pick out the active constraints along the trajectory  according to the rule 1 +  T  ≤   .10: Pick out the active points for the active constraints.11: Construct problem (21) using the active points and solve it by the PSOC method with the initial guess  to get the optimal trajectory  * .V. The BOMP Model Verifications In this section, the simulations of the BOMP algorithm in autonomous parking problem are shown.The computational and precision benefits of the BOMP model over the area criterion (AC) based model [22] and the circle approximation method are demonstrated.And a real experiment on Turtlebot3 is conducted.C++ code is programmed in Linux system, and simulations are conducted on an Intel Core i7-7700K CPU with 8GB RAM that runs at 4.20GHz.

A. Simulation in Autonomous Parking
The vehicle kinematics model ( 1) and the optimization performance index (3) are used in this section.As the parking process is in low speed and to bound the trajectory curvature and its derivative [22], the following mechanical and physical constraints are considered.
And the initial and the final state constraints are treated as: where ( 0 ,  0 ,  0 ) is the vehicle initial configuration, (   ,   ) is the coordinate of vehicle corner point  described in {}, (  ,   ), (  ,   ) and (  ,   ) have the similar meanings,   ≥ 0 and   ≥ 0 denote the position deviation and the orientation deviation of the vehicle central axis with respect to the parking spot central axis, respectively.Therefore, the constraints (1), ( 22) and ( 23) form the upper-level constraints of the BOMP model.In this application, four scenarios and three cases in each scenario are designed to reflect the generality, the robustness and the advantages of the BOMP algorithm.Fig. 7 illustrates the four scenarios and one case in each scenario.Red vehicles are obstacles, the black vehicle denotes its initial location, and the dashed vehicle denotes the ideal parking.All the four scenarios correspond to irregularly placed obstacles that can be encountered easily in daily life and the differences between cases in each scenario are only the initial configurations of the vehicle (see Table II, the abbreviations , ,  represent the scenario, Fig. 7. Four scenarios used to conduct the simulations.Red vehicles are obstacles, the black vehicle denotes the initial location, the dashed vehicles illustrate the ideal parking and each obstacle is the same size as the vehicle.The first two scenarios demonstrate the parallel parking and the third one demonstrates the vertical parking.While the last scenario is not usual in daily life, it's just designed to show the algorithm's capability. TABLE II.Transformation parameters (, , ) of each obstacle with respect to the world coordinate system and the vehicle initial configuration ( 0 ,  0 ,  0 ) in each case.The angle unit here is degree, but at other place it is radian.the obstacle and the case, respectively).For simplicity, each obstacle is the same size as the vehicle and collision avoidance is equivalent to the plane rectangle pair is collision free.The transformation parameters (, , ) of the obstacle coordinate system (it is originated at the blue corner point of the obstacle) relative to the world coordinate system are used to represent each obstacle compactly.These transformation parameters (, , ) are listed in Table II.
The specified simulation parameters for the parking spot, the vehicle and the Algorithm 1's input are listed in Table III, where the vehicle size parameters, mechanical and physical constraints parameters are originated from [22].As for the initial trajectory guess  0 in Algorithm 1, it is specified very easily.Guess the parking completion time arbitrarily, then  0 is composed of () =  0 , () =  0 ,  () =  0 , () = 0, () = 0, () = 0, () = 0, () = 0, () = 0 for any .At last, we point out that the coordinate system used for the  2 -function is established at the center of the vehicle, i.e., the inverse coordinate transformation is applied to transform the obstacles into vehicle center coordinate system.Under these conditions, the BOMP algorithm solves all these 4 × 3 problems with the same sequence  ∈ {0.1, 0.01}.That is, through two iterations in the initial stage and one iteration in the final stage, the BOMP algorithm solves the whole autonomous parking problems.The values in the  sequence are much larger than the algorithm convergence tolerance 1 −12 , so the two problems in the initial stage are very easy to solve.The computation results are shown in Table  Then the computation complexity comparison between the BOMP algorithm and the AC algorithm is made.Given a point  and four corner points , , ,  of a rectangle in the plane, the AC collision avoidance constraint used in this paper is: where  △ and  □ denote the area of a triangle and a rectangle, respectively.And there are eight nonlinear constraints (24) for a pair of plane rectangles collision avoidance problem.Then The active constraints threshold 0.30 replacing (18d) and (18e) by these constraints, the AC based motion planning problem can be obtained, and the AC algorithm is shown in Algorithm 2. Correspondingly, steps 1-2 are called the initial stage, and step 3 is the final stage.Because the AC does not define distance explicitly, the active constraints filtrating process does not appear in Algorithm 2. Since in [22], the IPOPT options and the computation time were not shown, the algorithm options in Table I are used.And the initial guess  0 is selected the same way () =  0 , () =  0 ,  () =  0 , () = 0, () = 0, () = 0.However, the AC algorithm does not solve the whole autonomous parking problems and it fails in the initial stage.The results are shown in Table IV and Fig. 8.According to the concept that a near-optimal initial guess always facilitates the solving of a complicated problem.And in order to show the impact of the active constraints filtrating process, the collision avoidance constraints in the BOMP algorithm's final stage (step 11) are just replaced by the AC based method.That is, the MJ function in the initial stage generates a collision-free approximate optimal trajectory , picks out the active constraints, and then the AC method figures out the

Algorithm 2 The AC algorithm
Input: An initial trajectory guess  0 , the discrete nodes number  in the PSOC method.Output: The optimal trajectory  * .
1: Let  = 15, using the area criterion constraint (24) to construct the motion planning problem and solve it by the PSOC method with the initial guess  0 to get a new trajectory .
3: Construct the new problem and solve it by the PSOC method with the initial guess  to get the optimal trajectory  * .
▲ ▲ ▲▲▲▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲▲▲▲ ▲ ▲ optimal trajectory using  as the initial guess.This algorithm is called the MJAC algorithm.Under this circumstance, the MJAC algorithm solves all these 4 × 3 problems, and the results are shown in Table IV and Fig. 8.The success rates of the MJAC algorithm and the AC algorithm show that a good initial guess reduces the difficulty of the AC algorithm.The computation time in the final stage of the MJAC algorithm and the AC algorithm demonstrates that the active-constraints filtrating process accelerates the computation speed several times.From Table IV, we can see that the shortest and the longest time of the BOMP algorithm to find an optimal collision-free trajectory for the 12 problems are 2.18 and 7.30 seconds, respectively.For the MJAC algorithm, they are 12.65 and 60.90 seconds.While for the AC algorithm in the solved problems, they are 127.66 and 208.51 seconds.The reasons why the BOMP algorithm has significant superiorities in computation speed may be: •  2 -function collision avoidance is linear programming, while the AC based collision avoidance method needs to calculate the distance between points, then the area of a triangle, such that it has a high degree of non-linearity.Linear programming is the most well-studied problem, and finding the solution of ▲ ▲ ▲ ▲▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲ ▲▲▲ ▲ ▲    Comparisons of the trajectory results between the two stages in the BOMP algorithm.The green and purple trajectories are the results from the initial stage, while the black and magenta trajectories are from the final stage.The results demonstrate that the final exact optimal trajectory is a small variation of the previous approximate optimal trajectory, and the APMJ function convergence strategy is appropriate.
it is intrinsically much faster than the problem with nonlinear constraints.• Variables in the  2 -function make the BOMP algorithm only concern the closest components implicitly in the initial stage.However, the AC based collision avoidance method needs to consider the eight points between a pair of rectangles simultaneously and explicitly, whose optimization direction is difficult to determine.• The BOMP algorithm finds a sequence of solution progressively approaching the optimal solution such that it significantly reduces the difficulty to solve the problem, while the AC method uses a global step to find the solution which tends to break down in complicated problems.
Limited by the length of this paper, the optimized vehicle motion and the optimized trajectories of state and control variables by the BOMP algorithm are just partly plotted in Figs.9-11.The results demonstrate that the BOMP algorithm is capable of handling parking motion planning problems successfully and efficiently.Besides, the BOMP algorithm can autonomously park a vehicle in a much more complicated scenario that needs several maneuvers like a skillful driver.Meanwhile, the terminal orientation deviation and the position deviation are considered, so the vehicle stops as parallel and close as to parking spot central axis.It is worth emphasizing that all these 4 × 3 simulations are conducted without adjustment of any algorithm options, like , and this shows the robustness, generality, and unification of the BOMP model and the BOMP algorithm.However, because the vehicle kinematics, mechanical and physical constraints in our application is similar to that of [22], the control variables smoothness issue and terminal steering angle issue also exist in the BOMP algorithm, readers may consult that paper for the reasons.These issues are inherent in the modeling process and need some unique but not complicated techniques to overcome.Nevertheless, this paper focuses on the BOMP model and the solution to it, so these issues are left to resolve.

B. Polygon VS Circle Approximation
One motion planning problem in scenario 3 of the previous section is used to illustrate the different impacts of polygon and circle approximation methods.The planning task is to let the vehicle move from the initial configuration (1.0, −2.0, 0.0) to the final configuration (1.0, 10.0, /2.0).Fig. 12(a) shows the result of the BOMP algorithm, and the vehicle and obstacles are still represented as rectangles.Fig. 12(b) shows the result of circle approximation method.The vehicle and the obstacles are approximated by three overlapping circles of radius 1.3m (the red circles), respectively.The results demonstrate that the circle approximation makes the object expanding, and the vehicle can not pass through narrow passages.To approximate the object exactly, there should be many more different radius circles, and this will increase the computation time dramatically.The drawbacks of circle approximation will become increasingly severe if the ratio of rectangle width and length reduces further.Because of the high approximation precision and low overlapping rate of the polyhedron or polygon approximation method, the simplicity of  2 -function and the fast computation speed of the APMJ function, the BOMP model will compensate motion planning methods in many domains.

C. Physical Experiment
We performed a physical experiment involving a mobile robot (TurtleBot3) to verify the correctness and effectiveness of the BOMP model (see in Fig. 13).To simplify the experiment, the robot dynamics was not considered, and the robot moved at very low speed.Using SLAM technique, the environment geometry can be obtained and then through convex decomposition [42] the polygons information are available.Then given an initial robot location, the global control commands (linear and angular speed) are calculated by the BOMP algorithm.In this experiment, the time-optimal trajectory is sought, and the global control variables trajectories corresponding to Fig. 13 are shown in Fig. 14.When the robot is executing the control commands, the dynamic uncertainty and drift will accumulate the driving error.When this error exceeds tolerance, replanning process is triggered.The details of robot odometry, localization, control framework, error accumulation calculation, and replanning cycles will be presented in our future works.The videos of this experiment are available at http://www.hust.edu.cn.

VI. Conclusion and Prospects
In this paper, we have proposed a general and unified BOMP model for robot trajectory generation and optimization in obstacles environment, which makes the optimal control method a practical approach for complex high-precision robot motion planning problems.The upper-level optimal control is designed for robot nonlinear dynamics, while the lower-level  2 -function is for geometry collision-free constraint.Simulations in complex autonomous parking scenarios and experiment on Turtlebot3 demonstrate the computation superiorities and efficiency of the 2) The BOMP model is an open framework that any userspecified constraints can be incorporated into the upperlevel or the lower-level constraints.And the BOMP model is dimensionality independent.Therefore, this model can be utilized in manipulators, vehicles and humanoid robots, etc. 3) The BOMP algorithm makes full use of the convergence property of the MAKKT theory and applies the MJ function and the APMJ function in separate stages.This convergence strategy makes the BOMP algorithm very fast.However, many aspects need to be studied deeply and comprehensively.First, one main issue is the computation efficiency for robots moving in cluttered and obstacles intensive environment.The future studies should be concentrated on the initial stage of the algorithm, and one possible solution is to combine the previous research theory, like sampling based methods.While in real applications, some simple techniques can be used.Take the parking problem as an example [23], make finite classifications of the parking cases and calculate the trajectory offline to construct a database.Then in real parking process, pick out a closest recorded solution to the current situation to initialize the algorithm and finally solve the problem online.Second, motion planning for robots in dynamical environment with moving obstacles are regarded as the most difficult, important and significant research field.
Whereas the BOMP model only solves the static problem at present, there are still extensive works to adopt the obstacles movement prediction and estimation theory into the BOMP framwork.Third, with appropriate objectives and constraints in different levels, the multilevel problems have a close connection to multiple objectives optimization problems.However, solving the multilevel problem is much more difficult than the bilevel problem, and the related theories have not been proposed.All these three unresolved issues are challenging and meaningful for the development of intelligent robots and will be discussed in our future works.Moreover, despite the simulation results and simple experiment on Turtlebot3 in this paper, extensive work is needed to conduct experiments on real complex applications and to verify the proposed algorithm.

Fig. 1 .
Fig. 1.The coordinate systems attached to the vehicle and the parking spot.

Fig. 2 .
Fig. 2. The geometric meaning of the artificial variables ,  +1 and  +2 .(a) The shrinking model of convex polygon  with respect to the origin and  is the fractional shrink. +1 and  +2 are related to the fractional shrinks of objects  and .(b) The  1 distance model between a point and a convex polygon and  denotes the vector from point  to .

𝐵 1 −Fig. 4 .
Fig. 4. (a) The significance comparison between the shrink   and the  1 distance.(b) The active points (red points) can characterize the collision behavior at the instant.
A.  2 -function Based BOMP Model Consider a convex polyhedron robot  moves around a static convex polyhedron obstacle  from an initial state to a final state.And suppose that the purpose is to generate an optimal smooth collision-free trajectory.According to properties (c) and (d) of the  2 -function, a new constraint  2 ( , ) ≥  > 0 and the MJ function replace the constraints (4d) and (4e), an exact  2 -function based BOMP model is obtained: min (  ,   ,   ) + (1 − )

4 : 5 :
Solve problem(20) by the PSOC method with the initial guess   to get a new trajectory  +1 .if the trajectory  +1 is collision free then 6:

Fig. 8 .
Fig. 8.Total computation time of the BOMP, the MJAC and the AC algorithms used to solve the whole 12 simulation cases.Missing values in the AC algorithm mean that the algorithm fails to solve these problems.

Fig. 9 .
Fig. 9. Optimization results for scenario 2 case 3 by the BOMP algorithm.In (a), the orange rectangles represent the vehicle initial and final locations, and the magenta curve shows the motion trajectory of vehicle rear wheel axis mid-point.The state trajectories in (b) are very smooth, while the control trajectories in (c) are oscillating.The state and control trajectories in all other problems have the same characteristics.

𝑆 4 𝐶 1 Fig. 11
Fig.11.Comparisons of the trajectory results between the two stages in the BOMP algorithm.The green and purple trajectories are the results from the initial stage, while the black and magenta trajectories are from the final stage.The results demonstrate that the final exact optimal trajectory is a small variation of the previous approximate optimal trajectory, and the APMJ function convergence strategy is appropriate.

Fig. 12 .
Fig.12.The vehicle motion obtained by the BOMP algorithm and the circle approximation method, respectively.The green rectangle in (b) represents the vehicle at an instant moment, and the three overlapping circles represent the approximation of the vehicle and the obstacles.

Fig. 13 .Fig. 14 .
Fig.13.The environment setup for this experiment.There are seven obstacles in a very small area (1.5 × 1.5) and the goal of all the experiments is to let Turtlebot3 stop at the center of the white square which is 0.5 × 0.5.
2×  is the configuration of the vehicle coordinate {} with respect to the world coordinate {} which origins at one corner point of the parking spot, see Fig.1.The vehicle coordinate {} origins at the mid-point of the rear wheel axis, (, ) describes the vehicle position, and  denotes the orientation.denotesthelinear velocity of point (, ),  denotes the steering angle of the front wheel and  denotes the steering velocity.Consider vehicle state variable  as  = (, , , ) and the control input  = (, ), then vehicle kinematics model (1) can be abstracted as  =  (, ).  and   are vehicle initial and final states, respectively.However, at some times, the vehicle needs to stop at a goal set instead of an exact goal state, i.e. (  ) ∈   , e.g., the vehicle stops inside a parking spot other than a perfect location.The cost function (  ,   ,   ) depends only on the initial state   , final state   and the completion time   , while (,

TABLE I .
The IPOPT algorithm options.

TABLE III .
The specified parameters for the parking spot, the vehicle and Algorithm 1's input.

TABLE IV .
The computation results comparison between the BOMP, the MJAC and the AC algorithms, where I and F denote the computation time in the initial stage and final stage, respectively, T is the total computation time and  * is the optimal value.The MJAC algorithm has the same computation time in the initial stage as the BOMP algorithm.The symbol × denotes that the AC algorithm fails to solve the corresponding problem, or more precisely, it all fails in the initial stage.