第3章 雅可比矩陣和動力學(xué)分析

1、第第3 3章章 雅可比矩陣雅可比矩陣和動力學(xué)分析和動力學(xué)分析上一章討論了剛體的位姿描述、齊次變換，機(jī)器人各連桿間的位移關(guān)系，建立了機(jī)器人的運(yùn)動學(xué)方程，研究了運(yùn)動學(xué)逆解，建立了操作空間與關(guān)節(jié)空間的映射關(guān)系。本章將在位移分析的基礎(chǔ)上，進(jìn)行速度分析，研究操作空間速度與關(guān)節(jié)空間速度之間的線性映射關(guān)系雅可比矩陣(簡稱雅可比)。雅可比矩陣不僅用來表示操作空間與關(guān)節(jié)空間之間的速度線性映射關(guān)系，同時也用來表示兩空間之間力的傳遞關(guān)系。3.1 機(jī)器人速度雅可比與速度分析一、機(jī)器人速度雅可比可寫成：YF(X)將其微分，得：),(),(),(654321666543212265432111xxxxxxfyxxxxxx

2、fyxxxxxxfy也可簡寫成：666226116666222211226612211111ddddddddddddxxfxxfxxfyxxfxxfxxfyxxfxxfxxfyXXFYdd雅可比矩陣用J表示二自由度平面關(guān)節(jié)型機(jī)器人端點(diǎn)位置X、Y與關(guān)節(jié)1、2的關(guān)系為112 12112 12ccssXllYll1212( ,)( ,)XXYY 即12121212ddddddXXXYYY微分得121212ddddXXXYYY121212ddddXXXYYY寫成矩陣形式為1212XXYYJ令簡寫為： dX=J d關(guān)節(jié)空間微小運(yùn)動d與手部作業(yè)空間微小位移dX的關(guān)系。2R機(jī)器人的速度雅可比矩陣為：112

3、122 12112 122 12l sl sl sl cl cl cJ已知關(guān)節(jié)和角速度,可求出該機(jī)器人手部速度。若J1，J2分別為雅可比的第1列矢量和第2列矢量，則:1 122JJv右邊第一項表示僅由第一個關(guān)節(jié)運(yùn)動引起的端點(diǎn)速度；右邊第二項表示僅由第二個關(guān)節(jié)運(yùn)動引起的端點(diǎn)速度；總的端點(diǎn)速度為這兩個速度矢量的合成。因此，機(jī)器人速度雅可比的每一列表示其他關(guān)節(jié)不動而某一關(guān)節(jié)運(yùn)動產(chǎn)生的端點(diǎn)速度。 dX=J d( )XJ q qvn自由度機(jī)器人J 陣關(guān)節(jié)變量用廣義關(guān)節(jié)變量q表示： q = q1, q2, , qnT當(dāng)關(guān)節(jié)為轉(zhuǎn)動關(guān)節(jié)時qi=i；當(dāng)關(guān)節(jié)為移動關(guān)節(jié)時qi=di關(guān)節(jié)空間的微小運(yùn)動： dq = d

4、q1，dq2, , dqnT機(jī)器人末端在操作空間的位姿X表示，它是關(guān)節(jié)變量的函數(shù)，X=X(q)，是一個6維列矢量。J（q）：反映了關(guān)節(jié)空間微小運(yùn)動dq與手部作業(yè)空間微小運(yùn)動dX之間的關(guān)系。121212T121212nnnXXXnYYYnZZZnXXXqqqYYYqqqZZZqqqqqqqqqqqqXqJ(q)dX = J(q) dqdX=dX，dY，dZ，X，Y，ZT反映了操作空間的微小運(yùn)動，由機(jī)器人末端微小線位移和微小角位移(微小轉(zhuǎn)動)組成。二、機(jī)器人速度分析 對dX = J d兩邊各除以dt得dd( )ddttXqJ q( )XJ q qv或表示為 式中：v為機(jī)器人末端在操作空間中的廣義速

5、度； q為機(jī)器人關(guān)節(jié)在關(guān)節(jié)空間中的關(guān)節(jié)速度；與操作空間速度v之間關(guān)系的雅可比矩陣。 qJ(q)為確定關(guān)節(jié)空間速度反之，假如給定工業(yè)機(jī)器人手部速度，可解出相應(yīng)的關(guān)節(jié)速度，即：VJq1式中：J-1稱為工業(yè)機(jī)器人逆速度雅可比。當(dāng)工業(yè)機(jī)器人手部在空間按規(guī)定的速度進(jìn)行作業(yè)，用上式可以計算出沿路徑上每一瞬時相應(yīng)的關(guān)節(jié)速度。例1 如圖示的二自由度機(jī)械手，手部沿固定坐標(biāo)系X0軸正向以1.0 m/s的速度移動，桿長l1=l2=0.5 m。求當(dāng)1=30，2=60時的關(guān)節(jié)速度。解 由推導(dǎo)知，二自由度機(jī)械手速度雅可比為112 122 12112 122 12ssscccllllllJ二自由度機(jī)械手手爪沿X0方向運(yùn)動示

6、意圖2 122 121112 12112 121 22cs1ccssslllllll lJ逆雅可比為1Jv12 122 12112 12112 121 22210cs1ccssslllllll l 且vX=1 m/s，vY=0，因此11221212cc4 rad/sssll在兩關(guān)節(jié)的位置分別為1=30,2= 6012 rad s/24 rad/s速度分別為，手部瞬時速度為1 m/s。 rad/s)(2)(-60 sin5 . 0)60-03( cossin)( cos21211l三、雅可比矩陣的奇異性由此可見，當(dāng)雅可比矩陣的行列式為0時，要使手爪運(yùn)動，關(guān)節(jié)速度將趨于無窮大。當(dāng)雅可比不是滿秩矩陣

7、時，J的行列式為0。 qJqJqJ*1 VqJq1 qJ1則 0qJ若J矩陣的伴隨陣 qJ*當(dāng)雅可比不是滿秩矩陣時，可能出現(xiàn)奇異解，機(jī)器人的奇異形位，相應(yīng)操作空間的點(diǎn)為奇異點(diǎn)。機(jī)器人的奇異形位分為兩類：(1) 邊界奇異形位：當(dāng)機(jī)器人臂全部伸展開或全部折回時，手部處于機(jī)器人工作空間的邊界上或邊界附近，逆雅可比奇異。相應(yīng)的機(jī)器人形位叫做邊界奇異形位。(2) 內(nèi)部奇異形位：兩個或兩個以上關(guān)節(jié)軸線重合時，機(jī)器人各關(guān)節(jié)運(yùn)動相互抵消，不產(chǎn)生操作運(yùn)動。相應(yīng)的機(jī)器人形位叫做內(nèi)部奇異形位。當(dāng)機(jī)器人處在奇異形位時會產(chǎn)生退化現(xiàn)象，喪失一個或更多的自由度。這意味著在工作空間的某個方向上，不管怎樣選擇機(jī)器人關(guān)節(jié)速度，手

8、部也不可能實(shí)現(xiàn)移動。當(dāng)l1l2s20時無解，機(jī)器人逆速度雅可比J-1奇異。因l10，l20，所以，在20或2180時，機(jī)器人處于奇異形位。機(jī)器人二臂完全伸直，或完全折回，兩桿重合。在奇異形位下，手部正好處在工作域的邊界上，該瞬時手部只能沿著一個方向(與臂垂直的方向)運(yùn)動，退化了一個自由度。如果希望機(jī)器人手部在空間按規(guī)定的速度進(jìn)行作業(yè)，雅可比是滿秩矩陣，可以計算出沿路徑每一瞬時相應(yīng)的關(guān)節(jié)速度。對空間機(jī)器人，J的行數(shù)為6。二維平面機(jī)器人，J的行數(shù)為3，列數(shù)則為機(jī)械手含有的關(guān)節(jié)數(shù)目。平面運(yùn)動機(jī)器人手的廣義位置向量x,y,T容易確定，且方位與角運(yùn)動的形成順序無關(guān)，可直接采用微分法求J 。對于空間機(jī)器人

9、,根據(jù)機(jī)器人運(yùn)動學(xué)方程，可以獲得直角坐標(biāo)位置向量 x,y,zT 的顯式方程，但找不到方位向量 的一般表達(dá)式。空間機(jī)器人雅可比矩陣J確定：不能用直接微分法，采用構(gòu)造法構(gòu)造法。Tzyx機(jī)器人關(guān)節(jié)速度向量定義為：手爪在基系中的廣義速度向量為： Tnqqqq21TzyxzyxvVnzyxqqqJzyx21四、雅可比矩陣的構(gòu)造法 qqJV*qqJ*)(n個關(guān)節(jié)機(jī)器人，J是6n矩陣。naaainiiqqqJJJJJJV2122121前三行稱為位置雅可比矩陣，代表對手爪線速度 V 的傳遞比；后三行稱為方位矩陣，代表相應(yīng)的關(guān)節(jié)速度 對手爪角速度的傳遞比。將J分塊為：矢量積法構(gòu)造雅可比矩陣對于移動關(guān)節(jié)對于移動關(guān)

10、節(jié)i i: :0,0iiiizJqzwv對于轉(zhuǎn)動關(guān)節(jié)對于轉(zhuǎn)動關(guān)節(jié)i i: :iioniiqzpzwv zi是i坐標(biāo)系z軸單位矢量在基系中的表示。手爪坐標(biāo)原點(diǎn)在i系的位置矢量手爪坐標(biāo)原點(diǎn)的位置矢量在基系的表示onipnip inioiiioniiizpRzzpzJ iziyixiziyixzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxaaaoonnapapapaaaopopopoonpnpnpnn 0 0 0 o 0 0 0 n 0 0 0)()()( )()()( o)()()(n666666微分變換法微分變換法坐標(biāo)系之間的速度變換關(guān)系坐標(biāo)系之間的速度變換關(guān)系矢量運(yùn)算矢量運(yùn)

11、算kajaiaazyx kbjbibzyx bzzyyxxbabababakbabajbabaibaba)( )( )(xyyxzxxzyzzy bazyxzyxbbbaaakji轉(zhuǎn)動關(guān)節(jié)轉(zhuǎn)動關(guān)節(jié)i：系系i只繞只繞zi軸以角速度軸以角速度 轉(zhuǎn)動轉(zhuǎn)動i .666666100000000000000)()()()()()(ooo)()()(nizyxzyxzyxzyxzyxzyxzyxzyxzyxzyxzyxaaaooonnnapapapaaaopopopnpnpnpnnkbabajbabaibaba)( )( )(baxyyxzxxzyzzy1000zzzzyyyyxxxxpaonpaonpao

12、nzzzzzziaonapopnpJ)()()(（2）移動關(guān)節(jié)）移動關(guān)節(jié)i：系系i只沿只沿zi軸以速度軸以速度 移動移動i izyxzyxzyxzyxxxxzyxxxxzyxzyxzyxzyxaaaooonnnapapapaaaopopopnpnpnpnn 000100000000000)()()()()()(ooo)()()(n666666中的元素中的元素i6T中的元素中的元素i6TzzzzzziaonapopnpJ)()()(000zzziaonJ56T46T16T26T36T1)()()()()(0)()()()()(0)()()()()(0)()()()()(0)()()()()(0)

13、()()()()(564636261656463626165646362616565646463636262616165656464636362626161656564646363626261616zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzaaaaaooooonnnnnapapapapapopopopopopnpnpnpnpnpJ前置坐標(biāo)系前置坐標(biāo)系 T6 = A1 A2 A3 A4 A5 A6 A1-1 T6 = 1T6 ( ( 1T6 = = A2 A3 A4 A5 A6 ) ) A2-1 A1-1 T6 = 2T6 ( ( 2T6 = A3 A4 A5 A6 ) ) A

14、3-1A2-1 A1-1 T6 = 3T6 ( ( 3T6 = A4 A5 A6 ) ) A4-1 A3-1A2-1 A1-1 T6 = 4T6 ( ( 4T6 = A5 A6 ) ) A5-1 A4-1 A3-1A2-1 A1-1 T6 = 5T6 ( ( 5T6 = A6 ) ),0,0)Rot(x)Trans(ad,)Trans(0,0Rot(z,Aiiiii10000000T56565665656546ccssscsssccc555654 cs ss 0 0 0 cJPUMA560雅可比各列的計算實(shí)例雅可比各列的計算實(shí)例knpnp)(n)(pxyyxzzzzizzzaonapopnpJ

15、)( )( )( )()()(i6i66i6i6i6i6i6i6in x = c23（c4c5c6 s4s6） s23s5c6n y = s4c5c6 c4s6 n z = s23c4c5c6 s4s6 c23s5c6o x = c23c4c5c6+s4s6 + s23s5c6o y = s4c5c6 c4s6 o z = s23c4c5c6+s6s6 + c23s5s6a x = c23c5s5 s23c5a y = s4s5a z = s23c4s5 c23c5p x = a2c2 + a3c23 d4s23p y= d3p z = a3c23 a2s2 d4s23knpnp)(n)(px

16、yyxzJ11=(a2c2 + a3c23 d4s23)( s4c5c6 c4s6 ) d3c23(c4c5c6 s4s6) s23s5c616TzzzizzzaonapopnpJ)( )( )( )()()(i6i66i6i6i6i6i6i6i例2 如圖示的二自由度機(jī)械手，手部沿固定坐標(biāo)系X0軸正向以1.0 m/s的速度移動，桿長l1=l2=0.5 m。求： 1）當(dāng)1=30，2=60時的機(jī)械手位姿。 2）機(jī)械手J 3) 當(dāng)1=30，2=60時關(guān)節(jié)速度x2Y2x3Y3解：1）D-H坐標(biāo)系建立 2）確定各連桿的D-H參數(shù)和關(guān)節(jié)變量連 桿變 量ad123x1Y13）求兩桿之間的位姿矩陣Ai連 桿變

17、 量ad11l10022l20030000 x2Y2x3Y3x1Y110000100000011111csscA),()0 , 0 ,(), 0 , 0(),(iiiiixRotaTransdTranszRotAx2Y2x3Y3x1Y1100001000010001223lA000001000002212212cslscA)iiiii,0,0)Rot(x)Trans(ad,)Trans(0,0Rot(z,A x2Y2x3Y3x1Y14）當(dāng)1=30，2=60時末桿的位姿5）若給定機(jī)械手位姿，求逆解100001000012211121212211121232103slslcsclclscAAAT已

18、知機(jī)械手末端桿的位姿：求：求：1 2100001000003yyyxxxponponT1112211122slslpclclpyx),(2tan),(2tan211221221xyxynnaclpslpa6）求）求 J=?knpnp)(n)(pxyyxzzzzizzzaonapopnpJ)( )( )( )()()(i6i66i6i6i6i6i6i6i1 0 0 0 221211lclslJ 21JJJ 求求J113T1000010000222222122231213slcscllscAAT1 0 0 0 0 22lJ23Tknpnp)(n)(pxyyxz1000010000100012232

19、3lATx2Y2x3Y3x1Y1求求J22R平面機(jī)器人坐標(biāo)系如圖所示。A陣和T矩陣分別為：1000010000111111111slcsclscA1000010000222222222slcsclscAx1Y1x2Y210000100001221112121221112122102slslcsclclscAAT后置坐標(biāo)系后置坐標(biāo)系02Tkbaba)(b)(axyyxz求求J110000100001221112121221112122102slslcsclclscAAT1 0 0 0 221211lclslJzzzizzzaonapopnpJ)( )( )( )()()(i6i66i6i6i6i

20、6i6i6i后置坐標(biāo)系后置坐標(biāo)系zzzizzzaonapopnpJ)( )( )( )()()(i6i66i6i6i6i6i6i6i1 0 0 0 0 22lJ12T100001000022222222212slcsclscATkbaba)(b)(axyyxz求求J2后置坐標(biāo)系后置坐標(biāo)系 3.2 機(jī)器人靜力分析機(jī)器人在作業(yè)過程中，各關(guān)節(jié)產(chǎn)生相應(yīng)的作用力。關(guān)節(jié)力由機(jī)器人各關(guān)節(jié)的驅(qū)動裝置提供，通過連桿傳遞到手部，克服外界作用力。本節(jié)討論操作臂在靜力平衡關(guān)系。兩類靜力學(xué)問題：(1) 已知機(jī)器人手部作用力F ，求關(guān)節(jié)驅(qū)動力矩 。 （滿足靜力學(xué)平衡條件）(2) 已知關(guān)節(jié)驅(qū)動力矩，確定機(jī)器人手部的作用力F

21、或負(fù)荷的質(zhì)量。定義：機(jī)器人末端力矢量機(jī)器人末端力矢量：力f和力矩n，記做：在靜止?fàn)顟B(tài)下，F(xiàn) 應(yīng)與各關(guān)節(jié)的驅(qū)動力或力矩平衡。關(guān)節(jié)力矢量關(guān)節(jié)力矢量：n個關(guān)節(jié)的驅(qū)動力矩組成n 維矢量：Tn21nfF FqJT假定關(guān)節(jié)無摩擦，忽略各桿件的重力，廣義關(guān)節(jié)力矩與機(jī)器人末端力F的關(guān)系為： 力雅可比矩陣力雅可比力雅可比JT是工是工業(yè)機(jī)器人速度雅業(yè)機(jī)器人速度雅可比可比J的轉(zhuǎn)置的轉(zhuǎn)置。利用虛功原理證明。設(shè)各個關(guān)節(jié)的虛位移為qi，手部的虛位移為X。 手部及各關(guān)節(jié)的虛位移X0Y0O0iqi-nn，n+1-fn，n+1d ddx dy dz T， x y zT機(jī)器人關(guān)節(jié)虛位移矢量（關(guān)節(jié)空間）： qq1，q2qn TdX

22、機(jī)器人手部的虛位移和虛角位移（作業(yè)空間） 設(shè)各關(guān)節(jié)力矩為i(i1，2，n) 機(jī)器人手部的作用力和力矩為-fn，n+1和-nn，n+1 根據(jù)虛位移原理，各關(guān)節(jié)所作的虛功之和與末端執(zhí)行器所作的虛功相等。 即：1q1+2q2+nqn= fn，n+1d + nn，n+1 簡寫成：Tq F TX 虛位移q和X符合桿件的幾何約束條件。 有： XJdq， 代入：Tq F TX 有： JTF JT 稱為機(jī)械手的力雅可比。 表示在靜態(tài)平衡狀態(tài)下，操作力向關(guān)節(jié)力映射的線性關(guān)系。 Y01FFxFy1=0X02=90l1l22(b)X011l122l2F=Fx，F(xiàn)yT(a)Y0例3 圖示為二自由度平面關(guān)節(jié)型機(jī)械手，已

23、知手部端點(diǎn)力FFx，F(xiàn)yT，若關(guān)節(jié)無摩擦力存在，求力 F的等效關(guān)節(jié)力矩。另求當(dāng)10，290時的等效關(guān)節(jié)力矩。解：由前面推導(dǎo)知，該機(jī)械手的速度雅可比為：)c()c(c)s()s(s2122121121221211llllllJ則該機(jī)械手的力雅可比為：)c()s()c(c)s(s2122122121121211llllllTJ根據(jù) JTF，得： 1 = -l1sin1+ l2sin(1+2)Fx +l1cos1+ l2cos(1+2)Fy 2 = -l2sin(1+2)Fx+ l2 cos(1+2)Fy當(dāng)10，290yxFFllllll212212212112121121css)ccss1=-l2

24、Fx+ l1Fy ， 2=- l2Fx機(jī)器人動力學(xué)研究各桿件的運(yùn)動和作用力之間的關(guān)系，是機(jī)器人設(shè)計、運(yùn)動仿真和動態(tài)實(shí)時控制的基礎(chǔ)。機(jī)器人動力學(xué)問題有兩類：動力學(xué)正問題已知關(guān)節(jié)的驅(qū)動力矩，求機(jī)器人系統(tǒng)相應(yīng)的運(yùn)動參數(shù)（包括關(guān)節(jié)位移、速度和加速度）。動力學(xué)逆問題已知運(yùn)動軌跡點(diǎn)上的關(guān)節(jié)位移、速度和加速度，求出所需要的關(guān)節(jié)力矩。3.3 機(jī)器人動力學(xué)分析 機(jī)器人是由多個連桿和多個關(guān)節(jié)組成的復(fù)雜的動力學(xué)系統(tǒng)，具有多個輸入和多個輸出，存在著嚴(yán)重的非線性和耦合關(guān)系。采用方法： 拉格朗日(Lagrange)方法 牛頓歐拉方法(Newton-Euler)方法 高斯(Gauss)方法 凱恩(Kane)方法等。拉格朗日

25、方法以簡單的形式求得系統(tǒng)動力學(xué)方程，而且具有顯式結(jié)構(gòu)，物理意義比較明確，對理解機(jī)器人動力學(xué)比較方便。因此，本節(jié)只介紹拉格朗日方法，并結(jié)合簡單實(shí)例進(jìn)行分析。機(jī)器人動力學(xué)問題的求解比較困難，而且需要較長的運(yùn)算時間。因此，簡化求解的過程，最大限度地減少機(jī)器人動力學(xué)在線計算的時間是持續(xù)研究的課題。 一、拉格朗日方程1. 拉格朗日函數(shù)定義：機(jī)械系統(tǒng)的動能Ek和勢能Eq之差，即： LEk - Eq令 qi 廣義關(guān)節(jié)變量， 是廣義關(guān)節(jié)速度。系統(tǒng)動能Ek是 qi 和 的函數(shù)，系統(tǒng)勢能Eq是qi的函數(shù)，因此L是 qi 和 的函數(shù)。 iq iq iq 2. 拉格朗日方程 關(guān)節(jié)i的廣義驅(qū)動力為：iiiqLqLtFd

26、di1，2，nFi為關(guān)節(jié)i的廣義驅(qū)動力。 移動關(guān)節(jié)：Fi為驅(qū)動力； 轉(zhuǎn)動關(guān)節(jié)：Fi為驅(qū)動力矩。l1k1m2k2m121p2l2p1X0Y0二、二自由度平面關(guān)節(jié)機(jī)器人動力學(xué)方程推導(dǎo)1.關(guān)節(jié)變量及關(guān)節(jié)力矩選取圖示笛卡爾坐標(biāo)系。關(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。l1k1m2k2m121p2l2p1X0Y02112121pyx桿1質(zhì)心kl的位置坐標(biāo)為：x1p1sin1y1-p1cos1桿1質(zhì)心kl的速度平方為： l1k1m2k2m121p2l2p1X0Y0桿2質(zhì)心k2的位置坐標(biāo)為：x2llsi

27、nl + p2sin(l +2)y2-llcosl - p2cos(l +2)22121212212221212222cos2plplyx212121112212121112)sin(sin)cos(cosplyplx桿2質(zhì)心k2的速度平方為： 22121212221222212122112122121212221222212122212111cos2121cos212121plmpmlmpmEEplmpmlmEpmEikikkk2.系統(tǒng)動能 21221121121cos1cos1gpmglmpmEEipip3.系統(tǒng)勢能1111cos1gpmEp21221122cos1cos1gpmglmEp

28、pkEEL4.拉格朗日函數(shù) 2122112112212121222122221212211cos1 cos1 cos 2121gpmglmpmplmpmlmpm拉格朗日方程 i1，2計算各關(guān)節(jié)上的力矩，得到系統(tǒng)動力學(xué)方程。 5.系統(tǒng)動力學(xué)方程iiiLLtdd111 LdtdLL222 LdtdLL)sin(sin2122112111gpmglmpmL dd111LLt6.計算關(guān)節(jié)1上的力矩1：2122222121212122111cos2pmplmlmpmL注意：這里只求顯因變量的偏導(dǎo)數(shù))(sinsinsin sin2cos cos2212211211222212212212222122221

29、2212212222211gpmglmpmplmplmplmpmplmlmpmpm 簡寫為： 122122211122121111DDDDD 其中：)(sinsinsinsin2cincos2212211211122121222212112221222212221221222221111gpmglmpmDplmDplmDplmpmDplmlmpmpmD21212212222cosplmpmL)(sinsinsincos dd2122212212212212212222212212222222gpmplmplmplmpmplmpmLLt 7.計算關(guān)節(jié)2上的力矩2：)(sinsin)(212222

30、1212122gpmplmL簡寫為： 221211212122221212DDDDD 其中： )(sinsin0sincon212222212211221221221222222221222221gpmDplmDplmplmDpmDplmpmD2112212212121211122221222211122111212221121121DDDDDDDDDDDDDD 力矩力矩 慣性力慣性力 向心力向心力 哥氏力哥氏力 重力重力上式表示了關(guān)節(jié)驅(qū)動力矩與關(guān)節(jié)位移、速度、加速度之間的關(guān)系，即力和運(yùn)動之間的關(guān)系，稱為二自由度工業(yè)機(jī)器人的動力學(xué)方程。有效慣量：D11、D22 D11、D22：關(guān)節(jié)1和關(guān)節(jié)2加

31、速度引起的慣性力矩；耦合慣量：D12、 D21 D12：關(guān)節(jié)2加速度對關(guān)節(jié)1的耦合慣性力矩； D21：關(guān)節(jié)1加速度對關(guān)節(jié)2的耦合慣性力矩。2112212212121211122221222211122111212221121121DDDDDDDDDDDDDD 力矩力矩 慣量慣量 向心加速度系數(shù)向心加速度系數(shù) 哥氏加速度系數(shù)哥氏加速度系數(shù) 重力重力力矩力矩 慣性力慣性力 向心力向心力 哥氏力哥氏力 重力重力向心加速度系數(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：哥

32、氏力對關(guān)節(jié)2的耦合力矩。 哥氏力是由于牽連運(yùn)動有轉(zhuǎn)動成分造成的。哥氏力是由于牽連運(yùn)動有轉(zhuǎn)動成分造成的。 2112212212121211122221222211122111212221121121DDDDDDDDDDDDDD 力矩力矩 慣量慣量 向心加速度系數(shù)向心加速度系數(shù) 哥氏加速度系數(shù)哥氏加速度系數(shù) 重力重力重力項： D1：連桿1的質(zhì)量對關(guān)節(jié)1引起的重力矩； D2：連桿2的質(zhì)量對關(guān)節(jié)2引起的重力矩。簡單的二自由度平面關(guān)節(jié)型機(jī)器人動力學(xué)方程非常復(fù)雜。多自由度機(jī)器人，動力學(xué)方程更復(fù)雜。通常的簡化處理方法：(1) 當(dāng)桿件質(zhì)量不很大，重力矩項可以省略；(2) 當(dāng)關(guān)節(jié)速度不很大，不是高速機(jī)器人，二次

33、項可以省略。2112212212121211122221222211122111212221121121DDDDDDDDDDDDDD 力矩力矩 慣量慣量 向心加速度系數(shù)向心加速度系數(shù) 哥氏加速度系數(shù)哥氏加速度系數(shù) 重力重力二桿機(jī)器人二桿機(jī)器人有效慣量系數(shù)：有效慣量系數(shù)： 2211121222122()2cos()Dmm dm dm d d22222Dm d耦合慣量系數(shù)：耦合慣量系數(shù)： 21221222122cos()DDm dm d d向心力項系數(shù)：向心力項系數(shù)： 0)sin()sin(22222122112212122111DddmDddmDDD哥氏力系數(shù)：哥氏力系數(shù)： 0)sin(2221

34、2122212121112DDddmDD重力項：重力項： 112112212()sin()sin()Dmmg dm g d22212sin()Dm g d二、關(guān)節(jié)空間和操作空間動力學(xué) 1關(guān)節(jié)空間的動力學(xué)方程關(guān)節(jié)空間的動力學(xué)方程寫成矩陣形式,D q qH q qG q( )()( )1212q12 q12q操作臂在關(guān)節(jié)空間的動力學(xué)方程的一般結(jié)構(gòu)形式反映了關(guān)節(jié)力矩與關(guān)節(jié)變量、速度、加速度之間的函數(shù)關(guān)系。 222211212122221222222122222cc( )cm pm lpl pmpl pmpl pm pD q()()()22 12222 1221222 1221s2s,sm l pm l pm l p H q q()112 1122122212sssm pm l gm p gm p gG q()( )操作臂的慣性矩陣：操作臂的慣性矩陣：對于n個關(guān)節(jié)的操作臂，D(q)是nn的正定對稱矩陣，是q的函數(shù)。G(q)是n1的重力矢量，與操作臂的形位q有關(guān)。 H q q(), ,是n1的離心力和科氏力矢量。2操作空間動力學(xué)方程操作空間動力學(xué)方程 在笛卡兒操作空間中用末端操