Trajectory Planning for a Rigid Body Based on Voronoi Tessellation and Linear-Quadratic Feedback Control

Abstract: When a rigid body moves in 3-dimensional space, it is of interest to nd a trajectory such that it avoids obstacles. With this report, we create an algorithm that nds such a trajectory for a 6- DOF rigid body. For this trajectory, both the rotation and translation of the rigid body are included. This is a trajectory planning problem in the space of Euclidean transformations SE(3). Because of the complexity of this problem, we divide it into two consecutive parts where the rst part comprises translational path planning in the 3-dimensional Euclidean space and the rotational path planning. The second part comprises the design of control laws in order to follow the designed trajectory. In the rst part we create virtual spheres surrounding each obstacle in order to obtain approximate central points for each obstacle, which are then used as input for our augmented Voronoi tessellation method. We then create a graph containing all feasible paths along the faces of the convex polytopes containing the central points of the virtual spheres. A simple global graph-search algorithm is used to nd the shortest path between the nodes of the polytopes, which is then further improved by approximately 3%. Where random obstacles are uniformly distributed in a conned space. During the work process, we discovered that our translational path planning method easily could be generalized to be used in n-dimensional space. In the second part, the control law for the rigid body such that it follows the rotation and translation trajectory is designed to minimize the cost, which is done by creating a Linear-Quadratic feedback loop for a linear system. The translational control is designed in an inertial reference frame and the rotational control is designed in a body frame, rigidly attached to the rigid body's center of mass. This allows us to separate the control laws for the translational and rotational systems. The rotational system is a nonlinear system and has been linearized to be able to use the Linear-Quadratic feedback controller. All this resulted in a well performing algorithm that would nd and track a feasible trajectory as long as a feasible path exists.