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

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

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

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

dX=Jdθ2R機器人的速度雅可比矩陣為：已知關(guān)節(jié)θ和角速度,可求出該機n自由度機器人J

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

…,qn]T當(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é)空間的微小運動：dq=[dq1，dq2,

…

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

二自由度機械手手爪沿X0方向運動示意圖例1如圖示的二自由度機械手，手部沿固定坐標(biāo)系X0軸正向以逆雅可比為 且vX=1m/s，vY=0，因此 逆雅可比為 且vX=1m/s，vY=0，因此 在兩關(guān)節(jié)的位置分別為θ1=30°,θ2=

–60°速度分別為，手部瞬時速度為1m/s。在兩關(guān)節(jié)的位置分別為θ1=30°,θ2=

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

。對于空間機器人,根據(jù)機器人運動學(xué)方程，可以獲得直角坐標(biāo)位置向量[x,y,z]T的顯式方程，但找不到方位向量的一般表達式。空間機器人雅可比矩陣J確定：不能用直接微分法，采用構(gòu)造法。對空間機器人，J的行數(shù)為6。二維平面機器人，J的行數(shù)為3，列四、雅可比矩陣的構(gòu)造法n個關(guān)節(jié)機器人，雅可比矩陣是6×n矩陣。前三行稱為位置雅可比矩陣，代表對手爪線速度V的傳遞比；后三行稱為方位矩陣，代表相應(yīng)的關(guān)節(jié)速度對手爪角速度ω的傳遞比。將J分塊為：四、雅可比矩陣的構(gòu)造法n個關(guān)節(jié)機器人，雅可比矩陣是6×n矩陣把機器人關(guān)節(jié)速度向量定義為：式中，為連桿相對于的角速度或線速度。手爪在基坐標(biāo)系中的廣義速度向量為：與之間的線性映射關(guān)系稱為雅可比矩陣J。把機器人關(guān)節(jié)速度向量定義為：矢量運算矢量運算雅可比各列的計算公式：雅可比各列的計算公式：轉(zhuǎn)動關(guān)節(jié)i：系i只繞zi軸以角速度轉(zhuǎn)動轉(zhuǎn)動關(guān)節(jié)i：系i只繞zi軸以角速度轉(zhuǎn)動第3章-雅可比矩陣和動力學(xué)分析ppt課件（2）移動關(guān)節(jié)i：系i只沿zi軸以速度移動（2）移動關(guān)節(jié)i：系i只沿zi軸以速度移動中的元素中的元素第3章-雅可比矩陣和動力學(xué)分析ppt課件第3章-雅可比矩陣和動力學(xué)分析ppt課件全轉(zhuǎn)動關(guān)節(jié)機器人計算公式全轉(zhuǎn)動關(guān)節(jié)機器人計算公式PUMA560雅可比各列的計算實例PUMA560雅可比各列的計算實例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]nx=c23（c4c5c6s4s6）s23

3.2

機器人靜力分析機器人在作業(yè)過程中，當(dāng)末端操作器與環(huán)境接觸時，各關(guān)節(jié)產(chǎn)生相應(yīng)的作用力。機器人各關(guān)節(jié)的驅(qū)動裝置提供關(guān)節(jié)力矩，通過連桿傳遞到手部，克服外界作用力。本節(jié)討論操作臂在靜力平衡關(guān)系。兩類靜力學(xué)問題：(1)已知外界環(huán)境對工業(yè)機器人手部作用力F，求相應(yīng)的滿足靜力學(xué)平衡條件的關(guān)節(jié)驅(qū)動力矩。(2)已知關(guān)節(jié)驅(qū)動力矩，確定工業(yè)機器人手部對外界環(huán)境的作用力F或負荷的質(zhì)量。3.2

機器定義：末端廣義力矢量：機器人在外界接觸處產(chǎn)生力f和力矩n，記做：在靜止?fàn)顟B(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)置。定義：假定關(guān)節(jié)無摩擦，并忽略各桿件的重力，廣義關(guān)節(jié)力矩與機利用虛功原理證明。設(shè)各個關(guān)節(jié)的虛位移為qi，手部的虛位移為X。手部及各關(guān)節(jié)的虛位移X0Y0O0iqi-nn，n+1-fn，n+1d利用虛功原理證明。手部及各關(guān)節(jié)的虛位移X0Y0O0iqi式中：

d＝[dx

dy

dz]T，＝[xyz]T對應(yīng)于手部的虛位移和虛角位移（作業(yè)空間）q＝[q1，q2…qn]T為各關(guān)節(jié)虛位移qi組成的機器人關(guān)節(jié)虛位移矢量（關(guān)節(jié)空間）

式中：

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

fn，n+1d+

nn，n+1簡寫成：Tq

＝

F

TX

設(shè)各關(guān)節(jié)力矩為i(i＝1，2，…，n)虛位移q和X符合桿件的幾何約束條件。

有：

X＝Jdq，代入Tq

＝

F

TX有：＝JTF

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

虛位移q和X符合桿件的幾何約束條件。Y01FFxFy1=0X02=90l1l22(b)X011l122l2F=[Fx，F(xiàn)y]T(a)Y0例2圖示為二自由度平面關(guān)節(jié)型機械手，已知手部端點力F＝[Fx，F(xiàn)y]T，若關(guān)節(jié)無摩擦力存在，求力F的等效關(guān)節(jié)力矩。另求當(dāng)1＝0，2＝90時的等效關(guān)節(jié)力矩。Y01FFxFy1=0X02=90l1l22(b)解：由前面推導(dǎo)知，該機械手的速度雅可比為：則該機械手的力雅可比為：解：則該機械手的力雅可比為：根據(jù)＝JTF，得：1=-[l1sin1+l2sin(1+2)]Fx

+[l1cos1+l2cos(1+2)]Fy2=-l2sin(1+2)Fx+l2cos(1+2)Fy當(dāng)1＝0，2＝901=-l2Fx+l1Fy，2=-l2Fx根據(jù)＝JTF，得：1=-[l1sin1+機器人動力學(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é)分析機器人動力學(xué)研究各桿件的運動和作用力之間的關(guān)系，是機器人設(shè)計機器人是由多個連桿和多個關(guān)節(jié)組成的復(fù)雜的動力學(xué)系統(tǒng)，具有多個輸入和多個輸出，存在著錯綜復(fù)雜的耦合關(guān)系和嚴重的非線性。采用的方法：拉格朗日(Lagrange)方法牛頓—歐拉方法(Newton-Euler)方法高斯(Gauss)方法凱恩(Kane)方法等。機器人是由多個連桿和多個關(guān)節(jié)組成的復(fù)雜的動力學(xué)系統(tǒng)，具有多個拉格朗日方法以簡單的形式求得非常復(fù)雜的系統(tǒng)動力學(xué)方程，而且具有顯式結(jié)構(gòu)，物理意義比較明確，對理解機器人動力學(xué)比較方便。因此，本節(jié)只介紹拉格朗日方法，并結(jié)合簡單實例進行分析。機器人動力學(xué)問題的求解比較困難，而且需要較長的運算時間。因此，簡化求解的過程，最大限度地減少機器人動力學(xué)在線計算的時間是持續(xù)研究的課題。拉格朗日方法以簡單的形式求得非常復(fù)雜的系統(tǒng)動力學(xué)方程，而且具一、拉格朗日方程1.拉格朗日函數(shù)拉格朗日函數(shù)L的定義是一個機械系統(tǒng)的動能Ek和勢能Eq之差，即：

L＝Ek-Eq

令qi(i＝1，2，…，n)是使系統(tǒng)具有完全確定位置的廣義關(guān)節(jié)變量，是相應(yīng)的廣義關(guān)節(jié)速度。由于系統(tǒng)動能Ek是qi和的函數(shù)，系統(tǒng)勢能Eq是qi的函數(shù)，因此拉格朗日函數(shù)也是qi和的函數(shù)。一、拉格朗日方程

2.拉格朗日方程系統(tǒng)的拉格朗日方程為：i＝1，2，…，n式中，F(xiàn)i稱為關(guān)節(jié)i的廣義驅(qū)動力。如果是移動關(guān)節(jié)，則Fi為驅(qū)動力；如果是轉(zhuǎn)動關(guān)節(jié)，則Fi為驅(qū)動力矩。2.拉格朗日方程i＝1，2，…，n式中，F(xiàn)i稱為關(guān)節(jié)i3.建立機器人動力學(xué)方程步驟(1)選取坐標(biāo)系，選定完全而且獨立的廣義關(guān)節(jié)變量qi(i＝1，2，…，n)(2)選定相應(yīng)的關(guān)節(jié)上的廣義力Fi，當(dāng)qi是位移變量時，F(xiàn)i為力；當(dāng)qi是角度變量時，F(xiàn)i為力矩。(3)求出機器人各構(gòu)件的動能和勢能，構(gòu)造拉格朗日函數(shù)。(4)代入拉格朗日方程求得機器人系統(tǒng)的動力學(xué)方程3.建立機器人動力學(xué)方程步驟l1k1m2k2m121p2l2p1X0Y0舉例：二自由度平面關(guān)節(jié)型工業(yè)機器人動力學(xué)方程l1k1m2k2m121p2l2p1X0Y0舉例：二自由1.廣義關(guān)節(jié)變量及廣義力選取圖示笛卡爾坐標(biāo)系。連桿l和連桿2的關(guān)節(jié)變量分別為轉(zhuǎn)角1和2關(guān)節(jié)1和關(guān)節(jié)2的力矩是1和2。連桿1和連桿2的質(zhì)量分別是ml和m2桿長分別為ll和l2，質(zhì)心分別在kl和k2處，離關(guān)節(jié)中心的距離分別為pl和p2。l1k1m2k2m121p2l2p1X0Y01.廣義關(guān)節(jié)變量及廣義力l1k1m2k2m121p2l桿1質(zhì)心kl的位置坐標(biāo)為：

x1＝p1sin1 y1＝-p1cos1桿1質(zhì)心kl的速度平方為：

桿1質(zhì)心kl的位置坐標(biāo)為：桿2質(zhì)心k2的位置坐標(biāo)為：

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

桿2質(zhì)心k2的位置坐標(biāo)為：2.系統(tǒng)動能2.系統(tǒng)動能3.系統(tǒng)勢能3.系統(tǒng)勢能4.拉格朗日函數(shù)4.拉格朗日函數(shù)根據(jù)拉格朗日方程i＝1，2，…，n

計算各關(guān)節(jié)上的力矩，得到系統(tǒng)動力學(xué)方程。5.系統(tǒng)動力學(xué)方程根據(jù)拉格朗日方程i＝1，2，…，n計算各關(guān)節(jié)上的力矩，得因為所以6.計算關(guān)節(jié)1上的力矩1：因為所以6.計算關(guān)節(jié)1上的力矩1：上式可簡寫為：由此可得上式可簡寫為：由此可得因為所以7.計算關(guān)節(jié)2上的力矩2：因為所以7.計算關(guān)節(jié)2上的力矩2：上式可簡寫為：由此可得

上式可簡寫為：由此可得式(3-26)、(3-27)及式(3-28)、(3-29)分別表示了關(guān)節(jié)驅(qū)動力矩與關(guān)節(jié)位移、速度、加速度之間的關(guān)系，即力和運動之間的關(guān)系，稱為圖3-6所示二自由度工業(yè)機器人的動力學(xué)方程。對其進行分析可知：式(3-26)、(3-27)及式(3-28)、(3-29)

(1)含有或的項表示由于加速度引起的關(guān)節(jié)力矩項，其中：含有D11和D22的項分別表示由于關(guān)節(jié)1加速度和關(guān)節(jié)2加速度引起的慣性力矩項；含有D12的項表示關(guān)節(jié)2的加速度對關(guān)節(jié)1的耦合慣性力矩項；含有D21的項表示關(guān)節(jié)1的加速度對關(guān)節(jié)2的耦合慣性力矩項。(1)含有或的項表示由于加速度引起的關(guān)節(jié)

(2)含有和的項表示由于向心力引起的關(guān)節(jié)力矩項，其中：含有D122的項表示關(guān)節(jié)2速度引起的向心力對關(guān)節(jié)1的耦合力矩項；含有D211的項表示關(guān)節(jié)1速度引起的向心力對關(guān)節(jié)2的耦合力矩項。(2)含有和的項表示由于向心力引起

(3)含有的項表示由于哥氏力引起的關(guān)節(jié)力矩項，其中：含有D112的項表示哥氏力對關(guān)節(jié)1的耦合力矩項；含有D212的項表示哥氏力對關(guān)節(jié)2的耦合力矩項。(3)含有的項表示由于哥氏力引起的

(4)只含關(guān)節(jié)變量1、2的項表示重力引起的關(guān)節(jié)力矩項。其中：含有D1的項表示連桿1、連桿2的質(zhì)量對關(guān)節(jié)1引起的重力矩項；含有D2的項表示連桿2的質(zhì)量對關(guān)節(jié)2引起的重力矩項。(4)只含關(guān)節(jié)變量1、2的項表示重力引起的關(guān)節(jié)

從上面推導(dǎo)可以看出，很簡單的二自由度平面關(guān)節(jié)型工業(yè)機器人其動力學(xué)方程已經(jīng)很復(fù)雜了，包含很多因素，這些因素都在影響工業(yè)機器人的動力學(xué)特性。對于復(fù)雜一些的多自由度工業(yè)機器人，動力學(xué)方程更龐雜了，推導(dǎo)過程也更為復(fù)雜。不僅如此，對工業(yè)機器人實時控制也帶來不小的麻煩。通常，有一些簡化問題的方法：從上面推導(dǎo)可以看出，很簡單的二自由度平面關(guān)節(jié)型工業(yè)

(1)當(dāng)桿件質(zhì)量不很大，重量很輕時，動力學(xué)方程中的重力矩