Safe Optimal Control with SynthesizedWaypoints as Guidance

This paper proposes a safe and optimal control framework using waypoints and associated reference trajectories as guidance. The waypoints are obtained by efficiently solving satisfiability problems over linear constraints. The waypoint synthesis formulation can incorporate control errors and ensure that the computed reference trajectory still satisfies given specifications even in the presence of error. We further use the optimal control framework with an augmented tracking-error-minimization objective to closely follow the reference trajectory. Since the reference trajectory is safe by design, the ideal tracking performance of the reference also implies the safety of the closed-loop state trajectory, which eliminates the need for additional safety constraints that may hinder solution optimality. We compute the optimal control law for stochastic systems using a path-integral reformulation. Our approach outperforms the optimal control framework without a synthesized reference trajectory in terms of trajectory optimality and goal-reach accuracy, as validated by numerical simulations on a UAV model passing through an obstacle-cluttered environment.