Safe and Robust Map Updating For Long-Term Operations in Dynamic Environments

1. Introduction

Autonomous mobile robots are becoming increasingly popular in industrial applications such as logistics, manufacturing, and warehousing. One of the key requirements for the operation of such robots is the ability to navigate safely and efficiently in a dynamic environment, repeatedly doing the same task all day for days or months. A mobile robot’s autonomous navigation is achieved using Simultaneous Localisation and Mapping (SLAM) techniques, in which the robot maps the environment, navigates, and locates itself by merely ”looking” at this environment, without the need for additional hardware to be installed in the workplace. This is achieved by using sensor measurements (such as LIDAR, cameras, or other sensors) to update the map and estimate the robot’s pose at the same time [2]. However, these methods face significant challenges in dynamic environments, mainly when the environment changes over time due to the movement of obstacles or people. Obstacles may appear, disappear, or move around, and the robot’s map needs to be updated continuously to reflect these changes. While localisation and local obstacle avoidance algorithms can handle small changes in the environment, the robot’s map can diverge from the present working area, reducing navigation performance [3]. Conventional SLAM methods struggle to update the map accurately and efficiently over long periods of time due to the accumulation of errors in the robot’s pose estimates and in the map. This is because small errors in the robot’s pose estimation can accumulate over time, leading to significant inaccuracies in the map. Moreover, the robot needs to constantly re-localise itself, which can be challenging and time-consuming. Indeed, traditional SLAM algorithms are computationally intensive and require significant processing power, memory, and time [4]. This can limit the robot’s ability to operate for extended periods, particularly in industrial environments where robots need to operate continuously.

Therefore, the long-term operation of autonomous mobile robots in dynamic industrial environments requires continuous map updating to ensure accurate navigation and obstacle avoidance and achieve good localisation performance [5,6]. Moreover, having an updated static map, coherent with the current environment, allows the robot to reduce its effort in re-planning over time to adjust its trajectory. Updating the map in real-time requires the robot to quickly detect and track changes in the environment, such as the movement of people, equipment, and obstacles. For this reason, and in order to consider long-term operations, the map updating algorithm has to be computationally light and use limited memory [7].

In our paper [1], we proposed a 2D LIDAR-Based map updating to detect possible changes over time in the environment and to accordingly update a previously built map with limiting memory usage and neglecting the presence of highly dynamic obstacles. The system was developed using the Robot Operating System framework [8] using a 2D occupancy grid map representation. This map representation is commonly used in the industrial field due to its ability to facilitate path-planning [9,10]. In fact, companies such as AgileX RoboticsAgileX,https://global.agilex.ai/ , AvidbotsRobotnik, https://robotnik.eu/ , and BoschBosch, https://www.bosch.com/, currently employ 2D occupancy grid maps in their assets, demonstrating the technology’s high level of industrial readiness. As such, it is important to continuously improve and strengthen the robustness of this technology for long-term use.

1.1. Paper Contribution

Although the proposed map updating [1] provide promising result in map quality and localisation performance in a dynamic environment, the method relies heavily on accurate knowledge about the initial pose of the robot in a previously built map. That information might not be available with high accuracy, and due to changes in the map, it might be hard to estimate immediately. This could lead to bias in the estimated pose of the robot, which could result in overwriting free and occupied cells, effectively corrupting the occupancy grid and the resulting map. Indeed, as the robot moves through its environment, small errors in its position estimate can accumulate, resulting in a significant deviation from its true position over time. This issue is often referred to as drift and can be caused by a variety of factors, such as wheel slippage, sensor noise, and environmental changes.

To overcome this drawback and make the approach more robust, in this paper, we first introduce a fail-safe mechanism based on localisation performance to prevent erroneous map updating when the localisation error increases, and then we integrate a pose update algorithm that is activated when the robot effectively gets lost.

In our work, we propose a map updating algorithm in the presence of localisation error able to integrate localisation recovery procedures preventing degradation of the map in long-term operations. Extensive simulations have been conducted to validate the system in its components and as a whole. Finally, real-world experiments results are provided to highlight the performance of the approach in a real environment and to show its robustness in realistic uncertain conditions.

2. Related Works

Over the past few decades, an immense and comprehensive body of research has emerged regarding the issue of updating maps in a dynamic environment to support long-term operations. Sometimes, the researchers refer to the problem as a lifelong mapping or a lifelong SLAM one. In this section, we present the most relevant solutions proposed in the literature related to the occupancy grid map representation and methods for updating an existing one, going to underline the difference with conventional slam methods.

2.1. Occupancy grid

Occupancy grid maps are a spatial representation that captures the occupancy state of an environment by dividing it into a grid of cells. Each cell is associated with a binary random variable indicating the probability that the cell is occupied (free, occupied, or unknown). One common method for updating the occupancy grid map is to use a probabilistic model such as the Bayesian filter [11]. This filter estimates the probability distribution of the occupancy state of each cell based on the likelihood of sensor measurements and the prior occupancy state. Most occupancy grid map algorithms derive from the Bayesian filters and differ from the posterior calculation. However, the Bayesian approach has drawbacks in the difficulty of correct map updates based on possible uncertainties in the robot poses, [12]. Although the construction of a static environment map using the occupancy grid approach has already been solved [13], its effectiveness in dynamic environments over extended periods of time still requires further investigation. Different methods have expanded the occupancy grid map algorithms to handle dynamic and semi-static environments [14,15,16,17]. These approaches assess the occupancy of cells irrespective of the type of detected obstacle, thereby mitigating the challenges related to multi-obstacle detection and tracking and improving the speed of the mapping process [18]. However, their primary focus is on accelerating the mapping construction process rather than in the updating process of an initial occupancy grid map for an extended period of time. This may limit the use of such approaches in, possibly large, industrial scenarios where a full map reconstruction requires a computational and resources effort to replan continuously the whole robot motion in the new map.

2.2. Lifelong Mapping

In the literature, there exist grid-based methods aimed at lifelong mapping. One group of these algorithms is based on the update of local maps, as demonstrated in [7], based on a weighted recency averaging technique to detect persistent variations. They merge these maps using Hough Transformation. However, this approach assumes that no dynamic obstacles are in the environment. In contrast, [19] still generates local maps and continuously update them online requiring a substantial memory usage. To address this, [20] proposed pruning redundant local maps to reduce computational costs while maintaining both efficiency and consistency. However, these systems are not currently used in industrial applications, except for closed navigation systems, and are not comparable with occupancy grid-based methods like the one proposed in this paper because the map representation they rely on is different. Instead, in [6] the authors update a 2D occupancy map of a non-static facility logistics environment using a multi-robot system, where the local temporary maps built by each robot are merged into the current global one. While the approach is promising for map updating of non-static environments, the need for precise localisation and assumptions on the environment can be challenging in cluttered and dynamic environments, providing a limit of the method.

2.3. Lifelong Localisation

In order to perform long-term operations in a dynamic environment, the map used by the robot to autonomously navigate needs to be up-to-date and accurately reflect the current state of the environment. To update a map correctly, a robot must first accurately localise itself within the environment. If the robot’s position is incorrect or uncertain, any updates made to the map may be inaccurate, leading to errors in navigation and task performance. In the literature, there are several works that take into account the localisation problem in order to achieve better results in the map update process which, like our proposed approach, does not fall within SLAM systems. In [21], the author proposed a method for the long-term localisation of mobile robots in dynamic environments using time series map prediction. They produce a map of the environment and predict the changes in the 2D occupancy grid map over time using the ARMA time series model. The robot’s localisation is then estimated by matching its sensor readings to the predicted map. The proposed method achieves better localisation accuracy and consistency compared to traditional methods that rely on static maps. However, there are some limitations related to the computational complexity of the time series model, which can make real-time implementation challenging. Additionally, the accuracy of the map prediction is highly dependent on the quality of the sensor data and the ability of the time series model to accurately capture the dynamics of the environment. In [22], Sun et al. proposed an approach for achieving effective localisation of mobile robots in dynamic environments. They solve the localisation problem through a particle filter and scan matching. When the matching score is above a certain threshold, the map updating step is performed. However, they consider only environments with a clearly defined static structure. In [23], instead, they discourage map updating if the robot is in an area with low localisability (i.e. the map is locally poor in environmental features). To further improve robustness, they introduce a dynamic factor to regulate how easily a map cell changes its state. Although they do not make any explicit assumption on the environment, the dynamic factor works well in scenarios where the changes occur regularly (e.g. a parking lot, where the walls are fixed, and the parking spots change occupancy state frequently). Instead, it is not well-suited for environments that slowly change in an irregular and unpredictable fashion. All the works mentioned above follow the same pattern, on which we took inspiration for our approach, starting from a previously created map and applying various methods to ensure a robust localisation that leads to a correct map update. However, all of them do not provide results concerning the robot lost localisation case. On the contrary, our approach does not provide a continuous localisation algorithm but an evaluation of the localisation error so as not to update the map incorrectly. Finally, our approach handle also the case of a robot get lost in the environment due to localisation errors.

2.4. Conventional Lifelong SLAM

Other systems that try to solve autonomous navigation in long-term operations rely on lifelong SLAM. In such scenarios, the SLAM technique is used to provide either a new map every time the robot performs a new run or an updated map, both robust to localisation error and environmental dynamics. Thus, they are oriented to reduce e.g. computational performance, loop closure, and pose graph sparsification [4], resulting in a stand-alone system based on internal structure that can not be straightforwardly integrated and hence easily used in industrial systems. This is also due to the fact that they may be not based on occupation grid map approaches. In the paper [24], for example, the authors present a new technique based on a probabilistic feature persistence model to predict the state of the obstacles in the environment and update the world representation accordingly. [25], on the other hand, approaches each cell’s occupancy as a failure analysis problem and contributes temporal persistence modeling (TPM), which is an algorithm for probabilistic prediction of a cell’s "occupied" or "empty" state given sparse prior observations from a task-specific mobile robot. Although the introduced concept is interesting, it suffers from the limitation due to temporary map building for motion planning and hence it does not provide an entire updated representation of the environment.

Due to their incremental approach, most life-long SLAM available systems rely on graphs, [26,27], or other internal structures. Even if an occupancy grid map can be built for navigation purposes, an existing graph structure is required as input rather than a previous occupancy grid map.

Finally, our intent and difference with a slam method should be clear. Our proposed approach is based on the localisation error evaluation of the robot’s estimated pose provided by any localisation algorithm to update the map correctly and continue its update in case of loss of the robot’s pose. The proposed approach is proven to be able to capture environmental changes while neglecting the presence of moving obstacles such as humans or other robots. Moreover, the approach is designed to guarantee limited memory usage and to be possibly and easily integrated into typical industrial mobile systems.

3. Problem Formulation

Consider a mobile robot $\mathcal{R}$, represented by $X_{\mathcal{R}}\left(k\right)=\left[\begin{array}{cc}p\left(k\right)& \theta \left(k\right)\end{array}\right]$ with $p\in {\mathbb{R}}^2$ and $\theta \in \mathbb{R}$ be the robot position and orientation, is equipped with a 2D lidar and encoders and operates in a dynamic environment $W_0$. To navigate autonomously, $\mathcal{R}$ requires a map and a generic localisation algorithm that provides an estimate global robot pose ${\tilde{X}}_{\mathcal{R}}\left(k\right)=\left[\begin{array}{cc}\tilde{p}\left(k\right)& \tilde{\theta }\left(k\right)\end{array}\right]$, where $\tilde{p}\in {\mathbb{R}}^2$ and $\tilde{\theta }\in \mathbb{R}$ are the robot estimated position and orientation, such that the localisation error $e_l\left(k\right)={|{\tilde{X}}_{\mathcal{R}}\left(k\right)-X_{\mathcal{R}}\left(k\right)|}_2$ is smaller than a threshold $ϵ$ to allow the robot completing its task. In the case of dynamic environments, the arrangement of the obstacles present in the environment $W_0$ changes in time, effectively creating new environments $W_i$ with $i=1...N$ that are similar to the precedent ones $W_{i-1}$. What happens is that over time a static map $M_0$ no longer reflects the current configuration of $W_i$ leading to localisation errors $e_l\left(k\right)>ϵ$ hence degrading the operational performance of $\mathcal{R}$. One solution is to provide a map $M_i\left(k\right)$ that is updated over time to reflect $W_i$, and such that $e_l\left(k\right)<ϵ$.

Occupancy Grid Map: In this paper, we consider the occupancy grids technique to represent and model the environment in which the robot operates. A 2D occupancy grid map is a grid that represents the space around the robot through cells $c_j\left(q\right)$ that contains information about the area associated with its position $q\in {\mathbb{R}}^2$ in the space. Each cell in the grid represents the probability of the cell being occupied by an obstacle (probability equal to 1, black cells in Figure 1), free (probability equal to 0, grey cells in Figure 1), or unknown (otherwise, dark grey cells in Figure 1).

Figure 1 shows the 2D occupancy grid map representation considered in this paper for a map of 8x8 cells. The map’s cells are identified through the grid coordinates $(u,v)$ that define the actual resolution r of the occupancy grid and the finite locations of obstacles. The origin of grid coordinates $(x_{map},y_{map})$ is in the bottom-left corner of the grid, with the first location having an index of (0,0). Given the 2D occupancy grid map, the cell identified by a point $p=(x,y)$ has the following grid coordinates:

$$\left\{\begin{array}{c}u=\frac{x-x_{map}}{r}\hfill \\ v=\frac{y-y_{map}}{r}\hfill \end{array}\right.$$

(1)

There are several sensors that can be used to build 2D occupancy grid maps such as Lidars, cameras, sonar, infrared sensors, etc…, but in our work, we considered only 2D Lidars to generate a 2D point cloud. It is worth noting that our method is also suitable for any sensors able to provide this kind of information.

Lidar Point Cloud: Given a laser range measurement $Z\left(k\right)={\left[\begin{array}{ccc}{z}_1\left(k\right)& \cdots & z_n\left(k\right)\end{array}\right]}^T$ with n laser beams at time k, it is possible to transform the data in a point cloud $P\left(k\right)={\left[\begin{array}{ccc}{p}_1\left(k\right)& \cdots & p_n\left(k\right)\end{array}\right]}^T$, where $p_i\left(k\right)\in {\mathbb{R}}^2$ is the i-th hit point at time step k, i.e. the point in which the i-th beam hit an obstacle. Moreover, given $Z\left(k\right)$ and the estimated robot’s pose ${\tilde{X}}_{\mathcal{R}}\left(k\right)$, it is possible to obtain the coordinates of $p_i\left(k\right)$ in a fixed frame asto ease the notation, the time dependence k is omitted:

$$\begin{array}{c}\hfill p_i=\left(\begin{array}{c}{x}_i\\ y_i\end{array}\right)=\tilde{p}+z_i\left(\begin{array}{c}cos({\theta }_0+i\Delta \theta +\tilde{\theta })\\ sin({\theta }_0+i\Delta \theta +\tilde{\theta })\end{array}\right),\end{array}$$

(2)

where with $x_i,y_i\in \mathbb{R}$ we represent the cartesian coordinates of the hit point $p_i$ along the i-th laser range measurement $z_i$. The angular offset of the first laser beam with respect to the orientation of the laser scanner is represented by ${\theta }_0\in \mathbb{R}$ considering that the angular rate between adjacent beams is $\Delta \theta \in \mathbb{R}$. Finally, given $p_i$ the set of cells passed through by the measurement is $C_{p_i}=\left\{\begin{array}{ccc}{c}_1,& \cdots ,& c_{\mu }\end{array}\right\}$ where $c_{\mu }=c\left(p_i\right)$ is the cell associated to $p_i$.

3.1. Ideal Scenario Vs Real Scenario

Given a mobile base $\mathcal{R}$, equipped with a 2D lidar, operating in an environment represented by a 2D occupancy grid map, the ideal simplified scenario is depicted in Figure 2: a laser beam hits an obstacle in a cell that is occupied, and it is identified as the only cell selected by the laser measurement thanks to an accurate knowledge of the robot pose.

However, the real scenario presents a different situation, depicted in Figure 3. Since there are localisation errors, the robot’s pose is affected by uncertainty, represented in the figure by the green cells around the robot. The localisation error and the addition of measurement errors and noises lead to the possibility that a laser beam may not identify the correct cell but another one in its neighborhood (green cells around $p_i$). For this reason, a direct check on the occupancy of the identified cell may lead to errors in the map update process. Hence, a procedure that is robust with respect to such localisation and measurement errors and noises must be developed.

4. Method

The proposed system architecture is represented in Figure 4, where based on an initially provided occupancy grid map $M_{i-1}\left(k\right)$, the robot pose ${\tilde{X}}_{\mathcal{R}}\left(k\right)$ provided by any localisation algorithm, and the current laser measurements $Z\left(k\right)$, our approach computes a newly updated map $M_i$ and a new robot pose ${\tilde{X}}_{\mathcal{R}}\left(k\right)$ accordingly to the localisation error. Indeed, as shown in Figure 5, only state of the cells that correspond to relevant changes in the environment are updated in case the localisation error is limited. Otherwise, the map updating process is suspended until the localisation algorithm recovers the right estimated pose. If the latter is not able to provide a new ${\tilde{X}}_{\mathcal{R}}\left(k\right)$ and the drift accumulation is too big, a pose updating process is activated to estimate a new robot pose.

To evaluate the localisation error, we first classify with the Beams Classifier the laser measurement as either “detected change measurement” or “non-detected change measurement”, this characterization is computed checking for discrepancy of the measurements with the initial map $M_{i-1}$ as described in Section 4.1. Then, the detected change measurements are evaluated by the Localisation Check as described in Section 4.2 to assess the localisation error. If the error is small enough, the map updating process can continue. Otherwise, the system stores the last updated map $M_i\left(k\right)$ and the correct ${\tilde{X}}_{\mathcal{R}}\left(k\right)$ and continues to monitor the localisation error, pausing the map update process. If the error continues to increase, the pose updating process is activated to provide a new ${\tilde{X}}_{\mathcal{R}}\left(k\right)$ based on $M_i\left(k\right)$, ${\tilde{X}}_{\mathcal{R}}\left(k\right)$, and $Z\left(k\right)$. The map updating process, depicted in Figure 6, is responsible of the detection of changes in static obstacles (i.e., removal or addition) while it neglects the possible presence of dynamic obstacles in the environment that can lead to a corrupted map, e.g. people or other robots. If the Localisation Check is successful, it means that the localisation error is limited and hence the outputs of the Beam Classifier together with $Z\left(k\right)$ and $M_{i-1}\left(k\right)$ are used by the Changed Cells Evaluator and the Unchanged Cells Evaluator to evaluate possible changes in the cells associated with current measurements $z_i\left(k\right)$ with respect to the initial map $M_{i-1}\left(k\right)$. Confirmation procedures are necessary to address the localisation errors, even if limited, and possible noises that can affect the information retrieved by measurements. To simplify detection, all unknown cells in $M_{i-1}\left(k\right)$ are considered occupied during the measurement process to reduce to two the possible type of cell’s states. A rolling buffer $B_{c_j}$ of a fixed size $N_b$ is created for each cell $c_j\in C_{pi}$, and to record the outcomes of the evaluator blocks at different time instants. The Changed Cells Evaluator fills $B{c}_j$ with a "changed" flag only if the associated measurement $z_i\left(k\right)$ is classified as "detected" and the change is confirmed as described in Section 4.3. The Unchanged Cells Evaluator fills $B_{c_j}$ with an "unchanged" flag only if the associated measurement $z_i\left(k\right)$ is classified as "non-detected" and the evaluation is confirmed as described in Section 4.4. In the end, the Cell Update module changed between free and occupied the state of evaluated cells $c_j$ only if a sufficiently large amount of "changed" flags are in the associated buffer $B_{c_j}$, as described in Section 4.5. This module is the one responsible for the detection of changes in the environment while neglecting dynamic obstacles.

4.1. Beam Classifier

To detect a change in the environment, the outcomes of measurement $z_i\left(k\right)$ must be compared to the ones the robot would obtain if it were in an environment fully corresponding to what memorized in map $M_{i-1}\left(k\right)$. The correct hit point as in (2) is hence compared to the one computed with $z_{e,i}\left(k\right)\in {\mathbb{R}}^{+}$ that is the expected value for the i-th range measurement $z_i\left(k\right)$ obtained from the initial map $M_{i-1}$. A ray casting approach, [28], is hence used to compute the corresponding expected hit point $p_{e,i}\left(k\right)\in {\mathbb{R}}^2$:

$$\begin{array}{c}\hfill p_{e,i}=\left(\begin{array}{c}{x}_{e,i}\\ y_{e,i}\end{array}\right)=\tilde{p}+z_{e,i}\left(\begin{array}{c}cos({\theta }_0+i\Delta \theta +\tilde{\theta })\\ sin({\theta }_0+i\Delta \theta +\tilde{\theta })\end{array}\right).\end{array}$$

(3)

To detect a change, we do not directly compare the measured hit point $p_i\left(k\right)$ with the expected hit one $p_{e,i}\left(k\right)$. Indeed, we use a 1-to-N comparison strategy with a set of expected hit points $P_{e,i}\left(k\right)$ consisting of hit points along virtual neighbouring range measurements.

Note that (3) computes the expected hit point $p_{e,i}$ as the sum of an offset $\tilde{p}$ and a point with polar coordinates $(z_{e,i},{\theta }_0+i\Delta \theta +\tilde{\theta })$, the set $P_{e,i}$ is generated by adding different perturbations v to the angular second point. More in detail, the set $P_{e,i}=\left\{p_{e,i}^{(-n_e)},\cdots ,p_{e,i}^{\left(0\right)},\cdots ,p_{e,i}^{\left(n_e\right)}\right\}$ of $N=2n_e+1$ points is generated by adding a perturbation v to the angular components of $p_{e,i}$ in (3):

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill & p_{e,i}^{\left(l\right)}=\tilde{p}+z_{e,i}^{\left(lv\right)}\left(\begin{array}{c}cos({\theta }_0+(i+lv)\Delta \theta +\tilde{\theta })\\ sin({\theta }_0+(i+lv)\Delta \theta +\tilde{\theta })\end{array}\right),\hfill \end{array}\end{array}$$

(4)

where l is integer and $l=-n_e,\cdots ,0,\cdots ,n_e$, while $\left|v\right|<\frac{\Delta \theta }{n_e}$ and $n_e\in {\mathbb{N}}_0$ are design parameters. The distance $z_{e,i}^{\left(lv\right)}\in {\mathbb{R}}^{+}$ is defined, in an analogous way as $z_{e,i}$, as the measurement in $M_{i-1}$ along the virtual laser ray of amplitude $(i+lv)\Delta \theta$ with respect to the i-th real ray. Note that, v is chosen so that perturbations $(i+lv)\Delta \theta \in \left((i-1)\Delta \theta ,(i+1)\Delta \theta \right)$, and hence between the $i-1$-th and the $i+1$-th laser rays. An example of $v=\frac{\Delta \theta }{2}$, $n_e=1$ and hence, $N=3$ is reported in Figure 7).

For the 1-to-N comparison, each point $p_i$ is compared to the entire set $P_{e,i}$ by computing the minimum Euclidean distance between $p_i$ and the N points in $P_{e,i}$. Finally, a change is detected only if the minimum distance between $p_i$ and the expected points in $P_{e,i}$ is greater than a threshold $D_{t}h$:

$$\underset{p_{e,i}\in P_{e,i}}{min}{\parallel p_i-p_{e,i}\parallel }_2>D_{t}h.$$

(5)

As described in detail in Appendix A.1, the threshold can depend linearly on the distance $z_i$ in order to take into account errors coming from localisation and measurement noises.

The motivation for the 1-to-N comparison is represented in Figure 8: the removal of an obstacle (in red) creates a change in a portion of the map, and a false change detection associated with the hit point computation may occur. Figure 8 shows the hit point $p_i$ identifying a cell of the new map that has not changed compared to the previous map $M_i$. On the other hand, small localisation errors may lead to an associated $p_{e,i}$ identifying an occupied cell belonging to the obstacle in $M_{i-1}$. A direct comparison between $p_i$ and$p_{e,i}$ creates an incorrect change detection. This does not happen in Figure 8, where thanks to the 1-to-N comparison with the set $P_{e,i}$ the presence of a $p_{e,i}^{\left(k\right)}$ close to $p_i$ prevent the incorrect change detection.

4.2. Localisation Check

Changes in the map may have two drawbacks, first, they may prevent the robot from correctly localising itself in the changed map, second, as a consequence, large localisation errors may lead to an incorrect update of the map. Hence, monitoring the amount of localisation error is fundamental for a correct map update. For this purpose, the proposed dedicated Localisation Check module takes in input all the "detected change" measurements from the Beam Classifier and provides a quantification of the current localisation error. This module is important to make more robust the entire system by preventing corrupted maps update and by triggering a pose updating algorithm to decrease the localisation error. The idea is to evaluate the amount of localisation error and until this is below a given threshold the map can be updated neglecting the error. On the other hand, if it is too large, the robot is considered to be definitely lost and a pose update is requested to a dedicated algorithm. On the other hand, in between those two extreme cases and thresholds, it is possible to have a sufficiently large localisation error that would provide a corrupted map update but not so large to consider the robot definitely lost. In this case, the map update is interrupted and restarted once the localisation error decreases below the corresponding threshold.

More formally, given the number n of hit points in $P\left(k\right)$, derived from laser measurement $Z\left(k\right)$, and the number $n_{dc}\le n$ of the "detected change" measurements, the map is updated as described next if

$$n_{dc}\le D_{{ϵ}_{min}}·n$$

(6)

while the robot is considered lost but with the possibility of recovering the localisation and the map is not updated, if the following holds:

$$D_{{ϵ}_{min}}·n<n_{dc}<D_{{ϵ}_{max}}·n$$

(7)

Finally, the localisation system is considered not able to recover the pose and this must be updated by a dedicated algorithm if

$$n_{dc}\ge D_{{ϵ}_{max}}·n$$

(8)

where $D_{{ϵ}_{min}}\in [0,1]$ is the minimum fraction of acceptable changed measurements with respect to the number of laser beams that allows a good localisation performance, while $D_{{ϵ}_{max}}\in [0,1]$ is the maximum fraction of acceptable changed measurements with respect to the number of laser beams in addition to which the robot has definitively lost its localisation. The critical parameter to choose is $D_{{ϵ}_{min}}$ because it is a trade-off between the great change between the previous and current environments and the likelihood that you have actually lost your localisation. In this paper, the values $D_{{ϵ}_{min}}=0.75$ and $D_{{ϵ}_{max}}=0.9$ are considered based on the results of several experimental tests.

4.3. Changed Cells Evaluator

Let $state\left(c_j\right)\in \{free,occupied\}$ represents the occupancy state of a cell $c_j$ in the initial map $M_{i-1}$. Once a change is detected by the Beam classifier along the i-th range measurement $z_i$, it is necessary to evaluate whose states of cells in $C_{p_i}=\left\{\begin{array}{ccc}{c}_1,& \cdots ,& c_n\end{array}\right\}$ must be modified, i.e. the cells passed through by i-th laser ray. The Changed cells evaluator is the module that determines which are the cells in $c_j\in C_{p_i}$ involved in the detected change. Once a change is confirmed in a cell, the module stores a “changed” flag in the buffer of each cell. The status of a cell will be changed based on how many “changed” flags are in its buffer at the end of the procedure, as described in Section 4.5. The two possible events that cause a change in the environment are illustrated in Figure 9: the appearance of a new obstacle (Figure 9) and the removal of an existing one (Figure 9). A combination of these two events may also occur (Figure 9). The following paragraphs will describe how the cells in $C_{p_i}$, i.e. along the beam, are checked by the Changed cells evaluator module in the occurrence of such events. A distinction between the cell $c\left(p_i\right)$ and all other cells along the ray, i.e. $c_j\in C_{p_i}\setminus c\left(p_i\right)$, is necessary for the evaluation as described next.

4.3.1. Change detection of cells in $C_{p_i}\setminus c\left(p_i\right)$

A first evaluation is conducted on the cells along the ray, except for $c\left(p_i\right)$ that corresponds to the hit point $p_i$. Indeed, all those cells should have a free status, since they correspond to points of the laser beam that are not hit points. To increase algorithm robustness, we choose to not modify the status of the cells corresponding to the laser beam final chunk, since such cells may correspond to points that actually lay on obstacles due to localisation errors. For this reason, only cells between the robot’s estimated position $\tilde{p}$ and a point $p_{t}h$ that lay on the laser beam (i.e. on the segment with extremes $\tilde{p}$ and $p_i$) are evaluated, see Figure 10. More formally, given a fixed parameter $m\in [0,1]$ and $p_{t}h=m\tilde{p}+(1-m)p_i$, we analyse only the cells $c_j\in C_{p_i}$ that satisfy:

$$\parallel p_{t}h-p_i{\parallel }_2<{\parallel p_{c_j}-p_i\parallel }_2,$$

(9)

where $p_{c_j}\in {\mathbb{R}}^2$ is the point in the centre of the cell $c_j$. If a cell $c_j$ satisfies (9) with $status\left(c_j\right)=\mathit{occupied}$ it means that an obstacle has been removed with respect to $M_{i-1}$ and a “changed” flag is hence added to the cell’s buffer to report this change.

4.3.2. Change detection of $c\left(p_i\right)$

The status of $c\left(p_i\right)$ must also be evaluated. However, in this case, the events of a newly added obstacle and of a removed one must be distinguished. Indeed, in an ideal situation, when a new obstacle is added the hit point $p_i$ lies on the new obstacle, and hence its associated cell $c\left(p_i\right)$ has a free status in $M_{i-1}$, and hence it should be changed to occupied, see Figure 11. On the other hand, when an obstacle is removed, the hit point $p_i$ lies on another obstacle or part of the environment hence its associated cell $c\left(p_i\right)$ has an occupied status in $M_{i-1}$, and hence it should not be changed, see Figure 11.

Unfortunately, in case of localisation errors it may occur that the (not ideally computed) cell $c\left(p_i\right)$ corresponding to the measured hit point may be free in both cases of added or removed obstacles, see Figure 12. Hence, in case of localisation errors, the distinction among added or removed obstacles is not possible with a direct evaluation of single-cell occupancy. On the other hand, in case of added obstacle also all cells sufficiently close to $c\left(p_i\right)$ are free in $M_{i-1}$ while in case of a removed one some of those cells would be occupied and hence a distinction between the two cases is possible if closed cells status are also checked, see Figure 13.

To conclude, a new "changed" flag is effectively added to the buffer of $c\left(p_i\right)$ if the added obstacle is detected, i.e. the state of neighbouring cells is free, more formally if:

$$\begin{array}{c}\hfill state\left(c\left(q\right)\right)=free,\forall q\in {\mathbb{R}}^2\mid {\parallel q-p_i\parallel }_2<tol\left(z_i\right),\end{array}$$

where $q\in {\mathbb{R}}^2$ is a point in the space and $tol\in {\mathbb{R}}^{+}$ is a function of the measured range as $D_{t}h$ in (5).

4.4. Unchanged Cells Evaluator

In case the Beam Classifier does not detect changes along the i-th range measurement $z_i$, none of the maps cells $c_j\in C_{p_i}$ associated with $z_i$ has undergone any changes with respect to the initial map $M_{i-1}$. To take into account localisation errors, a similar procedure to the one described in the previous section can be applied, and only cells between the robot’s estimated position $\tilde{p}$ and the point $p_{th}$ that lay on the laser beam are evaluated. In particular, for such cells, the Unchanged Cells Evaluator add an “unchanged” flag to their buffer. Also for cell $c\left(p_i\right)$ localisation errors must be taken into account. Indeed, since the Beams Classifier did not detect any change, the status of cell $c\left(p_i\right)$ should be occupied in map $M_{i-1}$ but localisation and noise errors may lead to a measured hit point $p_i$ with associated free cell and nearby occupied cells. Therefore, if $state\left(c\left(p_i\right)\right)=free$, an “unchanged” flag is added to the buffers of all occupied cells adjacent to $c\left(p_i\right)$. Otherwise, an “unchanged” flag is added only to the buffer of $c\left(p_i\right)$.

4.5. Cells Update

Once each ray $z_i$ has been processed by the Beam classifier and cells along the ray have been evaluated by the two previously described modules, the Cells Updating module is responsible for updating/changing the state of each cell $c_j$ in $C_{p_i}$ for all measurements $z_i$ with an update rate that depends on both linear and angular speed of the robot. For each cell $c_j$, this update is based on the occurrence of “changed” flags in the corresponding buffer $B_{c_j}$. Let $N_b$ denote the size of the rolling buffers, and let $n_{c_j}\in \{0,\cdots ,N_b\}$ count the occurrences of the “changed” flag in $B_{c_j}$. A cell’s occupancy state is changed only if $n_{c_j}$ exceeds a given threshold, which balances the likelihood of false positives with the speed of map updates and change detection. This threshold, and $N_b$ itself, are critical parameters; the threshold is used to make the cell updated status more robust with respect to dynamic events in the environment while $N_b$ represents the memory of past measurements. Indeed, with a good balance in the choice of such parameters, using this approach, highly dynamic obstacles are initially detected as changes in measurements but discarded during cell update if there is an insufficient number of “changed” flags in the corresponding buffer, and hence are not considered in the new map $M_i$.

It is worth noting, that a change of cell state from occupied to free can lead to a partial removal of an obstacle. Referring to Figure 14, cells corresponding to the border of an obstacle can hence be set as free leading to an updated map $M_i$ with obstacles without borders characterized by measurable cells with unknown state, see Figure 14 where black cells are occupied while red ones are unknown. To avoid this, the state of those cells must be set to occupied, leading to the creation of the new border as in Figure 14. At this point, the cell state update procedure is finished, and the new map $M_i$ can be created. To do so, it is important to recall that cells in the initial map $M_{i-1}$ with “unknown” state have been considered, and treated, as occupied cells in the Beam Classifier module. However, if their state has not been changed to occupied to recreate an obstacle border, they are represented in $M_i$ still as “unknown” cells.

4.6. Pose Updating

The Pose Updating module is activated when the Localisation Check one detects that the robot is lost with no possibility to recover its pose. To avoid possible collisions, when the localisation error is too high, the robot is stopped until the end of the pose updating process.

The module takes as input: the last estimated pose of the localisation algorithm ${\tilde{X}}_{\mathcal{R}}\left(k\right)$, the last updated map $M_i\left(k\right)$, and the current laser measurement $Z\left(k\right)$. To retrieve the robot pose, the idea is to compare what the robot currently sees, i.e. the laser measurements $Z\left(k\right)$, with what it would see if it were in the last correctly estimated pose ${\tilde{X}}_{\mathcal{R}}\left(k\right)$, i.e. the expected point cloud $P_{exp}\left(k\right)$ computed from the last updated map $M_i\left(k\right)$ from the estimated pose. Practically, the Pose updating module estimates the rigid transformation $T\left(k\right)$ between the point cloud $P\left(k\right)$, obtained from $Z\left(k\right)$, and the point cloud $P_{exp}\left(k\right)$, computed from map $M_i\left(k\right)$ in the last correct pose ${\tilde{X}}_{\mathcal{R}}\left(k\right)$. To compute $P_{exp}\left(k\right)$, we used the same approach as in Section 4.1, while to estimate $T\left(k\right)$ it is possible to apply any methods that can be used to find a rigid transformation between two point clouds, e.g. Iterative Closest Point [29], or Coherent Point Drift [30].

5. Experiments and simulations

Results of simulations and experimental data of the proposed approach are now reported. First, we present a simulation example of how the localisation check module works in case condition (7) occurs, and therefore the map update is suspended. Then, we show the reason why this mechanism is important by comparing our new approach with our previous one [1] in a simulation example.

Subsequently, we provide a numerical validation carried out in a hundred simulation scenarios by giving a quantitative performance evaluation of the system in terms of map quality and localisation improvement. We compare the updated computed maps with their corresponding ground-truth ones based on quantitative metrics. Moreover, localisation accuracy with and without our updated maps are analysed through the Evo Python Package[31] and the uncertain covariance matrix associated with the estimated pose. Next, we present how the pose update module works in simulation confirming the robustness of the method, and in the end, we report the results performed during experiments in a real-world environment.

The code developed for the experiments is available at

https://github.com/CentroEPiaggio/safe_and_robust_lidar_map_updating.git, while the videos of the reported experiments are available in the multimedia material and at this linkhttps://drive.google.com/drive/folders/1Uwa3lXIQeyXnNTykkr46e6xlvpJbzlXD?usp=share_link.

5.1. Map benchmarking metrics

To evaluate the quality of our updated map with respect to the ground-truth ones, we adopt, as in [1], three metrics, reported here for reader convenience. All the ground-truth maps were built using the ROS Slam Toolbox package [32].

Let $m\left(q\right)$ and $\tilde{m}\left(q\right)$ be the occupancy probability of the cells that contain the point q in the maps M and, $\tilde{M}$ respectively. Furthermore, let $\nu$ the number of cells in the maps, and

$$\begin{array}{cc}\hfill \langle M\rangle =& \frac{1}{\nu }\sum _{m\left(q\right)\in M}m\left(q\right),\hfill \\ \hfill \langle M\tilde{M}\rangle =& \frac{1}{\nu }\sum _{\begin{array}{c}m\left(q\right)\in M,\tilde{m}\left(q\right)\in \tilde{M}\end{array}}(m\left(q\right)·\tilde{m}\left(q\right)),\hfill \\ \hfill \sigma \left(M\right)=& \sqrt{\frac{1}{\nu }{\sum }_{m\left(q\right)\in M}{(m\left(q\right)-\langle M\rangle )}^2}.\hfill \end{array}$$

1) Cross-correlation (CC). The Cross-correlation metric measures the similarity between two maps based on means of occupancy probabilities of the cells and is given by:

$$CC(M,\tilde{M})=100\left(\frac{\langle M\tilde{M}\rangle -\langle M\rangle ·\langle \tilde{M}\rangle }{\sigma \left(M\right)·\sigma \left(\tilde{M}\right)}\right),$$

2) Map score (MS). The Map score metric compares two maps on a cell-by-cell basis:

$$MS(M,\tilde{M})=100\left(1-\frac{1}{\nu }\sum _{\begin{array}{c}m\left(q\right)\in M\\ \tilde{m}\left(q\right)\in \tilde{M}\end{array}}{(m\left(q\right)-\tilde{m}\left(q\right))}^2\right),$$

(10)

taking into account only cells that are occupied in at least one map to avoid favouring the map with large free space.

3) Occupied Picture-Distance-Function (OPDF). The Occupied Picture-Distance-Function metric compares the cells of a map M to the neighbourhood of the corresponding cells in the other map $\tilde{M}$ and vice versa. The Occupied Picture-Distance-Function can be computed as:

$$OPD{F}_{as}(M,\tilde{M})=100\left(1-\frac{1}{{\nu }_M·r}\sum _{i=1\cdots {\nu }_M}^{{\nu }_M}{d}_i\right),$$

(11)

where ${\nu }_M$ is the number of occupied map cells in M, $d_i$ is the minimum between the search space amplitude r (e.g., a rectangle of width w and height h, $r=\sqrt{w^2+h^2}$) and the Manhattan-distance of each occupied cell of the first map M to the closest occupied cell on the second map $\tilde{M}$. Since the function in (11) is not symmetric, we consider the average of the OPDF distance function from M to $\tilde{M}$ and from $\tilde{M}$ to M as follows:

$$OPDF(M,\tilde{M})=\frac{OPD{F}_{as}(M,\tilde{M})+OPD{F}_{as}(\tilde{M},M)}{2}.$$

5.2. Experiments and Examples Procedure

All the experiments and examples conducted follow the same procedure:

1.

The robot is immersed in an initial world, usually denoted with $W_1$, and it is teleoperated to build an initial static map $M_1$ through the ROS Slam Toolbox package. Given $i=2$ the proposed map update procedure starts from 2.

2.

The world is changed to create $W_i$ similar to the previous world $W_{i-1}$.

3.

The robot autonomously navigates in the new environment by localising itself with the Adaptive Monte Carlo Localisation (AMCL) [33] using the previous static map $M_{i-1}$, while our approach provides a new updated map $M_i$.

4.

Increase i by one and restart from 2.

For all the world created $W_i$ with $i=2,...,N$, a ground-truth map $G_i$ is built for the map quality comparison.

5.3. Simulation Environment

The laptop utilized for the simulations had an Intel Core i7-10750H CPU, 16 GB of RAM, and Ubuntu 18.04 as the operating system. To simulate a 290 square meter industrial warehouse, we employed models from Amazon Web Services Robotics [34]. These models were employed within the Gazebo simulator [35]. The specific robot used in the simulation was the Robotnik XL-Steel platform, which was equipped with two SICK s300 lidarsRobotnik xl-steel simulator, https://github.com/RobotnikAutomation/summit_xl_sim.

5.3.1. Localisation Check

The goal of the first simulation is to show how the procedure described in Section 4.2, for which the map update system is suspended due to excessive localisation errors, works. Let the robot operates in a world $W_2$ localising itself based on the map $M_1$ built while navigating in the previous world $W_1$, and let the proposed algorithm updates the static map to provide $M_2$. Simulations are reported for the world $W_1$ reported in Figure 15 with the corresponding built map $M_1$ in Figure 15. In this case, we suppose that the map has been built correctly and hence it can be considered as a ground-truth map, i.e., $G_1=M_1$. The changed worlds $W_2$ and its ground-truth map $G_2$ are reported in Figure 15 and Figure 15 respectively, where green circles are the added obstacles and red circles are the removed ones.

The system evolution is reported in Figure where the map update and the position of the robot in the world $W_2$ are reported at different time instants. In Figure 16 and Figure 16 the robot is immersed in the world $W_2$ with $X_{\mathcal{R}}\left(0\right)=[2.0,9.5,0]$ while the localisation filter use the map $M_1$ to localise the robot with an initial estimated pose ${\tilde{X}}_{\mathcal{R}}\left(0\right)=[2.0,9.0,0]$. In Figure 16 we can see: our initial updated map, $M_2\left(0\right)=M_1$, the robot in ${\tilde{X}}_{\mathcal{R}}\left(0\right)$ with the particles of the localisation filter represented as red arrows close to the robot and the laser measurement $Z\left(0\right)$ as red points. Observing Figure 16, it is possible to state that the robot is not localised correctly also because part of the laser measurements does not overlap with the edges of the map (see e.g. the laser beams indicated by the blue arrow). However, the presence of a part of the measurements overlapping the map edges correctly (indicated by the yellow arrow), suggests that the localisation system can still recover the pose. Under these conditions, the localisation check module suspends the map update, and the localisation error is kept monitored. Figure 16 and Figure 16 represent the system after around 10 seconds: the localisation error is reduced as expected, and this is deduced by the fact that the laser beams at the top left of the map start to overlap the borders. However, the localisation error is still in the boundaries in (7), and hence the map updating is still prevented. After 20 seconds, the situation is represented in Figures and , where the localisation system has recovered the right pose of the robot, and the map updating process has been started. Indeed, the robot recognizes that boxes in ellipses A, C, and E (in Figure 15) have been removed and part of the corresponding cells have been set to free (light gray in the figure). On the other hand, laser measurements (red points) are detecting the presence of boxes in ellipses B, D, and F (in Figure 15). Finally, the algorithm proceeds for another 40 seconds with the map updating module working properly. The situation is represented in Figures and where the previously detected left border of box B has been added to the map (cells in black).

To fully understand the necessity of such interruption in the map update process, we provide the comparison results of our old approach [1] (where the map was always updated) with the one proposed in this paper with an initial localisation error. The comparison is performed in the scenario reported in Figure 17: the first environment $W_1$ is in Figure 17, while the robot’s operating in world $W_2$, Figure 17. The ground-truth map of $W_2$ used for the comparison results is reported in Figure 20.

In this example, the robot is navigating in the world $W_2$, the localisation algorithm will use $M_1$ to provide the global estimated robot pose while both approaches will update the map providing $M_2$. As can be deduced from the red arrows and the not overlapping measurements with map borders in Figure 18, we intentionally mis-initialised the localisation filter with an error around $0.7m$. The robot will move in a straight line until the localisation algorithm recovers the right pose.

In the left column of Figure , we provide the updated map computed using the approach in [1] during the robot navigation. On the other hand, in the right column of Figure the maps created with the proposed approach are reported.

The final maps obtained from the two approaches are shown in Figure 20. From a qualitative point of view, we can note that our old approach provides a wrong updated map compared to the ground-truth one of Figure 20. Indeed, the method wrongly changed the status of some cells belonging to the walls and correctly updated the status of some cells, highlighted in yellow, but in a wrong position, as we can see comparing Figure 20 and . Instead, our new approach has started to update the map later, but the resulting map is more precise.

Furthermore, to quantify the increase in map quality with our new approach, we applied the metrics described in Section 5.1 in order to compare the two updated maps with the reference ground-truth $G_2$. The results are in Table 1 where a $100\%$ score is a full correspondence of the two maps. It is worth noting that not updating the obstacles’ internal cells affects the first two metrics, but not the third, which reports more accurate results. For each metric, the percentage value for the procedure proposed in previous work and the one introduced here with the localisation check module is reported. The updated map provided by our new approach with the localisation check module has, for each metric, a higher percentage value with respect to the one built without that module when compared with the ground-truth map, and this motivates the need for such module.

5.3.2. Numerical Validation

To validate our algorithm for updating maps, we created a hundred variations of the same environment. We introduced increasing changes to mimic how the arrangement of goods in a warehouse can evolve over time. In the initial environment, denoted as $W_1$, Figure 15, the robot was manually operated to construct an appropriate initial map $M_1$. In the remaining environments, denoted as $W_i$, where i ranges from 2 to 100, the robot autonomously followed a predetermined trajectory (shown in Figure 22) to simulate the placement of materials within the warehouse. The 50th and final environments are depicted in Figure 21 and Figure 21, respectively.

In each scenario $Wi$, where i ranges from 2 to 100, the robot utilized the map $Mi-1$ as the initial map for self-localisation and generated an updated map $M_i$ using the proposed updating method. Since we simulated a material deployment task in an industrial scenario, we assumed that the initial pose of the robot is known, albeit with minor localisation errors. Thus, it is assumed that in all scenarios, the localisation system can recover the pose, and only condition (7) may occur.

To assess the effectiveness of the method, we obtained ground-truth maps $G_i$ for each world $W_i$ (where i ranges from 1 to 100) using the Slam Toolbox.

1) Updating Performance. The initial map $M_1$ associated with $W_1$ and the planned trajectory can be seen in Figure 22All maps have dimensions of 13.95m x 20.9m with a resolution of 5 cm.

A qualitative evaluation of our updating method is presented in Figure 23, which compares the ground-truth maps $G_i$ (for i equal to 50 and 100) with their corresponding updated maps $M_i$ (also for i equal to 50 and 100). It is important to note that in the ground-truth maps (depicted on the left side), the cells within obstacles are marked as unknown (light grey). However, in the maps $M_{50}$ and $M_{100}$, those cells are designated as free (white). This distinction arises because the obstacles were not present in the initial map $M_1$ but were later introduced. As a result, these cells are physically unobservable by the robot’s lidar, and there is no need to update them since they do not impact autonomous navigation.

This qualitative comparison validates that our technique effectively detects environmental changes, with each map accurately reflecting the simulated scenario.

To perform a quantitative assessment, refer to Figure 24, which provides a comparison between the initial map $M_1$ and our updated maps $M_i$ (where i ranges from 2 to 100) in relation to the ground-truth maps $G_i$ (where i ranges from 2 to 100). This evaluation employs the metrics described in Section 5.1, where a perfect correspondence between the two maps corresponds to a score of 100

In order to measure the differences between the current and initial environments, a map comparison is conducted between $M_1$ and $G_i$, as indicated by the blue data in Figure 24,Figure 24, and Figure 24. As expected based on each metric, the updated map $M_i$ consistently outperforms the initial map $M_1$ when compared to the ground-truth.

It is worth noting that the predefined trajectory of the robot is not specifically designed to explore the warehouse area, but rather to simulate item deployment. Consequently, there is a possibility that the measurements may overlook environmental changes that are not observable along the path of the robot’s movement.

2) Localisation Performance.

To assess the enhancements in localisation performance resulting from the utilization of updated maps, we compared the AMCL pose estimate based on two different maps: the initial map $M_1$ and the most recent available updated map $M_{i-1}$. These estimates were compared with the reference ground-truth obtained from the simulator.

The results are shown in terms of mean ($\mu$) and variance ($\sigma$) for each world. Figure 25 depicts the comparison of both the estimated position errors and the maximum position errors. As anticipated, utilizing an updated map led to a reduction in localisation errors.

Furthermore, we examined the uncertainty associated with the estimated robot pose by calculating the trace and the maximum eigenvalue ($\lambda$) of the covariance matrix (P) related to the estimated pose. As illustrated in Figure 26, the utilization of an updated map significantly decreased the uncertainty in the robot’s pose.

It is important to highlight that despite the presence of localisation errors, the Localisation Check played a crucial role in achieving excellent results in terms of map updating.

3) Hardware Resource Consumption. In this section, we provide the hardware resources of the CPU percentage and memory MB utilisation computed by the ROS package “cpu_monitor”cpu_monitor, https://github.com/alspitz/cpu_monitor during the map update and localisation phases in terms of $\mu$ and $\sigma$. Figure 27 depicts the percentage of CPU used in each simulation, whereas Figure 27 depicts the memory utilisation per map update. The figures demonstrate that the proposed solution is a memory-limited algorithm suited for long-term operations.

5.4. Case of compromised localisation

For the numerical validation, we assumed that the robot’s initial pose was known to reflect the conditions of an industrial task as closely as possible. However, aware that the robot could still get lost, we have also developed a pose update procedure to ensure our map update process can continue properly. In this section, though, we give an example of what happens when the robot gets lost and what is actually the condition (8) for which the map update system is suspended due to localisation errors, and the localisation system can’t recover the pose. In this example, we use the same worlds and maps of Sec Section 5.3.1, but this time the filter is initialized with an initial estimated pose in ${\tilde{X}}_{\mathcal{R}}\left(0\right)=[1.8,7.5,0]$ with an error of around $1.5m$, as we can see from Figure 28 and Figure 28. Since the localisation error is too big, the localisation system is unable to recover the robot pose and the condition (8) is verified. At this point, the map update process is suspended, and the robot continues to move. As we can see from the system evolution in Figure 28 and Figure 28, the robot gets lost, and the map $M_2\left(20\right)$ has not been updated and hence coincides with $M_2\left(0\right)$ but it does not represent the world $W_2$. In order to make the system robust to such occurrences, a dedicated pose recovery module has been developed and integrated with the map updating process, as described next.

5.4.1. Pose Updating

We present two cases to show how our pose update algorithm works and makes the map updating process more robust. In both cases, during robot navigation, to activate the pose update module the localisation error is forced in the scenario by manually setting a wrong robot’s estimated pose through a dedicated ROS service.

In the first case, we tested the algorithm in a condition where the map used by the robot to localise itself reflects the current environment $W_2$, i.e. $M_2\equiv G_2$. This experiment was conducted to assess the performance of the pose updating algorithm module in nominal conditions, e.g. the best-case scenario. During navigation, the robot’s estimated pose is manually changed through the ROS service, see the green arrow in Figure 29. As desired, the new pose violates condition 8 as represented by the red arrows in Figure 29, and the robot gets lost. Finally, the Pose Update module is activated and the robot pose is correctly retrieved, see Figure 29.

In the second case, instead, we tested the algorithm using a possibly non-correct map as the proposed approach described in Section 5.2. Indeed, the robot uses the previously updated map $M_1$ to localise itself, while the localisation check, the map updating, and the pose updating modules are active to produce an updated map $M_2$. The whole evolution of the system is reported in Figure 30 where the last built map $M_1$ does not reflect the current world $W_2$, i.e. $M_1\ne G_2$ as visible in Figure 30. After a while the robot’s estimated pose is manually changed through the ROS service, see the green arrow in Figure 30 and the map updating process is suspended due to the high localisation errors introduced. Once the robot estimated pose becomes even larger, see red arrows in Figure 30, the Pose Update module is activated, and it recovers the right global robot pose, Figure 30, and the map updating process can be restarted. As can be appreciated from Figure 30 and Figure 30 the maps are the same and hence have not been corrupted by the presence of localisation errors.

5.5. Real World Environment

To evaluate our newly proposed system in a real-world setting, we utilized bag files containing sensor measurements and odometry topics from our previous work. The experiments were conducted in a laboratory environment (depicted in Figure 31) using a Summit-XL-Steel mobile platform equipped with two 2D-LIDAR Hokuyo-UST-20LX sensors.

We recreated four distinct environments by introducing changes in the positions of obstacles. The testing environment had an approximate size of 80 square meters. The robot constructed the initial map $M_1$ (shown in Figure 31), and the ground-truth maps $G_i$ (where i ranges from 2 to 4) were obtained through Slam Toolbox while performing teleoperated navigation at a speed of $0.15m/s$.

Similar to the simulations, we quantified the performance of map updates and the utilization of hardware resources. Since there was no external ground-truth tracking system available, we compared the uncertainty in the estimated pose obtained using both the initial map $M_1$ and the most recently updated maps $M_{i-1}$ to evaluate the localisation performance. Given the real-world conditions, the localisation system was initialized with a robot pose in close proximity to the true pose.

5.5.1. Updating Performance

The qualitative and quantitative outcomes can be observed in Figure 32All maps have dimensions of 10.5m x 8.55m with a resolution of 5 cm. and Table 2. The observations made in SubSection 5.3.2 regarding the metric findings are applicable in this context as well. Despite the presence of noisy real data and higher localisation errors, the proposed method consistently generates updated maps that surpass the initial map when compared to ground-truths. This highlights the capability of the system to dynamically add and remove obstacles without affecting the walls, thanks to the Localisation Check.

5.5.2. Localisation Performances

The analysis of localisation uncertainty has been conducted in worlds $W_3$ and $W_4$ as reported in Figure 33 and Figure 34. However, we must stress that the low improvement percentage is due to the similarity between the worlds. Unfortunately, it was not possible to increase the differences between worlds due to the robot dimensions with respect to the available area. However, results are encouraging and validate the approach even in dynamic real-world environments.

5.5.3. Hardware Resource Consumption

The CPU utilization and memory usage are illustrated in Figure 35. Due to the smaller size of the real environment and the limited number of environmental changes compared to the simulation, the CPU consumption decreases from 15% to 7%, and the memory usage decreases from 55-58Mb to 52-55Mb. These reductions are observed when comparing the real environment to the simulated environments.

6. Discussion

The aim of this work was to extend our previous work in order to update the map robustly with respect to big localisation and measurement errors. Although our previous method gave promising results in terms of map quality and localisation improvement, in Sec Section 5.3.1 we presented the big limitation of that approach: without a perfect localisation system, the map updating process provides a corrupted map. We have therefore developed a safety mechanism to pause the map update in case of big localisation errors. This mechanism is related to the number of detected changes measurements in the current environment compared to the previous map, as shown in Section 5.3.1. As numerical validation confirmed, the mechanism was sufficient to prove both the map quality and localisation improvement. It is worth noting that, to the authors’ best knowledge, as in all other available approaches, in case of substantial changes in the environment the proposed system would detect numerous detected change measurements and would not update the map even in the absence of localisation errors. The only solution, in this case, would be to recompute the map from scratch.

From the hardware resource consumption point of view, the proposed algorithm does not require high computation capabilities. Indeed, differently from other approaches such as those based on graphs, the resource consumption does not depend on the robot’s travelled path but only on the laser scan measurements processing. In this case, the CPU usage can be further significantly reduced by simply discarding some range measurements from the processed laser scan. Moreover, the memory usage depends only on the number of changed cells and the size $N_b$ of the buffers and not on the environment dimension. Concluding, as both the numerical validation and real-data experiments confirmed, the proposed memory-limited solution is suitable for life-long operation scenarios.

So far, we used only the lidar measurements to update the map, since they are the sensors more common to build a 2D occupancy grid map. As future work, we plan to extend our system to manage also 3D sensors in order to update the 3D occupancy grid map. Moreover, the validation of the localisation performance in a real environment could be improved with an external tracking system as a benchmark.

7. Conclusions

In the present study, we proposed a robust strategy for dealing with dynamic situations in long-term operations based on occupancy grid maps. The goal was to improve our prior work in order to update the map reliably in the face of large localisation and measurement mistakes. Extensive simulations and real-world trials have been used to validate the approach. Because of the fail-safe technique devised, the updated maps exhibit no symptoms of drift or inconsistency even when the localisation error is relatively substantial. Furthermore, they mirror the setup of the environment and improve the AMCL localisation performance in simulations. In addition, we proved that our system is able to correctly detects when the map updating should be suspended and, if the robot is lost, update the estimated robot pose accordingly. Simulation and experiments have been conducted to validate the approach in different and challenging dynamic environments.

Appendix A.1

To correctly classify the "detected change measurement", we choose a linear function for the distant threshold that considers the localisation and measurement noise errors. Indeed, due to these errors, there will always be a certain distance between a measured hit point $p_i$ and the closest expected hit point $p_{e,i}$. An incorrect choice of $D_{t}h$ can lead to a wrong classification, and the error in the estimated yaw $\tilde{\theta }$ can affect this classification process. From (2) it is clear that an error $\delta \theta \in \mathbb{R}$ in the yaw estimation will produce a certain error $\delta p_i\in {\mathbb{R}}^2$ in the computation of the measured hit point $p_i$ in a fixed frame. The true yaw $\overline{\theta }\in \mathbb{R}$ and the true hit point ${\overline{p}}_i\in {\mathbb{R}}^2$ are defined as:

$$\begin{array}{cc}\hfill & \overline{\theta }=\tilde{\theta }+\delta \theta ,\hfill \end{array}$$

(A1)

$$\begin{array}{cc}\hfill & {\overline{p}}_i=p_i+\delta p_i.\hfill \end{array}$$

(A2)

From (2), (A1) and ():

$$\begin{array}{cc}\hfill \delta p_i& ={\overline{p}}_i-p_i=\hfill \\ \hfill & =\tilde{p}+z_i\left(\begin{array}{c}cos({\theta }_0+i\Delta \theta +\tilde{\theta }+\delta \theta )\\ sin({\theta }_0+i\Delta \theta +\tilde{\theta }+\delta \theta )\end{array}\right)+\hfill \\ \hfill & -\tilde{p}-z_i\left(\begin{array}{c}cos({\theta }_0+i\Delta \theta +\tilde{\theta })\\ sin({\theta }_0+i\Delta \theta +\tilde{\theta })\end{array}\right)=\hfill \\ \hfill & =z_i\left(\begin{array}{c}cos({\theta }_0+i\Delta \theta +\tilde{\theta }+\delta \theta )-cos({\theta }_0+i\Delta \theta +\tilde{\theta })\\ sin({\theta }_0+i\Delta \theta +\tilde{\theta }+\delta \theta )-sin({\theta }_0+i\Delta \theta +\tilde{\theta })\end{array}\right)=\hfill \\ \hfill & =z_i\left(\begin{array}{c}-2sin\left(\frac{\delta \theta }{2}\right)sin(\frac{\delta \theta }{2}+{\theta }_0+i\Delta \theta +\tilde{\theta })\\ 2sin\left(\frac{\delta \theta }{2}\right)cos(\frac{\delta \theta }{2}+{\theta }_0+i\Delta \theta +\tilde{\theta })\end{array}\right).\hfill \end{array}$$

(A3)

Finally, assuming that the yaw estimation error $\delta \theta$ is small:

$$|\delta p_i|=z_i·2\left|sin\frac{\delta \theta }{2}\right|\approx z_i·\left|\delta \theta \right|.$$

Since the effect of an error on the yaw estimation is amplified by the measured range, the threshold $D_{t}h$ can be taken as a function of the measured range $z_i$. Hence the choice of a saturated linear function:

$$D_{t}h\left(z_i\right)=min(m_{t}h·z_i+q_{t}h,D_{max})$$

where $m_{t}h\in {\mathbb{R}}^{+}$, $q_{t}h\in {\mathbb{R}}^{+}$, $D_{m}ax\in {\mathbb{R}}^{+}$. In this way, hit points associated with small range measurements are more likely to be flagged as `inconsistent with the map’ detecting tiny changes. On the other side, the classification of hit points associated with big range measurements is more robust to yaw estimation error.