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

1、第3章 雅可比矩陣和動力學(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)系，同時(shí)也用來表示兩空間之間力的傳遞關(guān)系。,3.1 機(jī)器人速度雅可比與速度分析,一、機(jī)器人速度雅可比,可寫成：YF(X) 將其微分，得：,也可簡寫成：,雅可比矩陣用J表示,二自由度平面關(guān)節(jié)型機(jī)器人 端點(diǎn)位置X、Y與關(guān)節(jié)1、2的關(guān)系為,即,微分得,

2、寫成矩陣形式為,令,簡寫為： dX=J d,關(guān)節(jié)空間微小運(yùn)動d與手部作業(yè)空間微小位移dX的關(guān)系。,2R機(jī)器人的速度雅可比矩陣為：,已知關(guān)節(jié)和角速度,可求出該機(jī)器人手部速度。 若J1，J2分別為雅可比的第1列矢量和第2列矢量，則:,右邊第一項(xiàng)表示僅由第一個(gè)關(guān)節(jié)運(yùn)動引起的端點(diǎn)速度； 右邊第二項(xiàng)表示僅由第二個(gè)關(guān)節(jié)運(yùn)動引起的端點(diǎn)速度； 總的端點(diǎn)速度為這兩個(gè)速度矢量的合成。 因此，機(jī)器人速度雅可比的每一列表示其他關(guān)節(jié)不動而某一關(guān)節(jié)運(yùn)動產(chǎn)生的端點(diǎn)速度。,dX=J d,n自由度機(jī)器人J 陣 關(guān)節(jié)變量用廣義關(guān)節(jié)變量q表示： q = q1, q2, qnT 當(dāng)關(guān)節(jié)為轉(zhuǎn)動關(guān)節(jié)時(shí)qi=i； 當(dāng)關(guān)節(jié)為移動關(guān)節(jié)時(shí)qi=

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

4、：J-1稱為工業(yè)機(jī)器人逆速度雅可比。 當(dāng)工業(yè)機(jī)器人手部在空間按規(guī)定的速度進(jìn)行作業(yè)，用上式可以計(jì)算出沿路徑上每一瞬時(shí)相應(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時(shí)的關(guān)節(jié)速度。 解 由推導(dǎo)知，二自由度機(jī)械手速度雅可比為,二自由度機(jī)械手手爪沿X0方向運(yùn)動示意圖,逆雅可比為,且vX=1 m/s，vY=0，因此,在兩關(guān)節(jié)的位置分別為1=30,2=60,手部瞬時(shí)速度為1 m/s。,三、雅可比矩陣的奇異性,由此可見，當(dāng)雅可比矩陣的行列式為0時(shí)，要使手爪運(yùn)動，關(guān)節(jié)速度將趨于無窮大。 當(dāng)雅可比不是滿秩矩陣

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

6、時(shí)無解，機(jī)器人逆速度雅可比J-1奇異。 因l10，l20，所以，在20或2180時(shí)，機(jī)器人處于奇異形位。 機(jī)器人二臂完全伸直，或完全折回，兩桿重合。 在奇異形位下，手部正好處在工作域的邊界上，該瞬時(shí)手部只能沿著一個(gè)方向(與臂垂直的方向)運(yùn)動，退化了一個(gè)自由度。 如果希望機(jī)器人手部在空間按規(guī)定的速度進(jìn)行作業(yè)，雅可比是滿秩矩陣，可以計(jì)算出沿路徑每一瞬時(shí)相應(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ī)器人,根據(jù)機(jī)器人運(yùn)動學(xué)方程

7、，可以獲得直角坐標(biāo)位置向量 x,y,zT 的顯式方程，但找不到方位向量 的一般表達(dá)式。 空間機(jī)器人雅可比矩陣J確定：不能用直接微分法，采用構(gòu)造法。,機(jī)器人關(guān)節(jié)速度向量定義為： 手爪在基系中的廣義速度向量為：,四、雅可比矩陣的構(gòu)造法,n個(gè)關(guān)節(jié)機(jī)器人，J是6n矩陣。,前三行稱為位置雅可比矩陣，代表對手爪線速度 V 的傳遞比；后三行稱為方位矩陣，代表相應(yīng)的關(guān)節(jié)速度 對手爪角速度的傳遞比。 將J分塊為：,矢量積法構(gòu)造雅可比矩陣,對于移動關(guān)節(jié)i:,對于轉(zhuǎn)動關(guān)節(jié)i:,zi是i坐標(biāo)系z軸單位矢量在基系中的表示。,手爪坐標(biāo)原點(diǎn)在i系的位置矢量 手爪坐標(biāo)原點(diǎn)的位置矢量在基系的表示,矢量運(yùn)算,已知關(guān)節(jié)速度，求末桿

8、速度,PUMA， 關(guān)節(jié)速度：,1、廣義關(guān)節(jié)速度,PUMA 末桿速度,2、末桿速度的定義：,沿末桿坐標(biāo)軸的速度矢量,繞末桿坐標(biāo)軸的角速度矢量,3、計(jì)算公式：,4、計(jì)算原理：速度疊加原理,關(guān)節(jié)1的速度對末桿速度的影響系數(shù)，傳動比,二、基本公式,已知,已知,求解,1、原始公式（不推導(dǎo)）,(1)轉(zhuǎn)動關(guān)節(jié)i：,系i只繞zi軸以角速度 轉(zhuǎn)動,（2）移動關(guān)節(jié)i：,系i只沿zi軸以速度 移動,中的元素,中的元素,前置坐標(biāo)系,T6 = A1 A2 A3 A4 A5 A6 A1-1 T6 = T16 ( T16 = A2 A3 A4 A5 A6 ) A2-1 A1-1 T6 = T26 ( T26 = A3 A4

9、 A5 A6 ) A3-1A2-1 A1-1 T6 = T36 ( T36 = A4 A5 A6 ) A4-1 A3-1A2-1 A1-1 T6 = T46 ( T46 = A5 A6 ) A5-1 A4-1 A3-1A2-1 A1-1 T6 = T56 ( T56 = A6 ),PUMA560雅可比各列的計(jì)算實(shí)例,nx = c23（c4c5c6 s4s6） s23s5c6 ny = s4c5c6 c4s6 nz = s23c4c5c6s4s6 c23s5c6 ox = c23c4c5c6+s4s6 + s23s5c6 oy = s4c5c6 c4s6 oz = s23c4c5c6+s6s6

10、+ c23s5s6 ax = c23c5s5 s23c5 ay = s4s5 az = s23c4s5 c23c5 px = a2c2 + a3c23 d4s23 py= d3 pz = a3c23 a2s2 d4s23,J11=(a2c2 + a3c23 d4s23)( s4c5c6 c4s6 ) d3c23(c4c5c6 s4s6) s23s5c6,例2 如圖示的二自由度機(jī)械手，手部沿固定坐標(biāo)系X0軸正向以1.0 m/s的速度移動，桿長l1=l2=0.5 m。求： 1）當(dāng)1=30，2=60時(shí)的機(jī)械手位姿。 2）機(jī)械手J 3) 當(dāng)1=30，2=60時(shí)關(guān)節(jié)速度,解：1）D-H坐標(biāo)系建立 2）確

11、定各連桿的D-H參數(shù)和關(guān)節(jié)變量,3）求兩桿之間的位姿矩陣Ai,4）當(dāng)1=30，2=60時(shí)末桿的位姿,5）若給定機(jī)械手位姿，求逆解,已知機(jī)械手末端桿的位姿：,求：1 2,6）求 J=?,求J1,求J2,2R平面機(jī)器人坐標(biāo)系如圖所示。 A陣和T矩陣分別為：,后置坐標(biāo)系,求J1,后置坐標(biāo)系,求J2,后置坐標(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ū)動力矩

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

13、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)系。,例3 圖示為二自由度平面關(guān)節(jié)型機(jī)械手，已知手部端點(diǎn)力FFx，F(xiàn)yT，若關(guān)節(jié)無摩擦力存在，求力 F的等效關(guān)節(jié)力矩。 另求當(dāng)10，290 時(shí)的等效關(guān)節(jié)力矩。,解： 由前面推導(dǎo)知，該機(jī)械手的速度雅可比為：,則該機(jī)械手的力雅可比為：,根據(jù)JTF，得：,1 =

14、 -l1sin1+ l2sin(1+2)Fx +l1cos1+ l2cos(1+2)Fy 2 = -l2sin(1+2)Fx+ l2 cos(1+2)Fy 當(dāng)10，290,1=-l2Fx+ l1Fy ， 2=- l2Fx,機(jī)器人動力學(xué)研究各桿件的運(yùn)動和作用力之間的關(guān)系，是機(jī)器人設(shè)計(jì)、運(yùn)動仿真和動態(tài)實(shí)時(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ī)器人是由多個(gè)連桿和多個(gè)關(guān)節(jié)組成的復(fù)雜的動力學(xué)系統(tǒng)，具有多個(gè)輸

15、入和多個(gè)輸出，存在著嚴(yán)重的非線性和耦合關(guān)系。 采用方法： 拉格朗日(Lagrange)方法 牛頓歐拉方法(Newton-Euler)方法 高斯(Gauss)方法 凱恩(Kane)方法等。,拉格朗日方法以簡單的形式求得系統(tǒng)動力學(xué)方程，而且具有顯式結(jié)構(gòu)，物理意義比較明確，對理解機(jī)器人動力學(xué)比較方便。因此，本節(jié)只介紹拉格朗日方法，并結(jié)合簡單實(shí)例進(jìn)行分析。 機(jī)器人動力學(xué)問題的求解比較困難，而且需要較長的運(yùn)算時(shí)間。因此，簡化求解的過程，最大限度地減少機(jī)器人動力學(xué)在線計(jì)算的時(shí)間是持續(xù)研究的課題。,一、拉格朗日方程 1. 拉格朗日函數(shù) 定義：機(jī)械系統(tǒng)的動能Ek和勢能Eq之差，即： LEk - Eq 令 qi

16、 廣義關(guān)節(jié)變量， 是廣義關(guān)節(jié)速度。 系統(tǒng)動能Ek是 qi 和 的函數(shù)，系統(tǒng)勢能Eq是qi的函數(shù)，因此L是 qi 和 的函數(shù)。,2. 拉格朗日方程 關(guān)節(jié)i的廣義驅(qū)動力為：,i1，2，n,Fi為關(guān)節(jié)i的廣義驅(qū)動力。 移動關(guān)節(jié)：Fi為驅(qū)動力； 轉(zhuǎn)動關(guān)節(jié)：Fi為驅(qū)動力矩。,二、二自由度平面關(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。,桿1質(zhì)心kl的位置坐標(biāo)為： x1p1sin1 y1-p1cos1 桿1質(zhì)心kl的速度平方為：

17、,桿2質(zhì)心k2的位置坐標(biāo)為： x2llsinl + p2sin(l +2) y2-llcosl - p2cos(l +2),桿2質(zhì)心k2的速度平方為：,2.系統(tǒng)動能,3.系統(tǒng)勢能,4.拉格朗日函數(shù),拉格朗日方程,i1，2,計(jì)算各關(guān)節(jié)上的力矩，得到系統(tǒng)動力學(xué)方程。,5.系統(tǒng)動力學(xué)方程,6.計(jì)算關(guān)節(jié)1上的力矩1：,注意：這里只求顯因變量的偏導(dǎo)數(shù),簡寫為：,其中：,7.計(jì)算關(guān)節(jié)2上的力矩2：,簡寫為：,其中：,力矩 慣性力 向心力 哥氏力 重力,上式表示了關(guān)節(jié)驅(qū)動力矩與關(guān)節(jié)位移、速度、加速度之間的關(guān)系，即力和運(yùn)動之間的關(guān)系，稱為二自由度工業(yè)機(jī)器人的動力學(xué)方程。 有效慣量：D11、D22 D11、D2

18、2：關(guān)節(jié)1和關(guān)節(jié)2加速度引起的慣性力矩； 耦合慣量：D12、 D21 D12：關(guān)節(jié)2加速度對關(guān)節(jié)1的耦合慣性力矩； D21：關(guān)節(jié)1加速度對關(guān)節(jié)2的耦合慣性力矩。,力矩 慣量 向心加速度系數(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：哥氏力對關(guān)節(jié)2的耦合力矩。 哥氏力是由于牽連運(yùn)動有轉(zhuǎn)動成分造成的。,力矩 慣量 向心加速度系數(shù) 哥氏加速度系數(shù) 重力,重力項(xiàng)： D1：連桿1的質(zhì)量對關(guān)節(jié)1引起的

19、重力矩； D2：連桿2的質(zhì)量對關(guān)節(jié)2引起的重力矩。 簡單的二自由度平面關(guān)節(jié)型機(jī)器人動力學(xué)方程非常復(fù)雜。 多自由度機(jī)器人，動力學(xué)方程更復(fù)雜。 通常的簡化處理方法： (1) 當(dāng)桿件質(zhì)量不很大，重力矩項(xiàng)可以省略； (2) 當(dāng)關(guān)節(jié)速度不很大，不是高速機(jī)器人，二次項(xiàng)可以省略。,力矩 慣量 向心加速度系數(shù) 哥氏加速度系數(shù) 重力,二桿機(jī)器人 有效慣量系數(shù)：,耦合慣量系數(shù)：,向心力項(xiàng)系數(shù)：,哥氏力系數(shù)：,重力項(xiàng)：,二、關(guān)節(jié)空間和操作空間動力學(xué),1關(guān)節(jié)空間的動力學(xué)方程 寫成矩陣形式,操作臂在關(guān)節(jié)空間的動力學(xué)方程的一般結(jié)構(gòu)形式 反映了關(guān)節(jié)力矩與關(guān)節(jié)變量、速度、加速度之間的函數(shù)關(guān)系。,操作臂的慣性矩陣：對于n個(gè)關(guān)節(jié)的操作臂，D(q)是nn的正定對稱矩陣，是q的函數(shù)。,G(q)是n1的重力矢量，與操作臂的形位q有關(guān)。,是n1的離心力和科氏力矢量。,2操作空間動力學(xué)方程,在笛卡兒操作空間中用末端操作器位姿矢量X 表示機(jī)器人動力學(xué)方程。 操作力F與末端運(yùn)動參數(shù)之間的關(guān)系為 ：,慣性矩陣,離心力和科氏力,重力矢量,關(guān)節(jié)空間動力學(xué)方程和操作空間動力學(xué)方程的關(guān)系：,三、 機(jī)械手動力學(xué)方程 機(jī)械手的動力學(xué)方程建立的一般步驟： 1。計(jì)算任一連桿上任意一點(diǎn)的速度； 2。計(jì)