Obstacle Avoidance Strategy and Implementation for Unmanned Ground Vehicle Using LIDAR.
Obstacle/Collision Avoidance has been one of the most important features in Advanced Driver Assistant Systems (ADAS) for ground vehicles. The basic collision avoidance will significantly improve vehicle and drivers safety. Many commercial vehicles have actually been equipped with the feature, called 'collision warning with auto brake'. The conventional sensors--like ultrasonic sensor and Radar, sometimes laser and camera--have been used in commercial vehicle for obstacle detection. If sensors detect a potential imminent crash, the collision avoidance system would either warn driver or take actions to brake automatically, or even steer away. Due to numerous unexpected possibilities on the road, current applications of the collision avoidance feature could be helpful in some cases but could also be unhelpful or even harmful in other cases . Apparently, the market demand of obstacle avoidance system is huge , the desire to improve the technology is urgent. In addition, the obstacle avoidance is widely accepted as one significant trend in developing ground autonomous vehicle . The feature of obstacle avoidance which enables vehicle driving safely without hitting other vehicles, is the one of the basic requirements for autonomous vehicles.
In this research, we tried to develop one complete process of obstacle avoidance feature for ground vehicles by using Robot Car (RC). This process started with data collection, and then data processing, path planning and finished with precise vehicle control. In obstacle detection, the sensor LIDAR, short for Light Detection And Ranging, was employed to detect the obstacles on the fore-path because LIDAR has high accuracy within large detecting range [4, 5]. With data collected and processed, the path planning is one of the most critical steps for obstacle avoidance success.
Path planning research has been investigated for many years. Different approaches have been proposed to generate the path in the past, such as, curving fitting method , potential-field method , A*/D* search method, method of choose from pre-defined paths , random tree search [10, 11], and Multiple Point Constraint (MPC) optimization . Each method has its own pros and cons in terms of algorithm capability, computational load, smoothness of generated path, etc.
Curve fitting  is based on picking feasible points on the fore-path and drawing a polynomial function, to fit the chosen feasible points. It is straightforward but hard to handle complicated situations. Potential field method  is applied to make vehicles following the steepest gradient of an artificial potential field, however, the path could be trapped in local minima in the field. A*/D*  search algorithm has been very popular in path planning because it can tackle very complicated situations and efficiently calculate an optimal route if there is one, but the path generated by A* search is grid-size dependent that may not be smooth. It would be a challenge to control the vehicle to follow the non-smooth path precisely, especially when the vehicle moves at high speed. The method of choosing from pre-defined paths  could be very efficient, since the feasible solutions are calculated and ready before vehicles confront obstacles.
The vehicle is picking one path from the group of predefined pathes that can avoid obstacles. This method can cover certain pre-defined scenarios. Random tree search [10, 11] is recommended for more complicated cases, but it requires efficient guiding heuristics to reduce the computational time.
Using optimization algorithm under Multiple Point Constraint (MPC)  is another comprehensive method for path planning. This approach considers equality constraints, inequality constraints, upper and lower bounds, nonlinear constraints, and vehicle capability in one formula, to customize the distance or the velocity for optimized path. The variables to be calculated are the discrete points on the fore-path. The steering angle smoothness can be controlled by applying nonlinear constraints on consecutive path points. The number of variables and constraints, so as the computing time, are depending on the obstacles layout. In this paper, we will use a method similar to MPC optimization, and make a comparison between this method and other methods, like Curve fitting and A* search.
In Section II, we will discuss an overall approach and demonstrate the overall obstacle avoidance process. In Section III, the details of implementing the process will be explained, including data collection, data process, and path planning and vehicle control. The simulation results with detailed discussions will be discussed in Section IV. The conclusions and future work are discussed in Section V.
The purpose of this research is to explore the process of obstacle avoidance for ground vehicle. There are three major sections in this process: i) Data collection, ii) Path planning and iii) Vehicle control, as shown in Figure 1. Data collection is to prepare data for path planning and also monitor the real distance between the vehicle and the obstacles for the safety check. During the process, vehicle is expected to travel to designated point automatically without hitting any obstacles on its way.
In the process, sensor LIDAR is used as RC car's eye to detect obstacles around. Other sensors, like cameras, ultrasonic sensor are available but only used as optional alternatives for the car. The data collected by LIDAR, has to be processed instantly. The data processing includes filtering noise and environmental data, clustering out data for each obstacle, locating obstacles and self-locating. After data processing, the distance and angle data for obstacles will be obtained and analyzed.
The next step is to generate a obstacle avoidance path for RC car. As aforementioned, there are many ways to generate a path from LIDAR data. In this paper, three methods, curve fitting, A* search and discrete optimization all will be used in different obstacle layouts according to their own advantages. The computing time of curve fitting is almost negligible, but the path solution is not guaranteed. The curve fitting method is only applied in simple cases, like one or two obstacles. A* search algorithm is relatively fast on computation time, if grid size is not too small. This could be an alternative solution if discrete optimization method can not generate a path immediately. The path from discrete optimization is feasible and smooth for vehicle control, which is the primary path planning algorithm used in this work. However, the optimization algorithm is sometimes not fast and could take up to 100 ms for certain obstacle layouts.
The last step is to control the RC car, to precisely follow the obstacle avoidance path. For simplicity, a bicycle model is employed and Stanley method  is used. Note that the steering angle limit for the vehicle is speed dependent. The higher the vehicle speed is, the less steering capability is. Vehicle control shall also be affected by noise and delay caused by data processing. The general procedure for all steps is given in Figure 1. The technical details for each part will be discussed in the next section.
RC Car and LIDAR
The research is using RC car as a platform to implement the obstacle avoidance procedure. RC car used in this work is built of a car frame, Raspberry PI III, Arduino Due, RPLIDAR and other electronic hardwares. The four wheeled car can be seen in Figure 2. The data collection is achieved by sensor LIDAR. Robo-Peak RPLIDAR 360[degrees] is a single beam 2D laser scanner (LIDAR) with a maximum range of 6 meters. The produced 2D point data is used for mapping, localization and object modeling. This LIDAR was mounted on top of the car and is primarily used to detect distant obstacles and implement the collision avoidance applications. Data Processing and Path planning is finished in single-board computer Raspberry PI III. Vehicle is controlled by the microcontroller Arduino.
This section consists of four subsections: data collection, data process, path planning and vehicle control. The RC car is used as the demonstrator.
The experiment set up is given in Figure 2. There are five tall cones considered as obstacles for the RC car. Side walls in the room are considered as the boundary for car movement. The RC car with LIDAR mounted on the top was asked to travel from backside of the five obstacles to front side, without hitting any obstacles.
Various sensors, like camera, Radar, ultrasonic sensors etc. can be used to detect obstacles around the vehicle. In terms of accuracy within large range, LIDAR, known as Light Detecting And Ranging, is one of the best, so LIDAR was employed in this research to help the RC car to observe surrounding objectives.
Within the range, LIDAR gives 360 degree all-around scan and provide distance and angle data for any obstacles. LIDAR is a laser scanning sensor. It can see and calculate the data points only for the obstacle surface on the sensor side, so the LIDAR data can not give complete geometry information of the obstacle.
In the experiment, RC car is moving as one meter per second. The data collected by LIDAR is given in Figure 3. Red dots are the 360 degree data points observed by LIDAR. Green box is added to show RC car location and cyan circles are drawn to show cone locations. The top figure shows the data observed before RC car entering obstacles. It can be seen that only first two cones can be detected by RC car. The middle figure shows the data when RC car is among obstacles. With RC car in this position, the LIDAR actually can obtain data from all five cones. The bottom figure gives the data when RC car is exiting obstacles, similar to the top figure, only last two cones can be detected.
There are three steps to process data 1) Filtering out low quality data. Usually the low quality data either come from noise or data marginally in the LIDAR range. 2) Clustering the segment data into differen clusters. As shown in Figure 3, for each obstacle, the LIDAR sensor can only see the obstacle surface which are towards to LIDAR. For multiple obstacles and other objectives, we will see different clusters of data, which are representing different objects, and 3)Locating the objects' location according the different clusters of data.
Ideally, every object can be simplified as a single point for the path planning later on. To avoid the obstacle, it only needs the car path which is away from this point for certain distance. It is not difficult to imagine that the data points for an obstacle are changing, if the RC car is moving. Because LIDAR is looking at the obstacles either in different angles or different distances, after the relative changed positions. It is almost impossible to recover the full geometry of the objective using the partial observed geometric surface, if the full geometry information is not known before hand.
In the case of RC car is moving aside to static obstacle, every data frame is different to every other data frame. There are two ways to post-process the changing data on single object surface, 1) one can compare data from frame to frame and find out the continuity of the cluster of data. If all data points are continuously shifting, the new data is determined from the same obstacle and will be merged to the previous data. 2) The second way to treat the data as the data is from one moving obstacle. The data of every frame are treated independently as they are from different obstacle. Apparently, method two is more general than method one but it require more computational load due to its redundant calculation. This paper is mainly using method one to avoid the computational redundancy.
Path planning is the key algorithm for this research. A feasible and low cost path is the most critical prerequisite to the success of obstacle avoidance. The comparison between different methods are discussed in the Introduction section. Here, there are three methods, 1) Curve fitting method, 2) A* search and 3)MPC optimization, are chosen to apply in this study for comparison. The Curve fitting method is to pick several feasible points, which are away from observed obstacles and draw a polynomial functions to fit them. A* algorithm is a grid-search algorithm, specifically, A* find the path that minimize:
f(n) = g(n) + h(n) (1)
where g(n) is the cost of the path from start node to n node and h(n) is a heuristic that is lowest cost from n node to target point.
The discrete optimization method is using optimization algorithm to calculate N points on the planned path. To simply the process for optimization, variable [[x.sub.1], x2,..., [x.sub.N] ] on one direction can be fixed. Total N-2 variables to solve are:
[[y.sub.2], [y.sub.3],... , [y.sub.N-l]] (2)
The objective function is:
[mathematical expression not reproducible] (3)
Inequality nonlinear constraint includes steering angle constraints:
[[theta].sub.i] [[theta].sub.max](v) (4)
where [[theta].sub.max] (v) is the max steering angle at given vehicle speed.
[mathematical expression not reproducible] (5)
where [d.sub.j] is defined as a safe distance to jTH obstacle.
The path comparison for these three methods are shown in Figure 4. Red solid line is the path by curve fitting method. Blue line represent the path from A* search. The yellow line represents the path calculated by optimization. It can be seen all three path starts from the starting point and ends at the end point, which are feasible path and they are all avoiding obstacles. However, path from A* search is non-smooth due to grid size. Path by curve fitting shows larger variation that the path from optimization method, so the steering angle for the path by curve fitting varies more too. It will cost more challenge vehicle control.
All paths are planned based on observed obstacles. Figure 5 demonstrates how planned path evolutes when RC car is moving by using optimization method. Grey dashed line shows the path in the idea case that all obstacles are known when RC starts. It is considered as the most optimized path. In reality, when RC starts, within 3 meters radius, there is no obstacles can be seen by LIDAR, so the first optimized path is a straigth line form start point to end point. RC car is moving forward along the straight until the LIDAR sees the first obstacle. The path needs to be recalculated to avoid the first obstacle. This path is given as the red line. The next path adjustment happened when LIDAR observed the second obstacles. The path is not given here due to very small path change to the red line. But when RC car moves to see the third obstacle, the path has been changed to green line to avoid the third obstacle. Following this logic, the path is changed to yellow after the LIDAR observes the last obstacle.
For simplicity, this work use bicycle model and Stanley Method  to control the RC car. Stanley method is a feedback control method to control the vehicle to follow the planned path by compensating wheels' angle according to i)the angle difference between car orientation and path slope, and ii) the distance between car and path, i.e. the further the car is away from the path, more steering angle is added. The equation of steering angle is:
[delta](t) = [theta](t) + [tan.sup.-1] [K*ef(t)/v(t)] (6)
where [delta](t) is wheel angle, K is gain, v(t) is vehicle velocity and ef(t) is the minimum distance from RC car to the path. [theta](t) is the angle difference between car orientation and path slope:
[theta](t) = [[theta].sub.car] - [[theta].sub.p ](7)
where [[theta].sub.car] is the car orientation and [[theta].sub.p] is the path slope at the closest-to-car point on the path.
The bicycle model is sketched in Figure 6. One can see that the bicycle model is largely simplified but capturing the main dynamic behaviors for the vehicle. Point A is the center for the four wheels car. Point C is the rotating center if the front wheel angle is not aligned with car angle. Point B is the next vehicle center after turning. Note that the distance of AC is equal to distance BC. [delta](t) is front wheel angle, [[theta].sub.p] is the path slope and ef(t) is the minimum distance from RC car to the path.
IV SIMULATION RESULTS
In this section, the RC car shown in Figure 2 is controlled to follow the path has been calculated and shown in Figure 4. Two paths, one path from A* search algorithm and another path from discrete optimization method, are chosen for simulation demonstrations. The path by curving fitting method is skipped due because it is very similar to the path by discrete optimization method.
The simulation used practical variables and values from the RC car, so that our control algorithm can be applied to the RC car directly. The car is 31 cm long and has saturation of steering angle at 45 degree. It is controlled with 100 ms time interval. Two parameters are tuned to show the car capability of path following, velocity v(t) and control gain K. The baseline condition is defined as velocity equals to 1 meter per second, control gain K = 1.
The simulation results of controlling car to follow the path from A* search method is given in Figure 7a and 7b. The initial car position is intentionally moved half meter away from the path to better show the effect from gain K. Figure 7a is showing the effect of gain K on car control while keeping velocity as one meter per second. The values of K are set to be 0.01,0.1,1,10 and 20 respectively. We can see than when K value is low, e.g. 0.01 or 0.1. The car is following path slope but keeping distance below the path. It makes senses because low K value is diminishing the second term in Equation (6), which controls distance between car and path. On the on other hand, while the gain value K is too large, e.g. 10 or 20, the car goes to path quickly at beginning, but it is over controlled and over sensitive to distances error between car and the path. It turns out as real car path is oscillating rapidly around the desired path as shown in Figure 7a.
Figure 7a and 7b shows the car path with different velocity, 0.1,0.5,1,2 and 5m/s, while the gain K is set to be 1. The car generally cannot follow the planned path exactly due to the first order derivative discontinuity of the A* search path. The overshoot can be observed at every non-smooth corner on the path. The car performed better, by means of following the path, at the lower speed. It is very difficult to control the car to following the path at high speed, e.g. 5 m/s even the speed is within car's capability.
The simulation results of controlling car to follow the path from discrete optimization method is given in Figure 7c and 7d. Similar to the results for A* search path in Figure 7a and 7b. The affect of gain value K and the affect of velocity are shown in Figure 7c and 7d respectively. Thanks to the smoothness of the path from discrete optimization method, the path following is better than path following to A* searched path. The car follows the path precisely at low speed if the gain value K is tuned to be 1.
In this paper, the complete process/strategy of obstacle avoidance was demonstrated by using a robot car with sensor LIDAR. The process contains data collection, data process, path planning and vehicle control. Different algorithms were experimented to dynamically generate a path when RC car is moving towards obstacles. Within this process, the RC car can travel to designated position automatically without hitting any obstacles on the way.
There are still few challenges for these implementation down the road, for example, 1) noise : the noise includes electronic noise for car control, data noise from sensor LIDAR and accumulated error from calculation/estimation. 2) Delay: delay mainly comes from data processing, path planning computational time and vehicle control, 3) Localization: car can be self-positioned if car can observe any static obstacles, but when there is no static obstacles observed or obstacles are moving, localization will become a challenge issue.
[1.] "Crash avoidance features cut insurance claims", iihs. org., Retrieved 4 April 2015.
[2.] "U.S. DOT and IIHS announce historic commitment of 20 automakers to make automatic emergency braking standard on new vehicles" U.S. Department of Transportation National Highway Traffic Safety Administration, 17 March 2016. Retrieved 17 March 2016.
[3.] Luettel, T.Himmelsbach M., and Wuensche H. "Autonomous Ground Vehicles Concepts and a Path to the Future". Proceedings of the IEEE. Vol. 100. no. Special Centennial Issue. pp. 1831-1839, 2012.
[4.] Frank, M.Pink O. and Stiller C. "Segmentation of 3D Lidar Data in Non-flat Urban Environments Using a Local Convexity Criterion." Intelligent Vehicles Symposium, IEEE, pp. 215-220, 2009.
[5.] Ryan H. and Bruch M. "Velodyne Hdl-64e Lidar for Unmanned Surface Vehicle Obstacle Detection." SPIE Defense, Security, and Sensing. International Society for Optics and Photonics, vol. 7692, 2010.
[6.] Chu, K.Lee M. and Sunwoo M., "Local Path Planning for Off-Road Autonomous Driving With Avoidance of Static Obstacles", IEEE Transactions on Intelligent Transportation Systems, vol. 13, no. 4, pp. 1599-1616, 2012
[7.] Dolgov, D.Thrun S., Montemerlo M. and Diebel-Path J., "Planning for Autonomous Vehicles in Unknown Semi-structured Environments", The International Journal of Robotics Research vol. 29, no. 485, 2010.
[8.] Liu J. "A multi-stage optimization formulation for mpcbased obstacle avoidance in autonomous vehicles using a LIDAR sensor." ASME 2014 Dynamic Systems and Control Conference. American Society of Mechanical Engineers, 2014.
[9.] Roberto, S.Gardi A., and Richardson M. "LIDAR obstacle warning and avoidance system for unmanned Aircraft." International Journal of Mechanical, Aerospace, Industrial and Mechatronics Engineering, Vol. 8, no. 4, pp.718-729, 2014.
[10.] Kuwata, Y.Fiore G. A., Teo J., Frazzoli E., and How J. P. "How motion planning for urban driving using RRT". IEEE Intelligent Robots and Systems, pp. 1681-1686, 2008
[11.] LaValle S. M. and Kuffner J. J., "Randomized kinodynamic planning", The International Journal of Robotics Research, vol. 20, no. 5, pp. 378-400, 2001.
[12.] Waydo S. and Murray R. M., "Vehicle motion planning using stream functions", IEEE International Conference on Robotics and Automation, pp. 2484-2491, 2003.
[13.] Snider J. M., "Automatic Steering Methods for Autonomous Automobile Path Tracking, Technical report CMU-RI-TR-09-08, Robotics Institute, Carnegie Mellon University, 2009.
Yang Wang, Ankit Goila, Rahul Shetty, Mahdi Heydari, Ambarish Desai, and Hanlong Yang
AVL Powertrain Engineering Inc
The author can be contacted via email@example.com or via phone at 734-647-5032.
We would like to thank AVL PEI management team for the support and internal funds for this work.
|Printer friendly Cite/link Email Feedback|
|Author:||Wang, Yang; Goila, Ankit; Shetty, Rahul; Heydari, Mahdi; Desai, Ambarish; Yang, Hanlong|
|Publication:||SAE International Journal of Commercial Vehicles|
|Article Type:||Technical report|
|Date:||May 1, 2017|
|Previous Article:||Potentials for Platooning in U.S. Highway Freight Transport.|
|Next Article:||Time-Dependent Reliability-Based Design Optimization of Vibratory Systems.|