Trajectory Optimization for Autonomous Navigation in Dynamic Environments