Matlab - Inkrementelle Rückwärtsrechnung

From XennisWiki
Jump to: navigation, search

Iterationsschritte der inkrementellen Rückwärtsrechnung

Anmerkung: Bei dem verwendetem Script rob_functions handelt es sich um die Matlab - Robotik Formelsammlung

rob_functions

% ------ Gegeben ------

% Armlängen
l1 = 100;
l2 = 50;
l3 = 150;

% Zielstellung: Position (0_o_3) und Orientierung (0_R_3)
o_ziel = [ 93.2834;  93.2834;    288.4050 ];
R_ziel = [ 0.5792,   -0.7071,    0.4056;
           0.5792,   0.7071,     0.4056;
          -0.5736,  0,          0.8192 ];

% Startstellung: Gelenkwinkel (t_1=40°, t_2=40°, d_3=20mm)
theta = [ degtorad(40); degtorad(40); 20 ];


% ------ Berechnung ------

thetaSym = sym('t%d',[3 1]);

A_0_1 = A_iMinusEins_i( 0,    -pi/2,   l1,                  thetaSym(1) );
A_1_2 = A_iMinusEins_i( 0,    pi/2,    0,                   thetaSym(2) );
A_2_3 = A_iMinusEins_i( 0,    0,       l2+l3+thetaSym(3),   0 );

A_0_3 = A_0_1 * A_1_2 * A_2_3;

% Jakobimatrix
J = jacobian( A_0_3, thetaSym );
save J

% Homogene Matrix (0_A_3) der Zieldarstellung bestimmen
A_ziel = homogeneMatrix( R_ziel, o_ziel );

% Iterationsschritte
while true

    % Start
    A_0_3_Start = subs( A_0_3, thetaSym, theta );
    % Differenz von Ziel- und Startmatrix
    A_0_3_Diff = A_ziel - A_0_3_Start;
    
    da = [ A_0_3_Diff(:,1);
           A_0_3_Diff(:,2);
           A_0_3_Diff(:,3);
           A_0_3_Diff(:,4);];

    J_aktuell = subs( J, thetaSym, theta ); % Theta einsetzen
    
    dTheta = pinv( J_aktuell ) * da;
    theta = theta + dTheta;
    
    % Abbruchsbedingung
    if norm( dTheta ) <= 0.01
        break
    end
    
end

disp('Ergebnis')
theta

disp('Ergebnis überprüfen')
A_0_3_test = subs( A_0_3, thetaSym, theta )

A_ziel

Ausgabe

Ergebnis

theta =

    0.7854
    0.6109
   30.0001

Ergebnis überprüfen

A_0_3_test =

    0.5792   -0.7071    0.4056   93.2834
    0.5792    0.7071    0.4056   93.2834
   -0.5736    0.0000    0.8192  288.4050
         0         0         0    1.0000


A_ziel =

    0.5792   -0.7071    0.4056   93.2834
    0.5792    0.7071    0.4056   93.2834
   -0.5736         0    0.8192  288.4050
         0         0         0    1.0000

See also