機(jī)器人學(xué)-運(yùn)動學(xué)部分

1、齊次坐標(biāo)和齊次變換知識點(diǎn)： n點(diǎn)和面的齊次坐標(biāo)和齊次變換n三個基本旋轉(zhuǎn)矩陣n齊次變換的幾何意義n絕對變換：如果所有的變換都是相對于固定坐標(biāo)系中各坐標(biāo)軸旋轉(zhuǎn)或平移，則依次左乘，稱為絕對變換。n相對變換：如果動坐標(biāo)系相對于自身坐標(biāo)系的當(dāng)前坐標(biāo)軸旋轉(zhuǎn)或平移，則齊次變換為依次右乘，稱為相對變換。補(bǔ)充總結(jié)：合成變換n只有平移合成n繞同一個坐標(biāo)系的合成旋轉(zhuǎn)變換n繞同一個坐標(biāo)系的合成旋轉(zhuǎn)變換+平移變換n繞不同坐標(biāo)系的合成旋轉(zhuǎn)變換：左右兩側(cè)順次寫n繞不同坐標(biāo)系的合成旋轉(zhuǎn)變換+平移：左右兩側(cè)順次寫，平移放左邊平移矩陣間可交換n各變換矩陣不可交換例例1：動坐標(biāo)系：動坐標(biāo)系0起始位置與固定參考坐標(biāo)系起始位置與固定參

2、考坐標(biāo)系0重合重合,動坐標(biāo)系動坐標(biāo)系0做如下運(yùn)動：做如下運(yùn)動：R(Z,90) R（y,90） Trans(4，-3, 7)，求合成矩陣求合成矩陣 ozyx74-3owuvvuwzyxoo(o)xyzuvwzyxuwo(o) vTTrans(4, -3, 7) R(Y, 90 ) R(Z,90 )(start)例例2：先平移先平移Trans (4,-3,7)；繞當(dāng)前繞當(dāng)前 軸轉(zhuǎn)動軸轉(zhuǎn)動90； 繞當(dāng)前繞當(dāng)前 軸轉(zhuǎn)動軸轉(zhuǎn)動90；求合成旋轉(zhuǎn)矩陣。；求合成旋轉(zhuǎn)矩陣。 vw zyxo(o)vwuzyxoowuvozyxowvuxyzoowuv例題例題2 2：O O 與與O O初始重合，初始重合，O O 作

3、如下運(yùn)動：作如下運(yùn)動：繞繞X X軸轉(zhuǎn)動軸轉(zhuǎn)動9090 ；繞繞w w軸轉(zhuǎn)動軸轉(zhuǎn)動9090 ；繞繞Y Y軸轉(zhuǎn)動軸轉(zhuǎn)動9090 。求。求 T T；改變旋轉(zhuǎn)順序，如改變旋轉(zhuǎn)順序，如何旋轉(zhuǎn)才能獲得相同的結(jié)果。何旋轉(zhuǎn)才能獲得相同的結(jié)果。 x10000cos90-sin900R0sin90cos9000001cos90sin9000sin90cos900000100001wRcos900sin9000100sin900cos9000001yR1000001001000001yxwTR R R解解： 解解： 繞繞Z（w）？）？軸轉(zhuǎn)動軸轉(zhuǎn)動90； 繞繞X軸轉(zhuǎn)動軸轉(zhuǎn)動90； 繞繞Y軸轉(zhuǎn)動軸轉(zhuǎn)動90。 100000

4、1001000001yxwuvwxTR R Ryz 驗(yàn)證方法驗(yàn)證方法 ：計算加畫圖：計算加畫圖第三章 機(jī)器人運(yùn)動學(xué) n機(jī)器人運(yùn)動學(xué)主要是把機(jī)器人相對于固定參考系的運(yùn)動作為時間的函數(shù)進(jìn)行分析研究，而不考慮引起這些運(yùn)動的力和力矩n把機(jī)器人的空間位移解析地表示為時間的函數(shù)，研究機(jī)器人關(guān)節(jié)變量和機(jī)器人末端執(zhí)行器位置和姿態(tài)之間的關(guān)系3.1 機(jī)器人運(yùn)動學(xué)所討論的問題 3.1.1 研究的對象n機(jī)器人在基本機(jī)構(gòu)形式上分為兩種:關(guān)節(jié)式串聯(lián)機(jī)器人;并聯(lián)機(jī)器人. PUMA560HexapodFanuc manipulatorn這兩種機(jī)器人有所不同：串聯(lián)機(jī)器人：工作空間大，靈活；剛度差，負(fù)載小，誤差累積并放大。并聯(lián)機(jī)

5、器人：剛性好，負(fù)載大，誤差不積累；工作空間小，姿態(tài)范圍不大。本章講解以串聯(lián)機(jī)器人為主。運(yùn)動學(xué)研究的問題Where is my hand?Direct KinematicsHERE!How do I put my hand here?Inverse Kinematics: Choose these angles!研究的問題:n運(yùn)動學(xué)正問題-已知桿件幾何參數(shù)和關(guān)節(jié)角矢量，求操作機(jī)末端執(zhí)行器相對于固定參考作標(biāo)的位置和姿態(tài)（齊次變換問題）。n運(yùn)動學(xué)逆問題-已知操作機(jī)桿件的幾何參數(shù)，給定操作機(jī)末端執(zhí)行器相對于參考坐標(biāo)系的期望位置和姿態(tài)（位姿），操作機(jī)能否使其末端執(zhí)行器達(dá)到這個預(yù)期的位姿？如能達(dá)到，那么操

6、作機(jī)有幾種不同形態(tài)可以滿足同樣的條件？與前一次課的關(guān)系3.2 機(jī)器人桿件，關(guān)節(jié)和它們的參數(shù) 3.2.1 桿件，關(guān)節(jié)n操作機(jī)由一串用轉(zhuǎn)動或平移（棱柱形）關(guān)節(jié)連接的剛體（桿件）組成n機(jī)座上建立一個固定參考坐標(biāo)系，最后一個桿件與工具相連n關(guān)節(jié)和桿件均由底座向外順序排列，每個桿件最多和另外兩個桿件相聯(lián)，不構(gòu)成閉環(huán)。 關(guān)節(jié)：n一般說來，兩個桿件間是用低付相聯(lián)的n只可能有6種低付關(guān)節(jié)：旋轉(zhuǎn)（轉(zhuǎn)動）、棱柱（移動）、圓柱形、球形、螺旋和平面，其中只有旋轉(zhuǎn)和棱柱形關(guān)節(jié)是串聯(lián)機(jī)器人操作機(jī)常見的，各種低副形狀如下圖所示：旋轉(zhuǎn)旋轉(zhuǎn)棱柱形棱柱形柱形柱形球形球形螺旋形螺旋形平面平面AiAi+1Ai-1 桿件參數(shù)的定義 和

7、n li AA，由運(yùn)動學(xué)的觀點(diǎn)來看，桿件的作用在于保持其兩由運(yùn)動學(xué)的觀點(diǎn)來看，桿件的作用在于保持其兩端關(guān)節(jié)間的端關(guān)節(jié)間的結(jié)構(gòu)形態(tài)結(jié)構(gòu)形態(tài)不變。不變。由兩個參數(shù)決定，一是桿件的長度由兩個參數(shù)決定，一是桿件的長度 li，一個是桿，一個是桿件的扭轉(zhuǎn)角件的扭轉(zhuǎn)角 iAiAi+1iiliili 桿件參數(shù)的定義 和n L和L 在A軸線上的交點(diǎn)之間的距離n L和L 之間的夾角，由L 轉(zhuǎn)向L，由右手定則決定正負(fù)，對于旋轉(zhuǎn)關(guān)節(jié)它是個變量 確定確定桿件相對位置桿件相對位置關(guān)系，由另外關(guān)系，由另外2個參數(shù)決定，一個是桿個參數(shù)決定，一個是桿件的偏移量件的偏移量 ，一個是桿件的回轉(zhuǎn)角，一個是桿件的回轉(zhuǎn)角 iidiidi

8、AiAi+1iilid1iliAi-1id移動關(guān)節(jié)桿件參數(shù)的定義n確定桿件的結(jié)構(gòu)形態(tài)的2個參數(shù)Li與i與旋轉(zhuǎn)關(guān)節(jié)是一樣的。確定位置關(guān)系的參數(shù)i為常數(shù)，di為變量。n上述4個參數(shù)，就確定了桿件的結(jié)構(gòu)形態(tài)和相鄰桿件相對位置關(guān)系，在轉(zhuǎn)動關(guān)節(jié)中，Li, i, di是固定值，i是變量。在移動關(guān)節(jié)中，Li, i, i是固定值， di 是變量。n 對于每個桿件都可以在關(guān)節(jié)軸處建立一個正規(guī)的笛卡兒坐對于每個桿件都可以在關(guān)節(jié)軸處建立一個正規(guī)的笛卡兒坐標(biāo)系（標(biāo)系（xi, yi, zi），（），（i=1, 2, , n），），n是自由度數(shù)，再加是自由度數(shù)，再加上基座坐標(biāo)系，一共有（上基座坐標(biāo)系，一共有（n+1）個坐

9、標(biāo)系。）個坐標(biāo)系。n 基座坐標(biāo)系基座坐標(biāo)系 定義為定義為0號坐標(biāo)系（號坐標(biāo)系（x0, y0, z0）,它也是機(jī)它也是機(jī)器人的慣性坐標(biāo)系，器人的慣性坐標(biāo)系，0號坐標(biāo)系在基座上的位置和方向可號坐標(biāo)系在基座上的位置和方向可任選，任選，但但 軸線必須與關(guān)節(jié)軸線必須與關(guān)節(jié)1的軸線重合，位置和方向可的軸線重合，位置和方向可任選任選；n 最后一個坐標(biāo)系（最后一個坐標(biāo)系（n關(guān)節(jié)），可以設(shè)在手的任意部位，關(guān)節(jié)），可以設(shè)在手的任意部位，但但必須保證必須保證 與與 垂直垂直。o oO Oo on n- -1 1nX3.3 機(jī)器人關(guān)節(jié)坐標(biāo)系的建立n機(jī)器人關(guān)節(jié)坐標(biāo)系的建立主要是為了描述機(jī)器人各桿件和終端之間的相對運(yùn)動。

10、nD-H方法中的坐標(biāo)系建立原則如下： D-H關(guān)節(jié)坐標(biāo)系建立原則u右手坐標(biāo)系右手坐標(biāo)系u原點(diǎn)原點(diǎn)Oi：設(shè)在：設(shè)在Li與與Ai+1軸線的交點(diǎn)上軸線的交點(diǎn)上 uZi軸：軸： 與與Ai+1關(guān)節(jié)軸重合，指向任意關(guān)節(jié)軸重合，指向任意 uXi軸：軸： 與公法線與公法線Li重合，指向沿重合，指向沿Li由由Ai軸線指向軸線指向Ai+1軸線軸線 uYi軸：軸： 按右手定則按右手定則 關(guān)節(jié)坐標(biāo)系的建立原則AiAi+1iilid1iliAi-11iz1ix1iy1ioizixiyion原點(diǎn)Oi：設(shè)在Li與Ai+1軸線的交點(diǎn)上 nZi軸：與Ai+1關(guān)節(jié)軸重合，指向任意 nXi軸：與公法線Li重合，指向沿Li由Ai軸線指

11、向Ai+1軸線 nYi軸：按右手定則 沿 xi 軸， zi-1 軸與 xi 軸交點(diǎn)到 0i 的距離 繞 xi 軸，由 zi-1 轉(zhuǎn)向zi 沿 zi-1 軸，zi-1 軸和 xi 交點(diǎn)至0i 1 坐標(biāo)系原點(diǎn)的距離 繞 zi-1 軸，由 xi-1轉(zhuǎn)向 xi 兩種特殊情況n兩軸相交，怎么建立坐標(biāo)系？0iAi與Ai+1關(guān)節(jié)軸線的交點(diǎn)；ZiAi+1軸線；XiZi和Zi-1構(gòu)成的平面的法線 ；Yi右手定則； i-1ii-1i AiA Ai i+ +1 1o oi iz zi i- -1 1z zi ix xi iy yi in兩軸平行，怎么建立坐標(biāo)系(Ai與Ai+1平行)？先建立 0i-1然后建立0i+1

12、最后建立 0ii-1i-1O OD D注意：注意： 由于由于Ai和和Ai+1平行，所以公法線位置任意平行，所以公法線位置任意 目的：使目的：使di+1=0 使計算簡便使計算簡便，此時，此時di= 相鄰關(guān)節(jié)坐標(biāo)系間的齊次變換過程 機(jī)器人運(yùn)動學(xué)正解1.將xi-1軸繞 zi-1 軸轉(zhuǎn) i 角度，將其與xi軸平行；2.沿 zi-1軸平移距離 di ，使 xi-1 軸與 xi 軸重合；3.沿 xi 軸平移距離 Li，使兩坐標(biāo)系原點(diǎn)及x軸重合；4.繞 xi 軸轉(zhuǎn) i 角度，兩坐標(biāo)系完全重合AiAi+1iilid1iliAi-11iz1ix1iy1ioizixiy io111A(,)(,)( , ) ( ,

13、)iiiiransiiransiiiiR ZTZd Tx l R x右乘 根據(jù)上述坐標(biāo)系建立原則，用下列旋轉(zhuǎn)和位移我們根據(jù)上述坐標(biāo)系建立原則，用下列旋轉(zhuǎn)和位移我們可以建立相鄰的可以建立相鄰的 Oi-1 和和 Oi 坐標(biāo)系之間的關(guān)系坐標(biāo)系之間的關(guān)系 機(jī)器人的運(yùn)動學(xué)正解方程001112iiiTAAA D-H變換矩陣變換矩陣iiA1100010000100001id1000010000cossin00sincosiiii100001000010001il10000cossin00sincos00001iiii1000cossin0sincossincoscossincossinsinsincosco

14、siiiiiiiiiiiiiiiiidll=例題：二連桿機(jī)械臂運(yùn)動分析1l2l各變量111 1111 110000100001csc lscs lAil1l2l222 2222 220000100001csc lscs lA3.9 PUMA 560圖 機(jī)器人的連桿坐標(biāo)系( )a 結(jié)構(gòu)圖( )b 坐標(biāo)圖n根據(jù)齊次變換方法和表3.1所示連桿參數(shù)，可求得各連桿變換矩陣如下： 100001000000111110csscT100000100002222221csdscT100001000003323332csascT100000100044434443csdascT n各連桿變換矩陣相乘，得PUMA

15、560的機(jī)械手變換的T 矩陣： 即為關(guān)節(jié)變量 的函數(shù)。 該矩陣描述了末端連桿坐標(biāo)系6相對基坐標(biāo)系0的位姿。100000010000555554csscT100000010000666665csscT)()()()()()(66555444333222111060TTTTTTT 621,(3.59)n于是，可求得機(jī)械手的T 變換矩陣：n 其中nx, ny, nz, ox, oy, oz, ax, ay, az, px, py, pz見式（3.64）1000611060zzzzyyyyxxxxpaonpaonpaonTTT（3.64）1234 5 64 623 5 614 5 64 61234 5

16、 64 623 5 614 5 64 6234 5 64 623 5 61234 5 64 623 5 614 64 5 61234 5 64 623 5()(),()(),();()(),()xyzxync cc c cs ss s cs s c cc sns cc c cs ss s cc s c cc snsc c cs sc s coc cc c ss cs s ss c cs c sos cc c ss cs s s 614 64 5 6234 5 64 623 5 6123 4 523 51 4 5123 4 523 51 4 523 4 523 51223 234 232 112

17、23 234 232 13 232 242(),(),(),(),;,zxyzxyzc c cs c cosc c ss cc s sac c c ss cs s sas c c ss cc s sas c sc cpc a ca cd sd sps a ca cd sd cpa sa sd c 3.: 已知關(guān)節(jié)角度或位移，計算末端操作手的對應(yīng)位姿.: 已知末端操作手的位姿，求解對應(yīng)的關(guān)節(jié)變量.可能存在多解或無解通常需多次求解非線性超越方程3.6 運(yùn)動學(xué)逆問題 解的存在性n目標(biāo)點(diǎn)應(yīng)位于工作空間內(nèi)n可能存在多解，如何選擇最合適的解？存在雙解存在雙解! 求解方法n如果各關(guān)節(jié)可用某算法獲得，一個機(jī)械

18、手是有解的. 算法應(yīng)包含所有可能解.封閉形式解（解析解）封閉形式解（解析解）數(shù)值解數(shù)值解n方法方法n我們對封閉形式的解法更感興趣我們對封閉形式的解法更感興趣 代數(shù)方法代數(shù)方法 幾何方法幾何方法n 可解性的重要結(jié)論是：可解性的重要結(jié)論是： 所有具有轉(zhuǎn)動和移動關(guān)節(jié)的系統(tǒng)，在一個單一串聯(lián)中所有具有轉(zhuǎn)動和移動關(guān)節(jié)的系統(tǒng)，在一個單一串聯(lián)中總共有總共有6 6個（或小于個（或小于6 6個）自由度時，是可解的，其通個）自由度時，是可解的，其通解一般是數(shù)值解，它不是解析表達(dá)式，而是利用數(shù)值解一般是數(shù)值解，它不是解析表達(dá)式，而是利用數(shù)值迭代原理求解，它的計算量要比解析解大。迭代原理求解，它的計算量要比解析解大。

19、但在某些特殊情況下，如若干個關(guān)節(jié)軸線相交和或多但在某些特殊情況下，如若干個關(guān)節(jié)軸線相交和或多個關(guān)節(jié)軸線等于個關(guān)節(jié)軸線等于 0 0 或或 9090的情況下，具有的情況下，具有6 6個自由度個自由度的機(jī)器人可得到解析解。的機(jī)器人可得到解析解。 為使機(jī)器人有解析解，一般設(shè)計時，使工業(yè)機(jī)器人足為使機(jī)器人有解析解，一般設(shè)計時，使工業(yè)機(jī)器人足夠簡單，盡量滿足這些特殊條件。夠簡單，盡量滿足這些特殊條件。n 對于給定的機(jī)器人，能否求得它的運(yùn)動學(xué)逆解的解析式對于給定的機(jī)器人，能否求得它的運(yùn)動學(xué)逆解的解析式（也叫封閉解）。（也叫封閉解）。0140i000222018040i100040i 2 2選擇一個與前一采樣

20、時間最接近的解，例如：選擇一個與前一采樣時間最接近的解，例如：0140i000222018040i 若該關(guān)節(jié)運(yùn)動空間為若該關(guān)節(jié)運(yùn)動空間為 ，且，且 ，則應(yīng)選，則應(yīng)選 25001160i0220i3 3根據(jù)避障要求，選擇合適的解根據(jù)避障要求，選擇合適的解4 4逐級剔除多余解逐級剔除多余解 對于具有對于具有n n個關(guān)節(jié)的機(jī)器人，其全部解將構(gòu)成樹形結(jié)構(gòu)。個關(guān)節(jié)的機(jī)器人，其全部解將構(gòu)成樹形結(jié)構(gòu)。為簡化起見，應(yīng)逐級剔除多余解。這樣可以避免在樹形解中為簡化起見，應(yīng)逐級剔除多余解。這樣可以避免在樹形解中選擇合適的解。選擇合適的解。 n迭代法計算量大n幾何法適用于自由度較少的情況n反變換法 幾何解法（適用于自

21、由度較少時）例：二自由度機(jī)械臂，已知(x,y)坐標(biāo)求關(guān)節(jié)轉(zhuǎn)角原則: 將原始空間幾何問題轉(zhuǎn)化為若干個平面幾何問題應(yīng)用 “余弦定理”:x2+y2=l12+l22 2l1l2cos(1802)則有:22221221 2cos2xylll l再次利用余弦定理得到: cos = (x2+y2+l12 - l22 )/2l1 (x2+y2) 在 0 180范圍內(nèi)求解，最后利用 1=轉(zhuǎn)換為多項(xiàng)式n二、二、 逆向運(yùn)動學(xué)解析法逆向運(yùn)動學(xué)解析法（代數(shù)法）（代數(shù)法）n n 1000zzzzyyyyxxxxpaonpaonpaonA654321AAAAAA00011uvwxxyyAzz已知數(shù)據(jù)已知數(shù)據(jù)待求的各變量待求

22、的各變量公式A數(shù)據(jù)An用未知的逆變換逐次左乘，由乘得的矩陣方程的元素決定未知數(shù)，即用逆變換把一個未知數(shù)由矩陣方程的右邊移到左邊n考察方程式左、右兩端對應(yīng)元素相等，以產(chǎn)生一個有效方程式，理論上可得到12個方程。n然后求這個三角函數(shù)方程式，以求解未知數(shù) n把下一個未知數(shù)移到左邊n重復(fù)上述過程，直到解出所有解缺點(diǎn)：無法由數(shù)種可能的解中直接得出合適的解， 需要通過人為的選擇Paul 等人提出的方法(1981年，也叫求逆的方法，是解析解): Paul 等人提出的方法00123456123456AA AA AA A 655443322160-110AAAAATA）（1 6554433260-110-121

23、AAAATAA）（）（26560-110121132143154)()()()(ATAAAAA-）（5 ETAA60-110-165) ( ）（6 060001nxsxaxpxnysyaypyAnzszazpz例：二自由度機(jī)械臂逆向運(yùn)動學(xué)n不論關(guān)節(jié)變量有多少，最多只能得到12個有效等式。1l2l120001xxxxyyyyzzzznoapnoapAA Anoap111 1111 110000100001csc lscs lA222 2222 220000100001csc lscs lA111222 23311222 200001000010001xyXxyzc ps plcsc lRs pc

24、 pscs lp111111100000100001cslscA222221200000100001cslscA111222 211222 211111220000000100010000100010001xxxxyyyyzzzzcslnoapcsc lscnoapscs lA AA A AAnoapn左右平方相加1112 2112 2(1)xyxyc ps plc ls pc ps l222111112()()xyxyc ps pls pc pl n求解得1n代入（1）求22221112211111121211200000000100010000100010001xxxxyyyyzzzzcs

25、lcslnoapscscnoapA A AA A A AEnoap121 21 2122121 212121 21 21121010000010000100010000100010001xxxxyyyyzzzzc cs sc ss cll cnoapc ss cc cs sl snoapA A Anoap例2： PUMA 560求解可把PUMA 560的運(yùn)動方程（3.64）寫為：若末端連桿的位姿已經(jīng)給定，即 為已知，則求關(guān)節(jié)變量 的值稱為運(yùn)動反解運(yùn)動反解。用未知的連桿逆變換左乘方程(3.65)兩邊，把關(guān)節(jié)變量分離出來，從而求得 的解。 )()()()()()(10006655544433322

26、2111060TTTTTTpaonpaonpaonTzzzzyyyyxxxx(3.65)pao,n,和621,621,n1.求 用逆變換 左乘式(3.65)兩邊：1)()()()()()(100066555444333222111060TTTTTTpaonpaonpaonTzzzzyyyyxxxx(3.65)010123451162233445566()()()()()TTTTTTT0111T1111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoapn1.求 10

27、10123451162233445566()()()()()TTTTTTT1111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoap1112xyys pc ppd利用三角代換：cos ;sinxypp其中22;atan2,xyyxpppp(3.67)兩邊(2,4)項(xiàng)元素對應(yīng)相等：(3.68)n求 式中，正、負(fù)號對應(yīng)于 的兩個可能解。 212122221222122sin()/;cos()1(/)atan2,1atan2(,)atan2(,)yxxyddddppdp

28、pd 1(3.70)11112xyys pc ppdcos ;sinxypp22;atan2,xyyxppppn2. 求 31111111111111611110 00 0001 00001000 10001xxxxxxxxyyyyyyyyzzzzzzzzcsnoapnoapscnoapnoapTnoapnoap(3.67)1113 234 232213 234232 2xyxzzc ps ppa cd sa cppa sd ca s 兩邊(1,4)項(xiàng)和(3,4)項(xiàng)元素對應(yīng)相等：3 34 3a cd sk其中2222222232422xyzpppaaddkan求 1),(2atan),(2at

29、an22423433kdakda3(3.73)正、負(fù)號對應(yīng) 的兩種可能解。3 34 3a cd sk1112222122atan2(,)atan2(,)xyyyxxys pc ppdppdppdn求 3 式中，2222222232422xyzpppaaddkan3.求 201034531236445566,()()()TTTTT 1 231 23232 31 231 23232 336112000010001xxxxyyyyzzzzc cs csa cnoapc ss sca snoapTnoapscd1 231 23232 331 231 23232 34xyzxyzc c ps c ps

30、pa cac s ps s pc pa sd(3.75)兩邊(1,4)項(xiàng)和(2,4)項(xiàng)元素對應(yīng)相等：n求根據(jù) 解的四種可能組合可以得到相應(yīng)的四種可能值 ，于是可得到 的四種可能解：式中， 取與 相對應(yīng)的值。231和2323232(3.78)2332 3112 3423221142 3112 33232211232332 3112 3442 3112 33atan2,zxyzxyzxyzxyzxyzxyaa cpc ps pa sdspc ps pda cpc ps pa cacpc ps paa cpc ps pa sdda cpc ps pa ca(3.77)n4.求 40103453123

31、6445566,()()()TTTTT 1 231 23232 31 231 23232 336112000010001xxxxyyyyzzzzc cs csa cnoapc ss sca snoapTnoapscd1 231 23234 5114 5xyzxyc c as c as ac ss ac as s (3.75)兩邊(1,3)項(xiàng)和(3,3)項(xiàng)元素對應(yīng)相等：4111 231 2323atan2(,)xyxyza sa ca c ca s ca s(3.79)n5.求 5010454123465566,()()TTTT 1 23 41 41 23 41 423 42 3 42 43 4

32、1 23 41 41 23 41 423 42 3 4243 401412341 231 23232 34,0001c c cs ss c cc ss ca c cd sa cc c ss cs c sc cs sa c sd ca sTc ss sca sd 1 23 41 41 23 41 423 451 231 23235xyzxyzac c cs sas c cc sas csac sas sacc 兩邊(1,3)項(xiàng)和(3,3)項(xiàng)元素對應(yīng)相等：555atan2( ,)s c(3.84)n6.求 60105512345666,()TTT 1 23 41 41 23 41 423 461

33、23 41 451 23 51 23 41 451 23 523 4 523 56xyzxyznc c ss cns c sc cns ssnc c cs scc s sns c cc scs s sns c cc sc兩邊(3,1)項(xiàng)和(1,1)項(xiàng)元素對應(yīng)相等：666atan2(,)s c(3.84)nPUMA560的運(yùn)動反解可能存在8種解。但是，由于結(jié)構(gòu)的限制，例如各關(guān)節(jié)變量不能在全部360范圍內(nèi)運(yùn)動，有些解不能實(shí)現(xiàn)。n在機(jī)器人存在多種解的情況下，應(yīng)選取其中最滿意的一組解，以滿足機(jī)器人的工作要求。Four solutions of the PUMA560n代數(shù)法總結(jié)：代數(shù)法總結(jié)：n（1）左

34、乘逆陣，列方程，左右對應(yīng)元素相等，）左乘逆陣，列方程，左右對應(yīng)元素相等，可解方程，依次遞推?？山夥匠?，依次遞推。n（2）遞推一次，可解一個或多個變量。不需）遞推一次，可解一個或多個變量。不需全推，方程可能已經(jīng)全部解出。全推，方程可能已經(jīng)全部解出。n（3）由實(shí)際判斷偽根）由實(shí)際判斷偽根n3.7 機(jī)器人的速度分析機(jī)器人的速度分析n前面位移問題前面位移問題n現(xiàn)在速度問題現(xiàn)在速度問題n末端夾持器速度：末端夾持器速度：n線速度線速度 Vx,Vy,Vz（相對固定坐標(biāo)系度量）（相對固定坐標(biāo)系度量）.n姿態(tài)速度姿態(tài)速度夾持器繞三個固定坐標(biāo)系軸角速度夾持器繞三個固定坐標(biāo)系軸角速度n x, y, z.n一、速度分

35、析（正向運(yùn)動學(xué)）一、速度分析（正向運(yùn)動學(xué)）n廣義坐標(biāo)：廣義坐標(biāo)： n 移動或轉(zhuǎn)動移動或轉(zhuǎn)動n位置：位置： Px , Py ,Pz .n姿態(tài)：可用方向余弦?；蛴美@三個固定坐標(biāo)軸姿態(tài)：可用方向余弦?；蛴美@三個固定坐標(biāo)軸角度度量表示角度度量表示,PPPnqqq2, 1n用廣義坐標(biāo)表示機(jī)器人末端夾持器運(yùn)動方程用廣義坐標(biāo)表示機(jī)器人末端夾持器運(yùn)動方程:)(2, 1nxxqqqPP)(2, 1nyyqqqPP)(2, 1nzzqqqPP)(2, 1nqqqPP)(2, 1nqqqPP)(2, 1nqqqPPn廣義坐標(biāo)矩陣通式：廣義坐標(biāo)矩陣通式：n 1PPx4PP 2PPy3PPz6PP 5PP 654321

36、PPPPPPPnP- 操作空間。操作空間。 nq- 關(guān)節(jié)空間。關(guān)節(jié)空間。n正向運(yùn)動學(xué)正向運(yùn)動學(xué)n逆向運(yùn)動學(xué)逆向運(yùn)動學(xué)n )(2, 1nqqqPPP(P1,P2,P3,P4,P5,P6) )6, 5, 4, 3, 2, 1(qqqqqqqP(P1,P2,P3,P4,P5,P6) )6, 5, 4, 3, 2, 1(qqqqqqq答案唯一多解性 n求導(dǎo)求導(dǎo): 平移速度平移速度1112112nxnP dqP dqP dqVq dtqdtqdt2122212nynP dqP dqP dqVqdtqdtqdt3132312nznP dqP dqP dqVq dtqdtqdtn求導(dǎo)求導(dǎo): 旋轉(zhuǎn)速度旋轉(zhuǎn)速度

37、dtdqqPdtdqqPx224114dtdqqPdtdqqPy225115dtdqqPdtdqqPz22611611211133313461656/xnyznxynzndpdtdqdtdpVpqpqdtVdpdqVdpdtdtpqpqdpdtdtpqpqdpdtdqdtdpdt dtdqqpdtdp廣義坐標(biāo)速度矩陣通式：廣義坐標(biāo)速度矩陣通式：n廣義坐標(biāo)導(dǎo)數(shù)矩陣廣義坐標(biāo)導(dǎo)數(shù)矩陣:n雅可比矩陣雅可比矩陣 111616/nnpqpqpJqpqpq例題例題 二自由度平面關(guān)節(jié)機(jī)器人，手部沿固定坐標(biāo)二自由度平面關(guān)節(jié)機(jī)器人，手部沿固定坐標(biāo)系系X0軸正方向移動速度軸正方向移動速度1m/s。兩桿長。兩桿長0.5m，求瞬，求瞬時關(guān)節(jié)速度？時關(guān)節(jié)速度？301602解：)cos(cos21211llX)(insin21211sllY1212( ,),( ,)XXYY 2211dYdYdY2211dXdXdX212121ddYYXXdYdXJdqdP 1221221112212211clclclslslslJ現(xiàn)在已知末端速度求關(guān)節(jié)速度：n需先求1