Invariant set based variable headway time vehicle longitudinal control assistance

This paper presents the design and simulation test of an assistance system that helps the driver to perform vehicle following maneuvers. When activated, the assistance system drives the vehicle to the desired inter-vehicle distance. Based on Lyapunov theory and invariant sets, the assistance system is able to guarantee that the state trajectories remain bounded under bounded disturbance inputs, such as leading vehicle accelerations. The assistance system is designed in such a way that variable time headway strategy is naturally handled. The control law is computed using Linear and Bilinear Matrix Inequalities (LMI-BMI) methods. Some design parameters can be adjusted to handle the tradeoff between safety constraints and comfort specifications.


I. INTRODUCTION
Vehicle longitudinal control has been the object of intensive research over the last decades [8].Starting with cruise control, which is now widely implemented in today's commercial vehicles, research has shifted to the adaptive cruise control (ACC) which is an extension of the cruise control that manages both relative speed and distance to a preceding vehicle [9].Starting just before the new millennium, ACC is already available in luxury passenger cars and trucks.Several technological requirements have been also investigated in the context of automated highways, including sensor range characteristics, actuator performance and cooperation spacing policies.ACC or longitudinal control means acting on the throttle as well as the brake system.As mentioned in [12], ACC systems typically consist of two parts: a vehicle-independent part and a vehicle-dependent part [11], [14].The vehicle dependent part in the inner loops directly controls the actuator, in order to achieve the desired acceleration/deceleration generated by the vehicleindependent part.Concerning this latter aspect, several strategies have been studied.In [8], different spacing policies have been cited in order to enhance both safety and capacity.In [5], vehicle longitudinal control has been considered using a reference model inspired from a non-linear damper/spring model associated with safety zones.In [13], five assistance modes for shared driving have been established and classified from the least constraining (instrumented mode) to the most restricting (automated mode) against the driver.
is natural for the driver.In fact a time headway of about 2 seconds has been added as a vehicle following requirement in driving rules.However, in steady state, time headway is directly related to lane capacity.Thus, from the road manager point of view, if the control of the vehicle is delegated to the ACC, it would be appreciable that time headway could be tuned in order to achieve macroscopic traffic characteristics requirements.
This paper focuses on the vehicle-independent part.It aims at developing a controller for a follower vehicle that achieves control requirements of comfort and safety for a given range of headway times.When writing down the model, the leading vehicle acceleration enters as an unknown bounded disturbance input.The variable is hard to estimate from available car industry sensors.The proposed assistance is aimed to ensure both vehicle following and collision avoidance in case of driver inattention.For this purpose, invariant sets framework, which has significantly developed in control engineering over the last decades, is used [2].The existence of an invariant set (ellipsoidal or polyhedral set), under bounded disturbance, is equivalent to the "inputto state stability" (ISS) notion [6].This framework ensures that even if transitions exist between driver and assistance control of the vehicle, every trajectory that starts inside the invariant set will not exceed it, i.e. the resulting hybrid system trajectories will be bounded inside this set [10].
The remainder of this paper is divided as follows.The assistance specifications are given in Section II including the description of the safety driving zones and a switching strategy between the driver and the assistance.Section III contains the description of the vehicle inter-distance model.The design of the longitudinal control assistance is further conducted.Section IV contains simulation results for several maneuvers including ACC and stop-and-go traffic.Conclusions are presented in Section V.

A. Control objectives
The main goal of the proposed assistance is to achieve a kind of friendly and safe vehicle following.More specifically, the assistance should ensure vehicle following according to a chosen inter-distance policy and it should avoid rearend collisions due to a driver's lapse of attention during "normal following".The "normal following" means a driving situation during which the driver is following a leader vehicle without performing any special maneuver (e.g.hard braking, lane change), as well as keeping a certain distance to the leading vehicle.This will be defined in more details in the next section.
To achieve the above control goal, a vehicle following control law has been designed such that: (a) The closed loop system, corresponding to the vehicle following problem has to be asymptotically stable to zero steady state.The reference distance could be based on variable time headway in order to allow the lower or greater lane capacity when required by the infrastructure manager.(b) During the assistance intervention, the vehicle should not enter the collision zone.(c) The state variables and the vehicle acceleration/brake have to be bounded to guarantee safety and comfort.

B. Safety zones related to the car following
In order to characterize different safety levels, the following zones have been defined (see Fig. 1(a) and 1(b)): • In the normal green zone, the vehicle is in a safe operating region.The spacing distance s is around the nominal inter-distance.The assistance should switch on if the driver is not attentive.• The risky orange zone, incorporates two ranges.In the first one (see region (1) in Fig. 1(a)), the spacing is lower than the reference spacing.The assistance should override the driver in order to avoid reaching a collision red zone and to help him return in the green zone.
Reciprocally, in the second orange range (see region (2) in Fig. 1(a)), the inter-distance is higher than the reference distance.The assistance could also override the driver and accelerate in order to maintain the car following mode, if the requested acceleration is feasible.
If not, a cruise control mode could be activated.• In the collision red zone, the spacing is under the bias safe distance and a collision might occur if the leading vehicle performs a hard braking.In the other case, the cruising red zone, the inter-distance is too high and the vehicle is no more in a car following mode.The assistance deactivates and a cruise control mode takes its place.All the variables presented in Fig. 1(a) and 1(b) are described in Sections III-A, III-F and III-G.

C. Switching strategy
The switching strategy for assistance activation or deactivation uses of an invariant set ε p which includes the "normal following" zone and is located inside the limits defining the "safe following" zone (see Fig. 1(a)).Switching occurring inside this invariant set ensures that the controlled vehicle dynamics are stable and remains within this invariant set.Activation regions are shown in Fig. 1(a) and the switching follows the rules described below: • The assistance is always activable inside the "normal following" region when the driver is not attentive.Outside the "normal following" region, the switching on occurs in the following context: • When the leading vehicle is too far and approaching, the assistance is activable, but only inside the invariant set ε P .• When the leading vehicle is too far and going away, the assistance is not activable.The vehicle should enter in the cruise control mode.• In case the leading vehicle is close and going away, the assistance overrides the driver only if the system state is in the invariant set ε P .• When leading vehicle is too close and approaching, the assistance overrides the driver while the operating point is still inside the invariant set ε P .

A. Vehicle model
Vehicle longitudinal dynamics are known to be highly nonlinear, particularly when including the engine dynamics.However, in all longitudinal control architectures, the control is performed in two loops: an inner loop which compensates the nonlinear vehicle dynamics and achieves the desired algebraic acceleration by acting on the throttle or on the brakes, and an outer loop which is responsible for guaranteeing good tracking of the desired vehicle inter-distance.This decomposition is also called vehicle dependent and vehicle independent control parts.The vehicle-dependent loop is beyond the scope of this paper.It is assumed that it has already been designed and we are only interested here in the vehicle-independent part.
Longitudinal control includes the cruise control mode functionality which ensures the tracking of the desired cruise velocity v c .It switches automatically to the ACC mode when a preceding vehicle appears in the range of the host vehicle sensor with a slower speed.
Given the positions of the target and the following vehicles x t (t) and x f (t) respectively, the headway distance is s = x t (t) − x f (t) and the relative speed is v r = ẋt (t) − ẋ f (t).The headway distance dynamics can be represented as a double integrator driven by the difference between the leader vehicle acceleration and the follower vehicle acceleration, i.e.,

B. Spacing reference models
Several spacing strategies have been tested both for ACC systems and in the scope of the fully automated longitudinal control for automated highways.They include constant spacing, constant time headway and nonlinear time or space dependent headway.
1) Constant time headway: During the assistance intervention, the control law aims at tracking a constant time headway distance.This inter-distance policy is proportional to the following vehicle speed, v f (t), which leads to more spacing at higher speed.This is also very convenient since it can be implemented using only on board sensors.Desired spacing is thus given by s d (t) = s 0 + hv f (t), where s 0 is the minimum spacing at stopping and h is the time headway.
The spacing error, δ (t) = s(t) − s d (t), represents the difference between the actual spacing and the desired one.The final state-space model is given by: where u dr is the acceleration/brake input set by the driver.When the assistance is active, it is assumed that an x-by-wire system allows canceling the driver inputs (acceleration and braking) and thus the acceleration/braking input is that of the controller u a .In that case, the model of eq. ( 2) reduces to: The state vector is x a (t) (δ (t), v r (t)) T .The input γ l (t) = ẍl (t) is the leading vehicle acceleration which acts in this case as an unknown disturbance input.The whole state vector is used for control synthesis.The two variables are measured using on board sensors.In order to obtain a controller that is directly implementable on an Electronic Control Unit (ECU), the continuous-time state-space equation ( 3) is converted into a discrete-time model using a zero-order hold assumption on u a (t) and an exact discretization method with sample time T s .The sampling times t = kT s , where k represents the successive time steps belonging to I , the set of integers, Vehicle longitudinal control needs to incorporate constraints on the absolute value of the jerk j f (t) of the host vehicle.In addition, an integral action in the open-loop ensures zero steady state error, thus the model of equation ( 4) is converted into an incremental input-output model (IIO).The new control input is in this case u(k) = δ u a (k) = u a (k)− u a (k−1) and the IIO model is obtained by extending the state to x e (k) = (δ (k), v r (k), u a (k − 1)) T .In addition, it is assumed that the leading vehicle acceleration is bounded, |γ l | ≤ γ max l , one defines a normalized disturbance input |w| ≤ 1 such that γ l = γ max l w and then the IIO model takes the form: where In order to tackle with various traffic conditions, the headway time is assumed to be variable in the range [h mim , h max ].Matrices in system (8) are thus functions of h, namely A e (h) and B eu (h).It could thus be converted into an LPV system with two vertices system obtained for the extremal values of h.One can write: where The coefficients µ 1 and µ 2 are positive and are verify equations (µ 1 + µ 2 = 1) and (µ 1 h min + µ 2 h max = h), which determine uniquely the parameters.
Our aim is now to synthesize an LPV state feedback of the form By combining ( 9) and ( 10), the augmented closed-loop system is given by: µ i µ j (A ei + B eui F j ) x e (k)+B ew w(k) (11)   In the following, we denote Φ = ∑ 2 i, j=1 µ i µ j (A ei + B eui F j ), the closed loop system takes the form:

C. Invariant set under state feedback
Assume that there exists a quadratic function V (x e ) = x T  e Px e , where P is a symmetric, positive definite matrix that satisfies, for all x e , w satisfying (12), w T w ≤ 1, V (x e ) ≥ 1, the condition [2]: Consider the reachable set Λ defined by: The set ε P defined by: is an invariant set for system (12) with w ∈ R, w T w ≤ 1.This means that every trajectory that starts inside ε P remains inside it for k ≥ 0. Similarly, we will define the set ε ηP where η ∈ (0, 1] by The existence of such a function V (x e ) means that the set ε P is an outer approximation of the reachable set Λ.
ε P is also an outer approximation of the reachable set In this section, the control law and the invariant set ε P are synthesized.This is achieved using BMI (Bilinear Matrix Inequalities) optimization method such that the system without the disturbance is asymptotically stable and at the same time, the reachable set for an initial state values inside the invariant set is contained in this invariant set.

D. Invariant set -quadratic boundedness
According to the previous considerations, the closed loop linear system x e (k + 1) = Φx e (k) + B ew w(k) is strictly quadratically bounded with a common Lyapunov matrix P > 0 for all allowable w(k) such that w(k) T w(k) ≤ 1, for t > 0, if x e (k) T Px e (k) > 1 implies (Φx e (k) + B ew w(k)) T P (Φx e (k) + B ew w(k)) < x T e (k)Px e (k), for any w T w ≤ 1.
The corresponding condition is obtained using the S−procedure and invoking the Schur complement [3].We want to impose the condition w(k) T w(k) ≤ 1 and where and α > 0.

E. Constraints on input and state components
In addition, it is possible to handle constraints on the control signal and the state: where ū > 0 and Ψ := [ Ψ1 , Ψ2 , ..., Ψq ] T with Ψm > 0, m = 1, . . ., q, Ψ ∈ R q×3 and q is the number of imposed constraints.Notice that the bounds are provided separately on each state variables or a combination of them.
For a pre-specified scalar η ∈ (0, 1], the quadratic boundedness property ensures that if x e (0) ∈ ε ηP , then x e (k) ∈ ε ηP , ∀k ≥ 0, thus ∀w(k) such that w(k A similar procedure can be applied for the constraints on the state variables.Define ξ k as the k-th row of the q-ordered identity matrix.Then If there exists a symmetric matrix Ξ such that Ξ mm ≤ Ψ2 m , m = 1, ..., q, then Ψx e (k + 1) < Ψ, ∀k ≥ 0. By applying the Schur complement, and by pre-and postmultiplying the left-hand side of the obtained inequality with diag{G, I, I}, it is shown that (26) is equivalent to

F. Inclusion of the normal zone
For the beginning, the qualitative description of the safety zones has been fitted in a mathematical form.The green zone corresponds to a predefined strip of length 2δ N centered on the reference distance s d (see Figs. 1(a) and 1(b)).In addition, the relative speed has been supposed to be bounded.These two bounded regions have been considered symmetrical with respect to the origin and denoted by |v r | ≤ v N r and |δ | ≤ δ N .The bound on the vehicle acceleration/brake is handled through the |u a | ≤ u N a .Putting together the above constraints, by definition, during "normal following" the state x e stays in a bounded space region: 3, where x ei denotes the i-th component of the state vector x e which then belongs to the set where ) represents a polyhedron characterized by the diagonal matrix F N .The eight vertices of the normal zone have to be inside ε P , which leads to the LMI constraints:

G. Avoiding entering the collision zone
Similarly, the condition that the vehicle does not enter the red zone (see Fig. 1(a)) can be written as |δ | ≤ δ M .Moreover, if the relative speed is limited, |v r | ≤ v M r , and the required longitudinal acceleration/deceleration is remaining within acceptable comfort values |u a | ≤ u M a , the three above constraints also define a parallelepiped denoted by L(F M ).A "safe following" is then defined by x e ∈ L(F M ), set which is greater than L(F N ).The region of the state space between these parallelepipeds corresponds to the orange zone, while the near region outside L(F M ) is the red zone.The matrix F M is given by: The conditions ensuring that the invariant set ε P in inside this hypercube are [3]:

H. Optimization problem for control synthesis
We are now able to provide the BMI optimization problem with decision variables G, Y j , Θ i j , (i, j = 1, 2), Ξ and α, which ends with the control gain vectors F 1 and F 2 : 21), eq.( 24), LMIs f rom eq.( 27), eq.( 29), eq.( 31) This BMI optimization problem is solved using the software developed by the authors of [7].The trace of matrix G is minimized in order to reduce the excursions from the normal zone.It is also possible to maximize the trace of this matrix.This will make the assistance able to safely activate far from the normal zone.Notice that it is also possible to weight the magnitude of the maximum admissible disturbance which is also a design parameter.

IV. SIMULATION RESULTS
The assistance is now tested in a simulation environment which includes a vehicle dynamic model coupled to a driver model for car following, derived from the optimal velocity model of [1].

A. Numerical values
The "normal following" zone is defined with the following numerical values corresponding to an error of 10% of the nominal values for the speed v t f and inter-distance s t d = s 0 + hv t f , according to traffic situation: v N r = 0.1v t f , δ N = 0.1s t d .The "safe following" zone numerical values are chosen using the same idea but with different proportions: v M r = 0.5v t f , δ M = 0.7s t d .The optimization problem is solved while the BMI and LMI constraints are found feasible.The achieved numerical values are: α = 0.05, F 1 = 0.1839 0.1166 −0.0825 F 2 = 0.4398 −0.2786 −0.0995

 
The assistance is mainly designed in order to operate inside soft braking and acceleration modes.The brake and the acceleration bounds are chosen equal in absolute values to u max = 6m/s 2 , while the leading vehicle maximum brake magnitude is γ max l = 10m/s 2 .The solutions for the BMI optimization problems from eq. ( 32) lead to the invariant set shown in Fig. 2.This set includes the normal driving zone and is inside the limit zone.

B. Simulation results for braking and acceleration profiles
The distance at standing is set to s 0 = 5m and the time headway to h = 2 sec.Initial conditions are s(0) = 75m, v f (0) = 30m/s and v l (0) = v t f = 20m/s.Figure 3 illustrates the behavior of the assisted vehicle controlled by the control law.The test profile includes cruise control, hard stop and stop-and-go scenarios.The dash-dot lines in Fig. 3 correspond to the curves produced by the simulated motion of the lead vehicle.The solid line corresponds to the curves of the vehicle under the control assistance while the dash-dot line corresponds to the responses of the leader.The dashed curve in the first figure of Fig. 3 (a) corresponds to the reference inter-distance.When the follower vehicle comes near the leader vehicle, the velocity is adapted with comfortable deceleration and the vehicle is positioned to a safe distance.Then, at t = 25 sec the leader vehicle aims stopping with high braking value of 8.5m/s 2 , while the following vehicle comes to low speed of 5m/s and a corresponding headway distance of 15m.The critical distance s 0 = 5m is never reached.The braking is lower than 6m/s 2 .Thereafter, the leader vehicle is accelerated and decelerated (stop-andgo) with usual acceleration values but high jerk; however, the following vehicle is maintained to a safe distance, and with bounded jerk (< 3m/s 3 ).One can note from Fig. 2 that trajectory in the phase-plane starts at the limits of the ellipsoid and thus remains all the time inside.
In practice, the estimation of the leader vehicle acceleration could be affected by noise or a bias.Thus, the previous maneuver is simulated with non perfect measurement of the leader vehicle assuming a zero mean Gaussian additive noise signal with variance equal to 0.1 and a bias equal to 0.1m/s 2 .Results for the controlled vehicle are visible in Fig. 3.All  the responses are smooth, the control law achieves a high quality filtering of the noise.

C. Testing for different headway time values
Suppose now that starting with a time headway of 1.5 sec, the infrastructure manager decides to linearly increase it until 2.5 sec between [20, 35] sec and hold it for a time duration of 20 sec, Thus it is linearly decreased again towards 1.5 sec between [55, 70] sec and then maintained to this value.The reference distance which was initially of 35m is now needed to increase to 55m. Figure 1 shows the reference distance profile and the corresponding experienced inter-distance.One can notice that the assistance adapts for this profile as the two plots are very close.The adaptation is smooth since it is performed with a speed variation of less than 1.2m/s.

V. CONCLUSION
This paper presents the design and the simulation test of a vehicle following assistance for variable headway time handling and.In fact, a control law based on linking Lyapunov functions and invariant sets for multi-model systems with LMI and BMI optimization is proposed.This approach ensures four important features: asymptotic convergence of the vehicle trajectory to the reference headway distance with variable headway time, guarantee that the vehicle remains inside a "safe following" zone, and a bounded control input for all bounded leading vehicle acceleration.Simulation tests show that entering again in the collision red zone is avoided once the invariance set is reached.It has been verified that safety and comfort for the entire range of speed are achievable.The field test of the proposed strategy on prototype vehicles including implementation in platooning strategy is the object of future work.
23)By applying the Schur complement, and pre-and postmultiplying the left-hand side of the inequality the obtained matrix with diag{G, I}, the condition (23) is satisfied if

Fig. 3 .
Fig. 3. Follower response to leader profile in nominal conditions.