6自由度机械臂雅克比矩阵求解，结果同jacob0函数

雅可比矩阵描述了关节空间和笛卡尔空间的速度和角速度映射关系。

初学者入门通常会使用matlab中的robotics toolbox，在进行操作空间末端速度求解时会有疑问jacob0函数是怎样实现的。
在网上有很多人公布了jacobian求解的代码，但是都不能运行出正确的结果，本文给出了与机器人工具箱jacob0函数出一样结果的方法。
其公式基于：

syms theta1 theta2 theta3 theta4 theta5 theta6
theta1 = 0;theta2 = -90;theta3 = 90; theta4 =-180; theta5 = -90; theta6 = 0;
T01 = trans(theta1, 0, 0, 90); %theta是角度制
T12 = trans(theta2, 129,-310, 0);
T23 = trans(theta3,-129,-286, 0);
T34 = trans(theta4,93.5, 0, 90);
T45 = trans(theta5,93.5, 0,-90);
T56 = trans(theta6,67.5, 0, 0);
T02 = T01*T12;
T03 = T01*T12*T23;
T04 = T01*T12*T23*T34;
T05 = T01*T12*T23*T34*T45;
T06 = T01*T12*T23*T34*T45*T56;
P01 = [T01(1,4);T01(2,4);T01(3,4)];
P02 = [T02(1,4);T02(2,4);T02(3,4)];
P03 = [T03(1,4);T03(2,4);T03(3,4)];
P04 = [T04(1,4);T04(2,4);T04(3,4)];
P05 = [T05(1,4);T05(2,4);T05(3,4)];
P06 = [T06(1,4);T06(2,4);T06(3,4)];
z1 = T01(1:3,3);
z2 = T02(1:3,3);
z3 = T03(1:3,3);
z4 = T04(1:3,3);
z5 = T05(1:3,3);
z6 = T06(1:3,3);
j1 = [cross([0;0;1],P06 );[0;0;1]];
j2 = [cross(z1,P06-P01);z1];
j3 = [cross(z2,P06-P02);z2];
j4 = [cross(z3,P06-P03);z3];
j5 = [cross(z4,P06-P04);z4];
j6 = [cross(z5,P06-P05);z5];
jacobian0 = [j1,j2,j3,j4,j5,j6]

function T = trans(theta, d, a, alpha)
T = [cos(deg2rad(theta)),-cos(deg2rad(alpha))*sin(deg2rad(theta)), sin(deg2rad(alpha))*sin(deg2rad(theta)),a*cos(deg2rad(theta));
sin(deg2rad(theta)), cos(deg2rad(alpha))*cos(deg2rad(theta)),-sin(deg2rad(alpha))*cos(deg2rad(theta)),a*sin(deg2rad(theta));
0, sin(deg2rad(alpha)), cos(deg2rad(alpha)), d;
0, 0, 0, 1];
end

输出结果：

对你有帮助的话就帮忙点个赞吧~

机器人工具箱结果：