机械臂B样条插补+带源代码

Copyright © 2022AC今夜有雨. All rights reserved.

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§;

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’)

g,‘Interpreter’,‘tex’,‘FontSize’,10);

% 创建 xlabel

xlabel(‘t(s)’,‘Interpreter’,‘tex’,‘FontSize’,10);

end

saveas(figure2,figure2.Name,‘svg’)

源文件下载链接
咨询链接：matlab正逆运动学分析与轨迹规划