1 5.9INVERSION DU MODELE CINEMATIQUE / GEOMETRIQUE 5.9INVERSION DU MODELE CINEMATIQUE / GEOMETRIQUE

5.9 INVERSION DU MODELE CINEMATIQUE / GEOMETRIQUE

• Nombre mde variables dans l’espace articulaire = nombre de maillons si joints de classe 5

• Nombre nde variables de la tâche = nombre de DDL de la tâche (position + orientation)

• Problème directe: calcul de la configuration correspondant à un jeu de paramètres articulaires donnés

* Ce problème a toujours une solution unique

• Problème inverse: quelles sont les variables articulaires qui réalisent une configuration donnée?

* Problème très non linéaire qui n’a pas toujours de solution ou de solution unique TASK SPACE

x^T = [x … x ]1 n

JOINT SPACE

Inverse Problem

Direct Problem

5.9 INVERSION DU MODELE CINEMATIQUE / GEOMETRIQUE

PROBLEME INVERSE: CONDITIONS D’EXISTENCE PROBLEME INVERSE: CONDITIONS D’EXISTENCE

• 1/ La configuration xest à l’intérieur de l’espace de travail – Physiquement atteignables

– Pas de limitation mécanique (butées articulaires)

– Espace de dextérité: tout point est attaquable avec n’importe quelle orientation – Espace de travail en position, certains points ne peuvent pas être atteints avec toutes les

orientations

• 2/ Nombre de degrés de liberté du système

– m=n:il existe un nombre fini de solutions pour atteindre le point. La multiplicitéde la solution dépend du nombre de ddls. Cette multiplicité peut poser un problème. On le résout souvent en prenant la solution la plus proche de la configuration courante (précédente).

– m>n: il y a une infinité de solutions. Lorsque l’environnement est très contraignant, ces restrictions mènent généralement à ne garder qu’une solution.

– m<n: en général, il n’y a pas de solution au problème. On doit ajouter des ddls au système mécanique. On peut également rechercher la solution qui se rapproche le plus de celle

CINEMATIQUE DU ROBOT PUMA 560

5.10.1 DECOUPLAGE DES POSITIONS ET ORIENTATIONS 5.10.1 DECOUPLAGE DES POSITIONS ET ORIENTATIONS

• Position du poignet dans les coordonnées globales

où d₆est la longueur de l’outil

• Nouveau vecteur position de l’organe terminal

• Cette quantité est la nouvelle translation à considérer dans la transformation globale p=

2

4 C1(S23d4+a2C2)¡S1d2

S1(S23d4+a2C2) +S1d2

C23d4¡a2S2

3 5 p=T a with a^T = [0 0 ¡d6]

T =

· R p 0 1

¸

5.10 INVERSION ANALYTIQUE DU MODELE CINEMATIQUE DU ROBOT PUMA 560

5.10.2 TECHNIQUE DE PIEPER 5.10.2 TECHNIQUE DE PIEPER

• On considère les équations successives

• Pour chaque équation i donnant

– le membre de gauche est une fonction des coordonnées de la tâchex(connues, et contenues dans les vecteurs n, o, aet p) et des degrés de liberté q₁, q₂, … q_i.

– le membre de droite est une fonction de q_i+1, q_i+2, … q_mou bien de constante ou de zéros.

• L’astuce: identifier les éléments de qui nous permettent de trouver une expression analytique de q_i.

• Procédure récursive: on détermine d’abord q₁, puis q₂connaissant q₁, etc

i+ 1A6 i+ 1A6

T =⁰A₁¢¹A₂¢²A₃¢³A₄¢⁴A₅¢⁵A₆=⁰A₆

0A^¡₁¹¢T = ¹A₂¢²A₃¢³A₄¢⁴A₅¢⁵A₆=¹A₆

1A^¡₂¹¢⁰A^¡₁¹¢T = ²A₃¢³A₄¢⁴A₅¢⁵A₆=²A₆ ...

4A^¡₅¹: : :¹A^¡₂¹¢⁰A^¡₁¹¢T = =⁵A₆

5.10 INVERSION ANALYTIQUE DU MODELE CINEMATIQUE DU ROBOT PUMA 560

LA FONCTION ATAN2(u,v)

• BUT: retrouver de manière systématique la valeur exacte de l’arc tangent de u/v

• EN PRATIQUE: utiliser la fonction ATAN2(U,V) en FORTRAN

• ATAN2(u,v): calcule la valeur de ARCTAN(u/v) et utilise les signes de u et de v pour déterminer le quadrant de la solution

µ=ATAN2(u;v)

µ=AT AN2 (u; v) = 8<

:

tan^{¡1 u}_v ifv >0 tan^{¡1 u}_v +¼sign(u) ifv <0 +^¼₂sign(u) ifv= 0

5.10 INVERSION ANALYTIQUE DU MODELE CINEMATIQUE DU ROBOT PUMA 560

RESOUDRE L’EQUATION

• Poser

• Soient

• La solution

• Deux solutions si ½²=a²+b²> c² Pas de solution si ½²< c². sin(µ+Á) = c

½ cos(µ+Á) =§ s

1¡c²

½² Á=AT AN2 (a; b) a=½sinÁ b=½cosÁ

acos µ + bsin µ = c

½=p a²+b²

µ=ATAN2(c

½;§ s

1¡c²

½²)¡ATAN2(a;b) µ=ATAN2(c;§p

½²¡c²)¡ATAN2(a;b)

CINEMATIQUE DU ROBOT PUMA 560

CALCUL DE CALCUL DE θθ₁₁

• Exprimons

qui vaut après calcul

avec où le vecteur λreprésente

soit le vecteur n, o, aou p

• Identifions avec l’expression de (avec d₆=0)

0A^¡1¹¢T= 2 66 4

f11(n) f11(o) f11(a) f11(p) f12(n) f12(o) f12(a) f12(p) f13(n) f13(o) f13(a) f13(p)

0 0 0 1

3 77 5

f11(¸) =C1¸x+S1¸y

f12(¸) =¡¸z

f₁₃(¸) =¡S₁¸_x+C₁¸_y

1A6

1A6 = 2 66 4

? ? ? S2 3d4+a2C2

? ? ? ¡C2 3d4+a2S2

? ? ? d2

0 0 0 1

3 77 5

0A^¡₁¹T = 2 66 4

C1 S1 0 0

0 0 ¡1 0

¡S1 C1 0 0

0 0 0 1

3 77 5 2 66 4

nx ox ax px

ny oy ay py

nz oz az pz

0 0 0 1

3 77 5

5.10 INVERSION ANALYTIQUE DU MODELE CINEMATIQUE DU ROBOT PUMA 560

CALCUL DE CALCUL DE θθ₁₁

• L’examen du terme (3,4) fournit:

• La solution de cette équation est donnée par:

• Il y a deux solutions possibles à cette équation, chacune d’elles correspondant physiquement à une posture « bras à gauche » et « bras à droite ».

CALCUL DE

CALCUL DE θθ₂₂ET DE ET DE θθ₃₃

• On peut faire une entorse à la procédure de Pieper stricte et essayer d’exploiter au maximum l’information disponible à ce niveau.

• On extrait les relations suivantes:

f13(p) =¡S1px+C1py =d2

f11(p) =C1px+S1py =S23d4+a2C2=® f12(p) =¡pz =¡C23d4+a2S2=¯ µ1 =ATAN2(py;px)+ATAN2(d2;§q

p²_x+p²_y¡d²₂)

5.10 INVERSION ANALYTIQUE DU MODELE CINEMATIQUE DU ROBOT PUMA 560

CALCUL DE

CALCUL DE θθ₂₂ET DE ET DE θθ₃₃

• On part des relations suivantes:

• On en somme les carrés pour trouver:

• On peut encore en faire la combinaison linéaire

• Si on élimine S₃entre ces deux équations on trouve une relation nous donnant la valeur de θ₂:

f11(p) =C1px+S1py =S23d4+a2C2=® f12(p) =¡pz =¡C23d4+a2S2 =¯

®²+¯²=a²₂+d²₄+ 2a2d4S3

®C2+¯S2=a2+S3d4

®C₂+¯S₂= ®²+¯²+a²₂¡d²₄ 2a₂ =°

5.10 INVERSION ANALYTIQUE DU MODELE CINEMATIQUE DU ROBOT PUMA 560

CALCUL DE

CALCUL DE θθ₂₂ET DE ET DE θθ₃₃

• La solution de cette équation trigonométrique s’écrit:

• La solution introduit encore deux branches: la première branche correspond à une posture « bras en haut » et l’autre à une posture « bras en bas »

µ₂=¡AT AN2 (®; ¯) +AT AN2 (°;§p

®²+¯²¡°²)

CINEMATIQUE DU ROBOT PUMA 560

CALCUL DE

CALCUL DE θθ₂₂ET DE ET DE θθ₃₃

• Pour déterminer θ₃, on doit générer encore une autre combinaison des équations de base:

• En la combinant avec (déjà connue)

• On obtient:

• Remarques: la solution θ₃est unique, mais dépend des postures déjà effectués pour θ₁ et θ₂.

• Les solutions de θ₁θ₂et θ₃ne dépendent que de la position du poignet. On a un système découplé!

®S2¡¯C2=C3d4

®C2+¯S2=a2+S3d4

µ3 =AT AN2 (®C2+¯S2¡a2; ®S2¡¯C2)

p=f (µ1; µ2; µ3)

5.10 INVERSION ANALYTIQUE DU MODELE CINEMATIQUE DU ROBOT PUMA 560

CALCUL DE

CALCUL DE θθ₄₄ θθ₅₅ET ET θθ₆₆

• Appliquons à nouveau la technique de Pieper pour obtenir θ₄, θ₅et θ₆. Pour cela il faut au moins se placer au delà du poignet, soit

• Tous calculs faits cette équations se trouve sous la forme

avec les fonctions où le vecteur λreprésente

soit le vecteur n, o, aou p

• On trouve alors les solutions

2A^¡₃¹¢¹A^¡₂¹¢⁰A^¡₁¹¢T =³A6

2 66 4

f3 1(n) f31(o) f31(a) f3 1(p)¡a2

f3 2(n) f32(o) f32(a) f3 2(p) f3 3(n) f33(o) f33(a) f33(p)¡d2

0 0 0 1

3 77 5 =

2 66 4

? ? C4S5 0

? ? S4S5 0

¡S5C6 S5S6 C5 d4

0 0 0 1

3 77 5

f3 1(¸) =C1C23¸x+S1C2 3¸y¡S23¸z

f3 2(¸) =¡S1¸x+C1¸y

f3 3(¸) =C1S2 3¸x+S1S23¸y+C2 3¸z

µ4 =AT AN2 [f32(a); f31(a)]

µ5 =AT AN2 [C4f31(a) +S4f32(a); f33(a)]

µ6 =AT AN2 [f33(o);¡f33(n)]

5.11 SOLUTION NUMERIQUE DU PROBLEME INVERSE

• Formellement, on suppose que le modèle inverse a la forme:

et que pour un ensemble de x_idonnés il existe une solution approchée q_i* qui peut servir de point de départ pour une solution itérative

• Le principe des solutions itératives pour résoudre ce type de problème consiste à calculer des incréments δq_ide telle sorte que:

• On effectue un développement en séries de Taylor de cette équation:

xi=fi(qj) (i= 1; : : : n; j= 1; : : : m)

xi=fi(q_j^¤+±qj)

xi=fi(q_j^¤) + Xm i=1

@f_i

@qj

±qj+O(±q²_j)

5.11 SOLUTION NUMERIQUE DU PROBLEME INVERSE

• On définit:

– La valeur au point courant:

– le résidu:

– la matrice Jacobienne (n x m) du système:

• L’équation de base de la solution itérative devient:

• La méthode de résolution dépends des valeurs de met de n x^¤_i =fi(q_j^¤) ri=xi¡x^¤i

r=J±q+O(±q²) J =

·@fi

@qj

¸

CAS REGULIER M=N CAS REGULIER M=N

• Si Jest non singulière, on peut utiliser une méthode de Newton-Raphson pour résoudre l’équation

• Le flow-chart de la méthode

• Remarques:

– Le coût de la procédure dépend du nombre d’itérations qui dépend lui-même de paramètres tels que la distance du point de départ au point solution, du nombre de conditionnement de la matrice Jacobienne et du nombre de variables.

– La solution du problème n’est pas unique; différentes solutions numériques vont être générées en fonction du point de départ.

– Il se peut qu’il n’y ait pas convergence en dehors du domaine d ’attraction vers l’optimum

• Des progrès sont toujours nécessaires dans des méthodes de résolution robustes et rapides

r=J±q

5.11 SOLUTION NUMERIQUE DU PROBLEME INVERSE

CAS REGULIER M=N CAS REGULIER M=N

Starting Procedure q = q^{0 *}

Residual Vector r = x - f(q )^{k k}

q = q^k Convergence

Check

|r | < ^k ε

Evaluation of correction r = J q^{k k}δ^k

Correction q = q + q^{k+1 k} δ^k

k = k+ 1

Yes

No

5.11 SOLUTION NUMERIQUE DU PROBLEME INVERSE

CAS SUR

CAS SUR--DETERMINE M<NDETERMINE M<N

• Aucune solution n’existe en général

• Une solution peut être trouvée si on recherche la configuration qui minimise l’erreur de positionnement

Soit le problème

• L’erreur de position est minimum si

q1min;:::qm

( F=1

2 Xn i= 1

wi[xi¡fi(qj)]² )

minq

½ F=1

2[x¡f(q)]^TW[x¡f(q)]

¾

W =diag(w1: : : wn)

@F

@qj

=¡X

i

@fj

@qj

wi[xi¡fi(qj)] = 0 J^TW[x¡f(q)] = 0

5.11 SOLUTION NUMERIQUE DU PROBLEME INVERSE

CAS SUR

CAS SUR--DETERMINE M<NDETERMINE M<N

• En développant en séries de Taylor, on obtient l’équation de la correction:

où rest le résidu défini précédemment

• Puisque West une matrice diagonal définie positive, J^TW Jest aussi symétrique et définie positive et elle peut être inversée. C’est une matrice inverse généraliséede la matrice Jacobienne.

qui vérifie la propriété

• On vérifie que lorsque J est invertible,

J^TW[x¡f(q^¤)¡J±q] = 0 J^TW J±q=J^TW r

J⁺= (J^TW J)^¡1J^TW J⁺J=I

CAS SOUS DETERMINE M>N CAS SOUS DETERMINE M>N

• Il y a une infinité de solutions.

• La redondance peut être levée en imposant des contraintes

• Par exemple: trouver la solution qui minimise la déviation par rapport à une configuration de référence

avec

• Fonctionnelle augmentée (Lagrangien) (à n+m variables)

• Conditions d’optimalité minq

½ F=1

2[q¡q₀]^TW[q¡q₀]

¾

x¡f(q) = 0

G(q; ¸) =1

2[q¡q0]^TW[q¡q0] +¸^T[x¡f(q)]

@G

@q = 0 @G

@¸= 0

5.11 SOLUTION NUMERIQUE DU PROBLEME INVERSE

CAS SOUS DETERMINE M>N CAS SOUS DETERMINE M>N

• Système des conditions d’optimalité (n+m variables)

• La substitution de la première équation dans la deuxième donne

• En utilisant cette valeur dans la première équation, on trouve

• La matrice suivante a la signification d’une matrice pseudo-inversede la matrice Jacobienne Jsingulière:

W±q¡J^T±¸= 0 J±q=r

JW^¡1J^T±¸=r

±q=W^¡1J^T(JW^¡1J^T)^¡1r

J⁺=W^¡1J^T(J W^¡1J^T)^¡1

5.11 SOLUTION NUMERIQUE DU PROBLEME INVERSE

CAS SOUS DETERMINE M>N CAS SOUS DETERMINE M>N

• La matrice suivante a la signification d’une matrice pseudo-inversede la matrice JacobienneJsingulière:

• Elle vérifie l’identité

• Si Jest invertible on retrouve:

J⁺=W^¡1J^T(J W^¡1J^T)^¡1

JJ⁺=I

J⁺=J^¡1

5.12 RELATION DE PROJECTION DE LA MATRICE JACOBIENNE

• La matrice Jacobienne transforme l’espace des vitesses articulaires en espace des vitesses cartésiennes par la relation:

• N(J): Espace du noyau de Jcontient les solutions de

• R(J) représente toutes les vitesses possibles de l’effecteur qui peuvent être générées par les m joints.

x = 0.

R(J) N(J)

q. ∋ ∋

Q^m x^. Xⁿ

Jq_ = 0.

x_=Jq_