#### 13.3.3. Motion Planning for Nonholonomic Mobile Robots

This video introduces shortest paths for forward-only cars (“Dubins curves”) and for cars with a reverse gear (“Reeds-Shepp curves”). It also shows how Reeds-Shepp curves can be used for motion planning among obstacles.

Most path planners in Chapter 10 can be applied to omnidirectional mobile robots, because of their ability to move in any direction. The same is not true for nonholonomic mobile robots, due to their motion constraints.

In this video we'll look at optimal motion plans for car-like robots in an obstacle-free plane, as well as motion planning among obstacles.

Let's start with a car with no reverse gear. A typical path looks like this. Our goal is to find paths that minimize the length of the curve followed by the point midway between the rear wheels. Let C represent a circular arc that the car follows when it turns at its minimum turning radius, either to the right or to the left. And let C greater than pi represent such arcs that travel an angle of at least pi. Finally, let S represent the straight-ahead motion of the car.

Then it can be shown that all shortest paths between two configurations are either of the form C, S, C, or C, C greater than pi, C, where any of the C or S segments could be of length zero. These are called Dubins curves in honor of the mathematician who proved this result.

Here are two examples. In the first animation, the shortest path to the goal is a CSC path. In the second animation, the shortest path has the form C, C greater than pi, C.

Now let's consider a car with a reverse gear. A result due to Reeds and Shepp says that all shortest paths belong to one of nine classes of paths, consisting of circular segments at the minimum turning radius, straight-line segments, and direction reversals, also called cusps. The details of the nine classes are in the book.

Here are examples from three of the nine path classes. The first shortest path is a CSC path. The second path reverses the car's orientation using two cusps. The third shortest path has a single cusp.

Dubins curves and Reeds-Shepp curves allow us to consider only a finite number of possible paths when planning the shortest path between two configurations in an obstacle-free plane. Reeds-Shepp curves can also be useful in motion planning for a car among obstacles. Given the start and goal configurations, first we can try connecting them by a Reeds-Sheep curve. If the path is in collision, then we can plan a free path between the two configurations using any path planner, ignoring the car's motion constraints. Provided this path does not graze any obstacle, then, because the car is small-time locally controllable everywhere, even though the car cannot follow the path exactly, it can follow it arbitrarily closely.

To transform this infeasible path to a feasible path, first we can divide the path in half and try using Reeds-Shepp curves to connect q-zero to q-one-half, and q-one-half to q-one. The Reeds-Shepp path from q-one-half to q-one is collision-free, but the Reeds-Shepp path from q-zero to q-one-half is not. So we subdivide the first path segment again and find the Reeds-Shepp paths between q-zero and q-one-quarter and between q-one-quarter and q-one-half. These paths are both collision-free, so we have our final path. This subdivision process is guaranteed to converge to a collision-free path because: one, the original path has some free space around it; two, the car is small-time locally controllable, so it can follow the original path arbitrarily closely; and three, the Reeds-Shepp paths are short, so the distance from the original path goes to zero as the distance between the subdivision points goes to zero.

Once we have a path for the robot, we can convert it to a trajectory by applying a time scaling subject to the robot's velocity limits.

For a diff-drive robot, the shortest path in the obstacle-free plane for the point midway between the two wheels is trivial: spin in place, translate, then spin in place. A more interesting problem is to find the time-optimal motion if each wheel's speed is limited. This problem is discussed in the book. All of the optimal motion results discussed in this video can be derived using techniques from optimal control theory.