EP4669933A1 - Procede de navigation a kalman etendu invariant et vehicule equipe pour sa mise en ?uvre - Google Patents

Procede de navigation a kalman etendu invariant et vehicule equipe pour sa mise en ?uvre

Info

Publication number
EP4669933A1
EP4669933A1 EP24704775.6A EP24704775A EP4669933A1 EP 4669933 A1 EP4669933 A1 EP 4669933A1 EP 24704775 A EP24704775 A EP 24704775A EP 4669933 A1 EP4669933 A1 EP 4669933A1
Authority
EP
European Patent Office
Prior art keywords
algorithm
vehicle
lidar
displacement
input
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
EP24704775.6A
Other languages
German (de)
English (en)
Inventor
Camille CHAPDELAINE
Olivier BRUNE
Olivier Garcia
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Safran SA
Original Assignee
Safran SA
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Safran SA filed Critical Safran SA
Publication of EP4669933A1 publication Critical patent/EP4669933A1/fr
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR

Definitions

  • the present invention relates to the field of vehicle navigation.
  • a lidar is a device most often comprising a laser transmitter and a photodetector that are mounted on a support that can be controlled in bearing.
  • the laser transmitter is arranged to emit incident laser pulses at predetermined bearing angles and the photodetector detects the laser pulses reflected by the obstacles surrounding the vehicle. From the bearing angle and a measurement of the time between the emission of the laser pulse and the detection of the reflected laser pulse, it is possible to determine the position of the obstacle relative to the laser transmitter/photodetector assembly.
  • an image (commonly called a lidar scan) of the environment of the vehicle is obtained, consisting of the coordinates of all the reflection points of the laser pulses.
  • the electronic navigation unit preferably implements a simultaneous localization and mapping algorithm, commonly called SLAM, which agglomerates the lidar scans to both map the environment in which the vehicle is moving but also determine the trajectory followed by the vehicle in the environment thus mapped by detecting in the successive lidar scans the presence of the same points of interest.
  • SLAM simultaneous localization and mapping algorithm
  • an inertial measurement unit comprises three accelerometric sensors arranged along the axes of an accelerometric measurement frame and three angular sensors, gyroscopes or gyrometers, for measuring angular movements of the accelerometric measurement frame relative to a reference orientation of the measurement frame.
  • the accelerometric sensors subjected to gravity and accelerations of the vehicle, measure, in the accelerometric measurement frame, a quantity called specific force ("specific force” or "g-force” in English) and determine the three components of a specific force vector.
  • the signals from the sensors of the inertial measurement unit are used by an inertial navigation algorithm to determine from these a position of the vehicle in a local geographical reference frame.
  • Vehicles are also known that are equipped with a lidar and an inertial measurement unit, both connected to an electronic navigation unit. It is then possible to calculate, between two lidar scans, intermediate positions from the location data "acceleration, angular velocity" alone, which are available at a higher frequency than that of the lidar scans.
  • Figure 1 illustrates a first possible architecture of the electronic navigation unit combining lidar data and inertial data.
  • the electronic navigation unit implements a first simultaneous localization and mapping algorithm "SLAM” and a second data fusion algorithm “Fusion”.
  • the first algorithm receives as input first localization data “Lidar scans” from the lidar to calculate a first current position and a first displacement "y n " between the first current position and an initialization position.
  • the second algorithm receives as input the first displacement "y n " and second localization data "acceleration, angular velocity” from the electronic navigation unit. inertial measurement to provide as output a fused position of the vehicle allowing the determination of its trajectory “trajectory x n ”.
  • This first architecture is called loose fusion between the lidar location data and the inertial measurement unit location data because the second fusion algorithm does not directly process the lidar scans but a displacement determined from said lidar scans.
  • This loose fusion is not very demanding in terms of computing resources and memory but is sensitive to sudden movements of the vehicle which can lead to problems of blurring or loss of points of interest, problems which alter the precision of the navigation.
  • a second architecture represented in Figure 2
  • the electronic navigation unit then implements a data fusion algorithm which receives as input, on the one hand, directly the first location data “Lidar scans" from the lidar and, on the other hand, the second location data “acceleration, angular velocity” from the inertial measurement unit and which provides as previously as output a merged position of the vehicle making it possible to determine the trajectory "trajectory x n " of the latter.
  • the data fusion algorithm implements an iterated Kalman filtering which merges, in a large-dimensional vector, a state of the vehicle corresponding to the second location data with the coordinates of the reflection points of the laser pulses corresponding to the first location data.
  • Tight fusion provides a significant gain in accuracy but is very demanding in terms of computing resources and memory.
  • performing the calculations requires energy significant amount of energy that is taken from that stored in the vehicle.
  • the vehicle is a light drone (typically a few kilograms maximum)
  • the computing and memory resources, as well as the energy storage capacity, are too low for this type of fusion.
  • the invention aims to improve the navigation precision of a vehicle carrying a lidar and an inertial measurement unit as the only navigation sensors.
  • the invention provides a method for navigating a vehicle provided with a lidar and an inertial measurement unit, both connected to an electronic navigation unit implementing a first simultaneous localization and mapping algorithm and a second data fusion algorithm.
  • the first algorithm receives as input first localization data from the lidar to calculate a first displacement from an initialization position;
  • the second algorithm receives as input the first displacement and second localization data from the inertial measurement unit and provides as output a second merged position of the vehicle.
  • the second algorithm implements an invariant extended Kalman filtering and in that the second merged position is introduced into a feedback loop feeding as input the first algorithm to calculate a new position becoming the initialization position.
  • the method of the invention introduces feedback from the fusion algorithm to the simultaneous localization and mapping algorithm.
  • the method is then more robust to sudden changes in direction which are taken into account by exploiting the second localization data from the inertial measurement unit.
  • the fusion carried out by the method of the invention therefore has the advantages of a loose fusion (low computational cost) but benefits from greater precision and robustness.
  • the errors of the invariant extended Kalman filter (or IEKF) have the advantage of not depending on the state estimated by the filter: the feedback is therefore not likely to generate cascading errors.
  • the method also comprises all or part of the following characteristics, alone or in combination:
  • the first algorithm determines the first displacement from the initialization position and a last calculated current position
  • the feedback loop comprises a calculation block for calculating a relative displacement between the merged position and the last calculated current position, and wherein the first algorithm estimates the new position becoming the initialization position from the relative displacement and the last calculated current position
  • the calculation block is a reference change calculation block.
  • the invention also relates to a vehicle equipped for implementing this method.
  • Figure 1 is a representation of a first known architecture for merging data from a lidar and data from an inertial measurement unit;
  • Figure 2 is a representation of a second known architecture for merging data from a lidar and data from an inertial measurement unit;
  • Figure 3 is a representation of the architecture of the invention for merging data from a lidar and data from an inertial measurement unit;
  • Figure 4 is a schematic view of a vehicle equipped to implement the method of the invention.
  • the invention is described here in application to a light drone type vehicle, such as here an aerial drone.
  • such a vehicle bearing the general reference 1, comprises, in a manner known per se, a fuselage carrying an electronic control unit 2, a propulsion member 3, a steering member 4, a lidar 5 and an inertial measurement unit 6.
  • the propulsion member 3 comprises, for example, one or more motorized rotors with propellers by which the vehicle 1 can take off, remain in stationary flight and move in flight.
  • the steering member 4 comprises for example an actuator for steering one or more of the rotors, or an actuator for steering one or more control surfaces allowing to direct vehicle 1 in flight and modify its flight trajectory.
  • 5 has a sampling rate that depends on the scanning speed and provides the first location data at this rate.
  • the inertial measurement unit 6 comprises three accelerometric sensors arranged along the axes of an accelerometric measurement frame for measuring a specific force in the accelerometric measurement frame and three angular sensors, gyroscopes or gyrometers, for measuring angular movements of the accelerometric measurement frame relative to a reference orientation of the measurement frame.
  • IMU inverter
  • the propulsion unit 3, the steering unit 4, the lidar 5 and the inertial measurement unit 6 are known per se and will not be described further here.
  • the propulsion unit 3, the steering unit 4, the lidar 5 and the inertial measurement unit 6 are connected to the electronic control unit 2.
  • the electronic control unit 2 comprises a processor and a memory containing a computer program executable by the processor to implement the method of the invention in order to carry out navigation and control the propulsion member 3 and the steering member 4 to follow the navigation autonomously.
  • the computer program includes a first simultaneous localization and mapping algorithm (“SLAM” in Figure 3) and a second data fusion algorithm (“IEKF Fusion” in Figure 3).
  • SLAM simultaneous localization and mapping algorithm
  • IEEEKF Fusion second data fusion algorithm
  • the first simultaneous localization and mapping algorithm is here a classic SLAM algorithm which agglomerates the lidar scans to both map the environment in which the vehicle 1 is moving but also determine the trajectory followed by the vehicle 1 in the environment thus mapped by detecting in the successive lidar scans the presence of the same points of interest.
  • the first algorithm receives as a first input the first location data from the lidar 5 for calculate a first current position and a first displacement “y n ” between the first current position and a previous position.
  • the second algorithm receives as input the first displacement "y n " and the second location data from the inertial measurement unit 6 to provide as output a merged position of the vehicle 1 and the trajectory "x n " thereof.
  • the second algorithm implements an invariant extended Kalman filtering (or IEKF).
  • the program also implements a feedback loop by which the position initializing the first algorithm is calculated from the merged position and the first current position last calculated by the first algorithm. More precisely, the feedback loop includes a “Change of reference” calculation block receiving as input the output of the second algorithm and providing as output a second input of the first algorithm: the merged position (from the data provided for example at time t) is therefore transferred to the “Change of reference” calculation block which calculates the relative displacement between the merged position (time t) and the first current position last calculated (also from the data provided at time t) by the first algorithm; and this relative displacement is sent as input to the first algorithm.
  • the feedback loop includes a “Change of reference” calculation block receiving as input the output of the second algorithm and providing as output a second input of the first algorithm: the merged position (from the data provided for example at time t) is therefore transferred to the “Change of reference” calculation block which calculates the relative displacement between the merged position (time t) and the first current position last calculated (also from the data provided at
  • the first algorithm then estimates, from the first current position last calculated (time t) and the relative displacement, a new position which becomes the initialization position (time t). From this initialization position (time t), the first algorithm calculates the pre- first current position and the first displacement at the present time (from the data provided at time t+1) which will be used by the second algorithm to estimate the merged position (time t+1).
  • the SLAM calculates the first displacement y n by being initialized at the last position y nT that it calculated, incremented by the relative displacement between the merged position provided by the second IEKF algorithm, denoted X nT , and the last position calculated by the first SLAM algorithm, which corresponds to y nT .
  • y n which can be written as a function of its initialization with the expression of which is:
  • L-IEKF left-invariant extended Kalman filter
  • the vehicle may have a structure different from that described. It may be aerial, terrestrial, nautical or amphibious, and may or may not be piloted.
  • the lidar used here is a 3D lidar: a 2D lidar can also be used.
  • the use of a 2D lidar can be particularly suitable in the case of an application of the invention to a land or water vehicle.
  • the electronic control unit 2 can incorporate the electronic navigation unit (the two electronic units being produced on the same electronic or integrated circuit) or be connected to it (the two electronic units being produced on different electronic or integrated circuits).

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

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 sor- tie 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.

Description

PROCEDE DE NAVIGATION A KALMAN ETENDU INVARIANT ET VEHICULE EQUIPE 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 me- sure 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'en- semble émetteur laser / photodétecteur. Comme l'ensemble émetteur laser / photodétecteur balaye l'environnement du véhicule, on obtient une image (couramment appelé scan li- dar) de l'environnement du véhicule constituée des coor- donné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 si- multané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 suc- cessifs 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 mesu- rer 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 cap- teurs 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 lo- cal.
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 « ac- célération, vitesse angulaire » qui sont disponibles à une fréquence supérieure à celle des scans lidar.
La figure 1 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 car- tographie 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 « yn » entre la première position courante et une position d'initiali- sation. Le deuxième algorithme reçoit en entrée le premier déplacement « yn » 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 fu- sionnée du véhicule permettant de déterminer la trajectoire « trajectoire xn » 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, pro- blèmes qui altèrent la précision de la navigation.
Pour remédier à ces inconvénients, on a imaginé une deu- xième architecture, représentée à la figure 2, 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éra- tion, vitesse angulaire » issues de l'unité de mesure iner- tielle et qui fournit comme précédemment en sortie une position fusionnée du véhicule permettant de déterminer la trajectoire « trajectoire xn » 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 éner- gie, sont trop faibles pour ce type de fusion.
On aurait aussi pu imaginer de doter le véhicule de cap- teurs 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 en- visageable 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 naviga- tion d'un véhicule embarquant un lidar et une unité de mesure inertielle comme seuls capteurs de navigation.
RESUME DE L’INVENTION
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é électro- nique 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éhi- cule. 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'initialisa- tion. 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 lo- calisation issues de l'unité de mesure inertielle. La fu- sion 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éplace- ment à 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 calcu- lée, et dans lequel le premier algorithme estime la nouvelle position devenant la position d'initialisa- tion à 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 res- sortiront à la lecture de la description qui suit d'un mode de réalisation particulier et non limitatif de l'invention. BREVE DESCRIPTION DES DESSINS
Il sera fait référence aux dessins annexés, parmi les- quels :
[Fig. 1] la figure 1 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 iner- tielle ;
[Fig. 2] la figure 2 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 iner- tielle ;
[Fig. 3] la figure 3 est une représentation de l'architec- ture de l'invention pour fusionner des données issues d'un lidar et des données issues d'une unité de mesure iner- tielle ;
[Fig. 4] la figure 4 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 figure 4, 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 direc- tion 4, un lidar 5 et une unité de mesure inertielle 6.
L'organe de propulsion 3 comprend par exemple un ou plu- sieurs 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 action- neur 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'en- semble émetteur laser / photodétecteur balaye l'environne- ment du véhicule (sur 360° ou un angle inférieur par exemple 180° sur l'avant par référence à un sens d'avance- ment 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éhi- cule 1. Ces coordonnées constituent des premières données de localisation (« scans lidar » sur la figure 3). Le lidar
5 a une fréquence d'échantillonnage qui dépend de la vi- tesse de balayage et fournit les premières données de lo- calisation à cette fréquence.
L'unité de mesure inertielle 6 comprend trois capteurs ac- célérométriques disposés selon les axes d'un repère de me- sure 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 mou- vements 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 figure 3) et sont fournis à une fréquence d'échan- tillonnage 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écu- table 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 figure 3) et un deuxième algorithme de fusion de données (« Fusion par IEKF » sur la figure 3).
Le premier algorithme de localisation et cartographie si- multanées est ici un algorithme classique SLAM qui agglo- mère les scans lidar pour à la fois cartographier l'envi- ronnement dans lequel se déplace le véhicule 1 mais égale- ment 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 « yn » entre la première position courante et une position précédente.
Le deuxième algorithme reçoit en entrée le premier dépla- cement « yn » et les deuxièmes données de localisation is- sues de l'unité de mesure inertielle 6 pour fournir en sortie une position fusionnée du véhicule 1 et la trajec- toire « xn » 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étroac- tion par laquelle la position initialisant le premier al- gorithme 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 re- pè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 four- nies par exemple à l'instant t) est donc transférée au bloc de calcul « Changement de repère » qui calcule le déplace- ment 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 es- time alors, à partir de la première position courante der- nièrement calculée (instant t) et du déplacement relatif une nouvelle position qui devient la position d'initiali- sation (instant t). A partir de cette position d'initiali- sation (instant t), le premier algorithme calcule la pre- mière position courante et le premier déplacement à l'ins- tant 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 es- timé 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 Xn ~ état du véhicule où Xn est la position du véhicule dans un repère de référence, et Rn 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 con- sidère que Rn est une matrice orthogonale. D'autres quan- tité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 confor- mément à la figure 3, on utilise l'observation ou premier déplacement yn issu du premier algorithme SLAM. Comme ex- pliqué ci-dessus, par l'intermédiaire de la rétroaction, le SLAM calcule le premier déplacement yn en étant initia- lisé à la dernière position yn-T qu'il a calculée, incré- mentée par le déplacement relatif entre la position fu- sionnée fournie par le deuxième algorithme IEKF, notée Xn-T, et la dernière position calculée par le premier al- gorithme SLAM, qui correspond à yn-T . On a donc yn 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 en-T du filtre de Kalman inva- riant L-IEKF :
Or, pour un filtre de Kalman invariant L-IEKF, on peut montrer que l'erreur est indépendante de l'état estimé (°n 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 con- sulter la référence bibliographique suivante (et en parti- culier la proposition 24 de cette référence qui donne le résultat d'autonomie de exploité ici) : « Linear ob- served 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 yn se réécrit :
Dans la formule (*), en-T ne dépend pas de l'état estimé. En appliquant un raisonnement par récurrence, compte tenu qu'à l'instant Tl= 0, la première initialisation yn-T 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 ÿn 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 revendi- cations.
En particulier, le véhicule peut avoir une structure dif- fé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 naviga- tion (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).

Claims

REVENDICATIONS
1. 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 carto- graphie simultanées et un deuxième algorithme de fusion de données, le premier algorithme recevant en entrée des pre- mières données de localisation issues du lidar (5) pour calculer un premier déplacement depuis une position d'ini- tialisation, le deuxième algorithme recevant en entrée le premier déplacement et des deuxièmes données de localisa- tion issues de l'unité de mesure inertielle (6) et four- nissant en sortie une deuxième position fusionnée du véhi- cule (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 al- gorithme pour calculer une nouvelle position devenant la position d'initialisation.
2. Procédé selon la revendication 1, dans lequel le pre- mier 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'initia- lisation à partir du déplacement relatif et de la position courante dernièrement calculée.
3. Procédé selon la revendication 2, dans lequel le bloc de calcul est un bloc de calcul de changement de repère.
4. Véhicule (1) pourvu d'un lidar (5) et d'une unité de mesure inertielle (6) tous deux reliés à une unité élec- tronique 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 lo- calisation issues du lidar (5) pour calculer une première position courante et un premier déplacement entre la pre- mière position courante et une position précédente, le deuxième algorithme recevant en entrée le premier déplace- ment 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é électro- nique de navigation (2) est programmée pour mettre en œuvre le procédé selon l'une quelconque des revendications 1 à 3.
5. 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é élec- tronique 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.
6. Véhicule selon la revendication 5, formant drone aé- rien.
7. Véhicule selon la revendication 5, formant drone ter- restre.
EP24704775.6A 2023-02-21 2024-02-09 Procede de navigation a kalman etendu invariant et vehicule equipe pour sa mise en ?uvre Pending EP4669933A1 (fr)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
FR2301580A FR3145978B1 (fr) 2023-02-21 2023-02-21 Procédé de navigation à Kalman étendu invariant et véhicule équipé pour sa mise en oeuvre.
PCT/EP2024/053355 WO2024175380A1 (fr) 2023-02-21 2024-02-09 Procede de navigation a kalman etendu invariant et vehicule equipe pour sa mise en œuvre

Publications (1)

Publication Number Publication Date
EP4669933A1 true EP4669933A1 (fr) 2025-12-31

Family

ID=87036514

Family Applications (1)

Application Number Title Priority Date Filing Date
EP24704775.6A Pending EP4669933A1 (fr) 2023-02-21 2024-02-09 Procede de navigation a kalman etendu invariant et vehicule equipe pour sa mise en ?uvre

Country Status (4)

Country Link
EP (1) EP4669933A1 (fr)
CN (1) CN120731349A (fr)
FR (1) FR3145978B1 (fr)
WO (1) WO2024175380A1 (fr)

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11294397B2 (en) * 2017-02-24 2022-04-05 Teledyne Fur Detection, Inc. Control systems for unmanned aerial vehicles

Also Published As

Publication number Publication date
WO2024175380A1 (fr) 2024-08-29
FR3145978A1 (fr) 2024-08-23
CN120731349A (zh) 2025-09-30
FR3145978B1 (fr) 2025-01-31

Similar Documents

Publication Publication Date Title
EP2385347B1 (fr) Procédé de définition d'un système de navigation
EP2428934B1 (fr) Procédé d'estimation du mouvement d'un porteur par rapport à un environnement et dispositif de calcul pour système de navigation
EP3364213B1 (fr) Procede et systeme de perception contextualisee de corps materiels
EP2385346B1 (fr) Procédé d'élaboration d'une phase de navigation dans un système de navigation impliquant une corrélation de terrain
EP4436878A1 (fr) Système et procédé de navigation autonome base vision d'un satellite
FR3065297A1 (fr) Procede de suivi de cible par un drone, programme d'ordinateur, systeme electronique et drone associes
EP4002274A1 (fr) Procédé itératif d'estimation du mouvement d'un corps matériel par génération d'une grille de mouvement filtrée
EP2577231B1 (fr) Dispositif d'aide a la navigation, notamment a l'interieur de bâtiments
WO2014090697A1 (fr) Procede de pilotage d'un ensemble de robots et ensemble de robots
EP1617165A1 (fr) Procédé de guidage et/ou de pilotage d'un projectile et dispositif de guidage et/ou de pilotage mettant en oeuvre un tel procédé
FR3029672A1 (fr) Procede d'estimation du mouvement d'un porteur par rapport a un environnement et dispositif de calcul pour syteme de navigation
EP4669933A1 (fr) Procede de navigation a kalman etendu invariant et vehicule equipe pour sa mise en ?uvre
EP3333539A1 (fr) Dispositif électronique de pilotage d'un drone, drone, procédé de pilotage et programme d'ordinateur associés
EP4288847A1 (fr) Procédé et dispositif de guidage automatique d'un aéronef autonome
Liu et al. Tracking control based on gps intelligent buoy system for an autonomous underwater vehicles under measurement noise and measurement delay
WO2024121588A1 (fr) Procede de commande de vehicule a securite anti-collision amelioree
FR3094324A1 (fr) Procédé de pilotage autonome d’un organe de commande de trajectoire d’un appareil
Christensen Multi-sensor data fusion for spacecraft navigation
FR2802732A1 (fr) Dispositif d'hybridation d'un recepteur de positionnement par satellites avec une centrale inertielle
EP1840692B1 (fr) Procédé de pilotage et/ou guidage d'un projectile et dispositif de pilotage et/ou guidance mettant en oeuvre un tel procédé
EP4015332B1 (fr) Méthode de supervision pour le contrôle d'un véhicule automobile autonome
EP1785688B1 (fr) Procédé et dispositif de détermination de la vitesse de rotation d'une droite projectile-cible et dispositif de guidage d'un projectile, notamment d'une munition
EP2128731A1 (fr) Procédé de détermination de trajectoire en présence de courants variables dans le temps
WO2026032836A1 (fr) Vehicule a guidage par navigation proportionnelle sous contrainte
EP4705722A1 (fr) Calibration d'un système de fusion multi-capteurs pour la navigation inertielle

Legal Events

Date Code Title Description
STAA Information on the status of an ep patent application or granted ep patent

Free format text: STATUS: UNKNOWN

STAA Information on the status of an ep patent application or granted ep patent

Free format text: STATUS: THE INTERNATIONAL PUBLICATION HAS BEEN MADE

PUAI Public reference made under article 153(3) epc to a published international application that has entered the european phase

Free format text: ORIGINAL CODE: 0009012

STAA Information on the status of an ep patent application or granted ep patent

Free format text: STATUS: REQUEST FOR EXAMINATION WAS MADE

17P Request for examination filed

Effective date: 20250828

AK Designated contracting states

Kind code of ref document: A1

Designated state(s): AL AT BE BG CH CY CZ DE DK EE ES FI FR GB GR HR HU IE IS IT LI LT LU LV MC ME MK MT NL NO PL PT RO RS SE SI SK SM TR