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

θ_{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.

\vec{O_{0} O_{1}} = Z_{0} \vec{z}

;

\vec{O_{1} O_{2}} = X_{1} {\vec{x}}_{1} + Z_{1} {\vec{z}}_{1}

;

\vec{O_{2} O_{3}} = X_{2} {\vec{x}}_{2}

;

\vec{O_{3} O_{4}} = X_{3} {\vec{x}}_{3}

;

\vec{O_{4} O_{5}} = X_{4} {\vec{x}}_{4}

\vec{O_{5} O_{6}} = X_{5} {\vec{x}}_{5}

Où les vecteurs

\vec{x_{2}}

\vec{x_{3}}

\vec{x_{4}}

sont les vecteurs unitaires respectivement colinéaires aux vecteurs

\vec{O_{2} O_{3}}

\vec{O_{3} O_{4}}

\vec{O_{4} O_{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{matrix} A & P \\ [0, 0, 0] & 1 \end{matrix}]

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{matrix} A^{T} & - A^{T} . P \\ [0, 0, 0] & 1 \end{matrix}]

où AT est la transposée de A.

Notation robotique

Soit le solide

i

dans le repère

j

, en robotique avec :

$^{j} R_{i}$ la matrice de rotation
$^{j} P_{i}$ Le vecteur de position

^{j} T_{i} = [\begin{matrix} ^{j} R_{i} & ^{j} P_{i} \\ 0 & 1 \end{matrix}]

La notation donne

^{j} T_{i} = [\begin{matrix} 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{matrix}]

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

^{A} T_{B}

^{B} T_{C}

on peut calculer

^{A} T_{C} =^{A} T_{B} \times^{B} T_{C}

Matrice de transformation de base

Transformation d'une translation

T (x, y, z) = [\begin{matrix} 1 & 0 & 0 & x \\ 0 & 1 & 0 & y \\ 0 & 0 & 1 & z \\ 0 & 0 & 0 & 1 \end{matrix}]

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

T 0, 1 = [\begin{matrix} x \\ R_{w} (q) & y \\ z \\ 0 & 0 & 0 & 1 \end{matrix}]

Matrice de transformation globale

^{0} T_{7} =^{0} T_{1} \cdot^{1} T_{2} \cdot^{2} T_{3} \cdot^{3} T_{4} \cdot^{4} T_{5} \cdot^{5} T_{6} \cdot^{6} T_{7}

Modélisation cinématique inverse (MCI)

Soit la position de l'élément finale connue nommée

U_{0}

avec

U_{0} = [\begin{matrix} 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{matrix}]

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} =^{0} T_{1} \cdot^{1} T_{2} \cdot^{2} T_{3} \cdot^{3} T_{4} \cdot^{4} T_{5} \cdot^{5} T_{6} \cdot^{6} T_{7}

1ère itération à résoudre

^{1} T_{0} \cdot U_{0} =^{1} T_{0} \cdot^{0} T_{1} \cdot^{1} T_{2} \cdot^{2} T_{3} \cdot^{3} T_{4} \cdot^{4} T_{5} \cdot^{5} T_{6} \cdot^{6} T_{7}^{1} T_{0} \cdot U_{0} =^{1} T_{2} \cdot^{2} T_{3} \cdot^{3} T_{4} \cdot^{4} T_{5} \cdot^{5} T_{6} \cdot^{6} T_{7}

2ème itération à résoudre

^{2} T_{1} \cdot^{1} T_{0} \cdot U_{0} =^{2} T_{1} \cdot^{1} T_{2} \cdot^{2} T_{3} \cdot^{3} T_{4} \cdot^{4} T_{5} \cdot^{5} T_{6} \cdot^{6} T_{7}^{2} T_{1} \cdot^{1} T_{0} \cdot U_{0} =^{2} T_{3} \cdot^{3} T_{4} \cdot^{4} T_{5} \cdot^{5} T_{6} \cdot^{6} T_{7}

3ème itération à résoudre

^{3} T_{2} \cdot^{2} T_{1} \cdot^{1} T_{0} \cdot U_{0} =^{3} T_{4} \cdot^{4} T_{5} \cdot^{5} T_{6} \cdot^{6} T_{7}

4ème itération à résoudre

^{4} T_{3} \cdot^{3} T_{2} \cdot^{2} T_{1} \cdot^{1} T_{0} \cdot U_{0} =^{4} T_{5} \cdot^{5} T_{6} \cdot^{6} T_{7}

5ème itération à résoudre

^{5} T_{4} \cdot^{4} T_{3} \cdot^{3} T_{2} \cdot^{2} T_{1} \cdot^{1} T_{0} \cdot U_{0} =^{5} T_{6} \cdot^{6} T_{7}

6ème itération à résoudre

^{6} T_{5} \cdot^{5} T_{4} \cdot^{4} T_{3} \cdot^{3} T_{2} \cdot^{2} T_{1} \cdot^{1} T_{0} \cdot U_{0} =^{6} T_{7}

Transposé de la matrice

^{i - 1} T_{i} = [\begin{matrix} ^{i - 1} R_{i} & ^{i - 1} \vec{O_{i - 1} O_{i}} \\ 0 & 0 & 0 & 1 \end{matrix}]

Alor

^{i} T_{i - 1} = [\begin{matrix} ^{i - 1} R_{i}^{T} & -^{i - 1} R_{i}^{T} \cdot^{i - 1} \vec{O_{i - 1} O_{i}} \\ 0 & 0 & 0 & 1 \end{matrix}]

Exemple de transposé

^{0} T_{1} = [\begin{matrix} \cos θ_{1} & - \sin θ_{1} & 0 & 0 \\ \sin θ_{1} & \cos θ_{1} & 0 & 0 \\ 0 & 0 & 1 & Z_{0} \\ 0 & 0 & 0 & 1 \end{matrix}]

La matrice de rotation est donc

^{0} R_{1} = [\begin{matrix} \cos θ_{1} & - \sin θ_{1} & 0 \\ \sin θ_{1} & \cos θ_{1} & 0 \\ 0 & 0 & 1 \end{matrix}]

La matrice de transposée devient

^{1} R_{0} = [\begin{matrix} \cos θ_{1} & \sin θ_{1} & 0 \\ - \sin θ_{1} & \cos θ_{1} & 0 \\ 0 & 0 & 1 \end{matrix}]

Le vecteur :

^{0} \vec{O_{i - 1} O_{i}} = [\begin{matrix} 0 \\ 0 \\ Z 0 \end{matrix}]

Calcul :

-^{1} R_{0} = [\begin{matrix} \cos θ_{1} & \sin θ_{1} & 0 \\ - \sin θ_{1} & \cos θ_{1} & 0 \\ 0 & 0 & 1 \end{matrix}] [\begin{matrix} 0 \\ 0 \\ - Z 0 \end{matrix}]

Matrice complétée

^{1} T_{0} = [\begin{matrix} \cos θ_{1} & \sin θ_{1} & 0 & 0 \\ - \sin θ_{1} & \cos θ_{1} & 0 & 0 \\ 0 & 0 & 1 & - Z_{0} \\ 0 & 0 & 0 & 1 \end{matrix}]

Concrètement

En fonction de notre schéma cinématique, les équations sont les suivantes:

Pivot en $\vec{Z} 1$ de centre $(0, 0, Z_{0})$ de 0/1 dans le repère $R_{0}$ $T 0, 1 = [\begin{matrix} \cos θ_{1} & - \sin θ_{1} & 0 & 0 \\ \sin θ_{1} & \cos θ_{1} & 0 & 0 \\ 0 & 0 & 1 & Z_{0} \\ 0 & 0 & 0 & 1 \end{matrix}]$	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{matrix} \cos θ_{2} & 0 & \sin θ_{2} & X_{1} \\ 0 & 1 & 0 & 0 \\ - \sin θ_{2} & 0 & \cos θ_{2} & Z_{1} \\ 0 & 0 & 0 & 1 \end{matrix}]$	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{matrix} \cos θ_{3} & 0 & \sin θ_{3} & X_{2} \\ 0 & 1 & 0 & 0 \\ - \sin θ_{3} & 0 & \cos θ_{3} & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]$
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{matrix} 1 & 0 & 0 & X_{3} \\ 0 & \cos θ_{4} & - \sin θ_{4} & 0 \\ 0 & \sin θ_{4} & \cos θ_{4} & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]$	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{matrix} \cos θ_{5} & 0 & \sin θ_{5} & X_{4} \\ 0 & 1 & 0 & 0 \\ - \sin θ_{5} & 0 & \cos θ_{5} & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]$	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{matrix} 1 & 0 & 0 & X_{5} \\ 0 & \cos θ_{6} & - \sin θ_{6} & 0 \\ 0 & \sin θ_{6} & \cos θ_{6} & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]$
Juste pour placer le point $O_{7} (X_{6}, 0, 0)$ de 6/7 dans le repère $R_{6}$ $T_{6, 7} = [\begin{matrix} 1 & 0 & 0 & X_{6} \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]$

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 :

L'un, appelé problème de position, est fonction de $q_{1}, q_{2}, q_{3}$ , il permet de déterminer les paramètres $q_{1}, q_{2}, q_{3}$ .
L'autre, appelé problème d'orientation, est fonction de $q_{4}, q_{5}, q_{6}$ . , il permet de déterminer les paramètres $q_{4}, q_{5}, q_{6}$ .

Equation de position

L'équation de positon est liée au porteur. Puisque

O_{4} = O_{5} = O_{6} = O_{7}

on a :

[\begin{matrix} P_{x} \\ P_{y} \\ P_{z} \\ 1 \end{matrix}] =^{0} T_{1} \times^{1} T_{2} \times^{2} T_{3} \times^{3} T_{4} \times [\begin{matrix} 0 \\ 0 \\ 0 \\ 1 \end{matrix}]

Première étape

Pour se simplifier les notations, nous prendrons $c 1$ pour $\cos q_{1}$ , $s 1$ pour $\sin s_{1}$ , $c 23$ pour $\cos (q_{2} + q_{3})$ , ...

^{1} T_{0} \times [\begin{matrix} P_{x} \\ P_{y} \\ P_{z} \\ 1 \end{matrix}] =^{1} T_{4} \times [\begin{matrix} 0 \\ 0 \\ 0 \\ 1 \end{matrix}]

[\begin{matrix} c 1 \cdot P_{x} + s 1 \cdot P_{y} \\ - s 1 \cdot P_{x} + c 1 \cdot P_{y} \\ P_{z} - L 0 \\ 1 \end{matrix}] = [\begin{matrix} (c 2 \cdot s 3 + s 2 \cdot c 3) \cdot L 3 + s 2 \cdot L 2 \\ 0 \\ (- s 2 \cdot s 3 + c 2 \cdot c 3) \cdot L 3 + c 2 \cdot L 2 + L 1 \\ 1 \end{matrix}]

Avec les règles de trigo,

c 2 \cdot s 3 + s 2 \cdot c 3 = s 23

Cette fois, ce n'est pas utile, mais nous pouvons simplifier.

[\begin{matrix} (1) c 1 \cdot P_{x} + s 1 \cdot P_{y} \\ (2) - s 1 \cdot P_{x} + c 1 \cdot P_{y} \\ (3) P_{z} - L 0 \\ 1 \end{matrix}] = [\begin{matrix} s 23 \cdot L 3 + s 2 \cdot L 2 \\ 0 \\ c 23 \cdot L 3 + c 2 \cdot L 2 + L 1 \\ 1 \end{matrix}]

L'équation (2) nous permet de resoudre la valeur de

q_{1}

- s 1 \cdot P_{x} + c 1 \cdot P_{y} = 0

→

\frac{s 1}{c 1} = \frac{P_{y}}{P_{x}} =

→

q_{1} = a t a n (P_{x} / P_{y})

Deuxième étape

^{2} T_{0} \times [\begin{matrix} P_{x} \\ P_{y} \\ P_{z} \\ 1 \end{matrix}] =^{2} T_{4} \times [\begin{matrix} 0 \\ 0 \\ 0 \\ 1 \end{matrix}]

[\begin{matrix} (4) (c 2 \cdot c 1) \cdot P_{x} + (c 2 \cdot s 1) \cdot P_{y} - s 2 \cdot P_{z} + s 2 \cdot L 0 \\ (5) - s 1 \cdot P_{x} + c 1 \cdot P_{y} \\ (6) (s 2 \cdot c 1) \cdot P_{x} + (s 2 \cdot s 1) \cdot P_{y} + c 2 \cdot P_{z} - c 2 \cdot L 0 \\ 0 \end{matrix}] = [\begin{matrix} s 3 \cdot L 3 \\ 0 \\ c 3 \cdot L 3 + L 2 \\ 1 \end{matrix}]

(4) : c 2 (c 1 \cdot P_{x} + s 1 \cdot P_{y}) - s 2 \cdot P_{z} + s 2 \cdot L 0 = s 3 \cdot L 3

(6) : s 2 (c 1 \cdot P_{x} + s 1 \cdot P_{y}) + c 2 \cdot P_{z} - c 2 \cdot L 0 - L 2 = c 3 \cdot L 3

Nous prendrons $b 1 = c 1 \cdot P_{x} + s 1 \cdot P_{y}$

(4) : c 2 \cdot b 1 - s 2 \cdot P_{z} + s 2 \cdot L 0 = s 3 \cdot L 3

(6) : s 2 \cdot b 1 + c 2 \cdot P_{z} - c 2 \cdot L 0 - L 2 = c 3 \cdot L 3

En sommant ces 2 équations, préalablement mises au carré, on obtient l'équation suivante :

(4^{'} + 6^{'})^{2} : - 2 \cdot s 2 \cdot b 1 \cdot L 2 + 2 \cdot c 2 \cdot L 2 \cdot (L 0 - P_{z}) = L 3^{2} - L 2^{2} - b 1^{2} - (P_{z} - L 0)^{2}

soit l'expression:

X \cdot s 2 + Y \cdot c 2 = Z

X = - 2 \cdot b 1 \cdot L 2

Y = 2 \cdot L 2 \cdot (L 0 - P_{z})

Z = L 3^{2} - L 2^{2} - b 1^{2} - (P_{z} - L 0)^{2}

{\begin{cases} \sin (q_{2}) = \frac{X Z + ϵ Y \sqrt{X^{2} + Y^{2} - Z^{2}}}{X^{2} + Y^{2}} \\ a v e c ϵ = \pm 1 \\ c o s (q_{2}) = \frac{Y Z - ϵ X \sqrt{X^{2} + Y^{2} - Z^{2}}}{X^{2} + Y^{2}} \end{cases}}

Nous trouvons la valeur de

q_{2} = a t a n (\frac{X Z + ϵ Y \sqrt{X^{2} + Y^{2} - Z^{2}}}{Y Z - ϵ X \sqrt{X^{2} + Y^{2} - Z^{2}}})

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):

q_{3} = a t a n \frac{c 2 \cdot b 1 + s 2 (L 0 - P_{z})}{s 2 \cdot b 1 + c 2 (P_{z} - L 0 - L 2)}

Equation d'orientation

Il faut commencer par orienter l'effecteur, soit la matrice de rotation SNA, exemple du vecteur

\vec{A}

S N A = [\begin{matrix} S_{x} & N_{x} & A_{x} \\ S_{y} & N_{y} & A_{y} \\ S_{z} & N_{z} & A_{z} \end{matrix}]

Et positionner le point d'arrivée.

P (x, y, z)

ce qui donne la matrice

^{0} T_{7} = [\begin{matrix} 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{matrix}]

Application numérique

Evaluation de O₇

Nous pouvons maintenant évaluer la position (X, Y, Z) du point O₅ 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{matrix} \cos θ_{1} & - \sin θ_{1} & 0 & X_{0} \\ \sin θ_{1} & \cos θ_{1} & 0 & Y_{0} \\ 0 & 0 & 1 & Z_{0} \\ 0 & 0 & 0 & 1 \end{matrix}] \times [\begin{matrix} \cos θ_{2} & 0 & \sin θ_{2} & X_{1} \\ 0 & 1 & 0 & 0 \\ - \sin θ_{2} & 0 & \cos θ_{2} & Z_{1} \\ 0 & 0 & 0 & 1 \end{matrix}]

Appelons

\cos θ_{1} \to c_{1}, s i n θ_{1} \to s_{1}, \dots

T_{0, 2} = [\begin{matrix} 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{matrix}]

T_{0, 3} = T_{0, 2} \cdot T_{2, 3} = [\begin{matrix} 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{matrix}] \times [\begin{matrix} c_{3} & 0 & s_{3} & X_{2} \\ 0 & 1 & 0 & 0 \\ - s_{3} & 0 & c_{3} & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] = [\begin{matrix} c 1 \cdot c 2 \cdot c 3 - c 1 \cdot s 2 \cdot s 3 & - s 1 & c 1 \cdot c 2 \cdot s 3 + c 1 \cdot s 2 \cdot c 3 & c 1 \cdot c 2 \cdot X 2 + c 1 \cdot X 1 + X 0 \\ s 1 \cdot c 2 \cdot c 3 - s 1 \cdot s 2 \cdot s 3 & c 1 & s 1 \cdot c 2 \cdot s 3 + s 1 \cdot s 2 \cdot c 3 & s 1 \cdot c 2 \cdot X 2 + s 1 \cdot X 1 + Y 0 \\ - s 2 \cdot c 3 - c 2 \cdot s 3 & 0 & - s 2 \cdot s 3 + c 2 \cdot c 3 & - s 2 \cdot X 2 + Z 1 + Z 0 \\ 0 & 0 & 0 & 1 \end{matrix}]

Simplification ... c23=

\cos (θ_{2} + θ_{3})

T_{0, 3} = [\begin{matrix} c 1 \cdot c 23 & - s 1 & c 1 \cdot s 23 & c 1 \cdot s 2 \cdot L 2 \\ s 1 \cdot c 23 & c 1 & s 1 \cdot s 23 & s 1 \cdot s 2 \cdot L 2 \\ - s 23 & 0 & c 23 & c 2 \cdot L 2 + L 1 + L 0 \\ 0 & 0 & 0 & 1 \end{matrix}]

T_{0, 4} = T_{0, 3} \cdot T_{3, 4} = T_{0, 3} \times [\begin{matrix} 1 & 0 & 0 & X_{3} \\ 0 & c_{4} & - s_{4} & 0 \\ 0 & s_{4} & c_{4} & 0 \\ 0 & 0 & 0 & 1 \end{matrix}]

T_{0, 4} = [\begin{matrix} c 1 \cdot c 23 \cdot c 4 - s 1 \cdot s 4 & - c 1 \cdot c 23 \cdot s 4 - s 1 \cdot c 4 & c 1 \cdot s 23 & c 1 \cdot s 23 \cdot L 3 + c 1 \cdot s 2 \cdot L 2 \\ s 1 \cdot c 23 \cdot c 4 + c 1 \cdot s 4 & - s 1 \cdot c 23 \cdot s 4 + c 1 \cdot c 4 & s 1 \cdot s 23 & s 1 \cdot s 23 \cdot L 3 + s 1 \cdot s 2 \cdot L 2 \\ - s 23 \cdot c 4 & s 23 \cdot s 4 & c 23 & c 23 \cdot L 3 + c 2 \cdot L 2 + L 1 + L 0 \\ 0 & 0 & 0 & 1 \end{matrix}]

T_{0, 5} = [\begin{matrix} c 1 \cdot c 23 \cdot c 4 \cdot c 5 - s 1 \cdot s 4 \cdot c 5 - c 1 \cdot s 23 \cdot s 5 & - c 1 \cdot c 23 \cdot s 4 - s 1 \cdot c 4 & c 1 \cdot c 23 \cdot c 4 \cdot s 5 - s 1 \cdot s 4 \cdot s 5 + c 1 \cdot s 23 \cdot c 5 & c 1 \cdot (s 23 \cdot L 4 + s 23 \cdot L 3 + s 2 \cdot L 2) \\ s 1 \cdot c 23 \cdot c 4 \cdot c 5 + c 1 \cdot s 4 \cdot c 5 - s 1 \cdot s 23 \cdot s 5 & - s 1 \cdot c 23 \cdot s 4 + c 1 \cdot c 4 & s 1 \cdot c 23 \cdot c 4 \cdot s 5 + c 1 \cdot s 4 \cdot s 5 + s 1 \cdot s 23 \cdot c 5 & s 1 \cdot (s 23 \cdot L 4 + s 23 \cdot L 3 + s 2 \cdot L 2) \\ - s 23 \cdot c 4 \cdot c 5 - c 23 \cdot s 5 & s 23 \cdot s 4 & - s 23 \cdot c 4 \cdot s 5 + c 23 \cdot c 5 & c 23 \cdot L 4 + c 23 \cdot L 3 + c 2 \cdot L 2 + L 1 + L 0 \\ 0 & 0 & 0 & 1 \end{matrix}]

T_{0, 6} = [\begin{matrix} (c 1 \cdot c 23 \cdot c 4 \cdot c 5 - s 1 \cdot s 4 \cdot c 5 - c 1 \cdot s 23 \cdot s 5) \cdot c 6 + (- c 1 \cdot c 23 \cdot s 4 - s 1 \cdot c 4) \cdot s 6 & - (c 1 \cdot c 23 \cdot c 4 \cdot c 5 - s 1 \cdot s 4 \cdot c 5 - c 1 \cdot s 23 \cdot s 5) \cdot s 6 - (c 1 \cdot c 23 \cdot s 4 s 1 \cdot c 4) \cdot c 6 & c 1 \cdot c 23 \cdot c 4 \cdot s 5 - s 1 \cdot s 4 \cdot s 5 + c 1 \cdot s 23 \cdot c 5 & (c 1 \cdot c 23 \cdot c 4 \cdot s 5 - s 1 \cdot s 4 \cdot s 5 + c 1 \cdot s 23 \cdot c 5) \cdot L 5 + c 1 \cdot (s 23 \cdot L 4 + s 23 \cdot L 3 + s 2 \cdot L 2) \\ (s 1 \cdot c 23 \cdot c 4 \cdot c 5 + c 1 \cdot s 4 \cdot c 5 - s 1 \cdot s 23 \cdot s 5) \cdot c 6 + (c 1 \cdot c 4 - s 1 \cdot c 23 \cdot s 4) \cdot s 6 & - (s 1 \cdot c 23 \cdot c 4 \cdot c 5 + c 1 \cdot s 4 \cdot c 5 - s 1 \cdot s 23 \cdot s 5) \cdot s 6 + (c 1 \cdot c 4 - s 1 \cdot c 23 \cdot s 4) \cdot c 6 & s 1 \cdot c 23 \cdot c 4 \cdot s 5 + c 1 \cdot s 4 \cdot s 5 + s 1 \cdot s 23 \cdot c 5 & (s 1 \cdot c 23 \cdot c 4 \cdot s 5 + c 1 \cdot s 4 \cdot s 5 + s 1 \cdot s 23 \cdot c 5) \cdot L 5 + s 1 \cdot (s 23 \cdot L 4 + s 23 \cdot L 3 + s 2 \cdot L 2) \\ (- s 23 \cdot c 4 \cdot c 5 - c 23 \cdot s 5) \cdot c 6 + (s 23 \cdot s 4) \cdot s 6 & (s 23 \cdot c 4 \cdot c 5 + c 23 \cdot s 5) \cdot s 6 + (s 23 \cdot s 4) \cdot c 6 & - s 23 \cdot c 4 \cdot s 5 + c 23 \cdot c 5 & (- s 23 \cdot c 4 \cdot s 5 + c 23 \cdot c 5) \cdot L 5 + c 23 \cdot L 4 + c 23 \cdot L 3 + c 2 \cdot L 2 + L 1 + L 0 \\ 0 & 0 & 0 & 1 \end{matrix}]

T_{0, 7} = [\begin{matrix} (c 1 \cdot c 23 \cdot c 4 \cdot c 5 - s 1 \cdot s 4 \cdot c 5 - c 1 \cdot s 23 \cdot s 5) \cdot c 6 + (- c 1 \cdot c 23 \cdot s 4 - s 1 \cdot c 4) \cdot s 6 & - (c 1 \cdot c 23 \cdot c 4 \cdot c 5 - s 1 \cdot s 4 \cdot c 5 - c 1 \cdot s 23 \cdot s 5) \cdot s 6 - (c 1 \cdot c 23 \cdot s 4 s 1 \cdot c 4) \cdot c 6 & c 1 \cdot c 23 \cdot c 4 \cdot s 5 - s 1 \cdot s 4 \cdot s 5 + c 1 \cdot s 23 \cdot c 5 & (c 1 \cdot c 23 \cdot c 4 \cdot s 5 - s 1 \cdot s 4 \cdot s 5 + c 1 \cdot s 23 \cdot c 5) \cdot L 5 + c 1 \cdot (s 23 \cdot L 4 + s 23 \cdot L 3 + s 2 \cdot L 2) \\ (s 1 \cdot c 23 \cdot c 4 \cdot c 5 + c 1 \cdot s 4 \cdot c 5 - s 1 \cdot s 23 \cdot s 5) \cdot c 6 + (c 1 \cdot c 4 - s 1 \cdot c 23 \cdot s 4) \cdot s 6 & - (s 1 \cdot c 23 \cdot c 4 \cdot c 5 + c 1 \cdot s 4 \cdot c 5 - s 1 \cdot s 23 \cdot s 5) \cdot s 6 + (c 1 \cdot c 4 - s 1 \cdot c 23 \cdot s 4) \cdot c 6 & s 1 \cdot c 23 \cdot c 4 \cdot s 5 + c 1 \cdot s 4 \cdot s 5 + s 1 \cdot s 23 \cdot c 5 & (s 1 \cdot c 23 \cdot c 4 \cdot s 5 + c 1 \cdot s 4 \cdot s 5 + s 1 \cdot s 23 \cdot c 5) \cdot L 5 + s 1 \cdot (s 23 \cdot L 4 + s 23 \cdot L 3 + s 2 \cdot L 2) \\ (- s 23 \cdot c 4 \cdot c 5 - c 23 \cdot s 5) \cdot c 6 + (s 23 \cdot s 4) \cdot s 6 & (s 23 \cdot c 4 \cdot c 5 + c 23 \cdot s 5) \cdot s 6 + (s 23 \cdot s 4) \cdot c 6 & - s 23 \cdot c 4 \cdot s 5 + c 23 \cdot c 5 & (- s 23 \cdot c 4 \cdot s 5 + c 23 \cdot c 5) \cdot L 5 + c 23 \cdot L 4 + c 23 \cdot L 3 + c 2 \cdot L 2 + L 1 + L 0 \\ 0 & 0 & 0 & 1 \end{matrix}]

Exemple concret

Hypothèse prise en compte :

Hypothèse par liaison
Z₀=10	X₁=-30 Z₁=50 θ₁=6.12	X₂=200 θ₂=-63.00	X₃=100 θ₃=85.00	X₄=90 θ₄=22.00	X₅=65 θ₅=70.00	X₆=50 θ₆=41.00

Matrice T01: notée ⁰T₁
0.994	-0.107	0	0
0.107	0.994	0	0
0	0	1	10
0	0	0	1

Matrice T12: notée ¹T₂
0.454	0	-0.891	-30
0	1	0	0
0.891	0	0.454	50
0	0	0	1

Matrice T23: notée ²T₃
0.087	0	0.996	200
0	1	0	0
-0.996	0	0.087	0
0	0	0	1

Matrice T34: notée ³T₄
1	0	0	100
0	0.927	-0.375	0
0	0.375	0.927	0
0	0	0	1

Matrice T45: notée ⁴T₅
0.342	0	0.940	90
0	1	0	0
-0.940	0	0.342	0
0	0	0	1

Matrice T56: notée ⁵T₆
1	0	0	65
0	0.755	-0.656	0
0	0.656	0.755	0
0	0	0	1

Matrice T67: notée ⁶T₇
1	0	0	50
0	1	0	0
0	0	1	0
0	0	0	1

Matrice T01_: notée ⁰T₁
0.994	-0.107	0	0
0.107	0.994	0	0
0	0	1	10
0	0	0	1

Matrice T02: notée ⁰T₂
0.451	-0.107	-0.886	-29.829
0.048	0.994	-0.095	-3.198
0.891	0	0.454	60
0	0	0	1

Matrice T03: notée ⁰T₃
0.922	-0.107	0.372	60.452
0.099	0.994	0.040	6.482
-0.375	0	0.927	238.201
0	0	0	1

Matrice T04: notée ⁰T₄
0.922	0.041	0.385	152.642
0.099	0.937	-0.335	16.367
-0.375	0.347	0.860	200.741
0	0	0	1

Matrice T05: notée ⁰T₅
-0.047	0.041	0.998	235.613
0.349	0.937	-0.022	25.263
-0.936	0.347	-0.058	167.026
0	0	0	1

Matrice T06: notée ⁰T₆
-0.047	0.686	0.727	232.574
0.349	0.693	-0.631	47.949
-0.936	0.224	-0.272	106.189
0	0	0	1

Matrice T07: notée ⁰T₇
-0.047	0.686	0.727	230.237
0.349	0.693	-0.631	65.400
-0.936	0.224	-0.272	59.392
0	0	0	1

Coordonnées :
X : 230.237	Y : 65.400	Z : 59.392	Distance : 246.604

Modélisation cinématique inverse (MCI)

Maintenant que nous sommes capables de calculer la position X, Y, Z du point O₇ en fonction des angles

θ_{1}

θ_{2}

θ_{3}

θ_{4}

θ_{5}

θ_{6}

,
(position angulaire des servomoteurs), nous allons maintenant rechercher quelles valeurs d’angles sont nécessaires pour atteindre une position X₇, Y₇, Z₇ 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

θ_{1}

θ_{4}

θ_{6}

Après première simpification reprenons l'équation de T_1,7 nous donne:

x_{1, 7} = ((c 2 \cdot c 3 - s 2 \cdot s 3) \cdot c 5 - (c 2 \cdot s 3 + s 2 \cdot c 3) \cdot s 5) \cdot X_{6} + (c 2 \cdot c 3 - s 2 \cdot s 3) \cdot X_{4} + c 2 \cdot X_{2} + X_{1} z_{1, 7} = ((- s 2 \cdot c 3 - c 2 \cdot s 3) \cdot c 5 + (s 2 \cdot s 3 - c 2 \cdot c 3) \cdot s 5) \cdot X_{6} - (s 2 \cdot c 3 + c 2 \cdot s 3) \cdot X_{4} - s 2 \cdot X_{2} + Z_{1}

Equivalent à :

x_{1, 7} = (\cos (θ_{2} + θ_{3}) \cos θ_{5} - \sin (θ_{2} + θ_{3}) \sin θ_{5}) X_{6} + \cos (θ_{2} + θ_{3}) X_{4} + \cos (θ_{2}) \cdot X_{2} + X_{1} z_{1, 7} = (- \sin (θ_{2} + θ_{3}) \cos θ_{5} - \cos (θ_{2} + θ_{3}) \sin θ_{5}) X_{6} - s i n (θ_{2} + θ_{3}) X_{4} - s i n (θ_{2}) \cdot X_{2} + Z_{1}

x_{1, 7} = \cos (θ_{2} + θ_{3} + θ_{5}) X_{6} + \cos (θ_{2} + θ_{3}) X_{4} + \cos (θ_{2}) \cdot X_{2} + X_{1} z_{1, 7} = - \sin (θ_{2} + θ_{3} + θ_{5}) X_{6} - \sin (θ_{2} + θ_{3}) X_{4} - s i n (θ_{2}) \cdot X_{2} + Z_{1}

Rappel d'opérations Matrices et Rotations

Rappel sur les matrices de rotations

Si les angles

α

β

, et

γ

sont respectivements les angles autour de

\vec{x}

\vec{y}

\vec{z}

les matrices de rotation sont les suivantes:

\begin{array}{ccc} R o t_{X} (α) = (\begin{matrix} 1 & 0 & 0 \\ 0 & c o s α & - s i n α \\ 0 & s i n α & c o s α \end{matrix}) & R o t_{Y} (β) = (\begin{matrix} c o s β & 0 & s i n β \\ 0 & 1 & 0 \\ - s i n β & 0 & c o s β \end{matrix}) & R o t_{Z} (γ) = (\begin{matrix} c o s γ & - s i n γ & 0 \\ s i n γ & c o s γ & 0 \\ 0 & 0 & 1 \end{matrix}) \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 -

α

; (y:)Tanguage=pitch -

γ

; (x:)Roulis=roll -

β

Produit de matrice

\forall i, j : c_{i j} = \sum_{k = 1}^{n} a_{i k} b_{k j} = a_{i 1} b_{1 j} + a_{i 2} b_{2 j} + \dots + a_{i n} b_{n j}

Soit le produit de deux matrices

A = (a_{i j})

B = (b_{i j})

de type

(m, n)

noté

A \cdot B = (c_{i j})

, est une matrice de type

(m, n)

donnée par

c_{i j} = a_{i j} \times b_{i j}

Exemple :

A_{i j} \cdot B_{i j} = C_{i j}

(\begin{matrix} 1 & 3 & 2 & 4 \\ 1 & 0 & 0 & 2 \\ 1 & 2 & 2 & 3 \\ 4 & 3 & 5 & 1 \end{matrix}) \times (\begin{matrix} 5 & 0 & 0 & 2 \\ 7 & 5 & 0 & 8 \\ 2 & 1 & 1 & 1 \\ 2 & 1 & 1 & 0 \end{matrix}) =

= (\begin{matrix} (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{matrix}) =

= (\begin{matrix} 38 & 21 & 6 & 28 \\ 9 & 2 & 2 & 2 \\ 29 & 15 & 5 & 20 \\ 53 & 21 & 6 & 37 \end{matrix})

Transposé de matrice

C'est une opération simple, ou l'on inverse les lignes et les colonnes

A = (\begin{matrix} 38 & 21 & 6 & 28 \\ 9 & 2 & 2 & 2 \\ 29 & 15 & 5 & 20 \\ 53 & 21 & 6 & 37 \end{matrix})

A^{T} = (\begin{matrix} 38 & 9 & 29 & 53 \\ 21 & 2 & 15 & 21 \\ 6 & 2 & 5 & 6 \\ 28 & 2 & 20 & 37 \end{matrix})

Rappel sur la trigonométrie

Rappel sur les propriétés remarquables

\sin^{2} t + \cos^{2} t = 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 2 A = 2 \cdot \sin A \cos A

\cos 2 A = \cos^{2} A - \sin^{2} A

Rappel sur les dérivés trigonométrie

\cos (A)^{'} = - \sin (A)

\sin (A)^{'} = c o s (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{matrix} x_{1} \\ \dots \\ x_{n} \end{matrix}) \mapsto F (\begin{matrix} f_{1} (x_{1}, \dots, x_{n}) \\ \dots \\ f_{m} (x_{1}, \dots, x_{n}) \end{matrix})

La dérivée partielles de ces fonctions

F

en un point P donne:

J_{F} (P) = (\begin{matrix} \frac{\partial f_{1}}{\partial x_{1}} & \dots & \frac{\partial f_{1}}{\partial x_{n}} \\ ⋮ & ⋱ & ⋮ \\ \frac{\partial f_{m}}{\partial x_{1}} & \dots & \frac{\partial f_{m}}{\partial x_{n}} \end{matrix})

Exemple

F (x_{1}, x_{2}, x_{3}) = (x_{1}, 5 x_{3}, 4 x_{2}^{2} - 2 x_{3}, x_{3} \sin x_{1})

La matrice jacobienne de la fonction F:

J_{F} (x_{1}, x_{2}, x_{3}) = (\begin{matrix} 1 & 0 & 0 \\ 0 & 0 & 5 \\ 0 & 8 x_{2} & - 2 \\ x_{3} \cos x_{1} & 0 & \sin x_{1} \end{matrix})

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

Convention de Khalil-Kleinfinger.

Notation robotique

Matrice de transformation de base

Transformation d'une translation

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

Matrice de transformation globale

Transposé de la matrice

Exemple de transposé

Le vecteur :

Calcul :

Matrice complétée

Concrètement

Coeur de la méthode de Paul

Equation de position

Première étape

Deuxième étape

Résolution de q3

Equation d'orientation

Evaluation de O7

Rappel sur les matrices de rotations

Produit de matrice

Transposé de matrice

Rappel sur les propriétés remarquables

Rappel sur les dérivés trigonométrie

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

Résolution de $q_{3}$

Evaluation de O₇