# A Hierarchical Control System for Autonomous Driving towards Urban Challenges
## 2 Hierarchical Control System
### Vehicle Control Strategy
#### 2.3.1 The Longitudinal Controller  
The characteristics of the model are required to design a controller efficiently. However, the full modeling of a vehicle entails complexity and nonlinearity [34]. Besides, it is hard or impossible to identify the parameters of vehicle modeling by describing physical relationships. Instead, we identify the model by using a simple method where the workstation command as the input and the longitudinal velocity as the output. The model is represented as a second-order function with a time delay [34], as yielded in Equation (11). \[G_{Pv}(s)=\frac{V(s)}{Acc(s)}=\frac{K_{P1}s+K_{P1}}{T_{P2}s^{2}+T_{P1}s^{2}+T_{ P0}}e^{-s\tau_{d}}. \tag{11}\]  
Although the parameters of the model (11) depend on velocity and road terrain, the model's characteristics are helpful to adjust the controller parameters. Researchers [16, 17, 20, 25, 30, 34] did not present how to tune the controller into practice. In this work, we first collect data, then identify the vehicle model, next analyze its properties, and finally design the controllers. The SFF-PI controller [35] as in Figure 7 tracks the feedback signal including velocity error, windup affecting before and after saturation, and the vehicle velocity yielded as  
\[\psi_{SPI}(t)=\frac{K_{FF}(v)}{v_{0}}v_{ref}+\frac{K_{P}(v)}{v_{0}}e_{ref}+ \left(\frac{K_{I}(v)}{v_{0}}+K_{AW}e_{se}\right)\int\limits_{\tau=t_{1}}^{ \tau=t_{2}}e_{ref}(\tau)d\tau, \tag{12}\]  
where \(e_{ref}\) is the difference between the desired and measured velocity, \(e_{se}\) is the error of output before and after the saturation block which limits the acceleration and brake within \(-1\) and \(1\). \(K_{FF}(v)\), \(K_{P}(v)\), \(K_{I}(v)\) are respectively feedforward, propagation, integration coefficient, which depend on the velocity, and \(v_{0}\) is the nominal velocity of vehicle. Figure 6: The control system architecture consists of longitudinal control with the Scheduled Feedforward PI (SFF-PI) controller and lateral control using the adaptive-Pure Pursuit Controller (PPC). To implement the controller, we need to transform Equation (12) from continuous-time to discrete-time domain with a period of \(T\). So, the output of the controller at time \(t=kT\) is given as  
\[\psi_{SPI}^{k}=\frac{K_{FF}}{v_{0}}v_{ref}^{k}+\frac{K_{P}(v_{k})}{v_{0}}e_{ref}^{ k}+\left(\frac{K_{I}(v_{k})}{v_{0}}+K_{AW}e_{se}^{k}\right)T\sum_{i=0}^{k}e_{ref}^{ k}. \tag{13}\]  
Similarly, the output at the next time step \(t=(k+1)T\) is  
\[\psi_{SPI}^{k+1}=\frac{K_{FF}}{v_{0}}v_{ref}^{k+1}+\frac{K_{P}(v_{k+1})}{v_{0}} e_{ref}^{k+1}+\left(\frac{K_{I}(v_{k+1})}{v_{0}}+K_{AW}e_{se}^{k+1}\right)T \sum_{i=0}^{k+1}e_{ref}^{k+1}. \tag{14}\]  
The integration can be obtained by using the recursion  
\[\Gamma^{k+1}=\sum_{i=0}^{k+1}e_{ref}^{k+1}=e_{ref}^{k+1}+\sum_{i=0}^{k}e_{ref} ^{k}=e_{ref}^{k+1}+\Gamma^{k}. \tag{15}\]  
The proposed architecture of the longitudinal controller is illustrated in Figure 7. Using the recursive form of Equations (14) and (15) to implement the SFF-PI controller which is shown in Algorithm 1. Herein, the control algorithm is processed at each interrupt event of a timer. Line 2 gets the desired and measured velocities. Then line 3 calculates the error velocity. In line 5, the scheduled PI parameters are searched in a lookup table saved in the program. The integration value can be achieved by utilizing Equation (15) as in line 6, and the output value is processed using Equation (14) in line 7. From line 8 to line 10, if the control values before and after saturation are different, it needs to re-handle Equation (14) to get new \(e_{se}^{k+1}\). The controller must apply accelerating or braking to obtain the best control performance. To perform that, we only need to apply acceleration if it is more than zero, otherwise braking is used as from line 14 to line 19. In summary, The longitudinal controller based on the SFF-PI is designed as follows  
Step 1--recording data: the acceleration range \([0,1]\) is executed and recorded as the input, also the output is the observed velocity.