US Patent 9244462 Vehicle trajectory planning for autonomous vehicles

A method for controlling an autonomous vehicle includes obtaining, by one or more processors, information describing a current state of the autonomous vehicle and a goal state of the autonomous vehicle; determining, by the one or more processors, an initial vehicle trajectory from the current state of the autonomous vehicle to the goal state of the autonomous vehicle; determining, by the one or more processors, an optimized vehicle trajectory based the initial trajectory and a velocity profile by applying numerical minimization to minimize a trajectory length value and a lateral acceleration value; and controlling the autonomous vehicle to traverse the optimized vehicle trajectory.