Reference Path Generation and Obstacle Avoidance for Autonomous Vehicles Based on Waypoints, Dubins Curves and Virtual Force Field Method

: In this study, reference path generation based on waypoints, Dubins curves and obstacle avoidance are focused. The motion of a wheeled robot vehicle is modeled by following the principles of point mass approach. While motion planning is performed, the importance of obtaining the shortest distance between two target points is illustrated. Dubins curves and waypoints are used to construct an optimum path generation strategy. Path planning algorithm developed is combined with an obstacle avoidance approach, known as the virtual force field method. The whole system was tested for different scenarios. In the first scenario, the path generation was tested for the case where obstacles, start and finish points of the reference path, and initial and final robot vehicle orientations were defined. In this case, waypoints were not specified. In the second scenario, the performance of the algorithm was tested for generating a complete reference path. Waypoints were specified and the shortest path between the start and end points of the desired path was generated. Successful results were obtained and presented in this paper.


Introduction
Motion planning is a key factor for autonomous ground robots and vehicles. For example, consider an autonomous orchard vehicle which is equipped with a robotic hand used for pruning the branches of fruit trees. The vehicle may have a job such as it finds a predefined tree and prunes its unwanted branches. Before conducting the pruning operations, the vehicle needs a reference path to move to the place where the tree (of which braches is to be pruned) has been planted. In such a task, reference path generation is one of the important issues that should be taken into account. Considering the energy consumption, working time, maintenance of the running parts, an optimized path planning solution should also be used for the autonomous vehicle applications. While a reference path is generated, two points inside the path should be connected in a way that the possible shortest distance can be obtained. There are some restrictions while generating a desired path. The limitations depend on some parameters like vehicle's geometry, turning and working capability, terrain characteristics, surface inclination etc. Considering these limitations in motion modeling and path generation for the vehicle allows building optimum path geometry. Dubins curves [1] and waypoints together offer optimized solutions to create shortest distance between two points. Furthermore, the shapes of Dubins curves can be arranged according to the limitations of the vehicle. They can also be used together with the waypoints to create more effective and more usable path planning algorithms. There are two commonly used motion models for the mobile robots and autonomous ground vehicles. They are the car-like robot model and the point mass model [2]. The car-like robot model takes the steering angle information into consideration as the points mass model does not. The point mass model is the simplest approach since it is easy to use and easy to implement. If the working speed is not so fast and it is assumed that no slippage occurs in longitudinal and lateral directions, this modeling structure can be effectively used. Obstacle avoidance is another key factor that should be considered while designing desired path. The path generation algorithm should be cleverly designed so that when the vehicle is faced with unknown obstacle(s), the path planner is able to force the vehicle to avoid crashing. Some of the literature studies related to the path generation algorithms constructed using the methods of waypoints, Dubins curves and obstacle avoidance are reviewed as follows: In [3], the problem of motion planning for traveling salesman was taken into consideration. Predefined waypoints and finding the shortest global path of nonholomonic vehicles were studied. Motion constraints for the nonholomonic system were constructed using Dubins curves. In [4], real-time dynamic path planning for nonholomonic robots were studied. The motion model of the robot considered was set up by following the basis of the car-like robot approach. The shortest path optimization was done using Dubins curves method. In [5], minimum length curvature problem for traveling salesperson was studied. An algorithm, which solves the problem of traveling salesman with neighborhoods, was developed by the use of Dubins curves. It enables to convert the infinite dimensional combinatorial optimization problem to a finite dimensional asymmetric one. In [6], Dubins curves approach was adapted to the unmanned aerial vehicle application. An aircraft system was simply modeled using Dubins curves. The proposed algorithm was used in optimization for trajectory tracking control of multiple unmanned aerial vehicles. In [7], a methodology including path planning and obstacle avoidance methods was proposed for autonomous mobile robots working in known 2-D environment. Virtual obstacle concept, virtual goal concept and virtual force field concepts were integrated and a new methodology called hybrid virtual force field was developed. The primary objective of proposing the method was to achieve effective path planning. The virtual force field [8] and the vector field histogram [9] methods were proposed for real-time obstacle avoidance purposes. The methods can be used for identifying unknown objects. They are able to warn the control system for steering the autonomous vehicle to avoid crashing with the obstacles. An obstacle avoidance algorithm was proposed in [10]. The method was adapted for real-time navigation of fast mobile robots. The proposed system was fed by the feedback information coming from ultrasonic sensors. The methodology permits a smooth and continuous motion planning as well. In [11], three different methods used for obstacle avoidance and path planning objectives were discussed. The sensor-based methods of bug algorithms, potential field and vector field histogram were investigated and their powerful sides were reviewed. In this study, path planning algorithm constructed using waypoints, Dubins curves and obstacle avoidance is developed. Obstacle avoidance was performed by the use of virtual force field method. The overall system proposed was tested for different scenarios to show the performance of it. The details of the test studies are given in this paper. This paper is organized as follows: In Section 2 a wheeled robot's kinematic model is constructed. Point mass model is also presented in this Section. In Section 3 Dubins curves and its details are given. In Section 4, the principles of the virtual force field method used for obstacle avoidance task is presented. In Section 5 tests results are given in detail. The paper is concluded in Section 6 with an analysis of the results and the plans of the future works.

Motion Model of a Wheeled Vehicle
A simple 4-wheeled vehicle / robot representation is given in Figure 1. The vehicle shown cannot be driven in the lateral direction. It has capability of moving in longitudinal direction only. Its front wheels are rolling while the rear wheels are sliding during a turning motion. The vehicle shown in Figure 1 can be considered that it is in motion in 2D plane. The parameter space of this representation can be constructed by Q = (x, y, θ). The (x, y) coordinate axis is fixed in the mid-center of the rear axle. The orientation of the vehicle is denoted by θ. The steering angle is indicated by ɸ. The distance between front and rear axles is given by L. The sign convention is denoted as positive and negative for CCW and CW motions, respectively. When the steering system is rotated to a constant steering angle, the vehicle does a circular motion with the radius, r ( Figure 1). The kinematic model of the vehicle represented above can be constructed by following the principles of point mass and car-like robot models as shown in (1-3).
x cos( ) where the forward velocity of the vehicle is specified by υ.

Dubins Curves
Consider the system given in (1)(2)(3). It is assumed that the vehicle moves with a constant longitudinal velocity with υ = 1. Minimum turning radius (r min ) can be obtained when the steering angle reaches to its maximum value (ɸ max ). In terms of energy consumption, the length of the turning curve (L c ) should be minimized while the vehicle does turning from a starting point (Q s ) to a final point (Q f ). This goal can be considered as a bounded-curvature shortest path problem. In order to solve this problem, one should construct a methodology that creates a curvature bound for generating the shortest path, which may be called a straight line.
The following optimization function can be defined to find the optimum path [1]: where t f indicates the time elapsed during the vehicle's travel to reach to its final position targeted. The kinematic model can also be simplified and the point mass model can be constructed: where the variable υ c is between -tan(ɸ max ) and tan(ɸ max ). It is stated [1] that Dubins curves can be constructed by using only three motion definitions shown in Figure 2. As seen in this figure, straight motion is defined by S. In this motion case, no steering action is needed. Turning to left and right directions are specified by L and R, respectively. Turning left and right requires +1 and -1 steering actions, respectively. As stated above, in a path planning algorithm the shortest path between two points can be formed using Dubins curves (the definitions are shown in Figure 2). In Figure 3, two examples about configuring the shortest paths [1] are presented. RSL and RLR configurations are constructed in Figure 3-a and 3-b, respectively. The start and end points for two configurations are specified by Q s and Q f , respectively. In the first configuration, Dubins curves can be characterized by using the parameters of (R , S d , R μ ). In the other configuration (Figure 3-b), the parameters, (R , R β , R μ ), can be set to build the shortest path.

The Virtual Force Field Method
Real time obstacle avoidance is an important issue for the mobile robotic and autonomous vehicle applications. While performing motion planning and reference path following tasks, knowing the positions of the obstacles are the key factor for achieving safe drive. The obstacle avoidance concept was constructed first by Khatib [12] in 1985. Moravec [13] was proposed the other popular obstacle avoidance strategy, known as certainty grids. Borenstein and Koren [8,9] constructed the virtual force field methodology. In their approach, the work space is divided into cells by a certainty grid. Each cell created into the work space is assigned to a certainty value, which is the information of confidence whether there is an obstacle in the current cell or not. The strategy offers that if the certainty value of the cell is bigger than a predefined threshold value, the probability of an obstacle inside the cell is higher. In Figure 4, an obstacle is located inside the detecting range of the laser scanner that is mounted on a mobile robot. The distance between the laser scanner and the obstacle is shown by d. The coordinate systems for the environment and the obstacle are presented by (X, Y) and (x w , y w ), respectively. The certainty values of the cells are shown in Figure 5. While the robot is in motion, the certainty values of each cell are updated. When there is an obstacle around the robot, the cells having bigger certainty values creates repulsive forces on the robot moving. This causes the robot goes away from the cells that publish warnings. Note that the certainty values are directly proportional to the magnitude of the force. When the robot comes near to the obstacle, the magnitude of the repulsive force is gradually increased.  The mathematical representation can be constructed based on [8,9,10]: x x y ŷF x y dd d       (6) where F c indicates the force constant. The distance between the current cell (i, j) and the scanning system is shown by d i,j . The certainty value of the current cell (i, j) is specified by C i,j . Coordinates of the target, scanning system and the cell are presented by (x t , y t ), (x 0 , y 0 ) and (x i , y j ), respectively. The overall repulsive force is obtained by summing all the repulsive forces acting on each cell: The adaptation of the virtual force field method (performed using Matlab) is illustrated using an example given in Figure 6  In order to generate the desired path staring from the robot vehicle's current position to the target location, reaction force (F r ), attraction force (F t ) and the resultant force (R) are obtained by using the information shown in Figure 7. These results are presented in Figure 8. Note that they are obtained recursively while the robot vehicle is in motion (they are calculated for each cell). The desired path now can be generated by combining the virtual force field and Dubins curves methods. Once the locations of the robot vehicle, target and obstacle, and the safety circle information are defined, the algorithm constructed based on the methods mentioned above is able to generate the desired path.
The generated path is shown in Figure 9.

Test Studies and Results
To see the performance of the procedure constructed using the way points, Dubins curves and the virtual force field methods, a series of tests was performed in Matlab computational environment. The tests were conducted by using the maps (taken from the Google Maps) of the main campus area of the Bulent Ecevit University located in Zonguldak, Turkey. The test area is shown in Figure 10. In the first test study, the performance of the path planning algorithm developed using Dubins curves and the obstacle avoidance based on the virtual force field method was tested. As shown in Figure 11-a and 13-b, the map of the centre of the campus area (41°27'03.1"N 31°45'44.7"E) was used for this test. Instead of using waypoints, starting and end points of the planned motion were specified. The desired robot orientations at the start and finish were also defined. Six identical obstacles were located in the way of planned motion. They were defined as circular shapes. In order to create safe drive, safety circles were also added. The details and the desired path created using the developed algorithm are shown in Figure 11-a and 13-b with zoomed views.
In the second test study, the use of waypoints and the path generation were tested. The map of the university main campus area was used for testing the algorithm. Waypoints and start (Q s ) and end (Q f ) points of the motion were specified as shown in Figure 12. In this test study, to see the performance of the path planning algorithm including waypoints and Dubins curves, any obstacle was not be defined.  The generated path is illustrated in Figure 13. The algorithm developed connects waypoints using Dubins curves. The straight parts of the generated path are shown with black-colored lines whereas the curved paths are represented with red-colored lines.
To show the details of paths in turns, zoomed views are given. The accuracy and geometry of the connections between straight and curved motions or curved and curved motions are presented in these images. The surface information is not taken into account in the algorithms developed in this study since the reference trajectories are constructed by using the kinematic approach of the vehicle model. The methodology proposed can be adapted into any study that considers different terrain / surface characteristics. It can easily be combined with the dynamic, slippage, driving, etc. models of the autonomous vehicles / mobile robots. The reference trajectory generation procedure introduced here can also be connected to the motion models including surface inclination information.

Analysis and Conclusion
Autonomous ground vehicle and mobile robot applications need to use predefined desired path geometries. The distance between two points should be generated for creating shortest path. The geometry of the reference path is an important factor since the energy consumption and the travel time elapsed for performing a task can be minimized by configuring the shapes of the path geometry. To find an optimum solution for generating a desired path for a mobile robot / autonomous vehicle application, a path design procedure is proposed in the scope of this study. The procedure focuses on using waypoints, Dubins curves and an obstacle avoidance system based on the virtual force field approach. The entire system is tested for different scenarios to investigate the performance and accuracy of the proposed methodology. Test results show that the system developed can be used for desired trajectory tracking and reference path following control objectives of autonomous vehicle applications. As a future work, the processes presented in this paper will be implemented into a 4-wheeled mobile robot which is equipped with a laser scanner rangefinder for obstacle detection, dc actuators with high resolution encoders, an IMU and a computational unit for reference trajectory generation. By this way, when path planning algorithm is integrated with the steering and driving controllers, the performance of the proposed system will be able to be observed.