METHOD AND APPARATUS FOR PERFORMING SIMULTANEOUS LOCALIZATION AND MAPPING

Abstract

A method of performing simultaneous localization of a mobile body and mapping of its environment, includes the steps of: a) defining an occupancy grid (G) on a region of the environment and a pose grid (Π) comprising for the mobile body; b) receiving a first time series of distance measurements (z1, z2) and, upon reception of a distance measurement: either b1) updating occupancy probabilities of the occupancy grid; or b2) updating pose probabilities of the pose grid; and c) receiving a second time series of odometry measurements (u1, u2), each of the measurements being representative of a motion of the mobile body in the environment and, upon reception of an odometry measurement, updating the pose probabilities of the pose grid. An apparatus for carrying out such a method is also provided.

Description

CROSS-REFERENCE TO RELATED APPLICATIONS

This application claims priority to foreign European patent application No. EP 19315027.3, filed on Apr. 29, 2019, the disclosure of which is incorporated by reference in its entirety.

FIELD OF THE INVENTION

The invention relates to a method of performing simultaneous localization of a mobile body and mapping of its environment using distance (or “range”) and motion sensors. It also relates to an apparatus for carrying out such a method.

The invention applies, in particular, to the field of the navigation of robots, drones, autonomous vehicles, etc. and more generally to that of perception.

BACKGROUND

In the following, “mapping the environment” will designate the task of determining the position, in a suitable reference frame, of physical bodies within a region of space surrounding in whole or in part the mobile body (also called a “carrier”). “Localization” refers to the task of determining at least the position, and preferably also the heading, of the mobile body within such a region of space. “Performing simultaneous localization and mapping” refers to a situation wherein signals provided by sensors are processed to determine the position/heading of the mobile body (carrier) and simultaneously detect and localize obstacles in the surrounding space.

The moving body may be a robot, drone, autonomous vehicle or the like, or even a device which cannot move autonomously but is carried by a person or a machine (e.g. an “electronic white cane” assisting visually impaired people).

By “physical body” (also called an “obstacle”) it is meant any physical object or substance that exhibits individuality and can be detected by an appropriate sensor. Thus, inanimate objects, be they natural or artificial, plants, animals, human beings, but also liquid or solid particles in suspension in the air, such as clouds, or indeed liquid or gaseous masses, are considered to be physical bodies.

A distance (or “range”) sensor is a sensor which provides information about at least the distance to a nearest obstacle in its field of view. Its physical operating principle is irrelevant: sensors as diverse as LIDARs, radars, sonars, etc. may be used to implement the inventive method and apparatus. Some distance sensors may provide additional information, e.g. the distance of all obstacles in a field of view and in some cases the azimuth/elevation of said obstacles.

A distance sensor may be considered to be of the “narrow-angle” or of the “wide-angle” type. A distance sensor is considered to be “wide-angle” when there is a non-negligible likelihood that several different obstacles are simultaneously present inside its field of view, and at a same distance from it (taking into account the finite resolution of the distance measurement). Conversely, a distance sensor is considered to be “narrow-angle” when such likelihood is negligible. Therefore, these definitions are application-dependent. While in most cases a sensor having a field of view of 15° or more can be considered to be a “wide-angle” sensor this is not always the case, and conversely a narrower field of view may nevertheless be considered “wide” enough to benefit from the invention.

A motion sensor is a sensor which provides information about the movements (translation and/or rotation) of the moving body with respect to its environment. Its physical operating principle is irrelevant: sensors as diverse as rotary encoders, inertial sensors, Doppler radars or sonars etc. may be used to implement the inventive method and apparatus.

Environment mapping can be performed according to several different techniques, most of which belong to one of two main families: geometric procedures, which are aimed at identifying the geometry of the objects of the surrounding space, and occupancy grid based procedures, which are aimed at determining whether a certain location is occupied by an obstacle. The occupancy grid technique is disclosed e.g. by [1]

Localization, also called “position estimation”, can also be performed according to different techniques. In particular, the so-called “Markov localization” [2] approach aims at estimating the probability distribution in space of the carrier's position, and its evolution as the carrier moves and/or new signals are acquired by different sorts of sensors. It is important to note that it is not possible to use motion (or “odometric”) sensor alone to perform position estimation, as the growth of the estimation error over time would be unbounded.

In the simplest cases, Markov localization assumes that the environment is known and static, i.e. there is a known “map” of the environment on which the carrier has to be localized. Researchers, however, have also tackled the more complex problem of simultaneous localization and mapping (SLAM) wherein the environment is unknown and/or dynamic, and a map of it has to be built simultaneously with the estimation of the position of the carrier.

Document [3] discloses a SLAM algorithm called FastSLAM wherein sensors (e.g. laser sensors) are used for detecting “landmarks” of the environment. The measurements issued by the sensors are used for updating a carrier position using particle filters, while the landmarks themselves are localized using extended Kalman filters. The method supposes that a measurement can be unambiguously associated to a given landmark, which is a rather stringent hypothesis. Moreover, it is computationally heavy (it requires a high number of Kalman filters) and the error of the pose estimation may diverge in some cases.

Document [4] discloses a SLAM algorithm wherein the environment is represented using an occupancy grid. The heading of the carrier is supposed to be known and its position is determined using the “scan matching” method. A drawback of this method is that the carrier heading is simply supposed to be known, which is not usually the case in real-word applications. Moreover, the method does not account for the impact of the uncertainty of the carrier position on the updating of the occupancy grid.

Document [5] discloses a SLAM method combining odometry and sonar measurements. The environment is represented using an occupancy grid updated by a Kalman filter, while a particle filter is used for tracking the pose of the carrier. A drawback of this method is that it does not account for the impact of the uncertainty of the positions of the obstacle on the updating of the carrier position.

Document [6] discloses a SLAM method using an occupancy grid for representing the environment and a pose grid for representing the carrier position and orientation. This approach has an exponential computational complexity, which makes it difficult to apply to embarked systems, having a limited computing power. Moreover, it relies on restrictive hypotheses (an insufficiently general sensor model, a specific motion model).

U.S. Pat No. 8,340,852 discloses the application of SLAM using an inertial measurement unit (IMU) and range sensors to autonomous vehicles. SLAM is performed using an optimization algorithm, which does not allow taking into account the measurement incertitude.

US 2004/013295 discloses a method of performing SLAM using a pair of cameras for acquiring images of the environment, detecting obstacles and identifying landmarks, and odometric sensors.

US 2016/0062361 discloses a navigation method fora mobile robot, including a first step wherein the robot moves along a path following input commands from an operator and detecting obstacle, and a second step wherein the robot moves autonomously upgrading a map of the environment and estimating its own position.

SUMMARY OF THE INVENTION

The invention aims at overcoming, in full or in part, at least some of the drawbacks of the prior art. More particularly it aims at providing a SLAM method having an acceptable computational complexity, general applicability (i.e. avoiding overly-restrictive hypothesis) and explicitly accounting for uncertainties on sensor measurements. Advantageously, such a method should be able to be carried out by an embarked processor with limited computing power.

According to the invention, an object of the invention allowing achieving these aims in full or in part is a method of performing simultaneous localization of a mobile body and mapping of its environment, the method comprising the following steps, carried out by a data processor:

• • a) defining:
    • an occupancy grid on a region of said environment, the grid comprising a plurality of cells, an occupancy probability by an obstacle being associated to each cell of the occupancy grid, and
    • a pose grid comprising a plurality of cells, each representing a position and orientation of the mobile body, a pose probability being associated to each cell of the pose grid;
  • b) receiving a first time series of distance measurements, each of said measurements being representative of a distance between a point of the mobile body and at least one nearest obstacle and, upon reception of a distance measurement: either
    • b1) updating the occupancy probabilities of the occupancy grid as a function of present values of said occupancy probabilities, of the received distance measurement and of the pose probabilities of the pose grid; or
    • b2) updating the pose probabilities of the pose grid as a function of present values of said pose probabilities, of the received distance measurement and of the occupancy probabilities of the occupancy grid; and
  • c) receiving a second time series of odometry measurements (u₁, u₂), each of said measurements being representative of a motion of the mobile body in the environment and, upon reception of an odometry measurement, updating the pose probabilities of the pose grid as a function of present values of said pose probabilities and of the received odometry measurement.

Another object of the invention is an apparatus for performing simultaneous localization of a mobile body and mapping of its environment, comprising:

• • at least one input port for receiving a plurality of signals representative of a first time series of distance measurements arising from one or more distance sensor and a second time series of odometry measurements arising from one or more motion sensor; and
  • a data processor configured to receive as input said signals and to use them to construct an updated occupancy grid and an updated pose grid by applying such a method.

Yet another object of the invention is a mobile robot carrying such an apparatus.

BRIEF DESCRIPTION OF THE DRAWINGS

Additional features and advantages of the present invention will become apparent from the subsequent description, taken in conjunction with the accompanying drawings, wherein:

FIG. 1 illustrates the notion of “direct” model of a distance sensor;

FIG. 2 illustrates the notion of occupancy grid;

FIG. 3, illustrates the notion of “inverse” model of a distance sensor;

FIG. 4 is a plot representing the spatial discretization of an inverse model on an occupancy grid;

FIG. 5 is a schematic diagram of an apparatus according to an embodiment of the invention;

FIG. 6 illustrates the unfolding of a method according to an embodiment of the invention;

FIG. 7 illustrates in detail a step of the method of FIG. 6, wherein a distance measurement is used for updating an occupancy grid;

FIG. 8 is a flow-chart of a computation involved in the step of FIG. 7;

FIG. 9 illustrates how the inverse model of a narrow-angle distance sensor may be computed;

FIG. 10 illustrates in detail a step of the method of FIG. 6, wherein a distance measurement is used for updating a pose grid;

FIG. 11 is a flow-chart of the step of FIG. 10, when a narrow-angle range sensor such as a LIDAR is used for measuring distances; and

FIGS. 12A, 12B and 12C illustrate some calculations required to carry out the method step of FIGS. 10 and 11 when a wide-angle range sensor is used.

DETAILED DESCRIPTION

A range (or distance) sensor may be represented by use of a probabilistic model, which accounts for its finite precision, resolution and for possible measurement errors.

The idea is that a measurement output by the sensor does not necessarily indicate the exact distance between the sensor and the nearest obstacle in its field of view. Instead, for a given distance d, the output z of the sensor is a random variable characterized by a probability distribution p(z|d), constituting the “direct model” of the sensor. FIG. 1 presents an exemplary direct model of a sensor; a linear space 50 m long is considered and it is assumed that an obstacle is situated at d=25 m from the sensor. For a sensor with an error modeled by a Gaussian function, the most probable value taken by z will be 25 m (assuming that there are no systematic errors), but other values will be possible, with a probability density defined by the curve. In the case of an ideal sensor, one would have p(z|d)=δ(z−d), where δ is a Dirac Delta, and the measurement value z would always be equal to the true distance. The direct model of a sensor can be determined experimentally, or it can be obtained from data provided by the constructor (e.g., under a Gaussian hypothesis, the value of the standard deviation suffices to characterize the model).

Hereinafter, Ω will denote a spatial reference frame in one, two or three dimensions; an occupancy grid OG is a partition of a continuous and bounded region of a space generated by Ω into a number N of parts, called “cells” and designated by an index i∈[0, N−1]. The cell of index i is indicated by c_i. Without loss of generality, we shall consider a one-dimensional occupancy grid observed by a single distance sensor C (or a plurality of co-located sensors), the index i increasing with the distance from the sensor (c₀ therefore being the cell closest to the sensor and c_N−1 the one furthest away). This configuration is illustrated on FIG. 2.

An obstacle T is a bounded continuous subset of Ω. A cell c_i is said to be occupied by an obstacle T if T∩c_i≠Ø, to be not occupied by T if A∩c_i=Ø. Stated otherwise, if the obstacle covers the cell even partially, the latter is considered to be occupied. Other conventions are possible, but in any event a cell must be either free (vacant), or occupied.

For each of the cells of the grid, we consider the binary random experiment “state” which can have one of the two outcomes {occupied; vacant} consisting in knowing whether or not the cell contains an obstacle or not. The state of the cell c_i is denoted s_i∈{e_i, o_i}. o_i will denote the realisation s_i=o_i (I;e. the cell is occupied), and e_i will denote the realisation s_i=e_i (i.e. the cell is empty).

An occupancy probability P(o_i)—or, equivalently, a vacancy probability P(e_i)=1−P(o_i)—is associated to each cell c_i of an occupancy grid (in the following, a capital “P” will denote a probability, and a lowercase “p” a probability density). Before any measurement, an a priori occupancy probability—often, but not necessarily equal to 0.5, is associated to each cell.

It is also considered that the position of the obstacles can only be known with the aid of uncertain distance sensors, characterized by a probabilistic model such as described above which may be written, in the single obstacle approximation, p(z|{right arrow over (x)}), {right arrow over (x)} being the position of the nearest obstacle (in several dimensions, it is a vector, expressed in cartesian, spherical, polar coordinates, etc. and not a simple scalar). The sensors may be telemetric lasers (also called LIDARs), sonars, infrared radars, time-of-flight cameras, etc. More generally, by taking into account the possibility of having k>1 obstacles in the field of view of the sensor, the inverse model may be written as p(z|{right arrow over (x)}₁, {right arrow over (x)}₂, . . . , {right arrow over (x)}_k), where {right arrow over (x)}_i is the position of the i-th obstacle.

A measurement z arising from a sensor makes it possible to determine the conditioned probability of occupancy P(o_i|z) of a cell c_i. For a given measurement z, the set of probabilities P(o_i|z)∀i∈[0, N−1] constitutes what is called the “inverse model” of the sensor on the grid. Whilst the direct model of the sensor describes the response of the sensor as a function of the material world, the inverse model expresses the impact of the measurement on the occupancy grid, which is the model of the material world that is adopted, thereby justifying the term inverse model.

FIG. 3 presents a typical example of inverse model of a distance sensor, in a case where z=25 m. It may be verified that the occupancy probability is almost zero for the cells which are situated at a distance of less than 24.25 m from the sensor and attains a peak for a distance of 25 m (corresponding to the measurement provided by the sensor). Beyond 25 m, the occupancy probability decreases until it stabilizes at a value of 0.5, indicative of a total ignorance of the occupancy state of the cells which, being situated beyond the obstacle, are masked by the latter and therefore inaccessible to the sensor.

The inverse model, applied to range measurement, allows updating the occupancy probabilities of the occupancy grid.

Documents WO2017/050890 and EP 3 364 213 describe methods for updating the occupancy probabilities of an occupancy grid using a time series of range measurements from one or more “narrow-angle” distance sensors. European Patent Application 18305955 filed on Jul. 13, 2018 generalizes these methods to the case of “wide-angle” sensors. The teaching of these documents can be applied to the present invention

In accordance with the usage which prevails in the literature, FIG. 3 represents the inverse model by means of a smoothed curve. A more correct representation would be to display only the points corresponding to the limits of the cells of the grid: indeed, it is not possible to distinguish a “partially” occupied cell from another which would be “totally” occupied, in all cases the distance to the obstacle will be estimated as being the distance to the corresponding cell. This is the spatial error introduced by the grid.

Moreover, for reasons which are explained in WO2017/050890 and which will become apparent in the following, it is preferable to quantize the probability values by allowing them to only take values belonging to a finite set. Suitable quantizing schemes will be described later, see equations (13)-(28).

FIG. 4 shows the inverse model of FIG. 3 spatially discretized but not quantized (curve MI), spatially discretized and quantized taking, for each probability value of the continuous model, the smallest element of S₁ exceeding this probability value (curve MQE), spatially discretized and quantized taking, for each probability value of the continuous model, the closest element of S₁ (curve MQP).

Similarly, the pose of the carrier may be represented by a “pose grid” Π, comprising a plurality of cells, each associated to a pose probability. In a preferred embodiment of the invention, a “pose” is constituted by a position in two- or three-dimensions and a heading direction expressed by one or two angles. For some applications (e.g. a terrestrial robot such as an autonomous vehicle), the position will be defined in two dimensions and the heading direction will be expressed by an angle. The invention will be discussed with reference to this case, which is easier to represent on drawings, without loss of generality.

For instance, the position X_r of the carrier may take its values in L={x_i, i=0 . . . M−1} and its heading α_r may take its values in A={α_i, i=0 . . . M′−1}. The pose x_r=(X_r, α_r) will then take its values in Π=L×A, which is the pose grid. In the following, the pose grid Π will be represented by a two-dimensional grid; however, it should be understood that it often represents a parameter space of higher dimensions (e.g. three dimensions: a two-dimensional position and a one-dimensional heading) and that it may be represented, in a computer memory, by a one-dimensional, two-dimensional or higher-dimensional array.

As it will be discussed below, the pose grid is updated using both range measurement, provided by range sensors, and odometry measurement, provided by motion sensors. To this aim, a motion sensor may be represented by a “motion model” P(x_r*|x_r, u) which expresses the probability that the carrier takes a pose x_r* knowing that, at a previous time, it had a pose x_r and that an odometry measurement u has been received.

FIG. 5 is a very schematic representation of a localization and positioning apparatus LPA according to an embodiment of the invention, on a carrier R such as, e.g. a mobile robot or an autonomous vehicle. Apparatus LPA comprises a plurality of range (or distance) sensors RS₁, RS₂, RS₃, RS₄, of a same or of different types, each having a respective field of view and a motion or odometry sensor OS. In different embodiments, a single range sensor and/or several odometry sensors of a same or of different type may be used. Range measurements z and odometry measurements u are provided to a data processor DP (usually, a suitably-programmed microprocessor, microcontroller or digital signal processor, even if the use of a specific processing circuit based on programmable circuits such as FPGA and/or application-specific integrated circuits is also possible). Data processor DP includes a memory device storing data representing direct and inverse models of the range sensors and motion models for the motion sensors, and also storing executable code for carrying out a simultaneous localization and mapping method according to the invention, which will be described in detail hereafter.

As illustrated on FIG. 6, according to the invention, odometry measurements u are used to update the pose grid Π without affecting the occupancy grid G while distance measurements may be used for updating either the pose grid (combined with the previous knowledge of the pose grid and occupancy grid) or the occupancy grid (combined with the previous knowledge of the occupancy grid and pose grid). For instance, in the example of FIG. 6:

Before time t₁ the occupancy grid G and the pose grid Π are in an initial state st₀. The state is characterized by an occupancy probability value for each cell of the occupancy grid, and a pose value for each cell of the pose grid.

At time t₁, a first odometry measurement u₁ is received, and used for updating the pose grid Π (more precisely, the pose probability of at least some of its cells) while the occupancy grid G remains unchanged. This corresponds to state st₁.

At time t₂, a first distance measurement z₁ is received. The measurement z₁ and the pose grid Π are used to compute an updated occupancy grid (more precisely: updated occupancy probabilities of at least some of its cells), while the pose grid remains unchanged (state st₂).

At time t₃, a second distance measurement z₂ is received. This time, this measurement is used—together with the occupancy grid G—to update the pose grid Π (more precisely, the pose probability of at least some of its cells). The occupancy grid remains unchanged (state st₃).

At time t₄, a second odometry measurement u₂ is received, which is used for updating the pose grid Π (i.e. its pose probabilities) while the occupancy grid G remains unchanged (state st₄).

It is important to note that the distance and odometry measurements may arrive asynchronously, and it is even possible that several measurements arrive simultaneously. However, two simultaneous odometry measurements must be processed sequentially, while two simultaneous distance measurements may be processed simultaneously, provided that one of them is used for updating the pose grid and the other one for updating the occupancy grid. Similarly, if an odometry measurement and a position measurement are received simultaneously, they may be processed simultaneously provided that the latter is used for updating the occupancy grid. It should be understood that two measurements are deemed to be “received simultaneously” when they are separated by a time interval smaller than a time resolution of the data processor, and that they are “processed simultaneously” is they are applied to a same state st_i of the occupancy and pose grids.

In the example of FIG. 6 two consecutive distance measurements are used, alternatively, for updating the occupancy grid and the pose grid. This is by no means essential. Different, possibly application-dependent, strategies may be used to determine which grid to update upon receiving a distance measurement.

It is apparent from FIG. 6 that the inventive method comprises three main steps (carried out several time, but not necessarily in a fixed order—see above):

• • updating the occupancy probabilities of the occupancy grid as a function of present values of said occupancy probabilities, of a received distance measurement and of the pose probabilities of the pose grid;
  • updating the pose probabilities of the pose grid as a function of present values of said pose probabilities, of a received distance measurement and of the occupancy probabilities of the occupancy grid; and
  • updating the pose probabilities of the pose grid as a function of present values of said pose probabilities and of a received odometry measurement.

In the following, possible embodiments of each one of these three steps will be described in detail.

The following notations will be used:

• • P(·) will represent the probability of an event;
  • p(·) will represent the probability density of a continuous random variable;
  • G is an occupancy grid composed of N>1 disjoint cells c_i, i=0, . . . N−1;
  • o_i is the event that cell c_i is occupied by an obstacle;
  • e_i is the event that cell c_i is empty (i.e. not occupied by an obstacle);
  • Π is a pose grid, which may be decomposed into a position grid L and a heading grid A. The position grid L is composed of M disjoint cells, corresponding to M discrete values x_i, i=0 . . . M−1 for the position X_r of the carrier, while the heading grid A is composed of M′ disjoint cells, corresponding to M′ discrete values α_j j=0 . . . M′−1 for the heading α_r of the carrier.

x_r=(X_r, α_r) represents a pose—i.e. a position and a heading. P(x_r) is the probabilty that the carrier takes pose x_r.

z is the result of a distance (or range) measurement, representing the distance between the carrier (more precisely, a point of the carrier) and an obstacle. In the following, it will be assumed that z can only take discrete values, which is required for digital processing.

u is the result of an odometry measurement, representing a movement (a translation, a rotation or a combination thereof) of the carrier. In the following, it will be assumed that u can only take discrete values, which is required for digital processing.

P(o_i|z, x_r) is the inverse model of a range sensor (ISM—inverse sensor model), associating an occupancy probability to each cell of the occupancy grid for a given range measurement result z and a pose of the carrier, x_r, deemed to be known. The ISM is supposed to be tabulated for all possible values of z and x_r.

P(x_r*|x_r, u) is the motion model, which expresses the probability that the carrier takes a pose x_r* knowing that, at a previous time, it had a pose x_r and that an odometry measurement u has been received. Again, this model is supposed to be tabulated for all possible values of u and x_r.

Using a Range Measurement for Updating the Occupancy Grid

As illustrated on FIG. 7, the ISM P(o_i|z, x_r) is tabulated for all admissible values of z and x_r and stored in a memory device. The pose grid Π—and therefore the probability P(x_r) of all admissible poses—is also known. When a range measurement z is received, it allows computing the occupancy probabilities of a temporary occupancy grid P_xr(o_i|z)—reference TOG on FIG. 7—as follows:

$\begin{array}{cc}{P}_{\mathrm{xr}}\left(o_iz\right)=\sum _{x_r\in \Pi }P\left(o_iz,x_r\right)P\left(x_rz\right)=\sum _{x_r\in \Pi }P\left(o_iz,x_r\right)P\left(x_r\right)& \left(1\right)\end{array}$

where the second equality accounts for the fact that the pose probability is independent from the range measurement. FIG. 8 is a flow-chart illustrating an iterative algorithm for calculating P_xr(o_i|z).

Then, the current occupancy probabilities—represented by P(o_i|z^t−1)—is “fused” with the occupancy probabilities of the temporary grid P_xr(o_i|z) to obtain updated occupancy probabilities (i.e. an updated occupancy grid) P(o_i|z, z^t−1) as follows:

$\begin{array}{cc}{P}_{\mathrm{xr}}\left(o_iz,z^{t-1}\right)=\frac{P_{\mathrm{xr}}\left(o_iz\right)P\left(o_iz^{t-1}\right)}{P_{\mathrm{xr}}\left(o_iz\right)P\left(o_iz^{t-1}\right)+\left(1-P_{\mathrm{xr}}\left(o_iz\right)\right)\left(1-P\left(o_iz^{t-1}\right)\right)}& \left(2\right)\end{array}$

See WO2017/050890 and EP 3 364 213.

FIG. 9 shows how P(o_i|z, x_r) is computed for the case of a range sensor having a narrow field of view. The ISM of the sensor for a given measurement value z may be represented by a segment decomposed in one-dimensional cells, each associated to a probability value. For instance, cells ĉ₀ and ĉ₁ may be associated to a probability value ε close to zero, meaning that it is almost impossible that—given measurement z—said cells are occupied. Cells ĉ₂, ĉ₃ and ĉ₄ have probability values equal to 0.3, 0.9, and 0.7, respectively. This means that cell ĉ₃ has the highest occupancy probability, cell ĉ₂ is likely to be empty and cell ĉ₄ is quite likely to be occupied. Cells ĉ₅-ĉ₈ have a probability value of 0.5, representing a complete lack of information. The segment is superposed to the occupancy grid, pose x_r determining the position of its origin and its orientation. Each cell c_i of the occupancy grid intersected by the segment will receive a corresponding occupancy probability value; other cells take an occupancy probability equal to 0.5.

Using a Range Measurement for Updating the Pose Grid

Updating the pose grid upon reception of a range measurement means computing P(x_r|z). For x_r∈Π:

$\begin{array}{cc}P\left(x_rz\right)=\frac{p\left(x_r,z\right)}{p\left(z\right)}& \left(3\right)\end{array}$

The denominator p(z) is a constant factor, therefore (3) can be written

P(x_r|z)=η·p(x_r, z)   (4)

η being a constant.

For the sake of simplicity, it will be considered that the position grid L coincides with the occupancy grid G. Therefore, let x_r=(c_n, α_m). For given n and m, one may define G^xr be the projection of the occupancy grid G onto the range sensor field of view when the sensor itself is located at X_r=c_n and having a heading α_r=α_m. Then, summing over all the possible configurations G_k^xr of G^xr:

$\begin{array}{cc}p\left(x_r,z\right)=\sum _kp\left(x_r,z,G_k^{\mathrm{xr}}\right)=\sum _kp\left(zx_r,G_k^{\mathrm{xr}}\right)·P\left(G_k^{\mathrm{xr}}\right)·P\left(x_r\right)& \left(5\right)\end{array}$

and therefore (4) can be written

P(x_r|z)=η·P(x_r)·Σ_kp(z|x_r, G_k^xr)·P(G_k^xr)   (6)

All the terms of (6) are either known or easily computed:

• • η is simply a normalization factor;
  • P(x_r) is the previously-computed pose probability;
  • p(z|x_r, G_k^xr) is the range sensor model evaluated when the carrier takes a pose x_r and for a given configuration of the occupancy grid, which may be tabulated and stored in a memory device.

P(G_k^xr) is the probability of a given configuration of the occupancy grid, which in turn depends on the occupancy probabilities P(o_i). Assuming that the projected grid G^xr has N_xr elements, the a priori probability of any configuration is

$P\left(G_k^{\mathrm{xr}}\right)=\frac{1}{2^{N_{\mathrm{xr}}}}.$

The computation of P(x_r|z) is illustrated on FIG. 10.

The number of terms involved in the sum of (6) depends on the kind of range sensor considered. For instance, while in the most general case this number of terms is an exponential function of the number of cells N_xr, in the case of a LIDAR sensor it is equal to N_xr (i.e. the complexity is linear); the same is true for wide-field sensor if the nearest-target approximation applies (see below, equations 9 and 10, and above-cited European Patent Application 18305955). Indeed, a LIDAR only detects the nearest target, therefore two different configurations having a same nearest occupied cell give identical contributions. Let L_j^xr be the event “the first occupied cell in G_k^xr is ĉ_j”. This means that all cells ĉ_i, i<j, are empty and that cells ĉ_i, i>j can have any state. Summing all possible events L_j^xr, j={0, . . . N_xr−1}, (6) can be written as:

$\begin{array}{cc}P\left(x_rz\right)=\eta ·P\left(x_r\right)·\sum _jP\left(zx_r,L_j^{\mathrm{xr}}\right)·P\left(L_j^{\mathrm{xr}}\right)& \left(7\right)\end{array}$

wherein:

• • P(z|x_r, L_j^xr) is the sensor model evaluated when the carrier takes a pose x_r and an obstacle is located at the distance ĉ_j from x_r. Usually this model is given by a normal distribution (d_j(x_r), σ) where d_j(x_r) is the distance of ĉ_j to x_r and σ a standard deviation;
  • P(L_j^xr) is the probability of event L_j^xr, which is given by:

$\begin{array}{cc}P\left(L_j^{\mathrm{xr}}\right)=P\left(o\left(c_j\right)\right)·\prod _{i=0}^{j-1}P\left(e\left(c_j\right)\right)& \left(8\right)\end{array}$

For sensors having a large field of view, the occupancy grid may take the form illustrated on FIG. 12A, wherein several cells are situated at a same distance from the sensor and form an “arc”. In particular, in the example of FIG. 12A, cells ĉ₀ to ĉ₃ form a first arc Â₀, cells ĉ₄ to ĉ₇ form a second arc Â₁, cells ĉ₈ to ĉ₁₁ form a third arc Â₂ and cells ĉ₁₂ to ĉ₁₅= form a fourth arc Â₃=Â_Mxr−1 If a cell of the arc is occupied, the arc is considered to be occupied.

If the nearest-target hypothesis can be applied, for two configurations G_k1^xr and G_k2^xr sharing the same nearest occupied arc, p(z|x_r, G_k1^xr) can be taken to be equal to p(z|x_r, G_k2^xr). Thus for G^xr={ĉ₀, ĉ₁, . . . , }={, . . . , }Q_j^xr is defined, for j=0, . . . , N_xr−1, as the event “Â_j is the first occupied arc in G^xr” (as a consequence, all cells in arcs Â_ii<j are empty and cells in a Â_ii>j can be in any state. Summing all possible events Q_j^xr, equation (6) may be written:

$\begin{array}{cc}P\left(x_rz\right)=\eta ·P\left(x_r\right)·\sum _jP\left(zx_r,Q_j^{\mathrm{xr}}\right)·P\left(Q_j^{\mathrm{xr}}\right)& \left(9\right)\end{array}$

where P(Q_j^xr) is the probability of having the arc Â_j as the first occupied arc, i.e. the probability of having all arcs Â_ii<j empty, which is given by:

$\begin{array}{cc}P\left(Q_j^{\mathrm{xr}}\right)=\underset{{\hat{c}}_k\in {\hat{A}}_j}{\Pi }\left(1-P\left(e\left({\hat{c}}_k\right)\right)\right)·\prod _{i=0}^{j-1}\underset{{\hat{c}}_k\in {\hat{A}}_k}{\Pi }P\left(e\left({\hat{c}}_i\right)\right)& \left(10\right)\end{array}$

FIGS. 12B and 12C illustrate the events Q₂^xr and Q₁^xr, respectively.

Using an Odometry Measurement for Updating the Pose Grid

Different kind of motion sensors can be used in order to update the carrier pose, such as IMU (Inertial Measurement Units), GPS, wheel encoders . . . The pose distribution at time t=0, P₀(x_r) is deemed to be known, and updated when an odometry measurement u is received. The problem is then to compute P_t(x_r|u) from P_t−1(x_r).

For a fixed pose x_r*, P_t(x_r*|u) is computed by summing over all possible poses:

$\begin{array}{cc}{P}_t\left(x_r^{*}u\right)=\sum _{x_r}P\left(x_r^{*}x_r,u\right)·P\left(x_ru\right)& \left(11\right)\end{array}$

Taking into account the fact that P(x_r|u)=P_t−1(x_r), (11) becomes:

$\begin{array}{cc}{P}_t\left(x_r^{*}u\right)=\sum _{x_r}P\left(x_r^{*}x_r,u\right)·P_{t-1}\left(x_r\right)& \left(12\right)\end{array}$

wherein:

• • P(x_r*|x_r, u) is the motion model that depends on the type of the chosen motion sensor, and is usually tabulated and stored in a memory device;
  • P_t−1(x_r) is the previously computed (or initial, if t−1=0) pose distribution.

Integer Computations

A potential problem with the inventive method described above is that, in a nave implementation, computations would need to be performed using floating point arithmetic. The use of floating-point computation requires significant resources in terms of computing power, which are hardly compatible with the constraints specific to embedded systems.

According to some advantageous embodiments of the invention, however, occupancy and pose probability may be updated by making use of much simpler integer computation, while keeping an acceptable accuracy level.

It is worth noting that WO2017/050890 (already cited) discloses a method for updating occupancy grids only using integer computations. This method alone, however, is not sufficient to allow updating pose grids, too, without floating point computations.

Evaluating equations (1)-(12) requires five kind of elementary operations:

• • additions;
  • additive inversions—i.e. computing the complement to one of an operand;
  • multiplications;
  • divisions;
  • “probability fusion”, corresponding to equation (2).

Two alternative embodiments will be described, allowing carrying out all these operations using integer computation only. According to both embodiments of the invention, the probability values are discretized, i.e. they can only take values belonging to a finite set. The first embodiment uses a first, non-uniform discretization scheme and the second embodiment uses a second, uniform discretization scheme. A third discretization scheme (already disclosed in WO2017/050890) is used for carrying out “integer fusion”—it is therefore necessary to map the first or second discretization scheme onto the third and vice-verse, which can be done very cheaply using a lookup table.

First Embodiment

Let ϵ be a real value in ]0,1[. The discretization set D_log of size N_log is defined as:

D _log={1, (1−ϵ), (1−ϵ)², . . . , 0.5, . . . , (1−ϵ)^Nlog−2, 0}  (13)

It should be noted that ϵ is chosen such that it exists an integer n (usually n=N_log/2 or n=N_log/4) satisfying (1−ϵ)ⁿ=0.5.

It can be easily verified that, for i,j∈{0 . . . , N_log−1}, the product of D_log[i] by D_log[j] given by:

$\begin{array}{cc}D_{\log }\left[i\right]\otimes D_{\log }\left[j\right]=\{\begin{array}{cc}{D}_{\log }\left[i+j\right]& \mathrm{if}i+j\le N_{\log }-1\\ 0& \mathrm{otherwise}\end{array}& \left(14\right)\end{array}$

Therefore, the product of two values (probabilities, probability densities) discretized according to the discretization set D_log can be computed using an integer operation on integer indexes.

The division of D_log[i] by D_log[j] is given by:

$\begin{array}{cc}{D}_{\log }\left[i\right]\otimes D_{\log }\left[j\right]=\{\begin{array}{cc}\mathrm{undefined}& \mathrm{if}j=N_{\log }-1\\ 0& \mathrm{if}i=N_{\log }-1\\ D_{\log }\left[i-j\right]& \mathrm{if}i\ge j\\ 1& \mathrm{if}i<j\end{array}& \left(15\right)\end{array}$

Interestingly, in the inventive method, division is only used to compute the normalization factor η<1, therefore the first condition never occurs. Again, only an integer operation on indexes needs to be performed.

The addition of D_log[i] and D_log[j], however, gives a result which—in general—does not belong to D_log. Indeed, one should solve the following equation

(1−ϵ)^k+i,j=(1−ϵ)ⁱ+(1−ϵ)^j   (16)

which gives:

$\begin{array}{cc}{k}_{+}^{i,j}=\frac{\log \left({\left(1-ϵ\right)}^i+{\left(1-ϵ\right)}^j\right)}{\log \left(1-ϵ\right)}& \left(17\right)\end{array}$

Clearly, k₊^i,j is not necessarily an integer. However, one may take the floor (or, alternatively, the ceiling, or the closest integer etc.) of (17), so that k₊^i,j is redefined as:

$\begin{array}{cc}{k}_{+}^{i,j}=\mathrm{floor}\left(\frac{\log \left({\left(1-ϵ\right)}^i+{\left(1-ϵ\right)}^j\right)}{\log \left(1-ϵ\right)}\right)& \left(18\right)\end{array}$

Addition is then represented by:

$\begin{array}{cc}{D}_{\log }\left[i\right]\oplus D_{\log }\left[j\right]=\{\begin{array}{cc}{D}_{\log }\left[i\right]& \mathrm{if}j=N_{\log }-1\\ D_{\log }\left[j\right]& \mathrm{if}i=N_{\log }-1\\ D_{\log }\left[k_{+}^{i,j}\right]& \mathrm{for}k_{+}^{i,j}\mathrm{in}\left(18\right)\mathrm{and}\mathrm{if}0\le k_{+}^{i,j}\le N_{\log }-1\\ 1& \mathrm{if}k_{+}^{i,j}<0\\ 0& \mathrm{if}k_{+}^{i,j}>N_{\log }-1\end{array}& \left(19\right)\end{array}$

Computing the addition using (18) and (19) requires floating point operations. However, it is possible to compute (19) offline for all pairs (i,j) and store them in a lookup table. Note that, since addition is symmetric, the table only needs to store N_log²/2 terms.

Additive inversion is performed in a similar way:

• • k₋^i,j is defined as follows:

$\begin{array}{cc}{k}_{-}^i=\mathrm{floor}\left(\frac{\log \left(1-{\left(1-ϵ\right)}^i\right)}{\log \left(1-ϵ\right)}\right)& \left(20\right)\end{array}$

1⊖D_log[j] is then defined as:

$\begin{array}{cc}1\ominus D_{\log }\left[i\right]=\{\begin{array}{cc}1& \mathrm{if}i=N_{\log }-1\\ 0& \mathrm{if}i=0\\ D_{\log }\left[k_{-}^i\right]& \mathrm{for}k_{-}^i\mathrm{in}\left(20\right)\mathrm{and}\mathrm{if}0\le k_{-}^i\le N_{\log }-1\\ 1& \mathrm{if}k^i<0\\ 0& \mathrm{if}k_{-}^i>N{}_{\log }-1\end{array}& \left(21\right)\end{array}$

Like for addition, (21) is computed offline and stored in a table of size N_log.

As mentioned above, fusion requires mapping D_log to a different discretization set G_p, the result being then mapped back to D_log. The theory of integer probability fusion was disclosed in WO2017/050890 and will be briefly recalled here.

Let p be a probability value that verifies

0.5<p<1   (22)

A sequence p_n(n∈) is defined as follows:

$\begin{array}{cc}{p}_0=1\text{/}2p_1=pp_2=F\left(p,p\right)p_3=F\left(p_2,p\right)\dots p_{n+1}=F\left(p_n,p\right)& \left(23\right)\end{array}$

where F(.,.) is the fusion operator

$\begin{array}{cc}F\left(p,q\right)=\frac{p.q}{p.q+\left(1-p\right).\left(1-q\right)}& \left(24\right)\end{array}$

The definition of p_n is then extended to negative integer values of n as follows:

$\begin{array}{cc}{p}_0=1\text{/}2p_{-1}=1-pp_{-2}=F\left(1-p,1-p\right)p_{-3}=F\left(p_{-2},1-p\right)\dots p_{-n-1}=F\left(p_{-n},1-p\right)& \left(25\right)\end{array}$

The discretization schemes, depending on parameter p∈]0.5;1[ are then defined as follows:

G _p ⁻={(p_n), n≤0}  (26)

G _p ⁺={(p_n), n≥0}  (27)

It can be shown that the fusion of two elements p_i and p_j belonging to G_p=G_p⁺∪G_p⁻ is defined by:

F(p_i, p_j)=p_i+j   (28)

Therefore, G_p is a discretization scheme which does not introduce errors when used for performing probability fusion.

As explained above, according to the present embodiment of the invention, when fusion has to be performed in order to update the occupancy grid upon reception of a range measurement, D_log is mapped onto G_p, fusion is performed, and the result is mapped back onto D_log, the mapping being performed using lookup tables stored in a memory device.

More precisely, let f_Gp(i),∀i∈{0, . . . , N_log−1} be the index of the closest element in G_p to D_log[i].

The values of this function are then stored offline in a lookup table. Similarly, for pi∈G_p, let f_Dlog(i) be the index of the closest element to p_i in D_log. Then, for i,j∈{0, . . . , N_log−1} the fusion of D_log[i] and D_log[j] is defined by:

$\begin{array}{cc}F\left(D_{\log }\left[i\right],D_{\log }\left[j\right]\right)=f_{D_{\log }}\left(p_{f_{G_p}\left(i\right)+f_{G_p}\left(j\right)}\right)& \left(29\right)\end{array}$

Second Embodiment

Let N*∈*. The discretization set D_unif of size N*+1 is defined as:

$\begin{array}{cc}{D}_{\mathrm{unif}}=\left\{0,\frac{1}{N^{*}},\dots ,0.5,\dots ,\frac{N^{*}-2}{N^{*}},\frac{N^{*}-1}{N^{*}},1\right\}& \left(30\right)\end{array}$

The set D_unif represents a discretization of the probabilities in [0, 1].

For i,j∈{0, . . . N*} the addition of D_unif[i] and D_unif[j] is simply given by:

$\begin{array}{cc}{D}_{\mathrm{unif}}\left[i\right]\oplus D_{\mathrm{unif}}\left[j\right]=\{\begin{array}{cc}{D}_{\mathrm{unif}}\left[i+j\right]& \mathrm{if}i+j\le N^{*}\\ 1& \mathrm{otherwise}\end{array}& \left(31\right)\end{array}$

Therefore, the sum of two values (probabilities, probability densities) discretized according to the discretization set D_unif can be computed using an integer operation on integer indexes. The same applies to additive inversion. Indeed, for i∈{0, . . . N*}, 1−D_unif[i] is given by:

$\begin{array}{cc}1\ominus D_{\mathrm{unif}}\left[i\right]=\{\begin{array}{cc}{D}_{\mathrm{unif}}\left[N^{*}-i\right]& \mathrm{if}N^{*}-i\ge 0\\ 0& \mathrm{otherwise}\end{array}& \left(32\right)\end{array}$

For j∈{0, . . . N*} the product of D_unif[i] and D_unif[j] cannot be exactly represented in D_unif. Indeed, one should solve the following equation for k_×^i,j:

$\begin{array}{cc}\frac{k_{\times }^{i,j}}{N^{*}}=\frac{i}{N^{*}}·\frac{j}{N^{*}}& \left(33\right)\end{array}$

which yields

$\begin{array}{cc}{k}_{\times }^{i,j}=\frac{i·j}{N^{*}}& \left(34\right)\end{array}$

It can be easily understood that k_×^i,j is not necessarily an integer. As in the first embodiment, one can take the floor of (34)—or, alternatively, the ceiling, or the closest integer etc.—and redefine k_×^i,j as:

$\begin{array}{cc}{k}_{\times }^{i,j}=\mathrm{floor}\left(\frac{i·j}{N^{*}}\right)& \left(35\right)\end{array}$

Thus, multiplication in D_unif is represented by

$\begin{array}{cc}{D}_{\mathrm{unif}}\left[i\right]\otimes D_{\mathrm{unif}}\left[j\right]=\{\begin{array}{cc}{D}_{\mathrm{unif}}\left[i\right]& \mathrm{if}j=N^{*}\\ D_{\mathrm{unif}}\left[j\right]& \mathrm{if}i=N^{*}\\ 0& \mathrm{if}i=0\mathrm{or}j=0\\ D_{\mathrm{unif}}\left[k_{\times }^{i,j}\right]& \mathrm{for}k_{\times }^{i,j}\mathrm{in}\left(35\right)\mathrm{and}\mathrm{if}0\le k_{\times }^{i,j}\le N^{*}\\ 0& \mathrm{if}k_{\times }^{i,j}<0\\ 1& \mathrm{if}k_{\times }^{i,j}>N^{*}\end{array}& \left(36\right)\end{array}$

The values of D_unif[i]⊗D_unif[j], as defined in (36), for all i,j pairs are then computed offline and stored in a lookup table.

Division is dealt with similarly. One should solve the following equation for k_d^i,j:

$\begin{array}{cc}\frac{k_d^{i,j}}{N^{*}}=\frac{i}{N^{*}}÷\frac{j}{N^{*}}& \left(37\right)\end{array}$

which yields:

$\begin{array}{cc}{k}_d^{i,j}=\frac{i·N^{*}}{j}& \left(38\right)\end{array}$

As (38) does not necessarily gives an integer value, k_d^i,j is redefined by taking the floor of (38):

$\begin{array}{cc}{k}_d^{i,j}=\mathrm{floor}\left(\frac{i·N^{*}}{j}\right)& \left(39\right)\end{array}$

Division in D_unif is then represented by:

$\begin{array}{cc}{D}_{\mathrm{unif}}\left[i\right]\otimes D_{\mathrm{unif}}\left[j\right]=\{\begin{array}{cc}\mathrm{undefined}& \mathrm{if}j=0\\ D_{\mathrm{unif}}\left[i\right]& \mathrm{if}j=N^{*}\\ 0& \mathrm{if}i=0\\ 1& \mathrm{if}i=j\\ D_{\mathrm{unif}}\left[k_d^{i,j}\right]& \mathrm{for}k_d^{i,j}\mathrm{in}\left(39\right)\mathrm{and}\mathrm{if}0\le k_d^{i,j}\le N^{*}\\ 0& \mathrm{if}k_d^{i,j}<0\\ 1& \mathrm{if}k_d^{i,j}>N^{*}\end{array}& \left(40\right)\end{array}$

Note that, in the inventive method, the first condition (j=0, leading to an undefined result) can never occur. As for product, the values of D_unif[i]∅D_unif[j], as defined in (40), for all i,j pairs (except j=0) are then computed offline and stored in a lookup table.

Fusion is dealt with in a similar way than in the first embodiment. g_Gp(i),∀i∈{0, . . . , N*} is defined as the index of the closest element of G_p to D_unif[i]. The values of this function are stored in a lookup table. Similarly, for p_i∈G_p, let g_Dunif(i) be the index of the closest element to p_i in D_unif. Then, for i,j∈{0, . . . , N*} the fusion of D_unif[i] and D_unif[j] is defined by:

$\begin{array}{cc}F\left(D_{\mathrm{unif}}\left[i\right],D_{\mathrm{unif}}\left[j\right]\right)=g_{D_{\mathrm{unif}}}\left(p_{g_{G_p}\left(i\right)+g_{G_p}\left(j\right)}\right)& \left(41\right)\end{array}$

Both the first and the second embodiments allow then to carry out the inventive method using only:

• • integer computations on said integer indices; and
  • using integer indices for retrieving pre-computed results in lookup tables.

The invention has been described with reference to embodiments wherein a single mobile body, carrying odometry and distance sensors, is localized, and its environment is mapped. Other embodiments are possible. For instance, some or even all of the sensors may not be embarked on the mobile body—e.g. an external Doppler radar may be used to obtain relevant odometry measurements. Moreover, several sensor-carrying mobile bodies moving within a same environment may share—and contribute to update—a single occupancy grid, while having each a respective pose grid.

REFERENCES

[1] A. Elfes, “Occupancy grids: a stochastic spatial representation for active robot perception”, Sixth Conference on Uncertainty in AI, 1990.

[2] D. Fox, W. Burgard, and S. Thrun, “Markov localization for mobile robots in dynamic environments”, Journal of Artificial Intelligence Research, 1999.

[3] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit “FastSLAM: A factored solution to the simultaneous localization and mapping problem” in AAAI National Conference on Artificial Intelligence. AAAI, pp. 593-598, 2002.

[4] T. Duckett and U. Nehmzow “Mobile robot self-localisation using occupancy histograms and a mixture of gaussian location hypotheses” Robotics and Autonomous Systems, vol. 34, no. 2, pp. 117-129, 2001.

[5] J. Nordh and K. Berntorp “Extending the occupancy grid concept for low-cost sensor-based slam” in IFAC Proceedings Volumes, vol. 45, no. 22, Elsevier, pp. 151-156, 2012.

[6] K. P. Murphy, “Bayesian map learning in dynamic environments” in Advances in Neural Information Processing Systems, pp. 1015-1021, 2000.

Claims

1. A method of performing simultaneous localization of a mobile body and mapping of its environment, the method comprising the following steps, carried out by a data processor: a) defining: an occupancy grid on a region of said environment, the grid comprising a plurality of cells, an occupancy probability by an obstacle being associated to each cell of the occupancy grid, anda pose grid comprising a plurality of cells, each representing a position and orientation of the mobile body, a pose probability being associated to each cell of the pose grid;b) receiving a first time series of distance measurements, each of said measurements being representative of a distance between a point of the mobile body and at least one nearest obstacle and, upon reception of a distance measurement: either b1) updating the occupancy probabilities of the occupancy grid as a function of present values of said occupancy probabilities, of the received distance measurement and of the pose probabilities of the pose grid; orb2) updating the pose probabilities of the pose grid as a function of present values of said pose probabilities, of the received distance measurement and of the occupancy probabilities of the occupancy grid; andc) receiving a second time series of odometry measurements (u1, u2), each of said measurements being representative of a motion of the mobile body in the environment and, upon reception of an odometry measurement, updating the pose probabilities of the pose grid as a function of present values of said pose probabilities and of the received odometry measurement;part of the distance measurements being used for updating the pose probabilities of the pose grid and part of the distance measurements being used for updating the pose probabilities of the occupancy grid.

2. The method of claim 1, wherein steps b) and c) are carried out asynchronously.

3. The method of claim 1, wherein said updating the occupancy probabilities of the occupancy grid comprises: building a temporary occupancy grid comprising a plurality of cells, each associated to an occupancy probability computed as a weighted sum of a plurality of contributions obtained by applying to the distance measurement a plurality of inverse statistical models of a corresponding distance sensor, each inverse model being associated to a cell of the pose grid and weighted by the corresponding pose probability; andfusing the occupancy probabilities of the occupancy grid and of the temporary occupancy grid.

4. The method of claim 1, wherein said updating the pose probabilities of the pose grid upon reception of a distance measurement comprises multiplying the present values of said pose probabilities by a weighted sum of direct statistical models of a corresponding distance sensor for all possible configurations of the occupancy grid, a configuration attributing an ‘empty’ or an ‘occupied’ state to each cell of the occupancy grid, each weight being proportional to a probability of the corresponding configuration.

5. The method of claim 1, wherein said updating the pose probabilities of the pose grid upon reception of an odometry measurement comprises computing a weighted sum of a plurality of contribution obtained by applying to the odometry measurement a plurality of inverse statistical models of a corresponding motion sensor (OS), each inverse model being associated to a cell of the pose grid and weighted by the corresponding probability.

6. The method of claim 1, wherein probability values are constrained to take discrete values belonging to a first set of finite cardinality, each element of the first set being identified by an integer index, and wherein updating the occupancy probabilities of the occupancy grid, updating the probabilities of the pose grid upon reception of a distance measurement and updating the probabilities of the pose grid upon reception of an odometry measurement are carried out exclusively by: performing integer computations on said integer indices; andusing said integer indices for retrieving pre-computed results stored in a memory.

7. The method of claim 6, wherein said first set of finite cardinality corresponds to a discretization of a [0, 1] interval according to an exponential law, whereby multiplications and divisions are carried out by performing integer computations on integer indices of the first set, while additions and additive inversions are carried out using integer indices of the first set for retrieving pre-computed results stored in a memory.

8. The method of claim 6, wherein said first set of finite cardinality corresponds to a linear discretization of a [0, 1] interval, whereby additions and additive inversions are carried out by performing integer computations on integer indices of the first set, while multiplications and divisions are carried out using integer indices of the first set for retrieving pre-computed results stored in a memory.

9. The method of claim 3, wherein probability values are constrained to take discrete values belonging to a first set of finite cardinality, each element of the first set being identified by an integer index, and wherein updating the occupancy probabilities of the occupancy grid, updating the probabilities of the pose grid upon reception of a distance measurement and updating the probabilities of the pose grid upon reception of an odometry measurement are carried out exclusively by: performing integer computations on said integer indices; andusing said integer indices for retrieving pre-computed results stored in a memory;

10. The method of claim 9, wherein said second set of finite cardinality is formed by the union of two subsets Gp− et Gp+ defined by: Gp−={(pn), n≤0}Gp+={(pn), n≥0}wherein n is a relative integer index, p a parameter taking a value strictly comprised between 0.5 and 1 and values pn are recursively defined by p0=0,5;p1=p;pn+1=F(pn, p)∀n>1p−1=1−p; pn−1=F(pn, p−1)∀n<−1whereby fusing occupancy probabilities pi and pj is carried out by computing: F(pi, pj)=pi+j

11. An apparatus for performing simultaneous localization of a mobile body and mapping of its environment, comprising: at least one input port for receiving a plurality of signals representative of a first time series of distance measurements arising from one or more distance sensor and a second time series of odometry measurements arising from one or more motion sensor; anda data processor configured to

12. The apparatus of claim 11, also comprising one or more distance sensors adapted to generate signals representative of said distance measurements and one or more motion sensors adapted to generate signals representative of said odometry measurements, said sensor being linked to said input port or ports.

13. A mobile robot carrying an apparatus according to claim 12.