本研究建立以視覺輔助的同時定位與建圖(Simultaneous Localization and Mapping, SLAM)系統，提供機器人建立環境地圖與自我定位之用。本研究的內容分為兩部分：包括視覺式EKF SLAM架構的建立，以及人形機器人步態的規劃與實現。發展的同時定位與建圖系統應用在具備前視型攝影機的小型人形機器人系統上。

In this thesis, the vision-based Simultaneous Localization and Mapping (SLAM) be established, and provided for the robot to do self-localization and build up the environmental map. This research is divided into two parts, including the design of the vision-based EKF-SLAM and the gait planning of the humanoid robot. The developed SLAM system be applied to a small-size humanoid robot system with frontal camera.

目錄

中文摘要	I
英文摘要	II
目錄	III
圖目錄	V
表目錄	VIII

第1章 序論	1
1.1 研究計畫之背景與目的	1
1.2 研究範圍	2
1.3 國內外有關本計畫之研究情況、重要參考文獻之評述	2

第2章 人形機器人運動學分析	3
2.1正向運動學	3
2.2反向運動學	5
2.2.1 右腳的反向運動學	6
2.2.2 左腳的反向運動學	13
2.3 AGB65-RSC驅動器命令格式	19

第3章 人形機器人步態規劃	20
3.1基本步態規劃	20
3.2機器人步態的補償	21
3.3 行進步態規劃	23
3.3.1 向前直行	23
3.3.2 右平移	28
3.3.3 左平移	32
3.3.4 右旋轉(順時針旋轉)	35
3.3.5 左旋轉(逆時針旋轉)	39
3.4 連續步態範例	42

第4章 機器人同時定位與建圖	44
4.1 EKF狀態估測	44
4.2 EKF SLAM程序	48
4.3 EKF狀態估測範例	49
4.4 EKF SLAM實測範例	49

第5章 結論與建議	53
5.1 完成的研究項目	53
5.2 未來研究方向	53

參考文獻	54
附錄A人形機器人系統	56
附錄B 機器人步態補償表	59
附錄C Roll、Pitch、Yaw旋轉矩陣	73
附錄D EKF Jacobian矩陣	74

 
圖目錄

圖2.1 機器人伺服機編號	04
圖2.2 機器人各軸座標系	04
圖2.3 座標系{A}向量與座標系{B}向量	05
圖2.4 右腳底板旋轉向右	07
圖2.5  旋轉	07
圖2.6 右腳末端位置	07
圖2.7 右腳座標系{6}與{5}的轉換	09
圖2.8 右腳座標系{4}與{1}的轉換	10
圖2.9 右腳座標系{1}與{2}的轉換	10
圖2.10 x1y1平面求算	11
圖2.11 R-leg knee in front of body	11
圖2.12 R-leg knee in back of body	11
圖2.13 左腳底板旋轉向左	13
圖2.14  旋轉矩陣	13
圖2.15 左腳座標系{13}與{12}轉換	15
圖2.16 左腳座標系{11}與{8}轉換	16
圖2.17 左腳座標系{8}與{9}轉換	16
圖2.18 x8y8平面求算	17
圖2.19 L-leg knee in front of body	17
圖2.20 L-leg knee in back of body	17
圖2.21 AGB65-RSC編碼格式圖	19
圖3.1微蹲將重心放在左腳上並且抬右腳	21
圖3.2 抬腳的理想位置與實際位置	22
圖3.3完成微蹲將重心放在左腳上並且抬右腳	23
圖3.4規劃向前直行 Step1-Step3	24
圖3.5機器人向前直行 Step1-Step3	25
圖3.6規劃向前直行 Step4-Step6	25
圖3.7機器人向前直行 Step4-Step6	26
圖3.8向前直行 Step7-Step9	26
圖3.9機器人向前直行 Step7-Step9	27
圖3.10規劃向前直行 Step10-Step12	27
圖3.11機器人向前直行 Step10-Step12	28
圖3.12規劃右平移Step1-Step3	29
圖3.13機器人右平移Step1-Step3	29
圖3.14規劃右平移Step4-Step5	30
圖3.15機器人右平移Step4-Step5	30
圖3.16規劃右平移Step6-Step7	31
圖3.17機器人右平移 Step6-Step7	31
圖3.18規劃左平移 Step1-Step3	32
圖3.19機器人左平移 Step1-Step3	33
圖3.20規劃左平移 Step4-Step5	33
圖3.21機器人左平移 Step4-Step5	34
圖3.22規劃左平移 Step6-Step7	34
圖3.23機器人左平移 Step6-Step7	35
圖3.24規劃順時針旋轉 Step1-Step3	36
圖3.25機器人順時針旋轉 Step1-Step3	36
圖3.26規劃順時針旋轉 Step4-Step5	37
圖3.27機器人順時針旋轉 Step4-Step5	37
圖3.28規劃順時針旋轉 Step6-Step7	38
圖3.29機器人順時針旋轉 Step6-Step7	38
圖3.30 規劃逆時針旋轉 Step1-Step3	39
圖3.31機器人逆時針旋轉 Step1-Step3	40
圖3.32 規劃逆時針旋轉 Step4-Step5	40
圖3.33機器人逆時針旋轉 Step4-Step5	41
圖3.34 規劃逆時針旋轉 Step6-Step7	41
圖3.35 機器人逆時針旋轉 Step6-Step7	42
圖3.36 機器人行走場地	42
圖3.37 機器人路徑圖	42
圖3.38 連續步態截圖	43
圖4.1 攝影機座標與二維影像座標系統	46
圖4.2攝影機座標系與世界座標系	47
圖4.3(a) 攝影機測試場地	49
圖4.3(b) EKF狀態初步估測範例	49
圖4.4 地圖資料庫預存的已知區域特徵	50
圖4.5實測環境	50
圖4.6 機器人連續移動截圖	50
圖4.7 估測x-y平面	51
圖4.8 估測x-z平面	51
圖4.9 估測3D立體圖	52
圖5.1 人形機器人步態動作程序	53
圖A.1 人形機器人	56
圖A.2 AGB65-RSC伺服機控制器	57
圖A.3 研揚工業電腦PFM-5401	58

 
表目錄

表2.1 Right leg DH parameters	04
表2.2 Left leg DH parameters	05
表2.3 立正姿態的步態角度與控制命令初始值	19
表3.1 微蹲將重心放在左腳上並且抬右腳	23
表A.1人形機器人規格	56
表B.1直行步態Step1補償	59
表B.2直行步態Step2補償	59
表B.3直行步態Step3補償	59
表B.4直行步態Step4補償	60
表B.5直行步態Step5補償	60
表B.6直行步態Step6補償	60
表B.7直行步態Step7補償	61
表B.8直行步態Step8補償	61
表B.9直行步態Step9補償	61
表B.10直行步態Step10補償	62
表B.11直行步態Step11補償	62
表B.12直行步態Step12補償	62
表B.13右平移步態Step1補償	63
表B.14右平移步態Step2補償	63
表B.15右平移步態Step3補償	63
表B.16右平移步態Step4補償	64
表B.17右平移步態Step5補償	64
表B.18右平移步態Step6補償	64
表B.19右平移步態Step7補償	65
表B.20左平移步態Step1補償	65
表B.21左平移步態Step2補償	65
表B.22左平移步態Step3補償	66
表B.23左平移步態Step4補償	66
表B.24左平移步態Step5補償	66
表B.25左平移步態Step6補償	67
表B.26左平移步態Step7補償	67
表B.27右旋轉步態Step1補償	67
表B.28右旋轉步態Step2補償	68
表B.29右旋轉步態Step3補償	68
表B.30右旋轉步態Step4補償	68
表B.31右旋轉步態Step5補償	69
表B.32右旋轉步態Step6補償	69
表B.33右旋轉步態Step7補償	69
表B.34左旋轉步態Step1補償	70
表B.35左旋轉步態Step2補償	70
表B.36左旋轉步態Step3補償	70
表B.37左旋轉步態Step4補償	71
表B.38左旋轉步態Step5補償	71
表B.39左旋轉步態Step6補償	71
表B.40左旋轉步態Step7補償	72

[1]	Davison, A.J., I.D. Reid, N.D. Molton, and O. Stasse, 2007, “MonoSLAM Real Time Single Camera SLAM,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol.29, no.6, pp.1052-1067.
[2]	Dellaert, F., D. Fox, W. Burgard, and S. Thrun, 1999 ,Monte Carlo Localization for Mobile Robots, IEEE International Conference on Robotics and Automation (ICRA99).
[3]	Denavit, J. and R.S. Hartenberg, 1955, “A Kinematic Notation for Low Pair Mechanisms Based on Matrices,” ASME Journal of Applied Mechanisms, pp.215-221.
[4]	Dissanayake, M.W.M.G., P. Newman, S. Clark, H. Durrant-Whyte, and M. Csorba, 2001, A Solution to the Simultaneous Localization and Map Building (SLAM) Problem, IEEE Transactions on Robotics and Automation, vol.17, no. 3, pp. 229–241.
[5]	Durrant-Whyte, H., and T. Bailey, 2006, Simultaneous Localization and Mapping: Part 1, IEEE Robotics and Automation Magazine.
[6]	Durrant-Whyte, H., and T. Bailey, 2006, Simultaneous Localization and Mapping: Part 2, IEEE Robotics and Automation Magazine.
[7]	Hutchinson, S., G.D. Hager, and P.I. Corke, 1996 , “A Tutorial on Visual Servo Control,” IEEE Transactions on Robotics and Automation, vol.12, no.5, pp.651-670.
[8]	Ho, K.L. and P. Newman, 2007, Detecting Loop Closure with Scene Sequences, International Journal of Computer Vision, vol. 74, no.3, pp.261-286.
[9]	Kalman, R.E., 1960, A New Approach to Linear Filtering and Prediction Problems, ASME.
[10]	Montiel, J.M.M., J. Civera, and A.J. Davison, 2006, Unified Inverse Depth Parametrization for Monocular SLAM, Proc. Robotics: Science and Systems.
[11]	Murphy, K., and S. Russell, 2001, Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks, in Sequential Monte Carlo Methods in Practice, Springer Verlag.
[12]	Pfaff, P., R. Triebel, and W. Burgard, 2007, An efficient extension to elevation maps for outdoor terrain mapping and loop closing, The International Journal of Robotics Research, vol.26, no.2, pp.217-230.
[13]	Sciavicco, L., and B. Siciliano, 1996, “Modelling and Control of Robot Manipulators,”1st Edition, McGraw-Hill, New York, NY.
[14]	Smith, R., M. Self, and P. Cheeseman, 1990, “Estimating Uncertain Spatial Relationships in Robotics,” In Autonomous Robot Vehicles, I.J. Cox and G.T. Wilfong, Eds., Springer-Verlog, pp.167-193.
[15]	Thrun, S., 2002, 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.
[16]	Thrun, S., 2003, Learning Occupancy Grid Maps With Forward Sensor Models, Autonomous Robots, vol.15, pp.111-127.
[17]	Tsai, R.Y., 1987, 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.
[18]	Welch, G., and G. Bishop, 2006, An Introduction to the Kalman Filter, Department of Computer Science, University of North Carolina.
[19]	蔡鎮全，2007，PC-based控制之人形機器人的同時定位與建圖，淡江大學機械與機電工程學系碩士論文。
[20]	王人蔚，2008，具單眼視覺的小型人形機器人之自然特徵式同時定位與建圖，淡江大學機械與機電工程學系碩士論文。
[21]	朱溶琦，2009，影像特徵初始化及其在機器人同時定位與建圖的應用，淡江大學機械與機電工程學系碩士論文。