Robust 3-D Motion Estimation Based on Stereo Vision and Kalman Filtering

스테레오 시각과 Kalman 필터링을 이용한 강인한 3차원 운동추정

This paper deals with the accurate estimation of 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. This work is an extension of the previous work[1]. Emphasis is given to the 3-D pose estimation relative to the world (or robot base) frame under the presence of not only the measurement noise in 2 - D images[ 1] but also the camera position errors due to the random noise involved in joint angles of a robot manipulator. To this end, a new set of discrete linear Kalman filter equations is derived, based on the following: 1) the orientation error of the object frame due to measurement noise in 2 - D images is modeled with reference to the camera frame by analyzing the noise propagation through 3- D reconstruction; 2) an extended Jacobian matrix is formulated by combining the result of 1) and the orientation error of the end-effector frame due to joint angle errors through robot differential kinematics; 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 the significant reduction of estimation errors and also demonstrate an accurate convergence of the actual motion parameters to the true values.

본고는 로보트 팔의 선단에 부착된 카메라에 의하여 촬영된 일련의 스테레오 영상을 이용하여 운동물체의 3차원 자세 (위치와 방향)를 정확히 추정하는 방법을 다룬다. 본고는 이미 발표된 바 있는 연구결과를 확장한 것으로서[1], 2차원 영상의 측정잡음 뿐만아니라[1], 또한 로보트 팔의 죠인트 각도의 랜덤잡음이 함께 존재할 경우 world 좌표계 (또는 로보트 기지좌표계)를 기준으로 한 운동물체의 3차원 자세의 추정에 중점을 둔다. 이를 위하여, 다음 사항에 근거하여 선형 Kalman 필터를 유도한다. (1) 2차원 영상의 측정잡음이 3차원 공간으로 전파되는 것을 분석함으로써, 이에 기인한 물체좌표계의 방향오차를 카메라 좌표계를 기준으로 하여 모델링한다; (2) 죠인트 각도 오차에 의한 로보트 선단좌표계의 방향오차를 (1)의 결과와 결합하여 extended Jacobian matrix를 유도한다; 그리고 (3) 본질적으로 비선형인 물체의 회전운동을 quaternion을 도입함으로써 선형화 한다. 운동 파라메터는 추정된 quaternion으로부터 반복 최소자승 방법을 이용하여 계산된다. 모의실험 결과, 추정오차가 상당히 감소되고, 실제의 운동 파라메터가 참 값으로 정확히 수렴함을 알 수 있다.