机械臂-五次多项式与三次多项式对比
##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;
结果如下：
