FR 2656435 A1 19910628 FR 8917105 A 19891222 I PROCEDE DE RECALAGE SUR UNE TRAJECTOIRE THEORIQUE POUR UN VEHICULE EN MODIFIANT LA COURBURE DE LA TRAJECTOIRE REELLE DESCRIPTION La présente invention se rapporte à un pro- cédé de recalage sur une trajectoire théorique pour un véhicule, notamment robotisé, qui consiste à modi- fier la courbure de la trajectoire réelle. On connaît déjà plusieurs procédés destinés à ramener un véhicule sur une trajectoire de consigne ou, en d'autres termes, à faire conincider une trajec- toire que le véhicule suit réellement avec une trajec- toire théorique. D'une façon générale, la trajectoire théo- rique est modélisée par une succession de segments rectilignes ou courbes à rayon de courbure constant ou non qui se joignent en des points de localisation ou de repérage identifiés grâce des balises Le brevet français de numéro d'enregistrement 89 08049 décrit un certain nombre de moyens qui peuvent faire office de balises, en particulier des émetteurs d'ondes radio- phoniques, des poteaux réfléchissants ou des aimants enterrés dans le sol Quel que soit le mode de locali- sation adopté, des repérages de la position absolue du véhicule, appelés aussi localisations et qui per- mettent de déduire l'écart entre la position réelle du véhicule et le point de localisation, sont effec- tuées par des capteurs appropriés du véhicule quand ce dernier passe près de ces points de localisation. Un procédé connu de recalage consiste, une fois un écart déterminé, à calculer un tronçon de ralliement à la trajectoire théorique qui aboutit à un des points de localisation suivants de celle-ci. Pour cela, les coordonnées d'un certain nombre de points intermédiaires entre lesquels on décompose le tronçon de ralliement sont calculées Si on veut éviter des variations trop brusques de cap du véhicule et converger assez rapidement vers la trajectoire théorique, le nombre des points intermédiaires et donc le volume de calcul doit être considérablement accru, ce qui peut nécessiter un arrêt temporaire du véhicule. Un autre procédé consiste, une fois la loca- lisation faite et l'écart avec le point de localisation calculé, à faire suivre au véhicule un tronçon de raccordement à ce même point de localisation et non à un des points suivants Dans certains cas, cette méthode implique des déplacements transversaux du véhicule qui ne sont pas toujours possibles ou seule- ment avec une grande difficulté. L'invention permet d'éviter ces inconvénients en offrant la possibilité de recaler le véhicule sur sa trajectoire théorique au moyen de trajectoires de ralliement faciles à parcourir, qu'il n'est pas nécessaire de calculer explicitement et qui convergent rapidement sur la trajectoire théorique Les seuls calculs nécessaires sont beaucoup plus simples et sont répartis sur toute la durée de parcours de la trajectoire de ralliement. Sous sa forme la plus générale, le procédé consiste, quand un repérage de la position absolue du véhicule est effectué pour donner l'écart entre la position réelle du véhicule et la trajectoire théo- rique, à commander des modifications successives de la courbure de la trajectoire réelle en fonction du cap instantané du véhicule et de la courbure instan- tanée de la trajectoire réelle pour infléchir cette dernière vers la trajectoire théorique. Le véhicule est muni de capteurs de localisa- tion utilisés pour les repérages de la position du véhicule et d'un capteur de cap Ce capteur est un moyen de mesure directe ou un moyen de calcul permet- tant d'obtenir le cap du véhicule, par exemple par dérivation de sa position La courbure de la trajec- toire réelle peut s'apprécier de différentes manières selon la constitution du véhicule et selon son équipe- ment en capteurs de localisation Dans le cas d'un véhicule à roues motrices indépendantes et non orien- tables, elle s'évalue en fonction des vitesses diffé- rentes sur les roues motrices droite et gauche On peut procéder de même avec un véhicule à chenilles. Dans le cas de roues braquables, il faudra mesurer leur orientation. Les modifications successives de la courbure de la trajectoire peuvent être commandées suivant un échantillonnage temporel ou spatial, c'est-àdire à des intervalles déterminés de temps ou de distance parcourue Dans ce dernier cas, on utilise des cap- teurs de déplacements normalement présents sur les robots tels que des roues codeuses roulant sur le sol De tels capteurs n'ont pas une précision suffi- sante pour permettre de localiser le véhicule dans l'absolu, mais permettent cependant des évaluations suffisantes sur de courtes distances. La trajectoire théorique peut être définie ou modélisée sous forme d'un cap théorique, d'une droite, d'un point de ralliement, d'un cercle ou d'un arc de cercle, ou d'une façon générale par une courbe quelconque définie sous forme d'un tableau de points. Selon le cas, les algorithmes de recalage qui consti- tuent la mise en oeuvre effective de l'invention seront donc différents Certains d'entre eux vont maintenant être décrits à l'aide des figures suivantes annexées à titre illustratif et non limitatif: la figure 1 représente les deux méthodes décrites plus haut de l'art antérieur; et les figures 2 à 6 décrivent cinq réalisa- tions particulières de l'invention. On a représenté sur la figure 1 deux tronçons 51 et 52 d'une trajectoire théorique qui s'étendent respectivement entre des points de localisation P 1, P 2 et P 2, P 3 Quand un véhicule 1 essaie de suivre cette trajectoire théorique, par exemple à partir du point P 1, il le fait avec une certaine erreur et parcourt en réalité un tronçon S'1 joignant Le point Pl à un point P'2 distant latéralement ou longitudina- lement du point P 2 Quand le véhicule 1 arrive au point P'2, ses capteurs de localisation lui permettent de calculer la distance entre les deux points P 2 et P'2 Dans le cas o le point P 2 est matérialisé par une balise magnétique enterrée, le véhicule 1 est muni d'une rangée latérale 2 de capteurs magnétiques. On détermine alors l'écart des points P 2 et P'2 en identifiant le capteur magnétique qui enregistre le champ le plus intense, ou en effectuant une interpola- tion en fonction des champs enregistrés par deux capteurs voisins passant de part et d'autre de la balise magnétique. Une fois que la position du point P'2 est déterminée, une première méthode connue consiste à déterminer un tronçon de ralliement S'2 joignant les points P'2 et P 3 Le véhicule 1 est orienté pour suivre ce segment de ralliement S'2, qui est de même nature, rectiligne ou courbe, que le tronçon 52, et qui est défini par un certain nombre de points de passage P" dont les coordonnées sont calculées en fonction des points P'2 et P 3. Une seconde méthode connue consiste à faire parcourir au véhicule 1 un tronçon de raccordement S'3 entre P'2 et P 2 pour le faire repartir de ce der- nier point Le tronçon de raccordement S'3 est parfois sensiblement à angle droit avec les tronçons S'1 et 52 et de courte longueur, si bien qu'il ne peut être parcouru directement mais seulement à la suite de manoeuvres compliquées du véhicule 1 Il peut même arriver que P'2 soit devant P 2, ce qui impose un par- cours vers l'arrière. Sur la figure 2, on voit que le véhicule 1 est équipé d'une roue motrice gauche 3 g et d'une roue motrice droite 3 d, d'un capteur de cap 4 tel qu'une boussole et d'un capteur de déplacement relatif tel qu'une roue codeuse La trajectoire théorique est référencée Tth 1 et se définit par un cap théorique Od par rapport à une référence angulaire fixe en poin- tillés Le véhicule 1 vient d'arriver à un point Q 1 après avoir parcouru un segment So de la trajectoire réelle Trl, et le cap qu'il suit à ce moment est défini par rapport à la même référence angulaire par l'angle O Une localisation effectuée en ce point Q 1 consiste ici en une détermination du cap O. Le segment de ralliement 51 que le véhicule I va maintenant suivre pour se raccorder à la trajec- toire théorique Tth I, c'est-à-dire suivre le cap théorique Od, est maintenant défini par la formule ( 1): g = a(d-0 fc, o aet j sont des coefficients constants. Dans cette formule, c représente la courbure instantanée de la trajectoire réelle et g une variation de courbure qui permet d'infléchir peu à peu la trajec- toire réelle Trl vers la trajectoire théorique Tth 1. Plus exactement, si on désigne par S l'abscisse curvi- J ligne parcourue sur la trajectoire réelle Trl, c peut être définie mathématiquement par: do d 20 c d et g = ds ds 2 En pratique, la courbure c peut être évaluée à l'aide de la commande des organes locomoteurs du véhicule 1 Dans le cas de roues 3 roulant sans glisser sur le sol, si on appelle r le rayon de courbure ins- tantané de la trajectoire Trl et v la voie du véhi- cule 3, og et d les vitesses angulaires des essieux des roues 3 g et 3 d qui peuvent être facilement mesurées par des tachymètres, le rayon de courbure r peut s'éva- luer par la formule: v 2 wg r + 2 îd v Cd avec les notations de la figure 2 Or, r est l'inverse de c. Le segment de ralliement 51 est donc créé en effectuant, en un certain nombre de points R qui se succèdent sur ce segment 51, une évaluation de la courbure c, une lecture du cap O et une modification de la courbure c instantanée à l'aide de la formule ( 1) en modifiant, dans le cas considéré, les vitesses wg et îd Une des vitesses peut être accrue et l'autre diminuée de manière à maintenir la vitesse moyenne du véhicule 1 constante Ce procédé est poursuivi jusqu'au point de localisation suivant Q 2, o de nou- veaux calculs peuvent être repris si le cap théorique a changé Les points R se succèdent à des intervalles égaux de distance ou de temps de parcours définis à l'avance en fonction des caractéristiques du véhicule 1 et de la trajectoire théorique Tthl, mais leur posi- tion n'est pas calculée et reste inconnue a et sont des coefficients constants déterminés par l'utilisateur en fonction notamment des possibilités cinématiques du véhicule. La figure 3 représente le cas o la trajec- toire théorique est une droite Tth 2 qui doit être parcourue dans un sens correspondant à un cap Od. Le procédé de recalage ressemble au précédent avec cette différence qu'il faut tenir compte de la distance Y entre le point o se trouve le véhicule 1, tel que Q 1, et la droite Tth 2. La formule utilisée est la formule ( 2): 1 ( 9 = ( O d) Y fsin( O d) 'Yc cos( O Od) )cosc O-0 d) + c 2 sin( O Od), ou a, 3 et 'y sont des coefficients constants et o Y désigne donc la distance instantanée du véhicule 1 à la droite Tth 2 à chaque changement de courbure. La localisation fournit au véhicule I son cap instan- tané O au point de localication Q 1 et la distance Y de ce point à la droite Tth 2. Une procédure particulière est mise en oeuvre pour éviter l'indétermination lorsque cos (O Od) est proche de zéro On distingue alors deux cas en fonction de Y Si Y est supérieure à un seuil, c'est-à- dire si un véhicule 1 est éloigné de la droite Tth 2, le véhicule I est piloté temporairement dans la direction d'un cap perpendiculaire à la droite Tth 2 Quand Y devient inférieure au seuil fixé, le véhicule 1 est désormais orienté suivant un cap paral- lèle à la droite Tth 2, ce qui infléchit la direction du véhicule 1 et rend cos ( 0-0 d) non nul On peut alors revenir au recalage sur la droite Tth 2 par la formule ( 2) Les distances Y successives peuvent être évaluées simplement par les indications des capteurs relatifs de cap 4 et de déplacement 5 en fonction de la distance précédemment calculée et du déplacement du véhicule 1 depuis le point précédent correspondant, sans qu'il soit nécessaire de calculer au préalable la position exacte des points R. La figure 4 représente un autre cas d'appli- cation de l'invention o la trajectoire théorique est ici un point Tth 3 p et nées polaires du véhicule 1 par rapport au point Tth 3, et on utilise la formule ( 3) suivante pour obtenir la convergence souhaitée du tronçon de ralliement 51: g = a CO - de cap 4 et de déplacement 5, comme pour le cas pré- cédent. La figure 5 représente un nouveau cas envisa- geable dans lequel la trajectoire théorique Tth 4 est un cercle de centre O et de rayon Po Comme précédem- ment, p et l'asservissement sur Le cercle Tth 4 par l'asservisse- ment au point O en appliquant la formule ( 3) En même temps, on examine l'évolution de la quantité (p po) qui correspond à la distance du véhicule 1 au cercle Tth 4 Quand cette valeur passe au-dessous d'un seuil, on utilise désormais un asservissement sur une direc- tion tangente au cercle de manière à infléchir le tronçon de ralliement 51 Quand sin ( 0-0) est devenu non nul, on revient à l'asservissement sur le cercle Tth 4. L'emploi de la formule ( 4) peut être généra- lisé à des trajectoires théoriques Tth 5 de forme quel- conque, définies par exemple par les coordonnées de certains de leurs points U enregistrées dans le véhi- cule en tant que tableaux La figure 6 représente le mode de recalage utilisé alors: s'il s'agit de ramener le véhicule 1 sur un point Ui de la trajectoire théorique Tth 5, on calcule l'équation du cercle oscula- teur Tth 5 i à la courbe Tth 5 au point Ui, c'est-à-dire du cercle tangent à la trajectoire Tth 5 en ce point et dont le rayon correspond au rayon de courbure de la trajectoire Tth 5 en ce même point Une convergence rapide sur la trajectoire théorique Tth 5 est générale- ment obtenue en pratique Le procédé est poursuivi jusqu'à proximité du point Ui objectif, après quoi on calcule le cercle osculateur à la trajectoire au point suivant Uij+ 1 et on recommence le processus de recalage avec ce nouveau cercle. Dans tous ces cas, l'invention peut être mise en oeuvre d'autres manières, mais les formules indiquées permettent une convergence rapide et sûre. 1 1 REVENDICATIONS 1 Procédé de recalage sur une trajectoire théorique (Tth) pour un véhicule équipé d'un capteur de cap ( 4) et parcourant une trajectoire réelle (Tr), comprenant des localisations périodiques (Q) de la position du véhicule ( 1) par rapport à la trajectoire réelle, caractérisé en ce qu'il consiste, après les localisations, à commander des modifications succes- sives (g) de la courbure de la trajectoire réelle (Tr) en fonction du cap instantané ( 0) du véhicule et de la courbure instantanée (c) de la trajectoire réelle pour infléchir la trajectoire réelle vers la trajectoire théorique. 2 Procédé de recalage sur une trajectoire théorique pour un véhicule selon la revendication 1, dans lequel la trajectoire théorique (Tth 1) est définie par un cap théorique ( O d), caractérisé en ce qu'il consiste à appliquer à la courbure instantanée (c) de la trajectoire réelle des modifications de courbure (g) en fonction du cap instantané ( 0) du véhicule ( 1) selon la formule g = a(Od-0) Oc, o (a) et (y) sont des coefficients constants. 3 Procédé de recalage sur une trajectoire théorique pour un véhicule selon la revendication 1, dans lequel la trajectoire théorique est définie par une droite (Tth 2) de direction ( O d), caractérisé en ce qu'il consiste à appliquer à la courbure instan- tanée (c) de la trajectoire réelle des modifications de courbure (g) en fonction du cap instantané ( 0) du véhicule et de la distance instantanée (Y) du véhi- cule à la droite selon la formule: g = -t Y-Usin( O Od) Yc cos( O Od) cos( O Od) + c 2 sin( O Od)I, o (a), (a) et ( 7) sont des coefficients constants. 4 Procédé de recalage sur une trajectoire théorique pour un véhicule selon la revendication 1, dans lequel la trajectoire théorique est définie par un point (Tth 3), caractérisé en ce qu'il consiste à appliquer à la courbure instantanée (c) de la trajec- toire réelle des modifications de courbure (g) en fonction du cap instantané ( 0) du véhicule et de la position instantanée du véhicule par rapport au point, exprimée en coordonnées polaires ( pet o (a) et (ô) sont des coefficients constants. Procédé de recalage sur une trajectoire théorique pour un véhicule selon la revendication 1, dans lequel la trajectoire théorique est un cercle (Tth 4), caractérisé en ce qu'il consiste à appliquer à la courbure instantanée (c) de la trajectoire réelle des modifications de courbure (g) en fonction du cap instantané ( 0) du véhicule, du rayon (po) du cercle et de la position instantanée du véhicule par rapport au centre du cercle exprimée en coordonnées polaires (pet o (t, a et Y) sont des coefficients constants. 6 Procédé de recalage sur une trajectoire théorique pour un véhicu Le se Lon la revendication , caractérisé en ce que Le cercle (Tth Si) est obtenu à La suite d'une étape préLiminaire de modélisation d'une autre trajectoire théorique (Tths), effectuée après La Loca Lisation et avant Les modifications de courbure, Le cerc Le étant un cerc Le osculateur à cette autre trajectoire théorique.