Avec ces deux matrices, soient ~T et RA
, nous pouvons modifier l’équation 2.21, qui per- met d’aligner les référentiels FB et FA, afin de retrouver la pose relative du RobotB tout en considérant la non-colinéarité de la caméra ~CA:
AF
B= TABTRA ABR2ABR1FB0 . (2.52)
Non colinéarité de la caméra ~CB
Comme expliqué à la sous-section 2.4.1, lorsque la caméra ~CB n’est pas colinéaire avec les marqueurs ~LB et ~RB, la valeur de l’angle β est invalide. Cela a un impact sur l’estimation de la position du RobotB, mais la distance l demeure quant à elle bien estimée. Dans cette situation, l’erreur due à un mauvais angle β peut être corrigée par une rotation autour de l’axe
~
LARAainsi que par une faible rotation pour corriger l’orientation relative estimée du RobotB. Dans le but d’estimer une correction à la non colinéarité de ~CB, nous devons approximer l’angle θ de cette erreur de rotation.
Rappelons que le décalage de la caméra correspond à une translation, ~SB, qui est connue dans le référentiel FB de par la fabrication physique du RobotB. Ce décalage déplace la caméra ~CB à un angle de ΘB hors de la trajectoire du vecteur de translation réelle O
AT~ entre l’origine du référentiel FB et la caméra ~CA. Θ est défini par :
ΘB =acos( O AT · ~~ SB |O AT || ~~ SB| ). (2.53)
Cependant, le vecteur de translationO
AT~ n’est pas connu parce qu’il se trouve dans le référentiel FB et que notre méthode utilise uniquement le référentiel FA. Néanmoins, nous pouvons substituerO
AT~ en fonction du vecteur de translation inverse dans le référentiel FA, soit OBT~ :
ΘB ≈ arccos A BR1−1 ABR2−1(−OBT )~ |O BT |~ · SB~ |~SB| ! . (2.54)
Nous rappelons ici au lecteur queA
BR1etABR2ont respectivement été définis par les équations
2.15 et2.19à la sous-section2.1.3.
Tel qu’expliqué à la section 2.4.2, bien que le vecteurO
BT~ n’est pas connu par notre solution, il est possible de l’approximer parA
BT~ puisque l >> d et lorsque que le décalage ~SB est petit par rapport à la distance d, c.-à-d. lorsque |~SB| < d. Ces conditions étant remplies dans les cas réels d’utilisation de notre système, nous obtenons ainsi l’angle ΘB :
ΘB ≈ arccos A BR1−1 ABR2 −1 (−ABT )~ |A BT |~ · SB~ |~SB| ! . (2.55)
Suite au calcul de ΘB à l’équation 2.55, nous pouvons déterminer la matrice de rotation RB qui permet de corriger la translation estimée ABT~ ainsi que l’orientation relative estimée
A
BR2ABR1. Cette matrice RB est calculée en retrouvant l’erreur θ sur l’orientation résultant de la non-colinéarité de la caméra ~CB. θ = arcsin(sin(ΘB) |~SB| |A BT | + ~ l ) (2.56) RB =Rodrigues( ~LARA, θ) (2.57) l, défini à l’équation 2.49 et employé par l’équation 2.56 ci-dessus, peut être négligé si la caméra ~CA est colinéaire à ses marqueurs.
Puisque nous avons montré à la sous-section précédente que le décalage d’une caméra en- traîne des effets indépendants de la colinéarité de l’autre caméra, nous pouvons ajouter à l’équation 2.52 les corrections RB
. Cela nous amène à l’équation 2.58, qui permet d’estimer approximativement la pose relative du RobotB par rapport au référentiel du RobotA tout en considérant la non-colinéarité des caméras avec les marqueurs de leur robot respectifs.
AFB = RB
TABTRB RA ABR2ABR1F 0
B (2.58)
2.4.3 Simulations et résultats
Dans le but de vérifier les performances de la corrections lors de la non colinéarité, nous avons implémenté une simulation dans Matlab. Dans cette sous-section, nous allons détailler ce qui a été simulé et nous présentons une analyse de nos résultats. Nous invitons le lecteur à consulter l’AppendiceCpour plus de détails sur l’implémentation de la simulation.
Avant de débuter les différentes vérifications, nous souhaitons rappeler au lecteur qu’à la sous-section 2.2.3, nous avons montré que même lorsque les deux caméras sont colinéaires, la valeur de l’angle β est estimée à partir de la position des marqueurs lumineux ~LB et ~RB. Cette estimation de β engendre une erreur sur la position de 0.57 cm et sur l’orientation de 0.0534 deg lorsque la distance l qui sépare les robots est d’environ 11 m et lorsque les marqueurs de chaque robot sont séparés par une distance d de 80 cm. Ces valeurs servent donc d’étalon pour évaluer nos corrections à la colinéarité.
Pour la première série d’expériences, nous avons exécuté la simulation en la configurant de sorte que seule la caméra ~CAsoit non colinéaire. La translation ~SAque nous lui avons appliquée avait une norme variant entre 5% de la distance d, soit 4 cm, et 35 % de d ou 28 cm, lorsque d = 80 cm. ~SA est orientée dans une direction allant entre 0 rad et π rad dans le plan défini par les vecteursAP~
Bet ~nA(nous référons le lecteur aux équations2.6et2.10, respectivement). Ainsi, la caméra ~CA se retrouve au-dessus du plan. Nous avons observé le comportement des corrections décrites à l’équation2.52. Comme le montre la figure 2.31, les erreurs maximales lorsque le décalage est de 35 % de d sont de 0.589 cm sur la position et de 0.149 deg sur l’orientation après correction.
Figure 2.31 – Graphiques représentant les erreurs sur la position et sur l’orientation relatives du RobotB lorsque la caméra ~CA n’est pas colinéaire. Chaque courbe représente l’erreur résiduelle après
correction en fonction de la norme du vecteur de translation ~SAappliqué pour décaler la caméra ~CA.
L’erreur maximale sur la position avec 28 cm de décalage est de 0.589 cm et celle sur l’orientation est de 0.149 deg. Les robots sont séparés d’une distance l = 11 m.
Pour la seconde série d’expériences, nous avons refait la simulation en faisant en sorte que seule la caméra ~CB ne soit pas colinéaire. Les mêmes paramètres ont été utilisés pour déterminer la nature de la translation ~SB comme pour ~SA à l’étape précédente. Cette fois, la caméra
~
CB est positionnée sous le plan défini par les vecteurs AP~B et ~nA Nous avons fait varier la norme et l’orientation de cette translation afin d’observer le comportement de l’erreur après avoir appliqué les corrections définies par l’équation 2.57. Comme le montre la figure2.32, les erreurs maximales lorsque le décalage est de 35 % de d sont de 2.88 cm sur la position et de 0.152 degsur l’orientation. Nous constatons qu’après notre correction, l’erreur reste cependant cinq fois plus élevée pour la position que lorsque seule la caméra ~CA avait été décalée. Cette situation découle du fait que l’angle β joue un rôle important sur l’erreur du positionnement. En effet, plus β est grand, plus une faible incertitude sur cet angle a un grand impact sur la position estimée. Ce phénomène a été montré par Giguere et al.[2012].
Pour la troisième série d’expériences, afin de montrer que les erreurs peuvent être corrigées en les considérant comme indépendantes grâce à l’équation 2.58, nous avons simulé le cas où les deux caméras, ~CA et ~CB, étaient non colinéaires. Alors que la caméra ~CA est décalée au-dessus du plan, ~CB est placé sous ce dernier. La figure 2.33présente les résultats de cette simulation. Lorsque les vecteurs de translation ~SA et ~SB ont une norme de 28 cm, ce qui correspond à 35 % de la distance d entre les marqueurs d’un robot, nous obtenons une erreur maximale sur la position de 2.95 cm et une erreur maximale sur l’orientation de 0.310 deg. Il
Figure 2.32 – Graphiques représentant les erreurs sur la position et sur l’orientation relatives du RobotB lorsque la caméra ~CB n’est pas colinéaire. Chaque courbe représente l’erreur résiduelle après
correction en fonction de la norme du vecteur de translation ~SB appliqué pour décaler la caméra ~CB.
L’erreur maximale sur la position avec 28 cm de décalage est de 2.884 cm et celle sur l’orientation est de 0.152 deg. Les robots sont séparés d’une distance l = 11 m.
nous est au passage possible de constater que cela correspond approximativement à la somme des erreurs mesurées lorsque les caméras étaient non colinéaires une à la fois. Par conséquent, cette simulation confirme la validité de l’équation2.58.
Pour la quatrième série d’expériences, il nous fallait étudier l’efficacité de la correction de l’es- timation de la position lorsque les deux caméras ne sont pas colinéaires et lorsque l’angle β est estimé tel que décrit à la section2.2.2. L’un des motifs justifiant cette nouvelle simulation est que nous avions observé, tant dans le cas à deux dimensions que dans notre situation actuelle, que plus l’angle β était grand, plus l’erreur sur le positionnement pouvait croître rapidement lorsque β était mal estimé. De surcroît, nous avons constaté lors des simulations précédentes que la non-colinéarité de la caméra ~CB rendait plus difficile la correction de l’estimation de la position. Afin de mieux analyser cette situation, nous avons placé le RobotBà 10 m du RobotA et nous avons fait varier la position du premier en le faisant se déplacer dans l’image prise par la caméra ~CA. Nous l’avons progressivement éloigné du centre de l’image IA dans le premier quadrant, tant sur l’axe des abscisses que sur l’axe des ordonnées, d’un angle variant de 0 deg à 75 deg. Tout en déplaçant le RobotBde la sorte, nous nous sommes assurés de le conserver à la distance de 10 m du RobotA. Au terme de cette simulation, nous nous attendions à observer que plus l’angle β augmentait, plus l’erreur après la correction serait grande. De plus, étant donné que le RobotB était orienté pour avoir ses marqueurs lumineux à l’horizontale dans l’image IA, l’angle β tel que décrit à l’équation 2.8 allait être plus grand lorsque le RobotB
Figure 2.33 – Graphiques représentant les erreurs sur la position et sur l’orientation relatives du RobotB lorsque les caméras ~CA et ~CB ne sont pas colinéaire. Chaque courbe représente l’erreur rési-
duelle après correction en fonction de la norme des vecteurs de translation ~SA et ~SB appliquée pour
décaler respectivement les caméras ~CAet ~CB. L’erreur maximale sur la position avec 28 cm de déca-
lage est de 2.954 cm et celle sur l’orientation est de 0.31 deg. Les robots sont séparés d’une distance l = 11 m.
serait déplacé plus loin l’axe X que sur l’axe Y . Par conséquent, nous imaginions que l’erreur sur la position allait augmenter plus rapidement pour un déplacement en X dans l’image IA. C’est effectivement ce que nous avons observé, comme le montre la figure 2.34. Lorsque le RobotB est centré dans l’image IA, l’erreur sur la position est près de 1 cm. Inversement, lorsque l’angle β est à sa valeur la plus grande, l’erreur sur son estimation engendre une erreur sur le positionnement de 23.2 cm. Cependant, une telle valeur ne survient que lorsque β est d’environ 75 deg, soit lorsque les caméras s’observent à des angles extrêmes. Étant donné que nous utilisons pour notre système réel que des caméras qui possèdent un demi-angle de vue de 50 degrés ou moins, nous pouvons nous attendre à une erreur sur le positionnement en deçà de 3 cm dans la plupart des cas d’après les résultats de notre simulation.
Finalement, nous voulions analyser le comportement de la correction de l’erreur lorsque les caméras sont bruitées. Nous avons appliqué un bruit de σA = 0.0003 rad dans l’image IA pour la mesure de l’angle β et de σB = √2 ∗ 0.0003 rad dans l’image IB pour la mesure de l’angle α. Le bruit sur l’image IB est plus grand car il faut deux mesures d’angle dans cette image pour calculer l’angle α. Tout comme pour la précédente simulation, nous avons déplacé le RobotB dans l’espace en le conservant à 11 m du RobotA. La figure2.35affiche les résultats obtenus et les compare aux erreurs obtenues lorsqu’il n’y a pas de bruit sur les caméras. Les résultats indiquent que dans la plupart des positions, l’erreur sur le positionnement causé par
Figure 2.34 – Variation de l’erreur sur la position avec le déplacement dans l’image IA du RobotB.
Le déplacement par rapport au centre de l’image IA fait augmenter rapidement la valeur de l’angle β.
Cette augmentation a pour effet de hausser l’erreur sur la position, tel que démontré précédemment parGiguere et al.[2012].
la non-colinéarité vaut pour moins de 40 % du bruit qui est induit par les caméras.
Figure 2.35 – Variation de l’erreur sur la position avec le déplacement dans l’image IA du RobotB
et avec bruit. Nous illustrons, à la courbe du haut, l’erreur après la correction pour la non-colinéarité. Nous affichons, à la courbe du bas, l’impact du bruit des caméras sur l’erreur des estimations. Le bruit est σA= 0.0003 rad et σB =
√
2 ∗ 0.0003 rad. Le déplacement par rapport au centre de l’image IA fait
augmenter rapidement la valeur de l’angle β, ce qui a pour effet de hausser l’erreur sur la position, comme démontré précédemment parGiguere et al.[2012].
Par conséquent, nous pouvons en conclure que l’erreur causée par la non-colinéarité des ca- méras n’affecte pas significativement la performance du système. Cependant, cette erreur doit être considérée comme un biais sur l’estimation de la pose relative du RobotB. Ainsi, il faut en tenir compte si nous utilisons un tel système avec une technique de filtrage probabiliste, comme un filtre étendu de Kalman ou un filtre de Kalman non parfumé, pour améliorer la précision du positionnement sur de longues distances.
2.5
Conclusion
Dans ce chapitre, nous avons présenté une solution novatrice permettant la localisation coopé- rative à six degrés de liberté. Notre approche nécessite l’utilisation d’une caméra par robot à localiser et ainsi que de deux marqueurs par robot. Notre système n’a pas besoin d’appareil externe aux robots, ce qui le rend particulièrement pratique dans un environnement où les systèmes de positionnement global sont inaccessibles, comme dans des sous-terrains, à l’inté- rieur de bâtiments ou même sous l’eau. De plus, ce qui distingue notre méthode des autres est que nous utilisons deux images capturées au même moment, c.-à-d. une image par robot, et que nous utilisons strictement deux mesures angulaires, nommées α et β, et une mesure de distance fixe et connue d entre les marqueurs d’un des robots.
La section 2.1 a permis de définir le problème de la localisation coopérative à six degrés de liberté. En premier lieu, nous avons établi la nomenclature utilisée pour tout le chapitre et nous avons défini le problème étudié. En second lieu, nous avons rapproché le problème tridimensionnel avec sa simplification au cas en deux dimensions, pour calculer la distance entre deux robots s’observant mutuellement. En dernier lieu, nous avons expliqué comment nous pouvons utiliser l’information connue par les robots pour retrouver la position et l’orientation relatives d’un des robots par rapport à l’autre. Ainsi, nous avons pu obtenir une solution analytique
Ensuite, la section 2.2a été dédiée à la simulation de la solution analytique. Cette simulation était essentielle afin de valider les équations établies à la section 2.1. Premièrement, nous avons décrit l’objectif de la simulation, qui était de représenter le plus fidèlement la réalité dans laquelle un tel système de localisation serait utilisé. Nous avons également expliqué la manière dont nous avons représenté nos robots mutuellement observables et la technique que nous avons utilisée pour simuler les captures d’images simultanées. Deuxièmement, nous avons expliqué comment nous avons, dans cette simulation, évalué les angles nécessaires à notre méthode. Troisièmement, nous avons expliqué comment nous avons comparé les distances, angles et les positions estimées avec les valeurs réelles. Finalement, nous avons présenté les résultats de notre simulation. Parmi ceux-ci, nous notons qu’en l’absence de bruit dans les images, la pose des robots est recouvrée parfaitement. De plus, nous avons constaté que la majorité des erreurs d’estimation de la pose relative provient du bruit gaussien des images causé par la précision des caméras, qui est limitée.
Puis, la section 2.3a été consacrée à la description des expérimentations qui ont été réalisées pour démontrer concrètement la validité de notre approche. Dans celle-ci, nous avons d’abord expliqué comment nous avons conçu et construit deux prototypes de robots afin de mettre à l’épreuve notre approche. Nous avons également décrit comment nous avons su calibrer puis utiliser des systèmes de localisation par caméras très précis soient le système OptiTrack [Natu- ralPoint,2013] et, pour une expérimentation à longue portée, le système Vicon [Vicon,2013].
Enfin, nous avons démontré les performances de notre système. Nous avons calculé la précision des caméras de nos prototypes de robots ainsi que de celles du système OptiTrack. Nous avons obtenu que notre système mesurant la réalité de terrain fût assez précis pour localiser nos robots avec moins d’un millimètre d’erreur. Nous avons également calculé que les caméras de nos prototypes de robots avaient une précision de 0.19 pixels. Nous avons présenté les résultats de l’analyse de la robustesse de notre méthode et de notre système de réalité de terrain en fonction de l’orientation des robots. Nous avons discuté de la précision de notre solution sur de courtes distances, c.-à-d. variant entre 3.5 m et 6.5 m. Dans cette situation, nous avons obtenu une erreur sur la position relative de 1.99 cm et sur l’orientation relative de 1.18 deg. Nous avons finalement vérifié les performances de notre technique appliquée à de plus grandes distances, soit en séparant nos robots par une distance variant entre 12.1 m et 15.0 m. Lors de cette expérimentation, l’erreur sur la position était en moyenne de 5.37 cm et l’erreur moyenne sur l’orientation, elle, était de 1.36 deg.
Enfin, la section2.4a consisté en une étude sur l’extension de notre solution analytique au cas plus général où les caméras ne sont pas colinéaires avec les marqueurs lumineux de leur robot respectif. Cette colinéarité, qui était définie à la section2.1, permettait certaines simplifications utiles pour résoudre le problème de localisation coopérative. Ce n’est qu’après avoir analysé l’impact du décalage de chaque caméra sur la précision de nos estimations de pose relative que nous avons découvert que le décalage d’une caméra entraîne des effets indépendants de la colinéarité de l’autre caméra. Par la suite, nous avons utilisé ce fait pour développer les corrections pouvant être appliquées à notre solution originale afin d’optimiser la précision de nos estimations. Également, nous avons donné les détails d’une simulation de ce cas général. Nous avons obtenu que lorsque les deux caméras sont décalées de 28 cm par rapport à leur axe colinéaire, nous pouvions nous attendre à une erreur additionnelle sur le positionnement en deçà de 3 cm et à une erreur d’orientation sous les 0.31 deg. Enfin, nous avons constaté que les erreurs sur l’estimation de la pose relative causées par le bruit sur les caméras étaient de beaucoup supérieures à l’erreur de la correction pour le cas non colinéaire. Ainsi, nous pouvons considérer cette erreur de correction comme un biais qui est négligeable. Cependant, il demeure essentiel de conserver en tête ce biais si nous devons intégrer notre système de localisation à une technique de filtrage probabiliste comme un filtre de Kalman.
La solution que nous avons présentée dans ce chapitre permet donc à deux robots qui se dé- placent dans un environnement de se localiser relativement l’un par rapport à l’autre. Cepen- dant, d’une part, l’une des plus grandes difficultés avec la présente implémentation physique est de permettre l’établissement d’une observation mutuelle des robots sans interférer avec les opérations normales de ces robots. Néamoins, puisque cette méthode est complètement com- patible avec n’importe quelles caméras de type sténopé, il est possible d’utiliser par exemple des caméras omnidirectionnelles. D’autre part, la distance entre les robots est une seconde difficulté avec l’utilisation de notre système. En effet, plus les robots se localisant entre eux
sont éloignés l’un de l’autre, moins le système sera précis. Lorsque nous faisons face à ce genre de problème, il est toutefois possible d’utiliser différentes techniques de filtrage probabiliste. C’est à ce problème que nous nous attaquerons dans les prochains chapitres. En effet, nous discuterons, au chapitre 3, d’une technique basée sur les filtres de Kalman que nous avons développée afin d’améliorer la précision de notre système de positionnement relatif. Ce même