本論文以擴張型卡爾曼過濾器(extended Kalman filter, EKF)建立視覺式同時定位、建圖、與移動物體追蹤(simultaneous localization, mapping and moving object tracking, SLAMMOT)系統。此SLAMMOT系統的感測器使用視覺系統為唯一的感測裝置，搭配加速強健特徵(Speeded-Up Robust Features, SURF)的偵測方法進行環境中影像特徵的偵測，並且依據影像訊息求算特徵在空間中的三維座標，以建立SURF特徵式地圖。在估測器方面，本研究使用EKF方法遞廻估測機器人與影像特徵的狀態，並且規劃符合SURF特徵式地圖的資料新增、刪除、與更新等程序。移動物體運動模型方面，本系統使用交互式多模型(interacting multiple model, IMM)估測器對移動物體進行追蹤任務，使系統能同時處理靜態與動態移動的物體。本論文針對以上SLAMMOT系統三個部份所需的理論與技術進行探討，並且對文獻現有方法提出改善的方案。

    本論文也以模擬與實測方式，驗證所提方案的可行性。首先，使用單眼視覺實現具備未知輸入之系統的同時自我定位與特徵式地圖建立，同時也解決單眼視覺的影像深度量測問題。其次，以IMM估測器實現視覺式移動物體的追蹤，使用雙眼視覺做為感測器以簡化移動物體三維座標量測的問題。最後，整合EKF估測方法、特徵式地圖、與IMM估測器，實現雙眼視覺式同時定位、建圖、與移動物體追蹤之任務。

In this thesis, the visual simultaneous localization, mapping and moving object tracking (SLAMMOT) is established by using the extended Kalman filter (EKF). The theory and methodology of vision sensing, state estimation and motion modeling of the SLAMMOT system will be investigated in this thesis. For sensor perception, the visual system is the only sensing device in the SLAMMOT system. Meanwhile, the method of detecting the speeded-up robust features (SURF) is utilized to detect the image features in the environment. According to the extracted data of the image features, three-dimensional coordinates of the features are calculated and then the feature-based map based on SURF are built. For state estimation, the EKF is employed to predict and update the states of the robot and the features recursively. Furthermore, the procedures of adding, erasing and updating the data of the SURF in the map are planned. In modeling the motion of moving objects, the interacting multiple model (IMM) estimator is utilized to track the moving objects. Therefore, the system can handle both the stationary and moving objects at the same time.

    The proposed algorithms are validated through computer simulations and experimental works on real systems. First, simultaneous localization and feature-based mapping are implemented on a free-moving monocular vision system with unknown inputs. Meanwhile, the problem of determining image-depth in monocular vision is solved. Second, the vision-based moving object tracking is performed by using the IMM estimator. Furthermore, the sensor is replaced by a binocular vision to simplify the problem of calculating the three-dimensional coordinate of moving objects. Finally, the tasks simultaneous localization, mapping and moving object tracking using a binocular vision are implemented by integrating the EKF method, feature-based map and IMM estimator.

目錄
中文摘要 I
英文摘要 II
目錄 IV
圖目錄 VII
表目錄 IX
第1章 序論 1
1.1 研究動機與研究目的 1
1.2 問題描述 2
1.2.1 同時自我定位與建立地圖 3
1.2.2 移動物體追蹤 4
1.2.3 同時定位、建圖、與移動物體追蹤 4
1.3 研究範圍 5
1.4 論文架構 5
第2章 機器人EKF SLAM 6
2.1 攝影機運動模型 6
2.2 單眼視覺量測模型 7
2.3 地圖管理 10
2.3.1 新增地圖特徵 10
2.3.2 刪除地圖特徵 13
2.3.3 更新地圖資料 14
2.4 單眼視覺影像特徵初始化 14
2.5 系統流程 19
2.6 實測結果 20
2.6.1 範例一：方形路徑SLAM 21
2.6.2 範例二：室內環境SLAM 25
2.6.3 範例三：差異視線的SLAM 28
2.7 本章結論 31
第3章  移動物體追蹤 32
3.1 交互式多模型估測器 32
3.2 移動物體運動模型 35
3.2.1 等速度模型 35
3.2.2 等加速度模型 36
3.3 雙眼視覺量測模型 36
3.4 物體影像特徵管理 40
3.4.1 新增移動物體特徵 40
3.4.2 刪除移動物體特徵 41
3.4.3 更新移動物體資料 42
3.5 物體影像特徵初始化	42
3.6 系統流程	43
3.7 模擬與實測結果	45
3.7.1 範例一：IMM估測器模擬	45
3.7.2 範例二：三角形路徑MOT	49
3.7.3 範例三：路徑交錯MOT	55
3.8 本章結論	58
第4章  機器人EKF SLAMMOT	59
4.1 EKF SLAMMOT	59
4.2 EKF SLAM WITH DATMO	60
4.2.1 運動模型與量測模型	60
4.2.2 特徵管理與初始化	60
4.2.3 移動物體偵測	61
4.2.4 系統流程	62
4.3 實測結果	63
4.3.1 範例一：靜態環境SLAMMOT	63
4.3.2 範例二：動態環境SLAMMOT	65
4.4 本章結論	67
第5章 研究成果與未來研究方向	68
5.1 研究成果	68
5.2 研究貢獻	69
5.3 未來研究方向	69
參考文獻	70
附錄A 擴張型卡爾曼過濾器	71
附錄B  JACOBIAN矩陣	72
B.1 機器人EKF SLAM：量測矩陣 細部推導	72
B.2機器人EKF SLAM：特徵初始化J矩陣細部推導	75
B.3 機器人EKF SLAM：特徵點狀態轉換J矩陣細部推導	77
B.4 移動物體追蹤：CV模型與CA模型的量測矩陣 細部推導	78
B.5 機器人EKF SLAMMOT：SLAM問題之量測矩陣 細部推導	79

圖目錄
圖2.1 透視投影法示意圖	9
圖2.2 攝影機與特徵點三維示意圖	9
圖2.3 世界座標與初始攝影機座標示意圖	9
圖2.4 間隔視窗示意圖	12
圖2.5 間隔視窗的例外情形	12
圖2.6 刪除地圖特徵狀態與共變異數	13
圖2.7角度θi與ψi示意圖	15
圖2.8 新增地圖特徵狀態與共變異數	16
圖2.9 視差角α示意圖	18
圖2.10 特徵狀態與共變異數轉換	18
圖2.11 單眼視覺EKF SLAM系統流程圖	19
圖2.12 地圖資料庫預存的4個地標	21
圖2.13 系統啟動	21
圖2.14 方形路徑SLAM結果圖	23
圖2.15 方形路徑SLAM八張截圖	24
圖2.16 室內環境SLAM結果圖	26
圖2.17 室內環境SLAM八張截圖	27
圖2.18 差異視線的SLAM結果圖	29
圖2.19 差異視線的SLAM八張截圖	30
圖3.1 IMM估測器架構圖	34
圖3.2 標準立體視覺幾何結構	37
圖3.3 雙眼攝影機與特徵點3-D示意圖	39
圖3.4 雙眼攝影機視線向量求算示意圖	39
圖3.5 滑鼠選取新增移動物體特徵	40
圖3.6 左右影像特徵比對	41
圖3.7 移動物體追蹤流程圖	44
圖3.8 IMM估測器模擬結果：路徑與量測	46
圖3.9 IMM估測器模擬速度結合	47
圖3.10 IMM估測器模擬加速度結合	48
圖3.11 雙眼立體視覺系統	49
圖3.12 三角形路徑MOT物體x-y平面估測路徑結果圖	51
圖3.13 三角形路徑MOT編號0特徵	51
圖3.14 三角形路徑MOT速度結合	52
圖3.15 三角形路徑MOT加速度結合	53
圖3.16 三角形路徑MOT六張截圖	54
圖3.17 路徑交錯MOT物體移動示意圖	55
圖3.18 路徑交錯MOT物體1與物體2的x-y平面估測路徑結果圖	56
圖3.19 路徑交錯MOT編號0與編號3特徵	56
圖3.20 路徑交錯MOT六張截圖	57
圖4.1 EKF SLAMMOT流程圖	59
圖4.2 EKF SLAM with DATMO流程圖	62
圖4.3 靜態環境SLAMMOT上視圖	63
圖4.4 靜態環境SLAMMOT六張截圖	64
圖4.5 動態環境SLAMMOT上視圖	65
圖4.6 動態環境SLAMMOT六張截圖	66

表目錄
表2.1 刪除地圖特徵	13
表2.2 筆記型電腦規格表	20
表2.3 前視型單眼攝影機規格表	20
表3.1 刪除移動物體特徵	42

[1]	M.S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A Tutorial on Particle Filters for Online Nonlinear/Non-Gaussian Bayesian Tracking,” IEEE Transaction on Signal Processing, vol.50, no.2, pp.174-188, 2002.
[2]	T. Bailey, and H. Durrant-Whyte, “Simultaneous Localization and Mapping: Part 2,” IEEE Robotics and Automation Magazine, 2006.
[3]	H. Bay, A. Ess, T. Tuytelaars, L. Van Gool, “SURF: speeded up robust features,” Computer Vision and Image Understanding, vol.110, pp.346-359, 2008.
[4]	H. Blom, A.P, and Y. Bar-Shalom, “The interacting multiple-model algorithm for systems with Markovian switching coefficients,” IEEE Transactions on Automatic Control, vol.33, pp.780-783, 1988.
[5]	J. Civera, A.J. Davison and J.M.M. Montiel, “Inverse Depth Parametrization for Monocular SLAM,” IEEE Transactions on Robotics, vol.24, no.5, pp.932-945, 2008.
[6]	A.J. Davison, I.D. Reid, N.D. Molton, and O. Stasse, “MonoSLAM Real Time Single Camera SLAM,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.29, no.6, pp.1052-1067, 2007.
[7]	R.H. Deaves, “Covariance bounds for augmented state Kalman filter application,” IEE Electronics Letters, vol.35, no.23, pp.2062-2063, 1999.
[8]	M.W.M.G. Dissanayake, P. Newman, S. Clark, H. Durrant-Whyte, and M. Csorba, “A Solution to the Simultaneous Localization and Map Building (SLAM) Problem,” IEEE Transactions on Robotics and Automation, vol.17, no. 3, pp. 229–241, 2001.
[9]	H. Durrant-Whyte, and T. Bailey, “Simultaneous Localization and Mapping: Part 1,” IEEE Robotics and Automation Magazine, 2006.
[10]	S. Hutchinson, G.D. Hager, and P.I. Corke, “A Tutorial on Visual Servo Control,” IEEE Transactions on Robotics and Automation, vol.12, no.5, pp.651-670, October 1996.
[11]	D.G. Lowe, “Distinctive Image Features from Scale-Invariant Keypoints,” International Journal of Computer Vision, 60, 2, pp. 91-110, 2004. 
[12]	B.D. Lucas, and T. Kanade, “An Iterative Image Registration Technique with an Application to Stereo Vision,” International Joint Conference on Artificial Intelligence, pp.674-679, 1981.
[13]	M. Montemerlo, FastSLAM: a factored solution to the simultaneous localization and mapping problem with unknown data association, Ph.D. dissertation, Carnegie Mellon University, Pittsburgh, PA, 2003.
[14]	L. Sciavicco, and B. Siciliano, Modeling and Control of Robot Manipulators, McGraw-Hill, 1996.
[15]	R. Smith, M. Self, and P. Cheeseman, “Estimating Uncertain Spatial Relationships in Robotics,” In Autonomous Robot Vehicles, I.J. Cox and G.T. Wilfong, Eds., Springer-Verlog, pp.167-193, 1990.
[16]	S. Thrun, “Robotic mapping: A survey,” in Exploring Artificial Intelligence in the New Millennium, G. Lakemeyer and B. Nebel, Eds., San Mateo, CA: Morgan Kaufmann, ch. 1, 2002.
[17]	S. Thrun, W. Burgard, and D. Fox, Probabilistic Robotics, MIT Press, Cambridge, MA, 2005.
[18]	C.C. Wang, C. Thorpe, S. Thrun, M. Hebert, and H. Durrant-Whyte, “Simultaneous Localization, Mapping and Moving Object Tracking,” The International Journal of Robotics Research, vol. 26, no. 09, pp. 889-916, 2007.
[19]	鄭聖賢，機器人單眼視覺式同時定位與建圖的資料關聯問題研究，淡江大學機械與機電工程學系碩士論文，2010。