TRACE: Trajectory Refinement with Control Error Enables Safe and Accurate Maneuvers

Normal trajectory optimizations are carried out without the consideration of the closed-loop feasibility and performance. During aggressive flights, bias from the nominal trajectory could trigger feedback with larger control inputs, which might break through the actuator constraints, inducing stability issues. By leveraging the idea of TRAjectory refinement with Control Error (TRACE) for higher closed-loop performance, we develop a post-trajectory optimization approach for safe and accurate tracking control. A closed-loop model is established combining both control policy and saturations in order to model the dynamic progress of trajectory tracking. Subsequently, a time-varying optimal control problem (OCP) is constructed to enhance the tracking performance by weighted minimizing the tracking error and the variation from the nominal trajectory progress. Meanwhile, a flatness-based method is presented to provide aggressive time-optimal trajectories with high-order derivatives of the reference states. Examples show the effectiveness of our approach in refining aggressive trajectories, where safe and accurate tracking can be done using a baseline controller without fine-tuning and further control allocation design.