Motion Planning Algorithms

Subscribe Send me a message

#planning algorithm #planning #algorithm #motion planning #robotics

Introduction

This post is a reading note of Chapter 5 and Chapter 6 of the book Planning Algorithms. Recall that in the book, the search space is the configuration space, which consists of two parts

free space
obstacle space

The obstacle space is the unreachable space in the search space. The planning problem is to find a path from the initial configuration to the target configuration.

The search space is usually of high dimensions for any practical application. High-dimensional search space can cause many issues. For example:

Local optimum: It's easy to get stuck at a local optimum.
Complicated geometry: It's impossible to visualize a space with more than three dimensions.

Therefore a significant amount of efforts are focused on dealing with these issues in various algorithms.

In this post, we will present two categories of motion planning algorithms:

Samling-based motion planning
Combinatorial motion planning

As we will see, they share common strategies. This is not unexpected. The general idea is to discretize a continuous problem so that we can convert the original search problem to a graph search algorithm.

General Idea

As mentioned earlier, the general idea is to convert the planning problem to a graph search algorithm. So what would be the graph?

Definition: Topological graph

A topological graph is a graph for which every vertex corresponds to a point in \(X\) and every edge corresponds to a continuous, injective function, \(\tau: [0,1] \rightarrow X\). The image of \(\tau\) connects the points in \(X\) that correspond to the endpoints (vertices) of the edge.

The sampling-based algorithms and combinatorial algorithms differ in the way how they build a topological graph that can be used to solve the original problem. For a sampling-based algorithm, the graph is built from randomly sampled points (vertices) in the search space. For a combinatorial algorithm, it first performs a space partition (e.g. discretization) and then uses partitions as vertices of the graph.

Idea: Roadmap

A roadmap provides a discrete representation of the continuous motion planning problem without losing any original connectivity information needed to solve it. The diagram below illustrates the idea. Suppose we want to find a path between S (start configuration) and T (target configuration) and a roadmap is available. The roadmap in this case is a connected subset of the search space, which is the black segments in the diagram. To find a path between S and T, we can first connect them to the roadmap. The anchor points on the roadmap become representatives of S and T. As points in the roadmap are connected, there is a path between the two anchor points (the red segments in the diagram). To some extent, we project the original problem to a much smaller (and connected) space.

Fig 1: roadmap

Sampling-based Motion Planning

Here is the general framework described in the book

Initialization
Vertex Selection
Local Planning
Insert an edge in the graph
Check for a solution
Return to step 2.

This framework is almost identical to a graph search program. We can borrow many ideas from graph search algorithms. For example, when we search for a path, we could do a unidirectional search, bidirectional search or multi-directional search.

As mentioned earlier, one of the issues when working with high dimensional space is how to deal with local optimum. A common technique is to introduce randomnes into the algorithm. This strategy is used in the Random Potential Fields Algorithm. The idea is simple: if we feel stuck and are not making any progress, then start a random walk. The implementation is straightforward as well: there are three modes of the algorithm

Best First
Random Walk
backtrack

We just need to specify conditions for state transition and that's it.

Another algorithm mentioned in the book is the Rapidly Exploring Dense Tree Algorithm. The core of this algorithm is to build a dense roadmap. Initially, we only have one point in the roadmap. We could then extend the roadmap by selecting a random point \(alpha(i)\), identifying the closest point on the existing roadmap \(q_n\) and adding the segment \([\alpha(i), q_n]\) to the roadmap. The figure below illustrates the process.

The result after many iterations is given as follows:

Combinatorial Motion Planning

In a sampling-based motion planning algorithm, we reduce or project the original search space to a roadmap, which is constructed randomly. It's a sampling-based algorithm because the roadmap constructed in such a way is one of the many possible roadmaps.

On the other hand, a combinatorial motion planning algorithm would go through a more formal discretization process. In fact, the core part of a combinatorial motion planning algorithm is to partition the space. Each partition is called a cell and the resulted representation is called cell decomposition. The diagram below illustrates the idea:

A cell has the following attributes

Vertices
Faces
Half-edges

The orientation of edges matters. It's used to differentiate free regions and obstacle regions. There are two common types of cell decomposition

Simplicial Complex: This defines a high-dimensional notion of a triangulation.
Singular Complex: Instead of working with \(\mathbb{R}^n\), the search space can be any manifold \(X\).

We can imagine the most important part of a combinatorial motion planning algorithm is the cell decomposition. However, it's too complicated and we will not cover it in this post. Here, we only briefly describe some important aspects.

Cell decomposition has two aspects

Space partition
Connectivity information

Recall that the space we are referring to is the configuration space. Therefore, a cell in a configuration space represents a collection of collision-free transformations. In other words, if we move around inside a cell, we would not see anything important happen. Those "important things" are called critical events. It's the critical event that draws the boundary of the cells.

The connectivity represents the "state transition". Here is an example. Suppose we have a segment robot in a tube and it can move horizontally and rotate at an positive angle. It follows that the configuration space is a 2D space. In the diagram blow, we mark the critical events. The details may not 100% accurate but the idea is that towards the right end of the tube, we could image the robot is in one of the two different states C or D. The point here is that we may be able to create the cell B and cell D when we use the critical events to partition configuration space, but this is not enough. We still need to figure out the connectivity. For example, we need to figure out some conditions so that we know we cannot move from cell B to cell D.

Collision Detection

Collision detection is computationally expensive. To reduce computation efforts, many collision detection algorithms have two pahses

Broad phase: This is used to check agents that are expected to be far from each other. Instead of using accurate virtual representations, bounding boxes are used in this phase.
Narrow phase: In this phase, individual pairs of bodies are each checked carefully for collision.

To check collision for each individual part of two entities, hierarchical methods can be used. The basic idea is to use a tree structure to represent the geometries of different parts of the robot and then the collision detection between two robotes becomes a simple recursive procedure.

----- END -----