第3章 雅可比矩陣和動力學分析

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

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

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

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

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

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

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

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

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

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

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

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

13、()()()()(564636261656463626165646362616565646463636262616165656464636362626161656564646363626261616zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzaaaaaooooonnnnnapapapapapopopopopopnpnpnpnpnpJ前置坐標系前置坐標系 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雅可比各列的計算實例雅可比各列的計算實例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 如圖示的二自由度機械手，手部沿固定坐標系X0軸正向以1.0 m/s的速度移動，桿長l1=l2=0.5 m。求： 1）當1=30，2=60時的機械手位姿。 2）機械手J 3) 當1=30，2=60時關節(jié)速度x2Y2x3Y3解：1）D-H坐標系建立 2）確定各連桿的D-H參數和關節(jié)變量連 桿變 量ad123x1Y13）求兩桿之間的位姿矩陣Ai連 桿變

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

18、知機械手末端桿的位姿：求：求：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平面機器人坐標系如圖所示。A陣和T矩陣分別為：1000010000111111111slcsclscA1000010000222222222slcsclscAx1Y1x2Y210000100001221112121221112122102slslcsclclscAAT后置坐標系后置坐標系02Tkbaba)(b)(axyyxz求求J110000100001221112121221112122102slslcsclclscAAT1 0 0 0 221211lclslJzzzizzzaonapopnpJ)( )( )( )()()(i6i66i6i6i6i

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

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

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

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

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

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

26、di1，2，nFi為關節(jié)i的廣義驅動力。 移動關節(jié)：Fi為驅動力； 轉動關節(jié)：Fi為驅動力矩。l1k1m2k2m121p2l2p1X0Y0二、二自由度平面關節(jié)機器人動力學方程推導1.關節(jié)變量及關節(jié)力矩選取圖示笛卡爾坐標系。關節(jié)變量：1和2關節(jié)力矩：1和2。連桿1和連桿2桿長為ll和l2，質量分別是ml和m2質心分別在kl和k2處，離關節(jié)中心的距離分別為pl和p2。l1k1m2k2m121p2l2p1X0Y02112121pyx桿1質心kl的位置坐標為：x1p1sin1y1-p1cos1桿1質心kl的速度平方為： l1k1m2k2m121p2l2p1X0Y0桿2質心k2的位置坐標為：x2llsi

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

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

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

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

31、速度引起的慣性力矩；耦合慣量：D12、 D21 D12：關節(jié)2加速度對關節(jié)1的耦合慣性力矩； D21：關節(jié)1加速度對關節(jié)2的耦合慣性力矩。2112212212121211122221222211122111212221121121DDDDDDDDDDDDDD 力矩力矩 慣量慣量 向心加速度系數向心加速度系數 哥氏加速度系數哥氏加速度系數 重力重力力矩力矩 慣性力慣性力 向心力向心力 哥氏力哥氏力 重力重力向心加速度系數： D122：關節(jié)2速度引起的向心力對關節(jié)1的耦合力矩； D211：關節(jié)1速度引起的向心力對關節(jié)2的耦合力矩。哥氏加速度系數： D112：哥氏力對關節(jié)1的耦合力矩； D212：哥

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

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

34、2122212121112DDddmDD重力項：重力項： 112112212()sin()sin()Dmmg dm g d22212sin()Dm g d二、關節(jié)空間和操作空間動力學 1關節(jié)空間的動力學方程關節(jié)空間的動力學方程寫成矩陣形式,D q qH q qG q( )()( )1212q12 q12q操作臂在關節(jié)空間的動力學方程的一般結構形式反映了關節(jié)力矩與關節(jié)變量、速度、加速度之間的函數關系。 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個關節(jié)的操作臂，D(q)是nn的正定對稱矩陣，是q的函數。G(q)是n1的重力矢量，與操作臂的形位q有關。 H q q(), ,是n1的離心力和科氏力矢量。2操作空間動力學方程操作空間動力學方程 在笛卡兒操作空間中用末端操