Development of a 3D ego-motion estimation system for an autonomous agricultural vehicle

A stereo vision based three-dimensional (3D) ego-motion estimation system was proposed and tested to enable real-time navigation for a full-scale agricultural vehicle. A stereo camera was used to track features in image sequences which were then matched to obtain 3D point clouds that can be used for motion estimation. To overcome the challenges of uneven terrains, relative rotation and translation motions were taken into consideration by registering the point clouds using the iterative closest point algorithm. A smooth-motion constraint was employed to reduce estimation outliers, and a multi-frame estimation strategy was developed to limit estimation failures and error propagations. A series of field tests were conducted on different field surfaces. In a soybean field where the vehicle was driven for 2.5 km following a typical back-and-forth route, the maximum position estimation error was 5.12 m or 0.20%; while on a grass road, where available features were limited, the maximum error was 6.21 m, or 1.61%, for a driving distance of 386 m. To evaluate the estimation of heading angles using multiple frames, the vehicle was driven following a sine-wave route for 100 m; the root-mean-square error (RMSE) of heading angle was 1.43°, which was much lower compared to a RMSE of 6.69°obtained using consecutive frames for estimation. Finally, the vehicle was driven across a 0.24 m and a 0.1 m high bumps and an estimate of pitch angle and roll angle were obtained. An RMSE of 0.30°was obtained for both angles, indicating its feasibility of navigating vehicles on uneven terrains.