Spline-Based Multi-Level Planning for Autonomous Vehicles

Rahul Kala and Kevin Warwick

School of Systems Engineering, University of Reading, Whiteknights, Reading, Berkshire, RG6 6AY, UK rkala001@gmail.com, k.warwick@reading.ac.uk

Abstract Autonomous vehicles are expected to operate in real time scenarios without human intervention. In this paper we design a multi-level planner for coarser to finer planning of the vehicle. The top level planning is done by using dijkstras algorithm that computes the shortest path between source and goal, telling the roads that need to be taken for navigation. The second level of planning deals with making a rough roadmap of points, avoiding all obstacles on the way. Unlike the goal seeking approaches in planning, where the intent is to construct shortest feasible route between source and goal, the path following approaches applicable in traffic scenarios attempt to keep the vehicle in centre of the road, maintaining maximum possible distances from both ends. This separation maximization approach helps in correcting any control errors, allowing easy alignment in obstacle avoidance, sudden obstacle emergence and other similar cases. While on a road escaping all obstacles without getting a dead-end and deciding which side to escape the obstacle from is important. We use a dijkstras algorithm for the same purpose, which attempts to navigate the vehicle by widest and shortest paths. The last level of planning consists of generation of smooth trajectories as per the allowable maximum speed of the vehicle for which BSpline curves are used. These curves ensure the vehicle to be able to navigate with the maximum allowable speed at any instant. Keywords- Unmanned Ground Vehicles, Dijkstras Algorithm, Robotic Motion Planning, Nonholonomic constraints, autonomous vehicles

needs to be given to ensure path smoothness and the validity of the non-holonomic constraints. It is evident that the vehicle may not be able to steer through all kinds of ways that a small mobile robot may be able to. A number of vehicles made for the DARPA Grand Challenge [10, 16] show a promising future where these vehicles would come into physical roads making travel safe and efficient. While numerous efforts have been made in engineering of these vehicles; including effective sensing, image processing and understanding, etc.; a reasonably less effort is devoted towards planning of vehicles [6]. The present planning is further aided by presumed presence of straight roads, speed lanes, speed zones, etc. which may not be present in all traffic scenarios. In this paper we extend the basic hypothesis of [1] who showcased effective motion of mobile robots in crowded areas. The planning algorithm used by the authors attempted to drive the robots in areas where its distance from other moving robots/humans was largest. In this paper we attempt to design a planning architecture such that the planned trajectory maintains as large distance as possible from the other vehicles. The maximization of distance enables the vehicle to be more robust against the sensing and actuation errors that may arise at any time during the motion. A considerable effort has been made for planning in the domain of robotics. However the solutions offered are either not complete or are very time costly. In place of trivial algorithms, the use of hybrid planning techniques does enable the resultant algorithm to be superior to the basic planning algorithms used. In [8] the authors use probabilistic A* algorithm along with fuzzy based planning for mobile robots. While the A* algorithm attempted to make shortest possible route for the robot at the coarser level, the fuzzy planning attempted to make resultant path non-holonomic and enable the complete algorithm to work in real-time mode. However the algorithm may never be regarded as complete as the coarser planning done by A* algorithm is itself probabilistic. Similar problems exist in techniques which aim in decomposition of the map [7]. These techniques may attempt in decomposition of the map based on connectivity, which does lead to completeness of the algorithm, however the techniques are time consuming. In this paper notions of road boundaries and roads are used for decomposition of a map into segments that

I.

INTRODUCTION

Planning for semi-autonomous vehicles is a reasonably new problem broadly into the domain of robotics. The problem deals with complete planning for the vehicle from high-end to low end, at varying degrees of abstractions. The problem considers determination of the path of a vehicle that arrives into the planning scenario at any position inside the map. Traditionally a lot of emphasis has been given to planning for robots [2, 14]; where the task generally involves enabling the robots to reach their goal without collisions on their way. The basic difference in planning for autonomous vehicles and planning for mobile robots comes in the presence of traffic rules in case of vehicles. Further the vehicle movements are restricted by the presence of road boundaries that the vehicle must never cross. The bulky size of the vehicles further reduces the mobility making planning task more tedious. Extra caution

aids the planning process, making planning effective as well as real time. In another work [9, 19] evolutionary algorithms are used for designing good paths for motion of robot, however the algorithm is very time consuming. Similar problems exist in the class of probabilistic roadmap [12, 14] algorithms that require a time consuming local optimization, along with a costly offline generation of roadmap at the initialization. The vehicles need to be able to plan in a real time mode, with optimal plans which make these algorithms unlikely to work in real life. Our approach considers the general problem of navigation of vehicles, and does not assume the presence of predefined speed lanes. Simulation studies in traffic scenarios further assume presence of speed lanes and speed zones which highly simplifies the planning scenario, with the planning being highly restricted to decision making regarding lane changes [15, 17]. Further planning for specific applications of vehicles (as in [20]) usually neglects the presence of constraints of road boundaries and traffic rules. Further we do consider that the obstacles may be complex with the possibility of being spread across large part of road. While the algorithm constructs paths to avoid all static obstacles, the moving obstacles may be handled by modification of the vehicle speed (as in [13, 18]) or by using a prioritized scheme [4, 11]. The entire planning task of the paper may be divided into three levels. The first level of the algorithm deals with the determination of the road that the vehicle would use for its motion in the given map. This gives us the path (at the highest level of abstraction) that the vehicle must follow. This path may however have a number of obstacles on it. The next level of planning hence deals with determination of the mechanism by which each of the obstacles on the way would be avoided. The output of this stage is hence a roadmap that guides the motion of the vehicle. The output of this level or the roadmap may however not be smooth enough for the motion of the vehicle unto the road. The validity of the non-holonomic constraints or the smoothness of the path is essential for the vehicle. The third level deals with the generation of the smooth trajectory of the vehicle. This trajectory may be used for the physical navigation of the vehicle using its controllers. The architecture of the algorithm may hence be summarized by figure 1. Map Road Selection Roadmap Generation

The paper is organized as follows. Section 2 presents the highest level of planning done by the algorithm, i.e. the level of road selection. The next level of planning or the roadmap generation is discussed in section 3. Section 4 presents the trajectory generation algorithm. In section 5 we present the simulation results. Finally section 6 gives the conclusion remarks. II. ROAD SELECTION

The road selection algorithm is the first layer of planning to be done for the task of planning for the vehicle. This layer of planning takes a graph consisting of a road network, and decides the set of roads that must be used by the vehicle in order to reach its destination from the initial point or the point of emergence. This algorithm works at the highest level of abstraction and hence has its input map of the most abstract format. In this stage of the algorithm it is assumed that all the roads and their intersections are perfectly known. This becomes the input to the algorithm. We convert this input in the form of a graph G<V, E>. The resultant graph made has all the intersections of the roads as the vertices (V) and the roads as the edges (E). This graph is used for further processing. The conversion of a road network to graph converts our problem of determination of the path of the vehicle from the source to destination as a single source-shortest path problem. The current road where the vehicle is to be found becomes the source and the final road where the vehicle must end its journey becomes the goal. It is assumed that the destination is after the vehicle has crossed the road and the source is at the start of the current road. The search problem is solved by the use of dijkstras algorithm [5] which as per our implementation may be given by. Road-Selection(G, source, goal) d[v] for all v [v] NULL for all v d[source] 0 Q V While Q u vertex with minimum d value for all v neighbor of u if d[u]+w(u,v)<d[v] d[v]=d[u]+w(u,v) [v] u Q Q {u} v Goal Path While v NILL Path {v} U Path v [v] Here w(u,v) is the physical distance between the vertices or intersections u and v. [v] is the parent function and d[v] is the total distance from the source. We assume that reaching goal from the source is always possible. The output of the dijkstras algorithm is the set of roads that must be visited by the vehicle in the same order for traversal. This now converts the problem of planning in a large city-like

Trajectory

Trajectory Generation

Figure 1. The general layout of the algorithm

map to a much smaller map consisting of only a single set of roads. The roads may be broken down into segments for the next levels of planning, with planning algorithm being done for each of the segments that the road is broken down into. In this way we increase the degree of abstraction for the lower levels of planning. III. ROADMAP GENERATION

Once we get a road segmented to be planned, the task of this level is to make out a strategy for avoiding each of the obstacles that may be present in the road segment. We assume that in many cases the obstacles may be reasonably large in number and may be present making a complex grid. In such a case the vehicle has the high degree of responsibility to decide a strategy to avoid all these obstacles. The planning may not be made in an instantaneous mode seeing only the obstacles in front. This is because after passing an obstacle it is very likely for the vehicle to be found surrounded by a new set of obstacles, surpassing which would be very difficult or in certain cases impossible. The information about the road is well known in advance (as the data is not dynamic), which is not the case with this level where the obstacles may appear or disappear at any time. This further stresses upon the real time nature of the obstacle avoidance strategy. Hence for the sake of completeness and timeliness we propose the use of dijkstras algorithm for the problem. The task in this level is hence twofold. We first need to generate a graph of the available road segment with obstacles. Once the graph is available we need to use the dijkstras algorithm to compute the optimal path from source to the destination. The aim of graph generation is to generate a set of vertices and edges that lie on the road segment being planned, such that the separation from the obstacles or road ends is the maximum. This is in line with the separation maximization hypothesis of the paper. We hence use a sweeping line and make it travel across the two ends of the road in the direction of the road. The motion is made in intervals of d units of distance. This line naturally cuts through some road boundaries as well as the obstacles. This gives us an idea of the regions which are under obstacles and the regions which are accessible for the motion of the vehicle. The vehicle must travel in the center of the accessible regions as per the set hypothesis. The mid-points of the regions on map formed by the sweeping line, which lie in the accessible regions, become the vertices of the graph being produced. Any two of these vertices are connected to each other if a vehicle traveling from one point can reach the other point by any straight or curved path. This gives us a graph all around the obstacles to be found in the road segment. For a sample road segment, the graph is given in figure 2. The graph hence formed can be directly used for the computation of the roadmap. The search performed by the dijkstras algorithm further requires specification of the source and the goal in this graph. The source is taken as the point closest to the current position of the vehicle. The goal is taken to be the farthermost point in the road segment. Dijkstras algorithm is used to get the shortest path. Unlike the use of dijkstras algorithm, where the weights of the edges were simply the physical distance between the two

intersections, the aim of this step is not to minimize the distance. Hence the cost function of the dijkstras algorithm needs to be changed. In this algorithm we aim to maximize the width of the path from which the vehicle travels. Width of a vertex v is defined by the width of the sweeping line at the accessible region, whose center was v. Maximizing width means maximizing of the separation of the vehicle from all the obstacles and road ends. This ensures that the vehicle does reach the goal from the source (if such a path exists) and the traversal is by maximum width or separation. Width of a path is taken to be the width of the smallest segment or least wide vertex through which the vehicle traverses.

Figure 2. The generation of the roadmap graph

However width maximization or separation maximization would only be a good technique till some level. A way fairly wide enough may be preferable over a way very wide, if the vehicle may comfortably enter the fairly wide way. In other terms there needs to be a threshold over which all widths may be considered wide enough. We set the threshold to wmax. Further path length is added as the second objective. The resultant algorithm for roadmap generation may hence be given by Road-Selection(G, source, goal) c[v] for all v [v] NULL for all v d[source] 0 wi[source]=width(source) c[souce]= d[source]+wi[source] Q V While Q u vertex with minimum d value for all v neighbor of u d d[u]+w(u,v) wi min(wi[u],width(v),wmax) if d+wi<c[v] d[v]=d wi[v]=wi c[v]= d[v]+wi[v] [v] u Q Q {u} v Goal Path While v NILL Path {v} U Path v (v)

The roadmap selection algorithm outputs the segments, formed by the obstacles that the vehicle must select for its traversal by the maximum width (and possibly small path lengths) possible. IV. TRAJECTORY GENERATION

The roadmap produced by the planning of hierarchy discussed in section 3 does ensure that the vehicle would travel by the largest width possible, if it chooses to travel by the generated hierarchy. This would not only ensure a collisionfree path, but would also ensure that the separation from the obstacles and road ends is maximized. The roadmap however has very sharp turns at the vertices and straight lines throughout else. Hence in order for the vehicle to be able to traverse using this roadmap, it is essential to smoothen the roadmap to as large degree as possible. The smoothening must however ensure that the process does not end up in the vehicle colliding with any other obstacle or going outside the road boundary. The optimization is carried out by a fast and localized optimization algorithm. Splines [3] are used for the smoothing of curve. The optimization task involves iteratively constructing a spline with the vertices of the roadmap as the control points. In this spline the point where the speed of the vehicle s(t) is less than the maximum allowable speed smax is found. The less speed needs to be improvised. This is done by moving the closest control point. Let the vertices of the roadmap be V. The speed of the vehicle s(t) at any point in the spline S(t) may be given by s(t) = min((cu(t)/),smax) cu(t) = || S(t + ) + S(t - ) 2 S(t) || (1)

smooth curve would not be smoothened further if the vehicle is capable of exhibiting desired maximum speed throughout its journey. It may be noted that optimization reduces the separation, against the hypothesis, while increasing possible speed. The purpose of the optimization is not to reduce the length, but just to ensure vehicle may not be required to slow down in its journey. V. RESULTS

The algorithm is tested over a simulation tool developed in MATLAB. The maps are given to the algorithm as images, which the algorithm reads and interprets. The algorithm outputs the planned trajectory of the vehicle which is displayed separately. The first task was to test was the road selection algorithm. Here we made a synthetic map which the algorithm worked over. The resultant path given by the road selection module is given in figure 3.

Figure 3. Output of the road selection algorihtm

(2)

Here cu(t) is the curvature at point t. is the frictional constant. S(t) is the point in the spline at time t. Equation (2) computes the curvature using a discrete model of the spline rather than a continuous model. is a small constant. During the optimization at any iteration let the point S(t) have speed less than the desired speed. Let the closest control point to this be vi. In order to further smoothen the curve to increase the speed at point S(t) the control point vi is moved as per equation (3) vi = (vi+1+vi-1)/2 (3)

The output hence generated is worked over by the lower levels of planning algorithm. We hence take a segment of the road found in the above roadmap and use it for further experimentation. A number of obstacles are placed on the road segment to judge the ability of the vehicle to come out with a trajectory that is smooth and safe. The first test considers a simple road with no obstacle. In this case the algorithm generated trajectory such that the vehicle needs to travel in the center of the road. This case is important as in most scenarios the road is obstacle free that the vehicle needs to travel upon. Resulting trajectory is shown in figure 4(a). The scenario is further complicated by insertion of a single obstacle in the path of the vehicle. This test case judges the capability of the vehicle to avoid simple obstacles on its way. As expected the vehicle is able to avoid the obstacle, leaving large margins from it. The resulting trajectory is given in figure 4(b). The next couple of cases test the capability of the algorithm to generate paths in a complex network of obstacles. The algorithm must be able to find a path of maximum separation from the obstacles and to further construct a smooth trajectory around the obstacle. The trajectories generated by the algorithm are given in figure 4(c) and figure 4(d).

The changed value of vi is admitted only if the change produces a feasible curve. Further it is necessary that vi must not already be in the mean position and movement of vi must not have earlier produced an infeasible curve, without any feasible curve being generated thereafter. In this manner every iteration of the optimization algorithm produces a smoother curve, over which the vehicle is capable of exhibiting higher speeds. It is important to note that the optimization only cares whether the instantaneous speed of the vehicle is higher than the desirable speed or not. Hence a fairly

VI.

CONCLUSIONS

(a)

Planning for semi-autonomous vehicle requires a very fast decision which necessitates for planners being fast, at the same time complete in planning, to ensure the vehicle does not get trapped in any scenario from which it is difficult to come out. Effective planning techniques are hence necessary. In this paper we proposed the use of dijkstras algorithm for the selection of roads or the route of the vehicle. The planning for every segment of road hence selected was done by the hypothesis which attempted to maximize the separation of the vehicle from the obstacles. In order to plan we first generated a roadmap using dijkstras algorithm. The roadmap attempted to select wide paths around the obstacles, if any. At the same time the maximization of the width was threshold to an amount to avoid paths being too long for small increases in widths. The resulting roadmap was smoothened and optimized for speed by a fast optimization algorithm that used BSplines for the generation of smooth curves. Any point where the speed of the vehicle was less than the desired amount was modified by movement of the control points. Experimentation was done on a set of maps representing multiple scenarios. In all the cases it was observed that the algorithm was able to generate smooth curves that had high separation from all the obstacles. While the current approach generates smooth graphs in a real time mode, it needs to be extended to the presence of multiple vehicles. Further the assumption of the map being completely known may not be valid in a practical scenario. In the present approach the lower levels of planning may detect blockages, when the vehicle may not be able to move in a road. Abilities to replan and taking effective measures in such a scenario may be embedded into the algorithm. Testing the algorithm for dynamic environments may further expose new issues of the algorithm and the problem on a broader scale. REFERENCES
[1] J. R. Alvarez-Sanchez, F. de la Paz Lopez, J. M. C. Troncoso, D. de Santos Sierra, Reactive navigation in real environments using partial center of area method, Rob. Autonom. Syst. 58(12), 1231-1237 (2010). T. Arai, E. Pagello, L. E. Parker, Editorial: Advances in Multi-Robot Systems, IEEE Trans. Rob. Automat. 18(5), 655-661(2002). R. H. Bartels, J. C. Beatty, B. A. Barsky, An Introduction to Splines for Use in Computer Graphics and Geometric Modelling, Morgan Kaufmann (1987), San Francisco, CA. M. Bennewitz, W. Burgard, S. Thrun, Optimizing schedules for prioritized path planning of multi-robot systems, Proceedings of the 2001 IEEE International Conference on Robotics and Automation, Seoul, Korea (2001), pp 271 276. T. H. Cormen, C. E. Leiserson, R. L. Rivest, C. Stein, Section 24.3: Dijkstras Algorithm, Introduction to Algorithms, Second Edition, MIT Press (2001b), MA, USA, pp 595-601. C. Crane, D. Armstrong, A. Arroyo, A. Baker, D. Dankel, G. Garcia, N. Johnson, J. Lee, S. Ridgeway, E. Schwartz, E. Thorn, S. Velat, J. Yoon, J. Washburn, Team Gator Nation's Autonomous Vehicle Development for the 2007 DARPA Urban Challenge, J. Aerosp. Comput. Inform. Commun. 4(12), 1059-1085 (2007). J. Y. Hwang, J. S. Kim, S. S. Lim, K. H. Park, A Fast Path Planning by Path Graph Optimization, IEEE Trans. Syst. Man Cybern.Part A: Syst. Hum. 33(1) 121-128 (2003)

(b)

(c)

[2] [3]

[4]

[5]

[6]

(d)
Figure 4. Trajectories generated by the algorithm for road segment with various obstacles

[7]

[8]

[9]

[10]

[11]

[12]

[13]

R. Kala, A. Shukla, R. Tiwari, Fusion of probabilistic A* algorithm and fuzzy inference system for robotic path planning, Artif. Intell. Review, 33(4), 275-306 (2010). R. Kala, A. Shukla, R. Tiwari, Robotic Path Planning using Evolutionary Momentum based Exploration, J. Experimental and Theoretical Artif. Intell., Accepted, In Press (2011). M. Montemerlo, J. Becker, S. Bhat, H. Dahlkamp, D. Dolgov, S. Ettinger, D. Haehnel, T. Hilden, G. Hoffmann, B. Huhnke, D. Johnston, S. Klumpp, D. Langer, A. Levandowski, J. Levinson, J. Marcil, D. Orenstein, J. Paefgen, I. Penny, A. Petrovskaya, M. Pflueger, G. Stanek, D. Stavens, A. Vogt, S. Thrun, Junior: The Stanford entry in the Urban Challenge, J. Field Robotics, 25(9), 569597(2008). J. Peng, S. Akella, Coordinating Multiple Robots with Kinodynamic Constraints Along Specified Paths, Intl. J. Rob. Res. 24(4), 295-310 (2005). G. Snchez-Ante, J.C. Latombe, Using a PRM Planner to Compare Centralized and Decoupled Planning for Multi-Robot Systems, Proceedings of the IEEE International Conference on Robotics and Automation, Washington, DC (2002), pp 2112 211. J. Snape, J. van den Berg, S. J. Guy, D. Manocha, Independent navigation of multiple mobile robots with hybrid reciprocal velocity obstacles, Proceedings of the IEEE/RSJ International Conference on Intelligent Robotic Systems, St. Louis, MO (2009), pp 5917-5922.

[14] P. Svestka, M. H. Overmars, Coordinated motion planning for multiple car-like robots using probabilistic roadmaps, Proceedings of the 1995 IEEE International Conference on Robotics and Automation, Nagoya, Japan (1995), vol. 2, pp.1631-1636. [15] J. Sewall, J. van den Berg, M. C. Lin, D. Manocha, D, Virtualized Traffic: Reconstructing Traffic Flows from Discrete Spatiotemporal Data, IEEE Trans. Visualization Computer Graphics, 17(1), 26-37 (2011). [16] C. Urmson, C. Baker, J. M. Dolan, P. Rybski, B. Salesky, W. L. Whittaker, D. Ferguson, M. Darms, Autonomous Driving in Traffic: Boss and the Urban Challenge, AI Magazine 30(2) 17-29 (2009). [17] J. van den Berg, J. Sewall, M. Lin, D. Manocha, Virtualized Traffic: Reconstructing Traffic Flows from Discrete Spatio-Temporal Data, Proceedings of the 2009 IEEE Virtual Reality Conference, Lafayette, Louisiana, USA (2009), pp 183-190. [18] D. Wilkie, J. van den Berg, D. Manocha, Generalized Velocity Obstacles, Proceedings of the 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, St. Louis, USA (2009), pp 55735578. [19] J. Xiao, Z. Michalewicz, L. Zhang, K. Trojanowski, Adaptive evolutionary planner/navigator for mobile robots, IEEE Trans. Evol. Comput. 1(1), 18-28 (1997). [20] E. K. Xidias, P. N. Azariadis, Mission design for a group of autonomous guided vehicles, Rob. Autonom. Syst. 59(1), 34-43 (2011).