UFDC Home  myUFDC Home  Help 



Full Text  
PATH PLANNING FOR NONHOLONOMIC VEHICLES AND ITS APPLICATION TO RADIATION ENVIRONMENTS By ARFATH PASHA A THESIS PRESENTED TO THE GRADUATE SCHOOL OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF MASTER OF SCIENCE UNIVERSITY OF FLORIDA 2003 Copyright 2003 by Arfath Pasha I dedicate this thesis to my mother. The quality education that I have received is a result of her perseverance. ACKNOWLEDGMENTS To the members of my committee, Dr. Carl Crane, Dr. Ashok Kumar and Professor Tulenko, I would like to extend my thanks. To Dr. Crane for mentoring me through five years and two degrees, for his confidence in allowing me to undertake some of the most fascinating projects at CIMAR, I owe my grateful thanks. Dr. Dalton's knowledge on nuclear physics has been crucial to my project. I thank Donald MacArthur and Erica Zawodny for venturing to put to test the path planner in a real world situation. I thank Marinela Capanu my companion who has offered her unwavering support and long brainstorming sessions despite her hectic schedule. Finally, I thank Carol Chesney for her help during the final phase of the thesis. TABLE OF CONTENTS Page A C K N O W L E D G M E N T S ................................................................................................. iv L IST O F F IG U R E S .... ...... ................................................ .. .. ..... .............. vii A B STR A C T ................................................. ..................................... .. x CHAPTER 1 INTRODUCTION AND BACKGROUND ..................................... ............... P problem Statem ent ........................... ................. .................... K inem atic Constraints of CarLike R obots ........................................ .....................2 Path Planning A lgorithm s.................................. ...............................................3 Offline Path Planning for CarLike V vehicles ........................................ ....................4 Need for a Path Planner in Radiation Environments.............. ........... ...............6 Effects of Radiation on M obile Robots .................................... .......... .................. 7 2 LITERA TU RE SU RVEY ................................................... .............................. 9 Theoretical W ork ................................. ................................ .............. N onD eterm inistic A approaches .............................................................. ... .......... .11 D eterm inistic A approaches ................................................ .............................. 13 3 RESEARCH OBJECTIVES ......................................................... .............. 17 Formal Problem Statement: Path Planner...................................................17 Formal Problem Statement: Path Planner with a Radiation Constraint....................21 4 NONHOLONOMIC PATH PLANNING ALGORITHM ........................................22 P re P ro c e ss ........................................................................................................... 2 2 In p u t V alid atio n ............................................................................................. 2 2 PreComputation .................................................... ...................... 23 Obstacle Expansion ............................................................................... ... 24 Expansion of Convex Obstacle Vertices .................................. ...............29 Expansion of Concave Obstacle Vertices..........................................30 Main Result: Expansion Method Guarantees Admissible Trajectories .............32 V isib ility G rap h ................................................................4 0 S h o rte st P ath ............................................................................................................... 4 2 5 PATH PLANNING WITH A RADIATION CONSTRAINT............... ................44 R a d iatio n B a sic s ................................................................................... ............... 4 4 A lgorithm D description ........................................................................... ........... 46 6 RESULTS AND CONCLUSION....................... ..... ............................ 54 APPENDIX A A T TE N U A T IO N ............................................................................... ... ............62 B D A TA STR U C TU RE ........................................................................... ..............65 L IST O F R EFE R E N C E S ............................................................................ ...............89 B IO G R A PH IC A L SK E TCH ...................................................................... ..................92 LIST OF FIGURES Figure p 11 Geometry of a carlike vehicle ........................................... .....................2 12 Configuration space for a disc robot. .............................................. ............... 5 21 Dubin's Shortest paths for a particle with a minimum radius of curvature constraint. 10 22 Reeds and Shepp's Shortest paths for a car moving both forward and backward...10 23 Output of a probabilistic path planner developed by Laumond et al. [10]. (a) An initial holonomic path (bd) Optimization of the nonholonomic path ....................12 24 An output of a randomized algorithm developed by Bessiere et al. [13].................13 25 V isibility graph of a m ap ....................................................................... 14 26 Centerline paths developed by Suh and Shin [22] ............................................16 31 Minimum angle constraint on concave vertices of polygons..............................18 32 Constraint on edge lengths for the vehicle to brace the obstacle ...........................19 41 Flowchart of the nonholonomic path planner. ............. ......................................... 23 42 Graphic representation of program flow ........................................................24 43 Geometry of a pseudo obstacle placed around the vehicle's start configuration. ....25 44 A clothoid (C ornu spiral). ............................................................. .....................26 45 Paths smoothed with clothoid curves [24]. ................................... ............... 26 46 Polynomial curve suggested by the JAUS Reference Architecture [25] for the generation of a traj ectory......... ................. ................. ................. ............... 27 47 Effect of the weighting w factor on the trajectory............... .................... 28 48 A trajectory made up of two curve segments that share a common tangent............28 49 Geometry of expansion of a convex obstacle vertex..............................................30 410 Boundary conditions for the expansion of convex obstacle vertices .....................30 411 Geometry of expansion of a concave obstacle vertex...................... ...............31 412 Boundary conditions for the expansion of concave obstacle vertices...................31 413 The w orst case turn. ........................... ................ ......................... 33 414 Plot of curvature K versus parameter u for a symmetric curve with w = 0.707 .......34 415 Violation of the assumption on minimum path segment length lo .........................35 416 A problem instance where the assumption on minimum path segment length lo is violated. ............................................................................. 36 417 The worst case turn for the special case. ........................................ ............... 37 418 Plot of curvature K versus parameter u for the special case with w = 0.875............38 4.19 A problem instance where two or more consecutive path segments have lengths le ss th an lo ............................................................................ 3 9 420 The worst case scenario when the path is smoothed.............................................39 421 Radial sweep method used to find the visibility of vertices...............................41 422 Determining the visibility of vertices................ ............................. ............... 43 51 A point source of radiation ........................................................................ 46 52 Optimal paths in a map with no boundary. ................................... ............... 47 53 Growing pseudo obstacles around radiation sources to find an optimal path.........49 54 Radiation sources placed parallel with respect to a path between the start and goal p o in ts. ............................................................................ 5 0 55 A case with a constricting pseudo obstacle.............. ............................................51 56 Flow chart of the path planner with a radiation constraint.................. .............52 61 Shortest path in a map with nonconvex obstacles and a boundary .........................55 62 An admissible trajectory for the path generated in Figure 61 .............................56 63 A special case when the length of a path segment is less than lo ...............................56 64 An admissible trajectory for the path generated in Figure 63..............................57 65 A trivial case of a straight line path.................................... ........................ 57 66 A case where concave boundary vertices are used to find a path ...........................58 67 An admissible trajectory for the path generated in Figure 66.............................58 68 A radiation environment with no boundary. .................................. ............... 59 69 A bounded radiation environm ent ...................................................... ............... 59 610 Another bounded radiation environment...... ...................... ...........60 611 A case where a zero dose path does not exist.............. .... .................60 612 A case with a constricting pseudo obstacle................................... .................61 613 Path planner used for online path planning. ....................................................... 61 A1 Attenuation from a plane shield in front of a point source of radiation ..................62 A2 Radial sweep line method to compute attenuation................................................64 B l C ode layout. ....................................................... ................. 65 Abstract of Thesis Presented to the Graduate School of the University of Florida in Partial Fulfillment of the Requirements for the Degree of Master of Science PATH PLANNING FOR NONHOLONOMIC VEHICLES AND ITS APPLICATION TO RADIATION ENVIRONMENTS By Arfath Pasha August 2003 Chair: Carl D. Crane III Major Department: Mechanical and Aerospace Engineering Mobile robots are often used in hazardous work places such as nuclear power plants. Although the use of robots in such environments minimizes the danger of radiation exposure to humans, the robots themselves are susceptible to damage from radiation. In order to minimize the amount of radiation they receive, an efficient path planning algorithm was developed for autonomous mobile robots with nonholonomic constraints and modified to find safe paths in radiation environments. The objective of the algorithm was to find a reasonably close approximation to the safest path from a given start configuration to a goal configuration of a vehicle moving only forward in an environment cluttered with obstacles bounded by simple polygons. The path planning algorithm finds the shortest path in O(n2logn) time (n being the number of vertices in the discretized map) using a radial sweep line method to compute the visibility graph and Dijkstra's algorithm to find the shortest path. CHAPTER 1 INTRODUCTION AND BACKGROUND Motion planning is one of the most important components of an autonomous mobile vehicle. It deals with the search and execution of collision free paths by vehicles performing specific tasks. Motion planning is often broken down into two stagespath planning andpath tracking. The path planning stage involves the search for a collision free path, taking into consideration the geometry of the vehicle and it surroundings, the vehicle's kinematic constraints and any other external constraints that may affect the planning of a path. The path tracking stage involves the actual navigation of a planned path, taking into consideration the kinematic and dynamic constraints of the vehicle. This research presents a path planning algorithm for carlike vehicles. Problem Statement The objective of this effort is two fold: 1. To develop an efficient offline path planning algorithm that is capable of finding optimal collision free paths from a start point to a goal, for a carlike vehicle moving through an environment containing obstacles bounded by simple polygons. 2. To extend the path planning algorithm in order to find safe paths by imposing a radiation constraint on a mobile robot operating in a radiation environment. The first objective was intended to be an improvement of the work done by Arturo Rankin [1]. The second objective was intended to facilitate the optimal utilization of robotic vehicles in places such as nuclear power plants where prolonged exposure to radiation can cause substantial damage to robotic equipment. Kinematic Constraints of CarLike Robots The motion characteristics of a robot play an important role in planning its path. Robots that move in a plane generally have three degrees of freedomtranslation along the two axes in the plane and rotation about the axis perpendicular to the plane. Certain robots that are carlike cannot move freely in all three degrees of motion due to their steering constraints. The geometry of such robots is presented in Figure 11 and their equations of motion are given below. \ V Figure 11. Geometry of a carlike vehicle dx = cos(O).v(u) (1.1) du dy = sin(0).v(u) (1.2) du dO =c (u) (1.3) du In the Figure, V is a unit vector along the direction of motion of a vehicle moving with linear velocity v and angular velocity co. 0 is the angle that V makes with the positive xaxis. The position (x,y) of the vehicle is referenced from the midpoint of the vehicle's rear axle. As seen from the equations, a carlike robot has less number of controls (linear and angular velocities) than the number of configuration variables (x,y, 0), making the equations nonintegrable. Vehicles with such kinematic constraints are called nonholonomic vehicles. Although nonholonomic vehicles are controllable, their path planning is a difficult task because the motion at any moment is not free. In addition to this, carlike vehicles have a minimum turning radius. The combination of the nonholonomic constraints and the minimum radius of curvature constraint constitute the kinematic constraints of a carlike robot. Path Planning Algorithms Over the last decade, path planning for mobile robots has been broken down into two main categoriesoffline and online (also called dynamic) path planning. As the names suggest, offline path planning is a global optimization approach while online path planning performs only a local optimization. Offline algorithms require a priori an obstacle map of the robot's environment. The path is precomputed and then given to the robot to execute. The robot uses the path information to navigate itself efficiently through the environment with the help of a path tracking algorithm. A number of approaches have been explored using randomized and deterministic algorithms. While randomized algorithms are used to find solutions to the generalized form of the problem that is extremely complex, the problem is often simplified to create deterministic algorithms. The complexity measure of such algorithms is given as a function of the number of vertices present in the discretized input map. The main objective of online path planning is to avoid obstacles by reacting to data collected from onboard sensors. It may be used when a map of the mobile robot's environment is not known or, if an unexpected obstacle was encountered during the execution of a precomputed path. Since online path planners run in real time and on onboard computers that usually have very limited computing resources, they have to be efficient in terms of both memory utilization and time. This is accomplished by using a lightweight algorithm or heuristic that works on a highly discretized input. Their efficiency is usually measured by the amount of time they take. The quality of their results depends on the amount of lookahead distance of the sensors. Path length is usually used as a complexity measure for online path planning algorithms by setting bounds for the worst case path length as a function of environmental parameters such as the sum of perimeter lengths of obstacles [2]. A natural offshoot from creating a distinction between online and offline algorithms is the development of hybrid algorithms. Hybrid path planning algorithms usually work with sparse or low resolution maps that do not provide information about obstacles such as rocks, trees etc. These algorithms have both an online and an offline component that work in tandem to provide globally optimal paths. Offline Path Planning for CarLike Vehicles The generalized offline path planning problem for carlike robots is referred to as the continuous curvature constrained shortest path problem. The objective of the generalized problem is to find a continuous path that is the shortest among all paths and whose curvature at any point along the path is less than a given upper bound (the inverse of the minimum turning radius). Reif and Wang [3] proved that this problem is NPhard. This means that there is no existing algorithm that can solve this problem to optimality in polynomial time and it is highly unlikely that one even exists; a justification for the approaches based on discretization used in the past to yield polynomial time algorithms. Fortunately, these approaches have been found to work well for all practical applications. Discretization techniques simplify the problem by creating a configuration space from a map of the vehicle's environment. The concept of configuration space was introduced in the late seventies as a consequence of kinematic constraints on moving objects. The configuration of a mobile robot is a set of parameters that define its position and orientation in a plane. The configuration space defines a subset of the free space in a robot's environment that is reachable by a robot with kinematic constraints. Typically, a configuration space is built by reducing the robot down to a point and increasing the size of the obstacles such that they bound regions of the free space that are inaccessible to the point sized robot. The problem of motion planning then reduces to finding a sequence of feasible robot configurations in free space from an initial to a final configuration. Figure 12 shows the configuration space for a disc robot. G increased obstacle size S /the shortest path Figure 12. Configuration space for a disc robot. The planned path is considered to be an image of the final trajectory that is executed by the robot. The path may be discontinuous but the trajectory is always continuous. An admissible trajectory is a solution of the differential system corresponding to the kinematic model of the robot along with some initial and final conditions. Since the configuration space concept is purely geometric and does not allow for timedependant constraints (as required by the robot's kinematic model) the goal of a discretized nonholonomic path planner is to find an image of an admissible trajectorya collision free admissible path. Need for a Path Planner in Radiation Environments Robots are used in nuclear installations to perform jobs in areas where humans are at risk of being over exposed to radiation. Since radiation is detrimental to human health, the amount of access that humans have to portions of an installation is limited by regulation 10CFR20 of the Nuclear Regulatory Commission [4]. This limitation may call for the need to employ more labor than needed. In addition to this, there are as many as 7000 contaminated buildings among Department of Energy nuclear facilities that require decontamination and/or decommissioning. It is estimated that it will take more than 40 years and over 100 billion dollars to clean up these sites [5]. The Department of Energy: Office of Environmental Management, set up a program called the "CP5 Large Scale Demonstration Project" [6] to find innovative decontamination and decommissioning technologies that can help reduce the cost of this billion dollar effort. Some of the solutions that were developed were radiation imaging systems that could map radiation fields and mobile robots that were capable of finding hotspots in a building. Since robots themselves are affected by radiation and the cost of maintaining or replacing them is high, significant cost savings can be achieved by minimizing their exposure to radiation. One way of doing this is by using a path planner to compute routes that minimize the radiation exposure of mobile robots and thereby extending their life. Effects of Radiation on Mobile Robots Gamma rays, beta particles, neutrons and heavy charged particles such as protons and alpha particles are emitted from radioactive materials. Of these, gamma rays, which are photons with very short wave length, pose the greatest threat to electrical and electronic components onboard robots. Gamma radiation is capable of traveling many meters in air and readily penetrates most material, earning itself the name "penetrating radiation." Gamma rays have a very destructive effect on a number of materials used to build robots. Electrical parts such as transformers, motors, thermocouples, relays and circuit boards, which form vital components of a robot may be severely damaged from exposure to gamma radiation. Laurent Houssay [7] has detailed the effects of gamma rays and has referenced tables with threshold values for various materials that are used on robotic systems. The effect of radiation from gamma rays is increased by long exposure (time), close proximity to its source (distance) and the intensity of the source (quantity). Some effects of gamma radiation on commonly used materials are listed in table 11. Table 11. Effects of gamma radiation to commonly used materials. Material Ceramics Plastics Coatings Adhesives Resistors Glass Magnets Threshold level (Gy) 5 x 10 (mica) 5 x 1010 (alumina) 100 (Teflon) 2.1 x 106 (vinyl on aluminium) 8.7 x 106 (styrene on steel) 1 x 106 (neoprenephenolic) 5 x 106 (epoxy) 104 10 (carbon film) 10 109 (metal film) 10x 106 (quartz) 1 x 106 (soft magnets) Effect Dimensional swelling and decrease in density. Cracking, blistering and embrittlement. Cracking, blistering and flaking Decreases number of adhesive bonds. Chemical degradation causing a decrease in resistance. Darkening. Decrease in magnetic strength. CHAPTER 2 LITERATURE SURVEY This section summarizes previous research work done in offline path planning for nonholonomic mobile robots. A significant amount of work has been done in both theoretical analysis of the problem and the development of algorithms for finding effective solutions for planning globally optimal paths. Theoretical Work Research in nonholonomic path planning dates back to the pioneering work done by Dubins [8] in 1957. Dubins considered the problem of a particle moving in a plane with a constant velocity, and with a constraint on its minimum radius of curvature. He proved that the optimal trajectories are made up of arcs of circles C with minimum radius of curvature and segments of straight lines S that are limited to one of the following configurations. { CaCbCe, CaSdCe } where 0 <=a, e < 2PI, PI < b < 2PI, and d >= 0 The subscripts a, b, d and e, specify the length of each elementary piece. The results of this proof laid the foundation for path planning algorithms when only forward motion was considered. Figure 21 shows some examples of these paths. Reeds and Shepp [9] took Dubin's proof a step further and derived another set of finite shortest path configurations for nonholonomic vehicles that move both forward and backward. { CICIC, CCIC, CICC, CCaCaC, CICaCaC, CICpi/2SC, CSCpi/21C, CICpi/2SCpi/21C, CSC } Figure 21. Dubin's Shortest paths for a particle with a minimum radius of curvature constraint. The character "" denotes a cusp, or a point where the vehicle changes its direction of motion from forward to backward and vice versa. The paths between cusps are similar to Dubin's paths. Both Dubin's and Reeds and Shepp paths were developed without the presence of obstacles. Figure 22 shows some examples of Reeds and Shepp's shortest paths. Figure 22. Reeds and Shepp's Shortest paths for a car moving both forward and backward. Using these finite family of curves, it is simple to design an algorithm to find the shortest path between any two configurations. But, the presence of obstacles and the fact that a mobile vehicle needs a continuous path to be able to follow it accurately made such a solution impractical. Obstacles make the search difficult by limiting the number of admissible robot placements. Dubin's configurations guaranteed shortest paths for mobile vehicles with curvature constraints, but the configurations are discontinuous at the points where two elementary components are connected. A vehicle that attempts to follow these paths must come to a complete stop at these points to align its steering with the (tangent in case of an arc) direction of the next segment in order to follow these paths accurately. Such motions are undesirable. Finding shortest paths with continuous curvature came to be known as the generalized offline path planning problem for carlike robots. It is a difficult task even with the absence of obstacles and remained an open problem for a number of years until it was proved to be NPHard by Reif and Wang [3] in 1998. NonDeterministic Approaches A few attempts that used probabilistic methods and randomized algorithms were used to develop algorithms for the generalized path planning problem. Probabilistic path planners (PPP) are guaranteed to solve a problem for which a solution exists within infinite time. That is, as the running time of the algorithm goes to infinity, the probability of solving it converges to one. In some cases, a guaranteed probability value of reaching the optimal solution is given for reasonable (polynomial) running times. Just as in the case of PPP, randomized path planners (RPP) can also be proven to have a guaranteed probability of reaching an optimal solution in polynomial time. The difference in the two methods arise from the technique used to solve the problem. PPP algorithms use iterative procedures that perform neighborhood walks to find an optimal solution, while RPP algorithms use random walks through the solution space to find an optimal solution. PPP algorithms are typically broken down into two or more stages. The first stage checks if the problem is solvable (a decision problem). If the problem is found to be solvable, the second stage finds an approximate solution, which is then optimized by the third stage that uses an iterative search method. A good example of a PPP is given by Laumond et al. [10]. Their work uses a holonomic path planner to find a path of minimal length. The minimum length path is then approximated to a nonholonomic path using Reeds and Shepp's finite set of shortest paths. The approximated path is then optimized iteratively in the final stage of the algorithm. Figure 23 shows the stages of this algorithm. Similar methods have been tried by Lafferriere and Sussman [11] and Jacob [12]. (a) (b) (c) (d) Figure 23. Output of a probabilistic path planner developed by Laumond et al. [10]. (a) An initial holonomic path (bd) Optimization of the nonholonomic path. RPP algorithms are somewhat similar to PPP in the sense that this method also involves two or more search stages. RPPs typically use randomized heuristics such as simulated annealing and genetic algorithms. Bessiere et al. [13] have developed one such technique that has two stagessearch and explore. The search stage finds if it is possible to "simply" reach the goal. The explore stage collects information about the environment and optimizes the path found in the search stage by using landmarks placed all over the environment. Exploration of the environment is done with the help of a genetic algorithm. Figure 24 shows an output of their algorithm. Figure 24. An output of a randomized algorithm developed by Bessiere et al. [13]. Deterministic Approaches The bulk of the path planners designed for nonholonomic motion over the last decade make certain assumptions to help simplify the problem down to a more tractable one. This has allowed the use of simple and efficient heuristic type solutions to tackle practical problems at considerably low running times. The most popular of these assumptions is to assume that an admissible trajectory can be obtained from a discontinuous collision free path made up of straightline segments and circular arcs of bounded curvature. A well tried and tested method that was first introduced by Perez and Wesley [14] is the visibility graph search method. This method reduces the obstacle map along with the start and goal points down to a graph structure where the visibility of each node (vertices of obstacles) with respect to other nodes in the graph is computed. The shortest path is built by connecting visible nodes beginning from the start node and ending with the goal node. The straight lines in Figure 25 represent the visibility lines G Figure 25. Visibility graph of a map. from each node to every other visible node in the graph. Graph based algorithms such as A* search [15], Dijkstra's, BellmanFord and FloydWarshall algorithms [16] may be applied to the resulting visibility graph to find the shortest paths. Researchers that have used this technique include Bicchi et al. [17], Rankin [1] and Asano et al. [18]. There has also been a host of other deterministic methods that have been developed for a simplified version of the problem. Among them, Namgung and Duffy [19] proposed a new idea of using linear parametric curves to find collision free paths. Their method built an initial discontinuous collision free path that was later smoothed at the discontinuities to yield a continuous path. The continuous path was guaranteed to lie in free space after the smoothing operation was performed. Another approach called cell decomposition, was first developed by Brooks and Perez [20]. It consists of decomposing the configuration space into rectangular cells that lie in free space, and then planning a path as a sequence of empty cells (cells lying in free space). Many improvements have been made using this approach. One such improvement is the hierarchical cell decomposition approach presented by Zhu and Latombe [21]. Suh and Shin [22] explored yet another method that described free space as channels between obstacles that guaranteed a collision free path. Variational calculus and dynamic programming was used to develop these channels and a path along the center line of these channels was found. Suh and Shin's work is also the only known paper that plans a path with a secondary constraint. Robot safety is the secondary constraint that is met by finding a path along the centerline of the channels (as far away from the obstacles as possible). The method uses a weighted cost function to measure the "goodness" of a path with respect to distance and safety. Figure 2.6 shows two approaches for building centerline paths. The thick lines are boundaries of obstacles, and the dashed line is the centerline path lying in free space. Figure 26. Centerline paths developed by Suh and Shin [22] There has also been a substantial amount of work in the areas of multirobot path planning, path planning among moving obstacles and trailer path planning. These areas of research are beyond the scope of this thesis and are therefore not discussed here. CHAPTER 3 RESEARCH OBJECTIVES Formal Problem Statement: Path Planner Given: 1. The start and goal configurations (xs,ys, 0) and (xg,yg,Og) of a carlike robot in a plane; 2. Dimensions of the robotlength L, width W, minimum turning radius R; 3. A collection O = {o, 02, ...,n} of non overlapping obstacles described by simple polygons, each having a set of vertices V, {v,, j 1 ..., mi }, i 1, ..., n, and such that the length of each edge is at least lo and the angle subtended at each concave vertex is in the range [7t/2, 7]. (Note that O can be the empty set.) 4. A set of vertices B = {bl, b2, ..., bN} describing a simple polygon that defines a boundary such that the length of each edge is at least lo and the angle subtended at each convex vertex is in the range [7t/2, 7]. (Note that B can also be the empty set.) Find: A set of way points P = P1, ...., Pkstarting with the start position and ending with the goal position, that is an image of an admissible trajectory of minimal distance for a robot moving only forward. The input universe for each of the inputs mentioned above were made as general as possible to allow maximum flexibility. But, some constraints had to be imposed in order to guarantee a collision free admissible path. These constraints arise mainly from the method used to create the configuration space and are practical in nature. The constraint on concave vertex angles of the obstacles and boundary stem from the fact that a carlike robot moving only forward and with a minimum turning radius cannot use a portion of the concave region of an acute angle. This is shown in Figure 31(a) by the light gray region. If such a vertex exists with a concave region large enough to be used by the vehicle, it may be replaced by two concave vertices that satisfy the constraint as shown in Figure 31(b). (a) (b) Figure 31. Minimum angle constraint on concave vertices of polygons. The constraint loon obstacle edge lengths is imposed since a carlike robot cannot brace an obstacle that has edge lengths smaller than 2R, or four times R for edges containing two convex vertices. When the configuration space is built, paths that brace the sides of obstacles are considered as one possible worstcase scenario (Figure 3.2(a)). If these paths are infeasible, then the configuration space cannot guarantee that any shortest path is an image of an admissible trajectory. In Figure 32(b) it is seen that when the obstacle edges are smaller than lo, the car cannot properly brace the sides of the obstacle. If an obstacle is too small to meet this constraint or, has one or more edge lengths less than lo, the obstacle must be grown or reshaped to meet the constraint. If two or more obstacles are overlapping, they may be merged to make one obstacle. The concave vertex angles and edge lengths of the merged obstacle must then obey the constraints stated above. All of these constraints can be met with simple polygon refining algorithms. lo R; Figure 32. Constraint on edge lengths for the vehicle to brace the obstacle. The output of the path planner is intended to be the input to a path tracking algorithm such as the one developed by Jeff Wit [23]. Wit's Vector pursuit path tracking algorithm performs the actual control of the vehicle along the trajectory defined by the way points in the path. The path planning algorithm developed here is also intended to be an improvement over the algorithm developed by Arturo Rankin [1]. Rankin's algorithm found the shortest path using an O(n3) time algorithm (n being the number of vertices in the map) by using a brute force method to compute the visibility graph. The algorithm allowed obstacles that were discretized into nonoverlapping convex polygons. The objective of the current implementation is to improve the computational efficiency of the overall algorithm and allow for nonconvex polygonal representations of obstacles that may overlap after they have been expanded to create the configuration space. By making the algorithm computationally efficient, it may be applicable to online path planning also. There is very little conceptual difference between online and offline path planning. Both seek optimal paths from a start to a goal. Online path planners work on local data while offline path planners work on global data. Theoretically, an online path planner may be used for offline path planning and vice versa. The main difference that does not permit this interchange stems from the practical issues of computational efficiency, scalability and nature of the input. Offline path planners are not fast enough to run in real time while online algorithms are not built to be scalable. Online planners work on range data collected from sensors while offline planners work on discretized map data. By developing an offline algorithm that is computationally efficient enough to run in real time, it may also be applied to online path planning with an added overhead of converting the input into a discretized local map. By generalizing the obstacle input to include nonconvex polygons, more accurate representations of the obstacles may be used. Also, when the configuration space is built, the size of the obstacles is increased in order to reduce the robot to a point in the map. Increasing the size of the obstacles can cause the expanded obstacles to overlap. Such intersections are permitted by the current implementation. The boundary polygon was included into the input in order to be able to define a closed working space for the robot. This can be particularly useful in certain online path planning applications such as path following, and for path planning in radiation environments. When the boundary is specified, the edges of the boundary are shrunk in a manner similar to the expansion of the obstacles. The boundary vertices when specified are used in the construction of the visibility graph and hence the shortest path algorithm. The input set of vertices for the boundary may be an empty set when the boundary is not specified. Formal Problem Statement: Path Planner with a Radiation Constraint In addition to the inputs mentioned in the formal problem statement of the path planner, Given: 5. The maximum speed of the vehicle v,ax,; 6. A set of radiation sources G= {G1, ..., G,}with positions (x,, y,), and dose rates I, i= 1,., r. Find: A set of way points P = {P, ..., Pk starting with the start position and ending with the goal position, that is an image of an admissible trajectory of minimal distance for a robot moving only forward, such that the dose, Ic accumulated by the robot along the path is minimal. The objective of this part of the problem is to incorporate the new radiation constraint by building on the basic path planner. An important issue to consider is that the algorithm must optimize against two unrelated constraintsdistance and radiation absorption or dose. A popular approach to solving such problems is to devise a weighted cost function as done by Suh and Shin [22] that may have an inherent mixed units problem. The approach used here avoids the mixed units problem by minimizing the accumulated dose independent of distance. It is assumed that the radiation sources are point sources and that the accumulated dose is the sum total of dose received from all sources present in the robot's environment. CHAPTER 4 NONHOLONOMIC PATH PLANNING ALGORITHM The algorithm for planning of a collision free admissible path was broken down into four stages as shown in Figure 41 and 4.2. The algorithm is built on an edgecentric data structure that is listed in Appendix B. All points in the map are referenced to a cartesian coordinate system (x,y). The boundary polygon is treated as the inverse of the obstacle polygon. That is, the final trajectory must be contained by the boundary polygon while lying outside the obstacle polygons. If the operations on the boundary polygon are not explicitly stated, they must be considered as the inverse of the operations performed on the obstacle polygons wherever appropriate. PreProcess The preprocess stage of the algorithm validates the input and performs some pre computation that helps in speeding up the main body of the algorithm. The lists below, detail the validation and precomputations that are performed on the input. Failure of any of the validity checks may lead to the premature termination of the algorithm. Input Validation * Check for nonempty start and goal configurations that lie in free space. That is, the configurations must exist in the input and the position of the start and goal must not lie inside an obstacle polygon or outside the boundary polygon. * If the set of obstacles and boundary polygons in the map is a nonempty set, ensure that the polygons are non selfintersecting. The reason behind this check is that the orientation of a selfintersecting polygon is undefined. Polygon orientation is required to differentiate between convex and concave vertices of the polygon. * By convention, the vertices of all polygons are ordered counterclockwise in the cartesian coordinate system. If the orientation of an input polygon is found to be clockwise, the ordering of its vertices is reversed. * The length of each edge of an obstacle or boundary polygon must be greater than a minimum length lo = 2R (lo = 4R for edges between two convex vertices of an obstacle). * The angle of each concave polygon must lie in the range [7n/2, 7]. Figure 41. Flowchart of the nonholonomic path planner. PreComputation * Pseudo obstacles are placed at the start and goal points in order to ensure that the path generated obeys the initial and final orientations of the vehicle. This is as shown in Figure 43. ABCDE forms the pseudo obstacle that forces the path to go through a pseudo start point D. * Since the shortest path always goes through convex vertices of the obstacle polygons and concave vertices of the boundary, these vertices are tagged as legal, while all other vertices are tagged as illegal vertices. This reduces the search time vehicle data + map + start and goal configurations collision free admissible path of the algorithm by not considering illegal vertices in the search for the shortest path. work space C contiglii,tio n Space \ isibili grh shortest path Figure 42. Graphic representation of program flow. Obstacle Expansion As stated earlier, a popular simplification of the path planning problem is to grow the obstacles in order to reduce the vehicle down to a point. The obstacle expansion stage of the algorithm is responsible for this transformation of the vehicle to a point in the discretized map. This stage of the algorithm is important from the point of view of the correctness of the output path. The obstacle expansion stage must not only allow for the computation of a collision free path, but must also guarantee that the path is an image of an admissible trajectory. The method used to expand the obstacles and shrink the boundary is described here in detail. It is also proved to guarantee paths that are images of admissible trajectories. E R D A (Xs, Y0 C B Figure 43. Geometry of a pseudo obstacle placed around the vehicle's start configuration. Once the obstacles are expanded and the visibility graph computed, the shortest path algorithm finds a piecewise linear path from that start to the goal point. Since the path is highly discontinuous, it must be smoothed in order to make it feasible for a vehicle to follow it. Some researchers like Laumond et al. [24] suggest the use of clothoids to smooth the path into a feasible trajectory. Clothoids are curves whose curvature varies as a function of its length as seen in Figure 44. They have been used successfully in connecting straight line segments and circular arcs with a continuous change of curvature curve for applications such as railway and highway design. Figure 45 shows how a clothoid can be used to smooth discontinuous paths built out of straight line segments. xo is where the clothoid starts and C is the configuration of the robot at the midpoint of the clothoid. a is the imposed direction of motion and 6 is the imposed direction of rotation. Figure 44. A clothoid (Cornu spiral). 0>0 ..... lo oid ..... .lolto.hoid "' > "4 S5<0 Figure 45. Paths smoothed with clothoid curves [24]. The expansion algorithm in this paper utilizes another representation of a polynomial curve chosen by the JAUS Committee [25]. Each segment of this curve is defined by three waypoints Po, P1 and P2 and a weighting factor w. The parametric form of the curve is, (1 u)2 P +2u(1u)wP +u2 P p(u) = 2u, where u = [0,1], w > 0 (4.1) (1 u)2 +2u(1u)w+u2 In the example curve shown in Figure 46, it may be noticed that p(0) = Po and p(l) = P2. The weighting factor w has the effect of moving the trajectory closer or further away from the point P1, which is called the control point. This effect can be seen in Figure 47. As the trajectory moves closer to the control point, or further away from it, its maximum curvature Kmax tends to increase. Kmax can be found by equating the first derivative of equation 4.2 to zero. x'y"y'x" K = XY where (x,y) is a point on the trajectory p(u) (4.2) 5 P1 4y  wl=1.5 3 2 PO 1 \P2 0 0 1 2 3 4 5 6 x Figure 46. Polynomial curve suggested by the JAUS Reference Architecture [25] for the generation of a trajectory. An important feature of this trajectory is that the lines PoPi and PiP2 are tangent to the trajectory at Po and P2. By choosing Po and P2 to lie between the end points of a path segment, a continuous trajectory may be generated out of consecutive curve segments as seen in Figure 48. P2 is chosen to lie between the endpoints of the path segment P1P3 to yield a trajectory made up of two curve segments that share a common tangent P1P3 at P2. 5 3 I =1 y 2 1 0 P0 W^~I' p^^~ ^>,(' W~ I^: P2 3 4 5 6 7 8 9 x Figure 47. Effect of the weighting w factor on the trajectory. Figure 47. Effect of the weighting w factor on the trajectory. 6 P1 5 4 3  y 2 / curve segment PO 1 0  1 2 I 0 1 2 3 4 5 X curve segment P3 P4 6 7 8 9 Figure 48. A trajectory made up of two curve segments that share a common tangent. III This feature was used to develop an effective expansion method that guarantees an admissible trajectory for any shortest path in the map. Since the waypoints along the shortest path coincide with only convex vertices of the obstacles, the expansion of convex vertices must ensure that any trajectory going through them lies in free space and has a maximum curvature less than 1/R. The concave vertices must be expanded only to maintain a sufficient offset along the walls of the obstacles in order to ensure collision free paths in the concave regions of the obstacles. The following sections describe the geometry behind the expansion method for convex and concave vertices. Expansion of Convex Obstacle Vertices The geometry behind the expansion of convex vertices is shown in Figure 49. The convex obstacle vertex Po is replaced by two expanded vertices P1 and P2. This adds a new edge P1P2 to the expanded obstacle for every convex vertex in the original obstacle polygon. The length of the new edge is lo, and is determined by the minimum edge length constraint. A vehicle that braces the sides of the obstacle will follow a trajectory that comes closest to the obstacle at the midpoint of the new edge P1P2. The new expanded points Pi and P2 form the control points of this trajectory. By choosing a safe distance d that is a function of the vehicle's width W, the trajectory shown in Figure 49 will be collision free. The maximum curvature constraint of 1/R may be met by choosing an appropriate weighting factor w. Since the convex angle 0 lies in the range (0,7n) the boundary conditions occur when 0 & 0 and 0 n. The expansions for the boundary conditions are shown in Figure 410. Notice that the minimum offset of the vehicle from the obstacle is min(d, lo 2) in the range 0 =(0,n) when the vehicle moves along any trajectory in the neighborhood of the point Po. P Po lo/2 P2 Figure 49. Geometry of expansion of a convex obstacle vertex. lo/2 lo/2 00P 0=0 lo/2 Pi " obstacle lo/2 P2 Figure 410. Boundary conditions for the expansion of convex obstacle vertices. Expansion of Concave Obstacle Vertices The minimum offset that must be maintained between the vehicle and the obstacle walls is achieved by offsetting the concave vertices along their bisectors of the concave angle as seen in Figure 411. The concave obstacle vertex Po is replaced by a vertex P1 offset along the bisector by d/sin(O/2). When this is done in conjunction with the expansion of the convex vertices, the edges of the expanded obstacle will have a 0/2 0/2f minimum offset of min(d, 1o/2) all around the obstacle boundary. The boundary conditions for the expansion of concave vertices are shown in Figure 412. \ Po0 Figure 411. Geometry of expansion of a concave obstacle vertex. I p z/2 O r Po Figure 412. Boundary conditions for the expansion of concave obstacle vertices. Main Result: Expansion Method Guarantees Admissible Trajectories Any shortest path in a map i/ ith expanded obstacles is an image of an admissible trajectory. Proof: The shortest path between any given start and goal configuration uses the expanded convex vertices of the obstacles as waypoints. The shortest path is piecewise linear and is guaranteed to lie in free space when the obstacles are expanded with sufficient distances d and lo. The piecewise linear path is converted into a trajectory by using the second degree polynomial curve described by equation 4.1 to smooth the corners of the path. This is done by choosing the vertices or waypoints of the path as control points of the smoothing curve segments, and endpoints of the smoothing curves along the length of the path segments adjacent to the path vertices (Figure 48.). In order to prove that any shortest path among expanded obstacles is an image of an admissible trajectory, it is sufficient to prove that the curve segments used to smooth the path lie in free space and have a maximum curvature of 1/R. The curve segment shown in Figure 46 can be made symmetric about the control point by choosing segments PoP1 and P1P2 to be of equal length, say lo/2. Assuming that each path segment has a length of at least lo, the path will be made up of a sequence of segments of the type, CaSbCa, where n < a < 7t/2, b > 0. C represents segments of the polynomial curve used for smoothing and S represents a straight line segment of the path. a represents the acute angle at the control point and b, the length of the straight line segments. Notice that as the angle of a convex obstacle vertex ranges from (0, 7), the inner angle at each of the two expanded vertices (P1 andP2 in Figure 410) has the range (7t/2, 7t). Therefore the tightest turn that any path can take is a right angled turn. When the curve segment is made symmetric, the maximum curvature occurs at either u = 0.5 or u = 0 and u = 1 depending on the choice ofw. A case for which the maximum curvature occurs at u = 0 and u = 1 is seen in Figure 414 in the plot of K versus u for the trajectory shown in Figure 413. It may also be noticed that as the angle PoP1P2 increases, the maximum curvature Kmax decreases when w is kept constant. Also, decreasing the lengths of PoP1 = PP2 = 10/2 has a scaling effect on the symmetric curve that increases Kmax. Therefore, by fixing the length lo and then finding a weighting factor wl for which Kmax < 1/R for the worst case turn (which happens to be a 90 degree turn), we can ensure that Kmax for any symmetric curve that may be encountered will be less than 1/R as long as each path segment has a length of at least lo. Figure 413 below, shows the worst case turn. Figure 413. The worst case turn. 0.10001 0.1 0.09999 0 0.2 0.4 0.6 0.8 1 u Figure 414. Plot of curvature K versus parameter u for a symmetric curve with w = 0.707 The offset distance d may be set at half the vehicle width W/2 plus some corner clearance cc to account for inaccuracies such as drift that occur when the path is being tracked by the vehicle. Therefore, when lo = max(2R, d), w must be 0.707 for Kmax < 1/R. W d = + cc (4.3) 2 10 = max(2R, d) (4.4) w, = 0.707 (4.5) d, lo and wl in the equations above will guarantee that any trajectory will be collision free and have a maximum curvature less than 1/R as long as the length of each path segment is at least lo. Unfortunately, the assumption made about the minimum length of each path segment does not hold for the general case. There may be cases when the length of a path segment is much smaller than lo. This occurs when a path segment spans the free space between two obstacles as against bracing an obstacle edge. The case for which a path length is less than lo is treated as a special case as follows. Consider a point Pi on a shortest path that belongs to an expanded obstacle vertex as shown in Figure 415. The region around Pi is broken up into four quadrants numbered counter clockwise. When the worst case turn is considered, the region in the fourth quadrant is occupied by the expanded obstacle and therefore, the two path segments PiiPi and PiPi+1 can neither originate nor end in the fourth quadrant. Notice that these two path segments can neither originate nor end even in the second quadrant. This is because there will always be a shorter path that does not go through Pi when either Pi1 or Pi+1 lie in the second quadrant, thereby disproving the fact that the path through Pi is the shortest path. Therefore, any shortest path that goes through Pi must pass through the first and third quadrants only. Without loss of generality, let us assume that Pi1 lies in the third quadrant and Pi+1 in the first. For the path segment PiiPi to violate the minimum path segment length assumption, Pi1 must lie in the shaded region of Q3. '\ PiI Figure 415. Violation of the assumption on minimum path segment length lo. An instance of such a situation is seen in Figure 416, where two obstacles with convex vertices having 0 0 degrees are placed next to each other. The shortest path from Po to P6 is PoP1P2 P3 P4 P5 P6. The length of the segment P3P4 is less than lo and in general, can vary in the range (0, lo) as P3 moves around the shaded region of the third quadrant in Figure 415. As the length of the segment decreases, the maximum curvature of the trajectory between P2 and P5 increases and may even be greater than 1/R. In order to prevent this from happening, the waypoints P3 and P4 are replaced by Pa and P the midpoints of the line P2P3 and P4P5 respectively. Pb is the midpoint of the segment PaPc. Now, the lengths of the new path segment P2Pa is 1o/4 and PaPb will always be greater than 1o/4 as the angle P2PaPb varies in the range [27c/3, 7]. obstacle Figure 416. A problem instance where the assumption on minimum path segment length lo is violated. Another weighting factor w2 that ensures Kmax < 1/R may be found for the worst case when the angle P2PaPb is the least (27/3) and the length of PaPb is the least (1o/4). Figure 417 shows the plot of the curve between the points P2 and Pb when R = 10. Figure 418 shows the corresponding curvature plot when w2 = 0.875. For any point that P3 takes in the shaded region of Figure 415, either the angle P2PaPb will be greater than 27/3 and/or the length of P3Pa will be greater then lo/4. The same applies to the curve segment between Pb and P5. For this case, it is evident from Figure 415 that Kmax is slightly greater than 1/R. This implies that a vehicle tracking the worstcase path segment for this case will experience some error. Since the length of the path segment is fairly short (at most around 2R), the error will be fairly small. 2.2 Pb y f Figure 417. The worst case turn for the special case. K 0.11  0.1145 0.114 / \ 0.1135 0 0.2 0.4 0.6 0.8 1 Figure 418. Plot of curvature K versus parameter u for the special case with w = 0.875 For the case when two or more consecutive path segments have lengths smaller than lo, a smoothing operation that omits some intermediate waypoints may be performed in an effort to get all path lengths greater than or equal to lo. Figure 419 shows an example where consecutive path segments in gray have lengths less than lo. The black line shows a smoothed path segment with length greater than or equal to lo. Paths such as this will always form a zigzag pattern due to the fact that alternate way points lie in diagonal quadrants (refer Figure 415). The smoothed path segment tends to cut the free space that lies within the expanded polygons but always lies between the dashed lines that join the obstacle corners that are being circumnavigated. Figure 420 shows the worst case scenario when the path takes a near worst case turn through points PoPiP2. If the path segments PoPi and PiP2 happen to have lengths just short of lo, the waypoint Pi will be omitted to give the new path PoP2P3. As seen in the figure, this path cuts through the expanded obstacle and may intersect with the obstacle edge if a sufficient offset distance d is not provided. Figure 4.19. A problem instance where two or more consecutive path segments have lengths less than lo. P2 P3 Figure 420. The worst case scenario when the path is smoothed. To accommodate for this case, the variable d in equation 4.3 must be further relaxed and the final form of all the expansion variables are listed below. W d= R + + cc (4.6) 2 10 = max(2R, d) (4.7) w, = 0.707, w2 = 0.875 (4.8) The use of variables d, lo, wi and W2 in the equations above, will ensure that any shortest path in the configuration space created by the proposed expansion method is an image of an admissible trajectory. The shrinking of the boundary polygon is the exact opposite of the expansion of the obstacle polygons. Concave boundary vertices are shrunk in a manner similar to the convex vertex expansion of obstacles, and vice versa. This is easily done by running the obstacle expansion algorithm on the boundary polygon after its orientation has been switched from counter clockwise to clockwise temporarily. Once the boundary polygon is shrunk, the orientation is reversed again to obey the CCW convention. After the expansion, expanded convex vertices that lie in the interior of obstacles (due to polygon intersections) or to the exterior of the boundary are also tagged as illegal vertices. Visibility Graph Once the obstacles are expanded to guarantee that the shortest path is an image of an admissible trajectory, the map must be decomposed into a graph data structure denoted by G = (V E), where Vis a set of nodes and E is a set of edges connecting the nodes in V. G is a bidirectional graph whose nodes represent the legal vertices in the map and whose edges carry weights that are equal to the line of sight distance between the legal vertices. The graph is chosen to be represented as by an adjacency matrix M, where M[i][i] gives the distance between the vertex P, and vertex P, if both are legal vertices. Mis symmetric about its diagonal and has the size n2, where n is the number of legal vertices in the map. A natural brute force method to fill the adjacency matrix is to compute the visibility of every vertex P,, i = ..n with respect to every other vertex P,,j = 1..n. In order to do this, the line segment P,P, must be checked for intersection between every obstacle edge in the map. If no obstacle edge intersects the edge P,P,, the vertex P, is visible to vertex P, and the distance between them is entered into M[ij] and M[j,i]. This turns out to be an O(n3) algorithm. The computation of the visibility graph therefore, turns out to be the bottleneck of the shortest path algorithm. By improving the efficiency of this stage of the algorithm, the overall algorithm speed will be greatly enhanced. de Berg et al. [26] have applied the popular sweep line algorithm in computational geometry to the visibility graph problem and have created an algorithm that runs in O(n2logn) time, a significant improvement over the brute force method. This algorithm has been adapted to the path planning implementation and is briefly described below. WI1 Figure 421. Radial sweep method used to find the visibility of vertices. The visibility graph G is first initialized such that Vis the set of all legal vertices in the graph and E = 0. For each vertexpe V, i=l..n create a sorted list Wof legal vertices in the map according to the clockwise angle made by the half line joining p, to each vertex and the positive xaxis. Ties are broken by virtue of the distance from p,. Initialize the half line p with the positive xaxis as seen in Figure 421. Find all obstacle edges that properly intersect p and store them in a balanced search tree T in the order in which they intersect p. For each vertex w, e W, = 1..n the following operations are performed to find ifw, is visible top,. * If the line segment pw intersects the interior of the obstacle of which w, is a vertex then w, is not visible to p, (Figure 422(d)). * Ifpi is the first vertex whose visibility is being checked, or w,_l is not on the segment pw, check if the edge e in the left most leaf of T (papb in Figure 422(e)) intersects p,w,. If e exists and interesects pwj, then again wj is not visible top,. Else w, is visible top, and an arc (p,, w,) is added to G. * Ifw,_l is on the line segmentp,w, and wjl is not visible (Figure 422(b)), then wj is not visible either. If w,_1 is on the line segmentp,w, and w,_ is visible, check if there is an edge e in T that intersects the line segment w,_ wj (Figure 422(a)(c)). If such an edge exists, w, is not visible, else it is visible and an arc (p,, w,) is added to G. For each vertex p,, one sort operation that costs O(nlogn) and one full search of the binary tree (in the worst case) that costs O(logn) time must be performed. All other operations take constant time. Therefore, the total number of operations performed for all n vertices in Vis O(n(nlogn+logn)) whose asymptotic complexity is O(n2logn). Shortest Path Once the adjacency matrix M of the visibility graph is computed, finding the shortest path is a simple task that takes O(n2) time with Dijkstra's algorithm [27]. Dijkstra's algorithm uses a greedy heuristic that makes a one edge extension of minimum cost in each iteration. Beginning with the trivial path from the start vertex to itself, which 43 costs zero, single edge extensions of minimum cost are made until the goal is reached. If the goal vertex is not reached after the entire graph has been searched, the graph is disjoint and the goal vertex is unreachable. The worst case complexity of the preprocess and expansion is O(n2). Therefore, the overall worst case complexity of the path planning algorithm is therefore O(n2logn). P1 W4 P1 (a) (b) P1 (e) th Figure 422. Determining the visibility of vertices. CHAPTER 5 PATH PLANNING WITH A RADIATION CONSTRAINT This section describes one possible way of planning paths in a constrained environment using the path planner. The algorithm described here utilizes the path planner to lower the dose received by a mobile robot moving in a radiation field. We look at the problem in the plane of the robot and for the sake of simplicity, assume that the radiation sources are point sources that are placed in the same plane. The algorithm was initially designed and implemented without considering attenuation from obstacles. An extension that allows the algorithm to consider attenuation was later designed and is presented in appendix A. Radiation Basics Of the many different forms of radiation emanated from radioactive materials, gamma rays are by far the most harmful from the point of view of robotic equipment. Exposure to gamma radiation can be minimized by following a basic ALARA principle [28] that states, by reducing the amount of time spent near a source of radiation, increasing the distance between the source and the robot, and by using shielding material placed between the source and the robot reduces the exposure to radiation. The effect of radiation exposure may be measured using the units rad (radiation absorbed dose) and rem (Roentgen Equivalent Man). Rad is used to measure the quantity of absorbed dose in terms of the amount of energy actually absorbed by a material, while rem is used to derive an equivalent dose based on the type of radiation being emanated. Since different types of radiation have different effects, the absorbed dose in rad was multiplied by a quality factor Q, unique to the incident radiation, to get the equivalent dose in rem. In recent years, these units have been replaced by two new SI units gray (Gy) and sievert (Sv). One gray represents the energy absorption of 1 joule per kilogram of absorbing material. That is, 1 Gy = 1 JKg (5.1) The unit Gray replaced rad as the choice of absorbed dose and sievert replaced rem as the new choice for equivalent dose. The equivalent dose in sievert for a given type of radiation was defined as H=Dxwh (5.2) His the equivalent dose in sievert, D is the absorbed dose in gray and wh is the radiation weighting factor that is dependant on the type of incident radiation. The amount of radiation absorbed by a material per unit time is given by the equivalent dose rate or simply dose rate. Dose rate is usually measured in sieverts per unit time, that is Svh1 or more usually mSvh1. The cumulative dose is the integral of dose rate over time and represents the total amount of radiation absorbed. The intensity of the absorbed radiation depends on the absorption of radiation by the air and the shape and size of the source relative to the distance from which it is viewed. Gamma rays are not scattered by air and therefore, their intensity is determined as a function of the distance from the source. Most numerical computations and simulations that deal with gamma radiation consider the shape of the source to be a point, line or an extended source. Point sources are used when the radioactive material is believed to be emitting equal quantities of radiation in all directions. Line sources represent pipe like structures while extended sources are used to represent large areas like walls and floors. The algorithm proposed here assumes that all sources are point sources and obey the inverse square law. The inverse square law states that the radiation intensity at a point away from the source is the inverse of the square of the distance from the source. This implies that the radiation spreads over larger areas with decreasing intensity in all directions as shown in Figure 51. Id (5.3) d2 point source Figure 51. A point source of radiation. Algorithm Description The problem of finding the shortest safe path is a multiobjective problem with two objectives minimum distance and minimum dose. Due to this, the overall objective of the problem may be stated in several different ways. From the practical standpoint, more is gained from minimizing dose than distance. Therefore, one possible way of defining the global optimal path Popt is, a path with the least dose absorption. That is, no other path in the map has a cumulative dose absorption less than the path Popt. Sources may be limited to regions beyond which the radiation dose rate is so insignificant that it is not considered to be a risk to the robot. The radiation beyond these regions is considered to be zero. When the map contains no boundary, any path that steers clear of the circular regions around the sources that contain significant radiation is an optimal path. Paths Pa and Pb in Figure 52 are optimal paths. But, maps that contain Figure 52. Optimal paths in a map with no boundary. boundaries can make the problem very challenging. When a boundary is present, it is not always possible to steer clear of the sources. There may be situations where every possible path in the map receives some significant dose and this calls for a more guided and maybe even exhaustive search for the optimum path. Besides, the optimum path does not necessarily have to pass through the vertices of the discretized map. This implies that a graph type algorithm can only achieve a near optimal result. From this point of view, it may be better to approach this problem using some other approach like the potential field method or a randomized heuristic. But, due to the complex nature of radiation fields, these approaches also cannot guarantee the global optimal path. They may only provide better approximations when compared to a graph search method, but at higher computational costs. The cumulative dose absorbed by the robot is lessened by using the path planner to enforce the basic ALARA principle of increasing the distance of the robot from the sources to reduce exposure. The exposure is minimized by beginning with the shortest path and moving this path away from the radiation sources. At first, circular pseudo obstacles of unit radius are drawn around each source. The circles are then grown incrementally such that the intensity of the radiation at their circumference reduces by some small amount, say 1 mSvh1 with each iteration. Figure 53 illustrates this concept. The pseudo obstacles create a new type of configuration space where the dose at every point in free space, contributed by any given source, is less than or equal to the intensity at the circumference of the pseudo obstacles. Circles that enclose radiation sources with relatively smaller intensities grow at slower rates than circles that enclose high intensity sources. With each iteration, the pseudo obstacles are grown and the path planner is executed to find if a path still exists. The iterations continue until either all the circular pseudo obstacles have been grown to their maximum limit, (intensity of 0.1 mSvh' at the circumference to prevent d from going to infinity) or when a path cannot be found. In the former case, the shortest path happens to be the optimum path. In the latter case, the path found in the second last iteration represents a near optimal path. When the optimal path is found, the cumulative dose for the entire path is computed by interpolating dose per source per unit time absorbed along the length of each path segment and summing the total dose received from all the sources. The speed of the robot is found to have a scaling effect on the amount of radiation absorbed along a given path. (a) (b) (c) (d) Figure 53. Growing pseudo obstacles around radiation sources to find an optimal path. There may be two special cases when the radiation sources are placed in series or in parallel with respect to a path between the start and goal points. When they are placed in parallel, there may be cases where the optimal path found passes through the irradiated intersection regions of two or more radiation sources as shown in Figure 54. The combined dose from both sources for the resulting path may be higher than an alternate path like Palt that traverses through a region of possibly lower radiation intensity. But, a path like Palt tends to go closer to the source (where the intensity can spike) and travels a longer distance in the irradiated region. Therefore, there is very little gained, if any, by the alternate path Pal,. Finally, there is the case when the radiation sources are placed in series. When this is done, there may be a possibility where a path is not found due to the intersection of only one pseudo obstacle with the boundary or two or more obstacles as max radiation Sinter action region Poptamal boundary Figure 54. Radiation sources placed parallel with respect to a path between the start and goal points. shown in Figure 55. The intersecting pseudo obstacle C1 that can make the map disjoint is referred to as the constricting pseudo obstacle. Pseudo obstacles belonging to other sources (nonconstricting pseudo obstacles) may have the potential to be grown further to reduce the cumulative dose by freezing the growth of the constricting pseudo obstacle in successive iterations until all the pseudo obstacles are found to be constricting, or have reached their maximum growth limit (dose at their circumference equals zero). In Figure 55, the circle C2 may be expanded to its maximum limit C3 after C1 has been frozen in Figure 55. A case with a constricting pseudo obstacle. order to yield a safer path P2 when compared to the path P1 found at the point of constriction. Finding constricting pseudo obstacles is not a trivial task though. There may be two or more constricting pseudo obstacles in a single iteration that prevent a path from being found. In order to identify all these obstacles, first an intersection check is done to find a set Sc of all pseudo obstacles that have at least two points of intersection with the boundary, or intersect with two or more obstacles, or the boundary and at least one obstacle (including other pseudo obstacles). The path planner is then executed once for each of the pseudo obstacle in the set Sc. In each of these executions, one obstacle from Sc is retained in the map while all other obstacles belonging to Sc are removed from the map. If a path still does not exist under these conditions, the retained pseudo obstacle is considered as a constricting pseudo obstacle. The algorithm described thus far is illustrated in the form of a flowchart in Figure 56. A copy of the most recent successful path in the iterations is maintained in the variable lastPath. currPath represents the path found in the current iteration. maxDose is the intensity of the largest intensity radiation source, and doseCircumference is the intensity at the circumference of all the pseudo obstacles. Figure 56. Flow chart of the path planner with a radiation constraint. The algorithm expands all the pseudo obstacles that represent radiation fields from point sources to the maximum possible extent to find the safest path. The algorithm may easily be extended for other types of sources like line and extended sources just by incorporating the geometry of their dispersion into the expansion scheme. The cumulative dose calculated by interpolation is not an accurate representation of the dose received by 53 the robot since a highly simplified radiation field is used to find the path. It is only used as a relative measure to find the optimal path. The actual radiation field is more complex due to gamma interaction processes such as pair production, Compton scattering and buildup. Nevertheless, the resulting path is bound to be near optimal by virtue of the basic ALARA principle. CHAPTER 6 RESULTS AND CONCLUSION The algorithms were implemented in C and were tested with the help of an interactive graphical user interface developed in Javal.2. The interface linked to the C implementation through the Java Native Interface for C. The C implementation was built on a simple and efficient data structure that is general enough to be used for other map based algorithms like the path sweep algorithm or path planning among moving obstacles. Results from sample runs of both the path planner and the path planner with radiation constraints are shown below. Figure 61 shows a path being generated among a set of expanded nonconvex obstacles and a boundary. Pseudo obstacles are used to enforce the initial and final configurations of the robot. Figure 62 shows the trajectory for the path generated in Figure 61. Figure 63 is an example of the special case when the length of a path segment is less than lo. The fourth path segment from the goal is modified to ensure that the final trajectory is admissible. The final trajectory for this path shown in Figure 64. Figure 65 shows the trivial case when the sets of obstacles and boundary vertices are empty sets. The shortest path is a straight line path. Figure 66 and 67 show a case where concave boundary vertices are used to find the shortest path. Figures 68 to 612 show example runs of the path planner with radiations sources in the environment. Figure 68 shows a case when no boundary is present. Any path that avoids the circular (approximated by octagons) regions that represent the effective reach of the radiation is an optimum path. Figures 69 and 610 show cases where the optimum path is obtained despite the constraint imposed by a boundary polygon. In Figure 611, some dose is received by the vehicle as it passes through the two radiation sources closest to the start point. In such a case, there is no path with zero cumulative dose. If the path were to pass by the boundary, it would not only travel a longer distance within the circular regions of the sources near the boundary, but will also go closer to the source when compared to the path found by the path planner. Finally, Figure 612 shows a case where a constricting pseudo obstacle's expansion is frozen while all the other pseudo obstacles are expanded to their maximum limit to yield an optimal path. The path planner was also used successfully for online path planning onboard the UF Eliminator [29]. The data from onboard sensors was discretized using image processing algorithms developed by Donald McArthur. The discretized map was then used to find a path to a local goal. The process is illustrated in Figure 613. Figure 61. Shortest path in a map with nonconvex obstacles and a boundary. Figure 62. An admissible trajectory for the path generated in Figure 61. Figure 63. A special case when the length of a path segment is less than lo. Figure 64. An admissible trajectory for the path generated in Figure 63. KUMZD Figure 65. A trivial case of a straight line path. Figure 66. A case where concave boundary vertices are used to find a path. Figure 67. An admissible trajectory for the path generated in Figure 66. 4, e 4? Figure 68. A radiation environment with no boundary. Figure 69. A bounded radiation environment. Figure 610. Another bounded radiation environment. Figure 611. A case where a zero dose path does not exist. Figure 612. A case with a constricting pseudo obstacle. ........,...'. 6k Figure 613. Path planner used for online path planning. APPENDIX A ATTENUATION By considering attenuation, a better approximation of the safest path may be achieved. Although the current implementation does not consider attenuation, a method that can be incorporated has been developed and is described here from a purely geometric perspective. The radiation intensity after attenuation for a point source of radiation is computed with the help of equation A.1 below [30]. I l = e (A.1) where, t is the thickness of a homogeneous absorbing material with an attenuation coefficient a. In Figure Ai below, the intensity after attenuation at point P1 along the horizontal can be computed using equation A. 1, but at P2 (which is at an angle 0 with the horizontal) the intensity is given by equation A.2. t / ,P2 point source So 0 P1 Figure A1. Attenuation from a plane shield in front of a point source of radiation. Figure Ai. Attenuation from a plane shield in front of a point source of radiation. S Iso rsec 0 (A.2) 2 (xsec )2 From the equation A.2, it is seen that if the effective distance traveled by a ray inside the shielding material can be found, and if the attenuation factor of the material is known, the intensity of the radiation at a point on the other side of the shield can be computed. In the case of a map with obstacles and radiation sources, the effective distance can be found if we can find the edges of the obstacles that attenuate the rays passing through them for each ray from the sources to points on the path. These edges can be found by applying the radial sweep line algorithm used in the path planner to compute the visibility graph. Figure A2 below shows how this can be done. At first all the vertices in the map are sorted to create a sorted list W according to the clockwise angle made by the half line joining So to each vertex and the positive xaxis. For each path segment P,_P, as shown in the figure, the sorted vertices that lie in the region So P, 1P, are retained while all other vertices are discarded. These vertices now form event points el, e2,.. e,. The rays passing between event points either pass through air or intersect one or more obstacles. If they intersect an obstacle, the edges that define the obstacle boundary can be quickly retrieved from the balanced search tree T used in computing the visibility graph. The edges are retrieved in the order of distance from So and are used to find the effective distance t' (Figure A2) by finding the intersection between the edges and the ray passing through them. As long as the obstacles are not overlapping, the region between the edges in the balanced search tree will alternate between air and obstacle material, starting with material if the source of placed inside an obstacle polygon. As stated in Figure A2. Radial sweep line method to compute attenuation. Chapter 5, this approach does not give an accurate measure of cumulative dose received by the robot. It only serves as an efficient geometrical method to find a relative measure that can be used to find safe paths. When attenuation is considered, the total cumulative dose must be computed for each iteration and a copy of the least dose path must also be maintained since the final path may not be the safest path. APPENDIX B DATA STRUCTURE point. h ptArrayQSort. h edge.h edgeBinSearch.h edgeList.h obstacleList.h map.h vehicle.h io.h I preprocess.h weightedDiGraph.h visibilityGraph.h I radiation.h I shortestPath. h I pathPlanner. h main.h Figure B1. Code layout. /* point.h */ /* point.h declares the interface for a point in 2D space */ /* Author: Arfath Pasha */ /* version: pathplanner4.1 (8/8/01) */ /* max size of string returned by all to String functions (atleast 1000)*/ #define MAXSTRING 2000 /* tolerance value */ #define TOL 0.001 typedef struct Point Point; struct Point { double x; /* xcoordinate */ double y; /* ycoordinate */ short obstID; /* polygon ID of the polygon the point belongs to (1 if the point does not belong to a polygon */ unsigned short pointID; /* point ID used for computing the visibility graph (default value is 0 )*/ char fig; /* flag set to '1' if point is legal 'i' if point is illegal */ double ang; /* angle subtended by this point for computing visibility (default value is 0)*/ double visib; /* visibility value used in computing the visibility matrix 0 if invisible, +ve value if visible (default value is 0)*/ }; /** creates a new point @Parameters: x xcoordinate y ycoordinate obstID ID of polygon the point belongs to (1 if it does not belong to a polygon) fig character flag '1' for legal point that can be used for navigation 'i' for illegal point that cannot be used for navigation @Returns: pointer to the new point **/ Point createPoint(double x, double y, int obstID, char fig); /** creates a clone of the point @Parameters: orig point to clone clone pointer to the cloned point @Returns: void **/ void clonePoint(Point *orig, Point **clone) ; /** frees the memory allocated to the point @Parameters: point pointer to point to free @Returns: empty point **/ Point freePoint(Point *point); /** checks the equality of two points @Parameters: pl first point p2 second point @Returns: 0 if points are equal 1 if points are not equal **/ int pointsAreEqual(Point *pl, Point *p2); /** returns a character string of the point. used only for testing. max size of string is MAXSTRING uipoiit li @Parameters: point point str point converted to string @Returns: void **/ void pointToString(Point *point, char *str); /** returns the distance between two points @Parameters: pl first point p2 second point @Returns: distance between the two points **/ double getDist(Point *pl, Point *p2); /** returns the angle in radians subtended at pi by a line passing through the two points and the +ve xaxis note: CCW angle returned if origin is in the top left corner of the screen. CW angle returned if origin is in the bottom left comer of the screen. @Parameters: pl point p2 point @Returns: angle in radians **/ double getAngle(Point *pl, Point *p2); /** checks to see if p2 lies on the CW or CCW side of edge plp3 (plp3 is called ro) @Parameters: pl first point p2 second point p3 third point @Returns: 0 if p2 is on the CW side 1 if p2 is on the CCW side **/ int getPosWRTRo(Point *pl, Point *p2, Point *p3); /** rotate a point (pl) with respect to another point (pO) @Parameters: pl point to rotate p0 reference point (make p0=(0,0) for origin) theta angle to rotate (in radians) @Returns: void **/ void rotatePoint(Point *pl, Point *pO, double theta); /* ptArrayQSort.h */ /* ptArrayQSort.h declares the interface for an array of points in * 2D space that can be sort by angle using the quick sort algorithm */ /* Author: Arfath Pasha */ /* version: pathplanner4.1 (8/8/01) */ #include "point.h" 69 typedef double ElementType; /* type of element to sort */ #define Cutoff ( 3 ) /* cutoff point for quick sort */ /***************************** point array routines **************************/ /** creates an array of empty points @Parameters: numPts number of points in the array @Returns: pointer to the point array **/ Point createPointArray(int numPts); /** returns a character string of the point array. used only for testing. max size of string is MAXSTRING uipoiii li @Parameters: pArray pointer to the point array size array size str point array converted to string @Returns: void **/ void pointArrayToString(Point* pArray, int size, char *str); /** frees the memory allocated to the point array @Parameters: pArray pointer to pointer to the point array to free @Returns: empty point array **/ Point freePointArray(Point *pArray); /**************************** quick sort routines ****************************/ /** swaps two points @Parameters: Lhs point of lhs of median Rhs point of rhs of median @Returns: void **/ void swapPts( Point *Lhs, Point *Rhs) ; /** Returns median of Left, Center, and Right @Parameters: A[] array of points left left end of median right right end of median key 'a' if angle is the key 'i' if pointlD is the key @Returns: element in the median of 3 **/ ElementType Median3( Point A[ ], int Left, int Right, char key ); /** sorts points between left and right based on the given key @Parameters: A[] array of points left left end right right end key 'a' if angle is the key 'i' if pointID is the key @Returns: void **/ void Qsort( Point A[ ], int Left, int Right, char key ); /** sorts an array of points using the insertion sort algorithm based on the given key @Parameters: A[] array of points N size of the array key 'a' if angle is the key 'i' if pointID is the key @Returns: void **/ void InsertionSort( Point A[ ], int N, char key ); sorts an array of points using the quick sort algorithm based on the given key @Parameters: A[] array of points pp point being checked for visibility size size of the array key 'a' if angle is the key 'i' if pointID is the key @Returns: void **/ void ptArrayQSort( Point A[ ], Point *pp, int size, char key ); /* edge.h */ /* edge.h declares the interface for an edge in 2D space */ /* Author: Arfath Pasha */ /* version: pathplanner4.1 (8/8/01) */ #include "ptArrayQSort.h" struct Edge; typedef struct Edge Edge; struct Edge { Point *pl; /* first point */ Point *p2; /* second point */ int intOrder; /* integer that defines the order in which this edge intersects ro in computing the visibility graph (counting backwards from the total number of edges) (default value is 0) */ Edge *next; /* next edge (null if no edge follows)*/ }; /** creates a new edge @Parameters: pl first point p2 second point @Returns: pointer to the new edge **/ Edge createEdge(Point *pl, Point *p2); creates a clone of the edge @Parameters: orig edge to clone clone pointer to the cloned edge @Returns: void **/ void cloneEdge(Edge *orig, Edge **clone); /** frees the memory allocated to the edge @Parameters: edge pointer to edge to free @Returns: empty edge **/ Edge freeEdge(Edge *edge); /** checks two edges for equality @Parameters: el first edge e2 second edge @Returns: 0 if edges are equal 1 if edges are not equal **/ int edgesAreEqual(Edge *el, Edge *e2); /** returns a character string of the edge. used only for testing. max size of string is MAXSTRING i7poiii li @Parameters: edge edge str edge converted to string @Returns: void **/ void edgeToString(Edge *edge, char *str); checks to see if the given point lies on the given edge @Parameters: pp point ee edge @Returns: 0 if the point lies on the edge 1 if the point does not lie on the edge **/ int ptOnEdge(Point *pp, Edge *ee); /** Finds the point of intersection pp between two edges @Parameters: eel first edge ee2 second edge pp point of intersection (set pp to NULL if point of intersection is not required) @Returns: pp and a char with the following meaning: 'e': The segments collinearly overlap, sharing a point. 'v': An endpoint (vertex) of one segment is on the other segment, but 'e' doesn't hold. '1': The segments intersect properly (i.e., they share a point and neither 'v' nor 'e' holds). '0': The segments do not intersect (i.e., they share no points). Note that two collinear segments that share just one point, an endpoint of each, returns 'e' rather than 'v' as one might expect. **/ char edgelnt( Edge *eel, Edge *ee2, Point *pp); char Parallellnt( double a[], double b[], double c[], double d[], double p[] ) void Assigndi( double p[], double a[] ) ; int Between( double a[], double b[], double c[]) ; int Collinear( double a[], double b[], double c[]) ; int AreaSign( double a[], double b[], double c[]); /** Checks if ee2 turns to the left or right w.r.t. eel when eel,ee2 are two connected edges sharing a common point. i.e. if eel = plp2 and ee2 = p2p3, the function finds if p3 lies above or below the line passing through plp2. @Parameters: eel first edge ee2 second edge @Returns: 1 if ee2 turns left wrt ee 1 1 if ee2 turns right wrt eel 0 if eel, ee2 are collinear 74 **/ int turns (Edge *eel, Edge *ee2); /** Checks if the point common to two connected edges eel and ee2 is convex or concave. If eel = plp2 and ee2 = p2p3 in a polygon, the function checks if p2 is a convex point or a concave point. @Parameters: eel first edge ee2 second edge @Returns: 1 if p2 is a convex point 1 if p2 is a concave point 0 if eel, ee2 are collinear @note: The polygon is assumed to be oriented counter clockwise. **/ int isConvex (Edge *eel, Edge *ee2); /* edgeBinSearch.h */ /* edgeArrayWithBinSearch.h declares the interface for the binary search * of edges */ /* Author: Arfath Pasha */ /* version: pathplanner4.1 (8/8/01) */ #include "edge.h" struct TreeNode; typedef struct TreeNode *Position; typedef struct TreeNode *SearchTree; struct TreeNode { Edge *Element; SearchTree Left; SearchTree Right; }; /** frees memory allocated to the search tree @Parameters: T search tree to free @Returns: pointer to the empty search tree **/ SearchTree freeSearchTree( SearchTree T); /** finds an element in the search tree @Parameters: X element to find T search tree @Returns: position of the element in the search tree **/ Position findEdge( Edge *X, SearchTree T ) ; /** finds the minimum value in the search tree @Parameters: T search tree @Returns: position of the element with minimum value **/ Position findMinEdge( SearchTree T ) ; /** finds the maximum value in the search tree @Parameters: T search tree @Returns: position of the element with maximum value **/ Position findMaxEdge( SearchTree T ) ; /** inserts an element into the search tree @Parameters: X element to insert T search tree @Returns: modified search tree **/ SearchTree insertEdge( Edge *X, SearchTree T ); deletes an element from the search tree @Parameters: X element to delete T search tree @Returns: modified search tree **/ SearchTree deleteEdge( Edge *X, SearchTree T); /** retrieves the element at the given position of the search tree @Parameters: P position of the element in the search tree @Returns: element at position P **/ Edge retrieveEdge( Position P ); /** returns a character string of the search tree in inorder format used only for testing. max size of string is MAXSTRING uipoinil @Parameters: T search tree str edge converted to string offset set initial offset to 0 always @Returns: number of characters contained in the string **/ int edgeTreeToString(SearchTree T, char *str, int offset); /** performs an inorder search of the tree for an edge that intersects the given edge note: rVal must be initialized to 1 @Parameters: T search tree ee edge to intersect rVal pointer to the returned value @Returns: value of rVal is 0 if an intersecting edge was found 1 if no interesting edge was found **/ void searchEdgelnt(SearchTree T, Edge *ee, int *rVal); /* edgeList.h */ /* edgeList.h declares the interface for a list of edges in 2D space */ /* Author: Arfath Pasha */ /* version: pathplanner4.1 (8/8/01) */ #include "edgeBinSearch.h" typedef struct Bbox Bbox; struct Bbox { Point *pl; /* lower left point */ Point *p2; /* upper right point */ }; typedef struct EdgeList EdgeList; struct EdgeList { int size; /* size of the list */ double dose; /* radiation dose */ double radX, radY; /* coordinates of the radiation source */ int frozen; /* 0 if not frozen, 1 if frozen. Obstacle will not be expanded further if frozen.*/ double attenuation; /* attenuation coefficient */ Edge *edge; /* first edge */ Bbox *bbox; /* bounding box for the edge list */ EdgeList *next; /* next edge list (null if no edge list follows)*/ }; /** computes the bounding box of an edge list @Parameters: list edge list @Returns: void **/ void computeBbox(EdgeList *list); /** frees memory allocated to the bounding box @Parameters: bbox pointer to bounding box @Returns: empty bounding box **/ Bbox freeBbox(Bbox *bbox); /** creates an (empty) edge list dose for new edge list is 0 @Parameters: edge first edge of the list (may be empty) @Returns: pointer to the new edge list **/ EdgeList createEdgeList(Edge *edge); /** creates a clone of the edge list @Parameters: orig edge list to clone clone pointer to the cloned edge list @Returns: void **/ void cloneEdgeList(EdgeList *orig, EdgeList **clone); /** checks if the edge list is empty @Parameters: list edge list to check @Returns: returns the value of 0 if the edge list is empty 1 if the edge list not is empty **/ int edgeListIsEmpty(EdgeList *list); /** checks the size of the edge list @Parameters: list edge list to check @Returns: size of the list **/ int sizeOfEdgeList(EdgeList *list); gets the i'th edge in the edge list @Parameters: list edge list index index of the edge required @Returns: the i'th edge **/ Edge getEdge(EdgeList *list, int index); /** returns the index of the edge in the edge list @Parameters: list edge list edge edge @Returns: index of the edge in the edge list (1 if edge does not exist in the list) **/ int indexOfEdge(EdgeList *list, Edge *edge); /** removes the i'th edge from the edge list @Parameters: list edge list index index of edge to remove @Returns: modified edge list (null if edge list does not have any edges left) **/ EdgeList removeEdge(EdgeList *list, int index); /** adds an edge into the edge list @Parameters: list edge list to add edge to edge edge to add index index of the edge in the list @Returns: modified edge list **/ EdgeList addEdge(EdgeList *list, Edge *edge, int index); returns a character string of the edge list. used only for testing. max size of string is MAXSTRING uipoinil li @Parameters: list edge list str edge list converted to string @Returns: void **/ void edgeListToString(EdgeList* list, char *str); /** frees the memory allocated to the edge list @Parameters: list pointer to edge list to free @Returns: empty edge list **/ EdgeList freeEdgeList(EdgeList *list); /** checks two edge lists for equality @Parameters: list first edge list list2 second edge list @Returns: 0 if edge lists are equal 1 if edge lists are not equal **/ int edgeListsAreEqual(EdgeList *listl, EdgeList *list2); /** checks to see if the edge joining points pp and ww in the given obstacle intersects the interior of the obstacle. @Parameters: obst obstacle pp index of first point ww index of the second point @Returns: 0 if the edge intersects the interior of the obstacle 1 if the edge does not intersect the interior of the obstacle 2 if the edge is collinear with an edge in the obstacle **/ int interiorInt(EdgeList *obst, Point *pp, Point *ww); /** checks to see if the edge joining points pp and ww in the given obstacle intersects the exterior of the obstacle. @Parameters: obst obstacle pp index of first point ww index of the second point @Returns: 0 if the edge intersects the exterior of the obstacle 1 if the edge does not intersect the exterior of the obstacle 2 if the edge is coincident with an edge in the obstacle */ int exteriorInt(EdgeList *obst, Point *pp, Point *ww); /** checks to see if a point lies inside an obstacle @Parameters: obst obstacle pp point @Returns: i: pp is strictly interior to P o : pp is strictly exterior to P v : pp is a vertex of P e : pp lies on the relative interior of an edge of P **/ char ptInObst(EdgeList *obst, Point *pp); /** returns the orientation of the obstacle @Parameters: obs pointer to the obstacle whose polygon orientation is required @Returns: returns 1 if the obstacle is numbered CW 0 if the obstacle is numbered CCW 1 if polygon has no orientation (special case of a self intersecting polygon or straight line degenerate polygon) @note: This function checks the orientation in the cartesian coordinate systems. The CW and CCW values must be reversed for the graphic coordinate system. **/ int getObstOrientation(EdgeList *obs); /** switches the orientation of the obstacle from CW to CCW and vice versa @Parameters: obs pointer to the obstacle @Returns: void **/ void switchOrientation(EdgeList *obs); /* obstacleList.h */ /* obstacleList.h declares the interface for a list of obstacles in 2D space */ /* Author: Arfath Pasha */ /* version: pathplanner4.1 (8/8/01) */ #include "edgeList.h" typedef struct ObstacleList ObstacleList; struct ObstacleList { int size; /* number of obstacles */ EdgeList *obst; /* list of obstacles */ }; /** creates an (empty) obstacle list @Parameters: obst first obstacle of the list (may be empty) @Returns: pointer to the new obstacle list **/ ObstacleList createObstacleList(EdgeList *obst); /** creates a clone of the obstacle list @Parameters: orig obstacle list to clone clone pointer to cloned obstacle list @Returns: void **/ void cloneObstacleList(ObstacleList *orig, ObstacleList **clone); /** frees the memory allocated to the obstacle list @Parameters: list pointer to obstacle list to free @Returns: empty obstacle list **/ ObstacleList freeObstacleList(ObstacleList *list); /** checks if the obstacle list is empty @Parameters: list obstacle list to check @Returns: returns the value of 0 if the obstacle list is empty 1 if the obstacle list not is empty **/ int obstacleListIsEmpty(ObstacleList *list); /** checks the size of the obstacle list @Parameters: list obstacle list to check @Returns: size of the list **/ int sizeOfObstacleList(ObstacleList *list) /** gets the i'th obstacle in the obstacle list @Parameters: list obstacle list index index of the obstacle required @Returns: the i'th obstacle **/ EdgeList getObstacle(ObstacleList *list, int index); /** returns the index of the obstacle in the obstacle list @Parameters: list obstacle list obst obstacle @Returns: index of the obstacle in the obstacle list (1 if obstacle does not exist in the list) **/ int indexOfObstacle(ObstacleList *list, EdgeList *obst); /** removes the i'th obstacle from the obstacle list @Parameters: list obstacle list index index of obstacle to remove @Returns: modified obstacle list (null if obstacle list does not have any obstacles left) **/ ObstacleList removeObstacle(ObstacleList *list, int index); /** adds an obstacle to the tail of the obstacle list @Parameters: list obstacle list to add obstacle to obst obstacle to add @Returns: modified obstacle list **/ ObstacleList addObstacle(ObstacleList *list, EdgeList *obst); /** returns a character string of the obstacle list. used only for testing. max size of string is MAXSTRING uipoinil @Parameters: list obstacle list str obstale list converted to string @Returns: void **/ void obstacleListToString(ObstacleList* list, char *str); /* map.h */ /* map.h declares the interface for a map in 2D space */ /* Author: Arfath Pasha */ /* version: pathplanner4.1 (8/8/01) */ #include "obstacleList.h" typedef struct Map Map; struct Map { ObstacleList *obstList; /* obstacle list */ EdgeList *bdry; /* boundary */ Point *sPt; /* start point */ Point *gPt; /* goal point */ Point *pseudoSPt; /* pseudo start point */ Point *pseudoGPt; /* pseudo goal point */ double sOrient; /* starting orientation of the vehicle (in radians)*/ double gOrient; /* ending orientation of the vehicle (in radians)*/ EdgeList *path; /* shortest path */ short numPts; /* total number of points in the map */ double totalDist; /* distance of the path */ double totalDose; /* Total dose recieved by the vehicle */ }; /** creates a new map for the path planner @Parameters: obstList obstacle list bdry boundary sPt start point gPt goal point path shortest path @Returns: pointer to the new map @note: all input paramters must be null if empty **/ Map createMap(ObstacleList *obstList, EdgeList *bdry, Point *sPt, Point *gPt, EdgeList *path); /** frees the memory allocated to the map @Parameters: map pointer to the map to be freed @Returns: empty map **/ Map freeMap(Map *map); /** returns a character string of the map. used only for testing. max size of string is MAXSTRING points (&Parameters: map map str map converted to string @Returns: void **/ void mapToString(Map *map, char *str); /* vehicle.h */ /* vehicle.h declares the interface for a vehicle */ /* Author: Arfath Pasha */ /* version: pathplanner4.1 (8/8/01) */ typedef struct Vehicle Vehicle; struct Vehicle { double length; /* vehicle length */ double width; /* vehicle width */ double speed ; /* vehicle speed */ double tRad ; /* vehicle turning radius */ double gammaAu; /* allowable dose */ }; /** creates a new vehicle @Parameters: length vehicle length width vehicle width height vehicle height tRad vehicle turning radius gammaAu allowable dose @Returns: pointer to the new vehicle **/ Vehicle createVehicle(double length, double width, double speed, double tRad, double gammaAu) /** frees the memory allocated to the vehicle @Parameters: vehicle pointer to vehicle to free @Returns: empty vehicle **/ Vehicle freeVehicle(Vehicle *vehicle); /* weightedDiGraph.h */ /* weightedDiGraph.h declares the interface for a weighted digraph */ /* Author: Arfath Pasha */ /* version: pathplanner4.1 (8/8/01) */ typedef struct WDiGraph WDiGraph; struct WDiGraph { int numPoints; /* number of vertices */ int numEdges; /* number of edges */ double **graph; /* graph implemented as a 2D array */ }; /** creates a new weighted digraph @Parameters: size size of the weighted digraph @Returns: pointer to the new weighted digraph **/ WDiGraph createWDiG(int size); /** creates a 2D array of the specified size @Parameters: size size of the array @Returns: pointer to the 2D array **/ double **create2DArray(int size); /** puts the edge into the digraph @Parameters: gg graph ii first point jj second point wt weight of the edge @Returns: void **/ void putEdgelnWDiG(WDiGraph *gg, int ii, int jj, double wt); 88 /** returns 0 if edge(ij) exists, returns 1 otherwise @Parameters: gg graph ii first point jj second point @Returns: 0 if edge exists 1 if edge does not exist **/ int existsEdgelnWDiG(WDiGraph *gg, int ii, intjj); /** removes the edge (ij) @Parameters: gg graph ii first point jj second point @Returns: void **/ void removeEdgelnWDiG(WDiGraph *gg, int ii, int jj); /** returns a character string of the weighted digraph. used only for testing. max size of string is MAXSTRING 7poinil Ii @Parameters: gg weighted digraph str point converted to string @Returns: void **/ void wDiGraphToString(WDiGraph *gg, char *str); LIST OF REFERENCES 1. Rankin AL. Path planning and path execution software for an autonomous nonholonomic robot vehicle [master's thesis]. Gainesville (FL): University of Florida; 1993. 2. Rao NSV, Kareti S, Shi W, editors. Robot navigation in Unknown Terrains: Introductory Survey of NonHeuristic Algorithms. Oak Ridge National Laboratory; 1993. Contract No.: ORNL/TM12410. 3. Reif J, Wang H, The Complexity of the Two Dimensional Curvature Constrained Shortest Path Problem. In: Agarwal PK, Kavraki LE, Mason MT, Karaki LE, editors. Robotics: The Algorithmic Perspective: The Third Workshop on the Algorithmic foundations of Robotics, Natick (MA): A.K. Peters Ltd.; 1998. 4. Informed Consent, U.S. Nuclear Regulatory Commission. 10 C.F.R. Sect. 20.1201 (2002). 5. Chicago Operations Office. 1999. CP5 Large Scale Demonstration Project. Energy 100 Awards. Available from URL: http://www.ma.doe.gov/energy100/communit/82.html. Site last visited April 2003. 6. Strategic Alliance for Environmental Restoration. 2001. CP5 Large Scale Demonstration Project. Available from URL: http://www.netl.doe.gov/dd/projectsites/lsddp/cp5/doc/cp5 website/index.html. Site last visited April 2003. 7. Houssay L. Robotics and Radiation Hardening in the Nuclear Industry [master's thesis]. Gainesville (FL): University of Florida; 2000. 8. Dubins LE. On Curves of Minimal Length with a Constraint on Average Curvature, and the Prescribed Initial and Terminal Positions and Tangents. American Journal of Mathematics 1957; 79(3): 497516. 9. Reeds JA, Shepp RA. Optimal Paths for a Car that Goes Both Forward and Backward. Pacific Journal of Mathematics 1991; 145(2): 367393. 10. Laumond JP, Jacobs PE, Michel T, Murray RM. A Motion Planner for Non Holonomic Mobile Robots. IEEE Transactions on Robotics and Automation 1994; 10(5): 577593. 11. Lafferriere G, Sussman HJ. Motion Planning for Controllable Systems Without Drift: A Preliminary Report. Tech. Report. Busch Campus (NJ): SYSCON9004, Rutgers Center for Systems and Control; 1990. 12. Jacob G. Lyndon Discretization of Exact Motion Planning. In: European Controls Conference; 1991; Grenoble, France. p. 15071512. 13. Bessiere P, Ahuactzin JM, Talbi EG, Mazer E. The "Ariadne's Clew" Algorithm: Global Planning With Local Methods. In: IEEE Intelligent Robots and Systems Conference; 1993; Yokohama, Japan. 14. Perez TL, Wesley MA, An Algorithm for Planning Collision FreePaths Among Polyhedral Obstacles. Communications of the ACM 1979; 22(10): 560570. 15. Nilsson NJ. Problemsolving Methods in Artificial Intelligence. New York (NY): McGraw Hill; 1971. 16. Corman HC, Leiserson CE, Rivest RL, Stein C. Introduction to Algorithms. Cambridge (MA): MIT Press; 2001. 17. Bicchi A, Casalino G, Santilli C. Planning Shortest BoundedCurvature Paths for a Class of NonHolonomic Vehicles Among Obstacles. Journal of Intelligent and Robotic Systems 1996; 16: 387405. 18. Asano T, Guibas L, Hershberger J, Imai H. Visibility Polygon Search and Euclidean Shortest Paths. Proceedings of the 26th IEEE Symposium on Foundations of Computer Science; 1985 Oct 2123; Portland, OR. p. 155164. 19. Namgung I., Duffy J. Two Dimensional CollisionFree Path Planning Using Linear Parametric Curve. Journal of Robotic Systems 1997; 14(9): 659673. 20. Brooks RA, Perez TL. A subdivision Algorithm in Configuaration Space for Find Path with rotation. IEEE Transactions on Systems, Man and Cybernetics 1985; SMC15(2): 224233. 21. Zhu D, Latombe JC. New Heuristic Algorithms for Efficient Hierarchical Path Planning. IEEE Transactions on Robotics and Automation 1991; 7(1): 920. 22. Suh SH, Shin KG. A Variational Dynamic Programming Approach to RobotPath Planning with a DistanceSafety Criterion. IEEE Journal of Robotics and Automation 1988; 4(3): 334349. 23. Wit JS. Vector Pursuit Path Tracking for Autonomous Ground Vehicles [dissertation]. Gainesville (FL): University of Florida; 2000. 24. Fleury S, Sou'eres P, Laumond JP, Chatila R. Primitives for Smoothing Mobile Robot Trajectories. IEEE Transactions on Robotics and Automation 1995; 11: 441 448. 