%%计算工具坐标系下的雅可比矩阵
clear,clc,close all;
format compact
syms d1 d2 d3 d4 d5 d6 a2 a3 alp1 alp4 alp5
syms q1 q2 q3 q4 q5 q6
%% 建立机器人DH参数,初始状态为竖直状态
% 连杆偏移d,连杆长度a,连杆扭转角alpha
L(1)=RevoluteMDH(‘d’,d1,‘a’,0,‘alpha’,0);
L(2)=RevoluteMDH(‘d’,0,‘a’,0,‘alpha’,alp1,‘offset’,0); %-pi/2
L(3)=RevoluteMDH(‘d’,0,‘a’,a2,‘alpha’,0);
L(4)=RevoluteMDH(‘d’,d4,‘a’,a3,‘alpha’,0,‘offset’,0);
L(5)=RevoluteMDH(‘d’,d5,‘a’,0,‘alpha’,alp4);
L(6)=RevoluteMDH(‘d’,d6,‘a’,0,‘alpha’,alp5);
robot=SerialLink(L,‘name’,‘robot’);
q=[q1 q2 q3 q4 q5 q6];
%工具坐标系下的几何雅可比矩阵,雅可比将关节空间速度映射到末端效应器速度(相对于基坐标系)。
je0=robot.jacob0(q);
je0=subs(je0,[cos(alp1),sin(alp1),cos(alp4),sin(alp4),cos(alp5),sin(alp5)],[0,1,0,1,0,-1]);
je0=simplify(je0);
je=robot.jacobe(q); %空间雅可比,即空间固定坐标形下的雅克比矩阵,参考现代机器人学5.1.1
je=subs(je,[cos(alp1),sin(alp1),cos(alp4),sin(alp4),cos(alp5),sin(alp5)],[0,1,0,1,0,-1]);
je=simplify(je);
T01=L(1).A(q(1));T12=L(2).A(q(2));T23=L(3).A(q(3));
T34=L(4).A(q(4));T45=L(5).A(q(5));T56=L(6).A(q(6));
T01=double(T01);T12=double(T12);T23=double(T23);
T34=double(T34);T45=double(T45);T56=double(T56);
%% parameters
syms db dt
%% transition matrix for robot
Tbase = [1 0 0 0;
0 1 0 0;
0 0 1 db;
0 0 0 1];
Ttool = [1 0 0 0;
0 1 0 0;
0 0 1 dt;
0 0 0 1];
T = {Tbase*T01 T12 T23 T34 T45 T56};
%%
U = Ttool ;
for i = 6👎1
Jn(:,i) = [ -U(1,1).*U(2,4)+U(2,1).*U(1,4);
-U(1,2).*U(2,4)+U(2,2).*U(1,4);
-U(1,3).*U(2,4)+U(2,3).*U(1,4);
U(3,1);
U(3,2);
U(3,3)];
U = T{i}*U;
end
%Jn 关节速度映射到世界坐标系下末端执行器空间速度,几何雅可比
Jn=subs(Jn,[cos(alp1),sin(alp1),cos(alp4),sin(alp4),cos(alp5),sin(alp5)],[0,1,0,1,0,-1]);
Jn=simplify(Jn)
Jn =
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-MZgxHrGU-1660735004146)(media/ef186a475bac2a9cf751347c644eba92.png)]
FK = Tbase * T01 * T12 * T23 * T34 * T45 * T56 * Ttool;
FK=subs(FK,[cos(alp1),sin(alp1),cos(alp4),sin(alp4),cos(alp5),sin(alp5)],[0,1,0,1,0,-1]);
FK=simplify(FK)
FK =
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-BYx8EWsk-1660735004147)(media/ec773b640730c680f17349e12e14b86f.png)]
FK66 = FK(1:3,1:3);
RR = [ FK66 zeros(3,3);
zeros(3,3) FK66];
Jn_Base = RR*Jn; %末端执行器自身坐标系的雅可比
-1660735004147)]
FK66 = FK(1:3,1:3);
RR = [ FK66 zeros(3,3);
zeros(3,3) FK66];
Jn_Base = RR*Jn; %末端执行器自身坐标系的雅可比
源文件下载链接
咨询链接:matlab正逆运动学分析与轨迹规划