Hardware‐in‐the‐loop validation of an adaptive model predictive control on a connected and automated vehicle

Connected and automated vehicles will characterize the future of the mobility, featured by Smart Roads and Internet of Things technologies. Vehicles will behave as mobile nodes of a network enabling communication between them and with respect to the road infrastructure. In order to provide advanced driver assistance and automated driving along a Smart Road, control strategies need to take into account both parametric variations due to environmental and/or weather conditions (e.g., wind speed, presence of ice, and/or rain), and constraints related to road safety and vehicular traffic. For this reason, advanced adaptive control strategies result particularly suitable to provide a complete control of the vehicle. This article proposes an extension to a previous conference paper, concerning a Processor in the Loop testing of an Adaptive Model Predictive Control for coupled longitudinal/lateral vehicle dynamics, where the ego vehicle was simulated through a bicycle model. In the present work, a low‐cost real‐time Hardware In the Loop test bench, featured by Raspberry boards, is used to verify the control strategy in a way closer to a real application. A four‐wheel vehicle dynamic model is considered to simulate the ego vehicle, then robustness and sensitivity tests with respect to parametric variations related to road friction, wind speed and vehicle mass, and a comparison with an existing adaptive MPC are performed. The achieved results show good robustness properties, safety performances and confirm the real‐time execution of the control strategy, paving the way for future developments, where the goal will be the testing in a real road scenario by using an IoT gateway purposefully designed by the NetCom company.


INTRODUCTION
defined here as vehicle-based intelligent comfort and safety systems, which could improve road safety in terms of crash avoidance, crash severity mitigation and protection, and post-crash phases. ADAS allow the vehicle to be automated and are based on an extensive use of multiple and heterogeneous sensors (e.g., radar, camera, and lidar), with the aim to obtain a more comprehensive description of the environment. 1 Modern and future vehicles will achieve not only advanced automation levels, such as High Automation (L4) and Full Automation (L5) defined by the Society of Automotive Engineers (SAE International), 2 but will also benefit from the Internet of Things (IoT) technologies becoming connected elements of a network. In particular, these technologies will enable the communication between vehicles and the road infrastructure, which will aim to improve driving awareness and prevent unknown intentions of other road users. 3 With this in mind, Cooperative-Intelligent Transport Systems (C-ITS) aim at connecting vehicles among them and with the road infrastructure, the so-called Smart Road, in order to increase traffic safety and efficiency. 4 Thus, the combination of ADAS and C-ITS results in a Connected and Automated Vehicle (CAV), which will be able to support a wide range of ITS applications, resulting permanently connected to the elements of the Smart Road. 5 Controlling a CAV requires modeling of a complex Multi-Input Multi-Output (MIMO) system featured by time-varying parameters and constraints. In particular, the control strategies of CAVs must consider aspects related to vehicle dynamics (e.g., keeping the vehicle within the lane), parametric variations concerning road, weather and environmental conditions (e.g., road slope, wet asphalt, strong wind gusts), and constraints deriving from both the ego vehicle (e.g., actuator limits and allowed values for the output variables) and the interaction with other vehicles and the environment (e.g., safety distance and speed limits imposed by the Smart Road). Similarly, the control law must react safely to unpredictable braking and/or behavior deriving from other connected vehicles. Therefore, one of the most suitable control strategies is the adaptive Model Predictive Control (MPC), because of its capability to solve tracking problems for MIMO systems, manage constraints on both manipulated and output variables, and given its adaptive behavior against time-varying parameters.
This work proposes an extension to a previous conference paper, 6 related to an early stage of the research project C-Mobility (POR Campania FESR 2014-2020). In that work, an MPC was presented considering a vehicle bicycle model both for predictions and to simulate the ego vehicle. The control strategy was able to perform different ADAS functionalities, such as Adaptive Cruise Control (ACC), Lane Keeping System (LKS) and Electronic Emergency Brake (EEB). In order to verify MPC execution times on a real Micro-Controller Unit (MCU), a Processor-In-the-Loop (PIL) testing was performed, using the development board ST NUCLEO-H743ZI2. The main objective was to verify that the cycle execution time of the proposed MPC was lower than 10 ms, the typical cycle time of CAN messages.
The present work aims at the design of an updated MPC and a more realistic real-time validation process, featured by a low-cost Hardware-In-the-Loop (HIL) test bench in which a new embedded board (Raspberry Pi 4) is considered to run the control law. In particular, the MPC is based on the same prediction model but it is designed using the adaptive MPC of the MATLAB/Simulink toolbox, 7 considering the latest optimization solvers. Furthermore, the low-cost HIL test bench allowed not to use expensive real-time simulators, making possible a validation of the control law considering a more realistic simulation scenario. The use of the Raspberry Pi 4 is due to the developments of the C-Mobility research project in the last year, which led to the design of a more performing in-vehicle IoT Gateway (Italian patent 8 ) by the NetCom Engineering company. This device will provide the vehicle with the real connectivity to the Smart Road (for on road tests by the end of 2023) and allow the real-time execution of the Adaptive MPC.
Furthermore, the simulation scenario for the HIL testing is based on more realistic models, considering a 4-wheel vehicle model for the ego vehicle dynamics, nonlinear equations to simulate the position of the vehicle in the lane, in terms of errors with respect to the road (lateral displacement and yaw angle error), a simple car-following model, and time varying parameters related to the Smart Road (road slope, reference speed, safety distance), environmental and weather conditions (wind speed, road friction, ahead vehicle parameters). The whole simulation scenario is executed on a Raspberry Pi Zero W board, connected through CAN and LAN channels to the Raspberry Pi 4 board running the adaptive MPC. The control strategy receives data and parameters through real communication channels that emulate the automotive in-vehicle networks (e.g., B-CAN, C-CAN, FlexRay), providing the 4-wheel vehicle model with the torque and steering angle at the wheels.
Eventually, in order to check the effectiveness of the proposed control strategy and to contrast the obtained results with previously published methods on the MPC strategy, a comparison is performed in terms of manipulated and output variables, as well as of execution time on the Raspberry Pi 4 board. In particular, with respect to the state of art, the proposed adaptive MPC highlights the benefits of the vehicle connectivity with the Smart Road, which implied the use of a prediction model structurally more suitable for CAVs.
The remainder of the article is organized as follows. Section 2 is a revision of the state-of-the-art advanced control strategies for ADAS, CAVs and HIL testing. Section 3 describes the methodology used for this work, considering the complete nonlinear ego vehicle model, the design of the adaptive MPC and the experimental setup for HIL testing. Section 4 shows the obtained results, analyzing the performance of the adaptive MPC, the PIL and HIL tests and the comparisons with previously published methods. Section 5 presents final remarks and future developments.

RELATED WORK
In this section, the scientific background related to advanced control strategies for ADAS and CAVs is introduced. Then, real-time HIL testing methodologies treated in existing literature are highlighted.

Advanced control strategies for ADAS and CAVs
Controlling a CAV requires the use of nonlinear time-varying models not only to simulate both the vehicle and the environment, but also to design an efficient control strategy. Moreover, the controlled vehicle is subject to multiple constraints, involving both actuator limits and constraints on maneuvers. Thus, because of the need to exploit available models and satisfy constraints in control systems, advanced control strategies, such as MPC, are particularly suitable for ADAS and CAVs. 3 Regarding ADAS, there are many works in the literature in which MPC for ACC, EEB and LKS [9][10][11][12][13][14] has been proposed. A recent comparative study between different control strategies for ADAS has been presented in Reference 15, analyzing Stanley, LQR (Linear Quadratic Regulator) and MPC controllers for path tracking applications (ADAS L4 and L5), where the latter resulted to be the best one. In Reference 16 a learning-based approach has been studied, considering a framework for autonomous driving which can learn from human demonstrations, applied to the longitudinal control of an autonomous car. In Reference 17 a novel learning MPC technique has been applied to the autonomous racing problem. The controller allowed the vehicle to operate at the limit of its handling capability, as proved by tests in simulation with CarSim. In Reference 18 nonlinear MPC and Moving Horizon Estimation (MHE) have been proposed to deal with highly nonlinear, constrained, unstable and fast dynamic systems. Eventually, in Reference 19 a context-aware MPC control has been presented, aimed at improving fuel consumption and comfort.
The interest in control systems for CAVs is quite recent, mainly since 2018. Many topics have been studied, [20][21][22] considering aspects related to both the road infrastructure and vehicles. In particular, the main topics concern platooning control, [23][24][25] Cooperative Adaptive Cruise Control (CACC), 26 cooperative driving with multiple CAVs (e.g., during lane changing) in mixed traffic conditions featured by human driven and automated vehicles, 27,28 maneuvers management at roundabouts 29 and intersections, [30][31][32] and control strategies for CAVs involving Energy Management Systems (EMS). [33][34][35] From the state-of-the-art literature, MPC results to be the most applied control strategy, used for both cooperative purposes and for longitudinal/lateral vehicle control. In particular, most of the recent published methods concern predictive control strategies for ADAS and Automated Vehicles (AVs), mainly adaptive 36-39 and nonlinear. 40,41 The Adaptive MPC is considered one of the most suitable control strategies for CAVs, since it allows the vehicle to cope with time-varying parameters also during the prediction horizon. These parameters (e.g., speed limits, safety distances, road friction, road slope, wind speed and road curvature) could be provided in advance and point-by-point by the Smart Road so as to allow the control action to adapt over the above horizon. 42 Furthermore, the Adaptive MPC results into a good trade-off between tracking performances and computational load, compared to more elaborate algorithms based on nonlinear MPC. 43 Different applications have been discussed in literature, concerning lateral and longitudinal dynamics in autonomous driving, 36 comfort optimization, 38 lane keeping and lateral control, 13,44 path planning, 45 and trajectory tracking. 46 Thus, in view of the numerous applications in terms of ADAS and CAVs and given its characteristics in managing constrained optimal problems based on MIMO prediction models and time-varying parameters, the adaptive MPC is the most suitable choice for an industrial application to be executed in real-time on an embedded hardware device.

2.2
Hardware in the loop testing for ADAS and CAVs Different 50 radar sensor target simulation, 51 driver and/or human in the loop, 52 and rapid prototype testing, 53 which is the focus of this work. In Reference 11 a longitudinal collision avoidance and lateral stability adaptive control system, based on MPC, has been proposed and tested in HIL through a National Instruments (NI) PXIe. An HIL test bench for CAVs, based on dSPACE Scalexio, has been proposed in Reference 54. An implementation and HIL validation of SAE level 2 automated vehicle, based on a dSPACE simulator, has been proposed in Reference 55. In Reference 25 the authors described different HIL test-beds (e.g., dSPACE, NI), proposing a low-cost HIL platform to check the robustness of a Distributed Adaptive MPC, for vehicle platoon control, with respect to communication delays. A Processor In the Loop (PIL) validation of an adaptive MPC including nonlinear ego vehicle dynamics and a simple car-following model has been proposed in Reference 6, based on a very low-cost testbed. However, although PIL testing is able to verify the real-time execution of C Code generated for embedded systems, it is not a real-time test, since it is based on an asynchronous communication between the embedded board and the PC-Host.
Since the proposed adaptive MPC is a high-level controller, that is an intermediate layer between the Smart Road and the vehicle power-train, which gives references to low-level controllers, supposed to be ideal and faster in the present work, there is no need to configure expensive simulators (e.g., dSPACE, NI). In general, these simulators are used for testing low-level controllers, since the number of ECUs (Electronic Control Units) and actuators (real and/or emulated) to be considered is high, and more complex models are needed (e.g., electric motors, internal combustion engine, transmission and vehicle dynamics). Thus, since the aim of this work is to verify the real-time performance of an adaptive MPC for execution on an IoT gateway, based on a Raspberry Pi 4, a simpler HIL test bench results to be suitable, low-cost and implies a faster testing procedure.

METHODOLOGY
This section presents the models used for the simulation scenarios. In particular, a 4-wheel longitudinal/lateral vehicle dynamics model is used to simulate a more realistic ego vehicle (3.1). In order to get the position of the ego vehicle on the lane, a model based on lateral displacement and yaw-angle error is shown (3.2). Then, a simple car-following model (3.3) is considered to obtain the ahead vehicle speed and the relative distance. Furthermore, the parameters related to the vehicles, road and environment are provided. In addition, a few details about the IoT gateway are given (3.5), emphasizing the difference between the feasibility of the real road test and the proposed HIL. Then, the design of the adaptive MPC is described (3.6), considering a simplified prediction model based on the longitudinal/lateral single-track vehicle dynamics, the position of the ego vehicle on the lane and the simple car-following model. Then the constrained optimization problem and the whole control scheme are reported.
Hereafter, all quantities are expressed in SI units unless otherwise explicitly stated.

Four-wheel vehicle dynamics
This section reports vehicle and tire dynamics equations in Sections 3.1.1 and 3.1.2, respectively. In particular, a 4-wheel model is used to represent the ego vehicle dynamics in a more realistic way, hence to validate the proposed control strategy (based on a simplified prediction model such as the single-track one, see Section 3.6.1) both in PIL and HIL tests, and to verify its robustness against parametric variations.

Four-wheel model
Longitudinal and lateral vehicle dynamics have been obtained from the 4-wheel model presented in Reference 56, taking into account additional terms related to the presence of road grade and rolling resistance forces. All the forces, parameters and variables are defined below with reference to the vehicle body (top-view) and longitudinal forces (side-view), shown in Figures 1 and 2, respectively. The whole coupled longitudinal and lateral vehicle dynamics model is characterized by the following equations: where v x is the longitudinal speed, v y the lateral speed and the yaw rate. The parameter m is the vehicle mass, F aero is the aerodynamic drag force, defined as F aero = 0.5 AS d (v x + v w ) 2 , being = 1.225 kg/m 2 the air density, A is the maximum vehicle cross area defined in Reference 57 as an approximated relationship depending on the car mass m if this is in the range [800, 2000] kg: A = 1.6 + 0.00056(m − 765), S d is the drag coefficient depending on vehicle body shape and v w is the wind speed. The term F r = F rf + F rr refers to the sum of the front and rear wheels rolling resistance forces, defined as F r = C r (F zf + F zr ) = C r mg cos , where C r = 0.01(1 + v x ∕27.77) is the rolling resistance coefficient, 58,59 F zf and F zr are the normal tire forces, at the front and at the rear of the vehicle, respectively, calculated as in Reference 57 and their sum is mg cos . The term mg sin , is the gravity force due to the road slope angle . The term I w is the moment of inertia of the wheel, R eff is the effective wheel radius, while the terms ij and w ij , where the subscripts ij indicate the positioning of the wheel on the vehicle (front-right fr , front-left fl , rear-right rr , and rear-left rl ), are the wheel speeds and torques, respectively. The terms L 3 and I 3 are defined in Reference 56 as follows:

F I G U R E 3
Tire forces and tire slip angle. 64 where L f and L r are the distances between the front and the rear axles from the Center of Gravity (CoG) of the vehicle, respectively. The terms m fr , m fl , m rl , m rr are the wheel masses, I z is the vertical moment of inertia of the vehicle and E its front axle track. The forces F ax , F ay , F x , and F y in Equations (1a)-(1c) are defined in Reference 56 as follows: where F xfl , F xfr , F xrl , and F xrr are the longitudinal tire forces, F yfl , F yfr , F yrl , and F yrr are the lateral tire forces, fl and fr are the front left and the front right steering angles, assumed here as equal, that is, fl = fr = , where is the only steering angle at the front wheels, which will correspond to the manipulated variable in the MPC design.

Tire dynamics
The movement of the vehicle depends both on the forces applied to it and on the sliding in the contact area between the tires and the road surface ( Figure 3). Thus, in order to get the relations between the above sliding and the tire forces, a model is needed. The most well-known models in the literature are those by Pacejka and Dugoff. [61][62][63] In this work the second one was considered, because of its simplicity and dependency on the friction coefficient , which can be assumed known through the I2V communication with the Smart Road. Furthermore, the Dugoff's model considers the coupling between longitudinal and lateral tire forces, wheels adhesion and stiffness, slip ratio and the vertical load. 56 In particular, defining the longitudinal slip ratio of the ijth tire as during acceleration, and the slip angle ( Figure 3) of each tire as the longitudinal and lateral tire forces from Dugoff's model are defined as where C and C are the longitudinal and cornering stiffness of the tire (assumed to be known), respectively, while the parameters ij and the corresponding function f ( ij ) are defined as where F zij are the vertical loads on the four wheels, defined as follows where L = L f + L r is the distance between the front and the rear axles, h is the height of the CoG, a x =̇v x , and a y =̇v y are the vehicle longitudinal and lateral accelerations, respectively.

Equations of errors with respect to the road
In order to simulate the position of the ego vehicle on the lane, a model based on two variables was considered. These variables are expressed in terms of position and orientation errors with respect to the road reference system. 57 Thus, as shown in Figure 4, defining e y as the lateral displacement (the distance of the CoG of the vehicle from the center line of the lane) and e as the relative yaw angle error concerning the road center-line (angle between the longitudinal velocity direction and the tangent to the center line), assuming that the road radius R r is so large that the small angle assumption can be made and supposing that the road curvature R c = 1∕R r is known from in-vehicle ADAS map and/or I2V communication, the desired yaw rate of the vehicle is defined as in Reference 57, that is, The dynamics of the lateral displacement, according to Reference 57 iṡ being In general, the signals related to the variables e y and e are given by ADAS sensors (e.g., camera and Inertial Measurement Units) and maps.

Car-following model
Considering the ego vehicle preceded by an ahead vehicle, the relative distance and velocity must be considered. The corresponding values are given both by ADAS sensors (e.g., radar and/or LiDAR and/or camera), up to a certain distance such as the range of the radar (e.g., 150 m), and V2V/I2V communications for higher distances (e.g., up to 500 m). 65,66 Thus, two additional state variables are necessary, for example, the velocity of the vehicle located ahead v ah and the relative distance d r of the ego vehicle from the latter, as shown in Figure 5. The following information are assumed available from the Smart Road C-ITS (e.g., I2V) services: road slope , wind speed v w and road friction . In addition, the traction/braking force of the ahead vehicle u ah is assumed to be known thanks to the V2V communication (considering the longitudinal slip ratio negligible). Thus, the equations of the car-following model are defined as where K ah aero = 0.5 A ah C d ah (A ah is the maximum cross area of the ahead vehicle and C d ah its drag coefficient) and m ah is the ahead vehicle mass 57 . The term r m ah g cos = r F z ah is the rolling resistance depending on the normal force (F z ah ) and rolling resistance coefficient v = 0.01(1 + v ah ∕27.77). 58,59 All these parameters are known and transmitted to the ego vehicle through V2V .
In particular, one of the objectives of the proposed control strategy will be keeping a relative distance at a constant value, defined as safety distance and given by the I2V communication. Furthermore, the control action has to take into account that the relative distance must always be greater or equal than the emergency distance, defined as depending on L (defined above) and a response time T h ∈ [1.5, 2] s of the ego car during braking.

Vehicles, road, and environmental parameters
The complete list of parameters related to the models presented in Sections 3.1-3.3, regarding the ego and ahead vehicles, road and environment is shown in Table 1.

The NetCom IoT gateway
The IoT Gateway whose design will be finalized by NetCom in 2023 is an OBU (On-Board Unit) equipped with a Micro Processor Unit (MPU) featured by a Raspberry Pi 4 board, two Micro Controller Units (ESP32 and STM32F401RE), so as to distribute the computational load, and different peripherals enabling multi-channel communication. In particular, such an OBU will allow the data collection from different electronic devices, such as in-vehicle Electronic Control Units (ECUs), sensors, embedded devices, remote platforms and mobile devices. The data acquisition can occur by using both wired and wireless connections. In the first case, such connections include peripherals dedicated to the Controller Area Network of the vehicle (CAN bus), Local Area Network (LAN), serial communications such as Serial Peripheral Interface (SPI), Universal Serial Bus (USB) and Recommended Standard (RS-232 and RS-485), Inter Integrated Circuit (I2C), and analog/digital channels. Instead, wireless connections are managed by modules for Bluetooth Low Energy (BLE), Wireless Fidelity (Wi-Fi), Long Term Evolution (LTE) 4G, 5G, Cellular-Vehicle to Everything (C-V2x), and Wi-Fi 802.11.p (based on the standard ITS-G5) for V2V/V2I communications. In order to trace the position of the vehicle on which it is installed, the IoT Gateway is equipped with a Global Navigation Satellite System (GNSS).
One of the applications of the IoT Gateway concerns the execution of advanced control strategies for CAVs, such as the proposed adaptive MPC, acting at an intermediate layer between the ego vehicle and the Smart Road (see Figure 6 in Section 3.6). The proposed HIL testing is a necessary stage to verify that the execution time of the control law is lower than the CAN cycle time to be considered (10 ms), so as to allow the feasibility of the real road tests, for which additional aspects must be considered. In particular, the control strategies will have to be robust with respect to V2V/I2V communication delays. In Reference 25 this aspect was deeply analyzed through HIL testing, considering an adaptive MPC for platooning control and discrete saw-tooth time laws emulating communication delays, thus, mainly focused on the interaction between vehicles and Smart Road.

Adaptive MPC design
The proposed adaptive MPC is performed on the IoT gateway described in the previous section at an intermediate layer between the Smart Road Environment (SRE) and the ego vehicle, as shown in Figure 6. The control algorithm receives set-points, road and weather parameters from C-ITS services through I2V and Maps, such as, safety distance, point-to-point speed limits, wind speed, road curvature, slope and friction. Therefore, it receives information related to the ahead vehicle, such as, parameters (mass, traction/braking forces, etc.), relative distance and speed through V2V communication. Then, in order to provide the power-train with the torque and steering angle at the wheels as actuator set points for low-level controllers, the control algorithm receives the vehicle speed, lateral displacement and yaw angle error from the in-vehicle network (e.g., CAN bus) and ADAS sensors. A simplified ego vehicle prediction model in Section 3.6.1 is considered to solve the constrained optimization problem described in Section 3.6.2. The whole control scheme will be shown in Section 3.6.3. In this work, low-level controllers (e.g., single-loop controllers) are supposed to work faster than the MPC.
3.6.1 Simplified ego vehicle model for the adaptive control Starting from the 4-wheel vehicle dynamics model presented so far, and which will be used for the validation of the proposed control strategy and the verification of its robustness, a simplified single-track model is considered in the ego vehicle prediction model of the adaptive MPC, considering the following assumptions: 1. only the front wheels are considered to be driving, so the overall wheel torque w is the sum of the torque at front wheels during accelerations. Furthermore, w is assumed as the sum of the torque at all the wheels during braking; 2. the steering angles of the front wheels are equal to and the small angle approximation is used, considering that the control range on the steering angle at the wheels is [−5, 5] • ; thus, sin ≈ and cos ≈ 1; 3. the longitudinal slip ratio xij is assumed negligible.
From assumption 3, it follows that From the wheel dynamics defined in (1d-1g), the longitudinal tire forces F xij can be obtained, depending on the longitudinal acceleration, considering (14), and on the torque at the wheel w ij With the assumptions above and taking into account the lateral speed v y and the yaw rate , a simplified nonlinear single-track model is obtained, characterized by three state variables Furthermore, defining x e = [e y e ] T , two additional states defined in (10,11) are added to the simplified ego vehicle model. Eventually, the state variables x ext = [v ah d r ] T are added to the previous ones so as to define the complete ego vehicle model for the MPC design, featured by the following seven-dimensional state x v , two-dimensional control input u v and four-dimensional output y v In order to allow the MPC to work around reference set-points (e.g., for safety distance, lateral displacement and yaw angle error) and trajectory (e.g., vehicle speed), the nonlinear model in (17) is linearized so as to obtain a linear time-varying system. 67 Then the model is discretized through the explicit Euler method, considering a sampling time T s = 5 ms. Therefore, using a procedure similar to the one described in Reference 68, the following linear time-varying system, which corresponds to the prediction model of the adaptive MPC, is obtained where 1)), and the matrices A d (k), B d (k) are obtained from Jacobians and discretization, considering Thus, the matrices of the LTV system are where the time-varying parameters 11 (k), 12 (k), … , 33 (k), (k) and 11 (k), 2 (k), … , 32 (k) are shown in Table 2, , 12 (k) = AS d (v x (k − 1) + v w ) and 13 (k) = 0.01mg cos (k)∕27.77.
In this way, considering the above time-varying matrices, possibly also along the prediction horizon, the proposed MPC is able to adapt its behavior to parametric variations supposed to be known through I2V communication, such as road slope, friction and curvature, speed limits and wind speed, which can be given in advance by the SRE to the ego vehicle, based on its position along a known origin-destination path. This is the main reason for considering the adaptive MPC control law as the most suitable one for CAVs, together with the possibility to ensure the constraints discussed in the next section.

Constrained optimal problem
The cost function and the constraints for the formulation of the MPC are shown in (22). The components of the vehicle outputs are y v = [y v1 y v2 y v3 y v4 ] T = [v x e y e d r ] T as in (17), obtained at each time instant k + j|k (omitted for brevity) over the prediction horizon N p by an iterative procedure applied to the LTV model in (18). 69 The corresponding reference targets are y vt = [y vt1 y vt2 y vt3 y vt4 ] T , where y vt1 = v ref (k), y vt2 = y vt3 = 0 and y vt4 = d safe (k). In particular, the target speed v ref (k) is assumed to be known via C-ITS (e.g., through Intelligent Speed Adaptation-ISA-, providing point-to-point speed) and/or sensor fusion of ADAS sensors and V2V communication (e.g., evaluating the ahead vehicle speed). Similarly, the target safety distance d safe (k) is given by the Smart Road through I2V communication.
In particular, the cost function includes a term depending on the output references tracking over the prediction horizon of length N p and two additional terms depending on the control variables over the control horizon of length N c . Regarding the terms in the cost function containing the manipulated variables u v = [u v1 u v2 ] T , where u v1 = w and u v2 = , the first one depends on the manipulated variables tracking u t = [u t1 u t2 ] T . In particular, the term u t1 = wt is calculated by the steady-state values of the torque at the wheels from (16a), while the term u t2 is equal to the wheel steering reference ref , given by the following relation 57 TA B L E 2 Time-varying parameters of the LTV system.

Parameter Value
Considering the sampling time T s = 5 ms, the prediction horizon has been chosen in order to cover a typical time period of a cyclic Cooperative Awareness Message (CAM) T CAM = 100 ms, while the control horizon has been chosen to cover the CAN cycle time T CAN = 10 ms. Thus, the selected values of the two parameters are N p = 20 and N c = 2, respectively.
All the parameters of the constrained optimal problem defined in (22) are summarized in Table 3; the MPC has been designed in the Simulink environment using the MPC Toolbox through the Adaptive MPC methodology, as in Reference 70. The selected optimizer is based on the "Active-Set" method 71 using a constraint tolerance equal to 10 −6 and a maximum of 1000 iterations. For the robustness and stability analysis of the MPC, the Matlab command "review" applied to the MPC object has been used, from which no critical issues emerged. One of the advantages of the Simulink MPC Toolbox is the possibility to generate fast and robust C code for embedded development, also compatible with advanced software such as ODYS Embedded MPC. 72

3.6.3
Control scheme The whole control scheme is shown in Figure 7. In particular, the lower block, featured by the 4-wheel vehicle dynamics, errors with respect to the road and car-following model, provides the adaptive MPC with all the states necessary for the determination of the measured variables and to obtain the LTV model. The latter receives as inputs also the previous values of the torque, steering angle at the wheels and time-varying parameters and references from I2V communication, ADAS sensors and maps, and provides the Adaptive MPC with the matrices A d (k) , B d (k) , C d , states x v (k), rates of change of their set-points Δx v and outputs y v (k), necessary for the iterative procedure over the prediction horizon so as to allow the evaluation of the predicted outputs y vz (k + j|k) in the cost function in (22).

Parameters Description Value
Then the control algorithm, based on the optimization problem, evaluates the updated torque and steering angle at the wheels ( w (k), (k)). The latter is sent directly to the lower block, in particular to the 4-wheeled vehicle dynamic model. Instead, the torque at the wheels is split into the torques at the four wheels through the algorithm 1, which provides the vehicle dynamics with the torque at each wheel ( w fl , w fr , w rl , w rr ). This algorithm is a simple policy for torque split and its design is not the focus of this work.

HIL test bench
The HIL test bench is shown in Figure 8. Two embedded boards have been used: Raspberry Pi 4 Model B 73 and Raspberry Pi Zero W. 74 The first board is equipped with a Quad core Cortex-A72 (ARM v8) 64-bit SoC (System on Chip) running at

F I G U R E 8 HIL Test bench featured by: (i) a Raspberry Pi 4 to perform in real-time the adaptive MPC; (ii) a Raspberry Pi Zero to
simulate the Smart Road Environment, ego and ahead vehicles using CAN and LAN communications to emulate the data exchanges with different throughput; (iii) a PC-host to start and stop HIL simulations and for data logging.
1.5 GHz and with 4 GB RAM; the second one is equipped with a single core ARM Cortex-A53 running at 1 GHz and 512 MB RAM. These devices are both connected to the hardware module Waveshare RS485 CAN HAT through the 40 pin GIPIO header. This module enables CAN communication with a data throughput of 500 kbit/s (standard CAN) through the on-board CAN controller MCP2515 via SPI interface, and it is equipped with the on-board transceiver SN65HVD230. 75 The two CAN HAT modules are connected through a twisted pair cable. Both the Raspberry boards are also connected to an Ethernet switch so as to emulate a data exchange considering signals coming from ADAS control units and V2V/I2V communications. In particular, this emulation takes into account a data exchange with a throughput that could be greater than the standard CAN, such as the Fast CAN (1000 kbit/s) or the Flexray (10 Mbit/s). Analyzing the HIL test bench from a functional point of view, the Raspberry Pi 4 has been used to run in real-time the C code generated for the Adaptive MPC. Similarly, the C code generated for the ISA, the time-varying road and weather parameters, the 4-wheel vehicle dynamics, errors with respect to the road and car-following model has been performed on the Raspberry Pi Zero. In particular, two protocols are used to emulate the following data exchange between the Raspberry boards:

RESULTS
This section presents the simulation scenarios in Section 4.1, the robustness and sensitivity tests on MPC with respect to parametric variations related to wind speed, road friction and ego vehicle mass in Section 4.2, PIL and HIL tests in Section 4.3 and 4.4, respectively, and a comparison of the proposed strategy with an existing approach based on Adaptive MPC in Section 4.5.

Simulation scenarios
Ahead and ego vehicles are assumed travelling on an extra-urban road where speed limits are dynamically established by a Smart Road Infrastructure Manager (SR-IM) of the SRE (e.g., based on the traffic conditions, presence of accidents, work in progress and weather conditions). In particular, the ahead vehicle is assumed to be the leader that receives via I2V new speed limits based on its position transmitted to SR-IM through V2I. More specifically, the ahead vehicle position p ah is considered in terms of the traveled distance along the road section managed by the SR-IM (i.e., 3 km long). Furthermore, the ahead vehicle speed v ah is subjected to an ISA, based on a parameter defined as "time to change speed" t tcs provided by the SR-IM through I2V. This parameter represents the time imposed by the SR-IM to allow the ahead vehicle to reach the new speed limit. For the sake of brevity, ISA is the reference speed v p2p (k) calculated through a linear equation depending on t tcs , previous and new speed limits v old and v new , respectively, based on the following equations In particular, the parameters necessary for ISA vary based on two simulation scenarios denoted with "50-30-50" and "90-50-70", respectively, where the numbers indicate the considered dynamic speed limits expressed in km/h. Both scenarios are featured by an initial speed (50 km/h for the first and 90 km/h for the second) and two changing speeds: the first one is related to a braking (30 km/h for the first and 50 km/h for the second) and the second one to an acceleration (50 km/h for the first and 70 km/h for the second). In more detail, the parameters necessary to the ISA for the two test cases based on (23) are shown in Table 4 for the 50-30-50 case and Table 5 for the 90-50-70 case. Furthermore, it is assumed that the Smart Road is featured by different curvatures defined on the basis of the ego vehicle position in terms of the distance p x traveled along the Smart Road section as in Table 6. Figure 9 shows a detail about the data exchange concerning V2V, V2I, and I2V communications. In particular, the IM-SR provides through I2V both the ahead and ego vehicles with the wind speed v w and the road friction . Using the same communication, the IM-SR provides the ahead vehicle with the data necessary to the ISA, that is, t tcs , v old , and v new . The ahead vehicle transmits via V2I its position p ah to the SR-IM and through V2V its speed v ah to the ego vehicle.  Table 7. In particular, parameters for road friction and wind speed are chosen according to References 77 and 78, respectively, supposing normal (dry asphalt and calm wind) and adverse (wet asphalt and strong breeze) conditions. Furthermore, the variation on the vehicle mass is assumed to be 30% greater (e.g., four passenger on board) than the nominal one in adverse conditions. Robustness test results for the 50-30-50 scenario are shown in Figures 10 and 11. The most obvious result regards the torque at the wheels w (Figure 10

F I G U R E 11
Results of the robustness test on the ego vehicle outputs for the test case 50-30-50. of the Error (MINE) and Root Mean Square Error (RMSE). These indices confirm that the largest deviations were obtained for the torque at the wheels, but the adaptive MPC is robust against the supposed variations since the outputs follow the corresponding references with negligible tracking errors (RMS): 0.09 km/h for v x ; 1.7 cm for e y ; 0.13 • for e ; 13 cm for d r . Similar results have been obtained for robustness tests on the 90-50-70 case, as shown in Figures 12 and 13. In particular, the deviation of w with respect to the nominal values is greater than in the previous case, reaching a MAXE of about 78 Nm and a MINE of about −98 Nm, as shown in Table 9. Also the steering angle at the wheels is affected by higher   Table 13) w is affected by a MINE of about −57 Nm (torque trend in this condition is greater than in the nominal conditions), since in order to keep the reference speed, the MPC requires more torque to counteract a larger aerodynamic drag force. The RMSEs for all the outputs are very low, thus the MPC allows the vehicle to keep the reference trajectories and set-points.
Similar results have been obtained for the test case 90-50-70. In particular, significant deviations have been obtained on the torque, resulting greater than in the previous case. In presence of maximum load (test case 2,

PIL testing
With respect to the version of the MPC presented in Reference 6, in the present work two updates have been proposed: (i) the constrained optimization problem is solved using an Active-Set method of the Simulink MPC Toolbox, based on the QPKWIK algorithm, 79 instead of the Hildreth's method; (ii) the target hardware is a Raspberry Pi 4 (more suitable for the design of an IoT gateway) instead of the ST NUCLEO-H743ZI2. The use of a different Active-Set method resulted in better performances in terms of tracking errors on the ego vehicle outputs. In particular, the most significant improvements have been obtained in terms of RMSE for both the speed tracking and the yaw angle error, which passed from 0.35 to 0.02 m/s and from 0.13 to 0.002 rad/s, respectively. On the other hand, the execution time has increased, but it still remains well below the sampling time. Table 17 shows a comparison between the PIL tests of the two versions of MPCs performed on the Raspberry Pi 4. Even though the average execution time using the Simulink toolbox is greater than the one obtained with Hildreth's method, 80 it still results about 10 times lower than the sampling time. This result is encouraging and supports the choice made in the present work.

HIL testing
A comparison between HIL and PIL simulations has been performed over a duration of 150 s for both the simulation scenarios (50-30-50 and 90-50-70). One of the advantages of the HIL simulation is to outline a more realistic signal trend over the time, considering the presence of any noises, delays and jitters due to real hardware interfaces and wire cabling.
Regarding the test case 50-30-50, comparisons of torque and steering angle at the wheels between HIL and PIL are shown in Figure 14. In particular, w shows the same trends ( Figure 14A), as well as ( Figure 14C). The torque shows differences in the range [−2, 2] Nm ( Figure 14B) while the steering angle at the wheels in [−0.13, 0.1] • ( Figure 14D). These differences are considered acceptable since the trends of the output variables are not particularly affected for both HIL and PIL tests, following the references as shown in Figure 15A (v x ), Figure 15B (e y ), Figure 15C (e ), and Figure 15D (d r ). This result is confirmed also by the differences shown in Figure 16, where the following results are observed: (i) a maximum value of 0.02 km/h for v x ( Figure 16A); (ii) a difference of about ±2 mm for e y (Figure 16B), and within the range [−0.04, 0.03] • for e ( Figure 16C); (iii) a difference in the range [−5, 4] mm, for d r ( Figure 16D). Figure 17 shows the trajectory of the distance p x traveled by the vehicle over the simulation time (about 1.5 km as shown in Figure 17A) and the corresponding road curvature R c ( Figure 17B).
Results for the case 90-50-70 are shown in Figures 18-21. In particular, the speeds are higher than in the previous case, thus higher torque ( Figure 18A) and steering angle ( Figure 18C) at the wheels with similar trends for HIL and PIL tests have been obtained. The maximum torque difference has a peak of 6.7 Nm (Figure 18B), about 2.5 times greater than the one obtained in the previous test case, during the first rising of the torque, while the vehicle speed tends to reach 50 km/h after the braking. This is due to a delay of about 150 ms given by the real-time simulation, as shown in the zoom of the Figure 22. This delay is the worst case obtained by the HIL simulations. Also maximum differences on ( Figure 18D) resulted about 2 times greater than in the previous case but limited in the range [−0.3, 0.25] • . Nevertheless, the trends of the output variables in Figure 19A-D similar for both HIL and PIL tests and well follow the references. The differences in outputs are shown in Figure 20A Figure 21 shows the distance traveled by the ego vehicle (about 2.6 km as in Figure 21A) and road curvatures ( Figure 21B).

Comparison with previously published methods
In order to check the effectiveness of the proposed control strategy and to contrast the obtained results with previously published methods exploiting the MPC approach, a comparison with the adaptive MPC based on a simpler prediction The prediction model adopted in the control method considered for the comparison has been obtained by linearizing the following nonlinear model where a x is the longitudinal acceleration, = 0.5 is a the time constant to reach the desired acceleration, while all other parameters and variables are the same previously described. The torque at the wheels is obtained by (16a) considerinġ v x = a x . Figures 23 and 24 show the obtained results with reference to the test case 90-50-70, where the labels MPC DYN and MPC LKS refer to the proposed and compared MPCs, respectively. Although the trends on the manipulated variables are similar, more evident differences have been obtained on the output variables. In particular, the MPC DYN shows higher peaks on v x ( Figure 24A): the most obvious occurs around 4 s with a difference of about −0.8 km/h for 1.5 s. But on the other side, MPC LKS is affected by a steady-state error of −0.02 km/h. Also on e y ( Figure 24B), MPC DYN is affected by some slightly higher peak, but contained in the admissible range featured by the constraints (∈ [−35 , 35] cm). In particular, the higher peaks are equal to −12 and 15 cm for MPC LKS and MPC DYN , respectively. Regarding e ( Figure 24C), MPC DYN shows lower peaks and less oscillations with respect to MPC LKS . However, the most obvious difference has been obtained on d r , where MPC LKS tends to slowly diverge. Probably this is due both to the presence of a simple car-following model in MPC DYN and to the constraints set on the relative distance, which make MPC DYN safer.
Considering an additional test case featured by a sudden brake of the ahead vehicle from 90 to 30 km/h, occurring at the beginning of the path along the Smart Road, most noticeable differences have been obtained, as shown in Figures 25  and 26. In particular, with the MPC LKS , significant oscillations on the torque ( Figure 25A) are obtained to reach the new speed set-point, while the steering lasts longer ( Figure 25B). The vehicle speed shows higher tracking errors, lateral deviation and yaw angle error for the MPC LKS ( Figure 26A  excessive spacing at the steady state, acting on the control of the vehicle speed only. This result confirms the importance of considering the car-following in the prediction model of the MPC so as to benefit from the vehicle connectivity.
In terms of real-time performance, the differences are negligible: PIL test on MPC LKS shows an average execution time of 0.38 ms, an average and maximum CPU utilization of 7.5% and 22.62%, respectively. Thus, on average, a difference of about 120 s for the execution time and 2.5% for the CPU utilization result to be acceptable, considering the more complex structure of the proposed MPC and the safer behavior.

CONCLUSIONS
This article reported an update and an extension to the work originally proposed in Reference 6 regarding the model-based design and Processor-In-the-Loop validation of an MPC for coupled longitudinal and lateral vehicle dynamics.
A new QP solver and a more realistic validation process have been considered, using the MATLAB/Simulink MPC Toolbox, a nonlinear 4-wheel vehicle model and a low-cost HIL test bench based on two Raspberry embedded boards that emulate both I2V and V2V communications: (i) a Raspberry Pi 4 for the real-time execution of the MPC; (ii) a Raspberry Pi Zero W to simulate the vehicle and the Smart Road environment.
Furthermore, the hardware board is different with respect to the one used in the previous work, given the need to design a more powerful IoT gateway, which will provide the vehicle both with V2V, V2I, and I2V communications and the real-time execution of advanced control strategies as such the proposed one, for the C-Mobility research project.
Based on two simulation scenarios at low and high speeds, respectively, robustness and sensitivity tests have been performed, considering variations on the vehicle mass, wind speed and road friction. In particular, larger deviations have been obtained on the torque at the wheels with higher vehicle and wind speeds, higher mass (i.e., maximum load) and lower friction (wet asphalt). Nevertheless, the outputs follow the references with acceptable RMSEs (i.e., 0.16 km/h for the vehicle speed, 3.7 cm for the lateral displacement and 18 cm for the relative distance). Then, sensitivity tests showed that the most evident differences with respect to nominal conditions are obtained on the torque at the wheels both at the maximum load and in presence of wet asphalt, but with acceptable tracking errors and RMSEs as in the robustness tests.
In addition, PIL tests on Raspberry Pi 4 have been performed comparing the MPC proposed in the previous work with the present one. Both average CPU load and execution time resulted higher (about 2.7 times), but well below (about 10 times lower) the selected sampling time. The updated MPC, moreover, resulted more performing in terms of tracking errors on the ego vehicle outputs; in particular, the most significant improvements have been obtained in terms of the RMSEs for the speed tracking and the yaw angle error, which passed from 0.35 to 0.02 m/s, and from 0.13 to 0.002 rad, respectively, encouraging the use of the new control design.
Furthermore, HIL tests have been performed and the obtained results have been compared with the PIL simulations. One of the advantages of HIL testing is to outline a more realistic signal trend over the time, considering the presence of any noises, delays and jitters due to real hardware interfaces and wire cabling. In the worst case, HIL simulation introduced a maximum delay of about 150 ms, but both inputs and outputs are not particularly affected by this condition.
Eventually, in order to check the effectiveness of the proposed control strategy, a comparison with a previously published adaptive MPC, based on a simpler prediction model, has been performed. The results showed the importance to consider the car-following model for the MPC design, so as to obtain a control not only on vehicle speed and errors with respect to the road, but also considering the relative distance and the related constraints. The obtained results showed better tracking performances and a safer behavior of the proposed MPC, considering also an additional test case with a sudden brake.
In conclusion, the obtained results pave the way to future developments, which will concern both real-road tests and the design of real-time path planning strategies, considering a cascade of two MPCs, one devoted to vehicle kinematics and the proposed one for the vehicle dynamic control.