Safe Vehicle Motion Planning Using Constraint Admissible Positive Invariant Sets on SE(3)

This letter extends the application of the invariant set motion planner (ISMP) to space vehicles operating in $\mathbb {SE}\text {(}3\text {)} = \mathbb {SO}\text {(}3\text {)} \rtimes {\mathbb {R}}^{3}$ , considering the quaternion representation of $\mathbb {SO}\text {(}3\text {)}$ . We provide a proof for a collision-free set by extending the concepts of configuration-space bubbles from robotics literature. We derive a constraint admissible positive invariant (CAPI) subset within the configuration-space bubble for a robust linearization of the nonlinear vehicle dynamics. The motion planner constructs a directed graph of position and orientation equilibria covering $\mathbb {SE}\text {(}3\text {)}$ . CAPI sets are constructed to verify that equilibria are connected by a feasible trajectory. Graph search is applied to determine a sequence of reference configurations, starting at an initial position-orientation and terminating at a goal position-orientation. Simulation results are included that demonstrate the safe navigation of a vehicle in the presence of an obstacle. The trajectory is shown to maintain the CAPI conditions and is therefore safe under the nonlinear translational and rotational closed-loop vehicle dynamics.