Accurate curvature estimation algorithm for path planning of autonomous driving vehicle

A method for determining roadway curvature in a vehicle system on a vehicle, said method comprising:

• collecting shape points from a map database on the vehicle from a current position of the vehicle to an end of a range of interest that define locations of the roadway;
• fitting third-order polynomial equations to define a roadway curve using the shape points;
• determining that the third-order polynomial equations exceed a predetermined curvature accuracy threshold and fitting multiple sets of polynomial equations to multiple contiguous roadway segments over the range of interest using the shape points;
• and
• determining the roadway curvature at a roadway location using solutions to the polynomial equations or the multiple polynomial equations if used.

A system for determining roadway curvature on a vehicle, said system comprising:

• means for collecting shape points from a map database on the vehicle from a current position of the vehicle to an end of a range of interest that define locations of the roadway;
• means for fitting third-order polynomial equations to define a roadway curve using the shape points;
• means for determining that the third-order polynomial equations exceed a predetermined curvature accuracy threshold and fitting multiple sets of polynomial equations to multiple contiguous roadway segments over the range of interest using the shape points;
• and
• means for determining the roadway curvature at a roadway location using solutions to the polynomial equations or the multiple polynomial equations if used.