Simultaneous Control and Trajectory Estimation for Collision Avoidance of Autonomous Robotic Spacecraft Systems

Matthew King-Smith,Panagiotis Tsiotras,Frank Dellaert

DOI: https://doi.org/10.48550/arXiv.2204.13251

2022-04-28

Abstract:We propose factor graph optimization for simultaneous planning, control, and trajectory estimation for collision-free navigation of autonomous systems in environments with moving objects. The proposed online probabilistic motion planning and trajectory estimation navigation technique generates optimal collision-free state and control trajectories for autonomous vehicles when the obstacle motion model is both unknown and known. We evaluate the utility of the algorithm to support future autonomous robotic space missions.

Robotics

What problem does this paper attempt to address?

The problem that this paper attempts to solve is to achieve collision - free navigation of autonomous robotic spacecraft systems in an environment with moving obstacles. Specifically, the paper proposes a method based on factor - graph optimization for simultaneous planning, control, and trajectory estimation to support collision - free navigation in future autonomous robotic space missions. This method aims to handle cases where the obstacle motion model is unknown or known and generate optimal collision - free states and control trajectories.

### Background and Problem Description of the Paper

In on - orbit satellite service (OSS) tasks, such as refueling, maintaining, upgrading, and repairing existing spacecraft, as well as actively removing orbital debris, the service spacecraft is required to adapt to a constantly changing environment while achieving the main mission objectives. This requires online implementation of vehicle trajectory planning while considering information collected from on - board system sensors and its uncertainty.

Traditionally, online trajectory optimization methods for spacecraft navigation usually solve the optimal control problem by performing vehicle trajectory estimation independently of planning. However, this two - step optimization process may lead to sub - optimal results because the estimation and planning problems can be regarded as different variants of a single trajectory optimization problem.

### Proposed Method

The paper proposes a new algorithm - Simultaneous Control and Trajectory Estimation (SCATE) for online probabilistic trajectory motion planning and estimation. The SCATE method addresses the following issues:
1. **Adding Real Vehicle Dynamics**: Incorporate the actual spacecraft dynamics model into the factor graph.
2. **Collision - Free Path Planning and Trajectory Estimation in an Environment with Moving Obstacles**: Handle obstacles in a dynamic environment to ensure that the spacecraft can navigate safely.
3. **Evaluating SCATE Navigation on a Physical Autonomous Spacecraft Simulation Platform**: Verify the effectiveness of the algorithm through experiments.

### Method Overview

The SCATE method is based on factor - graph optimization, unifying the trajectory estimation and planning problems within one framework. Specific steps include:
- **Trajectory Optimization as Probabilistic Inference**: Use the Gaussian process (GP) to define the prior distribution of the trajectory and combine the likelihood function to calculate the maximum a posteriori (MAP) trajectory.
- **Factor - Graph Representation**: Represent the posterior distribution as a factor graph and utilize the sparsity of the factor graph for efficient computation.
- **Dynamic System Modeling**: Assume that the dynamics of the system are described by a linear time - invariant (LTI) state - space model and add dynamic factors and control - limit factors to the factor graph.
- **Obstacle Factors**: Define obstacle factors and use the signed distance field (SDF) to calculate the obstacle cost function to ensure collision - free navigation.

### Experimental Verification

The paper verifies the SCATE method in the frictionless robotic spacecraft simulation experimental facility of the Dynamics and Control Systems Laboratory (DCSL). The experimental results show that SCATE can achieve collision - free navigation in static and moving - obstacle environments and can generate optimal reference trajectories and state estimates in real - time.

### Conclusion

The paper demonstrates the effectiveness of using factor - graph for simultaneous control and trajectory estimation (SCATE) to achieve collision - free navigation of autonomous robotic spacecraft in an environment with moving obstacles. Experimental and numerical verifications show that the SCATE method can:
- **Integrate Real Vehicle Dynamics**.
- **Generate Online Collision - Free Reference Trajectories and Estimates of Vehicle States, Controls, and Obstacle Trajectories**.
- **Support Future On - Orbit Satellite Service Tasks**.

Although the current research is limited to linear planar mechanics, the SCATE method can be extended to 6 - degree - of - freedom nonlinear single - body or multi - body dynamics, as well as 3D motion planning. These extensions are directions for future research.