Navigation method with invariant extended kalman filter and vehicle equipped to implement same
Patent Information
- Application Number
- EP2024704775
- Authority / Receiving Office
- EP · EP
- Patent Type
- Applications
- Current Assignee / Owner
- Priority Date
- 2023-02-21
- Filing Date
- 2024-02-09
- Publication Date
- 2025-12-31
AI Technical Summary
Vehicles equipped with lidar and inertial measurement units face challenges in navigation precision due to sensitivity to sudden movements, which existing loose fusion methods cannot adequately address without increasing computational and energy demands, especially in lightweight drones where resources are limited.
Implementing a navigation method that uses invariant extended Kalman filtering with a feedback loop to merge lidar and inertial data, allowing for robust and precise trajectory determination while avoiding cascading errors by keeping errors independent of the estimated state.
This approach enhances navigation precision and robustness, balancing the computational efficiency of loose fusion with the precision of tight fusion, while preventing errors from self-propagation, suitable for lightweight vehicles like drones.
Smart Images
Figure EP2024053355_29082024_PF_FP_ABST
Abstract
Description
[0001] INVARIANT EXTENDED KALMAN NAVIGATION METHOD AND VEHICLE EQUIPPED FOR ITS IMPLEMENTATION
[0002] The present invention relates to the field of vehicle navigation.
[0003] BACKGROUND OF THE INVENTION
[0004] Vehicles are known to be equipped with a lidar connected to an electronic navigation unit. It is recalled that a lidar is a device most often comprising a laser transmitter and a photodetector which 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. As the laser transmitter / photodetector assembly scans the vehicle's environment, an image (commonly called a lidar scan) of the vehicle's environment 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.
[0005] Vehicles are also known to be equipped with an inertial measurement unit connected to an electronic navigation unit. It is recalled that an inertial measurement unit (IMU) comprises three accelerometric sensors arranged along the axes of an accelerometric measurement frame and three angular sensors, gyroscopes or gyrometers, to measure angular movements of the accelerometric measurement frame relative to a reference orientation of the measurement frame. The accelerometric sensors, subject to gravity and the accelerations of the vehicle, measure, in the accelerometric measurement frame, a quantity called specific force (or "g-force" in English) and determine the three components of a specific force vector.The signals from the inertial measurement unit sensors (acceleration and angle data) are used by an inertial navigation algorithm to determine a position of the vehicle in a local geographical reference point.
[0006] Vehicles are also known to be equipped with a lidar and an inertial measurement unit, both connected to an electronic navigation unit. It is then possible to calculate intermediate positions between two lidar scans using only the location data "acceleration, angular velocity" which are available at a higher frequency than that of the lidar scans.
[0007] 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 location data “acceleration, angular velocity” from the inertial measurement unit to provide as output a fused position of the vehicle allowing the trajectory “trajectory x” to be determined n » of this one.
[0008] 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 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 accuracy of navigation.
[0009] To overcome these drawbacks, a second architecture, represented in Figure 2, was devised, performing a tight fusion between the location data from the lidar and the location data from the inertial measurement unit. 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 it. The data fusion algorithm implements an iterated Kalman filtering which merges, in a high-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.
[0010] Tight fusion provides a significant gain in accuracy but is very demanding in terms of computing resources and memory. In addition, performing the calculations requires significant energy, which is taken from the energy stored in the vehicle. When the vehicle is a light drone (typically a few kilograms at most), the computing and memory resources, as well as the energy storage capacity, are too low for this type of fusion.
[0011] It would also be possible to equip the vehicle with additional sensors to improve navigation accuracy, but this increases the installation cost, reduces the carrying capacity, and increases the consumption requirements of the batteries: this is not feasible on a vehicle with a small footprint and a low carrying capacity such as a light drone.
[0012] SUBJECT OF THE INVENTION
[0013] The invention aims to improve the navigation accuracy of a vehicle carrying a lidar and an inertial measurement unit as the only navigation sensors.
[0014] SUMMARY OF THE INVENTION
[0015] To this end, 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 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.Thus, 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 (light computational cost) but benefits from greater precision and robustness. In addition, unlike a standard extended Kalman filter (or EKF), 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.
[0016] The method also comprises all or part of the following characteristics, alone or in combination:
[0017] - the first algorithm determines the first displacement from the initialization position and a last calculated current position, wherein 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;
[0018] - the calculation block is a reference change calculation block.
[0019] The invention also relates to a vehicle equipped for implementing this method.
[0020] Other characteristics and advantages of the invention will emerge from reading the following description of a particular and non-limiting embodiment of the invention. BRIEF DESCRIPTION OF THE DRAWINGS
[0021] Reference will be made to the attached drawings, including:
[0022] [Fig. 1] Figure 1 is a representation of a first known architecture for merging data from a lidar and data from an inertial measurement unit;
[0023] [Fig. 2] Figure 2 is a representation of a second known architecture for merging data from a lidar and data from an inertial measurement unit;
[0024] [Fig. 3] Figure 3 is a representation of the architecture of the invention for merging data from a lidar and data from an inertial measurement unit;
[0025] [Fig. 4] Figure 4 is a schematic view of a vehicle equipped to implement the method of the invention.
[0026] DETAILED DESCRIPTION OF THE INVENTION
[0027] The invention is described here in application to a light drone type vehicle, such as an aerial drone.
[0028] With reference to figure 4, 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.
[0029] The propulsion member 3 comprises, for example, one or more motorized propeller rotors by which the vehicle 1 can take off, remain in stationary flight and move in flight.
[0030] 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 making it possible to steer the vehicle 1 in flight and modify its flight trajectory.
[0031] The lidar 5 comprises a laser transmitter and a photodetector which are mounted on a bearing-controllable support. 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 1. The laser transmitter / photodetector assembly scans the environment of the vehicle (over 360° or a smaller angle, for example 180° forward with reference to a direction of travel of the vehicle 1) so that the signals provided by the lidar 5 include the coordinates of all the reflection points of the laser pulses which form an image (commonly called a lidar scan) of the environment of the vehicle 1. These coordinates constitute first location data (“lidar scans” in FIG. 3). The lidar
[0032] 5 has a sampling rate that depends on the scanning speed and provides the first location data at this rate.
[0033] 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. The signals from the inertial measurement unit
[0034] 6 contain acceleration and angle data forming second location data (“inertia (IMU)” in Figure 3) and are provided at a sampling rate higher than the rate of provision of the first location data representative of a lidar scan.
[0035] The propulsion unit 3, the steering unit 4, the lidar 5 and the inertial measurement unit 6 are known in themselves and will not be described further here.
[0036] 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.
[0037] 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.
[0038] The computer program includes a first simultaneous localization and mapping algorithm (“SLAM” in Figure 3) and a second data fusion algorithm (“Fusion by IEKF” in Figure 3).
[0039] 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 vehicle 1 is moving but also to determine the trajectory followed by vehicle 1 in the environment thus mapped by detecting in the successive lidar scans the presence of the same points of interest.
[0040] The first algorithm receives as a first input the first location data from the lidar 5 to calculate a first current position and a first displacement “y n » between the current first position and a previous position.
[0041] 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» of it. The second algorithm implements invariant extended Kalman filtering (or IEKF).
[0042] 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 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 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).
[0043] We understand that the interest of using an invariant extended Kalman filtering upstream of the feedback loop is that it avoids self-feeding of errors since the errors of such filtering are independent of the estimated state so that the errors of this filtering do not feed the first algorithm which itself feeds the second algorithm.
[0044] Indeed, let us note Xn ~ vehicle condition where X n is the position of the vehicle in a reference frame, and R n the matrix of passage from the reference frame to the vehicle frame, commonly called the orientation. Here, we consider that R n is an orthogonal matrix. Other quantities can be added to this state vector, such as inertial measurement unit biases or velocity, but this is not necessary to explain the invention.
[0045] To recalibrate the filter of the second IEKF algorithm according to Figure 3, we use the observation or first displacement y n from the first SLAM algorithm. As explained above, through feedback, SLAM calculates the first displacement y n being initialized at the last position y n-T that he calculated, incremented by the relative displacement between the merged position provided by the second IEKF algorithm, noted X n-T, and the last position calculated by the first SLAM algorithm, which corresponds to y n-T . So we have there n which can be written as a function of its initialization with the expression of which is:
[0046] In this formula, is the orientation determined by the second IEKF algorithm, and denotes its matrix transposed. Here, the observation is equal to the position (up to measurement uncertainties), the Kalman filtering implemented in the second algorithm is therefore a so-called left-invariant extended Kalman filter (L-IEKF: "left-invariant extended Kalman filter"). Since the observation is equal to the position (up to measurement uncertainties), we have and therefore:
[0047] We then recognize the error e n-T of the invariant Kalman filter L-IEKF:
[0048] Now, for an invariant Kalman filter L-IEKF, it can be shown that the error is independent of the estimated state (° n also speaks of “autonomous error”): This result has been shown in the academic literature on the invariant Kalman filter. We can notably consult the following bibliographic reference (and in particular proposition 24 of this reference which gives the autonomy result of exploited here): “Linear ob- served systems on groups”, by Axel Barrau and Silvère Bonnabel, published in 2019 in “Systems & Control Letters”, volume 129, pages 36-42.
[0049] Thus, the initialization of the first SLAM algorithm for calculate the first displacement y n is rewritten:
[0050] In the formula (*), e n-T does not depend on the estimated state. By applying recursive reasoning, given that at time Tl= 0, the first initialization yn-T of the first SLAM algorithm does not depend on the state estimated by the fusion (since the fusion has not yet started), we see with the formula (*) that the initialization position ÿ n SLAM never depends on the estimated state, which avoids cascading errors thanks to the use of the invariant extended Kalman filter.
[0051] Of course, the invention is not limited to the embodiment described but encompasses any variant falling within the scope of the invention as defined by the claims.
[0052] In particular, 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.
[0053] 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 nautical vehicle.
[0054] By "associated with", we mean that 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).
Claims
CLAIMS 1. Method for navigating a vehicle (1) provided with a lidar (5) and an inertial measurement unit (6) both connected to an electronic navigation unit (2) implementing a first simultaneous localization and mapping algorithm and a second data fusion algorithm, the first algorithm receiving as input first localization data from the lidar (5) to calculate a first displacement from an initialization position, the second algorithm receiving as input the first displacement and second localization data from the inertial measurement unit (6) and providing as output a second merged position of the vehicle (1),characterized in that the second algorithm implements 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., 2. The method of claim 1, wherein the first algorithm determines the first displacement from the initialization position and a last calculated current position, wherein 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.
3. The method of claim 2, wherein the block calculation block is a reference change calculation block.
4. Vehicle (1) provided with a lidar (5) and an inertial measurement unit (6) both connected to an electronic navigation unit (2) implementing a first simultaneous localization and mapping algorithm and a second data fusion algorithm, the first algorithm receiving as input first localization data from the lidar (5) to calculate a first current position and a first displacement between the first current position and a previous position, the second algorithm receiving as input the first displacement and second localization data from the inertial measurement unit (6) and providing as output a merged position of the vehicle (1), the electronic navigation unit (2) is programmed to implement the method according to any one of claims 1 to 3.
5. Vehicle according to claim 4, forming a drone, comprising an electronic control unit associated with the electronic navigation unit, a propulsion member and a steering member both connected to the electronic control unit which is programmed to control the propulsion member and the steering member from the merged positions to follow a trajectory autonomously.
6. Vehicle according to claim 5, forming an aerial drone.
7. Vehicle according to claim 5, forming a terrestrial drone.