%% 基于MATLAB的关节型六轴机械臂仿真
 
%% 参数定义
clear;
close all;
clc;
 
%角度转换
angle=pi/180;  %转化为角度制
 
%D-H参数表
theta1 = 0;   D1 = 0.4;   A1 = 0.025; alpha1 = pi/2; offset1 = 0;
theta2 = pi/2;D2 = 0;     A2 = 0.56;  alpha2 = 0;    offset2 = 0;
theta3 = 0;   D3 = 0;     A3 = 0.035; alpha3 = pi/2; offset3 = 0;
theta4 = 0;   D4 = 0.515; A4 = 0;     alpha4 = pi/2; offset4 = 0;
theta5 = pi;  D5 = 0;     A5 = 0;     alpha5 = pi/2; offset5 = 0;
theta6 = 0;   D6 = 0.08;  A6 = 0;     alpha6 = 0;    offset6 = 0;
 
%% DH法建立模型,关节转角，关节距离，连杆长度，连杆转角，关节类型（0转动，1移动），'standard'：建立标准型D-H参数
L(1) = Link([theta1, D1, A1, alpha1, offset1], 'standard')
L(2) = Link([theta2, D2, A2, alpha2, offset2], 'standard')
L(3) = Link([theta3, D3, A3, alpha3, offset3], 'standard')
L(4) = Link([theta4, D4, A4, alpha4, offset4], 'standard')
L(5) = Link([theta5, D5, A5, alpha5, offset5], 'standard')
L(6) = Link([theta6, D6, A6, alpha6, offset6], 'standard')
 
% 定义关节范围
L(1).qlim =[-180*angle, 180*angle];
L(2).qlim =[-180*angle, 180*angle];
L(3).qlim =[-180*angle, 180*angle];
L(4).qlim =[-180*angle, 180*angle];
L(5).qlim =[-180*angle, 180*angle];
L(6).qlim =[-180*angle, 180*angle];
zlim([-2,2]);
hold on;
robot2=SerialLink([L(1),L(2),L(3),L(4),L(5),L(6)],"base",transl(0,-1,0));
robot2.plot([0,0,0,0,0,0]);  
%% 显示机械臂（把上述连杆“串起来”）
robot0 = SerialLink(L,'name','six');
theta = [0 pi/2 0 0 pi 0];				%初始关节角度
figure(1)
robot0.plot(theta);
title('双臂机械人');

robot0 = SerialLink(L,'name','six');
T1=transl(0.5,0,0);						%根据给定起始点，得到起始点位姿
T2=transl(0,0.5,0);						%根据给定终止点，得到终止点位姿
init_ang=robot0.ikine(T1);				%根据起始点位姿，得到起始点关节角
targ_ang=robot0.ikine(T2);				%根据终止点位姿，得到终止点关节角
step = 15;
%% 显示机械臂（把上述连杆“串起来”）
robot0 = SerialLink(L,'name','six');
theta = [0 pi/2 0 0 pi 0];				%初始关节角度
figure(1)
robot0.plot(theta);
title('双臂机械人');
robot0 = SerialLink(L,'name','six');
%%
T1=transl(0.5,-1,0.5);						%根据给定起始点，得到起始点位姿
T2=transl(0.5,-0.9,0.6);						%根据给定终止点，得到终止点位姿
init_ang=robot2.ikine(T1);				%根据起始点位姿，得到起始点关节角
targ_ang=robot2.ikine(T2);				%根据终止点位姿，得到终止点关节角
step = 15;

%轨迹规划方法1
figure(1)
 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot2.fkine(q);						%根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot2.plot(q);
%%
T3=transl(0.5,-0.9,0.6);						%根据给定起始点，得到起始点位姿
T4=transl(0.5,-0.7,0.6);						%根据给定终止点，得到终止点位姿
init_ang=robot2.ikine(T3);				%根据起始点位姿，得到起始点关节角
targ_ang=robot2.ikine(T4);				%根据终止点位姿，得到终止点关节角
step = 15; 
% 轨迹规划方法2
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot2.fkine(q);						%根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot2.plot(q);
%%
T5=transl(0.5,-0.7,0.6);						%根据给定起始点，得到起始点位姿
T6=transl(0.5,-0.6,0.4);						%根据给定终止点，得到终止点位姿
init_ang=robot2.ikine(T5);				%根据起始点位姿，得到起始点关节角
targ_ang=robot2.ikine(T6);				%根据终止点位姿，得到终止点关节角
step = 15; 
% 轨迹规划方法3
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot2.fkine(q);						%根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot2.plot(q);
%%
T7=transl(0.5,-0.6,0.4);						%根据给定起始点，得到起始点位姿
T8=transl(0.5,-1,0);						%根据给定终止点，得到终止点位姿
init_ang=robot2.ikine(T7);				%根据起始点位姿，得到起始点关节角
targ_ang=robot2.ikine(T8);				%根据终止点位姿，得到终止点关节角
step = 15; 
%轨迹规划方法4
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot2.fkine(q);						%根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot2.plot(q);
%%
T9=transl(0.5,-1,0);						%根据给定起始点，得到起始点位姿
T10=transl(0.5,-1.4,0.4);						%根据给定终止点，得到终止点位姿
init_ang=robot2.ikine(T9);				%根据起始点位姿，得到起始点关节角
targ_ang=robot2.ikine(T10);				%根据终止点位姿，得到终止点关节角
step = 15; 
% 轨迹规划方法5
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot2.fkine(q);						%根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot2.plot(q);
%%
T10=transl(0.5,-1.4,0.4);						%根据给定起始点，得到起始点位姿
T11=transl(0.5,-1.3,0.6);						%根据给定终止点，得到终止点位姿
init_ang=robot2.ikine(T10);				%根据起始点位姿，得到起始点关节角
targ_ang=robot2.ikine(T11);				%根据终止点位姿，得到终止点关节角
step = 15; 
% 轨迹规划方法6
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot2.fkine(q);						%根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot2.plot(q);

%%
T12=transl(0.5,-1.3,0.6);                     %根据给定起始点，得到起始点位姿
T13=transl(0.5,-1.1,0.6);                     %根据给定终止点，得到终止点位姿
init_ang=robot2.ikine(T12);              %根据起始点位姿，得到起始点关节角
targ_ang=robot2.ikine(T13);              %根据终止点位姿，得到终止点关节角
step = 15;
%轨迹规划方法7
figure(1)
 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot2.fkine(q);                      %根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot2.plot(q);
%%
T14=transl(0.5,-1.1,0.6);						%根据给定起始点，得到起始点位姿
T15=transl(0.5,-1,0.5);						%根据给定终止点，得到终止点位姿
init_ang=robot2.ikine(T14);				%根据起始点位姿，得到起始点关节角
targ_ang=robot2.ikine(T15);				%根据终止点位姿，得到终止点关节角
step = 15;

%轨迹规划方法8
figure(1)
 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot2.fkine(q);						%根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot2.plot(q);
%%
T16=transl(0.5,-1,0.5);						%根据给定起始点，得到起始点位姿
T17=transl(0.5,-1,1);						%根据给定终止点，得到终止点位姿
init_ang=robot2.ikine(T16);				%根据起始点位姿，得到起始点关节角
targ_ang=robot2.ikine(T17);				%根据终止点位姿，得到终止点关节角
step = 15;

%轨迹规划方法8
figure(1)
 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot2.fkine(q);						%根据插值，得到末端执行器位姿
nT=T.T;
%plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot2.plot(q);
%%
T18=transl(0.5,0,0);                       %根据给定起始点，得到起始点位姿
T19=transl(0.5,0.2,0);                       %根据给定终止点，得到终止点位姿
init_ang=robot0.ikine(T18);              %根据起始点位姿，得到起始点关节角
targ_ang=robot0.ikine(T19);              %根据终止点位姿，得到终止点关节角
step = 15; 
%轨迹规划方法
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot0.fkine(q);                      %根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot0.plot(q);
%%
T20=transl(0.5,0.2,0);                       %根据给定起始点，得到起始点位姿
T21=transl(0.5,0.2,0.6);                        %根据给定终止点，得到终止点位姿
init_ang=robot0.ikine(T20);              %根据起始点位姿，得到起始点关节角
targ_ang=robot0.ikine(T21);             %根据终止点位姿，得到终止点关节角
step = 15; 
%轨迹规划方法
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot0.fkine(q);                      %根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot0.plot(q);
%%
T22=transl(0.5,0.2,0.6);                        %根据给定起始点，得到起始点位姿
T23=transl(0.5,0.1,0.6);                        %根据给定终止点，得到终止点位姿
init_ang=robot0.ikine(T22);             %根据起始点位姿，得到起始点关节角
targ_ang=robot0.ikine(T23);             %根据终止点位姿，得到终止点关节角
step = 15; 
%轨迹规划方法
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot0.fkine(q);                      %根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot0.plot(q);
%%
T24=transl(0.5,0.1,0.6);                       %根据给定起始点，得到起始点位姿
T25=transl(0.5,0.1,0.1);                       %根据给定终止点，得到终止点位姿
init_ang=robot0.ikine(T24);              %根据起始点位姿，得到起始点关节角
targ_ang=robot0.ikine(T25);              %根据终止点位姿，得到终止点关节角
step = 15; 
%轨迹规划方法
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot0.fkine(q);                      %根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot0.plot(q);
%%
T26=transl(0.5,0.1,0.1);                       %根据给定起始点，得到起始点位姿
T27=transl(0.5,-0.1,0.1);                        %根据给定终止点，得到终止点位姿
init_ang=robot0.ikine(T26);              %根据起始点位姿，得到起始点关节角
targ_ang=robot0.ikine(T27);             %根据终止点位姿，得到终止点关节角
step = 15; 
%轨迹规划方法
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot0.fkine(q);                      %根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot0.plot(q);
%%
T28=transl(0.5,-0.1,0.1);                        %根据给定起始点，得到起始点位姿
T29=transl(0.5,-0.1,0.6);                        %根据给定终止点，得到终止点位姿
init_ang=robot0.ikine(T28);             %根据起始点位姿，得到起始点关节角
targ_ang=robot0.ikine(T29);             %根据终止点位姿，得到终止点关节角
step = 15; 
%轨迹规划方法
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot0.fkine(q);                      %根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot0.plot(q);
%%
T30=transl(0.5,-0.1,0.6);                       %根据给定起始点，得到起始点位姿
T31=transl(0.5,-0.2,0.6);                        %根据给定终止点，得到终止点位姿
init_ang=robot0.ikine(T30);              %根据起始点位姿，得到起始点关节角
targ_ang=robot0.ikine(T31);             %根据终止点位姿，得到终止点关节角
step = 15; 
%轨迹规划方法
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot0.fkine(q);                      %根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot0.plot(q);
%%
T32=transl(0.5,-0.2,0.6);                        %根据给定起始点，得到起始点位姿
T33=transl(0.5,-0.2,0);                        %根据给定终止点，得到终止点位姿
init_ang=robot0.ikine(T32);             %根据起始点位姿，得到起始点关节角
targ_ang=robot0.ikine(T33);             %根据终止点位姿，得到终止点关节角
step = 15; 
%轨迹规划方法
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot0.fkine(q);                      %根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot0.plot(q);
%%
T34=transl(0.5,-0.2,0);                       %根据给定起始点，得到起始点位姿
T35=transl(0.5,0,0);                       %根据给定终止点，得到终止点位姿
init_ang=robot0.ikine(T34);              %根据起始点位姿，得到起始点关节角
targ_ang=robot0.ikine(T35);              %根据终止点位姿，得到终止点关节角
step = 15; 
%轨迹规划方法
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot0.fkine(q);                      %根据插值，得到末端执行器位姿
nT=T.T;
plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot0.plot(q);
%%
T36=transl(0.5,0,0);                       %根据给定起始点，得到起始点位姿
T37=transl(0.5,0,1);                        %根据给定终止点，得到终止点位姿
init_ang=robot0.ikine(T36);              %根据起始点位姿，得到起始点关节角
targ_ang=robot0.ikine(T37);             %根据终止点位姿，得到终止点关节角
step = 15; 
%轨迹规划方法
figure(1) 
%关节空间轨迹规划
[q ,qd, qdd]=jtraj(init_ang,targ_ang,step); %五次多项式轨迹，得到关节角度，角速度，角加速度，20为采样点个数
grid on
T=robot0.fkine(q);                      %根据插值，得到末端执行器位姿
nT=T.T;
%plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:)));%输出末端轨迹
robot0.plot(q);

以下是对上述 MATLAB 代码功能的解释：

一、初始化和参数定义

  1. 清除和关闭操作：
    • clear; 清除工作区中的变量。
    • close all; 关闭所有图形窗口。
    • clc; 清空命令窗口。
  2. 角度转换：
    • angle=pi/180; 将弧度转换为度的转换因子。
  3. D-H 参数表定义：
    • 定义了关节型六轴机械臂的 D-H 参数，包括 theta（关节转角）、D（关节距离）、A（连杆长度）、alpha（连杆转角）和 offset 等，这些参数描述了机械臂各连杆和关节的几何和运动学特性。

二、机械臂模型建立

  1. 建立连杆：
    • 通过 Link 函数结合 D-H 参数和 'standard' 类型，创建了 6 个连杆 L(1) 到 L(6)。
    • 为每个连杆设置关节范围 L(i).qlim，将角度范围从弧度转换为度表示。
    • zlim([-2,2]); 限制 z 轴的显示范围。
    • hold on; 用于在同一图形窗口中保持图形显示。
    • robot2=SerialLink([L(1),L(2),L(3),L(4),L(5),L(6)],"base",transl(0,-1,0)); 将连杆组合成一个机械臂对象 robot2，并设置了基坐标系的平移。
    • robot2.plot([0,0,0,0,0,0]); 绘制机械臂在关节角度为 [0,0,0,0,0,0] 时的初始姿态。
    • 还有 robot0 = SerialLink(L,'name','six'); 创建另一个机械臂对象 robot0，并命名为 'six'。

三、机械臂姿态显示和轨迹规划

  1. 初始姿态显示：
    • 多次使用 robot0.plot(theta); 或 robot2.plot(theta); 显示机械臂在不同初始关节角度 theta 下的姿态，如 theta = [0 pi/2 0 0 pi 0];。
  2. 逆运动学计算和轨迹规划：
    • 定义了多组起始点和终止点的位姿 T1 到 T37，通过 transl 函数将坐标转换为齐次变换矩阵表示。
    • 对于每组起始点和终止点，使用 ikine 函数进行逆运动学计算，得到起始点关节角 init_ang 和终止点关节角 targ_ang。
    • 对于每个轨迹规划部分：
      • [q,qd, qdd]=jtraj(init_ang,targ_ang,step); 利用 jtraj 函数进行关节空间的五次多项式轨迹规划，生成从起始关节角到终止关节角的关节角度 q、角速度 qd 和角加速度 qdd 轨迹，其中 step 表示采样点个数。
      • T=robot2.fkine(q); 或 T=robot0.fkine(q); 使用正运动学 fkine 函数根据关节角度轨迹计算末端执行器的位姿。
      • nT=T.T; 获取位姿矩阵的元素。
      • plot3(squeeze(nT(1,4,:)),squeeze(nT(2,4,:)),squeeze(nT(3,4,:))); 绘制末端执行器的轨迹，squeeze 函数用于去除多余的维度。
      • 最后使用 robot2.plot(q); 或 robot0.plot(q); 显示机械臂在关节角度轨迹 q 下的动画，展示机械臂沿着规划轨迹的运动。

总体而言，该代码实现了一个关节型六轴机械臂的仿真，包括以下几个主要方面：

  • 建立机械臂的运动学模型，使用 D-H 参数法。
  • 对机械臂进行不同初始姿态的显示。
  • 规划多组不同起始点和终止点之间的关节空间轨迹，使用五次多项式轨迹规划方法。
  • 对每组轨迹进行逆运动学求解得到关节角度，再通过正运动学得到末端执行器的位姿，并绘制末端执行器的轨迹。
  • 显示机械臂在不同轨迹下的运动动画，可视化机械臂的运动。
# matlab # 算法 # 人工智能
Logo
深开鸿 技术专区

更多推荐

cover

【技术解码】全视通“物联数据中台”如何打通医养结合全链路？

avatar 深开鸿 技术专区
cover

在瑞芯微RK平台下的开源鸿蒙OpenHarmony分区镜像/散包固件烧录

avatar 深开鸿 技术专区

iOS / 安卓做设备连接，鸿蒙做任务连续，这就是全场景最本质差别

前两年的鸿蒙全场景生态确实以华为自有设备为主，但在鸿蒙7上，基于开源鸿蒙（OpenHarmony）底座，鸿蒙的全场景版图正在不断扩大：海尔、美的、TCL等主流家电品牌，支持通过鸿蒙实况窗展示设备工作进程；随着鸿蒙生态不断完善，全场景协同早已覆盖手机、平板、电脑、手表、耳机、智慧屏、车机等多种设备，导航流转到车机、音乐流转到耳机、影音投智慧屏、手表承接步行导航。鸿蒙追求人不动，任务跟着你流转。鸿蒙键

avatar 深开鸿 技术专区