# A Hierarchical Control System for Autonomous Driving towards Urban Challenges
## 1 Introduction
In 2016, the Tesla Model S had a disastrous accident in Florida. The reason for this accident was the simultaneous failure of both camera and radar sensors. Despite suffering severe problems, the self-driving automobiles are continuously developed. Recently, Zoox and nuTonomy have led the autonomous taxi business. Numerous start-up businesses have been establishing and reinforced to bring safe autonomous vehicles to trade as early as 2020. Autoware [11], Apollo [12], NVIDIA DriveWorks [13] and Openpilot [14] are the most popular open-source frameworks. We highlight that Autoware [11] and Apollo [12] using the Robot Operating System (ROS) [15] that can be flexibly integrated into real-world applications. Roughly speaking, the autonomous vehicle system can be commonly divided into three primary classes [16; 17], including perception and environment mapping, motion planning and control system. In this article, we develop an autonomous driving system, as shown in Figure 1. The perception and environment mapping are applied to determine where the self-driving vehicle is and what surrounding environments are. One of the most challenging issues in the urban scene is less-texture and high illumination. So that the localization algorithm could achieve precision and be robust in the area, the localization uses the inertial navigation system (INS) with an Inertial Measurement Unit (IMU) and Global Positioning System (GPS) fusion to avoid drift over time [16; 17]. In GPS-denied environments such as a tunnel scene, the 3D LiDAR odometry [18] technique is implemented to localize itself. The prediction uses the Interactive Multiple Model-Unscented Kalman Filter-Joint Probabilistic Data Association Filter (IMM-UKF-JPDAF) method [19], which predict the trajectories of the dynamic objects such as vehicles, pedestrian. The motion planning categorizes into three groups [16; 17; 20] global path planning (GPP), decision-making and local planning. The GPP makes route planning, which plots through a series of lanes or roads efficiently. The GPP generally uses a High Definition (HD) Map, which provides ad-hoc structured information regarding the streets and Vehicle-to-X (V2X) communication technology, then it chooses the best route via a series of waypoints. This paper focuses on handling a hierarchical control system including decision making, local path planning and control. The decision-making can be divided into Finite State Machines (FSM) and Markov Decision Processes (MDPs) [16; 17]. MDPs is useful for solutions with uncertainty information of perception and quite complicates to implement with a large variety of scenarios. In contrast, FSM is a simple method and handles well with many circumstances. In the DARPA Urban Challenge, Stanford Junior team succeeded in applying FSM with several scenarios in urban traffic roads [4]. However, the main drawback of FSM is the difficulty in solving uncertainty and in large-scale scenarios. In this work, we propose the decision-making with a new structure to manage various traffic scenarios and road conditions. This FSM contains the mission that the vehicle should perform and the behavior of the car, such as lane-changing, lane-keeping, obeying traffic lights, and rule-based speed limits. The local path planner (LPP) generates a regional trajectory for the autonomous vehicle in both structured and unstructured scenarios. Researchers in [21] used the decoupled approach of path and velocity to produce a speed profile, although this solution is not optimal but straightforward. Appolo [12] performs the spline path by determining a QP optimization problem with linearized constraints. Despite this algorithm is an effective method but not optimal with the temporal variable. In this paper, we solve the LPP by exploring an optimization problem in the Frenet coordinate [22]. Herein, we define the optimization problem with not only trajectory parameters but also travel time, and additionally join the constraints of input and output velocity.