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

  MATLAB仿真验证

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

通过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; 000 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

%机器人工具箱测试clcclear;% 输入关节角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.六自由度机器人动力学