A robust 3-D motion estimation with stereo cameras on a robot manipulator

Accurately estimating the 3-D pose (position and orientation) of a moving object with reference to the world frame (or robot base frame) based on a sequence of stereo images taken by cameras mounted on the end-effector of a robot manipulator is considered. Emphasis is given to the 3-D pose estimation in the presence of measurement noise in 2-D images and camera position errors due to the random noise involved in the joint angles of a robot manipulator. To this end, a set of discrete Kalman filter equations is derived, based on the following: (1) the orientation error of the object frame due to the measurement noise in 2-D images is modeled with reference to the camera frame; (2) an extended Jacobian matrix is formulated by combining the result of (1) with the orientation error of the end-effector frame; and (3) the rotational motion of an object, which is nonlinear in nature, is linearized based on quaternions. Motion parameters are computed from the estimated quaternions based on the iterated least-squares method. Simulation results show a significant reduction in estimation errors