機械電子工程畢業(yè)論文-基于matlab的雙足步行機器人的腿部模型的建立及運動仿真

1、<p><b>　　本科畢業(yè)論文</b></p><p><b>　　（20 屆）</b></p><p>　　基于Matlab的雙足步行機器人腿部運動模型的建立與運動仿真</p><p><b>　　誠信聲明</b></p><p>　　本人鄭重聲明：本論文及其研

2、究工作是本人在指導教師的指導下獨立完成的，在完成論文時所利用的一切資料均已在參考文獻中列出。</p><p>　　本人簽名： 年 月 日</p><p><b>　　畢業(yè)設計任務書</b></p><p>　　設計題目： 基于MATLAB的雙足步行機器人腿部運動模型的建立與運動仿真 <

3、;/p><p>　　1．設計的主要任務及目標</p><p>　　1）通過查閱有關資料，了解雙足型機器人主要技術參數(shù)；</p><p>　　2）雙足型機器人的腿部模型建立及運動部件設計</p><p>　　3）利用Pro/E完成動作的仿真</p><p>　　2．設計的基本要求和內容</p><p>

4、;　　1）雙足型機器人的腿部功能選擇；</p><p><b>　　2）模型的建立；</b></p><p><b>　　3）運動的仿真</b></p><p>　　4）完成畢業(yè)設計說明書的撰寫</p><p><b>　　3．主要參考文獻</b></p><

5、;p>　　[1] 孫增圻.機器人系統(tǒng)仿真及應[ J ].系統(tǒng)仿真報 ，1995 ，7( 3 ):23-29.</p><p>　　[2] 蔣新松，主編.機器人學導論[ M ].沈陽：遼寧：遼寧科學技術出版社，1994.</p><p>　　[3] 蔡自興.機器人學[ M ].北京：清華大學出版社，2000.</p><p>　　[4] 薛定宇，陳陽泉.基于MA

6、TLAB/Simulink的系統(tǒng)仿真技術與應用[ M ].北京：清華大學出版社，2002</p><p><b>　　4．進度安排</b></p><p>　　審核人： 年 月 日 </p><p>　　基于Matlab的雙足步行機器人腿部運動模型的建立與運動仿真</p>

7、<p>　　摘 要：最近幾年，雙足仿人步行機器人發(fā)展很快，有很高的科學研究價值。步行機器人的運動是模仿人的步行運動的形式，相比其它機器人有更好的靈活性，所以可以完成各種生活中的難度更大的任務，實用價值遠高于其它機器人，當然研究難度和控制也相當復雜。</p><p>　　本設計主要研究了一些雙足步行機器人的一些簡單的問題，如自由度的配置，根據(jù)控制的難易選擇驅動裝置等。通過學習機器人學的基本理論，建立數(shù)

8、學模型，運用MATLAB進行運動學的分析設計，做簡單的運動模型的研究；然后用Pro/E建立一個簡單的雙足步行機器人的腿部的三維模型；最后根據(jù)之前的的分析和得出的運動數(shù)據(jù)，完成運動仿真。 </p><p>　　關鍵詞：雙足步行機器人；運動學；建模仿真</p><p>　　Based on Matlab of biped walking robot leg movement model and

9、</p><p>　　movement simulation</p><p>　　Abstract：The biped walking humanoid robot is developing quickly in recent years, and has a very high scientific value. The motion of Walking robot imitates

10、 the way of human’s walking, has better flexibility than other types of robots, so the more difficult and practical tasks can be completed, and the practical value is much higher than that of other robots.However, the re

11、search difficulty and control of the biped walking humanoid robot is very complex.</p><p>　　This design studies some fundamental questions, such as DOF disposition, the choice of drive device according to th

12、e difficulty of control and so on. Through the learning of basic theory of Robotics，the mathematical model is established, the analysis and design of simple movement patterns Using MATLAB and the research of simple movem

13、ent patterns is completed； Then the three-dimensional model of the legs of a simple biped robot with Pro/E software is established；In the last，according to the previ</p><p>　　Keywords: Biped walking robot；Ki

14、nematics；3D Simulation</p><p><b>　　目 錄</b></p><p>　　第一章 前言1</p><p>　　1.1課題背景及研究意義1</p><p>　　1.2國內外研究現(xiàn)狀1</p><p>　　1.2.1國外研究狀況1</p>

15、<p>　　1.2.2國內研究狀況2</p><p>　　1.3雙足步行機器人的腿部結構2</p><p>　　1.4本論文研究的主要內容3</p><p><b>　　1.5 小結3</b></p><p>　　第二章 雙足機器人的模型建立4</p><p>　　2.1

16、 雙足機器人的腿部自由度4</p><p>　　2.2 機器人腿部的驅動方式4</p><p>　　2.3腿部各個部位的尺寸與運動范圍5</p><p>　　2.3.1人體測量5</p><p>　　2.3.2靜態(tài)人體測量及肢體尺寸確定6</p><p>　　2.3.3動態(tài)人體測量及運動范圍確定7</

17、p><p><b>　　2.4小結7</b></p><p>　　第三章機器人學的基本相關理論8</p><p><b>　　3.1坐標變換8</b></p><p>　　3.1.1 局部坐標系和齊次變換8</p><p>　　3.1.2齊次變換的鏈乘法則10<

18、/p><p>　　3.2 轉動特性11</p><p>　　3.2.1 側擺、俯仰和轉動12</p><p>　　3.2.2 旋轉矩陣的說明13</p><p>　　3.2.3 旋轉矩陣的性質14</p><p>　　3.2.4 角速度矢量14</p><p>　　3.2.5 旋轉矩陣的微

19、分與角速度矢量16</p><p>　　3.2.6 矩陣指數(shù)17</p><p>　　3.3 雅克比矩陣18</p><p>　　3.4 本章小結19</p><p>　　第四章雙足機器人建模20</p><p>　　4.1雙足機器人數(shù)據(jù)結構的建立20</p><p>　　4.1.

20、1數(shù)據(jù)選擇20</p><p>　　4.1.2 數(shù)據(jù)的遍歷21</p><p>　　4.2數(shù)學模型的構造22</p><p>　　4.3 正運動學24</p><p>　　4.3.1 三維矢量與反對稱矩陣24</p><p>　　4.3.2 矩陣指數(shù)計算旋轉矩陣25</p><p>

21、　　4.3.3 正運動學計算25</p><p>　　4.4 逆運動學26</p><p>　　4.4.1 解析法求解逆運動學26</p><p>　　4.4.2 雅克比矩陣28</p><p>　　4.5.1 模擬步行數(shù)據(jù)計算與參數(shù)分析29</p><p>　　4.5.2 上體旋轉一定角度行走32<

22、/p><p><b>　　4.6 小結36</b></p><p>　　第五章機器人模型仿真38</p><p>　　5.1 機器人腿部結構三維建模38</p><p>　　5.1.1 機器人腿部結構零件尺寸設計38</p><p>　　5.1.2 機器人腿部結構的組裝與約束的確定39&l

23、t;/p><p>　　5.1.3 驅動的添加40</p><p>　　5.2 極限位置仿真與模型修改41</p><p>　　5.2.1 上體豎直行走41</p><p>　　5.2.2 上體保持轉動角度行走42</p><p><b>　　5.3 小結44</b></p>&

24、lt;p><b>　　總 結44</b></p><p><b>　　參考文獻46</b></p><p><b>　　致謝47</b></p><p><b>　　第一章 前言</b></p><p>　　1.1課題背景及研究意義</

25、p><p>　　移動型的機器人的應用非常廣泛，移動的方式也各不相同，主要的有履帶式，輪式和步行式，其中，步行式有很大的優(yōu)越性，適應環(huán)境的能力強，不管是相對狹隘的地方還是障礙性比較大的地方，它都可以越過，因此，步行式機器人的實際應用更為廣泛。</p><p>　　雙足步行機器人的靈活性更好，其主要特點在于：首先，它對行走環(huán)境的要求不高，能適應各種地面。其次雙足機器人具有廣闊的工作空間，由于行走系

26、統(tǒng)占地面積小，活動范圍很大，機械手的配置使它具有更廣闊的活動空間，難度最高的步行動作是雙足步行，但其步行性的優(yōu)越性與其它步行結構不可同日而語。因此，雙足機器人的研究工作可以有力的推動機器人學以及其它相關學科的發(fā)展。它是智能機器人理論和技術的集中體現(xiàn)，能夠帶動許多相關學科和技術的交叉發(fā)展和進步。因此，雙足步行機器人的研制具有十分重大的價值和意義。</p><p>　　目前，雙足機器人設計的目的以及應用的領域主要有，

27、在仿生工程學中，為殘疾人、下肢癱患者或截肢者提供室內和戶外的行走工具，利用人工假腿盡可能的使殘疾人恢復正常行走功能。其次，在極限環(huán)境下代替人工操作，像太空星球表面考察、海底探險、水下資源的開發(fā)和設備維修、沉船的尋找和打撈、代替浮游式機器人作為運載工具，以減少推選器對水底的擾動，提高能見度，核電站的監(jiān)視和維護作業(yè)等等各個危險工作。再者，在教育、藝術和大眾服務行業(yè)都有潛在的應用空間，使雙足步行機器人逐步走向大眾。</p>&l

28、t;p>　　1.2國內外研究現(xiàn)狀</p><p>　　1.2.1國外研究狀況</p><p>　　最早在1968年，英國的Mosher試制了一臺名為“Rig”的操縱型雙足步行機器人，揭開了雙足機器人的研究序幕。該機器人只有踝和髖兩個關節(jié)，操縱者靠力回饋來保持機器人平衡。同期間，南斯拉夫的Vukobratovic提出了一種重要的研究雙足機器人的的理論方法，并研制出全世界第一臺真正的雙足

29、機器人。雙足機器人的研制成功促進了康復機器人的研制。</p><p>　　法國Poitiers大學力學實驗室和國立信息與自動化研究所INRIA機構共同開發(fā)了一種具有15個自由度的雙足步行機器人BIP2000，其目的是建立一整套具有適應未知條件行走的雙足機器人系統(tǒng)。他們采用了分層遞解控制結構，使其雙足機器人實現(xiàn)站立、行走、爬坡和上下樓梯等。</p><p>　　1.2.2國內研究狀況<

30、/p><p>　　國內雙足機器人的研制工作起步較晚。1985年以來，相繼有幾所高校進行了相關的研究并取得了一定的成果。值得一提的是，北京理工大學研制成功我國首例擬人機器人BRH-01。該機器人身高1.58米，體重76公斤，具有32個自由度，每小時能夠行走1公里，步幅0.33米。除了能打太極拳，這個機器人還會騰空行走，并根據(jù)自身的平衡狀態(tài)和地面高度變化，實現(xiàn)未知路面的穩(wěn)定行走。它在系統(tǒng)集成、步態(tài)規(guī)劃和控制系統(tǒng)等方面實現(xiàn)

31、了重大突破，標志著我國雙足機器人研究已經跨入世界先進行列。國內其它院校如哈爾濱工業(yè)大學、上海交通大學、北京航空航天大學等高等院校也在近幾年投入了相當?shù)娜肆ξ锪M行研制工作。目前，日本和美國對雙足步行機器人的研究已經達到了相當高的水平，研制出了能靜態(tài)或動態(tài)行走的多種樣機。國內由于起步較晚，與國際最高水平還有一段差距，需要迎頭趕上其它國家。</p><p>　　1.3雙足步行機器人的腿部結構</p>&

32、lt;p>　　難度最大是模仿人類雙足行走的仿人機器人，無論從控制還是配合都十分困難。因為當我們的雙足在運動時，總是在單腳支撐，雙腳支撐及雙腳騰空的幾個狀態(tài)中。對應的驅動方式有完全約束，過度約束，欠約束狀態(tài)?？偠灾?，腿部結構是最需要重點設計和研究的。</p><p>　　當然，我們的腿部運動十分復雜，為了方便研究我們將模型簡化為有六個自由度的腿部結構，六個自由度完全滿足直線行走的要求。</p>

33、<p>　　由于很多的設計者都采用簡單的轉動副來完成腿部關節(jié)的結構。這樣驅動裝置的選擇就簡單多了，最常用的驅動裝置有伺服電機，步進電機，諧波減速電機等，這些驅動器最大的優(yōu)勢在于控制量較少，與控制方面比較容易匹配。更進一步的，在仿照人類腿部結構的基礎上，如何設計出最佳的腿部比例也要在試驗中來獲得。</p><p>　　1.4本論文研究的主要內容</p><p>　　本論文主要研究

34、雙足機器人腿部模型的自由多配置，驅動裝置的選擇，運動模型的建立，以及腿部機構基本尺寸的選擇。</p><p><b>　　本論文分為六章：</b></p><p>　　第一章：前言，介紹了雙足機器人的國內外發(fā)展形勢，提出研究的內容和方向。</p><p>　　第二章：確定運動方式方法及自由度配置，驅動方式的合理選擇及各個零件的尺寸大小和運動范圍

35、大小的選擇。</p><p>　　第三章：研究機器人學的基本理論，包括坐標系的建立，變換矩陣的確定，旋轉矩陣的指數(shù)，雅克比矩陣的定義及意義。</p><p>　　第四章：為雙足機器人建立腿部的數(shù)學模型，用MATLAB編程相關計算，正逆運動學，機器人關節(jié)角度的計算。分析其運動狀態(tài)，判斷運動的可行性。</p><p>　　第五章：進行雙足機器人的腿部三維模型建立，在Pr

36、o/E中設計腿部結構，然后輸入相關數(shù)據(jù)進行運動仿真。</p><p>　　第六章：全文總結，對未來的設計做出展望。</p><p><b>　　1.5 小結</b></p><p>　　本章中全面的分析了雙足步行機器人當前的發(fā)展現(xiàn)狀和未來的發(fā)展趨勢，總結了當前研究成績和方法理論，闡明本論文的研究目的和提出的本論文研究趨勢和理論方法。</p

37、><p>　　第二章：雙足機器人的模型建立</p><p>　　2.1 雙足機器人的腿部自由度</p><p>　　根據(jù)當前的研究發(fā)現(xiàn)，我們人類腿部有多達76個自由度來供我們完成靈活自由的活動，所以，更多的自由度可以使機器人在運動中具有更高的靈活性，但這也增加了驅動與控制的難度。以此，在腿部自由度的設計中，只能配置較少自由度的情況下盡量模擬人類腿部運動。</p&g

38、t;<p>　　根據(jù)自由度的配置，需要髖、踝、膝關節(jié)的共同作用來完成平穩(wěn)步行。三個關節(jié)的主要作用，髖關節(jié)：保證上部身體的位姿，保持上半身的穩(wěn)定，步行中輔助平衡調整步幅。膝關節(jié)：調整重心高度以及調整小腿的擺動高度，以適應相應路況。踝關節(jié)：起到支撐作用，調整擺動腿與地面的接觸，使其接觸平穩(wěn)。</p><p>　　基于分析，結合驅動與控制的情況，以及運動的可行性，將腿部自由度簡化為6個，首先滿足機器人的前

39、進運動，髖、膝、踝各一個自由度，側向的擺動運動由髖踝關節(jié)各布置一個自由度，為了實現(xiàn)轉動，在髖關節(jié)再布置一個自由度。綜上：一共需要配置12個自由度。</p><p>　　表格 2-1自由度分配表</p><p>　　2.2 機器人腿部的驅動方式</p><p>　　腿部結構的運動方式為關節(jié)處的轉動。各個關節(jié)通過轉動副來完成連接。通過轉動副便于數(shù)據(jù)的計算，數(shù)據(jù)全為角度。

40、單一的運動副也使得機器人的模型得以簡化，機器人關節(jié)的驅動裝置就是機電一體化系統(tǒng)中的執(zhí)行裝置，較為液壓執(zhí)行裝置和氣動執(zhí)行裝置，電動執(zhí)行裝置可以獲得較大扭矩和功率，而且速度和精度控制較容易實現(xiàn)。對于角度的大小與速度的快慢控制非常嚴謹，所以選擇微型伺服馬達。</p><p>　　3、微型伺服馬達的參數(shù)</p><p>　　轉速：無負載時轉過 60 °的時間，普通的速度一般在 0 .11

41、s /60° ~0.21s/60。</p><p>　　扭矩：單位為 kg? cm。對于微型伺服馬達是指在舵盤上距離輸出軸軸心水平距離 1c m處，微型伺服馬達能夠拉動物體的重量。</p><p>　　電壓：工作電壓一般為 4 .8V或 6V</p><p>　　重量：以克為單位，微型 9 g，中型 45 g ~100g等。</p><

42、p>　　2.3腿部各個部位的尺寸與運動范圍</p><p>　　雙足步行機器人腿部的設計需要模仿人類實際尺寸和運動范圍。人機工程學是研究人與機器及其工作環(huán)境之間的相互作用的學科。最基礎的便是人體測量學，對人體各部分肢體尺寸做了詳細研究和分析，從而對雙足步行機器人的仿人的各構件的長度及運動范圍的設計提供有力的理論支持。人體尺寸一般是指人體在空間中占用的狀態(tài)，主要包括高度、寬度和厚度，這些資料根據(jù)測量和統(tǒng)計的方

43、法得出。</p><p><b>　　2.3.1人體測量</b></p><p>　　通過參數(shù)的測量來研究人體肢體外觀形態(tài)特征包括高度、長度、厚度及活動范圍。人體測量基準，采用嚴格規(guī)定的站姿或坐姿，人體測量的基本體型也必須采用標準型，才能達到數(shù)據(jù)要求的嚴謹性，身體均勻，骨骼硬朗，強而有力，肌肉發(fā)達，豐滿結實，不肥胖。這是人體測量的基本要求。</p>&l

44、t;p>　　圖2-1 人體的基準面和基準軸</p><p>　　2.3.2靜態(tài)人體測量及肢體尺寸確定</p><p>　　靜態(tài)尺寸研究是研究人體肢體長度的問題。我國早在1989年實施的《中國成年人人體尺寸》中，選取對機器人下肢尺寸有用的數(shù)據(jù)。</p><p>　　在男性人體第 50 百分位數(shù)的人體數(shù)據(jù)中選擇參考數(shù)據(jù)：身高： 1678 mm，大腿長度：4 65

45、mm，小腿長：3 69 mm，足寬：9 6 mm，脛骨點高：4 44 mm，臀寬：3 06 mm，坐姿大腿厚： 130 mm，坐深： 457 mm，臀膝距： 554 mm，最終確定設計都使用比例數(shù)據(jù)來完成確定，為了便于確定數(shù)據(jù)首先選擇身高作為數(shù)據(jù)基準。</p><p>　　雙足步行仿人機器人的腿部結構設定尺寸大小時，選擇步行機器人的身高為1000mm，按照合適的比例，然后計算各部分下肢的長度，具體取值如下圖<

46、;/p><p>　　表格 2-2機器人的主要設計尺寸</p><p>　　2.3.3動態(tài)人體測量及運動范圍確定</p><p>　　人類大部分時間處于運動狀態(tài)，每一個運動時刻我們的肢體總在不同的姿勢時伸展運動來達到活動范圍。對于步行機器人來說，正是因為這些角度范圍的限制，反而有利于計算運動學逆解的時候排除不合實際的情況。根據(jù)已經查閱的資料基本確定了人體腿部各關節(jié)活動的角

47、度范圍以及初步確定雙足步行機器人的各關節(jié)角度變化范圍。</p><p>　　表格2-3人體腿部關節(jié)角度范圍</p><p>　　根據(jù)實際各個關節(jié)的擺動角度范圍，擬設計規(guī)定了機器人腿部關節(jié)的運動范圍</p><p>　　表格2- 4腿部關節(jié)角度變化范圍</p><p><b>　　2.4小結</b></p>

48、<p>　　根據(jù)雙足步行機器人最少自由度和驅動裝置的選擇，按照人體真實測量數(shù)據(jù)，為雙足步行機器人下肢結構的實體建立模型確定了尺寸的大小和各個關節(jié)角的角度范圍。</p><p>　　機器人學的基本相關理論</p><p><b>　　3.1坐標變換</b></p><p>　　為了控制雙足機器人各個肢體的活動位置，我們必要的定義一個空間

49、中的全局坐標系來確定各個部件在空間中的相對位置。為了便于參考，坐標系的初原點設在機器人處于正常站立時重心的正下方，然后相對機器人的視野來定義，X軸指向正前方，Y軸指向左側，Z軸指向正上方。如下 ：</p><p>　　圖3-1機器人世界坐標系</p><p>　　3.1.1 局部坐標系和齊次變換</p><p>　　通過髖部關節(jié)轉動而引起的雙足位置的變化來研究其對應

50、位置關系。在圖3.2(a)中，機器人左腿在世界坐標系中的位置為，因而髖部到腳踝部的位置用矢量來表示。從圖中可以得出：</p><p>　　=+ 式 3-1</p><p>　　右腿站立，左腿擺動時，如圖 3.2(b)位置，腿部伸出狀態(tài)下髖部到腳踝的矢量用表示，這時腳踝處可以表示為：</p><p>　　=+

51、 式 3-2</p><p>　　以上兩個狀態(tài)是在髖部位置 固定不變的情況下，矢量 隨腿部轉動到矢量。 </p><p>　　圖3-2世界坐標系和左腿局部坐標系</p><p>　　根據(jù)圖 3.2 中與腿固定的坐標系就被稱作為局部坐標系 。其與大地固定的坐標系不同的是，局部坐標系是隨著機器人大腿小腿構成的連桿的位姿

52、變化的，是一個可以運動的“動坐標系”。</p><p>　　坐標系是由以髖部作為原點和表示 x、y 和 z 軸的單位向量組成的。局部坐標系與轉角的數(shù)學關系就可以表示成：</p><p>　　Eax= Eay= Eaz= 式 3-3 </p><p>　　由于擺動腿的局部坐標系是繞 x 軸轉的，所以只有Eay和Eaz

53、 發(fā)生改變。所以這三個矢量可以用一個 3×3 矩陣表示出來，具體矩陣如下： </p><p>　　Ra=[Eax Eay Eay] 式 3-4</p><p>　　通過上面的矩陣，左腿在運動和靜止時的矢量關系如下：</p><p>　　=Ra

54、 式3-5</p><p>　　由上面的公式我們可以得出結論，矩陣和原矢量的叉乘可以表示原矢量轉動后得到的矢量。為了研究位置信息，定義左腳腳踝處在局部坐標系 中的位置為。其中左上角標表示的位置點是相對的坐標系，相對的坐標系為世界坐標系時左上角標省略。于是，從圖 3.2(a)中我們可以得到：</p><p>　　=

55、 式 3-6</p><p>　　在圖 3.2 運動過程中，左腿局部坐標系與左腿一起擺動，因而 固定不變。所以擺動腿端部（腳踝處）的位置可用兩種方式來描述：</p><p>　　在世界坐標系中表示腳踝的位置為</p><p>　　在左腿局部坐標系中表示腳踝的位置為</p><p>　　而這兩種表示方式之間的關系可有式 3-2、

56、式 3-5 和式 3-6 聯(lián)立得出：</p><p>　　=+Ra 式3-7</p><p>　　式 3-7 用矩陣的方式表示出來為：</p><p>　　=× 式3-8</p><p>　　從

57、公式可以得出，為了使矩陣運算準確，在轉動矩陣和位置矢量組成矩陣后須添加 0 和 1，湊成為 4× 4矩陣完成計算。于是我們定義這樣的4 × 4矩陣：</p><p>　　Ta= 式3-9</p><p>　　這個矩陣Ta稱為齊次變換矩陣，矩陣中是旋轉矩陣和位置矢量有規(guī)律的添加0和1構成的。于是局部坐標系描述變換到世界

58、坐標系描述就可以表示為：</p><p>　　=Ta× 式3-10</p><p>　　3.1.2齊次變換的鏈乘法則</p><p>　　為了建立左腿髖關節(jié)處與膝關節(jié)處兩個局部自由度之間的關系，我們現(xiàn)在假想的將世界坐標系移動到左腿髖關節(jié)處，這樣膝關節(jié)坐標系就被稱為髖關節(jié)坐標系

59、的局部坐標系，這樣坐標系分別分配為：和</p><p>　　坐標系中觀察腳踝的位置 到坐標系 中觀察腳踝的位置 的矩陣變換與 3.1.2 節(jié)中的變換相似，可以表示為：</p><p>　　=Tb’× 式3-11</p><p>　　這時的齊次變換矩陣Tb’定義如下：</p><

60、;p>　　Tb’= 式3-12</p><p>　　其中是在局部坐標系 中描述局部坐標系的原點位置。</p><p>　　進而將式 3-11 代入式 3-8 中可以得到：</p><p>　　=TaTb’ 式3-13<

61、/p><p>　　式 3-13 表示了，從膝關節(jié)的局部坐標系中描述腳踝位置到從世界坐標系中描述腳踝位置的變換。其中可以公式中右側的兩個齊次變換矩陣的乘積構成了一個新的齊次變換矩陣。</p><p>　　Tb=TaTb’ 式3-14</p><p>　　對于以上分析，我們可以做迭代處理而得出更加

62、有用的結論。假設有一個機械鏈條把 到 局部坐標系連接起來，而且定義相鄰兩個坐標系之間 和的齊次變換矩陣為Ti+1，那么迭代以上討論就可以得出：</p><p>　　Tn=T1T2T3…Tn 式3-15</p><p>　　公式中Tn 為世界坐標系中表示第 N 個關節(jié)的位姿的齊次變換矩陣。如果需要追加關節(jié)只需將對應的齊次變換矩陣右乘到之前的矩陣之

63、上即可。所以將齊次矩陣依次右乘進行坐標變換的計算規(guī)則稱為鏈乘法則。</p><p><b>　　3.2 轉動特性</b></p><p>　　3.2.1 側擺、俯仰和轉動</p><p>　　空間坐標系中，轉動特性是通過繞坐標軸的旋轉運動實現(xiàn)的，本說明書中使用的用 RPY 組合變換表示運動姿態(tài)。RPY 組合是一種常用的旋轉集合，它包含了側擺（r

64、oll）、俯仰（pitch）和轉動（yaw）。具體規(guī)定繞 x 軸旋轉稱為側擺；繞 y 軸旋轉稱為俯仰；繞 z 軸旋轉稱為轉動。如圖所示分別展示了側擺、俯仰和轉動的情況。</p><p>　　圖 3-3側擺、俯仰和轉動情況</p><p>　　下表中定義了 RPY 組合對應的轉動軸、名稱和轉動角的代表符號。</p><p>　　對應的側擺、俯仰和轉動的旋轉矩陣依次為：

65、 </p><p>　　表3-1RPY組合說明</p><p><b>　　Rx（）=</b></p><p><b>　　Ry（=</b></p><p><b>　　Rz()=</b></p><p>　　機械鏈的運動姿態(tài)往往由一

66、個繞軸的旋轉序列來規(guī)定。在任何旋轉序列系，旋轉次序都是尤為重要的。用公式作如下規(guī)定：</p><p>　　Rrpy()=Rz（Ry()Rz(</p><p><b>　　=××</b></p><p><b>　　=</b></p><p>　　其中C=cos，S=sin，其余類

67、似。</p><p>　　3.2.2 旋轉矩陣的說明</p><p>　　旋轉矩陣在應用中存在兩種含義。一種含義是在一個已知的坐標系中對其中矢量的轉動作描述，研究對象是矢量。當然這個矢量的轉動是有限制的，將這個矢量看作是一個射線的話，這個射線的端點與它運動所在的坐標系原點重合，而射線方向的改變就可以用旋轉矩陣來描述。</p><p>　　圖3-4矢量轉動描述<

68、/p><p>　　另一種含義是表示局部坐標系的姿態(tài)改變的描述。在坐標系中定義一個固定點，在坐標系姿態(tài)轉換后重新在新的坐標系中描述這個定點時用到旋轉矩陣，這時研究對象是一個固定點。當然坐標系姿態(tài)的轉動也是有限的，就是規(guī)定姿態(tài)變換前后兩個坐標系的原點是重合的。通俗的說就是換一個視角去觀察同一個物體。</p><p>　　圖3-4對坐標軸轉動描述</p><p>　　這兩種描

69、述方式看似相近，但其實有很多區(qū)別點：</p><p>　　1、研究的對象不同，第一種含義研究的是矢量，第二種含義研究的是固定點。</p><p>　　2、研究對象的運動狀態(tài)不同，第一種含義中的矢量是運動的，第二種含義中的</p><p><b>　　點是固定的。</b></p><p>　　3、研究中坐標系的個數(shù)不同，第

70、一種含義中在同一個坐標系中研究，第二種含</p><p>　　義中產生了新的坐標系。</p><p>　　3.2.3 旋轉矩陣的性質</p><p>　　下面來討論旋轉矩陣的一個重要性質：旋轉矩陣是正交矩陣。</p><p>　　假設討論一個描述局部坐標系姿態(tài)的旋轉矩陣，在這個 3×3 矩陣中，是由單位</p><

71、;p>　　矢量ex，ey，ez 構成的。這三個矢量是兩兩相互垂直的，故他們正交：</p><p>　　Eiej= 式3-17</p><p>　　用矩陣的形式重新表示時：</p><p><b>　　R’R=×===E</b></p><p>

72、　　所以經過以上推算，旋轉矩陣的逆矩陣等于其轉置。從而證明了旋轉矩陣是正交</p><p><b>　　矩陣。</b></p><p>　　3.2.4 角速度矢量</p><p>　　在三維空間中表示轉動速度時，可以表示成為繞某個軸線的轉動。如圖 3.6，是</p><p>　　繞坐標軸 z 軸做的轉動。</p&g

73、t;<p>　　圖3-5繞z軸轉動三維物體</p><p>　　以上也用矢量的形式表示：= 式3-20 </p><p>　　這樣就定義了角速度矢量，其中每個元素的單位都是rad/s。</p><p>　　之后在研究機器人運動時，其運動關節(jié)處的驅動形式就是以電機轉速表示的，故研究轉速作為機器人基礎量就尤

74、為重要。由式 3-20 可以發(fā)現(xiàn)，當轉動軸線定為坐標軸時，就可以將角速度矢量做歸一化處理，將速度矢量分解為一個單位速度矢量和轉速標量的乘積。</p><p>　　=q（表示繞坐標軸旋轉的單位角速度矢量） 式3-21</p><p>　　為了研究某點的速度，如關節(jié)給定角速度的情況下肢體某處的運動速度，就需要進一步研究角速度與速度的關系。在轉動的肢體上選擇一個研究點，并用矢

75、量的方式表示該點，如圖 3.7，而這一點的速度就可以用角速度矢量與該點矢量的叉積來表示。</p><p><b>　　圖3-6速度的表示</b></p><p>　　=× 其速度方向由叉積中的右手定則來規(guī)定，即彎曲右手四指，四指由 沿著小于180 °的方向轉，這時拇指指向的方向就是速度的方向。并且角速度矢量本身也能夠轉動，在式 3-21 兩邊同時左

76、乘旋轉矩陣 R 得：</p><p>　　R=Rq =R,=R，=q 式3-23</p><p>　　由以上推導可以得出，角速度矢量的旋轉可以直接用旋轉矩陣進行變換。當角速度矢量、位置矢量和速度矢量在旋轉矩陣的作用下轉動時有：</p><p>　　=R， =R ，=R</p><p>

77、　　由叉積定義可知，轉動前后的關系為：</p><p><b>　　=× =×</b></p><p>　　所以滿足：R（×）=（R）×（R） 式3-24</p><p>　　3.2.5 旋轉矩陣的微分與角速度矢量</p><p>

78、　　物體上的一點旋轉后局部坐標系與世界坐標系之間的關系為：= 式3-25</p><p>　　上式對時間微分可得該點在世界坐標系中的速度，但由于局部坐標系中的點相對</p><p>　　局部坐標系沒有運動，所以不存在微分關系，故：</p><p>　　= 式3-26</

79、p><p>　　將2-23兩邊左乘:=Rt 式3-27</p><p>　　將3-25代入3-24得：=R’R 式3-28</p><p>　　上公式描述了用局部坐標系中的點的位置來表示世界坐標系中該點的速度的</p><

80、;p>　　計算過程。聯(lián)系世界坐標系中某點速度的計算，式 3-22，本節(jié)中某點相對時間的微分表示的含義就是這一點的速度，即 =，故：= 式3-29</p><p>　　應用叉積計算公式可得：</p><p>　　=× 式3-30</p><p>　　用矩陣相乘形式重新編寫速度矢量可得：</p><p>

81、;　　=×= 式3-31</p><p>　　所得的新矩陣 S，也有很多良好的性質。所以為了表示方便現(xiàn)在需要定義一種新的計算。這個新的矩陣 S，通過觀察可以發(fā)現(xiàn)它在轉置后只是改變了符號： St=S 在數(shù)學上這樣的矩陣被稱為反對稱矩陣。</p><p>　　下面來為了計算方便來定義一種運算，由三維矢量導出對應的反對稱矩陣要操作符“∧”表示，由反對稱矩陣導

82、出對應的三維矢量用操作符“∨”表示。如下：</p><p><b>　　^=^=</b></p><p>　　=== 式3-32</p><p>　　于是角速度矢量在計算時就可以使用以下表示方式：</p><p>　　=^

83、 式3-33</p><p>　　為了表示方便，可以在表示三維角速度矢量導出對應反對稱矩陣時改寫為^，而且旋轉矩陣與角速度矢量的關系為：^=RRt</p><p>　　3.2.6 矩陣指數(shù)</p><p>　　為了得到旋轉矩陣與角速度矢量之間的關系，作如下分析。</p><p>　　在公式 3

84、-34 兩邊同時右乘 R 可得：</p><p>　　=^R 式 3-35</p><p>　　規(guī)定角度的方向時，是以角速度矢量垂直平面為基準平面，并按照右手法則確定的。先定義三維空間內矢量分解，在三維空間中給定任意矢量v 做正交分解，其中一個分量沿單位矢量δ 的方向，并稱為軸向分量，記為V∥；另一個分量沿著矢量垂

85、直方向，并稱為法向分量，記為V⊥ 。如圖 3.8 所示有</p><p>　　= V⊥=-∥=(E-) 式3-36</p><p>　　圖3-7某點p繞定軸轉</p><p>　　并且定點運動的剛體的任何位移，均可以以通過該定點的一個軸的旋轉運動來表示，這是理論力學中的歐拉定理闡述的。圖 3.8(a)就表示了這一旋轉運動，轉

86、軸 A 通過定點 O，轉軸所對應的角速度矢量為 。剛體上的一點 P 所對應的位置矢量為 ，經過? 角度的旋轉，轉動至p’點（位置矢量為 p '）。旋轉所在平面交旋轉軸于 Q 點。按照矢量關系有</p><p>　　=Q+P 式3-37</p><p>　　其中，將 做正交分解， 就為δ 方向的軸向分量，

87、根據(jù)式 3-36 有：</p><p>　　Q=’ 式 3-38</p><p>　　QP 就為δ 方向的法向分量，根據(jù)式 3-36 有：</p><p>　　P=(E-’) 式3-39</p><p

88、>　　如圖 3.8(b)在剛體旋轉平面上可得：</p><p>　　P=(cos)P+（sin）P 式3-40</p><p>　　在三維坐標中向量P同時垂直于ω 和 p 兩個矢量，故可用ω 和 p 的叉積來表示：P==^ 式3-41</p><p>　　

89、將以上聯(lián)立得: P=cos(E-r)+sin^ 式3-42</p><p><b>　　3.3 雅克比矩陣</b></p><p>　　雅克比矩陣描述了關節(jié)微小位移與空間運動之間的關系。由機器人雅克比矩陣可以推導出關節(jié)力矩、機器人與外部外力之間的關系。在機器人運動學和動力學中都有廣泛的應用。</p><p>　　本節(jié)利用機器人逆運動

90、學來推導雅克比矩陣。如圖3.9所示為機器人單條腿的腿部結構，從原點到端點分別定義連桿為1到N。并且在正運動學已經計算完成的情況下完成后續(xù)的推導。</p><p>　　圖3-9雅克比矩陣計算</p><p>　　假設在如圖3.9的腿部關節(jié)中，只有第二個關節(jié)轉 動微小角度，引起第N桿的微小位移和微小轉動可得到以下關系：</p><p>　　n=w（n-2）q2 <

91、;/p><p>　　n=wq2 式3-43</p><p>　　根據(jù)以上推導，為每一個桿件都做計算，之后將各個桿件對N桿的運動影響疊加</p><p>　　起來，就是N桿最終的運動狀態(tài)：</p><p><b>　　N=</b></p>

92、<p>　　N= 式3-44</p><p><b>　　3.4 本章小結</b></p><p>　　本章內容是在闡述機器人相關數(shù)學基礎，其中推導了變換矩陣、旋轉矩陣的矩陣指數(shù)運算、定義了轉動運動、雅克比矩陣等，為后續(xù)數(shù)學建模提供了理論基礎。</p><p

93、><b>　　雙足機器人建模</b></p><p>　　4.1雙足機器人數(shù)據(jù)結構的建立</p><p><b>　　4.1.1數(shù)據(jù)選擇</b></p><p>　　雙足機器人的本體結構可以理解為是由很多桿件和關節(jié)連接構成的，最重要的問題就是簡化數(shù)據(jù)編寫和計算，進一步對機器人進行有效的分解，是本章研究的要點。<

94、/p><p>　　步行機器人的數(shù)據(jù)庫是建立在很多局部坐標系與世界坐標系之間點、速度、角速度和力、力矩等量的關系。它的肢體可以抽象為桿件，聯(lián)動運動的部分是連接桿件的關節(jié)部分，為了建立合理的數(shù)據(jù)結構先將機器人做如下分解。</p><p>　　圖4-1機器人腿部結構的分解</p><p>　　每一個桿件都包含一個關節(jié)。編程時，將每一個結構定義為對應的名稱。</p>

95、<p>　　各個結構之間的關系采用二叉樹數(shù)據(jù)模式構成，構成原則是：本級別的分解部分作為二叉樹的根結點，下一級別的分解部分作為二叉樹的左子樹（可以看作家庭關系中的“子”），同一級別的最近的分解部分作為二叉樹的右子樹（可以看作家庭關系中的“兄弟”），具體分配圖如圖：</p><p>　　圖4-2機器人腿部結構分解</p><p>　　為了將上述分解變成實際數(shù)據(jù)結構，必須給每個分解

96、部分對應標號并建立數(shù)據(jù)</p><p><b>　　表4-1數(shù)據(jù)列表</b></p><p>　　這些數(shù)據(jù)可以在 MATLAB 程序中直接輸入：</p><p>　　圖4-3結構體變量輸入</p><p>　　4.1.2 數(shù)據(jù)的遍歷</p><p>　　在遞歸算法使用時，必須有一個明確的遞歸結束條

97、件，又被稱為遞歸出口。其實這種方式也對應二叉樹的遍歷方式，以下用前序遍歷法編寫顯示各個連桿名稱的函數(shù)。</p><p>　　圖4-4遞歸算法的編寫</p><p>　　函數(shù)在每次調用自己的時候就會在結構中向下移動一層，直到無姐妹和子的結點。這樣如圖 4.2 的樹形結構就可以完成整個結構樹結點的訪問計算，并且不會無窮循環(huán)，提供了可靠的遞歸出口。同時也是得編程語句變得極為精簡，而且便于理解。&

98、lt;/p><p>　　4.2數(shù)學模型的構造</p><p>　　構造數(shù)學模型是為步行機器人對應它的分解部分給他們賦予標號，然后為每一個分解部分構造一個擁有大量數(shù)據(jù)的結構體變量，再構造結構體數(shù)組來完成整個機器人數(shù)字模型的建立。下面就一一介紹結構體數(shù)組中的各個變量的定義和分配。首先需要對有 12 自由度的仿人機器人腿部的連桿進行編號，每個自由度驅動裝置必須滿足運動：</p><

99、;p>　　圖4-5關節(jié)標號分配</p><p>　　各個局部各個局部坐標系之間的相對位置關系是由關節(jié)軸矢量和相對位置矢量來確定，如下圖，結構體中添加對應變量：關節(jié)軸矢量→“a”，其中，關節(jié)軸矢量是指相對于母連桿轉動的單位角速度矢量，其旋轉方向是由右手法則來定義，膝關節(jié)關節(jié)軸矢量為：</p><p><b>　　5=11=</b></p><p

100、>　　相對位置 表示的是一個局部坐標系的原點在其母連桿坐標系中的位置。如踝關節(jié)處，R7 和R13 分別與其母連桿R6 和R12 所在的局部坐標系原點重合，故：</p><p><b>　　7=15=</b></p><p>　　圖4-6關節(jié)參數(shù)設定</p><p>　　連桿關節(jié)角、關節(jié)速度、桿件質量等一些參數(shù)的設定，具體設定見表</

101、p><p>　　表4-2連桿結構體變量表</p><p><b>　　4.3 正運動學</b></p><p>　　正運動學可以用于機器人重心計算、機器人可視化位姿計算和機器人與環(huán)境位置方位關系等計算。</p><p>　　4.3.1 三維矢量與反對稱矩陣</p><p>　　生成的反對稱矩陣是在原有

102、軸矢量的數(shù)據(jù)基礎之上生成的特殊形式，其操作符在第三章 3.2.5 節(jié)中已經定義，對應的變換定義式為式 3-32。根據(jù)以上定義分別編寫函數(shù) a_0 與函數(shù) a_1 來完成操作符轉換的任務。其中函數(shù) a_0 完成操作符“∧”的定義，函數(shù) a_1 完成操作符“∨”的定義。</p><p>　　圖4-7函數(shù)完成操作符</p><p>　　需要注意的是，這兩個函數(shù)都同時具有傳送和返回數(shù)據(jù)的作用。調用

103、對應函數(shù)時</p><p>　　需注意對應的傳遞值和返回值的形式，如函數(shù) a_0 中傳遞值為三維矢量，返回值為反對稱矩陣，函數(shù) a_1 反之。</p><p>　　4.3.2 矩陣指數(shù)計算旋轉矩陣</p><p>　　R=E+^sinq+^2(1-cosq) 式4-1</p><

104、p>　　根據(jù)式 4-1 可以編寫對應函數(shù) Rodrigues，如下：</p><p>　　圖4-8旋轉矩陣計算函數(shù)</p><p>　　函數(shù) Rodrigues 中的傳遞輸入值為兩個數(shù)據(jù)分別為關節(jié)軸矢量和關節(jié)角。關節(jié)軸矢量為三維單位矢量，是根據(jù)機器人關節(jié)的轉動方向已經完成規(guī)定的，對應關節(jié)數(shù)據(jù)已經分配在 LINK 結構體數(shù)組中。關節(jié)角表示對應關節(jié)彎曲的角度值，數(shù)據(jù)以弧度制表示。<

105、/p><p>　　4.3.3 正運動學計算</p><p>　　正運動學如下圖所示，為單個連桿的齊次變換矩陣的推導。首先將連桿的局部坐標系 的原點設定到轉軸上，這樣在其母連桿坐標系中，關節(jié)軸矢量為，相對位置矢量（局部坐標系原點在母連桿坐標系中的位置）為。</p><p>　　圖4-9單個連桿位姿</p><p>　　連桿 相對于母桿的齊次變換矩陣

106、為：</p><p>　　Tj= 式4-2</p><p>　　之后研究如圖 4.10 兩連桿連接情況。假定母連桿的絕對位置和姿態(tài) ，那么</p><p>　　母桿 的齊次變換矩陣（相對世界坐標）就為：</p><p>　　T= 式4-3</p&g

107、t;<p>　　這樣，連桿 的齊次變換矩陣（相對世界坐標）可由鏈式法則得到：</p><p>　　Tj=TTj 式4-4</p><p>　　聯(lián)立式 4-1、式 4-2 和式 4-3 可知，對應連桿 的位置和姿態(tài)可以分別求出：</p><p>　　=+Rbi

108、 式4-5</p><p>　　Rj=Rieaj 式4-6</p><p>　　以上程序在主程序中調用之前，需要對 BODY 連桿的位姿首先予以確定，也就是給 LINK(1).p 和 LINK(1).R 確定的值以確定 BODY 的絕對位姿。然后給出各個關節(jié)的關節(jié)

109、角（LINK(*).q），之后在主程序中運行 ForwardKinematics(1)就能完成全部桿件的位姿計算。</p><p><b>　　4.4 逆運動學</b></p><p>　　運動學可以用于給定機器人腿部和身體姿態(tài)后求取對應關節(jié)角度的計算。就是在給定端部桿件的位姿之后求出全部關節(jié)角度，可以看作正運動學的逆運算。</p><p>　

110、　4.4.1 解析法求解逆運動學</p><p>　　先分析雙足步行機器人模型的右下肢，在計算準備時，必須讓上半身（1 R1）和左下肢（7 R7）定義為靜止。然后定義從身體坐標系原點到髖關節(jié)距離為 D，大腿長度為 A，小腿長度為 B，詳細圖示如下：</p><p>　　2=1+R 式4-7</p><p&

111、gt;　　圖4-10腿部關節(jié)逆運動計算</p><p>　　在足部踝關節(jié)的局部坐標系中，髖關節(jié)的位置為：</p><p>　　=R7（p2-p7）= 式4-8</p><p>　　如上圖膝關節(jié)，故 C 可以求出：</p><p>　　C=

112、 式4-9</p><p>　　這樣在三角形 ABC 中，運用余弦定理有：</p><p>　　C=A+B-2ABcos（） 式4-10</p><p>　　根據(jù)式 4-10 可導出膝關節(jié)角度：</p><p>　　q=-cos{ }+ 式4-11&

113、lt;/p><p>　　由正玄定理：= 式4-12</p><p>　　由此a=-sin｛｝ 式4-13</p><p>　　根據(jù)矢量分解，踝關節(jié)角q=atan2（Ry，Rz）</p><p>　　q’=-atan2｛Rx，sign（Rz）｝-a</p>

114、;<p>　　sign（） 函數(shù)，含義為取符號函數(shù)，即當 x 為正時返回+1，當 x 為負時返回-1，當 x為零時返回 0。</p><p>　　之后根據(jù)已經求出的角度值代入連桿姿態(tài)關系式中：</p><p>　　R7=R1Rz（q2）Rx（q3）Ry（q4）Ry(q5+q6)Rx(q7) 式4-14</p><p>　　根據(jù)

115、式 4-9 推導可得：</p><p>　　Rz（q2）Rx（q5）Ry（q4）=R1R7{Rx（q7）}{Ry(q5+q6)} 式4-15</p><p>　　式 4-15 左右兩邊分別展開和計算有：</p><p><b>　　= 式4-16</b></p><p>　　根據(jù)4-16矩陣得：</p

116、><p>　　q=atan2(-R,R)</p><p>　　q=atan2(R,-RS+RC)</p><p>　　q=atan2（-R，R）</p><p>　　4.4.2 雅克比矩陣</p><p>　　以 3.3 節(jié)中雅克比方程的數(shù)學推導作為基礎，運用 MATLAB 建立方程，完成雅克比矩陣的計算。根據(jù)式 3-59