Pose Estimation of Vehicles Over Uneven Terrain

This paper presents a method for pose estimation of off-road vehicles moving over uneven terrain. It determines the contact points between the wheels and the terrain, assuming rigid contacts between an arbitrary number of wheels and ground. The terrain is represented by a 3D points cloud, interpolated by a B-patch to provide a continuous terrain representation. The pose estimation problem is formulated as a rigid body contact problem for a given location of the vehicle's center of mass over the terrain and a given yaw angle. The contact points between the wheels and ground are determined by releasing the vehicle from a given point above the terrain, until the contact forces between the wheels and ground, and the gravitational force, reach equilibrium. The contact forces are calculated using singular value decomposition (SVD) of the deduced contact matrix. The proposed method is computationally efficient, allowing real time computation during motion, as demonstrated in several examples. Accurate pose estimations can be used for motion planning, stability analyses and traversability analyses over uneven terrain.