Índice

• 1. Pose

• 2. Direct Kinematics

• 3. Inverse Kinematics

  • 3.1. Práctica

• 4. Differential Kinematics

  • 4.1. Introducción
  • 4.2. Jacobiano por Diferenciación Directa
  • 4.3. Jacobiano por Métodos Geométricos
    • AQUÍ TENGO QUE EXPLICAR DE DONDE VIENEN LAS FÓRMULAS
  • 4.4. Jacobiano por Propagación de Velocidades
  • 4.5. Análisis del Jacobiano
    • 4.5.1. Singularidades
    • 4.5.2. Redundancia
    • 4.5.3. Estática
  • 4.6. Inversa del Jacobiano
  • 4.7. Práctica

• 5. Control Cinemático

  • 5.1. Funciones del Control Cinemático
  • 5.2. Tipos de Trayectorias
  • 5.3. Generación de Trayectorias Cartesianas
  • 5.4. Selección de Puntos en Trayectorias Cartesianas
  • 5.5. Inerpolación de Trayectorias
  • 5.6. Práctica

• 6. Modelo Cinemático de un Robot con Ruedas

  • 6.1. Robot Diferencial
  • 6.2. Generalización Robots con Ruedas
  • 6.3. Composición y Condiciones de No Deslizamiento

• 7. Modelo Dinámico de los Robots

• Trabajo Final

1. Pose

Todo está explicado aquí

2. Direct Kinematics

Todo está explicado aquí

3. Inverse Kinematics

3.1. Práctica

3.1.1. Introducción

En esta práctica se pide realizar el modelo cinemático inverso del siguiente manipulador. El código a presentar se encuentra en este enlace

Para ello se creará una función de matlab la cual se devolverá tanto un punto A (posiviones en $X$ e $Y$ y el ángulo $\varphi$) al que ir como un parámetro m (si es positivo codo abajo y si es negativo arriba) y devolverá un vector Q con las posiciones de cada una de las articulaciones.

function [Q] = InvKinRRR(P,m)

# Cuerpo

end

3.1.2. Parámetros

Obtendremos tanto de los datos del enunciado como los dados por el usuario las longitudes de los eslabones y la matriz de rotación $^{0}R_{3}$

a1 = 20;
a2 = 15;
a3 = 10;

X = P(1);
Y = P(2);
phi = P(3);

PH = [X,Y];
R03 = rotZ(phi);
R03 = R03(1:3, 1:3);

3.1.3. Muñeca del Manipulador

Con el fin de solucionar este problema, se utilizará la técnica de desacomplo cinemático (separando la articulación 3 de la 1 y la 2 mediante la muñeca). Esto implica saber en que posición se encuentra la muñeca.

Esto se consigue con la "ecuación 3.1.3.1." la cual resta a la posición del Efector Final la distancia del último eslabón. Como el eje $Z$ del Efector Final se encuentra en el eje $X$ del sistema global, solo obtenemos la culumna $X$ de la matriz de rotación $^{0}R_{3}$.

$$ \Huge{{ P_{muneca} = P_{EfectorFinal} - a_3 * \ ^{0}R_{3} * \ ^{3}X_{3} }\tag{3.1.3.1}} $$

PW = [PH(1) ; PH(2) ; 0] - a3*R03*[1 0 0]';

Xw = PW(1);
Yw = PW(2);

3.1.4. Articulaciones 1 y 2

Una vez realizado el desacoplo cinemático, calculamos los valores de las articulaciones 1 y 2 basándonos el el método gráfico.

• Lineas Grises: Eslabones $a_1$ y $a_2$
• Línea Amarilla: R en el código.
• Ángulos Verdes: Articulaciones $Q_1$ y $Q_2$
• Ángulo Marrón: alpha en el código.
• Ángulos Azules: beta en el código.
• Ángulos Morados: gamma en el código.

Utilizando el teorema de pitágoras y dadas las posiciones $X$ e $Y$ de la muñeca, podemos obtener el valor de la línea amarilla R

R = sqrt(Yw^2 + Xw^2);

Utilizando la función atan2 de Matlab y dadas las posiciones $X$ e $Y$ de la muñeca, podemos obtener el valor del ángulo marrón alpha

alpha = atan2(Yw,Xw);

Utilizando el teorema del coseno, el valor de R y el valor de los eslabones $a_1$ y $a_2$ podemos obtener los ángulos morados gamma y azules beta.

beta = acos((a1^2+a2^2-R^2)/(2*a1*a2));
gamma = acos((R^2+a1^2-a2^2)/(2*R*a1));

Una vez conseguidos todos estos ángulos, podemos calcular el valor de las articulaciones $Q_1$ y $Q_2$. Este valor dependerá de si el manipulador se encuentra con el codo arriba o con el codo abajo. Por ello se utilizará la siguiente sentencia con el fin de separar ambos casos if m > 0.

En el casó verdadero (codo abajo), el valor de las articulaciones se obtendrá con las ecuaciones "3.1.4.1" y "3.1.4.2"

$$ \Huge{{ \theta_1 = alpha - gamma }\tag{3.1.4.1}} $$

$$ \Huge{{ \theta_2 = pi - beta }\tag{3.1.4.2}} $$

theta1 = alpha - gamma;
Q(1) = theta1;
theta2 = pi - beta;
Q(2) = theta2;

En el casó falso (codo arriba), el valor de las articulaciones se obtendrá con las ecuaciones "3.1.4.3" y "3.1.4.4"

$$ \Huge{{ \theta_1 = alpha + gamma }\tag{3.1.4.3}} $$

$$ \Huge{{ \theta_2 = -(pi - beta) }\tag{3.1.4.4}} $$

theta1 = alpha + gamma;
Q(1) = theta1;
theta2 = -(pi - beta);
Q(2) = theta2;

3.1.5. Articulación 3

Finalmente se calculará el valor de la articualción 3 en cada uno de los 2 casos anteriores (codo arriba o abajo). Para ello se utilizará la matriz de rotación $^{2}R_{3}$ (la cual es un giro en $Z$) y la función de Matlab atan2 (utilizaremos el seno y el coseno de la matriz $^{2}R_{3}$)

Para el cálculo de la matriz $^{2}R_{3}$ se utilizará la "ecuación 3.1.5.1.".

$$ \Huge{{ ^{2}R_{3} = \ ^{0}R_{2}' * \ ^{0}R_{3} }\tag{3.1.5.1}} $$

La matriz $^{0}R_{3}$ se obtuvo al principio del problema y la matriz de rotación $^{0}R_{2}$ está formada por los ángulos de las articulaciones 1 y 2. Esta útlima matriz dependerá del caso (codo arriba o codo abajo).

syms theta1 theta2 real
R01 = rotZ(theta1);
R01 = R01(1:3, 1:3);
R12 = rotZ(theta2);
R12 = R12(1:3, 1:3);

% Dentro de cada caso del if

R23 = eval(R02)'*R03;
Q(3) = atan2(R23(2,1),R23(1,1));

3.1.6. Resultados

Finalmente se comprueba el modelo cinemático inverso en los puntos $A = [30 25 0]$ y $B = [30 5 0]$ propuestos por el enunciado tanto utilizando el codo arriba como el codo abajo.

• Punto A

• Punto B

4. Differential Kinematics

4.1. Introducción

Teniendo las velocidades y las posiciones angulares de las articulaciones de nuestro manipulador (espacio articular) queremos pasarlas a las velocidades cartesianas (espacio cartesiano).

4.2. Jacobiano por Diferenciación Directa

Realizado ya el Modelo Cinemático Directo (DKM), tenemos una matriz de transformación $T_N^0$ (sinedo $N$ el número de la última articulación del manipulador).

Esta matriz tiene el siguiente aspecto:

$$ \Huge{ T_N^0 = \begin{bmatrix} r_{11} & r_{12} & r_{13} & X(q_1,q_2,\ldots,q_N) \ r_{21} & r_{22} & r_{23} & Y(q_1,q_2,\ldots,q_N) \ r_{31} & r_{32} & r_{33} & Z(q_1,q_2,\ldots,q_N) \ 0 & 0 & 0 & 1 \end{bmatrix}} $$

Las funciones $X$, $Y$ y $Z$ son aquellas que dan la posición del Efector Final del manipulador y dependen de todas las articulaciones. La matriz $R$ (compuesta de todos los $r_{ij}$), es una matriz que da los ángulos $\phi$, $\theta$ y $\psi$ del Efector Final y que depende también de todas las articulaciones.

Dicho esto, tenemos lo siguiente:

$$ \Huge{X(q_1,q_2,\ldots,q_N)} $$ $$ \Huge{Y(q_1,q_2,\ldots,q_N)} $$ $$ \Huge{Z(q_1,q_2,\ldots,q_N)} $$ $$ \Huge{\phi(q_1,q_2,\ldots,q_N)} $$ $$ \Huge{\theta(q_1,q_2,\ldots,q_N)} $$ $$ \Huge{\psi(q_1,q_2,\ldots,q_N)} $$

El jacobiano por definición tiene en cada uno de sus elementos la derivada parcial de todas las funciones respecto a todas las variables.

$$ \Huge{ J = \begin{bmatrix} \frac{\partial y_1}{\partial x_1} & \frac{\partial y_1}{\partial x_2} & \ldots & \frac{\partial y_1}{\partial x_N} \ \frac{\partial y_2}{\partial x_1} & \frac{\partial y_2}{\partial x_2} & \ldots & \frac{\partial y_2}{\partial x_N} \ \vdots & \vdots & \ddots & \vdots \ \frac{\partial y_M}{\partial x_1} & \frac{\partial y_M}{\partial x_2} & \ldots & \frac{\partial y_M}{\partial x_N} \end{bmatrix}} $$

Por lo que en nuestro caso, el jacobiano se calculará de la siguiente manera:

$$ \Huge{ J = \begin{bmatrix} \frac{\partial X}{\partial q_1} & \frac{\partial X}{\partial q_2} & \ldots & \frac{\partial X}{\partial q_N} \ \frac{\partial Y}{\partial q_1} & \frac{\partial Y}{\partial q_2} & \ldots & \frac{\partial Y}{\partial q_N} \ \vdots & \vdots & \ddots & \vdots \ \frac{\partial \psi}{\partial q_1} & \frac{\partial \psi}{\partial q_2} & \ldots & \frac{\partial \psi}{\partial q_N} \end{bmatrix}} $$

Finalmente tenemos la ecuación para conseguir nuestro propósito.

$$ \Huge{ v = J * \dot{q} } $$

4.3. Jacobiano por Métodos Geométricos

Realizado ya el Modelo Cinemático Directo (DKM), tenemos todas las matrices de transformación $T_1^0$, $T_2^0$, $\ldots$, $T_N^0$ (sinedo $N$ el número de la última articulación del manipulador).

Estas matrices tienen el siguiente aspecto:

$$ \Huge{ T_i^0 = \begin{bmatrix} R_i^0 & P_i^0 \ 0 & 1 \end{bmatrix}} $$

Los elementos del Jacobiano siempre serán los mismos independientemente del método utilizado al calcular el DKM.

$$ \Huge{ j = \begin{bmatrix} d_1^0 & d_2^0 & \ldots & d_N^0 \ \Omega_1^0 & \Omega_2^0 & \ldots & \Omega_N^0 \end{bmatrix}} $$

El cálculo de $d_i^0$ y de $\Omega_i^0$ dependerá de si hemos utilizado DH1 o DH2.

Antes de empezar con los cálculos, siempre empezaremos con estas tres premisas:

$$ \Huge{ P_0^0 = \begin{bmatrix} 0 \ 0 \ 0 \end{bmatrix}} $$

$$ \Huge{ z_0^0 = \begin{bmatrix} 0 \ 0 \ 1 \end{bmatrix}} $$

$$ \Huge{ z_i^0 = R_i^0 * z_0^0 } $$

4.3.1. Denavit-Hartenberg 1 (DH1)

Cálculos:

• Si la articulación $q_i$ es una articulación de revolución ($\theta_i$).

$$ \Huge{ d_i^0 = z_{i-1}^0 \times (P_N^0-P_{i-1}^0) } $$

$$ \Huge{ \Omega_i^0 = z_{i-1}^0 } $$

• Si la articulación $q_i$ es una articulación pristmática ($d_i$).

$$ \Huge{ d_i^0 = z_{i-1}^0 } $$

$$ \Huge{ \Omega_i^0 = 0 } $$

4.3.2. Denavit-Hartenberg 2 (DH2)

Cálculos:

• Si la articulación $q_i$ es una articulación de revolución ($\theta_i$).

$$ \Huge{ d_i^0 = z_i^0 \times (P_N^0-P_i^0) } $$

$$ \Huge{ \Omega_i^0 = z_i^0 } $$

• Si la articulación $q_i$ es una articulación pristmática ($d_i$).

$$ \Huge{ d_i^0 = z_i^0 } $$

$$ \Huge{ \Omega_i^0 = 0 } $$

4.4. Jacobiano por Propagación de Velocidades

4.4.1. Denavit-Hartenberg 1 (DH1)

4.4.2. Denavit-Hartenberg 2 (DH2)

4.5. Análisis del Jacobiano

Muchas veces, por la configuaración del robot, el jacobiano no será cuadrado, es decir, el rango de $J$ no será máximo. Esto suele ocurrir cuando el número de grados de libertad del brazo robótico es distinto a 6, aunque también puede ocurrir si la disposición física de las articulaciones no puede permitir actuar en una de las coordenadas cartesianas (poner varias articulaciones en el mismo eje, poner articulaciones prismáticas...).

El ejemplo de la imagen anterior, no puede actuar linearmente en el eje $Z$ ni angularmente en los ejes $X$ e $Y$.

4.5.1. Singularidades

• Pérdida de Movilidad: El Efector Final no puede moverse en ciertas direcciones del espacio cartesiano.
• Multiples Soluciones a la Inversa: En puntos singulares, hay infinitas convinaciones de velocidades articulares que producen el mismo movimiento cartesiano.
• Amplificación de Velocidades: Cerca de una singularidad, una pequeña velocidad cartesiana en el Efector Final puede requerir grandes velocidades articulares (movimientos bruscos o inestables).

Tipos de Singularidades

• Singularidades de Frontera: Ocurren en los límites del trabajo (por ejemplo, si tienes el brazo estirado como máximo te puedes mover siguiendo una esfera, pero no te puedes mover perpendicularmente a dicha esfera).

• Singularidades Internas: Son causadas por las alineaciones de los ejes (por ejemplo, si en el ejemplo superior las articulaciones $0$ y $2$ están en el mismo eje ($a_1$ y $a_2$ se superponen), no puedo moverme en la dirección en la que se encuentran los eslabones $a_1$ y $a_2$ sin moverme perpendicularmente primero.

4.5.2. Redundancia

4.5.3. Estática

4.7. Práctica

4.7.1. Introducción

En esta práctica se nos pide calcular las velocidades de las articulaciones del manipulador IRB 120 dado parte del código y una función la cual calcula la cinemática inversa del manipulador.

Se pide también saber una gráfica que muestre en que punto el manipulador se encuentra más cerca de una singularidad.

4.7.2. Explicación Código Dado

El código dado por la práctica da las matrices de transformación del MCD del manipulador (utilizando variables simbólicas para las articulaciones).

El Efector Final del manipulador viajará desde una pose $A$ hasta una pose $B$ dadas manteniendo una velocidad lineal de $0.1 m/s$ en el espacio cartesiano. El código devuelve las poses y las velocidades en el espacio cartesiano en cada uno de los instantes de tiempo. Se utiliza la función del MCI del manipulador para obtener las posiciones angulares de cada articulación en cada instante de tiempo.

Finalmente grafica las posiciones y las velocidades de cada articulación (en el espacio articular).

4.7.3. Cálculo del Jacobiano

Se utilizará el método geométrico con el fin de obtener el Jacobiano del manipulador. Para ello es necesario las matrices de transformación del MCD del manipulador.

zL=[0 0 1]';
z0=zL;
z1 = T01(1:3,1:3) * zL;
z2 = T02(1:3,1:3) * zL;
z3 = T03(1:3,1:3) * zL;
z4 = T04(1:3,1:3) * zL;
z5 = T05(1:3,1:3) * zL;

p0=[0 0 0]';
p1 = T01(1:3, 4);
p2 = T02(1:3, 4);
p3 = T03(1:3, 4);
p4 = T04(1:3, 4);
p5 = T05(1:3, 4);
p6 = T06(1:3, 4);

J1=simplify([cross(z0,(p6-p0));z0]);
J2=simplify([cross(z1,(p6-p1));z1]);
J3=simplify([cross(z2,(p6-p2));z2]);
J4=simplify([cross(z3,(p6-p3));z3]);
J5=simplify([cross(z4,(p6-p4));z4]);
J6=simplify([cross(z5,(p6-p5));z5]);

JPO=simplify([J1 J2 J3 J4 J5 J6]);

4.7.4. Obtención de las Velocidades de las Articulaciones

EL código dado por la práctica utiliza la función IRB120_IK (MCI) para obtener las posiciones angulares de cada articulación en cada intante de tiempo k. Una vez obtenidas, las evaluamos en el jacobiano JPO compuesto por variables simbólicas.

El código dado devuelve las velocidades en el espacio cartesiano del Efector Final. Se utilizará la ecuación $\dot{q} = J^{-1}* \dot(v)$ con el fin de obtener las velocidades de las articulaciones en cada instante de tiempo k.

Finalmente se obtendrá el determinante de dicho jacobiano en cada instante de tiempo con el fin de ver en qué momento se aproxima más a 0 (cuando el determinante del jacobiano es 0, el sistema se encuentra en una singularidad).

for k=1:length(trajTimePoints)

   Jq=subs(JPO,{q1,q2,q3,q4,q5,q6},{q(k,1),q(k,2),q(k,3),q(k,4),q(k,5),q(k,6)});
   qd(:,k) = Jq\ve(:,k);
   dj(:,k) = det(Jq);

end

4.7.5. Resultados

Grabación de pantalla desde 21-11-25 14:33:42.webm

5. Control Cinemático

5.1. Funciones del Control Cinemático

Las trayectorias y objetivos dichos por el usuario deben ser logrados a tiempo. Las trayectorias deben elegirse acorde a las restricciones físicas del manipulador, a lo suave que lo queremos, a la precisión...

• Espacio/Dominio Articular (EA): Todas las posibles posiciones de las articulaciones del robot.
• Espacio/Dominio Operacional/Tarea (EO): La pose del efector final en coordenadas cartesianas y orientación.

Para viajar entre espacios utilizamos la cinemática directa (MCD) e indirecta (MCI). Una linea o curva entre 2 puntos en un espacio no significa una linea o curva entre esos puntos en el otro espacio. Es decir, si en una linea en el EO cojo 2 puntos y estos puntos crean una curva en el EA,los puntos de la curva del EA no corresponden con puntos de la linea del EO

Cuantos más puntos cojamos en la linea del EO (y del EA al usar MCI), menor será el error sobre la linea.

5.2. Tipos de Trayectorias

Hay varias formas de generar trayectorias.

• Movimiento Eje a Eje: Primero muevo una articulación y cuando termino muevo la siguiente.

• Movimiento simultáneo de los ejes: Todas las articulaciones empiezan el movimiento al mismo tiempo pero no terminan en el mismo momento.

• Movimiento Simultáneo Temporal: Todas las articulaciones empiezan y terminan a la vez.

• Movimiento Rectilineo: Un movimiento rectilíneo en el EO puede implicar grandes aceleraciones y velocidades en el EA.

5.3. Interpolación de Puntos

Con el fin de obtener más puntos entre 2 puntos

5.5. Inerpolación de Trayectorias

5.6. Práctica

5.6.1. Introducción

Se utiliza el código de la práctica anterior el cual se conseguía la matriz jacobiana del manipulador para obtener tanto la posición como la velocidad de cada una de las articulaciones.

En esta práctica se pide realizar la interpolación de cada articulación entre los distintos instantes de tiempo. Con esto se obtendrá una mayor cantidad de puntos y se comprobará como funciona el manipulador entre dichos puntos.

Finalmente se pide observar como funciona el efector final del manipulador y comprobar el error que existe entre la pose actual de este y la dada por referencia.

5.6.2. Métodos de Interpolación

Se utilizan tres puntos consecutivos y se comprueba si la dirección del punto 1 al 2 ($q_2 - q_1$) es la misma que la del 2 al 3 ($q_3 - q_2$), o una de las direcciones es nula. Después calculo la velocidad del punto medio (2) como la media entre las velocidades entre el punto 1 y 2 ($\frac{q_2 - q_1}{t_2 - t_1}$), y el 2 y el 3 ($\frac{q_3 - q_2}{t_3 - t_2}$).

% CRAIG
function qd = PointsVelCraig(q,t)
% Computing joint velocities of via points using Craig criteria.
% Input data size
    num_joints=size(q,2); num_points=size(q,1);
    % Zero initialization of joint velocities
    qd=zeros(size(q));
    for j=1:num_joints
        for i=2:num_points-1
            if (sign(q(i,j)-q(i-1,j))~=sign(q(i+1,j)-q(i,j))) && (q(i-1,j)==q(i,j)) && (q(i,j)==q(i+1,j))
                qd(i,j)=0;
            else
                qd(i,j) = (1/2)*((q(i+1,j)-q(i,j))/(t(i+1)-t(i))+ (q(i,j)-q(i-1,j))/(t(i)-t(i-1)));
            end
        end
    end
end

Se utilizarán dos puntos consecutivos de q (y sus velocidades qd) para obtener los parámetros a, b, c y d del polinomio de tercer grado el cual será utilizado para obtener los M puntos interpolados entre los dos puntos del principio.

% SPLINE
function [qs,time] = CubicSpline_Interp(q,qd,t,M)
    % Interpolation using cubic spline
    L=length(q);
    time=0;
    qs=q(1,:); % first joint configuration
    for i=1:L-1
        T=t(i+1)-t(i);
        a=q(i,:);
        b= qd(i,:);
        c = (3/T^2)*(q(i+1,:)-q(i,:))-(1/T)*(qd(i+1,:)+2*qd(i,:));
        d = (-2/T^3)*(q(i+1,:)-q(i,:))+(1/T^2)*(qd(i+1,:)+qd(i,:));
        
        temp=linspace(t(i),t(i+1),M);
        temp=temp(2:M);
        time=[time,temp];

        for k=1:M-1
            qtemp(k,:)=a+b*(temp(k)-t(i))+c*(temp(k)-t(i))^2+d*(temp(k)-t(i))^3;
        end
        qs=[qs;qtemp];
    end
end

5.6.3. Resultados de la Interpolación

Obtenemos los valores interpolados de cada una de las articulaciones dados unos 37 puntos iniciales.

M = 100; % Numero de puntos en la interpolación

% Da una matriz QD(i_tiempo, :) en el que da las velocidades deseadas de las articulaciones en cada instante de tiempo.
QD = PointsVelCraig(q, trajTimePoints);

% Si había 37 puntos ahora habrá aproximadamente M veces más.
% TIME es un vector que da los nuevos tiempos interpolados entre cada instante de tiempo anterior
% QS[i_tiempo, :] son las posiciones nuevas de las articulaciones en cada instante de tiempo
[QS, TIME] = CubicSpline_Interp(q, QD, trajTimePoints, M);

Tras realizar esto, la variable QS tiene los valores interpolados de cada una de las poses de articulaciones. En la siguiente imagen se mostrará los valores de las posiciones tanto antes (círculos) como después (líneas) de la interpolación.

A continuación obtenemos las velocidades de los valores de posición interpolados derivando la variable QS de la siguiente manera.

D_QS = diff(QS);
D_TIME = diff(TIME); % Es constante debido a que la deferencia entre cada instante de tiempo es la misma.
D_QS = D_QS/D_TIME(1);
D_QS = [D_QS;zeros(1,6)]; % Mismo tamaño que TIME. Vel = 0 al final

Mostraremos de nuevo la diferencia entre los valores de velocidad interpolados (líneas) y los obtenidos en la práctica anterior (círculos).

Finalmente obtendremos las aceleraciones de las articulaciones derivando D_QS de nuevo.

DD_QS = diff(D_QS(1:end-1,:)); % Quito los ceros añadidos anteriormente
DD_QS = DD_QS/D_TIME(1);
DD_QS = [zeros(1,6); DD_QS]; % Tamaño menor a TIME. Accel = 0 al principio

5.6.4. Resultados del Efector Final

Como último apartado de esta práctica es la comprobación de si movimiento del efector final es igual al querido. Se sabe que al pasar del espacio articualar al espacio de la tarea no existe una linealidad. Al realizar una interpolación lineal en el espacio articular esto no supone una linealidad en el espacio cartesiano, por lo que se observará el error realizado debido a la interpolación.

Se obtendrá el valor de la posición del Efector Final (descritas con variables simbólicas) y se evaluarán los valores de las posiciones de las articulaciones en cada instante de tiempo.

% POSICIÓN
X = p6(1);
Y = p6(2);
Z = p6(3);

L = length(QS);
x_vec = zeros(L,1);
y_vec = ones(L,1);
z_vec = zeros(L,1);
y_ref = y_vec*0.302; % Y target

% Evaluo las variables simbólicas y creo vectores "x(i_tiempo)", "y(i_tiempo)", y "z(i_tiempo)" que muestran 
% la posición del efector final en cada instante de tiempo
for k=1:L
    x_vec(k) = subs(X,{q1,q2,q3,q4,q5,q6},{QS(k,1),QS(k,2),QS(k,3),QS(k,4),QS(k,5),QS(k,6)});
    y_vec(k) = subs(Y,{q1,q2,q3,q4,q5,q6},{QS(k,1),QS(k,2),QS(k,3),QS(k,4),QS(k,5),QS(k,6)});
    z_vec(k) = subs(Z,{q1,q2,q3,q4,q5,q6},{QS(k,1),QS(k,2),QS(k,3),QS(k,4),QS(k,5),QS(k,6)});
end

El robot no se mueve en el eje $Y$ teniendo siempre el valor de $0.302 m$. Se graficará el valor de $Y$ respecto a los otros 2 ejes en cada instante de tiempo y se observará si se sigue esa referencia querida.

Se puede observar que el error debido a la interpolación es de $0.00004 m$. Si se quisiese reducir se deberá aumentar el número de puntos de interpolación.

Utilizando el jacobiano se pueden obtener las velocidades del efector final en el espacio cartesiano.

v_vec = zeros(L,6);
for k=1:L
    J = subs(JPO,{q1,q2,q3,q4,q5,q6},{QS(k,1),QS(k,2),QS(k,3),QS(k,4),QS(k,5),QS(k,6)});
    v_vec(k,:) = (J*D_QS(k,:)')';
end

Finalmente si derivamos v_vec obtenemos la aceleración del efector final.

a_vec = diff(v_vec);
a_vec = a_vec/D_TIME(1);
a_vec = [zeros(1,6);a_vec];
a_vec = a_vec(1:end-1,:);

6. Modelo Cinemático de un Robot con Ruedas

6.1. Robot Diferencial

$$ \Huge {{ \rho = \frac{d (V_{Ry_2} + V_{Ry_1})}{V_{Ry_2} - V_{Ry_1}} }\tag{6.1.1}} $$

$$ \Huge {{ w_c = \frac{V_Cy}{\rho} = \frac{V_{Ry_1}}{\rho - d} = \frac{V_{Ry_2}}{\rho + d} }\tag{6.1.2}} $$

$$ \Huge {{ V_Cy = \frac{V_{Ry_1} + V_{Ry_2}}{2} }\tag{6.1.3}} $$

$$ \Huge {{ w_c * \rho = V_Cy }\tag{6.1.4}} $$

$$ \Huge {{ w_c = \frac{V_{Ry_2} - V_{Ry_1}}{2d} }\tag{6.1.5}} $$

Aunque en cuanto a manipuladores, el Jacobiano relacione velocidades articulares con la velocidad de la herramienta y el MCD relacione las posiciones de las articulaciones con las posiciones de la herramienta, en cuanto a robots móviles llamaremos a lo primero (relación entre velocidades) el MCD.

6.2. Generalización Robots con Ruedas

Supongamos un robot con $N$ número de ruedas. Este robot tendrá un sistema central {$C$}, el sistema de la junta entre el cuerpo y la pata {$F_i$} (desplazamiento), el sistema de la junta entre el cuerpo y la pata {$D_i$} (orientación), el sistema de la rueda {$R_i$}. Podemos suponer que tanto la junta cuerpo-pata puede tener una velocidad angular de $w_{D_i}$ y la junta pata-rueda otra de $w_{R_i}$.

En este ejemplo existen dos distancias: $d_{R_i}$ (distancia entre la junta cuerpo-pata y la rueda) y $d_{F_i}$ (distancia entre el centro y la junta cuerpo-pata).

Existen también dos ángulos: $\theta_{D_i}$ (ángulo entre de la junta cuerpo-pata respecto al centro) y $\alpha_{R_i}$ (ángulo de la rueda respeto a la junta cuerpo-pata). La suma de estos ángulos será $^{C}\theta_{R_i}$.

Sabiendo esto tenemos estas 3 ecuaciones.

$$ \Huge {{ ^{C}d_{R_i} = Rot (\theta_{D_i}) * d_{R_i} + d_{F_i} }\tag{6.2.1}} $$

$$ \Huge {{ v_c = Rot (^{C}\theta_{R_i}) * v_{R_i} - w_{R_i} \times ^{C}d_{R_i} + w_{D_i} \times ^{C}d_{F_i} }\tag{6.2.2}} $$

$$ \Huge {{ w_c = w_{R_i} - W_{D_i} }\tag{6.2.3}} $$

Pasandolo a matrices obtenemos lo siguiente

$$ \Huge { V_C = \begin{bmatrix} v_{cx} \ v_{cy} \ w_c \end{bmatrix}= \begin{bmatrix} cos(^{C}\theta_{R_i}) & -sin(^{C}\theta_{R_i}) & ^{C}d_{Ry_i} & -^{C}d_{Fy_i} \ sin(^{C}\theta_{R_i}) & cos(^{C}\theta_{R_i}) & -^{C}d_{Rx_i} & ^{C}d_{Fx_i} \ 0 & 0 & 1 & -1 \end{bmatrix}* \begin{bmatrix} v_{Rx_i} / Radio_i \ v_{Ry_i} / Radio_i \ w_{R_i} \ w_{D_i} \end{bmatrix} }\tag{6.2.4}} $$

Separando las ruedas en fijas (la junta cuerpo-pata no gira) y rotativas (la junta cuerpo-pata si gira) y sabiendo que las ruedas no tienen velocidad en $Rx_i$, las matrices $J_i$ de ambas serán.

• Ruedas Fijas (non-steering) $$ \Huge { J_i = \begin{bmatrix} -R_i * sin(^{C}\theta_{R_i}) & ^{C}d_{Ry_i} \ R_i * cos(^{C}\theta_{R_i}) & -^{C}d_{Rx_i} \ 0 & 1 \end{bmatrix} }\tag{6.2.5}} $$

• Ruedas Rotativas (steering) $$ \Huge { J_i = \begin{bmatrix} -R_i * sin(^{C}\theta_{R_i}) & ^{C}d_{Ry_i} & -d_{Fy_i}\ R_i * cos(^{C}\theta_{R_i}) & -^{C}d_{Rx_i} & d_{Fx_i}\ 0 & 1 & -1 \end{bmatrix} }\tag{6.2.6}} $$

Un ejemplo puede ser el siguiente

6.3. Composición y Condiciones de No Deslizamiento

Utilizando el jacobiano calculado anteriomente tanto en el MCD como en el MCI, se pueden dar casos en el que se pidan ciertas velocidades que al efectuarse simultáneamente proboquen que el robot deslice. Para comprobar este error hay que realizar lo siguiente.

• Modelo Cinemático Directo Creamos un vector con todas las velocidades de las articulaciones sobre las cuales podemos actuar y creamos una matriz jacobiana cuya diagonal esté compuesta por todas las jacobianas de cada rueda.

$$ \Huge { \begin{bmatrix} I_1 \ I_2 \ \vdots \ I_N \end{bmatrix}V_c= \begin{bmatrix} J_1 & 0 & \ldots & 0 \ 0 & J_2 & \ldots & 0 \ \vdots & \vdots & \ddots & \vdots \ 0 & 0 & \ldots & J_N \end{bmatrix} \begin{bmatrix} \dot{q_1} \ \dot{q_2} \ \vdots \ \dot{q_N} \end{bmatrix} }\tag{6.3.1}} $$

Llamaremos a cada matriz de la siguiente manera:

$$ \Huge { A * V_c = B * \dot{q} }\tag{6.3.2}} $$

Para comprobar y/o evitar el deslizamiento deberemos seguir los siguientes pasos:

1. Comprobar si $\Omega(A) * B = 0$ (Siendo $\Omega(U) = U*(U^T*U)^{-1}*U^T-I$).
2. Si la ecuación anterior es 0, es un sistema determinado (una sola rueda).
3. Si la ecuación es distinta de 0, se calculará $\Omega(A) * B * \dots{q}$ y se igualarán todos sus elementos a 0 para que el robot no deslice.

• Modelo Cinemático Inverso El procedimiento es prácticamente igual:

1. Comprobar si $\Omega(B) * A = 0$.
2. Si la ecuación anterior es 0, es un sistema determinado (todas las ruedas son giratorias).
3. Si la ecuación es distinta de 0, se calculará $\Omega(B) * A * V_C$ y se igualarán todos sus elementos a 0 para que el robot no deslice.

matlabFunction(DK, 'Vars', Q, 'File', 'DK.m');

7. Modelo Dinámico de los Robots

7.1. Introducción

Queremos una relación entre las fuerzas del robot (ejercicdas por actuadores o externas) y su movimiento.

Sabiendo que las variables articulares de los robots son:

• Robot Móvil: $q_{móvil} = [W_x, W_y, W_{\phi}]$
• Robot Manipulador: $q_{manipulador} = [\theta_1, \theta_2, \ldots, \theta_n]$

Modelo Dinámico Directo:

$$ \Huge{{ \ddot{q} = B^{-1}(q) * \big(D(q)\tau - A^T(q)\lambda - C(q,\dot{q})\dot{q} -G(q) - \tau_d \big) }\tag{7.1.1}} $$

Modelo Dinámico Inverso:

$$ \Huge{{ B(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) + \tau_d = D(q)\tau - A^T(q)\lambda }\tag{7.1.2}} $$

Matrices:

7.2. Ecuaciones Lagrange-Euler

Cada robot es considerado como un conjunto de $N$ cuerpos rígidos con:

• Masa: $m_i$
• Tensor de Inercia: $I_i$
• Posición del Centro de Masas: $p_i$
• Velocidad Lineal del Centro de Masas: $v_i$
• Velocidad Angular del Centro de Masas: $w_i$
• Grados de Libertad: $q_i$

Energía Cinética: Ecuación de elementos.

$$ \Huge{{ E_c = \frac{1}{2} * v^2 * m }\tag{7.2.1}} $$

Ecuación de matrices.

$$ \Huge{ k_i = \frac{1}{2} \int_{V_i} \big( \ ^0\dot{p}^T * \ ^0\dot{p}) * \rho * dV } $$

$$ \Huge{{ k_i = \frac{1}{2} \int_{V_i} \big[ \big( \ ^0\dot{p}{C_i} + \ ^0 w_i \times \ ^0 [ \ ^{C_i}p] \big)^T * \big( \ ^0 \dot{p}{C_i} + \ ^0 w_i \times \ ^0 [ \ ^{C_i}p] \big) * \rho * dV }\tag{7.2.2}} $$

Esto se puede dividir en 3 ecuaciones que finalmente se quedan en 2

Se cambia de marco de referencia

Estas velocidades se pueden realcionar con las velocidades de las articulaciones utilizando el jacobiano (solo los elementos hasta el elemento $j$).

Con el fin de calcular el tensor de inercia (Ejemplo de uno de los elementos):

syms l h w x y z real
T=[z^2+y^2, -y*x, -z*x; -x*y, z^2+x^2, -z*y; -x*z, -y*z y^2+x^2]
Ia=int(int(int(T,z,0,h),y,0,l),x,0,w);
pretty(simplify(Ia))

Energía Potencial: Ecuación de elementos.

$$ \Huge{{ E_p = mgh }\tag{7.2.3}} $$

Ecuación de matrices: Se considera una masa puntual en el centro de masas ($C_i$) y la gravedad es $0^G = {[0, 0, -g]}^T$

$$ \Huge{{ u_i = m_i * \ ^0 G^T * \ ^0 p_{C_i} }\tag{7.2.4}} $$

Sabiendo que las ecuaciones de movimiento se calculan de la siguiente manera:

7.2.1. Robot Manipulador

Tienen componente $K$ y $U$ por lo que $L = K - U$. $K$ depende tanto de las posiciones de los elementos como de las velocidades, sin embargo, $U$ solo depende de las posiciones.

Sabiendo que $B(q)$ es una matriz cuadrada utilizamos cada fila para cada $\tau_i$

Sabiendo que $J * \frac{dq}{dt} = \frac{dp}{dt} \rightarrow J = \frac{dp}{dq}$

Finalmente:

Siendo $\tau_d = J(q)^T * F$ (siendo $F$ las fuerzas aplicadas al efector final), y $F_v$ y $F_s$ las matrices de fricción.

7.2.2. Robot Móvil

Se mueven en el plano por lo que $U = 0 \rightarrow L = K$

Siendo $A(q)$ la matriz de restricciones obtenidas por $A(q) * \dot{q} = 0$

7.2.3. Restricciones

Estos multiplicadores ($A$ y $S$) aparecen en la ecuación cuando un sistema mecánico está sometido a restricciones que limitan sus movimientos.

En estos ejemplos se utilizará $B(q) = I_{3 \times 3}$.

Sin utilizar la matriz $A$ podríamos aplicar un torque en una dirección en la que no es posible físicamente.

$$ \huge{ \ddot{q} = \tau + A\lambda } $$

Si mi robot se mide por las coordenadas $q = {[x, y, \phi]}^T$ pero no se puede mover en el eje $y$, la matriz $A(q)$ será la siguiente:

$$ \huge{ A(q)= \begin{bmatrix} -sin(\phi) \ cos(\phi) \ 0 \end{bmatrix} } $$

Esto hará que si queremos aplicar un torque de $\tau = {[2, 5, 0]}^T$ sobre un robot con $\phi = 0$, el sistema obligue a tener un $\lambda = -5$ para que se cumpla restingir la aceleración en el eje $y$.

$$ \huge{ \ddot{q} = \begin{bmatrix} 2 \ 5 \ 0 \end{bmatrix}+ \begin{bmatrix} 0 \ -5 \ 0 \end{bmatrix}= \begin{bmatrix} 2 \ 0 \ 0 \end{bmatrix} } $$

Para prescindir de $\lambda$ se utiliza la matriz $G$ la cual en vez de seleccionar las direcciones con restricciones, selecciona las direcciones sin restricciones. Se calcula de la siguiente manera:

$$ \huge{ A(q)^T * G(q)^T = 0 } $$

Si mi robot se mide por las coordenadas $q = {[x, y, \phi]}^T$ pero no se puede mover en el eje $y$, la matriz $G(q)$ será la siguiente:

$$ \huge{ G(q)= \begin{bmatrix} cos(\phi) & 0 \ sin(\phi) & 0 \ 0 & 1 \ \end{bmatrix} } $$

Esta matriz $G(q)$ cumple que $\dot{q} = G(q) * {[v, w]}^T$.

Debido a esto tenemos las siguientes ecuaciones

La matriz $S(q)$ utiliza las ecuaciones de movimiento del robot. Transforma las velocidades de las articulaciones en velocidades de las articulaciones usando estas ecuaciones las cuales restringen el movimiento del robot.

$$ \huge{ \dot{q}= \begin{bmatrix} cos(\phi) & 0 \ sin(\phi) & 0 \ 0 & 1 \end{bmatrix}* \begin{bmatrix} v \ w \end{bmatrix} } $$

$$ \huge{ \begin{bmatrix} v \ w \end{bmatrix}= \begin{bmatrix} \frac{r}{2} & \frac{r}{2} \ \frac{r}{2d} & \frac{r}{-2d} \end{bmatrix}* \begin{bmatrix} \dot{\theta_R} \ \dot{\theta_L} \end{bmatrix} } $$

$$ \huge{ S(q)= \begin{bmatrix} cos(\phi) & 0 \ sin(\phi) & 0 \ 0 & 1 \end{bmatrix}* \begin{bmatrix} \frac{r}{2} & \frac{r}{2} \ \frac{r}{2d} & \frac{r}{-2d} \end{bmatrix} } $$

Su aplicación es pasar de torques a torques pero teniendo las restricciones pertinentes.