Brevet FR3145978A1 — publié le 20240823

Procédé de navigation d’un véhicule (1) pourvu d’un lidar (5) et d’une unité de mesure inertielle (6) tous deux reliés à une unité électronique de navigation (2) mettant en œuvre un premier algorithme de localisation et cartographie simultanées et un deuxième algorithme de fusion de données, le premier algorithme recevant en entrée des premières données de localisation issues du lidar (5) pour calculer un premier déplacement depuis une position d’initialisation, le deuxième algorithme recevant en entrée le premier déplacement et des deuxièmes données de localisation issues de l’unité de mesure inertielle (6) et fournissant en sortie une deuxième position fusionnée du véhicule (1), caractérisé en ce que le deuxième algorithme met en œuvre un filtrage de Kalman étendu invariant et en ce que la deuxième position fusionnée est introduite dans une boucle de rétroaction alimentant en entrée le premier algorithme pour calculer une nouvelle position devenant la position d’initialisation. FIGURE DE L’ABREGE : Fig. 1 Procédé de navigation à Kalman étendu invariant et véhicule équipé pour sa mise en œuvre. La présente invention concerne le domaine de la navigation des véhicules. ARRIERE PLAN DE L’INVENTION Il est connu des véhicules pourvus d’un lidar relié à une unité électronique de navigation. On rappelle qu’un lidar est un appareil comprenant le plus souvent un émetteur laser et un photodétecteur qui sont montés sur un support pilotable en gisement. L’émetteur laser est agencé pour émettre des impulsions lasers incidentes à des angles de gisement prédéterminés et le photodétecteur détecte les impulsions laser réfléchies par les obstacles environnant le véhicule. A partir de l’angle de gisement et d’une mesure du temps entre l’émission de l’impulsion laser et la détection de l’impulsion laser réfléchie, il est possible de déterminer la position de l’obstacle par rapport à l’ensemble émetteur laser / photodétecteur. Comme l’ensemble émetteur laser / photodétecteur balaye l’environnement du véhicule, on obtient une image (couramment appelé scan lidar) de l’environnement du véhicule constituée des coordonnées de tous les points de réflexion des impulsions laser. L’unité électronique de navigation met de préférence en œuvre un algorithme de localisation et cartographie simultanées, couramment appelé SLAM, qui agglomère les scans lidar pour à la fois cartographier l’environnement dans lequel se déplace le véhicule mais également déterminer la trajectoire suivie par le véhicule dans l’environnement ainsi cartographié en détectant dans les scans lidar successifs la présence de mêmes points d’intérêt. Il est également connu des véhicules pourvus d’une unité de mesure inertielle reliée à une unité électronique de navigation. On rappelle qu’une unité de mesure inertielle (UMI ou IMU de l’anglais « inertial measurement unit ») comprend trois capteurs accélérométriques disposés selon les axes d’un repère de mesure accélérométrique et trois capteurs angulaires, gyroscopes ou gyromètres, pour mesurer des mouvements angulaires du repère de mesure accélérométrique par rapport à une orientation de référence du repère de mesure. Les capteurs accélérométriques, soumis à la gravité et aux accélérations du véhicule, mesurent, dans le repère de mesure accélérométrique, une grandeur nommée force spécifique (« specific force » ou « g-force » en anglais) et déterminent les trois composantes d’un vecteur de force spécifique. Les signaux des capteurs de l’unité de mesure inertielle (données d’accélération et d’angle) sont exploités par un algorithme de navigation inertielle pour déterminer à partir de ceux-ci une position du véhicule dans un repère géographique local. Il est en outre connu des véhicules pourvus d’un lidar et d’une unité de mesure inertielle tous deux reliés à une unité électronique de navigation. Il est alors possible de calculer, entre deux scans lidar, des positions intermédiaires à partir des seules données de localisation « accélération, vitesse angulaire » qui sont disponibles à une fréquence supérieure à celle des scans lidar. La illustre une première architecture possible de l’unité électronique de navigation combinant données lidar et données inertielles. L’unité électronique de navigation met en œuvre un premier algorithme de localisation et cartographie simultanées « SLAM » et un deuxième algorithme de fusion de données « Fusion ». Le premier algorithme reçoit en entrée des premières données de localisation « scans Lidar » issues du lidar pour calculer une première position courante et un premier déplacement « y n » entre la première position courante et une position d’initialisation. Le deuxième algorithme reçoit en entrée le premier déplacement « y n » et des deuxièmes données de localisation « accélération, vitesse angulaire » issues de l’unité de mesure inertielle pour fournir en sortie une position fusionnée du véhicule permettant de déterminer la trajectoire « trajectoire x n » de celui-ci. On parle pour cette première architecture de fusion lâche entre les données de localisation du lidar et les données de localisation de l’unité de mesure inertielle car le deuxième algorithme de fusion ne traite pas directement les scans lidar mais un déplacement déterminé à partir desdits scans lidar. Cette fusion lâche est peu gourmande en ressource de calcul et en mémoire mais est sensible aux mouvements brusques du véhicule qui peuvent conduire à des problèmes de flou ou de perte des points d’intérêt, problèmes qui altèrent la précision de la navigation. Pour remédier à ces inconvénients, on a imaginé une deuxième architecture, représentée à la , réalisant une fusion serrée entre les données de localisation du lidar et les données de localisation de l’unité de mesure inertielle. L’unité électronique de navigation met alors en œuvre un algorithme de fusion de données qui reçoit en entrée, d’une part, directement les premières données de localisation « scans Lidar » issues du lidar et, d’autre part, les deuxièmes données de localisation « accélération, vitesse angulaire » issues de l’unité de mesure inertielle et qui fournit comme précédemment en sortie une position fusionnée du véhicule permettant de déterminer la trajectoire « trajectoire x n » de celui-ci. L’algorithme de fusion de données met en œuvre un filtrage de Kalman itéré qui fusionne, dans un vecteur de grande dimension, un état du véhicule correspondant aux deuxièmes données de localisation avec les coordonnées des points de réflexion des impulsions laser correspondant aux premières données de localisation. La fusion serrée apporte un gain notable en précision mais est très gourmande en ressources de calcul et en mémoire. En outre, la réalisation des calculs demande une énergie importante qui est prélevée sur celle stockée dans le véhicule. Lorsque le véhicule est un drone léger (typiquement quelques kilogrammes au maximum), les ressources en calcul et en mémoire, ainsi que la capacité de stockage en énergie, sont trop faibles pour ce type de fusion. On aurait aussi pu imaginer de doter le véhicule de capteurs supplémentaires pour améliorer la précision de la navigation, mais cela augmente le coût de l’installation, diminue la capacité d’emport, et augmente les besoins de consommation au niveau des batteries : cela n’est pas envisageable sur un véhicule ayant un faible encombrement et une faible capacité d’emport comme un drone léger. OBJET DE L’INVENTION L’invention a pour but d’améliorer la précision de navigation d’un véhicule embarquant un lidar et une unité de mesure inertielle comme seuls capteurs de navigation. A cet effet, on prévoit, selon l’invention, un procédé de navigation d’un véhicule pourvu d’un lidar et d’une unité de mesure inertielle tous deux reliés à une unité électronique de navigation mettant en œuvre un premier algorithme de localisation et cartographie simultanées et un deuxième algorithme de fusion de données. Le premier algorithme reçoit en entrée des premières données de localisation issues du lidar pour calculer un premier déplacement depuis une position d’initialisation ; le deuxième algorithme reçoit en entrée le premier déplacement et des deuxièmes données de localisation issues de l’unité de mesure inertielle et fournit en sortie une deuxième position fusionnée du véhicule. Le deuxième algorithme met en œuvre un filtrage de Kalman étendu invariant et en ce que la deuxième position fusionnée est introduite dans une boucle de rétroaction alimentant en entrée le premier algorithme pour calculer une nouvelle position devenant la position d’initialisation. Ainsi, le procédé de l’invention introduit une rétroaction de l’algorithme de fusion vers l’algorithme de localisation et de cartographie simultanées. Le procédé est alors plus robuste aux changements brusques de direction qui sont pris en compte par l’exploitation des deuxièmes données de localisation issues de l’unité de mesure inertielle. La fusion opérée par le procédé de l’invention présente donc les avantages d’une fusion lâche (coût de calcul léger) mais bénéficie d’une précision et d’une robustesse supérieures. En outre, contrairement à un filtre de Kalman étendu standard (ou EKF de l’anglais extended Kalman filter ), les erreurs du filtre de Kalman étendu invariant (ou IEKF de l’anglais invariant extended Kalman filter ) ont pour avantage de ne pas dépendre de l’état estimé par le filtre : la rétroaction ne risque donc pas d’engendrer des erreurs en cascade. Le procédé comprend également tout ou partie des caractéristiques suivantes, isolément ou en combinaison : le premier algorithme détermine le premier déplacement à partir de la position d’initialisation et d’une position courante dernièrement calculée, dans lequel la boucle de rétroaction comprend un bloc de calcul pour calculer un déplacement relatif entre la position fusionnée et la position courante dernièrement calculée, et dans lequel le premier algorithme estime la nouvelle position devenant la position d’initialisation à partir du déplacement relatif et de la position courante dernièrement calculée ; le bloc de calcul est un bloc de calcul de changement de repère. L’invention concerne également un véhicule équipé pour la mise en œuvre de ce procédé. D’autres caractéristiques et avantages de l’invention ressortiront à la lecture de la description qui suit d’un mode de réalisation particulier et non limitatif de l’invention. Il sera fait référence aux dessins annexés, parmi lesquels : la est une représentation d’une première architecture connue pour fusionner des données issues d’un lidar et des données issues d’une unité de mesure inertielle ; la est une représentation d’une deuxième architecture connue pour fusionner des données issues d’un lidar et des données issues d’une unité de mesure inertielle ; la est une représentation de l’architecture de l’invention pour fusionner des données issues d’un lidar et des données issues d’une unité de mesure inertielle ; la est une vue schématique d’un véhicule équipé pour mettre en œuvre le procédé de l’invention. DESCRIPTION DETAILLEE DE L’INVENTION L’invention est ici décrite en application à un véhicule de type drone léger, comme ici un drone aérien. En référence à la , un tel véhicule, portant la référence générale 1, comprend, de manière connue en elle-même, un fuselage embarquant une unité électronique de commande 2, un organe de propulsion 3, un organe de direction 4, un lidar 5 et une unité de mesure inertielle 6. L’organe de propulsion 3 comprend par exemple un ou plusieurs rotors motorisés à hélice par lequel ou lesquels le véhicule 1 peut décoller, se maintenir en vol stationnaire et se déplacer en vol. L’organe de direction 4 comprend par exemple un actionneur d’orientation d’un ou plusieurs des rotors, ou un actionneur d’orientation d’une ou plusieurs gouvernes permettant de diriger le véhicule 1 en vol et modifier sa trajectoire de vol. Le lidar 5 comprend un émetteur laser et un photodétecteur qui sont montés sur un support pilotable en gisement. L’émetteur laser est agencé pour émettre des impulsions lasers incidentes à des angles de gisement prédéterminés et le photodétecteur détecte les impulsions laser réfléchies par les obstacles environnant le véhicule 1. L’ensemble émetteur laser / photodétecteur balaye l’environnement du véhicule (sur 360° ou un angle inférieur par exemple 180° sur l’avant par référence à un sens d’avancement du véhicule 1) de sorte que les signaux fournis par le lidar 5 comprennent les coordonnées de tous les points de réflexion des impulsions laser qui forment une image (couramment appelé scan lidar) de l’environnement du véhicule 1. Ces coordonnées constituent des premières données de localisation (« scans lidar » sur la ). Le lidar 5 a une fréquence d’échantillonnage qui dépend de la vitesse de balayage et fournit les premières données de localisation à cette fréquence. L’unité de mesure inertielle 6 comprend trois capteurs accélérométriques disposés selon les axes d’un repère de mesure accélérométrique pour mesurer une force spécifique dans le repère de mesure accélérométrique et trois capteurs angulaires, gyroscopes ou gyromètres, pour mesurer des mouvements angulaires du repère de mesure accélérométrique par rapport à une orientation de référence du repère de mesure. Les signaux issus de l’unité de mesure inertielle 6 contiennent des données d’accélération et d’angle formant des deuxièmes données de localisation (« inertie (IMU) » sur la ) et sont fournis à une fréquence d’échantillonnage supérieure à la fréquence de fourniture des premières données de localisation représentatives d’un scan lidar. L’organe de propulsion 3, l’organe de direction 4, le lidar 5 et l’unité de mesure inertielle 6 sont connus en eux-mêmes et ne seront pas plus décrits ici. L’organe de propulsion 3, l’organe de direction 4, le lidar 5 et l’unité de mesure inertielle 6 sont reliés à l’unité électronique de commande 2. L’unité électronique de commande 2 comprend un processeur et une mémoire contenant un programme informatique exécutable par le processeur pour mettre en œuvre le procédé de l’invention afin de réaliser une navigation et commander l’organe de propulsion 3 et l’organe de direction 4 pour suivre la navigation de manière autonome. Le programme informatique comprend un premier algorithme de localisation et cartographie simultanées (« SLAM » sur la ) et un deuxième algorithme de fusion de données (« Fusion par IEKF » sur la ). Le premier algorithme de localisation et cartographie simultanées est ici un algorithme classique SLAM qui agglomère les scans lidar pour à la fois cartographier l’environnement dans lequel se déplace le véhicule 1 mais également déterminer la trajectoire suivie par le véhicule 1 dans l’environnement ainsi cartographié en détectant dans les scans lidar successifs la présence de mêmes points d’intérêt. Le premier algorithme reçoit en une première entrée les premières données de localisation issues du lidar 5 pour calculer une première position courante et un premier déplacement « y n » entre la première position courante et une position précédente. Le deuxième algorithme reçoit en entrée le premier déplacement « y n » et les deuxièmes données de localisation issues de l’unité de mesure inertielle 6 pour fournir en sortie une position fusionnée du véhicule 1 et la trajectoire « x n » de celui-ci. Le deuxième algorithme met en œuvre un filtrage de Kalman étendu invariant (ou IEKF). Le programme met également en œuvre une boucle de rétroaction par laquelle la position initialisant le premier algorithme est calculée à partir de la position fusionnée et de la première position courante dernièrement calculée par le premier algorithme. Plus précisément, la boucle de rétroaction comprend un bloc de calcul « Changement de repère » recevant en entrée la sortie du deuxième algorithme et fournissant en sortie une deuxième entrée du premier algorithme : la position fusionnée (issue des données fournies par exemple à l’instant t) est donc transférée au bloc de calcul « Changement de repère » qui calcule le déplacement relatif entre la position fusionnée (instant t) et la première position courante dernièrement calculée (elle aussi issue des données fournies à l’instant t) par le premier algorithme ; et ce déplacement relatif est envoyé en entrée du premier algorithme. Le premier algorithme estime alors, à partir de la première position courante dernièrement calculée (instant t) et du déplacement relatif une nouvelle position qui devient la position d’initialisation (instant t). A partir de cette position d’initialisation (instant t), le premier algorithme calcule la première position courante et le premier déplacement à l’instant présent (issue des données fournies à l’instant t+1) qui sera exploité par le deuxième algorithme pour estimer la position fusionnée (instant t+1). On comprend que l’intérêt d’utiliser un filtrage de Kalman étendu invariant en amont de la boucle de rétroaction est qu’il évite une auto-alimentation des erreurs puisque les erreurs d’un tel filtrage sont indépendantes de l’état estimé de sorte que les erreurs de ce filtrage ne viennent pas alimenter le premier algorithme qui lui-même alimente le deuxième algorithme. En effet, notons l’état du véhicule où est la position du véhicule dans un repère de référence, et la matrice de passage du repère de référence au repère du véhicule, communément appelée l’orientation. Ici, on considère que est une matrice orthogonale. D’autres quantités peuvent être ajoutées à ce vecteur d’état, comme des biais de l’unité de mesure inertielle ou la vitesse, mais ceci n’est pas nécessaire pour expliquer l’invention. Pour recaler le filtre du deuxième algorithme IEKF conformément à la figure 3, on utilise l’observation ou premier déplacement issu du premier algorithme SLAM. Comme expliqué ci-dessus, par l’intermédiaire de la rétroaction, le SLAM calcule le premier déplacement en étant initialisé à la dernière position qu’il a calculée, incrémentée par le déplacement relatif entre la position fusionnée fournie par le deuxième algorithme IEKF, notée , et la dernière position calculée par le premier algorithme SLAM, qui correspond à . On a donc pouvant s’écrire comme fonction de son initialisation , avec l’expression de qui est : Dans cette formule, est l’orientation déterminée par le deuxième algorithme IEKF, et désigne sa matrice transposée. Ici, l’observation est égale à la position (aux incertitudes de mesure près), le filtrage de Kalman mis en œuvre dans le deuxième algorithme est donc un filtre de Kalman dit invariant à gauche (L-IEKF : « left-invariant extended Kalman filter »). L’observation étant égale à la position (aux incertitudes de mesure près), on a et donc : On reconnaît alors l’erreur du filtre de Kalman invariant L-IEKF : Or, pour un filtre de Kalman invariant L-IEKF, on peut montrer que l’erreur est indépendante de l’état estimé (on parle aussi « d’erreur autonome ») : ce résultat a été montré dans la littérature académique sur le filtre de Kalman invariant. On peut notamment consulter la référence bibliographique suivante (et en particulier la proposition 24 de cette référence qui donne le résultat d’autonomie de , exploité ici) : « Linear observed systems on groups », par Axel Barrau et Silvère Bonnabel, parue en 2019 dans « Systems & Control Letters », volume 129, pages 36-42. Ainsi, l’initialisation du premier algorithme SLAM pour calculer le premier déplacement se réécrit : Dans la formule , ne dépend pas de l’état estimé. En appliquant un raisonnement par récurrence, compte tenu qu’à l’instant , la première initialisation du premier algorithme SLAM ne dépend pas de l’état estimé par la fusion (puisque la fusion n’a pas encore commencé), on voit avec la formule que la position d’initialisation du SLAM ne dépend jamais de l’état estimé, ce qui évite les erreurs en cascade grâce à l’utilisation du filtre de Kalman étendu invariant. Bien entendu, l’invention n’est pas limitée au mode de réalisation décrit mais englobe toute variante entrant dans le champ de l’invention telle que définie par les revendications. En particulier, le véhicule peut avoir une structure différente de celle décrite. Il peut être aérien, terrestre, nautique ou amphibie, être piloté ou pas. Le lidar utilisé ici est un lidar 3D : on peut également utiliser un lidar 2D. L’utilisation d’un lidar 2D peut être particulièrement adaptée dans le cas d’une application de l’invention à un véhicule terrestre ou nautique. Par « associée à », on entend que l’unité électronique de commande 2 peut incorporer l’unité électronique de navigation (les deux unités électroniques étant réalisées sur un même circuit électronique ou intégré) ou être reliée à celle-ci (les deux unités électroniques étant réalisées sur des circuits électroniques ou intégrés différents). Procédé de navigation d’un véhicule (1) pourvu d’un lidar (5) et d’une unité de mesure inertielle (6) tous deux reliés à une unité électronique de navigation (2) mettant en œuvre un premier algorithme de localisation et cartographie simultanées et un deuxième algorithme de fusion de données, le premier algorithme recevant en entrée des premières données de localisation issues du lidar (5) pour calculer un premier déplacement depuis une position d’initialisation, le deuxième algorithme recevant en entrée le premier déplacement et des deuxièmes données de localisation issues de l’unité de mesure inertielle (6) et fournissant en sortie une deuxième position fusionnée du véhicule (1), caractérisé en ce que le deuxième algorithme met en œuvre un filtrage de Kalman étendu invariant et en ce que la deuxième position fusionnée est introduite dans une boucle de rétroaction alimentant en entrée le premier algorithme pour calculer une nouvelle position devenant la position d’initialisation. Procédé selon la revendication 1, dans lequel le premier algorithme détermine le premier déplacement à partir de la position d’initialisation et d’une position courante dernièrement calculée, dans lequel la boucle de rétroaction comprend un bloc de calcul pour calculer un déplacement relatif entre la position fusionnée et la position courante dernièrement calculée, et dans lequel le premier algorithme estime la nouvelle position devenant la position d’initialisation à partir du déplacement relatif et de la position courante dernièrement calculée. Procédé selon la revendication 2, dans lequel le bloc de calcul est un bloc de calcul de changement de repère. Véhicule (1) pourvu d’un lidar (5) et d’une unité de mesure inertielle (6) tous deux reliés à une unité électronique de navigation (2) mettant en œuvre un premier algorithme de localisation et cartographie simultanées et un deuxième algorithme de fusion de données, le premier algorithme recevant en entrée des premières données de localisation issues du lidar (5) pour calculer une première position courante et un premier déplacement entre la première position courante et une position précédente, le deuxième algorithme recevant en entrée le premier déplacement et des deuxièmes données de localisation issues de l’unité de mesure inertielle (6) et fournissant en sortie une position fusionnée du véhicule (1), l’unité électronique de navigation (2) est programmée pour mettre en œuvre le procédé selon l’une quelconque des revendications 1 à 3. Véhicule selon la revendication 4, formant drone, comprenant une unité électronique de commande associée à l’unité électronique de navigation, un organe de propulsion et un organe de direction tous deux reliés à l’unité électronique de commande qui est programmée pour commander l’organe de propulsion et l’organe de direction à partir des positions fusionnées pour suivre une trajectoire de manière autonome. Véhicule selon la revendication 5, formant drone aérien. Véhicule selon la revendication 5, formant drone terrestre.