Piecewise Bezier Curves Path Planning with Continuous Curvature Constraint for Autonomous Driving

Abstract

We present two practical path planning algorithms based on Bezier curves for autonomous vehicles operating under waypoints and corridor constraints. Bezier curves have useful properties for the trajectory generation problem. This paper describes how the algorithms apply these properties to generate the reference trajectory for vehicles to satisfy the path constraints. Both algorithms generate the piecewise-Bezier-curves path such that the curves segments are joined smoothly with C ² constraint which leads to continuous curvature along the path. The degree of the curves are minimized to prevent them from being numerically unstable. Additionally, we discuss the constrained optimization problem that optimizes the resulting path for a user-defined cost function.

1 Introduction

Bezier Curves were invented in 1962 by the French engineer Pierre Bezier for designing automobile bodies. Today Bezier Curves are widely used in computer graphics and animation. The Bezier curves have useful properties for the path generation problem as described in Section 2 of this paper. Hence many path planning techniques for autonomous vehicles have been discussed based on Bezier Curves in the literature. Cornell University Team for 2005 DARPA Grand Challenge used a path planner based on Bezier curves of degree 3 in a sensing/action feedback loop to generate smooth paths that are consistent with vehicle dynamics [5]. Skrjanc proposed a new cooperative collision avoidance method for multiple robots with constraints and known start and goal velocities based on Bezier curves of degree 4 [6]. In this method, four control points out of five are placed such that desired positions and velocities of the start and the goal point are satisfied. The fifth point is obtained by minimizing penalty functions. Jolly described Bezier curve based approach for the path planning of a mobile robot in a multi-agent robot soccer system [3]. The resulting path is planned such that the initial state of the robot and the ball, and an obstacle avoidance constraints are satisfied. The velocity of the robot along the path is varied continuously to its maximum allowable levels by keeping its acceleration within the safe limits. When the robot is approaching a moving obstacle, it is decelerated and deviated to another Bezier path leading to the estimated target position.

Our previous works introduced two path planning algorithms based on Bezier curves for autonomous vehicles with waypoints and corridor constraints [1, 2]. Both algorithms join cubic Bezier curve segments smoothly to generate the reference trajectory for vehicles to satisfy the path constraints. Also, both algorithms are constrained in that the path must cross over a bisector line of corner area such that the tangent at the crossing point is normal to the bisector. Additionally, the constrained optimization problem that optimizes the resulting path for user-defined cost function was discussed. Although the simulations provided in that paper showed the generation of smooth routes, discontinuities of the yaw angular rate appeared at junction nodes between curve segments. This is because the curve segments are constrained to connect each other by only C ¹ continuity, so the curvature of the path is discontinuous at the nodes. (Section 2 describes this more detail.)

To resolve this problem, we propose new path planning algorithms. The algorithms impose constraints such that curve segments are C ² continuous in order to have curvature continuous for every point on the path. In addition, they give the reference path more freedom by eliminating redundant constraints used in previous works, such as the tangent being normal to the bisector and symmetry of curve segments on corner area. The degree of each Bezier curve segments are determined by the minimum number of control points to satisfy imposed constraints while cubic Bezier curves are used for every segments in previous works. The optimized resulting path is obtained by computing the constrained optimization problem that controls the tradeoff between shortest and smoothest path generation. Furthermore, the proposed algorithms satisfy the initial and goal orientation as well as position constraint while previous works only made the position constraint satisfied. The numerical simulation results demonstrate the improvement of trajectory generation in terms of smoother steering control and smaller cross track error.

2 Bezier Curve

A Bezier Curve of degree n can be represented as

$$\mathbf{P}(t) = \sum_{i=0}^{n}{B}_{ i}^{n}(t){\mathbf{P}}_{ i},\quad t \in[0,1]$$

where P _i are control points and ${B}_{i}^{n}(t)$ is a Bernstein polynomial given by

$$B_i^n (t) = \left( {\begin{array}{rll} n \\i \\\end{array} } \right)\,(1 - t)^{n - i} t^i,\quad i\, \in \{ 0,\,1,\, \ldots,\,n\}$$

Bezier Curves have useful properties for path planning:

• They always start at P ₀ and stop at P _n .

• They are always tangent to $\overline{{\mathbf{P}}_{0}{\mathbf{P}}_{1}}$ and $\overline{{\mathbf{P}}_{n}{\mathbf{P}}_{n-1}}$ at P ₀ and P _n respectively.

• They always lie within the convex hull consisting of their control points.

2.1 The de Casteljau Algorithm

The de Casteljau algorithm describes a recursive process to subdivide a Bezier curve P(t) into two segments. The subdivided segments are also Bezier curves. Let $\{{\mathbf{P}}_{0}^{0},{\mathbf{P}}_{1}^{0},\ldots,{\mathbf{P}}_{n}^{0}\}$ denote the control points of P(t). The control points of the segments can be computed by

$$\begin{array}{rll}{ \mathbf{P}}_{i}^{j}& =(1 - \tau ){\mathbf{P\,}}_{ i}^{j-1} + \tau\, {\mathbf{P\,}}_{ i+1}^{j-1}, \\& \quad \tau\in(0,1),\;j \in \{ 1,\ldots,n\},\;i \in \{ 0,\ldots,n - j\}\end{array}$$

(1)

Then, $\{{\mathbf{P}}_{0}^{0},{\mathbf{P}}_{0}^{1},\ldots,{\mathbf{P}}_{0}^{n}\}$ are the control points of one segment and $\{{\mathbf{P}}_{0}^{n},{\mathbf{P}}_{1}^{n-1},$ …, P _n ⁰} are the another. (See an example in Fig. 1.) Note that, by applying the properties of Bezier Curves described in the previous subsection, both of the subdivided curves end at ${\mathbf{P}}_{0}^{n}$ and the one is tangent to $\overline{{\mathbf{P}}_{0}^{n-1}{\mathbf{P}}_{0}^{n}}$ and the other is to $\overline{{\mathbf{P}}_{0}^{n}{\mathbf{P}}_{1}^{n-1}}$ at the point. Since ${\mathbf{P}}_{0}^{n}$ is chosen on $\overline{{\mathbf{P}}_{0}^{n-1}{\mathbf{P}}_{1}^{n-1}}$ by using (1), the three points ${\mathbf{P}}_{0}^{n-1}$, ${\mathbf{P}}_{0}^{n}$, and ${\mathbf{P}}_{1}^{n-1}$ are collinear.

Remark 1

A Bezier curve P(t) constructed by control points $\{{\mathbf{P}}_{0}^{0},{\mathbf{P}}_{1}^{0},\ldots,{\mathbf{P}}_{n}^{0}\}$ always passes through the point ${\mathbf{P}}_{0}^{n}$ and is tangent to $\overline{{\mathbf{P}}_{0}^{n-1}{\mathbf{P}}_{1}^{n-1}}$ at ${\mathbf{P}}_{0}^{n}$.

2.2 Derivatives, Continuity and Curvature

The derivatives of a Bezier curve can be determined by its control points. For a Bezier curve $\mathbf{P}(t) ={ \sum\nolimits }_{i=0\,}^{n}{B}_{i}^{n}(t){\mathbf{P}}_{i}$, the first derivative is given by

$$\dot{\mathbf{P}}(t) = \sum_{i=0}^{n-1}{B}_{ i}^{n-1}(t){\mathbf{D}}_{ i}$$

(2)

where D _i , control points of $\dot{\mathbf{{P}}}(t)$ are

$${ \mathbf{D}}_{i} = n({\mathbf{P}}_{i+1} -{\mathbf{P}}_{i})$$

Geometrically, (2) provides us with a tangent vector. The higher order derivative can be obtained by using the relationship of (2), iteratively.

Fig. 1

Subdividing a cubic Bezier curve with τ = 0. 4 by the de Casteljau Algorithm

Two Bezier curves P(t) and Q(t) are said to be C ^k at t ₀ continuous if

$$\mathbf{P}({t}_{0}) = \mathbf{Q}({t}_{0}),\;\dot{\mathbf{{P}}}({t}_{0}) = \dot{\mathbf{{Q}}}({t}_{0}),\;\ldots,\;{\mathbf{P}}^{(k)}({t}_{ 0}) ={ \mathbf{Q}}^{(k)}({t}_{ 0})$$

(3)

The curvature of a Bezier curve P(t) = (x(t), y(t)) at t is given by

$$\kappa (t) = \frac{\vert \dot{x}(t)\ddot{y}(t) -\dot{ y}(t)\ddot{x}(t)\vert } {{(\dot{{x}}^{2}(t) +\dot{ {y}}^{2}(t))}^{\frac{3} {2} }}$$

(4)

Lemma 1

For the path constructed by two Bezier curve segments $\mathbf{P}(t){\vert }_{t\in [{t}_{0},{t}_{1}]}$ and $\mathbf{Q}(t){\vert }_{t\in [{t}_{1},{t}_{2}]}$, if P(t) and Q(t) are at least C² continuous at t ₁ then the path has continuous curvature for every point on it.

Proof

The curvature is expressed in terms of the first and the second derivative of a curve in (4). Since the Bezier curves are defined as polynomial functions of t, their k-th derivative for all k = 1, 2, … are continuous. Hence, they always have continuous curvature for all t. For two different Bezier curves P(t) and Q(t), it is sufficient that κ(t ₁), the curvature at the junction node is continuous if $\dot{\mathbf{{P}}}(t) = \dot{\mathbf{{Q}}}(t)$ and $\ddot{\mathbf{{P}}}(t) = \ddot{\mathbf{{Q}}}(t)$ are continuous at t ₁.

3 Problem Statement

Consider the control problem of a ground vehicle with a mission defined by waypoints and corridor constraints in a two-dimensional free-space. Our goal is to develop and implement an algorithm for navigation that satisfies these constraints. That is divided into two parts: path planning and path following.

To describe each part, we need to introduce some notation. Each waypoint is denoted as ${\mathbf{W}}_{i} \in{\mathbb{R}}^{2}$ for i ∈ {1, 2, …, N}, where $N \in\mathbb{R}$ is the total number of waypoints. Corridor width is denoted as w _j for j ∈ {1, …, N − 1}, j-th widths of each segment between two waypoints, W _j and W _j+1. For the dynamics of the vehicle, the state and the control vector are denoted q(t) = (x _c (t), y _c (t), ψ(t))^T and u(t) = (v(t), ω(t))^T respectively, where (x _c , y _c ) represents the position of the center of gravity of the vehicle. The yaw angle ψ is defined to the angle from the X axis. v is the longitudinal velocity of the vehicle. $\omega=\dot{ \psi }$ is the yaw angular rate. State space is denoted as $\mathcal{C} = {\mathbb{R}}^{3}$. We assume that the vehicle follows that

$$\dot{\mathbf{q}}(t) = \left (\begin{array}{rll} \cos \psi (t)&0 \\\sin \psi (t)&0 \\0 &1 \end{array} \right )\mathbf{u}(t)$$

With the notation defined above, the path planning problem is formulated as: Given an initial position and orientation and a goal position and orientation of the vehicle, generate a path λ specifying a continuous sequence of positions and orientations of the vehicle satisfying the path constraints [4]. In other words, we are to find a continuous map

$$\lambda: [0,1] \rightarrow \mathcal{C}$$

with

$$\lambda (0) ={ \mathbf{q}}_{init}\;\hbox{ and }\;\lambda (1) ={ \mathbf{q}}_{goal}$$

where q _init = (W ₁, ψ ₀) and q _goal = (W _N , ψ _f ) are the initial and goal states of the path, respectively.

Given a planned path, we use the path following technique with feedback corrections as illustrated in Fig. 2. A position and orientation error is computed every 50 ms. A point z is computed with the current longitudinal velocity and heading of the vehicle from the current position. z is projected onto the reference trajectory at point p such that zp is normal to the tangent at p. The cross track error y _err is defined by the distance between z and p. The steering control ω uses a PID controller with respect to cross track error y _err .

$$\delta \omega= {k}_{p}{y}_{err} + {k}_{d}\frac{d{y}_{err}} {dt} + {k}_{i} \int\nolimits \nolimits {y}_{err}dt$$

Fig. 2

The position error is measured from a point z, projected in front of the vehicle, and onto the desired curve to point p

4 Path Planning Algorithm

In this section, two path planning methods based on Bezier curves are proposed. To describe the algorithms, let us denote ${\hat{\mathbf{b}}}_{j}$ as the unit vector codirectional with the outward bisector of ∠ W _j−1 W _j W _j+1 for j ∈ {2, …, N − 1} as illustrated in Fig. 3. The planned path must cross over the bisectors under the waypoint and the corridor constraints. The location of the crossing point is represented as ${\mathbf{W}}_{j} + {d}_{j} \cdot {{\hat{\mathbf{b}}}}_{j}$, where ${d}_{j} \in\mathbb{R}$ is a scalar value. The course is divided into segments G _i by bisectors. G _i indicates the permitted area for vehicles under corridor constraint w _i , from W _i to W _i+1.

Fig. 3

An example of the course with four waypoints. Gray area is the permitted area for vehicles under a corridor constraint

Bezier curves constructed by large numbers of control points are numerically unstable. For this reason, it is desirable to join low-degree Bezier curves together in a smooth way for path planning [6]. Thus both of the algorithms use a set of low-degree Bezier curves such that the neighboring curves are C ² continuous at their end nodes. This will lead to continuous curvature on the resulting path by Lemma 1.

The Bezier curves used for the path plannings are denoted as ${}^{i}\mathbf{P}(t) ={ \sum\nolimits }_{k=0}^{{n}_{i}}\,{B}_{k}^{{n}_{i}}(t) {\cdot\,}^{i}{\mathbf{P}}_{k}$ for i ∈ {1, …, M},  t ∈ [0, 1], where M is the total number of the Bezier curves and n _i is the degree of ⁱ P. The planned path λ is a concatenation of all ⁱ P such that

$$\lambda ((i - 1 + t)/M) =\ ^{i}P(t),\quad i \in \{ 1,\ldots,M\},\;t \in[0,1]$$

4.1 Path Planning Placing Bezier Curves within Segments (BS)

In this path planning method (BS), the Bezier curve ⁱ P for i ∈ {1, …, N − 1} are used within each segment G _i . The adjacent curves, ^j−1 P and ^j P are C ² continuous at the crossing point, ${\mathbf{W}}_{j} + {d}_{j} \cdot {{\hat{\mathbf{b}}}}_{j}$. The control points of ⁱ P, ⁱ P _k for k = {0, …, n _i } are determined to maintain these conditions.

• The beginning and the end point are W ₁ and W _N .

  $${ }^{1}{\mathbf{P}}_{ 0} ={ \mathbf{W}}_{1},{\quad }^{N-1}{\mathbf{P}}_{{ n}_{N-1}} ={ \mathbf{W}}_{N}$$

  (5)

• The beginning and end orientation of λ are ψ ₀ and ψ _f .

  $${ }^{1}{\mathbf{P}}_{ 1} ={ \mathbf{W}}_{1} + {l}_{0}(\cos {\psi }_{0},\sin {\psi }_{0}),\quad {l}_{0} \in{\mathbb{R}}^{+} $$

  (6a)
  $${ }^{N-1}{\mathbf{P}}_{{ n}_{N-1}-1} ={ \mathbf{W}}_{N} - {l}_{f}(\cos {\psi }_{f},\sin {\psi }_{f}),\quad {l}_{f} \in\mathbb{R}^{+} $$

  (6b)

• ^j−1 P and ^j P, ∀j ∈ {2, …, N − 1} are C ² continuous at the crossing point.

  $${ }^{j-1}{\mathbf{P}}_{{ n}_{j-1}} {=\ }^{j}{\mathbf{P}}_{ 0} ={ \mathbf{W}}_{j} + {d}_{j} \cdot {{\hat{\mathbf{b}}}}_{j} $$

  (7a)
  $${n}_{j-1}{(}^{j-1}{\mathbf{P}}_{{ n}_{j-1}} {-}\ ^{j-1}{\mathbf{P}}_{{ n}_{j-1}-1}) = {n}_{j}{(}^{j}{\mathbf{P}}_{ 1} {-}\ ^{j}{\mathbf{P}}_{ 0}) $$

  (7b)
  $$\begin{array}{rll}{n}_{j-1}({n}_{j-1} - 1){(}^{j-1}{\mathbf{P}}_{{ n}_{j-1}} - 2 \,{\cdot }\,^{j-1}{\mathbf{P}}_{{ n}_{j-1}-1} {+\, }^{j-1}{\mathbf{P}}_{{ n}_{j-1}-2}) \\\quad = {n}_{j}({n}_{j} - 1){(}^{j}{\mathbf{P}}_{ 2} - 2 {\cdot }^{j}{\mathbf{P}}_{ 1} {+ }\,^{j}{\mathbf{P}}_{ 0}) \end{array}$$

  (7c)

• The crossing points are bounded within the corridor.

  $$\vert {d}_{j}\vert\ <\ \forall \frac{1} {2}\min ({w}_{j-1},{w}_{j})$$

  (8)

• ⁱ P _k , k = {1, …, n _i − 1} always lie within the area of G _i .

  $${ }^{i}{\mathbf{P}}_{ 1} \in{G}_{i},\;\ldots {,\;}^{i}{\mathbf{P}}_{{ n}_{i}-1} \in{G}_{i}$$

  (9)

Equations (6a, b) are derived by using the tangent property of Bezier curves at their end points. Equations (7a–c) are obtained by applying (2) and (3). Equation (9) makes the resulting Bezier curve satisfy the corridor constraint by the convex hull property. At each crossing point, three control points of each adjacent Bezier curve are dedicated to the C ² continuity constraint by (2), (4), and Lemma 1. So the minimum number of control points to satisfy the constraints independent of the others are six for ⁱ P, i ∈ {2, …, N − 2}. On other hand, ¹ P needs five: three to connect ² P plus two to connect W ₁ with slope of ψ₀. Likewise, ^2N−3 P needs five. Thus, n _i is determined as

$${ n}_{i} =\bigg\{ \begin{array}{rll} 4,\quad i \in \{ 1,N - 1\} \\5, \quad i \in \{ 2, \ldots, N - 2\} \end{array}$$

Note that ¹ P ₀ and ${}^{N-1}{\mathbf{P}}_{{n}_{N-1}}$ are fixed in (5). ¹ P ₁ and ${}^{N-1}{\mathbf{P}}_{{n}_{N-1}-1}$ are fixed in (6a, b). ${}^{j-1}{\mathbf{P}}_{{n}_{j-1}}$ and ^j P ₀ rely on d _j in (7a–c). ${}^{j-1}{\mathbf{P}}_{{n}_{j-1}-1}$ and ${}^{j-1}{\mathbf{P}}_{{n}_{j-1}-2}$ rely on ^j P ₁ and ^{j − 1} P ₂. So the free variables are, ∀j ∈ {2, …, N − 1}, P ₁ = {^j P ₁}, P ₂ = {^j P ₂}, d = {d _j }, and L = {l ₀, l _f }. The number of the variables or the degrees of freedom is 5N − 8. The variables are computed by minimizing the constrained optimization problem:

$$\mathop{ \min }_{{\mathbf{P}}_{1},{\mathbf{P}}_{2},\mathbf{d},\mathbf{L}}J = \sum_{i=1}^{N-1}{J}_{ i}$$

(10)

subject to (8) and (9), where J _i is the cost function of ⁱ P(t), given by

$${J}_{i} ={ \int\nolimits \nolimits }_{0}^{1}[({a}_{i}(\dot{{x}}^{2}(t) +\dot{ {y}}^{2}(t)) + {b}_{ i}{\vert }^{i}\kappa(t){\vert}^{2}]dt $$

(11)

where ${a}_{i} \in\mathbb{R}$ and ${b}_{i} \in\mathbb{R}$ are constants that control the tradeoff between arc lengths versus curvatures of the resulting path.

4.2 Path Planning Placing Bezier Curves on Corners (BC)

Another path planning method (BC) adds quadratic Bezier curves on the corner area around W _j , j ∈ {2, …, N − 1}. The quadratic Bezier curves are denoted as ${}^{j}\mathbf{Q}(t) ={ \sum\nolimits }_{k=0}^{2}\,{B}_{k}^{2}(t) {\cdot\,}^{j}{\mathbf{Q}}_{k}^{0}$ intersects the j-th bisector. The first and the last control point, ${}^{j}{\mathbf{Q}}_{0}^{0}$ and ${}^{j}{\mathbf{Q}}_{2}^{0}$ are constrained to lie within G _j−1 and G _j , respectively. Within each segment G _i , another Bezier curve is used to connect the end points of ^j Q with C ² continuity and/or W ₁ with slope of ψ ₀ and/or W _N with ψ _f . Hence, ^j Q is the curve segments of even number index:

$${ }^{2(\,j-1)}\mathbf{P}(t) =\ ^{j}\mathbf{Q}(t),\quad j \in \{ 2,\ldots,N - 1\}$$

The degree of ⁱ P is determined by the minimum number of control points to satisfy C ² continuity constraint, independently:

$${ n}_{i} = \left \{\begin{array}{rll} 4,\quad i \in \{ 1,2N - 3\} \\2,\quad i \in \{ 2,4,\ldots,2N - 4\} \\5,\quad i \in \{ 3,5,\ldots,2N - 5\} \end{array} \right.$$

Let t _c denote the Bezier curve parameter corresponding to the crossing point of ^j Q(t) on the bisector, such that

$${ }^{j}\mathbf{Q}({t}_{ c}) ={ \mathbf{W}}_{j} + {d}_{j} \cdot {{\hat{\mathbf{b}}}}_{j}.$$

(12)

Let θ _j denote the angle of the tangent vector at the crossing point from X-axis:

$${ }^{j}\dot{\mathbf{{Q}}}({t}_{ c}) = \left({\vert }^{j}\dot{\mathbf{{Q}}}({t}_{ c})\vert \cos {\theta }_{j},\;{\vert }^{j}\dot{\mathbf{{Q}}}({t}_{ c})\vert \sin {\theta }_{j}\right).$$

(13)

The notations are illustrated in Fig. 4. Due to the constraint of ${}^{j}{\mathbf{Q}}_{0}^{0}$ and ${}^{j}{\mathbf{Q}}_{2}^{0}$ within G _{j − 1} and G _j , the feasible scope of θ _j is limited to the same direction as W _j+1 is with respect to ${{\hat{\mathbf{b}}}}_{j}$. In other words, if W _j+1 is to the right of ${{\hat{\mathbf{b}}}}_{j}$, then θ _j must point to the right of ${{\hat{\mathbf{b}}}}_{j}$, and vice versa.

Fig. 4

The geometry of the ^j Q(t) in corner area around W _j

Given ${}^{j}{\mathbf{Q}}_{0}^{0}$, ${}^{j}{\mathbf{Q}}_{2}^{0}$, d _j , and θ _j , the other control point ${}^{j}{\mathbf{Q}}_{1}^{0}$ is computed such that the crossing point is located at ${\mathbf{W}}_{j} + {d}_{j} \cdot {{\hat{\mathbf{b}}}}_{j}$ and the angle of the tangent vector at the crossing point is θ _j . Since each control point is two-dimensional, the degrees of freedom of ^j Q(t) are six. Since d _j and θ _j are scaler, representing ^j Q(t) in terms of ${}^{j}{\mathbf{Q}}_{0}^{0}$, ${}^{j}{\mathbf{Q}}_{2}^{0}$, d _j , and θ _j does not affect the degrees of freedom. However, it comes up with an advantage for corridor constraint. If we compute ${}^{j}{\mathbf{Q}}_{1}^{0}$ such as above, the points computed by applying the de Casteljau algorithm such that two subdivided curves are separated by the j-th bisector are represented as ${}^{j}{\mathbf{Q}}_{0}^{0}$ and ${}^{j}{\mathbf{Q}}_{2}^{0}$ as described in the following. The two curves are constructed by ${\{}^{j}{\mathbf{Q}}_{0}^{0}{,\ }^{j}{\mathbf{Q}}_{0}^{1}{,\ }^{j}{\mathbf{Q}}_{0}^{2}\}$ and ${\{}^{j}{\mathbf{Q}}_{0}^{2}{,\ }^{j}{\mathbf{Q}}_{1}^{1}{,\ }^{j}{\mathbf{Q}}_{2}^{0}\}$. We can test if the convex hull of ${\{}^{j}{\mathbf{Q}}_{0}^{0}{,\ }^{j}{\mathbf{Q}}_{0}^{1}{,\ }^{j}{\mathbf{Q}}_{0}^{2}\}$ lies within G _{j − 1} and if that of ${\{}^{j}{\mathbf{Q}}_{0}^{2}{,\ }^{j}{\mathbf{Q}}_{1}^{1}{,\ }^{j}{\mathbf{Q}}_{2}^{0}\}$ lies within G _j in (26), instead of testing that of ${\{}^{j}{\mathbf{Q}}_{0}^{0}{,\ }^{j}{\mathbf{Q}}_{1}^{0}{,\ }^{j}{\mathbf{Q}}_{2}^{0}\}$. (Note that ${}^{j}{\mathbf{Q}}_{1}^{0}$ is not constrained to lie within corridor as shown in Fig. 4.) So, the convex hull property is tested for tighter conditions against the corridor constraint without increasing the degrees of freedom.

In order to compute ${}^{j}{\mathbf{Q}}_{1}^{0}$, the world coordinate frame T is transformed and rotated into the local frame ^j T where the origin is at the crossing point, ^j Q(t _c ) and X axis is codirectional with the tangent vector of the curve at the crossing point, ${}^{j}\dot{\mathbf{{Q}}}({t}_{c})$. Let us consider the subdivision ratio, τ^∗ ∈ (0, 1) such that the location of ${}^{j}{\mathbf{Q}}_{0}^{2}$ computed by applying the de Casteljau algorithm with it is the crossing point. In other words, τ^∗ has ${}^{j}{\mathbf{Q}}_{0}^{2}$ be at the origin with respect to ^j T frame. Fig. 5 illustrates the control points of ^j Q(t) with respect to ^j T frame. Note that ${}^{j}{\mathbf{Q}}_{0}^{2}$ is at the origin by the definition of ^j T and τ^∗. ${}^{j}{\mathbf{Q}}_{0}^{1}$ and ${}^{j}{\mathbf{Q}}_{1}^{1}$ are on the X axis by the definition of ^j T and Remark 1. Let the coordinates of the control points be denoted as ${\mathbf{Q}}_{i}^{0} = \big{(}{x}_{i},{y}_{i}\big{)}$, i ∈ {0, 1, 2}, where all coordinates are with respect to ^j T.

Lemma 2

Given d_j and θ_j, for ^j Q(t) to intersect j-th bisector with the crossing point determined by the d_j and (12), and the tangent vector at the point determined by the θ_j and (13), it is necessary that y ₀ y ₂ ≥ 0.

Proof

Let (x(t), y(t)) denote the coordinate of ^j Q(t) with respect to ^j T. By the definition of ^j T and Remark 1, Q(t) passes through the origin with tangent slope of zero with respect to ^j T. That is, x(t _c ) = 0, y(t _c ) = 0 and $\dot{y}({t}_{c}) = 0$. Suppose that y ₀ = y(0) < 0. Since y(t) is a quadratic polynomial, $\dot{y}(t)\ >\ 0$ and $\ddot{y}(t)\ <\ 0$ for t ∈ [0, t _c ). Subsequently, $\dot{y}(t)\ <\ 0$ and $\ddot{y}(t)\ <\ 0$ for t ∈ (t _c , 1]. Thus, y ₂ = y(1) < 0 and y ₀ y ₂ > 0. Similarly, if y ₀ > 0 then y ₁ > 0. If y ₀ = 0 then $\dot{y}(t) = 0$“ for t ∈ [0, 1] and y ₂ = 0. Thus, y ₀ y ₂ = 0. □

We are to calculate ${}^{j}{\mathbf{Q}}_{1}^{0}$ depending on whether y ₀ y ₂ is nonzero. For simplicity, superscript j is dropped from now on. Without loss of generality, suppose that y ₀ < 0 and y ₂ < 0. ${\mathbf{Q}}_{0}^{2}$ is represented as

$${ \mathbf{Q}}_{0}^{2} = (1 - {\tau }{{^\ast}}){\mathbf{Q}}_{ 0}^{1} + {\tau }^{{\ast}}{\mathbf{Q}}_{ 1}^{1}$$

by applying (1). So the coordinates of ${\mathbf{Q}}_{0}^{1}$ and ${\mathbf{Q}}_{1}^{1}$ can be represented as

$${ \mathbf{Q}}_{0}^{1} = ( - \alpha {\tau }^{{\ast}},0),\;{\mathbf{Q}}_{ 1}^{1} = (\alpha (1 - {\tau }^{{\ast}}),0),\quad \alpha\ >\ 0$$

(14)

where α > 0 is some constant. Applying (1) with i = 0 and j = 1 and arranging the result with respect to ${\mathbf{Q}}_{1}^{0}$ by using (14) gives

$${ \mathbf{Q}}_{1}^{0} = \bigg( - \alpha-\frac{1 - {\tau }^{{\ast}}} {{\tau }^{{\ast}}} {x}_{0},-\frac{1 - {\tau }^{{\ast}}} {{\tau }^{{\ast}}} {y}_{0}\bigg)$$

(15)

Similarly, applying (1) with i = 1 and j = 1 yields

$${ \mathbf{Q}}_{1}^{0} = \left(\alpha- \frac{{\tau }^{{\ast}}} {1 - {\tau }^{{\ast}}}{x}_{2},- \frac{{\tau }^{{\ast}}} {1 - {\tau }^{{\ast}}}{y}_{2}\right)$$

(16)

where α and τ^∗ are obtained by equations (15) and (16):

$${\tau }^{{\ast}} = \frac{1} {1 + \sqrt{{y}_{2 } /{y}_{0}}},\quad \alpha= \frac{{x}_{0}{y}_{2} - {y}_{0}{x}_{2}} {2{y}_{0}\sqrt{{y}_{2 } /{y}_{0}}}$$

(17)

τ^∗ and α have the square root of y ₂/y ₀. So, if y ₀ y ₂ < 0 then τ^∗ and α are not determined, hence, Q(t) is infeasible. That is, (17) ends up with Lemma 2.

Fig. 5

The geometry of the control points of ^j Q(t) with respect to ^j T

If y ₀ = y ₂ = 0 then all control points of ^j Q are on X axis (see proof of Lemma 2). In the geometric relation of control points and the points computed by applying the de Casteljau algorithm as shown in Fig. 6, we obtain

$$\begin{array}{rll} {x}_{0} & = -(\alpha+ \beta )\tau\\{x}_{2} & = (\alpha+ \gamma )(1 - \tau ) \\\alpha& = \beta (1 - \tau ) + \gamma \tau\end{array}$$

(18)

where α > 0, β > 0, γ > 0 are some constants. Using (18), ${\mathbf{Q}}_{1}^{0} = ({x}_{1},0)$ is represented in terms of arbitrary τ ∈ (0, 1):

$${x}_{1} = -\frac{1} {2}\left(\frac{1 - \tau } {\tau } {x}_{0} + \frac{\tau } {1 - \tau }{x}_{2}\right)$$

(19)

Fig. 6

The geometry of ^j Q(t) with respect to ^j T when y ₀ = y ₂ = 0

The constraints imposed on the planned path are formulated as follows:

• The beginning and end position of λ are W ₁ and W _N .

  $${ }^{1}{\mathbf{P}}_{ 0} ={ \mathbf{W}}_{1},{\quad }^{2N-3}{\mathbf{P}}_{ 4} ={ \mathbf{W}}_{N}$$

  (20)

• The beginning and end orientation of λ are ψ ₀ and ψ _f .

  $${ }^{1}{\mathbf{P}}_{ 1} ={ \mathbf{W}}_{1} + {l}_{0}(\cos {\psi }_{0},\sin {\psi }_{0}),\quad {l}_{0} \in{\mathbb{R}}^{\,+\,} $$

  (21a)
  $${ }^{2N-3}{\mathbf{P}}_{ 3} ={ \mathbf{W}}_{N} - {l}_{f}(\cos {\psi }_{f},\sin {\psi }_{f}),\quad {l}_{f} \in{\mathbb{R}}^{\,+\,} $$

  (21b)

• ⁱ⁻¹ P and ⁱ P, ∀i ∈ {2, …, 2N − 3} are C ² continuous at the junctions.

  $${ }^{i-1}{\mathbf{P}}_{{ n}_{i-1}} =\ ^{i}{\mathbf{P}}_{ 0} $$

  (22a)
  $${n}_{i-1}{(}^{i-1}{\mathbf{P}}_{{ n}_{i-1}} {-\ }^{i-1}{\mathbf{P}}_{{ n}_{i-1}-1}) = {n}_{i}{(}^{i}{\mathbf{P}}_{ 1} {\,-\,}^{i}{\mathbf{P}}_{ 0}) $$

  (22b)
  $$\begin{array}{rll} {n}_{i-1}({n}_{i-1} - 1){(}^{i-1}{\mathbf{P}}_{{ n}_{i-1}} - 2 {\,\cdot\, }^{i-1}{\mathbf{P}}_{{ n}_{i-1}-1} {\,+\, }^{i-1}{\mathbf{P}}_{{ n}_{i-1}-2}) \\ \quad = {n}_{i}({n}_{i} - 1){(}^{i}{\mathbf{P}}_{ 2} - 2 {\,\cdot\, }^{i}{\mathbf{P}}_{ 1} {+ }^{i}{\mathbf{P}}_{ 0}) \end{array}$$

  (22c)

• The crossing points are bounded within the corridor.

  $$\vert {d}_{j}\vert\ <\ \frac{1} {2}\min ({w}_{j-1},{w}_{j}),\quad \forall j \in \{ 2,\ldots,N - 1\}$$

  (23)

• θ _j has the same direction as W _j+1 is with respect to ${{\hat{\mathbf{b}}}}_{j}$.

  $$\begin{array}{rll} & {\rm mod}\,\,(\angle ({\mathbf{W}}_{j+1} -{\mathbf{W}}_{j}) -\angle {{\hat{\mathbf{b}}}}_{j},2\pi )\ >\ \pi\\& \;\Rightarrow {\rm mod}\,\, ({\theta }_{j} -\angle {{\hat{\mathbf{b}}}}_{j},2\pi )\ >\ \pi, \end{array}$$

  (24a)
  $$\begin{array}{rll} & {\rm mod}\,\,(\angle ({\mathbf{W}}_{j+1} -{\mathbf{W}}_{j}) -\angle {{\hat{\mathbf{b}}}}_{j},2\pi )\ <\ \pi\\& \;\Rightarrow{\rm mod}\,\, ({\theta }_{j} -\angle {{\hat{\mathbf{b}}}}_{j},2\pi )\ <\ \pi\end{array}$$

  (24b)

• ${}^{j}{\mathbf{Q}}_{0}^{0}$ and ${}^{j}{\mathbf{Q}}_{2}^{0}$ with respect to ^j T satisfies Lemma 2.

  $${y}_{0}{y}_{2} \geq0$$

  (25)

  where y ₀ and y ₂ are with respect to ^j T.

• ${}^{j}{\mathbf{Q}}_{0}^{0}$ and ${}^{j}{\mathbf{Q}}_{0}^{1}$ lie within G _j−1. ${}^{j}{\mathbf{Q}}_{2}^{0}$ and ${}^{j}{\mathbf{Q}}_{1}^{1}$ lie within G _j .

  $${ }^{j}{\mathbf{Q}}_{ 0}^{0} \in{G}_{ j-1}{,\;}^{j}{\mathbf{Q}}_{ 0}^{1} \in{G}_{ j-1}{,\;}^{j}{\mathbf{Q}}_{ 2}^{0} \in{G}_{ j}{,\;}^{j}{\mathbf{Q}}_{ 1}^{1} \in{G}_{ j}$$

  (26)

• {ⁱ P ₁, …, ⁱ P _{n i−1}} always lie within the area of G _i .

  $${ }^{i}{\mathbf{P}}_{ 1} \in{G}_{i},\;\ldots {,\;}^{i}{\mathbf{P}}_{{ n}_{i}-1} \in{G}_{i},\quad i \in \{ 1,3,\ldots,2N - 3\}$$

  (27)

The free variables are, for j ∈ {2, …, N − 1}, Q = {^j Q ₀, ^j Q ₂}, d = {d _j }, θ = {θ _j }, and L = {l ₀, l _f }. The degrees of freedom is 6N − 10. The variables are computed by minimizing the constrained optimization problem:

$$\min \limits_{{\mathbf{Q},\mathbf{d},\mathbf{\theta },\mathbf{L}}}\ J = \sum_{i=1}^{N-1}{J}_{ i}$$

(28)

subject to (23), (24,b), (25), (26), and (27), where J _i is the cost function of ⁱ P(t), defined in (11). Notice that the convex hull property is tested for ${}^{j}{\mathbf{Q}}_{0}^{1}$ and ${}^{j}{\mathbf{Q}}_{1}^{1}$ of the divided curves instead of ${}^{j}{\mathbf{Q}}_{1}^{0}$ in (26). Thus, it comes up with tighter conditions for curves against the corridor constraint.

5 Simulation Results

Simulations were run for the course with four waypoints and identical corridor width of 8 as illustrated in Fig. 7. The initial and goal orientation are given by ψ ₀ = ψ _f = 0. For path following, the constant longitudinal velocity v(t) = 10 m/s is used. The magnitude of ω is bounded within |ω| _max = 25 rpm. The PID gains are given by: k _p = 2, k _d = 1, and k _i = 0.1.

Fig. 7

The planned paths by BS method (top) and by BC method (bottom). Each pair of results are obtained by minimizing arc lengths (left) and by minimizing curvatures (right)

In Fig. 7, the reference paths (dashed curves) planned by applying the BS and BC method are placed in top and bottom row, respectively. The actual trajectories (solid curves) are generated by using the proposed tracking method. In the figures, stars indicate the location of control points of Bezier curve segments at an even number index, and empty circles indicate those of the others. The paths in the left column are planned by minimizing the summation of (11) with a _i = 1 and b _i = 0. Since only arc length is penalized, the cost function leads to paths with minimum arc length, which we denote ${\lambda }_{BS}^{l}$ and ${\lambda }_{BC}^{l}$ for the BS and BC method, respectively. On other hand, the paths in the right column are planned by minimizing the cost function with a _i = 0 and b _i = 1 so that resulting paths with larger radii of curvature are provided. We denote them ${\lambda }_{BS}^{c}$ and ${\lambda }_{BC}^{c}$ for the BS and BC method, respectively. ${\lambda }_{BS}^{c}$ and ${\lambda }_{BC}^{c}$ generate longer but smoother trajectory guidance than ${\lambda }_{BS}^{l}$ and ${\lambda }_{BC}^{l}$. Taking a look at the tracking results on ${\lambda }_{BS}^{l}$ and ${\lambda }_{BC}^{l}$, the vehicle overshoots in sharp turns around W ₃ resulting in a large position error (see Fig. 8) due to the limit on steering angle rate. The commanded steering angle rate on ${\lambda }_{BC}^{l}$, marked by ‘*’ in Fig. 9 undergoes rapid changes and is constrained by the rate limit. However, on ${\lambda }_{BS}^{c}$ and ${\lambda }_{BC}^{c}$, the vehicle tracks the part of the planned paths accurately thanks to larger radii of curvature. We can verify that more tangibly in cross track errors plot provided in Fig. 8. Also, the steering control signal on ${\lambda }_{BC}^{c}$, marked by ‘o’ in Fig. 9 is smoother than that on ${\lambda }_{BC}^{l}$.

Fig. 8

The cross track errors by BC

Fig. 9

The steering controls by BC

The main difference of the proposed algorithm from the previous ones of [1] is the degree of continuity at the junctions: C ². Assuming that the vehicle tracks a reference path perfectly and v is continuous, if κ is continuous then ω is continuous because ω = κv. When v is constant as this simulation, since ω is proportional to κ, the continuity characteristic of ω tends to follow that of κ. Since the previous algorithms imposed only C ¹ continuity on junction nodes of the path, κ is discontinuous at the nodes. Hence the path underwent the discontinuity of angular rate, that is, large angular acceleration, which leads to large torque on the vehicle. On other hand, the proposed algorithm has κ continuous along the resulting paths. If the path has curvature small enough so that the vehicle is able to track it accurately given a maximum steering angle rate, then the steering control signal will be smooth, as that of ${\lambda }_{BC}^{c}$ in Fig. 9.

6 Conclusions

This paper presents two path planning algorithms based on Bezier curves for autonomous vehicles with waypoints and corridor constraints. Bezier curves provide an efficient way to generate the optimized path and satisfy the constraints at the same time. The simulation results also show that the trajectory of the vehicle follows the planned path within the constraints.