On Sharing Spatial Data with Uncertainty Integration Amongst Multiple Robots Having Different Maps

Abstract

Information sharing is a powerful feature of multi-robot systems. Sharing information precisely and accurately is important and has many benefits. Particularly, smart information sharing can improve robot path planning. If a robot finds a new obstacle or blocked path, it can share this information with other remote robots allowing them to plan better paths. However, there are two problems with such information sharing. First, the maps of the robots may be different in nature (e.g., 2D grid-map, 3D semantic map, feature map etc.) as the sensors used by the robots for mapping and localization may be different. Even the maps generated using the same sensor (e.g., Lidar) can vary in scale or rotation and the sensors used might have different specifications like resolution or range. In such scenarios, the ‘correspondence problem’ in different maps is a critical bottleneck in information sharing. Second, the transience of the obstacles has to be considered while also considering the positional uncertainty of the new obstacles while sharing information. In our previous work, we proposed a ‘node-map’ with a confidence decay mechanism to solve this problem. However, the previous work had many limitations due to the decoupling of new obstacle’s positional uncertainty and confidence decay. Moreover, the previous work applied only to homogeneous maps. In addition, the previous model worked only with static obstacles in the environment. The current work extends our previous work in three main ways: (1) we extend the previous work by integrating positional uncertainty in the confidence decay mechanism and mathematically model the transience of newly added or removed obstacles and discuss its merits; (2) we extend the previous work by considering information sharing in heterogeneous maps build using different sensors; and (3) we consider dynamic obstacles like moving people in the environment and test the proposed method in complex scenarios. All the experiments are performed in real environments and with actual robots and results are discussed.

1. Introduction

Mobile robots are increasingly being used to automate many tasks; tasks which are mostly dull, dangerous, or demanding are a good fit for autonomous robots. The industrial sector has already benefited a lot from ‘factory robots’. Recently, a new class of robots called ‘service robots’ have been increasing. These robots are used to provide several common services like cleaning and delivering, dispatching and moving items. These service robots are also used for specific tasks like patrolling and escorting people. Generally, multiple robots are used for such tasks in large service areas as there are several advantages. One of the major advantages of using multiple robots is wide area coverage. Multiple robots can cover a large area and perform several tasks simultaneously. Task parallelism is possible as different robots can perform different tasks at the same time. Some robots may be cleaning, some patrolling, while others may be delivering items to specific locations. Fault tolerance is another advantage of multi-robot systems. Even if one of the robots goes out of service the entire service does not stop as other robots can finish the task. Moreover, with task coordination, multiple robots can perform the task efficiently and quickly.

However, with the introduction of multiple robots in a system, there are several challenges which need to be addressed. Among these problems, effective communication between the multiple robots is a major challenge. Communication forms the basis of other major modules like task coordination, task distribution and collective execution. Accurate and content rich information is important for the successful execution of many tasks.

Although there are many benefits of sharing spatial information in a multi-robot system, in this paper, we consider the case of sharing obstacle information in a multi-robot system. The environments at many service places, like hospitals and warehouses, are very dynamic with moving entities and new obstacles. To navigate autonomously in such environments, robots need a map of the environment and need to localize themselves within it. This is generally achieved through a SLAM (Simultaneous Localization and Mapping) [1] module. Generally, if one robot finds a new obstacle in the environment, it only updates its own map. The other robots do not benefit from this knowledge. However, if the robot shares this knowledge with other robots along with updating its map, other robots can update their maps and plan better paths with the real-time information. This is shown in Figure 1, in which, Robot $\mathrm{R}1$ finds a new obstacle and blocked path at the center passage of the service area and shares the spatial coordinates of the obstacle with other robots $\mathrm{R}2,\cdots ,\mathrm{R}5$ which can use this information in generating optimal trajectories. The extension of use-case scenarios other than obstacle information sharing is straightforward.

Related Works

There is a plethora of previous works related to sharing information in multi-robot systems. Sharing corresponding matches of an object by two robots to calculate an accurate relative localization over time is proposed in Reference [2]. Work in Reference [3] proposes sharing visual information. In Reference [4], task negotiation between multiple robots by sharing information is proposed to decide the sequence in which the tasks should be performed by different robots. Work in Reference [5,6] proposes a protocol to share the region of interest between robots for efficient task cooperation. In-fact, multi-robot sport activities like Robo-soccer [7,8] heavily relies on meaningful information sharing between robots to achieve a common goal. Virtual pheromones have been proposed to be used for coordinating master-slave robots in References [9,10]. Path planning of multiple robots using information from external security cameras is proposed in Reference [11]. In addition, a direct obstacle coordinate information sharing was proposed in our previous work [12] without considering the uncertainty. However, this is a limitation as in practical systems there is always some uncertainty associated with robot’s localized information and mapped obstacle’s position due to sensor errors. RoboEarth [13,14,15] is another platform which heavily uses information exchange through cloud.

Such information sharing has huge merits in robot path planning. Path planning is an active area of research and in the context of multi-robot systems path-planning has shown promising advantages through information sharing between robots. Multi-robot collision avoidance has been discussed in Reference [16]. Work in Reference [17] presents a mechanism in which robots share information about their remaining battery power and accordingly avoid collision by giving priority to a robot with less battery power over the shortest path. An interesting approach of collaborative navigation through visual-servoing is presented in Reference [18,19] which heavily relies on reliable and efficient inter-robot communication to share information. The proposed work focuses on multiple robots sharing information about the dynamic changes in the remote area of the environment. This enables the robots to use updated and timely information to efficiently plan their paths. Information sharing among multiple robots for efficient path planning usually involves a decentralized approach [20] in which each robot calculates its path individually and decisions to change paths or avoid obstacles is done later based on the received messages from other robots. This is unlike centralized path planners [21] in which all the paths of all the robots are calculated simultaneously. In Reference [22], a motion planner is proposed for multiple robots with limited ranges of sensing and communication to reach the goal in dynamic environments. In Reference [23], a navigational technique for multiple service robots in a robotic wireless network (RWN) is presented in which robots download map information from map servers for safe navigation. Semantic information is used among multiple robots for efficient task coorindation in Reference [24].

In Reference [25], a practical case of multi-robot navigation in warehouse has been discussed. The proposed work also deals with the positional uncertainty of robots and obstacles. In this context, a decentralized approach for collaboration between multiple robots in presence of uncertainty are considered for robot action in Reference [26]. A review of multi-robot navigation strategies can be found in References [27,28,29].

The proposed work is an extension of our previous work [12]. Our previous work proposed the idea of a ‘Node-Map’ and obstacle’s confidence decay mechanism. However, there were many limitations which are addressed in this extended work. The new major contributions are:

• Uncertainty Integration in the Improved Confidence Decay Mechanism: The previous work [12] did not consider the amount of estimated positional uncertainty of obstacles in the confidence decay. Both were decoupled entities. However, this was a serious drawback in the previous work because irrespective of the amount of positional uncertainty, confidence of all the obstacles decayed at the same rate. This caused several false map updates corresponding to dynamic obstacles which generally have large uncertainty associated. In the extended work, we have mathematically modeled the integration of positional uncertainty in the confidence decay mechanism. This is discussed in ‘Section 4.1 Integrating Uncertainty in Confidence Decay Mechanism’.
• New Experiments with Heterogeneous Maps with Different Sensors: Another shortcoming of the previous work was that it only worked with the same type of 2D grid-maps made with the same type of sensors. However, in the extended work, we include new experiments with heterogeneous maps (3 dimensional RGBD map and 2D grid-map) made from different sensors. In this regard, the merits of using the ‘node-map’ as a means of smoothly sharing information coherently between heterogeneous maps are also discussed. This is discussed in ‘Section 6.1 Experiments with Heterogeneous Maps’.
• New Experiments in Dynamic Environment with Moving People and Testing Under Pressure: The previous work only worked with static obstacles. In the extended work, new experiments have been performed to test the method when people are randomly moving in the vicinity of the robot and obstructing its navigation. In this regard, the tight coupling of new obstacle’s uncertainty in the confidence decay mechanism plays a vital role to avoid false map-updates corresponding to the dynamic obstacles. This is discussed in ‘Section 6.2. Results with Dynamic Entities (Moving Obstacle)’.

The comparison of the previous work with the extended work is summarized in

Table 1. Comparison of this extended work with the previous work [12].

Feature	Previous Work [12]	Extended Work
Sharing New Obstacle’s Position Information	Yes	Yes
Consideration of Positional Uncertainty of Obstacles	No	Yes
Confidence Decay Mechanism	Yes	Yes
Uncertainty Influence Over Confidence Decay	No	Yes
Experiments in Very Dynamic Environment (e.g., Moving People)	No	Yes
Robots have Different Types of Sensors	No	Yes
Tests with Heterogeneous Maps	No	Yes

. In addition, the proposed work discusses the algorithm to generate the T-node map.

The paper starts by first explaining the correspondence problem in different maps in Section 2. The node-map representation is explained in Section 3. Section 4 briefly explains obstacle removal and update in the nodemap and Section 4.1 explains the integration of positional uncertainty in the confidence decay mechanism. Further, using this coupling with Extended Kalman Filter is explained in Section 5 with detailed algorithm. The experimental results are discussed in Section 6. Section 6.1 explains about the experiments with heterogeneous maps and Section 6.2 discusses the results with dynamic entities (moving people). Finally, Section 7 concludes the paper.

2. Correspondence Problem in Different Maps

In dynamic environments, the new objects in the environment could be the temporary or new permanent obstacles. Both needs to be estimated in the map for correct path planning. A robot estimates the absolute position ($x_{obs},y_{obs}$) of an obstacle in its map through its SLAM module. This estimation also has an uncertainty (${\Sigma }_{obs}$) associated with it which arises mainly from sensor errors. This information about the new obstacle ($x_{obs},y_{obs},{\Sigma }_{obs}$) is difficult to be directly shared with other robots.

A common problem occurring in multi-robot systems is information sharing in different types of maps (e.g., 2D grid-map, 3D semantic map, feature map etc.) made from different sensors used by the robots for mapping and localization. Even the maps generated using the same sensor (e.g., Lidar) can vary in scale or rotation and the sensors used might have different specifications like resolution or range. In such scenarios, the ‘correspondence problem’ in different maps is a critical bottleneck in information sharing. Moreover, the uncertainty of localization also adversely affects the information sharing. In other words, it is important to consider how to easily correspond local spatial information in one map to spatial information in a separate map of different type or scale while considering the uncertainty.

This is graphically explained in Figure 2. There is a scale difference between Map1 and Map2. Whereas, Map2 and Map3 differ by a rotation factor. A spatial obstacle information, for example, position ($x^1,y^1$) will correspond to different spatial coordinates in Map2 and Map3. In most real world scenarios, these scale and rotation differences are generally not known. Some previously proposed techniques [30] to find the necessary translation and rotation can be applied to transform the spatial information in one map to another. However, the computation costs are expensive and could introduce undesired delays.

Although the example in Figure 2 is simplified for illustration, in actual scenarios different maps may have different levels of noise and even feature dimensions. Moreover, some robots may only have a partial map information. Similarly, Figure 3 discusses the problem of robots having different types of maps [31]. Figure 3a is a map in the form of a graph, Figure 3b is a dense 3D map, while Figure 3c is the gridmap of the same environment. It is difficult to for the robots to correlate spatial information in such different types of maps.

In the proposed work, it is assumed that the robots work in the common service area whose map is available to the robots. This map itself could be heterogenous, for example, grid-map, RGBD map and so forth, which is built using different sensors mounted on different robots. Moreover, the maps could be build from different anchor points. Thus, different robots could have heterogeneous maps.

3. ‘T-Node’ Map Representation

A T-node representation of the map has been proposed in our previous work [12]. We briefly explain the T-node map and how obstacles are represented in it. Later, we describe how path planning is done on the node map. It is assumed that each robot is also assigned a unique robot-id (${\mathrm{R}}_{\mathrm{id}}$) and the robots are on the same network to exchange messages with each other.

A node is defined as a point of turn in a path of the map. The paths are represented as a network of these nodes in the map. Figure 4a shows the node representation of the environment shown in Figure 1. Notice that, the nodes ${\mathrm{n}}_1,{\mathrm{n}}_2,\cdots ,{\mathrm{n}}_{12}$ are the points of turns in the map. The terminal nodes are shown in red color in Figure 4a. Nodes are connected to each other through edges. Figure 4b shows the T-node map with an obstacle placed between the nodes ${\mathrm{n}}_9$ and ${\mathrm{n}}_3$. The distance between the nodes ${\mathrm{n}}_9{\mathrm{n}}_3$ is L and the distance of the obstacle from node ${\mathrm{n}}_3$ is x, which can easily be estimated using an on-board distance sensor.

A table stores T-node map’s information viz. traversable/blocked edges (paths) and any changes at the edges. All the robots have access to this table. In the context of Figure 4b, the corresponding information is shown in

Table 2. T-node map information corresponding to Figure 4b.

Node Path	New Obstacle	Path Blocked	Meta-Data
$n_1{n}_2$	0	0	-
⋯	⋯	⋯	⋯
$n_3{n}_9$	1	1	{ d:$({\mathrm{d}}_{\mathrm{x}},{\mathrm{d}}_{\mathrm{y}})$,w:$w_{obs}$,h:$h_{obs}$,$\Sigma$:(${\sigma }_x,{\sigma }_y$), ${\mathrm{T}}_{\mathrm{s}}$ }
$n_9{n}_{12}$	0	0	-

. The table contains a set of four information about each path: (1) binary information of whether a new obstacle is found on an edge, (2) a binary information if the path is blocked and cannot be traversed, (3) details of the obstacle if the path is changed and (4) the timestamp (${\mathrm{T}}_{\mathrm{s}}$) when the information was updated. The details of the information will vary according to the type of the sensor used. For example, in case of Lidar, the obstacle information will contain: the obstacle coordinates from the node (${\mathrm{d}}_{\mathrm{x}},{\mathrm{d}}_{\mathrm{y}}$), dimensions of the obstacle like width (${\mathrm{w}}_{\mathrm{obs}}$) and height (${\mathrm{h}}_{\mathrm{obs}}$) and the positional uncertainty associated in estimating the obstacle (${\sigma }_x,{\sigma }_y$). The uncertainty information comes from the SLAM module used in the robot. As shown in T-node map of Figure 4b, only one of the edges $n_3{n}_9$ is obstructed. This information is reflected in Table 2. It is possible that a new obstacle is found on a path, however the path could be still be traversed. A blocked path cannot be traversed by the robot.

Each robot has a copy of this table which has small memory requirement as the meta-data for only the changed paths are required. Moreover, information is communicated to other robots only when some path information is changed. A T-node representation makes it easier for a robot to share information with other robots. The local maps maintained by the two robots might differ by some rotation, translation or scale. As an example, Figure 5a shows the section of the map of Figure 4b with obstacles. Figure 5b shows a scaled version, Figure 5c a rotated version and Figure 5d a scaled and rotated version of Figure 5a. However, the nodes on the paths remains the same and information that there is an obstacle on one of the edges is still conveyed clearly from Table 2 which maintains the details of the obstacles. In addition, with a T-node representation a global map is not required. Even for a large number of nodes, only those edges which are changed is communicated to the robots. The small data size ensures fast and reliable communication with small communication bandwidth.

The T-node map can be generated by maneuvering the robot in the environment and setting points of turns (where the robot turns by around 90 degrees) as nodes. Automatic generation of T-node map is also possible if a map is available. For example, if there is a grid-map with obstacles (black), open (white) and unknown (grey) areas, the first step is to generate a binary image of the grid-map which is done by turning all unknown cells to blocked (black) value. This is shown in Figure 6a. Noise is removed by successively applying morphological erode and dilate operations [32,33]. The next step is to apply skeletonization algorithm [34,35]. Many skeletonization and thinning algorithms generate unnecessary tentacles which needs to be removed using pruning algorithm [36]. Result of showing skeletonization on binary map of Figure 6a is shown in Figure 6b. Line segments are then detected using techniques like SVD and Hough Transform [1]. The end-points of segments which are within a small threshold distance ($\delta$) can be clustered [37] using k-means [38,39], fuzzy c-means [40] or density based clustering methods [41] into a single node as shown in Figure 6c,d. A graph ‘N’ of these nodes $\{n_1,n_2,\cdots ,n_m\}$ form the T-node map of the environment. The pseudo-code is given in Algorithm 1.

Algorithm 1: T-node-map Generation

It should be noted that a ‘node’ is merely a point of turn in the navigational graph. It does not include any feature information (e.g., corners, line, color, etc.) of the map. Hence, map-merger on T-Node map is not possible. However, traditional methods [42,43] can be used to first merge feature-rich maps and thereby T-node map. Moreover, since the characteristics of the navigational paths are different for unmanned ground vehicles (UGVs) and unmanned aerial vehicles (UAVs), this work does not consider the case of heterogeneous robots. Only ground robots are considered and the proposed method will work well for both differential drive robots and skid-steer drive robots.

4. Obstacle Removal and Update in T-Node Map

This section briefly discusses about modeling the transience of obstacles first proposed in our previous work [12]. The new contribution of this extended work lies in integrating the positional uncertainty in the decay mechanism which is discussed in the next Section 4.1. The obstacles in the passages may be permanent or temporary and removed after some time. Regardless of the transience of the obstacles, all the robots update their respective T-node map. Newly added obstacles can only be re-confirmed if a robot actually visits the area near the obstacle. Hence, once an obstacle information on a particular node has been updated and communicated to other robots, robots update their map and if that obstacle is found again, the timestamp is updated. However, there is no upper time bound of when a robot would actually visit the particular location and update its map. The obstacle might already have been removed by that time. This problem needs to be modeled mathematically.

A timestamp (${\mathrm{T}}_{\mathrm{s}}$) is maintained for each obstacle representing the time at which the obstacle was last seen. If an obstacle has recently been added to the map and a short time has elapsed since its addition, then the probability that it has not been removed is high. On the contrary, if a lot of time has elapsed since the addition of the obstacle, the probability that it still exists in the map is less. We model a confidence (c) measure which represents this probability $0\le c_{th}\le 1$. The maximum value of confidence is 1 and its value decreases with time. The robots assumes that the obstacle still exists in the map until the corresponding confidence has not dropped to below a threshold confidence ($C_{th}$). Depending on the nature of the environment, a threshold time ($t_{th}$) is chosen in which the confidence decays to $c_{th}$ value and the time in which the confidence decays to zero is ($t_z$). To model the confidence decay, the following family of curves are chosen.

$$c=1-{\left(\frac{t_{th}}{t_z}\right)}^n$$

(1)

The curves given by Equation (1) have the desired characteristic that for higher values of n, the curve flattens out more and delays confidence decay until the threshold time ($t_{th}$) and after that it decays quickly to zero in $t_z$ time. For a given $c_{th}$, $t_{th}$ and $t_z$, the value of the degree of the curve (n) can be found by solving Equation (1) as,

$$\begin{array}{c}\hfill {\left(\frac{t_{th}}{t_z}\right)}^n=1-c_{th},(0\le c_{th}\le 1),\\ \hfill nlog\left(\frac{t_{th}}{t_z}\right)=log(1-c_{th}),\\ \hfill \Rightarrow n={\displaystyle \frac{log(1-c_{th})}{log\left(\frac{t_{th}}{t_z}\right)}}.\end{array}$$

(2)

Figure 7 shows the curves for the decay function given by Equation (1) for various values of n. The Ufactor in Figure 7 shows the uncertainty factor which is discussed in Section 4.1. The various curves have been generated for $c_{th}=0.55$ and $t_z=600$ s, for varying values of $t_{th}$ between 300 s to 600 s. It can be seen that the corresponding values of n can be found for different $t_{th}$ according to Equation (2). Moreover, as the value of n increases, the decay curves flatten out more taking more time to reach the threshold time and then quickly decrease to zero.

For a given instantaneous value of confidence c, the elapsed time t is calculated from Equation (1) as,

$$\begin{array}{c}\hfill t=e^{\frac{1}{n}log(1-c)+log(t_z)}.\end{array}$$

(3)

The time remaining ($t_{rem}$) to reach the threshold time ($t_{th}$) is,

$$t_{rem}=t_{th}-e^{\frac{1}{n}log(1-c)+log(t_z)}.$$

(4)

4.1. Integrating Uncertainty in Confidence Decay Mechanism

Uncertainty in obstacle’s position affects the rate of confidence decay. If there is a large uncertainty in obstacle’s spatial position, the threshold time $t_{th}$ is reduced by an uncertainty factor. In case of no uncertainty, Equations (1)–(4) are used. In SLAM, the obstacle’s uncertainty in state is generally represented by the covariance matrix (${\Sigma }_t$),

$$\begin{array}{c}\hfill {\Sigma }_t=\left[\begin{array}{ccc}{\sigma }_x^2& {\sigma }_{xy}& {\sigma }_{x\theta }\\ {\sigma }_{xy}& {\sigma }_y^2& {\sigma }_{y\theta }\\ {\sigma }_{x\theta }& {\sigma }_{y\theta }& {\sigma }_{\theta }^2\end{array}\right]\end{array}$$

(5)

If the uncertainty given by ${\Sigma }_t$ is large, the confidence falls down faster and vice-versa. Hence, the confidence decay is modeled as,

$$\mathrm{Confidence}\mathrm{decay}\propto {\displaystyle \frac{1}{\mathrm{Uncertainty}\mathrm{given}\mathrm{by}{\Sigma }_t}}.$$

(6)

The eigenvalues (${\lambda }_1,\cdots {\lambda }_n$) and eigenvectors ($\overrightarrow{v_1},\cdots \overrightarrow{v_n}$) of the matrix ${\Sigma }_t$ denotes the magnitude of the variance. Two largest eigenvalues ${\lambda }_1$ and ${\lambda }_2$ control the decay of confidence. The threshold time after uncertainty integration $t_{th}^{\prime }$ is given as,

$$t_{th}^{\prime }=t_{th}-{\displaystyle \frac{\Psi }{\sqrt{{\lambda }_1^2+{\lambda }_2^2}}},$$

(7)

where, $\Psi$ is a controlling factor. The new confidence $c^{\prime }$ is given as,

$$\begin{array}{cc}\hfill c^{\prime }& =1-{\left({\displaystyle \frac{t_{th}^{\prime }}{t_z}}\right)}^n\hfill \\ & =1-{\left({\displaystyle \frac{t_{th}-{\displaystyle \frac{\Psi }{\sqrt{{\lambda }_1^2+{\lambda }_2^2}}}}{t_z}}\right)}^n\hfill \\ & =t_z^{-n}{\left({\lambda }_1^2+{\lambda }_2^2\right)}^{\frac{-n}{2}}\left[t_z^n{\left({\lambda }_1^2+{\lambda }_2^2\right)}^{\frac{n}{2}}-{\left\{t_{th}{\left({\lambda }_1^2+{\lambda }_2^2\right)}^{\frac{1}{2}}-\Psi \right\}}^n\right].\hfill \end{array}$$

(8)

The degree of the curve is given as,

$$\begin{array}{cc}& n^{\prime }={\displaystyle \frac{log(1-c_{th}^{\prime })}{log\left(\frac{t_{th}^{\prime }}{t_z}\right)}},\hfill \\ \hfill \Rightarrow & n^{\prime }={\displaystyle \frac{log\left(1-t_z^{-n}{\left({\lambda }_1^2+{\lambda }_2^2\right)}^{-\frac{n}{2}}\left[t_z^n{\left({\lambda }_1^2+{\lambda }_2^2\right)}^{\frac{n}{2}}-{\left\{t_{th}{\left({\lambda }_1^2+{\lambda }_2^2\right)}^{\frac{1}{2}}-\Psi \right\}}^n\right]\right)}{log\left(t_{th}{\left({\lambda }_1^2+{\lambda }_2^2\right)}^{\frac{1}{2}}-\Psi \right)-log\left(t_z{\left({\lambda }_1^2+{\lambda }_2^2\right)}^{\frac{1}{2}}\right)}}.\hfill \end{array}$$

(9)

Figure 8 shows the results of integrating the spatial uncertainty in the obstacle confidence decay time for various values $c_{th}=0.55$, $t_z$ = 600 s and different values of the uncertainty factory (Ufactor). In Figure 8, Ufactor represents,

$$\mathrm{Ufactor}={\displaystyle \frac{\Psi }{\sqrt{{\lambda }_1^2+{\lambda }_2^2}}},$$

(10)

where, values ${\lambda }_1$ and ${\lambda }_2$ capture the amount of estimated uncertainty. In Figure 8, Ufactor is given as a factor of threshold time. It can be seen that for more uncertainty, the curve starts to fall faster to the threshold time. Appropriate values of Ufactor can be chosen depending on different scenarios. Moreover, this value can also be changed dynamically.

The obstacle confidence decay mechanism ensures a smooth robot operation in multi-robot system where multiple robots frequently inform each other about the new obstacle information. If a robot receives an obstacle information update from another robot while it is navigating towards its goal location, then it would have to stop and update its map information which consumes time and computation. To avoid this, a check is performed to see if the information received affects the current navigation towards the goal. This is easily achieved by checking the blocked flag of the corresponding edge. If the blocked flag is set to 1 and the current navigational path is affected, the timestamp and other meta-data for the blocked edge are checked. Based on the obstacle’s confidence value, path re-planning or continuation on the same path can be decided according to the priority of the task at hand.

A major benefit of tightly coupling the obstacle’s uncertainty with confidence decay mechanism is minimizing the false map updates corresponding to the dynamic obstacles in vicinity. Generally, the uncertainty of dynamic obstacles is larger than that of static obstacles estimated by the underlying SLAM module. In the absence of uncertainty integration, confidence all the obstacles irrespective of their positional uncertainty decays at the same rate. Therefore, if there is a false map update corresponding to a dynamic obstacle (like moving people), it decays at the same rate like other fixed obstacles. This increases the chances of false map updates due to dynamic obstacles. However, with the uncertainty integration, the confidence of obstacles with larger positional uncertainty decay faster than those with less uncertainty. In effect, this allows minimizing false map updates, as they decay out quickly. This also prevents false notifications to other robots.

5. Uncertainty Integrated Confidence Decay Mechanism with Extended Kalman Filter

The integration of confidence decay meachanism in EKF is given in Algorithm 2. The algorithm is straightforward and estimates the Kalman gain ($K_t$), robot’s pose (${\mu }_t$) and the covariance (${\Sigma }_t$) at time t until step 12. Later, Eigen values (${\lambda }_1\cdots {\lambda }_n$) are extracted from the covariance matrix (${\Sigma }_t$) by applying Singular Value Decomposition. The degree of the confidence curve is then determined using the amount of uncertainty represented by the Eigen values in steps 14 and 15 of Algorithm 2 as explained in the previous section. Essentially, the degree of the curve is chosen to fasten the confidence decay inversely proportional to the positional uncertainty.

6. Experimental Results

This section presents the results of the experiments. The extended work discuss information sharing in heterogeneous maps made with different sensors and tests the proposed method under pressure with dynamic obstacles in the vicinity of robots.

We used Pioneer-P3DX [44] and Kobuki Turtlebot [45] robot shown in Figure 9a. Both the robots are wheeled differential drive robots and the motion model is explained in our previous work [12]. Both the robots used ROS [46] on Ubuntu computer and were on the same network to communicate with each other.

Algorithm 2: Uncertainty Integrated Confidence Decay with Extended Kalman Filter
1	#$x_t$: robot state,$v_t,{\omega }_t$: translation and rotational velocity.
	$x_t={\left[xy\theta \right]}^T$
2	# EKF uses Jacobian to handle non-linearity.$G_t$: Jacobian of motion function w.r.t state
	$G_t\leftarrow \left[\begin{array}{ccc}1& 0& -\frac{v_t}{{\omega }_t}cos\theta +\frac{v_t}{{\omega }_t}cos(\theta +{\omega }_t\Delta t)\\ 0& 1& -\frac{v_t}{{\omega }_t}sin\theta +\frac{v_t}{{\omega }_t}sin(\theta +{\omega }_t\Delta t)\\ 0& 0& 1\end{array}\right]$
3	#$V_t$: Jacobian of motion w.r.t control
	$V_t=\left[\begin{array}{cc}\frac{-sin\theta +sin(\theta +{\omega }_t\Delta t)}{{\omega }_t}& \frac{v_t(sin\theta -sin(\theta +{\omega }_t\Delta t))}{{\omega }_t^2}+\frac{v_t(cos(\theta +{\omega }_t\Delta t)\Delta t)}{{\omega }_t}\\ \frac{cos\theta -cos(\theta +{\omega }_t\Delta t)}{{\omega }_t}& -\frac{v_t(cos\theta -cos(\theta +{\omega }_t\Delta t))}{{\omega }_t^2}+\frac{v_t(sin(\theta +{\omega }_t\Delta t)\Delta t)}{{\omega }_t}\\ 0& \Delta t\end{array}\right]$
4	#$M_t$: Covariance of noise in control space.${\alpha }_1,\cdots ,{\alpha }_4$: Error-specific parameters.
	$M_t=\left[\begin{array}{cc}{\alpha }_1{v}_t^2+{\alpha }_2{\omega }_t^2& 0\\ 0& {\alpha }_3{v}_t^2+{\alpha }_4{\omega }_t^2\end{array}\right]$
5	#$\overline{{\mu }_t}$: Prediction updates in state.
	$\overline{{\mu }_t}={\mu }_{t-1}+\left[\begin{array}{c}\frac{-v_t}{{\omega }_t}sin\theta +\frac{v_t}{{\omega }_t}sin(\theta +{\omega }_t\Delta t)\\ \frac{v_t}{{\omega }_t}cos\theta -\frac{v_t}{{\omega }_t}cos(\theta +{\omega }_t\Delta t)\\ {\omega }_t\Delta t\end{array}\right]$
6	#$\overline{{\Sigma }_t}$: Prediction updates in covariance.
	$\overline{{\Sigma }_t}=G_t{\Sigma }_{t-1}{G}_t+V_t{M}_t{V}_t^T$
7	#$\overline{Q_t}$: Covariance of the sensor noise.
	$Q_t=\left[\begin{array}{ccc}{\sigma }_r^2& 0& 0\\ 0& {\sigma }_{\varphi }^2& 0\\ 0& 0& {\sigma }_s^2\end{array}\right]$
8	#${\left[m_{ix}{m}_{iy}\right]}^T$: coordinates of the ith landmark.$z_t^i$: measurement. q: squared distance.
	$q={(m_{k,x}-{\overline{\mu }}_{t,x})}^2+{(m_{k,y}-{\overline{\mu }}_{t,y})}^2$
	${\widehat{z_t}}^k=\left[\begin{array}{c}\sqrt{q}\\ atan2(m_{k,y}-{\overline{\mu }}_{t,y},m_{k,x}-{\overline{\mu }}_{t,x})-{\overline{\mu }}_{t,\theta }\\ m_{k,s}\end{array}\right]$
9	#$H_t$: Jacobian of measurement with respect to state.
	$H_t^k=\left[\begin{array}{ccc}-\frac{m_{k,x}-{\overline{\mu }}_{t,x}}{\sqrt{q}}& -\frac{m_{k,y}-{\overline{\mu }}_{t,y}}{\sqrt{q}}& 0\\ \frac{m_{k,y}-{\overline{\mu }}_{t,y}}{q}& -\frac{m_{k,x}-{\overline{\mu }}_{t,x}}{q}& -1\\ 0& 0& 0\end{array}\right]$
10	#$S_t$: Measurement covariance matrix.
	$S_t^k=H_t^k\overline{{\Sigma }_t}{\left[H_t^k\right]}^T+Q_t.$
11	#$j(i)$: likely correspondence after applying maximum likelihood estimate.
	$j(i)=\mathrm{argmax}\frac{1}{\sqrt{\det (2\pi S_t^k)}}{e}^{-\frac{1}{2}{(z_t^i-{\widehat{z}}_t^k)}^T{\left[S_t^k\right]}^{-1}(z_t^i-{\widehat{z}}_t^k)}$
12	#$K_t$: Kalman gain,${\mu }_t$: state,${\Sigma }_t$: covariance.
	$K_t^i={\overline{\Sigma }}_t{\left[H_t^{j(i)}\right]}^T{\left[S_t^{j(i)}\right]}^{-1}$
	${\mu }_t={\overline{\mu }}_t+K_t^i(z_t^i-{\widehat{z}}_t^{j(i)})$
	${\Sigma }_t=(I-K_t^i{H}_t^{j(i)}){\overline{\Sigma }}_t$
13	# Apply Singular Value Decomposition and get Eigen-values${\lambda }_i$:
	${\lambda }_1,\cdots {\lambda }_n=\mathrm{svd}({\Sigma }_t)=\mathrm{svd}\left(\left[\begin{array}{ccc}{\sigma }_x^2& {\sigma }_{xy}& {\sigma }_{x\theta }\\ {\sigma }_{xy}& {\sigma }_y^2& {\sigma }_{y\theta }\\ {\sigma }_{x\theta }& {\sigma }_{y\theta }& {\sigma }_{\theta }^2\end{array}\right]\right)$
14	#n: degree of decay curve,$t_{th}$: threshold time,$c_{th}$: threshold confidence,$t_z$: time to decay to zero.
	$n={\displaystyle \frac{log(1-c_{th})}{log\left(\frac{t_{th}}{t_z}\right)}}.$
15	#$n^{\prime }$: degree of decay curve with uncertainty integrated,$\Psi$: decay control factor.
	$n^{\prime }={\displaystyle \frac{log\left(1-t_z^{-n}{\left({\lambda }_1^2+{\lambda }_2^2\right)}^{-\frac{n}{2}}\left[t_z^n{\left({\lambda }_1^2+{\lambda }_2^2\right)}^{\frac{n}{2}}-{\left\{t_{th}{\left({\lambda }_1^2+{\lambda }_2^2\right)}^{\frac{1}{2}}-\Psi \right\}}^n\right]\right)}{log\left(t_{th}{\left({\lambda }_1^2+{\lambda }_2^2\right)}^{\frac{1}{2}}-\Psi \right)-log\left(t_z{\left({\lambda }_1^2+{\lambda }_2^2\right)}^{\frac{1}{2}}\right)}}$

6.1. Experiments with Heterogeneous Maps

In this section, we describe the results of the proposed method with heterogeneous maps. The environment for experiments is shown in Figure 9b,c. The dimensions of the environment are shown in Figure 9d. The environment had two static obstacles ‘Obstacle1’ and ‘Obstacle2’ marked in Figure 9d. The start and goal positions are marked as ‘S’ and ‘G’, respectively, in Figure 9d. The T-node map is shown in Figure 9e.

The two robots used in the experiment were both equipped with 2D Lidar and RGBD sensors. As shown in Figure 9a, the Pioneer P3DX robot was equipped with a Sick-Lidar of 10 m range and ASUS Xtion-Pro RGBD camera. Turtlebot was equipped with a Hokuyo Lidar of 20 m range and a Kinect RGBD camera.

To test the proposed method with heterogeneous maps, Pioneer P3DX robot was programmed to use only the RGBD sensor to build a 3D map, and navigate in the environment. Pioneer P3DX first started navigation from location ‘S’ to the goal location ‘G’ as shown in Figure 10a. A* algorithm [47] and SHP algorithm [48,49] were used for path planning and path smoothing, respectively. As soon as the P3DX robot started moving, a long new obstacle was placed in the environment as shown in Figure 10b, well outside the range of the sensor. As shown in Figure 10c, the person moves in front of the robot and blocks its way purposefully. The details of dynamic obstacle are discussed in the next Section 6.2. P3DX perceives the new obstacle and alters its path towards the goal as shown in Figure 10d–f, while also updating the map with the newly added obstacle. P3DX was programmed to come back to its initial position ‘S’ and the navigation in shown in Figure 10g,h.

The updated 3D map build by P3DX robot is shown in Figure 11a. In this experiment, Turtlebot used only the 2D Lidar sensor with 2D gridmap. Hence, P3DX could not directly share the 3D point-cloud information due to the heterogeneous maps used by the two robots. By using the T-node map, P3DX blocked the path between nodes ${\mathrm{n}}_6$ and ${\mathrm{n}}_7$ and shared this information with the Turtlebot to plan appropriate path. More information regarding the dimensions of the new obstacle could also be shared for better path planning. Hence, the 3D information was converted to a 2D information to be shared with Turtlebot. Grid maps are the most commonly used 2D maps in which each grid-value represents whether the grid is occupied, free or unknown. The 3D point-cloud were projected to the ground which was detected using a RANSAC based plane detection [50]. This 2D information was shared by P3DX robot with Turtlebot and the updated T-node map is shown in Figure 11b. In the updated T-node map of Figure 11b, the obstacle is placed between the nodes ${\mathrm{n}}_6$ and ${\mathrm{n}}_7$ blocking it.

Turtlebot was programmed to navigate from the same start location ‘S’ to the goal location ‘G’. In the absence of the proposed information sharing mechanism, the path planned by Turtlebot would be (Figure 11b),

$$\mathrm{S}\to {\mathrm{n}}_2\to {\mathrm{n}}_5\to {\mathrm{n}}_6\to {\mathrm{n}}_7$$

The Turtlebot would encounter a new obstacle between the nodes ${\mathrm{n}}_6$ and ${\mathrm{n}}_7$ and would have to re-plan a new path towards the goal. However, with the proposed information sharing mechanism, Turtlebot could directly plan a path considering the newly added obstacle and the planned path was (Figure 11b),

$$\mathrm{S}\to {\mathrm{n}}_2\to {\mathrm{n}}_5\to {\mathrm{n}}_6\to {\mathrm{n}}_9\to {\mathrm{n}}_8\to {\mathrm{n}}_7.$$

Notice that, this appropriate path was generated by Turtlebot ‘remotely’ before actually encountering the new obstacle. Figure 12 shows the navigation of Turtlebot after considering the new obstacle. The entire navigation is illustrated between Figure 12a–h. In particular, it can be seen from Figure 12e–g, that Turtlebot maintains a safe threshold from the start itself. Turtlebot itself updated its map using the attached Lidar and the updated grid-map is shown in Figure 13.

The dimensions of the obstacles in the experiment are given in

Table 3. Obstacle Dimensions in the Experiment.

Obstacle	Length × Width × Height
Obstacle1	40 cm × 40 cm × 68 cm
Obstacle2	50 cm × 35 cm × 50 cm
Newly Added Obstacle	300 cm × 5 cm × 100 cm

.

The decay curve is shown in Figure 14. In the experiment, $C_{th}$ was set to 0.45 and $t_z$ to 20 min. Based on the uncertainty of the obstacle, the Ufactor was calculated as approximately 15% of $t_z$. Figure 14 shows the decay of confidence considering the uncertainty of the obstacles.

Figure 15 shows different decay curves for different amounts of estimated positional uncertainties of the new obstacle. Although Figure 14 shows the actual decay curve of the experiment, Figure 15 shows theoretical values for different values of uncertainty. Figure 15a–d shows the confidence decay with increasing uncertainty of 35%, 45%, 55% and 65%, respectively. It can be seen that, for increasing uncertainty, the curve decays much faster, as desired.

Thus, the T-node enables robots to share information across heterogeneous maps. Indeed, there is a need to transform the newly added obstacle’s information to spatial coordinates but it can easily be achieved in real-time. Moreover, to avoid the overheads of such computation for time-critical applications, only the blocked/un-blocked information could also be shared. Using the same approach, information among other type of maps could be shared effectively.

6.2. Results with Dynamic Entities (Moving Obstacle)

The proposed method was tested under complex scenarios by purposefully moving a person in front of the robot and blocking its way. This is shown in Figure 16. As P3DX robot started navigation from start location ‘S’ to goal location ‘G’, a person blocked its way by randomly moving in front of the robot. This is shown in Figure 16a–j.

Similarly, the path of P3DX robot was blocked again while it was navigating back from the goal location ‘G’ to its start location ‘S’. This is shown in Figure 17. The person randomly moved in front of the robot blocking its path as shown in Figure 17a–j.

In both the cases of Figure 16 and Figure 17, the robot attempted to avoid collision and planned alternate trajectories or stopped if the person stands dangerously close to the robot. Moreover, in both the cases, the robot did not update the map corresponding to the person as a new obstacle in the map. This is because the positional uncertainty corresponding to the moving obstacle was large as calculated by Algorithm 2. Even if the person is falsely identified as a new obstacle and the map is updated, it has no adverse effects in the proposed method, as uncertainty is integrated in the confidence decay mechanism. Any wrong map update corresponding to dynamic obstacles has high probability of larger positional uncertainty corresponding to the dynamic obstacle and therefore a quicker decay given by Equation (6), (7) and (9). On the other hand, for static new obstacles in the map, the underlying SLAM (Algorithm 2) algorithm estimates smaller positional uncertainty and therefore a larger decay time, ensuring its permanence in the map.

Thus, uncertainty integration has two merits in the information sharing scheme. First, it acts a filter for wrong map updates corresponding to the dynamic obstacles in the environment through a quick confidence decay. Second, it ensures that only the correct information is shared with other robots corresponding to the new static obstacles. It should be noted that the dynamic detection of moving people can be done using image processing for camera-based sensors [51], RGBD sensors [52] or leg detector for Lidar-based sensors [53] and integration of such approaches [54] will increase the robustness of the system.

7. Conclusions

Information sharing is a powerful technique which has many potential benefits in path planning of multi-robot systems. A node-map was proposed in our previous work to solve the problems of information sharing in different robots. We extended our previous work by integrating the positional uncertainty of the new obstacles in the confidence decay mechanism which models the transience of the obstacles. This minimizes false map updates and notifications in the system. New experiments were performed to share information about new obstacles in heterogeneous maps. The results shown that using the nodemap allows the robots to smoothly share the information. Moreover, since path planning is also done using the nodemap, efficient trajectories considering the position of new obstacles can be done in real-time. The information sharing mechanism allows the robots to obtain timely information about remote obstacles in the map without having to explicitly visit those areas. In addition, new experiments were performed to test the proposed mechanism in complex environments with moving people in the vicinity of the robots. Due to the tight coupling of uncertainty and decay mechanism, the dynamic obstacles could be filtered and avoided false update of the map. Even if there is some false update, the confidence corresponding to them decays fast due to larger uncertainty. Experiment results confirm that, in the long run in large environments employing multiple robots, the proposed method can improve the efficiency of the system in terms of shorter distance traveled by the robots and shorter planning time by eliminating path re-planning. In future, we will continue to test the robustness of the proposed method in more complex and realistic environments such as cafeterias and offices.