以限制型卡爾曼濾波器估測位置與姿態

Abstract

在機器人學、攝影測量、電腦視覺中，利用相機、攝影機等所提供的影像判斷物體的位置與姿態是很重要的課題。此問題可以被描述成尋找附體座標系與相機座標系兩者相對的位移(translation)與轉動(rotation)。其中位移即代表物體位置(position)，轉動則代表物體姿態(attitude)。 使用雙四元數(dual quaernion)同時表示兩座標系相對的位移與轉動是方便且有效的。利用空間中的直線在附體座標系的資訊以及相片上這些線的影像並且結合延伸型卡爾曼濾波器(Extended Kalman Filter)可以估測到表示物體位置與姿態的雙四元數。然而延伸型卡爾曼濾波器會無法完全滿足雙四元數本身的限制條件(constraints)，並可能因此影響解的正確性與收斂性。另外，以往利用雙四元數求解過程中，仍然將位移與轉動分開處理，因而損失了雙四元數求解的一致性。 本文設計兩個動態系統：主系統(nominal system)與修正系統(correction system)。主系統代表待估測的狀態，包含表示位置與姿態的雙四元數等；修正系統則定義為主系統的估測值與實際值的差異。使用卡爾曼濾波器估測修正系統，再利用這些估測值修正主系統，由此得到的主系統估計值將符合限制條件，從而減少得到錯誤解的機會。此外我們使用雙四元數的微分方程求解，保留了雙四元數同時表示位移與轉動的特性。 最後提出程式模擬以及拍照實驗。模擬結果驗證此濾波器不僅成功估測到物體位置及姿態，也符合雙四元數的限制條件。而拍照實驗顯示：我們設計的濾波器在實際應用上是可行的。

In this thesis, we propose a method to determine the 3-dimentional (3D) position and orientation (pose) of a moving object from image sequence. This problem, which is important in robotics, photometric, computer vision etc, is formulated as finding the translation and the rotation between body frame and camera frame. We use dual quaternion to represent this coordinate transformation and apply Kalman filter to estimate corresponding translation, rotation, velocity and angular velocity. Because of the constraints of the dual quaternion, we cannot use extend Kalman filter to estimate the parameters successfully, so that we use constrained Kalman filter instead. In the derivation of constrained Kalman filter, dual quaternion kinematics equation is applied to establish a nominal system whose state includes parameters of pose and motion. Besides, we establish a correction system which is defined as the estimation error of the nominal system and apply it to Kalman filter. Then we use the filtered estimation to correct the estimation of nominal system and make the state fit its constraints. Both simulation and experiments results show that our method estimates the pose, velocity and angular velocity successfully. Moreover, the experiments present the filtering method can be applied in the real world.