Cinemàtica Inversa

Fórmules

Podem determinar que:

$$\begin{gathered} x_{1}=l_{1} \cos (\theta_1) ; \quad y_{1}=l_{1} \sin (\theta_1) ; \\ x_{2}=l_{2} \cos (\theta_1+\theta_2) ; \quad y_{2}=l_{2} \sin (\theta_1+\theta_2) \\ x=x_{1}+x_{2}=l_{1} \cos (\theta_1)+l_{2} \cos (\theta_1+\theta_2) \\ y=y_{1}+y_{2}=l_{1} \sin (\theta_1)+l_{2} \sin (\theta_1+\theta_2) \end{gathered}$$

En aquesta part, calculem les posicions dels punts en el pla:

• $$x_{1}=l_{1} \cos (\theta_1)$$
  Aquesta fórmula calcula la coordenada \(x\) del primer segment del braç robòtic, on \(l_{1}\) és la longitud del primer segment i \(\theta_1\) l'angle que forma amb l'eix \(x\).
• $$y_{1}=l_{1} \sin (\theta_1)$$
  Aquesta fórmula calcula la coordenada \(y\) del primer segment del braç robòtic, utilitzant la mateixa longitud i angle.
• $$x_{2}=l_{2} \cos (\theta_1+\theta_2)$$
  Aquesta fórmula calcula la coordenada \(x\) del segon segment del braç, on \(l_{2}\) és la longitud del segon segment i \(\theta_1+\theta_2\) l'angle total respecte a l'eix \(x\).
• $$y_{2}=l_{2} \sin (\theta_1+\theta_2)$$
  Aquesta fórmula calcula la coordenada \(y\) del segon segment, utilitzant la longitud del segon segment i l'angle total.
• $$x=x_{1}+x_{2}=l_{1} \cos (\theta_1)+l_{2} \cos (\theta_1+\theta_2)$$
  Aquesta fórmula calcula la coordenada \(x\) total del extrem del braç robòtic, sumant les contribucions dels dos segments.
• $$y=y_{1}+y_{2}=l_{1} \sin (\theta_1)+l_{2} \sin (\theta_1+\theta_2)$$
  Aquesta fórmula calcula la coordenada \(y\) total del extrem del braç robòtic, sumant les contribucions dels dos segments.

Aquesta part és més difícil, perquè necessitem la cinemàtica directa per convertir-la de nou.

$$\begin{gathered} x^{2}+y^{2}=l_{1}^{2}+l_{2}^{2}+2 l_{1} l_{2} \cos (\theta_2) \\ \theta_{2}=\arccos \left(\frac{x^{2}+y^{2}-l_{1}^{2}-l_{2}^{2}}{2 l_{1} l_{2}}\right) \\ x=l_{1} \cos (\theta_1)+l_{2} \cos (\theta_1) \cos (\theta_2)-l_{2} \sin (\theta_1) \sin (\theta_2) \\ x=\cos (\theta_1)(l_{1}+l_{2} \cos (\theta_2))-l_{2} \sin (\theta_1) \sin (\theta_2) \\ y=l_{1} \sin (\theta_1)+l_{2} \sin (\theta_1) \cos (\theta_2)+l_{2} \cos (\theta_1) \sin (\theta_2) \\ y=\sin (\theta_1)(l_{1}+l_{2} \cos (\theta_2))+l_{2} \cos (\theta_1) \sin (\theta_2) \\ x+y=(l_{1}+l_{2} \cos (\theta_2))(\cos (\theta_1)+\sin (\theta_1))+l_{2} \sin (\theta_2)(\cos (\theta_1)-\sin (\theta_1)) \\ x-y=(l_{1}+l_{2} \cos (\theta_2))(\cos (\theta_1)-\sin (\theta_1))+l_{2} \sin (\theta_2)(\cos (\theta_1)+\sin (\theta_1)) \\ x+y+x-y=2x=2(l_{1}+l_{2} \cos (\theta_2)) \cos (\theta_1)+2 l_{2} \sin (\theta_2) \cos (\theta_1) \\ 2x=\cos (\theta_1)(2(l_{1}+l_{2} \cos (\theta_2))+2 l_{2} \sin (\theta_2)) \end{gathered}$$

Demostració de la Fórmula dels Quadrats

La fórmula que volem justificar és:

$$x^2 + y^2 = l_1^2 + l_2^2 + 2 l_1 l_2 \cos(\theta_2)$$

Aplicació de la Llei dels Cosinus

La llei dels cosinus en un triàngul qualsevol ens diu que:

$$c^2 = a^2 + b^2 - 2ab \cos(\gamma)$$

On:

• \(c\) és el costat oposat a l'angle \(\gamma\).
• \(a\) i \(b\) són els altres dos costats del triàngul

Per aplicar-ho al nostre cas, considerem un triàngul format pels dos segments del braç robòtic i la línia entre l'origen i l'extrem del braç.

Aquí, els costats del triàngul són:

• \(l_1\): longitud del primer segment del braç
• \(l_2\): longitud del segon segment del braç
• La distància entre l'origen \((0,0)\) i l'extrem \((x,y)\) , que és \(\sqrt{x^2 + y^2}\)

L'angle entre els segments és \(\theta_2\). Aplicant la llei dels cosinus:

$$\left(\sqrt{x^2 + y^2}\right)^2 = l_1^2 + l_2^2 - 2 l_1 l_2 \cos(\theta_2)$$

Aquesta fórmula és útil per determinar l'angle $$\theta_2$$ a partir de les coordenades i les longituds del braç robòtic.

Aquesta part és per calcular els angles a partir de les coordenades:

• $$x^{2}+y^{2}=l_{1}^{2}+l_{2}^{2}+2 l_{1} l_{2} \cos (\theta_2)$$
  Aquesta equació utilitza el teorema de cosinus per relacionar la distància \(x\) i \(y\) amb les longituds dels segments i l'angle entre ells.
• $$\theta_{2}=\arccos \left(\frac{x^{2}+y^{2}-l_{1}^{2}-l_{2}^{2}}{2 l_{1} l_{2}}\right)$$
  Aquesta fórmula resol per \(\theta_{2}\), l'angle entre els dos segments, a partir de les coordenades \(x\) i \(y\).
• $$x=l_{1} \cos (\theta_1)+l_{2} \cos (\theta_1) \cos (\theta_2)-l_{2} \sin (\theta_1) \sin (\theta_2)$$
  Aquesta fórmula expressa la coordenada \(x\) en termes d'angles i longituds, tenint en compte \(\theta_1\) i \(\theta_2\).
• $$x=\cos (\theta_1)(l_{1}+l_{2} \cos (\theta_2))-l_{2} \sin (\theta_1) \sin (\theta_2)$$
  Aquesta fórmula simplifica l'expressió anterior per a \(x\), agrupant termes comuns.
• $$y=l_{1} \sin (\theta_1)+l_{2} \sin (\theta_1) \cos (\theta_2)+l_{2} \cos (\theta_1) \sin (\theta_2)$$
  Aquesta fórmula expressa la coordenada \(y\) en termes d'angles i longituds.
• $$y=\sin (\theta_1)(l_{1}+l_{2} \cos (\theta_2))+l_{2} \cos (\theta_1) \sin (\theta_2)$$
  Aquesta fórmula simplifica l'expressió anterior per a \(y\), agrupant termes comuns.
• $$x+y=(l_{1}+l_{2} \cos (\theta_2))(\cos (\theta_1)+\sin (\theta_1))+l_{2} \sin (\theta_2)(\cos (\theta_1)-\sin (\theta_1))$$
  Aquesta fórmula combina les contribucions de \(x\) i \(y\) en una sola expressió.
• $$x-y=(l_{1}+l_{2} \cos (\theta_2))(\cos (\theta_1)-\sin (\theta_1))+l_{2} \sin (\theta_2)(\cos (\theta_1)+\sin (\theta_1))$$
  Aquesta fórmula combina les contribucions de \(x\) i \(y\) en una altra expressió.
• $$x+y+x-y=2x=2(l_{1}+l_{2} \cos (\theta_2)) \cos (\theta_1)+2 l_{2} \sin (\theta_2) \cos (\theta_1)$$
  Aquesta fórmula simplifica les expressions anteriors per obtenir una forma neta de \(2x\).
• $$2x=\cos (\theta_1)(2(l_{1}+l_{2} \cos (\theta_2))+2 l_{2} \sin (\theta_2))$$
  Aquesta fórmula aïlla \(x\) en termes de \(\theta_1\), \(\theta_2\), i les longituds dels segments.

$$\theta_{1}=\arccos \left(\frac{2x}{2(l_{1}+l_{2} \cos (\theta_2))+2 l_{2} \sin (\theta_2)}\right)$$

Finalment, aquesta fórmula calcula l'angle \(\theta_1\) utilitzant el valor de \(x\), \(\theta_2\), i les longituds dels segments.

Cinemàtica Inversa - Càlculs

Primer substituïm \( l_1 = 3 \), \( l_2 = 2 \), \( \theta_1 = 30^\circ \) i \( \theta_2 = 45^\circ \) en les equacions i fem els càlculs pas a pas. Recorda que has de convertir els angles a radians per a les funcions trigonomètriques, perquè \(30^\circ = \frac{\pi}{6}\) radians i \(45^\circ = \frac{\pi}{4}\) radians.

1. Càlcul de \( x \) i \( y \)

Coordenades dels segments:

• Coordenada \( x_1 \): \[ x_1 = l_1 \cos(\theta_1) = 3 \cos\left(\frac{\pi}{6}\right) \] \[ \cos\left(\frac{\pi}{6}\right) = \frac{\sqrt{3}}{2} \] \[ x_1 = 3 \times \frac{\sqrt{3}}{2} = \frac{3\sqrt{3}}{2} \]
• Coordenada \( y_1 \): \[ y_1 = l_1 \sin(\theta_1) = 3 \sin\left(\frac{\pi}{6}\right) \] \[ \sin\left(\frac{\pi}{6}\right) = \frac{1}{2} \] \[ y_1 = 3 \times \frac{1}{2} = \frac{3}{2} \]
• Coordenada \( x_2 \): \[ x_2 = l_2 \cos(\theta_1 + \theta_2) = 2 \cos\left(\frac{\pi}{6} + \frac{\pi}{4}\right) \] \[ \frac{\pi}{6} + \frac{\pi}{4} = \frac{5\pi}{12} \] \[ \cos\left(\frac{5\pi}{12}\right) \approx 0.2588 \] \[ x_2 = 2 \times 0.2588 \approx 0.5176 \]
• Coordenada \( y_2 \): \[ y_2 = l_2 \sin(\theta_1 + \theta_2) = 2 \sin\left(\frac{\pi}{6} + \frac{\pi}{4}\right) \] \[ \sin\left(\frac{5\pi}{12}\right) \approx 0.9659 \] \[ y_2 = 2 \times 0.9659 \approx 1.9318 \]

Coordenades totals:

• Coordenada \( x \): \[ x = x_1 + x_2 = \frac{3\sqrt{3}}{2} + 0.5176 \approx 2.598 + 0.5176 = 3.1156 \]
• Coordenada \( y \): \[ y = y_1 + y_2 = \frac{3}{2} + 1.9318 \approx 1.5 + 1.9318 = 3.4318 \]

2. Càlcul de \(\theta_2\)

3. Càlcul de \(\theta_1\)

4. Solució Alternativa

Conclusió

• Coordenades totals: \( x \approx 3.1156 \), \( y \approx 3.4318 \).
• L'angle \(\theta_2\) és aproximadament \(45^\circ\), i \(\theta_1\) és aproximadament \(30^\circ\).

Si hi ha discrepàncies menors, és degut a l'arrodoniment durant els càlculs.

Solució Alternativa

Utilitzarem les expressions \(x + y\) i \(x - y\) per obtenir \(2x\). A continuació es detalla cada pas de la substitució i simplificació.

1. Expressió de \(x + y\)

2. Expressió de \(x - y\)

3. Obtenir \(2x\)

Conclusió