机械臂-五次多项式与三次多项式对比

news2024/11/16 5:54:05

##1、三次多项式算法
代码如下：

L(1) = Link(  'd',     0.081,  'a' ,-0.01 , 'alpha',   pi/2 ,'offset',0);
L(2) = Link(  'd',     0 ,  'a' , 0.099 , 'alpha',   0 ,'offset',pi/2);
L(3) = Link(  'd',  0 ,  'a' , -0.01 , 'alpha',pi/2,'offset',pi/2);
L(4) = Link( 'd',  0.117+0.123,  'a' , 0 , 'alpha', pi/2 ,'offset',0);
L(5) = Link( 'd',0 ,  'a' , 0.057 , 'alpha',pi/2,'offset',-pi);
robot = SerialLink(L,'name','五自由度机械臂'); %建立连杆机器
robot.display;
robot.teach;       %画出模型并进行调控
robot.plot([0 0 0 0 0]) %输入一定参数后机器人图形
%%
Theta=[40,-30,-30,30,-15;
       0,15,0,10,20;
       -40,30,30,-20,30];
Velocity=[0,0,0,0,0,0;40,40,20,30,20,10;0,0,0,0,0,0];
Accle=[0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0];
[m,n]=size(Theta);
% 三次多项式轨迹规划，定义插补次数n,根据驱动器支持的最大插补次数设定
Time=[0,0,0,0,0,0;5,5,5,5,5,5;10,10,10,10,10,10];
StopTime=Time(3,1);
FixedStep=0.2;
SimpleNum=0:FixedStep:StopTime;Cunt=length(SimpleNum);
for i=1:n
    %% 求每个关节角的五次多项式插补轨迹点
    [q(:,i),qd(:,i),qdd(:,i)] = threejtraj_Function(Theta(:,i),Time(:,i), ...
                                Velocity(:,i),Accle(:,i)); 
end
qT=q';vT=qd';aT=qdd';t=2000;
%% 求正解
T=robot.fkine(deg2rad(q));        %求正解，得到每次对应的空间位姿矩阵
JTA=transl(T);                  %空间位姿矩阵转化为位置矩阵
%% 绘制末端位置
W=[-700,+700,-700,+700,0,+1000]*0.001;
% 求约束角度的正解，获取末端位姿
T1=robot.fkine(deg2rad(Theta(1,:))); 
T2=robot.fkine(deg2rad(Theta(2,:))); 
T3=robot.fkine(deg2rad(Theta(3,:))); 
% 动态仿真
% figure('Color',[1,1,1],'Position',[485,180,386,310],'Visible','on');
hold on
plot3(JTA(1:end,1), JTA(1:end,2), JTA(1:end,3),'b','LineWidth',1.5);
axis equal;   %设置坐标轴范围
hold on;grid on;
plot3(T1(1,4),T1(2,4),T1(3,4),'o','color','r','LineWidth',2);%#7E2F8E
plot3(T2(1,4),T2(2,4),T2(3,4),'o','color','r','LineWidth',2);
plot3(T3(1,4),T3(2,4),T3(3,4),'o','color','r','LineWidth',2);
robot.plot(q*(pi/180)); 

%%
figure(3)
i=1:5;
subplot(3,1,1);
plot(q(:,i));
legend('theta1','theta2','theta3','theta4','theta5')
title('位置');
grid on;
subplot(3,1,2);

plot(qd(:,i));
legend('theta1','theta2','theta3','theta4','theta5')
title('速度');
grid on;
subplot(3,1,3);

plot(qdd(:,i));
legend('theta1','theta2','theta3','theta4','theta5')
title('加速度');
grid on;

结果如下：
在这里插入图片描述

##2、五次多项式
代码如下：

clear
clc
close all

%标准DH
%Link(DH,option):
L(1) = Link(  'd',     0.081,  'a' ,-0.01 , 'alpha',   pi/2 ,'offset',0);
L(2) = Link(  'd',     0 ,  'a' , 0.099 , 'alpha',   0 ,'offset',pi/2);
L(3) = Link(  'd',  0 ,  'a' , -0.01 , 'alpha',pi/2,'offset',pi/2);
L(4) = Link( 'd',  0.117+0.123,  'a' , 0 , 'alpha', pi/2 ,'offset',0);
L(5) = Link( 'd',0 ,  'a' , 0.057 , 'alpha',pi/2,'offset',-pi);
% L(6) = Link( 'd',  0.225 ,  'a' , 0 , 'alpha',0,'offset',0);
robot = SerialLink(L,'name','五自由度机械臂'); %建立连杆机器
robot.display;
robot.teach;       %画出模型并进行调控
% robot.plot([0]) %输入一定参数后机器人图形
%%
%轨迹规划
Theta=[40,-30,-30,30,-15;
       0,15,0,10,20;
       -40,30,30,-20,30];
Velocity=[0,0,0,0,0,0;40,40,20,30,20,10;0,0,0,0,0,0];
Accle=[0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0];
[m,n]=size(Theta);
% 三次多项式轨迹规划，定义插补次数n,根据驱动器支持的最大插补次数设定
Time=[0,0,0,0,0,0;5,5,5,5,5,5;10,10,10,10,10,10];
StopTime=Time(3,1);
FixedStep=0.2;
SimpleNum=0:FixedStep:StopTime;Cunt=length(SimpleNum);
for i=1:n
    %% 求每个关节角的五次多项式插补轨迹点
    [q(:,i),qd(:,i),qdd(:,i)] = Fivejtraj_Function(Theta(:,i),Time(:,i), ...
                                Velocity(:,i),Accle(:,i),FixedStep); 
end
qT=q';vT=qd';aT=qdd';t=2000;
robot.plot(q*(pi/180)) 
Tc=robot.fkine(q*(pi/180))
Tjtraj=transl(Tc);
hold on
plot2(Tjtraj,'r');grid on;

figure(3)
i=1:5;
subplot(3,1,1);
plot(q(:,i));
legend('theta1','theta2','theta3','theta4','theta5')
title('位置');
grid on;
subplot(3,1,2);

plot(qd(:,i));
legend('theta1','theta2','theta3','theta4','theta5')
title('速度');
grid on;
subplot(3,1,3);

plot(qdd(:,i));
legend('theta1','theta2','theta3','theta4','theta5')
title('加速度');
grid on;