Route Planning For OSU-ACT Autonomous Vehicle in. DARPA Urban Challenge

Transcription

1 008 IEEE Intelligent Vehicles Symposium Eindhoven University of Technology Eindhoven, The Netherlands, June 4-6, 008 Route lanning For OSU-ACT Autonomous Vehicle in DARA Urban Challenge Lina Fu, Ahmet Yazici, and Ümit Özgüner, Member, IEEE Abstract The 007 Urban Challenge, hosted by the U.S. Defense Advanced Research rojects Agency (DARA), featured autonomous vehicle technology in an urban environment. This paper presents the route planning module for the vehicle developed by the Ohio State University Autonomous City Transport (OSU-ACT) team. Based on given information of the urban road network, this module is able to search for optimal routes to direct the vehicle through a series of checkpoints. It is also capable of dealing with dynamically changing road networks, through real time re-planning with updated road information. Simulations and field tests have validated the performance of the route planner. D I. INTRODUCTION ARA has organized a series of driverless challenges to promote research and technology in autonomous vehicles. The previous two Grand Challenges, in 004 and 005, featured lengthy and rough off-road courses in the desert. The 007 Urban Challenge [] focused on autonomous driving in an urban environment. Efficient course planning, appropriate operation in traffic, as well as obstacles avoidance was expected of the vehicles all through the missions. One of the key challenges for the autonomous vehicles was the optimal route planning for designated missions. For this purpose, both the general road network and specific set of location to be visited in a mission, needs to be known. DARA provided a Route Network Definition File (RNDF) [] defining the network of all available road segments and free zones in the mock urban area. The vehicles were only allowed to drive on the defined roads and were to comply with traffic rules. A set of checkpoints on the network were also designated in terms of specific waypoints on selected lanes. The missions were specified in the Mission Definition File (MDF) [] through sequences of checkpoints that the vehicles were required to traverse through. This year, the OSU-ACT team used a hybrid SUV as the vehicle platform. As in the previous events [-6], Manuscript received Jan 0, 008. Lina Fu is with the Ohio State University, Department of Electrical and Computer Engineering, Columbus, OH 430 USA; fu.83@osu.edu Ahmet Yazici is with Eskisehir Osmangazi University, Department of Computer Engineering, Eskisehir, 6480, Turkey; ayazici@ogu.edu.tr Ümit Özgüner is with the Ohio State University, Department of Electrical and Computer Engineering, Columbus, OH 430 USA; umit@ece.osu.edu drive-by-wire capability was developed to control steering, throttle, brakes, and transmission. The vehicle was mounted with GS, digital cameras and image processing systems, lidars, radars, and other sensors. An emergency stop mechanism was also integrated for safety concerns. The vehicle adopted a hybrid control architecture for real-time intelligent control. The control system consists of Route lanning (R), High Level Control (HLC), Situation Awareness (SA) and Low Level Control (LLC) layers, and a proper communication network is established among them. The route planning module searches for optimal routes for the specified missions. Based on these routes, SA analyzes the driving environment in the vicinity of the vehicle and HLC makes control decisions according to specific situations determined by SA. LLC implements the control scheme from HLC to drive the vehicle. The rest of this paper is organized as follows. Section states the problem and introduces the environment modeling for route planning in Urban Challenge. Section 3 elaborates the route planning scheme, followed by simulation and field test results in section 4. Section 5 gives the conclusion. II. ROBLEM DESCRITION AND MODELING A map of a city is essentially a network of many intersecting roads. The RNDF serves as a digital description of the urban area in the real world. In particular, it specifies the network of road segments and free-travel zones accessible to the autonomous vehicles. In the file, available roads and their intersections are defined in a systematic fashion. The route network consists of a number of segments, each having one or more lanes. A lane is uniquely defined by a series of waypoints, which are given by latitude and longitude coordinates. Intersections are defined by pairs of exit and entry points. Vehicles can exit from the exit point of one road segment and enter another road segment through the corresponding entry point. The RNDF also provides information such as locations of stop signs and checkpoints, lane widths and lane markers. In addition to road segments, the RNDF specifies free-travel zones to represent parking lots and obstacle fields. The zone area is restricted within a polygonal boundary defined by perimeter points. A zone may include one or more parking spots, each specified by a pair of waypoints. Fig. illustrates a part of the sample RNDF map /08/$ IEEE. 78

2 from DARA []. allowed to turn left onto the vertical road. If we modeled the T-junction with only one node, the no-left-turn policy could not be correctly conveyed by the graph. For free-driving zones, edges are assigned from entries of a zone to all nodes inside, and from both types of nodes to zone exits. Fig. 4 exemplifies a graph model associated with a zone. Our graph definition accurately describes the intricate urban road environment with consistency and flexibility, and therefore establishes a friendly model for the route planner to work with. Also, it enables the route planner to achieve a more accurate estimation of traveling time costs for the edges. What is more, the generated optimal routes can be represented in terms of series of waypoints, which remarkably reduces the work on the interpretation and execution of the routes in the control module. Fig.. A section of sample RNDF map from DARA. This environment setup can be interpreted as a directed graph G(N, E), where N corresponds to the set of nodes (vertices), and E is the set of edges. After appropriate translation from the route network information to a graph structure, the existing shortest path algorithms such as A* algorithm can be utilized to solve our route planning problem. An intuitive graph model would represent all intersections by nodes and all streets by edges. However, the actual construction of the graph turns out to be much more complicated. One potential problem is that some intersections may have restrictions on turns, for example, prohibiting left turns. Also, checkpoints are associated with lanes, which means vehicles have to pass through the checkpoints on specific lanes. And when it comes to zones, the graph should properly indicate the accessibility of the checkpoints inside the zones. In constructing our graph model, we consider exit and entry points and checkpoints as nodes. The edges between the nodes of the graph are defined with respect to available lanes, exit/entry pairs, lawful lane changes and U-turns. Following this logic, the graph representation of an intersection contains multiple nodes corresponding to the exit and entry points. The edges between them indicate admissible transitions from one road segment to another with proper left or right turns. This modeling scheme embraces all kinds of possible situations at intersections. Connections between different road segments can be defined appropriately by the edges in a consistent and straightforward way. Fig. and Fig. 3 illustrate a T-junction and a four-way intersection and their graph models. In Fig., only a right-turn exit/entry pair is defined, that is, vehicles on the horizontal road are not Fig.. Graph model of a T-junction. Only one exit/entry pair (,) is defined for right turn. Left turn onto the left lane on the vertical road is not allowed. Fig.3. Graph model of a four-way intersection. Fig. 4. Graph model of a zone. Edges are established from zone entries to all nodes inside, and from these nodes to zone exits. III. ROUTE LANNING The MDF defines the mission for the vehicles. It specifies a series of checkpoints that the vehicles must visit in order. Speed limits of the segments are also stated in the file. The vehicles need to direct themselves through the given checkpoints until arriving at the final point so as to accomplish the mission. They should proceed along the 78

3 routes quickly and safely during the entire mission. The route planning module is designed to calculate optimal routes for the vehicle for any given mission. The vehicle plans a route only from one checkpoint to the next, with possibly a two-checkpoint horizon. In certain situations, it needs the route planner to generate routes originating from its very position rather than a known node of the graph. In this case, the route planner has to locate the vehicle coordinates on the graph first, that is, to find the nearest approachable edge for the vehicle. Then it can plan a route from that edge. If the route planner is notified of a road blockage, it needs to update the graph structure before performing further searching tasks. Fig. 5 illustrates the flow chart of the route planner. The planner goes through a locating process if necessary, and updates the graph in cases of road blockage, then performs searching algorithm to provide the vehicle with the optimal route. kinematical capability, and then produce a route from that edge to continue with the mission. To find the nearest reachable edge for the vehicle, we need to search the graph with multiple criteria. We want to minimize the distance from the vehicle position to the edge, as well as the deviation of the vehicle heading from the direction of the edge. Consider an arbitrary edge E on the directed graph. As shown in Fig. 6, point represents the position of the vehicle, and vector N indicates its heading. θ is the angle between E and N. The angle between E and j is denoted by θ. We confine our searching scope to edges with θ < 90 and θ < 90. To determine whether edge E is a good match for our vehicle, we need to consider both how far away is from E and how well the vehicle orientation is aligned with the edge. N θ3 θ θ E i j k Fig. 6. Edges E and E on the graph and vehicle position with heading N. E Fig. 5. Flow chart of the route planning module. A. Locate the vehicle There are situations when the vehicle is not at a node of the graph, and needs a plan from its current coordinates to the next checkpoint. For example, due to road blockage or other possibilities, the vehicle may be unable to follow the original route at some point, and need to resume its mission from there. With the GS-INS positioning system, the latitude and longitude coordinates and the vehicle orientation are available to the route planner. The planner should direct the vehicle to the nearest approachable edge with respect to its The distance from point to edge d(, E ) = min{ Q, Q E } E is defined as We define a distance threshold D such that all edges E with de (, ) < D are considered nearby edges. An optimization function is then designed as: D d(, E ) f(, E ) = + cosθ () D This objective function leads to a weighted combination of distance and alignment optimization. The two parts in the function are convex with respect to de (, ) and θ respectively. f ( E, ) takes value between [0,], and achieves its maximum when de (, ) =0 and θ =0. We also need to consider the minimum turning radius of the vehicle, determined by [9] L ρmin = tanφmax where L is the distance between the front and rear axles of the vehicle, and φ max is the maximum steering angle. If the vehicle stands too close to the end node of an edge, it can hardly approach that node without backing up. However, reversing on the road is not allowed by rule. We can 783

4 determine whether the node is reachable for the vehicle by the following proposition: roposition Assume a vehicle is standing at point with an orientation N, and there is a nearby edge E. The vehicle has a minimum turning radius of ρ min. Fix point O such that O is normal to N, and O = ρmin, also fix point O such that jo is normal to E, and jo = ρmin (Fig. 7). If the angle between N and edge E is acute, and OO ρmin, vehicle can arrive at node j with proper non-reversal maneuver. i E N O R Q O j Fig. 7. Construction of a feasible trajectory from to node j. roof of roposition is as follows. Consider two circles centered at O and O respectively, both with radius ρ min. The assumption OO ρ min ensures the two circles are either separate or touching outside. It follows that they have one or two common inside tangent lines. Consider the tangent line closer to in the direction of N. Denote the tangent points as R and Q respectively (R and Q can be the same point). Then the curve comprising the arc R, line segment RQ and arc Qj is an admissible trajectory for the vehicle to arrive at node j, with its orientation aligned with the edge. We use roposition to decide whether edge E of the searching result is approachable. If not, we check the successors E of that edge, and choose the one with maximum f ( E, ) as the final result. In short, the searching algorithm works as follows: Step : in Graph G(N,E), find E = arg min d(, E ) E E, θ < 90, θ < 90 Step : In the set E of edges found in step, calculate E = arg max d(, E ) E E Step 3: Check whether the edge E is reachable by the vehicle. If not, choose an edge E 3 with the largest f ( E, ) from its successors. The edge found is the best match for the vehicle. B. Update the graph During the event, DARA may place obstacles to block one or more road segments, so as to simulate the situation where severe traffic jams or other road problems prevent vehicles from getting through. It is also possible that vehicles from other teams break down on the way, blocking the road. The autonomous vehicle is expected to react to such scenarios and re-plan its route. When the route planner receives the message of a road blockage, it needs to update the nodes and edges in the graph. First of all, the cost of the related edges are increased to a pre-defined high value, C blk, which is an estimate of the cost of sticking to the original plan, waiting there for the road to clear. This high cost leaves the original plan as the last yet still possible option. After that, new nodes and edges might need to be added to the graph to allow U-turns on both sides of the blocking barrier. In some situations, the next checkpoint might be placed on the un-reachable part of the road, beyond the barrier. As an alternative to waiting in front of the barrier for a chance to resume the original plan, the vehicle can navigate around into the other part of the road and get to the checkpoint by a U-turn maneuver, as shown in Fig. 8. It is important to note that this alternative plan only applies to static barrier, and should not be admitted for situations like traffic jams. To incorporate a U-turn edge into the defined graph on the checkpoint side of the barrier, the planner finds the lane of the checkpoint, and looks for a lane next to it with opposite direction. On the latter lane, the planner searches for a waypoint locating closest to the checkpoint, defines it as a node if it is not, and establishes an edge between this node and the checkpoint corresponding to the U-turn. Fig. 8. Definition of new edges for U-turns at the blockage. The blocked road is assumed to be cleared after a certain length of time, T blk, and the graph updates itself for later planning. The planner keeps records of all blocked segments with time stamps, and releases a record and undoes the corresponding modifications after T blk seconds have passed since the establishment of record. C. Search for the Optimal Route between Two Nodes As stated in section, the RNDF of the urban setting is converted to a directed graph. Therefore the route planning problem becomes the single-source and single-destination shortest path searching problem in graph theory. Our route planning module supports both shortest distance and shortest time route searching. Accordingly, two schemes of cost definition for the edges are determined, based on traveling distance and time respectively. Distance-based cost is the physical distance across the two ends of the edges. On the other hand, time-based cost is an estimate of time for the 784

5 vehicle to drive through the edges. Because the vehicle does not necessarily maintain a constant speed along the way, the minimum-distance route and the minimum-time route can be different. For a time-based cost system, accuracy of estimates in driving time is crucial. It is not possible to derive exact driving time for each edge due to uncertainties about the road condition and the traffic. However, we can still acquire reasonable estimates by taking into account the expect speeds of the vehicle over the edges, as well as stop signs and intersections, possible U-turn and obstacle avoidance maneuvers associated with them. Vehicle speeds setting strategy is predetermined in the HLC, while information about stop signs, intersections and zones is accessible in the RNDF. We calculate the time cost of an edge based on the vehicle speed and the expected delay with stop signs or U-turn maneuvers associated with the edge: LE ( ) te ( ) = + td, ve ( ) where LE ( ) is the physical length of E, and ve ( ) is the expected average driving speed the vehicle uses for the edge. According to HLC, the vehicle adopts different driving speeds depending on the speed limits and whether the edges are normal road segments, intersections or free zones. The term t d is the expected time delay due to stop signs or U-turn maneuvers. It can be constant for simplicity, or a function of the vehicle speed to achieve higher accuracy. Fig. 9 illustrates a minimum-time route on an example map. In order that the vehicle starting from node S arrives at the destination node T within shortest time, the module chooses the highlighted route which runs through nodes A, B and C, over the other candidate route with nodes D, E, F, G, H, J and C, which is shorter in distance but involves two more intersections. Fig. 9. An example of minimum-time route A* searching [8] is known as an efficient and effective algorithm for shortest path searching problem with single source and single destination. The key element of A* is the heuristic estimate h(n) for estimating the so called cost-to-go, which is the cost from node n to the destination. To ensure that the optimal solution always be found, h(n) has to be admissible and consistent [7]. When aiming at a minimum-distance route, our h(n) is the straight-line distance from node n to the destination. This distance heuristic h(n) fulfills the requirement [7]. For optimality in time, we define our h(n) as the straight-line distance from node n to the destination divided by the upper bound of maximum speed limits over the network. As a scaling of the distance heuristic, it preserves the properties of admissibility and consistency. The implementation of A* algorithm aiming a minimal cost route from a start node s to an end node t follows the logic below. The module maintains two sets of nodes, an open set, and a closed set Q. To estimate the lowest cost of routes passing through node n, function f(n)=g(n)+h(n) is defined for all nodes n in the open set. Here g(n) is the lowest cost of the routes from s to n, with the routes only consisting of nodes in set Q, and h(n) is estimate of the cost-to-to. Initialize set to contain only the start node s, and Q to be empty. At each step, move the node n with the smallest f(n) value from to Q. Add the successors of n to set if they are not already there, and update f(n) for all these successors in. Repeat the operation until t is included in Q to obtain the optimal route. If becomes empty before that, it means no route from s to t exists. The dimension of the graph model grows fast with the scale of the road network in terms of numbers of roads and intersections. Therefore it is very important that the route planner is economical in computational resource. roper choice of data structure in the module minimizes the use of memory, and appropriate implementation of A* algorithm ensures the computation speed keeps up to the update frequency of the vehicle controller. When the optimal route is found, the route planner compiles the plan and sends it to HLC and SA though the interfaces between the modules. Static information of the road network from RNDF is attached with the edges of the route. The information includes waypoints, lane widths, lane markers, speed limits, stop signs, as well as exit and entry points. The module also discerns the so-called Meta-States along the route from the graph model and provides the information in the output. The Meta-States (lane, intersection, zone, U-turn, etc.), are the top layer states for the complex multi-level finite state machine implemented in HLC (Further information can be found in [0]). They define the maneuver tasks and therefore determine the corresponding vehicle control strategies in HLC. With the information, the vehicle controller can execute the plan properly with respect to the environment. IV. SIMULATION AND FIELD TEST RESULTS A number of RNDF files with different route network scenarios have been prepared to test the route planning module. The generated optimal routes in all the test cases were exactly as we expected. Field tests with the OSU-ACT 785

6 vehicle were consistent with simulations results. Also, the module demonstrated its capability of solving planning problems in a dynamically varying route network with successful real time re-planning. During the 007 Urban Challenge National Qualification Event, the ACT vehicle exhibited excellent route planning skills for all given missions. Fig. 0 shows an example on the map for the DARA Site Visit on June st, 007. The road network consists of a traffic loop and two extended stubs, with U-turns permitted at the ends of the stubs. The vehicle at point was heading for the destination checkpoint Q when it found the blocking barrier in its way at point R. An alternative route to the destination was calculated, which happened to be much shorter thanks to the U-turn allowed at the blockage. Both the original route and the new plan are shown in the figure x 04 Route lanning: Original lan vehicle checkpoint x x 04 Route lanning: New lan Due to Road Blockage Q Q Blockage vehicle checkpoint x 0 4 R connections and driving restrictions. When mapping the vehicle coordinates onto the graph, we take into account both the vehicle orientation and the nonholonomic constrains on its motion to ensure the generated plan is executable. The proposed search algorithm is able to find the optimal routes for defined missions in real-time with given RNDF, and to adjust the plans when road blockages are encountered. Numerous simulation and field tests have validated the design of the route planner module. VI. ACKNOWLEDGEMENT We would like to thank all supporters of the OSU-ACT Team, especially the Ohio State University, College of Engineering. And we would also like to thank all team members for their help and contribution, especially Mr. John Martin who (apart from all the other work he was involved in) developed the software parsing the RNDF to give a graph. REFERENCES [] Defense Advanced Research rojects Agency, Urban Challenge, [online], Available [] H. Yu, Q. Chen and U. Ozguner, Control system architecture for TerraMax-the off-road intelligent navigator, roc. of the 5th IFAC symposium on intelligent autonomous vehicles, 004 [3] U. Ozguner, K. Redmill, and A. Broggi, Team TerraMax and the DARA Grand Challenge: a general overview, roc. IEEE Intelligent Vehicle Symposium, arma Italy, June 004, pp [4] Q. Chen, U. Ozguner, and K. Redmill, Ohio State University at the 004 DARA Grand Challenge--developing a completely autonomous vehicle, IEEE Intell. Syst., vol. 9, no. 5, pp. 8-4, Sep [5] U. Ozguner, C. Stiller, and K. Redmill, Systems for safety and autonomous behavior in cars: The DARA Grand Challenge experience, roceedings of the IEEE, vol. 95, no., pp , Feb [6] Q. Chen and U. Ozguner, Intelligent off-road navigation algorithms and strategies of team desert buckeyes in the DARA Grand Challenge 005, J. Field Robotics, vol. 3, no. 9, pp , Sep [7] Russell, S. J., and Norvig,., Artificial Intelligence: A Modern Approach, NJ, USA, rentice-hall, 995, pp [8] Hart E and Nilson N J. A formal basis of the heuristic determination of minimum cost paths, IEEE Transactions on Syst. Sci. Cybernetics., Vol. 4, pp.00-07, July 968. [9] L.E.Dubins, On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents, Amer. J. Math. Vol. 79, pp , July 957. [0] Kurt, A. and Ozguner, U., Hybrid state system development for autonomous vehicle control in urban scenarios, roceedings of the IFAC World Congress, July 008, Seoul, Korea. Fig. 0. Example routes on the Site Visit map. The dashed lines imply the center lines of the available lanes, with waypoints marked by dots. The vehicle was stopped at point, heading north, and waited for a plan to the destination checkpoint. The solid line in upper figure gives the original route, and the lower figure shows the new plan with a blocking barrier at point R in front of the vehicle. V. CONCLUSION This paper has presented the route planning module for the OSU-ACT autonomous vehicle for the 007 Urban Challenge. The module models the urban road network as a graph structure, which accurately describes the road 786