机器人运动学及轨迹规划— (7) 机器人雅可比矩阵
以ABB的 IRB2600 型号机器人为例，采用MDH和SDH方式分别给出了matlab的基础求解代码和机器人工具箱代码，由以上四种方式求得的雅可比矩阵J一致。
·
一、雅可比矩阵
1.雅可比矩阵的定义
雅可比矩阵是一个偏导数矩阵，在机器人的运动控制、力控制和轨迹规划等场景中均有涉及，用来描述关节空间和笛卡尔空间速度的转换关系。如果机器人有m个关节，末端执行器有n个自由度，则雅可比矩阵是一个m✖n的矩阵。
对于多为6自由度串联结构的工业机器人，雅可比矩阵如下，可以看出其雅可比矩阵为方阵。工业机器人仅在奇异点时雅可比矩阵不可逆，此时笛卡尔空间无法向关节空间映射，导致机器人求逆失败。而对于冗余自由度机器人，其雅可比矩阵非方阵，因此对于冗余机械臂常通过伪逆来求逆运动学解。
2.雅可比矩阵的计算
参考《机器人学》给出的雅可比矩阵表达式如下：
二、MDH求解雅可比矩阵
1.RTB-MDH-J
借助matlab机器人工具箱（Robot-Toolbox）通过MDH参数建立机器人模型，然后调用雅可比矩阵函数J=IRB2600.jacob0(q)。
%输入MDH参数
% theta d a alpha offset
SL1=Link([0 445 0 0 0 ],'modified');
SL2=Link([0 0 150 -pi/2 0 ],'modified');
SL3=Link([0 0 700 0 0 ],'modified');
SL4=Link([0 795 115 -pi/2 0 ],'modified');
SL5=Link([0 0 0 pi/2 0 ],'modified');
SL6=Link([0 85 0 -pi/2 0 ],'modified');
%保证和实际机器人一致，初始零位偏置
SL2.offset=-pi/2;
SL6.offset=-pi;
%关节限角
SL1.qlim=[-180, 180]*pi/180;
SL2.qlim=[-95, 155]*pi/180;
SL3.qlim=[-180, 75]*pi/180;
SL4.qlim=[-400, 400]*pi/180;
SL5.qlim=[-120, 120]*pi/180;
SL6.qlim=[-400, 400]*pi/180;
%建立名为IRB2600的机器人连杆模型
IRB2600=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','IRB2600');
%根据机器人模型进行示教仿真
q=[15*(pi/180) -15*(pi/180) 45*(pi/180) -15*(pi/180) 15*(pi/180) 15*(pi/180)];
IRB2600.teach(q);
J=IRB2600.jacob0(q);
%J=IRB2600.jacobn(q);相对于工具坐标系的雅可比矩阵运行结果如下：
2.MatLab-MDH-J
通过矢量积法求解雅可比矩阵，MatLab代码如下，运行得到的结果与RTB-MDH-J一致。
% 给定关节角度
q=[15*(pi/180) -15*(pi/180) 45*(pi/180) -15*(pi/180) 15*(pi/180) 15*(pi/180)];
% IRB2600 MDH
th(1) = q(1); d(1) = 445; a(1) = 0; alp(1) = 0;
th(2) = q(2)-pi/2; d(2) = 0; a(2) = 150; alp(2) = -pi/2;
th(3) = q(3); d(3) = 0; a(3) = 700; alp(3) = 0;
th(4) = q(4); d(4) = 795; a(4) = 115; alp(4) = -pi/2;
th(5) = q(5); d(5) = 0; a(5) = 0; alp(5) = pi/2;
th(6) = q(6)+pi; d(6) = 85; a(6) = 0; alp(6) = -pi/2;
% 初始化矩阵T
T=zeros(4,4,6);
% 计算各关节间的齐次变换矩阵
for i=1:6
T(:,:,i)=[cos(th(i)) -sin(th(i)) 0 a(i);
sin(th(i))*cos(alp(i)) cos(th(i))*cos(alp(i)) -sin(alp(i)) -d(i)*sin(alp(i));
sin(th(i))*sin(alp(i)) cos(th(i))*sin(alp(i)) cos(alp(i)) d(i)*cos(alp(i));
0 0 0 1];
end
T01 = T(:,:,1);
T12 = T(:,:,2);
T23 = T(:,:,3);
T34 = T(:,:,4);
T45 = T(:,:,5);
T56 = T(:,:,6);
T06=T01*T12*T23*T34*T45*T56;
% J1
R01=[T01(1,1) T01(1,2) T01(1,3)
T01(2,1) T01(2,2) T01(2,3)
T01(3,1) T01(3,2) T01(3,3)];
z1=[ T01(1,3)
T01(2,3)
T01(3,3)];
T16=T12*T23*T34*T45*T56;
p1=[T16(1,4)
T16(2,4)
T16(3,4)];
P1=R01*p1;
Z1=cross(z1,P1);
J1=[Z1
z1];
% J2
T02=T01*T12;
R02=[T02(1,1) T02(1,2) T02(1,3)
T02(2,1) T02(2,2) T02(2,3)
T02(3,1) T02(3,2) T02(3,3)];
z2=[T02(1,3)
T02(2,3)
T02(3,3)];
T26=T23*T34*T45*T56;
p2=[T26(1,4)
T26(2,4)
T26(3,4)];
P2=R02*p2;
Z2=cross(z2,P2);
J2=[Z2
z2];
% J3
T03=T01*T12*T23;
R03=[T03(1,1) T03(1,2) T03(1,3)
T03(2,1) T03(2,2) T03(2,3)
T03(3,1) T03(3,2) T03(3,3)];
z3=[T03(1,3)
T03(2,3)
T03(3,3)];
T36=T34*T45*T56;
p3=[T36(1,4)
T36(2,4)
T36(3,4)];
P3=R03*p3;
Z3=cross(z3,P3);
J3=[Z3
z3];
% J4
T04=T01*T12*T23*T34;
R04=[T04(1,1) T04(1,2) T04(1,3)
T04(2,1) T04(2,2) T04(2,3)
T04(3,1) T04(3,2) T04(3,3)];
z4=[T04(1,3)
T04(2,3)
T04(3,3)];
T46=T45*T56;
p4=[T46(1,4)
T46(2,4)
T46(3,4)];
P4=R04*p4;
Z4=cross(z4,P4);
J4=[Z4
z4];
% J5
T05=T01*T12*T23*T34*T45;
R05=[T05(1,1) T05(1,2) T05(1,3)
T05(2,1) T05(2,2) T05(2,3)
T05(3,1) T05(3,2) T05(3,3)];
z5=[T05(1,3)
T05(2,3)
T05(3,3)];
p5=[T56(1,4)
T56(2,4)
T56(3,4)];
P5=R05*p5;
Z5=cross(z5,P5);
J5=[Z5
z5];
% J6
R06=[T06(1,1) T06(1,2) T06(1,3)
T06(2,1) T06(2,2) T06(2,3)
T06(3,1) T06(3,2) T06(3,3)];
z6=[T06(1,3)
T06(2,3)
T06(3,3)];
p6=[0
0
0];
P6=R06*p6;
Z6=cross(z6,P6);
J6=[Z6
z6];
J=[J1 J2 J3 J4 J5 J6];三、SDH求解雅可比矩阵
1.RTB-SDH-J
借助matlab机器人工具箱（Robot-Toolbox）通过SDH参数建立机器人模型，然后调用雅可比矩阵函数J=IRB2600.jacob0(q)。
%输入SDH参数
% theta d a alpha offset
SL1=Link([0 445 150 -pi/2 0 ],'standard');
SL2=Link([0 0 700 0 0 ],'standard');
SL3=Link([0 0 115 -pi/2 0 ],'standard');
SL4=Link([0 795 0 pi/2 0 ],'standard');
SL5=Link([0 0 0 pi/2 0 ],'standard');
SL6=Link([0 85 0 0 0 ],'standard');
%保证和实际机器人一致，初始零位偏置
SL2.offset=-pi/2;
SL5.offset=-pi;
%关节限角
SL1.qlim=[-180, 180]*pi/180;
SL2.qlim=[-95, 155]*pi/180;
SL3.qlim=[-180, 75]*pi/180;
SL4.qlim=[-400, 400]*pi/180;
SL5.qlim=[-120, 120]*pi/180;
SL6.qlim=[-400, 400]*pi/180;
%建立名为IRB2600的机器人连杆模型
IRB2600=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','IRB2600');
%根据机器人模型进行示教仿真
q=[15*(pi/180) -15*(pi/180) 45*(pi/180) -15*(pi/180) 15*(pi/180) 15*(pi/180)];
IRB2600.teach(q);
J=IRB2600.jacob0(q);%相对于基坐标系的雅可比矩阵
%J=IRB2600.jacobn(q);相对于工具坐标系的雅可比矩阵2.MatLab-SDH-J
通过矢量积法求取机器人雅可比矩阵，，MatLab代码如下：
% 给定关节角度
q=[15*(pi/180) -15*(pi/180) 45*(pi/180) -15*(pi/180) 15*(pi/180) 15*(pi/180)];
% IRB2600 SDH
th(1) = q(1); d(1) = 445; a(1) = 150; alp(1) = -pi/2;
th(2) = q(2)-pi/2; d(2) = 0; a(2) = 700; alp(2) = 0;
th(3) = q(3); d(3) = 0; a(3) = 115; alp(3) = -pi/2;
th(4) = q(4); d(4) = 795; a(4) = 0; alp(4) = pi/2;
th(5) = q(5)-pi; d(5) = 0; a(5) = 0; alp(5) = pi/2;
th(6) = q(6); d(6) = 85; a(6) = 0; alp(6) = 0;
% 初始化矩阵T
T=zeros(4,4,6);
% 计算各关节间的齐次变换矩阵
for i=1:6
T(:,:,i)=[cos(th(i)) -sin(th(i))*cos(alp(i)) sin(th(i))*sin(alp(i)) a(i)*cos(th(i));
sin(th(i)) cos(th(i))*cos(alp(i)) -cos(th(i))*sin(alp(i)) a(i)*sin(th(i));
0 sin(alp(i)) cos(alp(i)) d(i);
0 0 0 1];
end
T01 = T(:,:,1);
T12 = T(:,:,2);
T23 = T(:,:,3);
T34 = T(:,:,4);
T45 = T(:,:,5);
T56 = T(:,:,6);
T06=T01*T12*T23*T34*T45*T56;
% J1
R01=[1 0 0
0 1 0
0 0 1];
z1=[ 0
0
1];
T16=T01*T12*T23*T34*T45*T56;
p1=[T16(1,4)
T16(2,4)
T16(3,4)];
P1=R01*p1;
Z1=cross(z1,P1);
J1=[Z1
z1];
% J2
T02=T01;
R02=[T02(1,1) T02(1,2) T02(1,3)
T02(2,1) T02(2,2) T02(2,3)
T02(3,1) T02(3,2) T02(3,3)];
z2=[T02(1,3)
T02(2,3)
T02(3,3)];
T26=T12*T23*T34*T45*T56;
p2=[T26(1,4)
T26(2,4)
T26(3,4)];
P2=R02*p2;
Z2=cross(z2,P2);
J2=[Z2
z2];
% J3
T03=T01*T12;
R03=[T03(1,1) T03(1,2) T03(1,3)
T03(2,1) T03(2,2) T03(2,3)
T03(3,1) T03(3,2) T03(3,3)];
z3=[T03(1,3)
T03(2,3)
T03(3,3)];
T36=T23*T34*T45*T56;
p3=[T36(1,4)
T36(2,4)
T36(3,4)];
P3=R03*p3;
Z3=cross(z3,P3);
J3=[Z3
z3];
% J4
T04=T01*T12*T23;
R04=[T04(1,1) T04(1,2) T04(1,3)
T04(2,1) T04(2,2) T04(2,3)
T04(3,1) T04(3,2) T04(3,3)];
z4=[T04(1,3)
T04(2,3)
T04(3,3)];
T46=T34*T45*T56;
p4=[T46(1,4)
T46(2,4)
T46(3,4)];
P4=R04*p4;
Z4=cross(z4,P4);
J4=[Z4
z4];
% J5
T05=T01*T12*T23*T34;
R05=[T05(1,1) T05(1,2) T05(1,3)
T05(2,1) T05(2,2) T05(2,3)
T05(3,1) T05(3,2) T05(3,3)];
z5=[T05(1,3)
T05(2,3)
T05(3,3)];
T566=T45*T56;% T566和T56区分
p5=[T566(1,4)
T566(2,4)
T566(3,4)];
P5=R05*p5;
Z5=cross(z5,P5);
J5=[Z5
z5];
% J6
T006=T01*T12*T23*T34*T45;% T006和T06区分
R06=[T006(1,1) T006(1,2) T006(1,3)
T006(2,1) T006(2,2) T006(2,3)
T006(3,1) T006(3,2) T006(3,3)];
z6=[T006(1,3)
T006(2,3)
T006(3,3)];
p6=[0
0
0];
P6=R06*p6;
Z6=cross(z6,P6);
J6=[Z6
z6];
J=[J1 J2 J3 J4 J5 J6];
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
7
1- 0
已为社区贡献8条内容
所有评论(0)