本論文在小型人型機器人系統上以擴張型卡爾曼濾波器(Extended Kalman Filter, EKF)實現單眼視覺的同時定位與建圖(Simultaneous Localization And Mapping, SLAM)。應用影像特徵點辨識與追蹤方法，找到影像特徵點，利用2張影像中相同的特徵點計算深度，得知特徵點在空間中的3D座標，提供機器人EKF SLAM之用。本研究也自製一部符合RoboCup小型(kid-szie)規格的人型機器人，搭配單板電腦(PC-based)控制器。整合機器人與EKF SLAM系統，將能夠在簡易的室內環境中，進行同時自我定位與地圖建立。

In this thesis, a single-camera simultaneous localization and mapping (SLAM) algorithm based on the extended kalman filter(EKF) is developed for a RoboCup Kidsize humanoid robot. In order to find the image-features, we applied the recognition of features and the tracking techniques, using two same image-features to calculate the depth. The three dimension feature positions would be used by robot EKF SLAM.We design and fabricate a PC-based humanoid robot which is conformed to the regulation of RoboCup Kidsize humanoid robot league. The integrated humanoid robot and EKF SLAM system can be implemented SLAM in simple indoor environments.

中文摘要                               Ⅰ
英文摘要                               Ⅱ
目錄                                   Ⅲ
圖目錄                                 Ⅵ
表目錄                                  Ⅹ
參數定義                              ⅩⅠ

第1章 序論	                       1
1.1研究動機與研究目的                    1
1.2文獻探討                              2
1.3研究範圍                              3
1.4論文架構                              3

第2章 同時自我定位與建圖	              4
 2.1機器人座標系統                       4
 2.2擴張型卡爾曼濾波器(EKF)              6
  2.2.1 EKF系統狀態方程式	              6
  2.2.2 EKF量測方程式                    8
  2.2.3 狀態與量測的估測(Prediction)     8
  2.2.4 估測的更新(Estimation update)    9
  2.3相機校正                           14
 2.4特徵點化初始化                      16

第3章 運動學分析	                      20
 3.1反向運動學與AGB-65步態命令轉換      23
 3.2行進步態規劃                        24
  3.2.1直走前進	                      24
  3.2.2右平移	                      28
 3.2.3左平移	                      31
 3.2.4右旋轉（順時針旋轉)               35
 3.2.5左旋轉（逆時針旋轉)               38

第4章 機器人機構設計與系統架構	     42
 4.1機器人機構設計                      43
  4.1.1腿部設計	                      44
  4.1.2上半身設計	                      45
  4.1.3頭部設計	                      46
  4.1.4手臂設計                         46
  4.1.5設計圖組合	                      46
 4.2 鈑金材料選用                       47
 4.3 機電整合系統設計                   47
  4.3.1伺服機	                      48
  4.3.2伺服機驅動板	             48
  4.3.3工業電腦	                      49
  4.3.4電源系統	                      50
  4.3.5攝影機	                      51
  4.3.6軟體	                      51
  4.4機器人實作與組裝	             53

第5章 實驗結果與分析	             57
 5.1相機校正結                          57
 5.2影像深度求算範                      60
 5.3 EKF特徵點不變動模擬範例            61
 5.4 EKF特徵點變動模擬範例              64
 5.5 EKF路徑閉合模擬範例                69
 5.6 EKF直線範例                        72
 5.7 EKF路徑閉合範例                    74

第6章 研究成果與討論                    76
 6.1 研究成果                           76
 6.2 未來研究方向                       76

參考文獻                                78

附錄A  相機校正	                      81
附錄B  KalmanFilter	             88
附錄C  機器人機構限制	             91
附錄D  KONDO伺服機	             92
附錄E  鋁合金化性、物性、特性一覽	    93
附錄F  伺服機驅動板AGB65-RCS	    95
附錄G  工業電腦	                      96
附錄H  攝影機	                      97

圖目錄
圖1.1 處在模擬居家環境中的機器人(一) 2
圖1.2 處在模擬居家環境中的機器人(二)2
圖2.1 系統流程圖 4
圖2.2 世界座標系統 5
圖2.3 攝影機座標與二維影像座標系統 6
圖2.4 二維影像平面座標 6
圖2.5 攝影機座標與特徵點 19
圖2.6 球體座標的經度角度標示 19
圖2.7 球體座標的緯度角度標示 19
圖2.8 移動攝影機座標 19
圖3.1 機器人各軸座標系 22
圖3.2 機器人馬達編號 23
圖3.3  AGB65-RSC編碼格式圖 23
圖3.4 直走前進 Step1-Step5 25
圖3.5 機器人直走前進 Step1-Step5 25
圖3.6 直走前進 Step6-Step10 26
圖3.7 機器人直走前進 Step6-Step10 26
圖3.8 直走前進 Step11-Step15 26
圖3.9 機器人直走前進 Step11-Step15 27
圖3.10 右平移Step1- Step5 28
圖3.11 機器人右平移Step1-Step5 29
圖3.12 右平移Step6-Step9 29
圖3.13 機器人右平移Step6-Step9 30
圖3.14 右平移Step10-Step12 30
圖3.15 機器人右平移Step10-Step12 31
圖3.16 左平移Step1-Step5 32
圖3.17 機器人左平移Step1-Step5 32
圖3.18 左平移Step6-Step9 33
圖3.19 機器人左平移Step6-Step9 33
圖3.20 左平移Step10-Step12 34
圖3.21 機器人左平移Step10-Step12 34
圖3.22 右旋轉（順時針旋轉）Step1-Step5 35
圖3.23 機器人右旋轉（順時針旋轉）Step1-Step5 36
圖3.24 右旋轉（順時針旋轉）Step6- Step9 36
圖3.25 機器人右旋轉（順時針旋轉）Step6-Step9 37
圖3.26 右旋轉（順時針旋轉）Step10-Step12 37
圖3.27 機器人右旋轉（順時針旋轉）Step10-Step12 38
圖3.28 左旋轉（逆時針旋轉）Step1-Step5 39
圖3.29 機器人左旋轉（逆時針旋轉）Step1-Step5 39
圖3.30 左旋轉（逆時針旋轉）Step6-Step9 40
圖3.31 機器人左旋轉（逆時針旋轉）Step6-Step9 40
圖3.32 左旋轉（逆時針旋轉）Step10-Step12 41
圖3.33 機器人左旋轉（逆時針旋轉）Step10-Step12 41
圖4.1 機器人尺寸規格圖 43
圖4.2 踝關節圖 44
圖4.3 髖關節圖 44
圖4.4 機器人上半身設計圖 45
圖4.5 機器人上半身設計圖 45
圖4.6 胸部零件配置圖 45
圖4.7 背包零件配置圖 45
圖4.8 頭部設計圖 46
圖4.9 手臂設計圖 46
圖4.10 機器人設計組合圖 47
圖4.11 系統架構圖 48
圖4.12  KRS-4014 HV 48
圖4.13  KRS-4024 SHV 48
圖4.14  KRS-788 HV 48
圖4.15  AGB65-RCS實體圖 49
圖4.16 研揚工業電腦PFM-5401 49
圖4.17  SAMSUNG Li電池 50
圖4.18  IDD-936260A電源穩壓器 50
圖4.19 電池供電配置圖 51
圖4.20  Microsoft LifeCam VX-6000 51
圖4.21 人機介面 52
圖4.22 機器人操作介面圖 53
圖4.23 伺服機與鈑金 54
圖4.24 機器人正視圖 54
圖4.25 機器人側視圖 55
圖4.26 機器人背視圖 55
圖4.27 機器人上視圖 56
圖5.1 測試用方格紙 58
圖5.2 還原曲線 58
圖5.3 特徵點不改變機器人的位置 62
圖5.4 特徵點不改變機器人的角度 62
圖5.5 特徵點不改變機器人的速度 63
圖5.6 特徵點不改變機器人的角速度 63
圖5.7 特徵點不改變第1特徵點 64
圖5.8 第1張特徵點影像圖 65
圖5.9 第2張特徵點影像圖 65
圖5.10 特徵點改變機器人的位置 66
圖5.11 特徵點改變機器人的角度 66
圖5.12 特徵點改變機器人的速度 67
圖5.13 特徵點改變機器人的角速度 67
圖5.14 特徵點改變第1特徵點 68
圖5.15 特徵點改變第5特徵點 68
圖5.16 特徵點改變第5特徵點 69
圖5.17路徑閉合xy平面機器人路徑與特徵點圖 70
圖5.18 路徑閉合機器人的位置 71
圖5.19 路徑閉合機器人的速度 71
圖5.20 自然環境攝影機位置(一) 72
圖5.21 自然環境攝影機位置(二) 72
圖5.22 網路攝影機擷取圖(一) 72
圖5.23 網路攝影機擷取圖(二) 72
圖5.24 機器人平移所建立的地圖，特徵點為深度計算值 73
圖5.25 機器人平移所建立的地圖，特徵點為EKF的估測值 73
圖5.26 網路攝影機擷取圖(三) 74
圖5.27 網路攝影機擷取圖(四) 74
圖5.28 機器人移動所建立的地圖，特徵點為EKF的估測值 75

表目錄
表3.1  Right leg DH parameters 20
表3.2  Left leg DH parameters 21
表3.3  Right hand DH parameters 21
表3.4  Left hand DH parametes 21
表3.5  Head DH parameters 21
表3.6 立正姿態的步態角度初始值與控制命令初始值 21
表3.7 馬達旋轉角度與θi之間的轉換 23
表4.1 機器人規格 42
表4.2 關節規格 44
表4.3 人機介面按鈕功能 52
表5.1 影像特徵點位置量測 59
表5.2 相機校正誤差值 59
表5.3 影像特徵點位置 60
表5.4 影像還原前誤差值 61
表5.5 影像還原後誤差值 61
表5.6 特徵點位置座標(一) 61
表5.7 特徵點位置座標(二) 65
表5.8 第1次更改特徵點位置 65
表5.9 第2次更改特徵點位置 65
表5.10特徵點位置座標(三) 70

[1] Bay, H., T. Tuytelaars, and L. Van Gool, “SURF Speeded Up Robust Features, ” Proceedings of the 9th European Conference on Computer Vision, Springer LNCS volume 3951, part 1, pp. 404-417, 2006.
[2] Chou, H.L., and W.H. Tsai, “A new approach to robot location by house corners,” Pattern Recognition, Vol. 19, No. 6, pp.439-451, 1986.
[3] Chen, C. S., C. K. Yu, and Y. P. Hung, “New Calibration free Approach for Augmented Reality Based on Parameterized Cuboid Structure,” ICCV, pp.30-37, 1999.
[4] Davison, A. J., Y. G. Cid, and N. Kita,“Real time 3D SLAM with wide-angle vision,” in Proc IFAC Symposium on Intelligent Autonomous Vehicles, Lisbon, 2004.
[5] Davison, A.J., 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.
[6] Dellaert, F., D. Fox, W. Burgard, and S. Thrun, “Monte Carlo Localization for Mobile Robots,” IEEE, 1999.
[7] Denavit, J. and R.S. Hartenberg, “A Kinematic Notation for Low Pair Mechanisms Based on Matrices,” ASME Journal of Applied Mechanisms, pp.215-221, 1955.
[8] Dissanayake, M.W.M.G., 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] Hutchinson, S., 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.
[10] Kalman, R. E., “A New Approach to Linear Filtering and Prediction Problems,” ASME, 1960.
[11] Kondo website, http://www.kondo-robot.com/.
[12] Lenz R. K., and R. Y. Tsai, “Techniques for Calibration of the Scale Factor and Image Center for High Accuracy 3-D Machine Vision Metrology”, IEEE Transactions on Pattern Analysis and Machine Intelligence, 1988 .
[13] Lowe, D.G., “Object recognition from local scale-invariant features,” Proceedings of International Conference on Computer Vision, pp.1150-1157, 1999.
[14] Montiel, J.M.M., J. Civera, and A.J. Davison, “Unified Inverse Depth Parametrization for Monocular SLAM,” Proc. Robotics: Science and Systems, 2006.
[15] Murphy, K., and S. Russell, “ Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks, ” in Sequential Monte Carlo Methods in Practice, Springer Verlag, 2001.
[16] Nelder, J.A., and R. Mead, “A simplex method for function minimization,” Computer Journal, pp.308-313,1965.
[17] RoboCup website, http://www.robocup.org.
[18] Sciavicco, L., and B. Siciliano, “Modelling and Control of Robot Manipulators,”1st Edition, McGraw-Hill, New York, NY, 1996.
[19] Shi, J., and C. Tomasi, “Good features to track, ” In Proceedings of the IEEE Conference on 
Computer Vision and Pattern Recognition, pp.593-600, 1994.
[20] Siegwart, R., and I. R. Nourbakhsh , “ Introduction to Autonomous Mobile Robots, ” 2004.
[21] Smith, R., M. Self, and P. Cheeseman, “Estimating Uncertain Spatial Relationships in Robotics,” In Autonomous Robot Vehicles, 1990.
[22] Sturm, P., and S. Maybank, “On Plane based Camera Calibration A General Algorithm, Singularities, Applications,” In Proceedings of the IEEE Conference on Computer vision and Pattern Recognitions, pp.432-437, 1999.
[23] Tsai, R.Y., “A versatile camera calibration technique for high accuracy 3D machine vision metrology using off the sheff TV cameras and lenses,” IEEE J. of Robotics and Automation, 3(4), pp.323-344,1987.
[24] Zhang, Z., “A Flexible New Technique for Camera Calibration,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 22, no. 11, 2000.
[25] Welch, G., and G. Bishop, “An Introduction to the Kalman Filter,” Department of Computer Science, University of North Carolina, 2006.
[26] 蔡鎮全，PC-based控制之人型機器人的同時自我定位與建圖，碩士論文，淡江大學機械與機電工程學系，2007。
[27] 鍾國亮，影像處理與電腦視覺，東華書局，2004。
[28] 謝睿仁，整合GPS自我定位與視覺伺服控制的自主機器人，碩士論文，淡江大學機械與機電工程學系，2008。
[29] 陳義智，多機器人平台之研發，碩士論文，淡江大學機械與機電工程學系，2008。
[30] 曹雅威，自主機器人對於環境中點特徵與區域特徵的辨識與追蹤，碩士論文，淡江大學機械與機電工程學系，2008。
[31] 蔡哲芳，SolidWorks 2006 實戰演練基礎入門，知城數位科技股份有限公司，2005。
[32] 研揚公司，PFM-5401工業控制器使用手冊。
[33] 康振豪、蕭又文、賴政賢，雙足機器人設計與製作，畢業專題，淡江大學機械與機電工程學系，2007。