第4章：機(jī)器人速度分析4.1微分運(yùn)動與雅可比已知二連桿機(jī)器人各關(guān)節(jié)的轉(zhuǎn)動速度，如何計(jì)算末端執(zhí)行器H的速度沿x和y軸的微分運(yùn)動關(guān)節(jié)的微分運(yùn)動雅可比矩陣4.1微分運(yùn)動與雅可比雅可比矩陣：將機(jī)器人在任意時(shí)刻的關(guān)節(jié)轉(zhuǎn)速（或平移速度）映射為末端執(zhí)行器的速度，雅可比矩陣是關(guān)節(jié)轉(zhuǎn)角的函數(shù)，由于關(guān)節(jié)轉(zhuǎn)角是隨時(shí)間變化的，所以雅可比矩陣也是時(shí)間的函數(shù)。對于一個(gè)完整的空間描述而言，雅可比矩陣是一個(gè)6×n維矩陣：思考：雅可比矩陣的某一列或者某一行的意義？等式兩邊同除以dt4.1微分運(yùn)動與雅可比對于v和ω的理解：v=[vx,vy,vz]T為機(jī)器人末端執(zhí)行器坐標(biāo)系原點(diǎn)的線速度，而ω=[ωx,ωy,ωz]T為機(jī)器人末端執(zhí)行器坐標(biāo)系的角速度。線速度v是針對剛體上每一點(diǎn)而言的，剛體上的任意一點(diǎn)都有自己的線速度。而角速度ω則是針對整個(gè)剛體（坐標(biāo)系）而言的，當(dāng)剛體繞某一固定軸旋轉(zhuǎn)時(shí)，剛體上所有的點(diǎn)擁有相同的角速度。也正是由于角速度的存在，才產(chǎn)生了每一點(diǎn)的線速度：如果機(jī)器人末端執(zhí)行器的運(yùn)動是由多個(gè)連桿的旋轉(zhuǎn)造成的，則末端執(zhí)行器坐標(biāo)系的角速度和任意一點(diǎn)的線速度則是由多關(guān)節(jié)的旋轉(zhuǎn)疊加而成。4.1微分運(yùn)動與雅可比例4.1二連桿機(jī)器人處于兩種不同的構(gòu)型：θ1=0°，θ2=0°；θ1=0°，θ2=30°。如果a1=a2=1m，rad/s，求連桿末端B點(diǎn)的速度VB。（a）θ1=0°，θ2=0°（b）θ1=0°，θ2=30°4.1微分運(yùn)動與雅可比例4.1：

4.2反對稱矩陣反對稱矩陣：如果一個(gè)矩陣滿足：

，則稱該矩陣為斜對稱矩陣或反對稱矩陣。一個(gè)角速度矢量ω=[ω1,ω2,ω3]T可以表示成斜對稱矩陣的形式：表征坐標(biāo)系x、y和z軸的單位矢量分別為：

其斜對稱矩陣形式為：4.2反對稱矩陣反對稱矩陣的性質(zhì)：4.2反對稱矩陣反對稱矩陣與旋轉(zhuǎn)矩陣R導(dǎo)數(shù)的關(guān)系：將求旋轉(zhuǎn)矩陣R對變量的導(dǎo)數(shù)轉(zhuǎn)化為一個(gè)斜對稱矩陣S與R的乘積4.2反對稱矩陣?yán)?.2：若

，那么有：4.3角速度疊加原理以二連桿機(jī)器人為例，其中

是第一個(gè)連桿相對于坐標(biāo)系0的角速度（旋轉(zhuǎn)軸為坐標(biāo)系0的z軸），

是第一個(gè)連桿相對于坐標(biāo)系1的角速度（旋轉(zhuǎn)軸為坐標(biāo)系1的z軸），若只考慮二連桿機(jī)器人末端執(zhí)行器的瞬時(shí)姿態(tài)（雖然坐標(biāo)系0、1和H的原點(diǎn)不重合），有：4.3角速度疊加原理任意數(shù)量的（連桿）坐標(biāo)系下角速度的疊加：雅可比矩陣關(guān)于角速度映射關(guān)系的下半部分4.4線速度疊加原理機(jī)器人末端執(zhí)行器坐標(biāo)系原點(diǎn)的線速度為各關(guān)節(jié)在同一參考坐標(biāo)系下產(chǎn)生的線速度的疊加。假設(shè)只有關(guān)節(jié)i旋轉(zhuǎn)而其他關(guān)節(jié)靜止，該關(guān)節(jié)的旋轉(zhuǎn)在機(jī)器人末端執(zhí)行器的坐標(biāo)系原點(diǎn)處產(chǎn)生的線速度為：雅可比矩陣關(guān)于線速度映射關(guān)系的上半部分4.5完整的雅可比矩陣若第i個(gè)關(guān)節(jié)為旋轉(zhuǎn)關(guān)節(jié)：

若第i個(gè)關(guān)節(jié)為平動關(guān)節(jié)：計(jì)算雅可比矩陣時(shí)需要得到坐標(biāo)系的z軸單位矢量

zi

和表征原點(diǎn)的矢量oi，都可以從機(jī)器人正向運(yùn)動學(xué)中獲得：zi和可以從變換矩陣的第3列的前3個(gè)元素中提??；oi可以從變換矩陣的第4列的前3個(gè)元素中提取。4.5完整的雅可比矩陣?yán)?.5：計(jì)算二連桿機(jī)器人的雅可比矩陣。

由于該機(jī)器人兩個(gè)關(guān)節(jié)都是旋轉(zhuǎn)關(guān)節(jié)，所以其雅可比矩陣的形式為：

4.5完整的雅可比矩陣?yán)?.5：計(jì)算二連桿機(jī)器人的雅可比矩陣。

4.5完整的雅可比矩陣?yán)?.6：計(jì)算斯坦福機(jī)械手的雅可比矩陣。斯坦福機(jī)械手雅可比矩陣的形式為：注意到第3個(gè)關(guān)節(jié)是平動關(guān)節(jié)，J3的形式與其他不同。

坐標(biāo)系0和坐標(biāo)系1原點(diǎn)重合，所以：4.5完整的雅可比矩陣?yán)?.6：計(jì)算斯坦福機(jī)械手的雅可比矩陣。

最后3個(gè)坐標(biāo)系以及末端執(zhí)行器坐標(biāo)系H的原點(diǎn)均重合，即o3=o4=o5=oH，所以所以：

當(dāng)末端執(zhí)行器坐標(biāo)系H的原點(diǎn)設(shè)在最后3個(gè)關(guān)節(jié)軸線的交點(diǎn)時(shí)，最后3個(gè)關(guān)節(jié)的旋轉(zhuǎn)不會在末端執(zhí)行器上產(chǎn)生線速度。

4.5完整的雅可比矩陣?yán)?.6：計(jì)算斯坦福機(jī)械手的雅可比矩陣。

oH可以從中提?。?/p>

zi可以從中提?。?/p>

4.5完整的雅可比矩陣?yán)?.6：計(jì)算斯坦福機(jī)械手的雅可比矩陣。由于：

所以：

4.5完整的雅可比矩陣?yán)?.6：計(jì)算斯坦福機(jī)械手的雅可比矩陣。由于：

所以：

4.5完整的雅可比矩陣?yán)?.6：計(jì)算斯坦福機(jī)械手的雅可比矩陣。由于：

所以：

4.5完整的雅可比矩陣?yán)?.6：計(jì)算斯坦福機(jī)械手的雅可比矩陣。

同理：

4.6雅可比矩陣的逆與奇異性在軌跡規(guī)劃時(shí)，往往是已知機(jī)器人末端執(zhí)行器的速度，而需要計(jì)算機(jī)器人各關(guān)節(jié)的轉(zhuǎn)動速速（或平動速度），這就是機(jī)器人速度問題的反解。

只要知道了雅可比矩陣的逆，就可以計(jì)算出每個(gè)關(guān)節(jié)需要以多快的速度運(yùn)動，才能使機(jī)器人末端執(zhí)行器產(chǎn)生所期望的速度。

可逆4.6雅可比矩陣的逆與奇異性例4.7二連桿機(jī)器人處于位形θ1=0°，θ2=30°，a1=a2=1m，若要使連桿末端B點(diǎn)的速度VB滿足：求各關(guān)節(jié)的角速度

4.6雅可比矩陣的逆與奇異性例4.7二連桿機(jī)器人的雅可比矩陣為：由于機(jī)器人不處于其奇異位形下，即雅可比矩陣的逆存在，所以：

4.6雅可比矩陣的逆與奇異性雅可比矩陣是θ的函數(shù)，但并不是針對所有的θ值雅可比矩陣都是可逆的，大多數(shù)機(jī)器人都有使雅可比矩陣不可逆的θ值，這些θ值對應(yīng)的機(jī)器人所處的位形就成為機(jī)器人的奇異位形，簡稱奇異性。這些位置一般出現(xiàn)在兩種情形：工作空間邊界的奇異位形：出現(xiàn)在操作臂完全展開或者收回，使得末端執(zhí)行器處于或者非常接近工作空間；工作空間內(nèi)部的奇異位形：通常是由于兩個(gè)或兩個(gè)以上的關(guān)節(jié)軸線共線引起。當(dāng)機(jī)器人處于奇異位形時(shí)，它會失去一個(gè)或多個(gè)自由度，這也意味著，在直角坐標(biāo)空間的某個(gè)方向上，無論選擇什么樣的關(guān)節(jié)速度，都不能使機(jī)器人按照期望的速度運(yùn)動。

4.6雅可比矩陣的逆與奇異性例4.8：對于二連桿機(jī)器人，求其奇異位形以及對應(yīng)的關(guān)節(jié)角度可以通過計(jì)算雅可比矩陣的行列式的值來求機(jī)器人的奇異點(diǎn)：如果一個(gè)方陣的行列式的值為0，那么該矩陣就是非滿秩的，也就是奇異的，所以：要滿足上式，很明顯顯然，當(dāng)θ2=0°或者180°時(shí)，機(jī)器人處于奇異位形：

4.7雅可比矩陣的偽逆如果雅可比矩陣是滿秩的方陣（機(jī)器人處于非奇異位形），很容易求得機(jī)器人關(guān)節(jié)的速度。但當(dāng)雅可比矩陣不是方陣，例如，對于冗余自由度機(jī)器人（m＜n），雅可比矩陣無法求逆，在這種情況下，若J為m×n階矩陣，m＜n，且Rank(J)=m，則

存在，且有：稱為雅可比矩陣J的右偽逆。

此時(shí)稱為方程的特解，該解使得關(guān)節(jié)速度矢量的范數(shù)最小；稱為方程的零空間解。

4.7雅可比矩陣的偽逆例4.9：三連桿機(jī)器人對于平面定位（僅考慮x、y方向自由度）而言是一種冗余自由度機(jī)器人，當(dāng)處于位形θ1=0°，θ2=30°，θ3=-30°，a1=a2=a3=1m，若要使連桿末端C點(diǎn)的速度VB滿足：

求：

4.7雅可比矩陣的偽逆例4.9：

在該形位下，平面三連桿機(jī)器人的雅可比矩陣為：

其偽逆為：

取b=0：

取b=[111]T:

4.8雅可比矩陣與靈巧度從實(shí)際的機(jī)器人操作及精度控制角度出發(fā)，機(jī)器人不但要避開幾個(gè)特定的奇異位形，還要盡量遠(yuǎn)離奇異位形區(qū)域。因?yàn)楫?dāng)機(jī)器人接近奇異位形時(shí)，其雅可比矩陣呈病態(tài)分布，其逆矩陣的計(jì)算精度降低，從而使運(yùn)動輸入與輸出之間的傳遞關(guān)系失真，這就需要定量的來描述雅可比矩陣的奇異程度。靈巧度（Dexterity）：定量地來衡量這種運(yùn)動失真程度的指標(biāo)雅可比條件數(shù)（Jacobian-based

Condition

Number）可操作度（Manipulability）

4.8雅可比矩陣與靈巧度可操作性橢球體

4.8雅可比矩陣與靈巧度例4.8：對于二連桿機(jī)器人，l1=l2=1m，當(dāng)處于以下位形時(shí)的可操作度，并繪制可操作橢圓。

①θ1=15°，θ2=-30°；②θ1=35°，θ2=-65°；③θ1=50°，θ2=-100°；④θ1=60°，θ2=-130°；⑤θ1=70°，θ2=-145°；⑥θ1=80°，θ2=-165°；

4.9機(jī)器人靜力學(xué)機(jī)器人靜力學(xué)：機(jī)器人維持某一位形處于靜止?fàn)顟B(tài)，各關(guān)節(jié)驅(qū)動力矩與機(jī)器人受到的外部載荷之間的平衡關(guān)系。正問題：已知外界環(huán)境對機(jī)器人末端的作用力F，求相應(yīng)的滿足靜力平衡條件的關(guān)節(jié)驅(qū)動力矩τ；反問題：已知關(guān)節(jié)驅(qū)動力矩τ，確定機(jī)器人末端對外界環(huán)境的作用力F或負(fù)載的質(zhì)量。

4.9機(jī)器人靜力學(xué)連桿的受力和平衡分析：機(jī)器人是由連桿和關(guān)節(jié)組成，若將機(jī)器人的連桿當(dāng)成剛體，以其中一個(gè)連桿i為對象對其進(jìn)行靜力分析，連桿i及其相鄰連桿i-1、連桿i+1之間的力和力矩關(guān)系：

fi：前一連桿i-1作用在連桿i上的力；fi+1：后一連桿i+1作用在連桿i上的力；ni：前一連桿i-1作用在連桿i上的力矩；ni+1：后一連桿i+1作用在連桿i上的力矩；mig：連桿i的重力，作用在連桿i的質(zhì)心上；ri為：連桿i的質(zhì)心位置矢量；iPi+1：關(guān)節(jié)坐標(biāo)系i+1的原點(diǎn)在關(guān)節(jié)坐標(biāo)系i中的位置矢量。4.9機(jī)器人靜力學(xué)連桿處于平衡狀態(tài)時(shí)，連桿受到的合力和合力矩為零，結(jié)合作用力（矩）與反作用力（矩）有：忽略重力：

若要使一個(gè)連桿受力平衡，作用在連桿上的力和力矩要相等；這些力或力矩均要在同一坐標(biāo)系下進(jìn)行表征才能進(jìn)行疊加運(yùn)算。4.9機(jī)器人靜力學(xué)連桿處于平衡狀態(tài)時(shí)，連桿受到的合力和合力矩為零，結(jié)合作用力（矩）與反作用力（矩）有：忽略重力：

若要使一個(gè)連桿受力平衡，作用在連桿上的力和力矩要相等；這些力或力矩均要在同一坐標(biāo)系下進(jìn)行表征才能進(jìn)行疊加運(yùn)算。4.9機(jī)器人靜力學(xué)若不考慮關(guān)節(jié)中的摩擦，關(guān)節(jié)除了繞轉(zhuǎn)軸的關(guān)節(jié)扭矩外，其余各方向的力和力矩都由關(guān)節(jié)結(jié)構(gòu)件來承受，為保持連桿平衡，施加的關(guān)節(jié)驅(qū)動力矩應(yīng)是關(guān)節(jié)軸矢量與施加在連桿上的力矩矢量的點(diǎn)積：

4.10力雅可比若將操作臂末端所受到的力和力矩組成六維矢量，將各關(guān)節(jié)驅(qū)動力矩組成n維矢量：虛功原理：令各關(guān)節(jié)的虛位移為

，末端操作臂的虛位移為D，虛位移是滿足機(jī)械系統(tǒng)的幾何約束條件的無限小位移，則各關(guān)節(jié)所做的虛功之和以及末端操作臂所做的虛功分別為：

4.10力雅可比根據(jù)虛功原理，任意虛位移產(chǎn)生的虛功和為零，即：關(guān)節(jié)空間虛位移產(chǎn)生的虛功等于操作空間虛位移產(chǎn)生的虛功：

力雅可比4.10力雅可比靜力學(xué)逆問題：若

存在，則靜力學(xué)的逆問題，即：已知關(guān)節(jié)驅(qū)動力矩τ，確定機(jī)器人末端對外界環(huán)境的作用力F：如果機(jī)器人的力雅可比矩陣不是方陣，則

JT

就沒有逆矩陣，此時(shí)不能得到惟一的解。

4.10力雅可比例4.10：圖中平面二連桿機(jī)器人，各關(guān)節(jié)z軸沿紙面向外，此時(shí)各關(guān)節(jié)的轉(zhuǎn)角θ1=45°，θ2=-45°，l1=l2=1.0m。忽略連桿的質(zhì)量，在末端施加一個(gè)力F=[0,-10,0]T，如果想維持二連桿機(jī)器人在該狀態(tài)下保持平衡，求各關(guān)節(jié)的力矩（注意：2f3表示該力矢量是關(guān)于坐標(biāo)系2的表征）

4.10力雅可比例4.10：對第2個(gè)連桿，有：，機(jī)械手末端對外施加的力3f3與外界力大小相等，方向相反。所以有：

4.10力雅可比例4.10：對第1個(gè)連桿，有：帶入計(jì)算可得：所以有：

4.10力雅可比例4.10：

可用輸出外力與力雅可比之間的關(guān)系來計(jì)算關(guān)節(jié)驅(qū)動力矩。對于二連桿，其速度雅可比為：

4.10力雅可比MATLAB代碼：

%定義參數(shù)f_23=[0;10;0];f_22=f_23;P_23=[1;0;0];n_22=cross(P_23,f_23);z_22=[0;0;1];Tau2=n_22'*z_22;R_12=[cos(pi/4)-sin(pi/4)0;sin(pi/4)cos(pi/4)0;001];P_12=[1;0;0];n_11=R_12*n_22+cross(P_12,R_12*f_22);z_11=[0;0;1];Tau1=n_11'*z_11;Tau=[Tau1;Tau2]%采用力雅可比矩陣與外界力的關(guān)系計(jì)算symstheta1theta2;l1=1;l2=1;F_H=[0;10;0];J=[-l1*sin(theta1)-l2*sin(theta1+theta2)-l2*sin(theta1+theta2);l1*cos(theta1)+l2*cos(theta1+theta2)l2*cos(theta1+theta2);00];pose=[pi/4,-pi/4];J_pose=vpa(subs(J,[theta1,theta2],pose),3);Tau=J_pose'*F_H4.10力雅可比例4.11：平面二連桿機(jī)器人，各關(guān)節(jié)的轉(zhuǎn)角θ1=45°，θ2=-45°，l1=l2=1m。兩連桿的質(zhì)量均為5.196kg，r1=r2=0.5m，在機(jī)械手末端施加一個(gè)力F=[0-100]T，如果希望二連桿機(jī)器人在該狀態(tài)下保持平衡，求各關(guān)節(jié)的力矩。

4.10力雅可比例4.11：對第2個(gè)連桿，有：

4.10力雅可比例4.11：對第1個(gè)連桿，有：

4.10力雅可比

%定義參數(shù)f_23=[0;10;0];%計(jì)算第二個(gè)關(guān)節(jié)轉(zhuǎn)矩m2=5.196;g=[0;-9.8;0];mg_22=m2*g;P_23=[1;0;0];r_22=[0.5;0;0];f_22=f_23-mg_22;n_22=cross(P_23,f_23)-cross(r_22,mg_22);z_22=[0;0;1];Tau2=n_22'*z_22;%計(jì)算第一個(gè)關(guān)節(jié)轉(zhuǎn)矩R_12=[cos(pi/4)-sin(pi/4)0;sin(pi/4)cos(pi/4)0;001];P_12=[1;0;0];r_11=[0.5;0;0];m1=5.196;m_21=m1*g;n_11=R_12*n_22+cross(P_12,R_12*f_22)-cross(r_11,R_12*m_21);z_11=[0;0;1];Tau1=n_11'*z_11;Tau=[Tau1;Tau2]4.10力雅可比例4.12：