Navigation autonome sans collision pour robots mobiles ...

Thèsepréparée au

Laboratoire d’Analyse et d’Architecture des Systèmes du CNRS

en vue de l’obtention du

Doctorat de l’Institut National Polytechnique de Toulouse

par

Olivier Lefebvre

Navigation autonome sans collision pour robotsmobiles nonholonomes

Soutenue le 12 juillet 2006 devant le jury composé de :

M. Florent Lamiraux Directeur de thèseM. Tarek Hamel RapporteurM. Fawzi Nashashibi RapporteurM. Pierre Rouchon RapporteurM. Jean-Paul Laumond PrésidentM. Javier Minguez Examinateur

LAAS-CNRS7, Avenue du Colonel Roche31077 Toulouse Cedex 4

Remerciements

Sur cette page qui sera probablement la plus consultée de ce mémoire, je tiens tout d’abord àremercier les personnes qui ont bien voulu se laisser convaincre par ma motivation et qui m’ontpermis de réaliser cette thèse, Florent Lamiraux et Raja Chatila.

Je remercie chaleureusement les rapporteurs de mon mémoire de thèse, Tarek Hamel, FawziNashashibi et Pierre Rouchon, ainsi que le membre invité du jury de soutenance, Javier Minguez.

Je souhaite à nouveau remercier Florent qui fut mon directeur de thèse ; pour son organisationet la disponibilité qui en découle, pour tout ce qu’il a pu m’apprendre, pour avoir tour à toursupporté et consolidé mes modestes talents mathématiques et pour sa sympathie, "qui ne gâcherien".

Pour leur aide, leurs conseils et leur soutien, merci aux membres dits «permanents» desgroupes 2I, RIA puis GEPETTO. Merci notamment à Jean-Paul pour les moments de discus-sion dont je ressortais toujours les idées plus claires.

Parce que nos échanges ne furent pas exclusivement scientifiques, un grand merci à tous les«apprenti-chercheurs» avec qui j’ai passé ces trois bonnes années à bronzer devant les écransd’ordinateur ou sur les sommets pyrénéens.

Merci enfin à Ester pour sa relecture ortho-typographique minutieuse, et pour m’avoir ac-compagné dans cette aventure.

Table des matières

1 Introduction 11.1 Contexte . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Navigation autonome des robots mobiles . . . . . . . . . . . . . . . . . . . . . . 1

1.2.1 Planification de mouvement . . . . . . . . . . . . . . . . . . . . . . . . 21.2.2 Localisation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.2.3 Suivi de trajectoire . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.2.4 Évitement réactif d’obstacles . . . . . . . . . . . . . . . . . . . . . . . . 21.2.5 Parking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.2.6 Intégration des différentes fonctionnalités de la navigation . . . . . . . . 3

1.3 Spécificités de notre approche . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.3.1 Nos contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51.3.2 Plan de lecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2 Notations et définitions 92.1 Espaces et positions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.2 Distances . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.3 Système dynamique . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3 Évitement réactif d’obstacles pour robots mobiles nonholonomes 153.1 État de l’art . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3.1.1 Méthodes d’évitement d’obstacles sans trajectoire de référence . . . . . . 163.1.2 Méthodes d’évitement d’obstacles avec une trajectoire de référence . . . 17

3.2 Déformation de trajectoire pour systèmes nonholonomes . . . . . . . . . . . . . 183.2.1 Déformation de trajectoire . . . . . . . . . . . . . . . . . . . . . . . . . 183.2.2 S’éloigner des obstacles . . . . . . . . . . . . . . . . . . . . . . . . . . 203.2.3 Calcul d’une déformation admissible s’éloignant des obstacles . . . . . . 223.2.4 Prise en compte des conditions aux limites . . . . . . . . . . . . . . . . 253.2.5 Calcul de la trajectoire déformée . . . . . . . . . . . . . . . . . . . . . . 273.2.6 Respect des contraintes nonholonomes . . . . . . . . . . . . . . . . . . 273.2.7 Algorithme de la déformation de trajectoire . . . . . . . . . . . . . . . . 293.2.8 Prise en compte des bornes sur les entrées du système . . . . . . . . . . 30

3.3 Application de la méthode de déformation de trajectoire : le robot Cycab . . . . . 323.3.1 Modèle cinématique . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

vi TABLE DES MATIÈRES

3.3.2 Calculs pour la déformation de trajectoire . . . . . . . . . . . . . . . . . 333.3.3 Exemple de déformation . . . . . . . . . . . . . . . . . . . . . . . . . . 34

3.4 Calcul des interactions robot-obstacles . . . . . . . . . . . . . . . . . . . . . . . 373.4.1 Potentiel dans l’espace des configurations . . . . . . . . . . . . . . . . . 373.4.2 Optimisation des calculs des interactions . . . . . . . . . . . . . . . . . 41

3.5 Optimisation de la trajectoire suivant d’autres critères . . . . . . . . . . . . . . . 463.5.1 Respect d’une contrainte sur la courbure durant le processus de déforma-

tion de trajectoire . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 473.5.2 Déformation d’une trajectoire non-admissible vers une trajectoire admis-

sible . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 493.6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

4 Parking référencé sur des amers 534.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 534.2 État de l’art . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

4.2.1 Asservissement visuel . . . . . . . . . . . . . . . . . . . . . . . . . . . 554.2.2 Planifications successives . . . . . . . . . . . . . . . . . . . . . . . . . 55

4.3 Tâche de parking référencé sur des amers . . . . . . . . . . . . . . . . . . . . . 564.3.1 Définitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 564.3.2 Formulation du problème . . . . . . . . . . . . . . . . . . . . . . . . . . 58

4.4 Calcul de la configuration de parking . . . . . . . . . . . . . . . . . . . . . . . . 604.4.1 Modélisation probabiliste . . . . . . . . . . . . . . . . . . . . . . . . . 604.4.2 Mise en correspondance . . . . . . . . . . . . . . . . . . . . . . . . . . 614.4.3 Mise à jour de la position de parking du capteur . . . . . . . . . . . . . . 624.4.4 Mise à jour de la configuration de parking . . . . . . . . . . . . . . . . . 644.4.5 Extension à plusieurs capteurs . . . . . . . . . . . . . . . . . . . . . . . 65

4.5 Déformation de trajectoire . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 664.5.1 Conditions aux limites . . . . . . . . . . . . . . . . . . . . . . . . . . . 66

4.6 Application au robot Hilare2 avec remorque . . . . . . . . . . . . . . . . . . . . 674.6.1 Tâche de parking : parking relativement à un quai de débarquement . . . 674.6.2 Résolution de la tâche de parking . . . . . . . . . . . . . . . . . . . . . 71

4.7 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75

5 Suivi de trajectoire, évitement d’obstacles et localisation 775.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 775.2 Suivi de trajectoire et évitement réactif d’obstacles . . . . . . . . . . . . . . . . 78

5.2.1 Suivi de trajectoire . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 785.2.2 S’arrêter sur une trajectoire . . . . . . . . . . . . . . . . . . . . . . . . . 805.2.3 Détection des obstacles . . . . . . . . . . . . . . . . . . . . . . . . . . . 835.2.4 Architecture pour le suivi de trajectoire et l’évitement d’obstacles . . . . 845.2.5 Conditions garantissant l’absence de collision lors du suivi de la trajectoire 85

5.3 Intégration de la localisation globale . . . . . . . . . . . . . . . . . . . . . . . . 885.3.1 Localisation globale . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88

TABLE DES MATIÈRES vii

5.3.2 Architecture pour le suivi de trajectoire avec localisation globale . . . . . 895.3.3 Conditions garantissant l’absence de collision pour les systèmes expo-

nentiellement stables . . . . . . . . . . . . . . . . . . . . . . . . . . . . 905.3.4 Cas réalistes avec un véhicule articulé . . . . . . . . . . . . . . . . . . . 92

5.4 Architecture pour l’intégration du suivi de trajectoire, de l’évitement d’obstacleset de la localisation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 945.4.1 Module de simulation du système . . . . . . . . . . . . . . . . . . . . . 945.4.2 Architecture pour le suivi de trajectoire . . . . . . . . . . . . . . . . . . 95

5.5 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98

6 Résultats expérimentaux 996.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 996.2 Architecture logicielle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1006.3 Portage sur plusieurs robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102

6.3.1 Le rover Dala . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1026.3.2 Le robot Cycab . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103

6.4 Parking référencé sur des amers . . . . . . . . . . . . . . . . . . . . . . . . . . 1046.4.1 Parking référencé sur des amers avec une localisation imprécise . . . . . 1056.4.2 Parking référencé sur des amers avec un motif de parking inexact . . . . 1056.4.3 Bilan . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106

6.5 Intégration de la localisation globale, du suivi de trajectoire et de l’évitementd’obstacles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1076.5.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108

7 Conclusion 109

A Preuve de l’absence de collision pour les systèmes respectant la propriété de stabilitéexponentielle 111

viii TABLE DES MATIÈRES

Chapitre 1

Introduction

1.1 Contexte

L’objet de la robotique est l’automatisation de systèmes mécaniques. En dotant le systèmede capacités de perception, d’action et de décision, l’objectif est de lui permettre d’interagirrationnellement avec son environnement, et de façon autonome.

Depuis les premiers automates jusqu’aux systèmes disponibles en ce début de XXIe siècle,on mesure tout à la fois le chemin parcouru et celui restant à parcourir avant de réaliser lesrêves qui animaient les pionniers. La robotique est un domaine de recherche qui se situe aucarrefour de l’intelligence artificielle, de l’automatique, de l’informatique et de la perception parordinateur ; cette interdisciplinarité est à l’origine d’une certaine complexité. Des applicationsdans des domaines aussi variés que l’industrie manufacturière, le spatial, l’automobile ou plusrécemment les loisirs et le secteur médical, démontrent aujourd’hui l’intérêt économique et socialde ces recherches.

La robotique mobile autonome vise plus spécifiquement à concevoir des systèmes capablesde se déplacer de façon autonome. Les applications directes se situent notamment dans les do-maines de l’automobile, de l’exploration planétaire ou de la robotique de service par exemple.De nombreuses applications restent à découvrir, qui ne découlent pas directement des avancéesde la robotique mais qui utilisent ses méthodes et ses développements.

Notre thèse a pour cadre général la navigation autonome des robots mobiles, et elle se focalisesur un type de système et d’application spécifiques : les véhicules articulés avec des applicationsdans le transport automatique.

1.2 Navigation autonome des robots mobiles

Pour exécuter une tâche de navigation autonome, un robot mobile doit mettre en oeuvre uncertain nombre de fonctionnalités, que nous détaillons ici.

2 Chapitre 1. Introduction

1.2.1 Planification de mouvementLa planification de mouvement dans sa formulation classique est le problème du calcul d’un

chemin sans collision pour un système mécanique, entre une configuration de départ et une confi-guration d’arrivée données, dans un modèle géométrique de l’environnement. Le concept d’es-pace des configurations introduit par [Lozano-Pérez ] en robotique permet de transformer leproblème de la recherche d’un chemin pour un système à n degrés de liberté dans l’espace eucli-dien en celui du mouvement d’un point dans un espace à n dimensions.

La résolution de ce problème, dit du déménageur de piano, par des méthodes exactes ([Schwartz a],[Schwartz b], [Schwartz c]), n’est pas applicable à des systèmes complexes avec de nom-breux degrés de liberté. Les méthodes effectives de planification de mouvement reposent sur desalgorithmes probabilistes qui explorent aléatoirement l’espace des configurations afin d’en ca-ractériser la connexité. On trouve une synthèse de ces techniques dans [Latombe ], ou dansdes références plus récentes [LaValle ], [Choset ].

1.2.2 LocalisationAfin d’exécuter le mouvement planifié, le robot doit se localiser dans l’environnement. La lo-

calisation est l’estimation de la position du robot par rapport à un repère fixe de l’environnement.Cette estimation de la position peut s’effectuer soit par une mesure des déplacements du robotsoit par une mesure de sa position absolue dans l’environnement. Un inventaire des techniqueset capteurs disponibles est recensé dans [Borenstein ]. L’estimation de la position absolue durobot dans l’environnement ne peut être traitée indépendamment de la construction d’une carted’amers de l’environnement [Thrun ]. Du fait des incertitudes sur les mesures utiles pour lalocalisation, le problème de la localisation est généralement modélisé dans un cadre probabiliste.

1.2.3 Suivi de trajectoireLe suivi de trajectoire consiste à calculer les commandes des actionneurs du système permet-

tant de réaliser le mouvement planifié. Un robot étant considéré comme un système dynamique,on utilise des méthodes de commande par retour d’état pour asservir le système sur une trajec-toire de référence.

1.2.4 Évitement réactif d’obstaclesLe suivi de la trajectoire planifiée ne permet pas de garantir l’absence de collision. En effet,

des collisions peuvent se produire lors de l’exécution de la trajectoire, dues à :– une localisation imparfaite,– un plan imprécis,– des obstacles qui n’étaient pas dans le modèle de l’environnement utilisé pour la planifica-

tion de trajectoire.Tout ces éléments font que le mouvement initialement planifié doit être adapté lors de son

exécution, et que des stratégies d’évitement réactif d’obstacles doivent être mises en oeuvre. On

1.3 Spécificités de notre approche 3

adoptera des stratégies différentes en fonction du type de système, de sa vitesse, et du champd’application.

1.2.5 ParkingLe parking est la phase finale de la navigation autonome. Il occupe une place particulière

pour deux raisons :– la manoeuvre de parking est en général un mouvement fortement contraint et qui requiert

une grande précision,– l’objectif d’une mission de navigation est souvent d’atteindre une configuration finale spé-

cifiée. Le succès de cette mission dépend de la réalisation de cet objectif.Par ailleurs, le parking s’effectue généralement «relativement» à des éléments de l’environne-ment, et des méthodes de mouvement référencé sur des amers doivent être utilisées.

1.2.6 Intégration des différentes fonctionnalités de la navigationL’intégration des différentes fonctionnalités de la navigation va au-delà de leur exécution

en parallèle. En effet toutes ces fonctionnalités sont liées entre elles, par des variables ou desobjectifs communs, et certaines doivent respecter des contraintes temps-réel. Ainsi la position durobot apparaît dans les fonctionnalités de localisation et de suivi de trajectoire par exemple, et laréalisation simultanée de ces tâches s’en trouve compliquée.

La conception d’une architecture prenant en compte ces interconnexions et assurant la réali-sation de chacune des tâches est une problématique à part entière.

1.3 Spécificités de notre approcheLes spécificités de notre approche sont de deux types : les spécificités liées aux systèmes

auxquels on s’intéresse et les spécificités liées aux applications.Les spécificités liées au système tout d’abord :– on s’intéresse dans notre étude aux robots mobiles à roues,– le système est soumis à des contraintes cinématiques, et les trajectoires générales pour

de tels systèmes n’ont pas d’expression simple comme des lignes droites ou des arcs decercles,

– la géométrie complète du système doit être prise en compte,– l’asservissement n’assure pas une décroissance uniforme de la distance à la consigne lors

du suivi de la trajectoire.Ainsi le type de système auquel on s’intéresse se range dans la classe des véhicules articulés, telsle robot Hilare2 et sa remorque. Les problématiques associées à de tels systèmes sont foncière-ment différentes de celles associées à des robots de type «guide de musée» par exemple (figure1.1). Ces spécificités impliquent qu’une trajectoire quelconque n’est pas nécessairement admis-sible pour le type de système auquel on s’intéresse. Ainsi par exemple un robot de type voiturene peut pas effectuer de créneau latéral sans exécuter des manoeuvres. La représentation d’un


FIG. 1.1 – À gauche le robot Hilare2 avec sa remorque, un véhicule articulé. À droite un robot«guide de musée».

chemin sous la forme d’une série de points de passage est donc exclue, et on va donc travailleravec une trajectoire de référence qu’il nous faut calculer.

Pour la sous-classe des systèmes «différentiellement plats» [Fliess ], dont le robot Hilare2(figure 1.1) avec sa remorque ou un robot de type voiture font partie, il existe des méthodepermettant de calculer analytiquement une trajectoire faisable [Rouchon ], [Lamiraux a],[Lamiraux ]. Pour d’autres systèmes on doit employer des méthodes numériques. Dans notretravail on utilise le logiciel Move3D [Siméon ] pour planifier une trajectoire sans collisiondans un modèle de l’environnement. Dans la suite de ce document, on supposera que l’on possèdedéjà une telle trajectoire de référence, que le robot doit ensuite exécuter.

Les spécificités de notre approche sont aussi liées aux applications envisagées :

– on souhaite pouvoir naviguer proche des obstacles,– le rapport entre la taille du système et la précision désirée est élevé,– on est amené à effectuer des manoeuvres,– les mouvements ne sont pas nécessairement réalisés à des vitesses élevées,– les obstacles sont supposés statiques.

Les applications directes de notre travail sont par exemple l’automatisation de véhicules lourds,l’aide à la manoeuvre pour des camions, ou encore le parking automatique de véhicules articulés.

Le cadre général de notre travail est donc celui du «véhicule intelligent», mais nous mettonsl’accent sur le fait que les spécificités de notre approche entraînent des problématiques singu-lières, et requièrent des méthodes de résolution originales. Ainsi le parking automatique d’uncamion et l’automatisation d’une voiture sur autoroute ne posent par exemple pas les mêmesproblèmes.


1.3.1 Nos contributions

Nos contributions portent sur la résolution de certaines fonctionnalités de la navigation au-tonome des robots mobiles dans le cadre défini par ces spécificités. Le fil conducteur de cedocument est l’idée que ces spécificités posent des problématiques singulières, pour lesquellesles méthodes classiques développées pour des systèmes plus simples sont inadéquates.

Perception

Loi de commande

Modèle cinématique

Modèle de l’environnement

Trajectoire

Suivi de trajectoire

Localisation

Carte

Motif de parking

Évitement d’obstacles

Parking

Exécution de trajectoire

Planification de trajectoire

FIG. 1.2 – Fonctionnalités de la navigation autonome. Nos contributions théoriques sont repré-sentées en gras.

Le diagramme 1.2 résume les différentes fonctionnalités nécessaires à la navigation autonomeet permet de situer nos contributions, représentées en gras, qui résident dans le développementde méthodes originales de résolution de certaines fonctionnalités et dans l’intégration et l’implé-mentation de l’ensemble.

Notre méthodologie repose sur deux principes. Tout d’abord la volonté de développer desméthodes génériques, qui s’appliquent à une classe de systèmes plutôt qu’à un robot en particu-lier. Une conséquence intéressante est que des méthodes développées pour résoudre un problèmedans un contexte applicatif particulier trouvent parfois des applications dans des domaines trèsdifférents, du fait de leur généricité. Ensuite le second principe est l’implémentation sur des sys-tèmes réels, afin de se confronter à des problèmes réels. Les motivations de ce second principesont développées dans le chapitre 6.

Nos recherches contiennent donc à la fois des contributions théoriques et expérimentales. Surle plan théorique :


– nous avons contribué au développement d’une méthode de déformation de trajectoire poursystèmes nonholonomes, et nous avons proposé plusieurs extensions et optimisations deson algorithme,

– nous avons développé une méthode de parking référencé sur des amers pour des systèmesnonholonomes,

– nous avons proposé une architecture permettant d’intégrer les différentes fonctionnalitésde la navigation autonome.

Sur le plan expérimental :– nous avons implémenté ou adapté les différentes fonctionnalités de la navigation auto-

nome pour trois robots évoluant dans des environnements différents : le robot Hilare2 et saremorque (figure 1.1), le rover Dala et le Cycab de l’INRIA Rhône-Alpes (figure 1.3),

– nous avons réalisé des expérimentations avec ces robots, qui valident notre approche.

FIG. 1.3 – Les robots Dala et Cycab, supports de nos expérimentations.

À l’issue de notre travail nous proposons donc un ensemble de techniques qui permettent derésoudre le problème de la navigation autonome pour des véhicules articulés en environnementfortement contraint. Ces méthodes ont été implémentées et expérimentées sur des robots réels.

1.3.2 Plan de lecture

Le chapitre 2 présente quelques notations et définitions utilisées tout au long de ce document,qui précisent le type de systèmes auxquels on s’intéresse.

Le chapitre 3 traite de l’évitement réactif d’obstacles. Nous présentons une méthode originaled’évitement d’obstacles pour des systèmes nonholonomes. Nous proposons plusieurs extensionsde cette méthodes pour des applications différentes, et présentons des optimisations de son algo-rithme.

Ensuite le chapitre 4 traite de la fonctionnalité de parking et propose une technique de parkingréférencé sur des amers pour des systèmes nonholonomes.

Le chapitre 5 présente l’intégration formelle des différentes fonctionnalités de l’exécutiond’une trajectoire (voir la figure 1.2). On s’intéresse plus particulièrement à l’intégration de lalocalisation, du suivi de trajectoire et de l’évitement d’obstacles, en tenant compte des spécificitésde notre approche.


Enfin le chapitre 6 expose des résultats expérimentaux de navigation autonome obtenus surdes robots réels.

Chapitre 2

Notations et définitions

Nous présentons dans ce chapitre les notations et définitions employées tout au long de cedocument.

2.1 Espaces et positionsEspace de travail

L’espace de travail notéW est dans notre étude l’espace euclidien à deux dimensions, assi-milé à R2. Un corps rigide dans cet espace occupe dans une configuration dite de référence unsous-espace B ⊆ W . On définit un repère fixe de W que l’on note O. La situation d’un corpsdans l’espace est alors donnée par un mouvement rigide x ∈ SE(2), définissant la position etl’orientation du corps relativement à O. L’espace occupé par le corps dans la situation x estnoté x(B).

L’ensemble des résultats que nous présentons est généralisable à SE(3).

Robot

Un robot R est une chaîne cinématique composée de corps rigides reliés entre eux pardes articulations. On considère que cette chaîne cinématique possède un corps racine, dont onnote x ∈ SE(2) la position dans l’espace de travail.

Espace des configurations

L’ensemble des variables permettant de repérer la position de chacun des corps du robot dansl’espace est appelé une configuration et est noté q. L’ensemble des configurations du robot estl’espace des configurations C, qui a une structure de variété différentielle. On note n la dimensionde C, et on identifie localement l’espace des configurations à Rn.

On note qint ∈ Cint les variables de configuration internes exprimées relativement au corpsracine, lorsque celui-ci est dans la configuration de référence. On a alors :

q = (x,qint) ∈ CS = SE(2)× Cint

10 Chapitre 2. Notations et définitions

où x est le mouvement rigide appliqué à tous les corps du robot.

Transformation solide

À tout élément m de SE(2), on associe une application de C dans C que l’on note de la mêmefaçon m :

m : SE(2)× Cint → SE(2)× Cint

(x,qint) 7→ m.q = (m.x,qint)

L’application m déplace le corps racine d’un robot et garde les variables internes inchangées.

2.2 DistancesDistance entre deux configurations

La distance dC entre deux configurations q1 et q2 est définie comme la distance maximaleentre deux points du robot dans les configurations q1 et q2. Soit p un point du robot, et q(p) laposition de ce point dans W lorsque le robot est dans la configuration q. La distance entre q1

et q2 est :dC(q1,q2) = max

p∈R‖q1(p)− q2(p)‖ (2.1)

Distance entre deux positions

Soit x1 et x2 deux éléments de SE(2). On définit la distance entre x1 et x2 :

dSE(2)(x1,x2) = maxqint∈Cint

dC((x1,qint), (x2,qint)) (2.2)

Distance entre deux corps

SoitA et B deux sous-espaces compacts deW , représentant deux corps. On appelle distanceentre ces deux corps la valeur :

d(A,B) = mina∈A,b∈B

‖a− b‖ (2.3)

Cette mesure n’est pas une distance au sens mathématique. On remarque en effet par exempleque si A ⊂ B, alors d(A,B) = 0, sans impliquer l’égalité entre les deux sous-espaces A et B.On vérifie cependant aisément que cette mesure respecte l’inégalité triangulaire.

Distance entre une configuration et un corps

Si l’on note O un sous-espace de W , et q une configuration du robot, on appelle distanceentre q et O la valeur :

dO(q,O) = minp∈R,o∈O

‖q(p)− o‖ (2.4)

2.3 Système dynamique 11

Dans le cas d’un robot composé de nb corps B1,B2, . . . ,Bnb, cette distance est la valeur minimale

de la distance de chacun des corps dans la configuration q au corps O :

dO(q,O) = mini∈{1,...,nb}

d(Bi(q),O)

où Bi(q) est le sous-espace occupé par le corps Bi quand le robot est dans la configuration q. Ladistance dO est utile pour exprimer la distance entre le robot et un obstacle, mais on remarquequ’elle n’est pas une distance au sens mathématique. Elle respecte cependant la propriété suivantequi correspond à l’inégalité triangulaire :

Propriété 1. ∀q1,q2 ∈ C, ∀O ⊂ W

dO(q1,O) ≥ dO(q2,O)− dC(q1,q2)

Démonstration. Si dO(q2,O) ≤ dC(q1,q2), alors dO(q1,O) étant positif, la conclusion est im-médiate. Sinon on suppose que dO(q2,O) ≥ dC(q1,q2) et alors on a :∀q1,q2 ∈ C,∀o ∈ O,∀p ∈ R

‖q1(p)− o‖ = ‖q1(p)− q2(p) + q2(p)− o‖

Et par inégalité triangulaire :

‖q1(p)− o‖ ≥∣∣∣‖q1(p)− q2(p)‖ − ‖q2(p)− o‖

∣∣∣ (2.5)

Étant donné que dO(q2,O) ≥ dC(q1,q2), on a :

minp∈R,o∈O

‖q2(p)− o‖ ≥ maxp∈R‖q1(p)− q2(p)‖

Ce qui implique que le terme dans la valeur absolue du membre de droite de l’équation (2.5) estnégatif. Ainsi ∀q1,q2 ∈ C,∀o ∈ O,∀p ∈ R :

‖q1(p)− o‖ ≥ ‖q2(p)− o‖ − ‖q1(p)− q2(p)‖‖q1(p)− o‖ ≥ min

p∈R,o∈O‖q2(p)− o‖ −max

p∈R‖q1(p)− q2(p)‖

Et par définition :max

p∈R,o∈O‖q1(p)− o‖ ≥ ‖q1(p)− o‖

d’où le résultat.

2.3 Système dynamiqueOn considère maintenant le robot comme un système dynamique. Tout système abordé dans

ce document respecte les propriétés suivantes.


Trajectoire

Une trajectoire de ce système est une fonction continue de [0, S] ⊂ R dans C, qui à touteabscisse s ∈ [0, S] associe une configuration :

q : [0, S] → Cs 7→ q(s)

La variable s n’est donc pas une abscisse curviligne.

Contrainte nonholonome

Le système est soumis à des contraintes de «roulement sans glissement». Celles-ci s’ex-priment comme des contraintes linéaires sur les vitesses, que l’on peut mettre sous la forme :

G(q)q′ = 0 (2.6)

où G est une fonction non-intégrable, c’est à dire qu’il n’existe pas de fonction F telle que∂F (q)

∂q= G(q). On note m le rang de l’application linéaire G(q). Ce type de contraintes qui

réduit l’espace des vitesses accessibles par le système sans réduire son espace accessible estappelé contrainte nonholonome. Alors l’équation (2.6) exprime l’appartenance de q′, la dérivéede q, au noyau de G(q) :

q′ ∈ Ker(G)

La dimension de Ker(G(q)) est (n−m), avec n = dim C.

Champs de vecteurs

Les vitesses du système soumis aux contraintes de l’équation (2.6) appartiennent donc à unespace de dimension k = (n −m). On note (X1(q),X2(q), . . . ,Xk(q)) k champs de vecteursformant une base de Ker(G(q)).

Trajectoire admissible pour un système sans dérive

Le système est sans dérive et une trajectoire admissible pour le système est telle qu’il existeun vecteur u = (u1, ..., uk) de k applications continues de [0, S] dans R, tel que :

∀s ∈ [0, S], q′(s) =k∑

i=1

ui(s)Xi(q(s)) (2.7)

Symétrie par rapport à SE(2)

On suppose que les vitesses suivant les champs de vecteurs du système sont indépendantesdu repère choisi. C’est à dire que les champs de vecteurs du système respectent la propriétésuivante :

2.3 Système dynamique 13

Propriété 2. ∀m ∈ SE(2),∀q ∈ C, en notant Tqm l’application linéaire tangente à m en q, ona :

X(m.q) = TqmX(q)

On en déduit le corollaire suivant pour les trajectoires du système :

Corollaire 1. ∀s ∈ [0, S], si q(s) est la configuration à l’abscisse s sur la trajectoire obte-nue avec les commandes u(s) depuis q(0), alors en appliquant les mêmes commandes depuism.q(0), à l’abscisse s, le système est à la configuration m.q(s).

Symétrie par changement d’échelle du temps

Le système étant sans dérive, on a la propriété suivante :

Propriété 3. Si (q(s),u(s)) est une trajectoire admissible solution de (2.7) définie sur [0, S],alors pour tout difféomorphisme φ de classe C1 de [0, S] dans [0, Sφ], la trajectoire :

(q(φ(s)), φ′(s)u(s))

est une trajectoire admissible sur [0, Sφ].

Chapitre 3

Évitement réactif d’obstacles pour robotsmobiles nonholonomes

Ce chapitre traite de la fonctionnalité d’évitement réactif d’obstacles. Tout d’abord nous pré-sentons un état de l’art des méthodes d’évitement réactif d’obstacles, et nous montrons leurinadéquation aux spécificités de notre problématique. Ensuite nous proposons une méthode ori-ginale d’évitement réactif d’obstacles pour des robots mobiles nonholonomes. Cette méthodeutilise une trajectoire de référence qui est déformée de façon à s’éloigner des obstacles et à satis-faire les contraintes cinématiques du système. Enfin nous présentons quelques optimisations del’algorithme de cette méthode et des extensions de ses applications.

3.1 État de l’art

La plupart des méthodes d’évitement réactif d’obstacles sont des méthodes locales. En effetles méthodes globales, qui considèrent un modèle complet de l’environnement, se ramènent auproblème de la planification de mouvement. Et pour des systèmes possédant de nombreux degrésde liberté (plus de 3), la complexité de la planification d’une trajectoire interdit son utilisation encours d’exécution. Par exemple l’approche de replanification rapide D∗ de [Stentz ] découpel’espace de travail en cellules étiquetées «libre» ou «occupée», pour diminuer la complexité.

Pour les systèmes considérés dans notre étude, tels que nous les avons définis au chapitre 2, laplanification d’une trajectoire sans collision dans un environnement fortement contraint est trèscoûteuse en temps de calcul. Cette étape est donc réalisée «hors-ligne» et on utilise les techniquesd’exploration aléatoire de l’espace des configurations mentionnées dans le chapitre d’introduc-tion (Probabilistic Roadmap [Kavraki ] et Rapidly-Exploring Random Tree [LaValle ]) pourcalculer ces trajectoires. Le modèle de l’environnement utilisé est construit auparavant par unautre robot ou à partir d’un plan métrique. On emploie une méthode de guidage (calcul d’une tra-jectoire admissible entre deux configurations sans prise en compte des obstacles) pour le systèmeconsidéré, si elle existe, pour connecter les configurations tirées aléatoirement. L’ensemble de cesfonctionnalités est intégré dans le logiciel de planification de mouvement Move3D [Siméon ]que l’on utilise dans notre étude.

16 Chapitre 3. Évitement réactif d’obstacles pour robots mobiles nonholonomes

On supposera donc toujours que l’on possède une trajectoire de référence calculée dans un mo-dèle de l’environnement.

Parmi les méthodes locales d’évitement réactif d’obstacles, on peut distinguer deux catégo-ries : les méthodes utilisant une trajectoire de référence et celles n’en utilisant pas. Le type desystème et d’application orientent le choix vers une catégorie ou l’autre.

Enfin un autre type de méthodes doit être mentionné, qui tient compte explicitement de lavitesse estimée des obstacles pour réaliser un mouvement sans collision, ce qui définit une nou-velle problématique. Nous renvoyons à [Fraichard ] pour un cadre formel et un état de l’art decette problématique. Dans notre étude nous considérons que les obstacles sont statiques.

3.1.1 Méthodes d’évitement réactif d’obstacles sans trajectoire de réfé-rence

Pour certains systèmes et dans certains contextes applicatifs, la définition d’une trajectoirecomme une série de points de passage est possible. Des méthodes d’évitement réactif d’obstaclesont été développées qui calculent des commandes permettant de rejoindre un point de passage enévitant les collisions avec les obstacles détectés.

Champs de potentiel

Les méthodes de champs de potentiel pour la navigation en robotique, initialement proposéespar [Khatib ] pour un bras manipulateur, consistent à construire une fonction de potentielqui résume les objectifs de la navigation : éviter les obstacles (potentiel répulsif) et atteindreune configuration but (potentiel attractif). À chaque position du robot, une «force» résultant del’action conjuguée des obstacles et du but est calculée, qui correspond à une direction à suivrepar le robot.

De nombreuses adaptations de cette technique ont été proposées. On peut par exemple citer[Borenstein ], qui calcule une direction de mouvement à partir d’informations proximétriques.

Ces méthodes purement réactives sont sujettes à des minima locaux, et peuvent nécessiterune replanification globale. Par ailleurs, elles peuvent entraîner un mouvement oscillatoire durobot dans certaines situations (des passages étroits par exemple).

Steering Angle Field (SAF)

Cette approche a été proposée par [Feiten ] pour un robot de type unicycle. Il s’agit decalculer pour un ensemble de vitesses linéaires des domaines de l’angle de braquage (SAF) quin’entraînent pas de collision avec les obstacles perçus. Cette méthode utilise une discrétisation engrille du plan de travail autour du robot. Elle exploite la possibilité de pré-calculer et de stockerdans des tables de recherche les SAF pour chaque cellule obstacle de la grille.

3.1 État de l’art 17

Dynamic Window

Cette technique proposée dans [Fox ] travaille dans l’espace des commandes du robot. Lataille du domaine de recherche des vitesses accessibles (c’est à dire n’entraînant pas de colli-sions) est réduite par la prise en compte explicite de la dynamique (les capacités d’accélération)du système. Les commandes envoyées au robot sont le résultat de la maximisation sur ce do-maine de recherche d’une fonction de coût liée à la position but. Cette méthode a été développéepour des robots se déplaçant à des vitesses élevées (1m.s−1) dans des environnements intérieursencombrés.

Nearness Diagram

Cette approche proposée par [Minguez ] repose sur un diagramme de proximité des obs-tacles, mis à jour au fur et à mesure du déplacement du robot. En fonction de l’allure de ce dia-gramme (nombre de passages sans obstacles, taille des passages, etc.), un comportement adapté(exploration, avancement vers le but, retour en arrière, etc.) est sélectionné. La direction de mou-vement la plus prometteuse par rapport au but est alors choisie. Cette méthode a été appliquéeà un véhicule de type unicycle grâce à une expression des obstacles dans l’espace des pointsaccessibles en un mouvement élémentaire (un arc de cercle en l’occurence), obtenue par unetransformation dans l’espace dit «Ego-cinématique» [Minguez ].

L’exposé de ces différentes méthodes et de leurs hypothèses fait immédiatement apparaîtreleur inadéquation à des systèmes à cinématique plus complexe évoluant dans des environne-ments fortement contraints. On ne peut en effet généralement pas présupposer de la forme destrajectoires. De même les discrétisations en grille de l’espace de travail, proposées par certainesméthodes, ne permettent pas les mouvements arbitrairement proches des obstacles.

3.1.2 Méthodes d’évitement réactif d’obstacles avec une trajectoire de ré-férence

Dès lors que la cinématique du système est plus complexe, le calcul préalable d’une trajec-toire de référence qui sera adaptée lors de l’exécution s’avère nécessaire. Nous présentons deuxméthodes utilisant une trajectoire de référence pour l’évitement réactif d’obstacles.

Trajectoires d’évitement

Une méthode proposée dans [Laugier ] repose sur l’enchaînement de trajectoires élémen-taires (Sensor Based Maneuvers SMB), dans le contexte d’une voiture automatisée roulant surune route. Les SMB sont par exemple le suivi d’un couloir de route, le changement de file oule parking parallèle. Les paramètres de chacune de ces modalités sont définis par les informa-tions sensorielles. Un contrôleur, amélioré dans [Large ] par un réseau de neurones, permetd’enchaîner ces différentes modalités.


Bande élastique

La méthode proposée par Quinlan [Quinlan ] utilise une trajectoire initialement planifiéequi est représentée par une série de boules adjacentes (appelées «bulles») dans l’espace des confi-gurations. Le rayon d’une boule centrée en une configuration est la distance de cette configura-tion à l’obstacle le plus proche. Ainsi une trajectoire est sans collision dès lors que les «bulles»qui la composent se recouvrent. Développée pour des systèmes sans contraintes cinématiques,cette technique considère la trajectoire comme une bande élastique, se modifiant sous l’action deforces répulsives générées par les obstacles, et de forces internes de contraction ou d’élasticité.Du fait de la représentation en bulles, la mise à jour de la trajectoire sous l’action des forces esttrès rapide.

Cette technique a été étendue à un robot de type voiture par [Khatib ]. La «forme» des«bulles» est ici donnée par la métrique des trajectoires de Reeds et Shepp (combinaisons d’arcsde cercle et de lignes droites) [Soueres ]. Le lissage de la courbe joignant les centres des«bulles» est réalisé en utilisant une courbe de Bézier.

Mais pour certains systèmes on ne connaît pas la plus courte distance entre une configurationet un obstacle. La méthode de la bande élastique ne peut donc s’appliquer qu’au prix d’approxi-mations sur la forme des trajectoires de tels systèmes, ce qui rend impossible la navigation enenvironnement très contraint.

L’ensemble de ces considérations, concernant les limitations des méthodes utilisant une tra-jectoire de référence ou des techniques proposées pour les systèmes à cinématique plus simple,motive le développement d’une méthode générique d’évitement d’obstacles pour systèmes non-holonomes.

3.2 Déformation de trajectoire pour systèmes nonholonomesNous présentons dans cette section la méthode de déformation de trajectoire développée pour

l’évitement réactif d’obstacles pour des systèmes nonholonomes, publiée dans [Lamiraux ].Comme nous l’avons déjà mentionné, on suppose que l’on possède une trajectoire sans col-

lision planifiée dans un modèle de l’environnement. Au cours de l’exécution de cette trajectoire,le robot est capable de percevoir les obstacles, et doit éventuellement adapter réactivement cettetrajectoire de référence pour éviter les collisions détectées.

La méthode consiste à perturber les entrées du système de façon à obtenir une trajectoiredéformée qui s’éloigne des obstacles. Tous les éléments théoriques développés ici seront repris àtravers un exemple dans la section 3.3.

3.2.1 Déformation de trajectoire

Une trajectoire admissible définie sur un intervalle est déterminée par une configuration ini-tiale q(0) et par des fonctions d’entrée (u1, ..., uk) définies sur cet intervalle. Si l’on perturbe cesentrées, on obtiendra par intégration du système (2.7) une nouvelle trajectoire admissible.

3.2 Déformation de trajectoire pour systèmes nonholonomes 19

Pour caractériser la perturbation des entrées, on introduit la variable τ ∈ [0, +∞[, et on noteu(s, τ) la nouvelle entrée du système. On définit alors un faisceau de trajectoire comme unefamille de trajectoires indexée par le réel τ . Un faisceau de trajectoire peut être considéré commeune fonction de deux variables réelles à valeurs dans l’espace des configurations :

q : [0, S]× [0, +∞[ → C(s, τ) 7→ q(s, τ)

Et pour chaque valeur de τ , on définit la fonction partielle s 7→ qτ (s) = q(s, τ). Ainsi qτ (s) estla trajectoire d’indice τ , dont les fonctions d’entrée sont s 7→ u(s, τ). Afin d’alléger la notation,on utilise donc la même écriture pour désigner une configuration, une trajectoire et un faisceaude trajectoire. La trajectoire q0 est donc la trajectoire initiale du système et qτ est une trajectoiredéformée.

Un faisceau de trajectoire admissible est tel que : ∀(s, τ) ∈ [0, S]× [0, +∞[

∂q

∂s(s, τ) =

k∑i=1

ui(s, τ)Xi(q(s, τ)) (3.1)

On exprime alors la variation de q(s, τ) en fonction de la variation du paramètre τ , en dérivantl’équation (3.1) par rapport à τ :

∂2q

∂s∂τ(s, τ) =

k∑i=1

(∂ui

∂τ(s)Xi(q(s, τ)) +

ui(s, τ)∂Xi

∂q(q(s, τ))

∂q

∂τ(s, τ)

)(3.2)

On introduit alors les notations suivantes :

perturbation d’entrée : v(s, τ) ,∂u

∂τ(s, τ)

direction de déformation : η(s, τ) ,∂q

∂τ(s, τ)

Ainsi l’équation (3.2) s’écrit :

η′(s, τ) = A(s, τ)η(s, τ) + B(s, τ)v(s, τ) (3.3)

où A(s, τ) est la matrice de taille (n× n) :

A(s, τ) =k∑

i=1

ui(s, τ)∂Xi

∂q(q(s, τ))

=k∑

i=1

ui(s, τ)

∂X1

i

∂q1 . . .∂X1

i

∂qn

......

∂Xni

∂q1

∂Xni

∂qn


et B(s, τ) est la matrice de taille (n×k) dont les colonnes sont les champs de vecteurs du systèmeévalués en q(s, τ) :

B(s, τ) =

(X1(q(s, τ)) · · ·Xk(q(s, τ))

)Ainsi, à partir de la trajectoire q(s, τ), des entrées u(s, τ), des perturbations des entrées

v(s, τ) et de la condition initiale η(0, τ), on peut calculer la direction de déformation correspon-dante par intégration de (3.3). Et réciproquement, toute direction de déformation admissible estsolution de (3.3).

Le système (3.3) est le linéarisé tangent autour de la trajectoire qτ du système dynamiqued’équation (2.7). On peut vérifier qu’en intégrant le système (3.3), on obtient l’expression sui-vante :

η(s, τ) = H(s, τ)

∫ s

0

H−1(u, τ)B(u, τ)v(u, τ)du

avec H(s, τ) la matrice de taille n× n vérifiant :

H(0, τ) = In

H ′(s, τ) = A(s, τ)H(s, τ)

Le système (3.3) est donc linéaire par rapport à la perturbation v des fonctions d’entrée.

3.2.2 S’éloigner des obstacles

Étant donné que l’on cherche à déformer une trajectoire initiale de façon à s’éloigner des obs-tacles, on suppose qu’au cours de l’éxecution de la trajectoire, le robot est capable de percevoirles obstacles et de calculer leurs interactions avec la trajectoire. Et on exprime ces interactionsau moyen d’un potentiel dans l’espace des configurations.

Potentiel d’une trajectoire

Soit U(q) le potentiel d’une configuration :

U : C → Rq 7→ U(q)

La fonction U est définie de façon à ce que le potentiel d’une configuration soit élevé lorsquele sous-espace occupé par le robot dans cette configuration est proche des obstacles, et qu’ildiminue lorsqu’il s’en éloigne (la section 3.4 détaille l’expression de cette fonction).

On note alors V (τ) le potentiel d’une trajectoire d’indice τ :

V (τ) =

∫ S

0

U(q(s, τ))ds


La variation de ce potentiel est :

dV

dτ(τ) =

∫ S

0

∂U

∂q(q(s, τ))

∂q(s, t)

∂τ

=

∫ S

0

∂U

∂q(q(s, τ))η(s, τ)ds (3.4)

où ∂U∂q

(q(s, τ)) est un vecteur ligne.Si on identifie l’espace des configurations C à Rn et si on suppose que Rn est muni de la base

canonique et du produit scalaire :

∀u,v ∈ Rn, (u|v) = uT .v

on définit alors le produit scalaire dans L2 entre deux fonctions d’une variable réelle à valeursdans Rn par :

(f |g)L2 =

∫ S

0

f(s)T .g(s)ds

Ainsi le membre de droite de l’équation (3.4) est le produit scalaire des fonctions : ∂U∂q◦ qτ :

s 7→ ∂U∂q

(q(s, τ)) et ητ : s 7→ η(s, τ) :

dV

dτ(τ) =

(∂U

∂q◦ qτ

∣∣∣ητ

)L2

On donne la propriété suivante du produit scalaire :

Propriété 4. Soient f et g deux fonctions C∞ d’une variable réelle à valeurs dans Rn. On a :

min‖f‖L2=1

(f |g)L2 = − g

‖g‖L2

Donc à norme L2 constante, la direction de déformation ητ = −∂U∂q◦ qτ est celle qui mini-

mise dVdτ

. Cependant rien ne garantit que cette déformation soit admissible, c’est à dire qu’ellesoit solution de (3.3).

Direction de déformation admissible

Le sous-espace vectoriel des directions de déformation admissibles d’une trajectoire qτ estnoté AD(qτ ) :

AD(qτ ) = {η ∈ C∞([0, S], Rn) | ∃v, η0,

∀s ∈ [0, S] η(s) = A(s, τ)η(s) + B(s, τ)v, η(0) = η0}

On donne la propriété suivante :


Propriété 5. Soient f et g deux fonctions de C∞([0, S], Rn) et soit D un sous-espace de C∞([0, S], Rn).On a :

min‖f‖L2=1,f∈D

(f |g)L2 = − pD(g)

‖pD(g)‖L2

où pD() est l’opérateur de projection sur D.

Donc une solution pour obtenir une direction de déformation admissible qui fasse décroîtrele potentiel dV

dτ(τ) à norme L2 constante pourrait être de projeter orthogonalement −∂U

∂q◦ qτ sur

le sous-espace AD(qτ ). En notant η⊥τ cette projection, on a pour toute direction de déformationadmissible η ∈ AD(qτ ) l’équation :(

−(

∂U

∂q◦ qτ

)− η⊥τ

∣∣∣η)L2

= 0

Cependant rien ne garantit que la projection orthogonale sur cet espace existe et nous n’avonspas pu caractériser ainsi la projection orthogonale sur l’espace des directions de déformationadmissibles.

Pour résoudre ce problème nous allons approximer l’espace des directions de déformationadmissibles par un sous-espace de dimension finie.

3.2.3 Calcul d’une déformation admissible s’éloignant des obstaclesOn cherche à calculer une direction de déformation ητ admissible et qui fasse décroître le

potentiel de la trajectoire. Pour cela on va se donner une base d’un sous-espace vectoriel dedimension finie de l’espace des directions de déformation admissibles et on va projeter orthogo-nalement −∂U

∂q◦ q sur l’espace engendré par cette base.

Base de fonctions de perturbations

L’espace des fonctions de perturbations des entrées v(s, τ) est, à τ fixé, l’espace des fonctionscontinues de [0, S] dans Rk. Cet espace est de dimension infinie et pour l’approximer on se donneune base de p fonctions continues notées ei :

ei : [0, S] → Rk

s 7→ ei(s)(3.5)

Chaque ei est dénommée fonction de perturbation élémentaire. En pratique, plusieurs choixsont possibles pour ces fonctions : les premiers termes d’une série de Fourier, des séries depolynômes, de sinus, de fonctions triangulaires, etc. On définit une perturbation d’entrée vτ :s 7→ vτ (s), où τ est un réel fixé, comme une combinaison linéaire des fonctions de perturbationsélémentaires. Soit λ ∈ Rp, on a alors :

vτ =

p∑i=1

λiei


Base E de directions de déformation

On note Ei la solution de l’équation différentielle (3.3) avec pour condition initiale ητ (0) = 0et pour fonction d’entrée ei. Ei est donc solution du système :

E′i(s, τ) = A(s, τ)Ei(s, τ) + B(s, τ)ei(s) (3.6)

Ei(0, τ) = 0

Chaque fonction Ei est dénommée direction de déformation élémentaire. On remarque que lesEi dépendent de qτ et de uτ par le biais des matrices A et B, et qu’il faut donc les recalculerpour chaque trajectoire.

Étant donné que le système (3.3) est linéaire par rapport à la perturbation vτ , on a :

ητ =

p∑i=1

λiEi

On noteADp(qτ ) le sous-espace deAD(qτ ) engendré par les fonctions Ei, et E = {E1, . . . ,Ep}est alors une base de ADp(qτ ). Le vecteur λ = (λ1, . . . , λp)

T représente donc les coordonnéesde ητ dans la base E .

Calcul d’une direction de déformation

Si l’on se rappelle que l’on cherche une direction de déformation admissible ητ ∈ ADp(qτ ) etqui minimise (∂U

∂q◦qτ |ητ )L2 , on comprend qu’il faut maintenant projeter orthogonalement−(∂U

∂q◦

qτ ) sur ADp(qτ ). Pour cela on doit d’abord se donner une base orthonormée pour le produitscalaire L2 de ce sous-espace. On construit cette base orthonormée en utilisant le procédé d’or-thonormalisation de Gram-Schmidt à partir de la base E .

Soit F = {F1, . . . ,Fp} cette base orthonormée, et soit P la matrice de passage triangulairesupérieure de taille (p × p) de E à F . La ieme colonne de P est formée des coordonnées de Fi

exprimé dans la base E . On a Fi =∑p

j=1 PjiEj . Soit alors η un élément de ADp(qτ ), et λF sescoordonnées dans la base F . Les coordonnées λE de η dans la base E sont :

λE = PλF

Soit η une fonction appartenant à C∞([0, S], Rn). La projection orthogonale de η surADp(qτ )est donnée par :

pADp(η) =

p∑i=1

(Fi|η)L2Fi

où les (Fi|η)L2 sont les coordonnées de pADp(η) dans la base F .Alors, si on note η0

τ la projection orthogonale de −(∂U∂q◦ qτ ) sur ADp(qτ ), on peut écrire :

η0τ =

p∑i=1

λ0Fi Fi (3.7)

avec λ0Fi = −

(∂U

∂q◦ qτ

∣∣∣Fi

)L2

(3.8)


Pour une valeur τ donnée, on définit µFi =

∫ S

0∂U∂q

(q(s, τ))Fi(s, τ)ds. On a donc :

λ0Fi = −µF

i (3.9)

et λ0F = (λ0F1 , . . . , λ0F

p )T ∈ Rp est le vecteur des coordonnées dans la base F de la direction dedéformation admissible cherchée η0

τ .En reprenant l’équation (3.4), l’expression de la variation du potentiel de la trajectoire en-

gendrée par la direction de déformation η0τ s’écrit :

dV

dτ=

(∂U

∂q◦ qτ

∣∣∣ p∑i=1

λ0Fi Fi

)L2

=

p∑i=1

λ0Fi

(∂U

∂q◦ qτ

∣∣∣Fi

)L2

=

p∑i=1

λ0Fi µF

i

= −‖µF‖2 ≤ 0

La direction de déformation η0τ fait donc baisser le potentiel de la trajectoire.

Remarquons enfin que l’écriture ητ =∑p

i=1 λFi Fi des éléments de ADp(qτ ) dans la base F

définit implicitement un isomorphisme entre ADp(qτ ) et Rp. L’image par cet isomorphisme duproduit scalaire dans L2 entre des éléments deADp(qτ ) est le produit scalaire canonique dans Rp

entre leurs vecteurs de coordonnées.

Expression dans la base E

L’intérêt d’exprimer les coordonnées de cette direction de déformation dans la base E appa-raîtra plus clairement lors du résumé de l’algorithme de la déformation à la section 3.2.7. Onverra en effet que l’expression de tous les éléments de la déformation dans la base E permet deréduire le nombre d’opérations, et donc le temps de calcul. Car ce sont les Ei qui sont d’abordcalculés, par intégration du système (3.6), et le calcul des Fi n’est donc pas nécessaire.

On note :

µEj =

∫ S

0

∂U

∂q(q(s, τ))Ej(s, τ)ds (3.10)

En reprenant alors l’équation (3.8), on a :

λ0Fi = −

(∂U

∂q◦ qτ

∣∣∣Fi

)L2

= −p∑

j=1

Pji

(∂U

∂q◦ qτ

∣∣Ej

)L2

= −P T µE(τ)


On en déduit :

λ0Ei = Pλ0F

i = −PP T µE

les λ0Ei étant alors les coordonnées de la direction de déformation η0

τ dans la base E .

3.2.4 Prise en compte des conditions aux limites

On a donc trouvé une direction de déformation admissible η0τ qui fait baisser le potentiel de

la trajectoire. On doit maintenant s’assurer que cette direction de déformation respecte certainesconditions aux bornes de l’intervalle de déformation. En effet, le processus de déformation detrajectoire n’agit pas sur tout l’intervalle de définition de la trajectoire, mais seulement sur unsous-intervalle centré autour de l’abscisse de collision. Or on impose comme contrainte que latrajectoire après déformation reste inchangée aux bornes de l’intervalle de déformation.

Si l’on note I = [0, S] l’intervalle de déformation, ces contraintes s’expriment par les condi-tions aux limites suivantes :∀τ ∈ R

ητ (0) = 0 (3.11)ητ (S) = 0 (3.12)

Par construction des directions de déformation élémentaire (équation (3.6)), la condition (3.11)est toujours respectée. Par contre, rien ne garantit que la condition (3.12) soit respectée par la di-rection de déformation η0

τ trouvée à l’équation (3.7).En notant L la matrice de taille (n × p) avec p > n, dont les colonnes sont les vecteurs

Fi(S, τ), la condition (3.12) s’écrit comme une contrainte linéaire sur le vecteur λF :

LλF = 0 (3.13)

À partir de notre solution initiale notée λ0F , nous cherchons donc λF solution de :

minLλF =0

‖λF − λ0F‖

La solution de problème est à nouveau donné par la projection orthogonale de η0τ sur le sous-

espace des directions de déformation deADp(qτ ) qui vérifient ητ (S) = 0. Cela revient à projeterorthogonalement λ0F sur le sous-espace de Rp défini par l’équation (3.13). On trouve :

λF = (Ip − L+L)λ0F (3.14)

où L+ est la pseudo-inverse de L, c’est a dire la matrice vérifiant LL+L = L. Dans le cas où(LLT ) est inversible, on a L+ = LT (LLT )−1.


Vérification de la solution trouvée

Montrons que λF est bien la solution cherchée. On note ητ la direction de déformation dontles coordonnées dans la base F sont λF :

ητ =

p∑i=1

λFi Fi

Tout d’abord, d’après l’expression trouvée à l’équation (3.14), on remarque que LλF = 0, cequi caractérise l’appartenance de λF à l’espace des solutions de l’équation (3.13) et assure queητ (S) = 0.

On doit maintenant s’assurer que la direction de déformation ητ fait bien décroître le potentielde la trajectoire. Pour cela vérifions que :

dV

dτ= (µF )T λF ≤ 0

où µF est donné par l’équation (3.9). En utilisant l’expression de λF trouvée à l’équation (3.14),on a :

(µF )T λF = (µF )T (Ip − L+L)λ0F

= −(µF )T µF + (µF )T L+LµF

Comme (L+L)(L+L) = L+L, on en déduit que (L+L) est la matrice d’un opérateur de projec-tion, dont les seules valeurs propres sont 0 et 1. On a donc ∀x ∈ Rp :

0 ≤ xT L+Lx ≤ xT x

Et on en déduit que (µF )T λF ≤ 0.

Expression de cette solution dans la base E

Exprimons à nouveau la direction de déformation ητ dans la base E . On note LE la matricedont les colonnes sont les vecteurs Ei(S, τ) (on rappelle que L est la matrice dont les colonnessont les vecteurs Fi(S, τ)). On a alors L = LEP , et comme λE = PλF , on peut écrire :

λE = PλF

= P (Ip − (LEP )+LEP )λ0F

= −(Ip − P (LEP )+LE)PP T µE (3.15)

cette dernière expression ne faisant intervenir que des termes exprimés dans la base E ou qui sontcalculables directement à partir des Ei.


3.2.5 Calcul de la trajectoire déforméeOn a donc calculé un vecteur λE définissant une fonction de perturbation des entrées vτ =∑p

i=1 λEi ei. Cette fonction de perturbation engendre par intégration du système (3.3) une direc-

tion de déformation ητ qui fait baisser le potentiel de la trajectoire et qui respecte les conditionsaux limites. Alors en fixant une valeur de ∆τ petite, on pourrait appliquer cette perturbation auxentrées courantes sur l’intervalle de déformation [0, S] :

u(s, τ + ∆τ) = u(s, τ) + ∆τ v(s, τ) (3.16)

et intégrer le système (2.7) avec ces nouvelles entrées, pour obtenir une trajectoire déformée.Mais en raison de cette linéarisation, l’intégration des nouvelles fonctions d’entrée u sur l’in-

tervalle de déformation produirait une trajectoire qui ne respecterait pas exactement la conditionaux limites de l’équation (3.12). Ainsi la trajectoire déformée ne rejoindrait pas exactement latrajectoire de référence à la fin de l’intervalle de déformation.

Pour s’assurer du respect des conditions aux limites, on choisit d’appliquer la déformationcalculée ητ =

∑pi=1 λEEi directement à la trajectoire initiale. Pour ∆τ petit on calcule :

q(s, τ + ∆τ) = q(s, τ) + ∆τ η(s, τ) (3.17)

Remarque

La méthode de déformation de trajectoire présentée ici a été appliquée dans un cadre où il n’yavait pas de conditions aux limites strictes, dans [Boyer ]. Le contexte est celui de l’évaluationde la conformité de véhicules automobiles dans des parcours de test. Étant donnée la trajectoireinitiale d’un véhicule de type voiture dans le parcours, il s’agit de déformer cette trajectoire defaçon à ce qu’elle effectue le parcours sans collision, à la vitesse la plus élevée possible. Il n’ya donc pas de contrainte forte sur la configuration finale. Dans ce contexte, une intégration dusystème (2.7) avec les nouvelles fonctions d’entrée a été possible.

3.2.6 Respect des contraintes nonholonomesL’approximation effectuée à l’équation (3.17) nous assure que la trajectoire déformée res-

pecte exactement les conditions aux limites. En contrepartie, en raison de cette linéarisation,cette trajectoire n’est pas exactement solution du système (2.7), et n’est donc pas à proprementparler admissible. Si l’on cherche à exécuter cette trajectoire, on se rendra compte que le sys-tème ne peut pas la suivre exactement. On appelle ce phénomène la déviation des contraintesnonholonomes, qui va s’amplifier au fur et à mesure des déformations successives de la trajec-toire. Pour résoudre ce problème, nous considérons le système étendu de (2.7) en ajoutant n− kchamps de vecteurs indépendants :

q′(s) =n∑

i=1

ui(s)Xi(q(s)) (3.18)

tel que ∀q ∈ C, (X1(q), . . . ,Xn(q)) engendre TqC.


Ce système n’est soumis à aucune contrainte cinématique et une trajectoire de ce système estadmissible pour le système (2.7) si et seulement si :

∀j ∈ {k + 1, .., n},∀s ∈ [0, S] uj(s) = 0

Correction de la déviation des contraintes nonholonomes

Étant donnée une trajectoire q(s), nous pouvons calculer les fonctions d’entrée ui(s) corres-pondantes, avec i ∈ {1, .., n}. Or il se peut que les composantes ui(s) ne soient pas nulles, pouri ∈ {k + 1, .., n}. L’objectif est donc de calculer une direction de déformation qui ramène cescomposantes vers 0.

Pour cela nous reprenons les calculs effectués à l’équation (3.2), en considérant cette fois lesystème (3.18). On note alors A(s, τ) la matrice de taille (n× n) :

A(s, τ) =n∑

i=1

ui(s, τ)∂Xi

∂q(q(s, τ)) (3.19)

et B(s, τ) la matrice de taille (n× n) :

B(s, τ) =(B(s, τ) B⊥(s, τ)

)(3.20)

avec B⊥(s, τ) =

(Xk+1(q(s, τ)), ..,Xn(q(s, τ))

)la matrice dont les colonnes sont les champs

de vecteurs additionnels.Enfin on note v = (v,v⊥), où v⊥ sont les fonctions de perturbations suivant les champs de

vecteurs additionnels.Le système (3.3) s’écrit maintenant :

η′(s, τ) = A(s, τ)η(s, τ) + B(s, τ)v(s, τ) + B⊥(s, τ)v⊥(s, τ) (3.21)

Pour ramener v⊥ vers 0, nous effectuons une régulation proportionnelle :

∀j ∈ {k + 1, .., n},∀s ∈ [0, S] vi(s, τ) =∂ui

∂τ(s, τ) = −αui(s, τ)

où α est un réel positif. Ainsi, en l’absence de perturbation, les commandes sur les champs devecteurs additionnels tendent exponentiellement vers 0 lorsque τ augmente :

∀s ∈ [0, S], ui(s, τ) = e−ατui(s, 0)

Soit alors η⊥τ la direction de déformation engendrée par la fonction de perturbation v⊥τ , à τfixé. η⊥τ est solution de l’équation différentielle suivante :

η′⊥(s, τ) = A(s, τ)η⊥(s, τ) + B⊥(s, τ)v⊥(s, τ)

η′⊥(0, τ) = 0 (3.22)


Déformation due aux obstacles

Les fonctions de perturbations v suivant les champs de vecteurs X1, ..,Xk restent identiquesà celles trouvées à l’équation (3.9). On rappelle la direction de déformation associée trouvée àl’équation( 3.7) :

ητ =

p∑i=1

λ0Fi Fi

Conditions aux limites

Étant donné que le système (3.21) est linéaire par rapport à v, la déformation engendrée, à τfixé, par la somme des fonctions de perturbations vτ et v⊥τ est :

ητ = ητ + η⊥τ

Alors en appliquant le même raisonnement qu’à la section 3.2.4, on cherche à ce que cette direc-tion de déformation respecte les conditions aux limites.

La condition initiale (3.11) est respectée par construction de ητ . La condition finale s’écritmaintenant, à τ fixé :

ητ (S) = ητ (S) + η⊥τ (S) = 0

En reprenant les notations de (3.13), cette équation s’écrit :

Lλ = −η⊥τ (S) (3.23)

On procède alors de la même façon qu’à la section 3.2.4, et on projette orthogonalement levecteur λF sur l’espace des solutions de (3.23). Soit λF cette valeur :

λF = −L+η⊥τ (S) + (Ip − L+L)λF

Expression dans la base E

Nous pouvons exprimer ce vecteur dans la base E en procédant de la même façon qu’en (3.15),et on trouve :

λE = −P (LEP )+η⊥τ (S) + P (Ip − (LEP )+)λF

= −P (LEP )+η⊥τ (S)− (Ip − P (LEP )+LE)PP T µE (3.24)

3.2.7 Algorithme de la déformation de trajectoireLes sections précédentes ont montré comment déformer la trajectoire de façon à ce que la

trajectoire déformée :– s’éloigne des obstacles (Cf. section 3.2.3),– respecte strictement les conditions aux limites de l’intervalle de déformation (Cf. sec-

tion 3.2.4),– soit une trajectoire admissible pour le système (2.7) (Cf. section 3.2.6).Nous présentons ici un résumé de ces différentes étapes, qui constituent l’algorithme de la

déformation de trajectoire.


Données d’entrée

– I = [0, S] l’intervalle de déformation,– une trajectoire q(s) définie sur I , non nécessairement solution du système (2.7),– les n fonctions entrées ui(s), s ∈ I suivant les n champs de vecteurs du système (3.18),– le gradient du potentiel que l’on souhaite minimiser ∂U

∂q(q(s)), s ∈ I ,

– p fonctions élémentaires de perturbation ei(s), s ∈ I ,– une valeur ηmax de la déformation maximale autorisée de la trajectoire,– τ ← 0.

Algorithme

Calcul des matrices A et B

– Calculer A(s, τ) et B(s, τ) en utilisant les équations (3.19) et (3.20)Correction de la déviation des contraintes nonholonomes– pour i ∈ {k + 1, .., n} calculer la correction vi(s, τ) = −αui(s, τ)– Calculer la déformation associée η⊥(s, τ) en intégrant (3.22)Déformation élémentaire– Pour i ∈ {1, .., p} calculer les directions de déformation élémentaires Ei(s, τ) en inté-

grant (3.6)– Pour i ∈ {1, .., p} calculer µE en utilisant l’équation (3.10)Base orthonormale– Calculer la matrice P en utilisant le procédé d’orthonormalisation de Gram-Schmidt sur

les vecteurs Ei(s, τ)

Conditions aux limites– Calculer λE en utilisant l’équation (3.24)Direction de déformation– Calculer la direction de déformation η(s, τ) =

∑pi=1 λE

i Ei(s, τ) + η⊥(s, τ)

Déformation de la trajectoire– Calculer ∆τ = ηmax

‖η(s,τ)‖∞– Calculer la nouvelle trajectoire : q(s, τ + ∆τ) = q(s, τ) + ∆τ η(s, τ)– Incrémenter τ : τ ← τ + ∆τ

Cet algorithme est appliqué itérativement jusqu’à ce que la trajectoire ne soit plus en collision.

3.2.8 Prise en compte des bornes sur les entrées du système

Un robot mobile est soumis à des contraintes sur les valeurs de ses entrées et de leurs dérivées.On suppose que ces contraintes peuvent de mettre sous la forme :∀i ∈ {1, ..., k}, et ∀s ∈ [0, S]

−uiMin ≤ ui(s) ≤ uiMax

−uiMin ≤ ∂ui

∂s(s) ≤ uiMax (3.25)


La méthode de déformation de trajectoire que nous avons présentée ne garantit pas que lesfonctions d’entrée de la trajectoire déformée respectent ces contraintes. En effet, on assure qu’àchaque itération de l’algorithme la perturbation des fonctions d’entrée de la trajectoire est petite,mais au fur et à mesure du processus de déformation, les entrées peuvent augmenter et dépasserles limites définies par le système d’inégalités (3.25). Bien souvent par exemple, les entrées dusystème sont des vitesses et la longueur de la trajectoire déformée pour éviter un obstacle estsupérieure à la longueur de la trajectoire initiale. Étant donné que la trajectoire déformée estdéfinie sur le même intervalle d’abscisse, cela signifie que la vitesse a augmenté.

Il faut donc contrôler l’évolution des fonctions d’entrée au cours du processus de déformationet éviter les dépassements des valeurs maximales. Nous présentons ici le principe de la méthodedéveloppée par Mathieu Hillion lors de son stage [Hillion ], à l’encadrement duquel nousavons collaboré.

La méthode se décompose en deux parties :– définir des fonctions de perturbations nulles sur les intervalles où les entrées dépassent

leurs limites et non nulles ailleurs,– calculer un reparamétrage de la trajectoire qui assure le respect des limites.

Fonctions de perturbations

Par exemple, une trajectoire étant définie sur l’intervalle [0, 10], on remarque qu’une de sesentrées ui ne respecte pas les contraintes de l’équation (3.25) sur l’intervalle [2, 10]. On va doncdéfinir des fonctions de perturbations de cette entrée telles qu’elles soient nulles sur l’inter-valle [2, 10] et non nulles sur l’intervalle [0, 2]. La figure 3.1 représente deux fonctions de pertur-bations sinusoïdales e1 et e2 pour l’entrée ui, qui respectent ces contraintes.

e1 e2

FIG. 3.1 – Fonctions de perturbations des entrées, bloquées sur l’intervalle [2, 10].


Cette première amélioration permet de s’assurer que les entrées de la trajectoire déforméesont bloquées quand elles dépassent leurs valeurs limites.

Reparamétrage de la trajectoire

Il s’agit de trouver une fonction de reparamétrage φ, s 7→ t = φ(s), telle que les entrées dela trajectoire définie comme une fonction de l’abscisse t sur l’intervalle [0, φ(S)] respectent lescontraintes de l’équation (3.25). Ce reparamétrage de la trajectoire a pour conséquence que lesentrées définies comme une fonction de la nouvelle abscisse t on l’expression suivante :∀i ∈∈ {1, ..., k}

ui(t) =1

φ′(s)ui(s)

L’allure de la fonction φ′ va déterminer le rapport entre les entrées avant et après reparamétrage.La fonction φ′ choisie est en l’occurrence une parabole, de façon à ce que l’influence soit

maximale au milieu de la trajectoire. Les paramètres de cette parabole sont des fonctions desvaleurs maximales des entrées et de leurs dérivées.

Ce reparamétrage n’est bien sûr pas optimal (il n’assure pas un mouvement en temps mi-nimal) mais il est très rapide à calculer et il pourra donc être exécuté en temps-réel, lors del’exécution de la trajectoire par le robot.

3.3 Application de la méthode de déformation de trajectoire :le robot Cycab

Nous détaillons cet algorithme sur un exemple, développé à partir du robot Cycab représentésur la figure (3.2).

3.3.1 Modèle cinématiqueLe robot Cycab possède plusieurs modes cinématiques. Il est par exemple possible de contrô-

ler indépendamment l’angle de braquage des roues arrière et celui des roues avant. Nous présen-tons ici le modèle cinématique dans lequel les roues arrière sont bloquées et seules les rouesavant peuvent braquer (figure 3.3).

Ce système possède 4 variables de configuration : q = (x, y, θ, φ). x et y donnent la positiondu centre P de l’essieu arrière du robot, exprimés en mètre. θ est l’orientation de l’axe perpen-diculaire à l’essieu arrière. On note O le centre de courbure du système, l étant la distance entrel’essieu arrière et l’essieu avant et Q le milieu de l’essieu avant. φ est alors l’angle qui placeraiten O le centre de courbure d’une roue avant située en Q. La courbure est notée κ et on a κ = tan φ

l.

Ce système est soumis à 2 contraintes nonholonomes :

– ~vP est colinéaire au vecteur(

cos θsin θ

)

3.3 Application de la méthode de déformation de trajectoire : le robot Cycab 33

FIG. 3.2 – Le robot Cycab, un robot de typevoiture

O

θ

φ

l

1κ

~vQ

~vP

QP = (x, y)

φ

FIG. 3.3 – Modèle cinématique du robot Cy-cab

– ~vQ est colinéaire au vecteur(

cos(θ + φ)sin(θ + φ)

)

Soient X1(q) =

cos θsin θtan φ

l

0

et X2(q) =

0001

deux champs de vecteurs du système. On a

alors :q′(s) = u1(s)X1(q(s)) + u2(s)X2(q(s)) (3.26)

où u1 représente la vitesse linéaire du véhicule et u2 la vitesse de braquage.

3.3.2 Calculs pour la déformation de trajectoireDes champs de vecteurs additionnels pour obtenir le système étendu (3.18) sont par exemple :

X3(q) =

− sin θcos θ

00

X4(q) =

0010

L’expression des matrices A et B de l’équation (3.21) à partir des champs de vecteurs X1, . . . ,X4

est immédiate.On trouve :

A(s) =

0 0 −u1(s) sin θ − u3(s) cos θ 00 0 u1(s) cos θ − u3(s) sin θ 00 0 0 00 0 0 0

B(s) =

cos θ 0 − sin θ 0sin θ 0 cos θ 0tan φ

l0 0 1

0 1 0 0


avec θ = θ(s) et φ = φ(s).La base de fonctions de perturbations ei définie en (3.5) que nous utilisons est une série de 2p

fonctions sinusoïdales de périodes décroissantes avec l’ordre p :

e1(s) =

(√2

Ssin(πs

S

), 0

)T

e2(s) =

(0,

√2

Ssin(πs

S

))T

......

e2p−1(s) =

(√2

Ssin(pπs

2S

), 0

)T

e2p(s) =

(0,

√2

Ssin

((p− 1)πs

2S

))T

où [0, S] est l’intervalle de définition de la trajectoire. Les deux composantes de ei correspondentrespectivement à une perturbation de u1 et de u2.

3.3.3 Exemple de déformationPour cet exemple on se donne une trajectoire initiale (τ = 0) définie sur l’intervalle [0, 10],

avec pour fonctions d’entrée :

∀s ∈ [0, 10] u1(s) = 1 u2(s) = 0

et pour configuration initiale q(0) = (0, 0, 0, 0)T , ce qui correspond à une trajectoire rectiligne.Les entrées suivant les champs de vecteurs additionnels X3(q) et X4(q) sont nulles. L’étapedite de correction des dérives des contraintes nonholonomes engendre donc ici une direction dedéformation η⊥ nulle.

Le gradient du potentiel dans l’espace des configurations est constant et vaut :

∀s ∈ [0, 10]∂U

∂q(qτ (s)) =

0−0.1

0

ce qui représente une répulsion de la trajectoire dans la direction des y positifs.

La figure 3.4 représente la première composante des fonctions de perturbations élémen-taires ei (perturbation de u1 donc). L’ordre p est fixé à 4, et les premières composantes desfonctions e2(s), e4(s), e6(s), e8(s) sont donc nulles pour s ∈ [0, 10] et ne sont pas représentées.

La figure 3.5 représente la direction de déformation élémentaire E2 correspondant à l’inté-

gration par le système (3.3) de la fonction de perturbation e2(s) =(0,√

2S

sin(

πsS

))T

.La direction de déformation calculée par l’équation (3.24) est représentée sur la figure 3.6.

On vérifie bien que la condition finale η(S) = 0 est bien respectée. Par ailleurs on remarque quecette direction de déformation fait apparaître une déformation vers les y positifs, ce à quoi ons’attendait intuitivement au vu du gradient du potentiel choisi.

Les coordonnées de la direction de déformation η dans la base (E1, . . . ,E8) sont :

λ = (0 0 0 − 2.14 0 0 0 4.28)T

3.3 Application de la méthode de déformation de trajectoire : le robot Cycab 35

−0.1

0

0.1

0.2

0.3

0.4

0.5

0 1 2 3 4 5 6 7 8 9 10−0.5

−0.4

−0.3

−0.2

e1e3e5e7

FIG. 3.4 – Premières composantes des fonctions de perturbations élémentaires

40

50

60

70

0 1 2 3 4 5 6 7 8 9 10 0

10

20

30

xy

φθ

FIG. 3.5 – Direction de déformation élémentaire E2 engendrée par la perturbation élémentaire e2

−4

−2

0

2

4

6

8

10

12

0 1 2 3 4 5 6 7 8 9 10

xy

φθ

FIG. 3.6 – Direction de déformation η


c’est à dire que les perturbations suivant la première composante u1 des fonctions d’entrée sonttoutes nulles (λ1 = λ3 = λ5 = λ7 = 0). En effet une modification de la vitesse linéaire du robotn’aurait aucun effet sur le potentiel de la trajectoire.

0

0.05

0.1

0.15

0.2

0.25

0.3

0 1 2 3 4 5 6 7 8 9 10−0.1

−0.05

y

x

FIG. 3.7 – Trajectoire déformée

La nouvelle trajectoire déformée est calculée en appliquant :

q(s, ∆τ) = q(s, 0) + ∆τη(s) (3.27)

Elle est représentée sur la figure 3.7, sur laquelle on observe effectivement une déformationvers les y positifs. La valeur de ∆τ est fixée par la valeur maximale de déformation autorisée :∆τ = ηmax

‖η(s)‖∞. En l’occurence ηmax vaut 0.2.

−2

−1

0

1

2

3

4

0 1 2 3 4 5 6 7 8 9 10

u1u1

FIG. 3.8 – Fonctions d’entrée de la trajectoire déformée

La figure 3.8 représente les fonctions d’entrée u1 et u2 de la trajectoire déformée. On vérifieque u1 reste inchangée (vitesse linéaire), et que u2 est légèrement perturbée (variation de l’anglede braquage).

3.4 Calcul des interactions robot-obstacles 37

Enfin on conclut cet exemple en remarquant que les fonctions d’entrée suivant les champsde vecteurs additionnels X3 et X4 ne sont pas tout à fait nulles (de l’ordre de 10−4, à comparerà u1(s) = 1), du fait de l’approximation effectuée à l’équation (3.27). Ces entrées seront réguléesà l’itération suivante grâce à la correction des dérives des contraintes nonholonomes présentée àla section 3.2.6.

3.4 Calcul des interactions robot-obstaclesLa déformation de trajectoire pour systèmes nonholonomes présentée dans ce chapitre est une

méthode d’évitement réactif d’obstacles. On doit donc calculer les interactions entre la trajectoireplanifiée et les obstacles détectés.

Plus précisément, cette méthode réalise successivement les tâches suivantes sur la trajectoirediscrétisée :

1. détection des collisions de la trajectoire avec les obstacles détectés,

2. calcul du potentiel de la trajectoire sur un intervalle centré sur la première collision,

3. déformation de la trajectoire sur cet intervalle.

La tâche 3 correspond à l’algorithme décrit à la section 3.2.7. L’intégration de ces différentestâches au sein d’une architecture unifiée est décrite dans le chapitre 5.

On s’intéresse dans cette section aux opérations effectuées par les tâches 1 et 2. Tout d’abordnous présentons le calcul du potentiel d’une trajectoire, dans le cas où le potentiel d’une confi-guration est une fonction de la distance aux obstacles. Ensuite nous présentons un algorithmepermettant d’optimiser les calculs réalisés dans ces deux tâches. Cet algorithme tire parti du faitque ces opérations (détections des collisions et calcul du potentiel) s’effectuent en parcourantitérativement la trajectoire discrétisée. Ces résultats ont été publiés dans [Lefebvre ].

3.4.1 Potentiel dans l’espace des configurationsLa méthode de déformation de trajectoire que nous utilisons fait appel au potentiel d’une

trajectoire, et calcule une déformation de la trajectoire qui fait baisser ce potentiel. En reprenantles notations de la section 3.2.2, on note :

V (q) =

∫ S

0

U(q(s))ds

le potentiel d’une trajectoire définie sur [0, S].Dans le cadre de l’évitement réactif d’obstacles, on définit ce potentiel comme une fonction

de la distance du robot aux obstacles.

Potentiel fonction de la distance aux obstacles

On souhaite que le potentiel soit une fonction décroissante de la distance aux obstacles, jus-qu’à une distance maximale d1 au-delà de laquelle les obstacles n’ont plus d’influence. On définit


la fonction Ud suivante qui remplit ces critères :

Ud : R+ → R

d 7→ Ud(d) =

{1

d+d0− 1

d1+d0si 0 ≤ d ≤ d1

0 si d > d1

(3.28)

où d0 est un réel positif permettant de paramétrer la valeur maximale du potentiel.Alors, étant donné un obstacle O, le potentiel engendré par l’interaction entre cet obstacle et

le robot dans la configuration q est :

U(q) = Ud(dO(q,O))

On étend naturellement cette définition au cas des véhicules articulés dans un environnementcontenant plusieurs obstacles. Soit un robot mobile composé de nb corpsB1,...,Bnb

, etBi(q) ⊂ W ,le sous-espace occupé par le corps Bi lorsque le robot est dans la configuration q. Soit no lenombre d’obstacles de l’environnement détectés par le capteur :O1,...,Ono . Le potentiel d’uneconfiguration est alors :

U(q) =

nb∑i=1

no∑j=1

Ud(dij(q)) (3.29)

où :dij(q) = d(Bi(q),Oj)

La valeur qui nous intéresse dans l’algorithme de déformation (3.2.7) est le gradient du po-tentiel dans l’espace des configurations, qui s’écrit :

∂U

∂q(q) =

nb∑i=1

no∑j=1

U ′d(dij(q))

∂dij

∂q(q)

En notant respectivement o(q) et r(q) les points de l’obstacle j et du corps i les plus prochesquand le robot est dans la configuration q (figure 3.9), on a :

dij(q) = ‖o(q)− r(q)‖

Ainsi, pour une configuration q donnée, le calcul de dij(q) et de U ′d(dij(q) ne pose pas de

problème particulier. Par contre, le calcul de ∂dij

∂q(q) n’est a priori pas aisé. C’est l’objet du

paragraphe suivant.

Calcul du gradient de la distance sans expression analytique des points les plus proches

Le gradient de dij est :

∂dij

∂q(q) =

(o(q)− r(q))T

‖o(q)− r(q)‖

(∂o

∂q(q)− ∂r

∂q(q)

)(3.30)


r(q)

o(q)

dij(q)

x

yB(q)

corps Bi

θe1(q)

e2(q)

q = (x, y, θ)

Oj

FIG. 3.9 – Le potentiel dans l’espace des configurations dépend seulement de la distance ‖o(q)−r(q)‖ entre le robot et l’obstacle. Le mouvement du point r(q) sur le corps du robot (mouvementrelatif) est orthogonal au vecteur ~or.

D’après cette expression, il semblerait que l’on doive exprimer analytiquement les positionsdes points o(q) et r(q). Ces expressions peuvent être délicates à obtenir, car elles dépendentde la forme de l’obstacle et de la forme du corps du robot considéré. Les travaux pionniersde [Khatib ] sur l’évitement réactif d’obstacles en robotique, basé sur des champs de poten-tiels, donnaient ainsi l’expression analytique de la distance entre deux corps pour quelques casparticuliers (cylindre, sphère, parallélépipède, etc.). Nous allons montrer que le mouvement despoints o(q) et r(q) sur leurs corps respectifs n’a pas d’influence pour le calcul de ∂dij

∂q(q), et qu’il

n’est donc pas nécessaire de posséder une expression analytique de la distance entre deux corpsen fonction de leur configuration.

Soit un repère (B(q), e1(q), e2(q)) attaché au corps Bi considéré du robot1. On note rB1 , rB

2

les coordonnées de r(q) exprimées dans le repère (B(q), e1(q), e2(q)) :

−−−−−−→B(q)r(q) =

2∑l=1

rBl (q)el(q)

On suppose que r(q) est un «point régulier» de la frontière de Bi tel que ∂r∂q

(q) existe. Alorson peut écrire :

∂r

∂q(q) =

mouvement du point coïncidant︷︸︸︷∂B

∂q(q) +

2∑l=1

rBl (q)

∂el

∂q(q) +

mouvement relatif︷︸︸︷2∑

l=1

∂rBl

∂q(q)el(q) (3.31)

1Ces résultats s’étendent immédiatement à un espace de travail de dimension 3


Le mouvement relatif de r(q) sur le corps considéré est orthogonal au vecteur (o(q)− r(q)),car r(q) se déplace sur la frontière du corps, comme illustré par la figure 3.9, et on a :

(o(q)− r(q))T

2∑l=1

∂rBl

∂q(q)el(q) = 0

Si l’on note r∈B(q) le point coïncidant avec r(q), on peut alors remplacer ∂r∂q

(q) par ∂r∈B∂q

(q)dans l’expression (3.30).

En appliquant maintenant le même raisonnement avec le point o(q) et sous l’hypothèse queles obstacles sont statiques, on a :

(o(q)− r(q))T ∂o

∂q(q) = 0

Et l’expression (3.30) s’écrit alors simplement :

∂dij

∂q(q) =

(r(q)− o(q))T

‖o(q)− r(q)‖∂r∈B∂q

(q) (3.32)

Ainsi le calcul du gradient du potentiel dans l’espace des configurations ne nécessite pasl’expression analytique des points les plus proches en fonction de la configuration du robot, maisseulement leur position dans l’espace de travail pour une configuration donnée.

Remarque Si r(q) n’est pas un «point régulier», par exemple s’il est situé à un angle de lafrontière du corps considéré, on fait l’hypothèse raisonnable que le mouvement relatif de cepoint est nul (voir la figure 3.10). Cela revient à faire l’hypothèse que le point le plus prochede l’obstacle est fixe dans le repère (B(q), e1(q), e2(q)). Et donc les ∂rB

l

∂q(q) sont nuls, c’est à

dire que le mouvement relatif est nul, et le raisonnement précédent reste valable : on ne doitconsidérer que le mouvement du point coïncidant, ∂r∈B

∂q(q).

e2(q)

e1(q)

x

B(q)

corps Bi

θy

dij(q)

Oj

r(q)

o(q)

FIG. 3.10 – Dans le cas où le point le plus proche n’est pas «régulier», on fait l’hypothèse queson mouvement relatif est nul, et le raisonnement reste valable.


Exemple d’un robot dans le plan

Nous détaillons, au travers d’un exemple illustré par la figure 3.9, le calcul du gradient dupotentiel généré par un obstacle Oj sur un robot composé d’un corps Bi dans un plan. La confi-guration du robot est notée q = (x, y, θ). Le potentiel généré par l’obstacle sur le corps Bi estdonné par l’équation (3.29) : U(q) = Ud(d(Bi(q),Oj).

En notant B(q) le centre cinématique du corps Bi, on définit le repère (B(q), e1(q), e2(q))attaché à ce corps. Soit o(q) = (o1, o2) le point de Oj le plus proche de Bi(q). Soit r(q) =(r1, r2) le point de Bi(q) le plus proche de Oj et (rB

1 , rB2 ) ses coordonnées dans le repère B. On

a alors :

r∈B =

(x + rB

1 cos θ − rB2 sin θ

y + rB1 sin θ + rB

2 cos θ

)Et en dérivant par rapport à q :(

1 0 −rB1 sin θ − rB

2 cos θ0 1 rB

1 cos θ − rB2 sin θ

)En notant A = −rB

1 sin θ − rB2 cos θ et B = rB

1 cos θ − rB2 sin θ, l’équation (3.32) devient

alors :

∂dij

∂q(q) =

(r(q)− o(q))T

‖o(q)− r(q)‖∂r∈B∂q

=1

d

r1 − o1

r2 − o2

(r1 − o1)A + (r2 − o2)B

T

où d =√

(r1 − o1)2 + (r2 − o2)2. Et on obtient finalement le gradient du potentiel dans l’espacedes configurations :

∂U

∂q(q) =

U ′d(d)

d

r1 − o1

r2 − o2

(r1 − o1)A + (r2 − o2)B

T

où Ud est la fonction (3.28). On remarque que le dernier terme correspond au moment d’uneforce appliquée au point r(q), qui entraîne une rotation du corps Bi autour de son centre B(q).

3.4.2 Optimisation des calculs des interactionsComme nous l’avons mentionné au début de cette section, les tâches de détections des col-

lisions (1) et de calcul du gradient du potentiel (2) parcourent un intervalle de la trajectoirediscrétisée pour effectuer leurs calculs. Ainsi pour chaque configuration de la trajectoire discré-tisée, on doit calculer les interactions (détection de collision ou calcul du gradient du potentiel)pour chacun des corps du robot avec chacun des obstacles perçus. Si l’on note ns le nombrede configurations sur l’intervalle considéré, la complexité du calcul est ns × nb × no, où nb est


le nombre de corps composant la chaîne cinématique du robot, et no est le nombre d’obstaclesperçus.

Dans la tâche (1), ns est proportionnel à la distance sur laquelle sont détectées les collisionssur la trajectoire planifiée, en partant de la configuration courante du robot. Dans la tâche (2),ns est proportionnel à la taille de l’intervalle de déformation. Ainsi si le pas de discrétisation esttrès petit, ce qui est le cas quand on navigue proche des obstacles, la valeur de ns peut être trèsgrande (de l’ordre de 1000 pour une trajectoire de 5 mètres par exemple). Pour cette raison nousavons cherché à diminuer la complexité de ces calculs et nous avons développé un algorithmequi les optimise, publié dans [Lefebvre ]. On peut trouver des idées similaires à cet algorithmebasé sur la cohérence spatiale de la trajectoire, dans des travaux d’animation graphique tels queceux de [Baraff ] et [Lin ].

Distance d’influence

Dans le cadre de l’évitement réactif d’obstacles on définit généralement deux distances :– une distance minimale aux obstacles en deçà de laquelle une configuration est considérée

en collision, notée Dclear,– une distance au delà de laquelle le potentiel généré par un obstacle est nul, ce qui corres-

pond au paramètre d1 dans la fonction de potentiel (3.28).Pour simplifier l’écriture, nous notons dinfl la distance d’influence de chacune des tâches,

Dclear et d1. Ainsi de nombreuses paires (corps Bi) - (obstacle Oj) telles que d(Bi(q),Oj) >dinfl ne génèrent aucune interaction et pourraient donc être éliminées des calculs réalisés par lestâches. Nous allons exploiter le principe suivant : «les obstacles très loin d’une configuration dela trajectoire discrétisée sont encore loin de la configuration suivante». Cela va nous permettrede «filtrer» les obstacles qui n’ont pas d’influence dans les calculs des interactions.

Borne inférieure de la distance à un obstacle

Nous formalisons ici le principe énoncé précédemment. Soit q1 et q2 deux configurations durobot et Bi le corps considéré. Soit m la transformation solide telle que :

Bi(q2) = m(Bi(q1))

On note ∆ la borne supérieure de la distance parcourue par les points du corps Bi entre lesconfigurations q1 et q2 :

∆ = maxr∈Bi(q1)

‖m(r)− r‖

Alors on a la propriété suivante :

Propriété 6. Pour tout sous ensemble O ⊂ W et pour tout d > 0,si :

d(Bi(q1),O) ≥ d

alors :d(Bi(q2),O) ≥ max(d−∆, 0)


Démonstration. Si d ≤ ∆, alors max(d −∆, 0) = 0 et la conclusion est immédiate. Si d ≥ ∆on remarque que par hypothèse on a pour tout o ∈ O et pour tout r1 ∈ Bi(q1) :

‖r1 − o‖ ≥ d

Et pour tout r2 ∈ Bi(q2) on peut écrire :

‖r2 − o‖ = ‖r2 −m−1(r2) + m−1(r2)− o‖≥

∣∣‖r2 −m−1(r2)‖ − ‖m−1(r2)− o‖∣∣

par inégalité triangulaire. De plus comme m−1(r2) ∈ Bi(q1), on a :

‖m−1(r2)− o‖ ≥ d

Par définition de ∆ on a :‖r2 −m−1(r2)‖ ≤ ∆

Comme d ≥ ∆ :‖r2 −m−1(r2)‖ ≤ ‖m−1(r2)− o‖

Finalement il vient :

‖r2 − o‖ ≥ ‖m−1(r2)− o‖ − ‖r2 −m−1(r2)‖≥ d−∆

Cette dernière inégalité étant valable pour tout o ∈ O et pour tout r2 ∈ Bi(q2). On obtientdonc le résultat d(Bi(q2),O) ≥ d−∆.

Cette propriété, qui est la version pour un seul corps de la propriété (1), permet de mettre àjour de façon simple une borne inférieure de la distance d’un corps du robot à un obstacle, lorsd’un mouvement entre q1 et q2. Il suffit pour cela de soustraire la borne supérieure de la distanceparcourue par les points du corps, ∆.

Algorithme de filtrage

Le principe est de maintenir pour chaque corps du robot une liste triée des bornes inférieuresdes distances entre les obstacles et le corps. Soit linfl le nombre d’éléments en tête de cetteliste qui correspondent à des distances robot-obstacle inférieures à la distance d’influence dinfl.Il suffit donc de ne considérer dans les calculs effectués par les tâches (1) et (2) que les linfl

éléments en tête de liste, linfl étant généralement très inférieur au nombre d’obstacles perçus no.À l’initialisation, la liste est remplie par les distances exactes entre les obstacles et le corps

dans la configuration initiale. Puis la trajectoire discrétisée qs est parcourue itérativement sur unintervalle (s ∈ {1, ..., ns}) par les tâches (1) et (2) afin de calculer les interactions du robot avecles obstacles. Lors de ce parcours de la trajectoire, pour chaque configuration qs la liste est miseà jour en soustrayant la distance maximale parcourue entre qs−1 et qs (notée ∆) à chacun deses éléments. C’est à dire que les obstacles qui sont plus éloignés de qs que de qs−1 sont traitéscomme s’ils s’étaient rapprochés. Ainsi, à chaque configuration, les linfl éléments de la liste quiatteignent la valeur dinfl entraînent les traitements suivants :


Algorithme 1 Filtrage des interactions robot-obstacles sur un intervalle de la trajectoireInitialisation des listesfor i ∈ {1, ..., nb} do

for j ∈ {1, ..., no} docalcul de la distance aux obstacles : dist[j, i]← d(Bi(qs),Oj)

end forTRI-RAPIDE(dist[., i])

end for

boucle sur l’intervalle de la trajectoirewhile (s ≤ ns) do

for i ∈ {1, ..., nb} dol← 1 ;while (dist[l, i] ≤ dinfl) do

j ← index du l-ème obstacle dans la liste dist[l, i]CALCULER_INTERACTION(Oj,Bi)calcul de la distance exacte à cet obstacle : dist[l, i]← d(Bi,Oj)l← l + 1

end whilelinfl ← ltrier les linfl premiers éléments de la liste :TRI-INSERTION(dist[., i], linfl)∆[i]← DIST_MAX_PARCOURUE(Bi,qs, qs+1)for j ∈ {1, ..., no} do

Soustraire la distance maximale parcourue :dist[j, i]←MAX(0, dist[j, i]−∆[i])

end forend fors← s + 1

end while


qs+1

qsdinfl

dno

d3

d′1

d4

d2

d1

d′2

∆

d1

d2

d3

d4

...

d′3

...dno−∆

d4 −∆

décrémente de ∆

linfl = 3

dno

...d4 −∆

dno−∆

d3 −∆d2 −∆

linfl = 3

exactes si d ≤ dinfl

calcul des distances

...d4 −∆

dno−∆

d1 −∆d′2d′1

d′1d′2

d′3d′3linfl = 1

tri-insertion des linfl

premiers éléments

O4

O2

dinfl

O3

Ono

de distancesliste triée

O1

FIG. 3.11 – Illustration de l’algorithme de filtrage des interactions entre les obstacles et le robot(seule la remorque est considérée). En bas les différentes étapes de mise à jour de la liste desbornes inférieures des distances aux obstacles. ∆ est la distance maximale parcourue par chacundes points de la remorque entre qs et qs+1.

– ils correspondent à des obstacles qui pourraient avoir une influence sur le robot et ils sontdonc pris en compte dans les calculs effectués par les tâches (1) et (2),

– leur distance exacte au corps considéré est recalculée et elle est insérée dans la liste déjàpartiellement triée.

L’algorithme 1 décrit ces différentes étapes et la figure 3.11 illustre une itération de l’al-gorithme entre deux configurations qs et qs+1, pour un corps du robot (la remorque du robotHilare2).


Bilan

L’algorithme de filtrage présenté donne lieu à de nouveaux calculs :– calcul de la distance de chacun des corps du robot aux obstacles,– tri par l’algorithme de TRI-RAPIDE des listes de distances. La complexité est O(n log(n)),– tri par l’algorithme TRI-INSERTION des linfl premiers éléments des listes déjà triées. La

complexité est O(n2) mais cet algorithme est très efficace sur les listes déjà partiellementtriées.

Il permet cependant de diminuer de façon importante les temps de calcul, car le calcul effectifdes interactions n’a lieu que pour un très petit nombre d’éléments.

Ainsi avant l’implémentation de cet algorithme de filtrage, les calculs effectués par les tâches (1)et (2) constituaient le «noeud d’engorgement» de l’algorithme de déformation de trajectoire pré-senté à la section 3.2.7. Son utilisation a permis de diminuer les temps de calcul de ces tâchesd’un facteur 6 à 8.

Il se révèle difficile d’évaluer sa complexité, mais les expériences menées sur plusieurs robots(le robot Hilare2 avec sa remorque et le robot Cycab) dans des environnements variés démontrentson efficacité.

3.5 Optimisation de la trajectoire suivant d’autres critèresLa méthode de déformation de trajectoire que nous avons présentée a été développée pour

l’évitement d’obstacles pour des systèmes nonholonomes. Nous montrons ici que cette méthodepeut en fait agir sur la trajectoire en fonction d’autres critères et en vue d’autres applications. Lesmoyens de contrôle de la trajectoire sont au nombre de trois.

Potentiel d’une trajectoire Le potentiel d’une trajectoire définie sur l’intervalle [0, S] est l’in-tégrale du potentiel d’une configuration U(q) sur cet intervalle. L’algorithme présenté en (3.2.7)permet de faire baisser ce potentiel et donc d’optimiser la trajectoire suivant le critère exprimé parle potentiel. Jusqu’à présent ce critère était fondé sur la distance aux obstacles, mais d’autres cri-tères pour d’autres applications sont envisageables. Avec un peu de recul, on peut donc considérercette méthode comme une méthode d’optimisation de trajectoire suivant un critère quelconque.Il a par exemple été essayé dans [Boyer ] de contrôler la vitesse le long d’une trajectoire enexprimant un potentiel sur la vitesse, c’est à dire sur q′(s).

Nous présentons à la section 3.5.1 l’utilisation d’un potentiel sur l’angle de braquage d’unsystème de type voiture, publiée dans [Lefebvre ].

Respect des contraintes nonholonomes Certaines approximations dans la méthode de défor-mation de trajectoire font que le processus de déformation fournit en sortie une trajectoire quin’est pas strictement admissible pour le système (2.7). L’idée de l’étape de correction de la dé-viation des contraintes nonholonomes (présentée à la section 3.2.6) est de modifier le processusde déformation afin de corriger ce phénomène indésirable. Un mécanisme est donc mis en oeuvrepour ramener les entrées suivant les champs de vecteurs additionnels du système vers 0.

3.5 Optimisation de la trajectoire suivant d’autres critères 47

On comprend donc que si la trajectoire initiale n’est pas strictement admissible, les itéra-tions successives du processus de déformation vont tendre vers une trajectoire admissible. Nousprésentons ce phénomène et ses potentielles applications à la section 3.5.2.

Respect des conditions aux limites L’étape qui s’assure que la configuration finale de l’inter-valle de déformation reste inchangée au cours du processus de déformation peut-être détournéede son objectif initial. Ainsi on peut souhaiter spécifier une configuration finale différente, etutiliser la méthode pour déformer la trajectoire de façon à ce qu’elle rejoigne une configurationspécifiée. Nous avons utilisé cette technique dans le cadre d’une méthode de parking référencésur des amers, qui est présentée au chapitre suivant.

3.5.1 Respect d’une contrainte sur la courbure durant le processus de dé-formation de trajectoire

Le robot Cycab présenté à la section 3.3 est soumis à des contraintes physiques sur son anglede braquage φ :−φmax ≤ φ ≤ φmax.

La méthode de déformation de trajectoire pour systèmes nonholonomes assure que la trajec-toire déformée respecte les contraintes cinématiques du système, mais elle ne garantit rien quantà la courbure de la trajectoire. Ainsi il se peut qu’à partir d’une trajectoire admissible mais en col-lision, la déformation produise un chemin sans collision mais avec une courbure non-admissiblepour le système.

Pour contrer cet effet on considère l’angle φmax comme un obstacle pour la variable φ, et ondéfinit une fonction de potentiel qui est fonction de la distance à cet obstacle, de façon similaireà la fonction de potentiel de l’équation (3.28). En pratique seul le gradient de cette fonction estcalculé, et on définit donc U ′

φ(φ) :

0 si 0 ≤ |φ| ≤ (φmax − d1)1

((φmax−|φ|)+d0)2− 1

(d1+d0)2si (φmax − d1) ≤ |φ| ≤ φmax

1(d0)2− 1

(d1+d0)2si |φ| ≥ φmax

Le paramètre d0 permet de régler la valeur maximale U′maxφ prise par la fonction U ′

φ en φ = φmax,et le paramètre d1 définit l’intervalle sur lequel cette fonction est nulle, comme illustré sur lafigure 3.12.

Application à un robot de type rover

Nous avons appliqué ce principe au robot Dala, un rover représenté sur la figure 3.13. Sonmodèle cinématique est celui d’un unicycle, mais nous ajoutons une roue directrice virtuellesituée à un mètre à l’avant du centre cinématique, afin de contrôler la courbure des trajectoiresdu système (figure 3.14). Le système est ainsi équivalent à un robot de type voiture, dont lemodèle cinématique est donné par l’équation (3.26). Le contrôle de la courbure lors du processusde déformation permet en pratique de limiter les glissements et les mouvements peu naturels durobot.


U′max

φ

φd1

−φmax φmax

d1

U ′φ(φ)

0

FIG. 3.12 – Gradient du potentiel sur l’angle de braquage φ

FIG. 3.13 – Le robot Dala, un robot de typeunicycle

(x, y)

θ

φ

FIG. 3.14 – Modèle cinématique d’un uni-cycle, avec une roue avant virtuelle située à unmètre du centre cinématique


La figure 3.15 représente les déformations successives d’une trajectoire. La valeur de φmax

est de 1 radian, et la trajectoire initiale ne respecte pas cette contrainte. La méthode de défor-mation de trajectoire va générer des perturbations des entrées qui font baisser le potentiel surl’angle de braquage Uφ. On observe que les déformations successives entraînent naturellementdes manoeuvres, qui diminuent la courbure maximale de la trajectoire.

q(0)

q(S)

FIG. 3.15 – Déformation d’une trajectoire initiale sans collision, mais avec une courbure tropélevée

3.5.2 Déformation d’une trajectoire non-admissible vers une trajectoireadmissible

Comme nous l’avons mentionné en introduction de cette section, la méthode de déforma-tion de trajectoire implémente un mécanisme qui assure que la trajectoire déformée respecte lescontraintes cinématiques du système, dont le détail est donné à la section 3.2.6. Nous en rappe-lons brièvement le principe.

Étant donné le système dynamique de l’équation (2.7), dont les vitesses sont une combinaisonlinéaire de k champs de vecteurs (X1, . . . ,Xk), on considère le système étendu en ajoutant n−kchamps de vecteurs indépendants :

q′(s) =n∑

i=1

ui(s)Xi(q(s)) (3.33)

tel que ∀q ∈ C, (X1(q), . . . ,Xn(q)) engendre TqC.Une trajectoire du système (3.33) est admissible pour le système (2.7) si et seulement si :

∀j ∈ {k + 1, .., n},∀s ∈ [0, S] uj(s) = 0

Mais les approximations effectuées au cours du processus de déformation font que les entréessuivant les champs de vecteurs additionnels ne sont pas exactement nulles, et que la trajectoiredéformée n’est pas exactement admissible ; nous avions alors appelé ce phénomène la «déviationdes contraintes nonholonomes».

Le principe de la correction de la déviation des contraintes nonholonomes est de réguler lescommandes uj, j ∈ {k + 1, . . . , n} suivant ces champs de vecteurs afin de les faire tendre vers0, par le biais des fonctions de perturbations vj .


Dans l’algorithme de la méthode de déformation de trajectoire (section 3.2.7), on calcule unedirection de déformation η⊥ engendrée par ces fonctions de perturbations, à laquelle s’ajouteéventuellement une direction de déformation η due aux obstacles. La trajectoire déformée estcalculée en ajoutant une fraction de cette direction de déformation η = η + η⊥ à la trajectoireinitiale :

∀s ∈ [0, S], q(s, τ + ∆τ) = q(s, τ) + ∆τ η(s, τ)

Ainsi, en donnant en entrée du processus de déformation une trajectoire qui n’est pas admis-sible, les itérations successives du processus vont tendre vers une trajectoire admissible. Nous enprésentons un exemple à la section suivante.

Exemple de déformation d’une trajectoire non-admissible

q(0) = (0, 0, π4)

q(0) = (0, 14.4, π4)

FIG. 3.16 – Déformations successives d’une trajectoire non-admissible

Soit un système de type unicycle et q = (x, y, θ) une configuration de ce système. Les deuxchamps de vecteurs pour ce système sont par exemple :

X1(q) =

cos θsin θ

0

X2(q) =

001

Un champ de vecteurs additionnel est par exemple X3(q) =

− sin θcos θ

0

.

Alors toute trajectoire admissible pour le système sur l’intervalle [0, 10] est telle que :

∀s ∈ [0, 10], q′(s) = u1(s)X1(q(s)) + u2(s)X2(q(s)) + u3(s)X3(q(s))

où u3 est nulle.


Dans cet exemple, la configuration initiale du système est q(0) = (0, 0, π/4). Les fonctionsd’entrée de la trajectoire initiale sont :

u1 = 1 u2 = 0 u3 = 1

Cette trajectoire, pour laquelle u3 n’est pas nulle, n’est donc pas admissible pour l’unicycle.Mais les déformations successives vont ramener cette entrée vers 0 et générer des entrées suivantles champs de vecteurs X1 et X2.

Dans cet exemple on suppose qu’il n’y a pas d’obstacles, et on fixe ∆τ à 0.2. La figure 3.16illustre les déformations successives de la trajectoire initiale, après respectivement 0, 3 et 15 ité-rations. La figure 3.17 présente les fonctions d’entrée de ces différentes trajectoires. On observela régulation vers 0 de l’entrée u3 suivant le champ de vecteurs additionnel X3 au fur et à mesuredes itérations. C’est à dire que la trajectoire après 15 itérations de déformation est admissiblepour le système d’équation (2.7) de l’unicycle.

−1

0

1

2

3

4

5

0 1 2 3 4 5 6 7 8 9 10

u1u2u3

u3u1

u2

−1

0

1

2

3

4

5

0 1 2 3 4 5 6 7 8 9 10

u1u2u3

−1

0

1

2

3

4

5

0 1 2 3 4 5 6 7 8 9 10

u1u2u3

FIG. 3.17 – Fonctions d’entrée de la trajectoire, respectivement aux itérations 0, 3 et 15

Singularités du système de la déformation Il faut noter que ce phénomène de déformationd’une trajectoire non-admissible est sujet à de nombreuses singularités qui apparaissent dansl’expression du système de l’équation (3.3). Si les matrices A ou B sont singulières, ce qui dé-pend de la trajectoire initiale et des champs de vecteurs du système, il n’existera pas de directionde déformation admissible qui diminue les entrées selon les champs de vecteurs non-admissibles.

Dans cet exemple, on a choisi de fixer : q(0) 6= 0 et u1 6= 0, pour éviter ce type de singularité.

Vers une méthode de planification de trajectoire pour systèmes nonholonomes ?

On pourrait envisager de coupler ce mécanisme de déformation d’une trajectoire non-admissibleavec la méthode permettant de contrôler la configuration finale de la trajectoire lors du processusde déformation, qui est présentée plus en détail au chapitre suivant.

Ces deux «contrôles» de la trajectoire laissent envisager le développement d’une méthodenumérique de planification de trajectoire pour systèmes nonholonomes. Cela pourrait être parti-culièrement intéressant pour des systèmes pour lesquels on ne possède pas de méthode analytique(les systèmes qui ne sont pas «différentiellement plats», par exemple), mais dont on possède uneexpression des champs de vecteurs.

Nous n’avons pas exploré toutes les potentialités de cette méthode de déformation en tantque méthode numérique de planification de trajectoire. Nous faisons cependant remarquer que


cette idée est assez proche de la méthode proposée par [Divelbiss ]. Les auteurs écrivent leproblème de la recherche d’une commande amenant le robot à une configuration finale sous laforme d’un système d’équations non-linéaires. Ce système est résolu en utilisant l’algorithmede Newton-Raphson, pour trouver les coordonnées de la commande dans l’espace des séries deFourier. Les obstacles sont pris en compte, comme des contraintes sur l’espace de recherche.Leur contribution réside donc dans une méthode numérique de planification de trajectoire poursystèmes nonholonomes, à laquelle notre méthode pourrait se comparer.

Enfin, il serait intéressant de vérifier que cette méthode numérique de planification de trajec-toire que nous suggérons respecte la propriété de commandabilité en temps petit :

Propriété 7. Soit q ∈ C une configuration et V (q) un voisinage de cette configuration. Uneméthode de guidage rend compte de la commandabilité en temps petit si l’ensemble des configu-rations accessibles à partir de q et sans sortir de V (q) est lui même un voisinage de q.

Cette propriété est nécessaire en pratique pour qu’une méthode de guidage puisse être utiliséedans un planificateur de trajectoire global qui tient compte des obstacles. Si par exemple oncherche à connecter deux configurations proches dans une zone fortement contrainte, il faut quele chemin qui les relie reste dans un voisinage de ces configurations.

3.6 ConclusionCe chapitre nous a permis d’évaluer dans quelle mesure les spécificités de notre approche

(spécificités du système et spécificités des applications telles que présentées au chapitre 1), nesont pas sans conséquences sur la réalisation d’une fonctionnalité particulière de la navigationautonome, à savoir l’évitement réactif d’obstacles. Ainsi la présentation des techniques existanteset de leur inadéquation à nos spécificités a motivé le développement d’une méthode originale dedéformation de trajectoire pour systèmes nonholonomes.

Nous avons ensuite présenté des techniques qui optimisent les calculs des interactions robot-obstacles nécessaires à cette méthode de déformation de trajectoire. Un des inconvénients decette méthode réside en effet dans son coût calculatoire. L’autre inconvénient est inhérent à l’uti-lisation des champs de potentiel basés sur la distance aux obstacles, et à leurs minima locaux. Cesdifférents aspects sont discutés dans le chapitre 6, en relation avec les résultats expérimentaux.

Enfin, nous avons montré que cette méthode générique, vue comme une méthode d’optimi-sation de trajectoire, peut être employée à d’autres fins que l’évitement d’obstacles.

Chapitre 4

Parking référencé sur des amers

4.1 Introduction

Dans le cadre de notre étude sur la navigation autonome de véhicules articulés à roues, nousappelons parking la phase finale du mouvement, quand celle-ci à lieu à proximité d’obstacles.

Au cours de l’exécution d’un mouvement, la phase de parking revêt un intérêt particulier carelle constitue généralement le but de la tâche de navigation. Il s’agit en effet souvent pour le robotde rejoindre une configuration finale spécifiée. La précision souhaitée varie d’une application àl’autre, mais on peut citer quelques exemples qui requièrent une très grande précision dans laphase de parking :

– le créneau parallèle pour une voiture,– l’amarrage d’un véhicule à remorque à un quai de débarquement (figure 4.1 à gauche),– le positionnement d’un instrument de mesure porté par un robot mobile (figure 4.1 à

droite).Dans ces différents exemples, la configuration de parking n’est pas définie comme une confi-

guration à atteindre dans un modèle de l’environnement, mais comme une configuration définierelativement à des éléments de l’environnement. Ainsi un créneau consiste à se garer entre 2voitures. Pour l’amarrage d’un camion il s’agit de se garer entre 2 camions et contre le quai.Enfin un robot mobile doté d’un instrument de mesure doit se positionner relativement à l’objetmesuré.

Au contraire, si la configuration de parking est définie de façon absolue dans un modèle del’environnement, on devra assurer l’exactitude de ce modèle et de la localisation du robot pourgarantir un parking précis.

Pour réaliser une tâche de parking dans laquelle la configuration de parking est définie rela-tivement à des éléments de l’environnement dénommés «amers»1 on doit se doter d’un capteurqui identifie et fournit des informations sur la position de ces éléments. Nous employons l’ex-pression de «parking référencé sur des amers» ou plus généralement de «mouvement référencésur des amers» pour désigner cette approche (parfois appelée «mouvement référencé capteur»).

1Terme de navigation maritime désignant un repère fixe et identifiable sans ambiguïté, utilisé pour la localisationpar triangulation.

54 Chapitre 4. Parking référencé sur des amers

FIG. 4.1 – À gauche l’amarrage d’un camion à un quai de débarquement de marchandises.À droite le positionnement d’un instrument de mesure par le robot Spirit (image NASA/JPL-Caltech).

Alors l’intérêt de définir une configuration de parking relativement à des éléments de l’en-vironnement est double. Tout d’abord cela permet de traiter les cas réalistes d’un modèle del’environnement imprécis et d’une localisation inexacte. Ensuite cela permet une adaptation dela tâche de parking à la variabilité du monde réel. En résumé le parking référencé sur des amerspermet de casser la corrélation entre précision du modèle de l’environnement et précision de latâche de parking.

La difficulté d’une tâche de parking référencé sur des amers dépend de plusieurs éléments telsque l’encombrement de l’environnement, la précision souhaitée, la qualité du modèle de l’envi-ronnement dans lequel on planifie le mouvement, ou encore la commandabilité du système. Ainsigarer une semi-remorque avec une marge de quelques centimètres est une tâche plus difficile quegarer un robot rond qui ne serait soumis à aucune contrainte cinématique dans un environnementdégagé.

Notre contribution porte sur le parking de systèmes articulés dans des environnements for-tement encombrés. Dans la section 4.2 nous présentons différentes approches traitant de ce pro-blème. Ensuite nous introduisons notre approche de tâche de parking référencé sur des amersdans la section 4.3. Les sections 4.4 et 4.5 présentent la résolution d’une tâche de parking. Enfinla section 4.6 développe au travers d’un exemple une tâche de parking référencé sur des amerspour un robot avec remorque.

4.2 État de l’art

L’idée de définir une configuration relativement à des éléments de l’environnement et de gé-nérer une trajectoire permettant de rejoindre cette configuration est la base de la «commanderéférencée capteur». Nous introduisons tout d’abord cette approche dans le cadre de l’asservis-sement visuel, qui a été utilisé pour résoudre le problème du parking référencé sur des amers

4.2 État de l’art 55

pour des véhicules nonholonomes. Nous présentons ensuite d’autres approches qui permettentde résoudre ce problème.

4.2.1 Asservissement visuel

Étant donné un robot dont la configuration est notée q et qui prend en entrée les commandesu, on souhaite générer une trajectoire qui amène ce robot à la configuration qpark. On dote lerobot d’un capteur qui fournit un vecteur d’observation o et on se donne alors une observationdésirée opark qui définit la configuration finale désirée qpark. On peut séparer les techniquesd’asservissement visuel en deux classes : l’asservissement visuel basé position dans lequel laconfiguration qpark est exprimée relativement à q dans l’espace cartésien, et l’asservissementvisuel basé image qui consiste à exprimer la variation de l’observation comme une fonction descommandes : o = f(q,u). Les commandes du système qui amènent le robot en qpark sont alors,lorsque Jf est inversible, u = kJ−1

f (opark−o), où Jf est la jacobienne de f évaluée en q, appeléematrice d’interaction, et k le gain de la loi de commande.

Les travaux de [Espiau ] formalisent cette approche, développée initialement pour la com-mande de bras manipulateurs. Ils ont été étendus aux systèmes nonholonomes par [Tsakiris ]et appliqués à un robot unicycle, en introduisant un degré de liberté supplémentaire par unerotation de la caméra.

Dans [Wei ] et [Wei ], les auteurs proposent une méthode d’asservissement visuel d’ununicycle avec une caméra panoramique et sans information de profondeur. Ces travaux sontbasés sur [Hamel ] et ils supposent que le système est capable de tourner sur lui même. Leurextension à des véhicules articulés n’est donc pas directement envisageable.

Dans [Usher ], l’auteur présente l’asservissement visuel avec une caméra panoramiqued’un véhicule de type voiture. Il repose sur l’enchaînement de deux lois de commandes, l’uneamenant le véhicule sur une droite orientée passant par la position finale et l’autre guidant lerobot le long de cette droite. Cette technique ne peut pas être généralisée à des systèmes pluscomplexes tels que des véhicules à remorque.

4.2.2 Planifications successives

Mobile Camera Space Manipulation

Une autre approche de parking référencé capteur repose aussi sur la perception visuelle :MCMS (Mobile Camera Space Manipulation) [Seelinger ]. Comme dans l’asservissement vi-suel, la configuration finale est spécifiée par une observation désirée. En fonction de l’observationcourante, la configuration finale est calculée relativement à la configuration courante du robot.Une trajectoire est planifiée vers cette configuration et exécutée par la suite. Quand une certaineportion de la trajectoire a été exécutée, le robot s’arrête et répète l’étape précédente. On réitèrele processus jusqu’à atteindre une précision souhaitée.

L’inconvénient de cette approche est qu’elle oblige le robot à s’arrêter et qu’elle fait de nom-breuses fois appel à un planificateur de trajectoire.


Localization-Planning-Execution Cycles

L’approche décrite dans [Paromtchik ] et [Laugier ] repose sur l’exécution successive decycles de localisation, planification et exécution de trajectoire. Cette méthode a été appliquée auparking parallèle d’un robot de type voiture. La place de parking est identifiée par des capteurs etles commandes du système sont des fonctions sinusoïdales paramétrées par les caractéristiquesde la place (largeur, profondeur, etc.). Puis le mouvement est exécuté et ce cycle est répété jusqu’àatteindre une précision désirée.

Cette approche est spécifique à certaines «géométries» de place de parking (les articles netraitent que du cas du parking parallèle) et n’est pas aisément généralisable à des véhicules sou-mis à des contraintes cinématiques plus complexes, comme les véhicules à remorque.

4.3 Tâche de parking référencé sur des amers

Nous proposons de définir une tâche de parking qui s’applique à tout système nonholo-nome dont les trajectoires sont solutions de (2.7), sans condition sur la géométrie de la placede parking, et qui s’exécute sans que le robot ne doive s’arrêter. Comme dans le chapitre 3on suppose que l’on possède une trajectoire initialement planifiée. Ces résultats ont été publiésdans [Lefebvre ].

D’une manière similaire à l’approche par asservissement visuel, nous définissons un motif deparking noté M associé à un capteur. Ce motif de parking provient d’un modèle de l’environne-ment et représente la perception du capteur lorsque le robot est en configuration de parking. Alorsà partir des observations P de ce capteur, le but est de repérer ce motif dans l’environnement réelpuis de déformer la trajectoire initiale de façon à ce qu’elle se termine en une configuration notéeqpark. La configuration de parking qpark est telle que depuis cette configuration l’observation Pest égale au motif de parking M.

4.3.1 Définitions

Position du capteur

Étant donné une configuration q du robot, on note c(q) ∈ SE(2) la position (position etorientation) du capteur dans le plan lorsque le robot est dans cette configuration.

La position c(q) définit un repère dans lequel on peut exprimer une position. Ainsi soit r ∈SE(2) un repère, la position de r par rapport à c(q) est notée r/c(q).

Amer

Un amer est un sous-espace deW repérable par le capteur, et dont la position est définie parun ensemble de k paramètres. On note l ∈ Rk le vecteur constitué des paramètres de l’amer. Onsuppose que l’environnement contient l amers repérables par le capteur, que l’on identifie par unindice : ∀i ∈ {1, .., l}, li est un amer.

4.3 Tâche de parking référencé sur des amers 57

Par exemple, un amer peut être un point de l’espace de travail, une droite, la position d’unobjet quelconque, etc. Pour un point, le nombre de paramètres nécessaires pour le repérer dansle plan est 2, et l’on a l ∈ R2.

Perception d’un amer

Étant donné une configuration q du robot et un amer l visible, la perception de cet amerpar le capteur est notée p/c(q). Ce sont les paramètres définissant la position de l’amer l dans lerepère c(q), et p/c(q) ∈ Rk.

Pour chaque type d’amer, on peut définir une fonction f qui donne les paramètres définissantla position d’un amer l dans le plan, à partir des paramètres z donnant la position de cet amerrelativement à la position c du capteur :

f : Rk × SE(2) → Rk

(z, c) 7→ l = f(z, c)(4.1)

On note p les paramètres de l’amer l calculés à partir de la perception p/c de cet amer réaliséedepuis la position c du capteur :

p = f(p/c, c) (4.2)

Alors en l’absence d’erreur sur p/c et sur c on a :

l = p (4.3)

On note P le vecteur constitué des paramètres définissant les positions dans le plan des pamers perçus :

P = (p1, ..,pp)T

où p ≤ l.

Position de parking du capteur

On définit la position de parking du capteur comme la position du capteur lorsque le robotest en configuration de parking.

cpark = c(qpark)

Motif de parking

Un motif de parking pour un capteur est un vecteur M = (m1, ..,mm)T constitué de mamers dont les paramètres sont exprimés relativement au capteur en position de parking cpark.Soit m ∈ Rk un de ces amers et l les paramètres définissant la position de cet amer dans le plan.En utilisant l’équation (4.2) on trouve :

l = f(m, cpark) (4.4)


motif de parking

motif de parking

capteur

FIG. 4.2 – Motifs de parking. Un motif de parking est un vecteur M d’amers définis relativementau capteur qui fournit une observation de ces amers. Ici les amers sont des segments.

Soit maintenant une perception p du capteur telle que p = l. Alors on a p/cpark = m, c’est àdire que les paramètres d’un amer du motif de parking sont égaux aux paramètres de la perceptionde cet amer par le capteur depuis la position de parking cpark.

La figure 4.2 présente des exemples de motifs de parking constitués de segments pour deuxrobots différents. En pratique un motif de parking M est calculé en positionnant le capteur dansla configuration désirée par rapport aux amers, puis en relevant ou en simulant la perceptionfournie par le capteur.

4.3.2 Formulation du problèmeÀ partir de ces définitions on spécifie une tâche de parking référencé sur des amers comme

un processus itératif, qui consiste successivement à :– repérer le motif de parking M dans l’environnement à partir d’un vecteur d’observation

noté P,– calculer la configuration de parking qpark,– déformer la trajectoire courante de façon à ce que la configuration finale se rapproche

de qpark.Les deux premiers points sont traités dans la section 4.4 et le dernier point est présenté dans

la section 4.5. La figure 4.3 illustre les différentes étapes de ce processus. Une trajectoire estplanifiée dans une carte de l’environnement, et un motif de parking est défini relativement à uncapteur situé sur la remorque du robot (voir la figure 4.2). Puis la trajectoire est itérativementdéformée de façon à ce que la configuration finale rejoigne qpark.

Une tâche de parking référencé sur des amers prend donc en entrée un motif de parking Met une estimation de l’incertitude sur qpark notée Vqpark . À l’initialisation, la configuration deparking est égale à la configuration finale de la trajectoire planifiée q(S). La configuration deparking est donc recherchée dans la zone définie par Vqpark et centrée en q(S).

Ensuite à chaque itération du processus, en fonction d’un vecteur d’observation P/c(q), dela configuration courante q du robot et de la trajectoire planifiée q(s), s ∈ [0, S], la tâche de

4.3 Tâche de parking référencé sur des amers 59

carte

q(0)

motif deparking

q(S)qpark

perception

FIG. 4.3 – Tâche de parking référencé sur des amers. À chaque itération du processus, la trajec-toire est déformée de façon à ce que la configuration finale se rapproche de qpark.

trajectoire q(s)

qparkVqpark

trajectoire q(s)q

parking Mmotif de

tâche de parking

référencé sur desamers

observation P/c(q)

FIG. 4.4 – Diagramme d’une tâche de parking référencé sur des amers


parking produit en sortie une nouvelle valeur de qpark et une nouvelle trajectoire admissible telleque celle-ci se rapproche de qpark. La figure 4.4 illustre les entrées et sorties d’une tâche deparking référencé sur des amers.

4.4 Calcul de la configuration de parkingEn l’absence d’information sur le motif de parking, qpark est définie comme la configuration

finale de la trajectoire planifiée : qpark = q(S). Ensuite la perception P des amers correspondantau motif de parking fournit des informations pour mettre à jour qpark.

4.4.1 Modélisation probabilisteEn pratique on doit tenir compte de plusieurs sources d’erreur :– la position du capteur c(q) n’est pas connue exactement,– le capteur ne fournit pas une observation exacte de la position relative pi

/c d’un amer,– les amers m du motif de parking ne sont pas définis avec exactitude.Pour tenir compte de ces imprécisions, on modélise les variables précédentes par des va-

riables aléatoire réelles. On suppose que les bruits de mesure sont distribués suivant une loinormale de moyenne nulle. On note alors pour tout vecteur réel Y de taille n :

la vraie valeur y

la valeur mesurée y = y + εy

le bruit de mesure εy ∼ N (0,Vy)

la matrice de covariance Vy = E(εyεTy )

Soit alors Z un vecteur de variables aléatoires réelles de taille p et g une fonction de Rn

dans Rp telle que : Z = g(Y ). On a z = g(y) et on approxime z par :

z = g(y)

En linéarisant la fonction g autour de y on trouve la variance de Z notée Vz :

Vz = JgVyJTg (4.5)

où Jg = dgdy

∣∣∣y

est la matrice jacobienne de taille p× n de g évaluée en y.

Configuration et position

La configuration courante du robot est modélisée par q ∼ N (q,Vq). La position courantedu capteur est donc :

c = c(q) (4.6)Vc = JcVqJ

Tc (4.7)

4.4 Calcul de la configuration de parking 61

L’estimation initiale de qpark est q(S). Sa variance est une entrée du problème :

qpark = q(S)

Vqpark

L’estimation de cpark est alors :

cpark = c(qpark)

Vcpark = JcVqparkJTc

Perception

La perception de l’amer li depuis la configuration courante est modélisée par pi/c(q) ∼

N (pi/c(q),Vpi

/c(q)). Cette valeur est fournie par le capteur, et on remarque que les pi

/c(q) sontindépendante deux à deux. En utilisant l’équation (4.2), on trouve les paramètres donnant l’esti-mation de la position dans le plan de cet amer :

pi = f(pi/c, c)

Vpi = Jf/pVpi/c(q)

JTf/p + Jf/cVcJ

Tf/c (4.8)

où Jf/p est la jacobienne de f par rapport à p et Jf/c est la jacobienne de f par rapport à c.

Motif de parking

Le motif de parking est le vecteur de variables aléatoires réelles noté M ∼ N (M,VM).C’est une entrée du problème. On suppose que les mi sont indépendants deux à deux et donc quela matrice VM est bloc-diagonale.

4.4.2 Mise en correspondanceLe robot arrivant à proximité de la fin de la trajectoire qpark, le capteur fournit une percep-

tion P de p amers, calculée à partir de l’équation (4.8). Il s’agit alors de déterminer quels sontles amers perçus pi qui correspondent à des amers du motif de parking M.

Soit mj un amer du motif de parking. En supposant que l’amer correspondant existe dansl’environnement, les paramètres permettant de repérer sa position dans le plan sont notés lj etsont donnés par l’équation (4.4) :

lj = f(mj, cpark)

Vlj = Jf/mVmjJTf/m + Jf/cVcparkJT

f/c (4.9)

lj est donc la prédiction de la perception de l’amer mj , qui est à comparer avec les perceptionsdu capteur pi.


On note zij la différence entre lj et pi :

zij = (pi − lj)

Si l’observation pi correspond au motif mj , on a zij = 0. Cependant ces valeurs vraies sontinaccessibles et l’on ne peut traiter que des estimations de ces valeurs. Et en raison des erreursde mesures, on n’a jamais exactement pi = lj , et donc zij 6= 0. Alors en remarquant que pi et lj

sont des variables indépendantes, on a :

zij = (pi − lj)

Vzij = Vpi + Vlj (4.10)

On utilise la distance de Mahalanobis pour mesurer la vraisemblance de la mise en corres-pondance de pi avec mj . On note Dij la valeur telle que :

(Dij)2 = zijTVzij zij (4.11)

Étant donné que z suit une distribution de loi normale, (Dij)2 suit une distribution du χ2k

où k est la dimension du vecteur z. Si l’on souhaite que la probabilité de rejeter une mise encorrespondance alors que celle-ci est vraie soit inférieure à α ∈ [0, 1], on va fixer le seuil λα telque p(X < λα) = 1− α, où X est distribué suivant χ2

k. En pratique, on prend par exemple α =0.05, ce qui donne pour k = 2 : λα = 5.99.

Si (Dij)2 < λα, alors on note uij = (pi, mj) le couple mis en correspondance et on noteU l’ensemble des u couples qui vérifient (Dij)2 < λα, parmi les (m.p) couples possibles. L’al-gorithme 2 décrit la mise en correspondance de p observations P avec m amers du motif deparking M.

4.4.3 Mise à jour de la position de parking du capteurL’information apportée par la mise en correspondance de tout ou partie du motif de parking

avec les perceptions permet de mettre à jour la position de parking du capteur cpark.En adoptant le formalisme du filtre de Kalman étendu, on utilise l’équation (4.9) comme fonc-

tion d’observation, que l’on va linéariser. Alors l’innovation z est donnée par l’équation (4.10).En notant cpark

0 l’estimation initiale de la position de parking du capteur et Vcpark0

sa matrice decovariance, on calcule alors récursivement pour chaque couple uk = (pi, mj) de U, une nouvelleestimation cpark

k :

cparkk+1 = cpark

k + Kck zk (4.12)

Vcparkk+1

= (I −KckJc)Vcpark

k

avec le gain de Kalman :

Kck = Vcpark

kJTc .(JcVcpark

kJTc + JmVmjJT

m + Vpi)−1


Algorithme 2 Mise en correspondance des éléments perçus et du motif de parkingDonnées d’entrée :– valeur α de la probabilité de rejeter une mise en correspondance alors que celle ci est valide.– configuration courante du robot q– p observations pi

/c(q), i ∈ {1, ...p}– m amers mj, j ∈ {1, ...,m} du motif de parking M– qpark et Vqpark

Données de sortie :– ensemble U des couples uij mis en correspondanceAlgorithme

λα ← p(X < λ) = 1− αqpark ← q(S)cpark ← c(qpark)U← {∅}for chaque perception pi

/c(q) docalculer les pi (équation 4.8) de obsvect.

end forfor chaque perception pi de P do

D2 ←∞m? ← ∅for chaque amer mj du motif de parking M do

lj ← équation 4.9, mj et cpark

zij ← équation 4.10, lj , pi

(Dij)2 ← équation 4.11, zij

if ((Dij)2 < λα ∧ (Dij)2 < D2 ) thenm? ←mj

D2 ← (Djk)2

end ifend forif m? 6= ∅ then

insérer {pi,m?} dans Uend if

end for


et l’innovation :zk = (pi − f(mj, cpark

0 ))

avec Jc = dfdc

∣∣cpark0

la jacobienne de f par rapport à c évaluée en cpark0 , et Jm = df

dm

∣∣mj la

jacobienne de f par rapport à m évaluée en mj .L’estimation de la position de parking calculée après les u mises à jour successives est

notée cparku . Elle correspond à l’estimation de variance minimale (c’est à dire minimisant :

trace(Vcparku

)).

Remarque sur l’indépendance des variables

Cette écriture de l’équation (4.12) n’est possible que sous l’hypothèse que les différentesperceptions pi sont indépendantes. Or l’équation (4.8) montre que ces différentes perceptionssont liées par la configuration courante du robot.

On note P = (p1, ...,pu) le vecteur composé des u perceptions mises en correspondance.Ce vecteur est obtenu en définissant la fonction F qui correspond à la fonction (4.8) pour uvariables :

P = F (P/c, c) p1

...pu

=

f(p1/c, c)...

f(pu/c, c)

Soit VP la matrice de covariance de P, calculée de façon similaire à Vpi dans l’équa-

tion (4.8), en remplaçant f par F . La dépendance des différentes perceptions s’exprime par lefait que la matrice VP n’est pas bloc-diagonale, et l’approche de mise à jour récursive n’est doncpas immédiatement utilisable.

Pour résoudre ce problème, deux solutions sont possibles :– diagonaliser la matrice de covariance Vp. Nous renvoyons par exemple à [Bar-Shalom ]

pour une présentation de la décorrélation des observations.– faire une mise à jour par l’équation (4.12) en une seule étape plutôt que de façon séquen-

tielle. Pour cela on définit le vecteur Z = P−L où L = (l1, ..., lu) est le vecteur constituédes u prédictions d’observations calculé par la «version à u variables» de l’équation (4.9),de façon similaire à la fonction F définie ci-dessus. L’équation (4.12) reste identique, seulsla taille du vecteur d’innovation et le calcul du gain changent. En pratique c’est la solutionque nous adoptons, car la dimension du vecteur P restant petite, cela ne pénalise pas lestemps de calcul.

4.4.4 Mise à jour de la configuration de parking

À partir de la nouvelle estimée cparku de la position de parking du capteur, on peut mettre à

jour l’estimation de la configuration de parking qpark. La fonction d’observation est ici donnée


par l’équation 4.6. L’innovation vaut (cparku − cpark

0 ). En notant qpark0 = q(S) l’estimation initiale

de la configuration de parking, on a :

qpark1 = cpark

0 + Kq0 (cpark

u − cpark0 )

Vqpark1

= (I −Kq0 Jq)Vqpark

0(4.13)

avec le gain de Kalman :

Kq0 = Vqpark

0JTq (JqVqpark

0JTq + Vcpark

u)−1

où Jq = dc(q)dq

∣∣∣qpark

0

est la jacobienne de la fonction c = c(q) évaluée en qpark0 .

4.4.5 Extension à plusieurs capteursOn peut naturellement étendre ce formalisme au cas où le robot possède plusieurs capteurs

pour la tâche de parking référencé sur des amers. Soit nc le nombre de capteurs utilisés. Pourchaque capteur ci :

– on définit un motif de parking associé Mi, avec i ∈ {1, .., nc}. On note Mi l’estimation dece motif de parking,

– on note Pi la perception des amers par le capteur ci,– on note ci(q) la fonction qui donne la position du capteur ci à partir de la configuration du

robot.Pour calculer les estimées cpark

i de chaque capteur à partir des perceptions, on utilise les mêmesétapes de mise en correspondance (section 4.4.2) et de mise à jour de la position de parking ducapteur (section 4.4.3).

Seule l’étape de mise à jour de la configuration de parking doit être reformulée, de façon àtenir compte des nc estimées des positions de parking cpark

i . On pourrait appliquer récursive-ment l’étape définie à la section 4.4.4, mais on retrouve le même problème que lors de la miseà jour de la position de parking à partir de plusieurs couples amer-perception quand les diffé-rentes perceptions ne sont pas indépendantes entre elles. En appliquant récursivement l’étapedéfinie à la section 4.4.4, on négligerait la dépendance entre les cpark

i , i ∈ {1, ..., nc}, qui sonttoutes des fonctions de la configuration courante du robot q et de l’estimation de la configurationfinale qpark

0 .Pour résoudre ce problème, on fait le calcul en une seule étape. On note C(q) le vecteur

colonne constitué de l’ensemble des positions des différents capteurs et VC sa matrice de cova-riance :

C(q) = (c1(q), c2(q), .., cnc(q))T

L’estimée initiale Cpark0 de ce vecteur colonne est calculée à partir de qpark

0 . La valeur miseà jour Cpark

u est donnée par l’étape de mise à jour des positions de parking de chaque capteur,comme présentée à la section 4.4.3.

Alors on calcule l’estimée de la configuration de parking en écrivant :

qparknc = qpark

0 + Kq(Cparku − Cpark

0 )


et le gain de Kalman vaut :

Kq = Vqpark0

JTq (JqVqpark

0JTq + VCpark

u)−1

avec Jq = dC(q)dq

∣∣∣qpark

la jacobienne de C(q) par rapport à q évaluée en qpark.

4.5 Déformation de trajectoireLa section précédente a montré comment calculer la configuration de parking à partir des

informations fournies par les capteurs à propos des motifs de parking. Il s’agit maintenant demodifier la trajectoire planifiée de façon à ce que celle-ci se termine en qpark

nc .Nous utilisons pour cela la méthode de déformation de trajectoire nonholonome présentée au

chapitre 3, en ajoutant une contrainte sur les conditions aux limites énoncées à la section 3.2.4.Nous avions pensé déformer la trajectoire en utilisant un potentiel attractif vers la configura-

tion de parking, en tirant parti des remarques de la section 3.5 sur l’optimisation de la trajectoiresur d’autres critères que la distance aux obstacles. L’utilisation des conditions aux limites a étéretenue afin de s’assurer un meilleur contrôle de la déformation sous les effets conjugués desobstacles et de la tâche de parking.

4.5.1 Conditions aux limitesNous rappelons la condition sur la fin de l’intervalle de déformation quand il s’agit de défor-

mer la trajectoire afin que celle-ci s’éloigne des obstacles :

η(S, τ) = η(S, τ) + η⊥(S, τ) = 0

qui peut se mettre sous la forme de l’équation (3.23) :

Lλ = −η⊥(S, τ)

Ces conditions aux limites sont valables même quand la fin de l’intervalle de déformationest la dernière configuration de la trajectoire, et elles imposent que q(S) reste inchangée aprèsdéformation. Dans une tâche de parking référencé sur des amers on désire au contraire que q(S)se rapproche de qpark

nc .Si l’on note δq la différence entre qpark

nc et qpark0 :

δq = (qparknc − qpark

0 )

la condition aux limites imposant que q(S) après déformation se rapproche de qparknc est :

η(S, τ) = η(S, τ) + η⊥(S, τ) = δq

Ce qui peut se mettre sous la forme :

Lλ = −η⊥(S, τ) + δq (4.14)

4.6 Application au robot Hilare2 avec remorque 67

On calcule alors les coordonnées dans la base E de la direction de déformation associée enutilisant l’équation (3.24) :

λE = −P (LEP )+(η⊥(S, τ)− δq)− (Ip − P (LEP )+LE)PP T µE (4.15)

Cette nouvelle condition aux limites est la seule modification apportée à l’algorithme de ladéformation présenté à la section 3.2.7.

4.6 Application au robot Hilare2 avec remorqueNous illustrons cette méthode par un exemple d’application avec le robot Hilare2 avec re-

morque, représenté sur la figure 4.5.

télémètres laser

d2

d3(x, y)

φd1

θ

FIG. 4.5 – Le robot Hilare2 avec sa remorque

Le robot possède 4 variables de configuration : q = (x, y, θ, φ). x et y donnent la position ducentre de l’essieu central du robot, exprimés en mètre. θ est l’orientation de l’axe perpendiculaireà l’essieu, et φ est l’orientation de la remorque par rapport au robot, exprimés en radians. d1 estla distance entre le centre du robot et le point d’attache de la remorque, d2 est la distance entrece point d’attache et le capteur situé sur la remorque et d3 est la distance entre le centre du robotet le capteur situé à l’avant.

4.6.1 Tâche de parking : parking relativement à un quai de débarquementLa tâche de parking consiste à positionner la remorque dans un emplacement représentant

un quai de débarquement de marchandise pour des camions, visant à reproduire la situationreprésentée sur la figure 4.1. On définit alors un motif de parking qui spécifie cet emplacementpar rapport au capteur situé sur la remorque, que l’on note c. Lors de nos expériences, nousutiliserons un télémètre laser SICK, qui fournit après traitement les segments perçus dans unplan horizontal. Aussi choisissons nous dans cet exemple d’utiliser comme amers des segments,que l’on représentera par leurs droites porteuses afin d’être robuste aux occlusions.


Position du capteur

Le capteur situé sur la remorque est noté c. Soit (xc, yc, θc)T = c(q) la situation du capteur

lorsque le robot est dans la configuration q : xc

yc

θc

=

x− d1 cos θ − d2 cos(θ + φ)y − d1 sin θ − d2 sin(θ + φ)

θ + φ + π

(4.16)

Motif de parking

Le motif de parking est constitué de trois segments [AiBi], i ∈ {1, 2, 3}, représentant un quaide débarquement. Les paramètres d’une droite porteuse (AiBi) sont notés (oi, ρi) ∈ ([−π, π], R).Les droites sont orientées et ~ni est le vecteur unitaire normal à la droite et pointant vers l’espace

AiO

oi

~ni

Bi

Ii

ρi

~i

~j

FIG. 4.6 – Les paramètres (oi, ρi) de la droite porteuse du segment [AiBi]. oi = (~i, ~ni) et ρi =<~IiO,~ni >.

libre. Alors oi est l’angle entre ce vecteur et l’axe des abscisses et on a :

~ni = (cos oi, sin oi)T

Soit I le point de concours de la droite (AiBi) et de la droite de vecteur directeur ~ni passantpar O. Alors ρi est :

ρi =<−→IiO,~ni >

L’équation de la droite (AiBi) est alors :

x cos oi + y sin oi + ρi = 0

La figure 4.6 illustre le calcul des paramètres d’une droite porteuse orientée et la figure 4.7donne les détails de construction du motif de parking utilisé dans cet exemple.


FIG. 4.7 – Les paramètres des droitesporteuses constituant le motif de par-king, exprimés relativement au cap-teur. Les valeurs des paramètres desamers exprimés dans le repère c sont :m1 = (π

2, 0.5)T , m2 = (π, 0.27)T et

m3 = (−π2, 0.5)T .

A3 B2

A2

B1

A1

~n3

~n2c

B3

~n1

m1

m2

remorque

m3Capteur c

Changement de repère des paramètres d’un amer

Le capteur étant dans la situation c = (xc, yc, θc)T , les paramètres d’un amer exprimés rela-

tivement au capteur sont notés p/c = (oc, ρc)T . Alors les paramètres de cet amer exprimés dans

le repère d’origine O sont notés p = (oO, ρO)T et ont pour valeur :

oO = oc + θc

ρO = ρc − xc cos(θc + oc)− yc sin(θc + oc) (4.17)

Cette équation définit la fonction f utilisée dans les équations (4.8) et (4.9) :

p = f(p/c, c)

l = f(m, c)

La figure 4.8 illustre ce changement de repère.

Valeurs initiales des incertitudes

On formule les hypothèses suivantes :– les amers du motif de parking sont indépendants deux à deux et la matrice VM est donc

bloc-diagonale. Cette matrice est une donnée d’entrée de la tâche de parking.– les perceptions p/c des amers par le capteur dans la position c sont indépendantes deux à

deux et la matrice Vp/cest donc bloc-diagonale.

– la configuration finale q(S) de la trajectoire planifiée est l’estimée initiale de la configura-tion de parking qpark

0 . La matrice de covariance Vqpark0

est diagonale.


oc

oOO

~nc

~nO

ρOθc

ρc

(xc, yc)

FIG. 4.8 – Changement de repère des paramètres d’une droite. La droite est perçue depuis la si-tuation c = (xc, yc, θc)

T , et ses paramètres dans ce repère sont notés (oc, ρc)T . Les paramètres de

cette droite dans le repère d’origine O sont notés (oO, ρO)T , et sont calculés par l’équation (4.17).

Dans cet exemple, on ajoute l’hypothèse que l’incertitude sur la configuration courante du robotest nulle, afin de simplifier les calculs. Cette hypothèse est bien entendu levée lors des expé-riences.

Incertitude sur les paramètres du motif de parking Pour chaque élément m = (o, ρ)T dumotif de parking, la matrice de covariance de m est :

Vm =

((0.1

3)2 0

0 (13)2

)

Incertitude sur les amers perçus Dans cet exemple, on considère que l’incertitude d’un amerperçu est constante, quelle que soit sa position par rapport au capteur. Et comme l’incertitudesur la configuration courante du robot est nulle, on a Vp/c

= Vp. C’est à dire que l’incertitudedes paramètres d’un amer perçu exprimé relativement au capteur est égale à l’incertitude desparamètres de cet amer exprimé dans le plan. Soit p = (op, ρp)T les paramètres d’un amerperçu. On a :

Vp =

((0.1

3)2 0

0 (13)2

)

Incertitude sur l’estimée initiale de la configuration de parking L’estimée initiale de laconfiguration de parking est la configuration finale de la trajectoire planifiée dans un modèle de


l’environnement : qpark0 = q(S). La matrice de covariance de qpark

0 est notée Vqpark0

, et vaut :

Vqpark0

=

(1

3)2 0 0 00 (1

3)2 0 0

0 0 (0.43

)2 00 0 0 (0.4

3)2

Les valeurs de la matrice Vqpark

0définissent un intervalle de confiance pour chacune des

variables de configuration, centré sur qpark0 . On a choisi ces valeurs de telle façon que l’intervalle

de confiance à 99% pour les variables x et y soit de plus ou moins 1 autour de la valeur moyenne(xpark ∈ [xpark − 1, xpark + 1]), et de plus ou moins 0.4 rad pour les variables θ et φ.

En simplifiant, on peut dire que si la configuration de parking calculée à l’issue de la tâchede parking n’est pas dans cet intervalle, elle est rejetée car jugée trop peu vraisemblable. Enréalité on utilise la distance de Mahalanobis pour mesurer la vraisemblance de la configurationde parking calculée par rapport à son estimée initiale.

4.6.2 Résolution de la tâche de parkingModèle de l’environnement

O

amers du motif de parking

q(S) = qpark0 q(0)

FIG. 4.9 – Modèle de l’environnement. Le robot est en configuration initiale q(0) et une trajec-toire est planifiée vers la position de parking calculée dans ce modèle.

Dans cet exemple, le modèle de l’environnement dans lequel est planifiée la trajectoire contienttrois amers l1, l2, l3, qui correspondent au motif de parking défini plus haut. Les paramètres deces amers exprimés dans le repère d’origine O sont :

l1 = (π

2, 0.5) l2 = (π, 10.27) l3 = (−π

2, 0.5)


Dans ce modèle, la situation du capteur en position de parking est donc : cpark0 = (−10, 0, 0)T .

Et une configuration finale est par exemple q(S) = (−8.383, 0, 0, 0)T , car elle vérifie c(q(S)) =cpark

0 . On note qpark0 l’estimée initiale de la configuration de parking, qui vaut donc q(S).

La configuration initiale de robot étant q(0) = (0, 0, 0, 0)T , une trajectoire en ligne droite estplanifiée de q(0) à q(S).

La figure 4.9 représente le modèle de l’environnement, dans lequel les amers du motif deparking sont positionnés. Une trajectoire rectiligne en marche arrière est planifiée, depuis q(0)jusqu’à la configuration de parking q(S) calculée dans ce modèle.

Environnement perçu

Dans notre exemple la perception simulée du capteur ne correspond pas au modèle de l’envi-ronnement. En effet on suppose qu’un seul amer est détecté, et ses paramètres exprimés dans lerepère O ne sont strictement égaux à aucun des paramètres des amers du modèle de l’environne-ment. Les paramètres de l’amer perçu exprimés dans le repère O sont : p1 = (π+ π

8, 10.27 cos π

8).

C’est à dire que cette perception correspond à l’amer l2 ayant subi une rotation d’angle π8

et decentre (−10.27, 0).

La figure 4.10 représente l’environnement perçu par le robot qui est visiblement différent dumodèle de l’environnement.

O

amer perçu q(0)q(S) = qpark0

FIG. 4.10 – Environnement perçu par le capteur. Un seul des trois amers du motif de parking estprésent, et ses paramètres sont différents.

Mise en correspondance

On calcule la différence entre la perception et les paramètres estimés d’un amer du motif deparking et on applique l’algorithme 2 de mise en correspondance, présenté à la section 4.4.2.


L’innovation est zij = pi − lj , avec i = 1 etj ∈ {1, 2, 3}.L’unique couple amer-perception retourné par l’algorithme est bien le couple u12 = (p1, l2).

La valeur de la distance de Mahalanobis calculée par l’équation (4.11) pour ce couple est de 5.94,qui est la plus faible valeur parmi les trois couples possibles.

Mise à jour de la position de parking du capteur

Avec cette information de mise en correspondance, on peut mettre à jour la position de par-king du capteur en procédant comme indiqué à la section 4.4.3. L’équation (4.12) de mise à jours’écrit :

cpark1 = c(qpark

0 ) + Kc(p1 − l2)

Vcpark1

= (I −Kc0Jc)Vcpark

0

avec Vcpark0

calculée par l’équation (4.6) et la donnée de Vqpark0

. Et le gain Kc0 est :

Kc = Vcpark0

JTc .(JcVcpark

0JTc + JmVm2JT

m + Vp1)−1

où Jc et Jm sont les jacobiennes de la fonction f définie dans cet exemple par l’équation (4.17),respectivement par rapport à c et évaluée en cpark

0 , et à m et évaluée en m2.La position de parking du capteur mise à jour est cpark

1 = (−9.739,−0.477, 3.511)T .

Mise à jour de la configuration de parking

On peut maintenant mettre à jour la configuration de parking du robot en procédant commeindiqué à la section 4.4.4. L’équation (4.13) de mise à jour de la configuration s’écrit :

qpark1 = qpark

0 + Kq(cpark1 − c(qpark

0 ))

Vqpark1

= (I −KqJq)Vqpark0

avec Jq la jacobienne de la fonction q 7→ c(q), évaluée en qpark0 .

La configuration de parking mise à jour est qpark1 = (−8.227, 0, 0.174, 0.174)T , à comparer

avec l’estimation initiale de la configuration de parking :qpark0 = (−8.383, 0, 0, 0). La matrice de

covariance Vqpark1

ainsi calculée est :

Vqpark1

=

0.044 0 0 0

0 0.057 0.003 −0.0030 0.003 0.009 −0.0080 −0.003 −0.008 0.009

On observe que l’incertitude sur la configuration de parking a diminué par rapport à sa valeur

initiale.


Déformation de la trajectoire

La configuration de parking calculée qpark1 n’est pas égale à la configuration finale de la

trajectoire. On va donc déformer la trajectoire de façon à ce que la configuration finale se rap-proche de la configuration de parking, en utilisant les résultats présentés à la section 4.5. Danscet exemple on ne tient pas compte des collisions avec les obstacles.

0.15

0.2

0 2 4 6 8 10 12 14 16 18−0.05

0

0.05

0.1

φ

xyθ

FIG. 4.11 – Direction de déformation η de la tâche de parking

On note δq = qpark1 − qpark

0 . Étant donné que la trajectoire initiale est parfaitement admis-sible pour le système, la direction de déformation η⊥ de correction de la dérive des contraintesnonholonomes est nulle sur tout l’intervalle de déformation (voir la section 3.2.6). La contraintesur la fin de l’intervalle de déformation exprimée par l’équation (4.14) s’écrit donc :

Lλ = δq

où λ = 0 car les obstacles n’ont pas d’influence.On calcule alors le vecteur λ des coordonnées de la direction de déformation η en utilisant

l’équation (4.15).La figure 4.11 présente la direction de déformation η calculée. On vérifie que cette direction

de déformation est telle que η(S) = δq.En appliquant maintenant une partie de cette direction de déformation à la trajectoire initiale,

on trouve une trajectoire déformée dont la configuration finale se rapproche de qpark1 :

∀s ∈ [0, S], q(s)← q(s) + ∆τ η(s)

Dans cet exemple on fixe ∆τ à 0.2.Après dix itérations, la distance entre la configuration finale et la configuration de parking est

inférieure à 10−2. La figure 4.12 représente cette trajectoire déformée.

4.7 Conclusion 75

O

q(0)q(S) = qpark1amer per cu

FIG. 4.12 – Trajectoire déformée par la tâche de parking référencé sur des amers

4.7 ConclusionLa section précédente nous a permis de définir tous les éléments nécessaires à une tâche de

parking dans un exemple réaliste. Les résultats expérimentaux obtenus lors de la réalisation decet exemple avec le robot réel Hilare2 sont présentés dans le chapitre 6.

Pour conclure ce chapitre nous proposons de résumer les spécificités de notre approche duparking référencé sur des amers. Tout d’abord on peut noter sa généricité, qui découle de laformulation très générale d’une tâche de parking référencé sur des amers, et de la généricité dela méthode de déformation de trajectoire employée. Les contreparties de cette généricité résidentdans la nécessité de posséder une trajectoire de référence.

Par ailleurs cette approche peut être employée dans une grande variété de situations. Ellepeut, par exemple, être directement utilisée avec plusieurs capteurs, ou dans des applications oùle motif de parking ne définit pas complètement la position de parking. On traite ainsi les pro-blèmes sous déterminés tels que se garer «le long d’un mur», ou bien «entre deux voitures», sansque ces éléments ne spécifient complètement la position de parking. De même, cette méthodes’accommode très bien de l’absence de tout ou partie du motif de parking lors de l’exécution.Par contre, notre formalisme impose que le motif de parking soit constitué d’éléments dont onpeut déterminer la position. Ainsi, les amers de type point d’intérêt dans une image sont exclus,et doivent être spécifiés sous la forme de points dans le plan ou dans l’espace.

De plus, le problème de l’évitement d’obstacles est directement intégré dans cette approche.Il peut cependant y avoir des cas d’échec que nous n’avons pas traités : par exemple si la configu-ration de parking calculée qpark est en collision. Ceci peut se produire car la tâche de parking neconsidère pas les éléments de l’environnement comme des obstacles, mais uniquement commedes amers. Un compromis entre tâche de parking et évitement d’obstacles doit alors être trouvé,mais cette problématique n’a pas été développée.

Chapitre 5

Intégration du suivi de trajectoire, del’évitement d’obstacles et de la localisation

5.1 Introduction

Certaines fonctionnalités nécessaires à la navigation autonome sont réalisées «hors-ligne».C’est le cas de la modélisation de l’environnement, de la planification d’une trajectoire dansce modèle et de la définition d’un motif de parking par exemple. D’autres fonctionnalités aucontraire doivent être réalisées au cours de la navigation. C’est le cas du suivi de trajectoire,de l’évitement réactif d’obstacles, de la localisation et du parking référencé capteur. L’objet dece chapitre est de montrer que la réalisation simultanée de ces différentes fonctionnalités ne serésume pas à la réalisation isolée de chacune d’entre elles. Nous proposons donc une architecturepermettant d’intégrer ces différentes fonctionnalités.

Dans la première section nous présentons une architecture permettant la réalisation simul-tanée du suivi de trajectoire et de l’évitement réactif d’obstacles. Nous mettons en évidence lanécessité de pouvoir ralentir le long de la trajectoire planifiée, et nous montrons que les spécifi-cités des systèmes abordés dans nos travaux posent des problèmes particuliers.

Dans la section 5.3 nous ajoutons à cette architecture la fonctionnalité de localisation glo-bale. En effet pour pouvoir effectuer de longs mouvements, le robot ne peut pas compter sur laseule intégration de son déplacement pour estimer sa position, et il doit pouvoir se localiser dansune carte de son environnement. Après avoir remarqué que cette localisation globale produit uneestimation discontinue de la position du robot, nous mettons en évidence les problèmes que celapose pour l’intégration dans une architecture de suivi de trajectoire, compte tenu des spécificitésde nos systèmes. En effet les systèmes que nous étudions, les véhicules articulés, et les environ-nements dans lesquels nous souhaitons les faire évoluer, les environnements fortement contraints,font que le suivi de trajectoire ne pourra pas «absorber» les perturbations de la localisation glo-bale. Et nous illustrons sur un exemple le fait que le rattrapage immédiat de ces discontinuitéspar un processus de suivi de trajectoire qui n’assure pas une décroissance uniforme de la distanceà la consigne, peut entraîner des collisions.

Enfin, à la section 5.4, nous présentons une architecture adaptée aux spécificités de notre

78 Chapitre 5. Suivi de trajectoire, évitement d’obstacles et localisation

approche, qui intègre ces différentes fonctionnalités et qui garantit l’absence de collision lors dusuivi de trajectoire avec une localisation globale.

5.2 Suivi de trajectoire et évitement réactif d’obstacles

Le suivi de trajectoire consiste à asservir la configuration du robot sur une trajectoire deréférence. L’évitement réactif d’obstacles, présenté au chapitre 3 doit assurer que la trajectoireest sans collision et la déformer sur un intervalle quand une collision est détectée. On rappelleque les obstacles sont supposés statiques.

Ainsi l’évitement d’obstacles peut amener le suivi de trajectoire à ralentir, voire même às’arrêter sur la trajectoire si la collision ne peut être éliminée.

5.2.1 Suivi de trajectoire

Le processus de suivi de trajectoire en boucle fermée consiste à calculer une commande àappliquer au robot étant données sa configuration courante et une configuration de référence.On note respectivement qref et uref la configuration et la commande de référence le long d’unetrajectoire admissible, c’est à dire solution de l’équation (2.7).

δu

qref

q

K

Σ

uref qodo

FIG. 5.1 – Système (Σ) de suivi de trajectoire en boucle fermée

Système de commande en boucle fermée

On note (Σ) le système de suivi de trajectoire en boucle fermée, représenté dans la figure 5.1.L’évolution de la configuration du robot représentée par le bloc q est donnée par l’équation (2.7),à un bruit de mesure près. L’état q du système est inconnu, et on note qodo l’estimation de laconfiguration du robot.

5.2 Suivi de trajectoire et évitement réactif d’obstacles 79

Cette estimation de l’état du système est fournie par l’odométrie ou par une centrale iner-tielle par exemple. Parfois qualifiée de proprioceptive1, ce type de localisation à l’estime («dead-reckoning» pour «deduced-reckoning» en anglais) ne repose pas sur une carte d’amers et sonerreur s’accroît au fur et à mesure de l’exécution de la trajectoire. On suppose pour l’instant quec’est la seule estimation de la configuration du robot que l’on possède. On admet cependant qu’àl’initialisation le robot est localisé de telle sorte que l’estimation de la configuration est égale àla configuration initiale de la trajectoire :

qodo(0) = qref (0)

La loi de commande notée K calcule la correction δu à appliquer à la commande de réfé-rence u :

δu = K(qodo,qref ,uref )

Nous ne donnons pas plus de détail sur la loi de commande adoptée. Une littérature très richeest en effet disponible sur le sujet, et nous renvoyons à [Luca ] pour une synthèse.

Stabilité

Le concept de stabilité d’un système autour d’une trajectoire est utile à notre étude, car lesuivi de trajectoire a en charge de stabiliser le système autour de sa trajectoire de référence. Lesdéfinitions de la stabilité que nous allons donner ne sont pas celles habituellement utilisées enthéorie de la commande. En effet, dans le cadre de notre étude, c’est l’évolution de la distancedu système à sa trajectoire de référence qui nous intéresse.

Propriété 8. Stabilité exponentielle :si (qref ,uref ) est une trajectoire admissible sur [0, S] exécutée par le système (Σ), alors ∃λ > 0∃µ > 0 tels que :

si ∃s1 ∈ [0, S] tel que dC(qodo(s1),qref (s1)) < µ alors

∀s ∈ [s1, S] dC(qodo(s),qref (s)) ≤ e−λ(s−s1)dC(qodo(s1),q

ref (s1))

Les systèmes complètement actionnés sont généralement exponentiellement stables.Les véhicules articulés et les véhicules nonholonomes en général n’ont par contre pas la pro-

priété de stabilité exponentielle. La distance à la consigne ne décroît pas uniformément, mais peutau contraire augmenter avant de diminuer, comme illustré sur la figure 5.2. Pour ces systèmes ondéfinit la propriété suivante.

Propriété 9. Stabilité effective :il existe une constante réelle positive Dsuivi telle que si (qref ,uref ) est une trajectoire admissiblesur [0, S] exécutée par le système (Σ), et si

qodo(0) = qref (0)

Alors pour tout s ∈ [0, S],dC(qodo(s),q

ref (s)) < Dsuivi

1c’est à dire qui mesure l’état interne du système, par opposition à extéroceptive


configuration courante

Dsuivi

trajectoire de référence

q(s1)

qref(s1)

configuration courante

d > Dsuivi

trajectoire de référenceqref(s2)

q(s2)

FIG. 5.2 – Pour les véhicules articulés, la distance à la consigne ne décroît pas uniformément

Dans les deux propriétés précédentes, la loi de commande en boucle fermée assure que l’es-timation de la configuration qodo reste dans un voisinage de la configuration de référence qref aucours du suivi de trajectoire.

5.2.2 S’arrêter sur une trajectoireUne propriété essentielle du suivi de trajectoire dans le contexte de l’évitement réactif d’obs-

tacles est de pouvoir s’arrêter sur la trajectoire quand une collision est détectée et que la trajec-toire ne peut pas être déformée suffisamment rapidement.

Pour un système soumis à des contraintes d’accélération et suivant une trajectoire quel-conque, cette propriété n’est pas immédiate. Nous rapportons ici des résultats de [Bonnafous b]et [Bonnafous a].

Bornes cinématiques

Un robot mobile est soumis à des contraintes sur ses vitesses et accélérations, qu’on supposede la forme : ∀i ∈ {1, ..., k}

−viM ≤ ui(t) ≤ viM (5.1)−viM ≤ ui(t) ≤ viM (5.2)

où viM et viM sont les bornes sur les commandes du système (vitesses) et leurs dérivées (accélé-rations).

Paramétrage d’une trajectoire

Le paramétrage d’une trajectoire qui respecte ces contraintes a été étudié dans le cadre dumouvement en temps minimal de robots manipulateurs (contraintes d’accélération concurrentes)dans [Bobrow , Shin , Slotine , Renaud ]. Ces résultats ont été étendus aux robots


mobiles en prenant en compte les contraintes sur les vitesses dans [Lamiraux b]. Le résultatd’un tel paramétrage est donc une trajectoire admissible définie sur un intervalle [0, S] et quisatisfait :

−viM ≤ ui(s) ≤ viM (5.3)−viM ≤ dui

ds(s) ≤ viM (5.4)

Ce paramétrage est tel que si s = t, le système respecte les bornes cinématiques.

Suivi de trajectoire

Le processus de suivi de trajectoire est un processus temps-réel qui est en charge de l’évolu-tion de l’abscisse s au cours de l’exécution de la trajectoire, de façon éventuellement à ralentir età s’arrêter sur cette trajectoire.

En considérant s comme une fonction du temps, on exprime les commandes réellement en-voyées au système en fonction des commandes de la trajectoire :

v(t) = s(t)u(s(t)) (5.5)

Et en dérivant cette expression :

v(t) = s(t)2du

ds(s(t)) + s(t)u(s(t)) (5.6)

L’expression des contraintes (5.1-5.2) devient alors :∀i ∈ {1, ..., k}

−viM ≤ s(t)ui(s(t)) ≤ viM (5.7)−viM ≤ s(t)2 dui

ds(s(t)) + s(t)ui(s(t)) ≤ viM (5.8)

Le processus de suivi de trajectoire est donc en charge de l’évolution de l’état (s, s), sous lescontraintes précédentes.

Si l’on suppose que :0 ≤ s(t) ≤ 1 (5.9)

alors (5.3) assure que la contrainte (5.7) est toujours respectée.On peut mettre la contrainte (5.8) sous la forme :

smin(s, s) ≤ s ≤ smax(s, s)

Pour s’arrêter le long de la trajectoire, le processus de suivi de trajectoire doit donc calculer entemps réel :

s = smin(s, s) (5.10)

jusqu’à s = 0.Pour des trajectoires simples telles des combinaisons d’arcs de cercle et de lignes droites,

le calcul de la distance de freinage est immédiat, mais pour des trajectoires quelconques il faut


s0 = sdecel

smax

s

ssstop

intégration de l’équation (5.10)

s0

courbe d’équation (5.11)

1

FIG. 5.3 – Courbe de décélération optimale calculée par intégration (en gras), et courbe de décé-lération minorante donnée par l’équation (5.11).

intégrer l’équation (5.10) depuis l’état courant (s, s) jusqu’à s = 0. Et cette opération peut êtreincompatible avec les contraintes temps-réel de la tâche de suivi de trajectoire.

La méthode donnée dans [Bonnafous b] permet de trouver directement une expression ana-lytique d’une courbe de décélération qui respecte la contrainte (5.8), sous l’hypothèse que :

0 ≤ s ≤ smax < 1

L’intérêt pour notre application de suivi de trajectoire est que le calcul de l’évolution de l’abs-cisse s (et de s) lors d’une décélération sur la trajectoire est immédiat, et qu’il peut donc êtreréalisé en temps-réel.

L’expression de la courbe de décélération dans le plan de phase (s, s) est :

s =√

1− (1− s20)e

2m(s−s0) (5.11)

où (s0, s0) est l’état initial de la décélération et m = mini∈[0,k]viM

viM. La figure 5.3 illustre ce

résultat. Cette courbe est bien un minorant de la courbe de décélération optimale correspondantà l’intégration de l’équation (5.10).

Fonctionnalités du suivi de trajectoire

On utilise ce résultat pour définir deux fonctions utiles au suivi de trajectoire dans notreétude : DECEL et RALENTIR. La fonction DECEL(s, sstop) calcule l’abscisse sdecel à partir delaquelle le robot doit décélérer de façon à s’arrêter à un sstop donné. En utilisant l’équation (5.11),on a :

sdecel = DECEL(s, sstop) = sstop +1

2mlog(1− s2) (5.12)

La fonction RALENTIR(sdecel, s) donne l’évolution de l’état (s, s) le long de la courbe dedécélération :

s = RALENTIR(sdecel, s) =√

1− (1− s20)e

2m(s−sdecel)


où s0 est la valeur de s en s = sdecel. Cette fonction garantit que les bornes cinématiques sontrespectées et elle assure un arrêt du robot à l’abscisse sstop désirée.

5.2.3 Détection des obstaclesSoient q = (x,qint) et qodo = (xodo,qint) respectivement la configuration réelle et la confi-

guration estimée du robot quand un obstacle O est détecté par les capteurs. On note O le sous-espace de l’espace de travail occupé par O. Seule la partie visible de l’obstacle, notée Ovisible,est perçue. L’obstacle détecté exprimé dans le repère du robot est donc x−1(Ovisible), au bruitde mesure près. On note alors Oloc l’union de la partie visible augmentée de l’erreur de percep-tion Dcapteur, et la partie cachée par cette partie, comme illustré sur la figure 5.4. On suppose queOloc est une donnée fournie par le capteur, et donc exprimée relativement au robot. L’estimationdu sous-espace occupé par l’obstacle est notée O, et correspond à la transformation de Oloc parl’estimation de la position du robot xodo :

O = xodo(Oloc)

partie

O

qodo

Ovisible

q = (x,qint)

Ox(Oloc)

Dcapteur

cachée

FIG. 5.4 – Détection des obstacles (voir texte)

Et du fait que Oloc inclut le bruit de mesure, on admet que :

O ⊂ x(Oloc) = x.x−1odo(O) (5.13)

En notant Dclear > 0 le réel exprimant la distance minimale désirée aux obstacles, une confi-guration sur la trajectoire est considérée en collision si et seulement si :

dO(qref , O) > Dclear (5.14)


Σ

traj

(qref ,uref )

(s, s)

qref (s)

suref (s)

qodo

s

sstop

capteur

sstop

tSuivi tCol

odométriemoteur

FIG. 5.5 – Architecture de contrôle de suivi de trajectoire et de la l’évitement d’obstacles.

5.2.4 Architecture pour le suivi de trajectoire et l’évitement d’obstaclesLe suivi de trajectoire et l’évitement d’obstacles sont naturellement séparés en deux tâches

périodiques différentes, notés respectivement tCol et tSuivi. La figure 5.5 présente le diagrammed’architecture de ces deux tâches.

tCol (Algorithme 3)

Cette tâche réalise l’évitement réactif d’obstacle par la méthode de déformation de tra-jectoire présentée au chapitre 3. Elle implémente la fonction CHECK qui détecte les colli-sions sur l’intervalle [s, sstop] et la fonction DEFORME qui déforme la trajectoire sur l’inter-valle [sstop −∆scol, sstop + ∆scol]. Sa période est grande du fait de la complexité des calculs desfonctions CHECK et DEFORME évoquée à la section 3.4.2.

Algorithme 3 tâche tColEntrée : s,qref , qodo, Oloc

Sortie : sstop

loopsstop ← CHECK (qref (s), xodo(Oloc) ) sur [s, s + ∆scheck]if sstop ≤ (s + ∆scheck) then

qref (s)← DEFORME(qref (s)) sur [sstop −∆scol, sstop + ∆scol]end if

end loop

tSuivi (Algorithme 4)

Cette tâche assure le suivi de la trajectoire à une fréquence élevée. Elle implémente la fonc-tion DECEL qui calcule l’abscisse sdecel à partir de laquelle le robot doit ralentir pour s’arrêter


en sstop et la fonction RALENTIR qui calcule une décélération faisable. Elle envoie les consignesau système (Σ). Sa période est petite, et égale à celle du système (Σ).

Algorithme 4 tâche tSuiviEntrée : sstop,qref ,uref

Sortie : s,qref (s), s.uref (s)loop

sdecel ← DECEL(s, sstop −∆scol)if s < sdecel then

augmenter s vers smax

elses← RALENTIR(sstop −∆scol, s)

end ifs + s∆t→ s

end loop

Accès concurrents à la trajectoire traj

Il est important de remarquer que la trajectoire traj est une donnée accédée en concurrencepar les tâches tCol et tSuivi. L’intégrité de cette donnée est assurée par le fait que l’accès enécriture par la tâche tCol s’effectue uniquement sur l’intervalle [sstop −∆scol, sstop + ∆scol] etque l’accès en lecture par la tâche tSuivi ne se fait que sur l’intervalle [s, sstop −∆scol].

5.2.5 Conditions garantissant l’absence de collision lors du suivi de la tra-jectoire

On montre ici que sous certaines hypothèses, l’architecture présentée permet de garantir l’ab-sence de collision lors de l’exécution de la trajectoire de référence.

Définissons tout d’abord l’accroissement d’erreur odométrique entre deux abscisses sur unetrajectoire.

Définition 1. On note qodo(s) = (xodo(s), qint) et q(s) = (x(s),qint) respectivement l’esti-mation de la configuration et la configuration réelle du robot à l’abscisse s. L’accroissementd’erreur odométrique entre l’abscisse s1 et l’abscisse s2 est :

dSE(2)(x−1odo(s1).xodo(s2),x(s1)

−1.x(s2))

on rappelle la propriété de la distance dSE(2) : ∀m,x1,x2 ∈ SE(2)

dSE(2)(m.x1, m.x2) = dSE(2)(x1,x2)

La figure 5.6 illustre cette définition.


xodo(s2)xodo(s1)

x(s1)

x(s2)

x(s1).x−1odo(s1).xodo(s2)

erreur odométrique

FIG. 5.6 – Accroissement de l’erreur odométrique entre s1 et s2

Propriété 10. Soit (qref ,uref ) une trajectoire définie sur l’intervalle [0, S]. À l’abscisse s = 0le robot effectue une perception de l’obstacle O. Si :

1. la trajectoire est vérifiée sans collision : pour tout s ∈ [0, S], dO(qref (s), O) > Dclear,2. l’équation (5.13) est respectée : O ⊂ x(0).x−1

odo(0)(O),3. l’erreur odométrique entre 0 et s est bornée par Dodo, pour tout s ∈ [0, S],4. qodo(0) = qref (0) et (Σ) satisfait la propriété 9 de stabilité effective,5. les variables de configuration internes sont parfaitement mesurées :

qodo(s) = (xodo(s),qint(s)),6. Dodo + Dsuivi < Dclear,

alors le robot n’est pas en collision le long de la trajectoire de référence :

∀s ∈ [0, S], dO(q(s),O) > 0

Démonstration. On note m0 la transformation solide x(0).x−1odo(0). En appliquant m0 à qref et

O dans l’hypothèse 1, on obtient :

dO(x(0).x−1odo(0).qref (s),x(0).x−1

odo(0)(O)) > Dclear

En utilisant l’hypothèse 2, il vient :

dO(x(0).x−1odo(0).qref (s),O) > Dclear (5.15)

En appliquant maintenant m0 à qref et qodo dans la propriété 9 on a :

dC(x(0).x−1odo(0).qref (s),x(0).x−1

odo(0).qodo(s)) ≤ Dsuivi (5.16)

En utilisant la définition de dSE(2) et l’hypothèse 5, on peut écrire :

dC(x(0).x−1odo(0).qodo(s),q(s)) = dSE(2)(x(0).x−1

odo(0).xodo(s),x(s))

= dSE(2)(x−1odo(0).xodo(s),x

−1(0).x(s))


On reconnaît là l’expression de l’erreur odométrique entre 0 et s, donnée dans la définition 1illustrée par la figure 5.6. En utilisant l’hypothèse 3 on peut alors écrire :

dC(x(0).x−1odo(0).qodo(s),q(s)) ≤ Dodo (5.17)

Alors en utilisant l’inégalité triangulaire avec l’expression à gauche de la somme de (5.16)et (5.17), il vient :

dC(x(0).x−1odo(0).qref (s),q(s)) ≤ Dsuivi + Dodo (5.18)

On applique alors l’inégalité triangulaire de la propriété 1 à la différence entre (5.15) et (5.18) :

dO(q(s),O) > Dclear −Dsuivi −Dodo > 0

La propriété 10 nous assure donc qu’en utilisant seulement l’odométrie comme estimation dela configuration, le robot n’entre pas en collision avec les obstacles en suivant la trajectoire plani-fiée. Pour ce faire les obstacles sont grossis de la somme de l’erreur de suivi de trajectoire Dsuivi

et de l’erreur odométrique Dodo, comme illustré dans la figure 5.7.Remarquons que nous avons utilisé la propriété de stabilité 9, que respectent les systèmes

abordés dans notre étude. Il est immédiat que les systèmes respectant la propriété 8 respectentaussi cette propriété, et que la démonstration reste valable.

m0

qodo(0)

Dcapteur

Dodo

q(0)

Dsuivi

m0 = x(0).x−1odo(0)

qref (s)Dclear

qodo(s)

m0

Oq(s)

Partie cachée

O

FIG. 5.7 – Illustration de la preuve de la propriété 10. La borne inférieure de la distance auxobstacles Dclear est supérieure à la somme des bornes supérieures de l’erreur d’odométrie Dodo

et de l’erreur de suivi de trajectoire Dsuivi.


5.3 Intégration de la localisation globaleDans la section précédente nous avons montré que l’on pouvait garantir l’absence de collision

lors du suivi d’une trajectoire avec l’odométrie (ou tout autre donnée de localisation à l’estime)comme seule estimation de la configuration du robot. Cependant pour réaliser de longs mouve-ments, il est nécessaire de localiser le robot dans une carte d’amers, de façon à réduire la dérivede l’odométrie (ou de la localisation «proprioceptive»). Nous appelons ce type de localisationbasée sur des amers la «localisation globale». Nous présentons tout d’abord l’intégration de lalocalisation globale dans notre architecture de contrôle. Nous montrons ensuite que cette archi-tecture ne permet pas de garantir l’absence de collision pour les systèmes respectant la propriétéde stabilité 9, c’est à dire les systèmes auxquels on s’intéresse dans notre étude.

Finalement nous montrons que sous certaines hypothèses très restrictives quant aux discon-tinuités de la localisation et à la distance minimale aux obstacles, il est possible de garantirl’absence de collision pour les systèmes exponentiellement stables avec cette architecture. Ce-pendant ces hypothèses ne sont pas réalistes, et ce résultat ne peut donc pas s’étendre aux casdéfinis par les spécificités de notre étude. Cela motivera le développement dans la section sui-vante d’une architecture adéquate pour le suivi de trajectoire sécurisé pour les véhicules articulésnaviguant proches des obstacles.

5.3.1 Localisation globaleNous appelons «localisation globale» la localisation du robot dans une carte. La localisation

par GPS ou par mise en correspondance d’amers perçus avec une carte d’amers rentrent danscette catégorie. En fusionnant ces données de localisation avec la localisation proprioceptive,dans un filtre de Kalman par exemple, on obtient une estimation de la position du robot devariance minimale.

Soit (ln)n∈{1,··· ,nloc} la suite d’abscisses auxquelles a lieu la localisation globale. À chacunede ces abscisses, une estimation de la position du robot xn(ln) est calculée. Pour la suite de notreétude il est utile d’exprimer la transformation solide mn qui transforme xodo(ln) en x(ln). On a :

mn = x(ln).x−1odo(ln) (5.19)

Ainsi entre deux localisations globales, l’estimation de la position du robot est obtenue enappliquant la dernière transformation mn à l’estimation donnée par l’odométrie : ∀s ∈ [ln, ln+1]

x(s) = xn(s) = mn.xodo(s) (5.20)

Discontinuités de l’estimation de la position

Du fait des mesures bruitées du capteur et des imprécisions ou incohérences du plan, l’esti-mation de la position du robot peut être discontinue. Et ce malgré la fusion entre la localisationglobale et l’odométrie.

On peut borner ces discontinuités, en testant la vraisemblance de la nouvelle estimée par rap-port à l’estimation courante, et en n’acceptant cette nouvelle estimée que si la vraisemblance est

5.3 Intégration de la localisation globale 89

assez grande. On peut par exemple utiliser la distance de Mahalanobis entre ces deux estimations(présentée dans la section 4.4) pour mesurer cette vraisemblance.

Nous notons Dloc la borne maximale autorisée des discontinuités de localisation, dont nouspréciserons plus loin la définition. Ce qu’il est important de remarquer, c’est que ces sauts dansl’estimation de la position du robot entraînent des discontinuités dans la transformation mn calcu-lée à chaque abscisse de localisation ln. Nous allons voir les conséquences de ces discontinuités.

5.3.2 Architecture pour le suivi de trajectoire avec localisation globale

Σ

tLocmn

cartetraj

(qref ,uref )

(s, s)

capteur

sstop

suref (s)

tSuivi tCol

qodo

moteur

mn

sstop

s

odométrie

m−1n .qref (s)

FIG. 5.8 – Intégration du processus de localisation globale tLoc dans l’architecture de suivi detrajectoire avec évitement d’obstacles.

Le processus de localisation globale décrit ci-dessus s’intègre aisément dans l’architectureproposée dans la section 5.2.4. La figure 5.8 présente le diagramme de cette architecture. Lalocalisation globale est assurée par une nouvelle tâche notée tLoc, qui est en charge du calculde mn. On suppose qu’initialement le robot est localisé de telle sorte que q(0) = qref (0).

Cependant le système (Σ) de suivi de trajectoire présenté à la section 5.2 assure la stabilitéde qodo autour d’une trajectoire de référence, mais ne dit rien à propos de q. Pour assurer lastabilité de q autour de qref , on définit qref

odo, la trajectoire obtenue en appliquant la transformationsolide m−1

n à qref :qref

odo(s) = m−1n .qref (s) (5.21)

C’est sur cette trajectoire que va s’asservir le système (Σ). En utilisant la propriété 1 desymétrie par rapport à SE(2) définie au chapitre 2, on vérifie aisément que si qodo est stable


autour de qrefodo, alors q est stable autour de qref .

On remarque néanmoins que la transformation m−1n n’est pas constante, et donc que qref

odo

n’est pas continue. Il nous faut alors trouver un ensemble d’hypothèses pour garantir l’absencede collision lors du suivi de trajectoire dans ces conditions. C’est l’objet de la section suivante.

5.3.3 Conditions garantissant l’absence de collision pour les systèmes ex-ponentiellement stables

Dans le cas des systèmes exponentiellement stables au sens de la propriété 8, nous allonsmontrer que les discontinuités de localisation peuvent être englobées dans la distance minimalesouhaitée aux obstacles Dclear.

Détection des collisions

Soit ci(i ≥ 1), l’abscisse à laquelle le robot perçoit l’environnement avec son capteur afin dedétecter les obstacles, et soit j le plus grand entier tel que lj ≤ ci : lj est l’abscisse de la dernièrelocalisation, et mj est la transformation correspondante définie à l’équation (5.19). En reprenantles notations du paragraphe 5.2.3, on a :

O = x(ci)(Oloc) = mjxodo(ci)(Oloc) (5.22)

Ainsi, alors que l’algorithme (3) de la tâche tCol détectait les collisions en appliquant la fonc-tion :

CHECK(qref (s), xodo(Oloc))

on doit maintenant appliquer :

CHECK(qref (s), mjxodo(ci)(Oloc))

Localisation

On suppose qu’entre les abscisses ci et ci + ∆scheck la tâche tLoc effectue k localisationsglobales :

lj ≤ ci < lj+1 < · · · < lj+k < ci + ∆scheck

Et entre 2 localisations globales lp et lp+1, la trajectoire de référence envoyée au système (Σ)est (m−1

p qref ,uref ).Alors on a la propriété suivante :

Propriété 11. Si :1. à l’abscisse ci, l’estimation de la configuration du robot est dans un voisinage de la

configuration de référence :

dC(qodo(ci),m−1j qref (ci)) ≤ Dloc

2. le système (Σ) est exponentiellement stable avec µ ≥ 2Dloc, (propriété 8),


3. la trajectoire de référence est détectée sans collision :

∀s ∈ [ci, ci + ∆scheck] dO(qref (s), O) > Dclear

4. les obstacles détectés englobent les obstacles réels (équations (5.13) et (5.22)) :

O ⊂ x(ci)x−1(ci)(O) = x(ci)x−1odo(ci)m−1

j (O)

5. l’accroissement d’erreur odométrique entre ci et s est bornée par Dodo pour tout s ∈[ci, ci + ∆scheck],

6. les variables de configurations internes sont parfaitement mesurées :qodo(s) = (xodo(s),qint(s))

7. les discontinuités de localisation sont bornées :∀p, q, j ≤ p, q ≤ j + k et ∀s ∈ [ci, ci + ∆scheck],

dC(m−1p qref (s),m−1

q qref (s)) ≤ Dloc

8. les abscisses de localisations sont suffisamment éloignées pour que le système ait letemps de converger vers la trajectoire de référence :∀p, j ≤ p ≤ j + k − 1

dC(qodo(lp+1),m−1p qref (lp+1)) ≤

12dC(qodo(lp),m−1

p qref (lp))

9. Dodo + 3Dloc < Dclear

alors le long de la trajectoire de référence le robot n’est pas en collision :

∀s ∈ [ci, ci + ∆scheck], dO(q(s),O) > 0

La preuve de cette propriété est donnée dans l’annexe A. L’intérêt de la propriété 11 estd’apporter une preuve formelle à une connaissance validée par l’expérience, à savoir que pourles systèmes exponentiellement stables, en prenant une marge raisonnable par rapport aux obs-tacles (hypothèse 9), le suivi de trajectoire s’effectue sans collision. Ainsi pour un robot rondévoluant dans un environnement peu contraint, les perturbations induites par les discontinuitésde la localisation peuvent être englobées dans la distance minimale aux obstacles.

Mais pour assurer la même propriété pour les systèmes ne respectant pas la propriété 8 il fau-drait augmenter encore la distance minimale aux obstacles Dclear. En effet pour de tels systèmesla distance à la consigne peut augmenter avant de décroître, comme illustré sur la figure 5.2. Dansle cadre de notre étude où les mouvements doivent parfois s’effectuer à proximité des obstacles,cette option n’est pas envisageable.


5.3.4 Cas réalistes avec un véhicule articuléDans des cas réalistes, la distance minimale aux obstacles Dclear est inférieure à la discon-

tinuité de la localisation Dloc. De plus les systèmes auxquels on s’intéresse ne respectent pas lapropriété de stabilité exponentielle 8. On ne peut donc pas utiliser la propriété 11 et on n’a doncpas de garantie de l’absence de collision lors du suivi de la trajectoire. La figure 5.9, dans laquellechaque image représente un instant différent, donne l’exemple d’une collision suite à un saut delocalisation dû à une imprécision de la carte, dans un cas réaliste. La trajectoire planifiée est uneligne droite sans collision avec les obstacles de la carte ou avec les autres obstacles perçus.

Figure du haut

Sur la figure du haut, l’abscisse ci du début de la détection des collisions sur l’intervalle [ci, ci+∆scheck] est représentée, ainsi que l’abscisse de la dernière localisation globale lj . On supposealors que le robot est parfaitement localisé et qu’il suit parfaitement sa trajectoire, c’est à direque :

mj.qodo(s) = q(s) = qref (s)

La tâche tCol vérifie à cet instant la configuration qref (scheck1 ), qui n’est pas en collision.

Figure du milieu

Sur la figure du milieu, le robot a avancé, et il se trouve maintenant à l’abscisse lj+1, à laquelleune nouvelle localisation globale a lieu. Du fait de l’imprécision du plan, la localisation globaleproduit une estimation de la configuration qui apparaît comme erronée, mais qui est cohérenteavec la carte : le segment perçu en haut est bien mis en correspondance avec le segment de lacarte. La vraie configuration du robot ne change pas et reste donc mj.qodo(lj+1). Du fait de cettediscontinuité dans la localisation du robot (mj 6= mj+1), la trajectoire de référence envoyée ausystème (Σ) est discontinue :

dC(m−1j .qref (lj+1), m−1

j+1.qref (lj+1)) = Dloc

Le système va donc effectuer une trajectoire de rattrapage pour ramener q vers la trajectoirede référence qref . On peut remarquer qu’à cet instant l’obstacle carré perçu (qui n’est pas dansla carte, mais cela est sans conséquence) est en collision avec la trajectoire de référence, mais latâche tCol est alors en train de vérifier la configuration qref (scheck

2 ). Cet obstacle s’est déplacéentre la figure du haut et la figure du milieu, car la transformation permettant d’exprimer laposition des obstacles dans le plan à partir de leur position exprimée relativement au robot (Oloc)est passée de mj à mj+1.

Figure du bas

Sur la figure du bas, la tâche tCol vérifie actuellement la configuration qref (scheck3 ), mais il

est déjà trop tard, le robot est entré en collision avec l’obstacle carré en cherchant à rattraper latrajectoire de référence.


perception capteur

qref (ci + ∆scheck)

imprécision de la carte

ci

mj.qodo(s) = q(s) = qref (s)

carte

lj

scheck1

perception capteur

carteimprécision de la carte

ci

mj.qodo(lj+1) = qref (lj+1)

lj

q(lj+1)

scheck2

Dloc

perception capteur


ci

mj.qodo(s)

lj

scheck3

q(s)

FIG. 5.9 – Collision suite à un saut de localisation dû à une imprécision de la carte. Voir le texte.


Bilan Deux hypothèses de la propriété 11 ont donc été violées ici. Tout d’abord l’hypothèse 2,car le système de commande du chariot à remorque n’est pas exponentiellement stable. Ensuitel’hypothèse 9, car le saut de localisation Dloc est supérieur à la distance minimale autorisée auxobstacles Dclear. C’est d’ailleurs pour cette dernière raison que sur la figure du milieu, aprèslocalisation, la trajectoire de référence apparaît en collision avec les obstacles perçus.

En résumé, dans cet exemple la collision provient du fait que la trajectoire suivie par le robotaprès la localisation, la trajectoire de rattrapage, n’a pas été testée. C’est ce défaut que nous allonscorriger dans la section suivante afin de garantir un suivi de trajectoire sans collision.

5.4 Architecture pour l’intégration du suivi de trajectoire, del’évitement d’obstacles et de la localisation

Dans cette section nous proposons une architecture de suivi de trajectoire robuste aux dis-continuités de la localisation, pour les systèmes ne satisfaisant pas la propriété de stabilité expo-nentielle. C’est à dire que nous proposons une solution au suivi de trajectoire compte tenu desspécificités de notre approche.

Le principe de cette architecture est de simuler le rattrapage de la trajectoire de référence suiteà une discontinuité de localisation et de repousser son application plus loin sur la trajectoire, defaçon à ce que la trajectoire suivie par le système soit toujours vérifiée avant d’être exécutée.

5.4.1 Module de simulation du systèmePour prédire les effets des discontinuités de la trajectoire de référence envoyée au système

(Σ), on se dote d’un module de simulation noté (Σ). Ce système prend en entrée une trajectoire deréférence (qref ,uref ). L’état et la sortie du système (Σ) sont identiques et sont notés q. De plusle système (Σ) produit la commande associée à la trajectoire q, que l’on note u. La figure 5.10donne le diagramme de ce système de simulation.

Le système (Σ) satisfait la propriété de convergence suivante :

Propriété 12. ∀ε > 0,∀µ > 0, ∃T > 0 tel que

si : dC(q(0),qref (0)) < µ

alors : dC(q(T ),qref (T )) < ε

et la trajectoire (q, u) est admissible.

Trajectoire de rattrapage

Partant de l’état initial q(0), le système (Σ) produit une trajectoire de rattrapage admis-sible (q, u) vers la trajectoire de référence (qref ,uref ) lorsque son entrée est cette trajectoirede référence. C’est cette propriété que nous utilisons pour calculer une trajectoire de rattrapage.

5.4 Architecture pour l’intégration du suivi de trajectoire, de l’évitement d’obstacles et dela localisation 95

δu

qref

K

q

Σq

uuref

FIG. 5.10 – Module de simulation (Σ) du système (Σ). L’entrée est une trajectoire de référence(qref ,uref ) non nécessairement admissible. La sortie est une trajectoire admissible (q, u).

Étant donné ε > 0 fixé et (qref ,uref ) une trajectoire définie sur [0, S], on note alors RAT-TRAP (q(0),qref ,uref ) la fonction qui renvoie la trajectoire (q, u) de rattrapage entre q(0)et qref sur l’intervalle [0, S], tel que dC(q(S),qref (S)) < ε.

(q, u, S) = RATTRAP(q(0),qref ,uref ) (5.23)

5.4.2 Architecture pour le suivi de trajectoire

Dans l’architecture de suivi de trajectoire présentée dans cette section, la tâche tCol estcomme auparavant en charge de l’évitement des collisions, et elle a en plus pour fonction degérer les effets de la localisation globale. À cette fin on ajoute dans la définition d’une trajec-toire (qref ,uref ) définie sur l’intervalle [0, S] la transformation solide m(s) telle que : ∀s ∈ [0, S]

qrefodo(s) = m(s)−1qref (s)

où qrefodo(s) est la trajectoire de référence envoyée au système (Σ), de manière similaire à l’équa-

tion (5.21). La figure 5.11 présente le diagramme de cette architecture. On peut remarquer quela donnée traj, qui contient maintenant m(s), est partagée par les tâches tSuivi et tCol, maisencore une fois l’accès en écriture par tCol ne s’effectue que sur l’intervalle [sstop−∆scheck, S],alors que l’accès en lecture par tSuivi s’effectue sur l’intervalle [s, sstop −∆scheck].

L’algorithme 5 décrit la tâche tCol qui est maintenant en charge de mettre à jour la transfor-mation solide m(s) en fonction des localisations globales réalisées par la tâche tLoc. La tâchetCol doit aussi calculer une trajectoire de rattrapage avec la fonction RATTRAP. Une premièrelocalisation globale a lieu à l’initialisation, telle que q(0) = qref (0). La transformation m0, telleque q = m0.qodo(0), est utilisée pour initialiser m(s) sur l’intervalle [0, S].


Σ

tLocmn

carte

(s, s)

capteur

sstop

suref (s)

tSuivi tCol

moteur odométrie

qodo

s

traj

sstop

(qref ,uref )

mn

m(s)

m(s)−1.qref (s)

FIG. 5.11 – Architecture pour le suivi de trajectoire sécurisée.

Algorithme 5 tCol avec gestion de la localisation globale

Entrée : s, traj, qodo, Oloc, mn

Sortie : traj, sstop

Initialisation : ∀s ∈ [0, S], m(s)← m0,loop

Détection des collisions sur [s, s + ∆scheck]sstop ← CHECK (mn.m(s)−1.qref (s), mn.xodo(Oloc))Calcul de la trajectoire de rattrapageqsstop ← mn.m(sstop)−1.qref (sstop)(q, u, S)← RATTRAP

(qsstop , qref (sstop), uref (sstop)

)if S ≤ S then∀s ∈ [sstop, S], qref (s)← q(s)∀s ∈ [sstop, S], m(s)← mn

end ifDéformation de la trajectoire sur [sstop −∆scol, sstop + ∆scol]if sstop ≤ (s + ∆scheck) then

qref (s)← DEFORME(mn.m(s)−1.qref (s), mn.xodo(Oloc))end if

end loop

5.4 Architecture pour l’intégration du suivi de trajectoire, de l’évitement d’obstacles et dela localisation 97

Cet algorithme ramène la problématique du suivi de trajectoire avec localisation globale dansle cadre des hypothèses de la propriété 10, qui garantit l’absence de collisions lors du suivi detrajectoire sans localisation. Il assure en effet que :

– la trajectoire qrefodo(s) = m(s)−1.qref (s) envoyée au système (Σ) est continue et admissible,

– la trajectoire qref (s) exécutée par le robot est toujours vérifiée avant d’être exécutée,À la fin de l’exécution le robot atteint la configuration finale désirée (q(S) = qref (S)), à l’erreurd’odométrie Dodo et de suivi de trajectoire Dsuivi près.

sstop

qref

m0

q(0)

qodo(0)

qrefodo

S

m(s) = m0

CHECK

m(s) = m0 m(s) = m1

qref

m0

m1

c1 sstop SS

m1

m1

m1.m(s)−1.qref (s)

q(c1)

q = m1.m(sstop)−1.qref (sstop)

RATTRAP

qrefodo

qodo(c1)

CHECK

FIG. 5.12 – Les calculs de la tâche tCol pour garantir l’absence de collision. La ligne en gras est latrajectoire de référence qref (s). La ligne d’épaisseur standard est qref

odo(s), la trajectoire envoyéeau système (Σ). La détection des collisions s’effectue sur la trajectoire que l’on considère commeréellement exécutée par le robot à partir des dernières informations de localisation disponibles.Le rattrapage calculé par la fonction RATTRAP assure que qref

odo(s) est continue.

La figure 5.12 illustre le comportement de l’algorithme 5 lors de l’exécution d’une trajectoire.À chaque abscisse s le long de la trajectoire, la configuration de référence envoyée au système (Σ)est qref

odo(s) = m(s)−1qref (s). Sur la figure du haut le robot est à l’abscisse s = 0 sur la trajectoire


planifiée et la transformation solide initiale vaut m0. La tâche tCol détecte alors les éventuellescollisions sur la trajectoire jusqu’à l’abscisse sstop.

Sur la figure du bas, le robot est à l’abscisse c1 et une localisation globale a eu lieu à uneabscisse non précisée située entre 0 et c1. La localisation globale produit une discontinuité :m1 6= m0. La tâche tCol vérifie alors jusqu’à une nouvelle abscisse sstop la trajectoire considéréeà partir de ces nouvelles informations de localisation comme celle réellement exécutée par lerobot : m1.m(s)−1.qref (s). Ensuite elle calcule une trajectoire de rattrapage entre q et qref , àl’aide de la fonction RATTRAP. La transformation solide m(s), qui transforme qref

odo en qref , estmise à jour avec la nouvelle transformation m1, sur l’intervalle [sstop, S].

5.5 ConclusionDans ce chapitre, nous avons abordé le problème de l’intégration de plusieurs fonctionnalités

de la navigation autonome au sein d’une architecture unifiée. Plus précisément on s’est inté-ressé à la réalisation simultanée du suivi de trajectoire, de l’évitement réactif d’obstacles et de lalocalisation globale.

Nous avons montré que les spécificités de notre approche, c’est à dire la navigation de vé-hicules articulés à proximité des obstacles, nécessitaient le développement d’une architecturespécifique. Les éléments clés sont :

– l’absence de décroissance uniforme de la distance à la trajectoire de référence lors du suivide trajectoire,

– la forme quelconque des trajectoires qui exclue des approximations simples pour le calculdes distances de freinage,

– la navigation à proximité des obstacles.Pour ces raisons, certaines hypothèses habituelles pour garantir l’absence de collision (marge desécurité, erreur de localisation faible par rapport à ces marges, etc.) ne sont pas applicables ànotre étude.

Nous avons donc proposé une architecture qui intègre ces différentes fonctionnalités et quitienne compte de ces spécificités. L’implémentation de cette architecture doit permettre de réali-ser des mouvements longs avec une localisation globale. Ainsi même en naviguant à proximitédes obstacles, les discontinuités de la localisation n’entraînent pas de collision, car leur effet estreporté afin d’être pris en compte dans l’évitement réactif d’obstacles.

Chapitre 6

Résultats expérimentaux

6.1 Introduction

Dans ce chapitre nous présentons quelques résultats expérimentaux de navigation autonomeobtenus sur des robots réels. Mais avant tout, il nous paraît important de motiver l’expérimenta-tion sur des systèmes réels, car elle constitue une part importante de notre travail.

Motivation de l’expérimentation L’expérimentation en robotique mobile autonome consistetout d’abord à implémenter sur un système informatique, lui même embarqué sur un systèmemécanique, les méthodes théoriques résolvant les différentes fonctionnalités de la navigation.Il s’agit ensuite de définir une mission de navigation que le robot doit exécuter, et d’évaluerdans quelle mesure le robot remplit sa mission. L’intégration des différentes fonctionnalités etleurs interconnexions, une problématique que nous avons abordée au chapitre 5, rendent parfoisdélicate l’évaluation indépendante de chaque fonctionnalité.

L’expérimentation permet de «valider» une méthode censée résoudre une fonctionnalité dela navigation, ou une architecture qui intègre plusieurs méthodes. Mais une expérience, mêmerépétée et couronnée de succès, ne laisse rien présager de la «véracité» d’une théorie ou d’unmodèle. Par «valider» nous entendons plutôt que l’expérimentation démontre deux propriétésd’une méthode : la faisabilité de son implémentation, et sa capacité à résoudre des problèmesréels.

En effet l’implémentation d’une méthode qui a été développée à partir du modèle mathéma-tique d’un phénomène physique (évitement d’obstacle, localisation, etc.) se heurte bien souventà des problèmes de complexité algorithmique (et de temps de calcul prohibitifs associés) et d’in-tégration dans une architecture temps-réel.

Quant à la capacité d’une méthode à résoudre des problèmes réels, c’est un point fonda-mental que l’expérimentation permet d’étudier. C’est en effet lors d’expériences réelles que vontapparaître les «points durs», d’un point de vue pratique, d’un problème, qui ne correspondentpas toujours aux problèmes théoriques résolus par la méthode. À titre d’exemple, on peut ci-ter les méthodes de planification de trajectoire basées sur une exploration aléatoire de l’espacedes configurations : les cas difficiles pour ces techniques sont les passages étroits, et ce sont

100 Chapitre 6. Résultats expérimentaux

précisément les cas que l’on rencontre le plus souvent dans la pratique.L’expérimentation est donc un élément logique de la boucle : observation d’un problème réel,

modélisation mathématique, implémentation, expérimentation, observation du résultat. Ainsi, denouveaux problèmes sont généralement soulevés par des expériences. Mais l’expérimentationsur des systèmes réels demande beaucoup de temps, et il est donc nécessaire de se doter d’envi-ronnements de simulation réalistes pour préparer les expériences.

Enfin, mais on quitte là le domaine de la recherche pour entrer dans celui de la valorisation dela recherche, l’expérimentation est un formidable outil de communication qui permet de valoriserune méthode ou une théorie, en démontrant sa capacité à résoudre des problèmes réels en vued’applications pratiques. Mais tous les domaines ne sont pas sur un même pied d’égalité de cepoint de vue : certains domaines donnent lieu à des développements théoriques complexes maisà des applications peu spectaculaires, quand d’autres trouvent des applications qui reflètent bienla complexité du problème abordé.

Présentation des expérimentations Tout d’abord nous présentons l’architecture logicielle quiimplémente les différentes fonctionnalités de la navigation autonome. Ensuite nous donnonsquelques résultats d’évitement réactif d’obstacles, obtenus avec différents robots. Nous présen-tons ensuite la méthode de parking référencé sur des amers dans un cas réaliste avec un robotde type chariot à remorque. Enfin nous présentons une expérience qui traite de l’intégration dela localisation globale dans le suivi de trajectoire, une problématique que nous avons exposée auchapitre 5.

6.2 Architecture logicielleNous présentons ici l’architecture logicielle qui implémente les différentes fonctionnalités de

la navigation autonome. Nous avons contribué au développement de certains modules, représen-tés en gras sur la figure 6.1.

On remarque que cette architecture logicielle s’abstrait de la couche matérielle. Cela estrendu possible par l’utilisation de l’environnement de programmation GenoM (Générateur deModules [Fleury ]). Chaque module (représenté par un rectangle sur la figure 6.1), est écrit enlangage C et implémente une fonctionnalité. GenoM gère alors les éléments suivants :

– la compilation spécifique pour le système informatique embarqué sur le robot,– les communications et le partage de ressources entre les différents modules,– la synchronisation temporelle des différents modules,

SICK Dans nos applications, les obstacles sont détectés par un télémètre laser SICK, qui four-nit la distance aux points les plus proches dans un plan horizontal. On peut extraire des segmentsà partir de ces données. Le module SICK implémente ces fonctionnalités.

SEGLOC Le module SEGLOC calcule la position «globale» du robot en mettant en corres-pondance les segments du module SICK avec une carte de segments de l’environnement.

6.2 Architecture logicielle 101

SEGLOC

s

uq

0

0 odo

q fuse

q glob

segs

LOCO

odofuse

SICK

LOC

PILO

SUIVI

moteur odometrie

q odo

trajectoire

Move3DVisualisation

T

contrôleInterface de

FIG. 6.1 – Architecture logicielle pour la navigation autonome


LOC Le module LOC fusionne les positions odométriques et la position calculée par SE-GLOC.

LOCO Le module LOCO implémente la loi de commande en boucle fermée de suivi de tra-jectoire et envoie les commandes aux moteurs.

SUIVI Le module SUIVI gère la progression (abscisse s) du robot le long de la trajectoire,lui permettant de ralentir pour éviter un obstacle si la déformation de trajectoire n’est pas assezrapide par exemple. Il correspond à la tâche tSuivi décrite à la section 5.2.4.

PILO Le module PILO «pilote» le robot au cours de l’exécution de sa trajectoire. Il gère l’évi-tement réactif d’obstacles par la méthode décrite au chapitre 3 et implémente donc la tâche tColprésentée à la section 5.2.4. Mais il implémente en plus la tâche de parking référencé sur desamers décrite au chapitre 4.

Move3D Une couche supérieure dite décisionnelle permet de définir des tâches de plus hautniveau pour le robot. Dans notre étude, cette couche est principalement constituée par le logicielde planification de trajectoire Move3D [Siméon ], qui calcule une trajectoire sans collisiondans un modèle de l’environnement entre une configuration initiale et une configuration finalespécifiée.

Autres logiciels Nous avons développé une interface conviviale permettant de contrôler l’exé-cution des modules (démarrage, arrêt, réglage des paramètres) en langage Tcl/Tk.

Pour la visualisation du déroulement de la navigation, nous utilisons le logiciel GDHE déve-loppé au LAAS-CNRS. Nous avons réalisé quelques développements pour ce logiciel, permettantnotamment d’afficher un modèle VRML du robot Cycab, ou encore de suivre en temps-réel ledéroulement d’une tâche de parking référencé sur des amers.

6.3 Portage sur plusieurs robotsCette architecture, initialement développée pour le robot Hilare2 avec sa remorque (voir la

figure 4.5, page 67), a été «portée» sur le robot Dala et le Cycab de l’INRIA Rhône-Alpes (voirla figure 1.3, page 6).

Ce portage a nécessité deux types de développements : des modifications des programmesdes modules GenoM et des adaptations des algorithmes. Nous précisons ici le travail effectué etles résultats obtenus.

6.3.1 Le rover DalaLe rover Dala est équipé d’un processeur pentium à 2 GHz (au moment des expérimenta-

tions), et utilise le système d’exploitation Linux. Le portage sur le rover Dala de l’architecture

6.3 Portage sur plusieurs robots 103

proposée pour la navigation autonome n’a pas posé de difficulté. En effet, seul le module LOCO,lié à la partie matérielle, était différent, et il avait déjà était développé pour ce robot.

Par contre il a fallu adapter la méthode d’évitement d’obstacles afin de limiter la courbure dela trajectoire lors du processus de déformation. Cette extension a été présentée à la section 3.5.Nous rappelons un résultat intéressant présenté à cette occasion : étant donné une trajectoirede référence dont la courbure dépasse une valeur maximale spécifiée notée κmax, les itérationssuccessives de la méthode de déformation de trajectoire génèrent des manoeuvres qui diminuentla courbure de la trajectoire (voir la figure 3.15, page 49).

6.3.2 Le robot Cycab

Le portage sur le robot Cycab a nécessité un travail différent, car ce robot n’était pas sur lesite du LAAS-CNRS à Toulouse où nous avons effectué notre thèse. En effet le robot Cycab surlequel nous avons travaillé se trouve à l’INRIA Rhône-Alpes, et le portage à été réalisé dans lecadre du projet ROBEA ParkNav (Interprétation de scène dynamique complexe et planificationréactive de mouvement). Ce robot possédait déjà une architecture pour la navigation autonome(utilisant le formalisme de la programmation Bayésienne orientée objets [Pradalier ]), sous lesystème d’exploitation RT-Linux, et seuls les modules PILO, SUIVI et le logiciel Move3D ontété portés, car la contribution du LAAS-CNRS résidait dans l’apport d’une méthode originaled’évitement d’obstacles.

Le développement et l’utilisation d’un environnement de simulation a été un élément fonda-mental dans la réussite de cette partie du projet. Cédric Pradalier, avec qui nous avons collaboré,a tout d’abord effectué un séjour de trois jours au LAAS pour présenter son environnement desimulation du Cycab. Puis nous avons réalisé une interface de communication entre les modulesGenoM et le Cycab. Enfin deux séjours de trois jours à l’INRIA Rhône-Alpes nous ont permis deréaliser toutes nos expérimentations sur le robot.

Ce travail a mis en valeur la généricité de l’architecture proposée pour la navigation et de laméthode de déformation de trajectoire pour systèmes nonholonomes. La seule donnée du mo-dèle cinématique du système permet d’appliquer la méthode. Encore une fois nous avons utilisél’extension de la méthode de déformation de trajectoire de façon à respecter l’angle de braquagemaximal des roues avant (de 0.35 radians).

Évitement d’obstacle sur un parking Nous avons réalisé une expérience qui valide l’inté-gration de la méthode d’évitement réactif d’obstacles sur le robot Cycab, dans un contexte denavigation dans un parking (le contexte du projet ParkNav). Le cadre expérimental est le sui-vant : une trajectoire a été planifiée afin d’amener le robot d’une place de parking à une autre, etun obstacle absent du modèle utilisé pour la planification est en collision avec cette trajectoire.La figure 6.2 présente les résultats obtenus. L’obstacle est détecté par le capteur SICK, et la tra-jectoire est déformée de façon à l’éviter tout en gardant la courbure de la trajectoire en deçà desa valeur maximale autorisée.

Lors de cette expérience, la méthode d’optimisation des calculs des interactions entre le ro-bot et les obstacles présentée à la section 3.4.2, n’avait pas encore été implémentée. Aussi le


DéformationConfiguration initiale

Configuration finale

Obstacles

FIG. 6.2 – Expérimentation avec le robot Cycab. Sur la figure du haut, le cadre expérimental :une trajectoire est planifiée, depuis une place de parking vers une autre, mais un obstacle nonmodélisé se trouve sur cette trajectoire. Sur la figure du bas, on observe les déformations de latrajectoire au cours de son exécution.

processus de déformation de trajectoire était très coûteux en temps de calcul, et le robot devaits’arrêter le long de sa trajectoire de référence pour avoir le temps de déformer la trajectoire lors-qu’une collision était détectée. C’est l’observation de ce phénomène indésirable qui a motivé ledéveloppement de cet algorithme d’optimisation.

6.4 Parking référencé sur des amers

Reprenant l’exemple développé dans la section 4.6, nous présentons ici les résultats obtenuslors d’expérimentations avec le robot Hilare2 et sa remorque.

La tâche de parking consiste ici à positionner la remorque dans un emplacement représentantun quai de débarquement de marchandise pour des camions, matérialisé ici par un mur et descubes (voir la figure 6.3). Le motif de parking est constitué de trois segments perçus par untélémètre laser SICK situé sur la remorque. Le robot Hilare2 possède 1 carte PPC et 4 cartesM68K reliées par un bus de communication VME. Il utilise le système d’exploitation temps-réelVxWorks. Du fait de la faible puissance de calcul de ces cartes, un ordinateur portable pentiumà 2 GHz est utilisé pour le module PILO. La position de la remorque est mesurée par un codeuroptique situé sur le point d’attache entre la remorque et le robot.

6.4 Parking référencé sur des amers 105

6.4.1 Parking référencé sur des amers avec une localisation impréciseÉtant donné un modèle de l’environnement, une trajectoire est planifiée de façon à amener la

remorque au centre du quai de débarquement. Dans cette expérience, le quai de débarquementmatérialisé par deux boîtes accolées à un mur, est positionné dans la scène réelle aussi fidèlementque possible par rapport au modèle de l’environnement.

Cependant l’estimation de la configuration courante du robot est imprécise, comme le montrela figure 6.3 : la perception de l’environnement est décalée par rapport au modèle. Si le robotexécute sa trajectoire en boucle ouverte, il est manifeste qu’il n’atteindra pas la position désiréepar rapport au quai de débarquement, et que des collisions vont se produire. Le robot doit doncadapter sa trajectoire afin de rejoindre la configuration définie par le motif de parking tout enévitant les obstacles. Les images du bas de la figure 6.3 représentent les déformations successivesde la trajectoire au cours de son exécution.

qpark

quai de débarquement

q(S)

q(0)

q(0)

quai dedébarquement

perception motif de parking

décalage

qpark

carte

télémètre laser

FIG. 6.3 – Tâche de parking dans un quai de débarquement avec une localisation imprécise

6.4.2 Parking référencé sur des amers avec un motif de parking inexactDans cette expérience la localisation du robot serait assez précise pour réaliser la tâche de

parking en boucle ouverte. Cependant les boîtes matérialisant le motif de parking ne sont pasexactement dans la même position que dans le modèle de l’environnement. Elles ont été dépla-cées vers la droite et l’écart entre elles est supérieur à la valeur du modèle : le motif de parking estinexact par rapport à la réalité. Ainsi si la trajectoire planifiée était exécutée sans tenir compte dece changement, la configuration finale serait en collision avec le quai de débarquement, commele montre la figure 6.4.


Le robot doit donc déformer la trajectoire planifiée afin que la configuration finale soit po-sitionnée «au mieux» par rapport aux amers perçus. Dans notre approche cela signifie que laposition de parking qpark est la position de variance minimale, compte tenu des incertitudes dela localisation, du modèle et de la perception et étant donné la différence entre le modèle et laperception. Les images du bas de la figure 6.4 représentent les déformations successives de latrajectoire au cours de son exécution.

q(0)

quai de débarquementdéplacé et élargi

qpark

qpark

motif de parkingquai dedébarquement

q(0)

FIG. 6.4 – Tâche de parking dans un quai de débarquement à un emplacement différent et avecune forme modifiée par rapport au modèle

6.4.3 Bilan

Dans ces deux expériences, nous avons obtenu de très bons résultats quantitatifs. La remorquese positionne à la distance désirée des amers, avec une erreur de l’ordre de 5 cm. Cette erreur estplus marquée dans la direction transversale au mouvement, du fait de la convergence lente de laloi de commande dans cette direction.

Nous avons cependant relevé certains problèmes lors des expériences réelles. On a en effetremarqué que le robot devait parfois s’arrêter au cours de l’exécution de la trajectoire. Nousavons recensé deux causes à ce phénomène.

Tout d’abord le robot doit parfois s’arrêter pour déformer la trajectoire, au moment où unamer qui était caché devient visible. Par exemple, sur les deux images en bas à gauche de lafigure 6.4, une partie du quai de débarquement n’est pas perçue, car elle est occultée par uncôté de la boîte. La tâche de parking va donc considérer qu’un des amers du motif de parkingest absent de l’environnement réel, et va aligner la remorque avec l’amer de droite, qui est perçu

6.5 Intégration de la localisation globale, du suivi de trajectoire et de l’évitementd’obstacles 107

entièrement (cette étape n’est pas représentée). Ce n’est que lorsque le robot sera assez proche dela configuration de parking (image en bas à droite de la figure 6.4) que cet amer occulté sera enfinrévélé. Le robot devra alors recalculer une configuration de parking qui tienne compte de tousles amers, et sera contraint de s’arrêter car le temps de calcul de la déformation de la trajectoiren’est pas connu a priori.

Ensuite, nous avons conclu que la datation des données de perception est primordiale. Il fauten effet connaître avec précision à quel moment une perception a été réalisée pour la mettre enrelation avec la position du robot lors de cette perception. Ceci devient crucial quand les sourcesde perception sont multiples. Dans nos expériences, le manque de précision sur cette informationentraînait un phénomène de «déplacement» de la perception du capteur SICK avec le mouve-ment du robot : la perception n’était pas associée précisément à la position du robot lors de cetteperception. En l’occurence, on peut imputer une grande part de ce décalage à la fréquence peuélevée des perceptions (5 Hz), le système informatique du robot Hilare2 ne permettant pas de trai-ter les données à une fréquence plus élevée. Ce type de problème illustre l’intérêt d’un systèmed’acquisition temps-réel tel que RTMAPS [Nashashibi ], utilisé par exemple pour la fusion dedonnées de localisation à grande fréquence dans un contexte automobile [Abuhadrous ].

6.5 Intégration de la localisation globale, du suivi de trajec-toire et de l’évitement d’obstacles

Nous présentons dans cette section les résultats de l’implémentation de l’architecture pro-posée à la section 5.4 pour l’intégration de la localisation globale, du suivi de trajectoire et del’évitement d’obstacles, de façon à garantir l’absence de collision.

Comme nous l’avons montré dans le chapitre 5, l’intégration de la localisation globale dansl’architecture de suivi de trajectoire peut entraîner des collisions dès lors que le robot navigueproche des obstacles, en raison des discontinuités de la localisation et des spécificités de notreapproche. Les expériences rapportées précédemment ont pourtant été réalisées avec cette archi-tecture et ces défauts, et parfois même lors de longs mouvements utilisant la fonctionnalité delocalisation globale. Mais les discontinuités de localisation n’étaient généralement pas suffisantespour entraîner des collisions.

Suivi de trajectoire sans collision L’expérimentation que nous avons simulée reproduit le casprésenté à la section 5.3.4 et à la figure 5.9, qui conduisait à une collision. Les expériences quenous rapportons ici ont été réalisées en simulant la perception (le module SICK), la commande (lemodule LOCO) et l’environnement. Tous les autres modules sont les mêmes que ceux employéspour une expérimentation sur le système réel, c’est à dire le robot Hilare2 avec sa remorque.L’intérêt de présenter des résultats expérimentaux obtenus en simulation est d’isoler le problèmequi nous intéresse, à savoir le rattrapage d’une trajectoire suite à une discontinuité de l’estimationde la position du robot.

La figure 6.5 illustre ainsi le rattrapage d’une trajectoire suite à une discontinuité de la lo-calisation, dans un environnement de simulation. L’image de gauche représente la trajectoire


rectiligne planifiée dans l’environnement de simulation et la perception de cet environnementpar le robot. Sur cette image, la localisation basée sur des amers (des segments de droite danscette expérience) n’a pas encore pris en compte la perception de l’obstacle qui constitue une im-précision de la carte. En ce sens on peut considérer que le robot est mal localisé car sa perceptionn’est pas correctement apparié avec la carte.

L’image de droite présente la vision du robot quelques instants plus tard. La localisation glo-bale a identifié la perception de l’imprécision de la carte à un amer de la carte et l’estimationde la position du robot représentée par son modèle filaire a subi un saut. Cette nouvelle estima-tion de la position n’est pas prise en compte immédiatement, et la perception relativement à latrajectoire planifiée reste. Le rattrapage de la trajectoire prenant en compte la discontinuité dela localisation a été calculé en «aval» sur la trajectoire, permettant à la tâche de détection descollisions de vérifier cette portion de trajectoire. Au final le robot suit une trajectoire continue etsans collision.

rattrapage

estimation de la position

perception


FIG. 6.5 – Suivi de trajectoire sans collision. Le rattrapage de la trajectoire n’est pas réalisé dèsla discontinuité de la localisation, mais est reporté en «aval» sur la trajectoire.

6.5.1 ConclusionDans ce chapitre nous avons tout d’abord motivé l’expérimentation en robotique et rappelé

les difficultés de sa mise en oeuvre. Nous avons présenté plusieurs expériences sur des robotsdifférents, qui rendent compte de la généricité des méthodes développées.

Après avoir présenté l’architecture logicielle utilisée pour la navigation autonome, nous avonsrapporté une expérience d’évitement réactif d’obstacle sur le robot Cycab. Nous avons ensuiteillustré la méthode de parking référencé sur des amers par une expérience reproduisant l’amar-rage d’un véhicule articulé à un quai de débarquement. Enfin nous avons présenté une expérienceréalisée dans un environnement de simulation qui montre que l’architecture proposée pour l’in-tégration des fonctionnalités de suivi de trajectoire, d’évitement d’obstacles et de localisationpermet de garantir l’absence de collision.

Chapitre 7

Conclusion

Notre travail traite du problème de la navigation autonome des robots mobiles dans un cadreparticulier. Ce cadre est défini par les spécificités des systèmes considérés et des applicationsenvisagées, que nous pouvons résumer par la navigation pour robot mobiles nonholonomes àproximité des obstacles.

Le fil conducteur de ce document est l’idée que ces spécificités posent des problématiquessingulières pour la résolution des fonctionnalités de la navigation. Notre contribution porte sur ledéveloppement de méthodes permettant de résoudre certaines de ces fonctionnalités.

Évitement réactif d’obstacles pour systèmes nonholonomes Nous avons présenté une mé-thode d’évitement réactif d’obstacles pour systèmes nonholonomes, et avons proposé plusieursextensions de cette méthode et des optimisations de son algorithme. Le principe de cette méthodeest de perturber les entrées d’une trajectoire de référence de façon à ce que la trajectoire déformées’éloigne des obstacles et respecte les contraintes cinématiques du système. Nous avons proposéun algorithme permettant de réduire la complexité du calcul des interactions entre la trajectoireet les obstacles. Nous avons aussi montré comment cette méthode de déformation de trajectoirepeut être considérée comme une méthode d’optimisation de trajectoire, suivant d’autres critèresque la distance aux obstacles.

Parking référencé sur des amers Nous avons proposé une méthode originale de parking réfé-rencé sur des amers pour systèmes nonholonomes. Le principe est de définir un motif de parkingrelativement à un capteur du robot, de repérer ce motif dans l’environnement et de déformer latrajectoire de façon à ce que la configuration finale rejoigne la configuration de parking. Cetteméthode permet par exemple de réaliser une tâche de parking pour un robot avec remorque, enpositionnant la remorque relativement à des amers de l’environnement. Le formalisme utilisé estindépendant du type de capteur, du type d’amer ou des contraintes cinématiques du système.

Intégration du suivi de trajectoire, de l’évitement réactif d’obstacles et de la localisationNous avons montré que l’intégration des fonctionnalités de suivi de trajectoire, d’évitement ré-actif d’obstacles et de localisation globale constituait une problématique à part entière de la

110 Chapitre 7. Conclusion

navigation autonome. Nous avons identifié les difficultés de cette intégration compte tenu desspécificités de notre approche et avons proposé une architecture qui intègre ces fonctionnalités.

Résultats expérimentaux Enfin nous avons présenté des résultats expérimentaux obtenus avecdifférents robots évoluant dans des environnements variés : le robot Hilare2 avec une remorque,le rover Dala et le robot Cycab de l’INRIA Rhône-Alpes. Ces résultats montrent la maturité desméthodes proposées et leur capacité à résoudre des problèmes réels.

PerspectivesPlusieurs perspectives sont envisageables suite à nos travaux. Tout d’abord, nous avons men-

tionné la possibilité d’utiliser la méthode de déformation de trajectoire en tant que méthodenumérique de planification de trajectoire pour des systèmes nonholonomes. Il faudrait pour celaétudier notamment la stabilité et la vitesse de convergence de cette technique. Il faudrait parailleurs vérifier que cette méthode respecte la propriété de commandabilité et temps petit.

Ensuite, il serait intéressant d’introduire la «dynamique» dans notre étude de la navigationautonome. D’une part la dynamique de l’environnement en prenant en compte les obstacles mo-biles, ce qui soulève des problèmes particulièrement délicats du fait des spécificités de notreétude (calcul d’une courbe de décélération, forme quelconque des trajectoires, difficulté de rat-traper une trajectoire de référence, etc.). D’autre part la dynamique du système, en tenant comptede la masse du système et des forces physiques auxquelles il est soumis. Nous pourrions alorsréaliser des mouvements à grande vitesse. Par ailleurs on pourrait envisager d’utiliser d’autrescapteurs pour détecter les obstacles, tels que des caméras. On pourrait ainsi détecter une plusgrande quantité et variété d’obstacles.

Enfin un lien pourrait être établi avec des travaux récents portant sur l’exécution de trajec-toires référencées sur des amers [Malti ]. Le principe est de définir une trajectoire sensori-motrice comme un chemin géométrique et un ensemble d’amers perçus lors de l’exécution de cechemin. Ainsi la trajectoire est exprimée relativement à l’environnement : passer au milieu d’uncouloir, le long d’un mur, etc. Il serait intéressant d’intégrer cette technique à l’architecture pourla navigation autonome que nous avons proposée.

Annexe A

Preuve de l’absence de collision pour lessystèmes respectant la propriété de stabilitéexponentielle

Nous donnons ici la preuve de la propriété 11 page 90, qui assure que sous certaines hy-pothèses, le suivi de trajectoire avec localisation globale pour les systèmes exponentiellementstables au sens de la propriété 8 s’effectue sans collision. Nous énonçons de nouveau ces hypo-thèses.

Tout d’abord on rappelle qu’on note ci(i ≥ 1), l’abscisse à laquelle le robot perçoit l’environ-nement avec son capteur afin de détecter les obstacles, et j le plus grand entier tel que lj ≤ ci :lj est l’abscisse de la dernière localisation, et mj est la transformation correspondante, telle quedéfinie à l’équation (5.19).

On suppose qu’entre les abscisses ci et ci + ∆scheck, c’est à dire sur l’intervalle sur lequelsont détectées les collisions, la tâche tLoc effectue k localisations globales :

lj ≤ ci < lj+1 < · · · < lj+k < ci + ∆scheck

Et entre 2 localisations globales d’abscisses lp et lp+1, la trajectoire de référence fournie en entréedu système (Σ) est (m−1

p qref ,uref ).Les hypothèses sont alors les suivantes :

1. à l’abscisse ci, l’estimation de la configuration du robot est dans un voisinage de laconfiguration de référence :

dC(qodo(ci),m−1j qref (ci)) ≤ Dloc

2. le système (Σ) est exponentiellement stable avec µ ≥ 2Dloc, (propriété 8),3. la trajectoire de référence est détectée sans collision :

∀s ∈ [ci, ci + ∆scheck] dO(qref (s), O) > Dclear

4. les obstacles détectés englobent les obstacles réels (équations (5.13) et (5.22)) :

O ⊂ x(ci)x−1(ci)(O) = x(ci)x−1odo(ci)m−1

j (O)

112Annexe A. Preuve de l’absence de collision pour les systèmes respectant la propriété de

stabilité exponentielle

5. l’accroissement d’erreur odométrique entre ci et s est bornée par Dodo pour tout s ∈[ci, ci + ∆scheck],

6. les variables de configurations internes sont parfaitement mesurées :qodo(s) = (xodo(s),qint(s))

7. les discontinuités de localisation sont bornées :∀p, q, j ≤ p, q ≤ j + k et ∀s ∈ [ci, ci + ∆scheck],

dC(m−1p qref (s),m−1

q qref (s)) ≤ Dloc

8. les abscisses de localisations sont suffisamment éloignées pour que le système ait letemps de converger vers la trajectoire de référence :∀p, j ≤ p ≤ j + k − 1

dC(qodo(lp+1),m−1p qref (lp+1)) ≤

12dC(qodo(lp),m−1

p qref (lp))

9. Dodo + 3Dloc < Dclear

Alors sous ces hypothèses, la propriété 11 assure que le robot n’est pas en collision lors du suivide la trajectoire de référence :

∀s ∈ [ci, ci + ∆scheck], dO(q(s),O) > 0

Démonstration. La preuve se décompose en deux parties, la première étant très similaire à lapreuve de la propriété 10, page 86.

Première partie En appliquant la transformation solide x(ci)x−1odo(ci) à l’inégalité de l’hypo-

thèse 3, on obtient ∀s ∈ [ci, ci + ∆scheck] :

dO(x(ci)x−1(ci)q

ref (s),x(ci)x−1(ci)(O)) > Dclear

Ce qui implique d’après l’hypothèse 4 que pour tout s ∈ [ci, ci + ∆scheck]

dO(x(ci)x−1(ci)q

ref (s),O) > Dclear (A.1)

De façon similaire à la preuve de la propriété 10, la définition de dSE(2) et les hypothèses 5et 6 entraînent :

dC(x(ci).x−1odo(ci).qodo(s),q(s)) = dSE(2)(x(ci).x

−1odo(ci).xodo(s),x(s))

= dSE(2)(x−1odo(ci).xodo(s),x

−1(ci).x(s)) ≤ Dodo (A.2)

113

Seconde partie La seconde partie de la preuve exploite la stabilité exponentielle du système (Σ).La trajectoire de référence (m−1

p qref ,uref ) envoyée au système est admissible par morceau surchaque intervalle [lp, lp+1] avec p compris entre j et j + k − 1. L’hypothèse 1 et la stabilitéexponentielle entraînent que pour tout s ∈ [ci, lj+1],

dC(qodo(s), m−1j qref (s)) ≤ Dloc (A.3)

À l’abscisse lj+1, une nouvelle localisation globale a lieu, et la trajectoire de référence en-voyée au système (Σ) devient (m−1

j+1qref (lj+1),u

ref ). L’hypothèse 7 assure que :

dC(m−1j qref (lj+1), m

−1j+1q

ref (lj+1)) ≤ Dloc (A.4)

En appliquant l’inégalité triangulaire à (A.3) avec s = lj+1 et (A.4), on obtient :

dC(qodo(lj+1), m−1j+1q

ref (lj+1)) ≤ 2Dloc (A.5)

À nouveau, la stabilité exponentielle du système implique que cette inégalité reste valablesur l’intervalle [lj+1, lj+2], et pour tout s ∈ [lj+1, lj+2] :

dC(qodo(s), m−1j+1q

ref (s)) ≤ 2Dloc (A.6)

En utilisant l’hypothèse 8 de convergence entre deux abscisses de localisation, il vient même lamajoration plus fine :

dC(qodo(lj+2), m−1j+1q

ref (lj+2)) ≤ Dloc (A.7)

Récurrence sur tout l’intervalle La séquence des inégalités (A.3)-(A.7) peut être appli-quée récursivement pour tout p, j + 1 ≤ p ≤ j + k − 1. Et on peut écrire : ∀s ∈ [lp, lp+1]

dC(qodo(s), m−1p qref (s)) ≤ 2Dloc (A.8)

On écrit alors l’hypothèse 7 de la façon suivante : pour tout s ∈ [ci, ci + ∆scheck]

dC(m−1j qref (s), m−1

p qref (s)) ≤ Dloc (A.9)

L’inégalité triangulaire entre (A.9) et (A.8) donne directement pour tout s ∈ [ci, ci + ∆scheck] :

dC(m−1j qref (s), qodo(s)) ≤ 3Dloc (A.10)

Conclusion En appliquant maintenant la transformation solide x(ci)x−1odo(ci) à (A.10), on

obtient :dC(x(ci)x

−1odo(ci)m

−1j qref (s),x(ci)x

−1odo(ci)qodo(s)) ≤ 3Dloc

Étant donné que x(ci) = mjxodo(ci), cette dernière inégalité s’écrit :

dC(x(ci)x−1(ci)q

ref (s),x(ci)x−1odo(ci)qodo(s)) ≤ 3Dloc (A.11)

114Annexe A. Preuve de l’absence de collision pour les systèmes respectant la propriété de

stabilité exponentielle

En appliquant alors l’inégalité triangulaire entre (A.2) et (A.11), on obtient :

dC(x(ci)x−1(ci)q

ref (s),q(s)) ≤ 3Dloc + Dodo (A.12)

En utilisant maintenant l’inégalité triangulaire définie par la propriété 1 avec la différenceentre (A.1) et (A.12), on obtient le résultat désiré :

dO(q(s),O) > Dclear − (3Dloc + Dodo) ≥ 0

Bibliographie

[Abuhadrous ] I. Abuhadrous, F. Nashashibi & C. Laurgeau. 3-D Land Vehicle Localization :a Real-time Multi-Sensor Data Fusion Approach Using RTMAPS. Internatio-nal Conference on Advanced Robotics. Coimbra, Portugal, July .

[Bar-Shalom ] Y. Bar-Shalom & X.R. Li. Estimation and tracking : Principles, techniques,and software. Artech House, Incorporated, .

[Baraff ] D. Baraff. Dynamic Simulation of Non-Penetrating Rigid Bodies. Thèse deDoctorat, CUCS, Ithaca, . Cornell Computer Science Technical Report92-1275.

[Bobrow ] J.E. Bobrow, S. Dubowsky & J.S. Gibson. Time-Optimal Control of Robo-tic Manipulators along Specified Paths. International Journal on RoboticsResearch, vol. 4, pages 3–17, .

[Bonnafous a] D. Bonnafous. Exécution réactive de trajectoire pour robots mobiles non-holonomes. Thèse de Doctorat, INP Toulouse, .

[Bonnafous b] D. Bonnafous & F. Lamiraux. Sensor Based Trajectory Following for Nonho-lonomic Systems in Highly Cluttered Environment. International Conferenceon Intelligent Robots and Systems [IEE ], pages 892–897.

[Borenstein ] J. Borenstein & Koren Y. The Vector Field Histogram-Fast Obstacle avoi-dance for Mobile Robots. IEEE Transactions on Robotics and Automation,vol. 7, no 3, pages 278–288, June .

[Borenstein ] J. Borenstein, B. Everett & L. Feng. Navigating mobile robots : Systems andtechniques. A. K. Peters, Ltd., Wellesley, .

[Boyer ] F. Boyer. Optimisation de trajectoires pour systèmes nonholonomes. Rapporttechnique, LAAS-CNRS, Toulouse, .

[Boyer ] F. Boyer & F. Lamiraux. Trajectory deformation applied to kinodynamic mo-tion planning for a realistic car model. International Conference on Roboticsand Automation. Orlando, USA, May .

[Choset ] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki& S. Thrun. Principles of robot motion : Theory, algorithms, and implemen-tations. MIT Press, Cambridge, MA, .

[Divelbiss ] A. Divelbiss & J. Wen. A Path Space Approach to Nonholonomic MotionPlanning in the Presence of Obstacles. IEEE Transactions on Robotics andAutomation, vol. 13, no 3, pages 443–451, June .

116 BIBLIOGRAPHIE

[Espiau ] B. Espiau, F. Chaumette & P. Rives. A New Approach to Visual Servoingin Robotics. IEEE Trans. on Robotics and Automation, vol. 8, no 3, pages313–326, June .

[Feiten ] W. Feiten, R. Bauer & G. Lawitzky. Robust obstacle avoidance in unknownand cramped environments. International Conference on Robotics and Auto-mation [IEE ].

[Fleury ] S. Fleury, M. Herrb & R. Chatila. GenoM : a tool for the specification andthe implementation of operating modules in a distributed robot architecture.IROS, Grenoble, France, vol. 2, pages 842–848, septembre .

[Fliess ] M. Fliess, J. Lévine, Ph. Martin & P. Rouchon. Flatness and defect of non-linear systems : introductory theory and examples. International Journal ofControl, vol. 61, no 6, pages 1327–1361, .

[Fox ] D. Fox, W. Burgard & S. Thrun. The Dynamic Window Approach to CollisionAvoidance. IEEE Robotics and Automation Magazine, vol. 4, no 1, pages 23–33, March .

[Fraichard ] Th. Fraichard & H. Asama. Inevitable collision states - a step towards saferrobots ? Advanced Robotics, vol. 18, no 10, pages 1001–1024, .

[Hamel ] T. Hamel & R. Mahony. Visual servoing of an under-actuated dynamic rigid-body system : An image based approach. IEEE Transactions on Robotics andAutomation, vol. 18, no 2, pages 187–198, Apr .

[Hillion ] M. Hillion. Optimisation de trajectoires pour systèmes nonholonomes. Rap-port technique, LAAS-CNRS, Toulouse, .

[IEE ] IEEE. International conference on robotics and automation, San Diego, CA,USA, mai .

[IEE ] IEEE/RSJ. International conference on intelligent robots and systems, LasVegas, NE, USA, octobre .

[Kavraki ] L. E. Kavraki, Svestka. P., J.-C. Latombe & M.H. Overmars. ProbabilisticRoadmaps for Path Planning in High-Dimensioinal Configuration Spaces.IEEE Transactions on Robotics and Automation, vol. 12, no 4, pages 566–580, .

[Khatib ] O. Khatib. Real-Time Obstacle Avoidance for Manipulators and Mobile Ro-bots. International Journal on Robotics Research, vol. 5, no 1, pages 90–98,Spring .

[Khatib ] M. Khatib, H. Jaouni, R. Chatila & J.P. Laumond. Dynamic path modifica-tion for car-like nonholonomic mobile robots. International Conference onRobotics and Automation. Albuquerque, NM, USA, avril . IEEE.

[Lamiraux a] F. Lamiraux & J.-P. Laumond. Flatness and small-time controllability ofmulti-body mobile robots : applications to motion planning. EuropeanControl Conference. Brussels, Belgium, .

BIBLIOGRAPHIE 117

[Lamiraux b] F. Lamiraux & J.-P. Laumond. From paths to trajectories for multi-bodymobile robots. International Symposium on Experimental Robotics, pages237–245. Springer Verlag, .

[Lamiraux ] F. Lamiraux & J.-P. Laumond. Flatness and small-time controllability ofmulti-body mobile robots : application to motion planning. IEEE Transac-tions on Automatic Control, vol. 45, no 10, pages 1878–1881, octobre .

[Lamiraux ] F. Lamiraux, D. Bonnafous & O. Lefebvre. Reactive Path Deformation forNon-holonomic Mobile Robots. IEEE Transactions on Robotics, vol. 20, no 6,pages 967–977, Dec .

[Large ] F. Large, S. Sekhavat, C. Laugier & E. Gauthier. Towards robust sensor-basedmaneuvers for a car-like vehicle. International Conference on Robotics andAutomation. San Francisco, CA, USA, avril . IEEE.

[Latombe ] J.-C. Latombe. Robot motion planning. Kluwer, Boston, MA, .[Laugier ] C. Laugier, Th. Fraichard, Ph. Garnier, I. E. Paromtchik & A. Scheuer.

Sensor-Based Control Architecture for a Car-Like Vehicle. Autonomous Ro-bots, vol. 6, no 2, .

[LaValle ] S.M. LaValle. Rapidly-Exploring Random Trees : A New Tool for Path Plan-ning. Rapport technique no TR 98-11, Computer Science Dept., Iowa StateUniversity, .

[LaValle ] S. M. LaValle. Planning algorithms. Cambridge University Press, Cambridge,U.K., .

[Lefebvre ] O. Lefebvre, F. Lamiraux, C. Pradalier & Th. Fraichard. Obstacles Avoidancefor Car-Like Robots : Integration And Experimentation on Two Robots. Inter-national Conference on Robotics and Automation. New Orleans, LA, USA,Apr . IEEE.

[Lefebvre ] O. Lefebvre, F. Lamiraux & D. Bonnafous. Fast Computation of Robot-Obstacle Interactions in Nonholonomic Trajectory Deformation. Internatio-nal Conference on Robotics and Automation. Barcelona, Spain, Apr .

[Lefebvre ] O. Lefebvre & F. Lamiraux. Docking Task for Nonholonomic Mobile Robots.International Conference on Robotics and Automation. Orlando, USA, May.

[Lin ] M.C Lin. Efficient collision detection for animation and robotics. Thèse deDoctorat, University of California, Berkley, .

[Lozano-Pérez ] T. Lozano-Pérez. Spatial Planning : A Configuration Space Approach. IEEETransactions on Computing, vol. C-32, no 2, pages 108–120, .

[Luca ] A. De Luca, G. Oriolo & C. Samson. Robot motion planning and control,chapitre Feedback Control of a Nonholonomic Car-like Robot, pages 171–253. Springer, NY, .

[Malti ] A. Malti. Planification et exécution de mouvements référencés sur des amers.Thèse de Doctorat, UPS Toulouse, .

118 BIBLIOGRAPHIE

[Minguez ] J. Minguez & L. Montano. Nearness diagram navigation (ND) : a new realtime collision avoidance approach. International Conference on IntelligentRobots and Systems. Takamatsu, Japan, novembre . IEEE/RSJ.

[Minguez ] J. Minguez, L. Montano & J. Santos-Victor. Reactive navigation for non-holonomic robots using the ego-kinematic space. International Conferenceon Robotics and Automation. Washingtom, DC, USA, mai . IEEE.

[Nashashibi ] F. Nashashibi, B. Steux, P. Coulombeau & C. Laurgeau. RTMPAS a frame-work for prototyping automotive multisensors applications. IEEE IntelligentVehicles Symposium. Deaborn, MI, USA, .

[Paromtchik ] I.E. Paromtchik & C. Laugier. Motion generation and control for parking anautonomous vehicle. Proceedings of the IEEE International Conference onRobotics and Automation, .

[Pradalier ] C. Pradalier. Navigation intentionnelle d’un robot mobile. Thèse de Doctorat,INP Grenoble, .

[Quinlan ] S. Quinlan & O. Khatib. Elastic Bands : Connecting Path Planning andControl. International Conference on Robotics and Automation. Atlanta, GA,USA, mai . IEEE.

[Renaud ] M. Renaud & J.-Y. Fourquet. Time-optimal motions of robot manipulatorsincluding dynamics. O. Khatib, J.J. Craig & T. Lozano-Pérez, editeurs, Therobotics Review 2. MIT Press, .

[Rouchon ] P. Rouchon, M. Fliess, J. Lévine & Ph. Martin. Flatness and motion plan-ning : the car with n-trailers. Procedings of ECC’93. Groningen, .

[Schwartz a] J. T. Schwartz & M. Sharir. On the Piano Movers’ Problem : I. The Case of aTwo-Dimensional Rigid Polygonal Body Moving Amidst Polygonal Barriers.Communications on Pure and Applied Mathematics, vol. 36, pages 345–398,.

[Schwartz b] J. T. Schwartz & M. Sharir. On the Piano Movers’ Problem : II. GeneralTechniques for Computing Topological Properties of Algebraic Manifolds.Advanced applied Mathematics, vol. 4, pages 298–351, .

[Schwartz c] J. T. Schwartz & M. Sharir. On the Piano Movers’ Problem : III. Coordinatingthe Motion of Several Independent Bodies. International Journal of RoboticsResearch, vol. 2, no 3, pages 97–140, .

[Seelinger ] M. Seelinger, J.-D. Yoder, E.T. Baumgartner & S.B. Skaar. High PrecisionVisual Control of Mobile Manipulators. IEEE Transactions on Robotics andAutomation, vol. 18, no 6, pages 957–965, December .

[Shin ] K.G. Shin & N.D. McKay. Minimum-Time Control of Robotic Manipulatorswith Geometric Path Constraints. IEEE Transactions on Automatic Control,vol. 30, pages 531–541, .

BIBLIOGRAPHIE 119

[Siméon ] T Siméon, J.-P. Laumond & F. Lamiraux. Move3D : a generic platform forpath planning. 4th International Symposium on Assembly and Task Planning,.

[Slotine ] J.J.E. Slotine & H.S. Yang. Improving the efficiency of time-optimal path-following algorithms. IEEE Transactions on Robotics and Automation, vol. 5,no 1, pages 118–124, .

[Soueres ] P. Soueres & J.-P. Laumond. Shortest paths synthesis for a car-like robot.IEEE Transactions on Robotics and Automation, vol. 41, no 5, pages 672–688, May .

[Stentz ] A. Stentz. Optimal and Efficient Path Planning for Partially-Known Environ-ments. International Conference on Robotics and Automation [IEE ].

[Thrun ] S. Thrun. Robotic Mapping : A Survey. G. Lakemeyer & B. Nebel, editeurs,Exploring Artificial Intelligence in the New Millenium. Morgan Kaufmann,.

[Tsakiris ] D. Tsakiris, P. Rives & C. Samson. Applying Visual Servoing Techniques toControl Nonholonomic Mobile Robots. Proceedings of the IEEE Internatio-nal Conference on Intelligent Robots and Systems (IROS). Grenoble, France,September .

[Usher ] K. Usher, P Ridley & P. Corke. Visual servoing of a car-like vehicle - anapplication of omnidirectional vision. International Conference on Roboticsand Automation, pages 4288–4293. Taipei, Taiwan, septembre . IEEE.

[Wei ] R. Wei, R. Mahony & D. Austin. A bearing-only control law for stable do-cking of unicycles. International Conference on Intelligent Robots and Sys-tems [IEE ], pages 3793–3798.

[Wei ] R. Wei, D. Austin & R. Mahony. Biomimetic applicaton of desert ant visualnavigation for mobile robot docking with weighted landmarks. InternationalJournal on Intelligent Systems Technologies and Applications, vol. 1, pages174–190, .

120 BIBLIOGRAPHIE

Navigation autonome sans collision pour robots mobiles non-holonomes

Cette thèse traite de la navigation autonome en environnement encombré pour des véhiculesà roues soumis à des contraintes cinématiques de type nonholonome. Les applications de cestravaux sont par exemple l’automatisation de véhicules ou l’assistance au parking. Notre contri-bution porte sur le développement de méthodes qui réalisent certaines des fonctionnalités de lanavigation autonome et sur l’intégration de ces différentes fonctionnalités au sein d’une architec-ture générique, en tenant compte des spécificités des systèmes considérés. Nous présentons uneméthode d’évitement réactif d’obstacles pour systèmes nonholonomes et nous proposons uneméthode de parking référencé sur des amers pour de tels systèmes. Ensuite nous présentons unearchitecture générique pour l’intégration des fonctionnalités de localisation, d’évitement d’obs-tacles et de suivi de trajectoire. Enfin nous illustrons l’ensemble de ces travaux par des résultatsexpérimentaux obtenus avec plusieurs robots.

Collision-Free Autonomous Navigation for Nonholonomic Mo-bile Robots

This work deals with autonomous navigation in cluttered environments for wheeled mobilerobots subject to nonholonomic kinematic constraints. The potential applications of this workare for instance the development of autonomous cars and of parking assistance systems. Ourcontribution lies in the development of original methods to solve some of the functionalities ofautonomous navigation and in their integration into a generic software architecture, while takinginto account the specificities of the systems we deal with. We present an obstacle avoidancemethod for nonholonomic systems and we propose a landmark-based parking method for suchsystems. Then, we present a generic architecture for the integration of the functionalities oflocalisation, obstacle avoidance and trajectory following. Eventually, we illustrate this work withsome experimental results obtained with several robots.

Navigation autonome sans collision pour robots mobiles ...

Documents

Transcript of Navigation autonome sans collision pour robots mobiles ...