**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 (Ts) 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 ≤ *cth* ≤ 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 (*Cth*). Depending on the nature of the environment, a threshold time (*tth*) is chosen in which the confidence decays to *cth* value and the time in which the confidence decays to zero is (*tz*). To model the confidence decay, the following family of curves are chosen.

$$c = 1 - \left(\frac{t\_{th}}{t\_z}\right)^n \tag{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 (*tth*) and after that it decays quickly to zero in *tz* time. For a given *cth*, *tth* and *tz*, the value of the degree of the curve (*n*) can be found by solving Equation (1) as,

$$\left(\frac{t\_{th}}{t\_z}\right)^n = 1 - c\_{th\nu} \quad \text{( $0 \le c\_{th} \le 1$ )},$$

$$n \log\left(\frac{t\_{th}}{t\_z}\right) = \log(1 - c\_{th}), \tag{2}$$

$$\implies n = \frac{\log(1 - c\_{th})}{\log\left(\frac{t\_{th}}{t\_z}\right)}.$$

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 *cth* = 0.55 and *tz* = 600 s, for varying values of *tth* between 300 s to

600 s. It can be seen that the corresponding values of *n* can be found for different *tth* 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,

$$t = e^{\frac{1}{n}\log(1-c) + \log(t\_z)}.\tag{3}$$

The time remaining (*trem*) to reach the threshold time (*tth*) is,

$$t\_{rm} = t\_{th} - e^{\frac{1}{n}\log(1-c) + \log(t\_z)} \,\tag{4}$$

**Figure 7.** Obstacle confidence decay function. *cth* = 0.55, *tz* = 600 s. Effects of uncertainty are not considered and Ufactor = 0.

#### *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 *tth* 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 (Σ*t*),

$$
\Sigma\_t = \begin{bmatrix}
\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{bmatrix} \tag{5}
$$

If the uncertainty given by Σ*<sup>t</sup>* is large, the confidence falls down faster and vice-versa. Hence, the confidence decay is modeled as,

$$\text{Confidence decay} \propto \frac{1}{\text{Uncertainty given by } \Sigma\_t}.\tag{6}$$

The eigenvalues (*λ*1, ··· *λn*) and eigenvectors (*v*1, ··· *vn*) of the matrix Σ*<sup>t</sup>* denotes the magnitude of the variance. Two largest eigenvalues *λ*<sup>1</sup> and *λ*<sup>2</sup> control the decay of confidence. The threshold time after uncertainty integration *t th* is given as,

$$\mathbf{t}\_{th}^{'} = \mathbf{t}\_{th} - \frac{\Psi}{\sqrt{\lambda\_1^2 + \lambda\_2^2}},\tag{7}$$

where, Ψ is a controlling factor. The new confidence *c* is given as,

$$\begin{split} c' &= 1 - \left(\frac{t\_{th}^{'}}{t\_z}\right)^n\\ &= 1 - \left(\frac{t\_{th} - \frac{\Psi}{\sqrt{\lambda\_1^2 + \lambda\_2^2}}}{t\_z}\right)^n\\ &= 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{\Psi}{2}} - \left\{t\_{th} \left(\lambda\_1^2 + \lambda\_2^2\right)^{\frac{1}{2}} - \Psi\right\}^n\right]. \end{split} \tag{8}$$

The degree of the curve is given as,

$$\begin{split} n' &= \frac{\log(1 - \epsilon\_{th}')}{\log\left(\frac{t\_{th}'}{t\_z}\right)}, \\ \Longrightarrow n' &= \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)}. \end{split} \tag{9}$$

Figure 8 shows the results of integrating the spatial uncertainty in the obstacle confidence decay time for various values *cth* = 0.55, *tz* = 600 s and different values of the uncertainty factory (Ufactor). In Figure 8, Ufactor represents,

$$\text{Ufactor} = \frac{\Psi}{\sqrt{\lambda\_1^2 + \lambda\_2^2}},\tag{10}$$

where, values *λ*<sup>1</sup> and *λ*<sup>2</sup> 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.

**Figure 8.** Obstacle confidence decay function with uncertainty integration. With more positional uncertainty, the confidence falls below the threshold confidence fast. In all the cases, *cth* = 0.55 and *tz* = 600 s.

#### **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 (*Kt*), robot's pose (*μt*) and the covariance (Σ*t*) at time *t* until step 12. Later, Eigen values (*λ*<sup>1</sup> ··· *λn*) are extracted from the covariance matrix (Σ*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.
