Fusion force-vision et suivi multi-modal

Le suivi de cible par vision est un domaine de recherche tr`es vaste. Nous n’abordons pas ici le probl`eme de la reconnaissance, mais utilisons simplement des marqueurs coll´es sur les objets et un syst`eme de vision sp´ecifique. Cet outil sp´ecifique, comme d’autres outils de vision, produit des donn´ees bruit´ees, notamment lorsque l’´eclairement varie ou que les marqueurs sont partiellement occult´es.

Le syst`eme de st´er´eovision calcule la position relative T<sub>V</sub>Cdu rep`ere C li´e `a l’objet par rapport au rep`ere V associ´e `a la paire de cam´era. TVCest une matrice de passage.

T_VC=       x R3×3 y z 0 0 0 1       (E.20)

o`u R est une matrice de rotation. Les cam´eras sont mont´es sur une platine qui s’oriente pendant les manipulations. La position de l’objet est donc d´efinie par :

T_BC= T_BVT_VO (E.21)

o`u T<sub>B</sub>C est calcul´ee `a partir du mod`ele cin´ematique de la platine. Bien que tr`es simple, ce calcul cache une difficult´e : une petite erreur dans cette matrice peut entrainer des erreurs importantes pour la localisation de l’objet et causer de s´erieux probl`eme pendant la ma- nipulation. Une proc´edure d’identification des param`etres d´efinissant la position relative

des cam´eras par rapport `a la main peut ˆetre utilis´ee, la litt´erature en propose de nombreux exemples [Dornaika 98].

Nous effectuons les calculs dans le rep`ere de base du robot. Ce choix est pr´ef´erable `a celui d’un rep`ere fixe de la sc`ene du point de vu pr´ecision car la position du robot dans la sc`ene n’est pas toujours connue avec pr´ecision. L’expression de la position de l’objet dans le rep`ere W de la sc`ene peut ˆetre obtenu par :

T_WC= TWBT V

BT

C

V (E.22)

o`u T<sub>W</sub>B est la position de la base par rapport `a la sc`ene. Les erreurs de localisation du robot se retrouvent dans cette position. Cette approche est n´ecessaire dans le cas o`u la base du robot bouge durant la tˆache de manipulation, ce qui n’est pas le cas pour les travaux effectu´e pendant cette th`ese.

Comme nous avons choisi d’utiliser le filtrage de Kalman non-lin´eaire pour la fusion des donn´ees, nous devons construire un mod`ele diff´erentiel du mouvement de l’objet ou un mod`ele r´ecurent car le calcul sera it´eratif. Nous supposons que l’objet est tendu par une personne ou par le robot et bouge `a vitesse constante avec un bruit sur la connaissance des position et vitesse.

Ce choix se justifie car nous travaillons dans le domaine du robot de service et la vitesse de l’objet est faible. D’autre part, consid´erer l’acc´el´eration et plus encore le jerk n’am´eliorerait pas n´ecessairement les performances car la proc´edure de filtrage ajuste d´ej`a la vitesse `a chaque it´eration. ´Etant donn´e que l’objet est tendu par un autre agent, le mod`ele `a vitesse constante est aussi suffisant pour assurer le suivi de l’objet. L’´etat du mod`ele x est de dimension 12 :

x = (xT_p x˙T_p)T (E.23)

o`u xp= (x y z ϕ θ φ ). Ce vecteur correspond `a la position et `a l’orientation suivies. Si on suppose que le syst`eme de suivi fonctionne avec une p´eriode de δ t entre chaque actualisation, au temps t l’´equation d’´etat de la dynamique du syst`eme peut s’´ecrire :

x(t + δ t) = A_{δ t}x(t) + ωt (E.24)

o`u la matrice Aδ t peut s’exprimer :

A(δ t) = " I6 I6δ t 0 I6 # (E.25)

I₃est la matrice identit´e de dimension 3. Ce mod`ele est bas´e sur l’hypoth`ese que la cible bouge avec une vitesse constante dans toutes les directions pendant une p´eriode. Le bruit du processus que nous supposons ˆetre un bruit blanc gaussien est le vecteur ωt. La matrice

de covariance devient alors : E{ωtωt+εT } =    Q(δ t) ε = 0, 0 ε 6= 0. (E.26)

Le terme Q(δ t) doit ˆetre calcul´e `a partir du bruit r´eel. Les valeurs peuvent ˆetre ajust´ees pour am´eliorer la dynamique du filtre, toutefois le plupart du temps le filtre identifie le pro- cessus mˆeme avec un r´eglage grossier des termes de covariance du bruit. La partie relative `a l’identification de l’orientation a ´et´e plac´ee dans le mod`ele d’observation plutˆot que dans le mod`ele dynamique. Durant chaque it´eration, les valeurs des incr´ements d’orientation sont simplement des valeurs de l’´etat et `a la fin de chaque it´eration ils sont remis `a z´ero.

Pour ´eviter les probl`emes des angles d’Euler qui n’ont pas une repr´esentation unique, nous utilisons des incr´ements d’angle. Ces incr´ements sont remis `a z´ero `a la fin de chaque it´eration de l’identification et l’observation de ces angles est simplement la diff´erence avec la derni`ere mise `a jour. `A la fin de la fonction de filtrage de Kalman, on fait le calcule:

(ϕ θ φ ) = (0 0 0) (E.27)

Les rotations absolues de l’objet est gard´ees `a l’ext´erieur du filtre de suivi, sous la forme de

q = {q0q1q2q3} (E.28)

E.2.2.1 Mod `eles de mesure

L’utilisation du filtrage de Kalman n´ecessite un mod`ele de mesure pour chaque source d’information. Comme nous ne consid´erons que des angles incr´ementaux dans le mod`ele dynamique, les valeurs absolues des angles doivent ˆetre incluses dans les valeurs externes du filtre. Les mod`eles de mesure d´efinissent les relations qui relient les incr´ements de rota- tion avec les donn´ees mesur´ees disponibles sur le robot. Dans notre cas, ce sont la vision 3D, le capteur de force et les mesures au niveau des articulations.

Mesure avec la vision: Pour des raisons de concision et de clart´e, nous utilisons les

quaternions pour repr´esenter les rotations absolues. La vision 3D fournit la position des objets dans le rep`ere du robot. L’expression de l’incr´ement de quaternion `a partir des angles incr´ementaux s’´ecrit :

∆q = h(ϕ θ φ ) (E.29)

o`u, ϕ, θ et φ sont les incr´ements des angles d’Euler comme dans l’´equationE.23 qui est

non-lin´eaire. ϕ, θ , et φ repr´esentent l’´etat du mod`ele du processus (E.24). Cette ´equation est l’´equation de l’observation du processus d’identification. A chaque ´etape, le quaternion

conservant la rotation absolue est mis `a jour par le produit de quaternion suivant :

b

q =q_bO∆q (E.30)

Mesure des forces et couples : Le processus de mesure des forces pr´esent´e dans ce

paragraphe fournit la position P_SC de l’objet C dans rep`ere S du capteur. Nous utilisons les notations homog`enes, aussi P_SC= [cx cy cz1] comporte un 1 comme quatri`eme ´el´ement. La position du capteur de force par rapport au rep`ere terminal du bras est donn´ee par la matrice T_ES. Le mod`ele g´eom´etrique du robot est d´efini par la matrice de passage TS

E. La position de l’objet dans le rep`ere du robot s’exprime alors :

P_RC= TRET S

EP

C

S (E.31)

La fr´equence d’actualisation de la position T_RE du robot est plus rapide que celle de la fusion. Il faut aussi mentionner que la mesure de la position des articulations est bruit´e et doit ˆetre filtr´e.