La robotique

L'ensemble des équations utiles
Basé sur la méthode de Richard P. Paul
Objectif: comprendre et réaliser les calculs liés aux robots.


Modélisation Géométrique Directe (MGD) Soit les lisaisons pivots d'un robot, chaque liaison est de centre $O_i$ à laquelle on associe un repère $R_i$ et un angle de rotation $\theta_i$, cette angle et l'angle de rotation moteur.
On commence par représenter les distances et les vecteurs en fonction du schéma ci contre.
$\overrightarrow{O_0O_1}=Z_0\vec z$ ;   $\overrightarrow{O_1O_2}=X_1\vec x_1+Z_1\vec z_1$ ;   $\overrightarrow{O_2O_3}=X_2\vec x_2$ ;    $\overrightarrow{O_3O_4}=X_3\vec x_3$ ;    $\overrightarrow{O_4O_5}=X_4\vec x_4$ et    $\overrightarrow{O_5O_6}=X_5\vec x_5$
Où les vecteurs $\vec{x_2}$ , $\vec{x_3}$ et $\vec{x_4}$ sont les vecteurs unitaires respectivement colinéaires aux vecteurs $\overrightarrow{O_2O_3}$, $\overrightarrow{O_3O_4}$ et $\overrightarrow{O_4O_5}$.
Il y a égalité remarquables $\vec y_2=\vec y_3$, $\vec y_4=\vec y_5$, $\vec x_3=\vec x_4$, $\vec x_5=\vec x_6$

Modélisation Géométrique Directe (MGD)

Convention de Khalil-Kleinfinger.

Elle permet d’exprimer le passage d’un point $i$ à un point $j$, à l’aide d’une matrice 4 × 4 sous la forme :
$[T_{i,j}] =\begin{bmatrix} A & P \\ [0,0,0] & 1 \end{bmatrix}$ Avec $A$ la matrice de rotation 3 × 3 et P le vecteur colonne 1 × 3 de déplacement.
Pour réaliser un changement de repère $[T_{i,k}]$ d'un point $i$ à un point $k$ :
$T_{i,k} = T_{i,j}\cdot T_{j,k}$
Notons que cette forme présente aussi l’intérêt de pouvoir être inversée facilement. Le changement de repère $[T_{j,i}]$ est donné par
$T_{j,i}={T_{i,j}}^{-1}= \begin{bmatrix} A^T & - A^T.P \\ [0,0,0] & 1 \end{bmatrix}$ où AT est la transposée de A.

Notation robotique

Soit le solide $i$ dans le repère $j$, en robotique avec :
$^jT_i =\begin{bmatrix} ^jR_i & ^jP_i \\ 0 & 1 \end{bmatrix}$
La notation donne $^jT_i= \begin{bmatrix} S_x & N_x & A_x & P_x \\ S_y & N_y & A_y & P_y \\ S_z & N_z & A_z & P_z \\ 0 & 0 & 0 & 1 \end{bmatrix} $
SNA sont les cosinus directeur.
S Vecteur x du projeter dans le repère précédent.
N Vecteur y du projeter dans le repère précédent.
A Vecteur z du projeter dans le repère précédent.
P Coordonnée du point dans le repère précédent.
Connaissant $^AT_B$ et $^BT_C$ on peut calculer $^AT_C=^AT_B\times^BT_C$

Matrice de transformation de base

Transformation d'une translation

$T(x,y,z) = \begin{bmatrix} 1 & 0 & 0 & x \\ 0 & 1 & 0 & y \\ 0 & 0 & 1 & z \\ 0 & 0 & 0 & 1 \end{bmatrix} $

Transformation d'une rotation autour de l'axe $w={x,y,z}$

$T{0,1} = \begin{bmatrix} & & & x \\ & R_w(q) & & y \\ & & & z \\ 0 & 0 & 0 & 1 \end{bmatrix} $

Matrice de transformation globale

$^0T_7=^0T_1\cdot ^1T_2\cdot ^2T_3\cdot ^3T_4\cdot ^4T_5\cdot ^5T_6\cdot ^6T_7$
Modélisation cinématique inverse (MCI) Soit la position de l'élément finale connue nommée $U_0$ avec $U_0= \begin{bmatrix} S_x & N_x & A_x & P_x \\ S_y & N_y & A_y & P_y \\ S_z & N_z & A_z & P_z \\ 0 & 0 & 0 & 1 \end{bmatrix} $
Avec S le vecteur d'orientation suivant de X
Avec N le vecteur d'orientation suivant de Y
Avec A le vecteur d'orientation suivant de Z
Et P le point (X,Y,Z)
Equation générale à résoudre :
$U_0=^0T_1\cdot ^1T_2\cdot ^2T_3\cdot ^3T_4\cdot ^4T_5\cdot ^5T_6\cdot ^6T_7$
1ère itération à résoudre
$^1T_0\cdot U_0=^1T_0\cdot ^0T_1\cdot ^1T_2\cdot ^2T_3\cdot ^3T_4\cdot ^4T_5\cdot ^5T_6\cdot ^6T_7\\ ^1T_0\cdot U_0=^1T_2\cdot ^2T_3\cdot ^3T_4\cdot ^4T_5\cdot ^5T_6\cdot ^6T_7$
2ème itération à résoudre
$^2T_1\cdot^1 T_0\cdot U_0=^2T_1\cdot^1T_2\cdot ^2T_3\cdot ^3T_4\cdot ^4T_5\cdot ^5T_6\cdot ^6T_7\\ ^2T_1\cdot^1 T_0\cdot U_0=^2T_3\cdot ^3T_4\cdot ^4T_5\cdot ^5T_6\cdot ^6T_7$
3ème itération à résoudre
$^3T_2\cdot ^2T_1\cdot^1 T_0\cdot U_0=^3T_4\cdot ^4T_5\cdot ^5T_6\cdot ^6T_7$
4ème itération à résoudre
$^4T_3\cdot ^3T_2\cdot ^2T_1\cdot^1 T_0\cdot U_0=^4T_5\cdot ^5T_6\cdot ^6T_7$
5ème itération à résoudre
$^5T_4\cdot ^4T_3\cdot ^3T_2\cdot ^2T_1\cdot^1 T_0\cdot U_0=^5T_6\cdot ^6T_7$
6ème itération à résoudre
$^6T_5\cdot ^5T_4\cdot ^4T_3\cdot ^3T_2\cdot ^2T_1\cdot^1 T_0\cdot U_0=^6T_7$

Transposé de la matrice

Si $^{i-1}T_i=\begin{bmatrix} & ^{i-1}R_i & & ^{i-1}\overrightarrow{O_{i-1}O_i} \\ 0 & 0 & 0 & 1 \end{bmatrix}$
Alor $^iT_{i-1}=\begin{bmatrix} & ^{i-1}R_i^T & & -^{i-1}R_i^T\cdot ^{i-1}\overrightarrow{O_{i-1}O_i} \\ 0 & 0 & 0 & 1 \end{bmatrix}$

Exemple de transposé

$^0T_1 = \begin{bmatrix} \cos \theta_1 & - \sin \theta_1 & 0 & 0 \\ \sin \theta_1 & \cos \theta_1 & 0 & 0 \\ 0 & 0 & 1 & Z_0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $
La matrice de rotation est donc
$^0R_1 = \begin{bmatrix} \cos \theta_1 & - \sin \theta_1 & 0 \\ \sin \theta_1 & \cos \theta_1 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} $
La matrice de transposée devient
$^1R_0 = \begin{bmatrix} \cos \theta_1 & \sin \theta_1 & 0 \\ - \sin \theta_1 & \cos \theta_1 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} $

Le vecteur :

$^0\overrightarrow{O_{i-1}O_i}=\begin{bmatrix} 0\\ 0\\ Z0 \end{bmatrix} $

Calcul :

$-^1R_0 = \begin{bmatrix} \cos \theta_1 & \sin \theta_1 & 0 \\ - \sin \theta_1 & \cos \theta_1 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} 0\\ 0\\ -Z0 \end{bmatrix}$

Matrice complétée

$^1T_0 = \begin{bmatrix} \cos \theta_1 & \sin \theta_1 & 0 & 0 \\ - \sin \theta_1 & \cos \theta_1 & 0 & 0 \\ 0 & 0 & 1 & -Z_0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $

Concrètement

En fonction de notre schéma cinématique, les équations sont les suivantes:
Pivot en $\vec Z1$ de centre $(0,0,Z_0)$ de 0/1
dans le repère $R_0$
$T{0,1} = \begin{bmatrix} \cos \theta_1 & - \sin \theta_1 & 0 & 0 \\ \sin \theta_1 & \cos \theta_1 & 0 & 0 \\ 0 & 0 & 1 & Z_0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $ 		Pivot en $\vec Y_2$ de centre $O_2(X_1,0,Z_1)$ de 1/2
dans le repère $R_1$
$T_{1,2} = \begin{bmatrix} \cos \theta_2 & 0 & \sin \theta_2 & X_1 \\ 0 & 1 & 0 & 0\\ -\sin \theta_2 & 0 & \cos \theta_2 & Z_1 \\ 0 & 0 & 0 & 1 \end{bmatrix} $ 		Pivot en $\vec Y_3$ de centre $O_3(X_2,0,0)$ de 2/3
dans le repère $R_2$
$T_{2,3} = \begin{bmatrix} \cos \theta_3 & 0 & \sin \theta_3 & X_2 \\ 0 & 1 & 0 & 0\\ -\sin \theta_3 & 0 & \cos \theta_3 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $
Pivot en $\vec X_4$ de centre $O_4(X_3,0,0)$ de 3/4
dans le repère $R_3$
$T_{3,4} = \begin{bmatrix} 1 & 0 & 0 & X_3\\ 0 & \cos \theta_4 & -\sin \theta_4 & 0 \\ 0 & \sin \theta_4 & \cos \theta_4 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $ 		Pivot en $\vec Y_5$ de centre $O_5(X_4,0,0)$ de 4/5
dans le repère $R_4$
$T_{4,5} = \begin{bmatrix} \cos \theta_5 & 0 & \sin \theta_5 & X_4 \\ 0 & 1 & 0 & 0\\ -\sin \theta_5 & 0 & \cos \theta_5 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $ 		Pivot en $\vec X_6$ de centre $O_6(X_5,0,0)$ de 5/6
dans le repère $R_5$
$T_{5,6} = \begin{bmatrix} 1 & 0 & 0 & X_5\\ 0 & \cos \theta_6 & -\sin \theta_6 & 0 \\ 0 & \sin \theta_6 & \cos \theta_6 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $
Juste pour placer le point $O_7(X_6,0,0)$ de 6/7
dans le repère $R_6$ $T_{6,7} = \begin{bmatrix} 1 & 0 & 0 & X_6\\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $

Coeur de la méthode de Paul

La méthode de Paul décrite par la suite peut s'appliquer dans le cas de robots industriels à 6 degrées de liberté ayant un poignet.
On appelle P le point d'intersection des axes concourants des 3 dernières articulations ($O_4, O_5,O_6,O_7$).
La position du point P (centre du poignet) est fonction des variables articulaires $q_1,q_2,q_3$.
Aussi, ce type de structure de robot permet de décomposer le problème du calcul des 6 variables articulaires en 2 problèmes :

Equation de position

L'équation de positon est liée au porteur. Puisque $O_4=O_5=O_6=O_7$ on a :
$\begin{bmatrix} P_x\\ P_y \\ P_z\\ 1 \end{bmatrix} =^0T_1\times ^1T_2\times ^2T_3\times ^3T_4\times \begin{bmatrix} 0\\ 0\\ 0\\ 1 \end{bmatrix}$

Première étape

Pour se simplifier les notations, nous prendrons $c1$ pour $\cos q_1$, $s1$ pour $\sin s_1$, $c23$ pour $\cos(q_2+q_3)$, ...
$^1T_0\times \begin{bmatrix} P_x\\ P_y \\ P_z\\ 1 \end{bmatrix}=^1T_4\times \begin{bmatrix} 0\\ 0\\ 0\\ 1 \end{bmatrix}$
$\begin{bmatrix} c1\cdot P_x+s1\cdot P_y\\ -s1\cdot P_x+c1\cdot P_y\\ P_z-L0\\ 1 \end{bmatrix}=\begin{bmatrix} (c2\cdot s3+s2\cdot c3)\cdot L3+s2\cdot L2\\ 0\\ (-s2\cdot s3+c2\cdot c3)\cdot L3+c2\cdot L2+L1\\ 1 \end{bmatrix}$
Avec les règles de trigo, $c2\cdot s3+s2\cdot c3=s23$
Cette fois, ce n'est pas utile, mais nous pouvons simplifier.
$\begin{bmatrix}(1) c1\cdot P_x+s1\cdot P_y\\(2) -s1\cdot P_x+c1\cdot P_y\\(3) P_z-L0\\ 1 \end{bmatrix}=\begin{bmatrix} s23 \cdot L3+s2\cdot L2\\ 0\\ c23 \cdot L3+c2\cdot L2+L1\\ 1 \end{bmatrix}$
L'équation (2) nous permet de resoudre la valeur de $q_1$
$-s1\cdot P_x+c1\cdot P_y=0$ → $\dfrac{s1}{c1}=\dfrac{P_y}{P_x}=$ → $\boldsymbol{q_1=atan(P_x/P_y)}$

Deuxième étape

$^2T_0\times \begin{bmatrix} P_x\\ P_y \\ P_z\\ 1 \end{bmatrix}=^2T_4\times \begin{bmatrix} 0\\ 0\\ 0\\ 1 \end{bmatrix}$
$\begin{bmatrix} (4) (c2\cdot c1)\cdot P_x+(c2\cdot s1)\cdot P_y-s2\cdot P_z+s2\cdot L0\\ (5) -s1\cdot P_x+c1\cdot P_y\\ (6) (s2\cdot c1)\cdot P_x+(s2\cdot s1)\cdot P_y+c2\cdot P_z-c2\cdot L0\\ 0\\ \end{bmatrix}=\begin{bmatrix} s3\cdot L3\\ 0\\ c3\cdot L3+L2\\ 1\\ \end{bmatrix}$
$(4): c2(c1\cdot P_x+s1\cdot P_y)-s2\cdot P_z+s2\cdot L0=s3\cdot L3$
$(6): s2(c1\cdot P_x+s1\cdot P_y)+c2\cdot P_z-c2\cdot L0-L2=c3\cdot L3$
Nous prendrons $\boldsymbol{b1=c1\cdot P_x+s1\cdot P_y}$
$(4): c2\cdot b1-s2\cdot P_z+s2\cdot L0=s3\cdot L3$
$(6): s2\cdot b1+c2\cdot P_z-c2\cdot L0-L2=c3\cdot L3$
En sommant ces 2 équations, préalablement mises au carré, on obtient l'équation suivante :
$(4'+6')^2: -2\cdot s2\cdot b1\cdot L2+2\cdot c2\cdot L2\cdot (L0-P_z)=L3^2-L2^2-b1^2-(P_z-L0)^2$
soit l'expression: $X\cdot s2+Y\cdot c2=Z$
$X=-2\cdot b1\cdot L2$
$Y=2\cdot L2\cdot (L0-P_z)$
$Z=L3^2-L2^2-b1^2-(P_z-L0)^2$
$\left\{ \begin{array}{l} \sin(q_2)=\dfrac{XZ+\epsilon Y \sqrt{X^2+Y^2-Z^2}}{X^2+Y^2} \\ & avec~\epsilon=\pm 1\\ cos(q_2)=\dfrac{YZ-\epsilon X \sqrt{X^2+Y^2-Z^2}}{X^2+Y^2} \end{array}\right\}$
Nous trouvons la valeur de $\boldsymbol{q_2=atan\left(\dfrac{XZ+\epsilon Y \sqrt{X^2+Y^2-Z^2}}{YZ-\epsilon X \sqrt{X^2+Y^2-Z^2}}\right)}$
Je fait le choix de prendre la plus grande valeur de Z pour le choix d'epsilon, soit la valeur du cosinus.

Résolution de $q_3$

A partir des équations (4) et (6):
$\boldsymbol{q_3=atan \dfrac{c2\cdot b1+s2(L0-P_z)}{s2\cdot b1+c2(P_z-L0-L2)}}$

Equation d'orientation

Il faut commencer par orienter l'effecteur, soit la matrice de rotation SNA, exemple du vecteur $\vec A$:

$SNA= \begin{bmatrix} S_x & N_x & A_x \\ S_y & N_y & A_y \\ S_z & N_z & A_z \\ \end{bmatrix} $


Et positionner le point d'arrivée. $P(x, y, z)$ ce qui donne la matrice $^0T_7=\begin{bmatrix} S_x & N_x & A_x & P_X\\ S_y & N_y & A_y & P_Y\\ S_z & N_z & A_z & P_Z\\ 0 & 0 & 0 & 1 \\ \end{bmatrix} $
Application numérique

Evaluation de O7

Nous pouvons maintenant évaluer la position (X, Y, Z) du point O5 dans dans le repère du robot en calculant la matrice de transfert $T_{0,5}$ qui s’exprime alors:
$T_{0,5}=T_{0,1}\cdot T_{1,2}\cdot T_{2,3}\cdot T_{3,4}\cdot T_{4,5}\cdot T_{5,6}\cdot T_{6,7}$
$T_{0,2}=T_{0,1}\cdot T_{1,2}= \begin{bmatrix} \cos \theta_1 & - \sin \theta_1 & 0 & X_0 \\ \sin \theta_1 & \cos \theta_1 & 0 & Y_0 \\ 0 & 0 & 1 & Z_0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \times \begin{bmatrix} \cos \theta_2 & 0 & \sin \theta_2 & X_1 \\ 0 & 1 & 0 & 0\\ -\sin \theta_2 & 0 & \cos \theta_2 & Z_1 \\ 0 & 0 & 0 & 1 \end{bmatrix}\\$
Appelons $\cos \theta_1\rightarrow c_1,~sin \theta_1 \rightarrow s_1,~\ldots$
$T_{0,2}=\begin{bmatrix} c_1\cdot c_2 & -s_1 & c_1 \cdot s_2 & X_1 \cdot c_1+X_0\\ s_1\cdot c_2 & c_1 & s_1 \cdot s_2 & X_1\cdot s_1 +Y_0 \\ -s_2 & 0 & c_2 & Z_0+Z_1 \\ 0 & 0 & 0 & 1 \end{bmatrix}$
$T_{0,3} = T_{0,2} \cdot T_{2,3}= \begin{bmatrix} c_1\cdot c_2 & -s_1 & c_1 \cdot s_2 & X_1 \cdot c_1+X_0\\ s_1\cdot c_2 & c_1 & s_1 \cdot s_2 & X_1\cdot s_1 +Y_0 \\ -s_2 & 0 & c_2 & Z_0+Z_1 \\ 0 & 0 & 0 & 1 \end{bmatrix}\\ \times \begin{bmatrix} c_3 & 0 & s_3 & X_2 \\ 0 & 1 & 0 & 0\\ -s_3 & 0 & c_3 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}\\ =\begin{bmatrix} c1\cdot c2\cdot c3-c1\cdot s2\cdot s3 & -s1 & c1\cdot c2\cdot s3+c1\cdot s2\cdot c3 & c1\cdot c2\cdot X2+c1\cdot X1+X0 \\ s1\cdot c2\cdot c3-s1\cdot s2\cdot s3 & c1 & s1\cdot c2\cdot s3+s1\cdot s2\cdot c3 & s1\cdot c2\cdot X2+s1\cdot X1+Y0 \\ -s2\cdot c3-c2\cdot s3 & 0 & -s2\cdot s3+c2\cdot c3 & -s2\cdot X2+Z1+Z0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$
Simplification ... c23= $\cos (\theta_2+\theta_3)$
$T_{0,3} =\begin{bmatrix} c1\cdot c23 & -s1 & c1\cdot s23 & c1\cdot s2\cdot L2\\ s1\cdot c23 & c1 & s1\cdot s23 & s1\cdot s2\cdot L2\\ -s23 & 0 & c23 & c2\cdot L2+L1+L0\\ 0 & 0 & 0 & 1 \end{bmatrix}$
$T_{0,4} = T_{0,3} \cdot T_{3,4}=\\ T_{0,3} \times \begin{bmatrix} 1 & 0 & 0 & X_3\\ 0 & c_4 & -s_4 & 0 \\ 0 & s_4 & c_4 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$
$T_{0,4} =\begin{bmatrix} c1\cdot c23\cdot c4-s1\cdot s4 & -c1\cdot c23\cdot s4-s1\cdot c4 & c1\cdot s23 & c1\cdot s23\cdot L3+c1\cdot s2\cdot L2\\ s1\cdot c23\cdot c4+c1\cdot s4 & -s1\cdot c23\cdot s4+c1\cdot c4 & s1\cdot s23 & s1\cdot s23\cdot L3+s1\cdot s2\cdot L2\\ -s23\cdot c4 & s23\cdot s4 & c23 & c23\cdot L3+c2\cdot L2+L1+L0\\ 0 & 0 & 0 & 1 \end{bmatrix}$
$T_{0,5}=\\ \small\begin{bmatrix} c1\cdot c23\cdot c4\cdot c5 -s1\cdot s4\cdot c5 -c1\cdot s23\cdot s5 & -c1\cdot c23\cdot s4-s1\cdot c4 & c1\cdot c23\cdot c4\cdot s5-s1\cdot s4\cdot s5+c1\cdot s23\cdot c5 & c1\cdot (s23\cdot L4+s23\cdot L3+s2\cdot L2)\\ s1\cdot c23\cdot c4\cdot c5+c1\cdot s4\cdot c5-s1\cdot s23\cdot s5 & -s1\cdot c23\cdot s4+c1\cdot c4 & s1\cdot c23\cdot c4\cdot s5+c1\cdot s4\cdot s5+s1\cdot s23\cdot c5 & s1\cdot (s23\cdot L4+s23\cdot L3+s2\cdot L2)\\ -s23\cdot c4\cdot c5-c23\cdot s5 & s23\cdot s4 & -s23\cdot c4\cdot s5+c23\cdot c5 & c23\cdot L4+c23\cdot L3+c2\cdot L2+L1+L0\\ 0 & 0 & 0 & 1 \end{bmatrix}$
$T_{0,6}=\\ \tiny\begin{bmatrix} (c1\cdot c23\cdot c4\cdot c5 -s1\cdot s4\cdot c5 -c1\cdot s23\cdot s5)\cdot c6+(-c1\cdot c23\cdot s4-s1\cdot c4)\cdot s6 & -(c1\cdot c23\cdot c4\cdot c5 -s1\cdot s4\cdot c5 -c1\cdot s23\cdot s5)\cdot s6-(c1\cdot c23\cdot s4s1\cdot c4)\cdot c6 & c1\cdot c23\cdot c4\cdot s5-s1\cdot s4\cdot s5+c1\cdot s23\cdot c5 & (c1\cdot c23\cdot c4\cdot s5-s1\cdot s4\cdot s5+c1\cdot s23\cdot c5)\cdot L5+c1\cdot (s23\cdot L4+s23\cdot L3+s2\cdot L2)\\ (s1\cdot c23\cdot c4\cdot c5+c1\cdot s4\cdot c5-s1\cdot s23\cdot s5)\cdot c6+(c1\cdot c4-s1\cdot c23\cdot s4)\cdot s6 & -(s1\cdot c23\cdot c4\cdot c5+c1\cdot s4\cdot c5-s1\cdot s23\cdot s5)\cdot s6+(c1\cdot c4-s1\cdot c23\cdot s4)\cdot c6 & s1\cdot c23\cdot c4\cdot s5+c1\cdot s4\cdot s5+s1\cdot s23\cdot c5 & (s1\cdot c23\cdot c4\cdot s5+c1\cdot s4\cdot s5+s1\cdot s23\cdot c5)\cdot L5+s1\cdot (s23\cdot L4+s23\cdot L3+s2\cdot L2)\\ (-s23\cdot c4\cdot c5-c23\cdot s5)\cdot c6+(s23\cdot s4)\cdot s6 & (s23\cdot c4\cdot c5+c23\cdot s5)\cdot s6+(s23\cdot s4)\cdot c6 & -s23\cdot c4\cdot s5+c23\cdot c5 & (-s23\cdot c4\cdot s5+c23\cdot c5)\cdot L5+c23\cdot L4+c23\cdot L3+c2\cdot L2+L1+L0\\ 0 & 0 & 0 & 1 \end{bmatrix}$
$T_{0,7}=\\ \tiny\begin{bmatrix} (c1\cdot c23\cdot c4\cdot c5 -s1\cdot s4\cdot c5 -c1\cdot s23\cdot s5)\cdot c6+(-c1\cdot c23\cdot s4-s1\cdot c4)\cdot s6 & -(c1\cdot c23\cdot c4\cdot c5 -s1\cdot s4\cdot c5 -c1\cdot s23\cdot s5)\cdot s6-(c1\cdot c23\cdot s4s1\cdot c4)\cdot c6 & c1\cdot c23\cdot c4\cdot s5-s1\cdot s4\cdot s5+c1\cdot s23\cdot c5 & (c1\cdot c23\cdot c4\cdot s5-s1\cdot s4\cdot s5+c1\cdot s23\cdot c5)\cdot L5+c1\cdot (s23\cdot L4+s23\cdot L3+s2\cdot L2)\\ (s1\cdot c23\cdot c4\cdot c5+c1\cdot s4\cdot c5-s1\cdot s23\cdot s5)\cdot c6+(c1\cdot c4-s1\cdot c23\cdot s4)\cdot s6 & -(s1\cdot c23\cdot c4\cdot c5+c1\cdot s4\cdot c5-s1\cdot s23\cdot s5)\cdot s6+(c1\cdot c4-s1\cdot c23\cdot s4)\cdot c6 & s1\cdot c23\cdot c4\cdot s5+c1\cdot s4\cdot s5+s1\cdot s23\cdot c5 & (s1\cdot c23\cdot c4\cdot s5+c1\cdot s4\cdot s5+s1\cdot s23\cdot c5)\cdot L5+s1\cdot (s23\cdot L4+s23\cdot L3+s2\cdot L2)\\ (-s23\cdot c4\cdot c5-c23\cdot s5)\cdot c6+(s23\cdot s4)\cdot s6 & (s23\cdot c4\cdot c5+c23\cdot s5)\cdot s6+(s23\cdot s4)\cdot c6 & -s23\cdot c4\cdot s5+c23\cdot c5 & (-s23\cdot c4\cdot s5+c23\cdot c5)\cdot L5+c23\cdot L4+c23\cdot L3+c2\cdot L2+L1+L0\\ 0 & 0 & 0 & 1 \end{bmatrix}$
Exemple concret
Hypothèse prise en compte :
Modélisation cinématique inverse (MCI) Maintenant que nous sommes capables de calculer la position X, Y, Z du point O7 en fonction des angles $\theta_1$, $\theta_2$, $\theta_3$, $\theta_4$, $\theta_5$, $\theta_6$,
(position angulaire des servomoteurs), nous allons maintenant rechercher quelles valeurs d’angles sont nécessaires pour atteindre une position X7, Y7, Z7 donnée.
L'hypothèse est tout d'abord de se mettre dans un plan, dans mon cas, XZ, dans lequel je n'ai plus que 3 rotations, on ignore pour l'instant $\theta_1$, $\theta_4$ et $\theta_6$.

Après première simpification reprenons l'équation de T1,7 nous donne:
$x_{1,7}=((c2\cdot c3-s2\cdot s3)\cdot c5-(c2\cdot s3+s2\cdot c3)\cdot s5)\cdot X_6+(c2\cdot c3-s2\cdot s3)\cdot X_4+c2\cdot X_2+X_1\\ z_{1,7}=((-s2\cdot c3-c2\cdot s3)\cdot c5+(s2\cdot s3-c2\cdot c3)\cdot s5)\cdot X_6-(s2\cdot c3+c2\cdot s3)\cdot X_4-s2\cdot X_2+Z_1$
Equivalent à :
$x_{1,7}=(\cos(\theta_2+\theta_3) \cos \theta_5-\sin(\theta_2+\theta_3) \sin \theta_5)X_6+\cos(\theta_2+\theta_3)X_4+\cos(\theta_2)\cdot X_2+X_1\\ z_{1,7}=(-\sin(\theta_2+\theta_3)\cos \theta_5-\cos(\theta_2+\theta_3)\sin \theta_5)X_6-sin(\theta_2+\theta_3)X_4-sin(\theta_2)\cdot X_2+Z_1\\$
ou
$x_{1,7}=\cos(\theta_2+\theta_3+\theta_5)X_6+\cos(\theta_2+\theta_3)X_4+\cos(\theta_2)\cdot X_2+X_1\\ z_{1,7}=-\sin(\theta_2+\theta_3+\theta_5)X_6-\sin(\theta_2+\theta_3)X_4-sin(\theta_2)\cdot X_2+Z_1$
Rappel d'opérations Matrices et Rotations

Rappel sur les matrices de rotations

Si les angles $\alpha$, $\beta$, et $\gamma$ sont respectivements les angles autour de $\vec x$,$\vec y$ et $\vec z$ les matrices de rotation sont les suivantes:
$\begin{array}{c c c} Rot_X(\alpha)=\begin{pmatrix} 1 & 0 & 0 \\ 0 & cos \alpha & -sin \alpha \\ 0 & sin \alpha & cos \alpha \end{pmatrix} & Rot_Y(\beta)=\begin{pmatrix} cos \beta & 0 & sin \beta \\ 0 & 1 & 0 \\ -sin \beta & 0 & cos \beta \end{pmatrix} & Rot_Z(\gamma)=\begin{pmatrix} cos \gamma & -sin \gamma & 0 \\ sin \gamma & cos \gamma & 0 \\ 0 & 0 & 1 \end{pmatrix} \\ \end{array}$
représente une rotation dont le lacet, le tangage et le roulis (également appelé angles de Cardan) ou en aéronautique (z:)Azimuth=lacet=yaw - $\alpha$ ; (y:)Tanguage=pitch - $\gamma$; (x:)Roulis=roll - $\beta$


Produit de matrice

$\forall i,j:c_{{ij}}=\sum_{{k=1}}^{n}a_{{ik}}b_{{kj}}=a_{{i1}}b_{{1j}}+a_{{i2}}b_{{2j}}+\cdots +a_{{in}}b_{{nj}}$
Soit le produit de deux matrices $A=(a_{ij})$ et $B=(b_{ij})$ de type $(m,n)$ noté $A\cdot B=(c_{ij})$, est une matrice de type $(m,n)$ donnée par $c_{ij}=a_{ij}\times b_{ij}$
Exemple :
$A_{ij}\cdot B_{ij} =C_{ij}$
$\begin{pmatrix} 1 & 3 & 2 & 4\\ 1 & 0 & 0 & 2\\ 1 & 2 & 2 & 3\\ 4 & 3 & 5 & 1 \end{pmatrix} \times \begin{pmatrix} 5 & 0 & 0 & 2\\ 7 & 5 & 0 & 8 \\ 2 & 1 & 1 & 1 \\2 & 1 & 1 & 0 \end{pmatrix}=$
$=\tiny\begin{pmatrix} (1\times 5+3\times 7+2\times 2+4\times 2) & (1\times 0+3\times 5+2\times 1+4\times 1) & (1\times 0+3\times 0+2\times 1+4\times 1) & (1\times 2+3\times 8+2\times 1+4\times 0) \\ (1\times 5+0\times 7+0\times 2+2\times 2) & (1\times 0+0\times 5+0\times 1+2\times 1) & (1\times 0+0\times 0+0\times 1+2\times 1) & (1\times 2+0\times 8+0\times 1+2\times 0) \\ (1\times 5+2\times 7+2\times 2+3\times 2) & (1\times 0+2\times 5+2\times 1+3\times 1) & (1\times 0+2\times 0+2\times 1+3\times 1) & (1\times 2+2\times 8+2\times 1+3\times 1)\\ (4\times 5+3\times 7+5\times 2+1\times 2) & (4\times 0+3\times 5+5\times 1+1\times 1) & (4\times 0+3\times 0+5\times 1+1\times 1) & (4\times 2+3\times 8+5\times 1+1\times 0) \end{pmatrix}=$
$=\begin{pmatrix} 38 & 21 & 6 & 28 \\ 9 & 2 & 2 & 2 \\ 29 & 15 & 5 & 20 \\ 53 & 21 & 6 & 37 \end{pmatrix}$

Transposé de matrice

C'est une opération simple, ou l'on inverse les lignes et les colonnes
$A=\begin{pmatrix} 38 & 21 & 6 & 28 \\ 9 & 2 & 2 & 2 \\ 29 & 15 & 5 & 20 \\ 53 & 21 & 6 & 37\\ \end{pmatrix}$
$A^T=\begin{pmatrix} 38 & 9 & 29 & 53 \\ 21 & 2 & 15 & 21 \\ 6 & 2 & 5 & 6 \\ 28 & 2 & 20 & 37 \\ \end{pmatrix}$
Rappel sur la trigonométrie

Rappel sur les propriétés remarquables

$\sin^2t + \cos^2t = 1$
$\sin(-A)=-\sin(A)$
$\cos(-A)=\cos(A)$
$\sin (A + B) = \sin A \cos B + \cos A \sin B$
$\sin (A – B) = \sin A \cos B – \cos A \sin B$
$\cos (A + B) = \cos A \cos B – \sin A \sin B$
$\cos (A – B) = \cos A \cos B + \sin A \sin B$
$\sin 2A = 2\cdot \sin A \cos A$
$\cos 2A = \cos^2 A – \sin^2 A$

Rappel sur les dérivés trigonométrie

$\cos(A)'=-\sin(A)$
$\sin(A)'=cos(A)$
$\cos(u)'=-u'\cdot \sin(u)$
$\sin(u)'=u'\cdot \cos(u)$
Rappel sur la jacobienne La matrice jacobienne est la matrice des dérivées partielles du premier ordre d'une fonction vectorielle.
Soit une fonction définie :
$F\begin{pmatrix} x_1\\ \dots\\ x_n \end{pmatrix} \mapsto F\begin{pmatrix} f_1(x_1,\dots,x_n)\\ \dots\\ f_m(x_1,\dots,x_n) \end{pmatrix} $
La dérivée partielles de ces fonctions $F$ en un point P donne:
$J_F(P)=\begin{pmatrix} \frac{\partial f_1}{\partial x_1} & \dots & \frac{\partial f_1}{\partial x_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial f_m}{\partial x_1} & \dots & \frac{\partial f_m}{\partial x_n} \\ \end{pmatrix}$
Exemple $F(x_1,x_2,x_3)=(x_1,5x_3,4x_2^2-2x_3,x_3\sin x_1)$
La matrice jacobienne de la fonction F:
$J_F(x_1,x_2,x_3)=\begin{pmatrix} 1 & 0 & 0 \\ 0 & 0 & 5 \\ 0 & 8x_2 & -2 \\ x_3\cos x_1 & 0 & \sin x_1 \end{pmatrix}$