# Lateral Control Calibration and Testing in a Co-Simulation Framework for Automated Vehicles
## 2 Vehicle Dynamics and Control
### Lateral Controller  
In this study, the path planning process involves a series of reference points. This discretization of data is necessary for practical implementation. To control the vehicle along this discrete trajectory, a discrete linear-quadratic regulator (dLQR) controller is used. Equation (10) is discretized to design the dLQR controller, which governs the vehicle's dynamics. During this process, we neglect the effect of \(C_{\theta_{s}}\) and apply the midpoint Euler and the forward Euler approach to clarify the model while preserving essential characteristics. As a result, we obtain the equation that describes the discrete tracking errors as:  
\[\dot{X}_{k_{+1}}=\tilde{A}X+\tilde{B}U \tag{11}\]  
with:  
\[\tilde{A}_{k}=(I-\frac{A\Delta t}{2})^{-1}(I+\frac{A\Delta t}{2})^{-1};\tilde{B} =B\Delta t \tag{12}\]  
Figure 2 shows the entire structure of the LQR controller for an automated vehicle, which includes four main parts: the perception, path planning, controller, and the vehicle model. In path planning, the EM planner [39] is used to generate the reference path and path tracking errors. As the path planning module is not the focus of our work, the exact algorithm can be referenced in the GitHub project [40]. The LQR controller is the main center of the second part. The aim of this study is to find the optimized matrix K for the LQR controller using three algorithm optimizations (ABC, GA, and PSO) as well as calculate \(\delta_{ff}\) in the feedforward control step to find the final signal control (steering angle \(\delta_{f}\)). Then, the steering angle \(\delta_{f}\) is sent to the vehicle model to control a car. We use the BMW5 car model and IPG Carmaker 11 software as a simulation environment. After comparing the performances of the vehicle when using the three optimization algorithms, we find the best algorithm for the car. Finally, we use this algorithm to simulate the real-time system and compare the gap results between the MiL and HiL. The cost function of LQR controller is defined as:  
\[\sum_{0}^{\infty}(X_{k}^{T}QX_{k}+U_{k}^{T}RU_{k})dt \tag{13}\]  
\[Q=diag(q_{1},q_{2},q_{3},q_{4}) \tag{14}\]  
\[R=[q_{5}] \tag{15}\]  
where \(Q\) is the weighting matrices of the state error and \(R\) is weighting matrices of the control signal. \(q_{1},q_{2},q_{3},q_{4},q_{5}\) are the weight factors of the lateral error, lateral error rate, heading error, heading error rate, and the steering angle in the front wheel, respectively. Substituting Equation (11) into (13), the Lagrange multiplier approach is employed to build the constraints as follows:  
Figure 2: Lateral control structure. \[J=\sum_{k=0}^{n-1}[X_{k}^{T}QX_{k}+U_{k}^{T}RU_{k}+\lambda_{k+1}^{T}(\overline{A_{k }}X_{k}+\overline{B}U_{k})-\lambda_{k+1}^{T}X_{k+1}]+X_{n}^{T}QX_{n} \tag{16}\]  
The Hamiltonian function is defined as:  
\[H_{k}=X_{k}^{T}QX_{k}+U_{k}^{T}RU_{k}+\lambda_{k+1}^{T}(\overline{A_{k}}X_{k}+ \overline{B}U_{k}) \tag{17}\]  
From Equations (16) and (17), one can obtain:  
\[J=\sum_{k=0}^{n-1}[H_{k}-\lambda_{k}^{T}X_{k}]+X_{n}^{T}QX_{n}+\lambda_{0}^{T} X_{0}-\lambda_{n}^{T}X_{n} \tag{18}\]  
The extreme value of Equation (18) is achieved:  
\[U_{k}=-(R+\overline{B}^{T}P_{k+1}\overline{B})^{-1}\overline{B}^{T}P_{k+1} \overline{A_{k}}X_{k} \tag{19}\]  
where K can be calculated as:  
\[\tilde{A}_{k}=(I-\frac{A\Delta t}{2})^{-1}(I+\frac{A\Delta t}{2})^{-1};\tilde{B} =B\Delta t \tag{12}\]0  
\(P\) is solved by the Riccati equation:  
\(P_{k}=\text{Q}+\overline{A_{k+1}}^{T}P_{k+1}\overline{A_{k+1}}-\overline{A_{k +1}}^{T}P_{k+1}\overline{B}(R+\overline{B}^{T}P_{k+1}\overline{B})^{-1} \overline{B}^{T}P_{k+1}\overline{A_{k+1}}\)  
Equation (19) can be rewritten as:  
\[\tilde{A}_{k}=(I-\frac{A\Delta t}{2})^{-1}(I+\frac{A\Delta t}{2})^{-1};\tilde{B} =B\Delta t \tag{12}\]1  
where K = \([K_{1},K_{2},K_{3},K_{4}]\) is the gain of the LQR controller.