速度与雅克比矩阵

 

 

 

首先建立关于雅克比矩阵的函数

function [J,T] = puma560_jacobian(theta)

%syms q1 q2 q3 q4 q5 q6

q1 = theta(1)/180*pi;

q2 = theta(2)/180*pi-pi/2;

q3 = theta(3)/180*pi+pi/2;

q4 = theta(4)/180*pi;

q5 = theta(5)/180*pi;

q6 = theta(6)/180*pi;

%             theta  d   a  alpha

SL(1)=Link([0 0 0 pi/2        0     ],'standard');

SL(2)=Link([0 0 0.4318 0           0     ],'standard');

SL(3)=Link([0 0.15 0.0203 -pi/2        0     ],'standard');

SL(4)=Link([0 0.4308 0 pi/2        0     ],'standard');

SL(5)=Link([0 0 0 -pi/2        0     ],'standard');

SL(6)=Link([0 0 0 0           0     ],'standard');

L1=SerialLink(SL,'name','puma560-std');

J = L1.jacob0([q1 q2 q3 q4 q5 q6]);

T = L1.fkine([q1 q2 q3 q4 q5 q6]);

end

然后再命令行窗口输入

J = puma560_jacobian([90 45 0 90 0 45])

输出

J =

   -0.0151   -0.0000    0.0000         0         0         0

    0.1500   -0.0136   -0.3190         0         0         0

   -0.0000    0.0151   -0.2903         0         0         0

   -0.0000    1.0000    1.0000   -0.0000    0.0000   -0.0000

    0.0000   -0.0000   -0.0000   -0.7071    0.7071   -0.7071

    1.0000   -0.0000   -0.0000    0.7071    0.7071    0.7071

以上表示的是puma560机械臂在关节角为[90 45 0 90 0 45]下的雅克比矩阵。

 

 

 

输入

>> T = trotx(pi/6)*transl(5,2,3);

>> TAB = tr2jac(T)

输出

TAB =

    1.0000         0         0         0    3.5981   -0.2321

         0    0.8660    0.5000   -3.0000   -2.5000    4.3301

         0   -0.5000    0.8660    2.0000   -4.3301   -2.5000

         0         0         0    1.0000         0         0

         0         0         0         0    0.8660    0.5000

         0         0         0         0   -0.5000    0.8660

求解速度矢量和角速度矢量

输入

>> vA=[3,1,4];

>> wA=[1,1,2];

>> V = [vA,wA];

>> VB = TAB*V'；

>> VB'

输出

ans =

    6.1340    6.0263   -4.3660    1.0000    1.8660    1.2321

则坐标系{B}速度矢量为v=[6.1340 6.0263 -4.3660]T，角速度矢量为w=[1.0000 1.8660 1.2321]T