Spatial Path Smoothing for Car-like Robots Using Corridor-Based Quadratic Optimization

Abstract

The global planning module of car-like robots usually plans a coarse spatial path, which may be non-smooth and kinematically infeasible for car-like robots. This study proposes an efficient spatial path smoothing approach, which is capable of optimizing a rough spatial path to be a high quality one. Two novel designs contribute to the proposed approach. One is a direct corridor construction method that provides an optimization region for path optimization. Based on a redefined path representation in the generated corridor, the second one is a core spatial path optimization method, where the optimization problem is formulated as a multi-objective quadratic programming (QP) with corridor and maximum-curvature constraints. Meanwhile, integrating a two-steps strategy into an optimization process yields a good trade-off between efficiency and quality. Experiment results validate that the proposed approach has the ability to online generate a high quality path.

1. Introduction

In the last decades, as the technology of autonomous navigation achieving huge progress, mobile vehicles with autonomous navigation system have been applied in various scenarios, such as floor cleaning, industrial inspection and cargo delivery. To handle the various tasks, mobile vehicles are expected to have the ability to avoid collision with surrounding objects [1]. As an essential module of autonomous navigation system, the planning module plays an important role in guaranteeing the safe and comfort vehicle motion by generating a smooth and collision-free path from the starting state to the terminal state [2,3]. In practical application, the planned path is also required to satisfy kinematic constraints of the vehicle. All these considerations make path planning for self-driving system be more challenging work.

A typical path planning framework mainly involves two parts: path finding and trajectory generation. Most of the path finding algorithms such as search-based methods [4,5], sampling-based methods [6,7] and others [8], can work well in collision-free path finding. However, the planned results are rough paths, which consist of straight lines and sharp turns, and therefore may violate certain constraints such as continuity and vehicle kinematics. A common method for continuous path generation uses parametric curves, such as polynomial [9], spline [10] and Bézier [11] curves. However, the kinematic feasibility of the generated path cannot be guaranteed by working with these curves directly [12]. The other pipeline for trajectory generation is to formulate an nonlinear optimization problem considering continuity, safety and vehicle kinematic. Due to the limited capability of trajectory optimization, a rough path may increase the optimization difficulty and results in a low quality trajectory. In addition, many methods formulate trajectory planning as nonlinear and nonconvex optimization problems, which cannot guarantee that the optimization converges to the optimal solution.

Our goal is to design a convex optimization approach considering the kinematic constraints of car-like robots for spatial path optimization. In our design, the path optimization problem is decomposed into spatial corridor generation and corridor-based path optimization. Firstly, a direct and efficient corridor generation method is proposed to construct a feasible optimization corridor along the reference path. This corridor is used to be the convex optimization domain in which the optimal solution can be found Secondly, on the basis of the generated corridor, multi-objective optimization cost function with a quadratic form is designed by introducing a new path point representation. Meanwhile, in order to guarantee a kinematically feasible path, our method incorporates corridor constraints and maximum-curvature constraints. The maximum-curvature constraints are formulated as approximate linear constraints. Finally, a two-steps optimization strategy is designed to solve the problem effectively. The contributions of this article are summarized as follows:

• A simple and efficient spatial corridor generation method for spatial corridor extraction is proposed. The problem of path optimization based on the generated spatial corridor are developed as quadratic forms.
• A convex multi-objective path optimization problem with corridor and maximum-curvature constraints is designed to generate a smooth and kinematically feasible path. The overall problem of path optimization is developed as a quadratic programming problem, which can be quickly solved. Meanwhile, a two-steps optimization strategy is designed to balance the optimization efficiency and quality.
• Our proposed method can be utilized to optimize the path in various scenarios. In addition, the experimental results demonstrate that our approach has a good performance in operational efficiency.

2. Related Work

The traditional path finding methods, such as search-based methods [4,5] and sampling-based methods [6,7], mainly focus on the efficiency or the optimality of finding a collision-free path from the starting position to the destination, but fail to concern the path feasibility for car-like robots. The planning results of these methods are usually rough and can not be used directly to navigate robots. To fill this gap, some methods post-process searching primitives via model predictive control methods to guarantee the kinematical feasibility of planning the results [13,14]. Inheriting the idea of the A* algorithm and introducing a model-based node expansion strategy can plan a vehicle-drivable trajectory in [15,16]. The variant of the rapidly exploring random tree is reported in [17], which considers kinematics constraints of robots in the searching process to enhance the feasibility of the planned path. However, several aspects of the planning objectives can not be fully considered in these methods, including length, smoothness and kinematical feasibility.

To pursue a high quality path, lots of results with regard to path optimization have been reported in recent years. Some of the works formulate their path optimization problem based on the pre-built corridor around the initial path. The reference [18] segments the safe region into the straight regions and corner region along the initial path. Convex polygons, then, are constructed within those segment regions and form a corridor jointly. A novel corridor structure is proposed in [19], namely, the spatial-temporal semantic corridor. This structure provides a representation for various kinds of semantic elements in the Frenét frame, extracting a free-space corridor for safe trajectory generation. Both the works in [18,19] can find collision-free and continuous paths by constraining the control points of each segment Bézier curve inside the corridor using the convex hull property of the Bézier curve. However, the feasibility of the generating paths in these methods cannot be guaranteed due to the lack of taking vehicle kinematic constraints into account. To deal with this problem, based on the corridor-based optimization method, ref. [20] introduces an approximate method to enforce kinematic constraints, which can find a kinematically-feasible path while ensuring the convexity of the optimization problem. The final optimization problem is formulated as a quadratic programming problem with quadratic constraints, and solved in several milliseconds, on average.

Path optimization is usually a multi-objective nonlinear optimization problem with various constraints [21,22,23,24,25,26,27]. Many path optimization problems are usually formulated as non-convex problems [28] and therefore have the following shortcomings: (i) optimization results may highly depend on the initial guess, (ii) optimization problems may easily stuck in local optimal solutions, and (iii) optimization problems may not be solved in a computationally efficient way [29]. To avoid these shortcomings, some works develop path optimization problems as convex problems, which can be solved by efficient techniques [29,30,31]. Thanks to the convexity of the problem, the global optimal solution can be found within finite iterations. Moreover, the problem with quadratic functions and linear constraints is a special case of convex problems, which is called the QP problem. In [25], based on the left and right side bound of the road map, a quadratic path length cost function is designed to minimize the required lap time. To find a path with the best trade-off between shortest length and minimum-curvature, approaches in [18,26], first give quadratic objective functions to find the shortest path and minimum-curvature path independently, and then find the best trade-off between the shortest path and minimum-curvature path. However, these methods focus on achieving the lowest lap time in the racing game, which may be not suitable for other scenarios. The method in [27] solves a trajectory optimization problem using sequential QP. In [20,32], an approximate method to enforce kinematic constraints is introduced to generate a kinematically feasible path using sequential QP. However, introducing kinematic constraints at the very start of the optimization process is not a good optimization strategy to quickly find a solution. This is because, in some cases, the optimization results are already kinematically feasible without introducing kinematic constraints. In addition, introducing kinematic constraints will increase the complexity of the optimization problem.

3. Spatial Corridor Generation

In this section, a simple and efficient method focusing on computing the corridor boundary points along the reference path is designed to generate a spatial corridor. The computed boundary points are used to represent the corridor, which are suited for the establishment of the path optimization problem.

Corridor Boundary Generation

Given a initial path ${\mathbf{P}}^{\mathrm{init}}=\{P_1^{\mathrm{init}},P_2^{\mathrm{init}},\dots ,P_m^{\mathrm{init}}\}$, this subsection computes the reference path ${\mathbf{P}}^{\mathrm{ref}}=\{P_1^{\mathrm{ref}},P_2^{\mathrm{ref}},\dots ,P_n^{\mathrm{ref}}\}$ and corridor boundaries represented by ${\mathbf{P}}^{\mathrm{L}}=\{P_1^{\mathrm{L}},P_2^{\mathrm{L}},\dots ,P_n^{\mathrm{L}}\}$ and ${\mathbf{P}}^{\mathrm{R}}=\{P_1^{\mathrm{R}},P_2^{\mathrm{R}},\dots ,P_n^{\mathrm{R}}\}$, as shown in Figure 1. A point has two dimension information $P_i^{\mathrm{j}}={[x_i^j,y_i^j]}^T$. We directly compute the slope $k_i$ of the line segment between $P_i^{\mathrm{ref}}$ and $P_{i+1}^{\mathrm{ref}}$. The corridor region is stretched along the reference path laterally by computing the perpendicular line of the corresponding line segment. Given the corridor lateral stretching distance parameters ${\mathbf{d}}^{\mathrm{L}}=\{d_1^{\mathrm{L}},d_2^{\mathrm{L}},\dots ,d_n^{\mathrm{L}}\}$ and ${\mathbf{d}}^{\mathrm{R}}=\{d_1^{\mathrm{R}},d_2^{\mathrm{R}},\dots ,d_n^{\mathrm{R}}\}$, the corridor boundary points can be computed by:

$$\begin{array}{cc}\hfill x_i^{\mathrm{L}}& =x_i^{\mathrm{ref}}+h\frac{d_i^{\mathrm{L}}}{\sqrt{{k_i}^2+1}},\hfill \\ \hfill x_i^{\mathrm{R}}& =x_i^{\mathrm{ref}}-h\frac{d_i^{\mathrm{R}}}{\sqrt{{k_i}^2+1}},\hfill \\ \hfill y_i^j& =k_i(x_i^j-x_i^{\mathrm{ref}})+y_i^{\mathrm{ref}},j\in \{\mathrm{L},\mathrm{R}\},\hfill \end{array}$$

(1)

where $k_i$ represents the slope of the perpendicular of i-th line segment and is computed by $k_i=(y_{i+1}^{\mathrm{ref}}-y_i^{\mathrm{ref}})/(x_{i+1}^{\mathrm{ref}}-x_i^{\mathrm{ref}})$. h is used to distinguish $P_i^{\mathrm{L}}$ and $P_i^{\mathrm{R}}$, defined as follows:

$$\begin{array}{c}\hfill h=\left\{\begin{array}{cc}\hfill -1,{\overrightarrow{v}}_{1,i}\times {\overrightarrow{v}}_{2,i}& >0\hfill \\ \hfill 1,{\overrightarrow{v}}_{1,i}\times {\overrightarrow{v}}_{2,i}& <0,\hfill \end{array}\right.\end{array}$$

(2)

with

$$\begin{array}{cc}\hfill {\overrightarrow{v}}_{1,i}& =[x_i^{\mathrm{ref}}-x_i^{\mathrm{L}},y_i^{\mathrm{ref}}-y_i^{\mathrm{L}}],\hfill \\ \hfill {\overrightarrow{v}}_{2,i}& =[x_{i+1}^{\mathrm{ref}}-x_i^{\mathrm{ref}},y_{i+1}^{\mathrm{ref}}-y_i^{\mathrm{ref}}].\hfill \end{array}$$

(3)

Noted that, in practice, a safe optimization region is guaranteed by pushing the corridor into the safe region of the road map. The size of corridor region is determined by ${\mathbf{P}}^{\mathrm{L}}$ and ${\mathbf{P}}^{\mathrm{R}}$. We only need to modify ${\mathbf{P}}^{\mathrm{L}}$ and ${\mathbf{P}}^{\mathrm{R}}$ to adjust the corridor.

Algorithm 1 shows the process of the proposed spatial corridor generation. The algorithm will begin with a linear interpolation for generating a path with uniformly and densely distributed waypoints, if there is a path with the sparse path point distribution. Then, we compute the segment line slope (Line 4 in Algorithm 1) and its vertical line slope (Line 5 in Algorithm 1) for extracting spatial corridor. There are n vertical line slope parameters computed in Algorithm 1 from line 3 to line 10. It is worth noting that the function (Line 11 in Algorithm 1) is introduced to reconstruct the local segment of the corridor so that a reasonable corridor along path can be obtained. An example for local corridor reconstruction is shown in Figure 2. Based on the final slope set ${\mathbf{k}}^{\mathrm{smooth}}$, boundary points are computed (Line 12–17 in Algorithm 1).

Algorithm 1 Spatial Corridor Generation.

Input:${\mathbf{P}}^{\mathrm{init}}$, point set of the initial reference path; ${\mathbf{d}}^{\mathrm{L}},{\mathbf{d}}^{\mathrm{R}}$, the set of corridor width parameters Output:${\mathbf{P}}^{\mathrm{L}},{\mathbf{P}}^{\mathrm{R}}$, boundary points of spatial corridor 1: $\mathbf{k}\leftarrow \varnothing ,{\mathbf{k}}^{\mathrm{smooth}}\leftarrow \varnothing ,{\mathbf{P}}^{\mathrm{L}}\leftarrow \varnothing ,{\mathbf{P}}^{\mathrm{R}}\leftarrow \varnothing$ 2: ${\mathbf{P}}^{\mathrm{ref}}\leftarrow$PathInterpolation(${\mathbf{P}}^{\mathrm{init}}$) 3: for$i=1$$\mathbf{to}$$n-1$do 4:     $k_i\leftarrow$ ComputeLineSlope($P_i^{\mathrm{ref}}$, $P_{i+1}^{\mathrm{ref}}$) 5:     $k_i^{{}^{\prime }}\leftarrow 1/k_i$ 6:     $\mathbf{k}\leftarrow \mathbf{k}\cup k_i^{{}^{\prime }}$ 7: end for 8: $k_n\leftarrow$ ComputeLineSlope($P_{n-1}^{\mathrm{ref}}$, $P_n^{\mathrm{ref}}$) 9: $k_n^{{}^{\prime }}\leftarrow 1/k_n$ 10: $\mathbf{k}\leftarrow \mathbf{k}\cup k_n^{{}^{\prime }}$ 11: ${\mathbf{k}}^{\mathrm{smooth}}\leftarrow$ ComputeSmoothSlopes$\left(\mathbf{k}\right)$ 12: for$i=1$$\mathbf{to}$ndo 13:     $P_i^{\mathrm{L}}\leftarrow$ ComputeLeftBoundaryPoint($k_i^{\mathrm{smooth}}$, $P_i^{\mathrm{ref}}$, $d_i^{\mathrm{L}}$) 14:     $P_i^{\mathrm{R}}\leftarrow$ ComputeRightBoundaryPoint($k_i^{\mathrm{smooth}}$, $P_i^{\mathrm{ref}}$, $d_i^{\mathrm{R}}$) 15:     ${\mathbf{P}}^{\mathrm{L}}\leftarrow {\mathbf{P}}^{\mathrm{L}}\cup P_i^{\mathrm{L}}$ 16:     ${\mathbf{P}}^{\mathrm{R}}\leftarrow {\mathbf{P}}^{\mathrm{R}}\cup P_i^{\mathrm{R}}$ 17: end for 18: return${\mathbf{P}}^{\mathrm{L}},{\mathbf{P}}^{\mathrm{R}}$

4. Path Optimization

The goal of path optimization is to optimize a discrete path to be a high quality path. To formulate the path optimization problem, firstly, we describe the path point representation based on the generated corridor. Secondly, we discuss quadratic objective functions and linear constraints of path optimization to jointly construct a QP problem. Finally, a two-steps optimization strategy is designed to quickly find a feasible solution.

4.1. Path Point Representation

Given the reference path, instead of directly taking the path points as the optimization variables, similar to [15], we use scale factors to identify the path points and take these factors as the optimization variables. Optimized points can be obtained by these factors, once the optimization procedure is completed. The representation of a path point within the generated corridor is described as follows:

$$\begin{array}{ccc}\hfill P_i=P_i^{\mathrm{L}}+{\rho }_i(P_i^{\mathrm{R}}-P_i^{\mathrm{L}}),& \hfill & \hfill i=\{1,\dots ,n\}\end{array}$$

(4)

where ${\rho }_i\in [0,1]$ is the factor to identify point $P_i$ within the line segment between $P_i^{\mathrm{L}}$ and $P_i^{\mathrm{R}}$. With this path point representation, we only need to use ${\rho }_i$ to represent a path point in the optimization process. Moreover, for a path with n waypoints, an optimization problem directly optimizing the x-direction and y-direction depends on $2n$ variables. Based on the above discussion, only n factors are optimized.

4.2. Multi-Objective Path Optimization

4.2.1. Path Length Optimization

A coarse reference path often includes redundant displacement due to unreasonable path shapes. An evaluation term for the computing squared euclidean distance between adjacent path points is used to minimize the path length. The objective function for path length minimization is formulated as:

$$\begin{array}{cc}\hfill J_l& =\sum _{i=1}^{n-1}{\parallel P_{i+1}-P_i\parallel }^2\hfill \\ \hfill & =\sum _{i=1}^{n-1}{(x_{i+1}-x_i)}^2+{(y_{i+1}-y_i)}^2.\hfill \end{array}$$

(5)

Let $\Delta P_{x,i}=x_{i+1}-x_i$, $\Delta P_{y,i}=y_{i+1}-y_i$, $\Delta x_i=x_i^{\mathrm{R}}-x_i^{\mathrm{L}}$ and $\Delta y_i=y_i^{\mathrm{R}}-y_i^{\mathrm{L}}$. According to the path point representation in Equation (4), here, we consider the case of x-direction. Then, $\Delta P_{x,i}$ can be rewritten as:

$$\begin{array}{cc}\hfill \Delta P_{x,i}& =\left[\begin{array}{c}-11\end{array}\right]\left[\begin{array}{cc}\Delta x_i& 0\\ 0& \Delta x_{i+1}\end{array}\right]\left[\begin{array}{c}{\rho }_i\\ {\rho }_{i+1}\end{array}\right]\hfill \\ & +\left[\begin{array}{c}-11\end{array}\right]\left[\begin{array}{c}{x}_i^{\mathrm{L}}\\ x_{i+1}^{\mathrm{L}}\end{array}\right],\hfill \end{array}$$

(6)

where $x_i^R$ and $x_i^L$ are boundary points information given by the generated spatial corridor. Moreover, the path length minimization problem can be viewed as a quadratic form:

$$\begin{array}{c}\hfill {(\Delta P_{x,i})}^2={\overline{\rho }}_i^T{H}_{l,x,i}{\overline{\rho }}_i+B_{l,x,i}{\overline{\rho }}_i+\mathrm{constant},\end{array}$$

(7)

where ${\overline{\rho }}_i={\left[{\rho }_i{\rho }_{i+1}\right]}^T$, while $H_{l,x,i}$ and $B_{l,x,i}$ are constant matrices. For y-direction, we have the same form:

$$\begin{array}{c}\hfill {(\Delta P_{y,i})}^2={\overline{\rho }}_i^T{H}_{l,y,i}{\overline{\rho }}_i+B_{l,y,i}{\overline{\rho }}_i+\mathrm{constant},\end{array}$$

(8)

Finally, the total quadratic form is a combination of Equations (7) and (8):

$$\begin{array}{c}\hfill J_l=\sum _{i=1}^{n-1}{\overline{\rho }}_i^T(H_{l,x,i}+H_{l,y,i}){\overline{\rho }}_i+(B_{l,x,i}+B_{l,y,i}){\overline{\rho }}_i+\mathrm{constant}.\end{array}$$

(9)

4.2.2. Path Smoothness Optimization

It is desirable to generate a smooth path for the vehicle to track so that the vehicle can run comfortably and steadily. In order to smooth the path, an elastic band cost function [33] is used to penalize the path smoothness:

$$\begin{array}{cc}\hfill J_s& =\sum _{i=1}^{n-2}{\parallel (P_{i+2}-P_{i+1})-(P_{i+1}-P_i)\parallel }^2\hfill \\ \hfill & =\sum _{i=1}^{n-2}{\parallel P_{i+2}-2P_{i+1}+P_i\parallel }^2\hfill \\ \hfill & =\sum _{i=1}^{n-2}{\parallel A_i\parallel }^2,\hfill \end{array}$$

(10)

where

$$\begin{array}{cc}\hfill A_i& =\left[\begin{array}{c}1-21\end{array}\right]\left[\begin{array}{ccc}\Delta x_i& 0& 0\\ 0& \Delta x_{i+1}& 0\\ 0& 0& \Delta x_{i+2}\end{array}\right]\left[\begin{array}{c}{\rho }_i\\ {\rho }_{i+1}\\ {\rho }_{i+2}\end{array}\right]\hfill \\ & +\left[\begin{array}{c}1-21\end{array}\right]\left[\begin{array}{c}{x}_i^{\mathrm{L}}\\ x_{i+1}^{\mathrm{L}}\\ x_{i+2}^{\mathrm{L}}\end{array}\right].\hfill \end{array}$$

(11)

The designed cost function is intended to shorten the distance among three adjacent points along the reference path. The shorter distance, the more likely these three adjacent points are in a straight line. The smoothness of the path with the straight line shape is guaranteed naturally. Therefore, curvature of the path can also be minimized by this cost function. Generally, the path tracking module of a self-driving car is prefered to a path with lower curvature due to vehicle kinematic restrictions.

For further consideration, an elastic band cost function in [34] is adopted to penalize the jerk of the B-spline for minimizing the curve smoothness. This cost function takes account of four adjacent points. In this paper, we used the similar term to smooth the path further:

$$\begin{array}{cc}\hfill J_j& =\sum _{i=1}^{n-3}{\parallel (P_{i+3}-2P_{i+2}+P_{i+1})-(P_{i+2}-2P_{i+1}+P_i)\parallel }^2\hfill \\ \hfill & =\sum _{i=1}^{n-3}{\parallel P_{i+3}-3P_{i+2}+3P_{i+1}-P_i\parallel }^2\hfill \\ \hfill & =\sum _{i=1}^{n-3}{\parallel S_i\parallel }^2,\hfill \end{array}$$

(12)

where

$$\begin{array}{cc}\hfill S_i& ={\left[\begin{array}{c}-1\\ 3\\ -3\\ 1\end{array}\right]}^T\left[\begin{array}{cccc}\Delta x_i& 0& 0& 0\\ 0& \Delta x_{i+1}& 0& 0\\ 0& 0& \Delta x_{i+2}& 0\\ 0& 0& 0& \Delta x_{i+3}\end{array}\right]\left[\begin{array}{c}{\rho }_i\\ {\rho }_{i+1}\\ {\rho }_{i+2}\\ {\rho }_{i+3}\end{array}\right]\hfill \\ & +{\left[\begin{array}{c}-1\\ 3\\ -3\\ 1\end{array}\right]}^T\left[\begin{array}{c}{x}_i^{\mathrm{L}}\\ x_{i+1}^{\mathrm{L}}\\ x_{i+2}^{\mathrm{L}}\\ x_{i+3}^{\mathrm{L}}\end{array}\right].\hfill \end{array}$$

(13)

Define ${\widehat{\rho }}_i={[{\rho }_i,{\rho }_{i+1},{\rho }_{i+2}]}^T$ and ${\tilde{\rho }}_i={[{\rho }_i,{\rho }_{i+1},{\rho }_{i+2},{\rho }_{i+3}]}^T$. Both cost functions $J_s$ and $J_j$ are represented as quadratic forms:

$$\begin{array}{c}\hfill J_s=\sum _{i=1}^{n-1}{\widehat{\rho }}_i^T(H_{s,x,i}+H_{s,y,i}){\widehat{\rho }}_i+(B_{s,x,i}+B_{s,y,i}){\widehat{\rho }}_i+\mathrm{constant},\end{array}$$

(14)

$$\begin{array}{c}\hfill J_j=\sum _{i=1}^{n-1}{\tilde{\rho }}_i^T(H_{j,x,i}+H_{j,y,i}){\tilde{\rho }}_i+(B_{j,x,i}^T+B_{j,y,i}^T){\tilde{\rho }}_i+\mathrm{constant},\end{array}$$

(15)

where $H_{s,x,i}$, $H_{s,y,i}$, $B_{j,x,i}$ and $B_{j,y,i}$ are the known parameters.

4.2.3. Path Deviation Optimization

Generally, the given reference path is safe and reliable. It is desirable that the optimized path keeps the shape of the reference path. For this, the sum of deviation between optimized points and reference points along the path is considered to minimize the deviation. As previously noted, the reference path points are taken as the middle points of the corridor to generate corridor boundaries. A reference path point in the corridor is represented as $g\left(i\right)=(P_i^L+0.5(P_i^R-P_i^L))$. The cost function of reference path deviation is measured as:

$$\begin{array}{cc}\hfill J_d& =\sum _{i=1}^n{\parallel P_i-g\left(i\right)\parallel }^2\hfill \\ \hfill & =\sum _{i=1}^n{\parallel D_i\parallel }^2,\hfill \end{array}$$

(16)

where

$$D_i=\left[\begin{array}{c}-\Delta x_i\end{array}\right]\left[\begin{array}{c}{\rho }_i\end{array}\right]+0.5\Delta x_i.$$

(17)

Then, $J_d$ can be rewritten as:

$$\begin{array}{c}\hfill J_d=\sum _{i=1}^n{\rho }_i^T(H_{d,x,i}+H_{d,y,i}){\rho }_i+(B_{d,x,i}+B_{d,y,i}){\rho }_i+\mathrm{constant}.\end{array}$$

(18)

where $H_{d,x,i}$, $H_{d,y,i}$, $B_{d,x,i}$ and $B_{d,y,i}$ are constants.

The overall objective function is a linear combination of the cost functions of path length, smoothness and deviation, given as:

$$\begin{array}{cc}\hfill J_{total}& ={\omega }_l{J}_l+{\omega }_s{J}_s+{\omega }_j{J}_j+{\omega }_d{J}_d,\hfill \end{array}$$

(19)

where ${\omega }_l$, ${\omega }_s$, ${\omega }_j$ and ${\omega }_d$ are the corresponding weighting parameters.

4.3. Optimization Constraints

The constraints for path optimization in this paper are separated into two classes, including corridor and kinematic constraints. The corridor constraints are to constrain the optimization domain of the problem into the corridor region, i.e., paths generated in the iteration optimization process must be inside the corridor. To satisfy vehicle kinematic constraints, we consider the maximum turning radius of the vehicle and introduce maximum curvature constraints to limit the curvature of the path during optimization.

4.3.1. Corridor Boundary Constraints

Since the vehicle always passes the starting and ending points, these two boundary points are directly fixed without modification. That is, the boundary constraints are viewed as affine equality constraints. We introduce boundary constraints by fixing scalar factors of two boundary points:

$$\begin{array}{cc}\hfill {\rho }_1& ={\rho }_{\mathrm{start}},\hfill \\ \hfill {\rho }_n& ={\rho }_{\mathrm{goal}},\hfill \end{array}$$

(20)

where ${\rho }_{start}$ and ${\rho }_{goal}$ represent the boundary factors of starting and ending points, respectively.

4.3.2. Corridor Internal Constraints

The feasibility of the path is guaranteed by enforcing each path segment inside the corresponding feasible domain. That is, the corridor internal constraints give the certain optimization scope to find the optimal solution within the corridor. These constraints are classified as affine inequality constraints:

$$\begin{array}{cc}\hfill 0+{\epsilon }_2⩽& {\rho }_2⩽1-{\epsilon }_2,\hfill \\ \hfill 0+{\epsilon }_3⩽& {\rho }_3⩽1-{\epsilon }_3,\hfill \\ \hfill ⋮\\ \hfill 0+{\epsilon }_{n-1}⩽& {\rho }_{n-1}⩽1-{\epsilon }_{n-1},\hfill \end{array}$$

(21)

where ${\epsilon }_i\in [0,0.5]$ and $i=2,3,\dots ,n-1$. ${\epsilon }_i$ is the constant parameter to adjust the optimization domain.

4.3.3. Curvature Constraints

Curvature constraints play an important role in guaranteeing the feasibility of the path kinematically. $R_{\min }$ is defined as the minimum turning radius, which is given by the vehicle’s physical restrictions. The maximal curvature can be represented as ${\kappa }_{\max }=1/R_{\min }$. The following descriptions are based on an assumption that each path point $P_i$ is uniformly and densely distributed over the path. Thus, with the illustration in Figure 3, several approximate formulas are obtained as follows:

$$\begin{array}{c}\hfill \Delta s\approx d,\end{array}$$

(22)

$$\begin{array}{cc}\hfill \sin \left(\alpha \right)& =\frac{\Delta s}{R}\approx \frac{d}{R},\hfill \\ \hfill \sin \left(\alpha \right)& \approx \alpha \hfill \end{array}$$

(23)

where $\Delta s\approx d={\sum }_{i=1}^{n-1}\parallel P_{i+1}^{\mathrm{ref}}-P_i^{\mathrm{ref}}\parallel /(n-1)$. To evaluate the path curvature, we first compute the following formulas according to the illustration in Figure 3:

$$\begin{array}{cc}\hfill \overrightarrow{\mu }& =\overrightarrow{P_{i+1}{P}_i}+\overrightarrow{P_{i+1}{P}_{i+2}},\hfill \\ \hfill \parallel \overrightarrow{\mu }\parallel & =\parallel \overrightarrow{P_{i+1}{P}_i}+\overrightarrow{P_{i+1}{P}_{i+2}}\parallel ,\hfill \\ \hfill & =\parallel P_i+P_{i+2}-2P_{i+1}\parallel ,\hfill \end{array}$$

(24)

$$\begin{array}{c}\hfill \parallel \overrightarrow{\mu }\parallel =2d\sin \left(\frac{\alpha }{2}\right)\approx \Delta s^2\kappa ,\end{array}$$

(25)

where $\kappa$ is the curvature and $\kappa \le {\kappa }_{\max }$. Equation (25) is used to approximately compute $\parallel \overrightarrow{\mu }\parallel$. Thus, we have:

$$\begin{array}{c}\hfill \parallel \overrightarrow{\mu }\parallel \le \Delta s^2{\kappa }_{\max },\end{array}$$

(26)

i.e.,

$$\begin{array}{c}\hfill \parallel P_i+P_{i+2}-2P_{i+1}\parallel \le \Delta s^2{\kappa }_{\max }.\end{array}$$

(27)

The square of Equation (27) is used to limit the curvature:

$$\begin{array}{c}\hfill \parallel P_i+P_{i+2}-2P_{i+1}{\parallel }^2\le {(\Delta s^2{\kappa }_{\max })}^2.\end{array}$$

(28)

The form of curvature constraint in Equation (28) is a nonlinear inequality. Directly introducing this constraint will make the optimization problem difficult to be solved. To full this gap, the Taylor approximation method retaining first-order item is used to linearize the curvature constraint in this paper. Denote $F\left({\xi }_i\right)={\parallel P_i+P_{i+2}-2P_{i+1}\parallel }^2$, where ${\xi }_i={[P_i,P_{i+1},P_{i+2}]}^T$. One has:

$$\begin{array}{c}\hfill F\left({\xi }_i\right)\approx F\left({\xi }_i^{\mathrm{ref}}\right)+F^{{}^{\prime }}\left({\xi }_i^{\mathrm{ref}}\right)({\xi }_i-{\xi }_i^{\mathrm{ref}}),\end{array}$$

(29)

where ${\xi }_i^{\mathrm{ref}}$ can be obtained from the reference path. Denote $E_i$ as the constant item of Equation (29) and rewrite $F\left({\xi }_i\right)$ as:

$$\begin{array}{c}\hfill F\left({\xi }_i\right)=F^{{}^{\prime }}\left({\xi }_i^{\mathrm{ref}}\right){\xi }_i+E_i.\end{array}$$

(30)

Equation (28) can be rewritten as:

$$\begin{array}{c}\hfill 0⩽F\left({\xi }_i\right)⩽{(\Delta s^2{\kappa }_{\max })}^2,\end{array}$$

(31)

Combining Equations (30) and (31), we have:

$$\begin{array}{cc}\hfill -E_i⩽F^{{}^{\prime }}\left({\xi }_i^{\mathrm{ref}}\right){\xi }_i& ⩽{(\Delta s^2{\kappa }_{\max })}^2-E_i,\hfill \end{array}$$

(32)

It is worth mentioning that optimization variables of the convex problem in this paper are the scalar factors . According to the path point representation in Equation (4), one has:

$$\begin{array}{c}\hfill -E_i⩽B_i{\tilde{\rho }}_i+G_i⩽{(\Delta s^2{\kappa }_{\max })}^2-E_i,\end{array}$$

(33)

where $B_i$ and $G_i$ are constants. Accordingly, the curvature constraints can be formulated as:

$$\begin{array}{cc}\hfill -E_1-G_1& ⩽B_1{\tilde{\rho }}_1⩽{(\Delta s^2{\kappa }_{\max })}^2-E_1-G_1,\hfill \\ \hfill -E_2-G_2& ⩽B_2{\tilde{\rho }}_2⩽{(\Delta s^2{\kappa }_{\max })}^2-E_2-G_2,\hfill \\ \hfill ⋮\\ \hfill -E_{n-2}-G_{n-2}& ⩽B_{n-2}{\tilde{\rho }}_{n-2}⩽{(\Delta s^2{\kappa }_{\max })}^2-E_{n-2}-G_{n-2}.\hfill \end{array}$$

(34)

To sum up, the optimization objectives presented in Equations (9), (14), (15) and (18) are quadratic functions (${\rho }^{T}H\rho +q\rho$). Boundary constraints in Equation (20) are formulated as affine equalities ($A_{\mathrm{eq}}\rho =b_{\mathrm{eq}}$). Both corridor internal constraints in Equation (21) and corridor boundary constraints in Equation (34) have the form of affine inequality ($A_{\mathrm{ie}}\rho ⩽b_{\mathrm{ie}}$). Therefore, the overall path optimization problem can be described as a general QP problem:

$$\begin{array}{ccc}\hfill & \mathrm{minimize}\hfill & \hfill {\rho }^{T}H\rho +q\rho ,\\ \hfill & \mathrm{subject}\mathrm{to}\hfill & \hfill A_{\mathrm{eq}}\rho =b_{\mathrm{eq}},\\ \hfill & \hfill & \hfill A_{\mathrm{ie}}\rho ⩽b_{\mathrm{ie}}.\end{array}$$

(35)

The implementation process of the path optimization is presented in Algorithm 2. Here, $\rho ={[{\rho }_1,{\rho }_2,\dots ,{\rho }_n]}^T$ is defined as the set of scalar factors. The constraints in the path optimization stage are separated into two classes, including boundary constraints and curvature constraints. In particular, the optimization problem without curvature constraints can be solved efficiently. The smoothing cost function can smooth the path and push the path curvature into a reasonable level. Due to most cases that the curvature of initial reference path is reasonable, the generated path after path optimization without curvature constraints is already feasible in curvature. Therefore, a two-steps path optimization strategy in Algorithm 2 is proposed to speed up the path optimization in this paper. The first step optimization does not consider the curvature constraints (Line 4 in Algorithm 2). To speed up the optimization, we provide a initial guess for optimization (Line 3 in Algorithm 2). Here, the initial guess is considered to stay within the corridor and keep close to the reference path. This step may obtain a path which violates the maximal curvature. To check the feasibility of the path, we compute the maximal curvature of the generated path (Line 6 in Algorithm 2). If the generated path passes the check of validity of the maximum curvature (Line 7-9 in Algorithm 2), a feasible solution is returned. If the generated path violates the maximum curvature constraint, the procedure will enter the second step optimization, which is the so-called path optimization with curvature constraints. In this step, the solution of the first step is retained to initialize the second step. Significantly, the parameter $\Delta s$ is computed approximately before the optimization, which cannot guarantee that the optimized result is strictly feasible. For this, the designed second step optimization will iteratively check and optimize the path until the optimized path passes the check of validity. We first use the last optimization results to update the parameter $\Delta s$ (Line 12 in Algorithm 2). Then, the path optimization with maximal curvature constraints is activated once to find a solution (Line 13 in Algorithm 2). Finally, the procedure will compute the maximal curvature and further check the validity of the maximum curvature of the solution (Line 15–18 in Algorithm 2). The iteration optimization procedure will terminate until the optimized result passes the feasible check (Line 15–18 in Algorithm 2) or the number N reaches the upper iteration number $N_{\max }$ (Line 11 in Algorithm 2).

Algorithm 2 Spatial path optimization.

Input:${\mathbf{P}}^{\mathrm{ref}}$, point set of reference path; ${\mathbf{P}}^{\mathrm{L}}$ and ${\mathbf{P}}^{\mathrm{R}}$, boundary points of spatial corridor; ${\kappa }_{\max }$, the maximal curvature; $N_{\max }$, the upper number of iterations. Output:${\mathbf{P}}^{\mathrm{opt}}$, waypoints of the optimized path 1: $N\leftarrow 0$ 2: $Flag\leftarrow False$ 3: ${\rho }^{\mathrm{init}}\leftarrow$ ComputeInitialGuess(${\mathbf{P}}^{\mathrm{ref}}$, ${\mathbf{P}}^{\mathrm{L}}$, ${\mathbf{P}}^{\mathrm{R}}$) 4: ${\rho }^{\mathrm{opt}}\leftarrow$ OptimizationWithoutCurvatureConstraints(${\rho }^{\mathrm{init}}$, ${\mathbf{P}}^{\mathrm{ref}}$, ${\mathbf{P}}^{\mathrm{L}}$, ${\mathbf{P}}^{\mathrm{R}}$) 5: ${\mathbf{P}}^{\mathrm{opt}}\leftarrow$ GetOptimizedPoints(${\rho }^{\mathrm{opt}}$, ${\mathbf{P}}^{\mathrm{L}}$, ${\mathbf{P}}^{\mathrm{R}}$) 6: $\kappa \leftarrow$ GetPathMaxCurvature(${\mathbf{P}}^{\mathrm{opt}}$) 7: if$\kappa$ < ${\kappa }_{\max }$ then 8:     $Flag\leftarrow True$ 9: end if 10: if$Flag is False$then 11:     while $N<N_{\max }$ do 12:         $\Delta s\leftarrow$ UpdateSegmentArcLength(${\mathbf{P}}^{\mathrm{opt}}$) 13:         ${\rho }^{\mathrm{opt}}\leftarrow$ OptimizationWithCurvatureConstraints(${\rho }^{\mathrm{opt}}$, ${\mathbf{P}}^{\mathrm{ref}}$, ${\mathbf{P}}^{\mathrm{L}}$, ${\mathbf{P}}^{\mathrm{R}}$, $\Delta s$) 14:         ${\mathbf{P}}^{\mathrm{opt}}\leftarrow$ GetOptimizedPoints(${\rho }^{\mathrm{opt}}$, ${\mathbf{P}}^{\mathrm{L}}$, ${\mathbf{P}}^{\mathrm{R}}$) 15:         $\kappa \leftarrow$ GetPathMaxCurvature(${\mathbf{P}}^{\mathrm{opt}}$) 16:         if $\kappa$ < ${\kappa }_{\max }$ then 17:            break 18:         end if 19:         $N\leftarrow N+1$ 20:     end while 21: end if 22: return${\mathbf{P}}^{\mathrm{opt}}$

5. Experiment Results

5.1. Implementation Details

Since the optimization problem can be formulated as a QP one, the path optimization method proposed in this article is implemented in C++11 and with QP solver OSQP [31]. All modules of the complete algorithm are tested on a computer with an Intel i5-8265U processor at 1.60 GHz and 8 GB RAM.

To verify the performance of the proposed algorithm, we first conduct experiments in a vector road map, which is built by an open source mapping tool Lanelets [35]. The vector roadmap is shown in Figure 4. In our test, a global planning module is used to find the global planning path according to the given starting position and the terminal position. Due to manually labeling the map elements, the quality of paths from the global planning module is not guaranteed and these paths are needed to be further optimized. Our proposed algorithm is integrated into the planning module for global reference path optimization. In order to demonstrate that our approach is capable of handling path optimization for different scenarios, we have applied our approach to achieve standard and racing path optimization. Secondly, we also apply our approach to the on-road environment with obstacles for the safe path generation. Finally, in order to demonstrate complexity and feasibility of the designed method, experiments in two different environments are also conducted. A visualization of the path optimization process is shown in Figure 5.

5.2. Standard Path Optimization

In the autonomous driving scene, a high quality reference path is desirable to be provided for self-driving, which primarily fulfills requirements of safety, smoothness and following the initial path as much as possible. The requirement of the shortest path is considered as the secondary goal. Different road conditions including the straight line and turning curve are tested, as shown in Figure 6. Reference paths in Figure 6a,c are given by the global planner module. The points along the reference path are distributed unevenly. These paths are hard to be executed directly, due to having impracticable curvature. After optimization, resulting paths in Figure 6b,d are high quality paths adjusted in several aspects, including smoothness, curvature, deviation and length of the path.

As shown in

Table 1. Comparison of evaluation items of paths.

${\mathit{w}}_{\mathit{l}}=0.500, {\mathit{w}}_{\mathit{s}}=0.250, {\mathit{w}}_{\mathit{j}}=0.800, {\mathit{w}}_{\mathit{d}}=0.300$
Path	Length Cost	Smoothness Cost	Deviation Cost	Maximum Curvature Cost
Figure 7a	105.170	43.371	0.000	0.381
Figure 7b	104.790	1.968	2.266	0.114
Figure 7c	177.627	48.650	0.000	0.427
Figure 7d	176.764	3.093	3.182	0.130

, the cost of smoothness of optimized paths is reduced, reaching a lower level. That is, the quality of smoothness of the path is improved. The maximum curvature is calculated to show that the curvature of the generated path is optimized and subject to the maximal curvature constraint.

5.3. Racing Path Optimization

In racing games, a high quality racing path takes the smoothness and length of path into consideration, so that lap-times of driving can be reduced as much as possible. In this test, we apply our approach to find a path mainly minimizing both length and smoothness. Weights of $w_l,w_a,w_j$ and $w_d$ are adjusted to suit for this case, without modification of the algorithm frame. The optimization results in Figure 7b,d show that the generated paths deviate from the reference path and have a better performance in smoothness and length than the initial paths in Figure 7a,c. The associated evaluation results are shown in

Table 2. Comparisons of evaluation items of paths.

${\mathit{w}}_{\mathit{l}}=2.050, {\mathit{w}}_{\mathit{s}}=1.200, {\mathit{w}}_{\mathit{j}}=0.001, {\mathit{w}}_{\mathit{d}}=1.880$
Path	Length Cost	Smoothness Cost	Deviation Cost	Maximum Curvature Cost
Figure 8a	125.215	41.550	0.000	0.107
Figure 8b	122.441	4.304	111.262	0.039
Figure 8c	183.819	32.078	0.000	0.063
Figure 8d	180.907	3.400	137.722	0.028

.

5.4. On Road Path Optimization

The above mentioned experiments are conducted in the obstacle-free environments. The corridor generation part is implemented using the consistent parameters, constructing a comparatively regular optimization corridor. The path optimization is achieved on the base of the regular corridor. In this subsection, we show that the proposed approach can be used to optimize the road path in the on-road environment with static obstacles shown in Figure 8. To ensure that the optimized path is passable in the obstacle environment, the corridor generation part first constructs an obstacle-free corridor. As shown in Figure 8, the generated blue corridor along the road is an obstacle-free corridor with irregular shapes. This irregular corridor is still suitable for the path optimization formulation. This corridor is also viewed as a convex domain for path optimization. We take the path connecting intermediate points of the generated corridor as the initial guess for the path optimization, as the red line shown in Figure 9. The initial guess is an non-smooth path with a large curvature. To consider the safe distance from the surrounding obstacles, we compute the parameters ${\epsilon }_i=D_{safe}/\parallel P_i^R-P_i^L\parallel ,i\in \{1,2,\cdots ,n-1\}$ to adjust the corridor internal constraints, where $D_{safe}$ is the safe distance from obstacles. In this test, we set $D_{safe}=1.5$ m. After path optimization, the returned result is a both smooth and kinematically feasible path, as the green line shown in Figure 9. Figure 10 shows the curvature distributions of the output path along the travel distance.

5.5. Complexity and Feasibility

Figure 11 shows the average time consumption of each part of the proposed algorithm. Paths with different lengths are adopted to test the efficiency of our proposed method. A path with more than 1900 waypoints can be optimized completely in less than 40 ms, which shows that the proposed algorithm is able to run in real-time.

In order to prove capabilities of the proposed algorithm to generate a both smooth and kinematically feasible path, we first make a comparison among following three cases: (i) path optimization only with boundary constraint. (ii) Path optimization with boundary constraint and curvature constraint. (iii) Two-steps path optimization with boundary constraint and alternative curvature constraint (showed in Algorithm 2). In Figure 12, the curvature distribution of the initial path surpasses the permissible maximum curvature, as shown in the red curve. In Figure 12b, the optimization result of case (i) violates the maximum curvature constraint, while results of case (ii) and case (iii) satisfy the maximum curvature constraint. In Figure 12a, all the cases can generate a feasible path satisfying the miximal curvature constraint.

Table 3. Performance of path optimization.

	Time Consumption (s)
Env.	Without Curvature Constraints	With Curvature Constraints	Two-Steps Optimization
Figure 9a	0.00611	0.00997	0.00653
Figure 9b	0.00537	0.01606	0.01678

indicates that the case (iii) has the same level of computation time as the case (ii) in the complex road environment shown in Figure 13b. In the simple road environment shown in Figure 13a, the computation time of the case (iii) is close to the case (i), possessing a high computing speed.

According to the above description of Figure 12 and Table 3, it can be concluded that the case (i) is capable of finding a path in the corridor with a lower computation complexity, but fails to ensure the curvature feasibility of the path. The case (ii) is able to generate a feasible path but runs in a higher computational complexity than others. The case (iii), which is called the two-steps path optimization in Algorithm 2, achieves a good trade-off between the curvature feasibility and computation complexity.

6. Conclusions and Future Work

This paper presented a corridor-based method for spatial path optimization, which is mainly containing two modules: spatial corridor generation and two-steps path optimization. The spatial corridor construction module is designed to extract an optimization domain for path optimization. The two-steps path optimization approach is designed to generate a smooth path in real-time while ensuring the feasibility of path curvature. The overall path optimization is formulated as a general QP problem and solved efficiently. To evaluate our method, we used our proposed method to achieve path optimization in different environments. Simulation experiments demonstrate the validity of the proposed method. Further work will be presented to investigate the path speed optimization and integrate it into our system. Moreover, it is promising to extend its applications to other scenarios.