Файл: Минобрнауки россии федеральное государственное бюджетное образовательное учреждение высшего образования Балтийский государственный технический университет военмех.docx

Приложения

Приложение А

functionA=DH11()

symsteta alpha1 a d;

A = [cos(teta) -cos(alpha1)*sin(teta) sin(alpha1)*sin(teta)

a*cos(teta);

sin(teta) cos(alpha1)*cos(teta) -sin(alpha1)*cos(teta) a*sin(teta);

0 sin(alpha1) cos(alpha1) d;

0 0 0 1];
end

functionTDH_solve(q)

B=DH11();
teta=q(1); alpha1=-pi/2 ;a=0; d=300;

A1=subs(B);
teta=q(2)-pi/2; alpha1=pi/2 ;a=0; d=3000;

A2=subs(B);
teta=q(3)+pi/2; alpha1=pi/2 ;a=0; d=3000;

A3=subs(B);
teta=q(4)+pi/2; alpha1=pi/2 ;a=0; d=700;

A4=subs(B);
teta=q(5); alpha1=-pi/2; a=0; d=700;

A5=subs(B);
teta=q(6); alpha1=0 ;a=0; d=500;

A6=subs(B);

T=A1*A2*A3*A4*A5*A6

p0=[0; 0; 0; 1];

p=T*p0

end

Приложение Б
l=[0.300 3.0 3.0 0.700 0.700 0.500];

robot = rigidBodyTree;

body1 = rigidBody('body1');

body2 = rigidBody('body2');

body3 = rigidBody('body3');

body4 = rigidBody('body4');

body5 = rigidBody('body5');

body6 = rigidBody('body6');
jnt1 = rigidBodyJoint('jnt1','revolute');

jnt2 = rigidBodyJoint('jnt2','revolute');

jnt3 = rigidBodyJoint('jnt3','revolute');

jnt4 = rigidBodyJoint('jnt4','revolute');

jnt5 = rigidBodyJoint('jnt5','revolute');

jnt6 = rigidBodyJoint('jnt6','revolute');
DH=[0 -pi/2 0 l(1)

-pi/2 0 l(2) 0

0 pi/2 l(3) 0

0 -pi/2 l(4) 0

0+pi/2 pi/2 0 0

0 0 0 l(5)+l(6)];

dhparams = [DH(:,3),DH(:,2),DH(:,4),DH(:,1)];

setFixedTransform(jnt1,dhparams(1,:),'dh');

setFixedTransform(jnt2,dhparams(2,:),'dh');

setFixedTransform(jnt3,dhparams(3,:),'dh');

setFixedTransform(jnt4,dhparams(4,:),'dh');

setFixedTransform(jnt5,dhparams(5,:),'dh');

setFixedTransform(jnt6,dhparams(6,:),'dh');

body1.Joint = jnt1;

body2.Joint = jnt2;

body3.Joint = jnt3;

body4.Joint = jnt4;

body5.Joint = jnt5;

body6.Joint = jnt6;
addBody(robot,body1,'base')

addBody(robot,body2,'body1')

addBody(robot,body3,'body2')

addBody(robot,body4,'body3')

addBody(robot,body5,'body4')

addBody(robot,body6,'body5')
addVisual(body1,"cylinder",[1 0.3])

addVisual(body2,"cylinder",[1 3])

addVisual(body3,"cylinder",[1 3])

addVisual(body4,"cylinder",[1 0.7])

addVisual(body5,"cylinder",[1 0.7])

addVisual(body6,"cylinder",[1 0.5])
q0 = homeConfiguration(robot);

q0(2).JointPosition=-pi/2;

q0(5).JointPosition=pi/2;
% q0(4).JointPosition=0.2;

% q0(6).JointPosition=0.2;

figure(1)

show(robot,q0)
xlim([-9 9])

ylim([-9 9])

zlim([0 9])

Приложение В
l=[0.300 3.0 3.0 0.700 0.700 0.500];

robot = rigidBodyTree;

body1 = rigidBody('body1');

body2 = rigidBody('body2');

body3 = rigidBody('body3');

body4 = rigidBody('body4');

body5 = rigidBody('body5');

body6 = rigidBody('body6');

jnt1 = rigidBodyJoint('jnt1','revolute');

jnt2 = rigidBodyJoint('jnt2','revolute');

jnt3 = rigidBodyJoint('jnt3','revolute');

jnt4 = rigidBodyJoint('jnt4','revolute');

jnt5 = rigidBodyJoint('jnt5','revolute');

jnt6 = rigidBodyJoint('jnt6','revolute');

DH=[0 -pi/2 0 l(1)

-pi/2 0 l(2) 0

0 pi/2 l(3) 0

0 -pi/2 l(4) 0

0+pi/2 pi/2 0 0

0 0 0 l(5)+l(6)];

dhparams = [DH(:,3),DH(:,2),DH(:,4),DH(:,1)];

setFixedTransform(jnt1,dhparams(1,:),'dh');

setFixedTransform(jnt2,dhparams(2,:),'dh');

setFixedTransform(jnt3,dhparams(3,:),'dh');

setFixedTransform(jnt4,dhparams(4,:),'dh');

setFixedTransform(jnt5,dhparams(5,:),'dh');

setFixedTransform(jnt6,dhparams(6,:),'dh');

body1.Joint = jnt1;

body2.Joint = jnt2;

body3.Joint = jnt3;

body4.Joint = jnt4;

body5.Joint = jnt5;

body6.Joint = jnt6;

addBody(robot,body1,'base')

addBody(robot,body2,'body1')

addBody(robot,body3,'body2')

addBody(robot,body4,'body3')

addBody(robot,body5,'body4')

addBody(robot,body6,'body5')

q0 = homeConfiguration(robot);

q0(2).JointPosition=-pi/2;

---

q0(5).JointPosition=pi/2;

% q0(4).JointPosition=0.2;

% q0(6).JointPosition=0.2;

figure(1)

show(robot,q0)

aik=analyticalInverseKinematics(robot);

opts = showdetails(aik);

q=[0;0;0;0;0;0];

Pr=[0;0;0;3;3;3];

ik = inverseKinematics('RigidBodyTree',robot);

weights = [0.25 0.25 0.25 1 1 1];

initialguess = robot.homeConfiguration;

%матрица инструмента

tform = [1 0 0 3;0 1 0 3;0 0 1 3;0 0 0 1];

%решение ОЗК

[configSoln,solnInfo] = ik('body6',tform,weights,initialguess)

figure(2)

show(robot,configSoln)

% xlim([-4.5,4.5])

% ylim([-4.5,4.5])

% zlim([-1,5])
env = {};

rrt = manipulatorRRT(robot,env);

startConfig = [0 0 0 0 0 0];

goalConfig = [configSoln(1).JointPosition configSoln(2).JointPosition configSoln(3).JointPosition configSoln(4).JointPosition configSoln(5).JointPosition configSoln(6).JointPosition ];
rng(0)

path = plan(rrt,startConfig,goalConfig);
interpPath = interpolate(rrt,path);

point=570;

size=point/5;

int=size/point;

t=[0:int:size-int];

c=t(2)-t(1);

k=570;

q1=interpPath(:,1);

q2=interpPath(:,2);

q3=interpPath(:,3);

q4=interpPath(:,4);

q5=interpPath(:,5);

q6=interpPath(:,6);
figure(3)

tiledlayout(3,2)
% 1 plot

nexttile

plot(t,q1)

title('Обобщённая координата 1')

xlabel('Время, с')

ylabel('Обобщённая координата, рад')

grid on
% 2 plot

nexttile

plot(t,q2)

title('Обобщённая координата 2')

xlabel('Время, с')

ylabel('Обобщённая координата, рад')

grid on
% 3 plot

nexttile

plot(t,q3)

title('Обобщённая координата 3')

xlabel('Время, с')

ylabel('Обобщённая координата, рад')

grid on
% 4 plot

nexttile

plot(t,q4)

title('Обобщённая координата 4')

xlabel('Время, с')

ylabel('Обобщённая координата, рад')

grid on
% 5 plot

nexttile

plot(t,q5)

title('Обобщённая координата 5')

xlabel('Время, с')

ylabel('Обобщённая координата, рад')

grid on
% 6 plot

nexttile

plot(t,q6)

title('Обобщённая координата 6')

xlabel('Время, с')

ylabel('Обобщённая координата, рад')

grid on
v1=gradient(q1,c);

v2=gradient(q2,c);

v3=gradient(q3,c);

v4=gradient(q4,c);

v5=gradient(q5,c);

v6=gradient(q6,c);
figure(4)

tiledlayout(3,2)
% 1 plot

nexttile

plot(t,v1)

title('Скорость обобщённой координаты 1')

xlabel('Время, с')

ylabel('Скорость, м/c')

grid on
% 2 plot

nexttile

plot(t,v2)

title('Скорость обобщённой координаты 2')

xlabel('Время, с')

ylabel('Скорость, м/c')

grid on
% 3 plot

nexttile

plot(t,v3)

title('Скорость обобщённой координаты 3')

xlabel('Время, с')

ylabel('Скорость, м/c')

grid on
% 4 plot

nexttile

plot(t,v4)

title('Скорость обобщённой координаты 4')

xlabel('Время, с')

ylabel('Скорость, м/c')

grid on
% 5 plot

nexttile

plot(t,v5)

title('Скорость обобщённой координаты 5')

xlabel('Время, с')

ylabel('Скорость, м/c')

grid on
% 6 plot

nexttile

plot(t,v6)

title('Скорость обобщённой координаты 6')

xlabel('Время, с')

ylabel('Скорость, м/c')

grid on
a1=gradient(v1,c);

a2=gradient(v2,c);

a3=gradient(v3,c);

a4=gradient(v4,c);

a5=gradient(v5,c);

a6=gradient(v6,c);
figure(5)

tiledlayout(3,2)
% 1 plot

nexttile

plot(t,a1)

title('Ускорение обобщённой координаты 1')

---

xlabel('Время, с')

ylabel('Ускорение, м/с^2')

grid on
% 2 plot

nexttile

plot(t,a2)

title('Ускорение обобщённой координаты 2')

xlabel('Время, с')

ylabel('Ускорение, м/с^2')

grid on
% 3 plot

nexttile

plot(t,a3)

title('Ускорение обобщённой координаты 3')

xlabel('Время, с')

ylabel('Ускорение, м/с^2')

grid on
% 4 plot

nexttile

plot(t,a4)

title('Ускорение обобщённой координаты 4')

xlabel('Время, с')

ylabel('Ускорение, м/с^2')

grid on
% 5 plot

nexttile

plot(t,a5)

title('Ускорение обобщённой координаты 5')

xlabel('Время, с')

ylabel('Ускорение, м/с^2')

grid on
% 6 plot

nexttile

plot(t,a6)

title('Ускорение обобщённой координаты 6')

xlabel('Время, с')

ylabel('Ускорение, м/с^2')

grid on


Приложение Г
ERROR=[];

Delta= [1e-2*ones(3,1); 1e-3*ones(3,1)];

N=10;

forq1=linspace(-pi/3,pi/3,N)

q1

forq2=linspace(-pi/3,pi/3,N)

forq3=linspace(-pi/3,pi/3,N)

q0(1).JointPosition=q1;

q0(2).JointPosition=-pi/2+q2;

q0(3).JointPosition=q3;

Jacob = geometricJacobian(robot,q0,'body6');

Error=inv(Jacob)*Delta;

ERROR=[ERROR;Error'];

% cla

% show(robot,q0);

% pause(0.01)

end

end

end

Jacob

min(abs(ERROR))
figure(1)

show(robot,q0)
xlim([-9 9])

ylim([-9 9])

zlim([0 9])

Приложение Е

---