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
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
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
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
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.
2012. , 59 p.