Improved method for simultaneous localization and mapping; Associated computer system and program.
The method integrates Doppler radar and inertial data to autonomously map and locate vehicles, addressing the challenge of creating accurate maps and vehicle positioning without satellite systems, achieving precise navigation in various conditions.
Patent Information
- Authority / Receiving Office
- FR · FR
- Patent Type
- Patents
- Current Assignee / Owner
- THALES SA
- Filing Date
- 2024-06-04
- Publication Date
- 2026-06-12
Smart Images
Figure 00000013_0000 
Figure 00000014_0000 
Figure 00000015_0000
Abstract
Description
Title of the invention: Improved method for simultaneous localization and mapping; Associated computer system and program.
[0001] The invention relates to the field of autonomous navigation of a vehicle, whether it be an aircraft, an automobile, a robot, etc.
[0002] More specifically, the invention relates to methods, implemented by computer in real time, for executing a simultaneous localization and mapping algorithm. These algorithms are often referred to by the acronym SLAM, from the English "Simultaneous Localization And Mapping". The term CML algorithm, for "Concurrent Mapping and Localization", is also used.
[0003] For an autonomous vehicle, a SLAM algorithm consists of building or improving a map of the environment in which the vehicle moves and simultaneously locating the vehicle in this map.
[0004] Mapping corresponds to a virtual reconstruction of the environment, two or three-dimensional, from points detected in the environment by means of a detection device on board the vehicle, such as a Lidar, one or more camera(s), or the equivalent.
[0005] Since the detection device is on-board, a vehicle displacement model is used to reposition the detected points from a moving reference frame attached to the vehicle to a ground reference frame, which is a fixed reference frame attached to the environment. This model can advantageously be informed by the outputs of a positioning device on-board the vehicle, such as an inertial measurement unit, one or more odometry sensors, or the equivalent.
[0006] The correlations that can be established between the positions of the points detected at the current time and those of the points detected in the past (taking into account the vehicle's displacement given by the displacement model) not only improve the mapping of the environment, but also the vehicle's displacement model, so that the latter gives the instantaneous positioning of the vehicle (consisting of three position coordinates and three orientation coordinates of the vehicle, when the latter has the possibility of moving along three axes) with precision.
[0007] A SLAM algorithm thus allows a vehicle to navigate in an initially unknown environment which it discovers progressively.
[0008] In particular, this navigation is advantageously done autonomously, that is to say without needing to use means external to the vehicle, such as a satellite positioning system - GNSS ("Global Navigation Satellite System") to determine its instantaneous position.
[0009] For example, the implementation of a SLAM algorithm is suitable for the following use case.
[0010] To make a landing on an unmarked runway, the pilot of a helicopter must first fly over the potential landing area to visually assess the surrounding obstacles (tree, power line, etc.), as well as the slope of the landing area (or gradient).
[0011] This preparatory phase for landing can take a considerable amount of time. To observe the landing area over 360°, it may take approximately three minutes.
[0012] The implementation of a simultaneous localization and mapping type process should make it possible to automate this preparatory phase.
[0013] However, this phase must be able to be carried out in degraded visual conditions, whether it be the general weather conditions of the flight (rain, fog, darkness, etc.) or the nature of the terrain of the landing area, which, on the approach of the helicopter, raises a cloud of dust, sand, snow, etc.
[0014] The problem is therefore to obtain in real time and regardless of visual conditions, an accurate map of the environment, as well as an accurate location of the vehicle in this map, and always without resorting to a satellite positioning system.
[0015] The aim of this invention is to solve this problem.
[0016] To this end, the invention relates to a method of simultaneous localization and mapping, implemented by a computer of an embedded system on board a vehicle, said system being equipped with an inertial measurement unit and a Doppler radar system, characterized in that the method comprises the steps of: acquisition of a set of points at the current time delivered by the Doppler radar system, a point of the set of points being characterized by a position and a radial velocity in a moving frame associated with the vehicle; acquisition of the attitude at the current time of the vehicle, delivered by the inertial measurement unit; orientation of the points of the set of points at the current time with respect to a fixed ground reference frame, taking into account the attitude at the current time; processing of the radial velocities of the points of the set of points at the current time to calculate an estimated speed at the current time of the vehicle;Calculation of the vehicle's current position, taking into account its estimated speed at that time; and execution of a simultaneous localization and mapping algorithm, taking into account the vehicle's current position over a plurality of successive times.
[0017] According to particular embodiments, the process comprises one or more of the following characteristics, taken individually or in all technically possible combinations:
[0018] - the method further comprises a step of filtering the set of points to the current moment to retain only the points corresponding to objects fixed relative to the ground reference frame.
[0019] - the filtering step takes into account a raw speed at the current instant delivered by the inertial measurement unit.
[0020] - the calculation of a position at the current time of the vehicle takes into account only the estimated speed of the vehicle at the current moment.
[0021] - the method includes a step of correcting the raw speed at the current time taking into account the estimated speed of the vehicle at the current time in order to obtain a corrected speed at the current time, and in which the calculation of a position at the current time of the vehicle is carried out from the corrected speed at the current time.
[0022] The invention also relates to a system adapted to implement a simultaneous localization and mapping method in accordance with the previous method, the system comprising: a Doppler radar system; an inertial measurement unit; and a suitably programmed computer.
[0023] Preferably, the Doppler radar system is of the frequency-modulated continuous wave radar type.
[0024] Preferably, the computer implements a simultaneous localization and mapping algorithm of the iterative determination of the nearest point type.
[0025] The invention also relates to a computer program product comprising software instructions which, when executed by the computer of a system conforming to the previous system, allows the implementation of all or part of the steps of a simultaneous localization and mapping process conforming to the previous process.
[0026] The invention and its advantages will be better understood upon reading the following detailed description of a particular embodiment, given solely by way of non-limiting example, this description being made with reference to the accompanying drawings in which:
[0027] [Fig-1] The [Fig. 1] is an illustration of a particular use case of the process and the system according to the invention;
[0028] [Fig.2] Fig.2 is a schematic representation in modular form functional aspects of an embodiment of a system according to the invention; and,
[0029] [Fig.3] Fig.3 is a block representation of a mode of implementation of the identification process according to the invention.
[0030] Fig. 1 is an illustration of a particular use case of system 10 and method 100 according to the invention.
[0031] In this use case, the system 10 according to the invention is mounted on board a helicopter 1, as a particular example of an autonomous vehicle adapted to move along three axes. The specific case of an aircraft, and more particularly a helicopter, is used to describe the invention, but the invention can be applied to any type of vehicle, either to assist the pilot of that vehicle or to pilot the vehicle automatically.
[0032] A movable frame of reference is associated with the helicopter 1. It is located by the X, Y and Z axes on [Fig. 1]. This movable frame of reference is attached to a point O of the helicopter 1.
[0033] The helicopter is moving with an instantaneous vector velocity y(t).
[0034] The system 10 comprises an inertial measurement unit 20, a Doppler radar system 30, and a computer 40.
[0035] The system 30 includes an antenna 32 and hardware and software means associated with the operation of this antenna.
[0036] These different components of system 10 will be presented in detail below with reference to [Fig.2].
[0037] Periodically, the radar system 30 scans an observation area located within the environment of the helicopter 1. This observation area covers, for example, all or part of an area of interest on the ground, such as a potential landing area.
[0038] A reference frame, fixed to the ground, or ground frame, is located by the axes Xo, Yo and Zo on the [Fig.1]. This ground frame is attached to a point S in the environment.
[0039] For each period of its operation, the radar system 30 delivers a set of points. For example, on [Fig.1] and for the current time t, a point Pi(t) associated with a building, a point P2(t) associated with the ground itself, a point P3(t) associated with an individual moving on the surface of the ground, and a point P4(t) associated with a structural element, such as an electricity pylon, have been represented.
[0040] A point P;(i integer between 1 and N(t), where N(t) is the total number of points in the set of points delivered by the radar 30 at time t) is associated with an object or a portion of an object which has reflected an echo after being "illuminated" by the radar system 30.
[0041] A point P; is characterized by a distance from the antenna 32 of the radar system 30. Knowing the direction in which the echo was received, the radar system 30 determines the position of point Pi in the moving frame XYZ.
[0042] Since the radar system 30 is a Doppler radar system, it is adapted to process a received echo in order to measure its Doppler velocity. Doppler velocity is a speed radial, that is to say the projection of the instantaneous velocity of the reflecting object onto the direction joining the radar system 30 and the reflecting object (the point Pi(t) source of the echo). This velocity is denoted VR;(t) on [Fig.1].
[0043] A particular embodiment of system 10 will now be presented in relation to [Fig.2].
[0044] The system 10 incorporates a Doppler radar system 30. Advantageously, it is a commercial or COTS (“Commercial Off-The-Shelf”) Doppler radar system.
[0045] Preferably, the Doppler radar system 30 is a frequency modulated continuous wave (FMCW) radar. By emitting a frequency modulated continuous wave, the echo reflected by an object in the environment is processed, in the frequency domain, to obtain the distance separating the radar from the reflecting object along the radar's pointing direction at the time the echo is received, as well as the Doppler velocity of this reflecting object.
[0046] The position and radial velocity of an echo constitute the output data of the Doppler radar system 30. The position can be a position in radial coordinates (direction and distance along this direction) or in Cartesian coordinates (position along the three axes of the moving frame) in the moving frame.
[0047] Other outputs can advantageously be associated with a point, such as the amplitude of the echo.
[0048] Considering a time interval allowing the Doppler radar system 30 to scan its observation domain once, a set of points is delivered by the Doppler radar at each scan period. In what follows, the scan period is taken as the elementary period of implementation of the method. In particular, "snapshot" means a value of a physical quantity obtained for the current elementary period.
[0049] The system 10 incorporates an inertial navigation system (INS) 20. Advantageously, it is a commercial or COTS (Commercial Off-The-Shelf) inertial navigation system.
[0050] In general, an inertial measurement unit 20 can be described as comprising an inertial measurement unit - IMU ("Inertial Measurement Unit") 22. This integrates a set of sensors 21, including accelerometers and gyroscopes, and associated electronics 23, enabling the processing of signals delivered by the sensors 21 and the generation of a first set of DI data.
[0051] This first set of DI data corresponds to the linear acceleration along the three axes of the ground frame and the instantaneous rotational velocities around each of these axes.
[0052] The inertial measurement unit 20 includes an attitude unit - AHRS ("Attitude Heading Reference System") 24. The unit 24 combines the IMU 22 with an attitude computer 25. The attitude calculator 25 takes as input the first data set D1, delivered by the IMU 22, and calculates as output a second data set D2.
[0053] This second data set D2 includes the instantaneous attitude of the aircraft with respect to the ground reference frame. The attitude is characterized by three angles, respectively the heading (i.e. the angle between the longitudinal axis of the vehicle and the direction of the magnetic or geographic pole), the pitch (i.e. the angle between the longitudinal axis of the vehicle and a horizontal plane) and the bank (i.e. the angle between the transverse axis and a horizontal plane).
[0054] Finally, the inertial unit 20 associates with the AHRS unit 24, a navigation computer 26. The navigation computer 26 takes as input the first and second sets of data, D1 and D2, and calculates as output a third set of data.
[0055] This third data set includes an instantaneous speed of the aircraft VB(t) and an instantaneous position of the aircraft with respect to the terrain reference frame O l(t).
[0056] The system 10 incorporates a computer 40, which is connected, on the one hand, to the output of the radar system 30 and, on the other hand, to the input and output of the inertial unit 40.
[0057] The calculator 40 is a computer comprising calculation means, such as a processor, and storage means, such as a memory.
[0058] The memory stores in particular the instructions of a computer program 42 whose execution enables the implementation of the method 100 according to the invention.
[0059] Schematically, the program 42 can be described as comprising an orientation module 44, a filtering module 46, a module for calculating the instantaneous speed of the aircraft relative to the ground 48, a georeferencing module 49 and a SLAM module 50.
[0060] A preferred embodiment of the process according to the invention will now be presented with reference to [Fig.3].
[0061] The method 100 begins with a step 110 of acquisition, by the computer 40, of the data delivered by the Doppler radar system 30. The latter periodically provides the results of a scan of its observation domain. The data delivered by the radar system 30 at the current time correspond to a set of points. Each point E(t) of this set is associated with a position and a radial velocity, these quantities being evaluated in the moving frame of reference XYZ of the aircraft, at the current time t.
[0062] The process 100 continues with a step 120 of acquisition, by the computer 40, of the second set of data D2 delivered at the current time t by the AHRS unit 24 of the inertial navigation system 20. This data is the attitude of the aircraft 1 at the current time, that is to say the direction of the axes XYZ with respect to the terrain reference frame X0Y0Z0.
[0063] In a step 130, corresponding to the execution of the orientation module 44 by the computer 40, each point of the set of points obtained in step 110 is correctly oriented in the ground coordinate system, taking into account the instantaneous attitude obtained at step 120. The attitude at the current time allows the moving frame to be oriented relative to the ground frame (by means of a rigid transformation between the radar system antenna 30 and the inertial unit 20, whose parameters are known, following for example a prior calibration step of the system 10).
[0064] Following the realization, by the navigation computer 26 of the inertial unit 20, of a first time integration of the data delivered by the units 22 and 24, allowing to determine a first instantaneous speed of the aircraft in the terrain frame, this speed is acquired by the computer 40 at step 140 as instantaneous raw speed VB(t).
[0065] Step 150, corresponding to the execution of module 46 by the computer 40, consists of filtering the radial velocities of the points of the set of points obtained in step 110 and suitably oriented in step 130.
[0066] Indeed, some of the points in this set of points correspond to fixed objects on the ground. Their radial velocity is related to the instantaneous velocity of the vehicle relative to the ground, i.e., the velocity of the moving frame of reference relative to the ground frame of reference. It is the information carried by these so-called fixed points that is relevant for the remainder of method 100.
[0067] However, in addition to these fixed points, the set of points at the current time may include points that correspond to objects moving relative to the ground, such as a tree that moves under the effect of the wind, a person who moves, a car that drives, etc.
[0068] The filtering step 150 then consists of extracting the moving points from the set of points Pi(t) so that their radial velocity, which is specific to them, does not disturb the continuation of the process 100.
[0069] To separate the moving points from the fixed points in the set of points at the current time, a filtering speed is taken into account, which corresponds to the instantaneous speed of the aircraft 1 relative to the ground.
[0070] To determine whether a point is fixed or moving, it is necessary to consider not only the value of the radial velocity of that point, but also the direction along which this radial velocity is evaluated, that is, the direction between the radar system and the point in question. Indeed, the radial velocity of a fixed point located along a direction parallel to that of the vehicle's velocity is substantially equal to the instantaneous velocity of the aircraft (negative radial velocity for a point located in front of the aircraft and positive for a point behind the aircraft, the front and rear being evaluated along the direction of the aircraft's instantaneous velocity). Conversely, the radial velocity of a fixed point located along a direction orthogonal to that of the aircraft's velocity is substantially zero.
[0071] Advantageously, this filtering step is carried out by considering that the instantaneous speed of the vehicle is equal to the instantaneous raw speed VB(t) acquired in step 140.
[0072] Advantageously, the points are filtered taking into account an uncertainty on the value of the filtering speed.
[0073] Alternatively, step 140 is not carried out, and the instantaneous speed of the vehicle around which the filtering is carried out is estimated by an appropriate processing, for example statistical, of the radial speeds and positions of the points of the set of points at the current time.
[0074] Step 150 thus makes it possible to obtain a subset of points corresponding to fixed objects on the ground.
[0075] Only the subset of fixed points is used in the continuation of method 100. However, the complementary subset corresponding to points associated with moving objects can be used in other processing, such as for example a presence detection algorithm in the area envisaged for the landing of the helicopter, which emits an alarm, for example audible, to the pilot, in case of detection of the presence of a person in this area.
[0076] The next step 160 of the method 100 consists of calculating a second instantaneous velocity of the aircraft 1 VE(t) from the radial velocities of the points in the subset of fixed points. Step 160 corresponds to the execution of module 48 by the computer 40.
[0077] Algorithms are known, such as those of a Janus system, which, starting from the radial velocity associated with at least four fixed points in as many different directions, allow the velocity of the carrier to be estimated. Advantageously, all the fixed points at the current time are used by performing a least-squares regression.
[0078] Thus, at the end of step 160, an estimated instantaneous speed VE(t) of the aircraft in the terrain frame is obtained.
[0079] Step 180 consists of determining, by the navigation computer 26 of the inertial navigation system 20, the instantaneous position Ol(t) of the aircraft 1 from its instantaneous speed by performing a time integration.
[0080] In a simple embodiment, the velocity which is integrated in step 180 is the estimated instantaneous velocity VE(t) obtained directly at the output of step 160.
[0081] Preferably, however, the estimated instantaneous speed VE(t) at the output of step 160 is applied as input to the navigation computer 26 of the inertial unit 20, for example as a quantity allowing the instantaneous speed calculated by the navigation computer of the inertial unit to be recalibrated, to prevent it from drifting.
[0082] Thus, in step 170 the instantaneous gross velocity VB(t) obtained in step 140 is corrected, taking into account the instantaneous estimated velocity VE(t) obtained in step 160.
[0083] An instantaneous corrected speed VC(t) is thus determined at the end of step 170. It is this corrected quantity which is applied at the input of step 180 to be integrated over time and thus obtain the instantaneous position Ol(t) of the vehicle in the ground frame.
[0084] This embodiment is particularly advantageous since it allows the reuse of the algorithms implemented by the navigation computer 26 of the inertial navigation system, in particular the "Kalman filter" type algorithms generally executed by the navigation computers of inertial navigation systems.
[0085] The instantaneous position of the vehicle Ol(t) then allows, in a step 190 corresponding to the execution of module 49, to georeference the points of the set of points at the current time, the fixed points but preferably also the moving points.
[0086] Then, following the iteration (step 195) of steps 110 to 190 over a succession of instants (and therefore of sets of points), in a step 200, a SLAM algorithm is executed.
[0087] For example, the last five sets of points acquired and processed by steps 110 to 180 are taken into account during the current execution of the SLAM algorithm.
[0088] Step 200 corresponds to the execution of module 50 by the computer 40. For example, it is a SLAM algorithm of the ICP type ("Iterative Closest Point" or iterative determination of the nearest point).
[0089] Thus, step by step, the point clouds acquired at each period of operation of the radar system 30 make it possible to reconstruct the three-dimensional environment within which the helicopter 1 moves.
[0090] Advantageously, the SLAM algorithm, when it performs point matching between two point clouds acquired at different times, leads not only to a correction of the position of the points in the map, but also to a correction of the instantaneous position of the vehicle (step 210). Thus, a particularly precise corrected instantaneous position O2(t) of aircraft 1 is obtained at the output of step 210, in addition to the precise map of the environment C.
[0091] Advantageously, this corrected instantaneous position O2(t) is used in step 180. For example, when step 180 is carried out by the navigation computer of the inertial navigation system, the corrected instantaneous position delivered by the SLAM algorithm allows the inertial navigation system to be recalibrated.
[0092] Thus, the present invention avoids the double time integration normally performed by the navigation computer of an inertial measurement unit, when the latter calculates the instantaneous position solely from the measurements taken by the sensors of the IMU. In other words, the present invention avoids the rapid time drift on the instantaneous position value calculated by an inertial measurement unit according to the prior art, or at least the time drift of a low-cost inertial measurement unit.
[0093] With the implementation of the invention, the instantaneous position is obtained by performing a single integration of the instantaneous velocity, whether it is the velocity estimated using only data associated with the point clouds delivered by the radar system or whether it is the velocity corrected by the inertial measurement unit taking into account the velocity estimated at the output of the processing of the data associated with the point clouds delivered by the radar system.
[0094] Furthermore, the present invention avoids the rapid time drift on the value of the instantaneous position calculated by an inertial measurement unit according to the prior art by constraining the calculation of the instantaneous position performed by the inertial measurement unit using the instantaneous position obtained at the output of the SLAM algorithm.
[0095] A radar, although it has a lower spatial resolution than a Lidar or an optical camera, operates in a wide range of visual conditions, including degraded, or even very degraded, visual conditions.
[0096] By implementing a fusion of data delivered by a radar system and an inertial navigation system, the method according to the invention makes it possible to achieve significant accuracy.
[0097] Standard commercial components can then be used, including low-priced components, which individually have reduced precision.
[0098] The invention is particularly well suited to aircraft such as drones, especially for autonomous landings. But it also has other applications, particularly for autonomous vehicles such as mobile robots operating in a hangar.
Claims
Demands
1. A method (100) for simultaneous localization and mapping, implemented by a computer (40) of a system on board a vehicle (1), said system being equipped with an inertial measurement unit (20) and a Doppler radar system (30), characterized in that the method comprises the steps of: - acquisition (110) of a set of points at the current time delivered by the Doppler radar system (30), a point (P;) of the set of points being characterized by a position and a radial velocity in a moving frame (XYZ) associated with the vehicle (1); - acquisition (120) of the attitude at the current time of the vehicle, delivered by the inertial measurement unit (20); - orientation (130) of the points of the set of points at the current time with respect to a fixed ground reference frame (XoYoZo), taking into account the attitude at the current time;- filtering (150) the set of points at the current time to retain only the points corresponding to objects fixed relative to the ground reference frame; - processing (160) the radial velocities of the points in the set of points at the current time to calculate an estimated velocity at the current time (VE(t)) of the vehicle (1); - calculation (180) of a position at the current time (Ol(t)) of the vehicle (1) taking into account the estimated velocity at the current time of the vehicle; and, - execution (200) of a simultaneous localization and mapping algorithm taking into account the position at the current time of the vehicle over a plurality of successive times.
2. Method according to claim 1, wherein the filtering step takes into account a raw velocity at the current instant (VB(t)) delivered by the inertial measurement unit (40).
3. A method according to any one of claims 1 to 2, wherein the calculation of a position at the current time of the vehicle takes into account only the speed estimated at the current time of the vehicle.
4. A method according to claim 2, comprising a step of correcting the raw speed at the current time by taking into account the estimated speed at the current time of the vehicle so as to obtain a corrected speed at the current time (VC(t)), and in which the calculation of a position at the current time of the vehicle is carried out from the corrected speed at the current time.
5. System (10) adapted to implement a method (100) of simultaneous localization and mapping according to any one of claims 1 to 4, the system (10) comprising: - a Doppler radar system (30); - an inertial measurement unit (20); and - a suitably programmed computer (40).
6. System according to claim 5, wherein the Doppler radar system (30) is of the frequency-modulated continuous wave radar type.
7. System according to claim 5 or claim 6, wherein the computer (40) implements a simultaneous localization and mapping algorithm of the iterative nearest point determination type.
8. Computer program comprising software instructions which, when executed by the computer (40) of a system (10) according to any one of claims 5 to 7, enables the implementation of all or part of the steps of a simultaneous localization and mapping method (100) according to any one of claims 1 to 4.