Coordinated Formation Control for Intelligent and Connected Vehicles in Multiple Traffic Scenarios

In this paper, a unified multi-vehicle formation control framework for Intelligent and Connected Vehicles (ICVs) that can apply to multiple traffic scenarios is proposed. In the one-dimensional scenario, different formation geometries are analyzed and the interlaced structure is mathematically modelized to improve driving safety while making full use of the lane capacity. The assignment problem for vehicles and target positions is solved using Hungarian Algorithm to improve the flexibility of the method in multiple scenarios. In the two-dimensional scenario, an improved virtual platoon method is proposed to transfer the complex two-dimensional passing problem to the one-dimensional formation control problem based on the idea of rotation projection. Besides, the vehicle regrouping method is proposed to connect the two scenarios. Simulation results prove that the proposed multi-vehicle formation control framework can apply to multiple typical scenarios and have better performance than existing methods.


Introduction
With the development of V2X communication, coordinated control of ICVs is attracting great attention. Compared with conventional vehicle control, coordinated control of multiple vehicles focuses on the payoff of group of vehicles instead of single vehicle.
There have been numerous research on the topic of vehicular coordinated control. Traffic scenarios for vehicular coordinated control can be categorized into one-dimensional and two-dimensional methods. In this paper, the one-dimensional scenario is defined as multi-lane scenarios where vehicles drive with the same direction, where the direction mainly indicates macroscopic direction in the road coordinate, although individual vehicles may have different orientation when they are changing lanes or driving on a curve. Typical one-dimensional scenarios include straight or curve multi-lane road segment. Intersections and on/off-ramps are typical two-dimensional scenarios.
Existing research regards the coordinated control of ICVs at one-dimensional scenarios as typical examples of convoy control or formation control. On one hand, the existing research of multi-vehicle formation control can be macroscopically divided into structuralized-road scenarios, [1][2][3][4][5][6][7][8][9][10][11], and unstructuralized-road scenarios, [12,13]. For the structuralized-road scenario, a group of vehicles within a given range will form a formation and drive coordinately. The geometric shape of the formation is built under a certain rule, and the shape changes when the number of the lanes changes, [2,5,6], or when there are some obstacles on the road, [1,4]. Formation adjusting functions when vehicles join or leave the convoy are discussed in [4] and [9]. And the coordinated lane change behavior in a built formation is studied in [3]. The methods to cooperatively control ICVs together with human-driving vehicles are studied in [10] and [11]. Other research about the coordinated behavior of multiple ICVs can be seen in [7][8][9]14]. Besides the research above which considers the multi-vehicle formation problem from the overall perspective, there is also some research which considers this problem from single-vehicle perspective, [15]. On the other hand, compared with vehicular formation control which has complex kinematic and dynamic constraints, there are much more research on robotic formation control which has simpler constraints. The robotic formation control, also known as swarm control, can be generally studied in the following three application areas: the autonomous grounded vehicles (AGV), [16][17][18], the unmanned aerial vehicles (UAV), [19,20], and the autonomous underwater vehicles (AUV), [21,22].
The method proposed in this paper is inspired by the robotic control methods.
The coordinated control methods of ICVs in two-dimensional scenarios concentrate on intersections and ramps. As discussed in [23], the traffic signal at intersections may bring the green time loss, which results in inefficiency for intersection management. Therefore, coordinated control methods of ICVs at unsignalized intersections is a potential solution for improving traffic efficiency. As a classical method which solves vehicle merging problem at on-ramps, the virtual platoon method is also an adoptable method for unsignalized intersections, [23,24]. By projecting vehicles on different legs of the intersection to a virtual platoon, the two-dimensional vehicle merging problem is transformed into one-dimensional platoon control problem. Besides of the virtual platoon method, another well-known method for the unsignalized intersections is the reservation method, [25][26][27], where vehicles adjust their speed and pass the intersections without collision according to the "First Come, First Served" policy. Other methods developed for the unsignalized intersections can be found in [28], [29] and [30] .
Here the shortcomings of the existing methods are summarized: • The existing methods for different scenarios are often separated and lack connectivity and generality. There are only a few recent works that have taken the problem of multi-scenarios into account or have proposed unified decision and control methods.
• The scenarios are often over simplified for vehicular coordinated control. For example, the classic virtual platoon method can only be applied to single-lane ramp merging scenario.
• Some methods have only focused on single vehicle and analyzed its payoff instead of vehicle formations' or sub-groups', which could not fully utilize the advantage of vehicular coordinated control.
• The desired assignment of the vehicles in a formation is often set beforehand and lacks principle explanation, which makes it hardly to be applied to the real world. Besides, the choosing of formation geometry is kind of arbitrary and lacks reasonable analysis among the existing research.
• Although there are some preliminary researches of robotic formation control, they can be hardly directly applied to vehicular formation control because of the lack of kinematic constraints.
This paper studies the problem of coordinated control of ICVs in multiple traffic scenarios. In the one-dimensional scenario, a multi-lane formation control method is proposed to form vehicular formations. In the two-dimensional scenario, an improved virtual platoon method is proposed to calculate conflict-free passing sequence of vehicles at multi-lane unsignalized intersections. The vehicle regrouping method is proposed to divide the vehicular formations into sub-groups that can pass the intersection ordered and with high efficiency.
The rest of this paper is organized as follows. Section 2 sets up the scenarios studied in this paper and provide vehicle controllers. Section 3 presents the formation control method for the one-dimensional scenario. Section 4 presents the improved virtual platoon method for the two-dimensional scenario. The vehicle regrouping method is proposed to connect the two scenarios in Section 5. Simulation experiments and results are given in Section 6. At last, this paper is concluded in Section 7.

System modeling
This paper considers a traffic system with multiple ICVs and multiple traffic scenarios. All the vehicles in this system are ICVs, and are both controllable and observable. The state feed-back of all the vehicles is supposed to be available and the quality of communication is supposed to be perfect. The model, parameters and controller of typical traffic scenarios and vehicles studied in this paper are discussed in this section.

Traffic scenarios
As defined in Section 1, this paper studies coordinated control methods for one-dimensional and two-dimensional traffic scenarios. The methods to be proposed in this paper are designed for multiple scenarios, but in order to detailedly explain our methods and algorithms, the multi-lane highway system with on-ramps and off-ramps and the multi-lane unsignalized intersection are chosen to be the basic scenarios of this paper. An isolated highway segment with an onramp and an off-ramp is shown in Fig. 1(a). Vehicles come from the on-ramp or the last segment and drive on the main road. The onramp and off-ramp are located on the rightside of the main road. More complex highway systems can be considered as combination of several simple road segments. The unsignalized intersection studied in this paper is shown in Fig. 1(b). In this paper, for simplicity, a symmetric multi-lane intersection which have 6 lanes of each leg, 3 approaching-lanes and 3 departure-lanes, is considered. Each approaching-lane consists of one turning-left lane on the left, one going-straight and turning-right lane in the middle and one turning-right lane on the right. Vehicles from different directions of the intersection can turn left, go straight and turn right, forming 12 potential traffic flow movements at this intersection. The traffic flow movements are numbered with 1-12. This paper aims to provide a coordinated control method which is not sensitive with scenarios. However, it's obvious that when the lane assignment of the intersection changes, the specific implementation process of the method also changes. Thus, the typical intersection scenario provided in Fig. 1(b) is chosen for this paper to clearly explain the proposed method.

Vehicle model, parameters and controller
In order to simplify computation, the bicycle model is used in this paper, as is shown in Fig. 2. The vehicle parameters used in this paper are listed in Table 1 where v represents the longitudinal speed, a represents the acceleration and φ represents the steering angle.
The state of the vehicle contains X, Y , θ and φ, where X and Y represent the horizontal and longitudinal position of the center of (a) One-dimensional highway segment (b) Two-dimensional typical intersection   vehicle's rear axle. The inputs of the vehicle model are a and φ, and the state is calculated as: In this paper, the control strategy of vehicles fuses the control inputs of both formation maintaining and desired speed keeping. The longitudinal controller is designed to form and maintain the generated formation and keep a desired speed simultaneously. When the vehicle is driving on the straight road, the forward direction is chosen as the positive direction of X-axis and the positive direction of Y-axis pointe to the left of the vehicle. The longitudinal feedback control law of vehicles is given as: where a i is the control input of vehicle i and k pi and k vi are feedback gains. The lateral controller is designed to perform lane-keeping and lane-changing manoeuvre. 3-order Bézier curve is used for trajectory generating, and the preview method is chosen to guide vehicle to follow the trajectory.

Formation control in one-dimensional scenarios
This section introduces the multi-lane formation control method under the one-dimensional scenario. As is previously discussed in [31], the one-dimensional formation control problem is solved in the following three steps: generating of target positions, assigning vehicles to target positions and target-following control of vehicles.

Geometric structure of multi-lane formation
First of all, the standard formation geometry of vehicle formations on the multi-lane straight road is defined.
Two typical types of formation geometries, the interlaced formation and the parallel formation, are commonly chosen in the existing research. As is shown in Fig. 3, although the interlaced formation reduces the vehicle density, it provides great convenience for lane changing and formation adjusting. Although the parallel formation has higher vehicle density, the flexibility of formation adjusting and the safety of coordinated driving are not as good as the interlaced formation. This paper mainly focuses on the real-time formation generating and adjusting, and the adjusting flexibility of formation is highly required when the number of lanes or number of vehicles changes. Thus, the interlaced formation geometry is adopted for multi-lane formation. The details of the interlaced formation can be found in [1], [2], [4], [5] and [9]. The parallel formation is chosen in [6].
In order to make the formation repeatable and the positions of targets inside the formation descriptive, targets are divided into different layers based on their longitudinal position. In each layer, there is at most one target in each lane, so the maximum number of targets equals to the number of lanes. The targets in the same layer are divided into two sublayers and targets in the same sublayer keep the same longitudinal position and the lanes they occupy are not adjacent. The two sublayers in the same layer keep a longitudinal distance of d and the two adjacent layer also keep a longitudinal distance of d. The layered structure of multi-lane formation is shown in Fig. 4.

Target generating rules for targets in multi-lane formation
This subsection provides the formation generating rule to generate targets in the multi-lane formation in real time. Suppose there are N l lanes, and their ID are numbered 1 to N l from left to right. The coordinate (xg, yg) is used to define the relative position of a target in a formation where xg represents the reference longitudinal distance between this target and the most forward target and yg represents the lane ID where the target locates. The i-th target position   in the formation can be calculated based on the following rules: where Ls represents the longitudinal distance between two adjacent sublayers. The distance between two sublayers in one layer is defined the same as the distance between two adjacent layers. The two polynomial functions P 1 (p) and P 2 (p) are defined as: The function mod(p, q) returns the remainder of q divided by p. The function ceil(p) returns the minimum integer no less than p. The comparing function com(p, q) is defined as: Take the first target position locating at the most left-forward of the formation as an example. Its coordinate can be calculated by (3) and it is (1, 0). The number of target positions equals to the number of vehicles. When the road has 5 lanes and there are different numbers of vehicles on it, the target positions can be calculated and shown in Fig. 4.

Position assignment
Vehicles are assigned to the generated target positions in this step. There are N ! different assignment results when the number of vehicles is N . By defining function F (i, j) to represent the cost for vehicle i to be assigned to target j, the total cost of every assignment result can be calculated, and F (i, j) is called the cost function. Accordingly, the cost matrix is defined as: The assignment matrix A is defined to represent the result of any specific assignment: And the assignment problem can be modeled as a 0-1 integer programming problem: where [c ij ] is a given cost matrix and [a ij ] is the variable assignment matrix.

Fig. 5:
Example result of Hungarian algorithm using Euclidean distance as the assigning cost between vehicles and targets. The white rectangles represent the targets and the yellow dashed lines represent the assignment relationship between vehicles and targets.
In this research, in order to consider lateral lane change safety and longitudinal following performance, the cost function Fw(i, j) is constructed as a weighted summation of longitudinal distance and lateral lane change numbers. The lane change number is chosen as part of the cost function to represent lateral movement of the vehicle because the target positions are always located on the center of the lanes. The cost function Fw(i, j) has the following structure: where xv and yv represent the longitudinal position and lane ID of a vehicle. By choosing different weight w 1 and w 2 , the cost function F (i, j) changes its preference of longitudinal distance or lateral distance between vehicles and target positions. For example, when w 2 is set as the width of the lane and w 1 is set 1, then Fw(i, j) represents the square of Euclidean distance. Complex parameter tuning methods are not adopted because we focus on the assignment framework designing, and different tunning methods can be chosen for different application scenarios. Among all the possible assignment results, the one with the minimum total cost is chosen as the best assignment. The Hungarian algorithm, [32,33], is chosen to solve the assignment problem for its low time complexity (O(N 3 )). Other research which uses the Hungarian method for assignment problem includes [18]. An example result of using Euclidean distance as the cost is shown in Fig.  5.

Vehicle control
After vehicles are assigned to target positions, trajectories are generated to guide vehicles towards their expected target positions. In this paper, the 3-order Bézier curve is chosen to be the candidate trajectory of lane change. The coordinate of the target position is used as inputs of the vehicle afterwards. For more detailed information about using Bézier curves to perform trajectory planning, please turn to [34].
Trajectory following and collision avoidance are also studied in this research, but this paper focuses on the upper-level decision making and control. For more information about the lower-level controller and hardware architecture, please turn to our previous conference paper [35].
In this paper, the obstacle avoidance problem is considered as: if the obstacle does not occupy enough space to block a lane for vehicles to drive, the formation doesn't change and the vehicle just need adjust its trajectory within the lane; if the obstacle causes the lane it occupied undrivable, this case will be the same as the case in which the number of lanes changes, and can be treat using the same method. Besides, it is easy to extend the straight-road method to the curve roads and this paper focuses on the adjusting functions of the formation in the straight-road scenario.

Virtual platoon method in two-dimensional scenarios
The two-dimensional scenarios include on-ramps, off-ramps, roundabouts and intersections. The two-dimensional vehicular formation control method proposed in this paper is scenario-insensitive and can be applied to all the mentioned scenarios. In order to make it easier for readers to understand the method, this paper focuses on the unsignalized multi-lane intersections. This section introduces the virtual platoon method used for unsignalized single-lane intersections and then apply it to the multi-lane scenarios.

Traditional virtual platoon method
The virtual platooning method is a well-known method to handle the two-dimensional traffic scenarios. Typical applications are on-ramp merging control and unsignalized intersection control. The main idea of virtual platooning method is to project vehicles on different lanes to a virtual lane according to the distance between vehicles and the center of the intersection. Then the single-lane platooning control method can be applied to the vehicles on the virtual lane. Thus, vehicles form a platoon without longitudinal conflict so that they can pass the intersection smoothly without conflict. Other research using virtual platoon method can be seen in [24].

Improved virtual platoon method
In the multi-lane intersections, vehicles are treat with a similar idea of the single-lane intersections. In the single-lane scenario, each single vehicle is treated as a virtual vehicle in the virtual platoon. However, in the multi-lane scenario, a group of vehicles with the same target direction (turning left, going straight or turning right) are treated as a node in the virtual platoon. If these nodes are put in a single-lane platoon, they can pass the intersection without braking. In order to improve traffic efficiency, different nodes (vehicle group) whose target directions are not in conflict can pass the intersection simultaneously. We first proposed this idea in [23]. In order to get a tighter virtual platoon, conflict model of the traffic flow movements is built.
There are three types of conflict in an intersection: converging, diverging and crossing, as shown in Fig. 6. The converging conflict happens when two kinds of traffic flow merge into the same lane. The diverging conflict happens when two kinds of traffic flow come from the same lane. It is important to notice that two vehicles coming from the same direction are always in conflict in the single-lane intersections, but not always in the multi-lane intersections because they may come from different lanes. The crossing conflict happens when the trajectories of two kinds of traffic flow cross in the intersection. The two turning-left traffic flow from the opposite direction are defined not in conflict, which can be achieved by special designing of intersection structure.
Compared with setting one vehicle group as a node in the virtual platoon, the traffic efficiency can be further improved by putting some nodes in the same tree-layer of the virtual platoon. The conflict relationship of traffic flow of a defined intersection is closely related to its lane assignment. The unsignalized multi-lane intersection studied in this paper is already shown in Fig. 1(b).

Remark 1.
In order to distinguish from the definition given in Section.3, the layers of the depth-first spanning tree are named as tree-layers. According to our previous work, two zones are necessarily defined: approaching zone and coordinating zone. Additionally, the adjusting zone is specially defined for multi-lane intersections. In the approaching zone, vehicles follow the multi-lane formation control method to keep a stable group. In the adjusting zone, vehicles perform coordinated lane change to drive on their expecting lanes and re-form smaller formations. In the coordinating zone, the formations in which vehicles have the same destination are taken as a node and use the virtual platoon control method to guide them to pass the intersection. Sets Sapp, S adj and Scor are defined to represent sets containing vehicles in the approaching zone, adjusting zone and coordinating zone respectively. R 1 and R 2 are defined to be the radius of the boundary circle of the coordinating zone and the adjusting zone. Given a vehicle veh i which is approaching the intersection and its distance to the center of the intersection d i , the state of the vehicle is decided according to the following rules: Because of the specific structure of the multi-lane intersection, the conflict relationship is different with the single-lane intersections. For example, the turning-left traffic flow and the going-straight traffic flow are in conflict in the single-lane intersection, but in the multi-lane intersection they are not in conflict because they use different lanes. Thus, a conflict relationship model is built for this specific multi-lane intersection, as shown in Fig. 7(a) ,where the numbers in the blue rectangles represent the traffic flow movements which have been defined in Fig. 1(b), dashed lines represent the converging relationship, solid lines refer to the cross relationship, and the hollow rectangles denote the diverging relationship. Because the conflict model differs, the results of depth-first spanning trees are also different for single-lane and multi-lane intersections. Fig. 8: Virtual platoon formed by vehicle groups according to the distance towards the center of the intersection. The numbers in the circles mean the order of these groups based on their distance towards the center of the intersection. The number in the rectangles represent their traffic flow movements, which corresponds to Fig. 1(b). Given an example of virtual platoon where the order of vehicle groups is calculated according to the distance between the first vehicle in the group and the center of the intersection, shown in Fig. 8. The No. 0 vehicle is a virtual leading vehicle used for generating control input for the vehicle group that is the closest to the center of the intersection. The directed graph is constructed, shown in Fig.  7(b), to show the conflict relationship of the real and virtual vehicle groups. In Fig. 7(b), numbers in circles represent vehicle groups given in Fig. 8. The solid lines show the conflict relationship between vehicle groups based on Fig. 7(a). Accordingly, the depth-first spanning tree is calculated based on the improved depth-first searching algorithm provided in [23], shown in Fig. 7(c). The spanning tree shows the order of vehicle groups to pass the intersection.
The traffic flow movement of the virtual leading vehicle in the virtual platoon is specially defined and in conflict with all the 12 traffic flow movements. In fact, the virtual vehicle conflict with all the other vehicle groups, but Fig. 7(b) omits some of the edges to make itself clearer and easier for understanding.
The vehicle groups given in Fig. 8 and the depth-first spanning tree given in Fig. 7(c) are combined and the passing sequence of the vehicle groups is constructed as shown in Fig. 9. Vehicle groups that are in the same tree-layer can pass the intersection simultaneously and groups in adjacent tree-layers keep a safe following distance and pass the intersection without collision.
The longitudinal control law of vehicles are given in (2) and the reference position is given as: where X k ref,i refers to the reference longitudinal position of the ith vehicle in formation k, X k 1 refers to the first and also the most forward vehicle in formation k, and x gi is defined in (3).
Equation (12) shows that vehicles in a formation follows the most forward vehicle's motion. In order to calculate control input for the most forward vehicle in a formation, the formation set is defined, which is represented as T i . T i is a set that contains the ID of formations that are in tree-layer i. Take the spanning tree shown in Fig. 9 as an example, the first formation set T 1 would be {1, 2, 5, 10}, and T 2 would be {3, 4, 8}.
The reference position of the most forward vehicle in a formation is given as: where X k ref,1 refers to the reference position of the most forward vehicle in formation k, X 0 refers to the position of the virtual leading vehicle, m is the tree-layer that formation k locates, D t refers to the ideal following gap between two adjacent formations in the virtual platoon, x n gj refers to the position of the jth vehicle in the nth formation. When given a certain n, x n gj is equal to x gj . The position of the virtual leading vehicle is given as: which means that the position of the virtual vehicle is defined based on the position of the No.1 vehicle group. Remark 2. In this section, the origin of coordinates is set at the center of the intersection so that the vehicle closer to the center of the intersection has the smaller longitudinal coordinate.

Vehicle regrouping method
In Section.3 and Section.4 the multi-lane formation control method for ICVs at one-dimensional straight road and two-dimensional unsignalized intersections are proposed respectively. As is talked in Section.4, vehicles whose target direction is the same are considered as a group. In this section, the vehicle regrouping method is proposed to divide the original formation into subformations in which vehicles have the same turning expectation, e.g., turning left or going straight. The detailed process of the vehicle regrouping method can be seen in Fig. 10. The process can be divided into the following steps: After this process, the subformations would drive on their expected lanes but vehicles inside subformations may still need to change lanes.
• Formation adjusting. Vehicles adjust their longitudinal and lateral position to form tighter subformations when coordinated lane change has finished. It has been mentioned before that the interlaced geometry is chosen for straight road formation to provided convenience for lane change and formation adjusting. In order to keep the geometric structure of the formation to better connect with the next multi-lane road segment, the interlaced formation structure is adopted.
• Formation merging. If there is no other conflicting vehicles between two subformations and they are within a desired merging range, they merge to form a new subformation.
The vehicles finish the five steps in the adjusting zone. Then the formations enter the coordinating zone. In this zone, each subformation is considered as a virtual vehicle and will pass the intersection continuously. Then the virtual platoon method can be applied to solve this unsignalized multi-lane intersection problem.

Simulation and results
Simulation experiments in one-dimensional and two-dimensional scenarios are carried out respectively to verify the proposed methods' functions and compare them with reference methods. Simulation environments are built using MATLAB 2018b and SUMO 0.32. Vehicle parameters can be seen in Section.2.

One-dimensional formation control
As is provided in Section.3, the proposed method enables vehicular formation to adjust their geometric shape automatically in the dynamical traffic environment. At first, the proposed method is implemented in the simulation environment to verify its capability, and then the lane assignment method proposed in [8] is chosen to compare the performance in a highway scenario with on-ramps and off-ramps. The parameters of this part of simulation is shown in Table.2. The basic idea of the optimal lane assignment method is to assign vehicles to the calculated optimal lanes of the main road at the onramps and off-ramps. The goal is to minimize the total traveling time of all the vehicles at each optimizing step. It models the multiple lane assignment problem into a linear programming problem. By taking the percentage of vehicles to be assigned to each lane as independent variable, while maintaining the number of vehicles in a certain lane less than the biggest lane capacity, the authors solve the linear programming problem and get the best result of lane assignment.
First of all, simulation under simple traffic environment is carried out. The results of implementation of the proposed method are shown in Fig. 11, where four shots of each scenario were selected. In Fig. 11, the red rectangles represent vehicles and the black solid and dashed lines represent lanes. There are five lanes in total. The left four lanes are common roads and the right most one is a ramp. The magenta circle on the top means that the lane is drivable.
In order to compare the proposed method with the reference method, a highway simulation environment with four regular lanes and one lane on the right to represent on-ramps and off-ramps is built. The total length of the highway is 10000 m with five on-ramps and five off-ramps. On-ramps are set every 2000 m and the first onramp is at the start of the highway. Off-ramps are set also every 2000 m and the first off-ramps is set at 1600 m after the first onramp. The repeatable basic segment of the simulation environment is shown in Fig. 1(a). In order to maintain the smoothness of traffic flow, the start of the 10000-meter road is connected with its end. All vehicles are generated at the five on-ramps randomly. Once a new vehicle is generated, it chooses its exit among the five off-ramps randomly. It should be noted that when using the proposed method in the simulation, the fourth lane, which is a transition lane, is open for vehicles only those who are going to leave or join a formation, so the formation is built on the left three lanes. Remark 3. In the simulation, the fourth lane is set as a transition lane at the on-ramps and the off-ramps. This is because the vehicles that have just merged into the main road from on-ramps or are going to leave the main road at the next off-ramp usually have lower speed than others. In order to avoid collision and keep high traffic efficiency, the vehicular formations are built only on the left lanes at the ramp areas.
The vehicle generating rate (VGR) is changed at on-ramps to watch the changing trend of the number of vehicles remaining on the road. It can be found that using the proposed method, which can fully use the capacity of the lanes, can significantly reduce the number of remaining vehicles, which is about 30% less at the same VGR compared with the reference method. It indicates that the proposed method can reduce the load of highways. The result of the proposed method is shown in Fig. 12(a) and the result of the reference method is shown in Fig. 12(b).
In order to show the distribution of vehicles during the simulation, the window is fixed at a road segment around an off-ramp. Fig. 13(a) shows the distribution of vehicles around an off-ramp at the simulation time around 200 s, 300 s, 400 s and 500 s respectively when using the proposed method. Correspondingly, Fig. 13(b) shows the distribution of vehicles of the reference method. Besides, the heat maps are used to show the distribution of vehicles at simulation time 600 s and at the VGR 1/3 vehicle/s inFig. 13(a) and Fig. 13(b). They are constructed to show the density of vehicles. The number of vehicles remaining on the main road is counted every 500 m. The brighter the color is, the more crowded the vehicle is. The darker the color is, the sparser the vehicle is. It can be seen from the heatmap that the main difference between the two methods is that the proposed method makes better use of capacity of all the lanes while the reference method may cause some congestion in some certain lanes.
The average speed and speed fluctuation of all the vehicles are calculated and the results are shown in Fig. 14. It can be found in Fig.  14(a) and Fig. 14(b) that using the proposed method can achieve a 30% higher average speed and a 60% lower average speed fluctuation compared with the result of the reference method. It indicates that the proposed method can do better in traffic efficiency and significantly reduce frequent accelerating and decelerating to achieve better fuel economy.

Two-dimensional unsignalized intersection control
The simulation environment for the two-dimensional unsignalized intersection control is built with the same lane assignment and lane numbers as is shown in Fig. 7. The Vehicle regrouping method   is applied in the adjusting zone to form subgroups and perform coordinated lane change to guide vehicles to their expected lanes. The multi-lane virtual platoon method is applied in the cooperating zone taking the subgroups formed by the Vehicle regrouping method as nodes in the spanning tree to calculate conflict-free passing sequence. The parameters of this part of simulation is shown in Table.3. Vehicles are generated at the entrance of four legs. The generating rate of vehicles are set to control the throughput of the intersection. Vehicles form multi-lane interlaced formation in the approaching zone. When the first vehicle of a formation enters the adjusting zone, the formation then performs regrouping process. Vehicles finish lane change in the adjusting zone and form tighter subformations in their expected lanes. The process of regrouping is shown in Fig.  15. Because no lane changes need to be done after vehicles entering the coordinating zone, vehicles drive with smaller following distance. When subgroups enter the coordinating zone, they are taken as nodes of the virtual platoon. Conflict-free passing sequence is then calculated using the depth-first spanning searching algorithm.
The reservation-based method and the classic virtual platoon method are chosen as reference methods. In the reservation method, vehicles pass the intersection one by one according to the "First Come First Serve" principle. In the classic virtual platoon method, each single vehicle is taken as a node in the spanning tree. The snapshots of the simulation using the above three methods are shown in Fig. 16. Fig. 16 indicates that compared with the reservation-based method, the virtual platoon method (either multi-vehicle or singlevehicle) can achieve a higher vehicle density near the intersection because vehicles that are not in conflict can pass the intersection simultaneously. Besides, compared with the single-vehicle platoon method, the proposed multi-vehicle platoon method can further increase vehicle density because vehicles with the same target direction can drive with a closer distance in a formation. Average speed and average speed fluctuation are chosen to evaluate traffic efficiency and fuel economy respectively. The vehicle generating rate is changed for different traffic throughout to get different performance. The results are shown in Fig. 17.
It can be seen from Fig. 17(a) that at low traffic throughput (lower than 2000 vehicle/h), both the multi-vehicle and single-vehicle virtual platoon method maintain an average speed which is close to the expected speed (10 m/s). When the traffic throughput becomes higher, the average speed goes down and the multi-vehicle method always exceeds the single-vehicle method. The average speed of the reservation-based method also goes down when the traffic throughput becomes higher, and is always lower than the two virtual platoon methods. This indicates that the virtual platoon method can improve traffic efficiency compared with the reservation-based method and the multi-vehicle method can achieve a better result than the single-vehicle method.
The result of average speed fluctuation is shown in Fig. 17(b). It can be seen that the reservation-based method always keeps a low fluctuation, because when using this method, vehicles just have to decelerate or brake until they reserve successfully for their passing time so that they don't need to frequently accelerate or decelerate. The speed fluctuation of the multi-vehicle virtual platoon method is always lower than the single-vehicle method which indicates that the multi-vehicle method can achieve better fuel economy than the single-vehicle method. Although the reservation-based method keeps the lower speed fluctuation at higher traffic throughput, it is clear that compared with the loss in fuel economy, the improvement in traffic efficiency is much more remarkable. It's interesting to find that the speed fluctuation of the single-vehicle method goes down at the highest traffic throughput. This is because that at such a high traffic throughput, many vehicles brake and stop for a long time. The speed of these stopping vehicles don't change so that the statistical average speed fluctuation becomes lower.

Conclusion
In this paper, a unified multi-vehicle formation control framework is proposed for Intelligent and Connected Vehicles (ICVs). In the framework, groups of vehicles drive as formations with flexible geomerty to improve driving safety and traffic efficiency. Vehicles may join, leave or perform coordinated behavior with other vehicles in a formation. The framework is scenario-insensitive and can be a general solution for the vehicle control problem in the intelligent transportation system. Firstly, the vehicular formation is built on multi-lane roads. Different formation geometries are analysised and the interlaced structure is mathematically modelized to improve driving safety while making full use of the lane capacity. The cost function is specially designed and the assignment problem for vehicles and target positions is modelized and solved using Hungarian Algorithm to improve the flexibility of the method. Bézier curves are adopted to guide vehicles to their assigned target positions.
Secondly, the scenario is extended to multi-lane unsignalized intersections. Conflict relationship between vehicular formations is studied and modelized. Conflict-free passing sequence is calculated using the depth-first spanning algorithm. Vehicle controller is designed for vehicles based on their relative positions in the spanning tree. Then, the vehicle regrouping method is proposed to connect the one-dimensioanl and two-dimensional scenarios. Formations driving on the multi-lane raods are divided into subformations according to their target directions and expected lanes. The subformations are then taken as the input for the conflict-free passing method in the intersections.
Finally, simulations are carried out to validate the performance of the proposed framework. The results prove that the proposed multi-vehicle formation control framework can cover both onedimensional and two-dimensional scenarios. Besides, compared with existing methods, the proposed framework has better performance in traffic efficiency and fuel economy while maintaining vehicles driving safely and in order.