Abstract
lane-change path imposes symmetric conditions on the path geometric properties. This paper presents the comparative study of time-optimal velocities to minimize the time needed for traversal of three planar symmetric parametric polynomial lane-change paths followed by an autonomous vehicle, assuming that the neighboring lane is free. A simulated model based on unicycle that accounts for the acceleration and velocity bounds and is particularly simple for generating the time-optimal path parameterization of each lane-change path is adopted. We base the time-optimal trajectory simulations on numerical integration on a path basis under two different end conditions representing sufficient and restricted steering spaces with remarkable difference in allowable maximum curvature. The rest-to-rest lane-change maneuvering simulations highlight the effect of the most relevant path geometric properties on minimal travel time: a faster lane-change curve such as a quintic Bezier curve followed by a unicycle tends to be shorter in route length and lower in maximum curvature to have achievable highest speed at the maximum curvature points. The results have implications to path selection for parallel parking and allow the design of continuous acceleration profile via time scaling for smooth, faster motion along a given path. This could provide a reference for on-road lane-change trajectory planning along a given path other than parametric polynomials for significantly more complex, complete higher-dimensional highly nonlinear dynamic model of autonomous ground vehicle considering aerodynamic forces, tire and friction forces of tire-ground interaction, and terrain topology in real-world.
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 , 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 [20, 21, 22, 26].
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 [17, 19] but also on the characteristics of the path such as the curvature defining the ratio of angular over linear velocity (see, e.g., [23, 24]). There exist a number of time-optimal velocity planning algorithms along a prespecified 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., [18, 20]). 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, 27–29]), 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) [20, 26] 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 ( 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 [30]. The authors in [27] 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 [30], 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 [36]. 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 of a unicycle, depicted in Figure 1, moving in a planar workspace along a parametric path parameterized by the path parameter is given by its Cartesian position coordinates and orientation denoting the heading of unicycle. For unicycle, is equal to the angle between the x-axis and the tangent of the path . The kinematic equations of motion are described as follows:where the symbol “” denotes the derivative with respect to the argument (here ), and 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:
(a)
(b)
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 [31], 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 nonholonomic vehicle motion restrict the class of path primitives suitable for a task, such as the lane change in this work.
From the unicycle kinematics, can be expressed as the functions of , and their first and second derivatives as follows:whereand the curvature is defined by the ratio of the angular speed and linear speed of (1):
The acceleration is obtained by differentiating (1) as
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 can be represented by a parametric smooth function of a scalar path parameter aswhere are smooth functions defined on satisfying the boundary conditions
General formula for signed curvature for a regular parametric curve with is
The curvature is related to second-order derivative of the curve and the acceleration.
Definition 1 (unit speed curve). Suppose a curve is parameterized by arc length . Then,
That is, if is parameterized by arc length, then is a unit vector tangent to for all . This implies that and are orthogonal.
The acceleration of a vehicle along a parameterized curve has two orthogonal components, where (the unit tangent vector) and (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 counterclockwise). The tangential component of acceleration in the tangent direction of the curve, or the longitudinal acceleration, is as follows: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:which is caused by changes in the vehicle’s moving direction. The total linear acceleration is
In [24, 25], it is supposed thatwhere 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 symmetric if
It is noted that the symmetry condition imposed on implies that and . The midpoint between the start point and final pointis the path inflection point with zero curvature and nonzero curvature derivative. Furthermore, that is, has a maximum tangent/slope at the midpoint.
Theorem ([32], p. 78). Two parametric curves represented as are connected with continuity at the junction, and it is necessary and sufficient that for continuity, for continuity, and for continuity, where are arbitrary constants.
Thus, continuity allows an arbitrary setting of second-order derivatives at the end point, while 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 [19, 33, 34, 35]. 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 bywhere the subscripts and denote the maximum and minimum, respectively. Note that the linear acceleration constraints could be asymmetric; that is, 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 [36] and limits on longitudinal acceleration are as given in ISO 2631-1 standard (Table 1).
From the relations and , 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 [24]. 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 with length , it is necessary to design its time-scaling function (or time parameterization) , an increasing function defined in the plane. should satisfy the interpolating boundary conditions: , where denote the path length, initial parametric velocity, and final parametric velocity, respectively, and is the free travel time. The function is assumed to be twice-differentiable and monotonic , 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 and angular velocity for the unicycle can be expressed aswhere and is the signed path curvature. By further differentiating (18), we obtain accelerations
Rewriting (19) in the phase plane, we obtain the equation of motion along the path with input of linear acceleration and angular acceleration :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 :where
The inertia-like vector is nonzero, but some of its components may be zero. We call the points of for some as the zero-inertia points since the vector represents an inertia-like term in the parameterized constraint equation [17, 20, 26]. At zero-inertia points, is not defined and can take any value between . For the unicycle model we deal with in this paper, corresponds to , or zero-inertia points of unicycle are the points with zero curvature, at which the angular velocity .
Note that the constraint (21) is linear in . Explicitly, we have the following lower and upper limits caused by the path acceleration limits of (21):
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 , where
Let
Therefore, given and , should satisfy the following inequality defining the envelope of dynamic feasibility to avoid overly large acceleration:
The maximum velocity curve (MVC) in the plane with is represented asfor all points with At the zero-inertia point, we set so that the velocity limit curve is continuous everywhere and has a horizontal tangent at this point. In this case, the velocity is also constrained by the curvature derivative , as seen from (21) as . Additionally, the velocity constraint in (21) induces another maximum velocity curve [20, 37]. Therefore, the velocity limit curve for velocity and acceleration bounds is the minimum of maximum velocity curve caused by acceleration bounds and maximum velocity curve caused by velocity constraint:where is part of formed by the partial of respectively.
It was proved that if the curvature is continuous, then is continuous [17]. 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) [18, 20] defined as the compact, connected region enclosed by the continuous curve and the lines , , and in the plane:
In other words, AR is the closed and bounded constraint set for the velocity in the plane. 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 is in general different from , 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 in plane for a lane-change parametric path followed by a unicycle with the acceleration command (the tangent) at any point of not violating the constraint (26). Formally, time-optimal velocity planning for a unicycle is to find a velocity function in the in 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 is minimized over all continuous functions . 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 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 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 is the design parameter for trajectory planning along a path segment connecting consecutive waypoints without violating the kinodynamic constraints, where gives a lower bound for operation time.
2.4. Numerical Integration (NI)
Numerical integration (NI) is an improved, robust phase-plane method proposed in [20, 26] 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 [20]. 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 [18]. TOPP finds a continuous time-scaling function , in the plane for the path velocity at each point along the given path by forward numerical integration of with maximum path acceleration from the start and switch points, and by backward numerical integration of with minimum path acceleration from the final and switch points. The time-optimal velocity 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 with tangent bounded by (26). The time-optimal velocity with its tangent satisfies (26) that may be entirely inside the AR or touch the boundary or part of is on the boundary of AR [18]. For detailed exposition of TOPP, we refer the readers to [20, 26] 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, , so that x and y coordinates are the displacements in the longitudinal and lateral directions, respectively. In addition, the configurations , and the velocities at the start and end points of lane-change maneuver are given. Alternatively, we are given the interpolating boundary data of position, velocity, and acceleration at the endpoints for lane-change curve denoted by conforming with the given boundary position data , .
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 -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 , 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 to pass through the midpoint of the line segment . 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 linking two endpoints with velocities is specified by the arrangement of control points , which forms a control polygon (Bezier polygon):where , and is the combination of choosing elements from elements without repeat, which can be written using factorials as . The derivative is given as follows [38]:
If is a Bezier curve of degree , 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:where . Combining (31) and (32), we can obtainand from (15), for symmetric Bezier curves, we have . Therefore,and for , from (33), it is required that and are collinear, so are and . Thus, symmetric Bezier curves at least of 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, 27, 34, 35, 39] 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 asBy employing the interpolating boundary data , , we obtain from (31) the expression of six control points asand the result in (36) corresponds to the setting and in [40]. Symmetry condition for and the requirement could be achieved by the conditions that , can be set symmetrically to be collinear separately along the same horizontal vector since . In general, can be placed at an equal distance from the endpoints in the direction of parallel lanes to ensure the tangent direction:where adjusts the magnitude of the tangent while maintaining at both endpoints. From (36), we can obtain that isand note that (37) and (38) are general forms of symmetric quintic Bezier curve. Note that the symmetric quintic Bezier with and symmetric quintic polynomial in [5] are equivalent. Several alternative designs of proposed in the literature are [41] and 1/5 [5, 42]. Furthermore, same as symmetric cubic Bezier curve, so that
(a)
(b)
3.2. Concatenated Cubic Bezier Curve
As mentioned, the minimum degree of a single Bezier curve is at least 5 to satisfy continuity. Motivated by Yang and Sukkarieh [43], here we propose a way to connect two cubic Bezier curves to meet the 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 , as shown in Figure 4, that shares midpoint is proposed to be an alternative lane-change curve. Let C1 be p(u), then C2 is pmid + p(1–u) by symmetry. This concatenation of two cubic Bezier curves C1, C2 is at least G1. Consider the cubic Bezier curve , it has to satisfy the following boundary conditions:
In Figure 4, we need to choose as the point to concatenate two cubic Bezier curves. Same as the quintic Bezier curve, 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
Substituting (41) into (36) yieldswhere denote and angle between and , respectively, as depicted in Figure 4. Notice that if , then only will satisfy the result (, is excluded due to the co-linear of four control points), which implies that coincide. As a result, the only remaining design freedom for the concatenated cubic Bezier curve is to design in the horizontal x-direction. Thus, we have by setting due to symmetry to simulate different concatenation of two cubic Bezier curves.
3.3. Simplified -Splines
A path primitive called -spline [28] can be the solution to the polynomial -interpolating problem. The solution is given by a seventh-order parametric polynomial curve , which is defined as follows:where the polynomial coefficients are relative to end configuration and real path shaping parameter vector that can be flexibly selected to modify the path shape without violating the endpoint interpolating conditions. The symmetric property of -spline is That is, denotes the set of real and nonnegative real numbers, respectively. are velocity parameters, the other four parameters and depend on the acceleration and jerk at the path endpoints, respectively. In this paper, the proposed simplified version of -spline is concerned for the computational efficiency. The simplified -spline is a version of -spline which reduces degrees of freedom with the following settings:
Then the coefficients in (43) and (44) can be written aswhere and .
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 [27] 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 (https://github.com/quangounet/TOPP/releases) [26] to account for 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 (, , 0). There are an infinite number of final configurations to consider. Here we examine two final configurations with unity lane-change ratio 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, (iii)Symmetric constant velocity and acceleration limits: and , and .
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 . 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 has the smallest maximum curvature. In Figure 6, lane-change paths of continuity and their curvatures are evaluated for the simplified -spline with varied values of . Some values of generate higher maximum curvature due to the nature of the seventh-order polynomial of -spline. To reduce the maximum curvature, an acceptable suboptimal solution [28] can be obtained by a rough heuristic rule: . Therefore, it can be observed in Figure 6(b) that, for the simplified -spline, the smallest maximum curvature occurs with ().
(a)
(b)
(a)
(b)
In Figure 7, different lane-change paths are compared in terms of path length, maximum curvature , minimum travel time that a unicycle takes to perform the lane-change maneuver, and trajectory computation time . 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, 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.
(a)
(b)
(c)
(d)
Concatenated cubic Bezier has longer travel time compared to symmetric quintic Bezier and the simplified -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 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 -spline might be available for planning shorter paths which require less travel time.
In Figure 8, 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 , , and due to their faster minimal travel time in Scenario 1. The gray region upper bounded by 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 [25], at which is touched with the switch point . 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 and AR of unicycle in new scenario.
We choose three curves , , and ( 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 . 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.
(a)
(b)
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 rc = 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 due to different lane-change ratios and different 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 of velocity and acceleration constraints in loose curvature condition, which was observed in [25] 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 is a switch point, is also a switch point (e.g., two switch points at 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 that has constant highest speed , where is the maximum allowed feasible velocity at . 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 .(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 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 [17] 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 . 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 timeis monotonically increasing with .(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:where is the maximum allowed feasible velocity at along the path. depends on the path (curvature) viawhere the subscript and superscript denote the maximum. Therefore,
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 [30], 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 function where is the execution time, or via speed change function with the constraint , where the data-driven functions 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 -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 [30] 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.
Notations
| : | Path coordinate (arc length or distance) along a specified path |
| : | Path velocity along a specified path |
| : | Path acceleration/tangent at each point 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.