$\newcommand{\V}{\mathbf #1}$ $\newcommand{\M} {#1}$ $\newcommand{\Reals} { \mathbb{R} }$

# Chapter 9. Motion Planning in Simple Geometric Spaces¶

We will introduce the key issues involved in motion planning in a simple, illustrative planar path planning setting. Here, the robot consists only of a point, moving in 2D in an environment with polygonal obstacles. Later we will extend this to handle robots with geometry that can translate and rotate. Planar path planning is useful for a number of applications involving mobile ground robots, autonomous boats, and defining waypoints for aerial vehicles. It is also useful for providing coarse guidance paths for legged robots and mobile manipulators.

In motion planning, it is impossible for an algorithm to consider the continuous infinity of paths in the free space. So, the main question to ask is how can an algorithm get away with considering a finite number of alternatives when the true number of alternatives is infinite? What must be sacrificed?

In the 2D context, we have the relative luxury of having a polygonal representation of C-obstacles (but not necessarily the free space), which provides much richer geometric information than in the high-dimensional problems we will consider in the next section. We will consider three classes of algorithm: bug algorithms, roadmap planners, and cell decomposition planners.

Bug algorithms are interesting because the robot only needs local information about the shape of obstacles, and a small amount of memory, to achieve the goal. They can therefore be executed on simple robots without knowledge of all of the obstacles in the space. However, they can be extremely inefficient. Roadmap planners build a network of paths in free space. Once a network is constructed, a graph search algorithm can be invoked to yield a solution path. Cell decomposition planners also build a network, but a network of adjacent regions in free space. In both roadmap and cell decomposition planners, the manner in which the graph is constructed dictates the performance, completeness, and solution quality of the algorithm.

These planners can largely be extended to robots that contain some geometry and can translate and rotate on the plane. We will describe some extensions of these techniques to generate smooth motions, which are applicable to car-like mobile robots and aerial vehicles that cannot rotate in place.

## Bug algorithms¶

Bug algorithms are simple, clever planning methods that do not actually need a map of obstacles to work! The only pieces of information needed on-board the robot are 1) a way to determine the robot's current location $X$ and goal location $G$, 2) a contact sensor, and 3) a bit of memory. By studying how the robot would behave in certain maps, this give us a sense of how to analyze the performance of planners.

The inspiration for bug algorithms comes from the right-hand rule_ for solving mazes. If you are in a maze, a sure-fire way not to get lost is to find a wall, face a direction so that the wall is on your right, and walk along the wall while keeping it immediately to your right. By following this process, only two possibilities may result: 1) you exit the maze, or 2) you return to the point at which you first touched the wall. If you return to the starting point, the wall makes a closed circuit, so the next step should be to find a new portion of wall that you did not touch, and apply the right-hand rule again for that new wall. Using this approach, you are guaranteed to explore the entire maze. (There is no significance to right vs left; you can just as easily apply a left-hand rule.)

The first method we will study is called Bug 1. Unlike the case of a maze, in planning we know which direction we would need to take to reach the goal. So, Bug 1 proceeds as follows:

1. Move in a straight line path toward $G$.

2. If the robot reaches $G$, we are done.

3. If the robot hits an obstacle, then follow the right-hand rule and traverse its boundary

4. This continues until the wall no longer is pointing against the straight line from $X$ toward $G$. Once this occurs, continue from Step 1.

It is important to note that in Step 4, this determination is done only locally. It does not examine the overall shape of the obstacle to see if the straight line does not overlap the obstacle, only locally whether taking the direction $G-X$ leads the robot into free space. Figure 1. The simple Bug 1 algorithm that simply 1) heads toward the goal, 2) turns left when it hits an obstacle, and proceeds to hug the wall, and 3) continues towards the goal whenever it can.

The second method is called Bug 2, and is a little more complex. Here, we shall add some memory to the robot.

1. Move in a straight line path toward $G$.

2. If the robot reaches $G$, we are done.

3. If the robot hits an obstacle, remember the point $P$ at which it is hit. Follow the right hand-rule.

4. If the robot crosses the line $\overline{PG}$ as it walks along the wall, determine whether $\| X-G \| < \|P-G\|$ and the wall is not pointing against the straight line from $X$ toward $G$. If both conditions are satisfied, continue from step 1. Otherwise, keep walking.

5. If $P$ is reached again, return "No path" Figure 2. The Bug 2 algorithm that remembers the line from the start to the goal, and circumnavigates an obstacle until it meets the line again at a closer point to the goal.

Bug 1 might seem at first to be a little more efficient than Bug 2, because it will opportunistically move in a straight line toward the goal whenever it can break away from the obstacle. Bug 2 has to hug the boundary of an obstacle until it reaches the line $\overline{PG}$ again. However, the problem in Bug 1 is that it may fall into an infinite loop, as shown in Fig. 3. Here, the obstacle is set up so that the robot breaks away from the obstacle, only to hit it again. Figure 3. A "bugtrap" environment designed so that Bug 1 falls into an infinite loop, while Bug 2 succeeds.

By contrast, Bug 2 delays breaking away from the obstacle, which traverses a longer length of the obstacle boundary. In this example, the breakaway point is chosen more judiciously to avoid the infinite loop. But this is only an anecdote; can we prove that Bug 2 will always succeed (i.e., is a complete planner)? It turns out that we can.

To prove completeness of Bug 2, we shall need to establish 3 facts:

A. For each obstacle, Bug 2 will break away from it at some point if there is a path to the goal.

B. The sequence of hit points decreases in distance to the goal.

C. If there is no path to the goal, then Bug 2 will terminate correctly with failure.

Let $O$ be a connected, bounded, planar obstacle, and $P$ any location at which Bug 2 hits it. Now consider the curve $\partial O$ traversed by the right hand rule. This curve will be the boundary of $O$ if $O$ contains zero holes, but if $O$ contains holes then it will be some connected component of its boundary. $\partial O$ is a closed curve dividing the plane into two connected components, and a fact of plane geometry is that a closed curve $C$ separates two points $A \notin C$ and $B \notin C$ iff $\overline{AB}$ intersects $C$ in an odd number of points. Let $P^\prime$ be the point immediately preceeding $P$ on the robot's path. We know that $P^\prime \notin O$, and $\|P^\prime - G \| > \|P-G\|$, because Bug 2 only moves toward the goal, never away from it. So, if there is a path to the goal, then the line $\overline{P^\prime G}$ intersects $\partial O$ an even number of times. Hence, there is at least one other intersection point other than $P$. Let $C$ be the closest intersection point. The line segment between $C$ and $G$ will be pointing away from $O$, hence, we know that $C\neq P$ since the line from $P$ to $G$ points into $O$. Hence, the robot must break away at from $\partial O$ at some closer point. This establishes fact A.

Second, let $L$ be the point at which the robot leaves obstacle $O$. If the robot hits another obstacle, it will have traveled in a straight line from $L$ to $G$, so its next hit point $P^\prime$ is closer to $G$ than $L$. Since $\|P - G \| > \| L - G \| > \|P^\prime - G\|$, the sequence of hit points are decreasing in distance, establishing fact B.

Finally, we need to establish that termination at Step 5 correctly determines that no path exists. As established in B, the sequence of hit points has decreasing distance, so an infinite loop is not possible: Bug 2 must either find the goal or terminate with "No path". As established in A, "No path" will not be returned if there is a path to the goal. By contradiction, returning "No path" implies there is no path to the goal. QED.

## Roadmap planners¶

Roadmap planners are able to achieve higher quality paths than bug algorithms, but require advance knowledge of all obstacles. They operate by computing a graph $G$ where vertices indicate free configurations and edges indicate collision-free paths (usually straight lines). Then, graph search is performed on this structure to produce a free path. See Appendix C.1 for more information about graph search algorithms.

A straightforward method for planning paths in 2D is grid search. Let us define the graph $G$ such that all vertices lie on a grid, and all edges are vertical or horizontal and connect subsequent vertices. Let us assume that $s$ and $g$ lie directly on grid vertices. If we remove all vertices from the grid that are contained within obstacles, and remove edges that intersect obstacles, then a search on the remaining graph will yield a path from the start to the goal (Fig. 4). Figure 4. Searching along a grid is a resolution complete method.

If the grid has $m$ divisions in the $x$ direction and $n$ divisions in the $y$ direction, then there are at most $mn$ vertices in the grid. As discussed in Appendix C.1, the running time of Dijkstra's algorithm will be $O(mn \log mn)$.

There are two main issues with grid search. First, if the obstacles are constructed to contain narrow passages smaller than the resolution of the grid, then it is unlikely for the graph to contain a feasible path even if one exists in the continuous space (unless we are lucky and the narrow passage lines up with the grid edges). The solution to this problem is to increase the resolution of the grid until it is sufficiently fine to find a path through the narrow passage, as shown below. {#fig:NarrowPassageGrid

Figure 5. (Left) A grid search is very likely to fail if the robot needs to pass through a narrow passage smaller than the grid resolution. (Middle) The clearance of a path is the minimum distance to obstacles of any point on the path. (Right) If there exists a path with clearance at least $d \geq h/\sqrt{2}$ away from obstacles, then a grid planner with resolution $h$ will find a path.

To make this more rigorous, let us denote the clearance of a point as the distance to the nearest obstacle, and the clearance of a path to be the minimum clearance of all points along the path. It can be easily shown that if there exists in the free space a path with clearance at least $h / \sqrt 2$, then search on a grid with resolution $h$ will find a feasible path. Hence, grid search is resolution-complete.

The second issue is that the grid does not contain shortest paths in the original space. Any diagonal movement can only be approximated by a series of axis-aligned movements, which is suboptimal regardless of the resolution of the grid. To partially mitigate this suboptimality, four $45^\circ$ diagonal edges between grid cells may be added with cost $h \sqrt 2$. For an even better result, the 8 diagonal edges passing two cells on each axis (moving in directions (2,1) (1,2), (-1,2), etc.) can be added to the graph. The drawback of adding additional edges is that more edges must be checked for collision when constructing the graph. Figure 6. Searching with 8-connected grid points allows the planner to compute slightly shorter paths.

Techniques that address these problems include the Any-Angle A* and the Fast Marching Method (FMM). Both of these methods perform the propagation of distances to a vertex's neighbor not only using the cost of the path along the edge, but also via off-edge paths. Any-Angle A* maintains information at each vertex about the source of straight line cost accumulation, while FMM approximates path costs via interpolation of the costs of any other explored vertices in adjoining grid cells. A complete description of these methods is beyond the scope of this book, but suffice to say the approximation quality of both methods improves with finer grids.

### Visibility graph¶ Figure 7. Illustrating the visibility graph method. First, edges from each vertex to every other vertex are added if collision free. Next, the visibility graph is completed. Finally, a shortest path is produced via graph search.

The visibility graph method was one of the earliest motion planning algorithms and is also notable because it is a complete, optimal algorithm for polygonal obstacles. The idea of the visibility graph is to construct a graph whose vertices are the start, goal, and all obstacle vertices, and the edges consist of all possible collision-free line segments and obstacle edges. As a result, the solution paths will pass directly through obstacle corners. Pseudocode for the basic visibility graph method is given below.

**Algorithm Visibility-Graph**

1. Let $V \gets \{ s, g \}$ and all obstacle vertices.
2. $E \gets \{ \}$.
3. for all pairs of distinct vertices $u, v \in V$
4.    if $\overline{uv}$ is an obstacle edge
5.        Add $(u,v)$ to $E$.
6.    else if $\overline{uv}$ is collision free
7.        Add $(u,v)$ to $E$.
8. Search $G = (V,E)$, with Cartesian distance as the edge cost, to connect $s$ and $g$.
9. return the path if one is found.

The number of iterations of Line 3 is O($n^2$) where $n$ is the number of obstacle vertices. Line 6 requires collision checking an edge with all other obstacle edges, which is an O($n$) operation. Overall, this brute-force construction visibility graph is constructed in O($n^3$) time. Proving the correctness and optimality of this planner is left as an exercise.

Because $n$ may be quite large, e.g., in the hundreds or thousands, it may be fruitful to reduce the computational complexity of this algorithm. First, we can observe that if two convex obstacles have many intermediate vertices, most of the possible edges between them cannot be a part of an optimal path. Only the edges connecting obstacle tangents --- that is, an edge such that the prior and next obstacle vertex are on the same side of the edge --- need be checked for collision. Second, we can check for collisions between related edges in a more efficient way.

One method for doing so is known as the rotational sweep. The rotational sweep methods collision checks all outgoing edges from a given vertex $u$ in $O(n \log n)$ time by tracking the closest obstacle edge as a ray centered at $u$ sweeps around the full circle. The procedure is as follows:

1. Sort all vertices around $u$ in order of increasing rotational angle. Ties are broken by closest distance. $O(n \log n)$ time.

2. For the vertex $v$ with minimum angle, perform a collision check with the infinite ray from $u$ toward $v$. Order all of the edges $I = (e_1,\ldots,e_k)$ intersected by the ray by order of increasing distance. If $v$ is on the closest edge, add the edge $(u,v)$ to $E$. $O(n + log n)$ time.

3. Proceed to the next vertex $w$ in the sorted list. If $w$ is the end of edge $e_1$, add $(u,w)$ to $E$ and remove $e_1$ from $I$. If it ends any other edge $e_k \in I$, remove it from $I$. If $w$ is the start of any edge $e \notin I$, then add it to $I$ in order of intersection distance. With binary search to identify edges in $I$, this takes $O(\log n)$ time.

4. Repeat Step 3 until all vertices are visited.

## Cell decomposition¶

The class of cell decomposition methods differs from the above techniques in that the graph does not characterize a network of configurations, but rather the connectivity between free-space regions that meet at their boundary. So, once a cell decomposition is created, we can identify the cells containing the start and goal configurations and then search the graph to find a sequence of cells that contains a feasible path.

One of the simplest cell-decomposition methods is the trapezoidal decomposition. This method divides free-space into trapezoids by imagining vertical upper and lower edges emanating from each obstacle vertex. The remaining cells are trapezoids and triangles, which are degenerate trapezoids with one edge of length zero. Each cell that borders another with a vertical line can be connected together with an edge in the graph. Because each cell is convex, any line connecting the left and right edges of the cell is also in free space.

To create a trapezoidal decomposition, we sweep a vertical line from left to right and output cells incrementally. First we order all of the obstacle vertices in increasing $x$ coordinate. The upper and lower edges of the cell to the left of the leftmost obstacle vertex are infinite. By beginning an obstacle, vertex will then split this cell into two. We output the leftward cell, and then maintain the lower and upper edges of these two cells in a list $L$ sorted in increasing $y$ coordinate.

Proceeding to the next vertex, one of three things may happen. 1) The next vertex may end two of the edges in $L$, causing two cells to merge, 2) the next vertex may end one of the edges in $L$, simply continuing on to another cell, or 3) the next vertex may split an existing cell. In case 1, we output the two cells above and below the ended edges, and remove the edges from $L$. In case 2, we output one cell either above or below the ended edge, remove that edge from $L$ and then insert the new edge in the same place as the old one. In case 3, we identify the location in $L$ into which the two new edges should be inserted. A cell is outputted, bounded by the two edges surrounding the insertion position, and the two new edges are inserted there.

This repeats until all vertices are processed. To maintain the connectivity of the cell decomposition, we can remember the rightmost cell added above or below each edge, and when a new cell is added it is attached to the previous cell associated with that edge, and the rightmost cell is changed to the new cell.

The trapezoidal decomposition as stated is complete but not optimal. However, simple modifications of the vertical coordinates --straightening the path when possible --- can be made to find the shortest path passing through the cells produced by the algorithm.

## Extensions to planar robots with geometry¶

Now let us consider the robot to have some geometry, and allow the robot to translate and rotate in the plane. We will show that if we were to convert the workspace obstacles into C-space obstacles, then we can directly apply the techniques for point robots described above! This forms the basis of most planar robot motion planning techniques. This section will also describe how to handle rotations and more realistic motion constraints.

### C-space obstacles¶

For each obstacle $O$ in the workspace, there exists a corresponding set $\mathcal{C}O$ called the C-obstacle that contains all configurations in which the obstacle collides with the robot. In other words, $$\mathcal{C}O = \{ q \in \mathcal{C} \quad |\quad R(q)\cap O \neq \emptyset \}.$$ The forbidden region can then be stated as the union of all C-obstacles: $$\mathcal{C} \setminus \mathcal{F} = \bigcup_{O \in E} \mathcal{C}O.$$ But can each $\mathcal{C}O$ be computed? Certainly this may be a nontrivial problem, particularly when $dim(\mathcal{C})$ is large, because it is not even clear how to represent a high dimensional set. We show that in some cases, we can compute C-obstacles, and in others, it is worth leaving a C-obstacle purely as an element of thought.

### Planar translating disks¶

For point-robots, determining a C-obstacle is trivial. If $\mathcal{C} = \mathbb{R}^2$, and the space occupied by the robot $R(q)$ is simply the point $\{ q \}$, then the C-space obstacle $\mathcal{C}O$ corresponding to an obstacle $O \subseteq \mathbb{R}^2$ is simply $\mathcal{C}O = O$ itself. Given obstacles $O_1,\ldots,O_n$, the forbidden region is then simply the union $\bigcup_{i=1}^n O_i$. If the obstacles overlap, we may calculate a representation of the boundary of the union using constructive solid geometry (CSG) operations.

Let us now consider the case in which the robot is a translating disk with radius $r$. The configuration $q$ denotes the translation of the robot with respect to some reference frame, and without loss of generality we choose this to have its origin at the center of the disk. Given some obstacle $O$ in workspace, the C-space obstacle $\mathcal{C}O$ is the set of points such that if the center of the disk is placed at that point, then the disk overlaps the obstacle. This is equivalent to the set of points within radius $r$ of the obstacle: $$\mathcal{C}O = \{ q \quad |\quad D(q,r) \cap O \neq \emptyset \| \{ q \quad |\quad dist(q,O) \leq r \}$$ where $D(q,r) \subseteq \mathbb{R}^2$ is the disk centered at $q$ with radius $r$. In other words, it is a uniformly "fattened" version of the workspace obstacle. Figure 8. (a) For a disk robot and polygonal workspace obstacles, (b) the corresponding C-obstacles are polygons "fattened" by the robot's radius $r$. (Top) If the obstacle is convex, the C-obstacle is determined by tracing arcs at every vertex. (Bottom) If the obstacle is non-convex, the C-obstacle may contain holes.

If $O$ is a convex polygon, then the boundary of the C-obstacle consists of a series of circular arcs and straight lines (Fig. 8). To construct the boundary, we may start with any vertex $u$ of the polygon, and place a point offset perpendicularly to its outgoing edge $\overline{uv}$. Tracing a straight line parallel to that edge until it is perpendicular to the next vertex $v$, we construct one edge of the C-obstacle. The next segment is a circular arc of radius $r$, swept about $v$ until it hits the point perpendicular to $v$'s outgoing edge. This continues around the boundary of the polygon until the process returns to $u$.

In the non-convex obstacle case, the boundary is not so straightforward to compute, because 1) for concave obstacle vertices, the straight edges meet prematurely creating a concave vertex of the C-obstacle, and 2) the boundary created by walking along the polygon in this manner may self-intersect and even create inner holes. To properly handle the case where holes exist requires some CSG-like techniques.

To plan in the presence of C-obstacles with arcs, you could discretize the arcs into small polyhedral segments and use the techniques described above for polygons. Or, you could modify the algorithms described above to handle arcs directly. For example, a visibility graph with arcs would construct graph segments consisting of 1) straight lines along obstacle boundaries, 2) arcs along obstacle boundaries, or 3) straight lines that are tangent to two arcs.

### Minkowski sums and differences¶

We can generalize the construction of C-obstacles to general translating robots using the notion of a Minkowski difference. The Minkowski sum $\oplus$ is a generalization of addition to be defined on sets of points. If $A$ and $B$ are sets, then $A \oplus B$ is defined as the set: $$A \oplus B \equiv \{ x + y \quad |\quad x\in A \text{ and }y\in B\}.$$ Similarly, the Minkowski difference $\ominus$ is defined as $$A \ominus B \equiv \{ x - y \quad |\quad x\in A \text{ and }y\in B\}.$$

If the reference frame of the robot is a shape $R_0$, then the volume occupied after translation $q$ is the set $$R(q) = \{ x \quad |\quad x-q \in R_0 \}$$ If $P$ is a workspace obstacle, the the condition that $R(q) \cap O \neq \emptyset$ requires that there exists some $x \in \mathbb{R}^2$ such that $x \in R(q)$ and $x \in O$. This is equivalent to the condition that: $$q \in O \ominus R_0.$$ To see this, observe that if $q \in O \ominus R_0$, then there exists $x \in O$ and $y \in R_0$ such that $q = x - y$. In other words, $y = x-q \in R_0$ implies that $x \in R(q)$. As a result, constructing the C-obstacle is equivalent to compute the Minkowski difference of the object minus the robot's reference shape.

As an example, consider a convex polygonal robot with reference $A=(a_1,\ldots,a_m)$ and a convex polygonal obstacle $B = (b_1,\ldots,b_n)$. The Minkowki sums which can be obtained by 1) flipping $A$ about the $x$ and $y$ axes to obtain $-A$, 2) tracing the flipped shape about the edges of $B$, and 3) determining the outer edges of the traced paths.

In the convex case, the C-obstacle can be shown to be convex with at most $m+n$ edges. One brute-force way to compute it is to calculate all extrema $a_i + b_j$, $i=1,\ldots,m$ and $j=1,\ldots,n$, and then computing the convex hull. With an appropriate convex hull algorithm, this can take $O(mn \log mn)$ time. There is however an optimal $O(m + n)$ algorithm that constructs the Minkowski sum. Briefly, this technique marches about the edges of each polygon and outputs an extremal vertex for each vertex of either polygon encountered in CCW order. In the general non-convex case, however, the Minkowski sum may have $O((mn)^2)$ complexity, and may have holes.

### C-obstacles for rotational motion¶

C-obstacles for rotational motion are much more complex. Consider, for example, a translating and rotating convex polygon with polygonal obstacles. The C-space is SE(2) and consists of configurations $q=(x,y,\theta)$. Imagine it as a volume with $\theta \in [0,2\pi)$ the vertical axis.

Then, for any fixed rotation $\theta$, if we were to consider only the translation components of the configuration, this would be a horizontal slice through configuration space. The slice of C-obstacle at height $\theta$ can be determined by the Minkowski difference of the obstacle and the robot reference polygon rotated by angle $\theta$: $$\{ (x,y) \quad |\quad (x,y,\theta) \in \mathcal{C}O \} = O \ominus R(0,0,\theta)$$ where $R(0,0,\theta)$ is the rotated robot reference polygon. This is a convex polygon.

However, as we slide $\theta$ slightly, the vertices of each slice rotate along a helix. At some points along the range $[0,2\pi)$, these helical paths will merge or split as different combinations of obstacle and robot vertices enter and leave the boundary of the Minkowski difference. This causes the C-obstacle to become a relatively complex shape.

### Motion planning in SE(2)¶

Although some geometric techniques have been developed for planning in SE(2), it is more common to adopt resolution-complete techniques to handle the rotational DOF. As an example, grid search can be applied fairly easily.

To apply grid search, the $\theta$ dimension is split into several divisions on a grid over $[0,2\pi)$, in addition to the $x$ and $y$ dimensions. This gives a 3D grid graph that can be handled via search. To properly handle the topology of SO(2), vertices just below $\theta=2\pi$ are connected to the ones at $\theta=0$. Specifically, suppose we have constructed a grid $(x_i,y_j,\theta_k)$ with indices $i=1,\ldots,m$, $j=1,\ldots,n$, and $k=1,\ldots,p$. We have $x_i = (i-1)\Delta x$, $y_j=(j-1)\Delta y$, and $\theta_k= (k-1)\Delta \theta$, with $k=2\pi/p$. Then, for each $i=1,\ldots,m$, $j=1,\ldots,n$ and $k=1,\ldots,p$, the graph has an edge from each $(x_i,y_j,\theta_k)$ to $(x_{i+1},y_j,\theta_k)$ and $(x_i,y_{j+1},\theta_k)$ (as long as that incremented index does not exceed the limit), and $(x_i,y_j,\theta_{k+1\mod p})$. This leads to at most 6 connections per vertex. Diagonal motions can also be added, which results in 26-connected vertices (each vertex can be thought of being in the center of a $3\times 3\times 3$ box).

Note that when applying search, a cost function needs to be assigned to each edge. Because translation is measured in different units (say, meters) than orientation (degrees or radians), it may not make much sense to use Euclidean distance as the cost! As an extreme example, let us assume that orientation is measured in degrees and translation in meters, and suppose the robot would need to turn $90^\circ$ to pass through a narrow gap, then rotate back $90^\circ$ to reach a goal 1 m away. A planner using Euclidean distance would prefer to take a roundabout path (up to 180 m longer!) to get to the goal without reorientation. At a different extreme, let us assume that orientation is measured in radians and translation in millimeters. Then, the robot would prefer to pass through the gap even when there exists a slightly longer path that does not require reorientation.

As a result it is usually prudent to adopt a weighted metric as cost, such as the following: $$c((x,y,\theta),(x^\prime,y^\prime,\theta^\prime)) = \sqrt{(x-x^\prime)^2+(y-y^\prime)^2+w_\theta diff(\theta,\theta^\prime)^2}.$$ Here, $diff$ is the geodesic angular distance in SO(2), and the weight term $w_\theta$ should be chosen to balance the tradeoff between translation and rotation. One reasonable choice is to set $w_\theta = R^2$ where $R$ is the radius of the robot. For axis-aligned motions, this weighted cost bounds the maximum distance that a point on the robot could move during a straight line motion $(x,y,\theta)\rightarrow (x^\prime,y^\prime,\theta^\prime)$.

Cell decomposition can also be extended in a resolution-complete fashion to SE(2) by splitting $\theta$ into discrete values, and thinking of the $(x,y)$ plane as a "layer" in 3D space. The C-obstacle for that layer can be computed using Minkowski sums, and then its cell decomposition computed. Then, an edge is added to connect cells on adjacent layers whose $(x,y)$ projections overlap. This layered cell graph can then be searched to connect any two points in SE(2).

### Path planning with curvature constraints¶

We have not yet described how a path could be followed. Supposing the robot was reasonably shaped like a disk, and could move forward and turn in place (e.g., a mobile robot with differential drive such as the iRobot Roomba, or a skid steer) any 2D polygonal path could be followed by a sequence of forward motions and turns. This strategy could also be followed by humanoid robots performing indoor navigation. If the robot were not disk-shaped, but could still move forward and turn in place, we could perform planning in SE(2) and follow the path just as easily.

However, for car-like ground vehicles and aerial vehicles, turning in place is not an option. The limited turning radius of the vehicle means that at maximum steering angle, the path follows a circular arc. No path of smaller turning radius is permitted (a vertex of a polygonal path can be thought of as a circular arc of radius 0). This places a constraint on the maximum curvature of the path. There are three general approaches for handling such constraints:

• Expand C-obstacles by the turning radius, and plan a path. This is only appropriate if the turning radius is small compared to the size of narrow passages between obstacles.

• Plan paths with straight line segments connected by minimum-radius arcs. The Reeds-Shepp curves can beextended to handle obstacle avoidance in some simple cases.

• Define a set of curvature-constrained primitive maneuvers, and perform search.

Here we will expand on the latter approach, since it is quite commonly used for autonomous vehicles and can also be adapted to handle lane following and other dynamic constraints. The state lattice approach defines a search graph on an $(x,y\theta)$ grid in which each connection corresponds to a primitive maneuver guaranteed to satisfy the curvature constraint. Each maneuver may move more than one unit along each axis. Usually, only a few values of $\theta$ (4 or 8) are allowed to take advantage of rotational symmetries in the $(x,y)$ grid. Figure 9. A state lattice for a curvature constrained vehicle, with 4 possible orientation values. Shown are the reachable states after one, two, and three primitives.

Fig. 9 illustrates this concept. From an initial state pointing upward, five primitive maneuvers are allowed. These are chosen so that curvature constraints are respected and no more than 1 grid cell is moved in any direction. After executing one maneuver, the vehicle can perform the same five maneuvers, properly translated and rotated to the vehicle's new position. This can be executed as a search on a grid $(x_i,y_j,\theta_k)$ where the orientation $\theta_k$ can take on the values $\{0^\circ,90^\circ,180^\circ,270^\circ\}$. A primitive $p$ is stored relative to the origin (where the vehicle points rightward) as $(\Delta x, \Delta y, \Delta \theta)$, and the application of the primitive in state $(x,y,\theta)$ yields a new state: \begin{aligned} (&x+\cos \theta \Delta x - \sin \theta \Delta y,\\ &y+\sin \theta \Delta x + \cos \theta \Delta y,\\ & (\theta + \Delta \theta) \mod 360^\circ). \end{aligned} This is then used as the successor function for a search like Dijkstra's or A*.

It should be noted that due to the coarse discretization of the orientation variable, to move diagonally the vehicle must swerve frequently. It is possible to extend this approach to more finely discretized orientations, but care must be taken to define primitive sets that transfer properly between integer cell coordinates for non-axis aligned orientation values. See the figure below for an illustration. Figure 10. A state lattice for a curvature constrained vehicle, with 8 possible orientation values. Shown are the axis-aligned primitive set (left) and the non-axis aligned set (right).

## Exercises¶

1. If all obstacles are convex and non-overlapping, is the Bug 1 complete? If so, prove it. If not, produce a counterexample.

2. Suppose that the bug had a $360^\circ$ distance sensor with some maximum range $R$ to detect the shape of obstacles around it. Specifically, suppose that it knows the shape of all obstacles in a circle of radius $R$. Could you modify Bug 1 so that the bug can reach the goal with a shorter path (when it terminates)? Bug 1 should be equivalent to your new algorithm when $R=0$. Prove that your method produces paths of length no longer than paths produced by Bug 1.

3. Prove that the minimum length path in a polygonal environment is either a straight line or passes through obstacle vertices. Then prove that the visibility graph method yields a path with minimum length. Hint: consider whether a locally suboptimal path can be improved by "shortcutting" one of the corners of the path.

4. Is grid search with 4-connected neighbors in a plane approximately optimal, provided that clearance is sufficiently large, and the length of an optimal path is greater than the grid resolution? If so, derive an approximation factor. If not, show a class of environments in which an arbitrarily poor approximation factor can be constructed.

5. Give pseudocode for computing the Minkowski sum of two convex polygons in O($m+n$) time.

6. What is the configuration space of two disk-shaped mobile robots that can translate in the plane? What C-space obstacles exist in this space, assuming they are not allowed to collide with the environment or each other? Extend your description of the C-space and C-space obstacles to $n$ robots.

7. Suppose $R$ is the maximum radius of the robot with respect to its reference point. Produce a cost metric for SE(2) that bounds the distance traveled by any point on the robot as it moves from $(x,y,\theta)\rightarrow (x^\prime,y^\prime,\theta^\prime)$ in a straight-line configuration-space motion.