Robot Motion Planning

1. Problem Description

Fig.1 The small cuboid represents a robot, trying to move from the start position in the left image to the goal position in the right image. The robot can not move outside the hidden bounding cube. 

As illustrated in Fig. 1, the robot needs to move from the start position to the goal position in the static 3D environment.

The robot is a cuboid in shape and has six degrees of freedom. The robot can float freely in the 3D environment and also rotate to any pose. It is considered to be a holonomic model. There are also many static rectangular blocks which may obstruct the robot’s ways in the environment, and the bounding cube can also be considered as an obstacle.

Finally, we need solutions of feasible paths by motion planning algorithm to guide the robot to move from different start positions to different goal positions under different obstacle environments.

2. Probabilistic Roadmap Algorithm

The probabilistic sampling-based roadmap algorithm is implemented  for solving this motion planning problem. The algorithms can be briefly divided into several steps as follows:
  • A learning phase. A probabilistic roadmap, which actually is an undirected graph, is constructed in a probabilistic way for a given scene. All the nodes in the graph correspond to a set of configurations of the robot appropriately chosen from the C-free space, and all the edges correspond to simple paths that connecting correspondent nodes. Some strategies are applied to ensure that nodes constitute a uniform distribution in the configuration space.
  • A query phase. Paths are to be found between arbitrary input start and goal configurations, based on the probabilistic roadmap. The paths can be computed based on different heuristic measures.  Dijkstra's algorithm is introduced to find the least-cost path;
More details can be found in this paper:

[1] L. E. Kavraki, P. Svestka, J. -C. Latombe and M. H. Overmars, "Probabilistic Roadmaps for Path Planning in High-Dimensional Configuration Spaces," in IEEE Transactions on Robotics and Automation, vol. 12, pp. 566-580, 1996.

3. Collision Detection


Fig.2 Separating Axis Theorem (SAT)

Given two polyhedrons, Separating Axis Theorem (SAT) can be used to judge if they intersect with each other. Obviously, if two polyhedrons do not intersect with each other, then there must exist a plane, of which each of them is located on either side. That is to say, if there exist an axis on which the projections of these two polyhedrons do not intersect with each other, then these two polyhedrons do not intersect with each other. This axis, is called separating axis.

For cuboids, we only need consider the following candidate axes: the plane normal vectors from the two cuboids and all their cross product vectors. So the total number of candidate axes is 3 + 3 + 3 * 3 = 15.

Suppose cuboid A has three sides 2*ea_1, 2*ea_2, 2*ea_3, with correspondent parallel unit vectors ua_1, ua_2, ua_3. cuboid B also has three sides 2*eb_1, 2*eb_2, 2*eb_3, with correspondent parallel unit vectors ub_1, ub_2, ub_3. The center points of cuboid A and B are C_a, C_b, separately.

For any point P_a on the surface of cuboid A, it can be represented as

P_a = C_a + ka_1*ua_1 + ka_2*ua_2 + ka_3*ua_3, with -ea_i <= ka_i <= ea_i

Similarly, for any point P_b on the surface of cuboid A, it can be represented as

P_b = C_b + kb_1*ub_1 + kb_2*ub_2 + kb_3*ub_3, with -eb_i <= kb_i <= eb_i

So for each axis s, the projection distance of P_a related to C_b is

r_a 
=   (P_a - C_a) * s
=   (ka_1*ua_1 + ka_2*ua_2 + ka_3*ua_3)  *  s
=   ka_1*(ua_1*s) + ka_2*(ua_2*s) + ka_3*(ua_3*s)
<= |ka_1*(ua_1*s) + ka_2*(ua_2*s) + ka_3*(ua_3*s)|
<= |ka_1*(ua_1*s)| + |ka_2*(ua_2*s)| + |ka_3*(ua_3*s)|
<= |ka_1|*|(ua_1*s)| + |ka_2|*|(ua_2*s)| + |ka_3|*|(ua_3*s)|
<= ea_1*|(ua_1*s)| + ea_2*|(ua_2*s)| + ea_3*|(ua_3*s)|

max(r_a) = ea_1*|(ua_1*s)| + ea_2*|(ua_2*s)| + ea_3*|(ua_3*s)|

similarly, max(r_b) = eb_1*|(ub_1*s)| + eb_2*|(ub_2*s)| + eb_3*|(ub_3*s)|

However, the projection distance of C_a and C_b is d = (C_a - C_b)*s

As a result, if |d| > max(r_a) + max(r_b), then this axis is the separating axis, these two cuboids do not intersect with each other.

4. Experiment Result and Analysis

For the experiment environment, the center of the cuboid robot can float to any position in a 100*100*100 cube. The robot can also rotate to any pose with a rotation range 0 ~ 360 degree. However, considering the symmetry of the cuboid robot, we reduce the sampling range for the rotation to 0~180 degree. About 216000 Points are sampled by uniform grid sampling for x, y, z coordinates and naive grid sampling for rotations. The roadmap graph is constructed based on the strategy of  12-connected neighborhood. As a result, it's not an exact traditional probabilistic roadmap algorithm.



Fig.3 Some experiment results. The videos are deliberately slowed down to show all the details clearly.

From the experiment results, we can know that
  • The larger the sampling density in the configuration space, the larger the possibility that the neighborhood sampled collision-free points can be directly connected without the collision to construct the roadmap.
  • How to balance the distance weight for different dimensions is also quite important. Each dimension represents different characteristic, as a result, the choice of weights actually reflect the strategy that which dimension is explored first.