雙足機器人站立姿態擾動平衡控制

全文

(1)國立臺灣師範大學機電工程學系碩士論文指導教授:陳美勇博士雙足機器人站立姿態擾動平衡控制 Bipedal Robot Standing Posture Balance Control under Disturbance. 研究生 : 李昀翰撰中. 華. 民. 國. 1. 0. 4. 年. 1. 月.

(2) 摘要本論文的主要目的為為實驗室展開中形人型機器人的研究，以期在機器人研究領域上有新的一步。人形機器人開發後的第一步首先要能成功站立，並可應付外界一定程度的干擾還能維持不倒，因此站立姿態擾動平衡恢復控制將為本文首要探討的運動規劃。人形機器人成功站立後，鎖定機器人膝關節，僅踝關節和髖關節可制動，由軌道能量的概念，針對外界水平向擾動後系統質心的位置和速度狀態去計算系統軌道能量，由此導出系統能量和地面壓力中心的關係，並賦予此壓力點為腳底的瞬時擷取點，當瞬時擷取點都維持在壓力中心臨界值所包圍的腳底支撐多邊形內部時則應用倒單擺系統所推展出來的踝關節穩定策略來維持機器人的恢復穩定；當超過壓力中心可能承受的臨界後，則切換到線性飛輪倒單擺所推展出來的髖關節穩定策略來導回機器人的平衡。踝關節和髖關節的制動扭矩由虛擬模型控制導出。針對俯仰軸面而言，上軀幹的質心僅受水平向、垂直向和俯仰軸的旋轉三個虛擬作用力，依虛擬腳指點合力矩為零的概念去推算水平向虛擬作用力，加上垂直向補償重力和關節策略下髖關節對應俯仰軸的旋轉虛擬力矩，三者即可依正向運動學概念導出各關節的相應扭矩。本論文應用 Solidworks 對機器人平台的機構做規劃與設計，以此設計好的組合件賦予材質等物理特性後，匯入 matlab 之 simulink 工具庫中所提供的 SimMechanics 模組，快速建立符合設計需求的虛擬實體，以此進行運動模擬可加快整個機電系統開發設計的速度與模擬可視化的直覺性。. 關鍵字：踝關節穩定策略、髖關節穩定策略、壓力中心、虛擬腳指點、虛擬模型控制、SimMechanics。. i.

(3) Abstract The thesis aims the development of the human-like robot for the lab, and hopes to set a new milestone of the research on humanoid robot. After developing the bipedal robot, the first step is to make robot stand up successfully and be able to fight against the disturbances to preventing falls. To this end, the balance control for fall prevention of the bipedal robot upright posture is the prime consideration of motion control. With the concept of the orbital energy, we figure out the system state of the position xc and the velocity x c to calculate the orbital energy and define it as the Instantaneous Capture Point. When the Instantaneous Capture Point stays in the support polygon under the foot, the ankle strategy derived from the linear inverted pendulum model is applied to maintain the balance of the robot. If the Instantaneous Capture Point is over the boundary of the center of pressure, it switches to the hip strategy derived from the linear inverted pendulum model plus fly wheel to get push recovery. The values of the ankle joint and hip joint are derived under the approach of Virtual Model Control. To the pitch axis side, the torso is applied by the virtual forces caused. by. the. horizontal. virtual. force,. the. vertical. virtual. force. of. gravity-compensation, and the virtual torque. With the concept that the resultant torque to Virtual Toe Point is zero, it could be used to calculate the horizontal virtual force. Combine these three virtual forces, the related joint torques would be derived with the forward kinematic. I design the structure of the bipedal robot, define material properties to the structure by Solidworks, and transport it into SimMechanics, one of tool boxes plugged in Simulink of matlab. It shortens the time to build the virtual model for ii.

(4) simulation and the result will be more visualized.. Key words: ankle strategy, hip strategy, center of pressure, virtual toe point, virtual model control, SimMechanics.. iii.

(5) 誌謝修讀碩士的日子雖然常埋首實驗室，所謂社交的生活相對大學時期減少許多，但似乎時間一眨眼也很快就過了。研究室中的助理、學長、同學和學弟除了讓我拓展朋友圈外，也讓我學習了為人處事的不同面貌以及解決事情的方式。當然，除了自己的進取心，最重要的莫過於實驗室的大家長，感謝陳美勇老師的教導，不只要求專業知識上的學習，連面對生活的態度都給了寶貴的建議。在研究室之外，遙遠南台灣的父母，是我最感謝的支柱，他們背後的關心與鼓勵，讓我可以無後顧之憂地專注在我的研究上，雖然每年回去次數甚少，但只要一回去，總能讓我再次充滿信心地返北繼續面對挑戰。很老派的引用陳之藩的《謝天》一文，「因為需要感謝的人太多了，就感謝天吧」。的確，我大學擅長於機械設計，其並非研究室的專長，但最後也踏進了自動控制的領域而且栽進了從未接觸過的機器人研究。一開始的確遭受了莫大阻力，但一點一滴的學習與解決，終究在中型人型機器人上跨出了一小步。這冥冥之中也許是一種無形的力量牽引著吧，也因為如此選擇才有今天有幸遇到的人事物！中大型人型機器人的開發，台灣已慢了國外好多步，在這條路上還有許多挑戰，期待後續的學弟妹能繼續在人型機器人研究領域上更臻完美。. 李昀翰 104/1/27. iv.

(6) 目錄摘要 .............................................................................................................i Abstract .............................................................................................................ii 致謝 ..................................................................................................................iv 目錄. ...................................................................................................................v. 圖目錄 ..............................................................................................................ix 表目錄. ...........................................................................................................xiii ...................................................................................................01. 第一章緒論. 1.1 前言 ..............................................................................................01 1.2 研究動機與目的 ..........................................................................03 1.3 文獻回顧. ....................................................................................03. 1.4 本論文貢獻與架構第二章運動學. ....................................................................08. ...............................................................................................09. 2.1 座標轉換 .....................................................................................09 2.2 Denative-Hartenberg 運動學表示法 ...........................................11 2.3 機器人之運動學 .........................................................................13 2.3.1 機器人運動模型. ............................................................13. 2.3.2 正向運動學 ....................................................................14 2.3.3 逆向運動學 ....................................................................15 v.

(7) 2.3.4 Jacobian 矩陣 .................................................................16 2.3.4 VMC 控制扭矩計算用之正向運動學 ...........................17 第三章雙足機器人的立姿穩定與平衡 .........................................................18 3.1 零力矩點 ZMP 與姿態穩定. ........................................................18. 3.1.1 ZMP 定義 .........................................................................19 3.1.2 機器人動態模型與 ZMP 導出 .........................................19 3.2 零力矩點 ZMP、虛零力矩點 FZMP、壓力中心 CoP .....................21 3.3 簡化模型 .....................................................................................21 3.4 穩定策略 .....................................................................................24 3.4.1 瞬時擷取點(Instantaneous Capture Point, ICP) ................25 3.4.2 踝關節策略 .......................................................................26 3.4.3 髖關節策略 .......................................................................27 3.5 簡化模型模擬之穩定策略分析 ....................................................29 3.5.1 IPM 模擬之穩定策略分析 ..............................................30 3.5.2 IPFM 模擬之穩定策略分析 ............................................31 3.6 小結 ................................................................................................32 第四章穩定策略下之機器人控制. ...............................................................33. 4.1 虛擬模型控制(Virtual Model Control) .......................................33 vi.

(8) 4.2 平衡控制. ......................................................................................35. 4.2.1 ICP 的計算與調整 .........................................................36 4.2.2 上軀幹動作的執行選擇 4.2.3 重力補償. ...............................................36. .......................................................................37. 4.2.4 關節扭矩的計算 ….......................................................38 第五章踝髖關節策略之立姿擾動控制模擬. ...............................................41. 5.1 SimMechanics 視覺化動力模擬 .................................................41 5.1.1 SimMechanics 特性 ........................................................42 5.1.2 Solisworks 機構建立與參數取樣. ..................................42. 5.1.3 SimMechanics 座標系統配置 ........................................44 5.2 雙足機器人踝髖關節控制之站立姿擾動模擬............................51 5.3 小結. ............................................................................................58. 第六章機器人機構設計與設備選擇.............................................................59 6.1 設計概念 ......................................................................................59 6.2 已開發機器人之設計回顧 ............................................................59 6.3 本研究之足式機器人設計構想 ....................................................63 6.3.1 馬達挑選 .........................................................................65 6.3.2 其他相關硬體 .................................................................67 vii.

(9) 6.4 小結 ..............................................................................................67 第七章結果討論與未來與展望 .....................................................................68 7.1 模擬結果與比較 ..........................................................................68 7.2 未來展望 ......................................................................................68 參考文獻 .........................................................................................................70. viii.

(10) 圖目錄圖 1.1(a) iRbot 公司設計具機械臂之輪型載具 …............................................. 2 圖 1.1(b) Festo 公司開發之 SmartBird…..............................................................2 圖 1.1(c) ETHZ 設計的水下仿魚機器人…...........................................................2 圖 1.1(d) 法國 Aldebaran 機器人公司研發之 NAO 服務機器人…...................2 圖 1.1(e) CMU 的 Biorobotics 實驗室設計的蛇形機器人 Snakebot…...............2 圖 1.1(f) 蜜蜂機器人…................................................................................…….2 圖 1.1(g) Boston Dynamics 旗下的野貓機器人 WildCat….................................2 圖 1.1(h) 台北市中山區某家冰淇淋店的服務型機械手臂…............................2 圖 1.2(a) 日本 JAIST 的 passive leg robot….......................................................4 圖 1.2(b) WL-5…........................................................................................……...4 圖 1.2(c) WABOT-1…............................................................................................4 圖 1.3(a) 前期原型與 ASIMO…...........................................................................6 圖 1.3(b) HRP 系列….............................................................................................6 圖 1.4(a) WABIAN-2R….......................................................................................7 圖 1.4(b) KHR 系列之 HUBO-2….........................................................................7 圖 1.4(c) 以 JOHNNIE 為開發基礎的 LOLA…..................................................7 圖 1.4(d) ViGIR 的 THRO 系列機器人…..............................................................7 圖 1.4(e) SCHAFT 公司所開發之機器人…..........................................................7 圖 1.4(f) 台大機械系所開發的人型機器人 Nino…............................................7 圖 1.4(g) 成大電機系所設計的人型機器人 David II…......................................7 圖 2.1 D-H 座標參數與結構之關係....................................................................12 圖 2.2 Solidworks 建構之簡模與座標結構.........................................................13 圖 3.1. 當 ZMP 落在 A 處時，無欠驅動翻轉。當落在 B 處時，腳底無法在 B 處給予地面作用力來平衡，故產生翻轉自由度。...........................19 ix.

(11) 圖 3.2. 最基本的倒單擺模型 ............................................................................22 圖 3.3. 踝膝髖三關節足式機器人與簡化的質心等高倒單擺模型。................23 圖 3.4. 互為等效的飛輪倒單擺模型。................................................................23 圖 3.5. 具飛輪之線性倒單擺與其等效力圖。....................................................24 圖 3.6. 左圖為質心 m 高度維持在等高 zCoM 的 LIPM，踝關節處  a 為恢復平衡的致動扭矩；右圖 IPM 狀態如左圖，唯差別在於質心高度受連桿長固定影響，其運動路徑為以踝關節處為支點的圓弧。..........30 圖 3.7. 踝關節策略應用在 IPM 下的質心位置、速度狀態的軌跡。............31 圖 3.8. 理想的 LIPFM 與右圖的 IPFM，除了基本架構如 IPM 外，多了質心位於轉軸上的飛輪。........................................................................31 圖 3.9. 髖關節策略應用在非線性的 IPFM 下的系統質心狀態軌跡圖。......32 圖 4.1. 虛擬模型控制，在上軀幹質心處僅受三個虛擬作用力，分別為水平向作用力、垂直向重力補償作用力以及 pitch 軸旋轉作用力。34 圖 4.2. 虛擬腳指點，延腳底板變動。此點的加入應用將有效推出水平向虛擬作用力的量值。..........................................................................38 圖 4.3. 雙關節穩定策略之虛擬模型控制器架構。...........................................40 圖 5.1. 第一代與第二代 SimMechanics 工具庫。.............................................41 圖 5.2. 為大腿和足部的簡化機構與其物質特性。............................................43 圖 5.3. CAD-SimMechanics 轉換流程。..............................................................44 圖 5.4. CAD 匯轉 SimMechanics 的模塊組合。.................................................44 圖 5.5. 由(a)到(e)分別為 Torso、Thigh、shank、ankle 以及彈性底板 rubber 的質心位置以及物性參數。.......................................................45 圖 5.6. 示範例之單足機構長度與絕對重心位置。.........................................46 圖 5.7. 組合件開迴路拓樸結構。.......................................................................47 圖 5.8. 左至右分別為絕對、相對座標配置和本文前述定義之 D-H 座標 x.

(12) 配置。....................................................................................................... 47 圖 5.9. Torso 構件自體局部座標關係參數。...................................................48 圖 5.10. Torso 和 Thigh 間 Joint 1 的設定示意。.................................................49 圖 5.11. Ground 模塊與場効模塊的設定。..........................................................50 圖 5.12. 重新設定後的模塊組合模型，相對於原本的圖，此已成為浮動式座標系統。........................................................................................51 圖 5.13. 本文所用以模擬的雙足半身機器人結構。..........................................52 圖 5.14. 三角波形式之水平向干擾外力。..........................................................53 圖 5.15. 套淺藍色底區域為踝關節策略下的控制軌跡；淺黃色區域為髖關節策略下的控制軌跡。髖關節區域中的軌跡可見能一定程度的避免機器人傾倒，踝關節策略下的抗擾程度最大達 22.6 Ns；而髖節策略下的抗擾程度可增達 23.14 Ns。.....................................54 圖 5.16. 擾動 22.6 Ns(a)和擾動 23.14 Ns(b)下的實際 ICP、CoP、x 軸向的 CoM 位置以及經過 ICP 控制器計算出來的預設 CoP。................55 圖 5.17. (a)、(c)為擾動極值 22.6Ns 下踝關節策略控制的關節扭矩、角度位置；(b)、(d)為擾動極值 23.14Ns 下踝關節策略控制的關節扭矩、角度位置。........................................................................................56 圖 5.18. (a)為擾動極值 22.6Ns 下踝關節策略控制的機器人 x 軸向的 CoM 位置和 CoM 速度；(b)為擾動極值 23.14Ns 下踝關節策略控制的機器人 x 軸向的 CoM 位置和 CoM 速度。.............................................57 圖 5.19. 作用在上軀幹的虛擬扭矩，(a)和(b)非別為擾動極值 22.6Ns 和 23.14Ns 下踝關節及踝-髖關節控制策略下的情形。.........................57 圖 5.20. (a)為踝關節策略下的恢復過程。(b)為髖關節策略下的恢復過程。....58 圖 6.1. KHR-3 的機構外觀尺寸與軸位架構。 ...................................................60 圖 6.2.(a) MARI-3 的機構外觀尺寸。(b)軸位配置。(c) MARI-3 的結構設 xi.

(13) 計使其有很大的活動裕度。(d)其在膝關節的部分為了維持高轉速又具有足夠大的轉矩，使用雙馬達帶動單關節的配置。 ......................61 圖 6.3. LOLA 的軸位配置，在髖關節採斜方交直架構；運用導螺桿驅動以將重心向腰部內縮。 ........................................................................ 62 圖 6.4.(a)步行運動之跨步動態。(b)下肢軸位配置(箭頭方為馬達轉動軸向)。 ...................................................................................................... 63 圖 6.5.(a)人體骨骼架構下肢結構。(b)本文設計之雙足半身機器人結構尺寸。 ........................................................................................................ 64 圖 6.6.(a)Maxon 之 EC 無刷馬達。(b)Maxon 之陶磁型減速機。 ................ 65 圖 6.7.(a)為踝關節 pitch 軸位的皮帶輪傳動機構，在此用皮帶輪是為了提高小腿質心。皮帶長度在 solidworks 內也可直接確定，但長度並非製造商所恰能提供，故設計惰輪插槽，連續六角軸插口設計，可以小幅度調整間距。(b)踝關節 roll 軸的配置。(c)膝關節的配置，將傳動齒輪以緊湊的方式配置於空間內。(d)髖關節的配置，因有十字交直軸的機構，故將傳動齒輪配在外側。(e)為了將兩顆馬達直行並列的方式排放，又為了將大腿寬度縮小，故壓縮了傳動齒輪的位置，因此犧牲了轉動角度，裕度只能容許到 45∘左右即會發生機構干涉。 ..................................................................................................... 66 圖 6.8.(a)Maxon 之驅動器 EPO S2 50/5。(b)PXI 工業電腦 PXIe 1071。.........67. xii.

(14) 表目錄表 1.1. 各國機構全人型機器人規格比較。........................................................06 表 2.1. D-H 準則之參數。.................................................................................... 14 表 3.1. IPM 之模擬參數。................................................................................... 30 表 3.2. IPFM 模擬參數。..................................................................................... 32 表 5.1. 雙足半身式機器人結構參數及控制器參數。........................................53 表 6.1. 各關節需求規格。 ..................................................................................65 表 7.1. 不同模型套用踝關節與髖關節穩定策略下的抗干擾比較。................68. xiii.

(15) 第一章. 緒論. 1.1 前言中文所謂之機器人，robot 一詞最早是出現在捷克籍作家的科幻劇作[1] 中，原意為「奴隸」，意即依據下達的指令無違地執行操作。當然，最初的機器人的確有著人形般的外觀，而隨著時代變遷，科技的進步一日千里，機器人的發展也益加多元，各式型態的機器人相應而生，依環境而分可歸類為陸、海、空三大環境類型。我們說造物主是神奇的，在大自然的演化下，各物種都有著對環境極高適應性的型態，這其中的靈感與奧秘被設計在人類開發的機器人當中，想必會具有存在的優勢，這其中當然涉及了仿生學的重要課題。空中型機器人主要為旋翼飛行器、鳥型機器人以及昆蟲型機器人；海中機器人意即水棲型機器人，以仿魚型結構為主；陸地型機器人，可說當今機器人研究領域中種類最多的一環。陸地型機器人又可下分為足式、輪式和爬蟲式等機器人，足式機器人有人型二足、獸型四足、昆蟲類六足、蜘蛛型八足等；輪型式有圓盤型和球型等；爬蟲式則仿蛇類為主。當然，各式的機器人除了依環境與形體來分類外，若依目的性投入應用端，則衍生出工業型、軍事型、娛樂型、醫療型、服務型等層面，參考如圖 1.1。無疑的，機器人是 20 世紀的一項重大發明，至今此學門的進展與應用無論對自動控制工程、感測技術抑或是機械動力工程來說都是高成就的展現。目前市面上商品化的機器人產品在種類上並不多元，但其實世界各國將機器人開發視為國家實力的重要指標。過往以 IT 產業作為發展重點的 google 都在 2013 年以來逐一買下 Boston Dynamics、SCHAFT 等機器人製造、創業公司，由此可見未來機器人在科技領域上足見其地位。相信在不久的將來，科幻電影中機器人與人類共存習以為常的景象將不是幻想。 1.

(16) (a). (b). (c). (e). (d). (g). (f). (h). 圖 1.1. (a)搭載機械臂的輪型載具，越野能力佳，常用於探勘與採集。(b)Festo 公司開發的 SmartBird，有如真鳥飛行般的擬真動作。(c)ETHZ 設計的水下仿魚機器人。(d)法國 Aldebaran 機器人公司研發的居家伴侶和服務機器人 NAO。(e)Carnegie Mellon University 的 Biorobotics 實驗室設計的蛇形機器人 Snakebot。(f)蜜蜂機器人，僅銅板大小。(g)Boston Dynamics 旗下的野貓機器人 WildCat，自搭能源，每秒有 7.15 公尺的衝刺速度。(h)台北市中山區某家冰淇淋店的服務型機械手臂。. 2.

(17) 1.2 研究動機與目的依據任務需求來設計相對應的結構通常是機械設計的原則，可有效達到高效能低成本的結果。對於人類來說，雖然人體結構並非對每種任務都有優勢，但人類的進化過程中，其高靈活度的生物系統確實可達成多種動作與任務需求。誠如前面所述，我們希望機器人最終可融入人類的社會環境並幫助人類解決事情，那仿照人類特徵設計的機器人自然是一種直覺的想法，除了可直接適用於人類生活環境，例如人類平常使用的器具或是上下樓梯等，不須再特別打造機器人專用的活動環境外，其特別之處的，就是從心理學角度的觀點。擬人化的過程中，表現出的就是所謂的親切感，這是凸顯人性化的一個重要觀感，因為人類對熟悉的事物會產生同理心，因而我們不會輕易地視機器人為一個冰冷的機具，而是做為「同伴」的存在，所以具有人類特徵的機器人基於這些因素有設計上的意義。對於人類的日常動作，站立姿勢是一個基本且常態的表現，若以人型機器人來說，要將其在日常空間中與人類共處，無可避免地擾動會不預期出現，對於維持機器人的站立姿行為，穩定平衡必定是個重要的目標。而通過對人類站立姿動作的平衡穩定分析，例如依據不同外力的大小以及碰撞點的不同會使身體產生不同的失衡狀態，人類的平衡會依據失衡狀態來採取快速回復姿勢的策略，因此應用機器人的站立姿態穩定策略來調整主要作用的關節具有其重要的意義。. 1.3 文獻回顧人形機器人(humanoid robot)，從驅動觀點來說可分為主動型和被動型兩種。被動型人形機器人是從降低活動耗能的角度來考慮的，又可細分成完全被動和部分被動。完全被動是指機器人如圖 1.2.(a)完全依賴自身質量所產生 3.

(18) 的慣性，除此之外沒有其他能源輸入，能量消耗在於碰撞和構件間的阻尼[2]；部分被動則是對少數幾個關節有能量輸入，使機器人除了在斜坡，還能在平地上行走[3]。但可惜的是，我們可以發現這類被動式機器人只能表現單調規律的動作，當碰到障礙物時動作就會被迫中止且容易摔倒，尤其非平坦地面，對此類機器人來說可說是舉步維艱，故實際的應用面上還是存在相當大的不便性。主動型人形機器人在各個關節都裝配有制動元件，目前世界各地發展的人行機器人大多都以此類為主。談到人形機器人，其實際機體的問世最早在 1971 年由日本早稻田大學開發出的能自主行動之雙足機器人 WL-5，圖 1.2.(b)，兩年後在 WL-5 的基礎上組成了第一台具備機械手、人工視覺和聽覺裝置的人形機器人 WABOT-1[4]，如圖 1.2.(c)。. (a). (b). (c). 圖 1.2. (a)日本 JAIST 的 passive leg robot。(b)WL-5。(c) WABOT-1。. 此後，1980 年代日本本田技術工業(Honda)也開啟一連串的雙足機器人研究，人型機器人的研究在日本有了長足的影響。到了 90 年代，隨著電腦運算技術和微機電技術的成熟，人形機器人的研究有了加速的發展，在 2000 年本田的里程碑 ASIMO(Advanced Step in Innovative MObility)更是讓各界大為驚嘆，如圖 1.3.(a)。2011 年發表的新成果，總重 48 公斤，身高 130 公分， 4.

(19) 正常負重下步行速度每小時 2.7 公里，跑步達每小時 9 公里，全身共計 57 個自由度，單足 6 個自由度[5]，具備與人類互動的知能。截至目前，世界各國的研究機構或學術單位都有自己代表性的人形機器人。例如日本產業技術綜合研究所(National Institute Industrial Science and Technology, AIST)的 HRP(Humanoid Robotics Project)系列，如圖 1.3.(b)，最新的 HRP-4 總重 39 公斤，身高 151 公分，步行速度每小時 1.57 公里，全身共計 34 個自由度，單足 6 個自由度[6]。早稻田大學的 WABIAN(WAseda BIpedal humANoid)[7]，延續自 WABOT 的成果，最新的 WABIAN-2R，如圖 1.4.(a)，總重 64 公斤，身高 148 公分，全身共計 41 個自由度，單足 6 個主動關節加上 1 個腳趾關節被動自由度，可做如人類原地向後轉的動作。WABIAN 機器人的最大特徵就是動作更接近人類，尤其行走時膝關節不用特別彎曲，這是與 ASIMO 最大的差異之處。韓國科學技術院(Korea Advanced Institute of Science and Technology, KAIST)的 KHR(KAIST Humanoid Robot)系列，HUBO 2 plus(KHR-4 plus)[8]，如圖 1.4.(b)，總重 43 公斤，身高 130 公分，步行每小時 1.5 公里，全身共計 38 個自由度。德國慕尼黑工業大學應用力學研究所(Institute of Applied Mechanics of TUM)的 LOLA[9]，如圖 1.4.(c)，客製化制動器以滿足低重量高動力，且讓整個關節配置體積更為緊湊，並採用長距離傳動裝置使總質心位置得以提高並消除肢體末端慣性。總重 55 公斤，身高 180 公分，最大步行速度每小時達 5 公里，全身共計 25 個自由度，單足 7 個自由度，腳趾是為主動關節。. 5.

(20) 表 1.1. 各國機構全人型機器人規格比較特徵. 發展機構. 國別. 總重(kg). 高度(cm). 行速(km/h). ASIMO. Hoda. Japan. 48. 130. 6. 6. 2. 鋰離子電池 51.8V. HRP-4. AIST. Japan. 39. 151. 1.57. 6. 2. 鎳氫充電池 48V-5.4Ah. WABIAN-2R. Waseda Univ.. Japan. 64. 148. 6. 2. 鋰離子電池. HUBO-2 plus. KAIST. Korea. 43. 130. 1.5. 6. 2. 鋰聚合物電池 24V-20Ah. LOLA. TMU-AM. Germany. 55. 180. 5. 7. 2. 名稱. 雙足自由度. 動力源. 由美國國防高等研究計劃署(Defense Advanced Research Projects Agency, DARPA)在 2013 年 DRC (DARPA Robotics Challenge)上更可見美國國家航空暨太空總署(NASA)、美國麻省理工學院(MIT)、美國維吉尼亞大學(University of Virginia)、香港大學等機構建造的驚人成果。雖然台灣的起步較晚，也沒有歷史資料可循，大多是國內大學自行開發研究而成，但至今台灣大學、台灣科技大學、成功大學等幾位在機器人領域投注許多心力的教授也都帶領著各自的研究團隊打造出屬於自己的人型機器人，如圖 1.4.(f)-(g)。. (a). (b). 圖 1.3. (a)前期原型與 ASIMO。(b)HRP 系列，右為 4C 型。. 6.

(21) (a). (b). (d). (c). (e). (g). (f). 圖 1.4. (a)WABIAN-2R。(b)KHR 系列之 HUBO-2。(c)以 JOHNNIE 為開發基礎的 LOLA。(d) ViGIR 的 THRO 系列機器人。(e)在 DRC 競賽中拿下殊榮的機器人，由 SCHAFT 公司所開發。(f)台大機械系所開發的人型機器人 Nino。 (g)成大電機系所設計的人型機器人 David II。 7.

(22) 1.4 本論文貢獻與架構為了本研究單位在中型人型機器人上的研究邁出第一步，本論文針對最基本且重要的雙足機器人站立姿態於擾動下的恢復穩定控制能有所實現，以軌道能量的概念並輔以簡化模型之動態導出關節穩定策略來分析與模擬，並依據模擬條件做雙足機構設計與配置。整個架構主要針對 Solidworks 和 matlab 之 SinMechanics 的聯合應用，同時考慮了機械設計的特點並將物質特性和可視化的直觀性帶入控制特性見長的 matlab 模擬環境中。在兩種軟體環境間的轉換與細節設定，本論文將會做詳細說明。希望能以有效的整合方式達到機械設計與控制一體化的效率。論文第二章描述了本文設計的單足機器人運動結構，並將機器人基本知識做概要說明；第三章探討雙足機器人的狀態穩定關係以及將人類站立姿態下受擾動的關節穩定策略套用到機器人的簡化動態模型上；第四章針對第三章推展出來的關節穩定概念，考量實際機器人的型態並應用 VMC 去做全域的穩定控制和關節操作域間的變換；第五章複合 Solidworks 和 matlab 之 SimMechanics 去呈現機構設計下的結構與物質特性以及快速的視覺化動態建模，並以此作為站立姿態穩定控制的分析平台，當中包含模擬所得的結果探討；第六章描述了本文所設計之機器人機構，並針對硬體設備做選擇；最後總結展望於第七章。. 8.

(23) 第二章. 運動學. 運動學(kinematics)是 mechanics 的一個分支，用來描述一個系統的位置、速度和加速度，但其不涉及質量和力等物理量。本文討論的機器人是針對串聯多連桿系統而言，其構成為連桿和關節的串接，一端連接基座，另一端自由活動的開放結構。在機器人的控制裡，最重要的事情就是要標定出目標物彼此間的位置關係以及找出機器人與標定物間的相對位置，使機器人能根據座標資料做出運算、轉換座標並成功達成定義的任務。在這一連串控制流程中可以發現最重要的工作就是座標的設定和轉換。而所有的運動依據 Chasle 定理都可用平移和旋轉來表示，在機器人控制中也是運用這些觀念來對各連桿、關節和標定物做座標定義，最後達到統一的世界坐標系定位。. 2.1 座標轉換在空間系統的描述上可分為局部空間和全域空間，通常全域空間都以世界座標定義之，而機器人各部件則各自定義局部空間。在這些連桿和物件上配置的描述系統為一個包含位置和姿態的框架結構(frame)。在不同座標系統間目標的描述需要座標變換，數學表示式為 A p A R B B p A p B ,ORG ，線性代數群乘積空間記為 SE3  p, R  : p  3 , R  SO3 3  SO3 ，也就是說當座標系 B 之原點相對於坐標系 A 原點的位移 A p B ,ORG 和坐標系 B 相對於坐標系 A 之姿態 A R B 定義後，系統的位姿即可由  A PB ,ORG , A R B 確定。. 9.

(24) 三維空間中的轉動特性 A. . A. XB. R B  SO3 為旋轉矩陣(rotation matrix)，表示系統的相對姿態 A. YB. A.  X B  X A YB X A Z B  X A  Z B    X B  YA YB  YA Z B  YA   X B  Z A YB  Z A Z B  Z A . (2-1). 為動坐標 B 系統相對於固定的慣性坐標 A 系統的形態變換，對固定坐標系的任一個軸做一次的旋轉即產生一個 3  3 的變換矩陣，全局符合右手定則規範並具正交特性，因此具有性質 R T R  I ，以此若同時右乘 R 1 ，即可知. R T  R 1 ，理所當然在滿足正交特性下其 det R  1 。除此之外，旋轉矩陣可以藉由矩陣乘法運算合成為一個新的旋轉矩陣，意即定義了 A 坐標系到 B 坐標系的旋轉矩陣 A R B ，也定義了 B 坐標系到 C 坐標系的旋轉矩陣 B R C ，則 A 坐標系到 C 坐標系的形態轉換可由下式確定 A. RC  A R B B RC. (2-2). 尤拉角旋轉矩陣中描述三維空間中的變換狀態有九個參數，但其實只靠三個獨立變數決定，即繞三個方向座標軸的轉動角度。但在描述這樣的坐標系轉換時非僅單一操作方式，常見有兩種，一種是所謂的 RPY 轉角，這是一種繞舊坐標系固定軸得到新作標系的方法，繞 x 軸旋轉的旋轉向量軸稱 Roll 軸；繞 y 軸旋轉的旋轉向量軸稱 pitch 軸；繞 z 軸旋轉的旋轉向量軸稱 yaw 軸。另一種是下節 D-H 座標採用的對新的座標系之軸旋轉的方式，即所謂的尤拉轉角，無論要轉哪一個軸，都是針對新的座標框架之軸。數學運算中很清楚的，相對於世界坐標系，對於每一次的轉動，轉移矩陣都是往左乘，但在尤拉轉動中每一次的轉動都是針對新框架之軸，其結果也就是世界座標系相對於新座標系的轉動狀態。但在處理運動關係中的狀態 10.

(25) 條件時，任何一切的相對都有一個不變的基準，那基準通常就是世界座標系，所以在看尤拉轉動法的時候，矩陣運算都是右乘，意即把相對於新座標系的左乘做逆運算，結果即為相對於世界座標系下的尤拉轉換。. 齊次坐標系與齊次轉換前述提及目標位姿在不同座標系統間的轉換關係 A p A R B B p A p B ,ORG ，此為一般性的複合定義。若線性化矩陣處理的簡潔表示法，引入符號構成四維列向量成為 A p A TB B p ，.  A p  A R B    1   000 A. A. p B ,ORG   B p   1  1 . (2-3). TB 稱之為齊次轉換矩陣(homogeneous transformation matrix)，此即為齊次座. 標變換表示法。齊次轉換矩陣的運算同旋轉矩陣滿足連乘運算.  A p A B  C p  A R B B R C    TB TC  1       0 0 0 1. A. p B ,ORG   A R B B p C ,ORG  A p B ,ORG    1  1 . (2-4). 2.2 Denative-Hartenberg 運動學表示法依附參考座標到不同的連桿上可以方便地定義出機器人結構的幾何樣貌，Denative 和 Hartenberg 兩位學者提出了一種表示準則[8]，只要四個參數就可定義前後連桿間的空間轉移關係，其參數分別為連桿長度 a、連桿扭轉 α、連桿分歧 d、關節轉角 θ，如圖 2.1。D-H 準則所得到的轉移矩陣操作方法為： z 軸皆為關節旋轉軸；a 為沿前後 z 軸的公法線走向，也是 x 軸的走向。 1. 選定編號 i-1 和 i 的連桿並定義各自 z 軸指向，接著使編號 i-1 連桿座標框架原點沿其 z 軸向移動 d 距離到編號 i 連桿並與之座標框架原點重和。 11.

(26) 2. 繞著編號 i-1 連桿框架之 z 軸旋轉 θ 角度使編號 i-1 連桿框架的 x 軸向對正編號 i 連桿框架的 x 軸向。 3. 編號 i-1 連桿的新框架原點沿著編號 i 連桿框架的 x 軸移動 a 距離使得編號 i-1 連桿的新框架原點重和編號 i 連桿框架原點。 4. 繞編號 i 連桿框架之 x 軸旋轉 α 角度使編號 i-1 連桿的新框架 z 軸對正編號 i 連桿框架的 z 軸。程序上即為旋轉(R)、平移(T)、平移(T)、旋轉(R)的結果 i 1. A i  Rot  zî 1 ,  i Trans  zî 1 , d i Trans  xî , ai Rot  xî ,  i  c i  s i 0 0 1 0 0 0  1 0 0 ai  c i  s i c i s i s i ai c i  s c 0 0 0 1 0 0  0 1 0 0   s c c  c s a s  i i i i i i i i    i   0 0 1 0  0 0 1 d i  0 0 1 0   0 s i c i di       0 0 1  0 0 0 1  0 0 0 1   0 0 0 1  0 cos i  cos i sin  i sin  i sin  i ai cos i   sin  cos cos  sin  cos a sin   i i i i i i i   (2-5)  0 sin  i cos i di    0 0 1   0. a. d. 圖 2.1. D-H 座標參數與結構之關係。 12.

(27) 其中在 D-H 座標建立上要注意的兩個規則 1. 軸 xi 必須和軸 zi 1 垂直。 2. 軸 xi 必須和軸 zi 1 相交。. 2.3 機器人之運動學機器人的運動學描述了機體各部件間通過關節的連接所表現的運動關係。當中的關節，對於主動型人形機器人來說，一個關節基本上安置有一個制動器，而致動關節表現的自由度各有不同，就使用性來說，目前常見為採用的就是一個關節配置一個自由度為 1 的旋轉馬達形成轉動關節。將這些轉動關節按序列串接模擬人類關節活動的配置，構成一個多自由度的開放式串聯機器人，並依據前述提出的理論來定義運動狀態。. 2.3.1 機器人運動模型. Joint1. L1. Joint2. L2. Joint3. L3. 圖 2.2. 左為 Solidworks 建構之簡模，右為座標結構。. 13.

(28) 2.3.2 正向運動學正向運動學(forward kinematics)，在知道關節資訊下反應出末端執行器 (end-effector)的運動情形，例如求得每根連桿當下位置與姿態、機器人質心位置或質心速度等計算。其正向運動學的計算在於利用前述的齊次轉移矩陣，通過鏈鎖法則(chain rule)，給定各個關節角度資訊即可得知末端操作器的位置姿態。本文中機器人關節配置下的 D-H 參數如下表 2.1 表 2.1. D-H 準則之參數 .. Para.. θ(deg). Frame. 0∘. 0. d(m). a(m). 0. 0. 90∘. 1.  90   1. 0. 0.2. 0∘. 2. 0   2. 0. 0.2. 0∘. 3. 0   3. 0. 0.083. 0∘. 對應之轉換矩陣如下：. 1 0 o A0   0  0. α(deg). 0 0 1 0. 0 1 0 0. 0 0 0  1.  sin1 cos1  cos sin 1 1 0 A1    0 0  0  0. 0 L1 sin 1  0  L1 cos1   1 0  0 1 . cos 2  sin 2  sin  cos 2 2 1 A2    0 0  0  0. 0 L2 cos 2  0 L2 sin  2  1 0   0 1  14.

(29) cos 3  sin 3  sin  cos 3 3 2 A3    0 0  0  0 0 0 3 Af   0  0 o. 0 0 1 0. 1 0 0 0. 0 L3 cos 3  0 L2 sin  3  1 0   0 1 . 0 0 0  1. Tf o A1 1 A 2 2 A3 3 A f. (2-6). 2.3.3 逆向運動學在運動學的轉換中，由 D-H 座標直接得到的是正向運動學，給定每個關節角度則可知末端操作器的位姿；而知道終端位姿要反求關節該轉幾度，此稱之為逆向運動學(inverse kinematic)，但其不若正向運動學定義每個關節轉角後就可唯一確定全機體位姿。在三維空間中有六個自由度，xyz 軸平移和 roll、pitch、yaw 軸旋轉，若機器人關節自由度組成超過六個；或是關節總數非冗於，但每個關節各自在帶有單一數值的情形下還需考慮順逆時鐘轉向的問題，這就造成一對多的求解問題，在機器人學裡還沒有一套完整有效的標準手法，所以推導逆向運動學相對來說是比較麻煩的工作。一般來說分為封閉解(closed-form solution)和數值解(numerical solution)，若機器人結構滿足 Pieper 準則[10]可適用封閉解，包含代數解法和幾何解法等解析型式；數值解法可通過所謂的 Newton-Raphson 迭代運算來得到。當然，對於人形機器人的研究已歷經了各方一系列的努力，其軸位配置大抵有一些習慣性的組合方法，因此也有關於特定軸位組合架構的人型機器人之逆向運動學的推導程序[11]。. 15.

(30) 2.3.4 Jacobian 矩陣在說明 Jacobian 矩陣之前，先說明機器人是由 n 個關節構成，通常一個關節配一個自由度，所以機器人的運動由這 n 個關節向量組合而成，這些關節向量的集合稱為關節空間(joint space)；而我們在生活環境用以描述工作狀態的空間稱為操作空間(operation space)，通常使用直角坐標系，所謂的笛卡爾空間(Cartesian space)。而我們總是控制制動端來達到操作端的條件，這就是關節空間狀態映射到操作空間狀態的轉換關係，也就是前述的正逆向的變換觀念。所以除了位移變量間的關係表示，jacobian 矩陣也可表示速度和力在關節空間與操作空間的映射關係。以速度分析來說，當關節空間向量 q  n ，操作空間位置向量 x   p ，存在 x  xq  ，對時間做一次導數得 x  Jq q ，其 J ij q  . xi q  的一階偏導數，是為 Jacobian 矩陣內的構成元素。 q j. 在此回想前述的逆向運動學求解問題，基於數值解的方式，由 Jacobian 的轉換關係，可以在定義末端操作器的速度已知情形下對 Jacobian 求逆 q  J 1 q x. (2-7). 接著對 q 積分即可得角度 q。當然，這樣的逆運算不是都一定存在有解，若是碰到有兩個關節驅動軸(D-H 設定之 z)軸位對正一直線時，即為奇異現象 (singularity)，逆矩陣無法計算正確解。但本篇的運動結構限制在一平面上運動，故不用考慮奇異狀況。. 16.

(31) 2.3.5 VMC 控制扭矩計算用之正向運動學 2.3.2 節所敘述之正向運動學是一般普遍使用之架構，也便於應用在數值模擬上的展現。本節所推導的正向運動學因導入腳底板下穩定點的概念，在座標結構上的配置使用 D-H 參數法來推導有不便之處，因軸位配置無法符合規則，其詳細推導將在後續提出。. 17.

(32) 第三章雙足機器人的立姿穩定與平衡機器人研究領域中為了便於分析運動狀態與設計相應的控制法則，一般都會將運動結構複雜的機器人簡化成特定模型。通過簡化模型分析人形機器人在站立姿下受外界擾動時，由自體運動狀態來判斷將採取何種關節運動策略來穩定姿勢。其運動狀態的判斷涉及腳底支撐點的座落位置能否支持對地面施力的有效性。. 3.1 零力矩點 ZMP 與姿態穩定零力矩點 ZMP(zero moment point)，字面上意思即合力矩為零的作用點。人類的步態運動之所以不會摔倒，是因為遵循物理條件下重心對地面的投影點一般來說都落在腳底所構成的支撐多邊形範圍內。將此概念套用到人型機器人上，M. Vukobratovic’和 J. Stepanenko 兩位學者在 1972 年首度提出 ZMP 觀點的研究[12]。而這個概念的出發點是由於早在人型機器人之前，具有固定基座類型的機械手臂已經研究一段時間了，所以把人型機器人的支撐腳底看成固定端，CoM 軌跡的計算就如手臂的末端操作器可簡化成關節路徑追跡的模式來達成機器人步態運行。但其實這樣的 ZMP 穩定性判據並不是絕對的充要條件，因為人類運動常常是處在動態平衡的狀態，ZMP 點超出支撐多邊形範圍還是可以藉由慣性力來維持平衡；抑或是前述提到的點足式機器人也是可以在不具支撐多邊形概念下實現穩定運動。而各國研究人型機器人的團隊利用 ZMP 作為運動穩定判據也占了一大門派，日本方面多屬此類。那既然 ZMP 穩定判據不是絕對的，那何以獲得廣泛應用？原因還是在於關節路徑追跡，當計算出的關節軌. 18.

(33) 跡可以使足底 ZMP 落在腳底支撐多邊形範圍內，即表示該組運動軌跡不會在腳底發生欠驅動的邊緣翻轉自由度，如圖 3.1。. 圖 3.1. 當 ZMP 落在 A 處時，無欠驅動翻轉。當落在 B 處時，腳底無法在 B 處給予地面作用力來平衡，故產生翻轉自由度。.. 3.1.1 ZMP 定義根據 Vukobratovic 和 Borovac 的論述，ZMP 定義為地面上的一個點，對於此點，慣性力與重力造成的合力矩將沒有沿地面水平軸向的分量。. 令 mi 為機器人各部件質量， c i 為各部件 CoM 於大地座標系之位置，ωi 為各部件繞 CoM 之角速度，τ 為卡氏座標三軸向力矩，則.  m c k. i. i.  i  ωi  I iωi   τ  ZMP   ci  g   I i ω. (3-1). i 1. x  y  0. (3-2). 3.1.2 機器人動態模型與 ZMP 導出除了確切的 D-H 運動模型，要描述機器人和環境間交互作用的動力模型也有其定義。估測 ZMP 的動力模型有將機器人視為實際的多連桿剛體系統如式 3-1，也有視為一個多質點系統或是單質點系統的倒單擺簡化模型。剛體系統和質點系統上的差別在於慣性矩陣的有無，令總質量為 m，CoM 位. 19.

(34) 置 c，大地水平面反作用力 f，繞 ZMP 點之 Z 軸向力矩 τ ZMP ，對原點的力矩關係式. τ o  ZMP  f  τ ZMP. (3-3). 根據動量 P、角動量 H 對應作用力與力矩的關係   mg  f P. (3-4).   c  mg  τ H o 0. (3-5). 將式 3-4 帶入式 3-3，再將結果代入式 3-5.  o  c  mg  P  mg  ZMP τ ZMP  H  ZMP , x   H o , x  c x   0    Px   0    ZMPx         H  c   m  0    Py  m  0     ZMPy   ZMP , y   o , y   y             c    g    Pz   g    ZMPz   ZMP , z   H o , z   z   ZMP , x   H o , x  ZMPz Py  ZMPy Pz  mg   mc y g      H  ZMP P  ZMP P  mg   mc g  ZMP , y z x x z x     o, y   ZMP , z   H o , z  ZMPz Py  ZMPz Px. (3-6). 根據 ZMP 的定義.  ZMP , x   H o , x  ZMPz Py  ZMPy Pz  mg   mc y g  0  ZMP , y    H  ZMP P  ZMP P  mg   mc g   0      o, y z x x z x . (3-7). 可導出平面上 ZMP 位置.  mgc x  ZMPz Px  H o , y ZMP  x  mg  Pz     ZMP  mgc y  ZMPz Py  H o , x y  mg  Pz . (3-8). 因為 ZMP 是落在地面上的點，所以 ZMPz=0，而 Pz  mcz ，套入式 3-8 並整理. 20.

(35)  mgc x  H o , y ZMPx  mcz  g     ZMP  mgc y  H o , x y  mcz  g  . (3-9). 式 3-9 為機器人之動態與 ZMP 的關係，此常應用在一般的分析中。. 3.2 零力矩點 ZMP、虛零力矩點 FZMP、壓力中心 CoP 前面小節提到，於地面上的一點，機器人對此點存在力矩作用，力矩來自於重力、慣性力的總和，其效果為零，且當此點坐落在腳底支撐多邊形內時此點稱為 ZMP。但當此點超出腳底支撐多邊形範圍時，此時該點不再稱之為 ZMP，進而轉變為所謂的虛零力矩點(Fictitious ZMP)[13]，在文獻[14] 則指出另一種別稱-足翻指示(Foot Rotation Indicator，FRI)。 CoP(Center of Pressure)也是地面上的一點，為地面反作用力的作用點。當機器人狀態為動態穩定時，意即 ZMP 落在腳底支撐多邊形內，此時 CoP 與 ZMP 是重合的一點；反之 CoP 和 FZMP 不會重合，CoP 會落在支撐多邊形的邊緣。一般在實作中 ZMP 可由運動狀態解析定義之；CoP 則會在腳底部安裝壓力感測器來計算。. 3.3 簡化模型機器人是一個複雜的多體之類剛體系統，其多個關節下高自由度間形成的強耦合性使得我們在描述其系統時面臨許多高階非線性方程式，因此對這樣的機器人做運動上的控制時會遭受到一些困難從而使許多基礎性問題無法輕易解決。若導出的方式採用拉格朗日公式(Lagrange formulation)的這一類動態模型通常可以較精準地描述整個系統，做為控制上的操作便利性，例. 21.

(36) 如[15]中使用的方式，對每一根構件做等效力分析以及能量計算進而構築全體的運動模型。在人形機器人領域中常見的簡化模型有單自由度的倒單擺模型(Inverted Pendulum Model)和為了更接近實際情形而作擴展和推廣的飛輪模型(Flywh eel Model)或稱角動量單擺模型(Angular Momentum Pendulum Model)、體操機器人模型(Acrobot Model)等。. 倒單擺模型(Inverted Pendulum Model, IPM) IPM 由一個總體質量集中的質點 m 和一根長度 l 且無質量的連桿構成，支點制動關節處猶如人型機器人的踝關節部位，示意如圖 3.2。. . 圖 3.2. 最基本的倒單擺模型其單擺的運動方程式為 ml 2   mgl sin   . (3-10). 線性倒單擺模型(Linear Inverted Pendulum Model, LIPM) LIPM 與 IPM 的差別在於線性倒單擺的連桿長度 l 是可變的，其 CoM 永遠維持在固定高度的水平面上運動，其連桿猶如腿部可提供伸縮力 f，如圖 3.3，此簡化模型常被用來表示多關節自由度的人型機器人動態。. 22.

(37) 圖 3.3. 踝膝髖三關節足式機器人與簡化的質心等高倒單擺模型。. 其腿部伸縮力提供了補償重力的效果而可將重心維持在固定高度上，其旋轉與線性的運動方程式為 ml 2  mgl sin   2mll. (3-11). ml   mgl cos  ml  2  f. (3-12). 飛輪模型(Flywheel Model, FM) 前述的簡化模型是視為一個集中的質量點，其中的旋轉慣性之物理特徵已不存在，如此就無法施予質點一個力矩的作用。飛輪模型將總質量集中在旋轉中心上並保留等效的轉動慣量，為維持對地面支點總力矩為零，外力合的作用線通過 CoM 和基底支點。其運動方程式與示意圖圖 3.4 如下 ml 2  mgl sin   . (3-13). J  . (3-14). 圖 3.4. 互為等效的飛輪倒單擺模型。. 23.

(38) 由方程式和上圖可看出飛輪的旋轉可以用來控制倒單擺整體的旋轉動態。當然，如此的模型可組合出另一種型態之模型。. 具飛輪之線性倒單擺 (Linear Inverted Pendulum Plus Flywheel Model, LIPFM) LIPFM 具備線性倒單擺易於線性化的特性外，也具備了飛輪具有的旋轉慣性可描述上身或人形機器人手臂的揮動效果。其運動方程式如下 ml 2  mgl sin   2ml l  . (3-15). ml   mgl cos  ml  2  f. (3-16). J  . (3-17). 如同前述模型對地面支點總力矩為零的限制，圖 3.5 中 Ft 、 Fr 與 f 的等效作用力滿足條件 Ft . f l. 、 Fr  f 、  f  . 圖 3.5. 具飛輪之線性倒單擺與其等效力圖。. 3.4 穩定策略面對外力干擾作用下，人類依據外力大小會動用踝關節或髖關節，甚至是跨步來穩定身軀[16-19]。本節將探討踝關節和髖關節運作策略的穩定方法。 24.

(39) 3.4.1 瞬時擷取點(Instantaneous Capture Point, ICP) 文獻[20]中 Shuuji Kajita 等學者不採用簡化模型導出機器人的運動方程式，並利用軌道能量的概念做步態規劃。Goswami 等學者也依軌道能量概念對簡化之單擺模型分析了能維持穩定平衡的踏點區域[21]。本文將對線性倒單擺系統，其定義在 Cartesian 坐標系下的運動方程式作維持穩定的探討。 mxCoM zCoM  mgxCoM  . (3-18a). g     xCoM   z CoM  mg . (3-18b). xCoM .    xCoM  02  xCoM  ，with 0  mg  . g. (3-18c). zCoM. 此線性倒單擺系統之 CoM 在垂直軸 z 方向上只受到重力作用，故地面反作用力為系統總重量 mg。對一個有足底機構的人形機器人而言，考慮足底平板機構，其承受了踝關節馬達運轉的反作用力以及地面反作用力，當足底處在動態穩定不翻轉時，足底 CoP 為 xCoP  . mg. ，因此式 3-18c 改寫為. xCoM  02  xCoM  xCoP . (3-19). 因 CoM 只沿 x 軸向(平行地面)運動，假設在站立姿平衡原點有個無質量虛擬彈簧連接著 CoM，總合成一個彈簧質量系統，其系統動能 TLIPM . 1 2 mxCoM 2. (3-20). 當 CoM 受到擾動，虛擬彈簧對 CoM 施力 FCoM ，則系統勢能 VLIPM    FCoM dx    mxcoM dx    m02 xCoM  xCoP dx  . 依系統總能量 Hamiltonian 的概念 E LIPM  TLIPM  VLIPM . mg xCoM  xCoP 2 2 zCoM. (3-21). 1 2 g xCoM  xCoP 2 xCoM  2 2 zCoM. (3-22). 這個操作子代表此線性倒單擺系統的軌道能量，它決定了系統的行為。 25.

(40) 當 ELIPM  0 時 1 2 g xCoM  xCoP 2  0 xCoM  2 2 zCoM. (3-23). 將式 3-23 中的 xCoP 解出，並賦予定義此位置為瞬時擷取點 ICP，則此解表示質心狀態能量可被削弱至最低抗力位置。 x ICP  xCoM . z CoM x x CoM  xCoM  CoM g 0. (3-24). 3.4.2 踝關節策略 Horak 和 Nasher 在文獻[22]中說明了人類在受到相對較小的干擾時動用踝關節來穩定身軀，此方式也在許多人形機器人控制中被採用，在此應用下其他關節是被鎖定的。因為只有基底的踝關節可制動，就如同倒單擺一般，因此以 LIPM 來分析其對恢復穩定的作用條件。依式 3-18(b)中笛卡爾坐標系下的運動方程式 xCoM  02 xCoM . a mzCoM. ，with 0 . g. (3-25). zCoM.  a 為踝關節力矩。令初始條件已知 xCoM 0  xc0 以及 xCo M 0  xc0 分別代表 CoM 的初始位置及速度，則二階線性微分方程式 3-25 之解為 1 x   1 x    xCoM t    xc 0  c 0  a e t   xc 0  c 0  a e  t  a 2 0 mg  2 0 mg  mg 0. 0. (3-26). 此系統具兩個特徵值 0 和  0 ，而特徵值 0 會使系統不穩定。欲消除此造成不穩定的因子，則令係數之括號內數值為零的條件可避開不穩定狀況. xc 0 . xc 0. 0.  xc 0 .  xc 0. 0. a mg . 0 (3-27). a mg 26.

(41) 由此可得系統運動狀態與踝關節扭矩間的平衡關係。又由前述提及的. a 為 mg. CoP，在機器人穩定不翻倒的情形下 CoP 與零力矩點等價。所以只要式 3-27 的值維持在腳底板支撐範圍內. l h  xc 0 . x c 0. 0.  lt. (3-28). lt 與 l h 分別代表前腳趾以及後腳跟距離踝關節的位置，此可確保機器人滿足. ZMP 穩定條件不翻倒且踝關節扭矩輸出足以用來保持機器人在干擾下回復姿態穩定。. 3.4.3 髖關節策略遭受較大的干擾時，出力有限的踝關節運動無法滿足平衡所需的恢復力矩，進而會動用髖部關節，在上軀幹繞轉時的慣性力會將 CoM 位置往反方向帶來達成平衡的效果。而髖部瞬間的加速繞轉對足部帶來的慣性容易使機體處在不穩定的狀態，因此一般不會單獨使用髖關節來平衡身體，所以會合併踝關節運動來補償足部慣性。為了簡化分析雙關節制動的平衡特性，在此以 LIPFM 作為分析模型。其運動方程式如下. mxCoM zCoM  mgxCoM   a   f. (3-29).  f  I fyf. (3-30).  f 為直接施加在飛輪上的關節扭矩、 I fy 為飛輪在 pitch 軸(y 軸)向的轉動慣量、f 為飛輪對其軸心的轉動加速度。飛輪的轉動加速影響著 CoM 加速度，因此當飛輪施以最大加速度並在特定時間轉向減速達停止時，飛輪轉動角度達最大，其相應的 ICP 位移距離也達最長，這樣的開關控制(bang-bang control). 27.

(42) 在文獻[23-24]皆有應用。開關控制屬於時間最佳控制的一類，其控制函數僅在控制邊界上切換。令飛輪控制力函數.  f t   I fyf   f _ max ut   2 f _ max ut  T1    f _ max ut  T2 . f .  f _ max I fy. u t  . 2 f _ max  f _ max u t  T1   u t  T2  I fy I fy. (3-31). (3-32). 在此要注意的是，飛輪制動的能量耗損比起踝關節制動還大且穩定性較差些，所以飛輪制動的目的在於將機器人 ICP 拉回踝關節策略下的應用範圍內即可，之後即切換回踝關節策略。因此就式 3-32 飛輪運動狀態，給定飛輪轉動角度在開關控制的停止階段時間 T2 達到最大且運動狀態下的 ICP 回歸腳底板支撐範圍內. lh  xCoM T2  . xCoM T2 . 0.  lt. (3-33). 即可應用踝關節策略將系統導回穩定狀態，並同時飛輪也可在過程中回歸初始零度位置。假設飛輪初始運動狀態  f 0   0 和  f 0  0 ，則依據上述推論，式 3-32 的一次積分滿足  f T2   0 的條件.  f T2  .  f _ max I fy. T. 2.  2T2  T1   0. (3-34). 滿足上式條件的解可得 T2  2T1  2T ，意即加速度和減速度相同時間長度。為此依需求設計時間 T，再積分一次，滿足在時間 T2 飛輪最終角位移不會超過轉角極限.  f T2    f 2T  .  f _ max I fy. T 2   f _ max. (3-35). 因此得出時間間距 T 之極值 28.

(43) Tmax . I fy f _ max. (3-36).  f _ max. 分析完飛輪關節的運動條件，接著分析與之配合的系統整體運動狀態。將式 3-29 之 LIPFM 系統動態移項整理，分別對應飛輪制動時間間距 2T 做兩次積分得. lh . 2  f _ max  eT  1  max.  mg . e 2T. max. xc 0. 2  f _ max  eT  1 .  lt    xc 0   0 mg  . max. e 2T. max.  . (3-37). 此即為髖關節策略穩定判據。. 3.5 簡化模型模擬之穩定策略分析 3.4.2 和 3.4.3 兩節分別導出了基於 LIPM 和 LIPFM 動態下的關節穩定恢復條件。在理論上為了便於推究穩定條件，所用的單擺模型都經過線性簡化，即 CoM 皆維持在同一水平高度。但在真實的情形下，單擺通常其桿長長度是不可變動的，即支點不滑移的情形下質心是對支點做圓弧擺動，CoM 在高度上自然不可能維持在固定水平位置。為了驗證線性化模型下推導出來的穩定理論在實際使用上是有效果的，如圖 3.6，在此將線性模型下推導的穩定理論應用在非線性化模型上以此檢驗理論的實用性。. 29.

(44) 3.5.1 IPM 模擬之穩定策略分析. 圖 3.6. 左圖為質心 m 高度維持在等高 zCoM 的 LIPM，踝關節處 a 為恢復平衡的致動扭矩；右圖 IPM 狀態如左圖，唯差別在於質心高度受連桿長固定影響，其運動路徑為以踝關節處為支點的圓弧。. x 軸向在單擺質心處施以一個三角波作為擾動外力的模擬。IPM 之物理模擬參數如表 3.1。其模擬結果的 xCoM 和 xCoM 的數值路徑如圖 3.7。0.03629. 表 3.1. IPM 之模擬參數模型參數單擺稈長質點質量 ICP 控制比例增益預設 ICP 位置足前翻上界足後翻下界. 標號 l m K pICP. 30. 數值 0.7 7.305 2.75 0 0.07645 -0.075. [m] [kg] [m] [m] [m].

(45) 圖 3.7. 踝關節策略應用在 IPM 下的質心位置、速度狀態的軌跡。. 3.5.2 IPFM 模擬之穩定策略分析. 圖 3.8. 理想的 LIPFM 與右圖的 IPFM，除了基本架構如 IPM 外，多了質心位於轉軸上的飛輪。 x 軸向在單擺質心處施以一個三角波作為擾動外力的模擬。IPM 之物理模擬參數如表 3.2。其模擬結果的 xCoM 和 xCoM 的數值路徑如圖 3.9。 31.

(46) 表 3.2. IPFM 模擬參數模型參數單擺稈長飛輪質量飛輪轉動慣量 ICP 控制比例增益預設 ICP 位置足前翻上界足後翻下界. 標號 l m Iwheel K pICP. 數值 0.7 [m] 40 [kg] 4.66279 [kg·m2] 2.75 0 [m] 0.07645 [m] -0.075 [m]. 圖 3.9. 髖關節策略應用在非線性的 IPFM 下的系統質心狀態軌跡圖。. 3.6 小結由理想的 LIPM 以及 LIPFM 導出的踝關節穩定策略以及髖關節穩定策略套用到非等高質心的實際固定長連桿之單擺系統，其模擬結果發現如此的穩定策略依然具有相當良好的抗干擾恢復效果。. 32.

(47) 第四章穩定策略下之機器人控制第三章使用了簡化模型分析了擾動下的穩定回復控制，比較中明確顯示出髖關節複合策略下抗擾動的能力較單獨踝關節策略佳。而髖關節策略的導出使用了飛輪模型，但實際的人形機器人並非如飛輪模型一般地總質心配置在髖關節上，也沒有飛輪裝置，故在此應用上需要一個比 bang-bang 控制更為適當的控制演算。. 4.1 虛擬模型控制(Virtual Model Control) 多自由度人形機器人是一個複雜的系統，全局的高階動態以低階的關節空間來表現，在推導上有一定複雜度。而智慧型控制器在設計上經常使用系統逆向動態方程導出關節控制律，若模型不精確，控制效果會打折扣；若使用 PID 一類的控制器，要表現全局任務，必須對各關節設計各個控制器，但以各個關節的調控來直接體現完整的全局任務作動也有相當的不便性，自由度越高越顯著，且要調整的增益參數數量也亦加繁瑣。虛擬模型控制[25-26]，是在機器人身上的制動端點安置假想的機械元件如彈簧、阻尼等，其虛擬機械元件產生的虛擬作用力，可以反導出預計的關節扭矩，形成相同的外部運動效用。因為套用了機械元件，所以制動效果可以直覺地被描述與設計，因此增益參數由機械元件的觀點去調整控制器作用將便利許多。原本不易制定的任務也因為虛擬機械元件的使用而具體化了任務描述，例如要求機械手臂以多少力量在定點施以多少力量敲擊目標，則在手臂末端添加虛擬質量並以虛擬彈簧阻尼元件和手臂基座端相接，經轉換矩陣即可清楚計算出各關節扭矩。虛擬模型控制在架構上不需使用系統逆向動態方程，意即不需要精確的動態模型，其只需正向運動學的計算，整體控制運作有效率許多。操作空間上的虛擬作用力經雅各比(Jacobian)轉換矩陣即得關節空間的操作扭矩 33.

(48) τ  A J TB W. (4-1). τ 為關節扭矩， A J B 為運動端坐標 B 對應到基座標 A 的雅各比矩陣，W 為作用在系統操作點位於 B 座標處的虛擬作用力。本文分析上只考慮矢向(sagittal) 切面的虛擬力，如圖 4.1，作用在軀幹質心的虛擬作用力有三個，分別為 x 軸橫向虛擬力 f x 、z 軸縱向虛擬力 f z 以及 pitch 軸虛擬扭矩  fy 。  fx  W   fy     fy . (4-2). 操作虛擬模型控制的流程可按次序分成三個階段，分別為定義虛擬構件的局部參考坐標系、正向運動學計算與相應之 Jacobian 矩陣、虛擬作用力函數與關節扭矩計算。. 圖 4.1. 虛擬模型控制，在上軀幹質心處僅受三個虛擬作用力，分別為水平向作用力、垂直向重力補償作用力以及 pitch 軸旋轉作用力。. 定義虛擬構件的局部參考坐標系為整個系統配置虛擬機械元件，一組需要三個局部參考座標系，分別為世界坐標系 O、基準參考坐標系 A 以及系統操控端的參考坐標系 B。. 34.

(49) 正向運動學計算與相應之 Jacobian 矩陣就機器人開鏈系統，由基座坐標到操控端坐標間的各構件長度以及相連的各關節轉角，其共同建構了一組映射關係 O. O. X B O R A A X B. (4-3). R A 為基座標對世界座標的轉換矩陣。今重點放在基座和操控端間的關係，. 就且忽略和世界座標間的變換。 A X B li , j  表示相對於 A 坐標，B 座標對關節變數的關係(桿長固定常數)，因此其 Jacobian 為 A. JB .  A XB θ. (4-4). 虛擬作用力函數與關節扭矩計算外部虛擬作用力由關節扭矩反應，包含力與力矩的虛擬作用力 W 與各關節實際轉矩 τ ，依據虛功與能量守恆可知. τ Tθ A WBT AX B. (4-5). 則關節扭矩矩陣可得式 4-1 之結果。. 4.2 平衡控制對於人形機器人的實際作動而言，第三章說明了運動狀態以及腳底板與地面間交互作用維持平衡的條件。於此將說明整個控制流程將如何執行來達成實際機器人的站立姿態於擾動下的平衡回復。人類感測神經不停地接收身體狀況來告知大腦下達反應訊號，人形機器人如同人類一般，安置的感測器不斷量測機體系統狀態，而感測訊號經計算可判斷當前運動狀態是否能維持平衡並使控制器給予制動器相應命令來驅使機器人協調動作。而姿態該如何作動，相對的關節扭矩與平衡判據該如何調整？以下列出整個控制程序要執行的重點. 35.

(50) 1. ICP 的計算與調控 2. 上軀幹動作的執行與否 3. 重力補償 4. 關節扭矩的計算. 4.2.1 ICP 的計算與調整控制流程中首先要執行的即為當前狀態下的 ICP。就第三章講述過的概念，ICP 是衡量穩定度與是否能回復平衡的指標，由系統 CoM 位置與 CoM 速度來決定，其 CoM 位置可安裝慣性量測單元(Inertial Measurement Unit, IMU)來量測，ICP 的計算同式 3-24。 xICP  xCoM . xCoM. (4-6). 0. ICP 可驅使 CoM 移動到期望的 CoP 上，在腳底支撐多邊形內其 ZMP 與 CoP 等價，故控制 ICP 即等同控制 ZMP 位置，且此控制器與踝關節扭矩的輸出決定有很大的相關性。而後續 CoP 將會送進 VMC 內作為計算關節扭矩的要素之一，所以過程中要對其理想位置做調控，於此運用比例控制來得到預計的 CoP。 des des xCoP  xICP  K p _ ICP xICP  xICP . (4-7). 當配置 COP 位置在 ICP 前方時會促使 ICP 向後方推，而預設理想的 ICP 位置為足底板中心 0 點位置，當系統受擾動時當下 ICP 皆會落在理想 ICP 前方，故差值為負，為了讓當前 ICP(等價於當前 CoP)被推回 0 點，故下一步的預設 CoP 為當前 CoP 加上增益量以滿足 ICP 落於 CoP 後方，因此式 4-7 的增益帶負值。. 4.2.2 上軀幹動作的執行選擇 ICP 的座落位置表示了何種穩定策略適合被採用，當 ICP 維持在踝關節策略穩定範圍時髖關節即被鎖定；若 ICP 達到髖關節策略穩定範圍時髖關節 36.

(51) 即相應作動。這裡的扭矩計算是對運動操控端而言的虛擬作用力而言。. 髖關節鎖定控制讓上身與腿部保持一直線，將馬達維持在定點即可，於此對髖關節馬達運用 PD 控制去計算運動操控端的轉矩，.  ty  K p _ ty  hdes   h   K d _ fy hdes  h . (4-8). K p _ ty 與 K d _ fy 分別為比例增益常數與微分增益常數， hdes 與 hdes 分別為髖關節預設角度與角速度，在此皆為 0 值。. 髖關節躬身控制當 ICP 落點超出踝關節穩定範圍外之後，即便踝關節施予最大扭矩皆無法維持系統的平衡，此時必須合併髖關節的應用，實現飛輪扭矩的平衡理論。 ICP 是判斷系統狀態與腳底壓力落點的依據，所以以 ICP 的位置資訊應用比例控制來得到髖關節躬身扭矩 des  ty  K p _ h xICP  xICP . (4-9). 當 ICP 離預設點越遠表示恢復力不足，上軀幹相應增大扭矩來獲得系統 CoM 的復歸，當 ICP 切換回踝關節策略穩定域後，上軀幹躬身角達離 0 位點最大，此時即切回 PD 控制將上軀幹導回直立姿態。. 4.2.3 重力補償前述提過在本文中討論的二維 VMC 應用需要三個虛擬作用力條件，分別為 x、z 和 pitch 軸向，上軀幹的干擾定義在 x 軸向上，z 軸向僅受重力影響，故 z 軸虛擬作用力僅補償重力 f z  mg. (4-10). 37.

(52) 4.2.4 關節扭矩的計算關節空間扭矩的推算，應用 VMC 如式 4-1，需要知道 Cartesian 坐標系下運動操控端的虛擬作用力以及相對座標框架間的正向運動關係。此正向運動關係在 2.3.5 節有提過，z 軸向與 pitch 軸向虛擬作用力前面小節已得結果，餘下運動關係在此做細部的說明與推導。. 虛擬腳趾點(Virtual Toe Points, VTPs) 截至目前已知 f z 與 ty ，x 軸向虛擬作用力尚未得知，在推算 f x 前先介紹一個虛擬腳趾點 VTPs[27]的概念。人類能順暢行走，腳趾關節自由度提供很大的作用，若機器人的足部運動有若腳趾關節的模態，則規劃出來的運動行為將會更為平滑，VTPs 就是因應此而發想出來的概念。其實 VTP 就如同 CoP 乃至於 ZMP 的概念，為與地面交互作用之點，虛擬作用力之合力對於 VTP 的總力矩表現為零，因此定義出 VTP 與運動操控點間的正向運動關係後，就可推算出僅餘未知的 f x 。 des 依 ZMP 的穩定概念，在此定義 VTP 等價為預設的 CoP xCoP ，限制其只. 能在水平面上移動。將基座座標框架 A 定義在 VTP 上，運動操控端座標框架 B 定義在上軀幹質心位置，如圖 4-2。. 圖 4.2. 虛擬腳指點，延腳底板變動。此點的加入應用將有效推出水平向虛擬作用力的量值。 38.

(53) A. xB   LVTP cosVTP  L1 sinVTP   a   L2 sinVTP   a   h . (4-11a). A. z B  LVTP sin VTP  L1 cosVTP   a   L2 cosVTP   a   h . (4-11b). 控制端座標框架 B 相對於基座標框架 A 的正向運動結構為 A.  x   LVTP cosVTP  L1 sin  a  L2 sin a   h  X B   z    LVTP sin VTP  L1 cos a  L2 cos a   h       VTP   a   h    . (4-12). des ；L1 和 L2 分別為腿部(大腿加 LVTP 為 VTP 到踝關節的距離，在此賦值為 xCoP. 小腿)和上軀幹 CoM 到髖關節的距離； VTP 、  a 與  h 分別為虛腳指關節、踝關節及髖關節轉角。因此 A X B 對關節轉角 θ  VTP  a  h  的 Jacobian 可以得 T. 到.  J 11 J 12 J 13  A J B   J 21 J 22 J 23   1 1 1 . (4-13). J11  LVTP sinVTP  L1 cosVTP   a   L2 cosVTP   a   b . J12  L1 cosVTP   a   L2 cosVTP   a   b  J13  L2 cosVTP   a   b  J 21  LVTP cosVTP  L1 sinVTP  1   L2 sinVTP   a   b  J 22  L1 sinVTP  1   L2 sinVTP   a   b  J 23   L2 sinVTP   a   b . 在此定義 VTP  0 。另外虛擬作用力矩陣 W   f x f z  ty  ，依式 4-1 之關係式 T.  VTP   J 11 J 21 1  f x   a    J 12 J 22 1  f z        h   J 13 J 23 1  ty . (4-14). 就 VTP 的概念，我們知道  VTP  0 ，因此乘開可解得 x 軸向虛擬作用力. J  1 f x    21 f y   ty  J 11   J 11. (4-15). 由此以滿足使用 VMC 來推算各關節扭矩的條件，因此關節扭矩解得 39.

(54)  fx   a   J 12 J 22 1   τ   fz  h   J 13 J 23 1    ty . (4-16). 整個控制架構如圖 4.3. 圖 4.3. 雙關節穩定策略之虛擬模型控制器架構。. 40.

(55) 第五章踝髖關節策略之立姿擾動控制模擬. 5.1 SimMechanics 視覺化動力模擬在做機器人的開發前，藉由動力學模擬來判斷反饋的資訊是否正確、符合預期，可降低開發的成本以及成品後續的修正困擾。Adams 是廣為人知的一套動力學分析軟體，提供合適的模擬環境和求解精度，也具有與其他軟體間支援互轉的便利性。 Matlab 在工程領域的知名度是無人不曉，其應用範圍遍佈系統模擬、數位訊號處理和科學計算等各個領域。其中 Simulink 延伸出的可視化編輯平台提高了作業直觀和解析問題的效率，而 Simulink 中提供的 SimMechanics 工具庫(圖 5.1)也是具備動力學分析的工具模組。SimMechanics 專門處理機械系統的模擬，方便地將物理模型以特定模塊組建起來。. 圖 5.1. 第一代與第二代 SimMechanics 工具庫。. 41.