5自由度雄克机械臂仿真描点

news2024/11/27 13:27:50

5自由度雄克机械臂仿真描点

雄克机械臂

任务

建立雄克机械臂的坐标系和D-H参数表，使用Matlab机器人工具箱（Robotics Toolbox），用机械臂末端执行器触碰8个红色的目标点。

代码

%% 机器人学
format compact
close all
clear 
clc

%% DH参数
L1 = Link([ 0, 0, 0, 0, 0], 'modified');
L2 = Link([ 0, 0.35, 0, -pi/2, 0], 'modified');
L3 = Link([ 0, 0, 0, pi/2, 0], 'modified');
L4 = Link([ 0, 0.305, 0, -pi/2, 0], 'modified');
L5 = Link([ 0, 0, 0, pi/2, 0], 'modified');
L6 = Link([ 0, 0.3, 0, -pi/2, 0], 'modified');
L7 = Link([0 0 0 pi/2 0],'modified');
bot = SerialLink([L1 L2 L3 L4 L5 L6 L7], 'name', '雄克机械臂')
b=isrevolute(L1);

%% 齐次变换矩阵
R = 0.2;
L=0.4;
tar_posi = [ R,L,0;-R,L,0;0,L,R;0,L,-R;
                R*cos(pi/4), L, R*sin(pi/4);
                R*cos(pi/4), L, -R*sin(pi/4);
                -R*cos(pi/4), L, R*sin(pi/4);
                -R*cos(pi/4), L, -R*sin(pi/4)];
plot3(tar_posi(:,1),tar_posi(:,2),tar_posi(:,3),'Marker','o','MarkerFaceColor','c','MarkerSize',5);
T = zeros(8,4,4);
q = zeros(8,7);
for(i=1:8)
    T(i,:,:) = transl(tar_posi(i,:));
end

%% 关节角
q = zeros(9,7);
q(1,:) = [0 0 0 0 0 0 0];
q(2,:) = bot.ikine(reshape(T(1,:,:),4,4),'q0',q(1,:));
q(3,:) = bot.ikine(reshape(T(2,:,:),4,4),'q0',q(2,:));
q(4,:) = bot.ikine(reshape(T(3,:,:),4,4),'q0',q(3,:));
q(5,:) = bot.ikine(reshape(T(4,:,:),4,4),'q0',q(4,:));
q(6,:) = bot.ikine(reshape(T(5,:,:),4,4),'q0',q(5,:));
q(7,:) = bot.ikine(reshape(T(6,:,:),4,4),'q0',q(6,:));
q(8,:) = bot.ikine(reshape(T(7,:,:),4,4),'q0',q(7,:));
q(9,:) = bot.ikine(reshape(T(8,:,:),4,4),'q0',q(8,:));

%% 轨迹规划
Time=0:0.05:2;
for i = 1:8    
    tra=jtraj(q(i,:),q(i+1,:),Time);
    plot(bot,tra);
end