机械臂B样条插补轨迹规划

clear,clc,close all

format compact

DH=DH_Param(4);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|

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

%% 绘制三种类型的B样条曲线，需要前面所给的所有.m文件

%控制顶点

P=[150,50,   -120,  -223,  -258, -303;

    -480,-420, -460, -485,  -306,-265;

      340, 340,340,340,340,340]*0.001;

  P(1:2,:)=P(1:2,:)*1;

[m,n]=size(P);    

k = 3;FixedStep=0.0001;StopTime=1;

flag = 2;% 绘制准均匀B样条曲线

% figure('Color',[1,1,1],'Position',[485,180,684,330],'Visible','on');

position=Switch_flag(P,n-1,k,flag,StopTime,FixedStep);

T=position';

SimpleNum=(0:FixedStep:StopTime-FixedStep)*50;

Cunt=length(SimpleNum);q=zeros(6,Cunt)';

InitAngle=[-60.865,-12.697,-69.496,-7.8067,90,119.13]/180*pi*0;

RPY=deg2rad([180,0,-90]);

[X,Y,Z]=deal(zeros(1,Cunt));

for i=1:Cunt

%     if i==1

%         FK  = Pos2Tr_XYZ([T(i,:),RPY]);

%         %自己编写求逆解的函数，已判断是否在转动范围以内

%         [~,Theta,~]=InverseMDH_ZY(DH,FK,InitAngle);

%     else

       FK  = Pos2Tr_XYZ([T(i,:),RPY]);

       %自己编写求逆解的函数，已判断是否在转动范围以内

       [~,Theta,~]=InverseMDH_ZY(DH,FK,InitAngle);

       InitAngle=Theta;

%     end

   q(i,:)=Theta*180/pi;

end

qT=q';

% figure1=figure('Color',[1,1,1],'Position',[485,180,536,325],'Name','B样条末端运动轨迹');

W=[-700,+400,-500,+400,-0,+600]*0.001;

hold on

plot3(position(1,:)',position(2,:)',position(3,:)','b');hold on;grid on;axis(W);

q=q'*pi/180;save('SimBSplineData.mat','q');

xlabel('x(m)');ylabel('y(m)');zlabel('z(m)');

robot.model3d = 'ROCR6/MDH_stl';

qc=q(:,1:round(end/100):end);

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

fileName= ['CartesianSpace_',datestr(datetime('now'),'yyyy-mm-dd-HH-MM'),'.gif'];

robot.plot3d(qc','workspace',W,'movie',fileName, ...

           'view',[40 31],'delay',0.001,'trail',{'r','LineWidth',4},'nowrist') %显示三维动画

Animate: saving video --> CartesianSpace_2022-05-01-15-29.gif with profile 'GIF'

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

axes=gca;

axes.CameraViewAngleMode='manual';

axes.CameraViewAngle=7.5;

gf=gcf;

gf.Position=[400,300,720,420];

saveas(gf,'B样条末端运动轨迹','svg')

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

for i=1:6

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

end

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

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

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

figure2 = figure('Color',[1,1,1],'Position',[485,180,684,330],'Name','关节角度');

% 创建 subplot

for i=1:6

   string=['theta',num2str(i),' (deg)'];

   subplot1 = subplot(3,2,i,'Parent',figure2);

   plot(SimpleNum,qT(i,1:Cunt),'Parent',subplot1,'LineWidth',2,'Color',[1 0 0]);grid on;

   % 创建 ylabel

   ylabel(string,'Interpreter','tex','FontSize',10);

   % 创建 xlabel

   xlabel('t(s)','Interpreter','tex','FontSize',10);

end

saveas(figure2,figure2.Name,'svg')

源文件见付费内容：（图片如下）

matlab 正逆运动学分析与轨迹规划服务

需要技术服务联系qq2386317960