第三章機器人運動分析

KinematicsofIndustrialRobot

§3—1概述

運動分析可以確定各關節、部位、機器人末端的位姿具體坐標值、速度、加速度。關節較多，桿件較多，如只在一個固定坐標系中分析困難。多坐標系，坐標轉換。

§3—2機器人末端位姿描述位置position姿態pose一、

坐標系末端坐標系Onxnynzn—末端夾持器固聯

固定坐標系（基礎坐標系）()—底座固聯二、

位姿描述1.位置描述矢量—機器人手端位置P=[Px,Py,Pz]T,

2。姿態描述（方位）手端姿態:xnynzn坐標軸在固定坐標系中的投影關系奇異點：敵機在正上方，方位自由度退化，難以瞄準，一旦出現仰俯角（而此時仰俯角位置所在方位是此前位置），而新的方位是敵機任意的，無法瞬時調節方位。

一列：xn對XYZ的方向余弦二列：

yn對XYZ的方向余弦三列：zn對XYZ的方向余弦機器人位姿矩陣：

positionandposematrix

矩陣A

與各關節運動有關，A?

§3—3坐標變換一個桿件--------一個坐標系桿件相對運動（關節運動）坐標系相對運動坐標變換一、

平移變換1、二維坐標平移變換

abyxy1x1P(x1,y1)oo1上式成立條件：當前坐標系O1從參考坐標系O重合位置向右上方移動(a,b)為了便于矩陣運算，改寫上式T—平移矩陣,左乘齊次坐標—n坐標增加一維。abyxy1x1P(x1,y1)oo12、三維坐標平移變換O1坐標以O坐標為參考,從O重合位置移開(a,b,c)T—平移矩陣,左乘二、旋轉變換1、二維坐標旋轉變換

P(x1,y1)y1yx1xααR—旋轉變換矩陣，方向余弦矩陣.R－當前坐標O1以參考坐標O為參考旋轉，左乘R左式成立條件：O1坐標系按右手定則從O坐標系重合位置旋轉α2、三維坐標旋轉變換繞三個坐標軸旋轉(相當二維旋轉)：P(x1,y1)y1x1xθθyR－當前坐標O1以參考坐標O為參考旋轉，左乘Rz1x1y1zyxR－當前坐標O1以參考坐標O為參考旋轉，左乘Rz1x1y1zyxR－當前坐標O1以參考坐標O為參考旋轉，左乘R例題：已知x1=7,y1=3,z1=2,①

求繞Z軸旋轉90°后x,y,z坐標值②

繞Z軸旋轉90°后，再繞Y軸轉90.解

(1)繞Z軸旋轉90°.sinθ=1,cosθ=0z1x1y1zyxP(7,3,2)解

(1)繞Z軸旋轉90°.sinθ=1,cosθ=0zyxx1y1z1P(-3,7,2)zyxx1y1z1P(-3,7,2)

2)

繞Z軸旋轉90°后再繞Y軸轉90.yx1y1zP(2,7,3)xz1P1(7,3,2)P(2,7,3)yx1y1P(2,7,3)xz1z注意：①②動作順序的結果是前(左)乘矩陣三、

平移與旋轉組合變換1、當前坐標系O1相對參考坐標系O變換P(x1,y1)y1x1xθθy

平移矩陣：旋轉變換矩陣：例；先繞Z軸旋轉再平移總變換矩陣：P(x1,y1)y1x1xyab動作順序的結果是左乘矩陣

平移變換矩陣：P(x1,y1)y1x1xyab例：先以O為參考平移a,b，再繞Z軸旋轉，則旋轉變換矩陣：總變換矩陣：xyP(x1,y1)y1x1θ兩種動作順序比較：兩種動作順序結果相同比較：先旋轉，后平移先平移，后繞Z1旋轉結論：繞自身軸線旋轉→右乘旋轉矩陣例題：已知點P（x1=7,y1=3,z1=2）,①

繞z1軸旋轉90°②

沿x1,y1,z1

軸移動[4,-3,7]單位③繞y1軸旋轉90°解

(1)變換矩陣A2、當前坐標系O1相對當前坐標系O1變換x1y1Z1x1y1Z1x1y1Z1x1y1Z1動作順序的結果是右乘矩陣

P（x1=7,y1=3,z1=2）,x1y1Z1平移變換矩陣：P(x1,y1)y1x1xyab例：先以O為參考平移a,b，再以當前坐標系O1為參考系繞Z1軸旋轉，則旋轉變換矩陣：3、當前坐標系O1相對當前參考坐標系O及當前坐標系O1混合變換總變換矩陣：P(x1,y1)y1x1xyab動作順序的結果是右乘矩陣

總結：（1）O1坐標系以O坐標系為參考變換，左乘矩陣（2）O1坐標系以O1坐標系為參考變換，右乘矩陣P(x1,y1)y1x1xyab例題：已知n1=1,o1=5,a1=4,①

繞x軸旋轉90°②

沿z1

軸移動3單位③繞z軸旋轉90°④沿y1

軸移動5單位解變換矩陣Ax1y1Z1§3—4機器人正向運動學forwardsolution正向-已知各關節角度，求末端執行器位姿。

----很少反向-已知末端執行器位姿，求各關節角度。

-----常見

一、

機器人關節與連桿二、

機器人坐標系的建立方法坐標系數=關節數＋10坐標系(前），n手部（后）坐標系編號方法：左邊、基礎－前右邊、手部－后1、后置法—連桿i的坐標系xiyizi

建立在后一個關節（右邊）i+1處－常用Z軸：關節軸線2、前置法—連桿i的坐標系xiyizi

建立在前邊（左邊）關節i處。

三、廣義連桿廣義連桿—相鄰兩個關節軸線處于空間任意位置。桿件長度ai—相鄰兩關節最短距離，X軸方向桿件扭角（關節軸線夾角）αi—相鄰兩關節軸線平移相交后的夾角。正負繞Xi軸旋轉右手定則。ai偏置量di—相鄰桿件長度線ai-1與ai在關節軸線上的距離，相鄰x軸間距。關節變量角θi—相鄰桿件長度ai-1與ai平移相交后的夾角，正負繞Zi軸右手定則三、

相鄰桿件運動學關系—坐標變換目的：將桿件i的位姿坐標xiyizI表現在xi-1yi-1zi-1坐標中。原理：將位姿坐標xiyizI進行坐標轉換到xi-1yi-1zi-1坐標中。方法：兩次平移，兩次旋轉。注意：旋轉角度θiαi正負符合右手定則。變換依據：設定Oi,Oi-1、最初重合－按當前坐標矩陣變換－右乘矩陣1、后置法（常用此法）變換矩陣Ai=R(zi,θi)T(0,0,di)T(αi,0,0)R(xi,α

i)設定Oi-1,Oi、最初重合－按當前坐標矩陣變換－右乘矩陣

變換矩陣Ai=R(xi,α

i-1)T(ai-1,0,0)T(0,0,di)R(zi,θi)2、前置法（略）四、

機器人運動學方程建立設用關節后置法

n個關節，n+1個坐標系。n+1個桿件（含機架）A—總變換矩陣－n向n-1轉換－1向0轉換運動學方程如下五、

運動學方程正向求解已知機器人結構參數：桿件長度ai，桿件扭角αi，偏置量di關節變量角θi求：機器人末端執行器位姿具體數值求解：【1】建立各坐標系【2】相鄰桿件坐標轉換→確定總變換矩陣A確定建立運動學方程【3】求解A,確定x0y0z0坐標下的機器人末端執行器位姿具體數值例題：PUMA560機器人運動分析—美國Unimation公司產品求解：末端執行器位姿

解：1、自由度、關節分析F=6六個關節

2、坐標數=6+1=7O1→O6放大桿件長度ai—相鄰兩Z軸距離，i桿件代號偏置量di—相鄰x軸間距已知：a2、a3、d2、d3、d6a1=0、a4=0、a5=0、a6=0、d1=0、d4=0、d5=0放大解：【1】建立各坐標系

x0y0z0x1y1z1…xnynzn—后置法【2】相鄰桿件坐標轉換

將已知數據帶入得：總變換矩陣

px,py,pz

－θ1,θ2,θ3,影響位置（末端夾持器坐標原點O6）n,o,a,－θ1,θ2,θ3

θ4,θ5,θ6,影響姿態確定建立運動學方程【3】求解A,得x0y0z0例如：圖示位置θ1=0°，θ2=0°,θ3=0°θ4=θ5=θ6=0°可見與圖完全一致。逆向運動學命題：已知xnynzn,x0y0z0已知A方陣數據未知A方陣中關節變量θ求解方陣A得各θ§3—5機器人逆向運動學inverse（converse)solution機器人運動學問題是逆向運動學問題,軌跡控制一、逆向運動學可解性

F≤6，轉動及移動關節開式鏈機器人，有數值通解。A中包含各關節變量θ關系。1、數值通解—方陣A左右對應元素相等，得多變量三角函數方程組－非線性超越方程組－無顯式解迭代計算量相當大，無法滿足機器人實時控制要求。2、解析法（代數法）—特殊情況，有解析解。利用矩陣對應元素存在零或常數項。3、作圖法。二、逆向運動學解析法（代數法）

已知數據待求的各變量θ例題：PUMA560機器人運動分析—美國Unimation公司產品（1）解θ1，θ3方程左乘：找常數項，對應項兩邊相等，解出θ1，θ3方程右邊方程左邊展開矩陣最后一列左右兩側分別：解θ1[＊]第三行式子:令:θ1具有兩個解解θ3[*][*]化簡θ3具有兩個解[＊][＊]（2）解θ2,θ4找常數項，兩邊相等，解出θ2.θ4方程右邊：方程左邊：方程左乘：四列前三行：θ2一個解可行，另一個偽根[*][*]解θ2解θ4解θ2式的三列前三行：[*][*]θ2一個解可行，另一個偽根（3）解θ5找常數項，兩邊相等，解出θ5.

方程右邊：方程左邊：方程左乘：三列前三行：θ5具有兩個解（4）解θ6找常數項，兩邊相等，解出θ6.

右邊：左邊：方程左乘：其中某行某列：θ2一個解可行，另一個偽根（5）討論θ1,θ2,θ3,－由px,py,pz,構成θ4,θ5,θ6,－由n,o,a,θ1,θ2,θ3等構成。代數法總結：（1）左乘逆陣，列方程，左右對應元素相等，可解方程，依次遞推。（2）遞推一次，可解一個或多個變量。不需全推，方程可能已經全部解出。（3）由實際判斷偽根三、逆向運動學多解問題同一位置：兩個解。原因：機構幾何關系多解。解反三角函數方程同樣證明多解性PUMA560機器人運動多解RRPR機器人去除多解方法：A)關節運動空間限制：如解θ=40°（或40+180=220°）而關節角度范圍：±100°。應選40.B)連續性：最接近上一時刻的解C)逐級剔除多余解。避免出現樹狀解結構。多解用途：實現避障要求§3—6機器人的速度分析及速度控制前面－位移問題現在－速度問題末端夾持器速度：位置速度—線速度Vx,Vy,Vz（相對固定坐標系度量）.

姿態速度—夾持器繞三個固定坐標系軸角速度ωx,ωy,ωz.一、速度分析（正向運動學）廣義坐標：移動或轉動位置：Px,Py,Pz.姿態：可用方向余弦。或用繞三個固定坐標軸角度度量表示通過運算可以求出與方向余弦之間的關系。用廣義坐標表示機器人末端夾持器運動方程:廣義坐標矩陣通式：

P-操作空間。q-關節空間。正向運動學逆向運動學

P(P1,P2,P3,P4,P5,P6)

P(P1,P2,P3,P4,P5,P6)

答案唯一多解性求導:平移速度求導:旋轉速度廣義坐標導數矩陣:雅可比矩陣廣義坐標速度矩陣通式：例題