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

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

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

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

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

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

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

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

8、速度,PUMA， 關(guān)節(jié)速度：,1、廣義關(guān)節(jié)速度,PUMA 末桿速度,2、末桿速度的定義：,沿末桿坐標軸的速度矢量,繞末桿坐標軸的角速度矢量,3、計算公式：,4、計算原理：速度疊加原理,關(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軸以速度 移動,中的元素,中的元素,前置坐標系,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雅可比各列的計算實例,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 如圖示的二自由度機械手，手部沿固定坐標系X0軸正向以1.0 m/s的速度移動，桿長l1=l2=0.5 m。求： 1）當1=30，2=60時的機械手位姿。 2）機械手J 3) 當1=30，2=60時關(guān)節(jié)速度,解：1）D-H坐標系建立 2）確

11、定各連桿的D-H參數(shù)和關(guān)節(jié)變量,3）求兩桿之間的位姿矩陣Ai,4）當1=30，2=60時末桿的位姿,5）若給定機械手位姿，求逆解,已知機械手末端桿的位姿：,求：1 2,6）求 J=?,求J1,求J2,2R平面機器人坐標系如圖所示。 A陣和T矩陣分別為：,后置坐標系,求J1,后置坐標系,求J2,后置坐標系,3.2機器人靜力分析 機器人在作業(yè)過程中，各關(guān)節(jié)產(chǎn)生相應(yīng)的作用力。 關(guān)節(jié)力由機器人各關(guān)節(jié)的驅(qū)動裝置提供，通過連桿傳遞到手部，克服外界作用力。 本節(jié)討論操作臂在靜力平衡關(guān)系。 兩類靜力學(xué)問題： (1) 已知機器人手部作用力F ，求關(guān)節(jié)驅(qū)動力矩 。 （滿足靜力學(xué)平衡條件） (2) 已知關(guān)節(jié)驅(qū)動力矩

12、，確定機器人手部的作用力F或負荷的質(zhì)量。,定義： 機器人末端力矢量：力f和力矩n，記做： 在靜止狀態(tài)下，F(xiàn) 應(yīng)與各關(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)置。,利用虛功原理證明。 設(shè)各個關(guān)節(jié)的虛位移為qi，手部的虛位移為X。,ddx dy dz T， x y zT 機器人關(guān)節(jié)虛位移矢量（關(guān)節(jié)空間）： qq1，q2qn T,機器人手部的虛位移和虛角位移（作業(yè)空間）,設(shè)各關(guān)節(jié)力矩為i(i1，2，n) 機器人手部的作用力和力矩為-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 稱為機械手的力雅可比。 表示在靜態(tài)平衡狀態(tài)下，操作力向關(guān)節(jié)力映射的線性關(guān)系。,例3 圖示為二自由度平面關(guān)節(jié)型機械手，已知手部端點力FFx，F(xiàn)yT，若關(guān)節(jié)無摩擦力存在，求力 F的等效關(guān)節(jié)力矩。 另求當10，290 時的等效關(guān)節(jié)力矩。,解： 由前面推導(dǎo)知，該機械手的速度雅可比為：,則該機械手的力雅可比為：,根據(jù)JTF，得：,1 =

14、 -l1sin1+ l2sin(1+2)Fx +l1cos1+ l2cos(1+2)Fy 2 = -l2sin(1+2)Fx+ l2 cos(1+2)Fy 當10，290,1=-l2Fx+ l1Fy ， 2=- l2Fx,機器人動力學(xué)研究各桿件的運動和作用力之間的關(guān)系，是機器人設(shè)計、運動仿真和動態(tài)實時控制的基礎(chǔ)。 機器人動力學(xué)問題有兩類： 動力學(xué)正問題已知關(guān)節(jié)的驅(qū)動力矩，求機器人系統(tǒng)相應(yīng)的運動參數(shù)（包括關(guān)節(jié)位移、速度和加速度）。 動力學(xué)逆問題已知運動軌跡點上的關(guān)節(jié)位移、速度和加速度，求出所需要的關(guān)節(jié)力矩。,3.3 機器人動力學(xué)分析,機器人是由多個連桿和多個關(guān)節(jié)組成的復(fù)雜的動力學(xué)系統(tǒng)，具有多個輸

15、入和多個輸出，存在著嚴重的非線性和耦合關(guān)系。 采用方法： 拉格朗日(Lagrange)方法 牛頓歐拉方法(Newton-Euler)方法 高斯(Gauss)方法 凱恩(Kane)方法等。,拉格朗日方法以簡單的形式求得系統(tǒng)動力學(xué)方程，而且具有顯式結(jié)構(gòu)，物理意義比較明確，對理解機器人動力學(xué)比較方便。因此，本節(jié)只介紹拉格朗日方法，并結(jié)合簡單實例進行分析。 機器人動力學(xué)問題的求解比較困難，而且需要較長的運算時間。因此，簡化求解的過程，最大限度地減少機器人動力學(xué)在線計算的時間是持續(xù)研究的課題。,一、拉格朗日方程 1. 拉格朗日函數(shù) 定義：機械系統(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é)機器人動力學(xué)方程推導(dǎo),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。,桿1質(zhì)心kl的位置坐標為： x1p1sin1 y1-p1cos1 桿1質(zhì)心kl的速度平方為：

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

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