Printer Friendly

Time-Optimal Trajectory Planning along Parametric Polynomial Lane-Change Curves with Bounded Velocity and Acceleration: Simulations for a Unicycle Based on Numerical Integration.

1. Introduction

As an essential part of the active safety system of autonomous driving or human-driven cars [1-3], a lane-change maneuver performed by a vehicle on a terrain [4, 5] is a path following or trajectory tracking task for avoiding vehicle-to-vehicle collisions. It involves decision, sensing of traffic flow and environmental conditions, planning, and control subject to the constraints such as path constraints, kinodynamic constraints, environmental constraints, and real-time requirements. The planning task of lane change requires the generation of path and velocity, or trajectory for the vehicle to follow. It is commonly used for testing autonomous or human-driving vehicle performance such as critical speed for no sideslip, or for path and trajectory design [6]. Besides, it is considered as a method for measuring vehicle-handling performance such as safety and completing time of lane-changing maneuver, especially at high speed or within restricted steering space.

Curves which produce a transition between parallel lanes (from current lane to a neighboring target lane) in the same direction are called lane-change curves [5]. The lane-change curve connects symmetric interpolating boundary configurations, that is, the same tangent angle and curvature. Following a lane-change curve, the vehicle will be traveling in the same direction at the end of the maneuver as it was traveling at the start; that is, there is no change of heading between the start and the final configurations. Lane-change trajectory consists of a lane-change path and a velocity profile along the path. For lane changing from current lane to a neighboring lane, the allowable longitudinal displacement for longitudinal trajectory planning and travel time for lateral trajectory planning [1, 7] are used to determine the feasibility of performing a lane change. Various forms of lane-change trajectories are proposed (e.g., [1-3, 5, 8-10]), which serve as reference trajectories for the vehicle controller (e.g., PID or model predictive control) to follow in high-speed autonomous driving. The approach of intervehicle traffic gap and time instance [1] was proposed to generate safe and smooth lane-change (longitudinal and lateral) trajectories for a discrete time vehicle model of double integrator. Bai et al. [2] and Altch'e et al. [11] proposed a 5-degree polynomial trajectory of time function as the vehicle trajectory. The jerk, as a measure of comfort, is a quadratic polynomial of time. Geng et al. [3] proposed a 6-degree polynomial trajectory of the form y = [[summa].sup.6.sub.i=3][a.sub.i][x.sup.i], where x and y denote the forward/longitudinal and lateral position, respectively. Based on field test vehicle-driver integration data, Wang et al. [9] proposed a lane-change trajectory represented by a combination of linear and sinusoidal functions in terms of lane-change ratio. McNally [10] used a cubic polynomial to blend a target lane-change curve. Under the assumption that the vehicle keeps its longitudinal vehicle constant throughout the lane-change maneuver, comparative simulations of candidate curves were performed in terms of path length, minimal yaw transients, and jerk but no time efficiency in an earlier work [8].

In addition to smooth shorter motion, time-efficient or faster motion is of major concern for trajectory optimization. Due to fuel economy, smooth (or continuous curvature) motion generation and motion optimization along the given path or for state-to-state transfer [12-16] subject to the velocity or acceleration and other constraints are desirable to achieve time or energy efficiency and safety. In [15], quintic B-splines are used to blend linear segments to enhance the smoothness of cornering motion and cycle time of CNC machine tools. Model predictive control is applied by Mahdi Ghazaei Ardakani [16] to deal with fixed-time trajectory-generation problem with a minimum-jerk cost functional under velocity and acceleration constraints. Aspects of time-efficient state-to-state motions for different mobility platforms like mobile robots, humanoid robots, robotic manipulators, or autonomous-driving vehicles have been studied for decoupled motion planning approach of trajectory optimization. This path-velocity decomposition approach gains its popularity due to the reduced complexity at the cost of losing generality. In the decoupled approach, two subproblems of the geometric path planning between two configurations and time scaling (or velocity planning) along the planned path subject to kinodynamic constraints are treated independently. Algorithms and properties of a time-optimal trajectory along prespecified paths parameterized by arc length, that is, the computation of switch points and optimal input subject to state and input constraints, were developed [17-25]. The solution is based on necessary conditions derived from Pontryagin maximum principle (PMP) or dynamic programming principle (DPP) for the optimality of trajectory and control. There are different numerical algorithms based on Pontryagin maximum principle or receding horizon techniques to generate the control input and state trajectory to move the systems from an initial state to a goal state as fast as possible, while respecting the equality or inequality constraints imposed on the state and input of systems [21, 24, 26, 27].

Velocity planning along a path not only depends on vehicle dynamics, its kinodynamic constraints such as the velocity and acceleration constraints, and all other constraints on motion [15, 17] but also on the characteristics of the path such as the curvature defining the ratio of angular over linear velocity (see, e.g., [30, 31]). There exist a number of time-optimal velocity planning algorithms along a pre-specified path whose study of motion optimization is to minimize the travel time required to move along the whole path subject to vehicle system dynamics, any other constraints such as path constraints, torque/acceleration constraints, and velocity constraints for different systems (see, e.g., [16, 24]). The adopted vehicle model can be varied for different studies [2, 11] where the bounding geometry of the vehicle shape could be circle, ellipse, or rectangular. Unicycle is a popular model for control and trajectory planning of nonholonomic wheeled mobile robots in that it offers very good compromise between accuracy and computational efficiency for simulation and prediction of nonholonomic autonomous vehicle motion (e.g., [12, 25, 32-34]), that is, zero lateral velocity for emulating the vehicle performance. It is used in this paper as a simulated vehicle model for lane-change trajectories.

As mentioned in [3], aggressive lane change is completed within the shortest possible time. This paper takes the overall travel time along the path as the cost to be minimized over the velocity for safety-critical concern of lane-change maneuvering by a unicycle subject to kinodynamic constraints on velocity and acceleration. More specifically, we focus on three types of symmetric parametric polynomial curves with closed-form position expressions that are very popular for robotic applications as candidate lane-change paths for autonomous driving. The aim of this work is to study via simulation-based evaluation the path geometric characteristic related to time-optimal lane-change path constrained trajectory planning. Among the methods to solve the time-optimal velocity planning or time-optimal path parameterization along the fixed lane-change path, numerical integration (NI) [21, 24] is directly employed as the main simulation tool to make a number of runs to obtain the time-optimal velocity profile on each path in different simulation conditions without violating the bounds on velocity and acceleration. In particular, the acceleration and velocity constraints for a unicycle can be expressed as linear in squared path velocity ([s'.sup.2] in (12)), which allows the solution to TOPP for each curve particularly simple. The major contribution of this paper based on time-optimal velocity simulation-based evaluation of parametric polynomials for lane-change maneuvering as a path following task by a unicycle in constrained maneuver space is as follows:

(i) Simulation results based on unicycle provides a good understanding of velocity and acceleration characteristics and the switching structure defined by the velocity limit curve caused by all the constraints of time-optimal solution along parametric polynomial lane-change curves. It provides a supplementary comparative study of assessment of least amount of travel time along parametric polynomial lane-change curves, as compared to earlier work [8]. After NI simulation-based evaluation of the minimum travel time along all candidate curves, in order to perform a lane-change maneuver faster, the selection of lane-change path based on path geometric characteristics is proposed. The results such as feasibility of parametric polynomials for a lane-change maneuver in a wider range of situations could be a reference for simulating more refined and complete dynamic model and lane-change curves other than the families of parametric polynomials. The minimal lane-changing time by a unicycle provides an ideal lower bound estimate for further optimization of lane-change maneuver along a given path, such as the data-driven design of continuous acceleration profile for smooth, faster motion. In addition, the highest speed allowed is also concerned for comfort and safety in autonomous driving.

(ii) Velocity along a given path is curvature dependent, which is determined by available constrained maneuver space, for example, as in parallel parking [35]. The authors in [25] among others pointed out that the travel time of mobile robot navigation depends on the path shape and the velocity profile, in which the admissible velocity is affected by the curvature. These observations are based on piecewise constant longitudinal accelerations defined over distance along the curve. Among all the evaluated curves, our simulations confirm how the minimum travel time required for a unicycle is affected by the path characteristics such as path length and maximum curvature coupled with the bounds on velocity and acceleration in a more concrete setting of simulating lane-change curves in different scenarios.

(iii) The comparison results of lane-change paths have implications for curve selection of parallel parking in constrained steering space studied in [35], since parallel parking maneuver bears similarity to lane-change maneuver in terms of interpolating boundary conditions (position, heading, and curvature at the endpoints).

This paper is an extension of our conference paper [28]. The rest of the paper is organized as follows. In Section 2, kinematic unicycle model, symmetric planar curve, time-optimal velocity planning with velocity and acceleration bounds, and numerical integration to compute the maximum velocity profile on the path are introduced. In Section 3, an offered set of three symmetric parametric polynomial curves for

lane change is derived. In Section 4, the comparative simulation-based evaluation of each parametric polynomial curve is presented for two end configurations (loose and hard curvature conditions) to highlight the effect of length and maximum curvature along the curve on the minimum travel time in distinct situations. Conclusion is made in the last section.

2. Background

2.1. Kinematic Model of Unicycle. Let O-x-y be a global coordinate frame with x-axis being the forward/longitudinal direction. The configuration [[p(u), [theta](u)].sup.T] of a unicycle, depicted in Figure 1, moving in a planar workspace along a parametric path p(u) = [[x(u), y(u)].sup.T] parameterized by the path parameter u is given by its Cartesian position coordinates (x(u), y(u)) and orientation [theta] denoting the heading of unicycle. For unicycle, [theta] is equal to the angle between the x-axis and the tangent of the path p (u). The kinematic equations of motion are described as follows:

[mathematical expression not reproducible], (1)

where the symbol "'" denotes the derivative with respect to the argument (here u), and v and [omega] are the inputs of linear and angular speed, respectively. The nonholonomic nature (1) is caused by the fact that wheels can only roll but not slip, which satisfies the following equation:

-x' sin [theta] + y' cos [theta] = 0. (2)

The command inputs to the unicycle (1) are reference longitudinal/translational and angular velocities. The unicycle model satisfies the Lie algebra rank condition for nonlinear controllability [36], and thus a diverse set of paths is traversable by the unicycle, where to traverse along the path, the velocity vector of the unicycle is parallel to the tangent of the path at each point on the path (i.e., zero lateral velocity). However, the smoothness requirement and curvature constraint of non-holonomic vehicle motion restrict the class of path primitives suitable for a task, such as the lane change in this work.

From the unicycle kinematics, [theta], v, and [omega] can be expressed as the functions of x, y, and their first and second derivatives as follows:

[mathematical expression not reproducible], (3)


[mathematical expression not reproducible], (4)

and the curvature is defined by the ratio of the angular speed and linear speed of (1):

[kappa] = [[omega]/v]. (5)

The acceleration is obtained by differentiating (1) as

[mathematical expression not reproducible]. (6)

The unicycle models (1) and (6) capture the basic velocity and acceleration characteristics of a vehicle for the study of velocity planning.

2.2. Symmetric Planar Parametric Curve. A geometric reference path p (u) can be represented by a parametric smooth function of a scalar path parameter u [member of] [0, 1] as

[mathematical expression not reproducible], (7)

where [p.sub.x], [p.sub.y] are smooth functions defined on [0, 1] satisfying the boundary conditions

[mathematical expression not reproducible]. (8)

General formula for signed curvature [kappa](u) for a regular parametric curve p" (u) with p' (u) [not equal to] 0, [for all]u [member of] [0, 1], is

[mathematical expression not reproducible]. (9)

The curvature is related to second-order derivative of the curve and the acceleration.

Definition 1 (unit speed curve). Suppose a curve p: [0, 1] [right arrow] [R.sup.2] is parameterized by arc length s. Then,

[mathematical expression not reproducible]. (10)

That is, if p" (s) is parameterized by arc length, then p'(s) is a unit vector tangent to p(s) for all s. This implies that p'(s) and p" (s) are orthogonal.

The acceleration [mathematical expression not reproducible] of a vehicle along a parameterized curve p(u) has two orthogonal components, where [??] = p' (u)/[parallel]p' (u)[parallel] (the unit tangent vector) and [mathematical expression not reproducible] (the derivative of the unit tangent vector [??], orthogonal to [??] itself) depends on the curve. Thus, the normal acceleration is perpendicular to the velocity vector (rotated [pi]/2 counterclockwise). The tangential component of acceleration in the tangent direction [??] of the curve, or the longitudinal acceleration, is as follows:

[a.sub.tan] = v' = [p' x p"/[parallel]p'[parallel]], (11)

which is caused by variation in the longitudinal speed, and the normal component in the direction of principal normal [??] to the curve, or the lateral (/normal/centrifugal/radial) acceleration, is as follows:

[mathematical expression not reproducible], (12)

which is caused by changes in the vehicle's moving direction. The total linear acceleration is

a = [square root of [a.sup.2.sub.tan] + [a.sup.2.sub.nor]]. (13)

In [31, 32], it is supposed that

[mathematical expression not reproducible], (14)

where the superscripts min and max denote minimum and maximum, respectively.

To satisfy the boundary conditions on the position and its derivatives and smoothness requirement, a variety of interpolating curves are proposed as the path primitives of autonomous driving, as mentioned in Introduction. For the specific task of lane change studied in this paper, generally we require the lane-change path to be followed by the vehicle is symmetric with respect to the midpoint with the same tangent angle and curvature at the end points.

Definition 2. We call p(u) symmetric if

[mathematical expression not reproducible]. (15)

It is noted that the symmetry condition imposed on p (u) implies that [kappa](0) = -[kappa](1) and [kappa](1/2) = 0. The midpoint between the start point and final point

[mathematical expression not reproducible], (16)

is the path inflection point with zero curvature and nonzero curvature derivative. Furthermore, p"(1/2) = 0; that is, p(u) has a maximum tangent/slope at the midpoint.

Theorem ([37], p. 78). Two parametric curves [C.sub.1], [C.sub.2] represented as [p.sub.1] (u), [p.sub.2] (u), u [member of] [0, 1] are connected with [G.sup.2] continuity at the junction, and it is necessary and sufficient that [p.sub.1](1) = [p.sub.2] (0) for [G.sup.0] continuity, [P'.sub.1] (1) = a[P'.sub.1] (0) for [G.sup.1] continuity, and [p".sub.1] (1) = [a.sup.2][P".sub.2] (0) + b[p'.sub.2](0) for [G.sup.2] continuity, where a and b are arbitrary constants.

Thus, [G.sup.2] continuity allows an arbitrary setting of second-order derivatives at the end point, while [G.sup.1] continuity requires the connection point of two segments has a parallel/proportional tangent (i.e., in the same direction but the magnitude can be different).

2.3. Time-Optimal Velocity Planning with Velocity and Acceleration Constraints

2.3.1. Admissible Region (AR) for Velocity and Acceleration Constraints in Phase Plane. Velocity planning along a constrained path from a start configuration to a final configuration is necessary for safe, fuel-economic, comfortable, or time-efficient operation of autonomous ground vehicles [17, 22, 23, 29]. The optimal velocity solution complexity varied significantly with the vehicle model, the constraint set, the objective function to be minimized, and the solution algorithms to compute the velocity profile for a vehicle to follow a prespecified path. This paper employs unicycle as the vehicle model. Given a (lane-change) curve that satisfies the boundary conditions on the position and its higher order derivatives, the velocity and acceleration limits for steering the unicycle along the given path are given by

[mathematical expression not reproducible], (17)

where the subscripts max and min denote the maximum and minimum, respectively. Note that the linear acceleration constraints could be asymmetric; that is, [a.sub.max], [a.sub.min] could be unequal in magnitude. The acceleration is allowed to be faster or slower than the deceleration. For example, limits on turn acceleration and linear acceleration include the maximum normal acceleration on the curve for comfort and no side-slip [28] and limits on longitudinal acceleration are as given in ISO 2631-1 standard (Table 1).

From the relations [kappa] = [omega]/v and [a.sub.nor] = v = [kv.sup.2], the curvature of each point along a given path constrains both the angular velocity and lateral acceleration of the unicycle. Since the angular velocity is bounded, the linear velocity must be curvature dependent. Two sets of points related to curvature critical for unicycle velocity planning are zero-curvature points and maximum-curvature points on the path [31]. In our case, these two sets are discussed in Section 4.3. At a path point with known curvature, the achievable highest speed can be computed, given the angular velocity limit or normal acceleration limit or both in (17). To obtain the velocity profile that minimizes the travel time for completing the maneuver along a symmetric path, the conflict along a trajectory that may exist between the specified velocity and acceleration bounds in (17) should be resolved via the admissible region (AR). For this purpose, the admissible region (AR) is defined as the set where the trajectory lies entirely within, or the trajectory should belong to, so that the constraints are not violated as the unicycle follows the path. It is commonly formulated as a path-constrained trajectory generation problem dealing with producing a velocity profile and an acceleration profile by an optimal time-scaling function of arc length along the curve.

To specify a trajectory of a given path p (s) with length [s.sub.end], it is necessary to design its time-scaling function (or time parameterization) s(t) : [0, T] [right arrow] [0, [s.sub.end]], an increasing function defined in the (s, s') plane. s(t) should satisfy the interpolating boundary conditions: s(0) = 0, s' (0) = [s'.sub.A] and s(T) = [s.sub.end], s' (T) = [s'.sub.B], where [s.sub.end], [s'.sub.A], and [s'.sub.B] denote the path length, initial parametric velocity, and final parametric velocity, respectively, and T is the free travel time. The function s(t) is assumed to be twice-differentiable and monotonic (s' > 0), which ensures that the acceleration is well-defined and bounded. The scaling function assigns the velocity magnitude to each point on the geometric curve and thus does not change the shape and length of the given path.

The velocity v(t) and angular velocity [omega](t) for the unicycle can be expressed as

[mathematical expression not reproducible], (18)

where [mathematical expression not reproducible] and [kappa](s) is the signed path curvature. By further differentiating (18), we obtain accelerations

[mathematical expression not reproducible]. (19)

Rewriting (19) in the phase plane, we obtain the equation of motion along the path with input of linear acceleration a and angular acceleration [alpha]:

[mathematical expression not reproducible], (20)

as compared to the command inputs of linear and angular velocities to the unicycle (1).

Now we consider velocity constraints, the linear and angular acceleration limitations in (20). The constraints (17) could be rewritten compactly as the inequality constraints of s, s', and s" :

[mathematical expression not reproducible], (21)


[mathematical expression not reproducible]. (22)

The inertia-like vector A (s) is nonzero, but some of its components may be zero. We call the points of [A.sub.i] (s) = 0 for some i as the zero-inertia points since the vector A represents an inertia-like term in the parameterized constraint equation [15, 21, 24]. At zero-inertia points, s" is not defined and can take any value between [L2(s, s'), U2 (s, s')]. For the unicycle model we deal with in this paper, [A.sub.1] (s) = [A.sub.3] (s) = 0 corresponds to [kappa](s) = 0, or zero-inertia points of unicycle are the points with zero curvature, at which the angular velocity [omega] = [kappa]v = 0.

Note that the constraint (21) is linear in [s'.sup.2]. Explicitly, we have the following lower and upper limits caused by the path acceleration limits of (21):

[mathematical expression not reproducible], (23)

On the other hand, for points other than zero-inertia points, another acceleration constraint caused by the angular acceleration lower and upper limits of (21) are L2(s, s') [less than or equal to] s" [less than or equal to] U2(s, s'), where

[mathematical expression not reproducible]. (24)


[mathematical expression not reproducible]. (25)

Therefore, given s and s', s" should satisfy the following inequality defining the envelope of dynamic feasibility to avoid overly large acceleration:

L(s, s') [less than or equal to] s" [less than or equal to] U (s, s'), (26)

The maximum velocity curve (MVC) in the plane (s, s') with s [member of] [0, [s.sub.end]] is represented as

[mathematical expression not reproducible]. (27)

for all points with [kappa](s) [not equal to] 0. At the zero-inertia point, we set s" = 0, so that the velocity limit curve MVC(s) is continuous everywhere and has a horizontal tangent at this point. In this case, the velocity s' is also constrained by the curvature derivative [kappa]'(s), as seen from (21) as [kappa](s) = 0. Additionally, the velocity constraint in (21) induces another maximum velocity curve [] (s) [24, 38]. Therefore, the velocity limit curve [MVC.sup.*] (s) for velocity and acceleration bounds is the minimum of maximum velocity curve caused by acceleration bounds and maximum velocity curve caused by velocity constraint:

[mathematical expression not reproducible], (28)

where [MVC.sup.*a](s), [MVC.sup.*v](s) is part of [MVC.sup.*] (s) formed by the partial of MVC (s), [] (s), respectively.

It was proved that if the curvature p" (s) is continuous, then [MVC.sup.*] is continuous [15]. Therefore, the resolution of the conflict between the velocity and acceleration constraints requires that the velocity profile of the unicycle on a smooth path is within the admissible region (AR) [16, 24] defined as the compact, connected region enclosed by the continuous curve [MVC.sup.*] and the lines s' = 0, s = 0, and s = [s.sub.end] in the (s, s') plane:

[mathematical expression not reproducible]. (29)

In other words, AR is the closed and bounded constraint set for the velocity in the (s, s') plane. [MVC.sup.*] (s) is the upper bound (boundary) of the AR defining the admissible velocity satisfying all the input and state constraints (21) on velocity and acceleration of unicycle in the phase plane. Since [MVC.sup.*] (s) is in general different from MVC, it is seen that the AR with acceleration and velocity constraints is changed by the velocity constraint that is different from the AR with only acceleration constraint.

2.3.2. Time-Optimal Velocity Planning for Lane-Change Path in the Phase Plane. The aim of velocity planning is to find a continuous velocity trajectory s' = [gamma] (s) [member of] AR in (s, s') plane for a lane-change parametric path p(s) followed by a unicycle with the acceleration command (the tangent) s" at any point of p(s) not violating the constraint (26). Formally, time-optimal velocity planning for a unicycle is to find a velocity function [[gamma].sup.*] (s) [member of] AR in the in (s, s') plane as high as possible, and its slope nowhere exceeds the limits given by the constraint (26) by applying the maximum or minimum acceleration such that the travel time following the path p (s) is minimized over all continuous functions [gamma](s) [member of] AR. This optimization problem in the phase plane involves a lane-change path satisfying the interpolating boundary data and smoothness condition, optimizing an optimality criterion of overall travel time T along the path over a unicycle kinematics subject to its state and input constraints expressed in terms of the AR. In addition, with the availability of the AR, design of [gamma] (s) [member of] AR can be pursued to achieve continuous acceleration profile (not bang-bang) so that the resulting motion is smooth and faster. In waypoint navigation, it is desirable to have a trajectory composed by connecting a set of waypoints via path primitives, and each of the trajectory segments has a continuous acceleration not violating all the constraints. Here, the operation time T > [T.sup.*] is the design parameter for trajectory planning along a path segment connecting consecutive waypoints without violating the kinodynamic constraints, where [T.sup.*] gives a lower bound for operation time.

2.4. Numerical Integration (NI). Numerical integration (NI) is an improved, robust phase-plane method proposed in [21, 24] for time-optimal trajectory planning in the presence of numerical inaccuracies due to floating point operations. Based on phase plane analysis, NI provides a solution to minimize the travel time by determining TOPP (time-optimal path parameterization) for parameterizing the given geometric path with optimal velocity profile along with the computation of switch points, where the velocity function switches between the maximum and minimum accelerations and lie on the velocity limit curve [MVC.sup.*]a (s) [24]. Given the bounds on velocity and acceleration (21), the solution is to use the maximal possible acceleration to increase the velocity and maximal deceleration to decrease it, so that the path velocity should be as large as possible at every time instant, but without violating the constraint [16]. TOPP finds a continuous time-scaling function s' = [[gamma].sup.*] (s) [member of] AR, [for all]s [member of] [0, [s.sub.end]], in the (s, s') plane for the path velocity at each point along the given path by forward numerical integration of s" = U (s, s') with maximum path acceleration from the start (0, [s'.sub.A]) and switch points, and by backward numerical integration of s" = L(s, s') with minimum path acceleration from the final ([s.sub.end], sB) and switch points. The time-optimal velocity [[gamma].sup.*] (s) is composed by a sequence of concatenation of the maximum accelerating (forward) curves and minimum accelerating (backward) curves that lie in the AR and touch [MVC.sup.*] with tangent bounded by (26). The time-optimal velocity [[gamma].sup.*] (s) [member of] AR with its tangent satisfies (26) that may be entirely inside the AR or touch the boundary or part of [[gamma].sup.*] (s) is on the boundary of AR [16]. For detailed exposition of TOPP, we refer the readers to [21, 24] and the associated numerical integration implementation along with their properties. NI is directly employed as the simulation-based evaluation tool in this paper for the computation of velocity and acceleration profiles to achieve the least travel time for a unicycle moving along lane-change paths satisfying interpolating boundary conditions.

3. Symmetric Lane-Change Curve Design

A symmetric lane-change curve followed by a unicycle is shown in Figure 2 in which two parallel lanes, A denoting the current lane and B denoting the target lane, are shown. In the study of lane-change maneuver, we assume that the x-axis is parallel to the lane, that is, [[theta].sub.A] = [[theta].sub.B] = 0, so that x and y coordinates are the displacements in the longitudinal and lateral directions, respectively. In addition, the configurations [W.sub.A] = ([x.sub.A], [y.sub.A], [[theta].sub.A]), [W.sub.B] = ([x.sub.B], [y.sub.B], [[theta].sub.B]) and the velocities [v.sub.A], [v.sub.B] at the start and end points of lane-change maneuver are given. Alternatively, we are given the interpolating boundary data [p.sub.A], [p'.sub.A], [p".sub.A], [p.sub.B], [p'.sub.B], [p".sub.B] of position, velocity, and acceleration at the endpoints for lane-change curve denoted by p (u) conforming with the given boundary position data p(0) = [p.sub.A], p(1) = [p.sub.B].

Polynomials have been widely used as trajectory primitives for autonomous driving or autonomous robots, since polynomials are easy to manipulate and efficient to compute the derivatives for fast simulation. It is known that higher order polynomial is not desirable for real-time path generation due to its high number of mathematical manipulations; therefore, it is advisable to limit the order of the polynomial. Lane-change curves can be defined in a number of ways. We are particularly interested in using symmetric parametric polynomials to describe the longitudinal and lateral motions for lane-change maneuvering in constrained space. Among the various families of splines, Bezier curves are one of the most popular in robotic or autonomous driving applications. In the following, three types of symmetric parametric polynomials satisfying interpolating boundary data are presented as candidate lane-change curves with null curvature at both end points. More specifically, quintic Bezier, concatenated cubic Bezier, and simplified [[eta].sup.3]-spline, each with respective defining path parameters to alter the shape and curvature of the curve. Across all three families of polynomial lane-change curves, the effect of path characteristics (for example the length and the curvature) on time efficiency of lane-change maneuver along a prespecified curve is simulated.

Consider a lane-change path p(u), u [member of] [0, 1], represented by a parametric polynomial, and the order of the polynomial should be determined a priori to meet the kinematic path constraints imposed on the autonomous vehicles. Symmetry condition furthermore ensures p(u) to pass through the midpoint p(1/2) = [p.sub.mid] of the line segment [bar.[p.sub.A][p.sub.B]]. Notice that the midpoint is the inflection point of the lane-change curve that has null curvature. Firstly, we recall the definition of Bezier curve in Bernstein form. A Bezier curve of degree n linking two endpoints with velocities [v.sub.A], [v.sub.B] is specified by the arrangement of n + 1 control points [p.sub.0], [p.sub.1], ..., [p.sub.n], which forms a control polygon (Bezier polygon):

[mathematical expression not reproducible], (30)

where [B.sub.i,n] (u) = [C.sup.n.sub.i] [u.sup.i] [(1 - u).sup.n-i], i = 0, ..., n, and [C.sup.n.sub.i] is the combination of choosing i elements from n elements without repeat, which can be written using factorials as (n!/(i!(n-i)!)). The kth derivative is given as follows [39]:

[mathematical expression not reproducible]. (31)

If p (u) is a Bezier curve of degree n, then it is tangent to the first and last control points. As in (27), the derivative at the endpoints is completely determined by the few neighboring control points. The curvatures at the endpoints are as follows:

[mathematical expression not reproducible], (32)

where [gamma](u) = p' (u) x p" (u), u [member of] [0, 1]. Combining (31) and (32), we can obtain

[mathematical expression not reproducible], (33)

and from (15), for symmetric Bezier curves, we have p' (0) = p' (1), p" (0) = - p" (1). Therefore,

[mathematical expression not reproducible], (34)

and for [[kappa].sub.A] = [[kappa].sub.B] = 0, from (33), it is required that [p.sub.1] - [p.sub.A] and [p.sub.2] - 2[p.sub.1] + [p.sub.A] are collinear, so are [p.sub.B] - [p.sub.n-1] and [p.sub.B] - [] + [p.sub.n-2]. Thus, symmetric Bezier curves at least of 5th order meet the requirements position, tangent, and curvature continuity.

3.1. Symmetric Quintic Bezier Curve. Quintic Bezier curves have been used in continuous-curvature path planning or velocity planning recently [13, 23, 25, 29, 40] due to the appealing feature of manipulation of curve shape by placing the control points. A quintic Bezier curve, as shown in Figure 3, is defined as

[mathematical expression not reproducible]. (35)

By employing the interpolating boundary data [p.sub.A], [p'.sub.A], [p".sub.A], [p.sub.B], [p'.sub.B], [p.sub.B], we obtain from (31) the expression of six control points as

[mathematical expression not reproducible], (36)

and the result in (36) corresponds to the setting [[beta].sub.1] = [[gamma].sub.1] = 1 and [[beta].sub.2] = [[gamma].sub.2] = 0 in [40]. Symmetry condition for n = 5 and the requirement [[kappa].sub.A] = [[kappa].sub.B] = 0 could be achieved by the conditions that {[p.sub.A], [p.sub.1], [p.sub.2]}, {[p.sub.3], [p.sub.4], [p.sub.B]} can be set symmetrically to be collinear separately along the same horizontal vector since [[theta].sub.A] = [[theta].sub.B] = 0. In general, [p.sub.1], [p.sub.4] can be placed at an equal distance [d.sub.q] = [r.sub.q] ([x.sub.B] - [x.sub.A]) from the endpoints in the direction of parallel lanes to ensure the tangent direction:

[mathematical expression not reproducible], (37)

where [r.sub.q] [member of] (0, 1) adjusts the magnitude of the tangent while maintaining [kappa] = 0 at both endpoints. From (36), we can obtain that [p.sub.2], [p.sub.3] is

[mathematical expression not reproducible], (38)

and note that (37) and (38) are general forms of symmetric quintic Bezier curve. Note that the symmetric quintic Bezier with [r.sub.q] = 1/5 and symmetric quintic polynomial in [5] are equivalent. Several alternative designs of [p.sub.1], [p.sub.4] proposed in the literature are [r.sub.q] = 3/10 [41] and 1/5 [5,42]. Furthermore, same as symmetric cubic Bezier curve, [r.sub.q] = 1/2 so that

[mathematical expression not reproducible]. (39)

3.2. Concatenated Cubic Bezier Curve. As mentioned, the minimum degree n of a single Bezier curve is at least 5 to satisfy [G.sup.2] continuity. Motivated by Yang and Sukkarieh [43], here we propose a way to connect two cubic Bezier curves to meet the [G.sup.2] continuity based on theorem in Section 2. The main advantage of the concatenated curve is that it reduces the degree and computational complexity of the curve compared to the quintic Bezier curve. A concatenation of two cubic Bezier curves [C.sub.1], [C.sub.2], as shown in Figure 4, that shares midpoint is proposed to be an alternative lane-change curve. Let [C.sub.1] be p(u), then [C.sub.2] is [p.sub.mid] + p(1-u) by symmetry. This concatenation of two cubic Bezier curves [C.sub.1], [C.sub.2] is at least [G.sup.1]. Consider the cubic Bezier curve [C.sub.1], it has to satisfy the following boundary conditions:

[mathematical expression not reproducible]. (40)

In Figure 4, we need to choose [p.sub.3] = [p.sub.4] = [p.sub.mid] as the point to concatenate two cubic Bezier curves. Same as the quintic Bezier curve, [p.sub.1] and [p.sub.6] should be set on lanes A and B, respectively, to meet the condition. From (27), the first and second derivatives of cubic Bezier curve at endpoints are

[mathematical expression not reproducible]. (41)

Substituting (41) into (36) yields

[mathematical expression not reproducible], (42)

where g, h, k, [theta], and [phi] denote [bar.[p.sub.0][p.sub.1]], [bar.[p.sub.1][p.sub.2]] and [bar.[p.sub.2][p.sub.3]] and angle between [mathematical expression not reproducible] and [mathematical expression not reproducible] and [mathematical expression not reproducible] and [mathematical expression not reproducible], respectively, as depicted in Figure 4. Notice that if [kappa](0) = [kappa] (1) = 0, then only h = 0 will satisfy the result ([theta] = [phi] = 2n[pi], n [member of] Z, is excluded due to the co-linear of four control points), which implies that [p.sub.1], [p.sub.2] coincide. As a result, the only remaining design freedom for the concatenated cubic Bezier curve is to design [p.sub.1], [p.sub.6] in the horizontal x-direction. Thus, we have [bar.[p.sub.A][p.sub.1]] = [bar.[p.sub.6][p.sub.B]] by setting [d.sub.c] = [r.sub.c]([x.sub.B] - [x.sub.A]), [r.sub.c] [member of] (0, 1) due to symmetry to simulate different concatenation of two cubic Bezier curves.

3.3. Simplified [[eta].sup.3]-Splines. A path primitive called [[eta].sup.3]-spline [33] can be the solution to the polynomial [G.sup.3]-interpolating problem. The solution is given by a seventh-order parametric polynomial curve p(u) = [[[alpha](u)[beta](u)].sup.T], u [member of] [0, 1], which is defined as follows:

[mathematical expression not reproducible], (43)

[mathematical expression not reproducible], (44)

where the polynomial coefficients are relative to end configuration and real path shaping parameter vector [eta] = [[[eta].sub.1] [[eta].sub.3] [[eta].sub.4] [[eta].sub.5] [[eta].sub.6]] that can be flexibly selected to modify the path shape without violating the endpoint interpolating conditions. The symmetric property of [[eta].sub.3]-spline is [[eta].sub.1] = [[eta].sub.2] = [[rho].sub.1] [member of] [R.sub.+], [[eta].sub.3] = -[[eta].sub.4] = [[rho].sub.2] [member of] R, [[eta].sub.5] = [[eta].sub.6] = [[rho].sub.3] [member of] [R.sub.+]. That is, [eta] = [[[rho].sub.1] [[rho].sub.1] [[rho].sub.2] [[rho].sub.2] [[rho].sub.3] [[rho].sub.3]] for symmetric [[eta].sub.3] - spline, where R, [R.sub.+] denotes the set of real and nonnegative real numbers, respectively. [[eta].sub.1], [[eta].sub.2] are velocity parameters, the other four parameters [[eta].sub.3], [[eta].sub.4] and [[eta].sub.5], [[eta].sub.6] depend on the acceleration and jerk at the path endpoints, respectively. In this paper, the proposed simplified version of [[eta].sup.3]-spline is concerned for the computational efficiency. The simplified [[eta].sup.3]-spline is a version of [[eta].sup.3]-spline which reduces degrees of freedom with the following settings:

[mathematical expression not reproducible]. (45)

Then the coefficients in (43) and (44) can be written as

[mathematical expression not reproducible], (46)

where [gamma] = [[eta].sub.2] cos [[theta].sub.B] and [delta] = [[eta].sub.2] sin [[theta].sub.B].

4. Simulation Results

In this section, we perform a comparative feasibility study of three types of candidate symmetric lane-change curves guaranteed followed by a unicycle. Available maneuver space constrains the maximum deflection allowed, thus introducing curvature constraint of the lane-change trajectory. This path constraint in turns affects significantly the achievable highest speed along the curve. We set up two scenarios of loose and hard curvature constraints for emulating the constrained maneuver space under given velocity and acceleration bounds. The unicycle is required to reach the final configuration with zero velocity in minimum travel time, by following the lane-change curve starting at rest from the same initial configuration in two scenarios. The two scenarios are sufficient to reveal the curve geometric properties most related to travel time along a given path, as was observed in [25] for ground mobile robot navigation experiments. The simulation is performed on Intel Core i5 2.7 GHz, 8G LPDDR3 Linux on a standard laptop computer with codes written based on the open-source code of TOPP library in C+ +/Python ( [26] to account for [](s) caused by the velocity constraint in (21). We assume the lane-change ratio is unity and NI is run with a time step size of 0.002. In addition, any of the lane-change curves studied here has the midpoint as zero-inertia point, also the inflection point of path. At this maximal slope portion of path near midpoint, we set the acceleration at midpoint as zero so that the unicycle drives with constant speed. It is feasible since the time-optimal velocity profile is accelerating before that point and is decelerating after that point or vice versa.

The rest-to-rest lane-change maneuvering simulation scenarios for steering a unicycle while minimizing the time for completing the lane-change maneuver are set as follows:

(i) Start configuration is fixed at (0, 0, 0) and final configuration is set as ([x.sub.B], [y.sub.B], 0). There are an infinite number of final configurations to consider. Here we examine two final configurations with unity lane-change ratio [x.sub.B] = [y.sub.B] = 10, 1 to mimic the sufficient or constrained (longer or shorter) intervehicle spacing to see the effect of geometric properties of curves on time efficiency of lane change.

(ii) Start and end speeds at lane-change maneuver endpoints are given and fixed at zero, [v.sub.A] = [v.sub.B] = 0.

(iii) Symmetric constant velocity and acceleration limits:

[v.sub.max] = -[v.sub.min] = [degrees].75 (m/s) and [a.sub.max] = -[a.sub.min] = 0.3 (m/[s.sup.2]), [[omega].sub.max] = -[[omega].sub.min] = 1.745 (rad/s) and [[alpha].sub.max] = -[[alpha].sub.min] = 1.745 (rad/s.sup.2]).

Since the initial and final curvatures are null, both the initial and final angular velocity and angular acceleration are zero, in addition to the setting of null linear velocity at the end points.

In each scenario, a simulation is run for each candidate lane-change curve for checking time optimality and that all the constraints are met and to determine which curve has the fastest execution to reach the goal position and orientation from the same start configuration. The trajectory comparison is in terms of path geometric properties of path length and curvature, minimal travel time obtained from NI, and trajectory computation time. In each scenario, AR for each candidate path is shown explicitly to verify that the velocity profile lies entirely within the AR, or the velocity and acceleration constraints are not violated. Since the curves we study are continuous, the boundary of AR is continuous.

4.1. Simulation Scenario 1: Loose Curvature Constraint. First, the end configuration is set to be (10, 10, 0). In Figures 5 and 6, three families of lane-change curves and their curvatures are evaluated. For Bezier curves, it can be observed from Figure 5 that concatenated cubic Bezier has the largest maximum curvature and symmetric quintic Bezier curve with [r.sub.q] = 3/10 has the smallest maximum curvature. In Figure 6, lane-change paths of [G.SUP.3] continuity and their curvatures are evaluated for the simplified [[eta].sup.3]-spline with varied values of [[eta].sub.1] = [[eta].sub.2]. Some values of [[eta].sub.1], [[eta].sub.2] generate higher maximum curvature due to the nature of the seventh-order polynomial of [[eta].sup.3]-spline. To reduce the maximum curvature, an acceptable suboptimal solution [33] can be obtained by a rough heuristic rule: [[eta].sub.1] = [[eta].sub.2] = [parallel][p.sub.A] - [p.sub.B][parallel]. Therefore, it can be observed in Figure 6(b) that, for the simplified [[eta].sup.3]-spline, the smallest maximum curvature occurs with [[eta].sub.1] = [[eta].sub.2] = 15 ([approximately equal to] [parallel][p.sub.A] - [p.sub.B][parallel] = 10 [square root of 2]).

In Figure 7, different lane-change paths are compared in terms of path length, maximum curvature [[kappa].sub.max], minimum travel time [T.sup.*] that a unicycle takes to perform the lane-change maneuver, and trajectory computation time [T.sub.c]. Trajectory computation time is the computational expense of the path computation. Since in real-time applications such as moving obstacle avoidance or car following for on-road autonomous driving, the velocity profile needs to be computed fast or recomputed very often and trajectory computational time is important. In our simulation, [T.sub.c] depends on the number of mathematical operations of the path representation and time-optimal parameterization along the path. The time parameterization part is run 100 times for average.

Concatenated cubic Bezier has longer travel time compared to symmetric quintic Bezier and the simplified [[eta].sup.3]-spline with similar path length, while the concatenated cubic Bezier curve has on average larger maximum curvature, and symmetric quintic Bezier curve seems to have the smallest curvature, as compared to all other possible curves with similar path length. In the case of loose curvature constraint, the distance-optimal path is also time optimal, as is for constant speed unicycle. The situation-specific criteria for selecting lane-change curves are identified and summarized as follows:

(1) Concatenated cubic Bezier is suitable and flexible for planning a longer [G.sup.2] path in real time such as emergency situations. Concatenated cubic Bezier curve has better numerical stability and computational efficiency due to the low degree and the use of Bernstein basis for path representation.

(2) Symmetric quintic Bezier is appropriate for planning paths with smaller maximum curvature such as the situation of only shorter lane-change distance available (Scenario 2 in the following).

(3) The most computationally demanding simplified [[eta].sup.3]-spline might be available for planning shorter paths which require less travel time.

In Figure 8, [MVC.sup.*] and AR, U-shape velocity profile, and smoothed bang-bang acceleration profile (i.e., acceleration input to unicycle is saturated at the extreme to move as fast as possible) are shown, respectively, for three types of paths with [r.sub.q] = 0.2, [r.sub.c] = 0.1, and [eta] = [[5 5 0 0 0 0].sup.T] due to their faster minimal travel time in Scenario 1. The gray region upper bounded by [MVC.sup.*] shows the AR for each candidate path. It can be seen that both velocities and accelerations lie within the allowed limits. Note that the time-optimal velocity profile exhibits the switching symmetrically and has the U-shape [32], at which [MVC.sup.*] is touched with the switch point ([s.sup.*], [MVC.sup.*] ([s.sup.*])). To minimize the overall travel time along symmetric parametric polynomials under loose curvature condition, it is necessary that the unicycle obeys the trapezoidal rule, i.e., driving from zero velocity and then accelerating with maximal path acceleration until the allowable maximum velocity within the AR is achieved. Then it maintains this maximum velocity (i.e., zero acceleration) as long as possible until it reaches another switch point, after which it reverses the acceleration symmetrically to decelerate with minimal path acceleration from the achievable highest velocity to satisfy the zero velocity condition at the end point, i.e., the phase of braking to stop.

4.2. Simulation Scenario 2: Hard Curvature Constraint. In simulation scenario 2, the end point is (1, 1) to simulate a crowded traffic to perform an aggressive lane-change maneuver within a very restricted steering space. The x-and y-distances required for lane-change maneuver with the same unity lane-change ratio are only one-tenth of Simulation Scenario 1, so that remarkable change (ten times) curvature variation compared to Simulation Scenario 1 can occur. The curvature and derivative of the curves linking the same initial but different final configurations from Scenario 1 followed by a unicycle should be examined for constraint consistency, i.e., to determine [MVC.sup.*] and AR of unicycle in new scenario.

We choose three curves [r.sub.q] = 1/5, [r.sub.c] = 0.1, and [eta] = [[0.50.50000].sup.T] ([eta] is scaled accordingly), as shown in Figure 9, among the set of paths simulated in Scenario 1, one for each family, for further comparison. Additional NI simulation is run for these three curves under hard curvature condition, which represents a severe constraint on the curvature. It can be seen from Figure 10 that the optimal velocity profile is very similar, symmetric but not U-shape, and the bang-bang switching of acceleration and deceleration is observed for the considered three parametric polynomials in constrained steering space. At the two points of maximum curvature along the parametric curves (Figure 9), the unicycle moves at the highest speed but does not reach [v.sub.max] = 0.75 m/s. Due to maximum radial/normal acceleration limit at high curvature, the actual maximum longitudinal velocity is moderate. In Figures 10(b) and 10(c), the longitudinal acceleration was scaled down by the angular velocity to make a sudden turn complying with the curvature condition to avoid excessive lateral acceleration while maintaining the original heading.

In addition to the exposition of the difference of minimum-time velocity and acceleration profiles in two simulation scenarios in Figures 8 and 10, Table 2 shows detailed comparisons in two conditions. The time for longer lane-change distance in Scenario 1 is between 4 and 5 times that of Scenario 2 with only 1/10 distance of Scenario 1. Besides, the ranking of paths in terms of travel time and trajectory computation time is changed under hard curvature condition. The concatenated cubic Bezier with [r.sub.c] = 0.1 is the fastest under loose curvature condition; however, it is shown to be the slowest under hard curvature condition. The effect of curvature on achievable highest speed or minimum travel time following a lane-change curve is more obvious in constrained steering environment such as the hard curvature constraint scenario. The time-optimal trajectory of the quintic Bezier curve allows larger highest speed than the other two curves, thus reducing the overall travel time. Therefore, it can be concluded that under hard curvature condition, the quintic Bezier curve will be a better option for planning faster lane-change trajectory in that it has smaller maximum curvature and less trajectory computational time.

4.3. Summary and Discussions. For maneuvering along a lane-change path, the velocity at each point of the path is constrained not only within AR defined by the acceleration constraint and the velocity constraint of the vehicle but also by the path defining the path following task of lane-change. It is noticed that the trajectory computation time of the symmetric parametric polynomials is very small; thus, these families of paths can be computed fast for real-time aggressive lane-change maneuver, offering both time optimality and real-time capability. Simulations confirm that the quintic Bezier curve [5] is a very good option for lane-change curves, since it has low computing time and fast lane changing time. It is clear that variations in interpolating boundary conditions with different target position ([x.sub.B],[y.sub.B]) due to different lane-change ratios and different [v.sub.A], [v.sub.B] of vehicle speed at endpoints alter the shape for lane-change trajectory, and different velocities and acceleration profiles emerge. It is interesting to consider nonzero initial or final velocity of the boundary conditions. However, some properties of time-optimal trajectory along lane-change curves, as a consequence of the very simple simulations for the particular rest-to-rest lane-change maneuver in this section or supported by the theoretical derivation in Section 2, can be described as follows:

(i) Due to the symmetry of lane-change curves, higher-order (velocity, acceleration, and curvature) profiles are also symmetric with respect to the midpoint between initial and final points. In particular, depending on the path constraint and which constraint in (21) is active, two patterns are observed from the simulated time-optimal trajectories in two conditions. One is the time-optimal velocity profile along symmetric parametric polynomial lane-change curves is symmetric and can exhibit U-shape (as Figure 8 depicts) that lies on part of [MVC.sup.*] of velocity and acceleration constraints in loose curvature condition, which was observed in [32] for time-optimal velocity planning of unicycle with only acceleration constraint. The other is the smoothed bang-bang acceleration profile typical for time-optimal control (i.e., at least one input is saturated at every time instant) that verifies the necessary condition of time optimality in hard curvature condition so that the optimal velocity profile may be inside the AR. Since the minimum-time velocity profile (for example, U-shape) is symmetric, the switch points are symmetric; that is, if [s.sup.*] is a switch point, 1 - [s.sup.*] is also a switch point (e.g., two switch points at s = 0.2, 0.8 shown in Figure 8 located at the accelerating or decelerating curves are symmetric). In case of the symmetric U-shape time-optimal velocity profile, there exists a portion of p(s), s [member of] [[s.sup.*], 1 - [s.sup.*]] that has constant highest speed v(1/2) = p' (1/2) = [v.sup.AR.sub.max], where [v.sup.AR.sub.max] [member of] AR is the maximum allowed feasible velocity at s = 1/2. The angular velocity is continuously differentiable at the constant-speed portion of the path. The path length of this constant speed portion of U-shape velocity profile is 1 - 2[s.sup.*].

(ii) Zero-inertia point is the zero-curvature point, also the midpoint or inflection point of the lane-change path. The midpoint is the inflection point, with maximum slope and null curvature, so that the unicycle moves with zero angular velocity and zero lateral acceleration as it traverse the midpoint. The velocity [v.sup.AR.sub.max] [member of] AR near the zero-curvature point as (i) mentioned above is nearly local maximum because of small normal acceleration. By our construction, the midpoint has zero linear acceleration since it is also a zero-inertia point of the time-optimal velocity. Thus, the time-optimal velocity profile has a continuous horizontal tangent at the midpoint to the everywhere continuous [MVC.sup.*] [15] in the case of the continuous-curvature lane-change path. This essentially tells us that when driving with time-optimal velocity along the path, the unicycle needs to change the direction of motion suddenly at the midpoint with maximum slope and zero angular velocity, zero acceleration, and constant speed. This conforms to the practice of safe driving at high curvature portion of path.

(iii) Maximum-curvature points: symmetric parametric polynomial has two points of maximum curvature (or minimum radius of turning), which are symmetric. The magnitude and location of maximum curvature of each path is determined by the respective curve-defining parameters [r.sub.c], [r.sub.q], n. This is visually obvious from the plots depicted in Figures 5, 6, and 9. The allowed highest speed along the path is constrained most severely at these two points and is maximized if the tangential component of acceleration is zero, while the normal component of acceleration is maximum. Thus, a higher normal acceleration at the high curvature portion of the path can lower the overall travel time of the path. Time-optimal trajectory tends to increase the speed at the low or gentle curvature portion (such as before or after the point of maximum curvature) of the path, though the highest speed allowed at high curvature portion is constrained most severely by the imposed vehicle acceleration bounds.

After comparing the travel times of the candidate lane-change curves with each other, a viable approach to select a fast trajectory is proposed based on the indicative geometric path characteristics that interplay with the imposed velocity and acceleration bounds. In order to perform lane-change maneuver faster, we choose the single curve which tends to have the following:

(i) Length as short as possible. This is obvious since the travel time

[mathematical expression not reproducible], (47)

is monotonically increasing with [s.sub.end].

(ii) Maximum curvature minimized (reducing the curvature at high-curvature part of the path) for trajectory optimization. We can infer from (47) the following result:

[mathematical expression not reproducible], (48)

where [v.sup.AR.sub.max] (s)[member of] AR is the maximum allowed feasible velocity at s along the path. [v.sup.AR.sub.max] depends on the path (curvature) via

[mathematical expression not reproducible], (49)

where the subscript and superscript max denote the maximum. Therefore,

[mathematical expression not reproducible]. (50)

It shows that via minimization of the maximum curvature, via increase of the allowable maximum acceleration, or achievable lowest velocity along the path can possibly reduce the travel time.

In the results of time-optimal velocity and acceleration profiles along polynomial lane-change curves we present here, we employ a simple kinematic unicycle model and NI to generate the sequence of acceleration curves, constant speed curves, deceleration curves, the times switch takes place, and their durations. The fast simulation results of unicycle provide very useful information for time-optimal lane-change trajectory planning along parametric polynomials under the steering space and kinodynamic constraints represented by the velocity and acceleration bounds: the decrease of path length and the maximum curvature along the path is most relevant to decrease the travel time cost. In addition to providing a good understanding of the properties of time-optimal velocity for lane-change maneuver in different scenarios, the results have implications for parallel parking. Parallel parking bears similarity to lane change in terms of interpolating boundary conditions [35], and the above reasoning about geometric properties related to minimize the execution time of lane-change maneuver also holds for parallel parking maneuver. This also has implications allowing the data-driven design of continuous acceleration profile (not bang-bang) for faster motion along a given curve via time-scaling functions = f([??]), [??] = t/T, where T > [T.sup.*] is the execution time, or via speed change function s" = F(s, s) with the constraint (s, s') [member of] AR, where the data-driven functions f, F are smooth.

From the model validation aspect, NI simulation in principle could be applied to more realistic, refined, higher dimensional dynamic vehicle model incorporating more degrees of freedom considering significant nonlinearity of vehicle dynamics at high-speed driving, aerodynamic forces, tire slip effect, and friction forces of tire-ground interaction and terrain topology for precise autonomous vehicle response [10, 11], such as race cars or design of 3D time-optimal driving on a spatial curve considering torsion effect due to road topology [6].

5. Conclusion

Minimum-time lane-change maneuvering along symmetric parametric polynomial paths under the constraints given by bounds on velocity and acceleration of a vehicle is studied by simulation-based evaluation on a path basis. The vehicle model we simulate is a unicycle, which is effective and particularly simple for generating the time-optimal velocity profile. NI-based simulations of the rest-to-rest time-optimal velocity planning for unicycle are carried out with three types of symmetric parametric polynomial continuous-curvature lane-change curves, including symmetric quintic Bezier, concatenated cubic Bezier, and simplified [[eta].sup.3]-spline with the same start configuration in two different end positions and unity lane-change ratio. The use of NI allows us to compute AR and the minimum-time-velocity profile of a lane-change curve for unicycle, so that a good understanding of velocity, acceleration characteristics, and the switching structure defined by the velocity limit curve caused by all the constraints of time-optimal solution along parametric polynomial lane-change curves are drawn. Given the limits on acceleration and velocity, it takes longer for a unicycle to follow a parametric polynomial lane-change path with longer length, higher curvature, or lower highest velocity. In particular, the quintic Bezier curve is verified in our simulations as a very good option of the lane-change curve. We observe from the numerical results that feasibility of different types of symmetric parametric polynomials for real-time lane-change maneuvering is identified in a wider range of situations. Our results on faster lane-change trajectory selection could be extended to parallel parking [35] in a straightforward way or for curves other than the families of parametric polynomials such as higher-order interpolating polynomials that meet curvature derivative constraint at end points or soft motion trajectory. These simulation-based evaluation results for fastest lane-change maneuvering along symmetric parametric polynomials performed by the unicycle model of autonomous ground vehicle could provide a reference, e.g., a lower bound estimate of the travel time, for trajectory optimization based on more refined and complete high-dimensional dynamic vehicle model in more complex scenarios such as moving vehicle avoidance. For a feasible high-speed lane-change maneuver, the longitudinal displacement (or lane-change ratio) and lane-changing time are the most difficult to determine [2]. It is very likely in real situation that the lane-change ratio of a feasible high-speed lane change, or the boundary conditions (the start and end points and their speeds), is determined mainly by vehicle dimensions, the surrounding vehicles (traffic flow), driving patterns, and other factors such as social or cultural norms [7]. Future work could simulate the effect of different lane-change ratio on the travel times.

s:  Path coordinate (arc length or distance) along a specified path
s': Path velocity along a specified path
s": Path acceleration/tangent at each point (s, s') along a
    specified path
t:  Time
T:  (Free)terminal time of maneuver.

Data Availability

The data used to support the findings of this study are included within the article.

Conflicts of Interest

The authors declare that they have no conflicts of interest.


[1] J. Nilsson, M. Brannstrom, E. Coelingh, and J. Fredriksson, "Lane change maneuvers for automated vehicles," IEEE Transactions on Intelligent Transportation Systems, vol. 18, no. 5, pp. 1087-1096, 2017.

[2] H. Bai, J. Shen, L. Wei, and Z. Feng, "Accelerated lane-changing trajectory planning of automated vehicles with vehicle-to-vehicle collaboration," Journal of Advanced Transportation, vol. 2017, Article ID 8132769, 11 pages, 2017.

[3] G. Geng, Z. Wu, H. Jiang, L. Sun, and C. Duan, "Study on path planning method for imitating the lane-changing operation of excellent drivers," Applied Sciences, vol. 8, no. 5, p. 814, 2018.

[4] F. You, R. Zhang, G. Lie, H. Wang, H. Wen, and J. Xu, "Trajectory planning and tracking control for autonomous lane change maneuver based on the cooperative vehicle infrastructure system," Expert Systems with Applications, vol. 42, no. 14, pp. 5932-5946, 2015.

[5] W. Nelson, "Continuous-curvature paths for autonomous vehicles," in Proceedings of 1989 IEEE International Conference on Robotics and Automation, Raleigh, NC, USA, May 1989.

[6] F. Lan and K. T. Miura, "Time optimal driving on curvilinear path with kinematic constraints," Computer-Aided Design and Applications, vol. 15, no. 1, pp. 122-128, 2018.

[7] C. Dong, Y. Zhang, and J. M. Dolan, "Lane-change social behavior generator for autonomous driving car by nonparametric regression in Reproducing Kernel Hilbert Space," in Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, Vancouver, BC, Canada, September 2017.

[8] N. H. Sledge and K. M. Marshek, "Comparison of ideal vehicle lane-change trajectories," SAE Technical Paper No. 971062, 1997.

[9] J. Wang, Q. Zhang, Z. Zhang, and X. Yan, "Structured trajectory planning of collision-free lane change using the vehicle-driver integration data," Science China Technological Sciences, vol. 59, no. 5, pp. 825-831, 2016.

[10] P. J. McNally, "Driver Control and trajectory optimization applied to lane change maneuver," in Optimization and Optimal Control in Automotive Systems, vol. 6, pp. 93-107, Springer, Berlin, Germany, 2014.

[11] F. Altch'e, P. Polack, and A. de La Fortelle, "High-speed trajectory planning for autonomous vehicles using a simple dynamic model," in Proceedings of 2017 IEEE 20th International Conference on Intelligent Transportation Systems, Yokohama, Japan, October 2017.

[12] G. L. Bianco, A. Piazzi, and M. Romano, "Smooth motion generation for unicycle mobile robots via dynamic path inversion," IEEE Transactions on Robotics, vol. 20, no. 5, pp. 884-891, 2004.

[13] J. W. Choi and K. Huhtala, "Constrained global path optimization for articulated steering vehicles," IEEE Transactions on Vehicular Technology, vol. 65, no. 4, pp. 1868-1879, 2016.

[14] K. R. Simba, N. Uchiyama, and S. Sano, "Real-time smooth trajectory generation for nonholonomic mobile robots using Bezier curves," Robotics and Computer-Integrated Manufacturing, vol. 41, pp. 31-42, 2016.

[15] B. Sencer, K. Ishizaki, and E. Shamoto, "A curvature optimal sharp corner smoothing algorithm for high-speed feed motion generation of NC systems along linear tool paths," International Journal of Advanced Manufacturing Technology, vol. 76, no. 9-12, pp. 1977-1992, 2015.

[16] M. Mahdi Ghazaei Ardakani, On trajectory generation of robots, Ph.D. thesis, Department of Automatic Control, Lund University, Lund, Sweden, 2016.

[17] T. Kunz and M. Stilman, "Time-optimal trajectory generation for path following with bounded acceleration and velocity," in Proceedings of Robotics: Science and Systems VIII, Sydney, NSW, Australia, July 2012.

[18] L. Zlajpah, "On time optimal path control of manipulators with bounded joint velocities and torques," in Proceedings of IEEE International Conference on Robotics and Automation, Minneapolis, MN, USA, April 1996.

[19] E. Ozatay, U. Ozguner, and D. Filev, "Velocity profile optimization of on road vehicles: Pontryagin's maximum principle based approach," Control Engineering Practice, vol. 61, pp. 244-254, 2017.

[20] P. Shen, X. Zhang, and Y. Fang, "Essential properties of numerical integration for time-optimal path-constrained trajectory planning," IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 888-895, 2017.

[21] L. Consolini, M. Locatelli, A. Minari, and A. Piazzi, "An optimal complexity algorithm for minimum-time velocity planning," Systems and Control Letters, vol. 103, pp. 50-57, 2017.

[22] L. Van den Broeck, M. Diehl, and J. Swevers, "A model predictive control approach for time optimal point-to-point motion control," Mechatronics, vol. 21, no. 7, pp. 1203-1212, 2011.

[23] J. Haugen and M. Breivik, "A speed control algorithm for planar path maneuvering," IFAC Proceedings Volumes, vol. 43, no. 20, pp. 219-224, 2010.

[24] A. Zdesar and I. Skrjanc, "Optimum velocity profile of multiple Bernstein-Bazier curves subject to constraints for mobile robots," ACM Transactions on Intelligent Systems and Technology, vol. 9, no. 5, p. 56, 2018.

[25] M. Lepetic, G. Klancar, I. Skrjanc, D. Matko, and B. Potocnik, "Time optimal path planning considering acceleration limits," Robotics and Autonomous Systems, vol. 45, no. 3, pp. 199-210, 2003.

[26] Q. C. Pham, "A general, fast, and robust implementation of the time-optimal path parameterization algorithm," IEEE Transactions on Robotics, vol. 30, no. 6, pp. 1533-1540, 2014.

[27] B. Lau, C. Sprunk, and W. Burgard, "Kinodynamic motion planning for mobile robots using splines," in Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, St. Louis, MO, USA, October 2009.

[28] A. Piazzi, C. G. Lo Bianco, and M. Romano, "[[eta].sup.3]-Splines for the smooth path generation of wheeled mobile robots," IEEE Transactions on Robotics, vol. 23, no. 5, pp. 1089-1095, 2007.

[29] D. Gonzalez, J. Perez, and V. Milanes, "Parametric-based path generation for automated vehicles at roundabouts," Expert Systems with Applications, vol. 71, pp. 332-341, 2017.

[30] D. Lyon, "A min-time analysis of three trajectories with curvature and nonholonomic constraints using a parallel parking criterion," JSME International Journal Series C Mechanical Systems, Machine Elements and Manufacturing, vol. 46, no. 4, pp. 1523-1530, 2003.

[31] J. P. Laumond, S. Sekhavat, and F. Lamiraux, "Guidelines in nonholonomic motion planning for mobile robots," in Robot Motion Planning and Control, pp. 1-53, Springer, Berlin, Heidelberg, Germany, 1998.

[32] Bu-Q. Su and D.-Y. Liu, Computational Geometry: Curve and Surface Modeling, Academic Press, San Diego, CA, USA, 1989.

[33] Y. Zhang, H. Chen, S. L Waslander et al., "Toward a more complete, flexible, and safer speed planning for autonomous driving via convex optimization," Sensors, vol. 18, no. 7, article E2185, 2018.

[34] L. Zhang, L. Sun, S. Zhang, and J. Liu, "Trajectory planning for an indoor mobile robot using quintic Bezier curves," in Proceedings of IEEE International Conference on Robotics and Biomimetics, Zhuhai, China, December 2015.

[35] D. Gonzalez, V. Milanes, J. Perez, and F. Nashashibi, "Speed profile generation based on quintic Bezier curves for enhanced passenger comfort," in Proceedings of IEEE 19th International Conference on Intelligent Transportation Systems, Rio de Janeiro, Brazil, November 2016.

[36] C. S. Wu, Z. Y. Chiu, and J. S. Liu, "Simulations for time-optimal trajectory planning along parametric polynomial lane-change curves for a unicycle," in Proceedings of IEEE International Conference on Robotics and Biomimetics, Macau, Macao, December 2017.

[37] F. Lamiraux and J.-P. Laumond, "From paths to trajectories for multi-body mobile robots," in Experimental Robotics V, pp. 301-309, Springer, Berlin, Heidelberg, Germany, 1998.

[38] G. Farin, Curves and Surfaces for Computer Aided Graphical Design, Academic Press, Cambridge, MA, USA, 2002.

[39] K. Kawabata, L. Ma, J. Xue, and N. Zheng, "A path generation method for automated vehicles based on Bezier curve," in Proceedings of 2013 IEEE/ASME International Conference on Advanced Intelligent Mechatronics, Wollongong, Australia, July 2013.

[40] L. Lu, "A note on quintic polynomial approximation of generalized Cornu spiral segments," Journal of Computational and Applied Mathematics, vol. 253, pp. 123-130, 2013.

[41] M. Kjargaard, N. A. Andersen, and O. Ravn, "Generic trajectory representation and trajectory following for wheeled robots," in Proceedings of 2014 IEEE International Conference on Robotics and Automation, Hong Kong, China, May-June 2014.

[42] V. Pitkanen, A. Tikanmaki, and J. Roning, "Synchronization of mobile robot's actuated wheels," in Proceedings of SPIE 9406, Intelligent Robots and Computer Vision XXXII: Algorithms and Techniques, San Francisco, CA, USA, February 2015.

[43] K. Yang and S. Sukkarieh, "An analytical continuous-curvature path-smoothing algorithm," IEEE Transactions on Robotics, vol. 26, no. 3, pp. 561-568, 2010.

Chien-Sheng Wu, (1) Zih-Yun Chiu, (1) and Jing-Sin Liu (ID) (2)

(1) Department of Electrical Engineering, National Taiwan University, Taipei, Taiwan

(2) Institute of Information Science, Academia Sinica, Taipei 115, Taiwan

Correspondence should be addressed to Jing-Sin Liu;

Received 15 June 2018; Accepted 27 August 2018; Published 19 November 2018

Academic Editor: Mohamed B. Trabia

Caption: Figure 1: (a) The structure of a unicycle mobile robot and (b) its kinematics of rolling without slipping (zero lateral velocity).

Caption: Figure 2: Symmetric lane-change curve (blue line) followed by a unicycle from a start configuration [W.sub.A] at Current Lane A to an end configuration [W.sub.B] at Target Lane B on a one-way two-lane road. The lanes denote the centrelines of roadways. The world coordinate frame is set as follows: x denotes the forward/longitudinal direction, y denotes the lateral direction, and [theta] is the angle between the x-axis and the heading direction of the vehicle (coincident with the tangent for a unicycle).

Caption: Figure 3: Path shape is altered by the placement of control points of the lane-change quintic Bezier curve, where two shapes are shown for demonstration.

Caption: Figure 4: Concatenation of two cubic Bezier curves at the midpoint with defining control points.

Caption: Figure 5: Symmetric parametric Bezier curves with [G.sup.2] continuity as lane-change curves: (a) path and (b) continuously differentiable curvature. There are two curvature maxima located symmetrically to the midpoint.

Caption: Figure 6: Simplified [[eta].sup.3]-splines as lane-change curves, with [[eta].sub.1] = [[eta].sub.2] = 2k - 1, k = 1, 2, ..., 10, and [[eta].sub.3] = [[eta].sub.4] = [[eta].sub.5] = [[eta].sub.6] = 0. (a) Path and (b) continuously differentiable curvature. There are two curvature maxima located symmetrically to the midpoint.

Caption: Figure 7: Scenario 1: comparative study of time-optimal trajectory simulation for candidate lane-change curves. The red, blue, and green bars denote, respectively, quintic Bezier ([r.sub.q] = 0.5, 0.3, 0.2), concatenated cubic Bezier ([r.sub.c] = 0.1, 0.2, 0.3, 0.4, 0.5), and simplified [[eta].sup.3]-spline ([[eta].sub.1] = [[eta].sub.2] = 5, 7, 9, 11, 13). (a) Path length, (b) maximum curvature [[kappa].sub.max], (c) minimum travel time [T.sup.*], and (d) trajectory computation time [T.sub.c].

Caption: Figure 8: Scenario 1: simulation of time-optimal velocity planning from (0, 0) to (10, 10) for (a) quintic Bezier with [r.sub.q] = 1/5, (b) concatenated cubic Bezier with [r.sub.c] = 0.1, and (c) simplified [[eta].sup.3]-spline with [eta] = [5, 5, 0, 0, 0, 0]. From left to right: the generated path in x-y plane, [MVC.sup.*] and AR (gray area) in (s, s') plane, the U-shape velocity profiles vs. time, and the longitudinal acceleration and angular acceleration profiles vs. time.

Caption: Figure 9: Scenario 2: from (0, 0) to (1, 1). (a) Travel paths and (b) their continuously differentiable curvature of the three faster lane-change curves chosen among candidate lane-change curves from Figure 7. There are two curvature maxima located symmetrically to the midpoint.

Caption: Figure 10: Scenario 2: simulation of time-optimal velocity planning along paths depicted in Figure 9(a). (a) Quintic Bezier with [r.sub.q] = 0.2, (b) concatenated cubic Bezier with [r.sub.c] = 0.1, and (c) simplified [[eta].sup.3]-spline with n = [0.5, 0.5, 0, 0, 0, 0]. From left to right: the generated [MVC.sup.*] and AR (gray area) in the (s, s') plane, the velocity profiles vs. time, and the longitudinal acceleration and angular acceleration profiles vs. time.
Table 1: ISO 2631-1 standard.

Overall acceleration   Consequence (m/[s.sup.2])

a < 0.315              Not uncomfortable
0.315 < a < 0.63       A little uncomfortable
0.5 < a < 1            Fairly uncomfortable
0.8 < a < 1.6          Uncomfortable
1.25 < a < 2.5         Very uncomfortable
2.5 < a                Extremely uncomfortable

Table 2: Details of comparison in Figures 8 and 10.

Curve type               Scenario   Length (m)

[r.sub.q] = 0.2          Hard          1.50
                         Loose        15.03

[r.sub.c] = 0.1          Hard          1.45
                         Loose        14.52

[[eta].sub.1] =          Hard          1.46
[[eta].sub.2] = 0.5, 5   Loose        14.61

Curve type               Scenario   [[kappa].sub.max] (1/m)

[r.sub.q] = 0.2          Hard                4.10
                         Loose               0.41

[r.sub.c] = 0.1          Hard                9.67
                         Loose               0.97

[[eta].sub.1] =          Hard                7.95
[[eta].sub.2] = 0.5, 5   Loose               0.80

Curve type               Scenario   [T.sup.*] (s)   [T.sub.c] (s)

[r.sub.q] = 0.2          Hard           4.60            0.0095
                         Loose          22.63           0.0085

[r.sub.c] = 0.1          Hard           5.61            0.0104
                         Loose          22.30           0.0083

[[eta].sub.1] =          Hard           5.12            0.0103
[[eta].sub.2] = 0.5, 5   Loose          22.31           0.0095
COPYRIGHT 2018 Hindawi Limited
No portion of this article can be reproduced without the express written permission from the copyright holder.
Copyright 2018 Gale, Cengage Learning. All rights reserved.

Article Details
Printer friendly Cite/link Email Feedback
Title Annotation:Research Article
Author:Wu, Chien-Sheng; Chiu, Zih-Yun; Liu, Jing-Sin
Publication:Modelling and Simulation in Engineering
Article Type:Report
Date:Jan 1, 2018
Previous Article:Analysis and Optimization of Passenger Flowlines at Zhongchuan High-Speed Railway Station.
Next Article:Parametric Studies of Flat Plate Trajectories Using VIC and Penalization.

Terms of use | Privacy policy | Copyright © 2022 Farlex, Inc. | Feedback | For webmasters |