Matlab - Robotik Formelsammlung

From XennisWiki
Jump to: navigation, search

Der folgende Code stellt eine Sammlung von verschiedensten Formel zu der Thematik Robotik dar.

%
% Formelsammlung Robotik
% (Stand: Nov. 2012)
%
% Anmerkung:
%     - Alle Winkel sind in Grad!
%

%% ###### Allgemein #######

% Kosinus und Sinus (x: Winkel)
c = @(x) cosd(x);
s = @(x) sind(x);
v = @(x) 1 - c(x);


%% ###### Rotationsmatrizen #######

% Rotationsmatrix für z-Achse (g: Winkel)
RZY = @(g) [ c(g),  -s(g),  0;
             s(g),  c(g),   0;
             0 ,    0,      1 ];

% Rotationsmatrix Roll-Pitch-Yaw (a: X-Winkel, b: Y-Winkel, g: Z-Winkel)
RPY = @(a, b, g) [ c(g)*c(b),    c(g)*s(b)*s(a)-s(g)*c(a),   c(g)*s(b)*c(a)+s(g)*s(a);
                   s(g)*c(b),    s(g)*s(b)*s(a)+c(g)*c(a),   s(g)*s(b)*c(a)-c(g)*s(a);
                   -s(b),        c(b)*s(a),                  c(b)*c(a) ];               

               
%% ###### Quaternionen #######

% Konjugierte Quaternion (Q: Quaternion)
    % Alternativ kann man auch die Matlab-Funktion "quatconj" verwenden
quaternionConj = @(Q) [ Q(1) -Q(2) -Q(3) -Q(4) ];

% Inverse Quaternion (Q: Quaternion)
    % Alternativ kann man auch die Matlab-Funktion "quatinv" verwenden
quaternionInv = @(Q) quaternionConj(Q) / ( ( Q(1)^2 + Q(2)^2 + Q(3)^2 + Q(4)^2 ) )^(1/2);


%% ###### Umrechnung der verschiendenen Darstellungen #######

% Achse-Winkel-Darstellung in Rotationsmatrix (k: Achse, t: Winkel)
Umrechnung_R = @(k, t) [ k(1)^2*v(t)+c(t),            k(1)*k(2)*v(t)-k(3)*s(t)    k(1)*k(3)*v(t)+k(2)*s(t);
                         k(1)*k(2)*v(t)+k(3)*s(t),    k(2)^2*v(t)+c(t),           k(2)*k(3)*v(t)-k(1)*s(t);
                         k(1)*k(3)*v(t)-k(2)*s(t),    k(2)*k(3)*v(t)+k(1)*s(t),   k(3)^2*v(t)+c(t) ];

% Rotationsmatrix in Achse-Winkel-Darstellung (R: Rotationsmatrix, t: Winkel)
Umrechnung_t = @(R) acosd( ( R(1,1) + R(2,2) + R(3,3) - 1 ) / 2 );
Umrechnung_k = @(R, t) 1 / (2 * s(t)) * [ R(3,2)-R(2,3); R(1,3)-R(3,1); R(2,1)-R(1,2) ];

% Quaterion in Winkel / Achse von Achse-Winkel-Darstellung (Q: Quaterion, t: Winkel)
Umrechung_Q_to_t = @(Q) acosd(Q(1))*2;
Umrechung_Q_to_k = @(Q,t) Q(2:4)' / sind(t/2);


%% ###### Weiteres #######
        
% Homogene Matrix (R: Rotationsmatrix, u: Punkt)
homogeneMatrix = @(R, u) [ R u; zeros(1, length(R)) 1 ];

% Denavit-Hartenberg-Transformation - Vorwärstrechnung (a: Links distance, al: Link twist, d: Joint distance, t: Joint angle)
DH_A = @(a, al, d, t) [ c(t)   -c(al)*s(t)     s(al)*s(t)      a*c(t);
                        s(t)   c(al)*c(t)      -s(al)*c(t)     a*s(t);
                        0      s(al)           c(al)           d;
                        0      0               0               1 ];
%
% Anmerkung:
%     - Alle Winkel sind in Radiant!
%
c = @(x) cos(x);
s = @(x) sin(x);

% Vorwärstrechnung (a: Links distance, al: Link twist, d: Joint distance, t: Joint angle)
 A_iMinusEins_i = @(a, al, d, t) [ c(t),  -c(al)*s(t),    s(al)*s(t),     a*c(t);
                                   s(t),  c(al)*c(t),     -s(al)*c(t),    a*s(t);
                                   0,     s(al),          c(al),          d;
                                   0,     0,              0,              1];