Title: Optimizing safe motion for autonomous vehicles