KIDR CABAN.docx

Ćwiczenie nr 1 - metoda 1

Do wyznaczenia położenia punktu N wyznaczyliśmy przy pomocy programu MATLAB. Zadanie to zostało rozwiązane metodami numerycznymi. Po narysowaniu robota w układzie współrzędnych wyznaczyliśmy krańcowe położenie – położenie punktu N. Robot w programie został narysowany na zasadzie rysowania linii pomiędzy kolejnymi punktami w przestrzeni. Położenie punktu zostało wyznaczone z układów równań wprowadzonych do programu.

Kod programu:

function kinematics
% Function that solves the position and velocity

% Constants.
L1=0.65;
L2=0.75;
L3=0.60;

xA=0; yA=0; zA=0; xB=0; yB=0; zB=L1;
q1=pi/4;
q2=pi/3;
q3=pi/6;


% Motion parameters.
n=10;
step=0.1;
delta_q1=4*2*pi/60*step;
delta_q2=5*2*pi/60*step;
delta_q3=6*2*pi/60*step;

% Initial guess of the remaining coordinates.
x1=1;
y1=1;
z1=1;
x2=1;
y2=1;
z2=1;

for i=1:n

   % POSITION PROBLEM.

   % Initial value of the coordinates vector.
   q=[x1 y1 z1 x2 y2 z2]';
       phi=[L2*sin(q2)*cos(q1)-x1;
            L2*sin(q2)*sin(q1)-y1;
            zB+L2*cos(q2)-z1;
            L3*sin(q2-q3)*cos(q1)+x1-x2;
            L3*sin(q2-q3)*sin(q1)+y1-y2;
            z1+L3*cos(q2-q3)-z2];

   error=norm(phi);
   while error > 1.e-05

         Phiq=[-1 0 0 0 0 0;
                 0 -1 0 0 0 0;
                 0 0 -1 0 0 0;
                 1 0 0 -1 0 0;
                 0 1 0 0 -1 0;
                 0 0 1 0 0 -1];
 A=Phiq;
       b=-phi;
       sol=A\b;
       deltaq=sol;
       q=q+deltaq;
       x1=q(1); y1=q(2); z1=q(3); x2=q(4); y2=q(5); z2=q(6);

       phi=[L2*sin(q2)*cos(q1)-x1;
            L2*sin(q2)*sin(q1)-y1;
            zB+L2*cos(q2)-z1;
            L3*sin(q2-q3)*cos(q1)+x1-x2;
            L3*sin(q2-q3)*sin(q1)+y1-y2;
            z1+L3*cos(q2-q3)-z2];
       error=norm(phi);
   end
 

W MATLABIE policzyliśmy metodami numerycznymi położenie punktu N. Wyniki są następujące:

wpsolrzedne =

  x= 0.6714 [m]
  y= 0.6714 [m]
  z= 1.5446 [m]

Rys1. Schemat manipulatora

Ćwiczenie nr 2 - metoda 1

W tym ćwiczeniu należało określić składowe oraz wypadkowe prędkości i przyspieszenia punktu N. Zadanie to również opracowaliśmy w programie MATLAB stosując metody numeryczne. Prędkości przyspieszenia zostały policzone na zasadzie różnic pomiędzy kolejnymi położeniami w czasie. Program liczy pozycje w 3 chwilach, a potem liczy pochodne między tymi pozycjami.

Kod programu:

   %% Postion solution for first step
   if i==1
   wpsolrzedne=q
   tx2=x2;
   tz2=z2;
   ty2=y2;
   end
   % Velocity solution for second step
   if i==2
       vx=(tx2-x2)/step
       vy=(tz2-z2)/step
       vz=(ty2-y2)/step
       predkosc=sqrt(vx^2+vy^2+vz^2)
   end

   %% Visualization

figure(1);
clf;
hold on;
A=[xA yA zA];
B=[xB yB zB];
C=[x1 y1 z1];
D=[x2 y2 z2];

x=[A(1) B(1)];
y=[A(2) B(2)];
z=[A(3) B(3)];
line(x,y,z);
x=[B(1) C(1)];
y=[B(2) C(2)];
z=[B(3) C(3)];
line(x,y,z);
x=[C(1) D(1)];
y=[C(2) D(2)];
z=[C(3) D(3)];
line(x,y,z);

grid...