Paso 0: Identificación de Eslabones y Articulaciones

Numera los eslabones desde la base fija como eslabón 0 hasta el efector final como eslabón $n$. Hay $n$ articulaciones numeradas de 1 a $n$. La articulación $i$ conecta el eslabón $i-1$ con el eslabón $i$.

Paso 1: Dibujar los Ejes $Z$ ($Z_0, Z_1, \dots, Z_{n-1}$)

• Si la articulación $i$ es de rotación, alinea $Z_{i-1}$ con el eje físico de giro. Asigna el sentido positivo arbitrariamente (por convenio hacia arriba o hacia la derecha para ángulos positivos en sentido antihorario).
• Si la articulación $i$ es prismática, alinea $Z_{i-1}$ con la dirección física de traslación. El sentido positivo se toma en la dirección de extensión.

Paso 2: Fijar la Base $\{S_0\}$ y el Origen $O_0$

Sitúa el origen $O_0$ en un punto conveniente sobre $Z_0$ (típicamente en la intersección con la mesa de apoyo). Elige el eje $X_0$ perpendicular a $Z_0$. Truco de examen: Orienta $X_0$ de tal forma que cuando la primera coordenada articular $q_1 = 0$, el eje $X_0$ sea paralelo a la dirección natural de reposo del robot (lo que forzará que $\theta_1 = q_1$ sin desfase de ángulo inicial). Finalmente, $Y_0 = Z_0 \times X_0$ por regla de la mano derecha.

Matriz de Transformación Homogénea Elemental

La transformación entre los sistemas $\{S_{i-1}\}$ y $\{S_i\}$ viene dada por la composición estándar:

$$A_i = \text{Rot}(Z, \theta_i) \cdot \text{Tras}(Z, d_i) \cdot \text{Tras}(X, a_i) \cdot \text{Rot}(X, \alpha_i) = \begin{bmatrix} \cos\theta_i & -\sin\theta_i \cos\alpha_i & \sin\theta_i \sin\alpha_i & a_i \cos\theta_i \\ \sin\theta_i & \cos\theta_i \cos\alpha_i & -\cos\theta_i \sin\alpha_i & a_i \sin\theta_i \\ 0 & \sin\alpha_i & \cos\alpha_i & d_i \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

Desacoplo de Singularidades para Manipuladores con Muñeca Esférica

Para un robot de 6 GDL con muñeca esférica (ejes 4, 5 y 6 cortándose en el punto de muñeca $W$), evaluando el Jacobiano en el punto $W$:

$$J_w = \begin{bmatrix} J_{11} & 0_{3\times 3} \\ J_{21} & J_{22} \end{bmatrix} \implies \det(J_w) = \det(J_{11}) \cdot \det(J_{22})$$

• Singularidades del brazo ($\det(J_{11}) = 0$): Dependen exclusivamente de las 3 primeras articulaciones $(q_1, q_2, q_3)$. Ocurren cuando el brazo alcanza el límite exterior del espacio de trabajo (brazo completamente estirado o replegado sobre sí mismo) o cuando la muñeca cruza el eje de la base $Z_0$.
• Singularidades de la muñeca ($\det(J_{22}) = 0$): Dependen exclusivamente de la orientación interna de la muñeca. Para una muñeca esférica estándar tipo Euler Z-Y-Z o RPY: $$\det(J_{22}) = -\sin\theta_5 = 0 \implies \theta_5 = 0^\circ \text{ o } 180^\circ$$ Físicamente, los ejes $Z_3$ y $Z_5$ se vuelven colineales, perdiendo la capacidad de girar alrededor del eje ortogonal a ellos instantáneamente.

5.1 Casuística D1: Manipuladores Planares por Euler-Lagrange (2 GDL y 3 GDL)

Es la casuística más frecuente en problemas analíticos. Para resolverlo en examen sin omitir ningún paso ni signo:

1. Posición de los Centros de Gravedad ($p_{c_i}$): Escribe los vectores de posición de los centros de masa de cada eslabón respecto a la base $\{S_0\}$. Para un robot antropomórfico planar de 2 eslabones de longitudes $L_1, L_2$ con centros de masa a distancias $l_{c1}, l_{c2}$: $$p_{c_1} = \begin{bmatrix} l_{c1}\cos q_1 \\ l_{c1}\sin q_1 \\ 0 \end{bmatrix}, \qquad p_{c_2} = \begin{bmatrix} L_1\cos q_1 + l_{c2}\cos(q_1+q_2) \\ L_1\sin q_1 + l_{c2}\sin(q_1+q_2) \\ 0 \end{bmatrix}$$
2. Velocidades Lineales y Angulares: Deriva respecto al tiempo o utiliza las columnas del jacobiano traslacional: $$v_{c_i} = \dot{p}_{c_i} = J_{v,c_i}(q)\dot{q}, \qquad \omega_i = (\dot{q}_1 + \dots + \dot{q}_i) z_0$$
3. Energía Cinética Total ($T$) y Deducción de $M(q)$ de Forma Compacta: $$T = \sum_{i=1}^n \left( \frac{1}{2}m_i v_{c_i}^T v_{c_i} + \frac{1}{2} I_{zi} \omega_i^2 \right) = \frac{1}{2} \dot{q}^T M(q) \dot{q}$$ Desarrollando los productos escalares con la identidad $\cos(a)\cos(b) + \sin(a)\sin(b) = \cos(a-b)$, los términos cruzados se reducen limpiamente a $\cos q_2$: $$m_{11}(q) = m_1 l_{c1}^2 + I_{z1} + m_2(L_1^2 + l_{c2}^2 + 2L_1 l_{c2}\cos q_2) + I_{z2}$$ $$m_{12}(q) = m_{21}(q) = m_2(l_{c2}^2 + L_1 l_{c2}\cos q_2) + I_{z2}, \qquad m_{22}(q) = m_2 l_{c2}^2 + I_{z2}$$
4. Matriz de Coriolis y Fuerzas Centrífugas ($C(q,\dot{q})$) por Símbolos de Christoffel: $$c_{kj}(q,\dot{q}) = \sum_{i=1}^n \Gamma_{ijk} \dot{q}_i = \frac{1}{2} \sum_{i=1}^n \left( \frac{\partial m_{kj}}{\partial q_i} + \frac{\partial m_{ki}}{\partial q_j} - \frac{\partial m_{ij}}{\partial q_k} \right) \dot{q}_i$$
   ¿POR QUÉ EXIGEN CHRISTOFFEL EN EL EXAMEN? Porque sólo con la definición de Christoffel se garantiza matemáticamente que la matriz $\mathbf{\dot{M}(q) - 2C(q,\dot{q})}$ sea estrictamente antisimétrica ($\mathbf{x^T(\dot{M}-2C)x = 0}$ para cualquier vector $x$). Si agrupas los términos de Coriolis de forma arbitraria, esta propiedad se pierde y suspenderás la demostración de estabilidad de Lyapunov.
   Para el manipulador planar, definiendo $h = m_2 L_1 l_{c2}\sin q_2$: $$C(q,\dot{q}) = \begin{bmatrix} -h\dot{q}_2 & -h(\dot{q}_1 + \dot{q}_2) \\ h\dot{q}_1 & 0 \end{bmatrix}$$
5. Energía Potencial ($V$) y Vector de Gravedad ($g(q)$) — LAS 3 TRAMPAS DE EXAMEN:
   • Trampa A (Gravedad vertical en $-Y_0$): Si el robot se mueve en el plano vertical $XY$ y la gravedad apunta hacia $-Y_0$, $g_0 = [0, -g, 0]^T$: $$V = m_1 g y_{c_1} + m_2 g y_{c_2} \implies g_k(q) = \frac{\partial V}{\partial q_k}$$ $$g_1(q) = (m_1 l_{c1} + m_2 L_1)g\cos q_1 + m_2 l_{c2}g\cos(q_1+q_2), \quad g_2(q) = m_2 l_{c2}g\cos(q_1+q_2)$$
   • Trampa B (Gravedad vertical en $-Z_0$): Si el dibujo sitúa la vertical en el eje $Z_0$, $g_0 = [0, 0, -g]^T$, entonces $V = \sum m_i g z_{c_i}$.
   • Trampa C (Robot trabajando en plano horizontal): Si las articulaciones giran alrededor de ejes verticales paralelos a la gravedad (como en un SCARA en $q_1, q_2$), las alturas de los centros de masa no varían con el giro: $V = \text{cte} \implies \mathbf{g(q) = \mathbf{0}}$ (no hay par gravitatorio).

5.2 Casuística D2: Extracción de Parámetros Dinámicos para Newton-Euler (Estilo Dic 2019 P3)

En exámenes recientes (como Diciembre 2019 P3), la pregunta práctica no pide desarrollar todas las ecuaciones algebraicas a mano, sino extraer los parámetros dinámicos exactos para alimentar el algoritmo de Newton-Euler implementado en MATLAB (plantilla Matlab/03_Dinamica_y_Modelado/NE_R3GDL.m o el script resuelto Matlab/Robots_Examen/examen_diciembre_2019_p3.m).

Parámetro	Símbolo	Definición y Sistema de Referencia	Trampa Típica de Examen
Masa total	$m_i$	Masa escalar del eslabón $i$ ($m_i = \sum_k m_{i,k}$).	Olvidar sumar masas puntuales o herramientas en el extremo.
Centro de masas local	$\mathbf{s_{ii}}$	Vector de posición del centro de gravedad del eslabón $i$ expresado en el sistema de referencia propio $\{S_i\}$: $\mathbf{s_{ii}} = [s_x, s_y, s_z]^T$.	¡GRAVE ERROR!: Expresarlo respecto a la base $\{S_0\}$ o respecto a $\{S_{i-1}\}$. Debe medirse desde el origen $O_i$ en los ejes $\{X_i, Y_i, Z_i\}$.
Tensor de inercia baricéntrico	$\mathbf{I_{ii}}$	Matriz de inercia del eslabón $i$ calculada respecto a unos ejes paralelos a $\{S_i\}$ pero con origen en el propio centro de masas $C_i$.	Calcular la inercia en el origen de la articulación $O_i$ en lugar del centro de masas. Si se conoce en $O_i$, hay que restar $m d^2$ (Steiner inverso).
Inercia y fricción del motor	$J_{mi}, B_{mi}, R_i$	Inercia del rotor, rozamiento viscoso y relación de reducción articular ($R_i = \dot{q}_i / \dot{\theta}_{mi}$).	Confundir si $R_i < 1$ (reductora) o si dan el factor $r_i = 1/R_i > 1$. En la ecuación se divide por $R_i^2$ (o multiplica por $r_i^2$).

5.3 Casuística D3: Actuadores Reales, Reductoras de Velocidad y Fricción

Cuando el examen incluye motores eléctricos y reductores mecánicos, la ecuación dinámica debe incorporar los efectos inerciales y disipativos reflejados en el eje de la articulación:

• Inercia equivalente del motor reflejada: Dado que el rotor gira a velocidad $\dot{\theta}_{mi} = \dot{q}_i / R_i$, su energía cinética es $T_{mi} = \frac{1}{2}J_{mi}\dot{\theta}_{mi}^2 = \frac{1}{2}\left(\frac{J_{mi}}{R_i^2}\right)\dot{q}_i^2$. La inercia reflejada es: $$J_{m,eq,i} = \frac{J_{mi}}{R_i^2}$$
• Fricción viscosa reflejada: $B_{eq,i} = \frac{B_{mi}}{R_i^2}$.
• Par motor efectivo disponible: El par generado por el motor $\tau_{mi}$ se multiplica mecánicamente por la reductora: $\tau_i = \frac{\tau_{mi}}{R_i}$.
• Ecuación completa matricial: $$\mathbf{\left( M(q) + J_m \right) \ddot{q} + C(q,\dot{q})\dot{q} + B\dot{q} + g(q) = \tau}$$ donde $J_m = \text{diag}\left( \frac{J_{m1}}{R_1^2}, \dots, \frac{J_{mn}}{R_n^2} \right)$ y $B = \text{diag}\left( \frac{B_{m1}}{R_1^2}, \dots, \frac{B_{mn}}{R_n^2} \right)$.

6.1 Casuística C1: Control Monoarticular Descentralizado (PD y PID)

Planteamiento de examen: Cada articulación $i$ se controla de forma independiente mediante un lazo SISO, tratando el acoplamiento con los demás eslabones y el par gravitatorio como una perturbación externa de baja frecuencia $\tau_{\text{ext},i} \approx g_i(q)$:

$$J_{i} \ddot{q}_i + B_{i} \dot{q}_i = \tau_i - g_i(q)$$ donde $J_i = m_{ii}(q) + J_{m,eq,i}$ es la inercia efectiva de la articulación.

¿Cómo se anula el droop gravitatorio en examen? Las dos soluciones canónicas:

• Solución A (Recomendada): Precompensación de Gravedad en Cadena Directa (Feedforward): $$\tau_i = K_{p,i}(q_{d,i} - q_i) - K_{d,i}\dot{q}_i + \mathbf{g_i(q_d)}$$ Al evaluar en régimen permanente: $K_{p,i}e_{ss,i} + g_i(q_d) = g_i(q_d) \implies \mathbf{e_{ss,i} = 0}$.
  Ventajas: Anula el error exactamente sin degradar la estabilidad de lazo cerrado ni ralentizar la respuesta.
• Solución B: Acción Integral (PID): $$\tau_i = K_{p,i} e_i + K_{i,i}\int_0^t e_i(\tau)d\tau - K_{d,i}\dot{q}_i$$ El integrador va acumulando error hasta generar un par igual a la gravedad: $K_{i,i}\int e_i dt = g_i(q_d)$, haciendo $e_{ss,i} \to 0$.
  Inconveniente de examen: La acción integral reduce el margen de fase, introduce sobreimpulso y puede causar saturación del actuador (efecto integrator windup).

6.2 Casuística C2: Control Multivariable por Par Calculado (Computed Torque)

Filosofía: Técnica de Linealización por Realimentación Exacta (Feedback Linearization). En lugar de ignorar la dinámica multivariable, utiliza el modelo inverso no lineal completo para cancelar las fuerzas inerciales cruzadas, Coriolis y gravedad.

1. Ley de Control no lineal: $$\mathbf{\tau = M(q) u + C(q,\dot{q})\dot{q} + g(q)}$$ donde $u \in \mathbb{R}^n$ es una entrada de control lineal auxiliar.
2. Cancelación dinámica exacta: Sustituyendo $\tau$ en la ecuación real del manipulador: $$M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = M(q) u + C(q,\dot{q})\dot{q} + g(q) \implies M(q)\ddot{q} = M(q)u$$ Dado que la matriz de inercia $M(q)$ es siempre invertible: $$\mathbf{\ddot{q} = u}$$ ¡El robot multivariable acoplado queda transformado exactamente en $n$ integradores dobles lineales desacoplados!
3. Ley lineal auxiliar con seguimiento de trayectoria: $$u = \ddot{q}_d(t) + K_v(\dot{q}_d - \dot{q}) + K_p(q_d - q) = \ddot{q}_d(t) + K_v \dot{e}(t) + K_p e(t)$$
4. Dinámica lineal del error en lazo cerrado: Sustituyendo $u$ en $\ddot{q} = u$ y definiendo el error $e(t) = q_d(t) - q(t)$: $$\mathbf{\ddot{e}(t) + K_v \dot{e}(t) + K_p e(t) = \mathbf{0}}$$ Eligiendo $K_p$ y $K_v$ como matrices diagonales $K_p = \text{diag}(\omega_{ni}^2)$ y $K_v = \text{diag}(2\xi_i\omega_{ni})$, cada coordenada articular converge asintóticamente a cero con la dinámica de segundo orden especificada.

6.3 Casuística C3: Demostración Formal de Estabilidad de Lyapunov (PD + Gravedad)

2. Encoders Ópticos (Incremental vs Absoluto)

• Encoder Incremental: Disco con ranuras transparentes y opacas. Utiliza dos fotodetectores desfasados un cuarto de período ($90^\circ$ eléctricos), generando dos trenes de pulsos $A$ y $B$ en cuadratura. La secuencia de avance (si $A$ adelanta a $B$ o viceversa) determina el sentido de rotación. Multiplicando los flancos de subida y bajada de ambos canales se multiplica la resolución por 4. Posee además un canal $Z$ (Index) con una sola marca por vuelta para calibración de cero. Desventaja: Al desconectar la alimentación pierde la posición actual y requiere homing.
• Encoder Absoluto: Posee $N$ pistas concéntricas leídas simultáneamente por $N$ sensores ópticos, generando una palabra digital única para cada posición angular. Conoce la posición absoluta instantánea al arrancar sin necesidad de calibración.
• ¿Por qué se usa Código Gray y no Binario Natural?: En binario natural, al pasar de un sector a otro pueden cambiar múltiples bits simultáneamente (ejemplo: de $0111_2 = 7$ a $1000_2 = 8$ cambian 4 bits). Debido a mínimas tolerancias mecánicas o de lectura, los sensores no cambian a la vez, produciéndose lecturas erráticas intermedias transitorias desastrosas para el lazo de control. En el Código Gray, entre dos sectores contiguos cambia exactamente un único bit ($\Delta \text{bit} = 1$), eliminando cualquier ambigüedad en las transiciones.

3. Tabla Comparativa de Selección Industrial

Criterio Técnico	Sincro-Resolver	Encoder Óptico Incremental	Encoder Óptico Absoluto
Principio físico	Inducción electromagnética (analógico)	Interrupción fotométrica (pulsos digitales)	Lectura de patrón óptico digital en código Gray
Resistencia a temperatura	Extrema: $-55^\circ\text{C}$ a $+155^\circ\text{C}$ (sin electrónica interna)	Media: $-20^\circ\text{C}$ a $+85^\circ\text{C}$	Media: $-20^\circ\text{C}$ a $+85^\circ\text{C}$
Robustez ambiental	Total a polvo, aceite, vibraciones y choques mecánicos	Sensible a polvo, grasa y rayaduras en disco óptico	Sensible a suciedad; requiere estanqueidad IP65+
Inmunidad EMI	Excelente (señales sinusoidales de baja impedancia)	Moderada (requiere apantallamiento en líneas de pulsos)	Alta con buses digitales industriales (SSI, BiSS, EnDat)
Pérdida de posición	Absoluto dentro de 1 vuelta ($0^\circ$ a $360^\circ$)	Pierde posición al apagar (requiere homing)	Posición absoluta retenida (multivuelta con batería/engranaje)
Aplicación robótica	Servomotores pesados, soldadura, fundición, aeroespacial	Robots cartesianos, bajo coste, lazos de velocidad	Brazos articulados modernos de 6 GDL de alta precisión

1. Fundamento y Estructura en Dos Fases

El algoritmo de Newton-Euler calcula los pares/fuerzas articulares $\tau$ necesarios para generar una trayectoria $(q, \dot{q}, \ddot{q})$ mediante dos pasadas recursivas basadas en las leyes de Newton ($\sum F = m a_c$) y de Euler ($\sum M = I \dot{\omega} + \omega \times (I \omega)$):

• Fase 1: Propagación Cinemática hacia Adelante (Forward Pass, desde la base $i=0$ al efector $i=n$): Conociendo las condiciones en la base fija ($\omega_0 = 0, \dot{\omega}_0 = 0, \ddot{p}_0 = -g$ para incorporar la gravedad de forma natural sin añadir términos potenciales), se propagan las velocidades y aceleraciones eslabón a eslabón: $$\omega_i = R_i^{i-1} \left( \omega_{i-1} + z_{i-1} \dot{q}_i \right)$$ $$\dot{\omega}_i = R_i^{i-1} \left( \dot{\omega}_{i-1} + z_{i-1} \ddot{q}_i + \omega_{i-1} \times (z_{i-1} \dot{q}_i) \right)$$ $$\ddot{p}_i = R_i^{i-1} \ddot{p}_{i-1} + \dot{\omega}_i \times p_{i-1,i} + \omega_i \times (\omega_i \times p_{i-1,i})$$ $$\ddot{p}_{c_i} = \ddot{p}_i + \dot{\omega}_i \times r_{i,c_i} + \omega_i \times (\omega_i \times r_{i,c_i})$$
• Fase 2: Propagación Dinámica hacia Atrás (Backward Pass, desde el efector $i=n$ a la base $i=1$): Se calculan las fuerzas de inercia $F_i = m_i \ddot{p}_{c_i}$ y momentos dinámicos $N_i = I_i \dot{\omega}_i + \omega_i \times (I_i \omega_i)$ sobre cada eslabón. Conociendo las cargas externas en el efector ($f_{n+1}, n_{n+1}$), se realiza el balance de fuerzas y momentos hacia la base: $$f_i = R_{i+1}^i f_{i+1} + F_i$$ $$n_i = R_{i+1}^i n_{i+1} + p_{i-1,i} \times f_i + r_{i,c_i} \times F_i + N_i$$ El par articular motor se obtiene proyectando sobre el eje de movimiento: $$\mathbf{\tau_i = n_i^T z_{i-1}} \quad (\text{si articulación } i \text{ de rotación}) \qquad \mathbf{\tau_i = f_i^T z_{i-1}} \quad (\text{si prismática})$$

2. Comparativa de Complejidad Computacional y Justificación

Característica	Formulación Euler-Lagrange	Formulación Newton-Euler Recursivo
Naturaleza matemática	Energética (escalares: $T$ y $V$, derivadas simbólicas)	Vectorial recursiva (ecuaciones de equilibrio por eslabón)
Complejidad computacional	$\mathbf{O(n^4)}$ en formulación simbólica directa naive; $\mathbf{O(n^3)}$ optimizado	$\mathbf{O(n)}$ (Estrictamente lineal con el número de articulaciones)
Operaciones para $n=6$ GDL	$\sim 66.000$ multiplicaciones y sumas	$\sim 850$ multiplicaciones y $\sim 740$ sumas
Aplicación idónea	Análisis analítico, diseño estructural y leyes de control por Lyapunov	Control dinámico en tiempo real (par calculado a 1 kHz) y simulación rápida

1. Enunciado del Teorema de Pieper

Un manipulador serie de 6 grados de libertad admite una solución cerrada analítica (determinista y explícita) para el problema cinemático inverso si cumple al menos una de las dos condiciones geométricas de diseño de Pieper:

1. Tres ejes de articulación consecutivos se cortan en un único punto común (el caso más habitual: muñeca esférica, donde los ejes $Z_3, Z_4, Z_5$ o $Z_4, Z_5, Z_6$ concurren en el centro de muñeca $W$).
2. Tres ejes de articulación consecutivos son paralelos entre sí (ejemplo: robots planares o configuraciones particulares de robots antropomórficos).

2. Demostración Matemática del Desacoplo (Posición + Orientación)

Sea la pose del efector final deseada especificada por la matriz homogénea conocida:

$$T_0^6 = \begin{bmatrix} R_0^6 & p_{TCP} \\ 0_{1\times 3} & 1 \end{bmatrix}$$

Supongamos una muñeca esférica donde los ejes 4, 5 y 6 se cortan en el centro de muñeca $W$. En el marco D-H estándar, esto implica que las longitudes $a_4 = 0$ y los desplazamientos $d_5 = 0$, estando $W$ situado en el origen del sistema $\{S_4\}$. La distancia fija a lo largo de $Z_5$ (o de la herramienta $Z_6$) hasta el extremo de trabajo ($TCP$) es $d_6$.

Paso 1: Cálculo del Centro de Muñeca $W$ (Subproblema de Posición):

La posición del centro de la muñeca $p_w$ respecto a la base es totalmente independiente de los ángulos de la muñeca $(q_4, q_5, q_6)$:

$$p_{TCP} = p_w + R_0^6 \begin{bmatrix} 0 \\ 0 \\ d_6 \end{bmatrix} \implies \mathbf{p_w = p_{TCP} - d_6 \cdot R_0^6 \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} = p_{TCP} - d_6 \cdot a}$$

donde $a$ es el vector director de aproximación (tercera columna de $R_0^6$). Puesto que $p_{TCP}$ y $R_0^6$ son datos del problema, $p_w$ es un vector puramente numérico conocido.

Paso 2: Resolución de las Primeras 3 Articulaciones $(q_1, q_2, q_3)$:

Como $p_w$ depende exclusivamente de $(q_1, q_2, q_3)$ a través de la cinemática directa de los tres primeros eslabones ($p_w = f_1(q_1, q_2, q_3)$), resolvemos geométricamente o algebraicamente:

$$q_1 = \text{atan2}(p_{wy}, p_{wx}) \quad (\pm \text{ offset de hombro si } d_2 \neq 0)$$ $$q_2, q_3 \text{ mediante el teorema del coseno en el plano del brazo definido por } (q_1).$$

Paso 3: Subproblema de Orientación de la Muñeca $(q_4, q_5, q_6)$:

Una vez calculados $(q_1, q_2, q_3)$, la matriz de rotación de los tres primeros eslabones $R_0^3(q_1, q_2, q_3)$ es perfectamente conocida. Como $R_0^6 = R_0^3 \cdot R_3^6$, despejamos la rotación propia de la muñeca premultiplicando por su traspuesta:

$$\mathbf{R_3^6(q_4, q_5, q_6) = \left(R_0^3\right)^T \cdot R_0^6}$$

La matriz $R_3^6$ corresponde a una secuencia de ángulos de Euler estándar (Z-Y-Z o X-Y-Z). Igualando término a término los elementos de la matriz conocida con la forma paramétrica, se despejan $(q_4, q_5, q_6)$ sin ambigüedad mediante funciones $\text{atan2}$.

1. Demostración del Error en Régimen Permanente (Droop Gravitatorio en PD Puro)

Consideremos el lazo de control PD clásico que busca posicionar el robot en una consigna constante $q_d$ ($\dot{q}_d = 0, \ddot{q}_d = 0$):

$$\tau = K_p \tilde{q} - K_d \dot{q} \qquad \text{con error } \tilde{q} = q_d - q$$

Sustituyendo esta ley de par en la ecuación dinámica del robot:

$$M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = K_p(q_d - q) - K_d \dot{q}$$

En estado estacionario (equilibrio estático final), el robot se detiene: $\dot{q} = 0$ y $\ddot{q} = 0$. La ecuación se reduce a:

$$g(q_{ss}) = K_p (q_d - q_{ss}) = K_p \tilde{q}_{ss} \implies \mathbf{\tilde{q}_{ss} = K_p^{-1} g(q_{ss}) \neq 0}$$

Conclusión física: Para que los actuadores sostengan el peso de los eslabones ($g(q_{ss})$), el regulador PD necesita un error elástico residual permanente no nulo $\tilde{q}_{ss}$. Para anularlo se requiere o bien acción integral (PID) o bien compensación directa del par gravitatorio.

2. Demostración de Estabilidad Asintótica Global de PD + Compensación de Gravedad (Lyapunov)

Propongamos la ley de control con compensación gravitatoria en lazo cerrado:

$$\mathbf{\tau = K_p \tilde{q} - K_d \dot{q} + g(q)}$$

La ecuación de lazo cerrado resulta:

$$M(q)\ddot{q} + C(q,\dot{q})\dot{q} = K_p \tilde{q} - K_d \dot{q}$$

Elegimos la siguiente Función Candidata de Lyapunov (energía cinética residual + energía potencial elástica artificial):

$$V(q, \dot{q}) = \frac{1}{2} \dot{q}^T M(q) \dot{q} + \frac{1}{2} \tilde{q}^T K_p \tilde{q}$$

Como $M(q)$ es simétrica y estrictamente definida positiva ($M(q) > 0$) y $K_p$ es una matriz de ganancias diagonal definida positiva ($K_p > 0$), se verifica que:

$$V(q, \dot{q}) > 0 \quad \forall (\tilde{q}, \dot{q}) \neq (0, 0), \qquad V(0, 0) = 0 \quad (\text{Definida Positiva})$$

Derivamos $V$ respecto al tiempo a lo largo de las trayectorias del sistema (recordando que $\dot{\tilde{q}} = -\dot{q}$):

$$\dot{V} = \dot{q}^T M(q) \ddot{q} + \frac{1}{2} \dot{q}^T \dot{M}(q) \dot{q} + \tilde{q}^T K_p (-\dot{q})$$

Despejando $M(q)\ddot{q} = K_p \tilde{q} - K_d \dot{q} - C(q,\dot{q})\dot{q}$ y sustituyendo:

$$\dot{V} = \dot{q}^T \left( K_p \tilde{q} - K_d \dot{q} - C(q,\dot{q})\dot{q} \right) + \frac{1}{2} \dot{q}^T \dot{M}(q) \dot{q} - \dot{q}^T K_p \tilde{q}$$ $$\dot{V} = -\dot{q}^T K_d \dot{q} + \frac{1}{2} \dot{q}^T \left( \dot{M}(q) - 2 C(q,\dot{q}) \right) \dot{q}$$

Propiedad Clave de Antisimetría: Si la matriz $C(q,\dot{q})$ se formula con los símbolos de Christoffel, la matriz $N = \dot{M}(q) - 2C(q,\dot{q})$ es estrictamente antisimétrica ($x^T N x = 0 \;\; \forall x$). Por tanto:

$$\mathbf{\dot{V} = -\dot{q}^T K_d \dot{q} \le 0} \quad (\text{Semidefinida Negativa})$$

Por el Principio de Invarianza de LaSalle, el conjunto donde $\dot{V} = 0$ exige $\dot{q} = 0$. En ese conjunto, $\ddot{q} = 0 \implies K_p \tilde{q} = 0 \implies \tilde{q} = 0$. El único punto de equilibrio invariante es el origen $(\tilde{q} = 0, \dot{q} = 0)$. En consecuencia, el sistema es Global y Asintóticamente Estable.

3. Control por Par Calculado (Computed Torque Control)

El control por Par Calculado es una técnica de linealización por realimentación de estados que desacopla la dinámica no lineal multivariable del robot:

$$\mathbf{\tau = M(q) \cdot u + C(q,\dot{q})\dot{q} + g(q)}$$

donde $u$ es una ley de aceleración de control lineal auxiliar. Al sustituir este par $\tau$ en la dinámica real del manipulador, los términos no lineales de Coriolis, centrífugos y gravitatorios se cancelan exactamente:

$$M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = M(q) u + C(q,\dot{q})\dot{q} + g(q) \implies M(q)\ddot{q} = M(q)u \implies \mathbf{\ddot{q} = u}$$

Eligiendo la ley auxiliar $u$ con realimentación del error y feedforward de la aceleración deseada:

$$\mathbf{u = \ddot{q}_d + K_v(\dot{q}_d - \dot{q}) + K_p(q_d - q)}$$

la dinámica del error de seguimiento $\tilde{q} = q_d - q$ queda gobernada por:

$$\mathbf{\ddot{\tilde{q}} + K_v \dot{\tilde{q}} + K_p \tilde{q} = 0}$$

Esto transforma el sistema altamente acoplado y no lineal en $n$ osciladores desacoplados de segundo orden. Seleccionando matrices diagonales $K_p = \text{diag}(\omega_{n,i}^2)$ y $K_v = \text{diag}(2\xi_i \omega_{n,i})$, se garantiza amortiguamiento crítico ($\xi = 1$) y seguimiento exponencial exacto de trayectoria.

1. Espacio Articular vs Espacio Cartesiano

Característica	Planificación en Espacio Articular	Planificación en Espacio Cartesiano
Cálculo en tiempo real	Muy simple y rápido (polinomios independientes $q_i(t)$)	Costoso: requiere resolver cinemática inversa $q(t) = f^{-1}(p(t))$ a cada ciclo de reloj (1 ms)
Singularidades	No se bloquea: los actuadores siguen sus perfiles suaves	Peligro crítico: si la trayectoria pasa cerca de una singularidad, las velocidades articulares tienden a infinito
Control de la trayectoria del TCP	Impredecible entre puntos inicial y final (curvada en el espacio)	Geometría exacta conocida (líneas rectas, arcos circulares para soldadura o corte)

2. Polinomios Cúbicos vs Quínticos

• Polinomio Cúbico (Tercer Orden): $q(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3$. Dispone de 4 coeficientes para satisfacer 4 condiciones de contorno: posición y velocidad inicial y final ($q(0)=q_0, q(t_f)=q_f, \dot{q}(0)=0, \dot{q}(t_f)=0$). Inconveniente mecánico: La aceleración $\ddot{q}(t)$ es lineal y discontinua en $t=0$ y $t=t_f$ (salto brusco de aceleración $\implies$ tirón / *jerk* infinito), lo que provoca vibraciones estructurales y desgaste de reductores.
• Polinomio Quíntico (Quinto Orden): $q(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5$. Dispone de 6 coeficientes, permitiendo imponer además aceleración nula en los extremos ($\ddot{q}(0)=0, \ddot{q}(t_f)=0$). La aceleración es continua en todo el tramo, eliminando impactos dinámicos en los motores.

3. Perfil Trapezoidal de Velocidad (LSPB - Linear Segment with Parabolic Blends)

Consiste en una fase de aceleración constante $a$, una fase intermedia a velocidad de crucero constante $V_{max}$, y una fase de deceleración constante $-a$.

• Tiempo de aceleración (tramo parabólico): $t_b = \frac{q_0 - q_f + V_{max} t_f}{V_{max}}$.
• Condición física de viabilidad: La velocidad de crucero debe verificar $\frac{q_f - q_0}{t_f} < V_{max} \le 2\frac{q_f - q_0}{t_f}$. Si $V_{max} = 2\frac{q_f - q_0}{t_f}$, el perfil se vuelve triangular sin tramo de velocidad constante.