A. Simulation Platform and Cooperative Driving Frameworks

In the field of cooperative driving research, simulation platforms and frameworks play a crucial role. Researchers utilize these tools to develop and test collaborative driving algorithms, assessing their performance and stability. In this section, we will review current popular simulation platforms in academic field, such as SUMO, MATLAB/Simulink, and CARLA, as well as cooperative driving frameworks built upon them.

SUMO is an open-source, highly portable, microscopic, and continuous traffic simulation package designed to handle large networks.

Segata et al. [8]develop Plexe based on SUMO and OMNeT++ for studying cooperative driving systems with a modular approach. This simulation toolkit is very popular in the study of platooning control, vehicle-to-vehicle (V2V) and vehicle-to-infrastructure communication (V2X).

Karl et al. [9] proposed MOSAIC, a multi-domain simulation framework integrated with software like SUMO, OMNeT++, and ns-3, for deploying and testing advanced traffic control strategies. It facilitates the coupling of leading simulators across traffic, application, and communication domains.

However, cooperative driving frameworks based on SUMO primarily focus on macroscopic traffic flow and mobility patterns rather than detailed physical interactions. This limitation restricts the ability to simulate cooperative driving scenarios that require high-fidelity vehicle dynamics.

MATLAB is a widely used numerical computing environment and its Simulink platform supports multi-domain simulation and model-based design, which is essential for the development of vehicle control systems. Leveraging its rich set of libraries, researches can simulate the dynamic behavior of vehicles with high precision.

Palmieri et al. [10] introduced a co-simulation approach using MATLAB Simulink to create high-fidelity digital twin models of distributed multi-agent cyber-physical systems (CPSs), using the platoon system as a case study. Their method employs the FMI co-simulation standard to handle CPS complexity by breaking them into interconnected components for specific domains.

Chen et al. [11] utilized a co-simulation approach combining PreScan, Matlab, and Vissim to construct a platform for evaluating the cooperative driving behavior of autonomous vehicles. Their methodology focuses on the interaction between autonomous and human-driven vehicles, offering valuable insights into the safety performance of cooperative driving systems.

Despite MATLAB/Simulink offers the most comprehensive vehicle dynamics simulation ecosystem, the proprietary nature of MATLAB/Simulink may hinder wider community collaboration as most of the code based on it is not publicly available.

Carla is an open-source autonomous driving simulation platform that offers a flexible and scalable environment for testing and developing the autonomous vehicles. It features a high-fidelity rendering game engine and supports various sensors and controllers.

Xu et al. [12] introduced OpenCDA, a widely used open-source cooperative driving framework based on Carla. This comprehensive framework provides modules for perception, localization, and vehicle control, abstracting various elements to facilitate cooperative driving in different scenarios.

Carletti et al. [13] enhanced OpenCDA by integrating advanced communication models from ns-3, thereby providing researchers with a comprehensive tool to evaluate the impact of vehicular cooperative perception under various communication technologies.

Liu et al. [14] introduced an optimization-based approach for cooperative driving in urban environments. They integrated Artificial Potential Fields (APF) into a Model Predictive Control (MPC) scheme to represent traffic rules, effectively handling cooperative driving scenarios in CARLA simulations.

CARLA strikes a balance between physical fidelity and computational efficiency. Its open-source nature has led to increasing popularity in academia, and its flexible and rich interface design makes it an excellent choice for establishing cooperative driving simulation platforms.

B. Asynchronous problems in cooperative driving

Xu et al. [15] noted that traditional synchronization updates, such as global blocking synchronization, are inefficient due to processes waiting at the barrier. Consequently, high synchronization overhead has consistently been a significant performance issue in large-scale parallel agent-based traffic simulations.

Bin et al. [16] investigated synchronous and asynchronous update methods in traffic simulation. They suggest that asynchronous updates may introduce inconsistency in the simulation results since identical vehicles tend to perceive different traffic scenarios on the same simulation step. In contrast, synchronous updates ensure all vehicles perceive the same traffic scenario.

Zhan et al. [17] highlight that in real-world scenarios, each agent updates its dynamics at distinct time intervals defined by its own clock, leading to delay and inconsistency between the ground truth and measurements. They explored the problem of asynchronous platoon cooperative control under these conditions, taking into account intermittent and delayed information transmission. The researchers proposed an asynchronous consensus-based protocol for first-order multi-agent systems with switching topologies, accommodating large but bounded update intervals and communication delays.

Hu et al. [18] further classified the asynchronous problem into the mismatched and matched disturbances and estimated it through a finite-time disturbance observer.

Vehicle Agent is a crucial component in Intelligent Transportation Systems and a vital unit in cooperative driving. In this chapter, we will explain in detail the implementation of the Vehicle Agent to facilitate future researchers in developing advanced cooperative driving algorithms using our basic Vehicle Agent model.

Due to the high computational performance requirements of the perception workflow, we chose to implement autonomous driving vehicles as control-in-the-loop rather than perception-in-the-loop. This means that information related to obstacles and maps is directly obtained from the simulator. The Autonomous Vehicle class in the ACDCarla framework comprises several components, such as the communication manager, sensor manager, global router etc. These components become sub-objects in the agent class at initialization, and the control logic is concentrated in the planner's control loop to complete route planning, perception, decision-making, and control functions.

The Vehicle Agent generates an A* global router planner that builds a topology and a graph representation of the road network using the waypoints and road segments provided by the CARLA HD map. It then employs the A* search algorithm to find the optimal path between an origin and a destination location within the graph.

The primary autonomous driving control logic resides in the Frenet local planner's control loop. During each control loop iteration, the planner first updates the ego vehicle state, processes obstacle information, and checks the local waypoint buffer.

The local planner calculates the curvature $c$ of the waypoint buffer using three consecutive points: $p_1$, $p_2$, and $p_3$:

$c = 1 - \frac{\overrightarrow{p_1 p_2}}{|\overrightarrow{p_1 p_2}|} \cdot \frac{\overrightarrow{p_2 p_3}}{|\overrightarrow{p_2 p_3}|}$

The target speed $v_t$ is then determined based on the total curvature $C$ of the waypoint buffer, calculated as $C = \sum_{i=1}^{n-1} c_i$, using the formula:

$v_t = \begin{cases} \max(10, v_m \cdot e^{-6C}) & \text{if } C > 0.0001 \\ v_m & \text{otherwise} \end{cases}$

where $v_m$ is the maximum speed and $n$ is the number of points in the buffer. This allows the vehicle to slow down appropriately when approaching turns or curves, ensuring smooth and safe navigation.

The system incorporates a collision avoidance check on the future waypoints buffer, which is based on the occupancy grid and considers both the current positions of obstacles and their predicted positions 3 seconds into the future. The prediction baseline method employs a simple physical dynamics model for forecasting. If the obstacles do not occupy the grid cells corresponding to the future waypoints, the system directly applies the cruising algorithm to reach the target speed, which by default utilizes the Intelligent Driver Model (IDM) algorithm, as shown in the following equation:

$\lbrace{ \begin{aligned} a &= \max\left(-b_{\max}, a_{\max}\left[1 - \left(\frac{v}{v_0}\right)^{\delta} - \left(\frac{s^*}{s}\right)^2\right]\right) \\ s^* &= \max\left(s_0 + vT + \frac{v\Delta v}{2\sqrt{a_{\max}b}}, s_0\right) \end{aligned} }$

Variable	Description
$a$	Acceleration of the ego vehicle
$b_{\max}$	Maximum brake deceleration
$a_{\max}$	Maximum acceleration
$v$	Current speed of the ego vehicle
$v_0$	Speed limit or desired speed
$\delta$	Acceleration exponent
$s_0$	Minimum safe distance
$T$	Human reaction time
$\Delta v$	Speed difference between the ego vehicle and the front vehicle (ego_v - front_v)
$b$	Normal deceleration
$s$	Current distance between the ego vehicle and the front vehicle

This approach ensures that the collision avoidance mechanism takes into account not only the real-time positions of obstacles but also their anticipated trajectories in the near future.

we utilize B-spline curves to generate smooth and feasible trajectories for the ego vehicle. The input data points, representing the waypoints of the ego vehicle's future path, are first preprocessed and then fitted using a B-spline curve approximation technique. The resulting B-spline curve is defined as a linear combination of B-spline basis functions and control points, as shown in Equation (1):

\begin{equation} \textbf{C}(u) = \sum_{i=0}^{n} \textbf{P}i N{i,p}(u) \end{equation}

where $\textbf{C}(u)$ is the point on the B-spline curve at parameter value $u$, $\textbf{P}i$ are the control points, $N{i,p}(u)$ are the B-spline basis functions of degree $p$, and $n$ is the number of control points.

Design of the platooning scenario and platoon vehicle agent

$v_{i}(t) = f(v_{i-1}(t), a_{i-1}(t), d_{i}(t), v_{i+1}(t), a_{i+1}(t), v_{i}(t), a_{i}(t), \tau_{i-1}, \tau_{i+1}, v_{i-1}^{target}(t))$

where $v_{i}(t)$ is the speed of the $i$-th vehicle at time $t$, $v_{i-1}(t)$ is the speed of the leader vehicle at time $t$, $a_{i-1}(t)$ is the acceleration of the leader vehicle at time $t$, $d_{i}(t)$ is the distance between the $i$-th vehicle and its preceding vehicle at time $t$, $v_{i+1}(t)$ is the speed of the front vehicle at time $t$, $a_{i+1}(t)$ is the acceleration of the front vehicle at time $t$, $v_{i}(t)$ is the speed of the ego vehicle at time $t$, $a_{i}(t)$ is the acceleration of the ego vehicle at time $t$, $\tau_{i-1}$ and $\tau_{i+1}$ are the communication delays from the leader and front vehicles, respectively, which can be used as parameters depending on the specific implementation, $v_{i-1}^{target}(t)$ is the target speed of the leader vehicle at time $t$.

The longitudinal control of the vehicle is performed using a PID controller. The control output $out(t)$, representing the throttle or brake command, is calculated based on the following equation:

$out(t) = K_P e(t) + K_I \int_{0}^{t} e(\tau) d\tau + K_D \frac{de(t)}{dt}$

where $e(t)$ is the error between the target speed and the current speed at time $t$, given by:

$e(t) = v_{target}(t) - v_{current}(t)$

Here, $v_{target}(t)$ is the target speed in m/s at time $t$, and $v_{current}(t)$ is the current speed of the vehicle in m/s at time $t$. The proportional gain $K_P$ is set to 1.0, the integral gain $K_I$ is set to 0.05, the derivative gain $K_D$ is set to 0, and the time step $dt$ is set to 0.03 seconds.

The integral term $\int_{0}^{t} e(\tau) d\tau$ is approximated using the sum of errors over a buffer of size 10:

$\int_{0}^{t} e(\tau) d\tau \approx \sum_{i=0}^{9} e(t - i \cdot dt) \cdot dt$

The derivative term $\frac{de(t)}{dt}$ is approximated using the difference between the current error and the previous error:

$\frac{de(t)}{dt} \approx \frac{e(t) - e(t - dt)}{dt}$

The control output $u(t)$ is then clipped to the range [-1, 1] to ensure that the throttle and brake commands remain within valid ranges. If $u(t) \geq 0$, it represents the throttle command, and if $u(t) < 0$, the absolute value represents the brake command.

I apologize for the oversight. Let me clarify the meaning of $\tau$ in the equations.

The longitudinal control of the vehicle is performed using a PID controller. The control output $u(t)$, representing the throttle or brake command, is calculated based on the following equation:

$u(t) = K_P e(t) + K_I \int_{0}^{t} e(\tau) d\tau + K_D \frac{de(t)}{dt}$

where $e(t)$ is the error between the target speed and the current speed at time $t$, given by:

$e(t) = v_{target}(t) - v_{current}(t)$

Here, $v_{target}(t)$ is the target speed in km/h at time $t$, and $v_{current}(t)$ is the current speed of the vehicle in km/h at time $t$. The proportional gain $K_P$ is set to 1.0, the integral gain $K_I$ is set to 0.05, the derivative gain $K_D$ is set to 0, and the time step $dt$ is set to 0.3 seconds.

In the integral term $\int_{0}^{t} e(\tau) d\tau$, $\tau$ is a variable representing time in the integration. It is used to indicate that the error is integrated over the time interval from 0 to the current time $t$. The integral term is approximated using the sum of errors over a buffer of size 10:

$\int_{0}^{t} e(\tau) d\tau \approx \sum_{i=0}^{9} e(t - i \cdot dt) \cdot dt$

The derivative term $\frac{de(t)}{dt}$ is approximated using the difference between the current error and the previous error:

$\frac{de(t)}{dt} \approx \frac{e(t) - e(t - dt)}{dt}$

The control output $u(t)$ is then clipped to the range [-1, 1] to ensure that the throttle and brake commands remain within valid ranges. If $u(t) \geq 0$, it represents the throttle command, and if $u(t) < 0$, the absolute value represents the brake command.

The longitudinal PID controller aims to minimize the error between the target speed and the current speed of the vehicle by adjusting the throttle and brake commands based on the proportional, integral, and derivative terms of the error. The specific gains ($K_P$, $K_I$, $K_D$) and time step ($dt$) were chosen to achieve a satisfactory control performance.

(1) PATH CACC controller

The ACC and CACC car-following models of Milanés and Shladover were used as the basic simulation models (9). The responses of ACC followers were modeled as a second-order transfer function and are described by

where $e_{i,k}$ is the gap error of vehicle $i$ at time step $k$. Equation 3 shows that vehicle acceleration depends on a gap error and a speed difference with the preceding vehicle, where their feedback gain $k_{1}$ and $k_{2}$ is 0.23 s$^{-2}$ and 0.07 s$^{-1}$​, respectively.

The classic CACC [50] implementation by California Berkeley PATH Laboratory is a widely used method. The PATH CACC control law is designed using a proportional-derivative (PD) controller and is given by:

where $\nu_{i,k}$ is the control input for vehicle $i$ at time step $k$, $e_{i,k}$ is the gap error, and $\dot{e}_{i,k}$ is its derivative The parameters used in the control law are described in the following table:

Parameter	Default Value	Description
$k_p$	0.45	Proportional gain for gap error
$k_d$	0.25	Derivative gain for gap rate error
$d_0$	2 m	Standstill distance
$t_{\mathrm{des}}$	0.5 s	Desired time-gap

The control law aims to regulate the gap between vehicle $i$ and its preceding vehicle $i-1$ by adjusting the control input based on the gap error and gap rate error. The proportional and derivative gains, $k_p$ and $k_d$, determine the response of the controller to the errors. The standstill distance, $d_0$, and the desired time-gap, $t_{\mathrm{des}}$, are used to calculate the desired gap.

Apologies for the confusion. Let me revise the academic paper section with the suggested changes:

CACC Algorithm

class Adaptive_CACCController:
    def __init__(self, headway_time=0.5, speed_control_gain=0.25, gap_control_gain_gap=-0.45,
                 gap_control_gain_gap_dot=0.125, collision_avoidance_gain_gap=0.45,
                 collision_avoidance_gain_gap_dot=0.05, emergency_threshold=2.0, headway_time_acc=1.0,
                 speed_control_min_gap=0.06, feedforward_gain=1.0, observer_gain=0.1, sliding_mode_gain=0.1,
                 spacing_err_queue_size=10, speed_err_queue_size=10):
        self.headway_time = headway_time
        self.speed_control_gain = speed_control_gain
        self.gap_control_gain_gap = gap_control_gain_gap
        self.gap_control_gain_gap_dot = gap_control_gain_gap_dot
        self.collision_avoidance_gain_gap = collision_avoidance_gain_gap
        self.collision_avoidance_gain_gap_dot = collision_avoidance_gain_gap_dot
        self.emergency_threshold = emergency_threshold
        self.headway_time_acc = headway_time_acc
        self.control_min_gap = speed_control_min_gap
        self.feedforward_gain = feedforward_gain
        self.observer_gain = observer_gain
        self.sliding_mode_gain = sliding_mode_gain
        self.spacing_err_queue = deque(maxlen=spacing_err_queue_size)
        self.speed_err_queue = deque(maxlen=speed_err_queue_size)
        self.ti = 0 

    def calc_speed(self, leader_v, leader_a, front_dis, front_v, front_a, ego_v, ego_a,
                   leader_delay=0, front_delay=0, leader_tv=0, is_radar = False):


        # Feedforward control with nonlinear acceleration profile
        feedforward_acc = self.feedforward_gain * (leader_tv - ego_v)
        feedforward_acc *= math.tanh(leader_a * leader_delay) * (leader_tv - leader_v)

        leading_tv_err = leader_tv -ego_v
        spacing_err = front_dis - self.headway_time * ego_v - self.emergency_threshold 
        speed_err = front_v - ego_v 



        # Error observer with queue for spacing and speed errors
        self.spacing_err_queue.append(spacing_err)
        self.speed_err_queue.append(speed_err)
        observed_spacing_err = sum(self.spacing_err_queue) / len(self.spacing_err_queue)
        observed_speed_err = sum(self.speed_err_queue) / len(self.speed_err_queue)

        # Sliding mode control with adaptive gain adjustment
        sliding_mode_gain_v = math.tanh(abs(observed_speed_err) / self.control_min_gap) * self.sliding_mode_gain
        sliding_mode_gain_gap = math.tanh(abs(observed_spacing_err) / self.control_min_gap) * self.sliding_mode_gain

        # Dynamic gain adjustment
        gap_control_gain_gap = self.gap_control_gain_gap * sliding_mode_gain_gap
        gap_control_gain_gap_dot = self.gap_control_gain_gap_dot * sliding_mode_gain_gap
        speed_control_gain = self.speed_control_gain

        if abs(observed_spacing_err) > self.control_min_gap:
            gap_control_gain_gap *= (1 + sliding_mode_gain_v)
            gap_control_gain_gap_dot *= (1 + sliding_mode_gain_gap)
        else:
            speed_control_gain *= (sliding_mode_gain_gap)

        # Safety constraints (remains the same as before)
        if front_dis < self.emergency_threshold:
            feedforward_acc = -5.0
            speed_control_gain = 0.0
            gap_control_gain_gap_dot = 0.0


        output = (feedforward_acc +
                speed_control_gain * leading_v_err -
                gap_control_gain_gap * observed_spacing_err +
                gap_control_gain_gap_dot * observed_speed_err)

The lower-level control of the vehicle is handled by a PID controller, which receives the target waypoint and target speed from the planner. The PID controller consists of two sub-controllers: lateral and longitudinal.

The longitudinal PID controller calculates the acceleration output based on the following equation:

$a = K_P e + K_I \int_0^t e(\tau) d\tau + K_D \frac{de}{dt}$

where $a$ is the acceleration output, $e$ is the error between the target speed $v_t$ and the current speed $v_c$, i.e., $e = v_t - v_c$, $K_P$ is the proportional gain, $K_I$ is the integral gain, $K_D$ is the derivative gain, and $dt$ is the time step. The integral term $\int_0^t e(\tau) d\tau$ represents the accumulated error over time, which is calculated using the following approximation:

$\int_0^t e(\tau) d\tau \approx \sum_{i=0}^{n} e(i) \cdot dt$

where $n$ is the number of time steps elapsed since the start of the control process, and $e(i)$ is the error at time step $i$. This approximation is known as the trapezoidal rule for numerical integration.

The derivative term $\frac{de}{dt}$ represents the rate of change of the error, which is calculated using the finite difference approximation:

$\frac{de}{dt} \approx \frac{e(t) - e(t - dt)}{dt}$

where $e(t)$ is the current error and $e(t - dt)$​ is the error at the previous time step.

To translate the given Python code into mathematical notation rendered in LaTeX, we first need to understand the variables and the operations being performed. The code snippet appears to manage vehicle control in a simulation, using acceleration and steering values to adjust throttle and brake levels.

Here are the necessary variables and their purpose:

• (a): acceleration (computed by
  self._lon_controller.run_step(target_speed)
  ).
• (s): current steering angle (computed by
  self._lat_controller.run_step(waypoint)
  , though it is not directly used in the throttle/brake logic).
• (throttle): the throttle value to be applied to control the vehicle's acceleration.
• (brake): the brake value to be applied to control the vehicle's deceleration.
• (max_{throt}): the maximum throttle limit.
• (max_{brake}): the maximum brake limit.

Given the code, we can define the control logic for throttle and brake as follows:

1. Throttle and Brake Control:

a_{ff} = \kappa \cdot (v_{t} - v) \cdot \tanh(a_l \cdot \tau_l) \cdot (v_{t} - v_l),

\hat{s}^{(k)} = \hat{s}^{(k-1)} + K_s \cdot (s - \hat{s}^{(k-1)}),

\sigma_s^{(k)} = (1 - K_s) \cdot \sigma_s^{(k-1)} + \eta,

\hat{v}^{(k)} = \hat{v}^{(k-1)} + K_v \cdot (v_e - \hat{v}^{(k-1)}),

\sigma_v^{(k)} = (1 - K_v) \cdot \sigma_v^{(k-1)} + \eta,

g_{s} = g_{s0} \cdot \tanh\left(\frac{|\hat{s}|}{d_{min}}\right) \cdot g_m,

g_{v} = g_{v0} \cdot \tanh\left(\frac{|\hat{v}|}{d_{min}}\right) \cdot g_m,

v_i = v_0 + a_{ff} + g_v \cdot (v_l - v_i) + g_s \cdot \hat{s} + g_{s_d} \cdot \hat{s}_d,

a_{ff} = \kappa_{ff} \cdot (v_t - v_i) \cdot \tanh(a_l \cdot \tau_l) \cdot (v_t - v_l),

\hat{s}{k} = \hat{s}{k-1} + K_s \cdot (s_k - \hat{s}_{k-1}),

\sigma_{s,k} = (1 - K_s) \cdot \sigma_{s,k-1} + \eta,

\hat{s}{d,k} = \hat{s}{d,k-1} + K_{s_d} \cdot (s_{d,k} - \hat{s}_{d,k-1}),

\sigma_{s_d,k} = (1 - K_{s_d}) \cdot \sigma_{s_d,k-1} + \eta,

g_s = g_{s,0} \cdot \tanh\left(\frac{|\hat{s}|}{d_{min}}\right) \cdot g_m,

g_{s_d} = g_{s_d,0} \cdot \tanh\left(\frac{|\hat{s}d|}{d{min}}\right) \cdot g_m,