UR5/rocr6机械臂五次多项式轨迹规划

clear,clc,close all

format compact

robotModel=4;DH_Param;JointNum=length(DH);

ROCR6v2 关节2、3偏置

qlim=deg2rad([-179,179;-146,146;-146,146;-179,179;-179,179;-179,179]);

for i=1:JointNum

   L(i)=RevoluteMDH('d',DH(i,3),'a',DH(i,2),'alpha',deg2rad(DH(i,1)), ...

               'offset',deg2rad(DH(i,4)),'qlim',qlim(i,:));

end

robot=SerialLink(L,'name','robot');

robot.display();

robot =

robot:: 6 axis, RRRRRR, modDH, fastRNE                          

+---+-----------+-----------+-----------+-----------+-----------+

| j |     theta |         d |         a |     alpha |    offset |

+---+-----------+-----------+-----------+-----------+-----------+

|  1|         q1|     0.1215|          0|          0|          0|

|  2|         q2|     0.1225|          0|     1.5708|    -1.5708|

|  3|         q3|     -0.102|       -0.3|          0|          0|

|  4|         q4|       0.09|     -0.276|          0|    -1.5708|

|  5|         q5|       0.09|          0|     1.5708|          0|

|  6|         q6|      0.082|          0|    -1.5708|          0|

+---+-----------+-----------+-----------+-----------+-----------+

% 设置约束条件

Theta=[40,-30,-30,30,-15,20;

      0,15,0,10,20,50;

      -40,30,30,-20,30,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;

%% 求正解

T=robot.fkine(deg2rad(q));        %求正解，得到每次对应的空间位姿矩阵

JTA=transl(T);                  %空间位姿矩阵转化为位置矩阵

rpy=tr2rpy(T,'xyz');            % T中提取姿态（rpy）

% times=n*0.256*4; %ms

%% 绘制末端位置

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');

plot3(JTA(1:end,1), JTA(1:end,2), JTA(1:end,3),'b','LineWidth',1.5);

axis(W);   %设置坐标轴范围

hold on;grid on;

plot3(T1.t(1),T1.t(2),T1.t(3),'o','color','r','LineWidth',2);%#7E2F8E

plot3(T2.t(1),T2.t(2),T2.t(3),'o','color','r','LineWidth',2);

plot3(T3.t(1),T3.t(2),T3.t(3),'o','color','r','LineWidth',2);

% robot.plot(q*rad2deg,'tilesize',0.150,'workspace',W,'trail',{'r','LineWidth',3});

robot.model3d = 'ROCR6/MDH_stl';

robot.plot3d(deg2rad(q),'workspace',W,'movie','JointSpace.gif', ...

           'view',[24 30],'trail',{'r','LineWidth',3.5}) %显示三维动画

Animate: saving video --> JointSpace.gif with profile 'GIF'

Loading STL models from ARTE Robotics Toolbox for Education  by Arturo Gil (http://arvc.umh.es/arte).......

light('Position', [1 0.5 1]);

%% 绘制曲线图

figure('Color',[1,1,1],'Position',[485,180,536,325]);

for i=1:n

plot(SimpleNum,qT(i,1:Cunt),'LineWidth',2);hold on;

end

string='关节角度(deg)';

xlabel('t(s)'),ylabel(string,'Interpreter','tex','FontSize',10);grid on;

legend1 = legend('joint1 ','joint2','joint3','joint4','joint5','joint6');

set(legend1,...

   'Position',[0.06 0.94 0.89 0.06],...

   'Orientation','horizontal');

%%

figure('Color',[1,1,1],'Position',[485,180,536,325]);

for i=1:n

plot(SimpleNum,vT(i,1:Cunt),'LineWidth',2);hold on;

end

string='关节角速度(deg/s)';

xlabel('t(s)'),ylabel(string,'Interpreter','tex','FontSize',10);grid on;

legend('joint1 ','joint2','joint3','joint4','joint5','joint6');

%%

figure('Color',[1,1,1],'Position',[485,180,536,325]);

for i=1:n

plot(SimpleNum,aT(i,1:Cunt),'LineWidth',2);hold on;

end

string='关节角加速度(deg/s^2)';

xlabel('t(s)'),ylabel(string,'Interpreter','tex','FontSize',10);grid on;

legend('joint1 ','joint2','joint3','joint4','joint5','joint6');