Autonomous Vehicle Control: A Nonconvex Approach ... - IEEE Xplore

IEEE TRANSACTIONS ON CONTROL SYSTEMS TECHNOLOGY, VOL. 25, NO. 2, MARCH 2017

469

Autonomous Vehicle Control: A Nonconvex Approach for Obstacle Avoidance Ugo Rosolia, Stijn De Bruyne, and Andrew G. Alleyne, Senior Member, IEEE Abstract— This paper develops a two-stage nonlinear nonconvex control approach for autonomous vehicle driving during highway cruise conditions. The goal of the controller is to track the centerline of the roadway while avoiding obstacles. An outer-loop nonlinear model predictive control is adopted for generating the collision-free trajectory with the resultant input based on a simplified vehicle model. The optimization is solved through the generalized minimal residual method augmented with a continuation method. A sufficient condition to overcome limitations associated with continuation methods is introduced. The inner loop is a simple linear feedback controller based on an optimal preview distance. Simulation results illustrate the effectiveness of the approach. These are bolstered by scaledvehicle experimental results. Index Terms— Autonomous vehicle, collision avoidance, nonconvex optimization, nonlinear model predictive control (NLMPC), path following, real-time optimization.

N OMENCLATURE Single Point Mass Model Position along y-axis [m]. yc Position along x-axis [m]. xc y Lateral displacement from target path [m]. s Curvilinear abscissa [m]. y Lateral displacement of the planned trajectory [m]. Curvilinear abscissa of the planned trajectory [m]. s∗ yi∗ Discretization of y ∗ [m]. ∗ Discretization of s ∗ [m]. si γ (s) Tangent vector angle at [rad]. V Magnitude velocity vector [m/s]. θ Angle velocity vector [rad].

Single Track Vehicle Model CG x Position of the center of gravity along the y-axis [m]. CG y Position of the center of gravity along the x-axis [m]. Manuscript received February 13, 2015; accepted April 28, 2016. Date of publication June 10, 2016; date of current version February 8, 2017. Manuscript received in final form May 8, 2016. Recommended by Associate Editor N. H. El-Farra. U. Rosolia is with the Mechanical Engineering, University of California at Berkeley, Berkeley, CA 94704 USA (e-mail: [email protected]). S. De Bruyne is with McKinsey & Company, Bruxelles 1050, Belgium (e-mail: [email protected]). A. G. Alleyne is with the Mechanical Science and Engineering, University of Illinois at Urbana–Champaign, Urbana, IL 61801 USA (e-mail: [email protected]). Color versions of one or more of the figures in this paper are available online at http://ieeexplore.ieee.org. Digital Object Identifier 10.1109/TCST.2016.2569468

CGxp

Predicted position of the center of gravity along the x-axis [m]. Predicted position of the center of gravity along the y-axis [m]. Discretization of CGxp [m]. Discretization of CGyp [m]. Vehicle longitudinal velocity [m/s]. Vehicle lateral velocity [m/s]. Absolute vehicle velocity [m/s]. Angle absolute vehicle velocity [rad]. Vehicle yaw rate [rad/s]. Front wheel longitudinal force [N]. Front wheel lateral force [N]. Rear wheel longitudinal force [N]. Rear wheel lateral force [N]. Front cornering stiffnesses [N/rad]. Rear cornering stiffnesses [N/rad]. Distance from vehicle CG to front axle [m]. Distance from vehicle CG to rear axle [m]. Vehicle mass [Kg]. Vehicle moment of inertia [Kg/m2 ]. Front wheel slip angle [rad]. Rear wheel slip angle [rad]. Wheel radius [m]. Motor torque [N]. Steering angle [rad].

CGyp CGixp CGiyp Uu Uv Va θa ω Fx1 Fy1 Fx2 Fx2 C1 C2 a b m I α1 α2 R T δ

Obstacle Modeling A B OS d

Major axis of the ellipse [m]. Minor axis of the ellipse [m]. Offset from the centerline of the roadway [m]. Threshold obstacles detection distance [m].

Receding x(t) u(t) p(t) H (·) h(·) m(·) J (·) C(·) Coi (·) Co (·) λ(t)

Horizon Optimization Problem State generic system. Input generic system. Costate. Hamiltonian. Running cost. Terminal cost. Objective function. General equality state constraints. State inequality constraint representing the obstacle. State equality constraint representing the obstacle. Time varying Lagrangian multiplier.

1063-6536 © 2016 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission. See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.

470

U (t) x 0 (t) ξ G(·) E(·) T¯ N +1


Vector of optimal discretized input and Lagrangian multiplier. Horizon starting point. Scalar for numerical integration correction. Vector discretizing the third necessary optimality condition. Deviation error due to numerical integration of the optimal solution. Horizon length [s]. Number of discrete points in the optimization window. Discretization step over the horizon [s]. Discretization step over the time axis [s].

Cθ Cθ˙ Cz ζ

Constant related to the minimization of θ [1/rad2 ]. Constant related to the minimization of θ˙ [(s/rad)2 ]. Constant related to the minimization of Z [1/m]. Scalar avoiding logarithm numerical singularity.

I. I NTRODUCTION

N

ONLINEAR MODEL PREDICTIVE CONTROL (NLMPC) is an appealing technique for autonomous driving because of its ability to handle input and state τ constraints as well as nonlinearities introduced by the vehicle T dynamics. Recently, the effectiveness and the real-time feasibility of this control strategy have been demonstrated in [1]–[5]. However, real-time path generation remains challenging in the presence of obstacles. If the obstacle is Controllers directly ahead of the vehicle, the controller has to decide on KPs Proportional coefficient for steering input which direction to turn in order to avoid it. If the obstacle conversion [s]. and the vehicle are on the same linear path, the decision KPv Proportional coefficient for torque input becomes a nonconvex one. conversion [N s]. Most optimization algorithms require convexity to converge Proportional coefficient. for steering input KPsc to the solution; for this reason, the work in [2], [5], and [6] correction [rad/m]. proposed a convex formulation of the obstacle avoidance Integral coefficient for steering input KIsc problem. The authors achieve convexity by transforming the correction [rad/m]. vehicle-road system from a time-dependent reference frame Proportional coefficient for torque input KPvc to a space-dependent one. Obstacle constraints can thus be correction [N] for simulation and [1/m] modeled as a bound on the state, thereby guaranteeing the for experiments. problem convexity. However, this strategy does not provide N p + 1 Number of predicted inputs used in the preview a decision as to which side should be used to overtake the distance controller. obstacle; the preference for passing on the left or right must PE Predicted error vector: difference between the be established a priori. predicted trajectory and the planned one To overcome this issue, the controller can be divided into a at s N∗ p [m]. two-stage approach consisting of outer-loop path planning and Predicted error vector’s component along the PEt inner-loop path tracking algorithms. In [1], an NLMPC is used tangent direction to the path at the curvilinear as a path planner to generate the feasible trajectory. However, abscissa s N∗ p [m]. the control decision is made using a distance-based method, Predicted error vector’s component along the PEn meaning that the obstacle location cannot be considered a normal direction to the path at the curvilinear hard constraint. Instead of a constraint, the controller utilizes abscissa s N∗ p [m]. a repulsive barrier function which, while effective, does not δol Outer-loop steering input [rad]. guarantee safety. This paper seeks to add the hard constraint. Tol Outer-loop torque input [Nm] for simulations To successfully implement autonomous driving real-time and adimentional for experiments. feasibility of any algorithms is a necessary condition. In [5], Input steering correction [rad]. δc the proposed control logic has been demonstrated to run Tc Input torque correction [Nm] for simulations in real time. However, the optimization algorithm uses an and adimentional for experiments. iterative method to compute the solution, and the number δ Total steering input to the system [rad]. of mathematical operations required to compute the optimal T Total torque input to the system [Nm]. solution is not known. D Total duty cycle input to the system [−]. This paper proposes an outer-loop optimization algorithm based on the generalized minimal residual (GMRES)/ continuation method [7]. This technique is explicit, so the number of mathematical operations to perform at each iteration Objective Function Design of the NLMPC is fixed. This ensures a finite computational Vtarget Target cruise velocity [m/s]. time for the online solution, as opposed to an unknown Constant related to the minimization of Y [1/m2 ]. computational time for a comparable iterative method, such Cy Constant related to the minimization of y˙ [(s/m)2 ]. as in [5]. Moreover, it is based on global optimality condiC y˙ Constant related to the minimization Cs˙ tions, and a nonconvex nonlinear dynamic optimization can of s˙ [(s/m)2 ]1. be solved. Therefore, obstacles can be considered by the Constant related to the minimization of V [s 2 ]. CV trajectory generation as hard constraints and provide safety

ROSOLIA et al.: AUTONOMOUS VEHICLE CONTROL: A NONCONVEX APPROACH FOR OBSTACLE AVOIDANCE

Fig. 2.

Fig. 1. Two-level control scheme with the path planning as an outer loop and the path tracking as an inner loop.

guarantees. An important question to be examined is whether the deterministic real-time solution can be performed suitably fast for relevant vehicle implementation. We revisit this in Section VI. This paper is organized as follows. Section II illustrates the high level control structure and the models used. The control structure consists of an inner and an outer loop where the outer loop performs trajectory planning based on a simple vehicle model. The outer-loop NLMPC computes the collision-free trajectory, which minimizes the distance from the centerline of the roadway. The inner loop is a trajectory tracking controller that uses the single track bicycle vehicle model along with a preview distance as inspired by the work in [8]. In Section III, the path following approach for the trajectory generation in the outer loop is described. The approach proposed in [9] is used, where a Fernet reference frame is attached to the path. This frame can be chosen as: 1) a moving target with its own rate of progression [10] or 2) the projection of the real vehicle onto the path [9]. The second technique is more convenient as it limits the degrees of freedom of the problem and, consequently, the computational burden. A singularity occurs when the lateral distance between the vehicle and the path equals the path curvature radius. However, this condition is not a limitation in this vehicle application, since an admissible deviation from the path is several orders of magnitude smaller than a highway sections radius of curvature. Section IV provides additional detail on the elements of the controllers of the inner and the outer loop. Section V characterizes the format of the class of obstacle and the relaxation method being used. Section VI provides the simulation and experimental results that validate the proposed approach. Section VII provides final remarks. II. C ONTROL A RCHITECTURE A. Two-Level Controller Structure The controller has two main components, an inner and an outer loop, as shown in Fig. 1. The outer loop does the openloop trajectory planning and input generation. An inner loop

471

Free body diagram of the STVM.

provides corrections to this input based on a vehicle dynamics model. Under normal highway cruise conditions, with dry road surfaces, relatively straightforward vehicle dynamics apply [11]. The longitudinal and lateral forces are low enough to utilize linear tire models and neglect wheel and drivetrain dynamics. For this reason, an NLMPC based on a single point mass model (SPMM) is used to compute the optimal trajectory and input in the outer loop. The inner loop uses a bicycle model or single track vehicle model (STVM) to correct the open-loop optimal input using a preview distance to anticipate the location of the actual system and its deviation from the planned trajectory. This control strategy, akin to a predictor–corrector approach, allows for the compensation of the differences between the real system and the simplified one while simultaneously reducing the computational burden and enabling real-time feasibility. In the following, we present the plant model used for the outer and inner loops, respectively. B. Single Point Mass Model For the SPMM used in the NLMPC (Fig. 1), the relationship between the position states (x c and yc ) and the control inputs of vehicle velocity V and heading angle θ is given as x˙c = V cos θ (1) y˙c = V sin θ. C. Single Track Vehicle Model In order to compute an accurate prediction for the preview distance block, an STVM shown in Fig. 2 is used. This model, under normal highway conditions, is considered as the representative of the vehicle’s system dynamics [12], [13]. The equations can be derived as follows: (2) Fu = m(U˙ u − ωUv ) = Fx2 + Fx1 cos δ − Fy1 sin δ (3) Fv = m(U˙ v + ωUu ) = Fy2 + Fx1 sin δ+Fy1 cos δ (4) M = I ω˙ = −a F y1 + b(Fx1 sin δ + Fy1 cos δ). The longitudinal force Fxi is determined from the input torque T and wheel radius R. In this case, we assume that both sets of wheels are being driven so i ∈ [1, 2] Fxi = T /R.

(5)

472


In this new coordinate system, the states are: the curvilinear abscissa s and the lateral distance from the path y. The inputs are: the angle of the velocity vector θ and the velocity magnitude V . The SPMM model can be converted to this coordinate system dynamics as ⎧ ⎨ s˙ = V cos (θ − γ (s)) ˙ (10) 1 − y γ (s) ⎩ y˙ = V sin (θ − γ (s))

Fig. 3. Representation of the system in the new coordinate system. States: curvilinear abscissa, s, and the lateral displacement y. Inputs: velocity vector angle θ and the velocity magnitude V .

The lateral tire forces Fyi i ∈ [1, 2] are computed by the following expressions [13]: Fy1 = C1 α1 Fy2 = C2 α2 .

(6) (7)

Given the steering input δ, the slip angles, α1 and α2 , can be calculated as V − bω (8) α2 = tan U V + aω α1 = tan + δ. (9) U The values of the parameters used for the simulations in Section VI can be found in Table I. During experiments, this plant model has been replaced by the one described in [14]. The inputs to the experimental system are the steering δ and the duty cycle D applied to the dc motor. The duty cycle is computed using the same procedure illustrated for the torque T , but the coefficients have different units.

where γ (s) is the angle of the tangent vector to the path (Fig. 3). In order to implement the system (10), an explicit expression for the path properties γ (s) and γ˙ (s) is required. Therefore, for this paper, the path is interpolated offline using a set of the fifth-order Bezier curves. Through this technique, the computation cost of the control logic is independent on the path complexity and length, as the online algorithm only needs one precomputed Bezier curve to evaluate the quantities in (10). A. Positioning Assumptions We assume that the terrain profile map is available offline and online. This map is used to compute, before navigation starts, the Bezier curves that describe the centerline of the roadway. An interpolation is performed selecting a set of points on the path and computing the set of the fifth-order Bezier curves passing through those points. The accuracy of this procedure depends on the number of points used to define the set of Bezier curves. We iterate this procedure offline, increasing the number of points used, until a sufficient accuracy is achieved. Finally, the global vehicle positioning could be performed online using techniques described in [15] and [16]. IV. C ONTROL A LGORITHM A. Outer-Loop Trajectory and Input Generation

D. System Assumptions We assume that the vehicle parameters are known, as shown in Table I. In addition, it is assumed that the vehicle location, orientation, and velocity are known accurately. With the advent of modern GPS and vehicle to vehicle or vehicle to infrastructure communication, such accuracy is feasible. We also assume that the obstacle is detected when the distance between its position and the vehicle’s center of mass is smaller than a constant d. Again, the suites of radar and vision sensing being deployed by vehicle manufacturers make this a reasonable supposition.

The outer-loop controller from Fig. 1 is determined by solving a receding horizon optimization problem online. Consider the SPMM in the curvilinear reference frame (10) with state constraints representing the obstacle, as described in Section V Co (s, y) = 0.

(11)

The control inputs (θ and V ) and the resulting collision-free trajectory (s ∗ and y ∗ ) are computed by minimizing, over a finite time window [t, t + T¯ ], the functional t +T¯ h(s, y, θ, V )dt. (12) J (θ, V ) = t

III. PATH F OLLOWING A PPROACH In the outer-loop NLMPC, the path following problem is solved to generate a collision-free trajectory. This trajectory is computed to minimize the lateral distance between the system’s output and the reference path, which in our case is the centerline of the roadway. Therefore, the origin of the reference system is taken as a moving point attached to the path: a Fernet reference frame [9]. This is shown in Fig. 3.

Afterward, the inputs (θ and V ) of the SPMM, computed via NLPMC, are converted to the outer-loop inputs (δol and Tol ) of the STVM through proportional controllers δol = K P s (θ − θa )

(13)

Tol = K P v (V − Va )

(14)

where Va is the velocity and θa is the heading angle of the STVM in Fig. 2.


473

The vector of optimal input and Lagrange multiplier values is defined as

U (t) = u ∗0 (t)λ∗0 (t) . . . u ∗i (t)λ∗i (t) . . . u ∗N−1 (t)λ∗N−1 (t) . (25)

Fig. 4. Discretization: though the interval is discretized, it moves continually in time.

B. GMRES/Continuation Optimal Control The GMRES method combined with a continuation method is applied to solve the receding horizon optimization problem. Global optimality conditions, stated by the minimum principle, are used, and a nonconvex nonlinear dynamic optimization is the outcome. Here, we derive the general optimality conditions; the specific derivation for the autonomous driving problem is treated in Appendix B. Consider a general MPC problem formulation with h(·) being the running cost and m(·) the terminal cost t +T¯ h(x(t), u(t))dt + m(x(t + T¯ )) (15) J (u(t)) = t

subject to

x(t) ˙ = f (x(t), u(t)) C(x(t)) = 0

(16)

H (x(t)∗ , p(t)∗ , u(t)) = h(x(t), u(t)) + λ(t)C(x(t)) + p(t) f (x(t), u(t)) (17) the necessary conditions for optimality are (18) (19) (20)

subject to the following boundary conditions: x(t0 ) = x 0 (t) ∂m (t0 + T¯ ). p(t0 + T¯ ) = ∂x

This is the necessary condition for optimality and it must hold for any time t. It can be used to compute the input through multiple shooting methods, but this procedure is computationally expensive and cannot guarantee a fixed number of mathematical operations. Thus, a continuation method [7] is applied. Given U (0) and x 0 (t) satisfying G(·) = 0, it is possible to compute U˙ (0), such that ˙ (t), x 0 (t)) = −ξ G(U (t), x 0 (t)) G(U

where x(t) ∈ Rn denotes the state, p(t) ∈ Rn denotes the costate variable in the dual space, and u(t) ∈ Rm u denotes the input. Given λ(t) ∈ Rm λ , the Lagrange multiplier associated with the constraint C(x(t)), and the Hamiltonian

u(t)∗ = argmin(u)H (x(t)∗ , p(t)∗ , u(t)) ∂H (x(t)∗ , p(t)∗ , u(t)∗ ) x(t) ˙ ∗= ∂p ∂H (x(t)∗ , p(t)∗ , u(t)∗ ) p(t) ˙ ∗=− ∂x

Therefore, given U (t) and the boundary condition x 0∗ (t), it is possible to compute the state and the costate. If the input is optimal, the following holds: ⎡ ⎤ ∂H ∗ ∗ ∗ ∗ x (t), λ (t), u (t), p (t) 0 0 ⎢ ∂u 0 0 ⎥ ⎢ ⎥ C x 0∗ (t), u ∗0 (t) ⎢ ⎥ ⎢ ⎥ : ⎢ ⎥ ⎢ ⎥ : ⎢ ⎥ G(U (t), x 0 (t)) = ⎢ ⎥ = 0. : ⎢ ⎥ ⎢ ⎥ : ⎢ ⎥ ⎢∂ H ⎥ ⎢ ∗ ∗ ∗ ∗ x N (t), λ N (t), u N (t), p N (t) ⎥ ⎣ ⎦ ∂u C x N∗ (t), u ∗N (t) (26)

(21) (22)

The span of the time horizon is fixed and discretized into N + 1 steps, but moves forward continuously along the time axis, as shown in Fig. 4. The optimal state and costate are discretely approximated as ∗ x i+1 (t) = x i∗ (t) + f x i∗ (t), u ∗i (t) τ (23) ∗ ∗ ∗ pi∗ (t) = pi+1 (t) + Hx x i+1 (t), u ∗i+1 (t), pi+1 (t) τ. (24)

(27)

where ξ is introduced to stabilize G(·) = 0 [17]. Then, the partial derivative with respect to the input (∂ G/∂U )(·), which is nonsingular as shown in Appendix B, can be inverted to compute an explicit expression for the derivative of the optimal solution ∂ G −1 U˙ (t) = (U (t), x 0 (t)) ∂U ∂G ˙ + −ξ G(U (t), x 0 (t)) . (U (t), x 0 (t))x(t) × − ∂x (28) Equation (28) can be discretized in time to describe the evolution of the optimal input sequence. It should be noted that, although (∂ G/∂ x)(·) is a matrix, the product (∂ G/∂ x)(·)x(t) ˙ is a vector derivative and it can be computed using a finite difference method that requires just an additional evaluation of the vector G(·) [7]. Finally, it is possible to use a Krylov method to solve the system (28), avoiding the inversion of the matrix (∂ G/∂U)(·), which could be computationally expensive. More details can be found in [18]. The continuation method converges to the optimal solution when its derivative (28) exists. However, a deviation error from the optimal solution is allowed as a result of the reactive nature of the algorithm [18], [19]. Namely, the optimal solution is computed by integrating its derivative, through Euler’s first-order method. Therefore, the resulting solution is an approximation of the continuous optimal trajectory. For this reason, the optimality condition (26) is not exactly satisfied, and the components of the vector G(·) are going to be close but not equal to zero. Here, we see the importance of the

474


Proof: The optimal control problem can be formulated using calculus of variations where the control system is viewed as a nonintegrable dynamic constraint

t +T¯

J (u(t)) =

˙ h(x(t), x(t))

t

˙ − f (x(t), u(t)))dt +λ(t)(x(t) Fig. 5.

Chebyshev discretization: nodes are equispaced on a semicircle.

correction term −ξ G(·) in (27). We define the dimensionless deviation error as E(U (t), x 0 (t)) = G(U (t), x 0 (t))∞ .

(29)

This quantity E(·) measures the magnitude of the numerical error and allows for checking the convergence of the discrete approximation to the optimal solution. The input is discrete and, for accurate numerical integration of (19) and (20), a sufficiently fine mesh is needed. Since the computational time grows exponentially with the number of nodes, a Chebyshev discretization, which guarantees a finer mesh near the boundary conditions (21) and (22), has been chosen (Fig. 5). This strategy reduces the error due to the numerical integration of the nonlinear differential equations. Since the algorithm must be initialized, iterative techniques, such as Newton’s method, can be used. For the vehicle trajectory planning, we set the initial position on the path and the target speed equal to zero. Therefore, the optimal solution starts from rest: V = 0 and θ = 0. Then, the target speed is increased gradually until the desired speed is reached. C. Sufficiency Condition to Guarantee Smooth Solutions for Outer-Loop NLMPC The GMRES/continuation method is extremely fast for the computation of the solution of a nonlinear algebraic system. However, it succeeds only when the solution is smooth. The following sufficient condition guarantees a priori smoothness of the optimal solution. Lemma: Consider the controllable system x(t) ˙ = f (x(t), u(t))

(30)

where x(t) ∈ Rn and f (x(t), u(t)) ∈ Rn can generate the entire space. Given the optimal trajectory x ∗ (t) and the optimal input u ∗ (t), minimizing the functional t +T¯ J (u(t)) = h(x(t), x(t))dt. ˙ (31) t

If the input is not directly included into the objective function and the following strict convexity of the running cost h(·) holds: x˙1 (t) + x˙2 (t) h(x(t), x˙1 (t)) + h(x(t), x˙2 (t)) h x(t), < 2 2 (32) ∀x(t), x˙1 (t), x˙2 (t) ∈ R n , then the resulting optimal trajectory x ∗ (t) is smooth.

(33)

where λ(t) ∈ Rn is a time-varying Lagrange multiplier. The variable u(t) is not included in the running cost h(x(t), x(t)), ˙ and it can be uniquely determined to satisfy the nonintegrable constraint. Therefore, the problem is reduced to the minimization of (31) over all possible trajectories x(t). Under these conditions, it is possible to apply the calculus of variation theorems. In particular, Tonelli’s regularity theorem states that given a function that is strictly convex in the velocity and bounded from below, the solution to the calculus of variations exists in the set of absolutely continuous functions [20].

D. Design of the Objective Function for the Outer-Loop NLMPC First, we design an objective function for the path following problem, which does not satisfy the Lemma. Afterward, this objective function is modified to satisfy the requirement stated in the Lemma. Comparing the results obtained using these two objective functions, the effectiveness of the sufficient condition stated in Section IV-C is analyzed. Finally, the objective function is augmented to reduce unnecessary lateral acceleration of the vehicle. For the path following problem, the objective function should be designed to minimize the lateral displacement from the centerline of the roadway y and the angle (θ − γ (s)) between the velocity vector of the SPMM and the tangent to the path at the curvilinear abscissa, s. Moreover, the deviation from a target cruise velocity Vtarget is penalized. The resultant objective function J1 is defined as

t +T¯

J1 (θ, V ) = t

+ Cθ (θ − γ (s))2

Cy y2 lateral displacement

set velocity vector parallel to path tangent

+ C V (V − Vtarget)2 − C z ln(Z + ζ ) dt set criuse velocity

obstacle related term discussed in section V

(34) where the subscript i of the constant Ci indicates to which state or input this constant is related to. The objective function J1 (34) is sufficient to achieve good path following performances [9]; however, it does not satisfy the Lemma. For this reason, we define the objective function J2 , where the cruise velocity is set on the curvilinear abscissa, s, and the minimization of the angle [θ − γ (s)] is


475

replaced by the minimization of the lateral velocity y˙ t +T¯ J2 (θ, V ) = C y y 2 + C y˙ y˙ 2 t lateral displacement

lateral velocity

+ Cs˙ (˙s − Vtarget) − C z ln(Z + ζ ) dt. 2

set criuse velocity along curvilinear abscissa

obstacle related term discussed in section V

(35) J2 satisfies the Lemma, as it is strictly convex in the state velocity (˙s and y˙ ); the inputs (θ and V ) are not directly included in the minimization, and the slack variable Z, defined in Section VI-B, is an implicit function of the states (s and y). Further details on the effect of the slack variable Z on optimality can be found in Section VI-B. Finally, an additional desired element minimizes the yaw velocity, so as to reduce unnecessary lateral acceleration of the vehicle. Thus, the heading angle θ is used to augment the system in (10) as follows: ⎧ V cos(θ − γ (s)) ⎪ ⎪ ⎨ s˙ = ˙ 1 − y γ (s) (36) y ˙ = V sin(θ − γ (s)) ⎪ ⎪ ⎩˙ θ =ω where the new control inputs are the velocity V and the rate of change of the heading angle ω. The resultant objective function J3 is defined as t +T¯ J3 (θ, V ) = J2 (θ, V ) + Cθ˙ θ˙ 2 dt. (37) t yaw velocity

This final problem formulation satisfies the Lemma, as J3 (θ, V ) is strictly convex in the derivative of the state, and the augmented system (36) is controllable. Details on the computations of the lie algebra for determining the controllability of nonlinear systems can be found in [21]. E. Inner-Loop Trajectory Tracking The inner-loop trajectory tracking is a simple PI feedback loop for a carefully chosen output. It is known that tracking performances can be improved using future information about the system state and input. In [8] and [22], a feedforward action based on previewed road curvature is computed to improve vehicle path following behavior when entering or leaving a curve. Here, a similar control algorithm is proposed that incorporates the predicted inputs and outputs computed via the NLMPC. The predicted input is applied to the STVM to compute a future position of the system if its dynamics were that of a point mass. The vehicle dynamics predicted by the STVM can be markedly different than those of the SPMM, particularly when the road is curving and the input vector is changing. A subset of the optimal inputs, u ∗i (t)i ∈ [0, . . . , N p ] with N p < N, from the NLMPC in Section III-B can be fed

Fig. 6. Computation of the error vector’s components, (PEt and PEn ), along the normal and tangent line to the path at s ∗N p .

to the STVM model to compute the predicted trajectory, (C G ixp , C G iyp )i ∈ [0, . . . , N p ]. This trajectory is compared with the planned trajectory of the SPMM, for i = N p , and an error residual is obtained, as shown in Fig. 6. The error’s components, along the tangent and normal direction to the path, are fed through P and PI controllers, respectively, and used to augment the open-loop control signal determined by the outer-loop NLMPC. A small integral action is used to compensate the difference in yaw rate that may accumulate during a continuous curve. The expressions for the correction to augment the control signals are given by Tc = KPvc (PEt ) δc = KPsc (PEn ) + KIsc

(38)

(PEn )

(39)

where the error vector components PEt and PEn are shown in Fig. 6. The resultant augmented input is applied to the system to create the total input signal T = Tol + Tc δ = δol + δc .

(40) (41)

V. O BSTACLE D EFINITION AND R EPRESENTATION A. Obstacle Representation The class of obstacle considered here is modeled using an ellipse, where the semiaxes of the ellipse A and B have been chosen, doubling the half length and the half width of a vehicle and adding a safety margin. This configuration provides a suitable safe zone around most types of stationary obstacles that would be present on a highway. For this paper, the center of the ellipse is assumed to not be on the path, and, if the obstacle is detected on the centerline of the roadway, a small offset is added to its lateral position, as shown in Fig. 7. This offset is used to avoid multiple optimal solutions. If the obstacle was centered on a straight path, then avoiding it from the left or right direction has the same cost. The offset can be determined by the current driving condition. For example, in a highway with two lanes going the same direction, the offset can be determined a priori to encourage the avoidance of the obstacle in the direction away from the other lane of traffic. For example, an obstacle in the right lane would cause the vehicle to circumvent it on the right side using the shoulder of the road.

476


5 4

Target Path STMV minimizing J2

3

STMV minimizing J

1

Obstacle

Position [m]

2 1 0 −1 −2 −3 −4 −5

Fig. 7. Ellipse with semiaxis A and B representing the obstacle. OS is the offset from the measured position of the car which, in this scenario, coincides with the roadway midline.

60

80

100

120 140 160 Position [m]

180

200

220

Fig. 8. Optimal trajectory comparison. The two trajectories are computed, minimizing the functional J1 , which does not respect the stated Lemma, and J2 , which fulfills its requirements.

B. Introducing a Slack Variable Here, it is important to highlight a limitation of the method used to compute the optimal outer-loop NLMPC solution. The GMRES/continuation is a reactive algorithm. Indeed, the solution is computed based on the error, and a deviation from the optimal solution is allowed (for more detail, see [7], [19]). This deviation could cause a violation of the Karush–Kuhn–Tucker conditions for modeling the following inequality [23] that defines the constraints the obstacle enforces on the trajectory generation: (y − yobstacle)2 (s − sobstacle )2 + − 1 ≥ 0. (42) A2 B2 An alternative approach suggested in [7] is to introduce a slack variable Z , which is defined as an implicit function of the states s and y C(s, y) =

C(s, y) =

(y − yobstacle)2 (s − sobstacle )2 + − 1 + Z 2 = 0. A2 B2 (43)

By doing so, the Lagrange multiplier associated with the constraint plays a role in computing the optimal solution. The drawback of this approach is that the slack variable has to be included in the objective function to avoid numerical problems [7], and, therefore, it affects the optimal solution, which, otherwise, would saturate the obstacle constraint. The conditions of the Lemma still hold, but this introduces a conservative choice, which is reasonable when dealing with the approximation of the system model and disturbances associated with an actual vehicle. VI. R ESULTS This section first presents the simulation results, which are then followed by scaled vehicle experimental results. In simulations, the utility of the sufficient condition stated in Section IV-C is shown. Then, the effectiveness of the preview distance on the inner-loop controller is demonstrated. Moreover, the proposed approached is compared with an existing

method that is used as a benchmark. In the experimental section, these results are validated on a scaled test bed, including multiple obstacles and complex vehicle paths. A. Simulation Results Simulations are performed using an STVM as described in Section II as the actual plant model. The outer-loop NLMPC uses the SPMM simplified model for the trajectory generation. The simplified SPMM system succeeds in capturing a majority of the dynamical behavior of a vehicle under highway cruise conditions, ignoring the higher order effects. Those higher order effects are considered by the inner-loop PI controller, utilizing a preview distance. With this configuration, the controller can be used for different vehicles by simply changing the parameters of the preview distance. 1) Influence of the Stated Sufficient Condition: To examine the effect of the sufficient condition stated in Section IV-C, simulations with and without satisfying this condition were carried out. In the following, we compare results obtained minimizing J1 (34) and J2 (35). J2 fulfills the requirements of the stated sufficient condition while J1 does not. Tables I–III list coefficients used for the controller design and parameters used for the STVM and the obstacle modeling. These represent a midsize vehicle operating at highway speeds. The results of the STVM model output when avoiding the obstacle are relatively similar whether J1 or J2 is minimized (Fig. 8). The difference can be seen when examining the dimensionless deviation error E(·) (29). The obstacle enters the optimization window at ∼46 m, since the obstacle is positioned at 110 m and the optimization horizon length is ∼60 m. The resultant dimensionless deviation error E(·) increases when the obstacle enters the horizon. This is an expected result due to the reactive nature of the algorithm [19]. If the gradient of the input is small, the error should then converge to zero. This behavior is confirmed by simulations, as shown in Fig. 9. Fig. 9 also shows that the stated sufficient condition helps stabilizing the GMRES/continuation method. Indeed, when the


25

70

Minimizing J2 Minimizing J1

60

Path to follow Obstacle

20 50 Position [m]

Deviation Error [adimensional]

477

15

10

40 30 20 10

5

0

0 40

60

80 100 Curvilinear abscissa [m]

120

0

140

Fig. 9. E(·), dimensionless deviation error. It is the error resulting from the numerical integration. When the Lemma is satisfied, indicating minimization of J2 , the overall error is small and converges faster.

100

200

300 Position [m]

400

500

600

Fig. 11. Reference path. Curvature radius: 250 m. The obstacle is positioned at 500 m distance from the starting point.

12 Minimizing J Minimizing J

1

6 4 2 0 −2 −4

5 4 3 2

−6

1

−8

0

−10 40

60

80

100 120 140 160 Curvilinear abscissa [m]

180

200

220

With preview distance Without preview

6 Lateral tracking error [m]

8

Steering [deg]

7

2

10

0

100


600

700

Fig. 10. Comparison of steering input (δ) when the sufficient condition in Section IV-C is satisfied, minimization of J2 , and when it is not, minimization of J1 .

Fig. 12. Lateral tracking error: difference between the optimal trajectories of the SPMM and the STVM.

sufficient condition in Section IV-C is satisfied, the overall error is smaller and it converges faster. In addition to greater numerical error between the solutions computed minimizing J1 and J2 , Fig. 10 shows, between 44 and 52 m, that not respecting the sufficient conditions induces a very high-frequency steering input that is undesirable. This behavior is caused by the high magnitude and the slow rate of convergence of the numerical error shown in Fig. 9. 2) Influence of Preview Distance on Inner-Loop Control: In this section, the effect of the preview distance is shown. In addition to the obstacle, a curved road profile is used, which allows us to test the preview controller. Indeed, during a curve, the dynamics of the SPMM and the STVM are markedly different; thus, the tracking error is reduced by the input correction factors computed by the preview distance controller. The preview distance in Table III is inserted and removed, and the results are compared. The objective function J3 is minimized, and the coefficients used for the simulations are reported in Tables I and IV. As can be clearly seen in Figs. 11–13,

using the preview distance with the correction factors (38) reduces the lateral tracking error between the optimal solution and the STVM (Fig. 12). The STVM’s lateral acceleration is also reduced with the preview (Fig. 13), thus improving driver comfort [24]. 3) Comparison With an Existing Method: The goal of this section is to examine the computational benefits of the explicit solution approach. The main advantage of this approach is that the number of mathematical operations needed to compute the optimal solution is fixed. In order to test the effectiveness of this explicit approach, we implemented a comparison controller where the NLMPC in Fig. 1 is solved using a nonlinear iterative optimization solver. We have decided to use an iterative solver for the comparison controller, as iterative solvers have been successfully used for NLMPC in autonomous driving applications [2], [25]–[30]. Moreover, it has been shown that these algorithms are suitable for NLMPC, as the performance can be significantly improved using warm start [3], [19].

478


2

0.9

Acceleration with preview distance Acceleration without preview distance

1.5 1

0.7

0.5

0.6 Time [s]

Acceleration [m/s2]

Comparison controller Proposed controller

0.8

0 −0.5

0.5 0.4

−1

0.3

−1.5

0.2 −2 0

Fig. 13.

100


600

700

Lateral acceleration.

0.1 0

50

100


350

400

Fig. 15. Comparison between the computational cost of the comparison controller and the proposed controller.

3 Comparison controller Proposed controller 2

Y axis [m]

1

0

−1

−2

−3 0

50

100

150

200 250 X axis [m]

300

350

400

Fig. 14. Comparison between the planned trajectories computed with the comparison controller and the proposed control logic, both the trajectories successfully avoid the obstacle represented by the red ellipse. Fig. 16.

The optimal control problem defined by the objective function J3 , the collision avoidance constraint (36), and the system dynamics (43) is Euler discretized and formulated as a nonlinear minimization. Thus, the comparison controller is implemented using the nonlinear iterative solver Ipopt, interfaced with the MATLAB 2015b through YALMIP. In Fig. 14, the trajectories computed with our proposed control strategy and with the comparison strategy are presented; both trajectories avoid the obstacle. Fig. 15 shows the computational time of the two control approaches. First, we notice that the variance associated with the comparison controller 0.0242 s 2 is significant. In particular, when the obstacle enters the horizon, between 80 and 90 m, the number of iterations in the comparison iterative algorithm increases, resulting in a higher computational time. On the other hand, the GMRES/continuation method used in the proposed outer loop maintains a low variance throughout the maneuver 1.834710−5 s 2 as the number of operations to perform at each time step is fixed [7], [31].

Experimental system.

It should be noted that in addition to increased variance, the proposed method is overall much more efficient than the comparison one. B. Experimental Results-I Experimental results were conducted in coordination with Siemens on a scaled vehicle system similar to [32]. Scaled vehicles have been previously used as surrogates for fullsized vehicles in order to gain insight as to the validity of various algorithms [33], [34]. Scaled vehicles are controlled via Bluetooth communication; the sensing is performed by an overhead color camera, and images are processed in real time. The computer collects the vehicle location, determines the appropriate control decisions using the two-stage algorithm in Section II, and then provides steering and acceleration commands to the vehicle. The reference path is the centerline of the track. The sampling time is 20 ms, which is sufficient for these vehicles. The algorithm is developed in Simulink and then exported to C code to interface with Linux 3.8.13


479

0.1 Lap 1 Lap 2

0.08

Lateral tracking error [m]

0.06 0.04 0.02 0 −0.02 −0.04 −0.06 −0.08 −0.1 0

1

2


7

8

Fig. 17. Lateral tracking error between the optimal trajectory planned in the outer-loop NLMPC and the actual position of the real system.

Fig. 19. First lap with no obstacle. The controller minimizes the steering gradient and the distance between the real system’s trajectory and the target path.

Fig. 18. Difference between the velocity profile planned in the outer-loop NLMPC and the measured velocity profile.

with a Preemp RT Patch. The results were obtained on a PC, featuring an Intel Core i3-3220 at 3.3 GHz. The computational cost of the overall control logic, the inner and the outer loop, is 1.12 ms, which is