雅可比矩陣和動力學分析

上一章討論了剛體的位姿描述、齊次變換，機器人各連桿間的位移關(guān)系，建立了機器人的運動學方程，研究了運動學逆解，建立了操作空間與關(guān)節(jié)空間的映射關(guān)系。本章將在位移分析的基礎上，進行速度分析，研究操作空間速度與關(guān)節(jié)空間速度之間的線性映射關(guān)系——雅可比矩陣(簡稱雅可比)。雅可比矩陣不僅用來表示操作空間與關(guān)節(jié)空間之間的速度線性映射關(guān)系，同時也用來表示兩空間之間力的傳遞關(guān)系。第1頁/共94頁第一頁，共95頁。3.1機器人速度雅可比與速度分析一、機器人速度雅可比可寫成：Y＝F(X)將其微分，得：第2頁/共94頁第二頁，共95頁。也可簡寫成：雅可比矩陣用J表示第3頁/共94頁第三頁，共95頁。二自由度平面關(guān)節(jié)型機器人端點位置X、Y與關(guān)節(jié)θ1、θ2的關(guān)系為即 微分得 第4頁/共94頁第四頁，共95頁。寫成矩陣形式為

令簡寫為：dX=Jdθ關(guān)節(jié)空間微小運動dθ與手部作業(yè)空間微小位移dX的關(guān)系。第5頁/共94頁第五頁，共95頁。2R機器人的速度雅可比矩陣為：已知關(guān)節(jié)θ和角速度,可求出該機器人手部速度。若J1，J2分別為雅可比的第1列矢量和第2列矢量，則:

右邊第一項表示僅由第一個關(guān)節(jié)運動引起的端點速度；右邊第二項表示僅由第二個關(guān)節(jié)運動引起的端點速度；總的端點速度為這兩個速度矢量的合成。因此，機器人速度雅可比的每一列表示其他關(guān)節(jié)不動而某一關(guān)節(jié)運動產(chǎn)生的端點速度。

dX=Jdθ第6頁/共94頁第六頁，共95頁。n自由度機器人J

陣關(guān)節(jié)變量用廣義關(guān)節(jié)變量q表示：q=[q1,q2,

…,qn]T當關(guān)節(jié)為轉(zhuǎn)動關(guān)節(jié)時qi=θi；當關(guān)節(jié)為移動關(guān)節(jié)時qi=di關(guān)節(jié)空間的微小運動：dq=[dq1，dq2,

…

,dqn]T機器人末端在操作空間的位姿X表示，它是關(guān)節(jié)變量的函數(shù)，X=X(q)，是一個6維列矢量。第7頁/共94頁第七頁，共95頁。J（q）：反映了關(guān)節(jié)空間微小運動dq與手部作業(yè)空間微小運動dX之間的關(guān)系。J(q)dX=J(q)dqdX=[dX，dY，dZ，φX，φY，φZ]T反映了操作空間的微小運動，由機器人末端微小線位移和微小角位移(微小轉(zhuǎn)動)組成。第8頁/共94頁第八頁，共95頁。二、機器人速度分析對dX=Jdθ兩邊各除以dt得或表示為 式中：v為機器人末端在操作空間中的廣義速度；為機器人關(guān)節(jié)在關(guān)節(jié)空間中的關(guān)節(jié)速度；與操作空間速度v之間關(guān)系的雅可比矩陣。J(q)為確定關(guān)節(jié)空間速度第9頁/共94頁第九頁，共95頁。反之，假如給定工業(yè)機器人手部速度，可解出相應的關(guān)節(jié)速度，即：式中：J-1稱為工業(yè)機器人逆速度雅可比。當工業(yè)機器人手部在空間按規(guī)定的速度進行作業(yè)，用上式可以計算出沿路徑上每一瞬時相應的關(guān)節(jié)速度。第10頁/共94頁第十頁，共95頁。例1如圖示的二自由度機械手，手部沿固定坐標系X0軸正向以1.0m/s的速度移動，桿長l1=l2=0.5m。求當θ1=30°，θ2=60°時的關(guān)節(jié)速度。解由推導知，二自由度機械手速度雅可比為

二自由度機械手手爪沿X0方向運動示意圖第11頁/共94頁第十一頁，共95頁。逆雅可比為 且vX=1m/s，vY=0，因此 第12頁/共94頁第十二頁，共95頁。在兩關(guān)節(jié)的位置分別為θ1=30°,θ2=

–60°速度分別為，手部瞬時速度為1m/s。第13頁/共94頁第十三頁，共95頁。三、雅可比矩陣的奇異性由此可見，當雅可比矩陣的行列式為0時，要使手爪運動，關(guān)節(jié)速度將趨于無窮大。當雅可比不是滿秩矩陣時，J的行列式為0。則若——J矩陣的伴隨陣第14頁/共94頁第十四頁，共95頁。當雅可比不是滿秩矩陣時，可能出現(xiàn)奇異解，機器人的奇異形位，相應操作空間的點為奇異點。機器人的奇異形位分為兩類：(1)邊界奇異形位：當機器人臂全部伸展開或全部折回時，手部處于機器人工作空間的邊界上或邊界附近，逆雅可比奇異。相應的機器人形位叫做邊界奇異形位。(2)內(nèi)部奇異形位：兩個或兩個以上關(guān)節(jié)軸線重合時，機器人各關(guān)節(jié)運動相互抵消，不產(chǎn)生操作運動。相應的機器人形位叫做內(nèi)部奇異形位。當機器人處在奇異形位時會產(chǎn)生退化現(xiàn)象，喪失一個或更多的自由度。這意味著在工作空間的某個方向上，不管怎樣選擇機器人關(guān)節(jié)速度，手部也不可能實現(xiàn)移動。第15頁/共94頁第十五頁，共95頁。當l1l2s2＝0時無解，機器人逆速度雅可比J-1奇異。因l10，l20，所以，在2＝0或2＝180時，機器人處于奇異形位。機器人二臂完全伸直，或完全折回，兩桿重合。在奇異形位下，手部正好處在工作域的邊界上，該瞬時手部只能沿著一個方向(與臂垂直的方向)運動，退化了一個自由度。如果希望機器人手部在空間按規(guī)定的速度進行作業(yè)，雅可比是滿秩矩陣，可以計算出沿路徑每一瞬時相應的關(guān)節(jié)速度。第16頁/共94頁第十六頁，共95頁。對空間機器人，J的行數(shù)為6。二維平面機器人，J的行數(shù)為3，列數(shù)則為機械手含有的關(guān)節(jié)數(shù)目。平面運動機器人手的廣義位置向量[x,y,φ]T容易確定，且方位φ與角運動的形成順序無關(guān)，可直接采用微分法求J

。對于空間機器人,根據(jù)機器人運動學方程，可以獲得直角坐標位置向量[x,y,z]T的顯式方程，但找不到方位向量的一般表達式?？臻g機器人雅可比矩陣J確定：不能用直接微分法，采用構(gòu)造法。第17頁/共94頁第十七頁，共95頁。機器人關(guān)節(jié)速度向量定義為：手爪在基系中的廣義速度向量為：

四、雅可比矩陣的構(gòu)造法n個關(guān)節(jié)機器人，J是6×n矩陣。第18頁/共94頁第十八頁，共95頁。前三行稱為位置雅可比矩陣，代表對手爪線速度V的傳遞比；后三行稱為方位矩陣，代表相應的關(guān)節(jié)速度對手爪角速度ω的傳遞比。將J分塊為：第19頁/共94頁第十九頁，共95頁。矢量積法構(gòu)造雅可比矩陣對于移動關(guān)節(jié)i:對于轉(zhuǎn)動關(guān)節(jié)i:zi是i坐標系z軸單位矢量在基系中的表示。手爪坐標原點在i系的位置矢量手爪坐標原點的位置矢量在基系的表示第20頁/共94頁第二十頁，共95頁。第21頁/共94頁第二十一頁，共95頁。矢量運算第22頁/共94頁第二十二頁，共95頁。已知關(guān)節(jié)速度，求末桿速度PUMA，關(guān)節(jié)速度：1、廣義關(guān)節(jié)速度PUMA末桿速度第23頁/共94頁第二十三頁，共95頁。①②③④⑤⑥ⅠⅡⅢⅣⅤⅥz1z2z5z4z6z3o2o5o6o1o3o4x1x2x4x5x6x3z0x0o02、末桿速度的定義：沿末桿坐標軸的速度矢量繞末桿坐標軸的角速度矢量第24頁/共94頁第二十四頁，共95頁。雅可比矩陣3、計算公式：第25頁/共94頁第二十五頁，共95頁。4、計算原理：速度疊加原理關(guān)節(jié)1的速度對末桿速度的影響系數(shù)，傳動比關(guān)節(jié)速度對x的影響系數(shù)，傳動比第26頁/共94頁第二十六頁，共95頁。二、基本公式①②③④⑤⑥ⅠⅡⅢⅣⅤⅥz1z2z5z4z6z3o2o5o6o1o3o4x1x2x4x5x6x3z0x0o0已知已知求解第27頁/共94頁第二十七頁，共95頁。1、原始公式（不推導）第28頁/共94頁第二十八頁，共95頁。(1)轉(zhuǎn)動關(guān)節(jié)i：系i只繞zi軸以角速度轉(zhuǎn)動第29頁/共94頁第二十九頁，共95頁。第30頁/共94頁第三十頁，共95頁。（2）移動關(guān)節(jié)i：系i只沿zi軸以速度移動第31頁/共94頁第三十一頁，共95頁。第32頁/共94頁第三十二頁，共95頁。中的元素中的元素第33頁/共94頁第三十三頁，共95頁。前置坐標系第34頁/共94頁第三十四頁，共95頁。

T6=A1A2A3A4A5A6

A1-1T6=T16(T16=A2

A3A4A5A6)A2-1A1-1T6=T26(T26=A3A4A5A6)A3-1A2-1A1-1T6=T36(T36=A4A5A6)A4-1A3-1A2-1A1-1T6=T46(T46=A5A6

)A5-1

A4-1A3-1A2-1A1-1T6=T56(T56=A6)第35頁/共94頁第三十五頁，共95頁。PUMA560雅可比各列的計算實例第36頁/共94頁第三十六頁，共95頁。nx=c23（c4c5c6s4s6）s23s5c6ny=s4c5c6c4s6

nz=s23[c4c5c6s4s6]c23s5c6ox=c23[c4c5c6+s4s6]+s23s5c6oy=s4c5c6c4s6

oz=s23[c4c5c6+s6s6]+c23s5s6ax=c23c5s5s23c5ay=s4s5az=s23c4s5–c23c5px=a2c2+a3c23

d4s23py=d3pz=

a3c23

a2s2

d4s23J11=(a2c2+a3c23

d4s23)(s4c5c6c4s6)－d3[c23(c4c5c6s4s6)s23s5c6]第37頁/共94頁第三十七頁，共95頁。例2如圖示的二自由度機械手，手部沿固定坐標系X0軸正向以1.0m/s的速度移動，桿長l1=l2=0.5m。求：1）當θ1=30°，θ2=60°時的機械手位姿。

2）機械手J

3)當θ1=30°，θ2=60°時關(guān)節(jié)速度x2Y2x3Y3解：1）D-H坐標系建立

2）確定各連桿的D-H參數(shù)和關(guān)節(jié)變量連桿變量αad123x1Y1第38頁/共94頁第三十八頁，共95頁。3）求兩桿之間的位姿矩陣Ai連桿變量αad1θ1l1002θ2l20030000x2Y2x3Y3x1Y1第39頁/共94頁第三十九頁，共95頁。x2Y2x3Y3x1Y1第40頁/共94頁第四十頁，共95頁。x2Y2x3Y3x1Y14）當θ1=30°，θ2=60°時末桿的位姿5）若給定機械手位姿，求逆解第41頁/共94頁第四十一頁，共95頁。已知機械手末端桿的位姿：求：θ1θ2第42頁/共94頁第四十二頁，共95頁。6）求J=?求J1第43頁/共94頁第四十三頁，共95頁。x2Y2x3Y3x1Y1求J2第44頁/共94頁第四十四頁，共95頁。2R平面機器人坐標系如圖所示。A陣和T矩陣分別為：x1Y1x2Y2后置坐標系第45頁/共94頁第四十五頁，共95頁。求J1后置坐標系第46頁/共94頁第四十六頁，共95頁。求J2后置坐標系第47頁/共94頁第四十七頁，共95頁。

3.2

機器人靜力分析機器人在作業(yè)過程中，各關(guān)節(jié)產(chǎn)生相應的作用力。關(guān)節(jié)力由機器人各關(guān)節(jié)的驅(qū)動裝置提供，通過連桿傳遞到手部，克服外界作用力。本節(jié)討論操作臂在靜力平衡關(guān)系。兩類靜力學問題：(1)已知機器人手部作用力F

，求關(guān)節(jié)驅(qū)動力矩

。（滿足靜力學平衡條件）(2)已知關(guān)節(jié)驅(qū)動力矩，確定機器人手部的作用力F或負荷的質(zhì)量。第48頁/共94頁第四十八頁，共95頁。定義：機器人末端力矢量：力f和力矩n，記做：在靜止狀態(tài)下，F(xiàn)

應與各關(guān)節(jié)的驅(qū)動力或力矩平衡。關(guān)節(jié)力矢量：n個關(guān)節(jié)的驅(qū)動力矩組成n維矢量：假定關(guān)節(jié)無摩擦，忽略各桿件的重力，廣義關(guān)節(jié)力矩與機器人末端力F的關(guān)系為：力雅可比矩陣力雅可比JT是工業(yè)機器人速度雅可比J的轉(zhuǎn)置。第49頁/共94頁第四十九頁，共95頁。利用虛功原理證明。設各個關(guān)節(jié)的虛位移為qi，手部的虛位移為X。手部及各關(guān)節(jié)的虛位移X0Y0O0iqi-nn，n+1-fn，n+1d第50頁/共94頁第五十頁，共95頁。d＝[dx

dy

dz]T，＝[xyz]T機器人關(guān)節(jié)虛位移矢量（關(guān)節(jié)空間）：q＝[q1，q2…qn]T機器人手部的虛位移和虛角位移（作業(yè)空間）第51頁/共94頁第五十一頁，共95頁。

設各關(guān)節(jié)力矩為i(i＝1，2，…，n)機器人手部的作用力和力矩為-fn，n+1和-nn，n+1根據(jù)虛位移原理，各關(guān)節(jié)所作的虛功之和與末端執(zhí)行器所作的虛功相等。即：1q1+2q2+…+nqn=

fn，n+1d+

nn，n+1簡寫成：Tq

＝

F

TX

第52頁/共94頁第五十二頁，共95頁。虛位移q和X符合桿件的幾何約束條件。

有：

X＝Jdq，代入：Tq

＝

F

TX有：＝JTF

JT稱為機械手的力雅可比。表示在靜態(tài)平衡狀態(tài)下，操作力向關(guān)節(jié)力映射的線性關(guān)系。

第53頁/共94頁第五十三頁，共95頁。Y01FFxFy1=0X02=90l1l22(b)X011l122l2F=[Fx，F(xiàn)y]T(a)Y0例3圖示為二自由度平面關(guān)節(jié)型機械手，已知手部端點力F＝[Fx，F(xiàn)y]T，若關(guān)節(jié)無摩擦力存在，求力F的等效關(guān)節(jié)力矩。另求當1＝0，2＝90時的等效關(guān)節(jié)力矩。第54頁/共94頁第五十四頁，共95頁。解：由前面推導知，該機械手的速度雅可比為：則該機械手的力雅可比為：第55頁/共94頁第五十五頁，共95頁。根據(jù)＝JTF，得：1=-[l1sin1+l2sin(1+2)]Fx

+[l1cos1+l2cos(1+2)]Fy2=-l2sin(1+2)Fx+l2cos(1+2)Fy當1＝0，2＝901=-l2Fx+l1Fy，2=-l2Fx第56頁/共94頁第五十六頁，共95頁。機器人動力學研究各桿件的運動和作用力之間的關(guān)系，是機器人設計、運動仿真和動態(tài)實時控制的基礎。機器人動力學問題有兩類：動力學正問題——已知關(guān)節(jié)的驅(qū)動力矩，求機器人系統(tǒng)相應的運動參數(shù)（包括關(guān)節(jié)位移、速度和加速度）。動力學逆問題——已知運動軌跡點上的關(guān)節(jié)位移、速度和加速度，求出所需要的關(guān)節(jié)力矩。3.3機器人動力學分析第57頁/共94頁第五十七頁，共95頁。機器人是由多個連桿和多個關(guān)節(jié)組成的復雜的動力學系統(tǒng)，具有多個輸入和多個輸出，存在著嚴重的非線性和耦合關(guān)系。采用方法：拉格朗日(Lagrange)方法牛頓—歐拉方法(Newton-Euler)方法高斯(Gauss)方法凱恩(Kane)方法等。第58頁/共94頁第五十八頁，共95頁。拉格朗日方法以簡單的形式求得系統(tǒng)動力學方程，而且具有顯式結(jié)構(gòu)，物理意義比較明確，對理解機器人動力學比較方便。因此，本節(jié)只介紹拉格朗日方法，并結(jié)合簡單實例進行分析。機器人動力學問題的求解比較困難，而且需要較長的運算時間。因此，簡化求解的過程，最大限度地減少機器人動力學在線計算的時間是持續(xù)研究的課題。第59頁/共94頁第五十九頁，共95頁。一、拉格朗日方程1.拉格朗日函數(shù)定義：機械系統(tǒng)的動能Ek和勢能Eq之差，即：

L＝Ek-Eq

令qi廣義關(guān)節(jié)變量，是廣義關(guān)節(jié)速度。系統(tǒng)動能Ek是qi和的函數(shù)，系統(tǒng)勢能Eq是qi的函數(shù)，因此L是qi和的函數(shù)。第60頁/共94頁第六十頁，共95頁。

2.拉格朗日方程關(guān)節(jié)i的廣義驅(qū)動力為：i＝1，2，…，nFi為關(guān)節(jié)i的廣義驅(qū)動力。移動關(guān)節(jié)：Fi為驅(qū)動力；轉(zhuǎn)動關(guān)節(jié)：Fi為驅(qū)動力矩。第61頁/共94頁第六十一頁，共95頁。l1k1m2k2m121p2l2p1X0Y0二、二自由度平面關(guān)節(jié)機器人動力學方程推導第62頁/共94頁第六十二頁，共95頁。1.關(guān)節(jié)變量及關(guān)節(jié)力矩選取圖示笛卡爾坐標系。關(guān)節(jié)變量：1和2關(guān)節(jié)力矩：1和2。連桿1和連桿2桿長為ll和l2，質(zhì)量分別是ml和m2質(zhì)心分別在kl和k2處，離關(guān)節(jié)中心的距離分別為pl和p2。l1k1m2k2m121p2l2p1X0Y0第63頁/共94頁第六十三頁，共95頁。桿1質(zhì)心kl的位置坐標為：

x1＝p1sin1 y1＝-p1cos1桿1質(zhì)心kl的速度平方為：l1k1m2k2m121p2l2p1X0Y0第64頁/共94頁第六十四頁，共95頁。桿2質(zhì)心k2的位置坐標為：

x2＝llsinl+p2sin(l+2) y2＝-llcosl-p2cos(l+2)桿2質(zhì)心k2的速度平方為：

第65頁/共94頁第六十五頁，共95頁。2.系統(tǒng)動能第66頁/共94頁第六十六頁，共95頁。3.系統(tǒng)勢能第67頁/共94頁第六十七頁，共95頁。4.拉格朗日函數(shù)第68頁/共94頁第六十八頁，共95頁。拉格朗日方程i＝1，2計算各關(guān)節(jié)上的力矩，得到系統(tǒng)動力學方程。5.系統(tǒng)動力學方程第69頁/共94頁第六十九頁，共95頁。6.計算關(guān)節(jié)1上的力矩1：注意：這里只求顯因變量的偏導數(shù)第70頁/共94頁第七十頁，共95頁。簡寫為：其中：第71頁/共94頁第七十一頁，共95頁。7.計算關(guān)節(jié)2上的力矩2：第72頁/共94頁第七十二頁，共95頁。簡寫為：其中：力矩

慣性力

向心力

哥氏力

重力第73頁/共94頁第七十三頁，共95頁。上式表示了關(guān)節(jié)驅(qū)動力矩與關(guān)節(jié)位移、速度、加速度之間的關(guān)系，即力和運動之間的關(guān)系，稱為二自由度工業(yè)機器人的動力學方程。有效慣量：D11、D22

D11、D22：關(guān)節(jié)1和關(guān)節(jié)2加速度引起的慣性力矩；耦合慣量：D12、

D21D12：關(guān)節(jié)2加速度對關(guān)節(jié)1的耦合慣性力矩；

D21：關(guān)節(jié)1加速度對關(guān)節(jié)2的耦合慣性力矩。力矩

慣量

向心加速度系數(shù)

哥氏加速度系數(shù)

重力力矩

慣性力

向心力

哥氏力

重力第74頁/共94頁第七十四頁，共95頁。向心加速度系數(shù)：

D122：關(guān)節(jié)2速度引起的向心力對關(guān)節(jié)1的耦合力矩；

D211：關(guān)節(jié)1速度引起的向心力對關(guān)節(jié)2的耦合力矩。哥氏加速度系數(shù)：

D112：哥氏力對關(guān)節(jié)1的耦合力矩；

D212：哥氏力對關(guān)節(jié)2的耦合力矩。哥氏力是由于牽連運動有轉(zhuǎn)動成分造成的。

力矩

慣量

向心加速度系數(shù)

哥氏加速度系數(shù)

重力第75頁/共94頁第七十五頁，共95頁。重力項：

D1：連桿1的質(zhì)量對關(guān)節(jié)1引起的重力矩；

D2：連桿2的質(zhì)量對關(guān)節(jié)2引起的重力矩。簡單的二自由度平面關(guān)節(jié)型機器人動力學方程非常復雜。多自由度機器人，動力學方程更復雜。通常的簡化處理方法：(1)當桿件質(zhì)量不很大，重力矩項可以省略；(2)當關(guān)節(jié)速度不很大，不是高速機器人，二次項可以省略。力矩

慣量

向心加速度系數(shù)

哥氏加速度系數(shù)

重力第76頁/共94頁第七十六頁，共95頁。二桿機器人有效慣量系數(shù)：

耦合慣量系數(shù)：

向心力項系數(shù)：

第77頁/共94頁第七十七頁，共95頁。哥氏力系數(shù)：

重力項：

第78頁/共94頁第七十八頁，共95頁。二、關(guān)節(jié)空間和操作空間動力學

1．關(guān)節(jié)空間的動力學方程寫成矩陣形式

操作臂在關(guān)節(jié)空間的動力學方程的一般結(jié)構(gòu)形式反映了關(guān)節(jié)力矩與關(guān)節(jié)變量、速度、加速度之間的函數(shù)關(guān)系。

第79頁/共94頁第七十九頁，共95頁。操作臂的慣性矩陣：對于n個關(guān)節(jié)的操作臂，D(q)是n×n的正定對稱矩陣，是q的函數(shù)。G(q)是n×1的重力矢量，與操作臂的形位q有關(guān)。是n×1的離心力和科氏力矢量。第80頁/共94頁第八十頁，共95頁。2．操作空間動力學方程在笛卡兒操作空間中用末端操作器位姿矢量X表示機器人動力學方程。操作力F與末端運動參數(shù)之間的關(guān)系為：

慣性矩陣離心力和科氏力重力矢量關(guān)節(jié)空間動力學方程和操作空間動力學方程的關(guān)系：

第81頁/共94頁第八十一頁，共95頁。三、機械手動力學方程機械手的動力學方程建立的一般步驟：1。計算任一連桿上任意一點的速度；2。計算各連桿的動能和機械手的總動能；3。計算各連桿的位能和機械手的總位能；4。建立機械手系統(tǒng)的拉格郎日函數(shù)；5。對拉格郎日函數(shù)求導，得到動力學方程。第82頁/共94頁第八十二頁，共95頁。系統(tǒng)的動力學方程為第83頁/共94頁第八十三頁，共95頁。第84頁/共94頁