Perception, Planning and Control for Self-Driving System Based on On-board Sensors

The autonomous vehicle can recognize and understand environment, self-control, and achieve the driving level of the human driver. To create this kind of system, the following work was carried out: (a) a real time lane detection system is proposed, based on vision system functions, using webcam camera; (b) In order to detect curve lanes, deep learning is applied for lane detection, based on fully Convolutional Neural Network (CNN); (c) The cubic spline interpolation method is used for path generation, based on Global Positioning System (GPS) data, where distance between two adjacent path generation points is same. Compared with the connection method of the cubic polynomial fitting algorithm, the curve fitted path by the cubic spline interpolation method is smoother and more satisfied with the vehicle motion pattern; (d) Based on frenet coordinate frame, optimize trajectory planning method is used to plan safe, easy and comfort trajectory. Compared to the Cartesian coordinate frame, frenet coordinate frame simplifies the solution of road curve fitting problems, especially in the case of complex road environment; (e) Fuzzy sliding mode control method is proposed for vehicle steering control. Finally, the simulation and experiment results are used to prove the effectiveness of the proposed methods.


Introduction
Autonomous vehicle systems have been a popular topic of research. The technical preparation of self-driving vehicles is maturing, with the increase in available computing power and the reduction in sensor costs. 1 The field of self-driving technology is not only related to vehicle control, path planning, sensory fusion, but also forewords such as artificial intelligence, machine learning, deep learning, and reinforcement learning. Self-driving will start a new technology and market revolution, in the next 5 to 10 years. Autonomous driving has been of interest to researches to push the boundaries of automotive racking, that is limited by the abilities of human drivers, with several companies and 100 self-driving vehicles. 4 Tesla provides self-driving function in their 2016 model S cars. 5 Uber's plans to eventually replace the human driven fleet with selfdriving cars. 6 The core of the self-driving system, including three parts: perception, planning, and control. Self-driving system technology means that: self-driving car can recognize the surrounding environment and status of the vehicle through a variety of on-board sensors (camera, lidar, millimeter wave radar, GPS, inertial sensors, etc.), and make analysis and judgment, then autonomously control vehicle movement, and finally achieve self-driving based on the acquired environmental information (including road information, traffic information, vehicle location and Obstacle information, etc.) This paper proposes methods for self-driving system in perception, planning and control, based on on-board sensors.
In lane detection and tracking, several issues should be considered, namely choosing an appropriate lane model, developing an image processing for features extraction, probability of lane tracking, and effectiveness of real-time image processing ability. 7 Based on, 8 three main methods for lane detection using cameras includes: model of the lane markings, color information, and feature information method including edge, gradient and intensity. 9,10 About road information, it makes the mathematical model of the lane markings, such as B-spline, 11 where candidate points are extracted from the lane markings. About color information, it applies a threshold to the image based on RGB and HSL color space, and places an 8-bit image. 12 To extract a lane, color threshold is based on RGB color space or HSL color space. Due to simplification of computer monitor design, the RGB color space is the most commonly used color space. However, it cannot be used for all applications. In order to describe a color in the RGB color space, the red, green, and blue color components are all used, so RGB is not as intuitive as other color spaces. The HSL color space uses only the hue component to describe color. Therefore, HSL is the best color space for image processing applications, for example color matching. 13 In this paper, a real-time lane detection system is proposed, which uses the capabilities of vision system functions, based on combination of feature information and color information.
The real-time lane detection system works well for clear straight lane. However, on curved lanes or sharp turns, the real time lane detection system would fail, due to complexity of the lane markers. The lane detector cannot distinguish the lane edge well. The lane detection system based on Convolutional Neural Network (CNN) can overcome this problem. CNN is the most popular deep learning architecture, due to the effectiveness of convnets. The main advantage of CNN is that it can automatically detect the important features without any human supervision. CNN is also computationally efficient. This enables CNN models to run on any device. 14 Path planning can be divided into two steps: global path planning and local path planning. Global path planning deals with generating the most ideal route to reach the final destination. Local path planning is responsible for obstruction avoidance and path planning for each segment generated by the global path planner. 15 There are some methods for local path planning, such as artificial potential field method, A* algorithm, and discrete optimization approach. The artificial potential method is calculated by the attractive and repulsive potential functions. However, the computations are complex and sometimes unsolvable, because the path is described in high order polynomial. 16 In addition, it does not always yield a path to the goal due to local minima in the vector field. 17 For A* algorithm, it does not work well on lane tracking based on driving with few obstacles. 17 The discrete optimization approach calculates a finite number of paths. A cost is computed for each path, and finally the path with least cost is selected. In this paper, a discrete optimization method 18 is used, based on the frenet coordinate system for autonomous vehicle path planning, which has strong practicability in assisted driving and self-driving.
Fuzzy logic is widely used in motion control, because it provides the ability to control system with construction of complex mathematical model. Fuzzy logic is used for mobile robot motion control, where sensors of mobile robot contain uncertain and incomplete information. 19 Fuzzy logic technology is also used in traffic management. There is applicability issue to create models of traffic, but IF-THEN logic can deal with vague expectations and respond to situations. 20 In this paper, fuzzy sliding mode control method is used to minimize vehicle's steering angle error. The vehicle's cross track error is defined as d iE . An angular velocity control law is designed to make the cross-track error to zero.
The contribution of this paper is: (a) A real time lane detection system is proposed based on vision system functions, where image acquisition and processing functions are used to extract lanes. Although there exists lane detection algorithms in previous work, the proposed lane detection system processes fast and the performance is good. (b) In order to detect curve lanes, deep learning is applied for lane detection, based on fully CNN. It shows how to perform lane segment detection by training a deep neural network for lane segmentation. Compared with, 21 in this paper, the number of layers is increased, and the number of convolution kernels is modified in each layer. A parallel jump structure is used to connect the encoding feature map to the decoding feature map. UpSampling2D is changed to Conv2D Transpose to implement the upsampling process. (c) The cubic spline interpolation method is used for path generation, based on GPS data, where distance between two adjacent path generation points is same. Compared with the connection method of the cubic polynomial fitting algorithm, the curve fitted path by the cubic spline interpolation method is smoother and more satisfied with the vehicle motion pattern. (d) Optimize trajectory planning method depending on frenet coordinate frame is used to plan the safe, easy and comfort trajectory. In addition, compared to the Cartesian coordinate frame, frenet coordinate frame simplifies the solution of road curve fitting problems, especially in the case of complex road environment. Compared with pure pursuit algorithm, the result shows that the path planned by optimize trajectory planning method is more closed to the predefined path. (e) fuzzy sliding mode control method is used to design angular velocity control law to minimize vehicle's steering angle error. (f) the simulation and experiment results show effectiveness of the proposed methods.
In this paper, section 2 gives the modeling of the vehicle. Section 3 describes two lane detection systems. Section 4 discusses the path generation and path planning methods. Section 5 outlines the fuzzy sliding mode method for vehicle steering control. Section 6 and Section 7 show the simulation result and experiment result, respectively.

Modeling
The local and global coordinate of the vehicle is shown as Figure 1. X l MY l is the local coordinate frame. X g OY g is the global coordinate frame. M r is the central point of rear axle, which is the original point of local coordinate frame. M f is the central point of front axle. The angular difference between the global and local coordinate is given by u. The orientation of the local coordinate frame about the global coordinate frame is expressed by the rotation matrix as The local coordinate of the system is shown in Figure 2, where l is the wheelbase, u is wheel steering angle, d is the distance between two rear wheels, ICR is instantaneous center of rotation, and d r and d f are instantaneous radius of rotation of M r and M f , respectively. The overall speed of the vehicle v r is the speed of rear wheels, along with the axes direction of X l of local coordinate frame. v f is the speed of front wheels. The relationship of v r and v f is: In Figure 3, point w is the center gravity point of the car. The configuration of the vehicle about point W is defined as ½x, y, d T in global coordinate. x, y and d are   the x coordinate, y coordinate, and orientation of the vehicle, respectively. v is the speed of the vehicle. w f and w r are the distances between the center gravity point and the front wheel and the rear wheel, respectively. The kinematic model of the vehicle is calculated as where b is calculated as: Lane detection system Luminance channel is for white lane detection. In order to remove or enhance isolated feature, grayscale morphology is used. Two functions are used in this block: erosion and dilation. Erosion reduces the brightness of pixels surrounded by neighbors with a lower intensity. Dilation increases the brightness of pixels, which are surrounded by neighbors with a higher intensity. Based on the environment condition, erosion or dilation is chosen for gray morphology. Finding edge block is used to find a line describing the edge with points along the edge, and output the coordinate information of the points.

Real time lane detection system
Converting pixel to real world is applied to transform pixel coordinate to real-world coordinate. For obtaining lane tracking points block, the algorithm is processed as Figure 5. B i x bi , y bi ð Þ and C i x ci , y ci ð Þ are the edge points coordinates, based on finding edge and converting pixel to real world blocks. The tracking points A i x ai , y ai ð Þ is based on its configuration in (3) and (4), and calculated as.

Lane detection system using fully CNN
In this section, deep learning is applied to detect lanes based on fully CNN. It shows how to perform lane segment detection by training a deep neural network for lane segmentation. SegNet is an encoding-decoding structure. It uses different method in upsampling and downsampling. Based on, 21 Figure 6 shows pipeline used in lane detection system. The neural network architecture includes seven convolutional layers, three polling layers, three upsampling layer and seven deconvolution layers. However, this model is not stable during lane detection, so the model is improved in this paper as shown in Figure 7. Different from network in Figure 6, the number of layers is increased in the network. The number of convolution kernels is modified in each layer. A parallel jump structure is used to connect the encoding feature map to the decoding feature  map, so that multi-layer information can be used in classification prediction. UpSampling2D is changed to Conv2D Transpose to implement the upsampling process, since Conv2DTranspose has the process of learning, which has better result. The BDD100K data is used, consisting of images and labels. For training, the dataset is divided into training set and validation set. The neural network architecture includes 10 convolutional layers, four polling layers, four concatenate layers and 10 deconvolution layers. The model is compiled with mean squared error as the loss function.
And the model is trained on the training set, and validated on the validation dataset. Images are inputs of prediction process. Using the trained model result, the prediction process outputs images with regions of the detected lane.

Cubic spline interpolation model
Autonomous vehicles do path planning and tracking algorithm based on GPS data. However, due to communication delay, the GPS data cannot be obtained continuously. Distances between each two conjunction GPS points are not same. Therefore, if the GPS data is directly used to do the path planning and tracking algorithm, without path generation algorithm, the speed of the vehicle is always changed. In order to control autonomous vehicle smoothly and safely movement, constant speed is pursued for the vehicle. The cubic spline interpolation method is used for path generation, based on GPS data, where distances between two adjacent path generation points are the same. Compared with the connection method of the cubic polynomial fitting algorithm, the curve fitted path by the cubic spline interpolation method is smoother and more satisfied with the vehicle motion pattern. The cubic polynomial fitting algorithm produces a curve arc in some straight lines' segments. This is not the expected vehicle operating characteristic. Based on vehicle kinematics, the vehicle should take a straight line as much as possible. Only in the direction of the transition, a curved path, which matches the vehicle turning characteristics, is needed. The feature of cubic spline interpolation method is as: 1. The cubic spline curve is continuous and smooth at the joint. Three points S 1 = x 1 , y 1 ð Þ, S 2 = x 2 , y 2 ð Þ, and S 3 = x 3 , y 3 ð Þ are defined to be fitted. The cubic functions to fit points S 1 and S 2 , to fit points S 2 and S 3 are calculated as Equation (7) passes through S 1 and S 2 , and equation (8) passes through S 2 and S 3 , so the following equations can be obtained: Based on feature 2, the first derivative of both sides of equation (11) can be obtained as: Based on feature 3, the second derivative at the start and end points should be continuous, the following are obtained.
Supposed that there are n + 1 path points: ., x n , y n ð Þ. The difference between two conjunction points is calculated as Combining the above six equations (9) to (14), the parameters a, b, c, d in the cubic spline interpolation function can be calculated.
where m i is the solution of the following matrix equation.
Optimization of trajectory planning method based on frenet coordinate frame Frenet coordinate frame. The centerline of the road is defined as the road reference line. As shown in Figure 8, a frenet coordinate frame is established using the tangent vector t of the reference line and the normal vector n. The coordinate frame is based on the starting road center line, and the coordinate axes are perpendicular to each other. The direction of the road reference line is s direction, which is called longitudinal direction. The normal direction of the reference line is d direction, which is called lateral direction. Compared to the Cartesian coordinate frame, frenet coordinate frame simplifies the solution of road curve fitting problems, especially in the case of complex road environment, since it is easy to find the road reference line and describe the location of the vehicle, based on the longitudinal direction and lateral direction. Compared with the Cartesian coordinate system, the frenet coordinate system simplifies the solution of the road curve fitting problem. In the road environment, we can always find the reference line of the road, which is the center line of the road. Therefore, the vehicle position, based on the reference line, can be simply represented using the distance along the longitudinal direction and the distance along the lateral direction. The calculation of the speed in both directions is also relatively simple, based on frenet coordinate system.
Optimization trajectory planning method. Due to lane change or obstacle avoidance, there is a certain lateral offset between the car and the desired lane. The car should return to its desired lane, based on the best compromise result between the comfort experience in the car and the time required to reach the desired lane position. In longitudinal direction, if the car is moving too fast or too close to the front car, it must slow down significantly without rushing. Comfort can be described by the jerk, which is difference of acceleration. 18 Based on, 22 a quantic polynomial is the jerk-optimal connection of a start state P 0 = p 0 , _ p 0 , € p 0 ½ and an end state within the time interval T = t 1 À t 0 in a one-dimensional problem. The jerk can be described as: where p t ð Þ is described as: Trajectory plan in lateral direction. In Lateral Direction, define the start state as Ã , based on equation (22), the lateral offset, lateral velocity, and lateral acceleration of the start state are described as Define the end state as , the lateral offset, lateral velocity, and lateral acceleration of the end state are described as Define t 0 = 0, a d0 , a d1 , and a d2 are calculated as a d3 , a d4 , and a d5 can be calculated based on the following matrix Instead of calculating the best trajectory, a series of candidate trajectories are generated, and then a valid trajectory with the least cost path is selected. The cost function is described as: where k j , k t , k d .0. Item k j J t d t ð Þ ð Þ is used to penalize the candidate trajectory with higher Jerk. Item k t T represents the breaking time. Item k d d 2 1 describes the difference between the goal state with the reference line. For the optimizing trajectory, the vehicle should move in parallel to the center line, so d 1 is a constant and Þand T j 2 T min , T max ð Þ , the candidate trajectories can be calculated as where d min , d max and T min , T max are the constraints for d i and T j , respectively. Define Dd and DT as sampling density, equations (35) and (36) can be described as: Trajectory plan in longitudinal direction. In order to plan trajectory in longitudinal direction, three conditions are considered: following, merging and stopping, and keeping at a constant speed. To calculate the cost function, for the following, merging and stopping cases, the start state is set as S 0 = s 0 , _ s 0 , € s 0 ½ , and the target position s target is defined. For the velocity keeping case, the target velocity _ s target is defined. For the following cases, the target position is defined as: where D 0 and t are constants. s lv and _ s lv are the position and velocity of the leader vehicle, which are described as below, with assumption € s lv t ð Þ = € s lv t 0 ð Þ = const.
The first derivative and second derivative of s target t ð Þ are given as For merging case, the target position is defined as: where s a t ð Þ and s b t ð Þ are pair of vehicles' positions, which the vehicle should merge.
For stopping case, the target position is defined as: where _ s stop = € s stop = 0. Combining different end conditions Ds i and T j as below, candidate trajectories can be computed.
For these two cases, the cost function is expressed as: In the keeping at a constant speed case, for a start state S 0 = s 0 , _ s 0 , € s 0 ½ and an end state S 1 = _ s 1 , € s 1 ½ , combining different end conditions Ds i and T j as below, candidate trajectories can be computed.
The cost function is expressed as: Finally, comparing the cost function in lateral direction and the cost function in longitudinal direction as below, the best trajectory can be selected.
where k lat and k lon are the constants. C d and C s are the cost function in lateral direction and in longitudinal direction. The trajectory, which has the minimize value of C total , is the optimization trajectory.

Fuzzy sliding control method for vehicle steering
The vehicle movement along a predefined trajectory is expected in constant speed. If constant steering angle is used to control the vehicle, the vehicle's movement will be oscillatory. For solving the problem, we propose a fuzzy sliding mode control method to control vehicle's steering angle. Define the vehicle's cross track error as d iE . The switching function s i is as: The exponential reaching law is designed as where k 1 .0, k 2 .0, sgn s i = s i s i j j+ s , s is a positive constant.
The control law is defined as In order to prove the stability of the control law, the Lyapunov function is designed as The derivative of the Lyapunov function is When d iE = 0, _ V = 0. When u iE 6 ¼ 0, _ V i \0. Therefore, the system is asymptotically stable. k 1 should be large to have fast trending motion from system status to sliding surface, if a moving point is far from the sliding surface. Otherwise, k 1 should be small to have slow trending motion to weaken the charting. Therefore, the fuzzy sliding mode controller is designed, where the fuzzy rule is described as Table 1. Seven fuzzy variables are used to describe the fuzzy partitions, as positive big (PB), positive medium (PM), positive small (PS), zero (ZO), negative small (NS), negative medium (NM), negative big (NB).
The triangular function is defined as: The output is based on centroid defuzzification method, and is given as (55). m k 1 À Á and m k 2 À Á are the membership value for point k 1 and point k 2 , respectively.
The fuzzy exponential reaching law is defined as The fuzzy sliding mode control law is as Simulations

Real time lane detection simulation
The powerful capabilities of LabVIEW are used to simulate lane detection. The original figure is shown in Figure 9.    Figure 9. The lane's information is in pixel coordinates. The lane's information is transformed in real world coordinates using the calibration information. Based on Figure 5 and equations (5) and (6), the yellow lane in Figure 9 is the tracking lane for vehicle.

Fully CNN lane detection simulation
Based on description in Figure

Trajectory planning and tracking simulation
In Figure 15, the reference points are defined as (0, 0), (0, -4), (20.5, 1), (30, 6.5), (40.5, 8), (50, 10) and (60, 6) in meters to generate the trajectory. Based on cubic spline interpolation method, the white trajectory is generated, where the white trajectory passes through the reference points. In the white trajectory, distances between two adjacent points are the same. Therefore, the vehicle can move in the constant speed. Obstacles are located at (20,10)     vehicle should avoid the obstacles. Using the optimization trajectory planning method based on frenet coordinate frame, the best trajectory is planned as the red lane. The trajectory avoids obstacles and finally tracks the generated path. Using the fuzzy sliding mode method, the vehicle's yaw angle error is minimized as shown in Figure 16. In the frenet coordinate frame, initially, the vehicle is located at (1,0). Firstly, the vehicle should track the white path from its initial point, so error occurs. At time step 26-42, the vehicle should avoid obstacle, instead of tracking the white trajectory. Therefore, the vehicle should change its orientation to avoid obstacle, error occurs. However, finally, the vehicle yaw angle is close to zero.
As shown in Figure 17, pure pursuit trajectory planning method is compared with frenet optimized trajectory planning method. Passing through points (0, 0), (8, -4), (12,2), (16,4), (20,2), (24, 0) and (27, 3) in meter, path is generated as blue lane in Figure 17. Based on frenet optimized trajectory planning method, the trajectory is planned as red lane. It is almost the same as the generated blue lane. However, based on pure pursuit trajectory planning method, green lane is planned. There is difference between the planned green lane and the generated blue lane, in x range of (0, 3).
In order to show the effectiveness of the trajectory, the proposed trajectory planning and tracking method is tested using a simulator. The simulator is made to demonstrate the performance on an outdoor autonomous vehicle. As shown in Figure 18(a), the left window is the simulator window, and the right window is shown the path based on the algorithm processing in LabVIEW. Based on the proposed path planning algorithm and fuzzy sliding control method for vehicle steer, the speed and steer commands are calculated. Then the commands are sent to the simulator based on RS232 communication. The vehicle in the simulator will move based on the speed and steer commands. Initially, the vehicle is located at (10 m, 0, 0). The light blue obstacle is located at (28 m, 0). The length of the vehicle is    around 1.1 m. The target speed is defined as 10 km/h. The green lane is described as the generated path, which guarantees that the vehicle avoids the obstacle well. The dark blue lane is shown as the vehicles actual path. In Figure 18(b), the vehicle arrives at (20 m, -1 m) to avoid the predefined obstacle. In Figure 18(c), the vehicle tracks back to its predefined straight line. In Figure  18(d), the vehicle moves forward to its predefined straight line. The test simulator video is shown in https://www.dropbox.com/s/5up9w26hra799xv/ path%20tracking.mp4?dl=0.

Experiment
The experiment is used to show the effectiveness of the trajectory planning method. As shown in Figure 19, the length of curved lanes is 4 m. The distance between two black lanes is 1.1 meter. Firstly, a curve path is generated, using cubic spline interpolation model. Then, the vehicle plans and tracks the trajectory successfully, based on optimization trajectory planning method in frenet coordinate frame. At t = 20 s, the vehicle passes through the curved part of the lane. At t = 30 s, the vehicle finishes the full path.

Conclusions
In this paper, a real time lane detection system is proposed based on vision system functions, using webcam camera. In order to detect curve lanes, deep learning is applied for lane detection, based on fully CNN. The cubic spline interpolation method is used for path generation, based on GPS data, where distances between two adjacent path generation points are the same. Optimization trajectory planning method depending on frenet coordinate frame is used to plan a safe, easy and comfort trajectory. In addition, compared to the Cartesian coordinate frame, frenet coordinate frame simplifies the solution of road curve fitting problems, especially in the case of complex road environment. Fuzzy sliding control method for  vehicle steering control is proposed. Finally, the simulation and experiment results show the effective performance of the proposed methods.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) received no financial support for the research, authorship, and/or publication of this article.