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

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

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

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

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

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

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

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

8、學(xué)模型，運用MATLAB進行運動學(xué)的分析設(shè)計，做簡單的運動模型的研究；然后用Pro/E建立一個簡單的雙足步行機器人的腿部的三維模型；最后根據(jù)之前的的分析和得出的運動數(shù)據(jù)，完成運動仿真。 </p><p>　　關(guān)鍵詞：雙足步行機器人；運動學(xué)；建模仿真</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國內(nèi)外研究現(xiàn)狀1</p><p>　　1.2.1國外研究狀況1</p>

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

16、 雙足機器人的腿部自由度4</p><p>　　2.2 機器人腿部的驅(qū)動方式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小結(jié)7</b></p><p>　　第三章機器人學(xué)的基本相關(guān)理論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 轉(zhuǎn)動特性11</p><p>　　3.2.1 側(cè)擺、俯仰和轉(zhuǎn)動12</p><p>　　3.2.2 旋轉(zhuǎn)矩陣的說明13</p><p>　　3.2.3 旋轉(zhuǎn)矩陣的性質(zhì)14</p><p>　　3.2.4 角速度矢量14</p><p>　　3.2.5 旋轉(zhuǎn)矩陣的微

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

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

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

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

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

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

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

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

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

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

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

30、/p><p>　　國內(nèi)雙足機器人的研制工作起步較晚。1985年以來，相繼有幾所高校進行了相關(guān)的研究并取得了一定的成果。值得一提的是，北京理工大學(xué)研制成功我國首例擬人機器人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、了重大突破，標志著我國雙足機器人研究已經(jīng)跨入世界先進行列。國內(nèi)其它院校如哈爾濱工業(yè)大學(xué)、上海交通大學(xué)、北京航空航天大學(xué)等高等院校也在近幾年投入了相當?shù)娜肆ξ锪M行研制工作。目前，日本和美國對雙足步行機器人的研究已經(jīng)達到了相當高的水平，研制出了能靜態(tài)或動態(tài)行走的多種樣機。國內(nèi)由于起步較晚，與國際最高水平還有一段差距，需要迎頭趕上其它國家。</p><p>　　1.3雙足步行機器人的腿部結(jié)構(gòu)</p>&

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

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

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

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

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

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

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

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

40、單一的運動副也使得機器人的模型得以簡化，機器人關(guān)節(jié)的驅(qū)動裝置就是機電一體化系統(tǒng)中的執(zhí)行裝置，較為液壓執(zhí)行裝置和氣動執(zhí)行裝置，電動執(zhí)行裝置可以獲得較大扭矩和功率，而且速度和精度控制較容易實現(xiàn)。對于角度的大小與速度的快慢控制非常嚴謹，所以選擇微型伺服馬達。</p><p>　　3、微型伺服馬達的參數(shù)</p><p>　　轉(zhuǎn)速：無負載時轉(zhuǎn)過 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>　　雙足步行機器人腿部的設(shè)計需要模仿人類實際尺寸和運動范圍。人機工程學(xué)是研究人與機器及其工作環(huán)境之間的相互作用的學(xué)科。最基礎(chǔ)的便是人體測量學(xué)，對人體各部分肢體尺寸做了詳細研究和分析，從而對雙足步行機器人的仿人的各構(gòu)件的長度及運動范圍的設(shè)計提供有力的理論支持。人體尺寸一般是指人體在空間中占用的狀態(tài)，主要包括高度、寬度和厚度，這些資料根據(jù)測量和統(tǒng)計的方

43、法得出。</p><p><b>　　2.3.1人體測量</b></p><p>　　通過參數(shù)的測量來研究人體肢體外觀形態(tài)特征包括高度、長度、厚度及活動范圍。人體測量基準，采用嚴格規(guī)定的站姿或坐姿，人體測量的基本體型也必須采用標準型，才能達到數(shù)據(jù)要求的嚴謹性，身體均勻，骨骼硬朗，強而有力，肌肉發(fā)達，豐滿結(jié)實，不肥胖。這是人體測量的基本要求。</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è)計都使用比例數(shù)據(jù)來完成確定，為了便于確定數(shù)據(jù)首先選擇身高作為數(shù)據(jù)基準。</p><p>　　雙足步行仿人機器人的腿部結(jié)構(gòu)設(shè)定尺寸大小時，選擇步行機器人的身高為1000mm，按照合適的比例，然后計算各部分下肢的長度，具體取值如下圖<

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

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

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

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

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

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

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

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

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

55、 式 3-6</p><p>　　在圖 3.2 運動過程中，左腿局部坐標系與左腿一起擺動，因而 固定不變。所以擺動腿端部（腳踝處）的位置可用兩種方式來描述：</p><p>　　在世界坐標系中表示腳踝的位置為</p><p>　　在左腿局部坐標系中表示腳踝的位置為</p><p>　　而這兩種表示方式之間的關(guān)系可有式 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、公式可以得出，為了使矩陣運算準確，在轉(zhuǎn)動矩陣和位置矢量組成矩陣后須添加 0 和 1，湊成為 4× 4矩陣完成計算。于是我們定義這樣的4 × 4矩陣：</p><p>　　Ta= 式3-9</p><p>　　這個矩陣Ta稱為齊次變換矩陣，矩陣中是旋轉(zhuǎn)矩陣和位置矢量有規(guī)律的添加0和1構(gòu)成的。于是局部坐標系描述變換到世界

58、坐標系描述就可以表示為：</p><p>　　=Ta× 式3-10</p><p>　　3.1.2齊次變換的鏈乘法則</p><p>　　為了建立左腿髖關(guān)節(jié)處與膝關(guān)節(jié)處兩個局部自由度之間的關(guān)系，我們現(xiàn)在假想的將世界坐標系移動到左腿髖關(guān)節(jié)處，這樣膝關(guān)節(jié)坐標系就被稱為髖關(guān)節(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 表示了，從膝關(guān)節(jié)的局部坐標系中描述腳踝位置到從世界坐標系中描述腳踝位置的變換。其中可以公式中右側(cè)的兩個齊次變換矩陣的乘積構(gòu)成了一個新的齊次變換矩陣。</p><p>　　Tb=TaTb’ 式3-14</p><p>　　對于以上分析，我們可以做迭代處理而得出更加

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

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

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

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、個繞軸的旋轉(zhuǎn)序列來規(guī)定。在任何旋轉(zhuǎn)序列系，旋轉(zhuǎn)次序都是尤為重要的。用公式作如下規(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 旋轉(zhuǎn)矩陣的說明</p><p>　　旋轉(zhuǎn)矩陣在應(yīng)用中存在兩種含義。一種含義是在一個已知的坐標系中對其中矢量的轉(zhuǎn)動作描述，研究對象是矢量。當然這個矢量的轉(zhuǎn)動是有限制的，將這個矢量看作是一個射線的話，這個射線的端點與它運動所在的坐標系原點重合，而射線方向的改變就可以用旋轉(zhuǎn)矩陣來描述。</p><p>　　圖3-4矢量轉(zhuǎn)動描述<

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

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

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

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

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

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

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

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

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

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

78、　　物體上的一點旋轉(zhuǎn)后局部坐標系與世界坐標系之間的關(guān)系為：= 式3-25</p><p>　　上式對時間微分可得該點在世界坐標系中的速度，但由于局部坐標系中的點相對</p><p>　　局部坐標系沒有運動，所以不存在微分關(guān)系，故：</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>　　應(yīng)用叉積計算公式可得：</p><p>　　=× 式3-30</p><p>　　用矩陣相乘形式重新編寫速度矢量可得：</p><p>

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

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

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

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

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

86、軸 A 通過定點 O，轉(zhuǎn)軸所對應(yīng)的角速度矢量為 。剛體上的一點 P 所對應(yīng)的位置矢量為 ，經(jīng)過? 角度的旋轉(zhuǎn)，轉(zhuǎn)動至p’點（位置矢量為 p '）。旋轉(zhuǎn)所在平面交旋轉(zhuǎn)軸于 Q 點。按照矢量關(guān)系有</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)在剛體旋轉(zhuǎn)平面上可得：</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>　　雅克比矩陣描述了關(guān)節(jié)微小位移與空間運動之間的關(guān)系。由機器人雅克比矩陣可以推導(dǎo)出關(guān)節(jié)力矩、機器人與外部外力之間的關(guān)系。在機器人運動學(xué)和動力學(xué)中都有廣泛的應(yīng)用。</p><p>　　本節(jié)利用機器人逆運動

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

105、/p><p>　　4.3.3 正運動學(xué)計算</p><p>　　正運動學(xué)如下圖所示，為單個連桿的齊次變換矩陣的推導(dǎo)。首先將連桿的局部坐標系 的原點設(shè)定到轉(zhuǎn)軸上，這樣在其母連桿坐標系中，關(guān)節(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 可知，對應(yīng)連桿 的位置和姿態(tài)可以分別求出：</p><p>　　=+Rbi

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

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

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

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

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

113、lt;/p><p>　　由正玄定理：= 式4-12</p><p>　　由此a=-sin｛｝ 式4-13</p><p>　　根據(jù)矢量分解，踝關(guān)節(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ù)已經(jīng)求出的角度值代入連桿姿態(tài)關(guān)系式中：</p><p>　　R7=R1Rz（q2）Rx（q3）Ry（q4）Ry(q5+q6)Rx(q7) 式4-14</p><p>　　根據(jù)

115、式 4-9 推導(dǎo)可得：</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ù)學(xué)推導(dǎo)作為基礎(chǔ)，運用 MATLAB 建立方程，完成雅克比矩陣的計算。根據(jù)式 3-59