# A Hierarchical Control System for Autonomous Driving towards Urban Challenges
## 2 Hierarchical Control System
### Vehicle Control Strategy
#### 2.3.2 Adaptive-Pure Pursuit Algorithm Based-Lateral Controller
```
Input: reference path in term of way-points, ego-vehicle position Output: index of the closest waypoint
1 index = 1;
2 forsearch in the whole waypoint trajectorydo
15 d(index) = distance from ego-vehicle to waypoint(index);
16 if\(d(index)>=d(index+1)\)then
17 index++;
18 d(index) \(\leftarrow\) d(index+1);
19
20 else
21 return index;
22 Break;
23
24 Return index;
```  
**Algorithm 9**Find the closest waypoint to the ego-vehicle. ```
Input: reference path in term of way-points, ego-vehicle position Output: index of the closest waypoint
1 index = 1;
3 foresearch in the whole waypoint trajectorydo
4 d(index) = distance from ego-vehicle to waypoint(index);
5 if\(d(index)>=d(index+1)\)then
6 index++;
7 d(index) \(\leftarrow\) d(index+1);
8
9 else
10 return index;
11 Break;
12
13 Return index;
```  
**Algorithm 10**Find the closest waypoint to the ego-vehicle. ```
Input: reference path in term of way-points, ego-vehicle position Output: index of the closest waypoint
1 index = 1;
3 foresearch in the whole waypoint trajectorydo
5 d(index) = distance from ego-vehicle to waypoint(index);
6
7 d(index) \(\leftarrow\) d(index+1);
8
9 else
11 return index;
12 Break;
13
14 Return index;
```  
**Algorithm 11**Find the closest waypoint to the ego-vehicle. ```
Input: reference path in term of way-points, ego-vehicle position Output: index of the closest waypoint
1 index = 1;
4 foresearch in the whole waypoint trajectorydo
15 d(index) = distance from ego-vehicle to waypoint(index);
16
17 d(index) \(\leftarrow\) d(index+1);
18
19
21 end if
30
22 end fore search in the whole waypoint trajectorydo
31
32 end fore search in the whole waypoint trajectorydo
33
34 end fore search in the whole waypoint trajectorydo
35
36 end fore search in the whole waypoint trajectorydo
37
38
39 end fore search in the whole waypoint trajectorydo
39 d(index) = distance from ego-vehicle to waypoint(index);
40



39 end fore search in the
```
Input: reference path in term of way-points, ego-vehicle position, ahead distance \(l_{a}\) Output: index of the ahead waypoint
1 index = closest waypoint;
2 forsearch in the whole waypoint trajectorydo
3 d(index) = distance from ego-vehicle to waypoint(index);
4 if\(-\epsilon<=d(index)\)-\(l_{a}<=\epsilon\)then
5 return index;
6 break;
7
8 else
9 index++;
10
11 Return index;
```

**Algorithm 4**Find the ahead waypoint in the reference trajectory. ## 3 Experimental Results

After developing the hierarchical control system in Section 2, this section provides experimental results of the decision making, local path planning and control. Firstly, the vehicle platform, hardware, and software are presented. Secondly, we demonstrate the robustness of the control system. Finally, the local path planner is given with the performance in both structured and unstructured environments. ### The System Integration of Autonomous Vehicle

The self-driving car uses the model Hyundai I-30 platform with the configuration illustrated as in Figure 9.

Figure 9: (**a**) Hyundai I-30 platform, (**b**) the perception with 3D LiDAR, Zed-camera, GPS and Inertial Measurement Unit (IMU), (**c**) the sensors configuration. The autonomous vehicle is equipped with various sensors to enhance the perception. The sensors system consists of 3D Light Detection and Ranging (3D LiDAR), Zed-camera, GPS, IMU, and encoders. The computational devices use two computers with the configurations as Xeon X5690 processors, NVIDIA GeForce GTX-1080 Ti, 32 GB RAM, 512 GB SSD, and networking gear as shown in Figure 10.

The perception workstation (PW) handles Object Detection and Tracking (ODAT) by fusing 3D LiDARs and cameras. The Control System Workstation (CSW) performs the localization by using GPS, IMU and encoders [39] and the controller. The perception, localization, and control tasks employ the Robot Operating System (ROS) middle-ware version-Kinetic Kame [15] on top of Ubuntu 16.04.5 LTS. The development utilizes two workstations connected by the ROS TCP/IP protocol.