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

Abstract

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 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.

1 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 (LaValle 2006; González et al. 2016; Nilsson et al. 2016; Rasekhipour et al. 2016; Ye et al. 2018; Vorobieva et al. 2015; Du and Tan 2015; Upadhyay and Ratnoo 2018; Muller et al. 2007; Xu et al. 2018; Dolgov et al. 2010; Likhachev and Ferguson 2009; Tazaki et al. 2017; Liu et al. 2017; Robinson et al. 2018; Li and Shao 2015; Li et al. 2016; Zips et al. 2016; Khatib 1986; Oetiker et al. 2009). Typically, autonomous parking is a critical maneuver especially in big narrow cities. Separated methods (Vorobieva et al. 2015; Du and Tan 2015; Upadhyay and Ratnoo 2018; Muller et al. 2007; Xu et al. 2018; Dolgov et al. 2010; Likhachev and Ferguson 2009; Tazaki et al. 2017) are common approaches for parking problems, that vehicle path planning and path tracking are handled separately. Direct learning (Liu et al. 2017) 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 (Li and Shao 2015; Li et al. 2016) 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.

Optimal control (Betts 2010; Bryson 2018; Ross and Karpenko 2012) 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. And the application challenges lie in how to represent the geometry collision-free constraints effectively. The point-point distance of circles in Robinson et al. (2018) and area criterion of rectangles in Li and Shao (2015) are both straightforward collision avoidance methods. However, circle bounding volume approximation is too conservative to realize motion in complex and high-precision 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 $J_2$-function (Xiong 1987; Xiong and Ding 1989) settles these challenges, and achieves fast high-precision motion planning for autonomous parking. The $J_2$-function is linear programming for collision checking between convex polyhedrons in any dimensional space, and it behaves as a distance function that $J_2>0$ indicates collision free. Treating $J_2\ge \delta$ ($\delta$ is a positive safety distance, and $J_2$ is a function of robot trajectory) as a constraint for the overall optimal control problem, a special $J_2$-function based bilevel optimal motion planning (BOMP) model for autonomous parking is obtained.

1.1 Previous work

Before reviewing the various parking motion planning approaches in the literature, we first show a typical three-point maneuver in parallel parking, as shown in Fig. 1. At first, the vehicle moves forward from an initial point to an intermediate point (maneuver A). Subsequently, it follows a collision-free path or trajectory backward to the destination point (maneuver B). If the process only contains one maneuver that connects the initial and terminal configurations without changing moving direction, then it is called a single maneuver process; otherwise, it is a multi-maneuver parking process.

Using the theorems from Dubins (1957), Reeds and Shepp (1990), Wang et al. (2009), many geometric path planning methods (Vorobieva et al. 2015; Du and Tan 2015; Upadhyay and Ratnoo 2018) were proposed in which the path is composed of concatenating arcs and line segments. However, the curvature at the transition point between segments is discontinuous, and this implies stopping the vehicle and steering the wheel that can wear the tires and waste time. For single maneuver parking, a function $y=f(x)$ exists in the plane coordinate system, and many approaches use a high-order differentiable function to smooth the planned path. Whereas, the feasibility conditions for single maneuver parking are critical, such as conditions on minimum parking slot length, initial vehicle configuration relative to the parking slot, and the surrounding obstacles. In Vorobieva et al. (2015), the detailed comparisons between single maneuver and multi-maneuver parallel parking using geometric methods were carried out. In Du and Tan (2015), the autonomous vertical parking maneuvers were classified by the segments number and steering direction, and this planning approach achieves high efficiency and high parking success rate. Focusing on the issue that the vehicle may not stop exactly at the prescribed intermediate point in the three-point maneuver process, Upadhyay and Ratnoo (2018) proposed a four parameter logistic curve to connect the terminal point to a neighborhood of the intermediate point.

Fig. 1

Typical three-point maneuver in parallel parking

Graph searching is another type of approach for autonomous parking. At first, the discretization of the environment is applied to construct a graph. Then informative heuristics are used to guide the search to generate a feasible path to the parking spot. Dolgov et al. (2010) applied a variant of $A^*$ search in vehicle state space to generate a feasible path first, then took advantages of the numerical optimization method to shorten and smooth the planned path. Likhachev and Ferguson (2009) utilized multi-resolution discretization method and anytime dynamic $A^*$ search to obtain long dynamically feasible maneuvers in real time. In Tazaki et al. (2017), a new multi-resolution discretization on manually predefined guidelines was proposed, then a graph-like roadmap was created to connect each guideline partition. However, this method heavily relies on guidelines that may cause unnecessary maneuvers. A common precomputation technique is used in these searching methods to reduce online path planning computation complexity. Such as the shortest path from a point to its neighborhood points (Dolgov et al. 2010), the occupied space of the vehicle after taking an action (Likhachev and Ferguson 2009), and the path from guideline partition to partition (Tazaki et al. 2017) are all precomputed.

In Zips et al. (2016), a fast optimization method was used for narrow environment parking, but the heuristic information leads to a non-optimal solution. Artificial potential field (APF) method (Khatib 1986) has made great achievements for robot motion planning in static and dynamic environments. Moreover, a recent application in autonomous parking (Oetiker et al. 2009) demonstrates its capability for nonholonomic systems. However, the APF method is likely to get stuck in a sub-optimal path, and it cannot handle the multi-maneuver parking because of local planning characteristic.

Previous approaches only solve vehicle path planning problem, but at some times, vehicle trajectory planning concerning vehicle kinematics and dynamics constraints is more useful and important. Kant and Zucker (1986) decomposed the trajectory planning as a combination of path planning and velocity planning, and the decomposition demonstrates apparently that trajectory planning problem is more difficult than path planning problem. In Muller et al. (2007), a two-step method was applied for trajectory planning. In the first step, a collision-free path is generated using sampling based method without considering the kinematics constraint. Then in the second step, a local trajectory planner solved the nonholonomic constrained problem. In Liu et al. (2017), a neural network based parking trajectory planning was proposed with constant speed input and discretized steering angle outputs. In this method, the steering angle is discontinuous. Moreover, obstacle avoidance and multi-maneuver parking cannot be treated easily. In Haddad et al. (2010), an optimization approach along with trapezoidal velocity constraints simplification solved the trajectory planning problem, and the simulation results showed considerable computation time reduction without losing much quality of the solution. In Li and Shao (2015) and Li et al. (2016), the optimal control method is applied to solve the optimal trajectories for autonomous parking. In Li and Shao (2015), rectangular obstacles together with area criterion were used to check collisions, and eight nonlinear constraints are generated between a pair of rectangles. In Li et al. (2016), collision avoidance in regular parallel parking was realized by restricting the vehicle between two function boundaries (red lines in Fig. 1).

1.2 Contributions and organization

In this paper, we present a general BOMP model in which the upper level is an optimal control problem designed for vehicle nonlinear dynamics, while the lower level is the $J_2$-function linear programming for geometry collision-free constraint (in Sect. 2). The BOMP model contains am embedded linear programming problem as constraint for the overall optimal control problem. This hierarchy property makes the problem complicated to solve, and traditional optimal control methods cannot solve the BOMP problem. Hence, the modified approximate Karush–Kuhn–Tucker theory (Dutta et al. 2013) is used to simplify the model to a traditional optimal control problem. Then the pseudospectral optimal control method (Ross and Karpenko 2012) is used to solve the converted problem, and an iterative BOMP algorithm is obtained (in Sect. 3). Twelve representative complex parking problems are designed to evaluate the efficiency of the BOMP algorithm, and they can be regarded as benchmarks for future parking approaches. 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. The BOMP algorithm benefits by the iterative convergence strategy. At each iterative, the approximate problem is easy to solve an approximate optimal trajectory to initialize the next iterative. Simulations in autonomous parking demonstrate that the computation speed increases almost two orders of magnitude compared with the area criterion based method (in Sect. 4).

Finally, concluding remarks are given in Sect. 5 and the issues to be researched in this field are also pointed out.

2 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 Sect. 1, the geometry collision-free constraint is the crucial matter. In this section, we will present the BOMP model with an embedded $J_2$-function linear programming constraint.

2.1 Vehicle kinematics and basic parking constraints

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:

$$\begin{aligned} \left\{ \begin{aligned}&\dot{x}(t)= \upsilon (t)\cos \theta (t),\quad \ \ \ \dot{y}(t)= \upsilon (t)\sin \theta (t) \\&\dot{\theta }(t) = \upsilon (t)\tan \alpha (t)/l,\, \quad \dot{\alpha }(t) = \omega (t) \\ \end{aligned} \right. \end{aligned}$$

(1)

where ${\varvec{q}}=(x,\,y,\,\theta )\in \mathfrak {R}^2 \times S$ is the configuration of the vehicle coordinate $\{{\varvec{C}}\}$ with respect to the world coordinate $\{{\varvec{W}}\}$ which origins at one corner point of the parking spot, see Fig. 2. The vehicle coordinate $\{{\varvec{C}}\}$ origins at the mid-point of the rear wheel axis, $(x,\,y)$ describes the vehicle position, and $\theta$ denotes the orientation. $\upsilon$ denotes the linear velocity of point $(x,\,y)$, $\alpha$ denotes the steering angle of the front wheel and $\omega$ denotes the steering velocity.

As the parking process is in low speed and to bound the trajectory curvature and its derivative (Li and Shao 2015), the following mechanical and physical constraints are considered.

$$\begin{aligned} \vert \upsilon (t) \vert \le \upsilon _{\text{max}},\quad \vert \alpha (t) \vert \le \alpha _{\text {max}},\quad \vert \omega (t) \vert \le \omega _{\text {max}} \end{aligned}$$

(2)

And the initial and the final state constraints are treated as:

$$\begin{aligned} \left\{ \begin{aligned}&x(0)=x_0,\ y(0)=y_0,\ \theta (0)=\theta _0 \\&\upsilon (0)=0,\ \upsilon (t_f)=0,\ \vert \theta (t_f) \vert \le \epsilon _\theta \\&\vert (A_y(t_f)+B_y(t_f)-SW)/2\vert \le \epsilon _p \\&\vert (C_y(t_f)+D_y(t_f)-SW)/2\vert \le \epsilon _p \\&\vert (A_x(t_f)+D_x(t_f)-SL)/2\vert \le \epsilon _p \\&\vert (B_x(t_f)+C_x(t_f)-SL)/2\vert \le \epsilon _p \end{aligned} \right. \end{aligned}$$

(3)

where $(x_0,\,y_0,\,\theta _0)$ is the vehicle initial configuration, $(A_x,\, A_y)$ is the coordinate of vehicle corner point ${\varvec{A}}$ described in $\{{\varvec{W}}\}$, $(B_x,\, B_y)$, $(C_x,\, C_y)$ and $(D_x,\, D_y)$ have the similar meanings, $\epsilon _p\ge 0$ and $\epsilon _\theta \ge 0$ denote the position deviation and the orientation deviation of the vehicle central axis with respect to the parking spot central axis, respectively.

Fig. 2

The coordinate systems attached to the vehicle and the parking spot

2.2 Simple collision avoidance constraints

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, vehicles are always abstracted as rectangles (cubes) or overlapping circles (spheres) to simplify the collision avoidance problem. In this section, the general point-point distance constraints between circles (or spheres) and area criterion constraints between convex polygons are summarized.

Suppose obstacle ${\varvec{A}}$ is approximated by a circle or sphere of radius $r_a$ that covers it, and robot ${\varvec{B}}$ is approximated by a circle or sphere of radius $r_b$. The centers of the approximate objects are ${\varvec{a}}$ and ${\varvec{b}}$, respectively. Then the geometry collision-free constraint between ${\varvec{A}}$ and ${\varvec{B}}$ is approximated as:

$$\begin{aligned} d({\varvec{a}},\,{\varvec{b}}) - r_a - r_b \ge \delta \end{aligned}$$

(4)

where $d({\varvec{a}},\,{\varvec{b}})$ denotes the Euclidean distance between the two points, and $\delta$ is a positive constant safety distance ($\delta$ has the same meaning in the whole paper). Usually, the robot and the obstacles are approximated by several overlapping circles or spheres to increase approximate precision, in this case, the geometry collision-free constraints are still straightforward.

Suppose plane obstacle ${\varvec{A}}$ and plane robot ${\varvec{B}}$ are approximated by convex polygons with $n_a$ vertexes and $n_b$ vertexes, respectively. The vertexes coordinates of the polygons are $\{{\varvec{a}}_i\},\, i=1,2,\ldots ,n_a$ and $\{{\varvec{b}}_j\},\, j=1,2,\ldots ,n_b$, respectively. Then the area criterion can describe the geometry collision-free constraints between ${\varvec{A}}$ and ${\varvec{B}}$:

$$\begin{aligned} \begin{aligned}&\sum _{j=1}^{n_b-1} S_{\triangle {\varvec{a}}_i{\varvec{b}}_j{\varvec{b}}_{j+1}} + S_{\triangle {\varvec{a}}_i{\varvec{b}}_{n_b}{\varvec{b}}_1} - S_{{\varvec{B}}} \ge \delta ,\,\, i=1,2,\ldots ,n_a\\&\sum _{i=1}^{n_a-1} S_{\triangle {\varvec{b}}_j{\varvec{a}}_i{\varvec{a}}_{i+1}} + S_{\triangle {\varvec{b}}_j{\varvec{a}}_{n_a}{\varvec{a}}_1} - S_{{\varvec{A}}} \ge \delta ,\,\, j=1,2,\ldots ,n_b \end{aligned} \end{aligned}$$

(5)

where $S_{\triangle }$ is the area of a triangle, $S_{{\varvec{A}}}$ and $S_{{\varvec{B}}}$ are the areas of the two approximate polygons. Moreover, for convex polyhedrons in 3D environment, the volume criterion can derive the collision avoidance constraints for.

As we can see, for a pair of polygons, there are $n_a+n_b$ nonlinear constraints in (5), and this number is usually larger than the circle approximation method. However, in theory, a complex geometry can be incorporated by several polygons or polyhedrons in any precision and low overlapping rate with fewer approximate objects than the circle or sphere approximation method. And to achieve planning and control in scenarios with long and thin obstacles, the polygon or polyhedron approximation is more efficient. Therefore, the convex polygon or polyhedron approximation method will play an important role in complex and high-precision robot motion planning problems. However, the constraints in (5) are high nonlinear and redundant. Therefore, neither collision avoidance methods (4) and (5) can be used to general fast high-precision motion planning problems.

2.3 BOMP model

Consider vehicle state variable ${\varvec{x}}$ as ${\varvec{x}}=(x,\, y,\, \theta ,\, \alpha )$ and the control input ${\varvec{u}}=(\upsilon ,\, \omega )$, then vehicle kinematics model (1) can be abstracted as ${\varvec{\dot{x}}}(t)={\varvec{f}}({\varvec{x}}(t),\,{\varvec{u}}(t))$. The optimal control problem tries to solve an optimal smooth trajectory corresponding to this differential equation constraints and with respect to a specified performance index. Consider a convex polyhedron robot ${\varvec{B}}$ moves around a static convex polyhedron obstacle ${\varvec{A}}$ (a case that a rectangular vehicle moves around rectangular vehicle obstacles). Since the $J_2$-function (Xiong 1987; Xiong and Ding 1989) acts as a distance function between ${\varvec{A}}$ and ${\varvec{B}}$, $J_2\ge \delta$ is added as an embedded constraint, the BOMP model is obtained:

$$\begin{aligned} \min \quad&\beta g({\varvec{x}}(0),\,{\varvec{x}}(t_f),\,t_f) + (1-\beta )\int _0^{t_f} L({\varvec{x}}(t),\,{\varvec{u}}(t)) \text{d}t \end{aligned}$$

(6)

$$\begin{aligned} \text{s.t.} \quad&{\varvec{\dot{x}}}(t)={\varvec{f}}({\varvec{x}}(t),\,{\varvec{u}}(t)) \end{aligned}$$

(6a)

$$\begin{aligned} \quad&{\varvec{h}}({\varvec{x}}(t),\,{\varvec{u}}(t))\le {\varvec{0}} \end{aligned}$$

(6b)

$$\begin{aligned} \quad&{\varvec{x}}(0)={\varvec{x}}_0, \quad {\varvec{x}}(t_f)\in {\varvec{X}}_f \end{aligned}$$

(6c)

$$\begin{aligned} \quad&J_2({\varvec{x}})\ge \delta \end{aligned}$$

(6d)

Equation (6a) represents vehicle dynamics constraints. Equation (6b) describes constraints on vehicle state and control variables, e.g., constraints (2). And Eq. (6c) corresponds to the parking initial and final state constraints (3), where ${\varvec{x}}_0$ and ${\varvec{X}}_f$ are vehicle initial state and final stopping region, respectively. $J_2({\varvec{x}})$ represents collision avoidance constraint on vehicle state. To simplify the understanding of the BOMP model, the exact $J_2$-function form is omitted here and can be found in the appendix. The readers just understand that the constraint (6d) acts the same role as (4) or (5), but $J_2$ is the optimal value of a linear programming problem. The cost function $g({\varvec{x}}(0),\,{\varvec{x}}(t_f),\,t_f)$ depends only on the initial state ${\varvec{x}}(0)$, final state ${\varvec{x}}(t_f)$ and the completion time $t_f$, while $L({\varvec{x}}(t),\,{\varvec{u}}(t))$ describes some objective along the trajectory such as the energy consumption and $0\le \beta \le 1$ is the weight. A particular performance index is to minimize the weight sum of completion time and energy consumption, and it’s used in Sect. 4.1.

$$\begin{aligned} t_f + \int _0^{t_f} \upsilon ^2(t) \text{d}t \end{aligned}$$

(7)

3 The BOMP model solution

3.1 BOMP model analysis

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 Benita and Mehlitz (2016), 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 (Sinha et al. 2017, 2018) 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 (Hansen et al. 1992) and that merely evaluating the optimality is also NP-hard (Vicente et al. 1994).

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 (Ross and Karpenko 2012) 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 (Albrecht et al. 2012). 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 (Dutta et al. 2013). 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 et al. (2013) 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 iterative strategy is used to decrease the relaxation factor to approach the optimal value. Therefore, the MAKKT theory and iterative convergence strategy are selected in this paper.

3.2 The BOMP algorithm

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 (Wright and Nocedal 1999), the continuous MAKKT form of the lower problem (12) is:

where the Lagrange multipliers ${\varvec{\lambda }}(t)\in \mathfrak {R}^{n^\prime }$ and ${\varvec{v}}(t)\in \mathfrak {R}^{m^\prime }$ correspond to the constraints ${\varvec{p}}(t)\ge {\varvec{0}}$ and ${\varvec{Q({\varvec{x}})}}{\varvec{p}}(t) = {\varvec{b}}$, respectively. Equations (8a) are feasible conditions, (8b) are equilibrium and complementary conditions and $\epsilon >0$ is the approximate relaxation. A point ${\varvec{p}}$ statisfing these constraints is called an $\epsilon$-MAKKT point (or $\epsilon$ approximate optimal point), and when $\epsilon =0$ it’s actually the KKT point (or optimal point).

Since in the BOMP model the optimal value $J_2({\varvec{x}})$ 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 (Dutta et al. 2013) helps the analysis, that is, at an $\epsilon$-MAKKT point, the objective value ${\varvec{c}}^\text{T}{\varvec{p}}(t)$ is at least $\epsilon$ larger than $J_2({\varvec{x}})$. Consequently, the converted single level optimal motion planning problem for (6) can be formulated as follow:

$$\begin{aligned} \min \quad&\beta g({\varvec{x}}(0),\,{\varvec{x}}(t_f),\,t_f) + (1-\beta )\int _0^{t_f} L({\varvec{x}}(t),\,{\varvec{u}}(t)) \text{d}t \end{aligned}$$

(9)

$$\begin{aligned} \text{s.t.} \quad&{\varvec{\dot{x}}}(t)={\varvec{f}}({\varvec{x}}(t),{\varvec{u}}(t)) \end{aligned}$$

(9a)

$$\begin{aligned} \quad&{\varvec{h}}({\varvec{x}}(t),\,{\varvec{u}}(t))\le {\varvec{0}} \end{aligned}$$

(9b)

$$\begin{aligned} \quad&{\varvec{x}}(0)={\varvec{x}}_0, \quad {\varvec{x}}(t_f)\in {\varvec{X}}_f \end{aligned}$$

(9c)

$$\begin{aligned} \quad&{\varvec{c}}^\text{T}{\varvec{p}}(t)\ge \epsilon +\delta \end{aligned}$$

(9d)

$$\begin{aligned} \quad&{\varvec{Q({\varvec{x}})}}{\varvec{p}}(t) = {\varvec{b}},\quad {\varvec{p}}(t)\ge {\varvec{0}},\quad {\varvec{\lambda }}(t)\ge {\varvec{0}} \end{aligned}$$

(9e)

$$\begin{aligned} \quad&\Vert {\varvec{c}}-{\varvec{\lambda }}(t)+{\varvec{Q}}^\text{T}({\varvec{x}}){\varvec{v}}(t) \Vert \le \sqrt{\epsilon },\quad {\varvec{\lambda }}^\text{T}(t) {\varvec{p}}(t) \le \epsilon \end{aligned}$$

(9f)

The relationship between the MAKKT and the exact KKT optimality conditions is as follow: if a sequence of points $\{{\varvec{p}}_k\}$ converge to a point $\bar{{\varvec{p}}}$ where the Mangasarian Fromovitz constraint qualification is also satisfied, each point ${\varvec{p}}_k$ is an $\epsilon _k$-MAKKT point, and $\epsilon _k \rightarrow 0$ as $k \rightarrow \infty$, then the point ${\varvec{\bar{p}}}$ is a KKT point. Therefore, in order to solve problem (6), a decreasing sequence $\epsilon$ should be applied to problem (9) and a sequence of approximate optimal trajectory is solved to converge to the exact optimal trajectory.

Combining the above theories, an iterative BOMP algorithm framework is formulated for robot trajectory generation and optimization mission. The algorithm details are shown in Algorithm 1, where the trajectory ${\varvec{\tau }}$ is composed of state trajectory ${\varvec{x}}(t)$, control trajectory ${\varvec{u}}(t)$, and ${\varvec{p}}(t),\,{\varvec{\lambda }}(t),\,{\varvec{v}}(t)$ in the $J_2$-function. The PSOC method solves problem (9). To limit the length of this paper, the PSOC method details are omitted, and readers can consult the reference paper (Ross and Karpenko 2012). The IPOPT (Wächter and Biegler 2006) algorithm of version 3.12.9 is used to solve the discrete NLP. The IPOPT algorithm is designed with special options, such as convergence tolerance $1e^{-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 1.

Table 1 The IPOPT algorithm options

4 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 (Li and Shao 2015) 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.

4.1 Simulation in autonomous parking

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. Figure 3 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 2, the abbreviations $S,\,O,\,C$ represent the scenario, 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 $(x,\,y,\,\theta )$ 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 $(x,\,y,\,\theta )$ are listed in Table 2.

Fig. 3

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 (color figure online)

Table 2 Transformation parameters $(x,\,y,\,\theta )$ of each obstacle with respect to the world coordinate system and the vehicle initial configuration $(x_0,\,y_0,\,\theta _0)$ in each case

Table 3 The specified parameters for the parking spot, the vehicle and Algorithm 1’s input

Table 4 The computation results of the BOMP and the AC algorithms, where T is the computation time and $v^*$ is the optimal value

Fig. 4

Optimization results for scenario 1 case 2 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 (color figure online)

The specified parameters for the parking spot, the vehicle and the Algorithm 1’s input are listed in Table 3, where the vehicle size parameters, mechanical and physical constraints parameters are originated from Li and Shao (2015). As for the initial trajectory guess ${\varvec{\tau }}_0$ in Algorithm 1, it is specified very easily. Guess the parking completion time arbitrarily, then ${\varvec{\tau }}_0$ is composed of $x(t)=x_0, y(t)=y_0, \theta (t)=\theta _0,\alpha (t)=0,\upsilon (t)=0,\omega (t)=0,{\varvec{p}}(t)={\varvec{0}},{\varvec{\lambda }}(t)={\varvec{0}}, {\varvec{v}}(t)={\varvec{0}}$ for any t. Under these conditions, the BOMP algorithm solves all these $4\times 3$ problems. The computation results are shown in Table 4 and Figs. 4, 5, 6 and 7.

Then the computation complexity comparison between the BOMP algorithm and the AC algorithm is made. Given a point ${\varvec{b}}$ and four corner points ${\varvec{A}},\,{\varvec{B}},\,{\varvec{C}},\,{\varvec{D}}$ of a rectangle in the plane, the AC collision avoidance constraint used in this paper is:

$$\begin{aligned} \frac{S_{\triangle bAB}+S_{\triangle bBC}+S_{\triangle bCD}+S_{\triangle bDA}}{S_{\square ABCD}} \ge 1.025 \end{aligned}$$

(10)

where $S_\triangle$ and $S_\square$ denote the area of a triangle and a rectangle, respectively. And there are eight nonlinear constraints (10) for a pair of plane rectangles collision avoidance problem. Then replacing (6d) by this constraint, the AC based motion planning problem can be obtained. Since in Li and Shao (2015), the IPOPT options and the computation time were not shown, the algorithm options in Table 1 are used. And the initial guess ${\varvec{\tau }}_0$ is selected the same way $x(t)=x_0,\, y(t)=y_0,\, \theta (t)=\theta _0,\,\alpha (t)=0,\,\upsilon (t)=0,\,\omega (t)=0$. However, the AC algorithm does not solve the whole autonomous parking problems. The results are shown in Table 4.

Fig. 5

Optimization results for scenario 2 case 2 by the BOMP algorithm

Fig. 6

Optimization results for scenario 3 case 2 by the BOMP algorithm

Fig. 7

Optimization results for scenario 4 case 2 by the BOMP algorithm

From Table 4, 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.36 and 8.60 s, respectively. While for the AC algorithm in the solved problems, they are 127.66 and 208.51 s. The reasons why the BOMP algorithm has significant superiorities in computation speed than the AC algorithm may be:

• $J_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 it is intrinsically much faster than the problem with nonlinear constraints.

• Variables in the $J_2$-function make the BOMP algorithm only concern the closest components implicitly. However, the AC method needs to consider the eight points between a pair of rectangles simultaneously and explicitly, whose optimization direction is difficult to be determined.

• 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. 4, 5, 6 and 7. 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\times 3$ simulations are conducted without adjustment of any algorithm options, like $\epsilon$, 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 Li and Shao (2015), 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.

4.2 Polygon VS circle approximation

Fig. 8

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 (color figure online)

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,\,\pi /2.0)$. Figure 8a shows the result of the BOMP algorithm, and the vehicle and obstacles are still represented as rectangles. Figure 8b shows the result of circle approximation method. The vehicle and the obstacles are approximated by three overlapping circles of radius 1.3 m (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 and the simplicity of the $J_2$-function, the BOMP model will compensate motion planning methods in many domains.

4.3 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. 9). To simplify the experiment, we only consider the robot kinematics, and the robot moved at very low speed. Using SLAM technique, the environment geometry can be obtained and then through convex decomposition (Mamou and Ghorbel 2009) 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. 9 are shown in Fig. 10. 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.

Fig. 9

The environment setup for this experiment. There are seven obstacles in a very small area ($1.5\;{\text {m}}\times 1.5\;{\text {m}}$) and the goal of all the experiments is to let Turtlebot3 stop at the center of the white square which is $0.5\;{\text {m}}\times 0.5\;{\text {m}}$

Fig. 10

Given robot initial location, the calculated global control variables trajectories by the BOMP algorithm

5 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 $J_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 BOMP model. The highlights of this paper lie in the following aspects.

• 1. The BOMP model utilizes the $J_2$-function linear programming to avoid collision. Because of the simplicity of the $J_2$-function, the BOMP model can solve robot motion planning problems in very high speed and high precision.

• 2. The BOMP model is an open framework that can handle any user-specified 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 which 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. 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 (Li et al. 2016), 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.

Appendix

The $J_2$-function represents one collision checking method between convex polyhedrons in any dimensional spaces including the convex polygons in 2D environment. Suppose obstacle ${\varvec{A}}$ and robot ${\varvec{B}}$ are approximated by convex polyhedrons with $n_a$ vertexes and $n_b$ vertexes, respectively. The vertexes coordinates are $\{{\varvec{a}}_i\},\, i=1,2,\ldots ,n_a$ and $\{{\varvec{b}}_j\},\, j=1,2,\ldots ,n_b$. Where ${\varvec{a}}_i,\,{\varvec{b}}_j \in \mathfrak {R}^m$ and m is the dimensionality. Then the $J_2$-function computes the minimum pseudo-distance between ${\varvec{A}}$ and ${\varvec{B}}$, and $J_2>0$ denotes collision free:

$$\begin{aligned} J_2({\varvec{A}},\,&{\varvec{B}}) = \min \sum _{k=1}^{m+2}z_k \end{aligned}$$

(11)

$$\begin{aligned} \text{s.t.} \quad&{\varvec{Ar}} - {\varvec{Bs}} + {\varvec{z}} = {\varvec{0}}, \end{aligned}$$

(11a)

$$\begin{aligned} \quad&\sum _{i=1}^{n_a}r_i + z_{m+1} = 1, \quad \sum _{j=1}^{n_b}s_j + z_{m+2} = 1, \end{aligned}$$

(11b)

$$\begin{aligned} \quad&{\varvec{r}}\ge {\varvec{0}},\ {\varvec{s}}\ge {\varvec{0}}, \ {\varvec{z}}\ge {\varvec{0}}, \ z_{m+1}\ge 0, \ z_{m+2}\ge 0. \end{aligned}$$

(11c)

where ${\varvec{r}}=[r_1 \ r_2 \ \cdots \ r_{n_a}]^\text{T}$, ${\varvec{s}}=[s_1 \ s_2 \ \cdots \ s_{n_b}]^\text{T}$, and ${\varvec{z}}=[z_1 \ z_2 \ \cdots \ z_m]^\text{T}$. In constraint (11a), the symbols ${\varvec{A}}$ and ${\varvec{B}}$ are actually matrixes composed of the points coordinates, i.e., ${\varvec{A}}=[{\varvec{a}}_1 \ {\varvec{a}}_2 \ \cdots \ {\varvec{a}}_{n_a}]$ and ${\varvec{B}}=[{\varvec{b}}_1 \ {\varvec{b}}_2 \ \cdots \ {\varvec{b}}_{n_b}]$. And the vector inequality is to be understood element-wise.

The $J_2$-function (11) can be wrote as a standard linear programming problem:

$$\begin{aligned} \begin{aligned} J_2({\varvec{A}},{\varvec{B}}&) = \min \ {\varvec{c}}^\text{T}{\varvec{p}} \\ \text{s.t.} \quad&{\varvec{Qp}}={\varvec{b}}, \quad {\varvec{p}}\ge {\varvec{0}}. \end{aligned} \end{aligned}$$

(12)

where the optimization variable is ${\varvec{p}}=[{\varvec{r}}^\text{T} \ {\varvec{s}}^\text{T} \ {\varvec{z}}^\text{T} \ z_{m+1} \ z_{m+2}]^\text{T}$, and the notations ${\varvec{Q}},\,{\varvec{b}},\,{\varvec{c}}$ can be constructed appropriately. ${\varvec{p}},\,{\varvec{c}}\in \mathfrak {R}^{n^\prime }$, ${\varvec{Q}}\in \mathfrak {R}^{m^\prime \times n^\prime },\,{\varvec{b}}\in \mathfrak {R}^{m^\prime }$ where $n^\prime =n_a+n_b+m+2$ and $m^\prime =m+2$.

Suppose obstacle ${\varvec{A}}$ is static and robot ${\varvec{B}}$ is moving, then the matrix ${\varvec{A}}$ in (11a) is constant, and ${\varvec{B}}$ depends on robot state ${\varvec{x}}$. Therefore, in robot trajectory optimization problem we can construct the collision avoidance constraint $J_2({\varvec{x}})\ge \delta$, the matrix ${\varvec{Q}}$ depends on ${\varvec{x}}$ and ${\varvec{p}}$ is a time-dependent variable.