Control Architecture for Cooperative Autonomous Vehicles Driving in Platoons at Highway Speeds

The situation in which a vehicle has to avoid a collision with an obstacle can be difficult to realise in optimum conditions when the roads are crowded. This paper uses the advantages of vehicle grouping and vehicle-to-vehicle (V2V) communication, and proposes a control architecture, which ensures a safe merging between the vehicles from two platoons. The architecture is formed by three layers, with the following tasks: i) to analyse the environment and to decide the best action for a certain vehicle, ii) to plan the new trajectory, and iii) to follow it at an imposed velocity or distance to the vehicle in front. The vehicles are equipped with a trajectory planner designed using two methods: the first one is based on a polynomial equation, and the second one is based on the model predictive control (MPC) algorithm. Each vehicle is also equipped with a trajectory follower, which has a cooperative adaptive cruise control (CACC) functionality based on a distributed model predictive control (DMPC) formulation. Also, the paper proposes a solution to compensate the data-packet-dropouts that are induced by the wireless communication network used to exchange information between vehicles. Moreover, to accommodate various realistic scenarios in the same control framework, the cost function for the DMPC algorithm was designed to take into account different communication topologies. The proposed architecture was tested in a simulation scenario, in which two platoons have to merge in order to avoid a fixed obstacle and the results show its efficiency.


I. INTRODUCTION
The current studies in the field of autonomous vehicles have the following main directions: i) to ensure people's safety by avoiding collisions, ii) to reduce costs and pollution by decreasing fuel consumption, iii) to improve traffic condition by avoiding congestions, and iv) to optimize the space occupied by vehicles by maintaining a small safety distance between them [1]. To accomplish these targets, the researchers proposed various solutions for the following functionalities: lane keeping [2], collision avoidance [3], trajectory planning and tracking [4]- [6], cruise control [7], vehicle platooning and vehicle grouping [8]. The last two functionalities are based on control algorithms that use The associate editor coordinating the review of this manuscript and approving it for publication was Yang Tang . the adaptive cruise control (ACC) function [9]. In the last years, the idea of connecting the vehicles with the infrastructure using networked communications has determined the development of new strategies based on the advantages of communications. Using vehicle-to-vehicle (V2V) and vehicle-to-infrastructure (V2I) communication, vehicles can make use of more information about their neighbours and environment. This information is afterward used to improve either: i) the performance in traffic (i.e., vehicles can form groups inside of which they negotiate to solve cases like deadlock or traffic congestion [10]) or ii) the performance of the existing solution (e.g., the ACC algorithm is turned into a cooperative adaptive cruise control (CACC) algorithm, in which a follower vehicle benefits by receiving the information regarding the velocity of the vehicle in front [11]- [14]).
In the literature, there are many studies that propose solutions for cooperative autonomous vehicles, aiming to solve some realistic situations met in traffic, such as, deadlock position, collision avoidance, platoon merging/splitting, etc. These solutions are based on different types of models for vehicles and various control strategies developed accordingly. In [15]- [17], the proposed solutions use predictive control algorithms for the lateral and longitudinal dynamics of the vehicles, the strategies being based on the nonlinear bicycle model. To design the CACC strategy, the vehicles are modelled using the point model (neglecting the dimensions of the vehicle) and the control strategies that use this kind of modelling are represented by: classical PID control [18], model based predictive control (MPC) [19]- [21], distributed model based predictive control (DMPC) [13], [22]- [24], control law representing a linear combination of velocity, acceleration and distance errors [25]- [28], and solutions based on the linear quadratic regulator (LQR) [29]. Also, there are solutions for trajectory planning, which use the kinematic/point model to predict the trajectory of vehicles [30]- [35]. Other solutions for path planning are based on computing the trajectory using polynomial equations [24], [26], [36], [37]. Applications of merging vehicles also receive a great interest nowadays. These studies mainly use the point model for the vehicles and the control solutions are based on LQR or MPC algorithms [26], [33], [38]- [40]. The merging manoeuvre often requires a combination between the CACC, trajectory planner and trajectory follower to ensure the required space for vehicles and a safe merging [41]- [44]. The exchange of information between vehicles from a platoon is performed under some specific communication topologies. The vehicles receive and send data only from and to certain vehicles. The most used methodologies which consider such communication topologies when designing the control solutions are based on: a) including the vehicle interconnection in the vehicle dynamics model [45], b) designing a control law which is defined as a linear combination between the error of the vehicle outputs (distance/velocity) [46], and c) considering these topologies in the cost function which has to be minimised to obtain the control command [47]. Note that, the last method is considered in this paper to test the efficiency of different communication topologies. This paper proposes a control architecture, suitable for vehicles driving in platoons formed by three levels as follows: • Level I obtains information from sensors and through V2V communication regarding the environment, such as, nearby obstacles, neighbouring vehicles, etc., decides the new actions for the vehicle, e.g., maintain the lane, change the lane, form triangle shapes with the vehicles from neighbouring lanes, change the velocity, etc. Moreover, this level sends information regarding the driving environment, e.g., positions and velocities of neighbour vehicles, positions of the obstacles, and control commands to the next two levels; • Level II is represented by the trajectory planner, which uses the information and commands received from Level I to plan the lateral trajectory for the vehicle and transmits it to Level III. This paper proposes two methods for designing the trajectory planner: i) a polynomial trajectory planner that requires the initial and target state of the vehicle (lateral position, lateral velocity and lateral acceleration) and computes the parameters of a 5 th order polynomial trajectory; ii) a MPC algorithm to determine the trajectory of the vehicle. The MPC algorithm uses a simple model for the dynamics of the vehicle to estimate its position and to minimise a cost function in order to lead the vehicle from an initial position to a given target position; • Level III is composed of a CACC algorithm and a trajectory follower: the CACC is implemented using a DMPC algorithm and its task is to ensure that the vehicles from a platoon are travelling at the imposed velocity and maintain a safe distance between them. Moreover, when Level I commands it, the CACC is used to form triangle shapes between vehicles from two platoons travelling on neighbouring lanes. Furthermore, for the CACC algorithm, different types of communication topologies were tested and their performances were evaluated. The trajectory follower is designed using a MPC strategy that computes the steering angle to guide the vehicle to follow the received trajectory from Level II.
The novelty of the proposed architecture with respect to our previous works [12], [24], [31], [44] and similar works from the literature [33], [40], [41], [43] is given by the following characteristics: • the CACC functionality is designed for disturbed communication channels, i.e., subject to frequent datapackets-dropouts; • different types of communication topologies for the CACC algorithm are considered, and their influence on the platoon performance was evaluated; • the paper compares two different methods to design the trajectory planner: the first solution for the trajectory planner is based on 5 th order polynomials, ensures a smooth path and requires reduced computational power; the second method for the trajectory planner is based on the MPC algorithm, requires more computational power, but it needs less information regarding the target position compared to the first method; • the trajectory follower is designed using a linear bicycle model, but the vehicles are simulated using a nonlinear bicycle model, which is closer to the dynamical behaviour of a real vehicle; • the solution combines both lateral and longitudinal dynamics and avoids a collision with a static obstacle by executing a manoeuvre to merge two platoons at highway velocity.
The proposed control architecture was tested in a simulation scenario, in which two platoons have to merge to avoid a VOLUME 9, 2021 collision with a static obstacle. The platoons successfully executed a safe merging manoeuvre and overpassed the obstacle. This paper is structured as follows: Section II presents the mathematical models used to describe the longitudinal and lateral dynamics of the vehicles. In Section III, the predictive control algorithms are described, while Section IV gives the solutions for trajectory planning. The proposed control architecture is presented in Section V. The collision avoidance manoeuvre, illustrative results and discussions are presented in Section VI, followed by the conclusion in Section VII.
Notations Z and Z + are the integer and non-negative integer numbers, R and R + are the real and non-negative real numbers. We use the notation Z [c 1 ,c 2 ] to denote the set {k ∈ Z + | c 1 ≤ k ≤ c 2 }, for some c 1 , c 2 ∈ Z + . X 0 denotes that matrix X ∈ R n×m is semipositive definite and X 0 denotes that matrix X is positive definite, n, m ∈ Z + . Matrix I n ∈ Z n×n represents the identity matrix of size n ∈ Z + . The symbol ||X i || ∞ denotes the infinity norm and represents the maximum of X i ∈ R n .

II. VEHICLE DYNAMICS MODELLING
This section presents the mathematical models which describe the longitudinal and lateral dynamics of the vehicles. The models are used in the design phase of the controller and to simulate the vehicles.

A. LONGITUDINAL DYNAMICS MODEL
The following longitudinal model describes how the velocity of the vehicle varies under the action of the input, i.e., longitudinal traction force, and disturbance forces [48]: where t represents the time,ẋ represents the longitudinal velocity of the vehicle, F x is the input, i.e., longitudinal traction force, m represents the mass of the vehicle, g represents the gravitational acceleration, α represents the road slope, ρ represents the air density, S represents the vehicle frontal area, C d represents the drag coefficient, f represents the rolling resistance, mg sin α represents the longitudinal component of the gravitational force, fmg cos α represents the rolling force, 0.5ρSC d (ẋ − v w ) 2 represents the air friction force and v w represents the velocity of the air.
The linearisation of (1) is performed considering a nominal operating point (ẋ 0 , α 0 ), obtaining: with: Model (2) is extended with an integrator state which has as input the error between the imposed longitudinal velocity and the actual velocity of the vehicle: where χẋ represents the integrator state andẋ ref is the imposed reference velocity for the vehicle. The integrator state has the advantages that it ensures a zero steady-state error. Equation (2) mathematically models the velocity of the leader vehicle from the platoon, the following model being used to describe the evolution of the distance between two consecutive vehicles, V i and V i−1 : where d V i ,V i−1 represents the distance between vehicle V i and the vehicle in front of it, i.e., V i−1 ,ẋ V i represents the longitudinal velocity of vehicle i, F xV i represents the input, i.e., the longitudinal traction force, andẋ V i−1 represents the velocity of vehicle V i−1 . Considering a follower vehicle, the aim is to maintain a desired distance to the vehicle in front, and for this reason, model (5) is extended with an integrator state, χ d , which has as input the error between the desired distance, d ref , and the distance between vehicles V i and V i−1 : where d 0 represents the parking distance and τ represents the headway time.

B. LATERAL DYNAMICS MODEL
The lateral dynamics model describes how the lateral position and yaw rate of a vehicle vary when the steering angle of the front tire changes.

1) NONLINEAR LATERAL DYNAMICS MODEL
In this subsection, the nonlinear bicycle model is described.
The model was obtained considering that the rear and front axles are joined and the vehicle is steered only by the front tire [49]: where y represents the lateral position, β represents the yaw angle,β represents the yaw rate, F yf and F yr represent the lateral front and rear forces, f represents the distance between the centre of gravity of the vehicle to the front axle, r represents the distance between the centre of gravity of the vehicle to the rear axle and I represents the vehicle's 153474 VOLUME 9, 2021 rotational inertia. The relation between the lateral forces and steering angles are given by [49]: where δ f and δ r represent the steering angles of the front and rear tire, respectively, C f and C r represent the cornering stiffness coefficients, γ f and γ r represent the velocity angles of the tires, and v Lf , v Lr , v lf , v lr represent the lateral and longitudinal velocities of the front and rear tires defined as:

2) LINEAR LATERAL DYNAMICS MODEL
To obtain a linear lateral bicycle model, the following assumptions were considered [49]: • the steering angle of the rear tire, δ r , is equal to zero, the vehicle being steered by the variation of the front steering angle, δ f ; • the steering angle of the front tire, δ f , has small values (|δ f | ∼ = 0.1745 [rad]); • the vehicle is moving with a constant longitudinal velocity; • the longitudinal velocity is much higher than the lateral velocity. Under these assumptions, the linear bicycle model becomes: where Model (11) is extended with an integrator state, χ y , which allows us to impose a nonzero reference for the lateral trajectory of the vehicle. It has as input the error between the imposed lateral trajectory, y ref , and the lateral trajectory of the vehicle:χ

III. PREDICTIVE CONTROL METHODOLOGY
The MPC strategy has the advantage of its simplicity and can be used to control a variety of processes, no matter how complex, with many inputs and outputs. The method can take into account the limitations of the actuators and allows operation closer to the imposed constraints. The MPC algorithm can compensate the measurable disturbances based on a feed-forward component [50].

A. MODEL BASED PREDICTIVE CONTROL STRATEGY
At each sample time, the MPC algorithm performs the following steps: • Obtains information regarding the process state and/or output from sensors/estimator; • Computes a finite optimal control sequence: --Minimising a cost function while satisfying all imposed constraints; --Predicting the state and output of the system based on its model; • Implements the first component of the optimal control sequence according to the receding horizon principle [50]. The model used to predict the behaviour of the system can be described as a state-space model: where k is the discrete time, χ ∈ R n is the state of the system, u ∈ R m is the input of the system, z ∈ R p is the output of the system, A d ∈ R n×n is the state matrix, B d ∈ R m is the input matrix, C d ∈ R p is the output matrix, n ∈ Z + is the number of the system states, m ∈ Z + is the number of the system inputs, p ∈ Z + is the number of the system outputs, the pair (A d , B d ) is controllable and the pair (C d , A d ) is detectable. Problem 1: Given an initial state χ(0), at each sample time k ∈ Z + , compute a finite horizon optimal input sequence U MPC * (k) = [u MPC * (k|k), . . . , u MPC * (k + N MPC − 1|k)] T that minimizes the cost function: over U MPC , subject to the constraints: where N MPC is the prediction horizon and P MPC , Q MPC , R MPC are the weight matrices for the terminal state, the intermediary states and inputs of the system, is the prediction of χ at time i within the prediction horizon window, computed at discrete time k ∈ Z + and u MPC i represents the input trajectory, u MPC (k + i).
The prediction equation for the state of the system is determined using (13), and yields as: VOLUME 9, 2021 and the matricial form of the cost function can be described as: where: The next step is to describe in a matricial form the imposed constraints (15): where: Combing the relations (16)-(18), Problem 1 becomes: where: The DMPC strategy has the advantage that it can be used to control complex systems such as: vehicle groups, chemical plants or wind farms. The algorithm assumes that a complex system can be decomposed in several coupled subsystems, and for each of them a predictive controller is designed [51]. Each subsystem is described using a simple model and requires a less sophisticated local controller, as opposed to a centralised control solution, designed from a global perspective. The following equation represents the mathematical model for subsystem's j dynamics, including its links with M j neighbouring subsystems [52]: where χ j ∈ R n j represents the state of subsystem j, u j ∈ R m j represents the input, u i ∈ R m i represents the input of subsystem i, χ i ∈ R n i represents the state of subsystem i, A d j,j ∈ R n j ×n j represents the state matrix of subsystem j, The dynamics of subsystem (21) depends on both the states and the inputs of its neighbours.
A particular case for this system is represented by a model in which the subsystems are interconnected only by inputs [53]: Another particular case of system (21) is represented by a model, in which the subsystems are coupled only through states: (23) Note that, the most common structure of a vehicle platoon can be described using (23), in which each subsystem is influenced only by its predecessor neighbour [13]: where z j ∈ Z p j + represents the output and C d j ∈ R p j ×n j represents the output matrix, p j ∈ Z + . Note that matrix A d 0,1 is equal to a null matrix because the subsystems communicate unidirectionally and subsystem 0, i.e., the leader, does not have a preceding subsystem.

C. COST FUNCTION DEFINITION FOR DMPC
To improve the performances and the stability of vehicle platoons, in the literature there are various types of communication topologies. According to the work in [54], [55], the most used communication topologies are: a) predecessor following (T 1), which assumes that each vehicle receives information from the vehicle in front, b) leader-predecessor following (T 2), which assumes that each follower vehicle uses information from its relative leader, i.e., the vehicle in front and from the leader of the platoon, c) bidirectional (T 3), which assumes that each vehicle computes its control command taking into account information from its neighbouring front and rear vehicles and d) leader-bidirectional (T 4), which is a combination between T 2 and T 3. The influence of the communication topologies on the platoon performances can be modelled through a cost function [47], [54], which includes the interconnections between the vehicles in the platoon.
Problem 2: Given an initial state χ j (0), at each sample time k ∈ Z + , compute a finite horizon optimal input sequence ] T that minimizes the cost function: with: over U DMPC j , subject to the constraints: where u j (i) = u j (k + i|k), z j (i) = z j (k + i|k) are the prediction of the future inputs, respectively outputs of subsystem j at N DMPC is the prediction horizon, χ j (i) = χ j (k + i|k) and χ q (i) = χ q (k + i|k) are the prediction of the states of subsystems j and q, λ jq is set 1 if the vehicle j receives information from vehicle q and zero otherwise, S j represents the set of neighbours of agent j. Fig. 1 [56] illustrates the four types of communication topologies considered in this paper. To obtain the cost function for each case, the coupling term V Z DMPC from (25) is particularised as follows: • Topology T 1 where, each vehicle receives information from the vehicle in front, the cost function is obtained considering all λ j,q set to zero: It has to be mentioned that in this case, the information sent by vehicle j − 1 to vehicle j is not considered in the cost function, but in the model, as in (24).
• For topology T 2 supposes that each agent j will receive information from its relative leader and the leader of the platoon, thus λ j,j−1 = λ j,1 = 1, λ j,q = 0, q ∈ S j − {j − 1, 1}: • Topology T 3 assumes that λ j,j−1 = λ j,j+1 are set to 1 and the rest of lambda coefficients are set to zero: • Topology T 4 is a combination between T 2 and T 3, in which the cost function is obtained considering λ j,j−1 = λ j,j+1 = λ j,1 = 1 and rest of lambda coefficients are set to zero: For all the above mentioned communication topologies, the state predictor for subsystem j is given by:  is received from subsystem j − 1 and represents the prediction of its state at step k − 1 and

D. DATA-PACKET DROPOUTS COMPENSATION
Subsystem j receives and sends information via a wireless communication network in the presence of communication disturbances, e.g., data-packet-dropouts. So, there exists the possibility that information ζ DMPC q will not be received by agent j resulting in a misuse within the predictor formulation (33). In this case, the lost information has to be replaced. This procedure will be accomplished using the last received information. Let us consider that the last received information is ζ DMPC q,j (k) = [χ q (k + 1|k), . . . , χ q (k + N DMPC |k)] T sent by subsystem q to subsystem j at step k. Let us assume that there are s ≤ N DMPC consecutive data-packet-dropouts, then the message ζ DMPC q,j (k + s) will be obtained using [12]: Remark 1: Note that the DMPC strategy offers the possibility to approximate the lost information but, as a disadvantage, the approximation is based on the prediction of subsystem state at least a sample time in the past, which leads to increasing estimation errors due to the differences between the models and the real subsystems.
Remark 2: Another problem which may occur is that the number of consecutive lost data-packets may be greater than the prediction horizon N DMPC . In this case, a possible solution will be a platoon split due to the high number of consecutive lost data, which may occur due to problems in the communication network and may lead to decreased performances.

IV. LATERAL TRAJECTORY PLANNING
In this section, two methods for designing the trajectory planner for the lateral movement of a vehicle are illustrated. The first trajectory planner determines the trajectory of the vehicle using the equation of a 5 th order polynomial; the second method is based on the MPC algorithm to estimate the lateral position of a vehicle and minimizes a cost function which depends on the lateral error.

A. 5 th ORDER POLYNOMIAL LATERAL TRAJECTORY PLANNER
The polynomial trajectory planning uses a 5 th order polynomial to compute the trajectory of the vehicle. The method requires the initial and the target states of the vehicle and uses them to compute the parameters of the polynomial. In this work, the initial and target states are represented by the lateral position, velocity and acceleration of the vehicle in the initial and target position, respectively. Note that, the advantages of using a 5 th order polynomial will be explained after its mathematical description.
Problem 3: Given the initial state P y0 = [y 0 ,ẏ 0 ,ÿ 0 ] T , P x 0 = x 0 and the target state P yf = [y f ,ẏ f ,ÿ f ] T , P x f = x f of the vehicle, determine the parameters [a 5 , a 4 , a 3 , a 2 , a 1 , a 0 ] of a 5 th order polynomial so that the following conditions are accomplished: where y ref represents the reference, for the lateral trajectory planner, and x represents the longitudinal position of the vehicle. The conditions (35) represent the values of the lateral position, velocity and acceleration of the vehicle in the initial position and those imposed for the vehicle in the target position. Applying these conditions, the analytical solution of Problem 3 can be determined as: The conditions (35) can be interpreted in two ways. First, the constraints ensure that the trajectory will lead the vehicle from its initial position to the target position via a smooth path, because the conditions (35b)-(35d) ensure that the lateral trajectory and its first and second derivatives (i.e., y ref (

B. MODEL BASED LATERAL TRAJECTORY PLANNER
This method is based on the MPC algorithm and requires a model for the lateral dynamics of the vehicle to predict its behaviour. To ensure a low computational power, a point model is used:ÿ where y p represents the lateral position and a yp is the lateral acceleration of the lateral point model, and also represents the input of the model. The lateral trajectory of the vehicle is obtained through the minimisation of the lateral error between the imposed lateral position and the estimated lateral position: Problem 4: Given the initial state P y0 = [y 0 ,ẏ 0 ,ÿ 0 ] T , and the target state P yf = [y f ,ẏ f ,ÿ f ] T , at each sample time, evaluate the lateral trajectory of the vehicle using (37) and minimize the cost function over a yp (k + i), subject to: where N p is the prediction horizon, λ yp and λ a yp represent tuning parameters. The solution of Problem 4 is represented by a sequence of N p lateral positions through which the vehicle has to pass. Due to the modelling errors between the model used to compute the lateral position of the vehicle and the real vehicle, only the first value will be actually used. At the next sample time, the trajectory planner computes another solution for Problem 4.
Consider that the last imposed position generated by the planner is y old and the new imposed position is y new . Moreover, the working time sample of the planner T sp is greater by M times than the sample time of the trajectory follower, so for the next M time samples, the trajectory follower has to steer the vehicle to pass through the following M th lateral positions [31]: (40) where y ref (k + j) represents the imposed lateral trajectory in the discrete-time interval [k + 1, k + M ], j = 1, M .

C. COMPARATIVE REMARKS
The differences between the two proposed trajectory planners can be pointed out as follows: i) the first method, i.e., the one which uses the polynomial trajectory planner (see Section IV-A), requires fewer computational resources and less time, when compared with the second method, MPC-based method (see Section IV-B), which has to compute the position of the vehicle and to solve an optimisation problem; ii) regarding the information required by the methods, the first method requires more information, especially in the target position, i.e., lateral position, velocity and acceleration imposed to the vehicle, compared to the second method, which only needs the value of the target lateral position, iii) although the first method requires more information regarding the vehicle in its initial and target position, these are used to ensure a smooth and feasible trajectory, without sudden variations compared to the second method, which ensures these properties of the trajectory through an empirical choice of the parameters λ yp and λ a yp .

V. CONTROL ARCHITECTURE
This section describes the proposed control architecture, see Fig. 2, which ensures the longitudinal and the lateral control for a vehicle from a platoon. The architecture is divided into three levels, each of them having several tasks to accomplish. The rules which manage the first level are given by: Level II is represented by the trajectory planner and has the following tasks: Level III is composed by the trajectory follower and the CC/CACC controller. It controls the velocity and the steering of the vehicle, so that the commands from superior levels are accomplished:

VI. SIMULATION RESULTS
In this section, the simulation results obtained for the obstacle avoidance manoeuvre using platoons merging are illustrated. Moreover, analyses of the platoons stability, longitudinal and lateral performances are performed. In the end, a comparison of performances for the CACC algorithm using four types of communication topologies is given.

A. PLATOONS MERGING MANOEUVRE SIMULATION
The following scenario is proposed. Two platoons are travelling in the same direction on two neighbouring lanes. On the VOLUME 9, 2021 lane of the second platoon, the leader of the second platoon, denoted L 2 detects a fixed obstacle. To avoid it, the leader asks the neighbouring platoon (which is travelling in the same direction) to accept a merging manoeuvre. The leader of the first platoon, denoted L 1 accepts, and the two platoons decrease their velocity to facilitate the execution of the merging manoeuvre. To accomplish a successful merging, the platoons need to ensure enough manoeuvre space, thus the vehicles start to create triangle forms which facilitate a fast merging. Vehicle L 1 will be the leader of the new merged platoon followed by its new follower, L 2 , which starts to maintain a desired distance from its new leader. Vehicle L 2 is followed by its new follower, F 1 1 and so on. This new organisation of the platoon has as result the triangle forms, see Fig. 3. When the vehicles are travelling with the imposed velocity and each follower maintains an imposed distance to the vehicle which will be in front of it after the merging manoeuvre, Level I of the leader L 1 commands to Level I of follower vehicles from the old platoon P2 to change the lane. After receiving this command, Level I of all vehicles commands to Level II, represented by the trajectory planner, to compute a new path, which leads the vehicle from its initial position to the target position from the new platoon. During lane changing, the distance between vehicles is ensured by the CACC functionality (Level III). The trajectory follower receives the new imposed trajectory from Level II and steers the vehicle to follow it. The proposed steps for merging the two platoons assume the cooperation between longitudinal and lateral control, i.e., the CACC controller ensures enough space between vehicles for a safe merging manoeuvre and the trajectory planners and followers lead the vehicles from their initial lane to the new position in the new platoon. Note that, the results from this subsection are obtained using the first communication topology T 1, for which the coupling term of the cost function in (25) is given by (29).
The CACC controller is designed using the DMPC algorithm and has the following parameters (see (25)-(29)): the prediction horizon N DMPC = 50 samples, the input weight matrix R DMPC = 5 * 10 −7 and the sample time T s1 = 0.1s. The trajectory follower controller is designed using the MPC method and has the following parameters (see (14)): the prediction horizon N MPC = 10 samples, the weight matrix of the state Q MPC = 3I 5 , the weight of the input R MPC = 0.32, and  the sample time T s2 = 0.01s. The velocity used to linearise the model of the lateral dynamics (11) isẋ = 15.27m/s. The method of designing the trajectory planner using the 5 th order polynomials does not have tuning parameters. The inputs of this method are represented by the initial and final state of the vehicle, which are chosen according to the description from Section IV. The initial state is P y0 = [y 0 ,ẏ 0 ,ÿ 0 ] T = [0, 0, 0] T and the imposed final state is chosen to correspond to the lateral state of the vehicle which will be in front, The initial and final values of the lateral velocity and acceleration are considered zero because the vehicles have to maintain the lane, they do not have variations along lateral axis except, the time in which the vehicles change the lane. The target longitudinal position of each vehicle is chosen at 300m ahead. The parameters of the second method used to design the trajectory planner are the following: prediction horizon N p = 30 samples, weight of the lateral error λ yp = 13, weight of the lateral acceleration variation λ a yp = 300, and the sample time T sp = 0.1s. The parameters of the CACC controller, trajectory follower and trajectory planners were determined heuristically so that, for our particular proposed scenario, the output (velocity or distance), depending on the position of the vehicle in the platoon, reaches the reference values without overshoot and with the lowest settling time. The parameters of the vehicles are illustrated in Table 1.    Compared to the solution proposed in [43] the advantages of the solution proposed in this paper are represented by: i) the solution proposes controllers for both longitudinal and lateral dynamics; ii) the vehicles are equipped with a trajectory planner; iii) the CACC controller is designed and tested in the presence of data-packet-dropouts, iv) the proposed solution is tested for high velocity travel.
The velocities of the vehicles from platoons P1 and P2 are illustrated in Figs. 4, 5, and the results show that the leaders are moving at the imposed velocity, and the follower   vehicles also increase or decrease their velocity to follow the vehicle in front at the desired distance, see Figs. 6, 7, 8, 9. At the beginning, the velocity of vehicles is increased until it reaches a value of 22.22m/s. When the leader of the second lane, L 2 detects a fixed obstacle on its lane, it starts the merging procedure according to the description provided in the beginning of this section.
To ensure a safe merging, the two platoons are decreasing their travelling velocity to 15.27m/s and, after that, the  vehicles start to form triangles. The traction forces are illustrated in Figs. 10, 11, and are computed by the CACC for the vehicles to travel at the imposed velocity or to maintain a desired distance. The longitudinal forces are satisfying the imposed constraints, and the sudden variations which appear when the vehicles start to form triangles are due to the sudden variation of the values for the distance errors. At that moment, the vehicles start to maintain an imposed distance to a new vehicle.
Analysing Figs. 4-9 with the velocity of vehicles and distances between them, it can be affirmed that each vehicle succeeds to maintain a desired distance to its new vehicle in front, thus facilitating the next step, lane changing. The same performances of the CACC controller are obtained even when the communication between vehicles is disturbed by data-packetdropouts, see Figs. 12, 13 and Figs. 14, 15. In Figs. 12, 13 the results of 55 simulations and the vehicles velocity average are illustrated. The time moments at which data losses occur and the number of consecutive lost data-packets are illustrated in Figs. 16, 17. The data-packet-dropouts are generated randomly with a condition to not have more than 10 consecutive lost datapackets, and the percent of the lost messages to be close to 5%. When the vehicles formed the triangles, see Fig. 20 (a),    Figs. 20 (b-d), where the results of both methods, based on   polynomial equations and MPC algorithm are illustrated. The trajectory followers of Level III succeed to steer the vehicles so that these follow the imposed trajectories computed by the trajectory planers. Fig. 20 (e-f) illustrate with green the paths computed using the trajectory planing strategies described in Section IV and with red colour the paths of actual vehicles. The vehicles from platoon P2 succeed to change the lane, and to merge with the platoon P1, following the imposed path. The steering angle and yaw rate of each vehicle from P2 are illustrated in Figs. 18, 19, 21, 22. These angles satisfy the

B. ANALYSIS OF THE PERFORMANCES FOR THE LATERAL AND LONGITUDINAL DYNAMICS AND THE STRING STABILITY FOR THE PLATOONS
This subsection illustrates the performance of the lateral dynamics and points out the advantages and disadvantages of the two types of trajectory planners. Moreover, an analysis of the string stability and performances of the CACC algorithm is performed. For this use case, the communication topology used to exchange information between the vehicles in the platoon is T 1.
Analysing the vehicles' trajectories generated by the two methods proposed for path planning (see Fig. 20), it results that the method based on the polynomial equations generates a smoother path w.r.t. the MPC-based method. This relates to the maximum values reached for the lateral acceleration (i.e., 1.87m/s 2 -method A and 8.27m/s 2 -method B), the yaw angle rate (see Figs. 21,22) and the steering angle (see Figs. 18,19). The computational time required for method A is smaller than the one for the second method (TC A = 3.6e − 05s < TC B = 0.29s). However, it must be noted that the second method has less input variables when compared to method A, as described previously.
Further on, the string stability of the platoons is studied. This propriety refers to the platoon characteristic to attenuate the distance error or velocity along upstream direction [45], [57]:    where X i and X i−1 represent the distance error or velocity of vehicle i and of its predecessor, vehicle i − 1 and i ∈ (0, 1) represents a parameter. When the communication between vehicles is disturbed by the data-packet-dropouts, the study of the string stability is done using the average velocities of vehicles. Analysing the velocities, distances and distance errors of vehicles and their average values in the case of presence of data-packet-dropouts results that these states are not amplified along upstream direction and relation (41) is satisfied. Remark 3: If the state trajectories X i , X i−1 do not exhibit an overshoot (which is also our case) then, the maximum values of these states are equal and (41) is satisfied as equality condition with i = 1.
In Tables 2-5, the maximum, the minimum, the mean and the standard deviation of i are illustrated. These values are computed for sets of 50 simulations, with the following characteristics: two values for the headway time, t k ∈ {0.5s, 1.6s}, and two percentage values for the data-packet-dropout, p ∈ {5%, 10%}, resulting 4 case studies. From the numerical values, it results that i ∈ [0.999, 1], which proves that the platoon is string stable.
In Tables 6-9, the maximum, the mean, the standard deviations for the mean longitudinal accelerations and the maximum distance between vehicles are given. These results are also based on the above mentioned four case studies, and illustrate the fact that the acceleration of the vehicles is decreasing along the upstream direction of the platoon. The comfort of the passengers is influenced by the value of the headway time, which relates to the distances between the vehicles and their acceleration. A small headway time will result in a short inter-vehicle distance, i.e., reducing the platoon length on the highway. However, as previously mentioned, the high values of the longitudinal acceleration, because of the small headway time, will negatively influence the passengers' comfort.
Remark 4: To compute the numerical values from Tables 2-9 the entire simulation test was split into two parts:     i) part 1 -initial phase, in which the two platoons travel independently on two separate lanes, and part 2 -final phase, after the merging procedure is completed, obtaining a single platoon. In the latter, to test the performances for the newly formed platoon, a velocity reference change for the leader vehicle was also performed (i.e., at time 131 seconds, the   velocity is increased from 15.27m/s to 20m/s and at time 157 seconds the velocity is decreased from 20m/s to 15.27m/s).

C. COMPARISON OF THE PERFORMANCES USING DIFFERENT COMMUNICATION TOPOLOGIES IN CACC ALGORITHM
To analyse the performances obtained using the communication topologies T 1-T 4, 50 simulations were performed in presence of data-packets-dropouts for each topology with the following characteristics: the headway time, t k = 1.6s, and percentage value for the data-packet-dropout, p = 5%. The reference velocities of the leaders of the two platoons were imposed from 0m/s to 22.22ms/s and after decreased to 15.27m/s, as it is illustrated in the first 700 sample times in Figs. 12-13. The model used to simulate the longitudinal dynamics of platoons is the same for all topologies (2)- (6). The influences of the communication topologies were considered in the cost function, each case having a particular one, (29)- (32). The performances of the platoons were illustrated in Table 10 (maximum acceleration and standard deviation) and in Table 11 (mean squared velocity error). From these numerical results it can be observed that the smallest values for the maximum acceleration were obtained using topologies T 3 and T 4, which means that the passengers will have a more comfortable travel and the fuel consumption will be reduced. Moreover, from Table 10 it can be noticed that smallest velocity errors are obtained using the same topologies T 3 and T 4 in which the vehicles communicate with their neighbouring vehicles from front and rear. From the point of view of string stability, all topologies ensure it for the platoons, the velocity error being attenuated along upstream direction.
Remark 5: For topology T 1 and T 2 the controllers of the leader vehicles have to control only their velocity so, the stability is analysed from the first follower to the last one. In case of bidirectional topologies T 3 and T 4, because the leaders receive the prediction of the velocity of their first followers, they will use their controllers to control the velocities in order to reach the imposed values but, also, to minimise the velocity error between them and the vehicles behind them. This means that, the leader vehicles can decrease their velocity to ensure that the first followers are not too far away. Thus, the leaders will have also the role of maintaining a target distance to the first followers and for this reason, the string stability can be analysed starting from the leader vehicle for these two topologies T 3 and T 4.
Remark 6: The advantage of the first topology T 1 is related to its simplicity, requiring only the exchange of information from a certain vehicle to its follower, but with higher values w.r.t. to the analysed parameters (maximum longitudinal acceleration and mean squared velocity error). The last topology T 4 has as disadvantage an increased volume of exchanged information between the vehicles in the platoon, but the obtained performances are increased as detailed previously.

VII. CONCLUSION
This paper proposes a control architecture formed by three levels, which can be used by the vehicles driving in platoons to ensure a safe travelling. Level I is used to obtain information about traffic and to decide the future actions. Levels II and III receive commands from Level I and accomplish them. The second level is represented by a trajectory planner for which two methods were proposed: the first method is based on polynomial equations, and the second method is designed using a MPC strategy. Level III is represented by a CACC controller and a trajectory follower having the tasks to maintain a desired distance between vehicles, and to steer them to follow an imposed lateral trajectory. Moreover, the CACC controller was designed to consider different types of communication topologies, resulting in a framework that is beneficial to be used for various realistic scenarios. The proposed control architecture was tested in a simulation scenario in which two platoons had to merge to avoid the collision with a fixed obstacle. The results prove the efficiency of the proposed solution and the string stability of platoons. The main contribution of this paper with respect to similar works is given by the following characteristics: the solution was tested in the presence of the disturbed communications, the trajectory planner was implemented using two different approaches, the lateral dynamics of the vehicles was simulated using the nonlinear bicycle model, the solution combines both lateral and longitudinal dynamics control, and the CACC algorithm was tested for different communication topologies to illustrate their advantages and disadvantages.
Future work will focus on extending the proposed control architecture to ensure a safe travel for vehicles in presence of moving obstacles. He is the author/coauthor of about 100 publications in journals, books, conference proceedings, and participated in more than ten collaborative projects. His current research interests include model predictive control, networked control systems, automotive control systems, and cooperative driving.