Recent years have seen great success in the integration of advanced wireless communication, computing, and control technologies with automotive systems. This advancement has given rise to connected and autonomous vehicles (CAVs), which are considered promising for significantly revolutionizing the automotive sector and our mobile society [1]. CAVs equipped with onboard intelligence and automation are expected to offer more benefits than traditional human-driven vehicles (HDVs) in terms of driving safety, comfort, traffic efficiency, and fuel consumption. These benefits have been demonstrated in numerous simulation-based and field-test-based studies. However, achieving a fully autonomous driving system at Level 5, as defined by the Society of Automotive Engineering (SAE), remains a distant goal. Full autonomy requires the transformation of not only automotive systems but also roadside systems in terms of connectivity and intelligence. With a low penetration rate of vehicular communication and automated control capacities, the mobility society will experience a long-term transition stage in which both CAVs and HDVs coexist to form mixed traffic flows in road networks. In this context, it is important to coordinate the dynamics of mixed traffic flows. Many studies (e.g., Refs. [2], [3], [4]) have focused on this mixed traffic system and have demonstrated the potential to coordinate the behavior of HDVs by using a number of CAVs as mobile actuators when they coexist in a traffic flow. The underlying reason is that the dynamics of HDVs depend on those of their surrounding vehicles. Therefore, controlling the dynamics of a mixed traffic flow by manipulating a small number of CAVs, which is also known as the Lagrangian control of mixed traffic flows [4], is an appealing concept. For instance, researchers from Ref. [2] conducted a series of pioneering field tests to show that even a single autonomous vehicle can dissipate stop-and-go waves in a mixed traffic flow. Additionally, Refs. [5], [6] have analyzed the controllability and stabilizability of mixed traffic flows in which a single CAV is used to guide multiple HDVs in a single-lane ring road scenario and showed that mixed traffic flows can achieve higher mobility through the control of a set of CAVs. In this work, we make progress toward the coexistence of both CAVs and HDVs. Differing from the results of existing studies [2], [3], [4], [5], [6], we delve into robust control design for a mixed vehicle platooning system comprising a single CAV as a leader and several HDVs as followers. This mixed platoon paradigm is widely discussed in recent literature, such as Refs. [5], [6], [7]. A general mixed traffic flow can be partitioned into various mixed sub-platoons [8], [9], each of which has a single CAV and multiple HDVs. At this point, the targeted vehicle platoon serves as the fundamental component of a mixed traffic system, and its control scheme is naturally extendable to a general mixed traffic flow. Furthermore, we differentiate our work here from many existing platoon control schemes by focusing on obstacle collision avoidance for the mixed vehicle platoon. Obstacle collision avoidance is a significant and common safety-oriented application scenario. This application requires not only the accurate detection and location of an obstacle that may be unknown beforehand to the platooning vehicles but also two-dimensional (2D) platoon manipulation with guaranteed safety, smoothness, and stability. Recently, numerous platoon control and formation approaches have emerged in autonomous driving research. Some of these approaches rely on ideal dynamics models of CAVs and HDVs without uncertainties, as shown in Refs. [8], [10], [11], [12], [13], [14], [15]. However, these approaches may not be practical in real-world scenarios, particularly for obstacle collision avoidance, owing to the inherent uncertain disturbances in the mathematical dynamics models of HDVs, which stem from human drivers’ imprecise operations, mismatched model errors, and unknown sensor measurements. The presence of external model uncertainties presents a challenge in providing a theoretically rigorous guarantee for the string stability of a mixed vehicle platoon control scheme. Efforts have been dedicated to robust platoon control, as evident in Refs. [9], [16], [17], [18], [19], [20], to offer a robust guarantee in the face of model uncertainties or external disturbances. However, these studies mainly concentrate on the one-dimensional dynamics of a pure CAV or mixed platoon. Existing robust platoon control methods considering only longitudinal dynamics fall short of satisfying the time-varying 2D geometry constraints necessary for obstacle collision avoidance. As a result, we explicitly model time-varying geometry constraints for obstacle collision avoidance in a mixed vehicle platoon and integrate such constraints into our control framework to address this research gap.

Model predictive control (MPC) methods are considered promising in the handling of constrained optimal control problems, as shown in recent research [11], [12], [17], [19], [20], whereas the performance of MPC methods crucially relies on prediction model accuracy. In Ref. [21], researchers developed distributed MPC controllers that consider the nonlinear vehicle dynamics and unidirectional information topologies of CAVs. In Ref. [22], a distributed lateral control scheme is proposed for CAVs’ obstacle avoidance based on a time-varying MPC algorithm. Additionally, some researchers have integrated motion-planning functionality into the MPC framework to achieve robust control for CAVs’ obstacle avoidance [23]. However, from a perspective of practical implementation, nonlinear MPC methods inevitably present challenges owing to their high computational complexity in solving nonlinear constrained optimization problems. Thus, exact linearization techniques are widely adopted to transform nonlinear platoon models into linearized ones so that a linear MPC with low computational complexity can be practically deployed. Unfortunately, such an idea introduces another challenge. Namely, conventional linear MPC methods employ static (linear time-invariant) plant models. Hence, they usually accumulate large errors resulting from unmodeled nonlinear dynamics over a prediction time horizon and cannot provide satisfactory control performance. This research gap has motivated us to develop an adaptive MPC method for the leading CAV that enables dynamic parameter updates of a linearized prediction model and geometry constraints for obstacle avoidance to improve the model’s accuracy. The leading CAV can provide a 2D reference trajectory for the following HDVs and guide them to realize obstacle avoidance. Using the controlled CAV as a guide, we propose to stabilize the following HDVs to thereby allow for external uncertain but bounded disturbances in the 2D dynamics of the HDVs in the mixed vehicle platoon.

Perception robustness is another key component enabling autonomous vehicles. In this context, a significant number of high-quality studies have focused on addressing vehicular perception uncertainty and integrating robust perception abilities into the decision-making functionality of autonomous vehicles, as evidenced in Refs. [24], [25], [26]. For example, researchers have proposed a novel scheme based on the Monte Carlo dropout (MCD) method to analyze the uncertainty of object detection algorithms using the advanced You Only Look Once (YOLO) framework [24].

They have also provided a dataset containing road images with extreme weather and adverse lighting conditions, and this dataset can be used to evaluate the uncertainty of object detection algorithms. In Ref. [25], the authors consider the input uncertainty (e.g., noisy sensor data, fuzzy or unfamiliar features) of neural network-based perception algorithms and propose an uncertainty-aware decision-making algorithm to accommodate the influence of potential perception uncertainty on autonomous vehicles. Other efforts have also been dedicated to addressing uncertainty in motion prediction and the novel design of robust decision-making and control solutions based on the awareness of prediction uncertainty [26]. Moreover, some researchers combine random vector functional link (RVFL) networks, an online sequential-extreme learning machine (OS-ELM) algorithm, and an initial-training-free-online extreme learning machine (ITF-OELM) algorithm to estimate non-parametric uncertainty in uncertain systems. This approach can facilitate the design of adaptive control laws with stability guarantees [27]. Although the aforementioned works provide effective solutions by which to address the uncertainty of perception algorithms or models (such as deep neural networks) in autonomous vehicles, they rarely focus on cybersecurity issues that platooning vehicles may encounter, such as the dissemination of misleading or malicious information through a vehicular wireless network. Excluding malicious nodes (i.e., vehicles or roadside units (RSUs)) from the network in a self-organized and distributed manner remains a key challenge, particularly when the nodes conducting malicious attacks on the vehicle platoon are unknown in advance.

Additionally, the majority of recent studies in the field of robust vehicle platoon control have not integrated sensing robustness and control robustness within a platoon control framework. In practical scenarios, even a single CAV equipped with advanced onboard sensors may encounter numerous noisy or erroneous signal measurements owing to unknown environmental conditions or sensor failures. When sensor faults, poor GPS reception, or sensor data outages occur (e.g., in urban canyons or tunnels), CAVs struggle to accurately detect, locate, and track static or dynamic obstacles. Furthermore, as vehicular wireless networks are susceptible to cyberattacks, CAVs face new challenges and cybersecurity concerns in the present era [28], [29], [30]. For instance, adversaries may attempt to inject misleading or malicious sensing information into a vehicular network composed of CAVs and other roadside infrastructure. These cyberattacks, known as spoofing or message falsification in connected vehicles, can lead to erroneous vehicular perception and chain collisions within a vehicle platoon. Consequently, numerous researchers (e.g., Refs. [31], [32], [33], [34], [35]) have developed diverse safe and resilient platoon control solutions. However, these existing works [31], [32], [33], [34], [35], [36] and their references primarily rely on linear robust control theory. In other words, most of the existing solutions addressing cybersecurity threats in CAVs are designed from a control perspective rather than an information sensing perspective. These works combine the impacts of various cyberattacks into either bounded external uncertainties or parametric uncertainties within a linear plant model. This allows them to apply classical Lyapunov-Krasovskii theory [31], Kalman filters [36], state estimators or observers [32], [33], and event-triggered mechanisms [34], [35] to develop robust controllers against uncertain disturbances. However, most recent works have not adequately addressed the safety challenges from the standpoint of cooperative vehicle-infrastructure sensing. In fact, with the increasing number of CAVs and smart RSUs equipped with advanced sensors such as far-field cameras and radars, the collaboration between CAVs and RSUs presents opportunities for vehicular autonomy through cooperative sensing, networking, and communication. In this context, two fundamental and challenging questions emerge: ① Can the collaborative intelligence of CAVs and RSUs be utilized for endogenous safety in environmental information sensing? and ② how can we design an algorithm that enables inherent robustness against local noisy measurements, misleading information, or malicious sensor data diffused in a cooperative vehicle-infrastructure system (CVIS) and thereby empower CAVs and RSUs to learn to locate a common but unknown object in a distributed cooperative manner? This study aims to leverage the cooperative sensing ability of CAVs and RSUs through a method that enables a mobile sensor network, comprising a set of willing CAVs and RSUs, to adaptively diffuse and fuse local sensing information via wireless communication. Our proposed cooperative sensing scheme combines distributed optimization and adaptive information fusion, which suppresses the impact of nodes with poor sensing information and malicious attacks with erroneous obstacle measurements to achieve high robustness and attack resilience and thereby enhance the detection and location of an obstacle, even when the CAVs and RSUs have no prior knowledge of the obstacle.

Despite the significant progress made in autonomous driving in recent years, the lack of robustness in vehicular sensing and control systems can hinder vehicle safety and autonomy. Therefore, the main goal of this study is to incorporate the robustness characteristics of cooperative intelligence’s into vehicular sensing for obstacle detection and location to overcome challenges such as stochastic mistakes in perception, GPS-denied/degraded environments, and malicious sensor data. We consider a common obstacle unknown to both CAVs and RSUs in advance as well as platooning vehicles facing malicious attacks in obstacle sensing. We propose a robust platoon control method for mixed CAVs and HDVs, aimed at obstacle collision avoidance. We present an adaptive MPC approach for the leading CAV that dynamically updates the dynamics model and geometry constraints over time. Additionally, we implement a robust car-following control solution for the following HDVs in the presence of uncertain disturbances. We provide an input-to-state string stability guarantee for the closed-loop platoon system under the 2D dynamics control. Our control method incorporates a proposed cooperative vehicle-infrastructure sensing scheme that equips the leading CAV of the mixed vehicle platoon with the ability to overcome noisy local measurements or even erroneous estimation information from malicious attackers. This approach enables CAVs and RSUs to gauge and respond to a malicious attacking scenario in a distributed cooperative manner, thus enhancing the robustness and safety of the mixed platoon system in terms of sensing and control.

Specifically, the main novel contributions of the study are summarized as follows.

(1) We propose a robust control method for a mixed vehicle platoon consisting of a leading CAV and a set of following HDVs to achieve obstacle collision avoidance. The platoon system considers the 2D dynamics of vehicles, and the overall control method incorporates robust characteristics from two perspectives: Integrating cybersecurity guarantees into cooperative sensing and robust platoon control into a unified framework.

(2) We develop a cooperative vehicle-infrastructure sensing scheme to realize the accurate detection and location of an obstacle whose exact position is unknown beforehand to the platooning vehicles. This scheme enables the adaptability of CAVs’ and RSUs’ local information estimation, sharing, and fusion to overcome the impact of noisy obstacle measurements from nodes with low-quality sensors or misleading sensor data from potential malicious attackers. We derive the theoretical guarantee for the mean stability of the mobile sensor network and theoretically characterize its steady-state mean-square deviation, thereby providing deep insight into the benefits from cooperative vehicle-infrastructure sensing in terms of sensing accuracy and robustness.

(3) We devise an adaptive model predictive control scheme for the leading CAV, which is embedded with the cooperative vehicle-infrastructure sensing scheme. Our adaptive MPC scheme enables the model parameters to evolve with time to address accuracy degradation incurred by the linearized dynamics model while obtaining a sequence of optimal control inputs over a prediction horizon at the cost of low computational complexity.

(4) We develop a robust, distributed car-following control scheme for the following HDVs while considering external uncertain disturbances in the 2D dynamics. We provide a theoretical guarantee for stabilizing the 2D dynamics of the disturbed HDVs in terms of the input-to-state string stability of the closed-loop platoon system. The derived theoretical results shed light on the robustness of practical car-following control. The remainder of this paper is organized as follows. In Section 2, we present a system model that describes the 2D dynamics of platooning vehicles. In Section 3, we propose a cooperative vehicle-infrastructure sensing scheme for the detection and location of an unknown obstacle. In Section 4, we further propose a robust platoon control method based on cooperative sensing. Then, in Section 5, we evaluate the performance of the closed-loop platoon system. Finally, in Section 6, we conclude this work and remark on our future work.

Throughout this paper we treat all vectors as column vectors unless otherwise specified. is used to denote the mathematical expectation operator. denotes a diagonal matrix whose diagonal components are, and denotes a column vector constructed by stacking on top of each other. denotes the 2-norm, whereas is given as, where is the th element of a column vector. In particular, we also introduce the -norm as. A function,, is said to be a class function if this function is strictly increasing and satisfies. When a function is of the class and satisfies as, it is said to be a class function. A continuous function is said to be a class function when it satisfies the following two conditions: ① is a class function for any fixed and ② ${{f}_{\mathcal{K}\mathcal{L}}}\left( a,\cdot \right)$ is a decreasing function for any fixed and satisfies as.

As demonstrated in Fig. 1, we consider a mixed vehicle platoon consisting of one single CAV as a leader, indexed by i=0, and M HDVs as followers, indexed by i=1,2,\cdots ,M. These HDVs move in the middle of a straight road with three lanes, and they are guided by the leading CAV. The leading CAV is considered to be equipped with some onboard sensors, such as LiDAR and cameras, so that it has the ability to monitor the surrounding environment in real time and measure the distance to an obstacle in front of the mixed vehicle platoon in the road. In this study, an obstacle in the same road can be static, such as a large rock, ditch, or pothole, or dynamic, such as a slow-moving vehicle. The maneuvering goal of the leading CAV is to accurately and robustly estimate the unknown location of the obstacle, denoted by p^◦, in real time and guide the other HDVs in the same platoon to temporarily change to another lane to avoid colliding with the obstacle, move past the obstacle, and drive back to the original lane. In this obstacle collision avoidance scenario, we consider that the location of the obstacle is unknown to the leading CAV and the following HDVs in advance. Therefore, to enhance the accuracy and robustness of real-time obstacle detection and location by the leading CAV, we further consider that there also exist some willing CAVs as mobile sensors and RSUs as fixed sensors in the scenario, indexed by j = 1, 2, …, N - 1, where the total number of these sensors is denoted by N. Using wireless communication (e.g., LTE-V2X technology), the CAVs, including the platoon leader and the RSUs, can measure their individual distances to the unknown obstacle in the same road independently and share their individual location estimations with the neighbors in their one-hop communication coverage. In this way, the CAVs and RSUs form a cooperative vehicle-infrastructure sensor network that has the potential to improve the estimation of the unknown obstacle location in a fully distributed and cooperative manner. For the simplicity of notation, we denote the set of platooning vehicles, excluding the leading CAV, by \mathcal{V}=\left\{1,...,M\right\}. The set of CAVs and RSUs forming a cooperative vehicle-infrastructure sensor network is denoted by \mathcal{N}=\left\{0,1,...,N-1\right\}. In the following subsections, we detail the heterogeneous dynamics models of the leading CAV and the subsequent HDVs. These models serve as the foundation for designing obstacle collision avoidance-oriented platoon control.

The leading CAV acts as an actuator of the mixed vehicle platoon and plays a key role in guiding the whole platoon to avoid obstacle collision. Without loss of generality, we consider that the platooning vehicles have a rectangular shape and they need to pass an obstacle from the left lane (in reality, this lane is usually the fast lane). We denote the real-time longitudinal position, lateral position, heading angle, and speed of the leading CAV, i = 0, at time t by x_i(t), y_i(t), θ_i (t), and v_i (t), respectively. We also note that the heading angle, θ_i(t), is defined to be positive when the leading CAV is facing east and that the speed, v_i (t), is a positive scalar quantity. Because the leading CAV needs to perform a maneuver in two dimensions to achieve obstacle collision avoidance, we describe its 2D dynamics using the following nonlinear model

where L_i is the length of vehicle i, C_i is the coefficient of transferring the throttle effect to the acceleration, T_i\left(t\right) and {\delta }_i\left(t\right) are the throttle and steering angle of vehicle i at time t, respectively. Here, T_i\left(t\right) and {\delta }_i\left(t\right) are selected as the manipulated variables, that is the control inputs of the leading CAV, which need to be optimally determined in real time to enable obstacle collision avoidance. The T_i\left(t\right) term will be positive if vehicle i accelerates at t. Otherwise, T_i\left(t\right) is negative when i decelerates. In addition, {\delta }_i\left(t\right) is counterclockwise positive and {\delta }_i\left(t\right)=0 when aligned with the vehicle.

Let s_i\left(t\right)={\left[x_i\left(t\right),y_i\left(t\right),{\theta }_i\left(t\right),v_i\left(t\right)\right]}^{\text{T}} and u_i\left(t\right)={\left[T_i\left(t\right),{\delta }_i\left(t\right)\right]}^{\text{T}} be the state and control vectors of vehicle i at t, respectively. Then, Eq. (1) can be rearranged into a nonlinear state-space form

where we define

$F\left( {{s}_{i}}\left( t \right),{{u}_{i}}\left( t \right) \right)={{\left[ \text{cos}\left( {{\theta }_{i}}\left( t \right) \right)\cdot {{v}_{i}}\left( t \right),\text{sin}\left( {{\theta }_{i}}\left( t \right) \right)\cdot {{v}_{i}}\left( t \right),\frac{\tan \left( {{\delta }_{i}}\left( t \right) \right)}{{{L}_{i}}}\cdot {{v}_{i}}\left( t \right),{{C}_{i}}{{T}_{i}}\left( t \right) \right]}^{\text{T}}}$.

To facilitate the optimization of the dynamics control of the leading CAV, we linearize the nonlinear dynamics model Eq. (2) by the first-order approximation of the model at a specific operating point. Specifically, we denote by $\left( {{s}_{i}}\left( {{t}_{k}} \right),{{u}_{i}}\left( {{t}_{k}} \right),{{t}_{k}} \right)$ the operating point of the dynamics system governed by Eq. (2) at time t_k, where t_k represents the beginning time instant of the kth sampling time interval, that is, t_k=k\text{Δ}\tau and k\in {\mathbb{Z}}_{+}. Here, \text{Δ}\tau denotes the duration of a time interval. Thus, we can analytically derive the Jacobian matrices of the nonlinear dynamics model Eq. (2) with respect to the state and control variables at the nominal operating point \left(s_i\left(t_k\right),u_i\left(t_k\right),t_k\right), respectively, as follows

Now, let the state and control deviations from the nominal operating point be \delta s_i\left(t\right)=s_i\left(t\right)-s_i\left(t_k\right) and \delta u_i\left(t\right)=u_i\left(t\right)-u_i\left(t_k\right), respectively, for t\in \left[k\text{Δ}\tau ,\left(k+p\right)\text{Δ}\tau \right], where p is the prediction time horizon denoted by the number of time intervals. We obtain the linearized state-space model in terms of the deviations as follows

Moreover, for the practical application and numerical computation, we obtain the discrete-time state-space model corresponding to Eq. (5) by using the well-known zero-order hold (ZOH) method as follows

where A\left[k\right] and B\left[k\right] are calculated by

By the linearized discrete-time state-space model Eq. (6), the exact linearization can provide a sufficiently valid approximation to the nonlinear dynamics system Eq. (2) in a small region around the nominal operating point \left(s_i\left(t_k\right),u_i\left(t_k\right),t_k\right). That is, the linearized discrete-time state-space model Eq. (6) can behave like the nonlinear system when the values of the system state and control variables, namely, s_i\left(t\right) and u_i\left(t\right), are close enough to the operating point \left(s_i\left(t_k\right),u_i\left(t_k\right)\right). In this situation, the deviation from t to t_k, namely, t-t_k, should be sufficiently small, which in turn requires that the prediction step number, p, should not be configured too large. Additionally, traditional model predictive control approaches utilize a linear-time-invariant (LTI) model that is different from Eq. (6) to forecast the future behavior of the system, which is highly sensitive to prediction errors. In reality, the dynamics of the leading CAV are strongly nonlinear and may undergo significant variations over time. As a result, the accuracy of LTI prediction deteriorates markedly, leading to degraded performance and rendering the model predictive control approach unacceptable. Therefore, in this study, we allow the parameters of our linearized discrete-time state-space model Eq. (6), namely, A\left[k\right] and B\left[k\right], to evolve over time to deal with the issue of declining prediction accuracy. Specifically, we adapt the coefficient matrices of the state and control variables, namely, A\left[k\right] and B\left[k\right], to accommodate changing operating conditions such that the model accuracy of Eq. (1) can be ensured to be comparable to that of the original nonlinear model Eq. (1). We estimate the nominal operating conditions at the onset of each time interval \left[k\text{Δ}\tau ,\left(k+1\right)\text{Δ}\tau \right], that is, at t_k=k\text{Δ}\tau, and update A\left[k\right] and B\left[k\right] according to Eqs. (3), (4), using the revised operating conditions. Once the model Eq. (6) is updated, we can utilize it for predictive control optimization across the prediction horizon \left[k\text{Δ}\tau ,\left(k+p\right)\text{Δ}\tau \right].

For the following HDVs, i\in \mathcal{V}, we also describe their 2D dynamics. Specifically, the longitudinal and the lateral positions of HDV i\in \mathcal{V} at time t are denoted by x_i\left(t\right) and y_i\left(t\right), respectively. The longitudinal and the lateral speeds are represented by v_{x,i}\left(t\right) and v_{y,i}\left(t\right), respectively. We also consider the effect of some uncertain disturbances on the HDVs, which may result from inaccurate maneuvers by human drivers, mismatched nonlinear car-following dynamics, and external disturbances. The uncertain disturbances affecting the 2D position and speed of HDV j are lumped into the parameters w_{x,i}\left(t\right), w_{y,i}\left(t\right), {\xi }_{x,i}\left(t\right), and {\xi }_{y,i}\left(t\right), respectively. We resort to a linear two-order model to describe the dynamics of the HDVs in the presence of uncertain disturbances as follows ( i\in \mathcal{V})

where u_{x,i}\left(t\right)and u_{y,i}\left(t\right) are the longitudinal and lateral maneuver inputs of the HDVs, respectively. From Eq. (8), for simplicity of notation, we further let p_i\left(t\right)={\left[x_i\left(t\right),y_i\left(t\right)\right]}^{\text{T}}, v_i\left(t\right)={\left[v_{x,i}\left(t\right),v_{y,i}\left(t\right)\right]}^{\text{T}}, u_i\left(t\right)={\left[u_{x,i}\left(t\right),u_{y,i}\left(t\right)\right]}^{\text{T}}, and {\omega }_i\left(t\right)={\left[w_{x,i}\left(t\right),w_{y,i}\left(t\right),{\xi }_{x,i}\left(t\right),{\xi }_{y,i}\left(t\right)\right]}^{\text{T}}. The velocity deviation between two successive HDVs, namely, i and i-1, are given by

According to the constant time headway (CTH) policy, we formulate the desired inter-vehicle distance between two successive HDVs as follows

where \tau =\text{diag}\left\{{\tau }_x,{\tau }_y\right\} is a diagonal matrix whose diagonal components denote the desired time headways of different motion dimensions between two successive HDVs, and s_i={\left[s_{x,i},s_{y,i}\right]}^{\text{T}} is a constant column vector whose components denote the desired inter-vehicle distances of different motion dimensions between the HDVs i and i-1. Thus, the 2D position deviation between any two HDVs i and i-1 can be formulated as follows

Combining Eqs. (8), (9), and (11), we derive the following error dynamics of the platooning HDVs in terms of position and speed deviations as follows

for i\in \mathcal{V}, where w_i\left(t\right)={\left[w_{x,i}\left(t\right),w_{y,i}\left(t\right)\right]}^{\text{T}} and {\xi }_i\left(t\right)={\left[{\xi }_{x,i}\left(t\right),{\xi }_{y,i}\left(t\right)\right]}^{\text{T}}. Let the error state vector be e_i\left(t\right)={\left[\text{Δ}{d}_i\left(t\right),\text{Δ}{v}_i\left(t\right)\right]}^{\text{T}}. Recalling {\omega }_i\left(t\right)={\left[w_i\left(t\right),{\xi }_i\left(t\right)\right]}^{\text{T}} for i\in \mathcal{V}, we can rearrange Eq. (12) into a compact state-space form as follows

where H_1, H_2, and H_3

are given by

In Eq. (14), ⊗ denotes the Kronecker product operator, and I2 is a 2 × 2 identity matrix.

By Eq. (13), the dynamics of the tracking error between any two successive HDVs in the platoon depend on not only the maneuver inputs of the HDVs but also the uncertainties induced by unmodeled nonlinear car-following dynamics and uncertain actuator dynamics (arising from human drivers’ inaccurate operation). The basic goal of the mixed platoon system is to realize the obstacle collision avoidance of the platoon while simultaneously regulating the error dynamics of the HDVs, that is, e_i\left(t\right) for all i\in \mathcal{V}, given the trajectory and control of the leading CAV as reference signals. According to the dynamics model (1), the reference trajectory provided by the leading CAV i=0 is formulated by

and the control inputs of the leading CAV as the reference signals for platoon guidance are formulated by

To describe the information flow topology among vehicles in the system, we employ algebraic graph theory. Specifically, in addition to the information interaction among the platooning vehicles (comprising a leading CAV and multiple HDVs), we also consider the information exchange between the leading CAV and other CAVs and RSUs that are not part of the platoon but are willing to assist in sensing unknown obstacles ahead of the platoon in a distributed cooperative manner. As depicted in Fig. 1, two types of information flow topologies exist in the mixed traffic flow: the sensing information flow topology and the control information flow topology. These two topologies can be represented by two distinct graphs, namely. {\mathcal{G}}_{\text{sensing}}=\left\{\mathcal{N},{\mathcal{E}}_{\mathcal{N}},{\mathcal{A}}_{\mathcal{N}}\right\} and {\mathcal{G}}_{\text{control}}=\left\{\mathcal{V},{\mathcal{E}}_{\mathcal{V}},{\mathcal{A}}_{\mathcal{V}}\right\} where {\mathcal{E}}_{\mathcal{N}}\subseteq \mathcal{N}\times \mathcal{N} and {\mathcal{E}}_{\mathcal{V}}\subseteq \mathcal{V}\times \mathcal{V} are two edge sets of {\mathcal{G}}_{\text{sensing}} and {\mathcal{G}}_{\text{control}}, respectively. Additionally, {\mathcal{A}}_{\mathcal{N}} and {\mathcal{A}}_{\mathcal{V}} are their adjacent matrices, respectively.

In the graph model of the sensing information flow topology, denoted by {\mathcal{G}}_{\text{sensing}}, each sensor node (either a CAV or a roadside sensor unit) is equipped with detection capacity and wireless communication capabilities. These nodes can share their sensing data, which includes real-time relative distance and location measurements of obstacles, with their neighbors via wireless communication. An element \left(j_1,j_2\right)\in {\mathcal{E}}_{\mathcal{N}} is an edge, describing the sensing information flow from a sensor node j_1\in \mathcal{N} to another sensor node j_2\in \mathcal{N}. Additionally, since a sensor node, for example, a CAV, is allowed to use its own sensing information, we consider the presence of ring structures in {\mathcal{G}}_{\text{sensing}}, that is \left(j,j\right)\in {\mathcal{E}}_{\mathcal{N}} for all j\in \mathcal{N}. The adjacent matrix is specified as {\mathcal{A}}_{\mathcal{N}}={\left[a_{j_1,j_2}\right]}_{j_1,j_2\in \mathcal{N}}where a_{j_1,j_2} denotes the weight assigned to the communication edge from j_2 to j_1. If \left(j_2,j_1\right)\in {\mathcal{E}}_{\mathcal{N}} that is, j_2 transmits its sensing data to j_1, then a_{j_1,j_2}=1; otherwise, a_{j_1,j_2}=0. Recalling \left(j,j\right)\in {\mathcal{E}}_{\mathcal{N}}, that is, the fact that each sensor can utilize its own sensing information, we set a_{j_1,j_2}=0 for all j\in \mathcal{N} in our situation. In addition, we can denote by {\mathcal{N}}_j=\left\{j^{\prime }|\left(j^{\prime },j\right)\in {\mathcal{E}}_{\mathcal{N}}\right\} the set of neighbors of sensor j\in \mathcal{N}, including j itself. Notably, the nodes in a cooperative vehicle-infrastructure sensing network can be either vehicular sensors or roadside sensors, provided that they are willing to share their local sensing and estimation information through wireless communication. The design of our cooperative vehicle-infrastructure sensing scheme does not make any specific assumptions about the structure or form of the information flow topology, {\mathcal{G}}_{\text{sensing}}. The algorithm design is applicable to other forms of cooperative vehicle-infrastructure sensor networks, as any cooperative sensor network form can be represented using the aforementioned algebraic graph model.

Similar to the definition of {\mathcal{G}}_{\text{sensing}}, we can also introduce the weight a_{i_1,i_2} of the edge \left(i_2,i_1\right) and let a_{i_1,i_2}=1 if \left(i_2,i_1\right)\in {\mathcal{E}}_{\mathcal{V}}, and a_{i_1,i_2}=0 otherwise. Then, the adjacent matrix of the control information topology is specified as {\mathcal{A}}_{\mathcal{V}}={\left[a_{i_1,i_2}\right]}_{i_1,i_2\in \mathcal{V}}. The set of neighbors of HDV i\in \mathcal{V} is defined by {\mathcal{V}}_i=\left\{i^{\prime }|\left(i^{\prime },i\right)\in {\mathcal{E}}_{\mathcal{V}}\right\}. Additionally, we introduce {\mathcal{D}}_{\mathcal{V}} as a degree matrix of {\mathcal{G}}_{\text{sensing}}, which is a diagonal matrix {\mathcal{D}}_{\mathcal{V}}=\text{diag}\left\{d_i,i\in \mathcal{V}\right\} where ${{d}_{i}}=\underset{{i}'\in {{\mathcal{V}}_{i}}}{\mathop \sum }\,{{a}_{i,{i}'}}$. Using the above notation, we can calculate the Laplacian matrix of {\mathcal{G}}_{\text{sensing}} by {\mathcal{L}}_{\mathcal{V}}={\mathcal{D}}_{\mathcal{V}}-{\mathcal{A}}_{\mathcal{V}}. To denote the control information interaction between the leading CAV and the following HDVs in the same platoon, we introduce the adjacent matrix associated with the leading CAV i=0 as ${{\mathcal{A}}_{0}}=\text{diag}\left\{ {{a}_{1,0}},{{a}_{2,0}},...,{{a}_{M,0}} \right\},M\in \mathcal{V}$, in which we let a_{i,0}=1 if a following HDV i\in \mathcal{V} can receive position and speed information from i=0; otherwise, we let a_{i,0}=0. Thus, using the Laplacian matrix of {\mathcal{G}}_{\text{sensing}} and the adjacent matrix of the leading CAV, we can calculate the matrix characterizing the control information flow topology among the leading CAV i=0 and the following HDVs, which determines the propagation behavior of the control effect of the leading CAV within the mixed vehicle platoon

where \overline{\mathcal{V}}\text{} is the set consisting of the leading CAV and the following HDVs, that is, \overline{\mathcal{V}}=\left\{0\right\}\cup \mathcal{V}.

Moreover, in practical scenarios, human drivers who do not have vehicular communication capabilities typically have access only to the information of their preceding vehicle. For instance, an advanced driver assistance system (ADAS), such as an adaptive cruise control (ACC) system, can utilize radar or camera sensors to obtain the position and speed of its preceding vehicle. However, owing to the lack of wireless communication, these systems have limited ability to detect and track targets beyond their visual range. Consequently, the control information topology of {\mathcal{G}}_{\text{control}} takes on a simple predecessor-follower (PF) structure. In other words, human drivers operate their vehicles based solely on the dynamics of their immediate predecessors, which is commonly referred to as car-following behavior. Therefore, according to Fig. 1, {\mathcal{D}}_{\mathcal{V}}, {\mathcal{A}}_{\mathcal{V}} and {\mathcal{A}}_0 can be specified as

Using Eqs. (17), (18), we can obtain the specific form of {\mathcal{T}}_{\overline{\mathcal{V}}} as follows

We address the challenge that the leading CAV of the mixed vehicle platoon may have low accuracy or temporarily fail in detecting and locating an unknown obstacle owing to some endogenous factors, such as functional mistakes, failures of onboard sensors, or exogenous disturbances, such as bad weather conditions. Let p^{°}\left[k\right] be the position of the obstacle in the same road (e.g., Fig. 1), which may be changing with time. Without loss of generality, we assume that the accurate position p^{°}\left[k\right] is unknown to all the vehicles. Therefore, the CAVs and RSUs involved in the sensing information flow topology {\mathcal{G}}_{\text{sensing}} aim to estimate p^{°}\left[k\right] by sharing and fusing their local real-time sensor measurements in a distributed cooperative manner. In this scenario, malicious attackers disguised as cooperative sensor nodes may share highly noisy or even erroneous estimations of the obstacle position. Hence, they pose a significant safety threat to the leading CAV. Here, we also consider addressing this malicious attack issue by incorporating an adaptive weight updating mechanism for distributed information fusion across the cooperative vehicle-infrastructure sensing network.

We consider a discrete time horizon and use k to denote the index of time intervals as in Section 2.1, whereas we use s to denote the index of onboard sensor samples. For any node j\in \mathcal{N}, denote the relative distance between the obstacle and the node using the sth sample of sensor measurements at time interval k by

where m_{j,s}^{°}\left[k\right]\in {ℝ}^{2\times 1} denotes a column unit direction vector of j pointing to p^{°}\left[k\right] at the sth sample, that is, \Vert m_{j,s}^{°}\left[k\right]{\Vert }_2=1. Moreover, p_{j,s}\left[k\right] denotes the sth measurement sample of node j on its own position at the kth time interval. In reality, owing to the aforementioned endogenous and exogenous factors, the actual measurements of a CAV or an RSU j are noisy. That is, the actual measurements of m_{j,s}^{°}\left[k\right] and d_{j,s}^{°}\left[k\right] should incorporate certain noises as follows

where n_{m,j,s}\left[k\right]\in {ℝ}^{2\times 1} and n_{d,j,s}\left[k\right]\in ℝ represent the noises in the measurements of m_{j,s}^{°}\left[k\right] and d_{j,s}^{°}\left[k\right], respectively. At this point, the local measurement of j on the relative distance to the obstacle, that is, d_{j,s}^{°}\left[k\right], can be represented by

where n_{j,s}\left[k\right] is a lumped noise term defined according to Eqs. (20), (21), (22), that is,

Moreover, the local measurement of j, j\in \mathcal{N}, on the location of the obstacle, p^{°}\left[k\right], can be represented by

where {\eta }_{j,s}\left[k\right] is also a lumped noise term. According to Eqs. (20), (21), {\eta }_{j,s}\left[k\right] can be formulated as follows

Based on the above measurement models Eqs. (22), (24), the global goal of the cooperative vehicle-infrastructure sensor network is to estimate the exact position of the obstacle, that is, the unknown column vector p^{°}\left[k\right], at each time interval k. Mathematically, the network needs to obtain an accurate approximation to p^{°}\left[k\right] by minimizing an aggregate estimation error in the mean sense

where S_max denotes the maximum number of sensor samples collected to estimate pk in time interval k.

To further design a distributed cooperative sensing scheme for the CAVs and RSUs in N, we propose solving a minimization problem by each node locally and enabling each node to fuse the immediate estimation from its neighbors, according to the sensing information flow topology {\mathcal{G}}_{\text{sensing}}. The local minimization model is motivated by Eq. (26) above and formulated as follows

where c_{l,j} is a weight assigned to the neighbors of node j, which satisfies

And

Each node j\in \mathcal{N} can apply a first-order optimization algorithm, for example, a gradient descent algorithm, to solve Eq. (27). Let z_{j,s-1}\left[k\right] be an individual estimation of p^{°}\left[k\right]by node j at sample s-1. Using the gradient descent algorithm, we can derive an iteration equation to update the individual estimation at node j as follows

where μ_j is a constant step size for updating the individual estimation at j.

Additionally, since each node can exchange its data via wireless communication, we further propose a distributed estimation fusion approach based on the control information flow topology {\mathcal{G}}_{\text{sensing}}. To be specific, we introduce another set of weights on the edges in {\mathcal{G}}_{\text{sensing}}, \left\{{\alpha }_{l,j},l\in \mathcal{N}\right\}, for each node j\in \mathcal{N}, and we let

And

We can use the weights above to combine the individual estimation updates collected from the neighbors at each node j\in \mathcal{N} locally. Then, we use the combined updates as a new estimation for j\in \mathcal{N}. Namely, we have

Now, using Eqs. (30), (33), we arrive at a distributed cooperative sensing algorithm for the obstacle location over {\mathcal{G}}_{\text{sensing}}

In Eq. (34), the first step aims to adapt the individual information estimation z_{j,s-1}\left[k\right] at node j\in \mathcal{N} by aggregating its neighbors’ local measurements \left\{{\widehat{d}}_{l,s}\left[k\right],m_{l,s}\left[k\right],l\in {\mathcal{N}}_j\right\}. The second step of Eq. (34) aims to combine the adapted estimations of node j and its neighbors, \left\{{\varphi }_{l,s}\left[k\right],l\in {\mathcal{N}}_j\right\}. Such a sensing scheme is similar to the adapt-then-combine (ATC) mechanism originally proposed in Refs. [37], [38] for least-mean square (LMS) estimation, whereas the ATC mechanism is extended here to endow the cooperative vehicle-infrastructure sensor network with a continuous learning ability to detect and track an unknown p^{°}\left[k\right] that may vary with time.

From Eq. (34), we can see that its implementation relies on the nodes’ indirect measurements, \left\{{\widehat{d}}_{j,s}\left[k\right]\right\} ( j\in \mathcal{N}), as in Eq. (22). Thus, to facilitate the practical implementation of Eq. (34), we can further exploit the geometric property of the nodes’ measurements as in Eq. (20) to simplify Eq. (34). Using Eqs. (22), (24), we can see

Recalling that z_{j,s-1}\left[k\right] is an estimation of node j on the position of the obstacle, the direction of node l pointing to the estimated position, z_{j,s-1}\left[k\right], that is, the direction of z_{j,s-1}\left[k\right]-p_{l,s}\left[k\right], can be approximated by m_{l,s}\left[k\right]. That is, we can approximate z_{j,s-1}\left[k\right]-p_{l,s}\left[k\right] by z_{j,s-1}\left[k\right]-p_{l,s}\left[k\right]\approx m_{l,s}\left[k\right]\Vert z_{j,s-1}\left[k\right]-p_{l,s}\left[k\right]{\Vert }_2. Substituting this approximation into Eq. (35) and using m_{l,s}^{\text{T}}\left[k\right]m_{l,s}\left[k\right]=1 can yield m_{l,s}\left[k\right]\left({\widehat{d}}_{l,s}\left[k\right]-m_{l,s}^{\text{T}}\left[k\right]z_{j,s-1}\left[k\right]\right)\approx q_{l,s}\left[k\right]-z_{j,s-1}\left[k\right], which, in turn, allows us to rearrange Eq. (34) into

From Eqs. (24), (36), we can see that the cooperative sensing scheme depends on the position, relative distance, and relative direction measurements of the nodes, \left\{p_{j,s}\left[k\right],d_{j,s}\left[k\right],m_{j,s}\left[k\right]\right\}for j\in \mathcal{N}, which can be directly accessed from their onboard sensors. Therefore, we implement Eq. (36) instead of Eq. (34) in actual application scenarios.

To analyze the error dynamics of the cooperative vehicle-infrastructure sensing scheme using Eq. (36), we first derive the corresponding network-wide error recursion model. Let {\beta }_{j,s}\left[k\right]=p^{°}\left[k\right]-z_{j,s}\left[k\right] be the error of node j\in \mathcal{N} in estimating the unknown p^{°}\left[k\right] using the sth sample in time interval k. Similarly, let {\tilde{\varphi }}_{j,s}\left[k\right]=p^{°}\left[k\right]-{\varphi }_{j,s}\left[k\right] for j\in \mathcal{N}. Substituting Eq. (24) into Eq. (36) and recalling Eqs. (29), (32), we can obtain

We further introduce the weight matrices C and S, whose \left(l,j\right)th components are c_{l,j} and {\alpha }_{l,j}, respectively, that is, C={\left[c_{l,j}\right]}_{l,j\in \mathcal{N}} and S={\left[{\alpha }_{l,j}\right]}_{l,j\in \mathcal{N}}. According to Eqs. (29), (32), these two weight matrices are left-stochastic, that is,

where ${{1}_{N}}$ is an N × 1 column vector, the components of which are all identical to 1. Moreover, we extend the weight matrices to a higher dimension as follows, since the location measurement is of two dimensions

and we collect the learning step sizes across the network into a diagonal matrix

Using the above notation, we can obtain the following from Eq. (37)

where β_s[k] and π_s[k] are given as follows

Recursion Eq. (41) characterizes the dynamics of the location estimation errors across the whole vehicle-infrastructure sensor network. Furthermore, such error dynamics involve a certain stochastic process incurred by the measurement noises in {\pi }_s\left[k\right]. To proceed, we consider that {\eta }_{j,s}\left[k\right] is a zero-mean stochastic process whose covariance matrix is denoted by Q_{j,s}\left[k\right] and that the trace of Q_{j,s}\left[k\right] is represented by {\sigma }_{j,s}^2\left[k\right]=\text{Tr}\left(Q_{j,s}\left[k\right]\right) for all j, s, and k. Generally, such noise covariance, that is, Q_{j,s}\left[k\right], can change over time as the topology of {\mathcal{G}}_{\text{sensing}} varies. However, as the leading CAV of the mixed vehicle platoon and some other CAVs in {\mathcal{G}}_{\text{sensing}} approach the obstacle, the accuracy of obstacle detection can be improved, and their measurement noises will become small. Thus, we can assume that the variation of the noise covariance is a stationary process with a limit point. That is, for j\in \mathcal{N}, the noise covariance converges to a steady state, denoted by Q_j\left[k\right], with an increasing sample number s, that is, Q_{j,s}\left[k\right]\to Q_j\left[k\right] as s\to \infty. Now, using recursion Eq. (41) and the noise property, we can obtain the following result characterizing the mean stability of the network-wide error dynamics.

Theorem 1. Given that the learning step sizes across the sensing network {\mathcal{G}}_{\text{sensing}} satisfy 0<μ_j<2 for all j\in \mathcal{N}, the mean error of the network, that is, \mathbb{E}\left[{\beta }_s\left[k\right]\right], can converge to zero along with s. That is, \mathbb{E}\left[{\beta }_s\left[k\right]\right]\to 0 as s\to \infty.

The proof of Theorem 1 is detailed in Part A in Appendix A. To analyze the steady-state performance in the sense of the mean-square error, we resort to the energy conservation principle and argument of Ref. [39] (Subsection 23.3 in Ref. [39]) that the variance relation can be formulated as

where \Vert ·{\Vert }_{\phi }^2 and \Vert ·{\Vert }_{\Sigma \phi }^2 denote the φ- and \Sigma \phi-weighted norms of a vector, respectively. vec· is the vectorizing operator on a matrix. Σ and φ are defined as

The covariance-dependent matrix {\Lambda }_s\left[k\right] is given as

Now, let \text{Ξ}\left[k\right] be the steady-state mean-square deviation of {\mathcal{G}}_{\text{sensing}}, that is,

Using (43), we derive the following result.

Theorem 2. Suppose that the covariances of the measurement noises across the sensor network {\mathcal{G}}_{\text{sensing}} have stationary limits, that is,

Then, the steady-state mean-square deviation of {\mathcal{G}}_{\text{sensing}} can be estimated by

The proof of Theorem 2 is provided in Part B in Appendix A. This theorem provides deep insight into how the mixed vehicle platoon can benefit from both the cooperation of a set of CAVs and RSUs equipped with high-quality sensors and vehicle mobility. By Theorem 2, the steady-state mean-square estimation errors in the mean inherently depend on the variances of the measurement noises, as given in Eq. (51). In other words, decreasing the noise variances can reduce the steady-state mean-square estimation errors. To improve the accuracy, the leading CAV of the mixed vehicle platoon can properly select a set of nearby CAVs and RSUs equipped with high-quality sensors to form a cooperative sensor network. Additionally, as the CAVs’ mobile sensors in {\mathcal{G}}_{\text{sensing}} move closer to the obstacle, the steady-state mean-square errors are expected to decrease. At this point, the vehicle mobility can help improve the accuracy in obstacle detection.

Another insight provided by Theorem 2 is that the effect of some sensor nodes’ noisy measurements with relatively high noise variances can be inhibited by dynamically adapting the combination weights in \mathcal{S}. Specifically, the idea motivated by Theorem 2 is that we can reduce the weights \left\{{\alpha }_{l,j},l\in {\mathcal{N}}_j\right\}( j\in \mathcal{N}) assigned to those nodes whose measurements have high noise variances to suppress the impacts of their noisy estimations across the network. To devise such an adaptive noise-aware combination scheme, we resort to a first-order smoothing filter technique to track the variations of the nodes’ estimations over time, denoted by \left\{{\gamma }_{l,j,s}^2\left[k\right],l,j\in \mathcal{N}\right\}, that is,

For j\in \mathcal{N}, where \left\{{\zeta }_j,j\in \mathcal{N}\right\}are positive discount factors that are close to 1 but meet {\zeta }_j\in \left(0,1\right), j\in \mathcal{N}. Therefore, we propose to adapt the combination weights for cooperative estimation fusion by using the following rule

Finally, we combine Eqs. (36), (49), and (50) to devise a cooperative vehicle-infrastructure sensing algorithm estimating an unknown obstacle location in a distributed manner in Algorithm 1. Notably, the finite sample number or the iteration number for cooperative sensing, S_{\text{max}}, should be specified properly and according to the actual application scenario. As suggested by Theorem 1, Theorem 2, a sufficiently large S_{\text{max}} can result in a good convergence accuracy of the network errors. Nevertheless, owing to the limit of each time interval, S_{\text{max}} is finite and depends on the sampling frequency of the nodes. A larger time interval can provide more measurement samples collected within this interval, whereas a larger time interval may be less efficient to track a fast-changing target. However, in practice, an obstacle is usually static or slow-moving, relative to the motion of the mixed vehicle platoon in the application scenarios of obstacle collision avoidance, that is, p^{°}\left[k\right] slowly varies with k. Therefore, the leading CAV is allowed to guide the platoon to temporarily move to an adjacent lane and drive pass the static or dynamic obstacle.

Additionally, from the construction Eq. (50) integrated into Algorithm 1, we can see that any node j\in \mathcal{N} will assign larger combination weights to those neighbors with lower noise power. On the contrary, those neighbors with higher estimation noises, relative to the neighborhood noise level, will be associated with smaller combination weights. Such an adaption scheme is quite physically meaningful, as it endows the cooperative vehicle-infrastructure sensor network with good robustness and an anti-cyberattack capacity. That is, the adaptation Eq. (50) allows the nodes in {\mathcal{G}}_{\text{sensing}} to adjust their combination weights for fusing local information estimations from their neighbors on the fly. As a result, those sufficiently small weights will cut down the sensing information flows over time from malicious neighbors who send misleading measurements generated by using a different measurement model rather than a true target model. Thus, this scheme reduces the impact of the sensor mistakes of some CAVs and RSUs and helps defend against malicious cyberattacks of potential attackers pretending to be cooperative nodes while feeding their neighbors incorrect obstacle location estimations. More importantly, by using the filter tracking Eq. (49), the adaptation of combination weights neither needs the nodes to know in advance which of their neighbors are affected by which measurement model nor requires prior knowledge of which model is affecting their own measurements.

As presented in Section 3, the leading CAV of the mixed vehicle platoon can locally estimate the location of an unknown obstacle with the assistance of a cooperative vehicle-infrastructure sensor network consisting of some CAVs (including the platooning CAV itself) and RSUs. To proceed, we first propose an adaptive model predictive control approach for the leading CAV to achieve obstacle collision avoidance using the information estimation in Section 3 and the dynamics model presented in Section 2.1. The trajectory and control information of the leading CAV can further provide a reference for the following HDVs to stabilize the vehicle platoon while simultaneously avoiding obstacle collision. In the following subsections, we detail the design of the adaptive MPC for the leading CAV as well as the control law for the following HDVs.

The basic goal of the adaptive MPC controller of the leading CAV is to make the vehicle maintain a desired speed and stay in the middle of the original lane as much as possible. Obstacle collision avoidance can be achieved by explicitly imposing a set of motion constraints. To be specific, our idea is to define a (virtual) region in terms of constraints, in which the detected obstacle is located and the leading CAV is not allowed to enter its region during the prediction horizon. In addition, such region is also redefined by updating the constraints based on the new position of the vehicle at the next control interval.

Let w_{\text{lane}} and n_{\text{lane}} be the width of each lane in the road and the lane number, respectively. The leading CAV i = 0 must stay within the upper and lower bounds of the road, that is, satisfying

To further present constraints for obstacle collision avoidance, we first introduce two safe distances relative to the obstacle position in the longitudinal and lateral directions, that is, d_{x,\text{safe}} and d_{y,\text{safe}}. For notation simplicity, the local estimated position of the obstacle in time interval k, z_{j,S_{\text{max}}}\left[k\right] ( j=0,j\in \mathcal{N}), as given by Algorithm 1, can be rearranged as z_{j,S_{\text{max}}}\left[k\right]={\left[{\tilde{x}}^{°}\left[k\right],{\tilde{y}}^{°}\left[k\right]\right]}^{\text{T}}, where {\tilde{x}}^{°}\left[k\right] is the longitudinal position estimation and {\tilde{y}}^{°}\left[k\right] is the lateral position estimation. Using the location estimation of the obstacle, we can represent the geometry information on the four corners of a rectangle virtual safe zone, where the obstacle is located as follows

Where \left(x_{\text{fl},\text{safe}}\left[k\right],y_{\text{fl},\text{safe}}\left[k\right]\right), \left(x_{\text{fr},\text{safe}}\left[k\right],y_{\text{fr},\text{safe}}\left[k\right]\right), \left(x_{\text{rl},\text{safe}}\left[k\right],y_{\text{rl},\text{safe}}\left[k\right]\right), and \left(x_{\text{rr},\text{safe}}\left[k\right],y_{\text{rr},\text{safe}}\left[k\right]\right) denote the front-left, front-right, rear-left, and rear-right corner locations of the virtual safe zone, respectively.

Considering that the vehicle needs to safely drive pass the obstacle via the left lane, we can further restrict the trajectory generated by the adaptive MPC controller over the prediction time horizon to be above the line formed from the initial end of the trajectory (that is, the vehicle position at the beginning of the prediction time horizon) to the rear-left corner of the safe zone. Without loss of generality, let \text{slope}\left[k\right] and \text{intercept}\left[k\right] be the constraint parameters in terms of the slope and intercept of the line at k, respectively. The unified representation of the constraint for obstacle collision avoidance is given as the initial end of the trajectory (that is, the vehicle position at the beginning of the prediction time horizon) to the rear-left corner of the safe zone. Without loss of generality, let slope[k] and intercept[k] be the constraint parameters in terms of the slope and intercept of the line at k, respectively. The unified representation of the constraint for obstacle collision avoidance is given as

In reality, the vehicle is expected to satisfy different trajectory constraints in different situations where the spatial position of the vehicle relative to the obstacle is different. Next, as shown in Fig. 2, we consider different situations for setting slope[k] and intercept[k] in Eq. (53) as follows.

Case 1: When the leading CAV i = 0 is approaching the rear end of the obstacle and has already been in the adjacent lane, that is, x_i(t_k) ≤ x_{rl, safe}[k] and y_i(t_k) > y_{rl, safe}[k], we use the rear-left bound of the safe zone as the trajectory constraint. Namely, according to Eq. (52), we set

Case 2: When the leading CAV i=0 is approaching the rear end of the obstacle but has not moved in the adjacent lane, that is, x_i\left(t_k\right)\le x_{\text{rl},\text{safe}}\left[k\right] and y_i\left(t_k\right)\le y_{\text{rl},\text{safe}}\left[k\right], the vehicle should be above the line formed from the nominal operating point, \left(x_i\left(t_k\right),y_i\left(t_k\right)\right), to the rear-left corner of the safe zone. Hence, we can set

Case 3: When the leading CAV is already in the adjacent lane and moving in the parallel direction to the obstacle, that is, x_i\left(t_k\right)>x_{\text{rl},\text{safe}}\left[k\right] and x_i\left(t_k\right)\le x_{\text{fl},\text{safe}}\left[k\right], we also employ the rear left bound of the safe zone as the trajectory constraint as in case 1, that is,

Case 4: When the leading CAV has already passed the obstacle, that is, xitk>xfl,safek, we simply set

to make the constraint condition Eq. (53) inactive, which allows the vehicle to drive back to the center lane.

According to Eqs. (51), (53), we are allowed to represent the constraints for obstacle collision avoidance as a compact form of mixed input/output (I/O) constraints

where u_i\left[t|k\right] and s_i\left[t|k\right] are the control input and state output variables obtained from the state-space model given in Eq. (6), respectively. K_u\left[k\right], K_s\left[k\right], and G\left[k\right] are the compatible matrices defined by