一種機械手臂順應性控制與安全設計

國立交通大學

電控工程研究所

碩士論文

一種機械手臂順應性控制與安全設計

A Compliance Control and Safety Design of a

Robotic Manipulator

研 究 生：許傑巽

指導教授：宋開泰 博士

羅佩禎 博士

一種機械手臂順應性控制與安全設計

A Compliance Control and Safety Design of a Robotic

Manipulator

研 究 生：許傑巽 Student: Chieh-Hsun Hsu

指導教授：宋開泰 博士 Advisor:

羅佩禎 博士

國 立 交 通 大 學 電 控 工 程 研 究 所 碩 士 論 文 A Thesis

Submitted to Institute of Electrical Control Engineering College of Electrical and Computer Engineering

National Chiao Tung University in Partial Fulfillment of the Requirements

for the Degree of Master in

Electrical Control Engineering September 2010

Hsinchu, Taiwan, Republic of China

中華民國九十九年九月

Dr. Kai-Tai Song

Dr. Pei-Chen Lo

i

一種機械手臂順應性控制與安全設計

學生:許傑巽 指導教授:宋開泰 博士 羅佩禎 博士 國立交通大學電機與控制工程學系

摘要

當機械手臂在執行任務時，往往會無法完全預期外界環境的狀態，有可能會 與外界環境產生意外碰撞而導致硬體上的損壞或人員的傷害。為避免上述情況， 必須加入順應性運動控制(例如：阻抗控制)，使得機械手臂碰撞到外界物體時能 產生順應能力來保持安全。本研究提出一套機械手臂的安全行為的控制策略。考 慮到機械手臂在居家環境裡可能面對的大多情況，以及各種任務類型，大致上可 以將機械手臂所有行為分為兩類。第一類情況為待命模式。第二類情況為運動模 式。在待命模式裡使用了本論文提出來的機械手臂順應性運動控制，以空間中的 質量-阻尼動態模型來實現機械手臂之順應性運動控制，當遇到外力或碰撞時， 能夠利用質量-阻尼動態模型來順應外力，以確保對環境(人)的安全。在運動模 式使用本論文提出的機械手臂安全移動策略，本策略同樣使用了順應性運動控制 的架構。機械手臂在移動的過程中，如果與外界(人或物體)發生碰撞，能夠即時 的感知並停止繼續運動，使碰撞程度維持在輕微接觸的範圍，並且會根據碰撞時 產生的力向量計算出一個繞路點，機械手臂會移動到繞路點後再次往原設定之目 標點前進，如此反覆進行。透過本設計的實現，可以確保機械手臂任何時刻的安 全性考量。文中以實驗室自行設計之四自由度機器手臂搭配安裝在終端效應器 (End-effector)的六軸力感應器以及工業電腦，驗證所提出方法的可行性。最後 在不同情況的測試下，進行順應外力之實驗以及遇到障礙物重新移動實驗，皆為 安全行為，消除機械手臂隊人員的威脅，以確保人員的安全。

ii

A Compliance Control and Safety Design

of a Robotic Manipulator

Student: Chieh-Hsun Hsu Advisor: Dr. Kai-Tai Song

Dr. Pei-Chen Lo

Department of Electrical and Control Engineering National Chiao Tung University

ABSTRACT

When the manipulator executes a task, it may cause some hardware damage or

injury if the manipulator does not know the environment all condition. To avoid the

above situation, we must adopt the compliance motion control. We must design the

manipulator with the ability of compliance to ensure safety of the manipulator. This

thesis presents a strategy for safe behavior of the robot. Taking into account the

manipulator in a home environment, we can classify the manipulator's behaviors in

two modes. The first mode is standby mode. The second mode is motion mode.

Standby mode uses a compliance motion control design for the manipulators. Motion

mode uses the same control architecture with safety motion strategy of this thesis.

Finally, we use a 4-DOF manipulator developed in our laboratory to test and verify

iii

誌謝

謹向我的指導教授宋開泰博士致上感謝之意，感謝他兩年多來給於我許多在 生活處事上的意見以及專業研究上的指導，以他豐富的學識與經驗，配合理論的 應用，使得本論文得以順利完成，且無論在知識或生活等各方面都收穫良多。 感謝學長孟儒、嘉豪、格豪和學姐巧敏在實做及理論上的指導，以及感謝與 我共同奮鬥的同學信毅、奕彣、哲豪、維峻及宗暘的相互鼓勵及提攜，同時也感 謝學弟仕晟、建宏、上畯、碩成、家昌和章宏在研究途中的協助及在生活上帶來 的樂趣。感謝我的朋友們立傑、祺祥、育民、資順、偉傑、逸暘、炳聰、子翔、 美月、郁婷、純琪、浩儒等…，時常為我打開不同於研究生涯的另一扇窗，讓我 得以喘息。感謝我女朋友虹羽，對於我日常生活大小事的照顧關心與幫忙，不時 的陪伴，與許多諒解與支持。 最後，特別感謝我的父母，由於他們的辛苦栽培與鼓勵，在生活上給予我細 心地關愛與照料，並支持我就讀碩士學位，使得我才得以順利完成此論文。在此 我願以此論文獻給我最感激及最偉大的父母親。

iv

目錄

摘要………i ABSTRACT………ii 誌謝………iii 目錄………iv 圖目錄………vii 表目錄………x 第一章.緒論………1 1.1 研究動機 ………1 1.2 機器人之安全設計 ………1 1.3 順應性運動控制之研究背景 ………6 1.3.1 被動式順應性運動控制 ………7 1.3.2 主動式順應性運動控制 ………9

1.3.2.1 混合運動控制(Hybrid motion control) ………9

1.3.2.2 阻抗控制(Impedance control) ………10 1.4 系統架構 ………11 1.5 章節說明 ………12 第二章.雙臂機器人設計 ………13 2.1 雙臂機器人之整體架構 ………13 2.2 雙臂機器人之機構設計與實現 ………13 2.2.1 雙臂機器人整體自由度配置 ………14 2.2.2 雙臂機器人手臂機構設計與實現 ………14 2.2.2.1 θ1 自由度之機構設計………15 2.2.2.2 θ2 自由度之機構設計………16

v 2.2.2.3 θ3 自由度之機構設計………17 2.2.2.4 θ4 自由度之機構設計………18 2.2.2.5 θ5 自由度之機構設計………19 2.2.2.6 θ6 自由度之機構設計………19 2.2.2.7 手爪之機構設計………20 2.3 雙臂機器人之硬體設計與實現 ………21 2.3.1 運算系統之實現 ………22 2.3.2 力感測系統之使用 ………22 2.3.3 馬達控制系統之設計與實現 ………23 2.4 雙臂機器人之軟體設計與實現 ………26 第三章.機械手臂之運動學分析 ………27 3.1 連桿參數與座標 ………27 3.2 順向運動學 ………29 3.3 四自由度機械手臂之逆向運動學分析………31 3.4 順向與逆向運動學總結及討論 ………36 第四章.機械手臂順應性運動控制 ………37 4.1 機械手臂順應性運動控制架構 ………37 4.2 力感測器單位校正………38 4.3 力感測器座標旋轉 ………40 4.4 力感測器座標實驗 ………41 4.5 自由質量模型………43 4.6 參數設定之影響………44 第五章.安全行為策略設計 ………46

vi 5.1 安全運動與重新移動策略………46 5.2 繞路點產生方法 ………48 5.3 安全設計之討論 ………50 第六章.實驗結果 ………51 6.1 機械手臂順應外力之實驗設置………51 6.1.1 機械手臂順應外力之實驗結果………51 6.2 機械手臂運行遭遇障礙物而重新移動實驗設置………54 6.2.1 機械手臂運行遭遇障礙物而重新移動實驗結果………54 6.3 機械手臂安全性評估實驗設置………61 6.3.1 機械手臂安全性評估實驗結果………61 6.4 實驗討論………63 第七章.結論與未來展望 ………65 參考文獻 ………67

vii

圖目錄

圖 1.1 Whole-Sensitive Anthropomorphic Robot Arm………2

圖 1.2 薄型光學力矩感測器………2

圖 1.3 joint impedance control model………3

圖 1.4 Personal Robot PR-2 Prototype………3

圖 1.5 PR-2 的重力補償結構示意圖………4 圖 1.6 DLR LWR………4 圖 1.7 DLR LWR 的軸關節整合結構………4 圖 1.8 流程圖與實際軌跡示意圖………5 圖 1.9 (a)一種由纜線構成的 SLM 的結構圖(b)此 SLM 結構受力時的動作圖……6 圖 1.10 此 SLM 結構的實際圖………7 圖 1.11 (a)一種 SJM 的結構圖(b)此 SJM 結構受力時的動作圖………7 圖 1.12 此 SJM 結構的 prototype………7 圖 1.13 此 SJM 結構實際安裝情形………7 圖 1.14 此 VSA 結構實際圖………8 圖 1.15 VSA 結構改變硬度時的動作圖………8 圖 1.16 混合運動控制控制架構圖………10 圖 1.17 阻抗控制控制架構圖………11 圖 1.18 系統架構圖………14 圖 2.1 單邊機械手臂自由度示意圖………15 圖 2.2 θ1 機構之配置圖………16 圖 2.3 θ1 機構之組立圖………16 圖 2.4 θ2 機構之配置圖………17 圖 2.5 θ2 機構之組立圖………17 圖 2.6 θ3 機構之配置圖………18 圖 2.7 θ3 機構之組立圖………18 圖 2.8 θ4 機構之配置圖………19 圖 2.9 θ4 機構之組立圖………19 圖 2.10 θ5 機構之配置組立圖………20 圖 2.11 θ6 機構之配置組立圖………21 圖 2.12 手爪機構之配置組立圖與使用情境………21 圖 2.13 雙臂機器人 3D 組合圖………22 圖 2.14 控制系統硬體架構圖………22 圖 2.15 工業電腦 FB2602A………23 圖 2.16 JR3 力感測器-67M25………24 圖 2.17 資料擷取卡………24 圖 2.18 EPOS 24/1………26

viii 圖 2.19 EPOS 24/5………26 圖 2.20 NI CAN-bus………27 圖 2.21 並聯控制器之電路分接板………27 圖 3.1 連桿參數及座標系變換………28 圖 3.2 關節和關節之間的座標關係………28 圖 3.3 機械手本體與 D-H 座標系………30 圖 3.4 手臂座標系與冗餘圏示意圖………31 圖 3.5 由手臂構成的三角形………32 圖 3.6手臂三角形裡各向量………33 圖 3.7 α 向量與各向量關係………33 圖 3.8 β 向量與各向量關係………34 圖 3.9 α 向量與 β 向量型成 γ 向量………34 圖 4.1 機械手臂順應性運動控制架構圖………38 圖 4.2 力感測器-X 軸的校正曲線………39 圖 4.3 力感測器+Y 軸的校正曲線………39 圖 4.4 力感測器-Z 軸的校正曲線………40 圖 4.5 機械手臂座標系與力感測器座標系………40 圖 4.6 機械手臂姿態受力示意圖………41 圖 4.7 力感測器安裝在手臂上的姿態………42 圖 4.8 力感測器正常的姿態………42 圖 4.9 各力向量於機械手臂座標系………42 圖 4.10 手腕點模擬成自由質量示意圖………43 圖 4.11 M 對速度的影響模擬………45 圖 4.12 B 對速度的影響模擬………45 圖 5.1 機械手臂行為分類………47 圖 5.2 安全移動流程圖………48 圖 5.3 繞路點產生說明圖………49 圖 5.4 碰撞反作用力對小質量模型施力………49 圖 6.1 機械手臂與力感測器配置………52 圖 6.2 機械手臂順應外力過程………52 圖 6.3 機械手臂的手腕點軌跡………53 圖 6.4 手腕點 XYZ 座標分別對時間作圖………53 圖 6.5 力感測器 XYZ 軸力資訊對時間作圖………54 圖 6.6 從(0,0,-522)到(400,-100-100)移動過程………55 圖 6.7 無障礙物時從(0,0,-522)到(400,-100-100)的移動軌跡………55 圖 6.8 有障礙物時從(0,0,-522)到(400,-100-100)的移動軌跡………56 圖 6.9 手腕點 XYZ 座標分別對時間作圖………56 圖 6.10 力感測器 XYZ 軸力資訊對時間作圖………57

ix 圖 6.11 從(300,100,-350)到(300,-200,-350)移動過程………58 圖 6.12 無障礙物時從(300,100,-350)到(300,-200,-350)的移動軌跡………58 圖 6.13 有障礙物時從(300,100,-350)到(300,-200,-350)的移動軌跡………58 圖 6.14 手腕點 XYZ 座標分別對時間作圖………59 圖 6.15 力感測器 XYZ 軸資訊對時間作圖………59 圖 6.16 從(300,-200,-300)到(300,100,-300)移動過程………60 圖 6.17 無障礙物時從(300,-200,-300)到(300,100,-300)的移動軌跡………60 圖 6.18 有障礙物時從(300,-200,-300)到(300,100,-300)的移動軌跡………61 圖 6.19 手腕點 XYZ 座標分別對時間作圖………61 圖 6.20 力感測器 XYZ 軸力資訊對時間作圖………62 圖 6.21 機械手臂安全性評估實驗過程………63 圖 6.22 手腕點移動軌跡………63 圖 6.23 手腕點速度 0.17m/s 記錄的外力………64 圖 6.24 手腕點速度 0.24m/s 記錄的外力………64

x

表目錄

表 2.1 馬達規格與機構活動之效能表………24 表 2.2 馬達控制器規格表………25 表 3.1 右機械臂每軸的 D‐H 參數表………29 表 4.1 座標旋轉誤差表………51

1

第一章 緒論

1.1 研究動機

在現今的世界，機器人慢慢地被應用在各個領域跟不同的環境裡。相較於當 今之機器手臂或是自動搬運車只會在工廠裡出現，未來機器人將會慢慢的出現在 人們的日常生活中，與人類的互動也會變得越來越頻繁。因此，如何讓機器人在 一個有人的環境中安全的完成工作將是項非常重要的議題。 考慮到機器人的安全性設計的話，有關力方面的控制是一個比較會直接想到 的議題。機器人要知道何時該出力大，何時該出力小，如何才能安全的與人類互 動，或是不傷害到環境周圍的人、物品或自己，並且有效的協助人類完成工作。 由於人類的世界各事各物被構築成適合人類型體的樣子，所以在機器人的結構方 面，也需要符合人類手臂結構。一來，仿人化的機械手臂也比較能夠在人類環境 有效的完成協助人類的工作。二來，對於人類來說比較容易接受並且習慣機器手 臂的工作空間，才能達到與機器人一起工作，或是讓機器人在人類環境中工作。 此研究的目的在於實現一個仿人的機械手臂，能夠安全的與人在環境中互動 來達成工作。藉由得知外界的力資訊，在不傷害到人或環境或機器人自己本身的 程度下即時快速地做出反應。

1.2 機器人之安全設計

由 Tsetserukou 等人打造了一隻機械手臂[1] (如圖 1.1)在手腕關節之前擁有四 個自由度，而且在每個關節上有他們自己設計的超薄型光學力矩感測器(如圖 1.2)， 而且選用了諧和式減速機(Harmonic Drive)，來達到對減輕重量、高準確性以及 高扭力的要求。每一軸都是由馬達、Harmonic Drive 減速機跟光學力矩感測器組 合而成，因此可以得到每軸的總力矩，再計算馬達出力後，就可以知道發生在每 軸上面的外力矩。

2

圖 1.1 Whole-Sensitive Anthropomorphic Robot Arm[1]

圖 1.2 薄型光學力矩感測器[1] 他們的控制方法是使用可變性的 joint-base 的阻抗控制(如圖 1.3)，以每一軸 當作一個彈簧與阻尼的組合，而外力就可經由每軸的力矩感測器得知。他們的機 械臂分成兩種操作模式，一種為工作模式，此模式需要較高的準確性，所以也需 要較高的剛性。另一種為碰撞模式，碰撞時為了不要造成環境人員或機器人本身 的傷害，所以需要較低的剛性，以達到比較柔軟的反應。而要達到此功能，可以 藉由調整阻抗控制律裡面 J、D、K 係數，來達到這樣的反應。並且有對因為阻 抗控制所造成的震盪做震盪阻尼控制[2]，對系統加上一個相位領先補償器來達 到目標。

3

圖 1.3 joint impedance control model[2]

Wyrobek 等人發表的文獻[3](如圖 1.4)提到對於家用機器人來說，最嚴苛的 考驗就是對於人類的安全考量的問題，因此他們提出了一種機械結構上一個新設 計來解決此問題。在減少背隙方面，他們使用的是皮帶帶動皮帶輪的方式來取代 傳統的齒輪傳動。再來是因為需要達到輕量化，所以馬達方面也使用規格較小的 馬達，如此一來能夠承受的負載一定會不會太大，因此，他們特別設計了一種重 力補償機構，讓馬達不需要承受連結馬達的連桿的重力影響，而且這重力補償機 構同時也可以即時的吸收外力，達到類似順應性的效果。這重力補償機構就是由 彈簧構成(如圖 1.5)，而且也可以讓機械手臂在受到碰撞時可以非常即時的反應， 看起來就像一隻很柔軟的手臂。

4

圖 1.5 PR-2 的重力補償結構示意圖[3]

由 Institute of Robotics and Mechatronics 跟 German Aerospace Center (DLR)共 同研發的 DLR LWR(lightweight robot) [4]是一個高度整合且控制方面完整性高的 機械手臂(如圖 1.6)。如圖 1.7 所示，他們將自行研發的馬達、驅動器、電源供應 器、諧和式減速機跟力矩感測器全部整合在一個軸關節上，機械手臂的外殼(也 是連桿結構)是使用碳纖維來降低重量。為了增加準確性，他們除了在馬達上裝 有位置感測器(即一般馬達的編碼器的功用)，也有裝配連桿位置感測器，來針對 輸出軸的端點的位置做更準確的定位。 圖 1.6 DLR LWR[4]

5 圖 1.7 DLR LWR 的軸關節整合結構[4] 在碰撞偵測方面[5]，DLR 機械手臂使用能量來偵測碰撞的發生。使用能量 守恆的觀念，當有外力對手臂發生碰撞時，就是有外來能量加諸在手臂上，以總 能量增加的量來計算出一個碰撞偵測訊號。此外，DLR 系統還有一個碰撞鑑定 的演算法，可以辨認出卡氏座標下力量的方向或是碰撞發生在哪一關節，以利於 後面碰撞後反應的部分。碰撞策略方面，他們對於一個機械手臂在工作時面對的 所有情況提出了五種策略(Strategy)，經由組合後可以達成有安全考量在裡面的反 應行為。其所提出之五種策略簡介如下： Strategy 0：為最一般的操作模式，機器手臂沒有反應在裡面，完全跟隨路徑規畫 好的路徑行進。 Strategy 1：一旦偵測到碰撞，機器手臂就會馬上停止。 Strategy 2：將控制模式從位置控制切換到零重力力矩控制，讓機器手臂可以有非 常順應式的行為。 Strategy 3：切換到有重力補償的力矩控制，可是跟 strategy2 不一樣的是，這裡 使用關節力矩回授，並且把馬達跟 link 的慣量經由一個 scale 比例縮小，因此可 以得到感覺行為、行動上好像更輕的機械手臂。 Strategy 4：使用估測到的外力來實現導納控制器。 結合軌跡規畫、碰撞偵測跟反應策略後，運作的流程圖如圖 1.8 所示。機械

6 手臂經由預先設定好的軌跡行進，如果力矩感測器偵測外力造成的高扭力的發生 (扭力大於一個臨界值)的話，此時就會從 Strategy0~4 選一種來進行反應(圖中是 選 Strategy2 來反應)，以此達到順應的目的。

1.3 順應性運動控制之研究背景

在追求高效率的時代，機械手臂被運用在工廠搬運與組裝日漸普遍。機械手 臂在執行任務的時候單純進行位置控制，將零件放在固定的物件上。由於沒有力 量方面的控制法則，因此如果沒有良好的位置規畫時，有可能導致硬體的損傷。 所以在位置控制裡面加入力量控制的想法就因此產生。 所謂順應性(compliance)是描述機械手臂與環境有碰觸時，對接觸時所反應 出來的力或力矩所追行之運動特性。順應性運動(compliance motion)則是指相互 作用期間機械手臂終端效果器之軌跡受接觸力作用而產生變動之運動過程。因此， 順應性運動是順應性之具體表現。所謂順應性控制(compliance control)，就是當 機械手臂接觸環境面時，讓機械手臂隨著與環境的接觸力量及運動情形對環境產 生適當之順應能力，除了控制接觸力外，還頇兼顧機械手臂之位置控制，其中順 應性運動控制分為被動式與主動式兩大類。 Trajectory Scaling Collision Reaction Collision over ? ˆ_{ext det} τ >τ ? no yes no yes 圖 1.8 流程圖與實際軌跡示意圖[5]

7

1.3.1 被動式順應性運動控制

安全連結機構(Safe Link Mechanism, SLM)(如圖 1.9)跟安全關節結構(Safe

Joint Mechanism, SJM)(如圖 1.10)[6-8]，是基於安全性設計的被動結構，其中包 含了線性彈簧、滑動機構、傳動軸等等組成。利用彈簧的彈性吸收外來突然發生 的外力，可以有效即時的對外力產生反應。但是這種機構上的彈簧設計也會使得 整隻機械手臂成為一彈性系統，在控制方面不但不好控制，負重方面也會有所受 限，而準確性也會隨著負重而不穩定。 圖 1.9 (a)一種由纜線構成的 SLM 的結構圖(b)此 SLM 結構受力時的動作圖[7] 圖 1.10 此 SLM 結構的實際圖[7] 圖 1.11 (a)一種 SJM 的結構圖(b)此 SJM 結構受力時的動作圖[8]

8

圖 1.12 此 SJM 結構的 prototype [8] 圖 1.13 此 SJM 結構實際安裝情形[8]

可變剛性致動器(Variable Stiffnes Actuators,VSA)[9-11]利用了可變硬度傳輸 結構來改變機械結構的硬度，通常需要用額外的致動器來改變機械結構(例如葉 彈簧的應力角度如圖 1.15)以達到硬度的改變，可以因應不同的工作內容來改變 硬度。例如:需要高準確性的工作時，硬度就可以因應這樣的工作內容而調高。 但是該種可變硬度致動器需要用到除了自由度以外的致動器，而且該何時改變多 少的剛性也很難被定義出來。 圖 1.14 此 VSA 結構實際圖[10] 圖 1.15 VSA 結構改變硬度時的動作圖[10]

9 上述幾種被動式順應通常是由機械式的結構組成，主要是改善硬體的架構來 改變接觸力，如夾爪機構、接觸機構等。使用被動式的設計的話，雖然可以有很 快速的反應，但是會造成外力被彈性結構吸收，而無法知道實際上到底有沒有外 力發生，如果有外力發生也無法知道外力大小有多少，無法達到後續應對外力產 生反應的工作目的，較不符合我們的需求。

1.3.2 主動式順應性運動控制

主動式順應主要利用軟體的控制器來使硬體具有順應性的功能，這樣的方法 比之前被動式控制的方法要更具強健性。力量回授雖然可以獲得良好的控制效果， 但是對於一個多自由度的機械手臂系統，系統的動態方程式裡的許多參數都無法 被實際測量，因此欠缺對位置與速度的直接控制，實際上會造成系統位置與速度 的響應不如預期。為了同時對系統進行位置與力量控制，近年來有許多學者在順 應性控制方面進行了許多研究，大致可分為混合式位置/力量控制(Hybrid motion control)以及阻抗控制(Impedance control)這兩種方法。

1.3.2.1 混合運動控制(Hybrid motion control)

混合運動控制的主要概念是將力與位置控制分開(如圖 1.16 所示)，當機械手

臂與拘束面接觸並沿著拘束面移動時，使機械手臂末端在非侷限自由度方向下實

行位置控制，在侷限自由度方向實行力量控制，最後算出每個關節扭矩來控制整

個機械手臂。首先在 1976 年由 Paul 與 Shimano[12]將機器人運動控制分成位置

控制部分與力量控制部分，分別屬於各自的控制迴路裡。1981 年 Mason[13]將混

合力/位置控制之觀念做了比較完整的描述。Raibert 與 Craig[14]使用 PID 控制法

則來設計力量與位置控制器，在經由選擇矩陣來決定機械手臂之終端效果器的工

作方向，作為判斷哪個方向為位置控制，哪個方向為力量控制，最後以電腦模擬

10

非侷限自由度的方向，如果今天是面對一個複雜的幾何形狀，將會大大增加此控

制法的困難度。

1.3.2.2 阻抗控制(Impedance control)

阻抗控制(Impedance control)[15]的概念是由 Hogan 在 1985 年提出的觀念與

控制方法，其主要構想認為力與位置在物理系統之任一自由度方向上無法被同時 決定，所以提出調整機械阻抗的方式達到順應運動，也就是期望的質量、阻尼以 及剛性的組合(如圖 1.17 所示)，使機械手臂對於環境能有適當的順應性，稱之為 目標阻抗的設計。Kazerooni[16]以頻率的觀點來討論阻抗控制，並且設計了一種 強健性阻抗控制器，用於排除模型之不確定性。Anderson 與 Spong[17]提出混合 阻抗控制來處理接觸力的部分。Goldenberg[18]以回授與前授補償來消除環境的 不確定項。Slotine 與 Li[19]延伸適應自由空間運動控制到適應受限運動控制。Lu

和 Meng[20]將 Slotine 與 Li 的適應控制法則，進一步應用在 Hogan 的阻抗控制

上，加入目標阻抗參考軌跡(target-impedance reference trajectory,TIRT)以處理力量

感測器的不精確問題以及系統參數的不確定性。 混合式位置/力量控制法需要事先明確的知道侷限自由度的方向與非侷限自 由度的方向，如果今天是面對一個複雜的環境，將會大大增加此控制法的困難度。 阻抗控制中，頇要先推導出機械手臂的動態方程式，才能有效的控制機械手臂。 由於動態方程式中的慣性項、離心力、科氏力項…等需要用估測的方式獲得，增 圖 1.16 混合運動控制控制架構圖

11 加控制實現上的許多困難。

1.4 系統架構

本系統的使用環境，預設為一有人類存在機器人活動工作區域的環境場 所。因此在機械手臂的工作過程中，必頇隨時面對可能發生人員闖入工作區域內， 以及任何可能發生的碰撞，也就是外力的發生。為了安全上的考量，所以面臨到 的問題的第一個問題是如何即時的知道目前發生了外力的發生，第二個問題是如 何讓碰撞程度可以不傷害環境或人類以及機器人本身，第三個問題是在外力發生 後機器人如何經由一些反應策略來反應進而繼續完成工作。未來經由克服這三個 問題來達到機械手臂在居家生活環境工作上面的安全要求。 在本論文裡，我們將機械手臂的行為分為兩類，第一類情況為靜止狀態，就 是機械手臂正在待命的任何情況。第二類情況為運動狀態，包括機械手臂在移動 的所有情況，無論是正在移動物體，或是正在前進到物體的路途中。 靜止狀態裡面我們設計一順應性運動控制於機械手臂上，讓機械手臂能夠跟 人類手臂一樣，是可以感受到外力並且隨著外力運動，以提高機械手臂在一般人 類活動環境中的安全性。 以人類來說，當我們伸手去拿東西時，若遭遇障礙物，在視覺還來不及看到 的時候，本能的會對障礙物進行繞路的動作，因此本設計的概念也由此產生。運 動狀態裡，設計了一個重新計算路線機制，使機械手臂在空間中移動時，萬一接 觸到障礙物時，會根據接觸力的大小以及方向計算出一個繞路點，使機械手臂退

Robot arm Target

Force feedback Joint feedback Desired position Impedance control Mx Bx kx  F 圖 1.17 阻抗控制控制架構圖

12 後到繞路點後，再次往目標點前進，以嘗試著繞過障礙物。如圖 1.18 所示，如 果機械手臂跟環境產生碰撞，經由力感測器得知外力的發生，遊安全行為策略決 定機械手臂反應後，由逆向運動學將卡式空間的命令轉換成軸空間的命令，下達 給機械手臂執行。

1.5 章節說明

本論文一共分為七章，第一章介紹研究動機與目的，並且概略說明所設計之 系統架構。第二章說明雙臂機器人實驗平台的設計與組建。第三章則為具冗餘自 由度的機械手臂之運動學分析。第四章為機械手臂順應性運動控制方法。第五章 為機械手臂的安全行為策略設計。第六章為實驗結果，驗證整體設計上之可行性。 第七章為結論與未來展望。

Force Sensor Robot Arm

Compliance Motion Control Re-route Strategy Environment Inverse Kinematics Safety Behavior 圖 1.18 系統架構圖

13

第二章 雙臂機器人設計與實現

本論文之主要目標為實現一具能夠安全執行任務並且自由度接近於人類之 雙臂機器人，並且設計一運動控制演算法使其可以在執行任務的時候即使發生了 碰撞，也可以安全的反應以及產生後續動作。關於本論文中雙臂機器人的整體架 構之設計與軟體、硬體、機構之實現都將在本章節中說明，其內容包含： 1. 本論文所提出之整體系統架構，包含系統訊號流程。 2. 實現設計所需之硬體設備及機構，包括感測器與機構設計概念等。

2.1 雙臂機器人之整體架構

本系統主要目的為接收由程式端使用者輸入之空間位置命令，以逆運動學即 時算出機械手臂每一軸的角度，並且使用安裝在手腕部位的六軸力感測器回授外 力資訊，使機械手臂能夠在靜止的狀態下，如果發生了與環境的碰撞，可以根據 外力資訊順應外力運動；而當機械手臂在運動的過程中，如果發生了與環境的碰 撞，根據外力資訊產生新的軌跡試圖繞過行進過程中的障礙物。使雙臂機器人在 執行工作時對普通環境有更安全的考量。

2.2 雙臂機器人之機構設計

本論文自行設計開發一具雙臂機器人機構，在機器人機構材料方面選用鋁合 金，主要優點在於鋁合金質輕且強度高，相當廣泛的使用於航太科技或是汽車上。 另外，以及在轉軸的部分使用鋼製作。在機器人構成整體結構的部件上，則是大 量使用了螺絲的以及 L 型的支撐設計方式，可以在不增加組裝的情況複雜度下， 強化部件的結構強度。 對於雙臂機器人來說，擁有足夠的自由度能夠讓機械手 臂能夠以各種姿態來執行各種任務是其重要之能力，因此整個手部機構的設計就 顯得相當重要。所以在設計之前必頇先決定手臂關節自由度的數量。若自由度數 量不足，則動作將較不靈活，行動也會受到較多的限制，但是若自由度過多，則

14 會增加控制上的複雜度與運算量以及機構設計上的困難，因此必頇取得設計上的 平衡點。雙臂機器人身體的部分主要是用來放置馬達控制器，因此必頇考量所以 需要裝進去的配備來進行設計，並且分配其安裝空間。另外，由於雙臂機器人在 運動中整隻機械手臂的姿態是處於不斷變化的狀態，在馬達配線部分也必頇注意 要留有足夠了拉伸容忍度以及避免被機構夾到，否則線路及機構都容易受損。

2.2.1 雙臂機器人整體自由度配置

在本論文之雙臂機器人，其設計重點主要在於使其動作靈活度與動作模式可以 類似於人類，並且為了使機構的設計上得以簡化與動作控制的複雜度得以降低， 我們必頇選擇使用最少但符合靈活度要求的自由度，來進行雙臂機器人的設計。 所需之動作靈活性如圖 2.1。

2.2.2 雙臂機器人手臂機構設計

本論文中雙臂機器人主要包含三個部分，分別是肩部、肘部和腕部與手爪。 圖 2.1 單邊機械手臂自由度示意圖

15

2.2.2.1

自由度之機構設計

自由度，本軸的減速模組包含了兩次減速裝置，第一次減速裝置裝設在馬達與 諧和式減速機中間，使用的是減速比 1:4 的時規皮帶輪，第二次的減速裝置為減 速比 1:120 的高扭力諧和式減速機，如圖 2.2 圖 2.3 為機構相關的配置圖與組立 圖。 圖 2.2 機構之配置圖 圖 2.3 機構之組立圖

16

2.2.2.2

自由度之機構設計

自由度，跟 自由度之設計一樣，包含了兩次減速裝置，第一次減速裝置裝設 在馬達與諧和式減速機中間，使用的是減速比 1:4 的時規皮帶輪，第二次的減速 裝置為減速比 1:120 的高扭力諧和式減速機，如圖 2.4 圖 2.5 為機構相關的配置 圖與組立圖。 圖 2.4 機構之配置圖 圖 2.5 機構之組立圖

17

2.2.2.3

自由度之機構設計

自由度，由於配置上的考量，所以使用馬達直驅減速比 1:120 諧和式減速機， 達到我們所需要的旋轉方式。並且在容易受力型變的柱子結構上，使用 L 型柱 的設計，來加強結構的強度。如圖 2.6 圖 2.7 為機構相關的配置圖與組立圖。 圖 2.6 機構之配置圖 圖 2.7 機構之組立圖

18

2.2.2.4

自由度之機構設計

自由度，跟 和 自由度之減速設計一樣，包含了兩次減速裝置，第一次減速 裝置裝設在馬達與諧和式減速機中間，使用的是減速比 1:4 的時規皮帶輪，第二 次的減速裝置為減速比 1:120 的高扭力諧和式減速機，如圖 2.8 圖 2.9 為機構相 關的配置圖與組立圖。 圖 2.8 機構之配置圖 圖 2.9 機構之組立圖

19

2.2.2.6

自由度之機構設計

自由度，考慮到在此處，背隙的影響較前面幾軸來說已經很小了，所以從此處 開始，減速機構開始使用跟馬達為同樣廠牌的 maxon 行星式減速機，而直接與 馬達接合，不需再多做處理，如圖 2.10 為機構相關的配置組立圖。

2.2.2.6

自由度之機構設計

自由度跟上述 自由度一樣，減速機構方面使用跟馬達為同品牌的 maxon 行星 式減速機，並在外部外加一組 1:1 正齒輪，讓轉動軸可以位移到馬達下方。在與 上一軸的連結上，使用止付螺絲讓本自由度零件與上一軸的馬達軸結合。如圖 2.11 為機構相關的配置組立圖。 圖 2.10 機構之配置組立圖

20 圖 2.11 機構之配置組立圖

2.2.2.7 手爪之機構設計

手爪部分，為了達成人類最常使用的手掌與手指功能(例如:握筆、拿瓶子、 拿杯子等等…)，在爪子尖端使用了凹槽設計，是為了從桌面上拿起筆，而手指 中間的曲折，是為了抓起一般外型簡單物體，而從減速機直驅手指的設計，也是 為了有強壯的握力，能夠達成功能。如圖 2.12 為機構相關的配置組立圖與使用 情境圖。而全部手臂機構組裝後的組合如圖 2.13。 圖 2.12 手爪機構之配置組立圖與使用情境

21 圖 2.13 雙臂機器人 3D 組合圖

2.3 雙臂機器人之控制系統硬體架構

圖 2.14 為雙臂機器人之控制系統硬體架構圖，在本論文中的雙臂機器人的 設計中，在機器人肩膀以下的身體部位除了用來支撐起手臂的高度外，主要是用 來放置 14 個直流伺服控制器和 CAN 匯流排介面的電路分接板以及 CAN 匯流排 介面控制器。工業電腦與電池則不放置在機器人主體上。力感測器安裝在右臂的 手腕部位，而力感測器的資料擷取卡則透過 ISA-bus 插在工業電腦上。 Motor Controller1 CAN_L CAN_H CAN-bus Interface Controller CAN_H CAN_L IPC Motor Controller7 DC Servo Motor 1 DC Servo Motor 7 Motor Controller8 Motor Controller14 DC Servo Motor 8 DC Servo Motor 14 Right arm Left arm DSP-based Data receiver ISA bus USB 6-axis Force/Torque Sensor 圖 2.14 控制系統硬體架構圖

22

2.3.1 運算系統之實現

本論文之雙臂機器人採用以 PC 為基礎的運算系統，為了配合力感測器的

A/D 轉換擷取卡(ISA bus 介面)，使用了函數科技的 ISA bus 介面的工業電腦

FB2602A，實體如圖 2.15 所示，選用 INTEL 公司時脈 1.4GHz 的中央處理器及 512MB 的記憶體。儲存媒體使用傳統 IDE 介面硬碟。

2.3.2 力感測系統之使用

在本論文中，使用了 Nitta 公司出的 JR3 力感測器系列中的 67M25(如圖 2.16)。 67M25 是 JR3 的一款六軸力和力矩感測器。外型為圓柱型，其 67 毫米厚，直徑 25 毫米。其中包括電子訊號放大器，類比轉數位轉換器。該 67M25 感測器可每 秒輸出 2 兆位的六軸資訊。而該 67M25 是從固體坯加工的鋁，由此產生的整體 結構可提供了無與倫比的性能和精密度。該 67M25 使用金屬箔應變計作為傳感 元件。該 67M25 可以連接各種不同的電子接收器。但 JR3 有配備了 DSP-based 的資料擷取卡(如圖 2.17)，可用於一些工業電腦的匯流排傳輸。此接收器能夠處 圖 2.15 工業電腦 FB2602A

23 理的力和力矩的全部數據並且有 8 kHz 的數據傳輸速率。並且可以提供去耦，坐 標轉換，低通濾波，向量計算，極限值監測，採集峰值和速度計算。

2.3.3 馬達控制系統之設計與實現

在本論文中由於雙臂機器人採用的是直流伺服馬達，這樣同時也具備了定位 的高精準度。在馬達的選用方面，主要是採用 MAXON 公司所生產的直流伺服 馬達，包含了直流馬達、減速機和編碼器三部份。而目前的手臂設計則是將馬達 區分成 4 種不同大小扭力的等級，分別為 RE-35、RE-max29、RE-max24 和 RE-max21，而各自由度的馬達規格以及減速比如表 2.3 所示，實體圖如圖 2.30。 圖 2.16 JR3 力感測器-67M25 圖 2.17 資料擷取卡

24 吾人共使用 14 個馬達，每自由度使用 1 個馬達，6 個 RE-35， 2 個 RE-max29， 2 個 RE-max24，4 個 RE-max21，左右手臂各 6 個自由度(不包含手爪開合的自 由度)。從肩膀負載比較大的地方，使用大扭力的馬達，到手肘使用中扭力的規 格，手腕使用較小扭力的規格，以符合實際需要。而手爪部分使用了相較於手腕 扭力規格大的配置，是希望可以有足夠的握力來應付實際上的各種需要。而各馬 達規格與機構活動之效能如下表 2.1。在馬達控制器的部分，使用的是同樣由

MAXON 所生產的直流伺服馬達控制器 EPOS 24/1 與 EPOS 24/5，實體如圖 2.18

與圖 2.19，概略規格如表 2.2，可藉由排線直接讀取編碼器數值並控制馬達進行

位置控制、速度控制和電流控制等數種控制，在本論文中，吾人所使用的控制方

式為位置控制，其控制方式如同一閉迴路，在吾人下達一位置命令後，控制器會

表 2.1 馬達規格與機構活動之效能表

DOF-1 DOF-2 DOF-3 DOF-4 DOF-5 DOF-6 DOF-7 描述 肩部旋轉 肩部彎曲 肘部旋轉 肘部彎曲 腕部旋轉 腕部彎曲 手爪開合 使用馬達 RE-35 RE-35 RE-35 RE-max29 RE-max21 RE-max21 RE-max24

額定電壓 24V 馬達原始連 續轉矩 (mNm) 93.3 26.9 6.89 11.2 馬達原始連 續轉速(rpm) 6910 7680 6670 7742 MAXON 減速機 無 無 無 無 1: 690 1: 690 1:900 皮帶輪組 1:4 1:4 無 1:4 無 無 無 諧和式 減速機 1:120 1:120 1:120 1:120 無 無 無 總減速比 1:480 1:480 1:120 1:480 1: 690 1: 690 1:900 輸出連續轉 矩(mNm) 44784 44784 11196 14208 4754 4754 10080 連續轉速 (rpm) 14.4 14.4 57.6 16 9.7 9.7 8.6 機構 活動角度 -180°~ 180° -30°~ 115° -180°~ 180° -105°~ 105° -105°~ 105° -110°~ 110°

25 不斷讀取編碼器讀值並加以控制馬達誤差修正以達成所設定之命令，而修正之方 式為 PID 控制，吾人可藉由調整 PID 各參數，使控制器的誤差修正效能有不一 樣的表現。另外，在馬達控制器的數量上，由於一個控制器只能控制一個馬達， 因此控制器的數量同樣也是 14 個。 圖 2.18 EPOS 24/1 ID switch Motor encoder connector Motor power connector 圖 2.19 EPOS 24/5 表 2.2 馬達控制器規格表 EPOS 24/1 EPOS 24/5 Supply voltage( ) 9V~24V 11V~24V

Max output voltage 0.98* 0.9*

Max output current(<1s) 2A 10A

Continuous output current _{1A 5A}

26

而在分配控制器命令的方面，是由 CAN 匯流排介面負責傳輸，因為與 RS232

傳輸介面相比，CAN 匯流排介面可以更輕易的連接多個節點與有著更高的傳輸

速 度 。 吾 人 在 此 所 選 用 的 是 與 MAXON 馬 達 控 制 器 相 容 的 由 National

Instrument(NI)所生產的 CAN 匯流排介面控制器，實體如圖 2.20，可藉由 USB 傳 輸 介 面 與 電 腦 相 連 接 ， 並 將 電 腦 端 所 發 出 的 命 令 藉 由 CAN‐High 與 CAN‐Low 兩條訊號線傳輸到各個控制器節點，如圖 2.21 為將馬達電源與 CAN 匯流排連接至各個並聯控制器之電路分接板。

2.4 雙臂機器人之軟體設計與實現

本論文之機械臂是在 PC 的視窗系統環境下進行控制程式與演算法的編寫， 只要透過 ISA-bus 載入類比轉數位轉換器與直流伺服馬達控制器的驅動程式，依 照原廠所提供的指令即可對硬體進行控制，整合性與方便性相當高，對於圖形化 界面的編寫也相當容易。 圖 2.20 NI CAN-bus 圖 2.21 並聯控制器之電路分接板

27

第三章 機械手臂之運動學分析

在機器人學中，分析的面向主要可分為運動學（Kinematics）以及動力學 （Dynamics）兩部份。以一個多連桿系統為例，在運動學的分析中，主要是在計 算各連桿之旋轉角度所產生在空間中的位移量。而其中可再分為順向運動學 （Forward‐Kinematics）和逆向運動學（Inverse‐Kinematics）。本章節分幾部分說 明，首先先針對座標系統與向量做介紹，再來根據 D-H 方法推導順向運動學， 接著以幾何分析方法對一個具有冗餘自由度的機械手臂進行逆向運動學分析推 導，使機械手臂能夠實際執行位置命令。

3.1 連桿參數與座標

在討論 D-H 變換矩陣之前，首先必頇建立機器人的連桿座標系，並定義相 關係數。這裡第 i 軸(或座標系{i})為連桿 i-1 與連桿 i 的連接處。而我們採用之手

臂軸系及係數是根據 R.P.Paul 所著<Robot Manipulator>[24]一書內之定義，綜合

如下： 1. 連桿 i：介於軸系{i+1}及軸系{i}間的連桿。 2. 軸：該軸方向的選取是沿著軸系{i}的線性移動或旋轉軸。 3. 軸：該軸方向的選取沿著軸系{i+1}及軸系{i}的共有正交線。 4. 軸：該軸方向的選取要使 、 及 軸成右手座標系法則。 5. ：連桿的長度。即沿著 X 軸方向與 軸 軸共有正交線的長度。 6. ：連桿的扭轉角。 軸相對於 軸的角度。 7. ：連桿間的距離。即在 軸上兩個共有正交線間距離。 8. ：連桿間的角度。可視為旋轉軸的轉角。 假設關節 0 與關節 1 的座標關係如圖 3.2 所示，其中關節與關節之間的座標 關係如下：首先對 軸旋轉 角，之後再沿著 x 軸方向平移距離 a，在沿著 z 軸 方向平移距離 d，最後在對 軸旋轉 角。其齊次變換公式為

28

( , )

( , )

( , )

( , )

i

A



Rot Z



Tran Z d Tran X a Rot X



(3.1)

cos

cos

sin

sin

sin

cos

sin

cos

cos

sin

cos

sin

0

sin

cos

0

0

0

1

i i i i i i i i i i i i i i i i i i

a

a

A

d





































_





















(3.2) 圖 3.1 連桿參數及座標系變換 圖 3.2 關節和關節之間的座標關係

29

3.2 順向運動學

順向運動學為給定各關節軸之旋轉角度數據，進而求得末端桿件（link）在 絕對座標系中的空間位置；而逆向運動學則是以給定末端桿件在絕對座標系中的 空間位置，反過來求解各軸關節的旋轉角度。若已知各桿件向量和各關節旋轉角 的資料，可利用順向運動學方程式推導出各關節點之空間座標，也可由連續的終 端效果器(end-effector)座標形成運動軌跡。本論文採用的是 D‐H model 建立機器 人運動學模型，用以進行順向運動學以及逆向運動學的計算。D‐H model 是一 種針對連桿運動設計的運動學分析法，可以對連桿在 3 度空間的移動與轉動有 著簡單而完整的描述。 利用 D‐H model 來建立運動學模型，建立順序為由肩膀部位到手腕部位， 如圖 3.3 所示，表 3.1 為雙臂機器人之右機械臂 D‐H model 的參數表，其中每 4 個 D‐H 參數（ 、 、a、d）可決定出一個手臂自由度的座標轉換矩陣，如式(3.2)， 左上角 3*3 矩陣代表的是旋轉方向，而第 4 行前三個元素則是代表 X、Y、Z 方 向的位移。整個手臂有多少自由度就有幾個座標轉換矩陣作為代表，將所有自由 度的代表矩陣照順序乘積起來即可得知操作臂末端的姿態與位置的矩陣。 表 3.1 右機械臂每軸的 D‐H 參數表 a d Joint1 0 0 Joint2 0 0 Joint3 0 L 1 Joint4 0 0 Joint5 0 0 L 2

30 1 1 1 1 1 cos 0 sin 0 sin 0 cos 0 0 1 0 0 0 0 0 1 A                    (3.4) 2 2 2 2 2 cos( 90 ) 0 sin( 90 ) 0 sin( 90 ) 0 cos( 90 ) 0 0 1 0 0 0 0 0 1 A           _{ }           (3.5) 3 3 3 3 3 1 cos( 90 ) 0 sin( 90 ) 0 sin( 90 ) 0 cos( 90 ) 0 0 1 0 0 0 0 1 A L           _{ }      _      (3.6) 4 4 4 4 4 cos 0 sin 0 sin 0 cos 0 0 1 0 0 0 0 0 1 A        _           (3.7) 5 2 1 0 0 0 0 0 1 0 0 1 0 0 0 0 1 A L               (3.8) 5 1 2 3 4 5

T



A A A A A

(3.9) 圖 3.3 機械手本體與 D-H 座標系

31

3.3 具冗餘自由度之逆向運動學分析

基於雙臂機器人之對稱性，我們先針對單邊機械手臂來進行分析在對應到另 一邊手臂。我們將針對右機械手臂進行逆運動學的分析。由於我們的力感測器將 會安裝在機械手臂的手腕部位，所以我們將採用 wrist‐partitioned 的類型[24]， 將逆運動學的分析從手腕部位分開處理。由於手爪姿態較容易由於任務不同有不 同姿態，且我們將力感測器裝在手腕部位，因此我們將針對手腕點來推導出較符 合人類自然姿態且具有冗餘自由度的逆向運動學公式，也就是從卡氏座標的 X、 Y 和 Z 座標推出 、 、 和 的座標。 圖 3.4 中定義了手臂座標系的 X、Y 和 Z 軸，以肩膀點為手臂座標系原點， u r 和r 分別為機械手臂上臂長與下臂長。當我們給定了一個目標座標，從肩膀延_f 伸到目標座標就為r_wrist。由於有一冗餘自由度，所以當我們手臂的末端效果器要 到目標座標時可以以無限多的姿態到達，求不出有效解。在這裡引用了冗餘圈[23] 的概念，也就是針對一個目標座標點所以可以到達的姿態，會在手肘點形成一個 圈，直徑為 R。而圖中由ru和r 形成的右手姿態來說，最重要的是要在這冗餘圈f 上找到適合的軸位置 (p_ex,p_ey,p_ez)來做為限制條件。 圖 3.4 手臂座標系與冗餘圏示意圖

32 Step 1：求 在已知手腕點的卡式座標 (p_wx,p_wy,p_wz)後，r_wrist就可以得到，我們就可以 利用三角關係來求出 ，如 3.10 式。 2 2 2 1 4 180 cos ( ) 2 u f wrist u f r r r r r  _{ }    (3.10) 其中ru和r 跟是上臂跟下臂的的長度，而 f 2 2 2 ( ) ( ) ( ) wrist wx wy wz r  p  p  p (3.11) 也就是從肩膀到手腕點的距離。 Step2：求手肘的位置 Step2.1 R ： _u 對右手的結構來說，手肘的位置通常會在從肩膀到手腕連線的右下方。所以 首先頇要先求得R_u向量的長度 R ，而_u R 就為冗餘圈的半徑長度R，如圖3.5_u 所示，我們可以根據海龍公式來求得由r_u、r 和_f rwrist構成的三角形的面積r

(

) (

) (

)

r

s s

r

wrist

s

r

u

s

r

f

 







(3.12) 而其中 ( ) 2 wrist u f r r r s   ，而我們要求的 R 就等於可表示成 _u 2 _r u wrist R r   (3.13) 圖 3.5 由手臂構成的三角形

33 Step2.2 r_m向量： 圖3.6手臂三角形裡各向量 m rm wrist r t r (3.14) 接著藉由R_u作為媒介算出r_m向量與r_wrist的比例關係，得到了r_m向量後，我 們就可以得到肘位置到肩膀與手腕連線的的直線上面的正交的點p_m。其中 2 2 2 2 ( ) ( ) ( ) 2 u f wrist rm wrist r r r t r    (3.15) Step2.3 向量： [ ( wy) _factor, _factor, 0] wx p C C p    (3.16) 接著定義通過pm且平行於水平面的 向量。而定義的考量為，首先由於是平行 於水平面的向量，所以 z 分量一定為零。再來，y 分量為C_factor，由於我們目前 為右手結構的推導，而 Y 軸正向的定義是朝左邊(如圖 3.7 所示)，所以C_factor定 圖 3.7 向量與各向量關係

34 義為”-1”，(如果是左手的話C_factor定義為”1”)，而我們定義 向量要垂直r_wrist， 因此就可以求出 向量的 x 分量，如 3.16 式。 Step2.4 向量： 如圖 3.8 所示，由於向量同時垂直於 向量與r_wrist向量，所以利用 向量 與r_wrist向量做外積就可以求得向量 wrist r    (3.17) Step2.5 (p_ex,p_ey,p_ez)： A B       (3.18) 由圖 3.9 所示可知，由 向量與向量做向量加法後可得到向量，我們可 以透過 向量與向量的係數 A 與 B 來控制向量的方向，而由前面已知 R 的_u 長度，可以算出向量與R_u 向量的 k 倍數關係， u R k   (3.19) 圖 3.8 β 向量與各向量關係 圖 3.9 向量與 β 向量型成 γ 向量

35 因此就可以求得手肘的位置 (p_ex,p_ey,p_ez)k  。 Step3：求  ₁, ₂, ₃ 算出我們需要的手肘的位置 (p_ex,p_ey,p_ez)後，  1, 2, 3就可以接續算出來。 由肩膀座標原點推導到手肘座標的順向運動學已知 1 1 2 1 2 1 2 2 2 2 1 2 1 1 1 2 1 2 1 2

cos sin sin sin cos sin cos 1

0 cos sin sin 1

( ) ( ) ( )

sin cos sin cos cos cos cos 1

0 0 0 1 y x z L L R R T L L                        _             (3.21) 而 1 1 2 1 2 1 2 2 2 2 1 1 2 1 2 1 2

cos sin sin sin cos sin cos 1

0 cos sin sin 1

sin cos sin cos cos cos cos 1

0 0 0 1 0 0 0 1 ex ey ez L p L n o a p L p                        _      _               (3.22) 因此我們可以得到下列聯立方程式 1 2 2 1 2 sin cos 1 (1) sin 1 (2) cos cos 1 (3) ex ey ez L p L p L p         _     (3.23) 將 3.23 式的(1)式除以(3)式可得 1 1 1 sin tan cos ex ez p p        (3.24) 利用 atan2 函式可求得 1 tan 2( ) ex ez p a p     (3.25) 將 3.23 式的(2)式除以(3)式可得 1 2 2 2 cos sin tan cos ey ez p p        (3.26) 利用 atan2 函式可求得 1 2 cos tan 2( ey ) ez p a p     (3.27) 再由肩膀座標原點推導到手腕座標的順向運動學

36 1 2 3 1 4 2 ( ) ( ) ( ) ( ) ( ) ( ) 0 0 0 1 wx wy y x z z y z wz p n o a p R R R T L R T L p                    (3.28) 將上式整理可得下式 1 1 3 1 4 2 2 1 ( ) ( ) ( ) ( ) ( ) ( ) 0 0 0 1 wx wy z z y z x y wz p n o a p R T L R T L R R p                      (3.29) 整理可得到下列聯立方程式 3 4 2 1 1 3 4 2 2 1 2 2 1 4 2 1 2 1 2 2 1

cos sin cos sin (1)

sin sin sin sin cos sin cos (2)

cos cos sin sin cos cos (3)

wx wz wx wy wz wx wy wz L p p L p p p L L p p p                                 (3.30) 將 3.30 式的(1)式和(2)式可得 2 1 2 2 1 3 3 3 1 1

sin sin cos sin cos sin

tan

cos cos sin

wx wy wz wx wz p p p p p                (3.31) 利用 atan2 函式可求得 2 1 2 2 1 3 1 1

sin sin cos sin cos

tan 2( ) cos sin wx wy wz wx wz p p p a p p             (3.32)

3.4 順向與逆向運動學總結及討論

本論文的順向運動學使用 D-H 變換矩陣來減少變數，僅使用四個變數(θ 、 α 、a 和 d)定義空間六軸的變換。而在推導具冗餘自由度的逆向運動學時，使用 了冗餘圈的概念來達成推導，再配合幾何與數學的關係，便可以從空間的座標 (x,y,z)轉換成軸空間的(1 ~4)。 透過本逆向運動學分析後，便可以給定卡氏目標座標，得到軸空間 1~ 4座 標，並且可以透過調整 A 與 B 係數使機械手臂整體運動姿態是類似人類平常運 動的自然姿態。當機械手臂在運動的過程中，從每軸的馬達編碼器讀回脈衝訊號 再計算成角度後，可以利用順向運動學來檢查是否已經到達給定的目標座標。

37

第四章 機械手臂順應性運動控制

當機器手臂存在於一般的人類活動的環境中時，由於傳統機械手臂沒有力方 面的控制法則，當機械手臂在待命狀態下時，對於在居家環境中活動的人類來說， 機械手臂本身是有可能成為被人類碰撞到的障礙物，此時基於安全性的考量，我 們希望機械手臂對碰撞的反應是可以降低碰撞的程度，並且順應外力使碰撞感” 消失”的，因此我們對此設計一順應性運動控制於機械手臂上，讓機械手臂能夠 跟人類手臂一樣，是可以感受到外力並且隨著外力運動，以提高機械手臂在一般 人類活動環境中的安全性。我們使用的方法是，將安裝六軸力感測器的機械手臂 末端，並將機械手臂看成是一個三維空間中的小質量模型，當力感測器感受到力 時，整個機械手臂的運動行為將會遵行小質量模型的運動行為運動，來達到順應 外力的行為。不同於傳統的機械手臂力控制或是阻抗控制對於環境持續接觸的訴 求，本機械手臂順應性運動控制的主要考量是在居家環境中的安全性，希望減少 任何情況下碰撞的程度。因此我們只需考量機械手臂終端效果器的空間中位移， 不需要考慮旋轉情況。傳統的機械手臂力控制或是阻抗控制，需要推導機械手臂 的動態模型，而其中又有許多參數無法確切得知，我們將機械手臂的動態模型簡 化成小質量模型的動態模型，且可以藉由改變小質量模型的物理參數，來達到機 械手臂的剛性的改變。本章節將介紹本論文使用的機械手臂順應性運動控制的設 計。 在[21]中，林振暘學長使用了將整台平面移動式機器人看成是一個放在平面 上的質量，並且使用量測馬達的電流來得知使用者的意向，讓輪式機器人可以順 應人類施予機器人的力移動。因此，根據這樣的想法，我提出了機械手臂順應性 運動控制架構如下。

4.1 機械手臂順應性運動控制架構

圖 4.1 為本論文提出之機械手臂順應性運動控制架構圖，當力感測器感受到 外力時，讀到對力感測器座標系 XYZ 三軸的力資訊，經由三軸的校正曲線，校

38 正成具有單位的力資訊。由於機械手臂在運動時，安裝在手腕部位的力感測器的 座標系也會隨著手臂運動的姿態而改變，所以需要隨時將力感測器的座標系旋轉 成機械手臂的座標系，在藉由自由質量模型算出此時外力所造成的空間中位移量， 經由逆運動學轉換成每軸的旋轉位移量，最後由機械手臂讀回編碼器訊號再經由 數值轉換成每軸角度，然後再經由順向運動學得到現在的位置資訊，算出此刻頇 要給每軸各多少位置命令。

4.2.力感測器單位校正

直接經由 JR3 的 DSP-based 的接收卡讀到的力資訊量，需要作力單位的識別。 單位校正的方法為，由不同已知重量的砝碼，分別對力感測器的三軸做校正。實 驗重量分別為 50g、100g、200g、300g、400g、500g、600g、700g、800g、900g、 1000g，而由於力感測器的固定方式有許多限制，因此無法全對正 XYZ 軸或負 XYZ 軸作校正，在此本校正實驗在 X 軸的校正實驗為重量往-X 方向施壓，在 Y 軸的校正實驗為重量往+Y 方向施壓，在 Z 軸的校正實驗為重量往-Z 方向施壓。 而校正曲線如下圖 4.2 圖 4.3 圖 4.4，而校正式如 4.1 式 Coordinate rotation             k k k k θ1 θ2 θ3 θ4           k -1 k -1 k -1 x y z             k -1 k -1 k -1 k -1 θ1 θ2 θ3 θ4 Environment           k k k x y z           k k k F x F y F z           ' k ' k ' k Fx Fy Fz Encoder pusle 1 ~ 4 Motor Encoder pulse cmd PW M Inverse kinematics Free mass model Force sensor Forward kinematics Numeric conversion Motion

controller Robot arm

39 (0.566153592 _ ) 9.8 (0.536743537 _ ) 9.8 (1.430475316 _ ) 9.8 Data x Fx Data y Fy Data z Fz       (4.1) (單位:mNt) 圖 4.2 力感測器-X 軸的校正曲線 圖 4.3 力感測器+Y 軸的校正曲線

40 圖 4.4 力感測器-Z 軸的校正曲線

4.3 力感測器座標旋轉

由於機械手臂在空間中移動時，安裝在手腕部位的力感測器也會隨著機械手 臂的移動而有姿態的變化，例如:當力感測器目前姿態是以 45 度朝上，此時如果 對手腕施向下的力，由力感測器感測到的會是往-Z 軸跟往-X 軸的合力，而機械 手臂也會因為這樣產生與我們期望不同運動。因此需要將讀到的三軸力資訊經由 座標旋轉，將力感測器的座標系隨時旋轉成機械手臂的座標系，如圖 4.5。直接 從力感測器讀到三軸的值為Fx_k、Fy_k和Fz_k，需要先後對 X 軸 Z 軸 Y 軸做旋轉， 而 ' k Fx 、Fy 和_k' Fz 是旋轉過後相對於機械手臂座標系的向量值。 _k' 圖 4.5 機械手臂座標系與力感測器座標系

41 ' ' ' _ _ _ k k k k k k Fx Fx

Fy Fy rot x rot z rot y

Fz Fz       _        _ _   (4.2) 而其中， 1 0 0 _ 0 cos( ) sin( ) 0 sin( ) cos( ) x x x x rot x         _  _     (4.3) cos( ) 0 sin( ) _ 0 1 0 sin( ) 0 cos( ) y y y y rot y            _    (4.4) cos( ) sin( ) 0 _ sin( ) cos( ) 0 0 0 1 z z z z rot z                 (4.5) 3 1 4 2 90 x y z               (4.6)

4.4 力感測器座標實驗

本實驗使機械手臂手腕部位安裝力感測器，並使手腕點到達機械手臂座標系 {300,-100,-100}(單位:mm)的位置(如圖 4.6 所示)，而後在力感測器上掛上 1000g 的砝碼來進行驗證。 圖 4.6 機械手臂姿態受力示意圖

42 圖 4.7 力感測器安裝在手臂上的姿態 圖 4.8 力感測器正常的姿態 圖 4.9 各力向量於機械手臂座標系 如圖 4.9 所示，藍線實線是當機械手臂手腕點到達機械手臂座標 {300,-100,-100}(單位:mm)的位置時，力感測器對於目前施加 1000g 的重量，而 力感測器的姿態與座標系如圖 4.7 所示，直接量測出來的向量。紅線點虛線是將 力感測器的座標經過計算而旋轉後得到的向量，綠色虛線是將力感測器實際平放 (如圖 4.8 所示)同樣施加 1000g 的重量而直接量測出來的向量，而實際數據如表 4.1。

43

表 4.1 座標旋轉誤差表

X-axis Y-axis Z-axis

Original gesture -40.3452 62.3608 -65.9742 After rotation 8.1882 -0.2547 -99.0056 Direct measurement -1.2845 -6.6655 -102.4796 Error 9.4727 6.1084 3.4739 (單位:mNt)

4.5 自由質量模型

我們希望機械手臂能夠跟人類手臂一樣，可以感受外力並且順應外力自由移 動。因此我們將安裝六軸力感測器的機械手臂手腕點看成是一個三維空間中的小 質量模型(如圖 4.10 所示)，所以當受到外力時，機械手臂的運動行為將會遵行小 質量模型的動態模型。由於 X 軸的力與位移的關係與另兩軸(Y 軸與 Z 軸)的力與 位移的關係彼此獨立，在計算時也可以分開討論。因此對機械手臂的 X 軸來說 ' k k k Mx Bx Fx (4.7) M 為質量係數，B 為阻尼係數，k 則是代表每個取樣時間點， ' k Fx 則是代表目前 取樣時間點的 X 軸上的分力。其中 1 k k k

x

x

x

t







(4.8) 圖 4.10 手腕點模擬成自由質量示意圖

44 1 1 2 2

2

k k k k k k

x

x

x

x

x

x

t

t

  











(4.9) k x、x_k和x_k分別為質量物體在 X 軸上的位移、速度和加速度， t則是取樣時間， 目前程式內設定為 10ms。將 4.8 式與 4.9 式代入 4.7 式，就可得 ' 2 1 2

(2

)

k k k k

Fx

t

M

B t x

M x

x

M

B t

 











(4.10) 所以當機械手臂系統在第 k 個取樣時間點感測到 X 軸上有分力的時候，就可以 根據 4.10 式來得到第 k 個取樣時間點的位置。同理 ' 2 1 2

(2

)

k k k k

Fy

t

M

B t y

M y

y

M

B t

 











(4.11) ' 2 1 2

(2

)

k k k k

Fz

t

M

B t z

M z

z

M

B t

 











(4.12)

4.6 參數設定之影響

在上一小節中提到順應性控制架構中有兩個參數(M、B)，而這兩個參數將 會影響機械手臂之順應性程度，因此在此小節將探討 M、B 的影響。M 為我們 所期望的質量，當 M 越大時，機器手臂的速度反應將越慢，反之 M 越小反應速 度越快。B 為我們所期望的阻尼係數，B 越大表示阻力越大，將造成在同樣的力 輸入下，B 越大則速度越慢，反之則越快。根據 4.7 式我們利用電腦模擬 M、B 的影響，圖 4.11 為當 B 不變，在固定的力輸入下，M 變化對速度的影響，我們 可以發現 M 的變化影響了速度的暫態，M 越大速度上升越慢。圖 4.12 為當 M 不 變，在固定的力輸入下，B 對速度的影響，我們可以發現 B 的變化影響了速度達 穩態的時間以及穩態的終值大小，B 越大則達穩態的時間越久，穩態的速度也越 慢，反之，則時間越短且終值速度也越快。因此我們可透過參數的調整去適應使 用者的需求。

45

圖 4.11 M 對速度的影響模擬

46

第五章 機械手臂安全設計

當機械手臂可以達到藉由感知外力資訊並且順應著外力運動，我們可以進一 步對機械手臂安全性考量方面的設計。對於機械手臂的實用性來說，只有單純可 以順應外力的能力是無法能使機械手臂實際執行在居家環境中的任務。要能夠兼 顧實用性與安全性的機械手臂控制系統，無法使用單一種控制方法來設計。考慮 人類手臂在面對各種不同的情況時，會由各種資訊(例：視覺、力覺、觸覺等等) 來調整目前手臂的控制策略。當人類手臂目前需要抬起及支撐物體時，以機械的 角度來看，就是將剛性調大。當人類手臂需要順應環境外力或是與另一人類互動 時，會將手臂放軟，也就是將剛性調小。在居家環境不同於工廠內的作業區，當 機械手臂目前明確的知道任務抓取物品並進行移動，途中還是有可能出現障礙物， 無法是單一明確的任務內容。因此，需要設計出一套機械手臂的安全行為策略， 以符合機械手臂的安全性與功能性。

5.1 安全運動與重新移動策略

考慮到機械手臂在居家環境裡可能面對的大多情況，以及各種任務類型，大 致上可以將機械手臂所有行為分為兩類(如圖 5.1)。第一類情況為待命狀態，就 是機械手臂正在待命的任何情況。第二類情況為運動狀態，包括機械手臂在移動 的所有情況，無論是正在移動物體，或是正在前進到物體的路途中。 當機械手臂在待命狀態下時，對於在居家環境中活動的人類來說，機械手臂 本身是有可能成為被人類碰撞到的障礙物，此時基於安全性的考量，我們希望機 械手臂對碰撞的反應是可以降低碰撞的程度並且順應外力使碰撞消失的，而此時 的控制策略就為上一章說明的順應性運動控制。