Modelo Cinemático Inverso - Laboratorio Análisis y Simulación de Robot Industrial

Cálculo de cinemática inversa con Método geométrico y Análisis de matrices de rotación

Para hallar el modelo geométrico de un robot ABB IRB 1600 se debe tener en cuenta que se pueden presentar varias configuraciones. En primer lugar, se calcula con una configuración codo arriba la cual se ve representada como se muestra en la figura a continuación:

Debido a que el parámetro inicial es la coordenada del efector final y se desea realizar el cálculo con método geométrico, es útil hallar primero la posición de la muñeca, para esto, se le resta al vector de posición del efector final el vector desde el TCP hasta la muñeca de la siguiente manera:

(1)

\begin{align} \vec{p_e} = x_e \hat{\imath} + y_e \hat{\jmath} + z_e \hat{k} \end{align}

(2)

\begin{align} \vec{p_w} = \vec{p_e} - L_5 \vec{a_6} = x_w \hat{\imath} + y_w \hat{\jmath} + z_w \hat{k} \end{align}

(3)

\begin{equation} q_1 = atan2(y_w,x_w) \end{equation}

Donde:

$$p_e$$ , es la coordenada del efector final (EF) y $$x_e$$ , $$y_e$$ , $$z_e$$ sus respectivas componentes.
$$p_w$$ , es la coordenada de la muñeca y $$x_w$$ , $$y_w$$ , $$z_w$$ sus respectivas componentes.
$$L_5$$ , la distancia del TCP a la articulación 6.
$\vec{a_6}$ , la dirección aproximación del EF.

% Valores de prueba
xe = 750/100;
ye = 100/100;
ze = 593/100;
roll = -45;
pitch = 0;
yaw = -90;
 
R0T = rpy2r(roll, pitch, yaw, 'deg');   % Matriz rotación efector final
 
pe = [xe; ye; ze];      % Coordenadas efector final
pw = pe - L5*R0T(:,3);  % Coordenadas muñeca
xw = pw(1);             % Coordenada x muñeca
yw = pw(2);             % Coordenada y muñeca
zw = pw(3);             % Coordenada z muñeca
 
q1 = atan2(yw,xw)       % Valor q1

Ahora, se calculan los valores de las distancias $$r$$ , $L_{xy}$ , $$L_z$$ y $$L_p$$ que se enseñan en la figura:

(4)

\begin{align} r = \sqrt{x_w^2 + y_w^2} \end{align}

(5)

\begin{equation} L_{xy} = r - L_2 \end{equation}

(6)

\begin{equation} L_z = z_w - L_1 \end{equation}

(7)

\begin{align} L_p = \sqrt{L_{xy}^2 + L_z^2} \end{align}

r = sqrt(xw^2 + yw^2);
Lxy = r - L2;
Lz = zw - L1;
Lp = sqrt(Lxy^2 + Lz^2);

Con la anterior, ya se puede calcular el valor de $$q_2$$ teniendo en cuenta los ángulos $\alpha$ , $\beta$ y las correspondientes identidades trigonométricas.

(8)

\begin{align} \alpha = atan2(L_z, L_{xy}) \end{align}

Usando la ley del coseno y despejando $\beta$ :
$L_4^2 = L_3^2 + L_p^2 - 2 L_3 L_p cos \beta$

(9)

\begin{align} cos \beta = \frac{L_3^2 + L_p^2 - L_4^2}{2 L_3 L_p} \end{align}

Al expresar el $sen \beta$ en términos de $cos \beta$ queda:

(10)

\begin{align} sen \beta = \sqrt{1 - cos \beta^2} \end{align}

(11)

\begin{align} \beta = atan2(sen \beta, cos \beta) \end{align}

Por consiguiente, encuentra que:

(12)

\begin{align} q2 = -\left(\frac{\pi}{2} - (\alpha + \beta)\right) \end{align}

alpha = atan2(Lz, Lxy);
cosB = (L3^2 + Lp^2 - L4^2)/(2*L3*Lp);
senB = sqrt(1 - cosB^2);
beta = atan2(senB, cosB);
q2 = -((pi/2) - (alpha + beta)) % Valor q2

El último valor que se encuentra a través del método geométrico es $$q_3$$ , el cual se halla por medio de los ángulos $\gamma$ y $\phi$ . Para esto, se realiza un procedimiento similar al anterior.

(13)

\begin{align} cos \gamma = \frac{L_3^2 + L_4^2 - L_p^2}{2L_3L_4} \end{align}

(14)

\begin{align} sen \gamma = \sqrt{1 - cos \gamma^2} \end{align}

(15)

\begin{align} \gamma = atan2(sen \gamma, cos \gamma) \end{align}

(16)

\begin{align} \phi = \pi - \gamma \end{align}

Obteniendo que:

(17)

\begin{align} q_3 = \frac{\pi}{2} - \phi \end{align}

cosY = (L3^2 + L4^2 - Lp^2)/(2*L3*L4);
senY = sqrt(1 - cosY^2);
gama = atan2(senY, cosY);
phi = pi - gama;
q3 = (pi/2) - phi       % Valor q3

Para calcular los valores de $$q_4$$ , $$q_5$$ y $$q_6$$ se tuvo en cuenta que:

(18)

\begin{align} (_{}^0R_3)^{-1 \; 0}R_6 = _{}^3R_6 \end{align}

Para calcular $_{}^0R_3$ se usaron los valores de $$q_1$$ , $$q_2$$ y $$q_3$$ encontrados anteriormente y en el caso de $_{}^0R_6$ se realizó la siguiente operación:

(19)

\begin{align} _{}^0R_6 = _{}^0R_T (_{}^6R_T)^{-1} = _{}^0R_T\;^TR_6 \end{align}

Donde $_{}^0R_T$ es la matriz de rotación del efector final con referencia en la base y $_{}^6R_T$ es la matriz de rotación del efector final visto desde la articulación 6 (la cual está definida como $$Robot.tool$$ ).

MTH03 = L(1).A(q1)*L(2).A(q2)*L(3).A(q3) % MTH de 3 con respecto a la base

[R03, P03] = tr2rt(MTH03);              % Separar rotación y translación
[RT6, PT6] = tr2rt(Robot.tool^(-1));    % Matriz de rotación de 6 con respecto al TCP
 
R06 = R0T*RT6;                          % Matriz de rotación de 6 con respecto a la base
R36 = (R03^(-1))*R06;                   % Matriz de rotación de 6 respecto a 3

Debido a que $_{}^3R_6$ es de la forma:

syms q4 q5 q6
threshold = 1e-10;
 
MTH34=mapSymType(L(4).A(q4),'rational', @(x) piecewise(abs(x)<=threshold, 0, x));
MTH45=mapSymType(L(5).A(q5),'rational', @(x) piecewise(abs(x)<=threshold, 0, x));
MTH56=mapSymType(L(6).A(q6),'rational', @(x) piecewise(abs(x)<=threshold, 0, x));
 
MTH36_sim = MTH34*MTH45*MTH56                 % Matriz de rotación de 6 respecto a 3 simbólica

Esta se puede igualar al valor numérico obtenido anteriormente para hallar los $$q$$ faltantes. Reescribiendo $_{}^3R_6$ en términos de las posiciones:

(20)

\begin{align} _{}^3R_6 = \begin{bmatrix} r_{11}& r_{12} & r_{13} \\ r_{21}& r_{22} & r_{23}\\ r_{31}& r_{32} & r_{33}\\ \end{bmatrix} \end{align}

Queda para $$q_4$$ que:

(21)

\begin{align} q_4 = atan2(r_{33},r_{13})= atan2 \left( \frac{sin (q_4) sin (q_5)}{cos (q_4) sin (q_5)} \right) \end{align}

En el caso de $$q_5$$ , posición angular de la articulación 5:

(22)

\begin{equation} c_5 = -r_{23}=-cos(q5) \end{equation}

(23)

\begin{align} s_5 = \sqrt{1 - c_5^2} \end{align}

(24)

\begin{equation} q_5 = atan2(s_5,c_5) \end{equation}

Y para el $$q$$ de la última articulación:

(25)

\begin{align} q_6 = atan2(-r{22},r{21})= atan2 \left( \frac{-sin (q_6) sin (q_5)}{cos (q_6) sin (q_5)} \right) \end{align}

% Valor q4 
q4 = atan2(R36(3,3),R36(1,3))
 
% Valor q5 
c5 = -R36(2,3);
s5 = sqrt(1 - c5^2);
q5 = atan2(s5,c5)
 
% Valor q6 
q6 = atan2(-R36(2,2),R36(2,1))
 
% Vector de posiciones articulares en radianes
q_rad = [q1 q2 q3 q4 q5 q6]
% Vector de posiciones articulares en grados
q_deg = [rad2deg(q1) rad2deg(q2) rad2deg(q3) rad2deg(q4) rad2deg(q5) rad2deg(q6)]

Como se puede ver en la figura a continuación, los valores de posición y orientación son correctos. Allí se muestra además los valor de $$q$$ en la interfaz de Peter Corke.

Robot.teach([q1 q2 q3 q4 q5 q6])        % Graficar el robot con el vector q
rpy = rad2deg(tr2rpy(Robot.fkine([q1 q2 q3 q4 q5 q6])));   % Ángulos RPY

Otra configuración que también se puede obtener es la de codo abajo, en la cual el manipulador se ubica tal y como como se muestra en la figura.

En este caso cambia la forma en la que están definidos $$q_2$$ y $$q_3$$ , sin modificar la expresión matemática para $\alpha$ , $\beta$ y $\phi$ , quedando:

(26)

\begin{align} q_2 = -\left(\frac{\pi}{2} - (\alpha - \beta)\right) \end{align}

(27)

\begin{align} q_3 = \frac{\pi}{2} + \phi \end{align}

% Valor q2 
q2 = -((pi/2) - (alpha - beta));
% Valor q3 
q3 = (pi/2) + phi;

Debido al cambio que se realizó para $$q_2$$ y $$q_3$$ , $_{}^0R_3$ se modifica y, por lo tanto, se debe calcular nuevamente $$q_4$$ , $$q_5$$ y $$q_6$$ , entonces:

MTH03 = L(1).A(q1)*L(2).A(q2)*L(3).A(q3); % MTH de 3 con respecto a la base
[R03, P03] = tr2rt(MTH03);              % Separar rotación y translación
[RT6, PT6] = tr2rt(Robot.tool^(-1));    % Matriz de rotación de 6 con respecto al TCP
 
R06 = R0T*RT6;                          % Matriz de rotación de 6 con respecto a la base
R36 = (R03^(-1))*R06;                   % Matriz de rotación de 6 respecto a 3
% Valor q4 
q4 = atan2(R36(3,3),R36(1,3));
% Valor q5 
c5 = -R36(2,3);
s5 = sqrt(1 - c5^2);
q5 = atan2(s5,c5);
% Valor q6 
q6 = atan2(-R36(2,2),R36(2,1));
 
% Vector de posiciones articulares en radianes
q_rad = [q1 q2 q3 q4 q5 q6]
% Vector de posiciones articulares en grados
q_deg = [rad2deg(q1) rad2deg(q2) rad2deg(q3) rad2deg(q4) rad2deg(q5) rad2deg(q6)]

En este caso también se logra apreciar que los valores de posición y orientación son los deseados.

clf
Robot.teach([q1 q2 q3 q4 q5 q6])        % Graficar el robot con el vector q
rpy = rad2deg(tr2rpy(Robot.fkine([q1 q2 q3 q4 q5 q6])));   % Ángulos RPY

Además de las configuraciones ya presentadas anteriormente, se encuentran otras dos configuraciones las cuales corresponden a la base del robot rotada ±180º (a partir del ángulo de la base obtenido en las dos configuraciones anteriores) y el brazo se doblado hacia atrás en configuraciones codo arriba y/o codo abajo. A continuación se representa la de codo arriba con base rotada.

Para esta configuración cambia la definición de la longitud $L_{xy}$ y las tres primeras posiciones articulares:

(28)

\begin{equation} L_{xy} = r + L_2 \end{equation}

(29)

\begin{equation} q_1 = atan2(-y_w,-x_w) \end{equation}

(30)

\begin{align} q_2 = \frac{\pi}{2} - (\alpha + \beta) \end{align}

(31)

\begin{align} q_3 = \frac{\pi}{2} + \phi \end{align}

Por lo tanto, al realizar dichas modificaciones:

Lxy = r + L2;
Lp = sqrt(Lxy^2 + Lz^2);
 
q1 = atan2(-yw,-xw);                    % Valor q1
 
alpha = atan2(Lz, Lxy);
cosB = (L3^2 + Lp^2 - L4^2)/(2*L3*Lp);
senB = sqrt(1 - cosB^2);
beta = atan2(senB, cosB);
q2 = ((pi/2) - (alpha + beta));         % Valor q2 
 
cosY = (L3^2 + L4^2 - Lp^2)/(2*L3*L4);
senY = sqrt(1 - cosY^2);
gama = atan2(senY, cosY);
phi = pi - gama;
q3 = (pi/2) + phi;                      % Valor q3
 
MTH03 = L(1).A(q1)*L(2).A(q2)*L(3).A(q3); % MTH de 3 con respecto a la base
[R03, P03] = tr2rt(MTH03);              % Separar rotación y translación
[RT6, PT6] = tr2rt(Robot.tool^(-1));    % Matriz de rotación de 6 con respecto al TCP
 
R06 = R0T*RT6;                          % Matriz de rotación de 6 con respecto a la base
R36 = (R03^(-1))*R06;                   % Matriz de rotación de 6 respecto a 3
% Valor q4 
q4 = atan2(R36(3,3),R36(1,3));
% Valor q5 
c5 = -R36(2,3);
s5 = sqrt(1 - c5^2);
q5 = atan2(s5,c5);
% Valor q6 
q6 = atan2(-R36(2,2),R36(2,1));
 
% Vector de posiciones articulares en radianes
q_rad = [q1 q2 q3 q4 q5 q6]
% Vector de posiciones articulares en grados
q_deg = [rad2deg(q1) rad2deg(q2) rad2deg(q3) rad2deg(q4) rad2deg(q5) rad2deg(q6)]

Volviendo a comprobar los valores obtenidos se encuentra:

clf
Robot.teach([q1 q2 q3 q4 q5 q6])        % Graficar el robot con el vector q
rpy = rad2deg(tr2rpy(Robot.fkine([q1 q2 q3 q4 q5 q6])));   % Ángulos RPY

Por último, se tiene la configuración con codo abajo, con brazo hacia atrás y base rotada que se muestra en la siguiente figura.

Las modificaciones con respecto a la otra configuración de codo abajo son:

(32)

\begin{equation} L_{xy} = r + L_2 \end{equation}

(33)

\begin{equation} q_1 = atan2(-y_w,-x_w) \end{equation}

(34)

\begin{align} q_2 = \frac{\pi}{2} - (\alpha - \beta) \end{align}

(35)

\begin{align} q_3 = \frac{\pi}{2} - \phi \end{align}

Como se aprecia, la definición de la longitud $L_{xy}$ es igual a la de la configuración anterior.

Por consiguiente:

Lxy = r + L2;
Lp = sqrt(Lxy^2 + Lz^2);
 
q1 = atan2(-yw,-xw);                    % Valor q1
 
alpha = atan2(Lz, Lxy);
cosB = (L3^2 + Lp^2 - L4^2)/(2*L3*Lp);
senB = sqrt(1 - cosB^2);
beta = atan2(senB, cosB);
q2 = ((pi/2) - (alpha - beta));         % Valor q2
 
cosY = (L3^2 + L4^2 - Lp^2)/(2*L3*L4);
senY = sqrt(1 - cosY^2);
gama = atan2(senY, cosY);
phi = pi - gama;
q3 = (pi/2) - phi;                      % Valor q3
 
MTH03 = L(1).A(q1)*L(2).A(q2)*L(3).A(q3); % MTH de 3 con respecto a la base
[R03, P03] = tr2rt(MTH03);              % Separar rotación y translación
[RT6, PT6] = tr2rt(Robot.tool^(-1));    % Matriz de rotación de 6 con respecto al TCP
 
R06 = R0T*RT6;                          % Matriz de rotación de 6 con respecto a la base
R36 = (R03^(-1))*R06;                   % Matriz de rotación de 6 respecto a 3
% Valor q4 
q4 = atan2(R36(3,3),R36(1,3));
% Valor q5 
c5 = -R36(2,3);
s5 = sqrt(1 - c5^2);
q5 = atan2(s5,c5);
% Valor q6 
q6 = atan2(-R36(2,2),R36(2,1));
 
% Vector de posiciones articulares en radianes
q_rad = [q1 q2 q3 q4 q5 q6]
% Vector de posiciones articulares en grados
q_deg = [rad2deg(q1) rad2deg(q2) rad2deg(q3) rad2deg(q4) rad2deg(q5) rad2deg(q6)]

Comprobando nuevamente, se aprecia que la posición y la orientación son las dadas inicialmente:

clf
Robot.teach([q1 q2 q3 q4 q5 q6])        % Graficar el robot con el vector q
rpy = rad2deg(tr2rpy(Robot.fkine([q1 q2 q3 q4 q5 q6])));   % Ángulos RPY

Cálculo de cinemática inversa con Robotics Toolbox (Peter Corke)

El toolbox de Peter Corke cuenta con 7 funciones para realizar la cinemática inversa de un robot, las cuales se diferencian unas de otras debido al número de articulaciones del robot, la configuración de la muñeca, el método de cálculo utilizado o el forma de representación de los parámetros de Denavit-Hartenberg (DH). Dichas funciones disponibles son:

ikine: Permite calcular la cinemática inversa de un robot de 6 grados de libertad (GDL) o más. Recibe como entrada la matriz de transformación homogénea del efector final ( $$T$$ ) y como opcional un vector de coordenadas articulares iniciales estimadas ( $$q_0$$ ), un vector máscara ( $$m$$ ) para poder usar la función con manipuladores de menos de 6 GDL y algunas opciones para el método de cálculo.

ikine6s: A diferencia de $$ikine$$ , se usa para cálculo de la cinemática inversa en robots de 6 GDL que presentan muñeca esférica y sólo se puede aplicar a manipuladores creados con la representación de DH estándar. Tiene como entrada a $$T$$ y como parámetro opcional las diferentes configuraciones en las que se puede calcular el robot, como brazo a la derecha o izquierda ( $$'r'$$ o $$'l'$$ ), codo arriba o codo abajo ( $$'u'$$ o $$'d'$$ ) y muñeca rotada o no rotada 180º ( $$'f'$$ o $$'n'$$ ).

ikine3: Esta función se emplea para robots sin muñeca, es decir, de 3 GDL, en las cuales sus articulaciones estén dispuestas como las tres primeras articulaciones de un robot Puma 560. Al igual que $$ikine6s$$ , esta función sólo es aplicable para robots creados con DH estándar y además sus entradas son similares. Debido a que no posee muñeca, las opciones de muñeca rotada o no rotada 180º ( $$'f'$$ o $$'n'$$ ) no se encuentran disponibles.

ikunc: Se utiliza para el cálculo (por medio de optimización) de robots con cualquier número de grados de libertad y creados con representaciones DH estándar o DH modificado. Tiene como entradas a $$T$$ , $$q_0$$ (en caso de tener un vector de coordenadas articulares iniciales estimadas) y algunas opciones provenientes de $$fminunc$$ . Aparte de la cinemática inversa, también permite ver el error y el estado de $$fminunc$$ . Esta función no tiene en cuenta los límites articulares.

ikcon: Al igual que el anterior, permite el cálculo para cualquier robot (sin importar el número de GDL) usando optimización. Sus entradas y salidas son similares, pero algunas de las opciones son de $$fmincon$$ . La principal diferencia radica en que esta función si tiene en cuenta los límites articulares del robot.

ikine_sym: Permite calcular la cinemática inversa simbólica de un robot creado a partir de DH estándar. Sus entradas son el número de soluciones y la opción de escribir la solución en un archivo m-file.

ikinem: Con esta función es posible hallar la cinemática inversa por medio de minimización. Recibe como entradas a $$T$$ y de manera opcional a $$q_0$$ y algunas opciones de configuración del método de cálculo.

Para el robot con el cual se está trabajando actualmente (ABB IRB 1600) se puede hacer uso de las funciones $$ikine$$ e $$ikunc$$ debido a que permiten hallar la cinemática inversa en robots de 6 GDL. No se hace uso de $$ikin6s$$ debido a que al calcular los parámetros de DH estándar para dicho robot, el eje de coordenadas 4 queda en el mismo origen del marco de coordenadas 3 y no junto a los marcos de coordenadas 5 y 6, razón por la cual $$Matlab$$ la toma como si ésta no fuera un muñeca esférica y no permite calcularla.

A continuación se muestra la implementación de $$ikine$$ :

% Uso de ikine
% Valores de prueba
xe = 750/100;
ye = 100/100;
ze = 593/100;
roll = -45;
pitch = 0;
yaw = -90;
 
R0T = rpy2r(roll, pitch, yaw, 'deg');
pe = [xe; ye; ze];
 
T = rt2tr(R0T, pe)
q_ikine = Robot.ikine(T)

Al utilizar la función $$ikine$$ se puede observar que la solución no logra llegar a la posición y orientación deseada y, además, presenta un error después de la iteración 1000. Por consiguiente, se procede a implementar $$ikunc$$ .

% Uso de ikunc
q_ikunc = Robot.ikunc(T)
deg_ikunc = rad2deg(q_ikunc)

clf
Robot.teach(q_ikunc);
rpy = rad2deg(tr2rpy(Robot.fkine(q_ikunc)))

Como se logra ver, el vector de posición articular generalizado encontrado por esta función permite ubicar el robot en los parámetros deseados, por lo tanto, se utiliza para calcular la cinemática inversa por iteración.

Al comparar el resultado con los métodos analíticos hallados anteriormente se aprecia que la función $$ikunc$$ encuentra los valores de $$q$$ para una configuración de robot con codo arriba y sin base rotada.

Cálculo de cinemática inversa con Robotics System Toolbox

Para calcular la cinemática inversa empleando el toolbox de RST primero se importa el robot usando $$loadrobot$$ y especificando que es el ABB IRB 1600. Luego se usa $$inverseKinematics$$ para definir la cinemática inversa de dicho manipulador, en esta se introducen como parámetros el robot (en este caso sería $$'abbirb1600'$$ ) y se especifica que es $$'RigidBodyTree'$$ . Se define un vector de 1 para especificar que todo debe tener la misma precisión, se brinda una posición inicial de los valores de $$q$$ (generalmente la configuración de home), la posición del efector final y luego se calculan los valores de $$q$$ . Esto se muestra a continuación:

% Importar robot
[robotRBT,robotData]= loadrobot('abbirb1600','DataFormat','row','Gravity',[0 0 -9.81]);
 
% Valores de prueba
xe = 750/1000;
ye = 100/1000;
ze = 593/1000;
roll = -45;
pitch = 0;
yaw = -90;
 
R0T = rpy2r(roll, pitch, yaw, 'deg');   % Matriz de orientación del efector final
pe = [xe; ye; ze];                      % Vector de posición del efector final
 
T = rt2tr(R0T, pe);                     % MTH en el efector final
 
ik = inverseKinematics('RigidBodyTree',robotRBT);   % Cinemática inversa    
weights = [1 1 1 1 1 1];                            % Vector para que todo tenga la misma precisión
initialguess = robotRBT.homeConfiguration;          % Configuración inicial
[configSoln,solnInfo] = ik('tool0',T,weights,initialguess); % MTH efector final
q_rad = configSoln
q_deg = rad2deg(configSoln)

Se debe prestar atención al hecho de que el modelo del robot ABB IRB 1600 que se importa del Robotics System Toolbox es el IRB1600-X/1.2, razón por la cual, para comprobar si los valores de los $$q$$ son correctos, se debe construir un robot con las mismas dimensiones. Además, es necesario invertir el signo de $$q_2$$ , $$q_3$$ y $$q_5$$ debido a que las rotaciones entre los dos modelos se hacen en sentidos contrarios.

q = [configSoln(1) -configSoln(2) -configSoln(3) configSoln(4) -configSoln(5) configSoln(6)]
rpy = rad2deg(tr2rpy(Robot.fkine(q)))
[~, pos] = tr2rt(Robot.fkine(q))

Al graficar el robot posicionado se obtiene:

Comparación de métodos

Como se logró apreciar, los tres métodos utilizados permiten encontrar de manera correcta los valores del vector de posición articular generalizado $$q$$ . En el caso del primer método, se lograron encontrar 4 configuraciones del robot diferentes para llegar al mismo punto. Esto se logró haciendo el desarrollo por el método geométrico (para las tres primeras articulaciones) y por análisis de las matrices de rotación (para las últimas tres articulaciones).

En el caso de la función $$ikunc$$ , dicho cálculo lo realiza por medio de iteración usando optimización y la configuración con la cual calcula los valores de $$q$$ es la de codo arriba sin rotar la base.

Finalmente, la función $$inverseKinematics$$ calcula la cinemática inversa de un robot creado o importado con el Robotics System Toolbox y presenta una configuración del robot diferente a las mostradas anteriormente. Como ya se había mencionado, se debe ser cuidadoso con las rotaciones de las articulaciones, la versión del robot de $$Matlab$$ y la versión con la cual se compara.

Resultados Cinemática Inversa

Para comprobar el funcionamiento de la cinemática inversa se ingresaron 4 posturas con el fin de calcular los valores de $$q$$ .

Para hacer esto se realizó una función llamada $$CinIversa$$ la cual recibe las coordenadas de posición y orientación del efector final, la longitud de los eslabones, el robot definido por medio de $$Link$$ , el robot y la configuración deseada. A continuación se muestra un ejemplo:

% Primera postura
xe = 59.243/100;
ye = -335.984/100;
ze = 1401.93/100;
roll = 69.332;
pitch = 35.079;
yaw = -24.999;
 
% Cinemática inversa
[q1, q2, q3, q4, q5, q6] = CinIversa(xe, ye, ze, roll, pitch, yaw, L1, L2, L3, L4, L5, L,...
                                     Robot, "codoArriba");

Nota: En el momento, sólo se implementó la configuración de codo arriba para la función

$CinIversa$

Como salida se obtiene:

Los resultados obtenidos para las 4 posturas seleccionadas fueron:

	$$x$$	$$y$$	$$z$$	$$roll$$	$$pitch$$	$$yaw$$	$$q_1$$	$$q_2$$	$$q_3$$	$$q_4$$	$$q_5$$	$$q_6$$
1	0.5924	-3.3598	14.0193	1.2101	0.6122	-0.4363	-1.4945	0.5706	-0.0324	-1.8825	0.5713	3.0397
2	-1.8481	-1.8481	13.2400	0.0000	0.0000	2.3562	-2.3562	0.6847	-0.2908	0.0000	1.1769	-1.5708
3	-0.0456	-2.6132	13.2400	2.2471	0.6455	-0.4498	-1.7660	0.6662	-0.1043	-2.3387	1.4182	-2.3503
4	7.0900	1.2894	4.1236	-3.0156	0.3368	-2.6604	0.1745	-0.6198	-0.7129	-0.4754	0.1301	1.1597

Uso de la GUI

A continuación se presentan las 4 posturas halladas anteriormente y mostradas en la GUI. En ellas se muestra el robot, el valor de cada $$q$$ , y las coordenadas de posición y orientación.

Primera postura

Segunda postura

Tercera postura

Cuarta postura