Traversability analysis and path planning for a planetary rover

A method of analyzing three-dimensional data such as might be produced by stereo vision or a laser range finder in order to plan a path for a vehicle such as a Mars rover is described. In order to produce robust results from data that is sparse and of varying accuracy, the method takes into account the accuracy of each data point, as represented by its covariance matrix. It computes estimates of smoothed and interpolated height, slope, and roughness at equally spaced horizontal intervals, as well as accuracy estimates of these quantities. From this data, a cost function is computed that takes into account both the distance traveled and the probability that each region is traversable. A parallel search algorithm that finds the path of minimum cost also is described. Examples using real data are presented.