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 samplingbased roadmap algorithm is implemented for solving this motion planning problem. The algorithms can be briefly divided into several steps as follows:
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 HighDimensional Configuration Spaces," in IEEE Transactions on Robotics and Automation, vol. 12, pp. 566580, 1996. 3. Collision Detection 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 12connected 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
