【机器人学】3-1.六自由度机器人速度域-雅可比矩阵【附MATLAB代码】

  MATLAB仿真验证

已知六轴机器人的D-H参数如下所示：

	关节1	关节2	关节3	关节4	关节5	关节6
	0	90	0	0	-90	90
a	0	0	425	393	0	0
d	160.7	0	0	113.3	99	93.6
	0	90	0	-90	0	0
	0	0	0	0	0	0

通过D-H参数，选用改进型的D-H参数，可以得到各个关节间的旋转矩阵。详细请看我的第一篇博客六自由度机器人正解。

% 求齐次变换矩阵
function T = DHTrans(alpha, a, d, theta)
T= [cos(theta)             -sin(theta)            0            a;
    sin(theta)*cos(alpha)  cos(theta)*cos(alpha)  -sin(alpha)  -sin(alpha)*d;
    sin(theta)*sin(alpha)  cos(theta)*sin(alpha)  cos(alpha)   cos(alpha)*d;
    0                      0                      0            1];
end

%微分法求解雅克比矩阵
function result =  differential(theta)
th(1) = theta(1); d(1) = 0.1607; a(1) = 0; alp(1) = 0;
th(2) = theta(2); d(2) = 0; a(2) = 0; alp(2) = pi/2;   
th(3) = theta(3); d(3) = 0; a(3) = 0.425; alp(3) = 0;
th(4) = theta(4); d(4) = 0.1133; a(4) = 0.393; alp(4) = 0;
th(5) = theta(5); d(5) = 0.099; a(5) = 0; alp(5) = -pi/2;
th(6) = theta(6); d(6) = 0.0936; a(6) = 0; alp(6) = pi/2;

T01 = DHTrans(alp(1), a(1), d(1), th(1));
T12 = DHTrans(alp(2), a(2), d(2), th(2)+pi/2);
T23 = DHTrans(alp(3), a(3), d(3), th(3));
T34 = DHTrans(alp(4), a(4), d(4), th(4)-pi/2);
T45 = DHTrans(alp(5), a(5), d(5), th(5));
T56 = DHTrans(alp(6), a(6), d(6), th(6));

T06 = T01*T12*T23*T34*T45*T56;
T46 = T45*T56;
T36 = T34*T45*T56;
T26 = T23*T34*T45*T56;
T16 = T12*T23*T34*T45*T56;


% j11 = [-T06(1,1)*T06(2,4)+T06(2,1)*T06(1,4);-T06(1,2)*T06(2,4)+T06(2,2)*T06(1,4);-T06(1,3)*T06(2,4)+T06(2,3)*T06(1,4);[T06(3,1);T06(3,2);T06(3,3)]];
j11 = [-T16(1,1)*T16(2,4)+T16(2,1)*T16(1,4);-T16(1,2)*T16(2,4)+T16(2,2)*T16(1,4);-T16(1,3)*T16(2,4)+T16(2,3)*T16(1,4);[T16(3,1);T16(3,2);T16(3,3)]];
j22 = [-T26(1,1)*T26(2,4)+T26(2,1)*T26(1,4);-T26(1,2)*T26(2,4)+T26(2,2)*T26(1,4);-T26(1,3)*T26(2,4)+T26(2,3)*T26(1,4);[T26(3,1);T26(3,2);T26(3,3)]];
j33 = [-T36(1,1)*T36(2,4)+T36(2,1)*T36(1,4);-T36(1,2)*T36(2,4)+T36(2,2)*T36(1,4);-T36(1,3)*T36(2,4)+T36(2,3)*T36(1,4);[T36(3,1);T36(3,2);T36(3,3)]];
j44 = [-T46(1,1)*T46(2,4)+T46(2,1)*T46(1,4);-T46(1,2)*T46(2,4)+T46(2,2)*T46(1,4);-T46(1,3)*T46(2,4)+T46(2,3)*T46(1,4);[T46(3,1);T46(3,2);T46(3,3)]];
j55 = [-T56(1,1)*T56(2,4)+T56(2,1)*T56(1,4);-T56(1,2)*T56(2,4)+T56(2,2)*T56(1,4);-T56(1,3)*T56(2,4)+T56(2,3)*T56(1,4);[T56(3,1);T56(3,2);T56(3,3)]];
j66 = [0;0;0;0;0;1];
T06 = T01*T12*T23*T34*T45*T56;
T = [T06(1,1) T06(1,2) T06(1,3) 0 0 0;
    T06(2,1) T06(2,2) T06(2,3) 0 0 0
    T06(3,1) T06(3,2) T06(3,3) 0 0 0
    0 0 0 T06(1,1) T06(1,2) T06(1,3)
    0 0 0 T06(2,1) T06(2,2) T06(2,3)
    0 0 0 T06(3,1) T06(3,2) T06(3,3)];
jacobian1 = T*[j11,j22,j33,j44,j55,j66];
result = jacobian1;

end

%矢量积法求解雅克比矩阵
function result =  vector(theta)
th(1) = theta(1); d(1) = 0.1607; a(1) = 0; alp(1) = 0;
th(2) = theta(2); d(2) = 0; a(2) = 0; alp(2) = pi/2;   
th(3) = theta(3); d(3) = 0; a(3) = 0.425; alp(3) = 0;
th(4) = theta(4); d(4) = 0.1133; a(4) = 0.393; alp(4) = 0;
th(5) = theta(5); d(5) = 0.099; a(5) = 0; alp(5) = -pi/2;
th(6) = theta(6); d(6) = 0.0936; a(6) = 0; alp(6) = pi/2;

T01 = DHTrans(alp(1), a(1), d(1), th(1));
T12 = DHTrans(alp(2), a(2), d(2), th(2)+pi/2);
T23 = DHTrans(alp(3), a(3), d(3), th(3));
T34 = DHTrans(alp(4), a(4), d(4), th(4)-pi/2);
T45 = DHTrans(alp(5), a(5), d(5), th(5));
T56 = DHTrans(alp(6), a(6), d(6), th(6));


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(z1,P06-P01);z1];
j2 = [cross(z2,P06-P02);z2];
j3 = [cross(z3,P06-P03);z3];
j4 = [cross(z4,P06-P04);z4];
j5 = [cross(z5,P06-P05);z5];
j6 = [cross(z6,P06-P06);z6];
jacobian0 = [j1,j2,j3,j4,j5,j6];
result = jacobian0;
end

%机器人工具箱测试
clc
clear;
% 输入关节角
q(1)=150.1/180*pi;
q(2)=-25.19/180*pi;
q(3)=-35.07/180*pi;
q(4)=-155.82/180*pi;
q(5)=256.86/180*pi;
q(6)=-1.51/180*pi;
%机器人工具箱建立机器人模型
%        theta(z)   d(z)     a(x)     alpha(x)  
L1=Link([  0       0.1607       0        0       ],'modified');
L2=Link([  0       0        0       pi/2    ],'modified');L2.offset = pi/2;
L3=Link([  0       0        0.425       0       ],'modified');
L4=Link([  0       0.1133       0.393       0    ],'modified');L4.offset = -pi/2;
L5=Link([  0       0.099        0       -pi/2    ],'modified');
L6=Link([  0       0.0936        0        pi/2    ],'modified');
My_Robot=SerialLink([L1,L2,L3,L4,L5,L6],'name','My_Robot');
vector = vector(q)
differential = differential(q)
%调用机器人工具箱的jacob0函数，求解雅克比矩阵
robot_data = My_Robot.jacob0(q)

测试结果：

下一章：4-1.六自由度机器人动力学