Navigation method adopting invariant extended kalman filtering and vehicle suitable for implementing method

Abstract

The invention relates to a method for navigating a vehicle (1) equipped with a lidar (5) and an inertial measurement unit (6) both connected to an electronic navigation unit (2) implementing a first, synchronous positioning and mapping algorithm and a second, data fusion algorithm; the first algorithm receives, as input, first positioning data originating from the lidar (5) in order to calculate a first displacement from an initial position; a second algorithm receiving the first displacement and second positioning data originating from the inertial measurement unit (6) as inputs and providing a second, fused position of the vehicle (1) as an output; characterized in that the second algorithm implements invariant extended Kalman filtering and the second, fused position is introduced into a feedback loop which provides the second, fused position as input to the first algorithm in order to calculate a new position, which is the initial position.

Description

[0001] The present invention relates to the field of vehicle navigation. Background Art

[0002] Vehicles equipped with lidar (LiDAR) sensors connected to an electronic navigation unit are known in the prior art. It should be noted that a lidar is a device typically comprising a laser emitter and a photodetector, both mounted on a azimuthally controllable support. The laser emitter is used to emit laser pulses incident at a predetermined azimuth angle, while the photodetector is used to detect laser pulses reflected by obstacles surrounding the vehicle. The position of obstacles relative to the laser emitter / photodetector assembly can be determined based on the azimuth angle and the time measurement between the emission of the laser pulse and the detection of the reflected laser pulse. As the laser emitter / photodetector assembly scans the vehicle's surroundings, an image of the vehicle's surroundings (commonly referred to as a lidar scan) is obtained, which includes the coordinates of all points of reflection of the laser pulses. The electronic navigation unit preferably implements a simultaneous localization and mapping algorithm (commonly referred to as SLAM), which detects the presence of identical points of interest in consecutive lidar scans, aggregates the lidar scans to map the environment in which the vehicle is traveling, and further determines the trajectory followed by the vehicle within the mapped environment.

[0003] Vehicles equipped with an inertial measurement unit connected to an electronic navigation unit are also known in the prior art. It should be noted that an inertial measurement unit (IMU) comprises three acceleration sensors and three angle sensors, wherein the acceleration sensors are arranged along the axes of an acceleration measurement coordinate system and the angle sensors can be gyroscopes or rate gyroscopes, which are used to measure the angular movement of the acceleration measurement coordinate system relative to the reference orientation of the measurement coordinate system. In the acceleration measurement coordinate system, the acceleration sensors exposed to gravity and the acceleration of the vehicle measure a parameter called specific force or gravitational acceleration and determine the three components of the specific force vector. The signals (acceleration and angle data) of the inertial measurement unit sensors are used by an inertial navigation algorithm to determine the position of the vehicle in a local geographic coordinate system based on the signals.

[0004] Vehicles equipped with a lidar and an inertial measurement unit, both connected to an electronic navigation unit, are also known in the prior art. Between two lidar scans, the intermediate position can be calculated solely based on positioning data (acceleration, angular velocity), which are available more frequently than the lidar scans.

[0005] Figure 1A first possible architecture of an electronic navigation unit combining lidar data with inertial data is shown. The electronic navigation unit implements a first algorithm - a simultaneous localization and mapping ("SLAM") algorithm and a second algorithm - a data fusion ("Merging") algorithm. The first algorithm receives as input first positioning data from the lidar ("lidar scan") in order to calculate a first current position and a first displacement "y" between the first current position and an initial position. n The second algorithm receives the first displacement "y n ” and the second positioning data (“acceleration, angular velocity”) from the inertial measurement unit as input in order to output a method for determining the vehicle trajectory (“trajectory x n ”) at the fusion site.

[0006] For the first architecture, the fusion of lidar positioning data and inertial measurement unit positioning data is called loose fusion because the second fusion algorithm does not directly process the lidar scan image, but processes the displacement determined based on the lidar scan image. This loose fusion consumes less computational resources and memory, but is sensitive to sudden movements of the vehicle, which may cause points of interest to be blurred or lost, thus affecting navigation accuracy.

[0007] To address these deficiencies, a second architecture has been designed (e.g. Figure 2 As shown in Figure 2), it closely integrates the positioning data of the lidar with the positioning data of the inertial measurement unit. At this time, the electronic navigation unit implements a data fusion algorithm that directly receives the first positioning data from the lidar ("lidar scan image") and the second positioning data from the inertial measurement unit ("acceleration, angular velocity") as input, and outputs the data used to determine the vehicle trajectory ("trajectory x n The data fusion algorithm uses an iterative Kalman filter that fuses the state of the vehicle (corresponding to the second positioning data) with the coordinates of the laser pulse reflection point (corresponding to the first positioning data) in a high-dimensional vector.

[0008] Tight fusion can significantly improve accuracy, but it consumes significant computational resources and memory. Furthermore, performing the computations requires significant energy, which must be drawn from the vehicle's stored energy. When the vehicle is a lightweight drone (typically weighing only a few kilograms), its computational resources, memory resources, and energy storage capacity are insufficient to support this type of fusion.

[0009] It is also envisaged to equip vehicles with additional sensors to improve navigation accuracy, but this will increase installation costs, reduce carrying capacity and increase battery consumption requirements; this solution is not feasible for vehicles with small overall size and low carrying capacity (such as light drones).

[0010] Purpose of the Invention

[0011] The purpose of the present invention is to improve the navigation accuracy of vehicles that use lidar and inertial measurement units as their only navigation sensors. Summary of the Invention

[0012] To this end, according to the present invention, a method for navigating a vehicle is provided, the vehicle being equipped with a lidar and an inertial measurement unit, both of which are connected to an electronic navigation unit that implements a first algorithm, a simultaneous positioning and mapping algorithm, and a second algorithm, a data fusion algorithm. The first algorithm receives as input first positioning data from the lidar in order to calculate a first displacement from an initial position; the second algorithm receives as input first displacement and second positioning data from the inertial measurement unit and outputs a second, fused position of the vehicle. The second algorithm implements an invariant extended Kalman filter, and the second, fused position is introduced into a feedback loop that provides the second, fused position as input to the first algorithm in order to calculate a new position, which becomes the initial position.

[0013] Therefore, the method of the present invention introduces feedback from the fusion algorithm into the simultaneous positioning and mapping algorithm. Since the second positioning data of the inertial measurement unit is used to account for sudden changes in direction, the method is more robust to sudden changes in direction. The fusion performed by the method of the present invention thus combines the advantages of loose fusion (low computational cost) with higher accuracy and robustness. In addition, unlike the standard extended Kalman filter (EKF), the advantage of the invariant extended Kalman filter (IEKF) is that its error does not depend on the state estimated by the filter, which means that there is no risk of cascading errors in the feedback.

[0014] The method further includes all or part of the following features (which may exist alone or in combination):

[0015] - the first algorithm determines the first displacement based on the initial position and the most recently calculated current position, wherein the feedback loop includes a calculation module for calculating the relative displacement between the fused position and the most recently calculated current position; and wherein the first algorithm estimates a new position based on the relative displacement and the most recently calculated current position, the new position becoming the initial position.

[0016] -The calculation module is a coordinate system conversion calculation module.

[0017] The invention also relates to a vehicle suitable for carrying out the method.

[0018] Other characteristics and advantages of the invention will become clear on reading the following description of particular and non-limiting embodiments of the invention.

[0019] BRIEF DESCRIPTION OF THE DRAWINGS

[0020] With reference to the accompanying drawings, in which:

[0021] [ Figure 1 ] Figure 1 is a diagram of the first known architecture for fusing lidar data with inertial measurement unit data;

[0022] [ Figure 2 ] Figure 2 is a diagram of a second known architecture for fusing lidar data with inertial measurement unit data;

[0023] [ Figure 3 ] Figure 3 is a schematic diagram of the architecture for fusing lidar data and inertial measurement unit data of the present invention;

[0024] [ Figure 4 ] Figure 4 is a schematic diagram of a vehicle suitable for performing the method of the present invention.

[0025] DETAILED DESCRIPTION OF THE INVENTION

[0026] Herein, the present invention is described by taking a light UAV-type vehicle as an example, such as the aerial UAV in this embodiment.

[0027] refer to Figure 4 Such a vehicle, generally indicated by reference numeral 1 , comprises in a known manner a fuselage carrying an electronic control unit 2 , propulsion means 3 , steering means 4 , a lidar 5 and an inertial measurement unit 6 .

[0028] The propulsion means 3 comprise, for example, one or more propeller-driven rotors, by means of which the vehicle 1 is able to move in take-off, hover and fly.

[0029] The steering means 4 comprise, for example, actuators for adjusting the orientation of one or more rotors, or actuators for adjusting the orientation of one or more control surfaces, which are used to guide the vehicle 1 in flight and to change its flight path.

[0030] The laser radar 5 includes a laser emitter and a photodetector, both of which are mounted on a bracket whose orientation can be controlled. The laser emitter is used to emit laser pulses incident at a predetermined azimuth angle, while the photodetector is used to detect laser pulses reflected by obstacles around the vehicle 1. The laser emitter / photodetector assembly scans the vehicle's surroundings (360° scanning or smaller angle scanning, such as 180° scanning in front of the vehicle's direction of travel), so the signal provided by the laser radar 5 includes the coordinates of all laser pulse reflection points, and these coordinates constitute an image of the vehicle's surroundings (commonly referred to as a laser radar scan image). These coordinates constitute the first positioning data ( Figure 3 (The figure shows a “LiDAR scan image” in FIG. 5 ). The LiDAR 5 has a sampling frequency that depends on the scanning rate and provides first positioning data at this frequency.

[0031] The inertial measurement unit 6 includes three acceleration sensors arranged along the axes of the acceleration measurement coordinate system for measuring the specific force in the acceleration measurement coordinate system; and three angle sensors (gyroscopes or rate gyroscopes) for measuring the angular motion of the acceleration measurement coordinate system relative to the reference orientation of the measurement coordinate system. The signal provided by the inertial measurement unit 6 includes acceleration and angle data, which constitute the second positioning data ( Figure 3 The "Inertial (IMU)" in the figure has a sampling frequency that is higher than the frequency at which the first positioning data representing the lidar scan image is provided.

[0032] The propulsion component 3, the steering component 4, the laser radar 5 and the inertial measurement unit 6 are known components themselves and will not be described in detail herein.

[0033] The propulsion component 3 , the steering component 4 , the laser radar 5 and the inertial measurement unit 6 are all connected to the electronic control unit 2 .

[0034] The electronic control unit 2 includes a processor and a memory, wherein the memory contains a computer program executable by the processor to perform the method of the present invention to perform navigation and control the propulsion component 3 and the steering component 4 to autonomously follow the navigation.

[0035] The computer program includes a first algorithm, a simultaneous positioning and mapping algorithm ( Figure 3 "SLAM") and the second algorithm, the data fusion algorithm ( Figure 3 "Fusion via IEKF" in

[15] ).

[0036] In this article, the first simultaneous localization and mapping algorithm is a conventional SLAM algorithm, which detects the existence of the same points of interest in consecutive lidar scans, aggregates the lidar scans to draw a map of the environment in which the vehicle 1 is traveling, and determines the trajectory followed by the vehicle 1 in the mapped environment.

[0037] The first algorithm receives as a first input the first positioning data from the laser radar 5 in order to calculate a first current position and a first displacement "y" between the first current position and the previous position. n ”.

[0038] The second algorithm receives the first displacement "y n and the second positioning data from the inertial measurement unit 6 as input to output the fused position of the vehicle 1 and its trajectory “x n The second algorithm implements the Invariant Extended Kalman Filter (IEKF).

[0039] The program also implements a feedback loop by which the initial position of the first algorithm is calculated based on the fused position and the first current position most recently calculated by the first algorithm. More specifically, the feedback loop includes a "coordinate system transformation" calculation module that receives as input the output of the second algorithm and outputs a second input for the first algorithm. The fused position (e.g., from data provided at time t) is thus transmitted to the "coordinate system transformation" calculation module, which calculates the relative displacement between the fused position (at time t) and the first current position most recently calculated by the first algorithm (also from data provided at time t). This relative displacement is then sent as input to the first algorithm. The first algorithm estimates a new position based on the most recently calculated first current position (at time t) and the relative displacement, which becomes the initial position (at time t). Based on this initial position (at time t), the first algorithm calculates the first current position and first displacement at the current moment (from data provided at time t+1), which are used by the second algorithm to estimate the fused position (at time t+1).

[0040] It will be appreciated that the advantage of using an invariant extended Kalman filter upstream of the feedback loop is that it avoids self-feeding of errors, as the error of such a filter is independent of the estimated state, which means that the error of the filter is not fed into the first algorithm, which in turn provides input to the second algorithm.

[0041] In fact, remember n =(R n ,x n ) as the state of the means of transport, where x n is the position of the vehicle in the coordinate system, R n is the transformation matrix from this coordinate system to the vehicle coordinate system (usually called orientation). In this paper, R n is considered as an orthogonal matrix. Other quantities may be added to this state vector, such as the bias or velocity of the inertial measurement unit, but this is not necessary for explaining the invention.

[0042] To adjust Figure 3The filter of the second algorithm IEKF shown uses the observation value or first displacement y from the first algorithm SLAM n As explained above, using feedback, SLAM calculates the last position y n-τ Plus the fused position provided by the second algorithm IEKF (denoted as ) and the final position calculated by the first algorithm SLAM (corresponding to y n-τ ) to initialize the relative displacement between them to calculate the first displacement y n This yields y n , which can be written as its initialization The function expression is Right now:

[0043]

[0044] In this formula, is the orientation determined by the second algorithm IEKF, and Denotes its transposed matrix. In this paper, the observation value is equal to the position (considering measurement uncertainty), so the Kalman filter implemented in the second algorithm is the left-invariant extended Kalman filter (L-IEKF). Since the observation value is equal to the position (considering measurement uncertainty), this gives y n-τ ≈x n-τ , and therefore:

[0045]

[0046] The error e of the invariant Kalman filter L-IEKF n-τ Can be identified as:

[0047]

[0048] However, for the invariant Kalman filter L-IEKF, it can be shown that the error e n-τ With the estimated state This result has been demonstrated in the academic literature on the invariant Kalman filter. Specifically, see the following literature (especially Proposition 24, which gives the e used in this paper). n-τ Autonomy results): Linear observed systems on groups, Axel Barrau and Silvère Bonnabel, Systems & Control Letters, vol. 129, pp. 36–42, 2019.

[0049] Therefore, for calculating the first displacement y nInitialization of the first SLAM algorithm Can be rewritten as:

[0050]

[0051] In formula (*), e n-τ By applying mathematical induction, considering that at time n=0, the first initialization y of the first algorithm SLAM n-τ Independent of the state estimated by fusion (because fusion has not yet begun), the initial position of SLAM is It never depends on the estimated state, thus avoiding cascading errors by employing an invariant extended Kalman filter.

[0052] It goes without saying that the invention is not limited to the embodiments described, but covers any variant coming within the ambit of the invention as defined by the claims.

[0053] In particular, the structure of the vehicle may be different from that described. It may be an airborne, ground, water or amphibious vehicle, and may be manned or unmanned.

[0054] The laser radar used herein is a 3D laser radar, but a 2D laser radar may also be used. When the present invention is applied to a ground or water vehicle, the use of a 2D laser radar may be particularly suitable.

[0055] “Linked to…” means that the electronic control unit 2 can integrate the electronic navigation unit (the two electronic units are implemented on the same electronic circuit or integrated circuit) or be connected to the electronic navigation unit (the two electronic units are implemented on different electronic circuits or integrated circuits).

Claims

1. A method for navigating a vehicle (1), the vehicle being equipped with a laser radar (5) and an inertial measurement unit (6), the laser radar (5) and the inertial measurement unit (6) both being connected to an electronic navigation unit (2), the electronic navigation unit implementing a first algorithm - a simultaneous positioning and mapping algorithm and a second algorithm - a data fusion algorithm; the first algorithm receiving as input first positioning data from the laser radar (5) in order to calculate a first displacement from an initial position; the second algorithm receiving as input the first displacement and second positioning data from the inertial measurement unit (6) and outputting a second, fused position of the vehicle (1); characterised in that The second algorithm implements an invariant extended Kalman filter, and the second, fused position is introduced into a feedback loop that provides the second, fused position as input to the first algorithm to calculate a new position, which becomes the initial position.

2. The method according to claim 1, wherein The first algorithm determines the first displacement based on the initial position and a most recently calculated current position, wherein the feedback loop includes a calculation module for calculating a relative displacement between the fused position and the most recently calculated current position; and wherein the first algorithm estimates the new position based on the relative displacement and the most recently calculated current position, the new position becoming the initial position.

3. The method according to claim 2, wherein The calculation module is a coordinate system conversion calculation module.

4. A vehicle (1) equipped with a lidar (5) and an inertial measurement unit (6), both of which are connected to an electronic navigation unit (2), which implements a first algorithm - a simultaneous positioning and mapping algorithm and a second algorithm - a data fusion algorithm; the first algorithm receives first positioning data from the lidar (5) as input in order to calculate a first current position and a first displacement between the first current position and a previous position; the second algorithm receives the first displacement and second positioning data from the inertial measurement unit (6) as input and outputs the fused position of the vehicle (1); the electronic navigation unit (2) is programmed to perform the method as described in any one of claims 1 to 3.

5. The vehicle according to claim 4, wherein: The vehicle forms an unmanned machine comprising an electronic control unit linked to the electronic navigation unit, propulsion means and steering means, both of which are connected to the electronic control unit, the electronic control unit being programmed to control the propulsion means and the steering means so as to autonomously follow a trajectory based on the fused position.

6. The vehicle according to claim 5, wherein: The vehicle forms an aerial drone.

7. The vehicle according to claim 5, wherein: The vehicle forms a ground unmanned vehicle.

Images