Path Planner Optimization