Optimal motion planning of autonomous vehicles in three dimensional terrains

A method is presented for optimally planning the motions of autonomous vehicles, considering vehicle dynamics, terrain topography, obstacles, and surface mobility. The terrain is represented by a smooth cubic B patch, and the geometric path consists of a two-dimensional B spline curve mapped to the three-dimensional surface. The path is optimized with a parameter optimization procedure, using motion time as the cost function. The optimal motion time along the path is computed by transforming the constraints between the vehicle and ground to limits on vehicle speeds. The optimal speeds which minimize motion time are then obtained below the velocity limits, using the maximum acceleration or deceleration at all times. Examples are presented which demonstrate the method for a simple dynamic model of a vehicle moving on mountainous terrain.