Invariant extended Kalman navigation method and vehicle equipped for its implementation.
A navigation process using a feedback loop with an invariant extended Kalman filter merges LIDAR and inertial data to enhance precision and robustness in lightweight vehicles, addressing inaccuracies and resource constraints.
Patent Information
- Application Number
- FR2023001580
- Authority / Receiving Office
- FR · FR
- Patent Type
- Patents
- Current Assignee / Owner
- Filing Date
- 2023-02-21
- Publication Date
- 2025-01-31
- Estimated Expiration
- 2043-02-21
AI Technical Summary
Existing navigation systems in vehicles using LIDAR and inertial measurement units are prone to inaccuracies due to sudden movements, especially in lightweight drones, which require significant calculation, memory, and energy resources, and additional sensors increase cost and consumption.
Implementing a navigation process with a feedback loop that integrates an invariant extended Kalman filter (IEKF) to merge LIDAR and inertial data, ensuring robustness and precision without relying on estimated states, thus avoiding cascade errors.
The process achieves precise and robust navigation with reduced computational and energy demands, maintaining accuracy even with sudden vehicle movements, while avoiding errors that propagate through the system.
Smart Images
Figure 00000011_0000 
Figure 00000011_0001 
Figure 00000011_0002
Abstract
Description
Title of the invention: Invariant extended Kalman navigation method and vehicle equipped for its implementation.
[0001] The present invention relates to the field of vehicle navigation.
[0002] BACKGROUND OF THE INVENTION
[0003] Vehicles are known that are 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 environment of the vehicle, 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.
[0004] Vehicles are also known that are provided 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, for measuring 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 (g-force) 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 from them a position of the vehicle in a local geographical reference.
[0005] Vehicles are also known which are equipped with a lidar and a measuring unit. inertial, 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.
[0006] [Fig.l] 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 “yn” between the first current position and an initialization position. The second algorithm receives as input the first displacement “yn” and second localization data “acceleration, angular velocity” from the inertial measurement unit to provide as output a merged position of the vehicle making it possible to determine the trajectory “trajectory xn” of the latter.
[0007] This first architecture is referred to as loose fusion between the location data of the lidar and the location data of the inertial measurement unit because the second fusion algorithm does not directly process the lidar scans but a movement 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 impair the precision of the navigation.
[0008] To overcome these drawbacks, a second architecture, represented in [Fig. 2], has been devised, achieving 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 xn” of the latter.The data fusion algorithm implements an iterated Kalman filtering that merges, in a high-dimensional vector, a vehicle state corresponding to the second location data with the coordinates of the reflection points of the laser pulses corresponding to the first location data.
[0009] Tight fusion provides a significant gain in precision but is very demanding in terms of computing resources and memory. In addition, performing the calculations requires significant energy which is taken from that 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.
[0010] It would also have been possible to imagine equipping the vehicle with additional sensors to improve the precision of navigation, but this increases the cost of installation, reduces the carrying capacity, and increases the consumption requirements at the battery level: this is not feasible on a vehicle with a small footprint and a low carrying capacity such as a light drone.
[0011] SUBJECT OF THE INVENTION
[0012] The invention aims to improve the navigation accuracy of a vehicle carrying a lidar and an inertial measurement unit as the only navigation sensors. Summary of the invention
[0013] 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.
[0014] 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. Furthermore, 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.
[0015] The method also comprises all or part of the following features, alone or in combination: - 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; - the calculation block is a reference change calculation block.
[0016] The invention also relates to a vehicle equipped for implementing this method.
[0017] 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
[0018] Reference will be made to the accompanying drawings, among which:
[0019] [Fig-1] [Fig.l] is a representation of a first known architecture for merge data from a lidar and data from an inertial measurement unit;
[0020] [Fig.2] [Fig.2] is a representation of a second architecture known for merge data from a lidar and data from an inertial measurement unit;
[0021] [Fig.3] [Fig.3] is a representation of the architecture of the invention for merging data from a lidar and data from an inertial measurement unit;
[0022] [Fig.4] [Fig.4] is a schematic view of a vehicle equipped to implement the method of the invention. DETAILED DESCRIPTION OF THE INVENTION
[0023] The invention is described here in application to a light drone type vehicle, such as here an aerial drone.
[0024] With reference to [Fig. 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.
[0025] 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.
[0026] 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 vehicle 1 to be steered in flight and its flight path to be modified.
[0027] 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° on the front 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 5 has a sampling frequency which depends on the scanning speed and provides the first location data at this frequency.
[0028] The inertial measurement unit 6 comprises three accelerometric sensors arranged along the axes of an accelerometric measurement frame to measure a specific force in the 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 signals from the inertial measurement unit 6 contain acceleration and angle data forming second location data ("inertia (IMU)" in [Fig. 3]) and are provided at a sampling frequency higher than the frequency of providing the first location data representative of a lidar scan.
[0029] 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.
[0030] 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.
[0031] 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.
[0032] The computer program comprises a first simultaneous localization and mapping algorithm (“SLAM” in [Fig.3]) and a second data fusion algorithm (“Fusion by IEKF” in [Fig.3]).
[0033] 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 successive lidar scans the presence of the same points of interest.
[0034] 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 “yn” between the first current position and a previous position.
[0035] The second algorithm receives as input the first displacement “yn” 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 “xn” thereof. The second algorithm implements invariant extended Kalman filtering (or IEKF).
[0036] 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 comprises 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).
[0037] It is understood that the advantage of using 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.
[0038] Indeed, let us denote %n = ( R,„ x„ ) the state of the vehicle where xn is the position of the vehicle in a reference frame, and Rn the matrix of passage from the reference frame to the vehicle frame, commonly called the orientation. Here, we consider that Rn is an orthogonal matrix. Other quantities can be added to this state vector, such as biases of the inertial measurement unit or the speed, but this is not necessary to explain the invention.
[0039] To recalibrate the filter of the second IEKF algorithm in accordance with figure 3, we use the observation or first displacement yn from the first SLAM algorithm.
[0040]
[0041]
[0042]
[0043]
[0044]
[0045]
[0046]
[0047]
[0048]
[0049] As explained above, through feedback, the SLAM calculates the first displacement yn by being initialized at the last position y^ that it calculated, in-cremented by the relative displacement between the merged position provided by the second IEKF algorithm, noted xn-T, and the last position calculated by the first SLAM algorithm, which corresponds to yiM-. We therefore have yn which can be written as a function of its initialization yn, with the expression of yn which is: ^ = yn-r+^(^-yn-r) In this formula, is the orientation determined by the second IEKF algorithm, and L7 denotes its transpose matrix. 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 yn-T ~ and therefore: A T yn yn-r ^nr ( “ ynT ) We then recognize the error in-r of the invariant Kalman filter L-IEKF: — ^nr ( ^nT - yn-r / Now, for an invariant Kalman filter L-IEKF, we can show that the error Cr-r is independent of the estimated state £ — ^nT $ ) (we also speak of "error autonomous"): 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 en-r, used here): "Linear observed Systems on groups", by Axel Barrau and Silvère Bonnabel, published in 2019 in "Systems & Control Letters", volume 129, pages 36-42. Thus, the initialization yn of the first SLAM algorithm to calculate the first displacement yn is rewritten: y„=yn.re^ (*) In formula (*), en-T does not depend on the estimated state. Applying recursive reasoning, considering that at time n - 0, the first initialization 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 formula (*) that the initialization position yn of the SLAM never depends on the estimated state, which avoids cascading errors thanks to the use of the invariant extended Kalman filter. 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 re- sales.
[0050] 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.
[0051] 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.
[0052] By “associated with”, it is meant 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 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
5. to 3. 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.
7. Vehicle according to claim 5, forming an aerial drone. Vehicle according to claim 5, forming a terrestrial drone.