Calcul garanti des contraintes pour la planification sécurisée de trajectoire Application à la génération de trajectoire articulaire pour un patient paraplégique sous Stimulation Électrique Fonctionnelle (SEF) Sébastien LENGAGNE, Nacim RAMDANI, Philippe FRAISSE LIRMM - équipe-projet DEMAR / INRIA 22 novembre 2007 LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 1 / 25
1 Paraplégie Qu est ce que c est? SEF Utilisation des robots humanoïdes : validation 2 Planification de trajectoire Problème d optimisation Les contraintes Discrétisation classique des contraintes Discrétisation garantie des contraintes 3 Modélisation mathématique Modélisation simplifiée Contraintes Modèle dynamique inverse Équilibre : ZMP 4 Résultats 5 Conclusions et Perspectives LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 2 / 25
Paraplégie Qu est ce que c est? Paraplégie : Qu est ce que c est? FIG.: Patient sain LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 3 / 25
Paraplégie Qu est ce que c est? Paraplégie : Qu est ce que c est? FIG.: Patient paraplégique LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 3 / 25
Paraplégie Qu est ce que c est? Paraplégie : Qu est ce que c est? FIG.: Patient paraplégique sous SEF LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 3 / 25
Paraplégie SEF : Stimulation Électrique Fonctionnelle SEF LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 4 / 25
Paraplégie Utilisation des robots humanoïdes : validation Utilisation des robots humanoïdes : validation LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 5 / 25
Paraplégie Utilisation des robots humanoïdes : validation Utilisation des robots humanoïdes : validation LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 5 / 25
Paraplégie Utilisation des robots humanoïdes : validation Utilisation des robots humanoïdes : validation LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 5 / 25
Paraplégie Utilisation des robots humanoïdes : validation Utilisation des robots humanoïdes : validation Les robots humanoïdes permettent de valider les méthodes développées. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 5 / 25
Planification de trajectoire Planification de trajectoire Objectif Déterminer les trajectoires articulaires pour avoir un mouvement optimal. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 6 / 25
Planification de trajectoire Problème d optimisation Problème d optimisation Trouver le vecteur x qui minimise la fonction objectif (énergie, temps,...) : min f (x) (1) x R n Et respecte des contraintes (butées, équilibre,...) : g in(x,t) 0 t [0,T ] g eq(x,t) = 0 (2) x min x x max (3) Pour résoudre ce problème, on utilise un algorithme d optimisation non linéaire. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 7 / 25
Planification de trajectoire Les contraintes Les contraintes Les contraintes se calculent à partir des paramètres x. (x) => g (x,t) (4) g (x,t) est une fonction continue, LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 8 / 25
Planification de trajectoire Les contraintes Les contraintes Les contraintes se calculent à partir des paramètres x. (x) => g (x,t) (4) g (x,t) est une fonction continue, Les programmes d optimisation ne prennent pas en compte le temps (continu), LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 8 / 25
Planification de trajectoire Les contraintes Les contraintes Les contraintes se calculent à partir des paramètres x. (x) => g (x,t) (4) g (x,t) est une fonction continue, Les programmes d optimisation ne prennent pas en compte le temps (continu), Il faut discrétiser! LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 8 / 25
Planification de trajectoire Discrétisation classique des contraintes Discrétisation classique des contraintes g (x,t) est une fonction de t. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 9 / 25
Planification de trajectoire Discrétisation classique des contraintes Discrétisation classique des contraintes g (x,t) doit rester entre ces valeurs min et max LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 9 / 25
Planification de trajectoire Discrétisation classique des contraintes Discrétisation classique des contraintes l algorithme d optimisation dispose de n t points de la fonction LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 9 / 25
Planification de trajectoire Discrétisation classique des contraintes Discrétisation classique des contraintes l algorithme en déduit que les contraintes sur g (x,t) sont respectées LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 9 / 25
Planification de trajectoire Discrétisation classique des contraintes Discrétisation classique des contraintes l algorithme en déduit que les contraintes sur g (x,t) sont respectées Inconvénient : Cette solution peut être jugée optimale, alors qu elle entrainera la chute ou la destruction du robot. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 9 / 25
Planification de trajectoire Discrétisation garantie des contraintes Discrétisation garantie des contraintes g (x,t) est une fonction de t. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 10 / 25
Planification de trajectoire Discrétisation garantie des contraintes Discrétisation garantie des contraintes g (x,t) doit rester entre ces valeurs min et max LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 10 / 25
Planification de trajectoire Discrétisation garantie des contraintes Discrétisation garantie des contraintes l algorithme d optimisation dispose du calcul des min et max sur n i intervalles LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 10 / 25
Planification de trajectoire Discrétisation garantie des contraintes Discrétisation garantie des contraintes l algorithme en déduit que les contraintes sur g (x,t) ne sont pas respectées LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 10 / 25
Planification de trajectoire Discrétisation garantie des contraintes Discrétisation garantie des contraintes l algorithme en déduit que les contraintes sur g (x,t) ne sont pas respectées Conclusion : Cette solution n est pas acceptable. L algorithme calculera un nouveau vecteur x. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 10 / 25
Modélisation simplifiée Modélisation mathématique Modélisation simplifiée Hypothèse Le patient gère son équilibre dans le plan frontal avec ses membres supérieurs : LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 11 / 25
Modélisation simplifiée Modélisation mathématique Modélisation simplifiée Hypothèse Le patient gère son équilibre dans le plan frontal avec ses membres supérieurs : Modèle 2D dans le plan sagittal Les efforts des membres supérieurs sont ramenés à un effort équivalent au niveau du bassin. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 11 / 25
Modélisation mathématique Modélisation simplifiée FIG.: Modélisation 2D d un patient paraplégique sous SEF Modèle 2-D On obtient un modèle 2-D avec 6 degrés de liberté. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 12 / 25
Modélisation mathématique Modélisation simplifiée Modélisation mathématique On modélise le patient / robot comme une chaîne cinématique ouverte avec : Le pied de support supposé fixe ( contact bilatéral ), LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 13 / 25
Modélisation mathématique Modélisation simplifiée Modélisation mathématique On modélise le patient / robot comme une chaîne cinématique ouverte avec : Le pied de support supposé fixe ( contact bilatéral ), L effort équivalent des membres supérieurs ramené au bassin, LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 13 / 25
Modélisation mathématique Modélisation simplifiée Modélisation mathématique On modélise le patient / robot comme une chaîne cinématique ouverte avec : Le pied de support supposé fixe ( contact bilatéral ), L effort équivalent des membres supérieurs ramené au bassin, Une phase de double support instantanée. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 13 / 25
Modélisation mathématique Contraintes Contraintes Au niveau articulaire : LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 14 / 25
Modélisation mathématique Contraintes Contraintes Au niveau articulaire : Butée, LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 14 / 25
Modélisation mathématique Contraintes Contraintes Au niveau articulaire : Butée, Vitesse de rotation, LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 14 / 25
Modélisation mathématique Contraintes Contraintes Au niveau articulaire : Butée, Vitesse de rotation, Accélération, LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 14 / 25
Modélisation mathématique Contraintes Contraintes Au niveau articulaire : Butée, Vitesse de rotation, Accélération, Efforts musculaire / couple articulaire. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 14 / 25
Modélisation mathématique Contraintes Contraintes Au niveau articulaire : Butée, Vitesse de rotation, Accélération, Efforts musculaire / couple articulaire. Équilibre du patient. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 14 / 25
Modélisation mathématique Modèle dynamique inverse Modèle A partir du vecteur X on calcule les positions, vitesses et accélérations articulaires : (q(t), q(t), q(t)) LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 15 / 25
Modélisation mathématique Modèle dynamique inverse Modèle A partir du vecteur X on calcule les positions, vitesses et accélérations articulaires : (q(t), q(t), q(t)) Le MDI permet de calculer les couples articulaires Γ en fonction : (q, q, q) et des efforts extérieurs F e Γ = f(q, q, q,f e ) (5) LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 15 / 25
Modélisation mathématique Modèle dynamique inverse Modèle A partir du vecteur X on calcule les positions, vitesses et accélérations articulaires : (q(t), q(t), q(t)) Le MDI permet de calculer les couples articulaires Γ en fonction : (q, q, q) et des efforts extérieurs F e Γ = f(q, q, q,f e ) (5) Modèle Dynamique Inverse (MDI) Le MDI peut être résolu par 2 méthodes : Lagrange ou Newton-Euler. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 15 / 25
Modélisation mathématique Modèle dynamique inverse Modèle A partir du vecteur X on calcule les positions, vitesses et accélérations articulaires : (q(t), q(t), q(t)) Le MDI permet de calculer les couples articulaires Γ en fonction : (q, q, q) et des efforts extérieurs F e Γ = f(q, q, q,f e ) (5) Modèle Dynamique Inverse (MDI) Le MDI peut être résolu par 2 méthodes : Lagrange ou Newton-Euler. Formalisme de Lagrange LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 15 / 25
Modélisation mathématique Modèle dynamique inverse Modèle A partir du vecteur X on calcule les positions, vitesses et accélérations articulaires : (q(t), q(t), q(t)) Le MDI permet de calculer les couples articulaires Γ en fonction : (q, q, q) et des efforts extérieurs F e Γ = f(q, q, q,f e ) (5) Modèle Dynamique Inverse (MDI) Le MDI peut être résolu par 2 méthodes : Lagrange ou Newton-Euler. Formalisme de Lagrange Algorithme de Newton-Euler LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 15 / 25
Modélisation mathématique Modèle dynamique inverse Formalisme de Lagrange M (q) : matrice inertie, Γ = M (q) q + H (q, q) + J T F e (6) H (q, q) : représente les forces centrifuge, coriolis et gravité, J : matrice jacobienne : répercute les efforts extérieurs sur les couples articulaires. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 16 / 25
Modélisation mathématique Modèle dynamique inverse Formalisme de Lagrange M (q) : matrice inertie, Γ = M (q) q + H (q, q) + J T F e (6) H (q, q) : représente les forces centrifuge, coriolis et gravité, J : matrice jacobienne : répercute les efforts extérieurs sur les couples articulaires. Inconvénients Non adapté pour les systèmes avec un grand nombre de degrés de libertés LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 16 / 25
Modélisation mathématique Modèle dynamique inverse Algorithme de Newton-Euler récurrence avant (base -> extrémité) En partant de (q, q, q), on calcule (X,Ẋ,Ẍ) afin de déterminer les efforts de chaque corps i dûs au mouvement :f i LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 17 / 25
Modélisation mathématique Modèle dynamique inverse Algorithme de Newton-Euler récurrence avant (base -> extrémité) En partant de (q, q, q), on calcule (X,Ẋ,Ẍ) afin de déterminer les efforts de chaque corps i dûs au mouvement :f i récurrence arrière : (extrémité -> base) On répercute l effet de f i sur le corps parent, pour déterminer les couples articulaires. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 17 / 25
Modélisation mathématique Équilibre : ZMP Équilibre : Zero-Moment-Point (ZMP) Le Zero-Moment-Point (ZMP) est le point sur la surface de contact où la résultantes des moments est nulle. Si ce point reste dans la surface de sustentation le robot conserve sont équilibre. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 18 / 25
Modélisation mathématique Équilibre : ZMP Équilibre : Zero-Moment-Point (ZMP) Le Zero-Moment-Point (ZMP) est le point sur la surface de contact où la résultantes des moments est nulle. Si ce point reste dans la surface de sustentation le robot conserve sont équilibre. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 18 / 25
Modélisation mathématique Équilibre : ZMP Équilibre : Zero-Moment-Point (ZMP) Le Zero-Moment-Point (ZMP) est le point sur la surface de contact où la résultantes des moments est nulle. Si ce point reste dans la surface de sustentation le robot conserve sont équilibre. bornes du ZMP Pour la marche on considère uniquement l équilibre statique : ZMP min < ZMP(t) < ZMP max (7) LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 18 / 25
Résultats Résultat : optimisation de trajectoires On veut obtenir un pas de 5 cm, en un temps minimal avec le modèle 2D de HOAP3 3 paramètres par articulations + durée du mouvement : 19 paramètres, LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 19 / 25
Résultats Résultat : optimisation de trajectoires On veut obtenir un pas de 5 cm, en un temps minimal avec le modèle 2D de HOAP3 3 paramètres par articulations + durée du mouvement : 19 paramètres, contraintes : LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 19 / 25
Résultats Résultat : optimisation de trajectoires On veut obtenir un pas de 5 cm, en un temps minimal avec le modèle 2D de HOAP3 3 paramètres par articulations + durée du mouvement : 19 paramètres, contraintes : butées articulaires : (2 n q 6) LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 19 / 25
Résultats Résultat : optimisation de trajectoires On veut obtenir un pas de 5 cm, en un temps minimal avec le modèle 2D de HOAP3 3 paramètres par articulations + durée du mouvement : 19 paramètres, contraintes : butées articulaires : (2 n q 6) vitesses articulaires : (2 n q 6) LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 19 / 25
Résultats Résultat : optimisation de trajectoires On veut obtenir un pas de 5 cm, en un temps minimal avec le modèle 2D de HOAP3 3 paramètres par articulations + durée du mouvement : 19 paramètres, contraintes : butées articulaires : (2 n q 6) vitesses articulaires : (2 n q 6) ZMP : (2 n ZMP ) LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 19 / 25
Résultats Résultat : optimisation de trajectoires On veut obtenir un pas de 5 cm, en un temps minimal avec le modèle 2D de HOAP3 3 paramètres par articulations + durée du mouvement : 19 paramètres, contraintes : butées articulaires : (2 n q 6) vitesses articulaires : (2 n q 6) ZMP : (2 n ZMP ) position initiale et finale du pied : (6). LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 19 / 25
Résultats Résultat : optimisation de trajectoires On veut obtenir un pas de 5 cm, en un temps minimal avec le modèle 2D de HOAP3 3 paramètres par articulations + durée du mouvement : 19 paramètres, contraintes : butées articulaires : (2 n q 6) vitesses articulaires : (2 n q 6) ZMP : (2 n ZMP ) position initiale et finale du pied : (6). critère : minimisation de la durée du mouvement. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 19 / 25
Résultats Discrétisation classique Résultats obtenus pour l optimisation d un pas de 5 cm en temps minimum. L optimisation converge normalement en indiquant un respect des contraintes. n q n ZMP t optim t mvt nb viol % viol max 5 5 16 s 2.33 s 1036 13.7 10 5 148 s 2.31 s 1028 7.8 20 5 424 s 2.45 s 1035 1.2 50 5 9912 s 2.42 s 621 0.2 LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 20 / 25
Résultats Discrétisation classique Résultats obtenus pour l optimisation d un pas de 5 cm en temps minimum. L optimisation converge normalement en indiquant un respect des contraintes. n q n ZMP t optim t mvt nb viol % viol max 5 5 16 s 2.33 s 1036 13.7 10 5 148 s 2.31 s 1028 7.8 20 5 424 s 2.45 s 1035 1.2 50 5 9912 s 2.42 s 621 0.2 Les contraintes articulaires sont toujours violées. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 20 / 25
Résultats Discrétisation garantie Résultats obtenus pour l optimisation d un pas de 5 cm en temps minimum. L optimisation converge normalement en indiquant un respect des contraintes. n q n ZMP t optim t mvt nb viol % viol max 5 5 371 s 2.52 s 0 0 10 5 1264 s 2.27 s 0 0 20 5 574 s 2.44 s 0 0 10 10 1841 s 2.19 s 0 0 LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 21 / 25
Résultats Discrétisation garantie Résultats obtenus pour l optimisation d un pas de 5 cm en temps minimum. L optimisation converge normalement en indiquant un respect des contraintes. n q n ZMP t optim t mvt nb viol % viol max 5 5 371 s 2.52 s 0 0 10 5 1264 s 2.27 s 0 0 20 5 574 s 2.44 s 0 0 10 10 1841 s 2.19 s 0 0 Les contraintes sont toujours respectées LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 21 / 25
Conclusions et Perspectives Conclusions : Intérêt de la Discrétisation garantie L information de validité des contraintes, donnée par le programme d optimisation, est garantie. Perspectives LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 22 / 25
Conclusions et Perspectives Conclusions : Intérêt de la Discrétisation garantie L information de validité des contraintes, donnée par le programme d optimisation, est garantie. Le temps de calcul est plus long (sans importance pour des calculs hors ligne) Perspectives LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 22 / 25
Conclusions et Perspectives Conclusions : Intérêt de la Discrétisation garantie L information de validité des contraintes, donnée par le programme d optimisation, est garantie. Le temps de calcul est plus long (sans importance pour des calculs hors ligne) Perspectives Modélisation : ajout d une phase de double support. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 22 / 25
Conclusions et Perspectives Conclusions : Intérêt de la Discrétisation garantie L information de validité des contraintes, donnée par le programme d optimisation, est garantie. Le temps de calcul est plus long (sans importance pour des calculs hors ligne) Perspectives Modélisation : ajout d une phase de double support. Critère : notion effort musculaire. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 22 / 25
Conclusions et Perspectives Conclusions : Intérêt de la Discrétisation garantie L information de validité des contraintes, donnée par le programme d optimisation, est garantie. Le temps de calcul est plus long (sans importance pour des calculs hors ligne) Perspectives Modélisation : ajout d une phase de double support. Critère : notion effort musculaire. Planification : diminution du temps de calcul. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 22 / 25
Conclusions et Perspectives HOAP 3 optimisation d un demi-pas Optimisation d un demi-pas avec une minimisation de la durée du mouvement LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 23 / 25
Conclusions et Perspectives HOAP 3 optimisation d un pas entier Optimisation d un pas avec une minimisation de l énergie électrique LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 24 / 25
Conclusions et Perspectives Merci de votre attention. LENGAGNE RAMDANI FRAISSE Calcul garanti des contraintes 22/11/07 25 / 25