METHOD AND SYSTEM FOR COLLECTING MATERIAL BODIES

专利摘要:
A method of perception of material bodies comprising the following steps: a) Acquisition of a plurality of distance measurements of said material bodies from one or more sensors (C1 ... Cn); b) Applying an inverse sensor model on a occupancy grid to determine a probability of occupation of a set of cells of said grid; and c) Constructing a consolidated occupancy grid by merging the probabilities of occupancy estimated during step b); characterized in that each said inverse sensor model is a discrete model, associating with each cell of the corresponding occupation grid, and for each distance measurement, a probability class chosen within the same set of cardinality finished and identified by an entire index; and in that said step c) is implemented by means of integer calculations performed on the indices of the classes of probabilities determined during said step b). Hardware body perception system adapted to implement such a method.
公开号:FR3041451A1
申请号:FR1558919
申请日:2015-09-22
公开日:2017-03-24
发明作者:Julien Mottin；Pascual Diego Puschini；Andriamahefa Tiana Rakotovao
申请人:Commissariat a lEnergie Atomique CEA；Commissariat a lEnergie Atomique et aux Energies Alternatives CEA；
IPC主号:

专利说明:

l’indice n prenant des valeurs entières relatives et le paramètre k étant un entier relatif non nul et, lors de ladite étape c), la fusion F(pi, pj) entre deux classes de probabilités p,, pj e Sk peut être calculée par application des équations suivantes lorsque les indices « i » et « j » ont le même signe : • F(pj,pj)=pi+j-k.i.j si i,j<0; • F(pi,pj)=pj+j+k ij si i,j>0.
Plus particulièrement, lors de ladite étape c), la fusion F(pi, Pi) entre deux classes de probabilités p,. Pj e Sk peut être calculée par application des équations suivantes lorsque les indices « i » et « j » ont des signes opposés : • F(pi,Pj)= P(i+j)+(i+kj} si i<0, j>0 et |i|>|j| ; • F(pi,Pj)= P(i+jHi-kj) si i<0, j>0 et |i|<|j|. où le symbole -î- désigne une division entière.
On peut avoir, en particulier, k=1.
Ladite étape c) peut comprendre la mise en œuvre de changements de repère pour construire ladite grille d’occupation consolidée à partir de probabilités d’occupation de cellules de grilles d’occupation associées à des capteurs non co-localisés.
Le procédé peut comprendre également une étape préliminaire de construction des modèles inverses d’au moins un dit capteur sur la grille d’occupation correspondante, mise en œuvre par application de l’équation suivante :
où : • P(Oj|z) représente la probabilité d’occupation de la cellule d’indice « i » de la grille d’occupation, lesdites cellules étant ordonnées par distance croissante dudit capteur ; • N est le nombre de cellules de ladite grille d’occupation ; • z représente une mesure de distance issue du capteur ; et • p(z|xk) représente le modèle direct du capteur, exprimant la densité de probabilité de la mesure « z » fournie par le capteur lorsque la position Xk de la cellule k de la grille d’occupation, mais aucune autre cellule plus proche du capteur, est occupée par un corps matériel.
Le procédé peut comprendre également une étape préliminaire de détermination d’une résolution spatiale d'au moins une dite grille d’occupation, ladite résolution spatiale étant choisi la plus petite compatible avec la contrainte max[(z|xk)]>pmjn, où p(z|xk) est un modèle direct du capteur, exprimant la densité de probabilité de la mesure « z » fournie par le capteur lorsque la cellule xk de la grille d’occupation, mais aucune autre cellule plus proche du capteur, est occupée par un corps matériel, et pmin est la plus petite classe de probabilité appartenant audit ensemble et supérieure à 0,5.
Un autre objet de l’invention est un système de perception de corps matériels comprenant : au moins un port d’entrée pour recevoir une pluralité de signaux représentatifs de mesures de distance desdits corps matériels issues d'un ou plusieurs capteurs ; un module de traitement de données configuré pour recevoir en entrée lesdits signaux et les utiliser pour construire une grille d’occupation consolidée en appliquant un procédé tel que défini ci-dessus; et au moins un port de sortie pour un signal représentatif de ladite grille d’occupation consolidée.
Selon des modes de réalisation particuliers :
Le système peut comprendre également un ou plusieurs capteurs de distance adaptés pour générer des signaux représentatifs d’une pluralité de mesures de distance desdits corps matériels et reliés audit ou auxdits ports d’entrée.
Ledit module de traitement de données peut comprendre au moins un bloc matériel de calcul de probabilités d'occupation comprenant une mémoire stockant, sous la forme d’une table de correspondance, un modèle inverse d’un capteur associant à chaque mesure de distance un vecteur d’entiers représentant des indices de classes de probabilité associées à des cellules respectives d’une grille d’occupation.
Ledit module de traitement de données peut comprendre un bloc matériel de calcul entier, dit de consolidation, configuré pour recevoir en entrée une pluralité d’entiers représentant des indices de classes de probabilité associées à des cellules de grilles d’occupation respectives, et pour calculer un indice d’une classe de probabilité associée à une cellule de ladite grille d’occupation consolidée.
Plus particulièrement, ledit module de traitement de données peut comprendre aussi au moins un bloc de calcul, dit de changement de repère, configuré pour : recevoir en entrée au moins un premier vecteur d’entiers, issu d’un bloc matériel de calcul de probabilités d’occupation correspondant et représentant des indices de classes de probabilité associées à des cellules respectives d’une grille d’occupation ; le convertir en un second vecteur d’entiers représentant des indices de classes de probabilité associées à des cellules respectives d’une autre grille d’occupation, coïncidant spatialement avec ladite grille d’occupation consolidée ; et fournir ledit second vecteur d’entier en entrée dudit bloc matériel de consolidation.
Ledit module de traitement de données peut être configuré pour recevoir en entrée des signaux représentatifs de mesures de distance acquises successivement par un même capteur ou des capteurs co-localisés et pour construire une grille d’occupation consolidée à partir d’une pluralité de ces signaux, correspondant à des instants d’acquisition successifs. D’autres caractéristiques, détails et avantages de l’invention ressortiront à la lecture de la description faite en référence aux dessins annexés donnés à titre d’exemple et qui illustrent respectivement :
La figure 1, la notion de modèle « direct » d’un capteur de distance ;
La figure 2, la notion de grille d’occupation ;
La figure 3, la notion de modèle « inverse » d’un capteur de distance ;
La figure 4, la discrétisation spatial d’un modèle inverse sur une grille d’occupation ;
Les figures 5A à 5D, une méthode de calcul du modèle inverse d’un capteur sur une grille d’occupation ;
Les figures 6A à 6D, une méthode de choix de la résolution spatiale optimale d’une grille d’occupation ;
Les figures 7A et 7B» divers systèmes de classes de probabilité ;
La figure 8, deux méthodes de quantification d’un modèle inverse d’un capteur sur une grille d’occupation ;
Les figures 9A et 9B, un système de perception d’obstacles selon un premier mode de réalisation de l’invention ;
Les figures 10A et 10B, un système de perception d’obstacles selon un deuxième mode de réalisation de l’invention ; et
La figure 11, un système de perception d'obstacles selon un troisième mode de réalisation de l’invention.
Dans la description détaillée qui suit, il sera fait référence au cas de la perception d’obstacles. Cependant, tout ce qui est décrit s’applique plus généralement à la perception de toute sorte de corps matériels.
Le plus souvent, les capteurs employés pour la navigation renseignent sur la distance des obstacles environnants ; on parle alors de capteurs de distance. Pour rendre compte de la précision d’un capteur, de son éventuelle erreur ou de sa résolution, on introduit un modèle probabiliste. L'idée est qu’une mesure en sortie du capteur n’indique pas nécessairement la distance exacte entre l’obstacle et le capteur, et que par conséquent il convient de raisonner sur la probabilité que l'obstacle soit à une distance donnée connaissant la réponse du capteur.
Si on note d la distance réelle entre un obstacle et le capteur, et z la sortie du capteur, on s'intéresse à la fonction de densité de probabilité conditionnelle p(z|d) qui modélise le lien entre la position réelle d'un obstacle et son estimation vue par le capteur (« modèle direct »). La figure 1 présente un exemple de modèle direct d’un capteur ; on considère un espace linéaire de 50 m de long et on suppose qu’un obstacle se trouve à d=25m du capteur. Pour un capteur avec une erreur modélisable par une fonction gaussienne, la réponse z la plus probable sera proche de 25 m, mais d’autres valeurs seront possibles, avec une densité de probabilité définie par la courbe. Dans le cas d’un capteur idéal on aurait p(z|d)=5(z-d), où δ est une Delta de Dirac, et la mesure serait toujours égale à la vraie distance. Le modèle direct d’un capteur peut être déterminé de manière expérimentale ; typiquement il peut être construit à partir de données fournies par le constructeur (dans le cas gaussien, la valeur de l’écart-type suffit pour caractériser le modèle).
Dans la suite, on notera Ω un référentiel spatial à une, deux ou trois dimensions ; une grille d’occupation GO est une partition d’un sous-ensemble continu et borné de Ω en un nombre N de parties, dénommées cellules et désignées par un indice ie[0, N-1]. On indique par c la cellule d’indice i. Sans perte de généralité, on considérera dans la suite une grille d’occupation monodimensionnelle observée par un seul capteur de distance C (ou une pluralité de capteurs co-localisés), l’indice i étant croissant avec l’éloignement du capteur (co étant donc la cellule la plus proche du capteur et cN-i la plus éloignée). Cette configuration est illustrée par la figure 2.
Un obstacle A est un sous-ensemble continu borné de Ω. On dit qu’une cellule c, est occupée par un obstacle A si Anc^0, qu’elle n’est pas occupée par A si Ancj=0. En d’autres termes, si l’obstacle recouvre même partiellement la cellule, on considère qu’elle est occupée. D’autres conventions sont possibles, mais en tout cas une cellule doit être soit libre, soit occupée.
On considère pour chacune des cellules de la grille, l'expérience aléatoire binaire « état » pouvant avoir l'une des deux issues {occupée; vide} consistant à connaître si la cellule contient un obstacle ou non. On notera e, l'état de la cellule q, o,· la réalisation ej = occupée et Vj la réalisation ei = vide. Dans une grille, on considère que toutes les cellules sont indépendantes, de sorte que v i,j e [0, N-1], P(Oi A Oj)= P(Oj)-P(Oj) (1 ) où λ est l’opérateur logique « et » et P(.) dénote la probabilité d’un évènement (à ne pas confondre avec une densité de probabilité, désignée par un « p » minuscule).
On considère également que la position des obstacles ne peut être connue qu’à l’aide de capteurs de distance incertains, caractérisés par un modèle probabiliste tel que décrit plus haut qui peut s’écrire de manière plus générale p{zx), x étant la position d’un obstacle (en plusieurs dimensions, il s’agit d’un vecteur, exprimés en coordonnées cartésiennes, sphériques, polaires, etc. et pas d’un simple scalaire). Ces capteurs peuvent être des lasers télémétriques (appelés aussi lidars), des sonars, des radars infrarouges, des caméras à temps de vol, etc.
Une mesure z issue d’un capteur permet de déterminer la probabilité d’occupation Ρ(ο,|ζ) d’une cellule c,. Pour une mesure z donnée, l’ensemble des probabilités Ρ(ο,|ζ) V i e [0, N-1] constitue le modèle inverse du capteur sur la grille. Alors que le modèle direct du capteur renseigne sur la réponse du capteur en fonction du monde physique, le modèle inverse exprime l'impact de la mesure sur la grille d'occupation qui est le modèle du monde physique que l'on adopte, ce qui justifie l'appellation modèle inverse.
La figure 3 présente un exemple typique de modèle inverse d'un capteur de distance, dans un cas où z=25m. On peut vérifier que la probabilité d’occupation est quasi-nulle pour les cellules qui se trouvent à une distance inférieure à 24,25 m du capteur et atteint un pic pour une distance de 25 m (correspondant à la mesure fournie par le capteur). Au-delà de 25 m, la probabilité d’occupation diminue jusqu’à se stabiliser à une valeur de 0,5, indicative d’une méconnaissance totale de l'état d’occupation des cellules qui, étant situées au-delà de l’obstacle, sont masquées par ce dernier et donc inaccessibles au capteur.
Conformément à l’usage qui prévaut dans la littérature, la figure 3 représente le modèle inverse au moyen d’une courbe lissée. Une représentation plus correcte serait de n’afficher que les points correspondant aux limites des cellules de la grille : en effet, on ne peut pas distinguer une cellule « partiellement » occupée d’une autre qui le serait « totalement », dans tous les cas la distance à l’obstacle sera estimée comme étant la distance à la cellule correspondante. C’est là l’erreur spatiale introduite par la grille.
Une version plus juste du modèle inverse de la figure 3, tenant compte de cette discrétisation spatiale induite par la grille est présentée sur la figure 4.
Il convient de souligner que les notions de « occupation » et de « distance de l’obstacle » ne sont pas tout à fait équivalentes. En effet, dire qu’un obstacle est à une distance z de capteur ne signifie pas seulement qu’une certaine cellule est occupée, mais aussi que toutes les autres cellules de la grille plus proches du capteur sont libres (autrement, le premier obstacle aurait été vu à une distance inférieure à z). Sur la figure 2 précitée, l’obstacle A se trouve dans la cellule d’indice i (en noir) ; les cellules d’indice inférieur à i sont représentées en blanc pour indiquer qu’elles sont libres, celles d’indice supérieur à i le sont en gris pour indiquer que leur état d’occupation est inconnu.
Si on tient compte de la notion de capteur Incertain caractérisé par son modèle (direct) p(zx) et que l'on note d, la distance de la cellule q par rapport au point de mesure et x[ le point de la cellule q le plus proche dudit point de mesure, on a :
(2) L'équation (2) indique que le modèle du capteur évalué en un point qui est à la frontière d’une cellule de la grille (xO est égal à la densité de probabilité de réponse du capteur pour une configuration de grille correspondante, à savoir une grille où les cellules les plus proches que la cellule i sont vides, la cellule i est occupée, et les états d’occupation des cellules plus éloignées que la cellule i ne sont pas déterminées* C'est en exploitant cette information que A. Elfes, dans son article précité, a proposé un procédé de construction du modèle inverse du capteur. Une explication de ce procédé est donnée ci-dessous.
Le théorème de Bayes permet d*exprimer le modèle Inverse d’un capteur P(Oi|z) dela manière suivante :
(3). où P(Oj) et P(Vj) désignent les probabilités a priorf (c’est-à-dire sans connaître la position des obstacles, ni la sortie du capteur) que la cellule et soit occupée ou libre, respectivement. Dans la suite on fera l’hypothèse P(oi) =
P(Vj) = 1/2, mais une généralisation ne pose pas de difficulté théorique fondamentale.
On obtient alors :
(4)
Le calcul des termes ρ(ζΐθί) et p(zjVj) peut se faire en utilisant le théorème de Kolmogorov sur toutes les configurations possibles de la grille. Une configuration est formée par un N-uplet {eo, ei ... θν-ι} où et e {o^Vj} ; une grille de N cellules a 2N configurations possibles.
Pour le terme ρ(ζ|ο,) les configurations de grille possibles sont de la forme G%. = (e0,... oit... où le la cellule i est occupée ; il y a 2N*1 de telles grilles. Pour le terme ρ(ζ|ν,) les configurations de grille possibles sont de la forme Gy. = (e0,... Vi,... où la cellule q est vide ; il y a 2N'1 de telles grilles.
On peut donc écrire : (5a)
(5b)
Les termes ) et ) valent 1/2N'1. En utilisant l’équation (2) liant l’occupation et la distance mesurée, on peut déterminer, à partir d’une configuration de grille G*., la position χγ telle que P(zÇk) — p(Mxk), Cette position est celle de la cellule occupée de la configuration Gjï. la plus proche de l’observateur. Les équations (5a) et (5b) peuvent donc se réécrire :
(6) dont les termes peuvent être calculés directement à partir du modèle direct du capteur. En réinjectant (6) dans (4) ont peut donc calculer, en principe, le modèle inverse du capteur sur la grille d’occupation considérée.
La limitation principale de cette méthode provient de l'explosion exponentielle du nombre de termes dans la somme de l'équation (6), rendant pratiquement impossible le calcul de la somme. En effet, dans l’art antérieur, on utilise généralement des expressions analytiques du modèle inverse, en renonçant à l'exprimer directement en fonction de ρ(ζ|ο,). Cela implique la perte du lien avec le modèle directe, qui est le seul à être directement accessible par l’expérience, et donc de la capacité de mesurer l’erreur introduit par la modélisation.
Un premier apport de la présente invention est une méthode simplifiée pour calculer le modèle inverse - linéaire en N au lieu d’être exponentielle - sans introduire d’approximation par rapport à l'utilisation des équations (6) et (4). Dans la suite on considérera, dans un but de simplification de l’exposé mais sans perte de généralité, le cas d’une grille d’occupation monodimensionnelle, où la position d’un obstacle est représentée par une variable scalaire x et pour laquelle (6) peut s’écrire :
(7)
Comme on considère une grille monodimensionnelle à N cellules, X|< peut prendre seulement la valeur de position de l’une des cellules cj avec je[0; N - 1]. Ainsi, les termes de la somme (7) ne peuvent prendre que N valeurs différentes. En factorisant toutes les grilles donnant la même valeur de p(z|xk), on peut donc ramener l'équatîon (7) à une somme de N termes seulement * au lieu de 2N'1.. La complexité du calcul passe d’exponentielle à linéaire.
On commence par considérer le cas e, = Oj. On fixe q dans son état occupé et on fixe la distance du premier obstacle comme étant celle de la cellule Cr, de sorte que cette distance est effectivement xr. On cherche donc le nombre de grilles qui sont telles que le premier obstacle est vu à la position xr sachant que la cellule ci est occupée. Trois cas de figure sont envisageables : 1. k < i : la cellule k est occupée (1 cellule), les cellules q avec j < k sont vides (k cellules), la cellule q est occupée (1 cellule), les autres sont soit occupées, soit vides. Cela est illustré sur la figure 5A, où une cellule libre est représentée en blanc, une cellule occupée en noir et une cellule qui se trouve dans un état indéterminé en gris. On a donc k+2 cellules dans un état fixe et N-k-2 cellules dans un état indéterminé ; il y a 2N k'2 telles grilles. 2. k = i (cf. figure 5B) : la cellule k = i est occupée (1 cellule), les cellules q avec j < i sont vides (i cellules), les autres sont soit occupées, soit vides. Ce qui fait 1+i cellules dont l'état est fixé et N-i-1 cellules dont l'état est indéterminé. Il existe en tout 2Nh"1 telles grilles. 3. k > 1 : cette configuration est impossible, car si k était strictement supérieur à i, le plus proche obstacle serait vu en Xjet non en xr. L’équation (7) dans le cas βι=θ| s’écrit donc :
(8)
On peut répéter le même raisonnement pour le cas eî=V|. Dans ce cas aussi on peut distinguer trois possibilités : 1. k<i - situation représentée sur la figure 5C : il y a 2N k ï cellules qui vérifient cette condition ; 2. k=i : situation impossible ; 3. k>i - situation représentée sur la figure 5D il y a 2N'k"2 cellules qui vérifient cette condition. L’équation (7) dans le cas βι=ν, s’écrit donc :
(9).
En injectant les expressions (8) et (9) dans (4), on parvient ainsi à construire le modèle inverse de capteur monodimensionnel sur la grille d’occupation à partir de son modèle direct avec une complexité linéaire par rapport à la taille N de la grille :
(10)
De telles simplifications sont également possibles en plus grande dimension, en utilisant notamment des coordonnées polaires ou sphériques.
La construction du modèle inverse dépend fortement de la définition de la grille. Il est donc intéressant d’étudier quel est l’impact d’une variation de résolution spatiale sur le modèle inverse. Les figures 6A - 6D montrent les modèles inverses d’un même capteur sur quatre grilles de résolution spatiales différentes : 1 m (6A), 50 cm (6B), 25 cm (6C), 12,5 cm (6D). On peut remarquer que quand la résolution augmente (le pas de la grille diminue), le maximum du modèle inverse diminue et tend vers 0,5. En effet, on ne doit pas s’attendre de pouvoir connaître la position d’un obstacle avec une précision supérieure à celle du capteur. A contrario, si on se contente de connaître l’occupation avec une précision bien inférieure à celle du capteur, on pourra déterminer avec une grande certitude la présence ou l’absence d’un obstacle (cas de la figure 6A, où le maximum du modèle inverse vaut 0,999994). Ces considérations permettent d’optimiser la résolution spatiale de la grille : on peut en effet réaliser une exploration permettant de déterminer la résolution maximale de la grille pour laquelle le maximum du modèle inverse reste supérieur à un seuil (strictement supérieur à 0,5 et strictement inférieur à 1) considéré « significatif ».
Il est intéressant de noter que le lien existant entre la précision du capteur et la résolution de la grille n'est apparent que si on calcule le modèle inverse à partir du modèle direct (équations 7-10). Ce lien est perdu si on se contente d’une expression analytique approchée du modèle inverse, comme
dans l’art antérieur ; cela constitue un avantage supplémentaire de l’approche proposée par l’invention. A partir des modèles inverses de deux capteurs sur une même grille d’occupation, la fusion des données des deux capteurs s’effectue à l’aide de l’équation suivante :
(11) où Zi et Z2 sont les mesures fournies par les deux capteurs (la généralisation à plus de deux capteurs est immédiate - il suffit de considérer ^ z%) comme le modèle inverse d’un capteur « virtuel » et le fusionner avec la mesure fournie par un troisième capteur, et ainsi de suite. L’équation (11) n’est valable que si P(Oj)= P(Vj)=1/2, mais sa généralisation à d’autres hypothèses est triviale.
Ce calcul, en virgule flottante, doit être effectué pour chaque cellule de la grille et à une fréquence au moins aussi rapide que la fréquence d'acquisition des capteurs - ce qui nécessite une puissance de calcul importante.
Un deuxième apport de l’invention consiste en un procédé pour la fusion bayésienne des données issues de multiples capteurs dans une grille d'occupation sans calcul en virgule flottante, ce qui permet de réduire considérablement la complexité des calculs nécessaires à la construction de grilles d'occupation et ainsi d'adresser des domaines applicatifs plus variés, notamment ceux pour lesquels la capacité de calcul embarqué est très limitée.
Cet aspect de l’invention repose sur la représentation de la probabilité sur l’intervalle [0 ; 1] de manière discrétisée, par l’intermédiaire de classes de probabilités identifiées par des indices entiers.
Dans ce qui suit, on appellera « système de classes de probabilités » S = (pn, nel} un sous-ensemble dénombrable de [0; 1], dont les éléments pn peuvent donc être caractérisés par un indice entier relatif « n ». Si on appelle « F » la fonction de fusion des données exprimée par l’équation (11) ci-dessus, on peut écrire :
(12).
Un cas particulièrement intéressant est celui d’un système de classes tel que le résultat de la fusion de deux classes de probabilités du système appartient également au système ; formellement : V Pi.PjeS, F(pi, p2)eS. On parle alors d’un système de classes « sans erreur », car la fusion n’introduit pas d’erreur ou d’approximation. Il est donc possible de repérer les valeurs de probabilité par les indices des classes correspondantes, et le résultat d’une fusion est également identifié par un indice. Le problème de la fusion revient alors à déterminer une fonction appropriée Fd qui, à deux indices entiers, associe un autre indice entier. Formellement : v(M) e Ez,3 i e 1 ; F{pk,p{) = p(· et on note Fd(k, l) = i.
Le calcul de Fd(k,l) ne requiert que la connaissance des indices k et I et de l’arithmétique d’indice entier ; aucun calcul en virgule flottante n’est nécessaire pour le calcul de la fusion des informations pk et p/. De plus, si le système de classes est considéré, l’indice obtenu à l’aide de Fd(k,l) désigne une valeur de probabilité strictement identique à celle obtenue - en utilisant des nombres en virgule flottante - en appliquant l’équation (11). La méthode permet ainsi la fusion des classes de probabilité sans erreur par rapport à un calcul flottant.
Il est possible de généraliser cette approche en considérant le cas où les système S est construit comme étant la réunion de plusieurs sous-systèmes de classes qui, individuellement, sont sans erreur. Dans ce cas il est possible que le système S considéré dans sa globalité ne soit pas sans erreur. Il est donc nécessaire d’introduire une étape d’approximation dans la définition de la fonction de fusion Fd : V(M) e 12,3 i e Z : F(pk,pt) = v et Pi <p< pi+1
On peut ensuite arrondir par défaut, par excès ou au plus proche, et de chosir donc Fd(k,l)=i ou Fd(k,l)=i+1 suivant le modèle d’approximation. L’erreur reste toutefois bornée par la largeur du support du système de classes.
Un exemple trivial de système sans erreur est S={1/2, 1}. Tout système de classes comprenant des probabilités différentes de 1/2, 1 et 0 comprend nécessairement une infinité d’éléments. En pratique, pour des raisons évidentes de mise en oeuvre, on considérera uniquement des systèmes de classes de probabilités de cardinalité finie. Toutefois, étant donné que les capteurs sont en nombre fini et que leurs sorties (quantifiées et numérisées) ne peuvent prendre qu’un nombre fini de valeurs, on démontre qu’il est possible de réaliser des fusions « sans erreurs » même à partir de systèmes de classes de probabilités de cardinalité finie.
Un premier exemple d’intérêt applicatif concerne un système de classes de probabilités formé par la réunion des deux sous-systèmes de classes sans erreur suivants : (13) (14) où k est un entier positif (keM*) ainsi que le système Sk = SkJSk , qui n’est pas sans erreur. La figure 7A illustre ces systèmes de classes de probabilités dans les cas k«1, k=2 et k=4. On peut observer que les systèmes Sk quantifient l’intervalle [0, 1] de manière non uniforme, avec un pas de quantification grand près de la valeur 0,5 et de plus en plus petit lorsqu’on s’approche des valeurs 0 et 1. Cela est doublement avantageux, car : une probabilité de 0,5 indique une occupation incertaine ; il n’est donc pas utile d’être très précis autour de cette valeur ; par contre, la précision est utile à proximité des valeurs extrêmes 0 et 1 ; au-delà d’une certaine valeur de |n|, les différentes valeurs des classes de probabilité pn deviennent extrêmement proches entre elles ; l’erreur introduite en tronquant les systèmes de classes est donc négligeable.
Il est préférable de choisir k de faible valeur (par exemple non supérieur à 5) pour éviter que seules les probabilités très proches de 0 ou de 1
soient échantillonnées finement. En fait, plus te capteur est précis, plus la valeur de k pourra être élevée.
Le modèle inverse de la figure 4 était discrétisé spatialement, mais la probabilité d’occupation de chaque cellule pouvait prendre n'importe quelle valeur comprise dans l’intervalle [0,1]. A présent, les valeurs de probabilité sont elles-mêmes discrétisées, ou quantifiées, pour appartenir au système de classes S|< (plus généralement, à un système de classes S). Ainsi, conformément à l’invention, il est nécessaire d’approcher les valeurs de probabilité du modèle inverse de la figure 4 par des éléments d’un système de classes S.
Une première possibilité consiste à remplacer les valeurs du modèle inverse - représenté par la courbe Ml sur la figure 8 - par les éléments de S les plus proches, de manière à minimiser l’erreur de quantification. Le résultat, dans le cas où le système de classes de probabilité est Si (Sk avec k=1 ) est représenté par la courbe MQP sur cette même figure 8. On voit que cette approche peut conduire à sous-estimer la probabilité d'occupation d’une cellule, ce qui peut ne pas être acceptable dans une application de détection d’obstacles. Une alternative consiste à approcher les valeurs du modèle inverse théorique par le plus petit majorant du système de classes S (courbe MQE sur la figure 8, toujours dans le cas du système Sk). Ainsi la probabilité d'occupation n'est jamais sous-estimée, ce qui peut être un avantage pour la détection d'obstacles. Dans d’autres applications, comme le comptage de personne, ce type d'approximation peut en revanche conduire à la génération de faux-positifs.
On remarque que, quel que soit le type d’approximation considéré, le système inverse discrétisé spatialement (courbe Ml sur la figure 8) prend un nombre de valeurs qui est très faible (7 dans l’exemple de la figure 8, tout au plus 18 si on prenait la résolution maximale de la grille, fonction de l’erreur du capteur). Par conséquent, le nombre d’éléments du système S nécessaires pour l’approcher le modèle inverse est également très faible. On peut donc se limiter à considérer un sous-ensemble fini, et de petite taille, du système de classes Sk dont la cardinalité est, en principe, infinie et dénombrable.
Il a été montré plus haut, en référence aux figures 6A - 6D, que - pour un capteur donné - plus la grille d’occupation spatiale est résolue, plus la valeur maximale du modèle inverse est proche de 0,5. Or, si on se limite à des valeurs de probabilité appartenant à un système de classes S, il existe une classe de probabilité pmin qui correspond à la plus petite valeur supérieure à 0,5. Dans le cas de Si, on a pmin=p1=2/3 ; plus généralement, pour un système de classes Sk, Pmin=Pi=(k+1)/(k+2). La résolution optimale de la grille d’occupation est donc celle pour laquelle la valeur maximale du modèle inverse est égale à Pmin. Toute diminution ultérieure du pas de la grille augmente la charge de calcul sans apporter aucun gain en termes d’information.
Comme cela a été expliqué plus haut, l’intérêt d’utiliser des systèmes de classes de probabilité du type de Sk est que la fusion des données ne nécessite que des calculs sur les indices « n », et donc sur des entiers.
On considère d’abord le cas de S£ Soient i, j < 0e Z. En remplaçant (13) dans (12) et en effectuant des simples calculs on trouve : F(Pî, Pj) = Pi+j-ki] (15).
On considère d’abord le cas de S*. Soient i, j > 0e I En remplaçant (14) dans (12) et en effectuant des simples calculs on trouve : F(Pi, Pj) = Pi+j+y-j (16).
Si i=0, la probabilité p, est égale à 0,5 et alors F(Pi, Pj) = Pj : un capteur pour lequel la probabilité d’occupation est 0,5 n’apporte aucune information.
Reste le cas où l’une des probabilités à fusionner est inférieure à 0,5 - et relève donc de la classe S^ - et l’autre est supérieure à 0,5 - et relève donc de la classe S£. Dans ce cas on a : • F{Po Pj) = Pn,n = ~~ si i<0, ]>0 et |i|>|j| ; (17) • F(Pi,Pj) = Pn.n = —- si i<0, j>0 et |i|<|j|. (18)
Le calcul des fractions entières ne donne pas, en général, un résultat entier. On peut cependant mettre en œuvre une opération de division entière « 4- », fournissant un résultat entier - l’indice d’une classe de probabilité dons la valeur est proche du résultat réel. L’équation ci-dessus devient alors : • F{Pi>Pj) = P(i+i)4(i+fc-j) si i<0. pû et |i|>|j| ; (19) • P{vuPj) = Pa+j)H 1-fe-i) si i<0> i>0 et mi (2°) L’erreur introduit par l’application des équations (19) et (20) est borné par la distance maximale entre deux classes successives. Les deux classes de distance maximale sont po et pi, par conséquent l'erreur maximale est donnée par E(k)=prpo=k/(2k+4) (21)
On remarque que la situation considérée ici signifie que les deux capteurs donnent des informations contradictoires ; dans certains cas, on préférera ne pas appliquer les équations (19), (20), mais une règle de gestion des conflits, pouvant être connue de l’art antérieur. Par exemple, notamment lorsqu’il s’agit d’éviter des collisions potentiellement dangereuses, il peut être préférable de prendre l’estimation de probabilité d’occupation la plus élevée ; dans ce cas on a donc simplement : F (.Pu Pj) = max(PuPj) (22)
Un autre exemple de système de classes présentant un intérêt applicatif peut être défini par récurrence.
Soit p une probabilité d’occupation strictement comprise entre 0,5 et 1 : 0,5<p<1. On définit alors par récurrence la suite pn de la façon suivante : p0=0,5 ;
Pi=P ; p2=F(p,p); P3=F(p2,p);
Pn+i=F(pn,p) (23)
On étend ensuite la définition de pn aux valeurs entières négatives de n de la façon suivante :
Po=0,5 ; P-1=1-P ; P-2=F(p-i, p-i); P-3=F(p-2, p.i);
Pn-1=F(Pn, P-l) (24)
On définit ensuite les deux systèmes de classes suivants, de paramètre pe ]0,5, 1[ :
Gp = ((Pn),n>0j (25)
Gi = ((p»),n< 0} (26) où pn est défini par (22) ou (23), selon que n soit positif ou négatif. Par constructions, les classes G~ et Gp sont sans erreur. En effet, si on note fp la fonction à une variable qui, à une probabilité x, associe fp(x)=F(x,p), on voit immédiatement que VI e 1*, pt = fj(p), où l’exposant « i » signifie « composé i fois avec lui-même ». Par conséquent : F(pi,pj) = F {fj(p),fp (p)) = fpl+J(p) = Pî+j (27)
On en déduit que Gp est sans erreur. En remarquant que Vx,y e [0,1] on a F(1-x, 1—y)=1-F(x,y), on peut appliquer le même raisonnement à G~, qui est donc aussi sans erreur.
En posant Gp = GPJ Gp, on obtient un nouveau système de classes pouvant être utilisé dans une méthode de fusion de données selon l'invention. Ce qui est remarquable est que - contrairement au système Sk défini plus haut, Gp est sans erreur sur tout son ensemble de définition. L’équation (27) permet de trouver la formule de fusion des classes dans Gp :
Fd(U) = i + jVi.j el (28)
De plus, le paramètre p permet de contrôler finement l’erreur introduite par la quantification ; en effet si on pose p=1/2 + ε on a : Ε(ρ)=ρι-ρ0=ε (29)
Le système Gp est très intéressant car il permet de réaliser la fusion entière de la manière la plus simple possible (une addition entière) et sans erreur, et de maîtriser l’erreur introduite par la quantification par le choix du paramètre p.
La figure 7B illustre le système Gp pour trois valeurs du paramètre p : 0,52 ; 0,55 et 0,7.
La figure 9A illustre l’application de l’invention à un véhicule terrestre ¥ équipé d’un capteur de distance C, par exemple un télémètre laser à balayage mécanique (LIDAR). Ce capteur est configuré pour effectuer une pluralité de balayages unidimensionnels de l’espace à l’avant du véhicule, chaque balayage définissant une « nappe » d’acquisition. De préférence, on réalise une pluralité de nappes N1, N2... à des hauteurs différentes. Lors de chaque balayage, le capteur produit un vecteur de mesures z, dont chacune est indicative de la présence d’un obstacle - piéton, autre véhicule, arbre sur le bord de la route ... - dans une direction respective et de sa distance (par exemple, lorsque z prend sa valeur maximale, cela signifie qu’il n’y a pas d’obstacle détecté dans la limite de portée du capteur). En variante, une pluralité de capteurs co-Iocalisés (c'est-à-dire ayant le même point d’origine pour la mesure de la distance) peut permettre de réaliser de manière simultanée une pluralité de nappes d’acquisition.
La figure 9B illustre un système de perception d’obstacles adapté à cette application. Ce système comprend ledit capteur C (ou un ensemble de capteurs colocalisés) et un module de traitement de données MTD1 recevant en entrée les vecteurs de mesures correspondant à chaque nappe d’acquisition du capteur et fournissant à sa sortie un signal (typiquement un vecteur d’entiers) représentatif d’une grille d’occupation obtenue par fusion des données de ces nappes d’acquisition.
Dans le mode de réalisation de la figure 9B, le module de traitement de données MTD1 comprend une pluralité de blocs matériels de calcul de probabilités d’occupation, COi...COn et un bloc matériel de calcul F dit de consolidation ou de fusion. Chaque bloc de calcul de probabilités d’occupation COk comprend une mémoire stockant, sous la forme d’une table de correspondance, un modèle inverse de la nappe d’indice k du capteur C, discrétisé au moyen d’un système de classes de probabilité, par exemple Si. On parle ici de « modèle inverse de la nappe » car ce sont les mesures des différentes nappes qu’on fusionne. Si un seul capteur est utilisé pour acquérir plusieurs nappes, ce capteur unique est en fait équivalent à une pluralité de capteurs acquérant une nappe chacun, et ayant chacun son propre modèle inverse (même si tous ces modèles inverses peuvent être identiques).
Chaque bloc de traitement COk reçoit donc en entrée les mesures correspondant à une nappe d’acquisition zk (références zi...zn) respective, et fournit en sortie une grille d’occupation, sous la forme un vecteur d’entiers gk représentant les indices des classes de probabilités associées aux différentes cellules de la grille. Ainsi, la grille gk renferme les informations d’occupation estimées à l’aide des mesures de la nappe k uniquement, c'est-à-dire du vecteur de mesures zk.
Le bloc matériel de consolidation F comprend quatre circuits logiques combinatoires Fi, F2, F3 et F4 mettant en œuvre les équations (15), (16), (19) et (20) respectivement ; il reçoit à son entrée les grilles d’occupation gi...gn et fournit à sa sortie une grille d’occupation « consolidée », représentée à son tour par un vecteur d’entiers, indices des classes de probabilités associées aux différentes cellules de cette grille consolidée.
Comme cela a été expliqué plus haut, les équations (19) et (20) peuvent ne pas être mises en œuvre ; ainsi les blocs F3 et F4 peuvent être absent, ou être remplacés par des circuits de gestion des conflits de détection, par exemple, mettant en œuvre la règle exprimée par l’équation 21 (choix de la probabilité maximale).
Si les modèles inverses associés aux différentes nappes d'acquisition sont identiques, les blocs COt ... COn sont aussi identiques, et peuvent être remplacés par un bloc matériel de calcul de probabilités d’occupation unique, effectuant les traitements de manière séquentielle.
Le module de traitement de données MTD1 peut aussi être associé à tout autre type de capteurs de distance.
Les figures 10A et 10B se rapportent à un autre mode de réalisation de l’invention, utilisant plusieurs capteurs disposés à des emplacements différents qui coopèrent pour fournir une grille d'occupation construite à l'aide de mesures effectuées depuis différents points de vue. Les capteurs peuvent être hétérogènes technologiquement, en précision, portée, champ de vision et/ou rapidité d'acquisition. Dans ce mode de réalisation la distance du premier obstacle est une information relative au capteur qui fait la mesure. Un exemple schématique du scénario est représenté sur la figure 10A, montrant deux capteurs Ci et C2 placés à différentes positions et ayant des portées et des champs de vision différents. Ainsi l’obstacle O est vu à des distances complètement différentes par Ci et C2.
Dans ce mode de réalisation, la principale difficulté réside dans le fait que la grille d'occupation d’une part et les capteurs d'autres parts ont chacun un repère propre qui leur est associé. Ainsi, l'évaluation de l'emplacement des obstacles requiert d'effectuer des changements de repères.
La figure 10B illustre un système de perception d’obstacles selon un tel mode de réalisation de l’invention. Ce système comprend, de manière générale, « n » capteurs non co-localisés et potentiellement hétérogènes Ci, C2 ... Cn et un module de traitement de données MTD2. Ce dernier se différencie du module de traitement de données MTD1 de la figure 9B en ce qu’il comprend aussi, intercalés entre les blocs matériels de calcul de probabilités d’occupation CO1 ... COn et le bloc matériel de consolidation F, de blocs de changement de repère Ri ... Rn. Chacun de ces blocs (¾ contient des unités de calcul, généralement en virgule flottante, pour effectuer le changement du repère d’un capteur respectif vers le repère de la grille d’occupation dite « de consolidation » par rapport à laquelle est effectuée la fusion des données. Le calcul effectué pour le changement de repère consiste à réaffecter l'occupation d'un emplacement connu dans le repère d'un capteur Cr (exprimé par un vecteur d’entiers gk) à la cellule correspondante dans le repère de la grille de consolidation. On représente par gx ... gn les vecteurs d’entiers représentatifs des occupations des cellules de la grille de consolidation. Cette réaffectation suppose le calcul de translations, rotations, etc. Le traitement des blocs Rk peut par exemple être réalisé en utilisant une unité arithmétique flottante d'un processeur embarqué (FPU : Floating Point Unit). Dans ce cas un même bloc matériel peut effectuer le calcul pour l'ensemble des blocs Rk (traitement séquentiel).
En variante, les équations de changement de répère peuvent être mémorisées dans des tables de conversion stockées dans des mémoires contenues dans les modules Rk. Ainsi, même dans ce cas, on peut s’affranchir du calcul en virgule flottante et n’effectuer que des opérations sur des entiers. Par contre, ces tables de conversion peuvent être assez volumineuses et leur stockage avoir un coût non négligeable en termes de surface de silicium.
La figure 11 illustre un troisième mode de réalisation de l’invention dans lequel un capteur unique C, non représenté, acquiert des mesures scalaires ou vectorielles zt, zt+1, ...zt+m··· à une cadence d’acquisition N fois plus élevée de ce qui est requis pour une application déterminée. Un bloc matériel de calcul de probabilités d’occupation CO produit une grille d’occupation gt, gt+i, ...gt+m... pour chacune de ces mesures. Puis un bloc matériel de fusion F fusionne N de ces grilles, acquises à des instants successifs, en une seule grille consolidée gfUS ; les grilles d’occupation consolidées gfUS sont donc générées à une cadence N fois moins élevée que la cadence d’application du capteur. Par exemple, si le capteur C fonctionne à une cadence de 100 Hz et une cadence de 10 Hz suffit pour l’application visée, on peut fusionner 10 acquisitions successives.
Dans les modes de réalisation des figures 9B, 10B et 11 on a considéré le cas où les traitements - ou au moins certains d’entre eux - sont réalisés par des blocs de calcul matériels, c'est-à-dire des circuits numériques dédiés. L’invention se prête cependant aussi à une mise en œuvre totalement ou partiellement logicielle, dans laquelle les traitements - ou au moins certains d’entre eux - sont réalisés par un processeur générique programmé de manière opportune.
The invention relates to a method and a system for perceiving and estimating the position of material bodies performing, efficiently in terms of computing power and energy consumption, a multi-sensor fusion. .
"Material body" means any substance or material object having an individuality and which can be detected and identified by an appropriate sensor. Thus, material bodies are considered to be inanimate objects, whether natural or artificial, plants, animals, human beings, but also liquid or solid particles suspended in the air, such as clouds or even masses. liquid or gaseous. The invention applies in particular to the field of navigation of robots, drones, autonomous vehicles, etc. and more generally to that of perception.
With the explosion of computing resources that can be integrated in a robot, the applications of robotics have multiplied in recent years, from industrial production to home automation, from space exploration and underwater to toy drones for the general public. The tasks performed in robotic applications have become progressively more complex, implying more and more often for robots to be able to evolve in unknown environments; this has made the development of means and techniques of perception, that is to say, allowing the discovery and interpretation of the surrounding space, more and more important. An important application that uses perception in robotics is navigation, which consists of setting a target destination to a robot, and allowing it to go there, taking care to avoid unknown and potentially mobile obstacles; the robot then has the task of planning its own trajectory. A typical example, the subject of intense research, is the autonomous car.
To allow knowledge of the entire environment by limiting the blind spots as much as possible, and to overcome a possible defect in a sensor, the integration of multiple sensors is generally used. When several sensors, possibly of different types, cover the same space, it is necessary to be able to combine the information extracted from each of them: this is called multi-sensor fusion.
There are two main families of perception techniques: geometric methods, which aim to identify the geometry of objects in the surrounding space, and those based on the occupancy grid, which aim to determine if a certain location is occupied by a obstacle (more generally, by a material body). The invention relates to occupation grid techniques.
The theoretical underpinnings of multi-sensor perception and fusion methods based on occupancy grids are described in A. Elfes' article "Occupancy grids: a stochastic spatial representation for active robot perception" (Sixth Conference on Uncertainty in Al, 1990). This publication does not focus on the practical implementation of methods, a direct application of which would require complex floating point calculations. K. Konolige's article "Improved occupancy grids for map building" (Autonomous Robots, 4, 351-367, 1997) and that of J. Adarve et al. "Computing occupancy grids from multiple sensors using linear opinion pools," (Proceedings - IEEE International Conference on Robotics and Automation, 2012) describe improvements in occupancy grid techniques. Again, the implementation of these techniques requires a massive recourse to floating point calculation.
Documents US 2014/035775, FR 2006/050860 and DE 102009007395 describe multi-sensor perception and fusion methods and systems based on occupancy gates, applied to the autonomous driving of land vehicles. All these methods require, for their implementation, floating-point calculations.
However, the use of floating point calculation requires significant resources in terms of computing power, which are difficult to match the constraints of embedded systems. As a reminder, the floating point format - defined by the IEEE 754 standard - represents a number by means of three elements: a sign (1 bit), a mantissa (23 or 52 bits) and an exponent (8 or 11 bits). Performing calculations using floating point numbers is much more complex (that is, it requires much more elementary operations) than doing integer calculations. This therefore requires the use of a faster processor and / or a dedicated hardware accelerator, with an adverse impact in terms of cost, size and power consumption.
As an example, consider an environment of 20 mx 50 m discretized in 100,000 cells of 10 cm × 10 cm, and two sensors operating at 25 Hz. If the data fusion must be done, according to the prior art Using the theory of A. Elfes using floating-point computations, the computing power required is of the order of 5 - 50 GFIops (billions of floating operations per second). If we consider a grid of 150,000 cells with 8 sensors operating at 50 Hz - assumptions that seem more realistic for applications in the automotive field - we get a computing power requirement of 60 - 600 GFIops. The aim of the invention is to provide a multi-sensor fusion method for material bodies requiring less computation resources and, therefore, better suited to embedded solutions. In the following we will specifically consider the application to the perception of obstacles, but the invention is not limited to this case. Other possible applications concern the detection of clouds or precipitation, or even concentrations of pollutants, or the distribution and movement of people in a public space. In all these cases, it is possible to detect material bodies using sensors, to measure the distance and to use the measurements to calculate a grid of occupation.
An object of the invention making it possible to achieve this goal is a method of perception of material bodies comprising the following steps, implemented by a computer or a dedicated digital electronic circuit: a) Acquisition of a plurality of distance measurements of said hardware bodies from one or more sensors; b) Applying, at each said distance measurement, an inverse model of the corresponding sensor on a occupancy grid providing a discrete spatial representation of an environment of said sensor, to determine a probability of occupation by a material body of a set of cells of said occupation grid; and c) constructing a consolidated occupancy grid in which each cell has a probability of occupancy calculated by merging the probabilities of occupation estimated during step b); characterized in that each said inverse sensor model is a discrete model, associating with each cell of the corresponding occupation grid, and for each distance measurement, a probability class chosen within the same set of cardinality finite, each said probability class being identified by an integer index; and in that, in said step c), the occupancy probability of each cell of the consolidated occupancy grid is determined by means of integer calculations performed on the indices of the classes of probabilities determined during said step b).
According to different embodiments of such a method:
Said set of finite cardinality of probability classes can be formed by the union of one or more subsets such that, in said step c), the merger of two probability classes belonging to the same subset provides a result also belonging to said subset.
Said set of finite cardinality of probability classes may constitute a non-uniform discretization of the probability interval [0,1], with a step of discretization increasing between 0 and 0,5, then decreasing between 0,5 and 1.
Said finite cardinality set of probability classes, designated Sk, can be formed by the union of two subsets Sfe and S £ defined by:
the index n taking relative integer values and the parameter k being a non-zero relative integer and, in said step c), the melting F (pi, pj) between two probability classes p ,, pj e Sk can be calculated by applying the following equations when the indices "i" and "j" have the same sign: • F (pj, pj) = pi + jk.ij if i, j <0; • F (pi, pj) = pj + j + k ij if i, j> 0.
More particularly, during said step c), the fusion F (pi, Pi) between two classes of probabilities p ,. Pj e Sk can be calculated by applying the following equations when the indices "i" and "j" have opposite signs: • F (pi, Pj) = P (i + j) + (i + kj) if i <0, j> 0 and | i |> | j | ; • F (pi, Pj) = P (i + jHi-kj) if i <0, j> 0 and | i | <| J |. where the symbol -i- denotes an entire division.
In particular, we can have k = 1.
Said step c) may comprise implementing mark changes to construct said consolidated occupancy grid from occupation probabilities of occupation grid cells associated with non-co-located sensors.
The method may also comprise a preliminary step of constructing the inverse models of at least one said sensor on the corresponding occupation grid, implemented by applying the following equation:
where: • P (Oj | z) represents the occupation probability of the cell of index "i" of the occupation grid, said cells being ordered by increasing distance from said sensor; • N is the number of cells of said occupation grid; Z represents a measurement of distance from the sensor; and • p (z | xk) represents the direct model of the sensor, expressing the probability density of the measurement "z" provided by the sensor when the position Xk of the cell k of the occupancy grid, but no other cell more near the sensor, is occupied by a material body.
The method may also comprise a preliminary step of determining a spatial resolution of at least one said occupancy gate, said spatial resolution being chosen the smallest compatible with the constraint max [ (z | xk)]> pmjn, where p (z | xk) is a direct model of the sensor, expressing the probability density of the "z" measurement provided by the sensor when the xk cell of the occupancy grid, but no other cell closer to the sensor, is occupied by a material body, and pmin is the smallest probability class belonging to said set and greater than 0.5.
Another object of the invention is a material body perception system comprising: at least one input port for receiving a plurality of signals representative of distance measurements of said material bodies from one or more sensors; a data processing module configured to receive said signals input and use them to construct a consolidated occupancy grid by applying a method as defined above; and at least one output port for a signal representative of said consolidated occupancy grid.
According to particular embodiments:
The system may also include one or more distance sensors adapted to generate signals representative of a plurality of distance measurements of said hardware bodies and connected to said one or more input ports.
Said data processing module may comprise at least one hardware block for calculating occupancy probabilities comprising a memory storing, in the form of a correspondence table, an inverse model of a sensor associating with each distance measurement a vector integers representing probability class indices associated with respective cells of a busy grid.
Said data processing module may comprise an integral computation hardware block, configured to receive as input a plurality of integers representing probability class indices associated with respective occupancy grid cells, and to compute an index of a probability class associated with a cell of said consolidated occupancy grid.
More particularly, said data processing module may also comprise at least one calculation block, called a change of reference block, configured to: receive as input at least one first integer vector, derived from a hardware block for calculating probabilities corresponding occupation and representing probability class indices associated with respective cells of a busy grid; converting it into a second integer vector representing probability class indices associated with respective cells of another occupancy grid, spatially coinciding with said consolidated occupancy grid; and providing said second integer vector as input to said hardware consolidation block.
Said data processing module may be configured to receive, as input, signals representative of distance measurements successively acquired by the same sensor or co-located sensors and to construct a consolidated occupancy grid from a plurality of these signals. , corresponding to successive acquisition moments. Other characteristics, details and advantages of the invention will emerge on reading the description given with reference to the accompanying drawings given by way of example and which respectively illustrate:
Figure 1, the concept of "direct" model of a distance sensor;
Figure 2, the concept of occupation grid;
Figure 3, the notion of "inverse" model of a distance sensor;
Figure 4, the spatial discretization of an inverse model on a grid of occupation;
FIGS. 5A to 5D, a method for calculating the inverse model of a sensor on a occupancy grid;
FIGS. 6A to 6D, a method of choosing the optimal spatial resolution of a busy grid;
Figures 7A and 7B various probability class systems;
Figure 8, two methods of quantization of a model inverse of a sensor on a grid of occupation;
FIGS. 9A and 9B, an obstacle perception system according to a first embodiment of the invention;
FIGS. 10A and 10B, an obstacle perception system according to a second embodiment of the invention; and
Figure 11, an obstacle perception system according to a third embodiment of the invention.
In the detailed description that follows, reference will be made to the case of perception of obstacles. However, all that is described applies more generally to the perception of all kinds of material bodies.
In most cases, sensors used for navigation provide information on the distance of surrounding obstacles; we are talking about distance sensors. To account for the accuracy of a sensor, its possible error or its resolution, we introduce a probabilistic model. The idea is that a measurement at the output of the sensor does not necessarily indicate the exact distance between the obstacle and the sensor, and that it is therefore necessary to reason about the probability that the obstacle is at a given distance knowing the sensor response.
If we note the real distance between an obstacle and the sensor, and z the sensor output, we are interested in the conditional probability density function p (z | d) which models the link between the actual position of a obstacle and its estimation seen by the sensor ("direct model"). Figure 1 shows an example of a direct model of a sensor; we consider a linear space of 50 m long and we assume that an obstacle is at d = 25m from the sensor. For a sensor with an error modelable by a Gaussian function, the most likely z response will be close to 25 m, but other values will be possible, with a probability density defined by the curve. In the case of an ideal sensor we would have p (z | d) = 5 (zd), where δ is a Dirac Delta, and the measurement would always be equal to the true distance. The direct model of a sensor can be determined experimentally; typically it can be constructed from data provided by the constructor (in the Gaussian case, the value of the standard deviation suffices to characterize the model).
In the following, we note Ω a spatial reference to one, two or three dimensions; an occupation grid GO is a partition of a continuous and bounded subset of Ω into a number N of parts, called cells and denoted by an index ie [0, N-1]. We denote by c the cell of index i. Without loss of generality, we will consider in the following a one-dimensional occupation grid observed by a single distance sensor C (or a plurality of co-located sensors), the index i being increasing with the distance of the sensor (co being therefore the cell closest to the sensor and cN-i the farthest). This configuration is illustrated in Figure 2.
An obstacle A is a bounded continuous subset of Ω. We say that a cell c, is occupied by an obstacle A if Anc ^ 0, that it is not occupied by A if Ancj = 0. In other words, if the obstacle covers even partially the cell, it is considered that it is occupied. Other conventions are possible, but in any case a cell must be either free or busy.
For each of the cells of the grid, consider the binary random experiment "state" which may have one of the two issues occupied; empty} of knowing if the cell contains an obstacle or not. Note e, the state of the cell q, o, · the realization ej = occupied and Vj the realization ei = empty. In a grid, we consider that all the cells are independent, so that vi, I [0, N-1], P (Oi A Oj) = P (Oj) -P (Oj) (1) where λ is the The logical operator "and" and P (.) denotes the probability of an event (not to be confused with a probability density, denoted by a lowercase "p").
It is also considered that the position of the obstacles can be known only by means of uncertain distance sensors, characterized by a probabilistic model as described above which can be written more generally p {z x), x being the position of an obstacle (in several dimensions, it is a vector, expressed in Cartesian, spherical, polar coordinates, etc. and not a simple scalar). These sensors can be telemetric lasers (also called lidars), sonars, infrared radars, time-of-flight cameras, etc.
A measurement z from a sensor makes it possible to determine the probability of occupation Ρ (ο, | ζ) of a cell c ,. For a given measure z, the set of probabilities Ρ (ο, | ζ) V ie [0, N-1] constitutes the inverse model of the sensor on the grid. While the sensor's direct model provides information about the sensor's response to the physical world, the inverse model expresses the impact of the measurement on the occupancy grid, which is the model of the physical world that we adopt. justifies the name inverse model.
Figure 3 shows a typical example of the inverse model of a distance sensor, in a case where z = 25m. It can be verified that the probability of occupancy is almost zero for cells that are at a distance of less than 24.25 m from the sensor and reaches a peak for a distance of 25 m (corresponding to the measurement provided by the sensor) . Beyond 25 m, the probability of occupation decreases until it stabilizes at a value of 0.5, indicative of a total lack of knowledge of the state of occupancy of the cells which, being located beyond obstacle, are hidden by the latter and therefore inaccessible to the sensor.
In accordance with the prevailing practice in the literature, Figure 3 represents the inverse model by means of a smoothed curve. A more correct representation would be to display only the points corresponding to the grid cell limits: in fact, we can not distinguish a "partially" occupied cell from one that would be "totally", in all cases the distance to the obstacle will be estimated as the distance to the corresponding cell. This is the spatial error introduced by the grid.
A more accurate version of the inverse model of FIG. 3, taking into account this spatial discretization induced by the grid, is presented in FIG.
It should be emphasized that the concepts of "occupation" and "distance from the obstacle" are not quite equivalent. Indeed, to say that an obstacle is at a sensor distance z does not only mean that a certain cell is occupied, but also that all the other cells of the grid closer to the sensor are free (otherwise, the first obstacle would have been seen at a distance less than z). In the aforementioned FIG. 2, the obstacle A is in the cell of index i (in black); cells with an index less than i are shown in white to indicate that they are free, those with indexes greater than i are gray to indicate that their occupation status is unknown.
If we take into account the notion of uncertain sensor characterized by its model (direct) p (z x) and that one notes d, the distance of the cell q with respect to the point of measurement and x [the point of the cell q closest to said measurement point, we have:
(2) Equation (2) indicates that the model of the sensor evaluated at a point that is at the boundary of a grid cell (xO is equal to the sensor response probability density for a corresponding grid pattern , ie a grid where the cells closest to the cell i are empty, the cell i is occupied, and the states of occupation of the cells farther than the cell i are not determined * This is by exploiting this information A. Elfes, in his article cited above, has proposed a method of constructing the inverse model of the sensor, an explanation of which process is given below.
The Bayes theorem allows to express the inverse model of a P (Oi | z) sensor in the following way:
(3). where P (Oj) and P (Vj) designate the probabilities a priorf (that is to say without knowing the position of the obstacles, or the output of the sensor) that the cell and be occupied or free, respectively. In the following we will make the hypothesis P (oi) =
P (Vj) = 1/2, but a generalization does not pose a fundamental theoretical difficulty.
We then obtain:
(4)
The calculation of the terms ρ (ζΐθί) and p (zjVj) can be done using the Kolmogorov theorem on all possible configurations of the grid. A configuration is formed by a N-tuple {eo, ei ... θν-ι} where and e {o ^ Vj}; a grid of N cells has 2N possible configurations.
For the term ρ (ζ | ο,) the possible grid configurations are of the form G%. = (e0, ... where ... the cell i is occupied, there are 2N * 1 such grids.) For the term ρ (ζ | ν,) the possible grid configurations are of the form Gy. = (e0, ... Vi, ... where cell q is empty, there are 2N'1 such grids.
We can write: (5a)
(5b)
The terms) and) are 1 / 2N'1. By using the equation (2) linking the occupation and the measured distance, it is possible to determine, from a grid configuration G *, the position χγ such that P (z Çk) - p (Mxk), This position is that of the occupied cell of the Gj configuration. closest to the observer. Equations (5a) and (5b) can therefore be rewritten:
(6) whose terms can be calculated directly from the direct model of the sensor. By re-injecting (6) into (4) can therefore calculate, in principle, the inverse model of the sensor on the occupancy grid considered.
The main limitation of this method comes from the exponential explosion of the number of terms in the sum of equation (6), making it virtually impossible to calculate the sum. Indeed, in the prior art, analytical expressions of the inverse model are generally used, renouncing the expression directly as a function of ρ (ζ | ο,). This implies the loss of the link with the direct model, which is the only one directly accessible by the experiment, and therefore the ability to measure the error introduced by the modeling.
A first contribution of the present invention is a simplified method for calculating the inverse-linear model in N instead of being exponential without introducing an approximation with respect to the use of equations (6) and (4). In the following we will consider, with the aim of simplifying the presentation but without loss of generality, the case of a one-dimensional occupation grid, where the position of an obstacle is represented by a scalar variable x and for which ( 6) can be written:
(7)
As we consider a one-dimensional grid with N cells, X | <can only take the position value of one of the cells cj with I [0; N - 1]. Thus, the terms of the sum (7) can only take N different values. By factoring all the grids giving the same value of p (z | xk), we can then reduce the equation (7) to a sum of N terms only * instead of 2N'1. The complexity of the calculation goes from exponential to linear.
We begin by considering the case e, = Oj. We fix q in its occupied state and we fix the distance of the first obstacle as that of the cell Cr, so that this distance is actually xr. We therefore seek the number of grids which are such that the first obstacle is seen at the xr position knowing that the cell is occupied. Three scenarios can be envisaged: 1. k <i: the cell k is occupied (1 cell), the cells q with j <k are empty (k cells), the cell q is occupied (1 cell), the others are either occupied or empty. This is illustrated in Figure 5A, where a free cell is shown in white, a cell occupied in black, and a cell in an indeterminate gray state. We thus have k + 2 cells in a fixed state and Nk-2 cells in an indeterminate state; there are 2N k'2 such grids. 2. k = i (see FIG. 5B): the cell k = i is occupied (1 cell), the cells q with j <i are empty (i cells), the others are either occupied or empty. What makes 1 + i cells whose state is fixed and Ni-1 cells whose state is undetermined. There exist in all 2Nh "1 such grids, 3. k> 1: this configuration is impossible, because if k was strictly greater than i, the nearest obstacle would be seen in Xjet not in xr.Equation (7) in the case βι = θ | is written thus:
(8)
We can repeat the same reasoning for the case ei = V |. In this case too we can distinguish three possibilities: 1. k <i - situation shown in Figure 5C: there are 2N ki cells that verify this condition; 2. k = i: impossible situation; 3. k> i - situation represented on the figure 5D there are 2N'k "2 cells which satisfy this condition The equation (7) in the case βι = ν, is written thus:
(9).
By injecting the expressions (8) and (9) into (4), we thus succeed in constructing the inverse one-dimensional sensor model on the occupation grid from its direct model with a linear complexity with respect to the size N of Grid :
(10)
Such simplifications are also possible in larger dimension, in particular by using polar or spherical coordinates.
The construction of the inverse model strongly depends on the definition of the grid. It is therefore interesting to study the impact of a spatial resolution variation on the inverse model. Figures 6A-6D show the inverse models of the same sensor on four different spatial resolution grids: 1 m (6A), 50 cm (6B), 25 cm (6C), 12.5 cm (6D). We can notice that when the resolution increases (the pitch of the grid decreases), the maximum of the inverse model decreases and tends to 0.5. Indeed, one should not expect to be able to know the position of an obstacle with a precision greater than that of the sensor. On the other hand, if one is satisfied with knowing the occupation with a precision much lower than that of the sensor, one can determine with a great certainty the presence or the absence of an obstacle (case of the figure 6A, where the maximum of the inverse model is 0.999994). These considerations make it possible to optimize the spatial resolution of the grid: it is indeed possible to perform an exploration to determine the maximum resolution of the grid for which the maximum of the inverse model remains greater than a threshold (strictly greater than 0.5 and strictly less than 1) considered "significant".
It is interesting to note that the link between sensor accuracy and grid resolution is only apparent if the inverse model is calculated from the direct model (equations 7-10). This link is lost if one is satisfied with an approximate analytical expression of the inverse model, as
in the prior art; this constitutes an additional advantage of the approach proposed by the invention. From the inverse models of two sensors on the same occupancy grid, the data fusion of the two sensors is carried out using the following equation:
(11) where Zi and Z2 are the measurements provided by the two sensors (the generalization to more than two sensors is immediate - just consider ^ z%) as the inverse model of a "virtual" sensor and merge it with the measurement provided by a third sensor, and so on. Equation (11) is only valid if P (Oj) = P (Vj) = 1/2, but its generalization to other hypotheses is trivial.
This calculation, in floating point, must be carried out for each cell of the grid and at a frequency at least as fast as the frequency of acquisition of the sensors - which requires a significant computing power.
A second contribution of the invention consists of a method for the Bayesian fusion of data from multiple sensors in a occupation grid without floating-point calculation, which considerably reduces the complexity of the calculations required for the construction of grids. occupation and thus address more varied application domains, especially those for which the embedded computing capacity is very limited.
This aspect of the invention is based on the representation of the probability over the interval [0; 1] in a discretized way, through classes of probabilities identified by integer indices.
In what follows, we will call "a system of classes of probabilities" S = (pn, nel) a countable subset of [0; 1], whose pn elements can therefore be characterized by a relative integer index "n". If we call "F" the data fusion function expressed by equation (11) above, we can write:
(12).
A particularly interesting case is that of a class system such that the result of the merger of two classes of probabilities of the system also belongs to the system; formally: V Pi.PjeS, F (pi, p2) eS. We then speak of a system of classes "without error", because the fusion does not introduce an error or an approximation. It is therefore possible to identify the probability values by the indices of the corresponding classes, and the result of a merger is also identified by an index. The problem of the fusion then amounts to determining an appropriate function Fd which, with two integer indices, associates another whole index. Formally: v (M) e Ez, 3 ie 1; F {pk, p {) = p (· and we write Fd (k, l) = i.
The computation of Fd (k, l) requires only the knowledge of indices k and I and integer index arithmetic; no floating-point calculation is necessary for the calculation of the merging of information pk and p /. Moreover, if the class system is considered, the index obtained using Fd (k, l) designates a probability value strictly identical to that obtained - using floating-point numbers - by applying the equation (11). The method thus makes it possible to merge the probability classes without error with respect to a floating calculation.
It is possible to generalize this approach by considering the case where the system S is constructed as the union of several subsystems of classes which, individually, are error free. In this case it is possible that the system S considered in its entirety is not without error. It is therefore necessary to introduce an approximation step in the definition of the fusion function Fd: V (M) e 12.3 ie Z: F (pk, pt) = v and Pi <p <pi + 1
We can then round up by default, by excess or as close as possible, and thus choose Fd (k, l) = i or Fd (k, l) = i + 1 according to the approximation model. However, the error is limited by the width of the class system support.
A trivial example of an error-free system is S = {1/2, 1}. Any system of classes comprising probabilities different from 1/2, 1 and 0 necessarily includes an infinity of elements. In practice, for obvious reasons of implementation, we will consider only finite cardinality probability class systems. However, since the sensors are finite in number and their outputs (quantized and digitized) can only take a finite number of values, it is shown that it is possible to carry out "error-free" mergers even from finite cardinality probability class systems.
A first example of application interest concerns a system of classes of probabilities formed by the union of the following two non-error class subsystems: (13) (14) where k is a positive integer (keM *) and the Sk system = Sk JSk, which is not without error. Figure 7A illustrates these systems of classes of probabilities in the cases k "1, k = 2 and k = 4. It can be seen that Sk systems quantize the interval [0, 1] non-uniformly, with a large quantization step near the value 0.5 and smaller and smaller when approaching the values 0 and 1. This is doubly advantageous because: a probability of 0.5 indicates an uncertain occupation; it is therefore not useful to be very precise around this value; on the other hand, accuracy is useful near extreme values 0 and 1; beyond a certain value of | n |, the different values of the pn probability classes become extremely close to each other; the error introduced by truncating the class systems is therefore negligible.
It is better to choose k of low value (for example not greater than 5) to avoid that only the probabilities very close to 0 or 1
be finely sampled. In fact, the more accurate the sensor, the higher the value of k can be.
The inverse model of Figure 4 was spatially discretized, but the occupancy probability of each cell could be any value in the range [0,1]. At present, the probability values are themselves discretized, or quantified, to belong to the class system S | <(more generally, to a system of classes S). Thus, according to the invention, it is necessary to approach the probability values of the inverse model of FIG. 4 by elements of a class S system.
A first possibility is to replace the values of the inverse model - represented by the curve M1 in FIG. 8 - by the elements of S that are closest, so as to minimize the quantization error. The result, in the case where the system of probability classes is Si (Sk with k = 1) is represented by the MQP curve on this same figure 8. It can be seen that this approach can lead to underestimating the probability of occupation. of a cell, which may not be acceptable in an obstacle detection application. An alternative is to approach the values of the theoretical inverse model by the smallest sum of the S-class system (MQE curve in FIG. 8, again in the case of the Sk system). Thus the probability of occupation is never underestimated, which can be an advantage for the detection of obstacles. In other applications, such as the counting of people, this type of approximation can, on the other hand, lead to the generation of false positives.
Note that whatever the type of approximation considered, the inverse system spatially discretized (curve M1 in FIG. 8) takes a number of values which is very small (7 in the example of FIG. 8, at the most 18 if we took the maximum resolution of the grid, depending on the sensor error). Consequently, the number of elements of the system S necessary to approach the inverse model is also very small. We can therefore limit ourselves to considering a finite subset of the class system Sk whose cardinality is, in principle, infinite and countable.
It has been shown above, with reference to FIGS. 6A-6D, that - for a given sensor - the more the spatial occupation grid is solved, the higher the maximum value of the inverse model is close to 0.5. However, if we limit ourselves to probability values belonging to a system of classes S, there exists a probability class pmin which corresponds to the smallest value greater than 0.5. In the case of Si, we have pmin = p1 = 2/3; more generally, for a system of classes Sk, Pmin = Pi = (k + 1) / (k + 2). The optimal resolution of the occupation grid is therefore that for which the maximum value of the inverse model is equal to Pmin. Any subsequent decrease of the grid step increases the computing load without bringing any gain in terms of information.
As explained above, the advantage of using Sk-type probability class systems is that the data merge only requires computations on the "n" indices, and thus on integers.
We first consider the case of S £ Soient i, j <0. Z. By replacing (13) in (12) and performing simple calculations we find: F (P1, Pj) = Pi + j-ki (15).
We first consider the case of S *. Let i, j> 0e I By replacing (14) in (12) and performing simple calculations we find: F (Pi, Pj) = Pi + j + yj (16).
If i = 0, the probability p, is equal to 0.5 and then F (Pi, Pj) = Pj: a sensor for which the probability of occupation is 0.5 gives no information.
There remains the case where one of the probabilities to merge is less than 0.5 - and thus belongs to the class S ^ - and the other is greater than 0.5 - and thus belongs to the class S £. In this case we have: • F {Po Pj) = Pn, n = ~~ if i <0,]> 0 and | i |> | j | ; (17) • F (Pi, Pj) = Pn.n = - if i <0, j> 0 and | i | <| J |. (18)
The calculation of whole fractions does not, in general, give an integer result. We can, however, implement an integer division operation "4-", providing an integer result - the index of a probability class given the value is close to the actual result. The equation above then becomes: • F {Pi> Pj) = P (i + i) 4 (i + fc-j) if i <0. pû and | i |> | j | ; (19) • P {vuPj) = Pa + j) H1-fe-i) if i <0>i> 0 and mi (2 °) The error introduced by the application of equations (19) and (20) is limited by the maximum distance between two successive classes. The two classes of maximum distance are po and pi, therefore the maximum error is given by E (k) = prpo = k / (2k + 4) (21)
Note that the situation considered here means that the two sensors give contradictory information; in some cases, it would be preferable not to apply equations (19), (20), but a conflict management rule, which can be known from the prior art. For example, especially when it comes to avoiding potentially dangerous collisions, it may be better to take the highest probability of occupancy estimate; in this case we have simply: F (.Pu Pj) = max (PuPj) (22)
Another example of a class system having an application interest can be defined by recurrence.
Let p be a probability of occupation strictly between 0.5 and 1: 0.5 <p <1. The sequence pn is then defined by recurrence as follows: p0 = 0.5;
Pi = P; p2 = F (p, p); P3 = F (p2, p);
Pn + i = F (pn, p) (23)
We then extend the definition of pn to the negative integer values of n as follows:
Po = 0.5; P-1 = 1-P; P-2 = F (pi, pi); P-3 = F (p-2, pi);
Pn-1 = F (Pn, Pl) (24)
We then define the following two class systems, of parameter pe] 0.5, 1 [:
Gp = ((Pn), n> 0j (25)
Gi = ((p), n <0} (26) where pn is defined by (22) or (23), depending on whether n is positive or negative. By constructions, the classes G ~ and Gp are without error. Indeed, if we denote by fp the function with a variable which, at a probability x, associates fp (x) = F (x, p), we see immediately that VI e 1 *, pt = fj (p), where l 'exponent' i 'means' composed once with himself'. Therefore: F (pi, pj) = F {fj (p), fp (p)) = fpl + J (p) = P i + j (27)
We deduce that Gp is without error. Noting that Vx, ye [0,1] has F (1-x, 1-y) = 1-F (x, y), we can apply the same reasoning to G ~, which is also error-free.
By posing Gp = GP J Gp, we obtain a new class system that can be used in a data fusion method according to the invention. What is remarkable is that - unlike the Sk system defined above, Gp is error free over its entire set of definition. Equation (27) finds the merge formula of classes in Gp:
Fd (U) = i + jVi.j el (28)
In addition, the parameter p makes it possible to finely control the error introduced by the quantization; indeed if we put p = 1/2 + ε we have: Ε (ρ) = ρι-ρ0 = ε (29)
The Gp system is very interesting because it makes it possible to perform the entire merger in the simplest possible way (an integer addition) and without error, and to control the error introduced by the quantization by the choice of the parameter p.
Figure 7B illustrates the Gp system for three values of the parameter p: 0.52; 0.55 and 0.7.
FIG. 9A illustrates the application of the invention to a terrestrial vehicle equipped with a distance sensor C, for example a mechanical scanning laser rangefinder (LIDAR). This sensor is configured to perform a plurality of one-dimensional sweeps of the space at the front of the vehicle, each sweep defining an acquisition "sheet". Preferably, a plurality of plies N1, N2 ... are made at different heights. During each scan, the sensor produces a vector of measurements z, each of which is indicative of the presence of an obstacle - pedestrian, other vehicle, tree on the side of the road ... - in a respective direction and its distance (For example, when z takes its maximum value, it means that there is no obstacle detected within the range of the sensor). Alternatively, a plurality of co-Iocalized sensors (i.e., having the same origin point for distance measurement) may make it possible to simultaneously perform a plurality of acquisition plies.
Figure 9B illustrates an obstacle perception system adapted to this application. This system comprises said sensor C (or a set of co-located sensors) and a data processing module MTD1 receiving as input the measurement vectors corresponding to each acquisition sheet of the sensor and supplying at its output a signal (typically a vector of 'integers') representative of an occupancy grid obtained by merging the data of these acquisition plies.
In the embodiment of FIG. 9B, the data processing module MTD1 comprises a plurality of hardware blocks for calculating occupancy probabilities, CO 1 ... COn and a hardware block F for consolidation or consolidation. Each occupancy probability calculation block COk comprises a memory storing, in the form of a correspondence table, an inverse model of the index subscript layer k of the sensor C, discretized by means of a probability class system. , for example Si. We are talking here about "inverse model of the water table" because it is the measurements of the different layers that merge. If only one sensor is used to acquire several layers, this single sensor is in fact equivalent to a plurality of sensors acquiring a sheet each, and each having its own inverse model (even if all these inverse models may be identical).
Each processing block COk therefore receives as input the measurements corresponding to a respective acquisition layer zk (references zi ... zn), and outputs an occupation grid, in the form of a vector of integers gk representing the indices of the classes of probabilities associated with the different cells of the grid. Thus, the grid gk contains the occupancy information estimated using measurements of the ply k only, that is to say the measurement vector zk.
The consolidation hardware block F comprises four combinational logic circuits Fi, F2, F3 and F4 implementing the equations (15), (16), (19) and (20) respectively; it receives at its input the gi ... gn occupancy gates and provides at its output a "consolidated" occupation grid, represented in turn by a vector of integers, indices of the classes of probabilities associated with the different cells of this consolidated grid.
As explained above, equations (19) and (20) may not be implemented; thus the blocks F3 and F4 may be absent, or be replaced by detection conflict management circuits, for example, implementing the rule expressed by equation 21 (choice of the maximum probability).
If the inverse models associated with the different acquisition plies are identical, the blocks COt ... COn are also identical, and can be replaced by a single block of calculation of occupancy probabilities, performing the processing in a sequential manner.
The data processing module MTD1 can also be associated with any other type of distance sensor.
Figs. 10A and 10B relate to another embodiment of the invention, using a plurality of sensors arranged at different locations that cooperate to provide a busy grid constructed using measurements from different points of view. The sensors can be technologically heterogeneous in precision, range, field of vision and / or speed of acquisition. In this embodiment, the distance of the first obstacle is information relating to the sensor that is measuring. A schematic example of the scenario is shown in Figure 10A, showing two sensors C1 and C2 placed at different positions and having different ranges and fields of view. Thus the obstacle O is seen at completely different distances by Ci and C2.
In this embodiment, the main difficulty lies in the fact that the occupancy gate on the one hand and the sensors of other parts each have their own mark associated with them. For example, assessing the location of obstacles requires changes in benchmarks.
FIG. 10B illustrates an obstacle perception system according to such an embodiment of the invention. This system comprises, in general, "n" non-co-located and potentially heterogeneous sensors Ci, C2 ... Cn and a data processing module MTD2. The latter differs from the data processing module MTD1 of FIG. 9B in that it also includes, interposed between the hardware blocks for calculating occupation probabilities CO1 ... COn and the material consolidation block F, blocks of change of reference Ri ... Rn. Each of these blocks (¾ contains calculation units, generally in floating point, for effecting the change of the marker of a respective sensor towards the reference of the occupancy grid called "consolidation" with respect to which is performed the merger The calculation performed for the change of marker consists of reallocating the occupation of a known location in the reference of a Cr sensor (expressed by a vector of integers gk) to the corresponding cell in the grid coordinate system. The vectors of integers representing the occupations of the cells of the consolidation grid are represented by gx ... gn This reassignment supposes the calculation of translations, rotations, etc. The processing of the blocks Rk can for example be carried out in using a Floating Point Unit (FPU) floating point arithmetic unit, in which case the same hardware block can perform the computation for all Rk blocks ( sequential treatment).
As a variant, the change of reference equations can be stored in conversion tables stored in memories contained in the modules Rk. Thus, even in this case, one can dispense with the floating-point calculation and only perform operations on integers. However, these conversion tables can be quite large and their storage have a significant cost in terms of silicon area.
FIG. 11 illustrates a third embodiment of the invention in which a single sensor C, not shown, acquires scalar or vectorial measurements zt, zt + 1, ... zt + m ··· at an acquisition rate N times higher than what is required for a specific application. A hardware block for calculating occupation probabilities CO produces a occupation grid gt, gt + i, ... gt + m ... for each of these measurements. Then a melting material block F merges N of these grids, acquired at successive instants, into a single consolidated grid gfUS; the consolidated occupancy gfUS gates are thus generated at a rate N times lower than the rate of application of the sensor. For example, if the C sensor operates at a rate of 100 Hz and a rate of 10 Hz is sufficient for the intended application, you can merge 10 successive acquisitions.
In the embodiments of FIGS. 9B, 10B and 11, the case is considered where the processes - or at least some of them - are carried out by hardware calculation blocks, that is to say dedicated digital circuits. . The invention, however, also lends itself to a totally or partially software implementation, in which the processing operations - or at least some of them - are carried out by a generic processor programmed in a timely manner.

权利要求:
Claims (16)
[1" id="c-fr-0001]
1. A method of perception of material bodies (O) comprising the following steps, implemented by a computer or a dedicated digital electronic circuit (MTD1, MTD2): a) Acquisition of a plurality of distance measurements (zi ... zn) said material bodies from one or more sensors (Ci ... Cn); b) Applying, at each said distance measurement, an inverse model of the corresponding sensor on an occupancy grid (GO) providing a discretized spatial representation of an environment of said sensor, to determine a probability of occupancy by a body material of a set of cells of said occupancy grid; and c) constructing a consolidated occupancy grid in which each cell has a probability of occupancy calculated by merging the probabilities of occupation estimated during step b); characterized in that each said inverse sensor model is a discrete model, associating with each cell of the corresponding occupation grid, and for each distance measurement, a probability class chosen within the same set of cardinality finite, each said probability class being identified by an integer index; and in that, in said step c), the occupancy probability of each cell of the consolidated occupancy grid is determined by means of integer calculations performed on the indices of the classes of probabilities determined during said step b).
[2" id="c-fr-0002]
2. The method according to claim 1, wherein said set of finite cardinality of probability classes is formed by the union of one or more subsets such that, during said step c), the merger of two probability classes belonging to the same subset provide a result also belonging to said subset.
[3" id="c-fr-0003]
3. Method according to one of the preceding claims wherein said set of finite cardinality probability classes constitutes a non-uniform discretization of the probability interval [0, 1], with a discretization step increasing between 0 and 0.5. , then decreasing between 0.5 and 1.
[4" id="c-fr-0004]
4. Method according to claims 2 and 3 wherein said set of finite cardinality classes of probability, designated by S *, is formed by the union of two subsets and 5¾ defined by:

the index n taking relative integer values and the parameter k being a non-zero relative integer, and in which, during said step c), the merger F (pi, Pj) between two probability classes pi, Pj and Sj < is calculated by applying the following equations when the indices "i" and "j" have the same sign: • F (pj, pj) = pi + jk.ij if i, j <0; • F (pi, pj) = pj + j + k.j.j if i, j> 0.
[5" id="c-fr-0005]
5. The method of claim 4 wherein, in said step c), the merger F (pi, pj) between two classes of probabilities p ,, pj e Sk is calculated by applying the following equations when the indices "i" and "J" have opposite signs: • F (pj, pj) = p (i + j) + (i + kj) If i <0, j> 0 and | i |> | j | ; • F (Pi.Pj) = P (i + jHi-k-i) if i <0, j> 0 and | i | <| j |. where the symbol 4- designates an entire division.
[6" id="c-fr-0006]
6. Method according to one of claims 4 or 5 wherein k = 1.
[7" id="c-fr-0007]
7. The method of claim 3 wherein said set of finite cardinality classes of probability, designated Gp, is formed by the union of two subsets Gp and G £ defined by: Gp = {(Prù.n £ 0 }

Gp = {(p "), n ^ 0) the index n taking relative integer values, p being a value parameter strictly between 0.5 and 1 and the pn probabilities being defined by recursion as follows: = 0.5; Pi = P; Pn + i = F (pn, p) Vn> 1 P-1 = 1-P; Pn-i = F (pn, Pi) Vn <-1 and in which, during said step c), the merger F (pi, pj) between two probability classes pif Pj e Sk is computed by applying the equation next: Fd (Pi.Pj) = Pi + j
[8" id="c-fr-0008]
The method according to one of the preceding claims, wherein said step c) comprises the implementation of marker changes for constructing said consolidated occupancy grid from occupation probabilities of occupation grid cells associated with non-co-located sensors.
[9" id="c-fr-0009]
9. Method according to one of the preceding claims also comprising a preliminary step of construction of the inverse models of at least one said sensor on the corresponding occupation grid, implemented by applying the following equation:

where: • P (o; | z) represents the occupancy probability of the cell of index "i" of the occupancy grid, said cells being ordered by increasing distance from said sensor; • N is the number of cells of said occupation grid;

Z represents a measurement of distance from the sensor; and • p (z | xk) represents the direct model of the sensor, expressing the probability density of the "z" measurement provided by the sensor when the xk position of cell k of the occupancy grid, but no other cell more near the sensor, is occupied by a material body.
[10" id="c-fr-0010]
10. Method according to one of the preceding claims also comprising a preliminary step of determining a spatial resolution of at least one said occupancy gate, said spatial resolution being chosen the smallest compatible with the stress max [p (z | xk)]> pmin, where p (z | xk) is a direct model of the sensor, expressing the probability density of the measure "z" provided by the sensor when the cell xk of the occupancy grid, but no other cell closer to the sensor, is occupied by a material body, and pmin is the smallest probability class belonging to said set and greater than 0.5.
[11" id="c-fr-0011]
11. A body body perception system comprising: at least one input port for receiving a plurality of signals (zi ..... zn) representative of distance measurements of said hardware bodies from one or more sensors; a data processing module (MTD1, MTD2) configured to input said signals and use them to construct a consolidated occupancy grid by applying a method according to one of claims 1 to 8; and at least one output port for a signal (gfUS) representative of said consolidated occupancy grid.
[12" id="c-fr-0012]
The system of claim 11 further comprising one or more distance sensors (Ci, ..., Cn) adapted to generate signals representative of a plurality of distance measurements of said hardware bodies and connected to said at least one input port. .
[13" id="c-fr-0013]
13. System according to one of claims 11 or 12 wherein said data processing module comprises at least one hardware block for calculating occupation probabilities (COi ... COn) comprising a storage memory, in the form of a correspondence table, an inverse model of a sensor associating with each distance measurement a vector of integers representing probability class indices associated with respective cells of a busy grid.
[14" id="c-fr-0014]
14. System according to one of claims 11 to 13 wherein said data processing module comprises an integral calculation hardware block, said consolidation (F), configured to receive as input a plurality of integers representing class indices. of probability associated with respective occupancy grid cells, and for calculating an index of a probability class associated with a cell of said consolidated occupancy grid.
[15" id="c-fr-0015]
15. The system of claim 14 when dependent on claim 13 wherein said data processing module also comprises at least one calculation block, called change of reference (Ri, .... Rn), configured for: receiving as input at least one first integer vector, derived from a corresponding hardware occupancy probability calculation block and representing probability class indices associated with respective cells of a busy grid; converting it into a second integer vector representing probability class indices associated with respective cells of another occupancy grid, spatially coinciding with said consolidated occupancy grid; and providing said second integer vector as input to said hardware consolidation block.
[16" id="c-fr-0016]
16. System according to one of claims 11 or 12 wherein said data processing module is configured to receive as input signals (z1t .... zn) representative of distance measurements acquired successively by the same sensor or sensors. co-located and to construct a consolidated occupancy grid from a plurality of these signals, corresponding to successive acquisition instants.

类似技术:

---

公开号 | 公开日 | 专利标题

---

FR3041451A1|2017-03-24|METHOD AND SYSTEM FOR COLLECTING MATERIAL BODIES

---

EP2599021B1|2017-03-22|Method for optimally determining the characteristics and arrangement of a set of sensors for monitoring an area

---

EP3364213A1|2018-08-22|Method and system for contextualised perception of material bodies

---

EP3252615B1|2021-09-08|Method and system for determining cells crossed by a measuring or viewing axis

---

EP3137355A1|2017-03-08|Device for signalling objects to a navigation module of a vehicle equipped with this device

---

WO2015185846A1|2015-12-10|Dynamic scene analysis method, and associated analysis module and computer programme

---

CA2621980A1|2007-03-15|Vehicle steering aid method and improved related device

---

FR2961897A1|2011-12-30|NAVIGATION FILTER FOR A FIELD CORRELATION NAVIGATION SYSTEM

---

EP3435029A1|2019-01-30|Resolution adaptive mesh for performing 3-d metrology of an object

---

US9767361B2|2017-09-19|Bathymetric techniques using satellite imagery

---

Javanmardi et al.2018|Factors to evaluate capability of map for vehicle localization

---

WO2015079152A1|2015-06-04|Method for simulating wave propagation; simulator, computer program and recording medium for implementing the method

---

EP3126864B1|2020-07-15|Method for geolocating the environment of a carrier

---

FR2977023A1|2012-12-28|GENERATION OF CARD DATA

---

Galloway et al.2014|Automated crater detection and counting using the Hough transform

---

Zang et al.2021|Non-fusion time-resolved depth image reconstruction using a highly efficient neural network architecture

---

WO2014060698A1|2014-04-24|System and method for the navigation of a movable vehicle, suitable for determining and displaying a safe navigation zone

---

EP3968219A1|2022-03-16|Cooperative method and system for detecting material bodies in an environment

---

EP3384462A1|2018-10-10|Method for characterising a scene by calculating the 3d orientation

---

EP3640841A1|2020-04-22|Electronic device and method for monitoring a scene via a set of electronic sensor|, associated computer program

---

WO2021005096A1|2021-01-14|Method for three-dimensional representation of the spatial coverage of one or more detection systems

---

FR3096637A1|2020-12-04|Method and device for determining a path for an autonomous vehicle

---

FR3104705A1|2021-06-18|PARTICULAR FILTERING AND NAVIGATION CENTRAL WITH MEASUREMENT CORRELATION

---

FR3104704A1|2021-06-18|PARTICULAR FILTERING AND NAVIGATION CENTRAL WITH MEASUREMENT CORRELATION

---

WO2021004987A1|2021-01-14|Method for determining the spatial coverage of a detection system

同族专利:

---

公开号 | 公开日

---

EP3353720A1|2018-08-01|

---

US20180247216A1|2018-08-30|

---

FR3041451B1|2018-02-16|

---

JP2018537745A|2018-12-20|

---

WO2017050890A1|2017-03-30|

---

JP6869230B2|2021-05-12|

引用文献:

---

公开号 | 申请日 | 公开日 | 申请人 | 专利标题

---

US10134152B2|2016-06-03|2018-11-20|Commissariat A L'energie Atomique Et Aux Energies Alternatives|Method and system for determining cells traversed by a measuring or visualization axis|DE102009007395B4|2008-03-25|2015-11-26|Volkswagen Ag|Method for map-based environment representation of a vehicle|

---

US9429650B2|2012-08-01|2016-08-30|Gm Global Technology Operations|Fusion of obstacle detection using radar and camera|US10635913B2|2016-10-17|2020-04-28|Mediatek Inc.|Path planning method and related navigation device|

---

FR3062924B1|2017-02-16|2019-04-05|Commissariat A L'energie Atomique Et Aux Energies Alternatives|METHOD AND SYSTEM FOR CONTEXTUALIZED PERCEPTION OF MATERIAL BODIES|

---

EP3594719A1|2018-07-13|2020-01-15|Commissariat à l'Energie Atomique et aux Energies Alternatives|Environment sensing method and apparatus using a wide-angle distance sensor|

---

EP3734388A1|2019-04-29|2020-11-04|Commissariat à l'Energie Atomique et aux Energies Alternatives|Method and apparatus for performing simultaneous localization and mapping|

---

EP3926434A4|2020-06-18|2021-12-22|Commissariat Energie Atomique|Method and apparatus for performing grid-based localization of a mobile body|

法律状态:
2016-09-28| PLFP| Fee payment|Year of fee payment: 2 |

---

2017-03-24| PLSC| Publication of the preliminary search report|Effective date: 20170324 |

---

2017-09-29| PLFP| Fee payment|Year of fee payment: 3 |

---

2018-09-28| PLFP| Fee payment|Year of fee payment: 4 |

---

2019-09-30| PLFP| Fee payment|Year of fee payment: 5 |

---

2020-09-30| PLFP| Fee payment|Year of fee payment: 6 |

---

2021-09-30| PLFP| Fee payment|Year of fee payment: 7 |

优先权:

---

申请号 | 申请日 | 专利标题

---

FR1558919|2015-09-22|

---

FR1558919A|FR3041451B1|2015-09-22|2015-09-22|METHOD AND SYSTEM FOR COLLECTING MATERIAL BODIES|FR1558919A| FR3041451B1|2015-09-22|2015-09-22|METHOD AND SYSTEM FOR COLLECTING MATERIAL BODIES|

---

JP2018512239A| JP6869230B2|2015-09-22|2016-09-22|Methods and systems for recognizing objects|

---

PCT/EP2016/072530| WO2017050890A1|2015-09-22|2016-09-22|Method and system for perceiving physical bodies|

---

EP16775572.7A| EP3353720A1|2015-09-22|2016-09-22|Method and system for perceiving physical bodies|

---

US15/753,540| US20180247216A1|2015-09-22|2016-09-22|Method and system for perceiving physical bodies|