Robot esfèric: diferència entre les revisions
Contingut suprimit Contingut afegit
Cap resum de modificació |
Afegida cinemàtica inversa. |
||
Línia 39:
On <math>q=[\theta_{1}, \theta_{2}, d_{3}]^T</math>. S'ha de notar que la tercera articulació, la prismàtica, òbviament no afecta a la matriu de rotació. A més a més, l'orientació del vector unitari <math>y_{3}^0</math> està determinat únicament per la primera articulació, ja que l'eix de revolució de la segona articulació és paral·lela a l'eix <math>y_{3}</math>. En aquest cas, el sistema de coordenades 3 pot representar un sistema de coordenades de vectors unitaris <math>(n_{e},s_{e},a_{e}), T_{e}^3=I_{4}.</math>{{sfn|Siciliano|Sciavicco|Villani|Oriolo|2009|p=73}}
Per altra banda, la [[cinemàtica inversa]] permet trobar els valors de les variables de les articulacions corresponents a una posició del terminal determinada. En aquest cas, la posició que es vol obtenir al terminal és <math>O_3 = [x_3, y_3, z_3]</math> i s'han de trobar els valors de les variables <math>\theta_{1}, \theta_{2}</math> i <math>d_{3}</math> que permeten assolir-la.
Per aïllar les variables de les que depèn <math>O_3</math> és convenient expressar la posició del terminal respecte l'origen de coordenades 1. L'equació de matrius s'obté és:{{sfn|Siciliano|Sciavicco|Villani|Oriolo|2009|p=95}}
<math>(A_{1}^0)^{-1} T_{3}^0 = A_{2}^1 A_{3}^2</math>
Igualant els primers tres elements de les quartes columnes de les matrius a cada banda s'obté:
<math>O_{3}^1 = \begin{bmatrix} x_3 c_1 + y_3 s_1 \\ -z_3 \\ -x_3 s_1 + y_3 c_1 \end{bmatrix} = \begin{bmatrix} d_3 s_2 \\ -d_3 c_2 \\ d_2 \end{bmatrix}</math>
I aquestes equacions només depenen de les variables <math>\theta_{1}</math> i <math>d_3</math>. Per resoldre aquestes tres equacions amb dues incògnites, s'aplica la següent substitució:
<math>t = tan \frac{\theta_{1}}{2}</math>
Aleshores s'obté que:
<math>c_1 = \frac{1-t^2}{1+t^2}</math>
<math>s_1 = \frac{2t}{1+t^2}</math>
Substituint aquestes equacions al tercer component a la banda esquerra de l'equació s'obté:
<math>(d_2+y_3)t^2 + 2x_3t + d_2 - y_3 = 0</math>
I aquesta [[equació de segon grau]] de la variable <math>t</math>es pot resoldre aplicant la fórmula convencional, amb el resultat següent:
<math>t = \frac{-x_3 \pm\sqrt{x_{3}^2+y_{3}^2-d_{2}^2}}{d_2 + y_3} </math>
Les dues solucions resultants es corresponen a les dues diferents postures del manipulador. Així doncs:
<math>\theta_{1} = 2 atan2 (-x_3 \pm\sqrt{x_{3}^2+y_{3}^2-d_{2}^2},d_2+y_3)</math>
Una vegada es coneix <math>\theta_{1}</math>, elevant al quadrat i sumant els primers dos components de l'equació de matrius s'obté:
<math>d_3 = \sqrt{(x_3 c_1 + y_3 s_1)^2 + z_{3}^2}</math>
On només la solució amb <math>d_3 \ge 0<math> s'ha considerat. S'ha de notar que el matiex valor de <math>d_3</math> es correspon a les dues solucions per <math>\theta_{1}</math>. Finalment, si <math>d_3 \ne 0</math>, dels primers dos components de l'equació de matrius en resulta:
<math> \frac{x_3 c_1 + y_3 s_1}{-z_3} = \frac{d_3 s_2}{-d_3 c_2}</math>
I a partir d'aquesta equació es pot extreure <math>\theta_{2}</math>
<math>\theta_{2} = atan2(x_3 c_1 + y_3 s_1, z_3)</math>
A destacar que, si <math>d_3 = 0</math>, aleshores <math>\theta_{2}</math> no té una única solució.
== Referències ==
| |||