Formation Control for Connected and Autonomous Vehicles Based on Distributed Consensus Embedded With Risk Potential Field

Cooperative control for multiple vehicles is a promising technology with the capability to improve traffic efficiency and fuel savings. Given its potential for both commercial and military applications, multiple unmanned vehicle formation has attracted considerable attention recently. In this paper, the use of formation control for connected and autonomous vehicles was explored and a novel distributed formation control approach was proposed. To begin, the evolution mechanism of multi-lane formation was investigated and a formation transition model based on a finite state machine was constructed. A bi-level formation control scheme was then proposed; this framework’s upper and lower levels were used to perform trajectory planning and MPC-based control, respectively. A novel trajectory planning approach was constructed by combining the distributed consensus algorithm and the potential field method. Moreover, additional acceleration constraints were imposed on the trajectory planning algorithm. Finally, three scenarios were designed to validate the proposed formation control algorithm using Webots. The results illustrate that a formation deployed with the proposed formation control algorithm can handle abnormal situations and realize consensus within 12s.

SECTION I. Introduction

California’s Partners for Advanced Transit and Highways (PATH) project has demonstrated that cooperative control of multi-vehicle system can enhance road safety, improve traffic throughput, and reduce fuel consumption to benefit the environment [1], [2].

Vehicle platooning is a typical multi-vehicle cooperative driving system where a group of vehicles drive in a single-lane at a desired speed while maintaining a short inter-vehicle distance [3], [4]. Vehicle-to-everything (V2X) technology enables information sharing between infrastructure and vehicles. A distributed controller is implemented in the vehicle using its neighbors’ information but achieving global coordination. In general, linear controllers [5], [6], optimal controllers [7], $\mathcal {H}_{\infty }$ controllers [8], [9], [10], sliding mode controllers (SMC) [11], [12], [13], or model predictive controllers (MPC) [14], [15], [16] may be used for the distributed control of vehicle platoons.

Majority studies focus on single-lane platoons which only considers the longitudinal platooning using spacing control methods. a nonlinear consensus drives them is proposed by incorporating the car-following interactions and heterogeneous time delays.

However, if the formation geometry or information flow topology is not properly designed, any error in the leader vehicle’s spacing or velocity will amplify as they propagate down the platoon; these problems are referred to as string instability issues [17]. In addition, as the number of vehicles in the platoon increases, packet loss and communication delay have a nontrivial influence on the robustness of the multi-vehicle system. Furthermore, the lead vehicle in the platoon influences the behaviour of each of the others; thus, problems with the lead vehicle may render the platoon unstable.

To improve the scalability and stability of multi-vehicle systems, Multi-lane platoon formations were proposed to improve the scalability and stability of multi-vehicle systems. Such frameworks have attracted significant attention in recent years. Multi-vehicle formation schemes have built upon the research conducted for multi-robot formulation that began in the 1980s [18]. The objective of formation control is to command a group of of autonomous vehicles to achieve a set of deployment requirements whilst maintaining a desired formation [19].

Formation control approaches are commonly divided into two distinct categories: centralized control and decentralized, or distributed control. In a centralized control scheme, one agent designated as the ground control unit, is responsible for utilizing a centralized organization structure to optimize the vehicle coordination. As for decentralized control, each agent can accomplish its part of the global mission based on local information and decentralized control law. Centralized control is less robust and more prone to failure in comparison to decentralized control [20].

Common formation control strategies include leader-follwer, virtual structure, behavior-based, potential function-based, and graph-based methods [21], [22], [23], [24], [25]. Fig. 1 displays schematic diagrams of the five formation control strategies. Within the leader-follower context, the followers deploy local control laws to achieve the desired gap with respect to the leader. Tao and Shan proposed four control laws to achieve leader-follower formation based on the straightforward input-output linearization method [26]. Xiao et al. implemented nonlinear MPC for leader-follower formation control. By virtue of the neural network, the computational complexity associated with MPC can be reduced [27].

FIGURE 1.

Formation control strategy.

The virtual structure approach is another essential formation control method proposed by Lewis and Tan [22]. Within this context, a collection of formation vehicles maintain rigid geometric relations to their neighbors to form a rigid entity. Ghomman et al. proposed an algorithm combining virtual structure and path following approaches to coordinate multiple mobile robots [28].

Balch and Arkin first proposed behavior-based formation control [29]. The central concept of the behavior-based approach is that the formation behavior is integrated with other navigation behaviors to enable a robotic team to reach the navigation goal, avoid obstacles and maintain in formation simultaneously. Lee and Chwa presented a decentralized behavior-based formation control algorithm using the relative position information between neighboring robots and obstacles [23].

The key to the potential function-based approach was to construct possible functions to define the interaction forces between the formation agents. Leonard and Fiorelli proposed a framework for coordinated control of multiple UGVs using artificial potentials and virtual leaders. The interacted control force between neighboring agents is defined by the artificial potentials to maintain the separation distances between the agents [30]. Liu et al. proposed a novel potential field method for formation control, where a global attractive potential field is added outside the influence range of the local formation potential field to enhance the formation robustness. Furthermore, two controllers were designed to achieve the formation stability and ensure the tracking of desired trajectories [31].

In the graph-based approach, formation is abstracted to a graph, where the formation vehicle is described as vertex, and the edge represents the information flow from one vehicle to another. Gao et al. proposed a multi-lane convoy control algorithm based on the distributed graph and the potential field approach [32]. Marjovi et al. proposed an approach for formation control of highway multi-lane convoy in highways based on graph-based Laplacian and distributed control law [33]. Navarro et al. investigated the heterogeneous convoys based on distributed, graph-based control law in a longitudinal coordinate system [24]. Gowal et al. proposed a local graph-based distributed control method for keeping a predefined formation of highway vehicle endowed with information of range and bearing to other vehicles [34].

Nevertheless, each of the four aforementioned approaches are hindered by certain drawbacks. The main demerit of the leader-follower approach is that it is less tolerant to component failure; that is, malfunctions in the leader may contribute to the formation failure. The virtual structure strategy provides significant performance in terms of formation maintenance as it is easy to maintain rigid geometric relationships among formation agent. However, it is not beneficial for formation reconfiguration. In addition, the inflexibility in the formation shape regeneration eventually jeopardize the ensemble’s stability. Compared with the virtual structure approach, the behavior-based approach shares advantages of the decentralized control. However, it is difficult to analyze the robustness and stability mathematically. As for the graph-based approach, it is easy to achieve convergence and internal formation stability. Yet, to the best of our knowledge, few studies simultaneously take into account the agent dynamic constraints and collision avoidance issues.

In this paper, multi-lane formation, and explored multi-lane formation control for connected and autonomous vehicles was explored. First, the evolution mechanism of multi-lane formation was investigated, and a formation transition model based on finite state machine was built. A bi-level formation control scheme was then proposed; this framework’s upper and lower levels are used to perform trajectory planning and MPC-based control, respectively. By combining the distributed consensus algorithm and potential field method, a novel trajectory planning approach was constructed. Moreover, additional acceleration constraints were imposed on the trajectory planning algorithm. The scheme of the multi-lane formation control algorithm is shown in Fig. 2.

FIGURE 2.

Formation behavior modelling and control.

The remainder of the paper is organized as follows: Section II introduces formation behavior modelling, and illustrates the state transition of formation based on finite state machine. Section III details the trajectory planning algorithm based on distributed consensus algorithm and risk potential field method. Section IV presents the tracking controller based on model predictive control. section V simulates and validates the proposed formation control algorithm in Webots. Finally, conclusions are drawn in Section VI.

SECTION II. Formation Behavior Modelling

Formation of connected and autonomous vehicle was designed to accomplish tasks in the dynamic environment. Situation awareness is an essential component of formation coordination in which each agent derives a sufficient volume of information to perform its own group commitments and conventions according to the group behavior and synchronize and coordinates its own individual behavior accordingly. In this paper, however, perception is out of the scope of our research. The assumption is made that vehicles in the convoy can obtain up-to-date information about the inter-vehicle dynamic by inter-vehicle communication. Equipped with sensors and OBU(on-board unit), the vehicle in the formation can get the information about environmental vehicles and road geometry by information fusion from the sensors and V2I communications.

Formation control can be divided into two subtasks: behavior planning and operational management. Behavior planning aims to allocate the structured set of scenario actions implementing goal-oriented behavior of the formation. Operational management intends to coordinate and synchronize the formation behavior and mitigate scenario performance deviation and achieve consensus-oriented formation control. It is essential to select appropriate system models, therefore, to characterize the underlying fundamental rules of multi-agent formation behavior. Intuitively, the evolving mechanism of formation can be heuristically defined by Finite State Machine.

A. Finite State Machine

Approaches based on Finite State Machine (FSM) are commonly applied to digital designs systems. The main features are: forced modularization defined by the states, and easy response to the environment changes. A determined finite state machine is described by a five-element tuple: $\left({Q, \sum , \delta , q_{0}, F}\right)$

AbbreviationExpansion

$Q$ :	a finite set of state
$\sum$ :	a finite and nonempty input alphabet
$\delta$ :	a series of transition functions
$q_{0}$ :	the starting state
$F$ :	the set of accepting states

The design methodology utilized in this work involved the inferring of a model for formation behavior. All state transitions in the model are event triggered; that is, if Event occurs - while Condition satisfies - perform Action.

B. Dynamic Formation Behavior Modelling

Within the context of multi-vehicle system, connected vehicles exchange messages to coordinate vehicles’ individual maneuvers in a distributed way. Specifically, combination of interacting state machines with internal states and distributed algorithm of self-organizing coordination of vehicles’ maneuver contributes to the implementation of group control. Hence, agent-based group behavior modelling is an appropriate technique as it is used to characterize the evolving mechanism of the multi-vehicle system with a dynamic environment.

In general, multi-vehicle systems face both internal and external challenges. Firstly, a multi-vehicle system can reach an unstable state. For example, platoons are likely to encounter the string instability issue if the information flow topology or control strategy is not designed properly. Further, it is difficult for multi-vehicle systems to maintain steady-state behaviour in a dynamic environment. Therefore, multi-vehicle systems aim to adjust their state by the internal and external disturbances. Without loss of generality, the state of the multi-vehicle system can be divided into: Base, Consensus building, Formation reconfiguration, and Abnormal situation awareness. Fig. 3 illustrates the finite state machine specifying the behavior of multi-vehicle systems. At the very beginning, selected vehicles flock together to accomplish the mission. Upon receiving a mission command, the multi-vehicle system transfers to the Consensus building module from the Base state. In this state, distributed formation control algorithm is deployed to construct a stable and rigid formation. However, the multi-vehicle system may run into an abnormal state due to the internal or external disturbances. Should this issue occur, the system shifts to Abnormal situation awareness. The abnormal state may be categorized as: Vehicle fail, Formation split, or Collision risk. Vehicle fail indicates a typical abnormality in which the behaviour of the vehicle deviates from what is expected. Formation split is another abnormal issue. In some scenarios, the multi-vehicle system must handle both internal abnormalities and external disturbances. For example, a single malfunctioning vehicle that blocks the trajectory of another in the platoon may lead to a collision. Each of the aforementioned abnormal issues require the multi-vehicle system to transform. Therefore, the multi-vehicle system will transfer to the formation reconfiguration module when it encounters abnormal issues. Specifically, the formation is supposed to adjust its formation shape, information flow topology, and travel route to deal with the abnormal situation. Once the motion planning is accomplished, the formation returns to the Consensus building module until the mission has been completed.

FIGURE 3.

State transition diagram of convoy.

SECTION III. Distributed Formation Control

In this section, a distributed formation control algorithm is proposed. This algorithm is intended to accommodate an arbitrary number of formation vehicles and achieves consensus-based formation control and formation reconfiguration.

Multi-vehicle systems adjust their behavior in accordance with the trigger events defined by finite state machine. When the reconfiguration is determined, a multi-vehicle system must transfer into a target consensus state using the formation control approach. Moreover, to enhance the robustness and adaptiveness of multi-vehicle systems, the formation control algorithm is design in a distributed fashion. However, consensus-oriented formation control methodology is less focused on the overall safety of the multi-vehicle system. Intuitively, a risk potential field was introduced in distributed formation control.

A. Background

It is important to outline the basic principles of graph theory before outlining the controller design. A finite, undirected graph $G$ is formally defined as the pair $(V,E)$ consisting of a set $V$ of vertices and set $E$ , disjoint from $V$ , of edges. For a undirected graph $G$ , the degree of a given vertex, $d(v_{i})$ , is the cardinality of the neighborhood set, that is, it is equal to the number of vertices that are adjacent to the vertex $v_{i}$ in G. The degree matrix of $G$ is the diagonal matrix, containing the vertex-degrees of $G$ on the diagonal, that is,

$$\begin{align*} {\Delta (G)} = \left ({ \begin{array}{cccc} d(v_{1}) &\quad 0 &\quad \ldots &\quad 0\\ 0 &\quad d(v_{2}) &\quad \ldots &\quad 0\\ \vdots &\quad \vdots &\quad \ddots &\quad \vdots \\ 0 &\quad 0 &\quad \ldots &\quad d(v_{n})\\ \end{array} }\right ) \tag{1}\end{align*}$$

The adjacent matrix $A(G)$ is the symmetric $n\times n$ matrix encoding of the adjacent relationships in the graph $G$ , in that

$$\begin{align*} \lbrack A(G)\rbrack _{ij} = \begin{cases} 1 & \text {if } v_{i}v_{j}\in E,\\ 0 & \text otherwise. \end{cases} \tag{2}\end{align*}$$

Another matrix representation of a graph $G$ is the graph Laplacian, $L(G)$ . The most straightforward definition of the graph Laplacian [35] associated with an undirected graph G is

$$\begin{equation*} \mathcal {L}(G)=\Delta (G)-A (G) \tag{3}\end{equation*}$$

B. Formation Control Architecture

Inspired by the consensus-based formation control scheme proposed by Wei et al. [36], Fig. 4 illustrates and distributed formation control architecture consists of three layers: consensus module, cooperative trajectory planning module, and tracking control module. In this formation control scheme, there is a global reference state serving as the basis for each individual vehicle on the team to deploy local control approach.

FIGURE 4.

Formation control architecture.

In the context of centralized control architecture, a central control unit governs the whole multi-vehicle system and broadcasts coordination variable to every vehicle in the system. However, the utilization of centralized control requires high computation performance, and may results in a single point failure. If each vehicle implements the same local control algorithm, the same coordination performance as centralized scheme can be achieved. The Reynolds boids model [37], originally proposed in the context of computer graphics and animation, illustrates the basic premise behind several multiagent problems, in which a collection of mobile agents are to solve a global task using local interaction rules; that is, convergence can be guaranteed via local interagent interactions only. However, due to situation awareness uncertainty of each vehicle, there exist discrepancies between each instantiation of the coordination variable. Accordingly, the consensus module is deployed and therefore guarantees that each implementation of the coordination variable converges to the target value. In the cooperative trajectory planning module, the virtual leader was introduced. By virtue of distributed consensus algorithm and local neighbor information exchange, the basic trajectory of each vehicle can be derived. Nevertheless, it has the risk of collision as it emphasis on the strict consensus while ignoring the collision risk in the process of formation reconfiguration. Intuitively, potential field was introduced to enforce the safe inter-agent spacing.

C. Distributed Consensus With Virtual Leader

The distributed consensus algorithm is applied on the group level to guarantee consensus on the time-varying group reference trajectory. Fig. 5 illustrates an example of formation composed of a virtual leader with four vehicles, where, $C_{o}$ represents the inertial frame and $C_{v}$ represents a virtual coordinate frame located at $(x_{v}, y_{v})$ with a orientation $\theta _{v}$ relative to $C_{o}$ . In addition, $r_{i}$ , $r_{i}^{d}$ , and $r_{iv}^{d}$ represent the actual state, desired state, and desired deviation of the $i^{th}$ vehicle relative to virtual leader, respectively, where

$$\begin{align*} \begin{bmatrix} x^{d}_{i}(t)\\[4pt] y^{d}_{i}(t) \end{bmatrix} = \begin{bmatrix} x_{v}(t)\\[4pt] y_{v}(t) \end{bmatrix} + \begin{bmatrix} cos[\theta _{v}(t)] & \quad -sin[\theta _{v}(t)]\\[4pt] sin[\theta _{v}(t)] & \quad cos[\theta _{v}(t)] \end{bmatrix} \begin{bmatrix} x^{d}_{iv}(t)\\[4pt] y^{d}_{iv}(t) \end{bmatrix} \\{}\tag{4}\end{align*}$$

FIGURE 5.

A formation composed of four vehicles with a virtual vehicle.

It should be noted that the scenarios in Fig. 5 rely on the assumption that individual vehicle can derive the realistic state of the virtual coordinate frame; that is, the position and orientation of the virtual leader. However, in realistic conditions, the formation vehicle may have an inconsistent understanding for the coordinate variable due to heterogeneity of vehicle or unreliable information exchange. Suppose that $C_{vi}$ with state $\xi _{i} = [x_{vi},y_{vi},\theta _{vi}]^{T}$ represents the virtual coordinate frame known by vehicle $\# i$ . Therefore, the consensus module is deployed to drive $\xi _{i}$ converge to $\xi ^{r}$ , where $\xi ^{r}$ is denoted as the real coordinate frame of the virtual vehicle.

A graph was used to model the information flow topology among the $\text{n}+1$ vehicle in the formation, where virtual vehicle $\#n+1$ serves as the virtual leader of the formation with state $\xi _{n+1} \triangleq \xi ^{r}$ . To achieve a consensus understanding of the virtual coordinate frame, each vehicle uses the coordinate variable as follow:

$$\begin{align*}& u_{i} = \frac {1}{\eta _{i}(t)}\sum _{j=1}^{n} a^{v}_{ij}(t)[\dot {\xi _{j}}-\lambda (\xi _{i}-\xi _{j})] \\ &\qquad \qquad +\frac {1}{\eta _{i}(t)}a^{v}_{i(n+1)}(t)[\dot {\xi ^{r}}-\lambda (\xi _{i}-\xi ^{r})],\quad i = 1, \ldots , n \\{}\tag{5}\end{align*}$$ where $a^{v}_{ij}(t)$ is the entry of $A^{v}_{n+1}\in \mathbb {R}^{(n+1)\times (n+1)}$ at time t, $\lambda$ is a positthe ive scalar, and $the \eta _{i}(t)\triangleq \sum _{j=1}^{n+1}a^{v}_{ij}(t)$

D. Speed Planning Based on Distributed Consensus

Suppose that the vehicle has a single-integrator dynamic given as:

$$\begin{equation*} \dot {r_{i}}=u_{i},\quad i =1, \ldots , n \tag{6}\end{equation*}$$ where $r_{i}\in \mathbb {R}^{m}$ is the state and $u_{i} \in \mathbb {R}^{m}$ is the control input. The objective of speed planning is to obtain a speed profile for each vehicle in the formation such that desired formation may be achieved. A consensus algorithm was applied to derive the control input in every control timestep $$\begin{align*} \dot {r_{i}}=\dot {r}^{d}_{i} - \alpha _{i}(r_{i} - r^{d}_{i})-\sum _{j=1}^{n} a_{ij}(t)[(r_{i} - r^{d}_{i})-(r_{j} - r^{d}_{j})] \\{}\tag{7}\end{align*}$$ where $\alpha$ is the positive scalar, $a_{ij}$ is the entry of the $n \times n$ adjacency matrix $\mathcal {A}_{n}$ associated with the interaction topology

In order to improve the scalability and stability of the multi-vehicle system, the vehicle only exchanges information with its local neighbors. The laplacian matrix, therefore, can be determined by the real-time information flow topology. Local neighbors are defined based on the topological paradigm. It was suggested in [38] that a multi-vehicle system with a topology interaction can change shape, fluctuate and even split, yet remain cohesion. Specifically, each vehicle enumerates the other vehicles in the vicinity using its own local right-handed frame coordinate. In this way the spatial position of the nearest neighbors are mapped. Fig. 6 displays a illustration that vehicle A and its local neighbors.

FIGURE 6.

Vehicle A (dark green) and its local neighbors (light green).

E. Risk Potential Field Enables Collision Avoidance Capability

A distributed consensus algorithm has advantage of achieving strict consensus when the information flow topology satisfies the algorithm’s prerequisite. However, forming a convoy or formation configuration based on the distributed consensus algorithm has a high risk of collision. This occurs as therelative distance may be short than the safe distance in the process of vehicles driving to the consensus state. On the other hand, if a formation vehicle reaches an abnormal state, it will exacerbate the instability of the formation. Intuitively, the risk potential field was imposed in the cooperative control algorithm.

In our previous research, potential field method was used for pathing planning of automated vehicle in hybrid highway traffic system [15]. In this paper, risk potential field functions were elaborated to guarantee free collision in the process of formation coordination or re-coordination. Generally, vehicle risks in collision due to either external disturbances or small distances between a vehicle and its neighbors. The consensus-based formation control approach pursues the global coordination of the whole multi-vehicle system while placing less focus on the inter-vehicle spacing. As a result, if only the distributed consensus algorithm is deployed, the distance between vehicles in a formation may be small enough to result in an accidental collision, which limits the usability of this algorithm for real-world applications. Collision risk may also stem from external disturbances or slow-moving vehicles ahead of the platoon. In addition, the degree of internal and external collision risk explicitly differs. Hence, the risk potential field functions were designed separately. For the sake of simplicity, we take the total risk potential as the superposition of several component functions, i.e., formation vehicle potential and obstacle vehicle potential.

1) Formation Vehicle Potential

Formation vehicle field was used for the inter-vehicle collision avoidance. Thanks to the inter-vehicle communication and distributed formation control, the probability of collision, to some extent, is small. Moreover, to improve energy-saving, the desired inter-vehicle distance is supposed to be small. With these taken into consideration, the range of the formation vehicle is displayed in Fig. 7.

FIGURE 7.

Range of formation vehicle potential.

The white region surrounding the vehicle body is reserved space; other vehicles should not enter these regions. The gray region that extends from the vehicle body is the vehicle potential domain of influence; it is used to help avoid collisions. The formation vehicle’s longitudinal potential is defined as:

$$\begin{align*} A_{vel}=\begin{cases} \displaystyle U_{vel} & P\in \beta \\ \displaystyle 0 & P\not \in \beta \end{cases} \tag{8}\end{align*}$$ where $U_{vel}$ determines the maximum amplitude of the vehicle potential, P is the calculated point, and $\beta$ represents the domain of the formation vehicle potential. The total potential value can be derived based on the longitudinal potential as: $$\begin{equation*} U=A_{vel} exp \left ({\frac {-d^{2}}{2\sigma _{vel}^{2}} }\right ) \tag{9}\end{equation*}$$ where $A_{vel}$ is the longitudinal potential, $d$ is the Euclidean distance to the nearest point on the perimeter of the white region within larger vehicle potential region, and $\sigma$ is a coefficient proportional to the lane width, used to determine the rate ar which the potential converge.

2) Obstacle Vehicle Potential

For simplicity, obstacle vehicles in this paper refer to those broken vehicle or vehicle at a lower speed blocking the formation. For one thing, the obstacle vehicle may lack of V2V (vehicle to vehicle) communication capacity. For another, it is difficult to make the intention prediction of the obstacle vehicle. The external disturbance from the obstacle vehicle, therefore leads to bigger probability of collision. The obstacle vehicle potential range is shown in Fig. 8.

FIGURE 8.

Range of obstacle vehicle potential.

The white reserved space at the rear of the vehicle is shaped like a wedge to encourage lane-change maneuvers. In addition, the range of obstacle vehicle potential is larger out of consideration of collision avoidance. Specifically, an obstacle vehicle’s longitudinal potential function is represented as a piece-wise function as follow.

$$\begin{align*} A_{obs}=\begin{cases} \displaystyle U_{obs}& P\in \beta \\ \displaystyle v_{r}/(K-L_{r}) & P\in \alpha \cap v_{r} > 0\\ \displaystyle 0 & otherwise \end{cases} \tag{10}\end{align*}$$ where $U_{obs}$ determines the maximum amplitude of the vehicle potential, P is the calculate point, and $\beta$ represents the domain of the formation vehicle potential. The total potential value can be derived based on the longitudinal potential as: $$\begin{equation*} U=A_{vel} exp \left ({\frac {-d^{2}}{2\sigma _{vel}^{2}} }\right ) \tag{11}\end{equation*}$$ where $A_{obs}$ is the longitudinal potential, $d$ is the Euclidean distance to the nearest point on the perimeter of the white region within larger vehicle potential region, and $\sigma$ is a coefficient proportional to the lane width used to determine the rate at which the potential converges.

Fig. 9 displays a snapshot of the risk potential heatmap. In this scenario, four vehicles are going to form a $2\times 2$ formation, and there exists a obstacle vehicle.

FIGURE 9.

A snapshot of risk potential heatmap.

F. Integrated Planning

The potential force can be derived from the total potential by deploying the gradient descent law.

$$\begin{equation*} f_{APF} =-\bigtriangledown U_{all} \tag{12}\end{equation*}$$ where $U_{all}$ is the total potential, $f_{APF}$ is the potential force.

Assume the velocity of controlled vehicle in next time step has positive correlation with the potential force at present. Under this assumption, the following equation may be derived.

$$\begin{equation*} V_{next}=K \ast f_{APF} \tag{13}\end{equation*}$$ where $f_{APF}$ is the potential force, $V_{next}$ is target speed in the next time step, and $K$ is a constant.

In the last section, the speed profile of each vehicle in the formation can be determined by deploying a distributed consensus algorithm. In order to enhance safety, a risk potential field is introduced to make a trade-off between the realization of formation structure and local path planning with collision avoidance taken into consideration. The speed of the vehicle in the next time step can be obtained as:

$$\begin{equation*} V =a\ast V_{consensus}+b \ast K \ast f_{APF} \tag{14}\end{equation*}$$ where $a$ and $b$ are the weight coefficients, $V_{consensus}$ is the planning speed obtained using the distributed consensus algorithm, and $\Delta t$ is unit time step.

The acceleration based on the proposed control law, will be terribly high in case of high position or velocity errors. In practical cases, there is a limit on the acceleration based on the design of the vehicle. Also the actuator may saturate due to high control effort. Moreover, high acceleration will greatly reduce the comfort of the vehicle passengers. Safety and performance constraints are formulated in terms of the bounded acceleration

$$\begin{align*} \begin{cases} \displaystyle -4 m/s^{2}\leq a_{x}\leq 4m/s^{2}\\ \displaystyle -4 m/s^{2}\leq a_{y}\leq 4m/s^{2} \end{cases} \tag{15}\end{align*}$$

SECTION IV. Bounded Control Inputs Considering Physical Vehicle

The proposed formation control algorithm generates the velocity invariant movement. moreover, velocity is supposed to transferred into control input. For simplicity but without loss of generality, a kinematic bicycle model was used is shown in Fig. 10.

$$\begin{align*} \begin{cases} \displaystyle \dot {x}=cos(\varphi )\ast v_{r}\\ \displaystyle \dot {y}=sin(\varphi )\ast v_{r}\\ \displaystyle \dot {\varphi }=\frac {tan(\delta _{f})}{L}\ast v_{r}\\ \displaystyle \dot {v_{r}}=a \end{cases} \tag{16}\end{align*}$$ where $[x, y]^{T}$ is the position of the midpoint of the rear axle of the controlled vehicle, $\varphi$ is the angle between the vehicle heading and the s-axis, $L$ is the wheelbase of the vehicle, $\delta _{f}$ is the front wheel angle, and $a$ is acceleration.

FIGURE 10.

Kinematic bicycle model.

Model predictive control (MPC) is a receding horizon control technique widely applied for vehicle control [39]. The optimal trajectory is completely derived, and the actual control input is front wheel angle to reach the center of the target lane. The general form of the vehicle control system can be described as follow:

$$\begin{equation*} \dot {\chi }=f(\chi ,u) \tag{17}\end{equation*}$$ where $\chi \doteq [x,y,v,\varphi ]^{T}$ is the state variable vector, and describes the position and orientation of the center of the axis of the wheels, $u\doteq [a,\delta _{f}]$ is manipulated variable.

Assuming a reference vehicle also described by kinematic bicycle model, and its trajectory $\chi _{r}$ and $u_{r}$ are related by the following equation:

$$\begin{equation*} \dot {\chi }=f(\chi _{r},u_{r}) \tag{18}\end{equation*}$$

Expanding the right side of (18) in Taylor series around the reference trajectory point $(\chi _{r},u_{r})$ and discarding the high order terms, we can transform into:

$$\begin{align*}& \dot {\chi }=f(\chi _{r},u_{r})+\frac {\partial f(\chi ,u)}{\partial \chi }\Bigg \arrowvert _{\substack {\chi =\chi _{r}\\ u=u_{r}}} (\chi -\chi _{r}) \\ &\qquad \qquad \qquad \qquad \qquad +\frac {\partial f(\chi ,u)}{\partial u}\Bigg \arrowvert _{\substack {\chi =\chi _{r}\\ u=u_{r}}} (u-u_{r}) \tag{19}\end{align*}$$

Then, the subtraction of (18) from (19) results in:

$$\begin{align*} \dot {\tilde {\chi }}&=\begin{bmatrix}{\dot {x}-\dot {x_{r}}}\\ {\dot {y}-\dot {y_{r}}}\\ {\dot {v}-\dot {v_{r}}}\\ {\dot {\varphi }-\dot {\varphi _{r}}}\end{bmatrix} \\ &=\begin{bmatrix}{0} &\quad {0}&\quad {cos\varphi _{r}}&\quad {-v_{r}sin\varphi _{r}} \\ {0} &\quad {0}&\quad {sin\varphi _{r}}&\quad {v_{r}cos\varphi _{r}}\\ {0} &\quad {0}&\quad {0}&\quad {0}\\ {0} &\quad {0}&\quad {\frac {tan\varphi _{r}}{L}}&\quad {0}\end{bmatrix}\begin{bmatrix}{x-x_{r}}\\ {y-y_{r}}\\ {v-v_{r}}\\ {\varphi -\varphi _{r}}\end{bmatrix} \\ &\quad +\begin{bmatrix}{0}&\quad {0}\\ {1}&\quad {0}\\ {0}&\quad {0}\\ {0}&\quad {\dfrac {v_{r}}{lcos^{2}\delta _{fr}}}\end{bmatrix}\begin{bmatrix}{a - a_{r}}\\ {\delta _{f}-\delta _{fr}}\end{bmatrix} \tag{20}\end{align*}$$ Using Euler discretization, the system may be represented using a discrete-time model as: $$\begin{equation*} {\tilde {\chi }{(k+1)}}=A_{k,t}{\tilde {\chi }{(k)}}+B_{k,t}{\tilde {u}{(k)}} \tag{21}\end{equation*}$$ with $$\begin{align*} A(k)&=\begin{bmatrix}{1} &\quad {0}&\quad {cos\varphi _{r} T}&\quad {-v_{r}sin\varphi _{r}T} \\[8pt]{0} &\quad {1}&\quad {sin\varphi _{r} T}&\quad {v_{r}cos\varphi _{r}T}\\ {0} &\quad {0}&\quad {1}&\quad {0}\\[8pt]{0} &\quad {0}&\quad {\dfrac {tan\varphi _{f}}{L}T}&\quad {0}\end{bmatrix} \tag{22}\\ B(k)&=\begin{bmatrix}{0}&\quad {0}\\ {0}&\quad {0}\\ {T}&\quad {0}\\ {0}&\quad {\dfrac {v_{r}T}{Lcos^{2}\delta _{r}}}\end{bmatrix} \tag{23}\end{align*}$$ where T is the sampling period and k is the sampling time.

Let:

$$\begin{align*} \xi (k\vert t)=\begin{bmatrix}{\tilde {\chi }(k\vert t)}\\ {\tilde {u}(k-1 \vert t)}\end{bmatrix} \tag{24}\end{align*}$$ new state-space model van be created $$\begin{align*} {\xi }{(k+1\vert t)}&={\tilde A_{k,t}}{\xi }{(k\vert t)}+{\tilde B_{k,t}}{\Delta U}{(k\vert t)} \\ \eta (k\vert t)&={\tilde C_{k,t}}{\xi }{(k\vert t)} \tag{25}\end{align*}$$ with $$\begin{align*} {\tilde A_{k,t}}&=\begin{bmatrix}{A_{k,t}} &\quad {B_{k,t}} \\ {0_{m\times n}} &\quad {I_{m}}\end{bmatrix}, \tag{26}\\ {\tilde B_{k,t}}&=\begin{bmatrix}{B_{k,t}} \\ {I_{m}}\end{bmatrix}, \tag{27}\\ {\tilde C_{k,t}}&=\begin{bmatrix}{0}&\quad {1}&\quad {0}&\quad {0} \\ {0}&\quad {0}&\quad {1}&\quad {0}\end{bmatrix}, \tag{28}\end{align*}$$ where $m$ is the dimension of manipulated variable, $n$ is the dimension of state variable.

A. Optimal Control Problem

the objective function can be described as follows:

$$\begin{align*}& J(k)=\sum _{j=1}^{N_{p}} \Vert \eta (k+i\vert t)-\eta _{ref}(k+i\vert t)\Vert _{Q}^{2} \\ &\qquad \qquad \qquad \qquad \qquad \qquad +\sum _{j=1}^{N_{c}-1} \Vert \Delta U(k+i\vert t))\Vert _{R}^{2} \\{}\tag{29}\end{align*}$$ where $Q,R$ are weighting matrices, $N_{p}$ is the prediction horizon, and $N_{c}$ is the control horizon, the first term represents the capability of the system to track the reference trajectory, the second term reflects the constraints of control input to utilize the smooth and minimal control.

The main goal of the controller is to ensure that the system accurately tracks the desired trajectory using smooth control input. The control input is subject to the physical limitations of the actuators. Therefore, the following control constraint are imposed upon the control input:

$$\begin{align*} \Delta u_{min}(t+k)&\leqslant \Delta u(t+k)\leqslant \Delta u_{max}(t+k) \\ u_{min}(t+k)&\leqslant u(t+k)\leqslant u_{max}(t+k) \tag{30}\end{align*}$$

Hence, the optimal control problem can be solved as $\tilde {u}^{\star}$ such that:

$$\begin{align*} \tilde {u}^{\star} =\,&argmin\lbrace \Phi (k) \rbrace \\ & s.t.~\Delta U_{min}\leqslant \Delta U_{t}\leqslant \Delta U_{max} \\ &\hphantom {s.t.~} U_{min}\leqslant A\Delta U_{t}+U_{t}\leqslant U_{max} \tag{31}\end{align*}$$

With this, the optimal control input increment sequence $\Delta U_{t}^{\star}$ can be derived. Only the first element of the sequence are applied to the vehicle in each control horizon.

$$\begin{align*} \Delta U_{t}^{\star} &=\begin{bmatrix}{\Delta u_{t}^{\star} } &\quad {\Delta u_{t+1}^{\star} }&\quad {\ldots }&\quad {\Delta u_{t+N_{c}-1}^{\star} }\end{bmatrix} \tag{32}\\ u(t)&=u(t-1)+\Delta u_{t}^{\star} \tag{33}\end{align*}$$

SECTION V. Simulation

Experimental verification and performance evaluation of the proposed formation control algorithm are carried out using Webots, a powerful submicroscopic, high-fidelity simulator originally developed for mobile robotics. Webots has been recently updated to support automotive platforms. The proposed formation controller was implemented in Webots using Python language.

A. Implementation Detail

The simulated vehicle, implemented using Webots, is based on the model of a BMW X5 car. The parameter of all the formation vehicle are each simulation vehicle is equipped with a radio communication device, an InertialUnit, a GNSS module. Fig. 11 displays the simulation vehicle and its parameters. Each formation vehicle can achieve localization and speed measurement by means of the GNSS module. In addition, the heading of the simulated vehicle can be derived using InertialUnit. The simulated vehicle can conduct information exchange with its local neighbors using an onboard Emitter and Receiver. Vehicle speed is provided as the Webot control input; this speed is converted from the desired vehicle acceleration, as the Webot platform cannot utilize acceleration as the control input. The vehicle steering angle acts as the second control input. Up to 8 vehicles are used in the various experiments of this study.

FIGURE 11.

Formation consensus building.

According to the formation behavior model, the motion of formation can be abstracted into: consensus building and formation reconfiguration in response to abnormal situation. In order to verify the proposed formation control algorithm, three typical scenarios were therefore designed. The first scenario was conducted to verify the effectiveness of formation control algorithm in term of consensus building. In this scenario, eight vehicles deployed with distributed consensus algorithm was supposed to make a stable formation. The left two scenarios are about abnormal situation response, one is for obstacle avoidance, and another is for formation split.

B. Performance Indicator

The formation control algorithm is based on the distributed consensus algorithm embedded with risk potential field, where there exists a virtual leader to govern the behaviour of the multi-vehicle system. The state of each individual vehicle at each timestep, therefore can be determined based on the formation shape and virtual leader.

Two metrics, namely, longitudinal error and latera error, are used to measure the performance of our formation control algorithm. These error values are taken to be difference in absolute value between the desired position and the actual position measured individually by each vehicle at each timestep. The trajectory of longitudinal error and lateral error can reflect the performance of the proposed formation control algorithm.

C. Formation Consensus Building

Formation consensus building is a basic scenario. For the very beginning, as is shown in Fig. 12, the eight connected and autonomous vehicles were randomly placed in the different lanes. The target shape of the multi-vehicle system is rectangular formation of eight vehicles being distributed across the two lanes. The initial speed of each vehicle is 14m/s; vehicle speeds will increase to 15m/s with longitudinal inter-vehicle spacing of 25m when the formation reaches stability. The trajectory of the longitudinal error and lateral error displayed in Fig. 13 illustrates that the formation reaches consensus at about 12s.

FIGURE 12.

Forming a convoy.

FIGURE 13.

Trajectory of the performance indicator in formation consensus building scenario.

D. Formation Reconfiguration

In this scenario, the formation encounters a broken vehicle ahead preventing the formation from moving in the current route. Nevertheless, a lane-change maneuver is available in this scenario to get rid of the obstacle vehicle. The formation is supposed to initiate a lane-change maneuver to avoid obstacle vehicles. Moreover, the risk potential from the broken vehicle encourages this maneuver too. Fig. 14 presents a schematic diagram of formation reconfiguration to avoid collision. Furthermore, this scenario was validated in Webots, and the snapshot of the scenario simulation is shown in Fig. 15. The formation consisting of eight vehicles, each moving at a speed of 15m/s with a constant spacing of 20m. The broken vehicle was located 200m ahead of the formation moving in current mode. The formation initiated lane change maneuver to avoid collision. As a result, the formation changed the route and avoided collision successfully. Eventually, the formation realized the formation consensus building. The trajectory of longitudinal error and lateral error displayed in the Fig. 16 illustrates that the formation reached to consensus at about 12s.

FIGURE 14.

Schematic diagram of formation reconfiguration to avoid collision.

FIGURE 15.

Snapshot of the formation reconfiguration to avoid collision.

FIGURE 16.

Trajectory of the performance indicator in formation configuration scenario.

E. Formation Split

Formation splits may be categorized as either mandatory or discretionary. Mandatory splits occur when a vehicle malfunction is detected. It is generally unlikely that a vehicle will fail, but vehicle failures may lead to the multi-vehicle system failure if not properly dealt with handled. In the case of a discretionary split, the formation vehicle takes the initiative to leave from the formation. Formation split belongs to the set of abnormal event. As a result, the multi-vehicle system is supposed to shift to the Formation reconfiguration module.

Fig. 17 illustrates the schematic diagram of a typical formation split scenario. In the beginning, the formation moves at the desired speed. However, a vehicle in the formation experiences a communication interruption, which leads to the mandatory split. Although the number of the formation vehicles decreases, the formation is capable of remaining stable. Then, a vehicle drives to the off-ramp and make an initiate split from the multi-vehicle system. Consequently, the formation transfers to Formation reconfiguration module, and the formation shape and communication flow topology should be re-planned. Last but not least, the formation turns to the Consensus building module.

FIGURE 17.

Vehicle failure and vehicle split.

To verify the proposed distributed consensus algorithm, the formation split scenario was simulated. The snapshot displayed in Fig. 18 illustrates the process of formation split. At the beginning of the scenario, the initial speed of all the eight vehicle is set to 15 m/s. The relative distances between each vehicle is set to be 20 m in the longitudinal direction and 3.5 m in the lateral direction. When a vehicle splits from the formation due to communication interruption, no further changes are applied in the formation control apart from the communication flow topology. If another vehicle makes an initial leave, the multi-vehicle system is supposed to reorganize the formation from the perspective of formation shape and communication topology. Snapshot 3 makes it clear that the formation changes the formation shape, and converge to the consensus. Since two vehicles split from the formation due to mandatory split and discretionary split, the states of the two vehicles will not been taken into consideration. Fig. 19 presents the longitudinal and lateral error of every vehicle at each timestep.

FIGURE 18.

Vehicle failure and vehicle split.

FIGURE 19.

Plots over time of metrics.

SECTION VI. Conclusion

In this paper, a distributed formation control algorithm for the multi-lane formation of connected and autonomous vehicles was proposed and validated. To begin with, the evolution mechanism of multi-lane formation was investigated and a formation transition model based on finite state machine was built. As a result, the multi-vehicle system can cope with the internal and external disturbance by behavior transition based on the finite state machine. A bi-level formation control scheme was then proposed; this framework’s upper and lower levels are used to perform trajectory planning and MPC-based control, respectively. By combining the distributed consensus algorithm and potential field method, a novel trajectory planning approach was constructed. Moreover, additional acceleration constraints were imposed on the trajectory planning algorithm. Experimental verification and performance evaluation of the proposed formation control algorithm is conducted using Webots. There typical scenarios, including consensus building and formation reconfiguration in response to abnormal situations, were designed for algorithm validation. Longitudinal error and lateral error were taken as the performance indicator to measure the performance of our formation control algorithm. The results illustrate that formation deployed with the proposed formation control algorithm can tackle abnormal situations and realize consensus within 12s.

One of the novelties of the proposed solution is the distributed consensus mechanism which creates and maintains the neighboring graphs and biases. This mechanism potentially facilitates changing formation state depending on the situation the vehicles involved in. Another novelty is that risk potential method is introduced in the distributed consensus algorithm, which alleviated the collision risk in the process of forming a formation or formation re-coordination. Moreover, acceleration constraints are imposed in the combined trajectory planning algorithm to avoid impractically high speed based on the combined trajectory planning algorithm when the position or speed error is large. In the future, the reinforcement learning approaches that are popular in current research are applied to handle the issues about the coulped lateral and longitudinal maneuvers, which could responses more complex and unknown scenarios through dataset’s training comparing with the proposed method. In addition, due to the limitations of experimental conditions, the more complicated comparison experiments could be carried out in the later research.