第3章

機器人運動學(xué)主要內(nèi)容3.1機械臂正向運動學(xué)3.2機械臂逆向運動學(xué)3.3機械臂微分運動與雅可比矩陣3.4本章小結(jié)正向運動學(xué)是指將設(shè)定好具體值的各關(guān)節(jié)變量代入建立起來的機械臂運動方程得出末端執(zhí)行器在笛卡爾空間中的具體位置與姿態(tài)。3.1機械臂正向運動學(xué)輸入：機械臂各個關(guān)節(jié)變量（包括關(guān)節(jié)旋轉(zhuǎn)角度量或平移量）輸出：機械臂末端的具體位置與姿態(tài)黑盒子：機械臂運動方程（需要求解）由于正向運動學(xué)是在各個關(guān)節(jié)坐標系之間的旋轉(zhuǎn)角度和位移距離已知的情況下計算出機械臂位姿，求解方向沿著唯一確定的運動方程正向推導(dǎo)，因此其通過正向運動學(xué)計算出來的位姿結(jié)果往往具有準確性和唯一性。3.1機械臂正向運動學(xué)列出D-H參數(shù)表建立連桿坐標系推導(dǎo)連桿變換矩陣求出機械臂運動方程3.1.1單連桿描述圖3-1單連桿描述連桿的運動學(xué)功能在于保持其兩端的關(guān)節(jié)軸線具有固定的幾何關(guān)系。對于單個連桿來說可借助以下變量準確描述，如圖3-1和表3-1所示：描述參數(shù)參數(shù)定義i(正整數(shù))連桿，關(guān)節(jié)標號連桿i-1連接于關(guān)節(jié)i-1與關(guān)節(jié)i之間的連桿機構(gòu)關(guān)節(jié)軸線i-1和關(guān)節(jié)軸線i的夾角ai-1關(guān)節(jié)軸線i-1和關(guān)節(jié)軸線i的公法線長度表3-1單連桿描述參數(shù)3.1.2多連桿連接描述對于多連桿機械臂，定義連桿0是機械臂的基座(底座)，但不屬于連桿。之后分別是連桿1到連桿n。關(guān)節(jié)1連接基座與連桿1，關(guān)節(jié)2相連接連桿1與連桿2……，關(guān)節(jié)n相連接連桿n-1與連桿n，以此類推。圖3-2兩連桿連接描述3.1.2多連桿連接描述當多根連桿連接成機械臂一體時，需要有一套統(tǒng)一的標準來描述這些連桿之間的關(guān)系。對于多個連桿按次序連接來說可借助以下變量準確描述，如圖3-2和表3-2所示：描述參數(shù)參數(shù)定義i(正整數(shù))連桿，關(guān)節(jié)標號連桿i-1連接于關(guān)節(jié)i-1與關(guān)節(jié)i之間的連桿機構(gòu)關(guān)節(jié)軸線i-1和關(guān)節(jié)軸線i的夾角ai-1關(guān)節(jié)軸線i-1和關(guān)節(jié)軸線i的公法線長度關(guān)節(jié)軸線i-1和關(guān)節(jié)軸線i，關(guān)節(jié)軸線i和關(guān)節(jié)軸線i+1的兩條公法線ai-1與ai之間距離關(guān)節(jié)軸線i-1和關(guān)節(jié)軸線i，關(guān)節(jié)軸線i和關(guān)節(jié)軸線i+1的兩條公法線ai-1與ai之間夾角表3-2多連桿連接描述參數(shù)圖3-2兩連桿連接描述3.1.2多連桿連接描述設(shè)一個機械臂擁有n個關(guān)節(jié)和n個連桿，那么對于首段連桿(即連桿1)和末端連桿(即連桿n)，由于連桿0為基座以及不存在關(guān)節(jié)0，n+1和連桿n+1，故規(guī)定：

3.1.2多連桿連接描述

123……………n表3-3D-H參數(shù)表3.1.3連桿坐標系建立（1）連桿坐標系標號為了確定各連桿之間的相對運動和位姿關(guān)系，需要在基座和每一根連桿上固接一個笛卡爾空間直角坐標系。在這里定義與基座固接的坐標系稱為基坐標系{0}；與連桿1固接的坐標稱為坐標系{1}……以此類推，與連桿i固接的坐標系稱為坐標系{i}(其中i為正整數(shù))。坐標系{i}都是建立在連桿i對應(yīng)的關(guān)節(jié)i上。如圖3-3所示：圖3-3連桿坐標系示意圖3.1.3連桿坐標系建立（2）中間連桿i的坐標系{i}Step1：坐標系{i}的Z軸Zi與關(guān)節(jié)軸線i共線，其指向任意規(guī)定；Step2：坐標系{i}的X軸Xi與連桿公垂線(即ai)重合，指向從關(guān)節(jié)i到關(guān)節(jié)i+1；當ai=0時，取Xi=±Zi+1

×Zi

；Step3：坐標系{i}的Y軸Yi根據(jù)右手法則確定，即Yi

=Zi×Xi

；Step4：坐標系{i}的原點Oi取Xi與Zi的交點作為原點。當Zi和Zi+1相交(即ai=0)時取其交點作為原點。圖3-3連桿坐標系示意圖3.1.3連桿坐標系建立

圖3-3連桿坐標系示意圖3.1.3連桿坐標系建立

圖3-3連桿坐標系示意圖3.1.3連桿坐標系建立（5）結(jié)合連桿坐標系規(guī)定D-H參數(shù)利用連桿坐標系，D-H參數(shù)可以明確地被定義，如表3-4所示：描述參數(shù)參數(shù)定義從Zi-1到Zi繞Xi-1旋轉(zhuǎn)的角度ai-1從Zi-1到Zi沿Xi-1正方向測量的距離從Xi-1到Xi沿Zi正方向測量的距離從Xi-1到Xi繞Zi旋轉(zhuǎn)的角度表3-4結(jié)合連桿坐標系定義D-H參數(shù)

3.1.3連桿坐標系建立例3.1一個三連桿平面機械臂系統(tǒng)如圖3-4(A)所示，其中三個關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)，連桿1長為0.2m，連桿2長為0.25m，連桿3長為0.05m。連桿1相對于基坐標系逆時針旋轉(zhuǎn)45度，連桿2相對于連桿1逆時針旋轉(zhuǎn)60度，連桿3相對于連桿2逆時針旋轉(zhuǎn)30度，請根據(jù)這個機械臂建立連桿坐標系并得出D-H參數(shù)表。圖3-4例3.1所示的三連桿平面機械臂系統(tǒng)正視圖(A)3.1.3連桿坐標系建立答案：根據(jù)連桿坐標系建立方法，由于Z軸代表關(guān)節(jié)軸線，在圖3.4的正視圖中以點的形式展現(xiàn)，故在這里省略Z軸。X軸與相鄰兩根連桿的公垂線重合，故方向與連桿平行。Y軸方向可通過右手法則確定。故在建立世界坐標系{0}的條件下，針對圖3-4(A)所示的三連桿平面機械臂系統(tǒng)，其連桿坐標系如圖3-4(B)所示。圖3-4例3.1所示的三連桿平面機械臂系統(tǒng)正視圖(A)(B)3.1.3連桿坐標系建立針對這個三連桿平面機械臂系統(tǒng)，根據(jù)D-H參數(shù)的定義可以得出D-H參數(shù)表如下：iai-1(m)di(m)100045200.2060300.25030表3-5例3.1所示的三連桿平面機械臂系統(tǒng)D-H參數(shù)表3.1.4剛體運動位置表示

圖3-5笛卡爾空間直角坐標系下三維空間六自由度3.1.4剛體運動位置表示

3.1.4剛體運動位置表示

求出這個點在世界坐標系{W}下的坐標。

3.1.5剛體方位與運動姿態(tài)

圖3-7剛體固接坐標系{B}在坐標系{A}下的表示3.1.5剛體方位與運動姿態(tài)

3.1.5剛體方位與運動姿態(tài)

圖3-8手爪運動位姿描述

3.1.5剛體方位與運動姿態(tài)（2）用手爪坐標系描述末端運動位姿圖3-8手爪運動位姿描述

3.1.5剛體方位與運動姿態(tài)（3）用偏轉(zhuǎn)、俯仰和回轉(zhuǎn)角(固定角坐標系)描述運動姿態(tài)

3.1.5剛體方位與運動姿態(tài)(a)(b)(c)圖3-9用固定角坐標系描述運動姿態(tài)。(a)偏轉(zhuǎn)角，(b)俯仰角，(c)回轉(zhuǎn)角

（3）用偏轉(zhuǎn)、俯仰和回轉(zhuǎn)角(固定角坐標系)描述運動姿態(tài)3.1.5剛體方位與運動姿態(tài)（3）用偏轉(zhuǎn)、俯仰和回轉(zhuǎn)角(固定角坐標系)描述運動姿態(tài)

其中Rot(坐標軸,旋轉(zhuǎn)角度)為4×4的齊次變換矩陣矩陣，表示繞坐標軸只旋轉(zhuǎn)而不平移。3.1.5剛體方位與運動姿態(tài)（4）用旋轉(zhuǎn)序列(歐拉角坐標系)描述運動姿態(tài)機械臂的運動姿態(tài)往往由繞軸X,Y,Z的旋轉(zhuǎn)序列確定，這種旋轉(zhuǎn)序列稱為歐拉(Euler)角。這其中涉及的旋轉(zhuǎn)次序(包括每次繞什么軸旋轉(zhuǎn)多少角度等信息)起到非常關(guān)鍵的作用，不同的旋轉(zhuǎn)次序可能會導(dǎo)致不同的運動姿態(tài)結(jié)果。圖3-10用歐拉角坐標系描述運動姿態(tài)3.1.5剛體方位與運動姿態(tài)（4）用旋轉(zhuǎn)序列(歐拉角坐標系)描述運動姿態(tài)

圖3-10用歐拉角坐標系描述運動姿態(tài)

3.1.5剛體方位與運動姿態(tài)（5）固定角坐標系與歐拉角坐標系描述運動姿態(tài)區(qū)別分析運動姿態(tài)描述方法固定角坐標系歐拉角坐標系所參考的坐標系固定的坐標系（例如世界坐標系）上一步變換得來的坐標系計算方法從右向左乘以旋轉(zhuǎn)矩陣從左向右乘以旋轉(zhuǎn)矩陣表3-6固定角坐標系與歐拉角坐標系描述運動姿態(tài)區(qū)別分析3.1.6連桿變換矩陣推導(dǎo)

3.1.6連桿變換矩陣推導(dǎo)

3.1.6連桿變換矩陣推導(dǎo)例3.3在一個機械臂對應(yīng)的D-H參數(shù)表中，第k行如下所示(k為正整數(shù))：ia

i-1(m)di(m)……………k300.25160……………表3-7例3-3對應(yīng)的D-H參數(shù)表

3.1.6連桿變換矩陣推導(dǎo)ia

i-1(m)di(m)……………k300.25160……………表3-7例3-3對應(yīng)的D-H參數(shù)表根據(jù)式子(3-13)，可以得出：

3.1.7機械臂運動方程表示任何機械臂從建模的角度上都可以視為若干根由關(guān)節(jié)連接起來的連桿組成。結(jié)合D-H參數(shù)、連桿坐標系、連桿變換矩陣以及歐拉角坐標系下旋轉(zhuǎn)序列，機械臂運動方程建立步驟如圖3-11所示：列出D-H參數(shù)表建立連桿坐標系推導(dǎo)連桿變換矩陣求出機械臂運動方程圖3-11機械臂運動方程建立流程3.1.7機械臂運動方程表示

3.1.7機械臂運動方程表示

故式子(3-18)也是n關(guān)節(jié)機械臂運動方程的表示通式。3.1.7機械臂運動方程表示例3.4在例3.1的基礎(chǔ)上求出機械臂的運動方程的值。答案：例3.1所展示的是一個具有三旋轉(zhuǎn)關(guān)節(jié)的機械臂，則根據(jù)其D-H參數(shù)表(即表3-3)可以得出各個連桿變換矩陣：

3.1.7機械臂運動方程表示所以運動方程的值為：

逆向運動學(xué)是指在相對于用戶工作臺(基座)坐標系的位姿已知的情況下計算機械臂各個關(guān)節(jié)滿足期望要求的角度解的過程，與正向運動學(xué)過程成互逆關(guān)系。3.2機械臂逆向運動學(xué)輸入：機械臂末端的具體位置與姿態(tài)輸出：機械臂各個關(guān)節(jié)變量（包括關(guān)節(jié)旋轉(zhuǎn)角度量或平移量）逆運動學(xué)求解方法：代數(shù)解法、幾何解法……3.2.1解的存在性分析機械臂的工作空間通常把逆解存在的區(qū)域稱為機械臂的工作空間。靈巧工作空間：指機械臂末端執(zhí)行器能夠從各個方向(無窮多個)到達的空間區(qū)域，在靈巧工作空間的各點上，可以以任意的姿態(tài)擺放機械臂。可達工作空間：表示機械臂末端執(zhí)行器至少從一個方向(有限數(shù)目)到達的目標點集合。靈巧工作空間講究任意性，約束條件較高；可達工作空間講究存在性，約束條件較低，因此靈巧工作空間是可達工作空間的子集。機械臂的工作空間靈巧工作空間可達工作空間

3.2.1解的存在性分析

3.2.1解的存在性分析

圖3-13機械臂裝置的正視圖3.2.1解的存在性分析

圖3-14機械臂裝置可達工作空間的正面剖視圖（陰影處）3.2.2解的多重性分析解的多重性定義機器人運動學(xué)逆解的多重性：于給定的機械臂可達工作領(lǐng)域內(nèi)，機械臂末端可以以不止一種的位形到達目標點，因此對于給定的在機械臂的工作域內(nèi)的末端位置可以得到多個解。圖3-15兩連桿操作臂，其中虛線代表第二個位形解

3.2.2解的多重性分析“最短行程”解由于機械臂系統(tǒng)只能從多個位形中選擇一個最優(yōu)解，因此對于解的選擇,當前比較容易接受的是“最短行程”解，指在此移動過程中使每一個運動關(guān)節(jié)的移動量最小。圖3-16到達B點有兩個位形解，其中一個會受障礙物阻礙無法實現(xiàn)

3.2.2解的多重性分析“最短行程”解如圖3-16所示，如果操作臂處于點A，我們希望它移動到點B。在沒有障礙物的情況下，可選擇圖3-16中上部虛線所示的位形；有障礙物約束時則需要按照下部虛線所示的位形才能到達B點。圖3-16到達B點有兩個位形解，其中一個會受障礙物阻礙無法實現(xiàn)

3.2.2解的多重性分析“最短行程”解解的個數(shù)取決于機械臂的關(guān)節(jié)數(shù)量，也與連桿D-H參數(shù)和關(guān)節(jié)運動范圍、工作空間的變化有關(guān)。通常連桿的非零D-H參數(shù)越多，達到某一特定目標的方式，即關(guān)節(jié)可行解也越多。圖3-16到達B點有兩個位形解，其中一個會受障礙物阻礙無法實現(xiàn)

3.2.3逆運動學(xué)解法概述逆運動學(xué)求解方法封閉解：從解的通用表達式中就可以算出任何對應(yīng)值，求解精度較高，解的可控性強代數(shù)法幾何法數(shù)值迭代解：求解速度慢，更加耗時神經(jīng)網(wǎng)絡(luò)解：模型設(shè)計的自由度較高，能夠以任意精度逼近解……3.2.3逆運動學(xué)解法概述例3.6請試圖分析代數(shù)解法和幾何解法的區(qū)別和聯(lián)系，以及各自的特點。答案：關(guān)于代數(shù)解法和幾何解法，二者之間的相同和不同點以及各自的特點如表3-8所示：方法代數(shù)解法幾何解法共同點都是在已知位形、末端相對于世界坐標系的變換矩陣和運動方程的情況下求出機械臂各個關(guān)節(jié)的旋轉(zhuǎn)角度/平移量解不同點通過解方程組的方式求取旋轉(zhuǎn)角度/平移量通過解析平面幾何的知識求取旋轉(zhuǎn)角度/平移量特點解的結(jié)果精確度高，能夠判斷解的存在性和多重性，遍歷出多種可行解集求解過程非常直觀，容易理解，得出部分可行解集表3-8代數(shù)解法與幾何解法對比3.2.4代數(shù)解法代數(shù)解法是在已知位形、末端相對于世界坐標系的變換矩陣和運動方程的情況下通過解方程組的方式求出機械臂各個關(guān)節(jié)旋轉(zhuǎn)角度或者平移量。例3.7一個三連桿平面機械臂，其正視圖結(jié)構(gòu)如圖3-17所示。由此通過該三連桿平面機械臂位形和對應(yīng)的變換矩陣可以得出其D-H參數(shù)表，如表3-9所示：圖3-17三連桿平面操作臂3.2.4代數(shù)解法例3.7一個三連桿平面機械臂，其正視圖結(jié)構(gòu)如圖3-17所示。由此通過該三連桿平面機械臂位形和對應(yīng)的變換矩陣可以得出其D-H參數(shù)表，如表3-9所示：表3-6三連桿平面機械臂D-H參數(shù)表10002003003.2.4代數(shù)解法答案：根據(jù)機械臂運動方程建立步驟，從基座標系{0}到末端坐標系{3}的正向運動學(xué)矩陣表達式為：

3.2.4代數(shù)解法

3.2.4代數(shù)解法可以得出：

因此：

3.2.4代數(shù)解法

3.2.4代數(shù)解法因此式子(3-29)可以改寫成：

因此可以得到：

3.2.4代數(shù)解法用代數(shù)方法求解運動學(xué)方程是求解操作臂的基本方法之一，在求解方程時，解的形式已經(jīng)確定。然而在求解過程中可以看出，對于許多常見問題，經(jīng)常會出現(xiàn)幾種形式的超越方程。針對超越方程問題，可通過式子(3-35)進行變換，用單一變量u來替換表示：

式子(3-35)是在求解運動學(xué)方程中經(jīng)常用到的一種非常關(guān)鍵的幾何變換方法，把超越方程變換成關(guān)于u的多項式方程，從而運用換元方式方便求解問題。3.2.5幾何解法

圖3-17三連桿平面操作臂3.2.5幾何解法

圖3-18三連桿平面操作臂的平面幾何關(guān)系

3.2.5幾何解法

圖3-18三連桿平面操作臂的平面幾何關(guān)系

3.2.5幾何解法

再運用余弦定理可得：

3.2.6三軸相交的Pieper解法對于六自由度的機械臂來說，逆運動學(xué)求解非常復(fù)雜，理論上各個關(guān)節(jié)角解的集合往往有無窮多個，不能得到有效地封閉解。但在某些充分條件下還是可以求解的：（1）三個相鄰關(guān)節(jié)軸相交于一點；（2）三個相鄰關(guān)節(jié)軸相互平行(在無限遠處交于一點)。這便是機器人運動學(xué)中的Pieper準則。Pieper方法就是在這種多解問題上進行研究，發(fā)現(xiàn)如果機器人滿足兩個充分條件中的任何一個，就會得到封閉解。Pieper方法本質(zhì)上也是一種代數(shù)解法。下面考慮六自由度轉(zhuǎn)動關(guān)節(jié)機器人最后三根軸交于一點（即充分條件1）的情況為例。3.2.6三軸相交的Pieper解法根據(jù)D-H參數(shù)坐標系建立方法，在條件1所述的情況下桿件坐標系{4}、{5}、{6}的原點將位于三根軸線的交匯點，該交匯點在機器人基座標系中的齊次坐標可表示為：

同時根據(jù)相鄰桿件坐標系之間的變換關(guān)系：

3.2.6三軸相交的Pieper解法進一步得到：

其中：3.2.6三軸相交的Pieper解法

將式子(3-47)代入(3-48)得

其中：3.2.6三軸相交的Pieper解法

3.2.6三軸相交的Pieper解法

這即是整個三軸相交的Pieper解法的過程機械臂微分運動指的是整個機械臂機構(gòu)微小運動，可以用這種微小運動來反推出機器人不同部件(例如關(guān)節(jié))之間的速度關(guān)系。在研究機械臂微分運動過程中，通過機械臂操作空間速度與關(guān)節(jié)空間速度間的線性映射關(guān)系可以得到雅可比矩陣。3.3機械臂微分運動與雅可比矩陣機械臂微分運動依據(jù)關(guān)節(jié)類型分類微分平移微分旋轉(zhuǎn)依據(jù)參考的坐標系分類基坐標系給定坐標系微分平移和微分旋轉(zhuǎn)在末端位姿變換矩陣相同的情況下，既可以用基坐標系也可以用給定的坐標系來表示。機械臂末端微分運動情形如圖3-19所示，這里只顯示末端機構(gòu)：3.3.1微分平移和微分旋轉(zhuǎn)圖3-19機械臂末端機構(gòu)微分運動示意圖基坐標系{W}下表示微分變換基坐標系{W}下的微分變換，是指相對于基座坐標系的微小平移或者旋轉(zhuǎn)運動從而導(dǎo)致機械臂末端執(zhí)行器的位姿發(fā)生變化。設(shè)末端位姿變換矩陣為T，則（左乘）3.3.1微分平移和微分旋轉(zhuǎn)

3.3.1微分平移和微分旋轉(zhuǎn)

由式子(3-54)可以推出：

3.3.1微分平移和微分旋轉(zhuǎn)

3.3.1微分平移和微分旋轉(zhuǎn)

末端坐標系{A}的微分變換把式子(3-60)代入式子(3-56)，可把微分旋轉(zhuǎn)齊次變換表示為3.3.1微分平移和微分旋轉(zhuǎn)

3.3.1微分平移和微分旋轉(zhuǎn)

3.3.1微分平移和微分旋轉(zhuǎn)

式子(3-65)在基坐標系{W}和末端坐標系{A}下均適用。微分運動的等價變換過程是指把一個坐標系內(nèi)的位置和姿態(tài)的微小變化變換為另一坐標系內(nèi)的等效表達式，這是求出某一個機械臂的雅可比矩陣的關(guān)鍵之處。3.3.2微分運動的等價變換

可得3.3.2微分運動的等價變換

所以

3.3.2微分運動的等價變換

3.3.2微分運動的等價變換

3.3.2微分運動的等價變換

即

例3.9已知變換矩陣3.3.2微分運動的等價變換設(shè)坐標系{A}的原點的速度矢量是

3.3.2微分運動的等價變換

因此

3.3.3雅可比矩陣雅可比矩陣是研究機械臂操作空間速度與關(guān)節(jié)空間速度間的線性映射關(guān)系的矩陣，可以表示從關(guān)節(jié)空間向操作空間運動速度的傳動比。設(shè)機械臂的運動方程為

式子(3-78)代表機械臂操作空間x與關(guān)節(jié)空間q之間的位移關(guān)系，且q與x是關(guān)于運動時間t的函數(shù)，因此將式(3-78)兩邊對時間t求導(dǎo)可以得出q與x之間的微分關(guān)系

3.3.3雅可比矩陣

故式子(3-81)可進一步轉(zhuǎn)化成

3.3.4微分變換法求解雅可比矩陣