Patent ID: 8306732

Claim:
A method for assessing road curvature of a path, including a plurality of waypoints, said method comprising the steps of: a) receiving a pre-path, including a plurality of waypoints; b) generating the path from the pre-path, the generating including generating a finer sampling of waypoints from the plurality of waypoints of the pre-path to create the plurality of waypoints of the path; c) determining an allowable speed profile of the path, wherein said allowable speed profile includes an allowable speed for the each of the plurality of waypoints, wherein the allowable speed for the each of the plurality of waypoints satisfies a cross track acceleration constraint; and, d) determining an optimal speed profile of the path, wherein said optimal speed profile includes an optimal speed for the each of the plurality of waypoints, wherein the optimal speed for the each of the plurality of waypoints satisfies both an along track acceleration constraint and a speed constraint, wherein said speed constraint is a minimum of the desired speed and the allowable speed, wherein determining the optimal speed profile includes: sweeping through the plurality of waypoints in a first direction to determine an along track acceleration profile, wherein the along track acceleration profile satisfies a negative acceleration constraint; and, sweeping through the plurality of waypoints in a second direction to update the along track acceleration profile, wherein the updated along track acceleration profile satisfies both a negative acceleration constraint and a positive acceleration constraint; wherein the method is performed by use of at least one processor.