• in contrast to the local navigation algorithms we studied in L5, now we will consider what a robot can do to get to a goal location if it does have information about the environment, such as a map
• another characterization is that the local navigation algorithms we studied are reactive, whereas the global navigation algorithms we will study here enable the robot to plan in advance
• in practice, often a combination of the two is required for reliable navigation
• map-making (mapping) and localization (where is the robot in the map?) are additional components often incorporated, we will discuss these in L8
• we will study the case of a mobile robot moving around in a planar environment
• as we developed in L3 the robot’s configuration in world frame at any time is given by the vector
• the environment may contain obstacles, which for our purposes will be regions of the plane which no part of the robot can enter
• we will assume that if there are no obstacles the robot can move directly from any starting configuration to any goal configuration
• we will ignore the fact that our robot cannot travel directly sideways (instead it has to perform e.g. a “parallel parking” motion; this is called nonholnomy)
• in practice, most of the algorithms we will cover can be adapted so that the robot can travel in straight segments
• at the beginning of each segment, the robot turns to face the endpoint of the segment
• it then drives the length of the segment
• finally it turns to face the endpoint of the next segment, or if the goal is reached, it turns to
• if there are obstacles, and this is the interesting case, and the robot has a map of the obstacles, then the robot can perform path planning (typically before it starts moving) to determine a feasible path to the goal

## Maps and Obstacles

• typically, a map will just give the geometric outline of the obstacles
• it may also include the geometry of any boundary of the robot’s workspace (which is really just another obstacle)
• usually these are given as polygons in world frame
• here a polygon can be defined as a non-self-intersecting closed chain of line segments
• curved surfaces can be approximated by chains of short segments
• if the polygon has sides then it also has vertices (corners)
• often the map will encode a polygon as a list of the coordinates of the polygon vertices, starting at an arbitrary vertex and traveling around the polygon in counterclockwise order if the robot is to remain outside the polygon, or clockwise order if the robot is to remain inside the polygon
• by this convention, if you were to walk along the edges of the polygon from vertex to vertex in the order given, then your right hand would always point away from the obstacle
• figure TBD

## Path Planning in Configuration Space

• TBD original ref
• the fact that our robot is a rectangle that can rotate can complicate path planning
• thought experiment: what if the robot was just a single point?
• then its pose could be given just as (no orientation needed)
• define the free space of a map as the set of all points in the plane which are not in collision with any obstacle
• for our imaginary point robot, path planning is “just” a matter of finding a connected path from the start to the goal such that the path is entirely within
• this assumes that the start and goal are actually in themselves, which we will consider a safe assumption
• we will see algorithms to solve this problem below
• but how can we generalize this for our actual robot, which is a rectangle that can rotate?
• first step: consider the robot as a rectangle, but fix the robot’s orientation
• in reality, this would mean the robot couldn’t turn
• but pretend that the robot could still travel in any direction, so it could go directly from any location to any other in the plane (as long as there are no obstacles in the way), it just would always be facing
• planning for this case can be reduced to planning for the point robot!
• redefine the free space by virtually expanding all the obstacles
• conceptually, for each actual obstacle , make a new virtual obstacle which is the trace of the robot reference point as the robot “slides around”
• the robot reference point is the origin of robot frame, which for our robot is the point on the ground between the two wheel contact points
• the expansion of the obstacles accounts for the robot’s boundary
• so planning in the free space of the expanded obstacles can again be accomplished by just considering the path of a single point
• in practice, can be computed relatively easily as long as is a convex polygon
• a polygon is convex if the line segment connecting any two points within it or on its boundary consists only of points also within it or on its boundary
• in that case, the following procedure can be used:
1. represent the robot shape as its own polygon
2. reflect about the robot reference point to get
3. place virtual copies of with their reference points at all the virtices of
4. take the convex hull of all the copies of
• figure TBD
• the convex hull is basically the polygon you would get by “snapping a rubber band” around all the vertices of all the copies of
• figure TBD
• the rubber band analogy does not actually help compute the convex hull, but there are good algorithms which will calculate it
• this procedure, which is called taking the Minkowsky Sum of and , will actually work for any convex polygonal obstacle and any non-rotating robot with convex polygonal outline (not just a rectangle robot)
• non-convex obstacles and/or robots can be decomposed (e.g. by a triangulation algorithm) into sets of smaller convex polygons
• then generate many , one for each pairing of decomposed polygons from the robot and obstacle
• these will generally overlap, but that’s ok; the point representing the robot for path planning purposes just needs to stay out of all of them
• figure TBD
• finally, here’s how to generalize to also allow rotations of the robot
• construct a 3D configuration space (C-space) that parameterizes the robot’s degrees of freedom
• for our robot, this could be a 3D space where the and coordinates are the corresponding parts of the robot pose and the third “ ” coordinate is
• this makes each horizontal plane effectively a snapshot of the robot in some particular orientation
• within each such plane, configuration space obstacle slices could be constructed as above (since the robot is at a fixed orientation in that plane)
• the whole (infinite) stack of these planes taken together is the configuration space, including both obstacles and (now 3D) free space
• the robot’s path can again be planned as if it were a single point in this space
• figure TBD
• in practice it can be complex to exactly compute configuration spaces, especially for robots that can rotate
• the above description is not really an algorithm because you can’t compute an infinite number of slices
• a conservative simplification can be taken: consider the robot boundary to be a virtual circle, centered at the reference point, so that the robot remains within the circle no matter its actual orientation
• in lab 2, we further simplify this by constructing a virtual square that encloses the virtual circle
• effectively, the square is just a conservative coarse polygonal approximation of the circle
• then the coordinate of configuration space doesn’t matter, and planning can be done in a planar configuration space with a non-rotating robot boundary
• the trade off is that there are some environments containing narrow gaps between obstacles that can only be planned for using a tight boundary representation for the robot
• figure TBD

## Graph Search Methods

• now we move on to consider the problem of planning the path of a point through free space
• we will restrict discussion to the planar case, but algorithms also exist for higher-dimensional configuration spaces
• a main class of algorithms takes the paradigm of graph search
• a graph consisting of vertices (points) and edges (typically straight, sometimes curved) connecting them, such that all vertices and all points on all edges are in free space, is constructed
• the start and goal configurations are also added as vertices in the graph and connected in
• we we will see several ways to make such graphs below
• then a search algorithm is applied to the graph to find a path from vertex to vertex, along graph edges, from the start to the goal vertex
• TBD figure

## Creating the Graph

• occupancy grid
• one basic way to generate the graph is to apply a grid of uniform square cells to configuration space
• extends to 3D and higher dimensional configuration spaces with (hyper-)cubic cells
• grid cells that intersect any obstacle are marked occupied, and cells which do not intersect any obstacle are marked free
• place a vertex at the center of every free grid cell
• this creates a uniform sampling of configuration space
• make graph edges between all pairs of neighboring vertices
• i.e. between pairs of vertices in adjacent free cells that share a common side
• TBD figure
• this can work, but the number of grid cells can grow large
• smaller cells enable a finer representation of the space
• the number of cells increases exponentially with the dimension of the configuration space
• instead of a regular grid with potentially a very large number of square cells, we could use irregularly shaped but potentially larger cells
• one version of this starts with an occupancy grid and then merges groups of free cells into larger free cells
• this process is repeated recursively with those larger cells up to some limit
• the result is a version of a quadtree (octree in 3D, and the idea also extends to higher dimensions)
• TBD diagram
• the graph vertices are again placed at cell centers
• this can help coalesce many small grid cells into single larger cells
• however, now it is possible that a straight graph edge connecting vertices at the centers of two cells which share a common edge may not stay fully within those cells
• so an additional edge culling step may be required
• after connecting adjacent vertices, any edge which does not stay fully within the octree free cells should be checked explicitly for collision with all C-space obstacles
• edges which do intersect any obstacle must be removed
• TBD diagram
• various other types of irregular cell decompositions can be used to partition the free space
• also other approaches to defining the graph are possible instead of just putting one vertex at the center of every cell
• TBD diagram
• Probabilistic Roadmaps and Rapidly Exploring Random Trees
• it is also possible to create vertices and edges randomly!
• these randomized approaches are actually very practical and popular
• just check that the randomly generated vertices are actually in free space
• note that this test can be implemented in some cases even without explicitly computing the shape of the C-space obstacles
• you just need code that can answer the question: “If the robot is in pose , will it collide with any obstacle in the map?”
• candidate edges can then be added either randomly or to all possible pairs of vertices
• these must also be checked to verify that they do not intersect any C-Space obstacle (if so, the edge must be discarded)
• many variations of this strategy, generally called probabilistic roadmaps (PRMs) have been developed
• TBD figure
• one common approach is the rapidly exploring random tree (RRT)
• the graph, which will take the shape of a tree (no loops) is initialized with just the start node
• other versions exist which grow trees from both the start and goal
• new vertices are randomly generated
• in many versions, sometimes (say 10% of the time) the goal node is used as the new vertex, to bias growth of the tree towards the goal
• as each new vertex is generated the vertex already in the tree closest to that new vertex is found
• the tree is then extended with a local planner from in the direction of , not necessarily all the way to it, but at least a fixed step distance
• a simple local planner could just try to connect the vertices with a straight line segment
• other local planners could implement e.g. a version of the Bug algorithm for local navigation, generating a free space path from in the general direction of that is not necessarily straight
• if the local planner is successful, meaning a new edge is found that extends the tree without colliding with any obstacle, the new edge and the vertex at its end is added to the graph
• if the tree is eventually extended all the way to (or within a given minimum distance of) the goal, a path has been found
• note that since the graph is tree-structured here (no loops), there would only be a single unique path in it from start to goal
• TBD figure
• visibility graph
• most of the above methods (except some forms of exact cell decomposition) are not complete: a path may exist that they do not find
• the probabilistic algorithms are usually set up in such a way so that the longer you allow them to run, the higher the probability they will find a path (probabilistic completeness)
• for the case of 2D environments with polygonal obstacles and a polygonal robot, one complete approach is to generate a visibility graph
• the vertices of the visibility graph are just the corners of all the C-space obstacles, plus the start and the goal vertex
• candidate edges are generated by connecting every possible pair of vertices with straight line segments
• edges which intersect any C-space obstacle are discarded
• the algorithm is deterministic and the resulting graph will contain a path from start to goal if one exists
• the resulting paths will “jump” from one obstacle to the next, and otherwise “hug” the obstacle boundaries
• TBD figure
• also, in most contexts, the shortest path in the visibility graph is actually the shortest physical path the robot could take from start to goal
• but in practice it can be more robust to try to stay as far away as possible from obstacles
• Voronoi diagram
• imagine, for the case of a 2D configuration space, that the C-space obstacles are drawn with flammable paint on a flat field of very dry grass
• then imagine that they are all ignited at the same time, so the fires spread outward from the obstacles with uniform speed
• fires spreading from individual obstacles will eventually meet
• the meeting places of all the fires would form the generalized Voronoi diagram (GVD) of the C-space obstacles
• in fact, the Voronoi diagram will form a network of edges (possibly curved) and vertices where those edges meet
• TBD diagram
• this can be transformed into a graph with the start and goal vertices hooked in
• the resulting paths will tend to stay as far away as possible from obstacles
• the “grassfire” analogy is not an algorithm to compute the GVD, but it can suggest approximate approaches based on variations of flood-filling
• there are also other algorithms which are exact in theory, though many are difficult to implement

## Checking Edges: Axis-Aligned Rect/Line Segment Intersection

• graph-based methods often require a way to check whether a graph edge intersects an obstacle
• here we will study an algorithm to check whether an edge represented as a line segment collides with an obstacle represented as an axis-aligned rectangle
• this is in a 2D setting
• TBD figure
• more general collision problems can take the form “does A collide with B” where typically
• A may be a line segment or curve
• B may be an axis-aligned rectangle, a general rectangle, a triangle, a convex polygon, or a general polygon
• Real Time Rendering by Moller et al is a good reference for intersection test algorithms, which are also important in graphics
• we can leverage the separating axis theorem: two polygons are disjoint iff their projections onto some line through the origin (the “separating axis”) are disjoint
• further, if two polygons are disjoint, there will be a separating axis perpendicular to a face of one of them
• thus it suffices to check axes parallel to all face normals; if the projections of the two polygons are not disjoint on any of them, then the polygons are not disjoint, and vice-versa
• TBD figure
• when one polygon is an AA Rect and the other a line segment, we have to check only three axes:
1. an axis parallel to , which is the normal of the top and bottom faces of the rect
2. an axis parallel to , which is the normal of the left and right faces of the rect
3. an axis perpendicular to the line segment
• TBD figure
• it is easiest to formulate this if we represent
• the rect as its center point plus a vector from to its upper right corner ( are half the width and half the height of the rect)
• the line segment as its center point plus a vector from to one of its end points (i.e. the two endpoints are )
• first translate both the rect and the segment by , so the rect is centered at the origin
• i.e. set and
• then the projection of the rect onto the axis is symmetric about the origin, and extends out from the origin
• the projection of the segment onto the axis is ; the closest this comes to the origin is
• the projections will be disjoint iff (1)
• the axis check is similar: (2)
• now rotate by to get a vector perpendicular to the segment; this boils down to
• the projection of the rect onto the axis with direction is also symmetric about the origin, and extends out at most
• are always positive, but each be positive or negative; reasoning about the four possible sign combinations shows that
• the projection of the segment onto is the point
• the distance of this point to the origin is
• thus, the projections will be disjoint iff
• which simplifies to
• writing out the dot products: (3)
• the rect and the segment are disjoint iff any of (1-3) is true; otherwise they intersect
• change the comparisons to to allow segments which are merely tangent to the rect (i.e. that just glancingly touch it) to be considered as not intersecting
• this is important in visibility graph approaches where most segment endpoints are actually on obstacle vertices
• this matlab script demonstrates the approach

## Finding the Path: Dijkstra’s Algorithm

• another key element of graph-based motion planning methods is a way to find a path from the start vertex to the goal vertex
• two simple algorithms are breadth first search and depth first search
• these approaches ignore the length of the edges, and hence will not generally find the geometrically shortest path from start to goal
• but they can be relatively easy to implement, and they will at least answer the question of if there is any path from the start to the goal
• BFS will find a path containing the fewest number of edges
• and DFS is very easy to write with a recursive implementation
• the asymptotic running time of both of these algorithms is where is the number of edges in the graph
• to actually find the shortest path including consideration of the edge lengths, Dijkstra’s Algorithm is a reasonable option in many cases
• Dijkstra’s algorithm solves the single source shortest paths problem in a graph with non-negative edge weights
• given a destination vertex , the algorithm will find a shortest path from every other vertex to
• we will use the notation to get the distance (edge weight) from vertex to
• we will also say that, once a vertex is “solved”, that is the length of a shortest path from to
• the algorithm will also calculate for all vertices the main idea is to incrementally grow a spanning tree T from
• invariant: for any vertex already in T, a shortest path from to follows the tree edges, and is the sum of the edge lengths along that path
• main idea: the tree is grown one vertex at a time by selecting a new vertex reachable in one step from some vertex in T such that is minimal; i.e. is the closest “unsolved” vertex reachable in one step from a “solved” vertex
• if multiple candidate are equally close to one or more then pick any of them
• it is easy to see that this is correct: if there were a shorter path from to by first going through some other vertex then
1. if was in T we would have picked it instead of
2. if was not in T then there must be some vertex on the way from to s.t. is a neighbor of a tree vertex; we would have picked instead of
• the parent of in the spanning tree T is , and
• Dijkstra’s original version had running time where is the number of vertices and the number of edges, since it visits every edge at least once, it solves one vertex per step, and it takes time to search for the mimimum distance new vertex at each step. This can be simplified to since .
• the asymptotic running time can be improved to amortized by using a Fibonacci heap datastructure to find the minimum distance new vertex to solve in less than time at each step
• the heap-based algorithm could be implemented like this:

``````init empty heap H of vertices sorted by dist
for each vertex v set v.visited = false, v.dist = infinity
set goal.dist = 0
add all vertices to H     # O(N) amortized with Fibonacci heap
while H not empty         # O(N)
v = extract-min H     # O(log N) amortized with Fibonacci heap
set v.visited = true
# this inner loop iterates a *total* of O(E) times
for each neighbor vertex n of v with n.visited = false and n.dist > (v.dist + dist(v,n))
set n.next = v, n.dist = v.dist + dist(v,n)
decrease-key of n in H    # O(1) amortized with Fibonacci heap``````
• afterwards, a shortest path from any vertex to the goal can be found by following the “next” references
• the heap just gives an efficient way to keep all the potential new vertices sorted in order of increasing distance
• unfortunately, there is no heap datastructure like this implemented in the Java standard libraries that implements the crucial `decrease-key` operation
• Java does offer a PriorityQueue class, which is almost what we want. A PriorityQueue efficiently sorts objects added to it so that the minimum object can always be extracted. However, Java’s implementation (probably based on a binary heap) does not allow the sort keys of already added objects to be changed. It is common to use this kind of datastructure instead of a Fibonacci heap (which is harder to implement) in Dijkstra’s algorithm. This recent paper analyzes the differences, and finds that the theoretical asymptotic running time is not as good as if a Fibonacci heap were used, but nevertheless implementations without `decrease-key` actually often perform better in practice.
• Here is one way to modify the above algorithm so that it does not require `decrease-key`, and so that it does not mutate the sort key of any object that has already been added to the priority queue:

``````init empty priority queue Q of (vertex, dist) pairs sorted by increasing dist
# the dist part of each pair is immutable; more than one pair can be created for a single vertex
for each vertex v set v.visited = false, v.dist = infinity
set goal.dist = 0
while Q not empty     # O(E) since inner loop adds a pair for every edge
(v, d) = pop Q    # O(log E) with binary heap (Java PriorityQueue)
if not v.visited
set v.visited = true
# this inner loop iterates a *total* of O(E) times
for each neighbor vertex n of v with n.visited = false and n.dist > (v.dist + dist(v,n))
set n.next = v, n.dist = v.dist + dist(v,n)
add make-pair(n, n.dist) to Q    # O(log E) with binary heap (Java PriorityQueue)``````
• The asymptotic running time increases to in this implementation.