Autonomous satellite service and repair is becoming increasingly important. As a result, solving for optimal control for orbital rendezvous and docking is an important operation.

We solve this problem with a probabilistic approach in the spirit of randomized exploration trees (RRT and Expansive Space frameworks), but we use a cost function to help converge our search quicker and to produce lower cost paths for kinodynamic systems. This approach has several advantages over the traditional discretized network approach and standard probabilistic path planning tree expansions.

We further refine the path using a variation of the Elastic Band technique. Our extension of this technique must not violate nonholonomic constraints and, instead of finding a short path, it should find a low cost path which avoids obstacles. To perform this dual task we write the gradient of the path in terms of the controls of the robot. By considering the controls we can easily derive the cost gradient and also easily avoid violating nonholonomic constraints which are logically derived from bounds on controls.

This path refinement can be run in real time to dynamically avoid moving obstacles while maintaining path cost optimality.