METHOD OF RECONSTRUCTING A VEHICLE TRAJECTORY

A method for generating a vehicle trajectory by optimizing a performance measure J. The trajectory may include a sequence of states x=(x)of the vehicle. The optimization is subject to predefined vehicle dynamics x=f(x, u), where uis a control input to the vehicle, and a condition that each position of the vehicle shall be close to a reference path X. The vehicle's position is constrained inside a variable-width corridor around the reference path. A quantity r controlling the width of the corridor is included as an additional optimization variable and the performance measure includes a penalty on the corridor width. To define the corridor, each point of the reference path may be associated with a pair of laterally spaced ellipses and requiring each vehicle position to be outside the ellipses.