具機械手臂之移動機器人系統整合與物件搬移控制器設計

全文

(1)國立臺灣師範大學科技與工程學院電機工程學系碩士論文 Department of Electrical Engineering College of Technology and Engineering. National Taiwan Normal University Master's Thesis. 具機械手臂之移動機器人系統整合與物件搬移控制器設計 Integration of Mobile Robot System with Robot Arm and Design of Object Moving Controller. 蔡坤育 Tsai, Kun-Yu 指導教授﹕呂藝光博士 Advisor: Yih-Guang Leu, Ph.D. 中華民國 109 年 7 月 July 2020.

(2) 誌. 謝. 從職場回歸研究所念書的我，這兩年碩士生涯感觸非常深，以前擔任電配工程師時，複雜的電力控制操作自動化產業設備，當時的我面對如此複雜的配電盤和控制系統是非常好奇以及興奮，但工作期間卻很難有時間讓我向公司內部研發控制的工程師們請教學習，就職期間也很感謝老闆們、主管們以及各位工程師前輩們用心培養以及能力認可，也鼓舞我去研究所深造學習。在碩士班這兩年，我很感謝呂藝光教授，認可我的努力成果，給我一個非常有挑戰性的研究方向，也感謝教授的嚴格指導，讓我更了解控制領域的深層概念。碩一也很榮幸教授安排我到業界與教育部合辦的 AI 計畫之智慧綠能能源組，接觸並認識更前端的研發技術以及博士、博後以及各領域專家的教授們，讓我體驗並學習到非常難得的經驗和知識。另外在實驗室裡，我也擔任了實驗室管理員，打理所有內部的設備財產、跑公文、管理帳單和薪資等等。到了碩二，參與了一些活動以及工作，包含全國無人飛機競賽和電路學實驗課程以及電子學實驗課程助教，這過程也是全新且難得的體驗。雖然這兩年接觸到很多不同領域的控制，剛開始非常的辛苦，但不認輸的我也不願意放棄每一項學習的機會，另外也很感謝實驗室的學長，哲宇、克泓、建華、浩瀚，在我剛開學的時候帶我熟悉環境，當我遇到困難時也提供協助，使我學習和工作上更順利。再來也感謝同學唐凡以及學弟財平、勝斌、祈勳，我們一起學習研究，創造不少回憶，使我的碩士生涯更佳豐富。另外還要感謝我的家人，有了他們的支持和鼓勵，讓我無後顧之憂地完成我的學業和研究，希望將來有能力可以報答他們的栽培之恩。接下來即將步入下一個階段，期許自己能為社會貢獻一份心力，將自己的所學發揮出來。坤育謹致於中華民國一零九年六月. i.

(3) 具機械手臂之移動機器人系統整合與物件搬移控制器設計. 學生:蔡坤育. 指導教授:呂藝光. 國立臺灣師範大學電機工程學系碩士班. 摘要. 本論文開發以具機械手臂之移動機器人，其中機械手臂具有 4 個自由度，機械手臂上的四顆馬達，可驅動關節伸縮、旋轉及夾取。另外四輪移動機器人，可驅動輪子前進、後退及旋轉。因此該移動機器人在三維空間中自由移動，並且搬移物件到指定區域內。具機械手臂之移動機器人是以微控制器作為控制的核心，透過霍爾元件偵測左右輪速度、超音波感測器偵測障礙物與影像偵測判斷物件的位置，最後將資料回傳至微控制器，另外本文由順向、逆向運動學來推導機械手臂上各關節變數以及 PID 控制器讓左右輪等速，使得具機械手臂之移動機器人可在室內自由移動、避障且可執行物件搬移功能。. 關鍵字: 具機械手臂之移動機器人、PID 控制器. ii.

(4) Integration of Mobile Robot System with Robot Arm and Design of Object Moving Controller Student:Kun-Yu Tsai. Advisor:Yih-Guang Leu. Department of Electrical Engineering National Taiwan Normal University. ABSTRACT. This paper develops a mobile robot with a robot arm. The robotic arm has 4 degrees of freedom, the four motors on the robotic arm can drive the joints to expand, contract and rotate. In addition, the four-wheel mobile robot can drive the wheels forward, backward and rotate. Therefore, the mobile robot moves freely in a three-dimensional space and moves objects to a designated area. The mobile robot with a robot arm uses a microcontroller as the core of control. The Hall sensors, ultrasonic sensors, and image sensor are used to detect the speed of the left and right wheels, obstacles, and the position of the object, respectively. Then, the sensor data are sent to the microcontroller. In addition, in this paper, the forward and reverse kinematics are used to derive the joint variables on the robotic arm and the PID controller to make the left and right wheels have constant speed, so that the mobile robot with a robotic arm can move freely indoors, avoid obstacles, and perform object movement functions.. Keywords: Mobile robot with a robot arm , PID controller. iii.

(5) 目. 謝. 錄. 辭.......................................................................................................................i. 摘要....................................................................................................................ii Abstract...................................................................................................................iii 目. 錄...................................................................................................................iv. 表目錄...................................................................................................................vi 圖目錄..................................................................................................................vii 第一章緒論..............................................................................................................1 1.1 研究動機與背景..........................................................................................1 1.2 研究目的......................................................................................................2 1.3 研究方法......................................................................................................2 1.4 論文架構......................................................................................................3 第二章. 文獻探討與回顧....................................................................................4. 2.1 具機械手臂之移動機器人應用與原理......................................................4 2.2 移動機器人運動學模型與控制應用..........................................................5 2.2.1. 移動機器人運動學模型..................................................................5. 2.2.2. PID 速度控制...................................................................................7. 2.3 影像辨識應用工程......................................................................................8 2.4 機械手臂運動學模型................................................................................10 2.4.1 順向運動學......................................................................................10 2.4.2 逆向運動學......................................................................................11 第三章. 具機械手臂之移動機器人之整合與控制..........................................12. 3.1 硬體架構....................................................................................................12 3.2 軟體架構....................................................................................................35 3.2.1 移動機器人軟體架構......................................................................36 3.2.2 顏色影像辨識軟體架構..................................................................38 iv.

(6) 3.2.3 機械手臂軟體架構..........................................................................39. 第四章. 控制方法與模擬驗證..........................................................................40. 4.1 具機械手臂之移動機器人室內移動模擬實驗........................................41 4.2 具機械手臂之移動機器人抓取物件模擬實驗........................................47 4.2.1 順向運動學......................................................................................47 4.2.2 逆向運動學......................................................................................50 4.2.3 順向運動學模擬實驗......................................................................55 4.2.4 逆向運動學模擬實驗......................................................................57 第五章. 實驗結果與討論..................................................................................62. 5.1 具機械手臂之移動機器人控速實驗........................................................62 5.1.1 具機械手臂之移動機器人左右輪控速實驗..................................62 5.1.2 具機械手臂之移動機器人影像辨識實驗......................................67 5.1.3 具機械手臂之移動機器人抓取物件實驗......................................70 5.2 具機械手臂之移動機器人系統整合實驗................................................80 第六章. 結論與未來展望..................................................................................96. 6.1 結論...........................................................................................................96 6.2 未來展望...................................................................................................96 參考文獻..................................................................................................................97 自傳........................................................................................................................100 學術成就..............................................................................................................101. v.

(7) 表目錄. 表 3-1. 移動機器人相關參數...........................................................................15. 表 3-2. Arduino Mega 參數表..........................................................................16. 表 3-3. 霍爾元件參數表...................................................................................17. 表 3-4. 馬達參數表...........................................................................................18. 表 3-5. 馬達驅動器參數表...............................................................................19. 表 3-6. HC-SR04 超音波模組參數表..............................................................21. 表 3-7. 機械手臂實體參數表...........................................................................22. 表 3-8. L298N 馬達驅動器參數表...................................................................23. 表 3-9. DC to AC 轉換器參數表......................................................................24. 表 3-10. 四路繼電器參數表...............................................................................25. 表 3-11. 2GN6K 交流馬達參數表.....................................................................26. 表 3-12. C170 網路攝影機參數表.....................................................................27. 表 4-1. 車輛整合參數表...................................................................................41. 表 4-2. D-H 參數表...........................................................................................47. 表 4-3. 機械手臂 PID 權重參數表...................................................................59. 表 5-1. 移動機器人的整合參數表...................................................................62. 表 5-2. 實際轉速對應表...................................................................................63. 表 5-3. 沿著 A 位置路線移動避障..................................................................81. 表 5-4. 影像辨識追蹤.......................................................................................88. 表 5-5. 手臂抓取物件到指定位置...................................................................90. vi.

(8) 圖目錄. 圖 1-1. 移動機器人 Shakey 示意圖.................................................................1. 圖 2-1. 四輪滑動轉向移動車輛幾何圖............................................................5. 圖 2-2. PID 系統架構圖.....................................................................................7. 圖 2-3. 顏色圖像座標系....................................................................................9. 圖 3-1. 具機械手臂之移動機器人實體圖......................................................13. 圖 3-2. 具機械手臂之移動機器人硬體架構圖...............................................14. 圖 3-3. 移動機器人實體圖內側.......................................................................15. 圖 3-4. Arduino Mega 控制板...........................................................................16. 圖 3-5. 霍爾元件 3144E....................................................................................17. 圖 3-6. 移動車輛馬達.......................................................................................18. 圖 3-7. 馬達驅動器...........................................................................................19. 圖 3-8. VDC2450RC 脈衝輸入規則................................................................20. 圖 3-9. HC-SR04 超音波模組..........................................................................21. 圖 3-10. 機械手臂實體圖...................................................................................22. 圖 3-11. L298N 直流馬達驅動器.......................................................................23. 圖 3-12. DC to AC 轉換器..................................................................................24. 圖 3-13. 四路繼電器模組...................................................................................25. 圖 3-14. 2GN6K 交流馬達..................................................................................26. 圖 3-15. C170 網路攝影機..................................................................................27. 圖 3-16. 鋁板……………...................................................................................28. 圖 3-17. 鋁塊如紅色框所示，與鋁板鎖在一起...............................................28. 圖 3-18. 出線口如黃色箭頭所示，將車內的線拉出來.....................................29. 圖 3-19. 圓盤底座與鋁板鎖在一起...................................................................29. 圖 3-20. 交流馬達與螺桿連結，如紅框所示.....................................................30. 圖 3-21. 特製螺母………………………………...............................................30. vii.

(9) 圖 3-22. 將特製螺母與連桿鎖在一起，如箭頭所示.........................................31. 圖 3-23. 齒輪咬合的部分如紅色框內所示.......................................................31. 圖 3-24. 馬達套入齒輪驅動爪子，如紅框內所示.............................................32. 圖 3-25. 具機械手臂之移動機器人之接線圖...................................................32. 圖 3-26. 加裝在爪子上的 Webcam....................................................................33. 圖 3-27. 在機械手臂的左後方架設筆電……...................................................33. 圖 3-28. 具機械手臂之移動機器人整線圖.......................................................34. 圖 3-29. 具機械手臂之移動機器人軟體架構圖...............................................35. 圖 3-30. 等速模式之程式流程圖……...............................................................36. 圖 3-31. 移動機器人程式流程圖……………...................................................37. 圖 3-32. 影像辨識程式流程圖...........................................................................38. 圖 3-33. 機械手臂程式流程圖...........................................................................39. 圖 4-1. 具機械手臂之移動機器人 PID 控制器...............................................40. 圖 4-2. 左右輪速度控制量與輸出量...............................................................42. 圖 4-3. 路徑循跡路線.......................................................................................43. 圖 4-4. 右輪速度...............................................................................................43. 圖 4-5. 左輪速度...............................................................................................44. 圖 4-6. 室內環境地圖.......................................................................................44. 圖 4-7. 物件放置圖...........................................................................................45. 圖 4-8. A 位置循跡路線圖...............................................................................45. 圖 4-9. B 位置循跡路線圖...............................................................................46. 圖 4-10. 機械手臂結構.......................................................................................47. 圖 4-11. 手臂俯瞰圖第一象限...........................................................................50. 圖 4-12. 手臂俯瞰圖第四象限...........................................................................50. 圖 4-13. 手臂俯瞰圖第二象限...........................................................................51. 圖 4-14. 手臂俯瞰圖第三象限...........................................................................51. 圖 4-15. 座標系 1 結構圖...................................................................................52. viii.

(10) 圖 4-16. 座標系 1 的二維座標圖.......................................................................52. 圖 4-17. 座標系 1 的幾何圖形...........................................................................53. 圖 4-18. 座標系 3 的幾何圖形……...................................................................54. 圖 4-19. 關節相對座標系...................................................................................55. 圖 4-20. 爪子中心與目標物重疊.......................................................................56. 圖 4-21. 爪子中心與目標物無重疊...................................................................56. 圖 4-22. 手臂初始狀態.......................................................................................57. 圖 4-23. 追蹤 X0 -Y0 平面之第一象限目標物示意圖.......................................57. 圖 4-24 追蹤 X0 -Y0 平面之第二象限目標物示意圖..........................................58 圖 4-25 追蹤 X0 -Y0 平面之第三象限目標物示意圖..........................................58 圖 4-26 追蹤 X0 -Y0 平面之第四象限目標物示意圖..........................................59 圖 4-27 各關節控制量.........................................................................................60 圖 4-28 各關節輸出量….....................................................................................60 圖 4-29 各關節速度量….....................................................................................61 圖 4-30 關節 1 相對於左右輪速度......................................................................61 圖 5-1. 左輪速度.................................................................................................63. 圖 5-2. 右輪速度.................................................................................................64. 圖 5-3. A 路線室內移動之左輪速度.................................................................65. 圖 5-4. A 路線室內移動之右輪速度.................................................................65. 圖 5-5. A 路線室內移動之軌跡…….................................................................66. 圖 5-6. 字元區域.................................................................................................67. 圖 5-7. 轉換實際的字元區域.............................................................................67. 圖 5-8. 原圖.........................................................................................................68. 圖 5-9. 掩膜處理的圖像.....................................................................................68. 圖 5-10 由 Python 視窗顯示字元區域................................................................69 圖 5-11. 基底關節旋轉角度值...........................................................................70. 圖 5-12. 左輪速度值...........................................................................................71. ix.

(11) 圖 5-13. 右輪速度值...........................................................................................71. 圖 5-14. 關節 2(肩膀)的角度值..........................................................................72. 圖 5-15. 關節 3(伸縮)的距離值.........................................................................73. 圖 5-16. 關節 4(手腕)的角度值.........................................................................73. 圖 5-17. 物件在右側的實際圖...........................................................................74. 圖 5-18. 物件在右側的圖像…...........................................................................74. 圖 5-19. 循跡右側物件時的 Pitch 角度變化…..................................................75. 圖 5-20. 循跡右側物件時的 Yaw 角度變化......................................................75. 圖 5-21. 物件在左側的實際圖...........................................................................76. 圖 5-22. 物件在左側的圖像………...................................................................76. 圖 5-23. 循跡左側物件時的 Pitch 角度變化......................................................77. 圖 5-24. 循跡左側物件時的 Yaw 角度變化......................................................77. 圖 5-25. 物件在正前方的實際圖.......................................................................78. 圖 5-26. 物件在正前方的圖像...........................................................................78. 圖 5-27. 循跡正前方物件時的 Pitch 角度變化..................................................79. 圖 5-28. 循跡正前方物件時的 Yaw 角度變化..................................................79. 圖 5-29. 實驗流程圖…………...........................................................................80. 圖 5-30. 循跡物件時的 Pitch 角度變化……......................................................93. 圖 5-31. 循跡物件時的 Yaw 角度變化..............................................................94. 圖 5-32. 螺桿上有明顯的鏽鐵及污漬...............................................................95. x.

(12) 第一章. 緒論. 1.1 研究動機與背景隨著時代科技進步，很多工廠內部的生產線幾乎都是由機械手臂取代繁瑣勞力和危險勞力，在工業時代機械技術提升後，像自動化、遙控也日益成熟，慢慢演變發展機器人的研究，然而自主移動式工業機器人也具有高度的靈活性，其特性可不受工作區域的限制，未來可應用於如航太業、汽車業等等複雜加工精密的技術。. 世界上第一個移動機器人 Shakey ，如圖 1-1 所示，是由查理·羅森（Charlie Rosen）於 1956-1972 年在美國斯坦福研究所研究出來的。Shakey 首次應用人工智慧技術，能夠自行感知、建立環境模型、行為規劃，最後再執行任務。Shakey 裝載電子攝影機、三角測距儀、碰撞感測器以及驅動馬達，並藉由無線通訊的方式，由兩台計算機控制。當時計算機運算速度很慢，使得 Shakey 需要數小時的時間來偵測和分析環境，並規劃移動路徑。雖然 Shakey 看似笨拙，但此成果也影響了許多後續的研究[1]。. 圖 1-1 移動機器人 Shakey 示意圖[1]. 1.

(13) 本研究所使用的平台為具機械手臂之移動機器人，其中移動機器人前後左右對稱，在運動過程中會有滑動和滾動的情況，另外也不具有轉向裝置，若要進行轉向可改變左右兩側輪速差。機械手臂為四個關節與一個爪子末端器所組成，其中移動機器人可視為手臂的基底關節，利用輪速差旋轉手臂到達指定的位置與方向，所以共有 5 個自由度，另外也有許多基於移動機器人的運動學控制研究[2]。本論文透過結合微控制器模組、影像辨識、霍爾元件與超音波感測器等功能，實驗觀察具機械手臂之移動機器人的運作，因需結合各關節的速度控制與搬移物件功能等等，如何結合複雜運算，整合所有機構、硬體和軟體之間的溝通和控制成為一大難題，最終透過實驗來驗證功能的完整性。. 1.2 研究目的本論文的研究目的在於移動機器人加裝機械手臂，結合所有的軟硬體，可以達到無人智慧化的移動式機械手臂。然而現今工廠產線上的機械手臂基座大多數固定在某個靜止定點上，手臂活動範圍依然有限。所以此研究使得機械手臂可成為能在三維空間內自由活動的移動載具，並且到達物件所在位置，最後機械手臂搬移目標物件至指定區域內，透過不同的感測器，系統間的互相通訊以及監控，使得未來使用者能夠對具機械手臂之移動機器人做靈活運用，進一步增加使用意願，且達到即時監看。. 1.3 研究方法具機械手臂之移動機器人為受控平台，主要利用微控制器作為控制的核心，對於移動機器人而言，需要搭配霍爾元件，在減速器上裝上強力磁鐵，利用 PID 控制器來控制左右兩側的輪子速度，輸出特定脈波至驅動器，使得驅動器能夠輸出對應的脈寬來驅動馬達，完成速度控制；另外對於機械手臂而言，Webcam 鏡頭則作為顏色辨識感測器，偵測到的目標顏色座標值回傳給控制平台作為參考值，使得手臂去追蹤目標值，讓爪子抓取物件，並搬移到指定區域內。具機械手臂之移動機器人的整合，首先移動機器人具有前後移動、左右原地旋轉以及避障. 2.

(14) 模式的功能，以上運動模式可使其在規劃路線上等速自由行走，走到指定目標物附近時，開啟筆記型電腦連接的 Webcam 攝影機，攝影機捕捉到的目標值再回傳到微控制器，經由逆向運動學的公式運算，找出每個關節需要旋轉/平移的變量，最後讓爪子搬移目標物件至指定區域內。. 1.4 論文架構本論文共分為六個章節，各章內容分述如下: 第一章緒論:敘述本論文之研究背景及動機、研究方法、論文架構。第二章文獻探討與回顧:探討具機械手臂之移動機器人運動學模型和速度控制，研究追蹤目標物的方法。第三章具機械手臂之移動機器人之整合與控制:此章節會介紹硬體架構、軟體架構及關節速度控制，如何整合並且實際應用於具機械手臂之移動機器人。第四章控制方法與模擬驗證:此章節會介紹控制方法與模擬驗證，模擬驗證會使用 Matlab 觀察各關節的控制量與輸出量以及具機械手臂之移動機器人的三維空間運動模式。第五章實驗結果與討論:實驗所使用的具機械手臂之移動機器人，套入參數並且整合實驗，最後對實驗數據做出分析和比較。第六章結論與未來展望: 總結本論文提出的方法，並且提出未來能夠修正改進的研究方向。. 3.

(15) 第二章. 文獻探討與回顧. 2.1 具機械手臂之移動機器人應用與原理移動式機械手臂在產業自動化的應用已經相當廣泛，如工業機器人、銲接機器人與塗裝機器人等等，具有模仿人類的行為，可以完成各種作業，由於各個國家產業分布的不同，對於機械手臂的需求量也有差異，另外具機械手臂之移動機器人的應用研究非常多，文獻參考如[3]-[7]，不同的關節功能，都有相對應的工作模式以及運動學推導，主要讓機械手臂可在三維空間上自由移動，路徑追蹤至搬移物件，移動機器人可利用輪速差進行不同的運動模式，如前進、後退及旋轉，旋轉的模式也可以應用於機械手臂，視為基座關節，使得手臂旋轉至目標物的方向。機器手臂技術的發展，在 1980 年成功應用汽車製造業等等，在機器人技術領域是應用最廣泛的自動化機械裝置。許多工業危險的工作，如焊接、組裝、噴漆等勞力，都能以機器手臂所取代。機械手臂是目前使用最廣泛的自動化裝置，除了用於工業操作上，商業、農業、醫療等等領域都會發現其應用裝置。從機械手臂的運動原理，可分為直角座標型、圓柱座標型、極座標型、關節型座標種類。對於所有不同型態的手臂都有不同的運動學模型，文獻參考如 [8]-[10]，也有許多手臂順向、逆向運動學推導流程以及控制應用工程，書籍文獻如[11]，利用順向、逆向運動學推導不同的座標系轉換。順向運動學為找出相對目標物的向量，觀察末端點中心與目標物的位置，逆向運動學則利用幾何方法或 Jacobian 矩陣公式反推每個自由度需要旋轉多少角度，或者平移多少距離，最後再使用控制設計系統讓手臂平穩又快速的抓取目標物。. 4.

(16) 2.2 移動機器人運動學模型與控制應用 2.2.1 移動機器人運動學模型移動機器人使用的模型為非轉向式四輪車[12]，透過左右輪速差以滑動的方式進行轉向，又稱滑動轉向移動車輛(Wheel Skid-Steered Mobile Car)，此模型有非常多研究與實驗[13]-[17]，其幾何圖如圖 2-1 所示。. 圖 2-1 四輪滑動轉向移動車輛幾何圖. 汽車在世界各地是屬於普遍的交通工具，1920 年開始到現今對於汽車自動化還在持續開發中，對於人工智慧的時代，汽車也慢慢演變為全自動駕駛，雖然全自動駕駛汽車在大部分的國家仍未普遍化，但還是有許多研究單位還在持續開發中。. 5.

(17) 在 1984 年，卡內基美隆大學推動 Navlab 計畫與 ALV 計畫；1987 年，梅賽德斯-賓士與德國慕尼黑聯邦國防大學共同推行尤里卡普羅米修斯計畫。從此以後，許多大型公司與研究機構開始製造可運作的自動駕駛汽車原型。21 世紀以後，伴隨著資訊科技的進步，更是突飛猛進，全自動駕駛的車輛在試驗車輛上已經被製造出來，特斯拉汽車率先推出特定環境下的自動駕駛汽車[18]。移動機器人的特徵為左右完全對稱，必須透過左右輪速差來改變行進方向，首先令 ω1、ω2、ω3、ω4 分別為左前、左後、右前、右後輪的旋轉角速度，並且假設左側前後輪速視為一致，另外右側前後輪速視為一致，可得出下式. L  1  2 , R  3  4. (2-1). 令 I(X,Y,Z)為世界座標軸 B(x,y,z)為車裡座標軸，其中 ICR、ICRl、ICRr 分別代表車體瞬心、左側輪子接觸點瞬心、右側輪子接觸點瞬心，且平行於 y 軸，假設 ICR(xgc,ygc,0)、ICRl(xlc,ylc,0)、ICRr(xrc,yrc,0) 對應到三個瞬心在 B(x,y,z)的位置，可得出其前進的速度為. xlc  xrc  xgc  . vGy. . (2-2). 透過[19]可以得知四輪滑動轉向移動車輛之運動方程式，本文以此為參考計算出相關轉向角對應的左右輪角速度。  W vGx  r      W  2    1. W  2 1.  L   R  . (2-3). r 為車輛的半徑、α 式 ICR 之地形依賴參數，vGX 為車體沿著前方之行進速度，ϕ 表示車體轉向角度，W 為車輛橫向軸距(Wheel Base)。. 6.

(18) 2.2.2 PID 速度控制本論文移動機器人左右穩定控速是使用 PID 控制器，此方法是控制領域中最常見的控制器[20]，使用上也非常簡單。PID 控制器透過比例單元 K p、積分單元 Ki 與微分單元 Kd 相加總所組成的，也需要調整這三個參數達到最穩定的訊號響應，再透過資料回授來計算下一刻的控制力，如式(2-4)所示： t. u  t   K p e  t   Ki  e  d  K d 0. d e t  dt. (2-4). 其中 K p 為比例權重參數、 Ki 為積分權重參數、 Kd 為微分權重參數、 e 為目標與實際之間的誤差值、 t 為當下時間、  為從開始運行到當下的累積時間。若只使用比例控制器時，會產生穩定值但未到達參考值的狀況，此現象稱作穩態誤差，若要消除穩態誤差則需要使用積分項與積分權重參數的相乘，積分項會積累每個取樣時間內的誤差值，積分項乘上積分權重參數會與前述的比例控制的結果相加使得系統到達參考值，當誤差項為負值時可能無法即時將系統輸出拉回而產生過衝問題，可以使用微分項與微分權重參數的相乘來解決，微分項取決於微分變化的斜率，當斜率為負值再乘上微分權重參數對系統產生一個相反的值，使其輸出不會迅速爬升，增加系統的阻尼，讓訊號輸出更快到達穩定平衡並且減少震盪。以下是 PID 的系統架構圖如圖 2-2 所示。 K p e(t ). + Ref_V. +. e(t). Σ. +. t. K i  e( )d. Σ. 0. -. + Kd. d e(t ) dt. 圖 2-2 PID 系統架構圖. 7. Driver. Motor.

(19) 2.3 影像辨識應用工程科技發展蒸蒸日上，現今的人工智慧時代，科技結合影像辨識的應用非常多，甚至提升人們的生活品質以及工作效率，像是自駕車、安全監控以及產品瑕疵檢測等等應用，都和影像深度學習息息相關，最常看到的深度學習應用如卷積神經網路[21](Convolutional Neural Network)以及遞歸神經網路[22](Recurrent Neural Network)。關於電腦影像辨識，Intel 所開發的開源影像辨識系統 OpenCV 便是不可或缺的主角。這套系統讓開發人員在進行辨識系統的開發時相較容易，同時也促進相關技術的發展。歐尼克斯實境互動的創辦人許哲豪(Jack) 曾經提到，若電腦辨識系統要仿效人腦得到相近的視覺感受，則需要經過複雜的運算，以當紅的虛擬實境（VR）、擴增實境（AR）或是結合兩者的混合實境（MR）來說，都需要計算影像中各個物件的運動狀況、環境的影像復原和重建等[23]。立體顯示技術[24]發展至今已有一百多年的歷史，從最早的手繪立體圖案到現在的立體電影，使用的技術日新月異。相機模型參數也有許多文獻介紹如 [25][26]，要將世界座標投影到對應的影像座標需要經過各項修正，與內部參數矩陣、外部參數矩陣都有關係，再利用這些模型去判讀相機畫面與實際物件的相對尺寸，最後將其應用實際工程上。三色 RGB 是由光的原理設計，分別紅綠藍三種顏色，另外這三色也可混成其他顏色，如紅綠色混合為黃色，最後轉換 HSV 分別色彩、飽和度、明暗程度，也根據[27]得知 RGB∈[0, 255]，H∈[0o, 180o]，S，V∈[0, 1]，讓 Max = max(R,G,B) 和 Min=min(R,G,B)，當 Max≠Min，RGB 空間轉換 HSV 空間定義為  R '  ( Max  R) / ( Max  Min)  G '  ( Max  G ) / ( Max  Min)  B '  ( Max  B) / ( Max  Min) . 8. (2-5).

(20) 因此.  H  30  H '   S  ( Max  Min) / Max  V  Max / 255 . (2-6). 其中 (5  B '), R  Max and G  Min  (1  G '), R  Max and G  Min   (1  R '), G  Max and B  Min H' (3  B '), G  Max and B  Min  (3  G '), B  Max and R  Min   (5  B '), others. (2-7). 再將指定目標顏色取上下限值，依照攝影機拍出來的圖像尺寸大小抓出每個目標顏色像素的座標值，最後再取二維座標系的橫軸及縱軸的平均，抓取目標中心位置，如圖 2-3 所示，二維平均值如(2-8)所示，n 代表目標顏色的像素量。 n. x _ center .  x(i) i 1. n n. y _ center .  y(i) i 1. n. 圖 2-3 顏色圖像座標系. 9. (2-8).

(21) 2.4 機械手臂運動學模型不同型態的機械手臂都有不同的運動學模型，市面上最常見的六軸機械手臂為 Stanford 機械手臂以及 SCARA 機械手臂。 1969 年斯坦福大學的 Victor Scheinman 發明了一台名為“Stanford Arm”的電動六軸機械臂。它可以準確及靈巧地伸展，能夠進行較複雜及精確的工作，包括裝配部件和焊接等。其後 Scheinman 更為麻省理工大學 AI 實驗室設計了第二台機械臂，這是第一個具有視覺傳感器和能夠識別定位的機械人系統。他深信其設計有著很大的發展潛力，於是他將設計賣給 Unimation 公司藉以得到 General Motors 的支持繼續進行研發，終於在 1978 年開發出可編程的通用機械臂 (Programmable Universal Manipulation Arm)[28]。 1978 年，日本山梨大學（University of Yamanashi）的牧野洋（Hiroshi Makino）發明了選擇順應性裝配機器手臂（Selective Compliance Assembly Robot Arm，SCARA）。SCARA 機器人具有四個軸和四個運動自由度（包括 X、Y、Z 方向的平動自由度和繞 Z 軸的轉動自由度）。SCARA 系統在 x、y 方向上具有順從性，而在 Z 軸方向具有良好的剛度，此特性特別適合於裝配工作。SCARA 的另一個特點是其串接的兩桿結構，類似人的手臂，可以伸進有限空間中作業然後收回，適合於搬動和取放物件，如積體電路板等[29]。以下會開始分別介紹順向運動學及逆向運動學是如何推導與應用。 2.4.1 順向運動學手臂關節共有兩種功能，分別旋轉(Revolute)與平移(Prismatic)，不同的功能與關節對應位置，使得相對座標系矩陣也有所不同，最常應用的矩陣為 D-H 表示法，D-H 表示法為式(2-9)所示，θ為關節角度，d 為連桿偏差，a 為連桿長度， α為連桿扭角。此公式的簡化符號 si 為 sin  i ，c i 為 cos i ， s i 為 sin  i ， c i 為 cos  i 。. 10.

(22) ci  s  i 0  0.  si c i ci c i s i 0. si s i ci s i c i 0. ai ci  ai si  di   1 . (2-9). 可利用 D-H 表示法套入實體機械手臂的參數，定義各關節的方向與位置，再將各關節的相對座標系矩陣相乘，推導出末端點(爪子)相對於基底關節的座標轉換矩陣，如式(2-10)所示。. H  TN0  A1 (1 ) A2 (2 ) A3 (3 )...AN (4 ). (2-10). 最後順向運動學座標轉換為式(2-11)以及爪子末端點相對於目標物向量為式 (2-12). p0  T40 p 4. (2-11). p4  (T40 )1 p0. (2-12). 2.4.2 逆向運動學若推導出的順向運動學要反推關節的變數，發現方程式的數量比未知變數還多，此線性系統稱為是過定的(Overdetermined)，變數會有無線多組解，所以文獻 [11]，提供了幾何方法或 Jocobian 矩陣公式推導逆向運動學，其中幾何方法應用甚廣。幾何方法可以利用三角函數、畢氏定理與餘弦定理等等數學公式獨立解出每個關節的變量，再將每個關節變量視為理想變數，最後再輸入 PID 控制器，使其手臂循跡追蹤目標物。. 11.

(23) 第三章. 具機械手臂之移動機器人之整合與控制. 本論文是將具機械手臂之移動機器人為實驗平台，移動機器人會使用到的磁場感測器為霍爾元件，監控左右輪的速度，超音波模組則偵測左側、中間、右側障礙物與實驗平台的距離。另外實驗平台上會使用到的視覺感測器為 Webcam 網路攝影機結合筆記型電腦，從系統畫面捕捉指定目標物，再將具機械手臂之移動機器人上的所有感測器判斷值利用串列通訊以及訊號觸發的方式回傳給微控制器 Arduino Mega 板，一種基於 ATmega2560 的微控制器板，最後再由微控制器輸出控制命令給具機械手臂之移動機器人，讓實驗平台能自主避障以及左右輪等速度控制，手臂也可搬移物件至指定區域內。本章節會對具機械手臂之移動機器人的硬體架構設計、軟體架構設計以及整合的控制方法做詳細解說，會逐一個別介紹所有的元件、軟硬體架構圖、顏色影像辨識、速度控制區塊以及最後的整合。. 3.1 硬體架構本論文透過具機械手臂之移動機器人做為測試平台，實體圖如圖 3-1 所示，其中硬體整合架構圖如圖 3-2 所示，首先移動機器人開始移動，在規劃路徑上行走與避障，移動機器人驅動方式為利用微控制器輸出 PWM 訊號給馬達驅動器，再由馬達驅動器控制左右輪，另外輸出電壓為 36V 分別給左側與右側馬達，才可使其驅動。移動機器人在移動的同時，霍爾元件監控左右輪轉速，利用磁場轉換電壓的功能，透過外部中斷負緣觸發以及計時器整合的方式，去算出左右輪馬達的速度，另外左側、中間以及右側的超音波感測器同時偵測每個方位的距離範圍內是否有障礙物，最後讓移動機器人同時達到避障效果與控速，並且走到指定物件所在的位置上。移動機器人到達物件所在位置時，微控制器透過串列通訊的方式告知筆記型電腦開啟 Webcam 攝影機，且向周遭環境進行拍照並存取圖像，存取圖像後系統開始進行顏色追蹤辨識，追蹤到的中心座標值再繼續用串列通訊的方式回傳給微控制器，接收到座標值之後，開始驅動機械手臂上的關節馬達以及移動機器人的 12.

(24) 左右輪。機械手臂上的馬達有三顆直流(DC)馬達以及一顆交流(AC)馬達，直流馬達則用微控制器輸出的 PWM 訊號控制轉速，交流馬達則使用繼電器模組，先讓電池經過 DC to AC 轉換器，將直流 12V 轉換為交流 110V 訊號，再使交流電源線的火線(L 線)接上兩顆繼電器上，繼電器上的觸發點以及交流的中性線(N 線)則分別接回交流馬達上的 L 線與 N 線，並且讓微控制器輸出訊號控制馬達左右旋轉。最後接收到的物件位置，透過逆向運動學找出各個關節之旋轉與平移關節的變量，使得具機械手臂之移動機器人驅動各關節，使得爪子抓取物件至指定區域內。. 圖 3-1. 具機械手臂之移動機器人實體圖. 13.

(25) Arduino Mega Webcam. Ultrasound. Ultrasound. Motor. PC. R T. Ultrasound. Motor. Relay. Relay. L298 N. Driver. Motor. Hall Sensor. Battery. Battery. Hall Sensor. DC Motor. AC Motor. DC Motor. Battery. L298 N. L298 N. N. L. DC to AC Converter. Motor. Four-Wheel Mobile. Manipulator. 圖 3-2 具機械手臂之移動機器人硬體架構圖. 14. DC Motor Battery.

(26) 圖 3-3 移動機器人實體圖內側. 本研究之移動機器人是由 Inspectorbots 所生產的，其設計是為了讓移動機器人可在不同的地形進行移動，所以需要較大的輪徑與扭力，實體圖內側如圖 3-3，以下是移動機器人的相關參數. 表 3-1 移動機器人相關參數車輛尺寸 : 長 24 吋/寬 15 吋/高 16 吋扭力 : 3000 磅車體重量 : 240 磅爬坡傾斜角 : 35 度最高時速 : 15 英里/小時軸距(Wheelbase) : 17 吋離地間隙 : (Ground Clearance) : 5.5 吋電池 : 12V 鉛酸電池 4 顆. 15.

(27) 圖 3-4 Arduino Mega 控制板. 本研究使用的微控制板為 Arduino Mega 板，實體如圖 3-4 所示，一塊以 ATmega 2560 為核心的微控制器之開發板，能通過 USB 燒錄程式而不需要經過其他外部燒錄器，其相關參數如表 3-2 所示。. 表 3-2 Arduino Mega 參數表型號 : Arduino Mega 2560 R3 尺寸 : 102mm(長)*53.6mm(寬)*15.3mm(高) 重量 : 35g 運行電壓 : 5V 輸入電壓 : 6-20V(建議 7-12V 內) 連接 : USB Type B SRAM : 8KB EEPROM : 4KB 工作頻率 : 16MHz. 16.

(28) 圖 3-5 霍爾元件 3144E. 本研究使用的霍爾元件為 3144E，實體圖如圖 3-5 所示，霍爾元件是屬於半導體製造的磁敏電路，其輸入為感應磁鐵強度並且轉化為輸出電壓。霍爾元件利用磁場的感應，將霍爾元件當作觸發訊號開關使用，將馬達的轉子上裝上兩顆磁鐵讓馬達旋轉，旋轉的過程中，霍爾元件會因為磁場的變化，將類比值透過 LM393 比較器轉換成數位低電平送入 Arduino Mega 板進行次數計算，以求得左右輪轉速，其相關參數如表 3-3 所示。. 表 3-3 霍爾元件參數表. 產品型號 : 3144E 產品尺寸 : 31.5mm(長)*14.0mm(寬)*9.0mm(高) 產品重量 : 3g 工作電壓 : 3.3-5V 操作點 : 感應磁通量輸出低電平，否則輸出高電平數位輸出 : LM393 比較器功能 : 可用於電機測速、位置檢測等. 17.

(29) 圖 3-6 移動車輛馬達. 本論文移動機器人所使用的馬達型號為 NPC-T64，其外觀示意圖如圖 3-6 所示，該馬達上有加裝減速器，其相關參數如表 3-4 所示。. 表 3-4 馬達參數表工作電壓 : 24V RPM : 230RPM 減速器特性 : 20:1 齒輪比重量 : 13 磅特性 : 電壓加高至 36V 時，增加 50%轉速. 18.

(30) 圖 3-7 馬達驅動器. 本論文所使用的馬達驅動器為 VDC2450，為 RoboteQ 公司研發用來應用在高壓直流馬達的速度控制，其應用範圍在重型機器人、陸地與水下機器人、工業控制與液壓控制，示意圖如圖 3-7 所示，參數表如表 3-5 所示。. 表 3-5 馬達驅動器參數表 1. RS232，0~5V RC pulse 脈衝命令模式 2. 每個通道輸出最大電流 150A 3. 每個通道有正轉與反轉電位輸出. 19.

(31) 透過 VDC2450 內的 RS232 串列通訊的方式傳送 RC 脈衝去驅動左右側馬達，這裡的 RC 脈衝驅動方式為 PWM 訊號抓取指定的正電壓脈寬範圍，意指若當馬達驅動器接收到 1ms~1.5ms 正電壓脈寬時，馬達會正轉，若接收到的正電壓脈寬訊號為 1.5ms~2ms，馬達則會反轉，本研究也發現，若只對一側馬達輸入 PWM 訊號，左右輪都會無法驅動，必須同時向兩側馬達輸入 PWM 訊號，才可使其驅動，另外也發現若左右側馬達若打入相同的 PWM 訊號，兩者旋轉的方向是不同的，意指假設輸入 PWM 訊號為 1.3ms，發現右側輪子會正轉，左側輪子則反轉，所以若要左右輪同時正轉，必須分別寫入不同的 PWM 訊號，PWM 訊號示意圖如圖 3-8 所示。. 圖 3-8. VDC2450RC 脈衝輸入規則. 20.

(32) 圖 3-9. HC-SR04 超音波模組. 本研究車上加裝超音波模組，為了使移動機器人達到避障功能。此超音波模組是一款可精準測距的感測元件，實體如圖 3-9 所示，可用 Arduino Mega 輸出脈寬訊號給超音波模組，模組會發送 8 個 40KHz 的方波，使其對外發出的頻率若經過障礙物的訊號反射，超音波模組接收端會檢測信號有無返回，收回信號並且回傳給 Arduino Mega 板，可利用回傳的脈寬長度與計時中斷功能去判斷距離障礙物多少公尺/公分，去做避障的程式撰寫，其相關參數如表 3-6 所示。. 表 3-6 HC-SR04 超音波模組參數表工作電壓 : DC 5V 輸出電位(1/0) : 5V/0V 精度 : 3mm 距離範圍 : 2~450cm 有效角度 : <15 度觸發輸入信號 : 10us TTL 脈衝尺寸 : 45mm(長)*20mm(寬)*15mm(高). 21.

(33) 圖 3-10 機械手臂實體圖. 當移動機器人到達指定目的地時，則開始準備驅動機械手臂，如圖 3-10 所示，此手臂為學長組裝而成，另外也與移動機器人做結合，形成 RRPR 型態的手臂，共有五個自由度，五個自由度分別為底部四輪車左右輪正反轉、手臂上下擺幅、手臂伸縮平移、手腕上下擺幅以及爪子部分，此手臂部分共有三顆直流馬達以及一顆交流馬達做關節旋轉與平移的功能，其相關參數為表 3-7 所示。. 表 3-7 機械手臂實體參數表關節馬達工作電壓 : DC 12V 手臂尺寸 : 378mm(長)*260mm(寬)*210mm(高) 手臂上下擺幅可調整的角度範圍 : 0~40 度手臂伸縮平移可調整的距離範圍 : 0~78mm 手腕上下擺幅可調整的角度範圍 : 0~107 度爪子離地面高度範圍 : 22~70cm. 22.

(34) 圖 3-11. L298N 直流馬達驅動器. 機械手臂上的每顆直流馬達必須經由馬達驅動器做驅動，如圖 3-11，L298N 模組常用於驅動車子馬達，此驅動器為雙 H 橋模組，一個驅動器可以同時控制兩顆馬達，提供四個數位訊號輸入接腳，另外也有供應 5V 與 12V 電源輸入腳，可以針對不同負載自由切換不同電源模式，最後再經由 Arduino Mega 板輸出 PWM 訊號控制直流馬達的轉速，本研究的機械手臂必須供應 L298N 驅動器 12V 才能驅動手臂各關節，其驅動器相關參數如表 3-8 所示。. 表 3-8 L298N 馬達驅動器參數表尺寸 : 43mm(長)*43mm(寬)*29mm(高) 工作電壓 : 5V 驅動電壓 : 5V~35V 電流 : 0mA~36mA 驅動電流 : 2A 工作溫度 : -20~135 度最大功率 : 25W. 23.

(35) 圖 3-12. DC to AC 轉換器. 本研究由於為了要使得 Arduino Mega 板輸入 PWM 訊號去驅動交流馬達，必須要先讓久大鋰電池直流 12V 轉換為交流 110V，必須要有 DC to AC 轉換器，如圖 3-12 所示，圖 3-12 左圖為俯視圖，右邊上下圖分別為左右側視圖，右上圖為接上久大電池輸出直流電 12V 給轉換器，右下圖為交流 110V 輸出，此轉換器有切換開關以及過載保護裝置，為了保護過載狀態以及短路事件發生，此裝置會自動切斷供電模式，此轉換器的相關參數如表 3-9。. 表 3-9 DC to AC 轉換器參數表尺寸 : 230mm(長)*108mm(寬)*61mm(高) 型號 : EP-600A1 最大功率 : 600W 直流輸入電壓 : 10.5-15.5V 輸出頻率 : 60Hz 輸出電壓 : 110Vrms AC 誤差在 10%左右. 24.

(36) 圖 3-13 四路繼電器模組. 本研究為了讓交流馬達可以用程式控制左右旋轉，這裡使用繼電器模組，如圖 3-13 所示，此種模組有四路繼電器模組，本研究會運用兩路繼電器，分別接上交流電源線的火線(L 線)以及中性線(N 線)，另外兩路繼電器上的觸發點接上交流馬達的火線(L 線)，另外模組有四組信號接腳，偵測低電平為觸發信號，分別觸發四路繼電器，此相關參數如表 3-10 所示。. 表 3-10 四路繼電器參數表尺寸 : 75mm(長)*55mm(寬)*18.5mm(高) 重量 : 61g 觸發訊號 : 低電平訊號交流電耐壓 : AC 250V 交流電耐流 : AC 10A 直流電耐壓 : 30V 直流電耐流 : 10A. 25.

(37) 圖 3-14. 2GN6K 交流馬達. 本研究的機械手臂馬達使用 2GN6K 交流馬達驅動手臂控制上下擺幅，如圖 3-14 所示，為了要承載高負載重量，使用交流馬達可使其驅動，另外也帶有減速機的效果，此馬達可以運用很多工業工程，如人機介面、工業用螢幕、馬達控制器、機械手臂以及工業控制相關附屬產品等等，其相關參數表如表 3-11。. 表 3-11 2GN6K 交流馬達參數表尺寸 : 15mm(長) 減速比 : 6 最大輸出 : 3.5W 重量 : 0.7kg 最大輸出 : 單相 110 V/60Hz :3.5W，單相 115 V/60Hz :3.5W 最大輸出轉矩 : 38mN.m 電容器容量 : 6uF. 26.

(38) 圖 3-15. C170 網路攝影機. 本研究影像辨識所使用的攝影機為網路攝影機，實體如圖 3-16 所示，此攝影機會向周遭環境進行拍照，再由 USB 線回傳給筆記型電腦進行圖像儲存，最後會將由 Python 去處理顏色分析。此種網路攝影機具有(1020*768)高品質視訊通話，照片可達到五百萬畫素，內建也有抗噪式麥克風，另外也有每秒 30 頁 Video 錄影的功能，其相關參數如表 3-13 所示。. 表 3-12. C170 網路攝影機參數表. 傳輸方式 : USB USB 類型 : USB 2.0 聚焦類型 : 定焦光學分辨率 : 真彩色 640x480，格行掃描 1.3MP 對角線視場(FOV) : 58 度相機焦距 : 2.3mm 偵速率(最大) : 640x480@30FPS 尺寸 : 71mm(長)*70.3mm(寬)*60.5mm(高). 27.

(39) 接下來介紹機構及硬體組裝，為了使得所有元件、電路板以及線路集中在移動機器人上，因此訂做一片鋁板，如圖 3-16 所示，然後再使用四個鋁塊將鋁板撐起來，如圖 3-17 所示，撐起來之後可以將移動機器人內部的所有線路全部拉出來，如圖 3-18 所示。. 圖 3-16 鋁板. 圖 3-17 鋁塊如紅色框所示，與鋁板鎖在一起. 28.

(40) 圖 3-18 出線口如黃色箭頭所示，將車內的線拉出來. 為了使得機械手臂與移動機器人連接，這裡將機械手臂底下的圓盤底座用三顆大螺絲與鋁板鎖在一起，如圖 3-19 所示。. 圖 3-19 圓盤底座與鋁板鎖在一起. 29.

(41) 這裡開始介紹機械手臂上每個關節的驅動方式。如圖 3-20 所示，可看到交流馬達與螺桿連接在一起，可將手臂上下擺動。如圖 3-21 所示，這裡使用特製的螺母，如圖 3-22 所示，與螺母及螺桿鎖在一起，螺母上的突起物與手臂連桿合在一起，因此可以驅動手臂伸縮。如圖 3-23 所示，這裡使用齒輪咬合的方法使得爪子可以上下擺動。最後如圖 3-24 所示，也是利用齒輪的方式驅動爪子閉合。. 圖 3-20 交流馬達與螺桿連結，如紅框所示. 圖 3-21 特製螺母. 30.

(42) 圖 3-22 將特製螺母與連桿鎖在一起，如箭頭所示. 圖 3-23 齒輪咬合的部分如紅色框內所示. 31.

(43) 圖 3-24 馬達套入齒輪驅動爪子，如紅框內所示. 將以上所有的機構、感測元件、轉換器、驅動裝置以及微控制器整合如圖 3-25 所示，另外在爪子上加裝 Webcam 攝影機，如圖 3-26 所示。. 圖 3-25 具機械手臂之移動機器人之接線圖. 32.

(44) 圖 3-26 加裝在爪子上的 Webcam. 再將筆記型電腦架設在機械手臂的左後方，如圖 3-27 所示，架設完畢後，最後將具機械手臂之移動機器人上的線路整線完如圖 3-28 所示。. 圖 3-27 在機械手臂的左後方架設筆電. 33.

(45) 圖 3-28 具機械手臂之移動機器人整線圖. 34.

(46) 3.2 軟體架構本論文的具機械手臂之移動機器人軟體架構圖如圖 3-29 所示，微控制器會與筆記型電腦、移動機器人以及機械手臂上的硬體設備和感測元件做軟體整合，另外 Webcam 連結 USB 線至筆記型電腦，使得資料可以在筆記型電腦與微控制器互相傳送。首先會介紹移動機器人的控速與避障，再者介紹 Python 顏色影像辨識的軟體流程圖，最後再介紹微控制器接收物件座標值時，讓機械手臂各關節追蹤旋轉角度與平移距離。. Robot arm. Mobile robot. Angle detect. Avoidance. Speed detection. Speed control. Bluetooth detect. Microcontroller Distance detect. UART Transmit. Capture image. Track color. Mask process. 3D coordinates. Laptop 圖 3-29 具機械手臂之移動機器人軟體架構圖. 35.

(47) 3.2.1 移動機器人軟體架構首先具機械手臂之移動機器人移動的同時，透過外部中斷以及計時中斷達到左右輪速度追蹤與速度控制，另外左側、中間以及右側超音波感測器開始感測障礙物距離，超音波測距的公式如(3-1)所示，若左側在指定的距離下偵測到障礙物時，移動機器人會向後旋轉一秒鐘，再向右原地旋轉，若右側在指定的距離下偵測到障礙物時，移動機器人則向後旋轉一秒鐘，再向左原地旋轉，最後中間超音波感測器在指定的距離下偵測到障礙物時，會依照不同 Beacon 的指示取決於每次避障時，移動機器人會向左/向右原地旋轉，到達最後一次避障時，則使得移動機器人停止循跡，再來使用串列通訊的方式告知筆記型電腦開啟 Webcam 攝影機進行周遭環境拍攝。等速模式之程式流程圖如圖 3-30 所示，移動機器人程式流程圖如圖 3-31 所示。 dis  344(m / s ) . Motor drive controls left and right wheels. Waiting for interrupt. time 2. (3-1). YES. External Interrupt 0?. Left_count+1. NO YES. External Interrupt 1?. Right_count+1. NO. NO. YES. Timer Interrupt 0?. Use the number of left and right wheel interrupts to calculate the left and right wheel speed. Speed control and return speed value to Arduino. Update the target speed of the left and right wheels. 圖 3-30 等速模式之程式流程圖. 36. Adjust left and right wheel PWM signal.

(48) start. Bluetooth detect. NO. Detect B. Detect A. Beacon?. Beacon?. YES. Reverse one second and rotate to the right. YES. YES. YES. Is Left detect within 20cm?. Is Left detect within 20cm? NO. Reverse one second and rotate to the left. YES. NO. Reverse one second and rotate to the right. NO. Is right distance sensor within 20cm?. Is right distance sensor within 20cm?. YES. NO. Reverse one second and rotate to the left. NO. Is second center detect within 50cm and detect beacon? YES. Mobile robot is stop and ready to drive the robotic arm. NO. Is first center detect within 50cm? YES. Is first center detect within 50cm?. NO. Is second center detect within 50cm?. YES. NO. YES. Is third center detect within 50cm?. NO. Is fourth center detect within 50cm and detect beacon?. YES YES. Reverse one second and rotate to the left. Reverse one second and rotate to the right. Reverse one second and rotate to the left. Mobile robot forward. 圖 3-31 移動機器人程式流程圖. 37. Reverse one second and rotate to the right. Mobile robot is stop and ready to drive the robotic arm.

(49) 3.2.2 顏色影像辨識軟體架構當筆記型電腦接收到微控制器傳送的字元至 Python 軟體，告知開啟 Webcam 攝影機，且向周遭環境拍照並儲存圖像，另外也抓取 Webcam 讀取的圖像尺寸，將儲存的圖像做目標顏色分析，先將圖像 RGB 轉換為 HSV 模式後，開始設定目標顏色的上下限值，再對圖像去做目標顏色追蹤，追蹤到的目標顏色像素點再用遮罩圖像處理去除其餘顏色，只保留目標顏色像素點，將每個目標顏色像素點依照圖像尺寸取座標值出來，透過式(2-8)將目標顏色像素的中心座標 XY 值取出來，另外超音波感測器抓取三維空間的 Z 軸值，最後再將 XYZ 值利用串列通訊的方式回傳給微控制器，影像辨識程式流程圖如圖 3-32 所示。. Arduino Mega. Wait for the control board to send characters. NO. YES. Take a photo and save the image. Convert images to HSV mode. Grab the threedimensional coordinate value of the target. Capture blue pixels. Image mask processing. 圖 3-32 影像辨識程式流程圖. 38.

(50) 3.2.3 機械手臂軟體架構當微控制器接收到物件的座標值時，透過逆向運動學推出每個關節的旋轉與平移變量，再控制每顆馬達，使得手臂循跡並且爪子中心到達目標物件，最後開始轉動爪子上的馬達，抓取目標物件，最後將物件放在指定的區域上，機械手臂程式流程圖如圖 3-33 所示。. PC. 3D coordinate value received?. NO. YES. Joint 1 inverse kinematic formula. Joint 2 inverse kinematic formula. Joint 3 inverse kinematic formula. Joint 4 inverse kinematic formula. Joint 1 tracking reference valueθ1. Joint 2 tracking reference valueθ2. Joint 3 tracking reference value d 3. Joint 4 tracking reference valueθ4. 圖 3-33 機械手臂程式流程圖. 39.

(51) 第四章. 控制方法與模擬驗證. 本章節將介紹具機械手臂之移動機器人整體的模擬實驗，使用 Matlab 觀察每個關節經過 PID 控制器的控制量與輸出量，PID 控制器如圖 4-1 所示，將每個關節理想變數視為輸入，設定 PID 權重參數後再經由控制器追蹤理想值，使得輸出訊號穩定又快速達到收斂效果。接下來會分別驗證移動機器人的路徑規劃以及機械手臂的運動模式。. sensor1 -. Ref_VR. +. K p e(t ). e1(t). Σ. t. K i  e( )d. +. 0. Kd. Mobile robot Ref_VL. +. PWM1. Σ. Driver1. +. d e(t ) dt. K p e(t ). e2(t). Σ. t. K i  e( )d. +. 0. Kd. Ref_Jω1. +. +. PWM2. Σ. Driver2. +. d e(t ) dt. K p e(t ). +. Σ. e3(t). t. K i  e( )d. +. 0. -. Kd. +. PWM3. Σ. Driver3. +. d e(t ) dt. platform. K p e(t ). Ref_Jω2. +. Σ. e4(t). t. K i  e( )d. +. 0. -. Kd. +. PWM4. Σ. Driver4. +. d e(t ) dt. K p e(t ). Ref_Jv3. Robot arm. +. Σ. e5(t). t. K i  e( )d. +. 0. -. Kd. +. PWM5. Σ. Driver5. +. d e(t ) dt. K p e(t ). Ref_Jω4. +. Σ -. e6(t). t. K i  e( )d 0. Kd. d e(t ) dt. +. +. PWM6. Σ. Driver6. +. sensor2. 圖 4-1 具機械手臂之移動機器人 PID 控制器. 40.

(52) 4.1 具機械手臂之移動機器人室內移動模擬實驗在移動機器人的研究中，透過 PID 控制器使得左右輪轉速追蹤參考值，並且設定 PID 權重參數，車輛整合參數如表 4-1 所示，這裡輸入速度的理想參考值為 65cm/s，觀察 Matlab 模擬的控制量與輸出量，如圖 4-2 所示。. 表 4-1 車輛整合參數表參數. 左右輪數值. Kp. 0.8. Ki. 0.1. Kd. 0.1. . 1.5. r. 0.2m. W. 0.8m. 41.

(53) 圖 4-2 左右輪速度控制量與輸出量. 42.

(54) 由以上左右輪模擬實驗觀察到轉速可在一秒內收斂成功，另外利用式(2-3)套入路徑規劃，將此功能模擬路徑循跡，如圖 4-3 所示，觀察對應到的左右輪轉速如圖 4-4 與圖 4-5 所示，利用左右輪的輪速差使得移動機器人旋轉方向可自由調整。. 圖 4-3 路徑循跡路線. 圖 4-4 右輪速度. 43.

(55) 圖 4-5 左輪速度由於移動機器人在室內走道移動，室內環境如圖 4-6 所示，行徑速度以等速的方式移動。本研究移動機器人實驗在室內會有四種運動模式，包含前進模式、後退模式、向右原地旋轉模式以及向左原地旋轉模式。. 圖 4-6 室內環境地圖為了讓移動機器人到達物件所在位置，這裡分別將兩種物件放置在走道上 (藍色點)與實驗室旁(紅色點)，如圖 4-7 所示，若要抓取此兩種物件，分別有兩種路線，一種為藍色路線，如圖 4-8 所示，另一種為紅色路線，如圖 4-9 所示，再以等速的方式循跡此兩種路線。. 44.

(56) 圖 4-7 物件放置圖. 圖 4-8 A 位置循跡路線圖. 45.

(57) 圖 4-9. B 位置循跡路線圖. 由以上循跡路線圖可以知道若要使得移動機器人是否已到達目標物附近，則利用 Beacon 告知移動機器人已到達目的地附近，最後使其停止。. 46.

(58) 4.2 具機械手臂之移動機器人抓取物件模擬實驗 4.2.1 順向運動學本論文使用的手臂結構為 RRPR 型態，R 代表旋轉(Revolute)，P 代表平移 (Prismatic)，依序關節 1、關節 2 以及關節 4 為旋轉模式，則關節 3 為平移模式，手臂結構如圖 4-10 所示。. 圖 4-10 機械手臂結構. 此手臂運動學公式推導參考為文獻[20]，D-H 參數表為表 4-2，θ為關節角度，. d 為連桿偏差，a 為連桿長度，α為連桿扭角。表 4-2 D-H 參數表自由度. θ. d. a. α. Link_1. θ1. 0.76. 0. -90. Link_2. θ2. 0. 0. -90. Link_3. 0. d3. 0. 90. Link_4. θ4. 0. 0.19. -90. 47.

(59) D-H 表示法為(4-1)所示，sin  i 簡化符號為 si ，cos i 簡化符號為 c i ，sin  i 簡化符號為 s i ， cos  i 簡化符號為 c i 。 ci  s  i 0  0.  si c i ci c i s i 0. si s i ci s i c i 0. ai ci  ai si  di   1 . (4-1). 各關節相對座標系矩陣為(4-2)所示，由上至下依序為座標系 1 相對於座標系 0 的轉換矩陣、座標系 2 相對於座標系 1 的轉換矩陣、座標系 3 相對於座標系 2 的轉換矩陣以及座標系 4 相對於座標系 3 的轉換矩陣。 c1  s 0 A1 (1 )  T1   1 0  0 c 2  s 1 A2 ( 2 )  T2   2  0   0. 0  s1 0  0 c1 0  1 0 0.76   0 0 1  0  s 2 0  0 c 2 0  1 0 0  0 0 1. 1 0 A3 (d3 )  T32   0  0. 0 0 0 0 1 0  1 0 d3   0 0 1 c 4 0  s 4 0.19c 4   s 0 c 4 0.19s 4  A4 ( 4 )  T43   4  0 1 0 0    0 0 1   0. 末端點(爪子)相對於基底(座標系 0)的座標轉換為(4-3). 48. (4-2).

(60) H  T40  A1 (1 ) A2 ( 2 ) A3 (3 ) A4 ( 4 )  c24  c1  c  s   24 1   s  24   0. s1.  s24  c1. c1.  s24  s1. 0. c24. 0. 0. c1  (19c24  100d3 s2 )   100  s1  (19c24  100d3 s2 )   100  19s24  19  d 3c2  25 100   1 . (4-3). 順向運動學座標轉換為式(4-4)以及爪子末端點相對於目標物向量為式(4-5). p0  T40 p 4. (4-4). p4  (T40 )1 p0. (4-5). 49.

(61) 4.2.2 逆向運動學利用三維空間座標反推導每個關節需要旋轉多少角度或者平移多少距離，首先先設定手臂各關節的初始狀態為 1 2 d3  4  = 0 - /2 1.172  /2 ，最後再使 T. T. 用幾何方法中的畢氏定理與餘弦定理推導方式獨立算出每個機械手臂關節的角度量或平移量。當目標點在 X0 -Y0 平面第一象限和第四象限時，關節 1 如式(4-6)。. 1  tan 1. y0 x0. 圖 4-11 手臂俯瞰圖第一象限. 圖 4-12 手臂俯瞰圖第四象限. 50. (4-6).

(62) 當目標點在 X0 -Y0 平面第二象限時，關節 1 如式(4-7)。. 1    tan 1. y0 x0. (4-7). 圖 4-13 手臂俯瞰圖第二象限當目標點在 X0 -Y0 平面第三象限時，關節 1 如式(4-8)。. 1    tan 1. y0 x0. 圖 4-14 手臂俯瞰圖第三象限. 51. (4-8).

(63) 關節 1 推導完之後，接下來要推導關節 2~關節 4，因此以座標系 1 的三維空間推算三個關節的逆向運動學公式，如圖 4-15 所示，由於輸入關節 1 的變量，使得手臂方向為指向目標物，使得 Z1 值為 0，因此可轉換為二維座標，不考慮 Z 軸，如圖 4-16 所示。另外為了讓爪子可以垂直於 X0 -Y0 平面，必須先推導出關節 3 以及關節 4 的變量，幾何圖形如圖 4-17 所示。. 圖 4-15 座標系 1 結構圖. 圖 4-16 座標系 1 的二維座標圖. 52.

(64) 圖 4-17 座標系 1 的幾何圖形. 接下來要求關節變數之前，必須將目標物轉換為對座標系 1 的向量，如下式。.  x1   x0  y     1   T 0 1  y0  1  z1   z0      1 1. (4-9). 再使用畢氏定理求出 d3，關節 3 的公式為，0.19 為抓子長度，單位為公尺，如下式。. d3  x12  ( y1  0.19) 2. (4-10). 關節 4 的公式推導為使用畢氏定理與餘弦定理找出  4 ，過程如下式. x12  y12  d32  0.192  2  d3  0.19  cos    4   x12  y12  d32  0.192  2  d3  0.19  cos  4. 53. (4-11).

(65) 由於關節 4 在不同的向量位置有不同的角度去表示，如圖 4-18 所示，因此關節 4 與初始角度相加為式(4-12)所示。.  x12  y12  d32  0.192   4   cos   2 0.38d3  . . 1. (4-12). 圖 4-18 座標系 3 的幾何圖形推導  4 之後，因此與對角有著相同角度，最後關節 2 的式子為 2 2 2 2     1  x1  y1  d3  0.19  2     cos   2  2 0.38d3   . (4-13). 與原本的初始角度相減，因此整理為下式.  x12  y12  d32  0.192   2   cos   0.38d3   1. (4-14). 最後再將以上四個關節的變量當做 PID 控制器的參考值，使得各關節去追蹤以上變量，達到手臂循跡並且夾取目標物。. 54.

(66) 4.2.3 順向運動學模擬實驗此研究的四軸機械手臂 D-H 參數表如表 4-2 所示，每個關節的相對座標系如圖 4-19 所示，可由圖 4-20 觀察 Matlab 模擬四軸機械手臂與目標物的相對位置，末端點的中心與目標物重疊時，兩者相對向量等於[0,0,0]T ，由此從 Command Window 視窗得知，然而從圖 4-21 觀察當末端點的中心與目標物沒有重疊時， Command Window 視窗顯示的相對向量就不等於[0,0,0]T。. 圖 4-19 關節相對座標系. 55.

(67) 圖 4-20 爪子中心與目標物重疊. 圖 4-21 爪子中心與目標物無重疊. 56.

(68) 4.2.4 逆向運動學模擬實驗先設定機械手臂的初始狀態為 1 2 d3  4  = 0 - /2 1.172  /2 ，如圖 4-22， T. T. 再將式(4-3)到式(4-14)套入四軸機械手臂，Matlab 可從圖 4-23 至圖 4-26 觀察機械手臂在不同象限可看出末端點中心與目標物重疊。. 圖 4-22 手臂初始狀態. 圖 4-23 追蹤 X0 -Y0 平面之第一象限目標物示意圖. 57.

(69) 圖 4-24 追蹤 X0 -Y0 平面之第二象限目標物示意圖. 圖 4-25 追蹤 X0 -Y0 平面之第三象限目標物示意圖. 58.

(70) 圖 4-26 追蹤 X0 -Y0 平面之第四象限目標物示意圖. 各關節設定 PID 權重參數，參數如表 4-3 所示，先重設機械手臂的初始狀態為 1  2 d3  4  = 0 - /2 0  /2 ，另外目標物三維值設定為[0.53,0.5,0.35]，可從 T. T. 圖 4-27 至圖 4-29 觀察 Matlab 分別模擬出每個關節的控制量、角度與距離輸出量以及角速度量與速度量，另外也觀察基底關節相對於移動機器人的左右輪速度，如圖 4-30 所示左右輪速度是經由運動學方程式(2-3)反推而成，利用左右輪正反轉速度去換算基底旋轉角度，達成移動機器人與機械手臂整合的目的。表 4-3 機械手臂 PID 權重參數表參數. 數值. Kp. 0.6. Ki. 0.1. Kd. 0.1. 59.

(71) 圖 4-27 各關節控制量. 圖 4-28 各關節輸出量. 60.

(72) 圖 4-29 各關節速度量. 圖 4-30 關節 1 相對於左右輪速度. 61.

(73) 第五章. 實驗結果與討論. 本章節將介紹在具機械手臂之移動機器人的研究中，實驗共分三個部份；實驗一，會在 A 位置室內走廊路線測試在 PID 控制的速度下，前後移動、左右旋轉及避障等等運動模式，觀察左右輪速度，以確認是否達到收斂值。實驗二，到達目的地時，開啟 Webcam 攝影機，從 Python 觀察攝影機捕捉物件的畫面與實際 Webcam 對應的位置是否符合，另外也觀察補捉物件的圓點是否在物件的中心位置上。實驗三，攝影機捕捉到目標物件時，驅動整體的活動關節，觀察運動模式，手臂抓取物件，將物件從擺放位置放到指定區域整個過程中的各關節之角度值、距離值及左右輪速度值。. 5.1 具機械手臂之移動機器人控速實驗 5.1.1 具機械手臂之移動機器人左右輪控速實驗在測試輪速時，必須考量移動機器人的整合參數表，如表 5-1 所示，另外找出在不同的正脈寬對應到的左右輪轉速，如表 5-2 所示，最後將整合運動方程式與控制器並且觀察左右輪速度。表 5-1 移動機器人的整合參數表參數. 左右輪數值. Kp. 0.8. Ki. 0.1. Kd. 0.1. . 1.5. r. 0.2m. W. 0.8m. 62.

(74) 表 5-2 實際轉速對應表正脈寬. 左輪角速度. 右輪角速度. 1000μs. -39.8 rad/s. 37.3 rad/s. 1190μs. -25.4 rad/s. 31 rad/s. 1280μs. -17.8 rad/s. 24.3 rad/s. 1505μs. 0 rad/s. 0 rad/s. 1640μs. 7.6 rad/s. -7.4 rad/s. 1730μs. 15.5 rad/s. -14.9 rad/s. 1820μs. 23.1 rad/s. -22.1 rad/s. 2000μs. 38 rad/s. -36.7 rad/s. 首先先測試具機械手臂之移動機器人的左右輪在空轉模式下，並且設定理想轉速參數為 65(cm/s)，觀察左右兩側的速度值，如圖 5-1 與圖 5-2 所示。. 圖 5-1 左輪速度 63.

(75) 圖 5-2 右輪速度由以上兩張圖可以發現速度值接近收斂值，那也因為在驅動馬達的過程中，會有震動的狀況發生，導致輸出量會有震盪的問題。再來測試具機械手臂之移動機器人在室內走廊移動軌跡依照圖 4-5 所示，A 位置路線觀察在移動過程中的左右輪速度，也由於原本要觀察具機械手臂之移動機器人在二維空間中旋轉的角度值，但因為 MPU6050 在移動的過程，整個震盪的問題會導致 USB 串列通訊的過程中會自行中斷，無法完整取出，因此，此實驗只觀察左右輪的速度值，另外測試的走道較窄，所以以等速約 40(cm/s)的速度前後移動、原地左右旋轉。依照 A 位置路線移動觀察左右輪速度如圖 5-3 與圖 5-4 所示，再來利用輪速觀察在室內移動的路線如圖 5-5 所示，座標上的單位以公尺代表。. 64.

(76) 圖 5-3. A 路線室內移動之左輪速度. 圖 5-4. A 路線室內移動之右輪速度. 65.

(77) 由軌跡圖如圖 5-5 所示可觀察到具機械手臂之移動機器人在移動的過程中，會因為地形摩擦力、震盪問題與左右側前後輪不等速的等等外部因素，導致移動機器人在行走與原地轉彎的過程中會與理想循跡有很大的誤差。. 圖 5-5. A 路線室內移動之軌跡. 66.

(78) 5.1.2 具機械手臂之移動機器人影像辨識實驗本節會觀察 Webcam 回傳筆記型電腦的畫面，進行物件追蹤和分析，另外由於微控制器與筆記型電腦在串列通訊的過程中雖然鮑率設定一致，但數值仍然無法即時傳送和接收，必須 Python 傳送一筆資料的次數多一點，微控制器才可接收到一筆資料，所以這裡我用字元區域的方式，去判斷物件在整體畫面中的哪一個字元區域，如圖 5-6 所示，此圖的座標值是由 Python 圖像定義，另外將圖 5-6 轉換實際的座標空間，如圖 5-7 所示。. 圖 5-6 字元區域. 圖 5-7 轉換實際的字元區域. 67.