Long-Term Robot Navigation in Indoor Environments Estimating Patterns in Traversability Changes
Lorenzo Nardi Cyrill Stachniss

Quorum-sensing active particles with discontinuous motility
    Thomas Speck
    December 30, 2023
===========================================================

empty
empty




Nowadays, mobile robots are deployed in many indoor environments, such as offices or hospitals. These environments are subject to changes in the traversability that often happen by following repeating patterns. In this paper, we investigate the problem of navigating in such environments over extended periods of time by capturing these patterns and exploiting this knowledge to make informed decisions. Our approach incrementally estimates a model of the traversability changes from robot's observations and uses a probabilistic graphical model to make predictions at currently unobserved locations. In the belief space defined by the predictions, we plan paths that trade off the risk to encounter obstacles and the information gain of visiting unknown locations. We implemented our approach and tested it in different indoor environments. The experiments suggest that in the long run, our approach leads to navigation along shorter paths compared to following a greedy shortest path policy.







§ INTRODUCTION



Over the last decade, many mobile robots have been deployed in indoor environments such as offices, hospitals, and shopping malls. Most robot navigation systems rely on a static representation of the environment such as occupancy grid maps or topological maps for planning and navigating to targeted locations. In reality, mobile robots are often employed in environments that are subject to changes in traversability. Traditional navigation systems avoid obstacles by performing reactive strategies <cit.> or planning local deviations <cit.>. These approaches are effective to tackle unforeseen obstacles but have no memory about previously experienced situations. Thus, when encountering the same situation multiple times, the robot may perform every time the same sub-optimal behavior.



In indoor environments, there are many changes in the traversability that happen following repeating patterns.
For example, the doors in an environment could be open or closed at the same time according to certain patterns.
In the environment illustrated in fig:motivation:work, the offices' doors are typically open (green) while people are working.
Whereas, if the kitchen is open, it is likely that people are enjoying a coffee and so that the offices' doors are closed (red), see fig:motivation:coffee.

When deploying a robot in such environments over a longer period of time, it can observe these repeating patterns and exploit this knowledge to navigate along shorter paths thus increasing the efficiency of its operations.

In this paper, we investigate the problem of (i) modeling and predicting the patterns of change in the environment traversability and (ii) planning paths that exploit the predictions to reduce the risk of encountering blocked passages.
While existing approaches propose to make decisions according to periodic patterns of change in the environment <cit.>, we focus on modeling spatial patterns.
























The main contribution of this paper is a novel system for robot navigation over extended periods of time in indoor environments where changes in traversability follow repeating spatial patterns.
We model the environment by using a topological map like the one illustrated in fig:motivation, where the nodes are the locations of interest and the edges are the traversable passages between these locations.
We incrementally model how the traversability of the edges changes from robot's observations during traversal.
We use a probabilistic graphical model to represent this knowledge and to make predictions of the environment traversability.
We exploit the predictions to plan navigation strategies that account for the risk to encounter obstacles and, at the same time, for the information gain of making observations to improve the model.











As a result of that, our approach is able to

(i) learn incrementally a model of the patterns of change in the environment traversability from robot's observations;

(ii) make predictions about the traversability at unobserved locations;

(iii) plan paths that exploit the predictions to make informed decisions for navigation.

Over time, this leads the robot to encounter a reduced number of blocked passages and, thus, to navigate along paths that are on average shorter than following greedy-reactive strategies. 














§ RELATED WORK











In the literature, several approaches have been proposed to model changing environments for robot navigation.
For example, Stachniss and Burgard <cit.> map the typical configurations of low-dynamic areas of the environment for improving localization.
Conditional Transition Maps <cit.> offer a grid-based representation for learning the motion patterns of objects in the environment.
Dynamic Gaussian Process Occupancy Maps <cit.> map long-term dynamics with a spatially-continuous representation that provides occupancy estimates.
Fremen <cit.> enhances a topological map with a spectral model that allows for predicting the traversability of the edges as a function of the time of day.
Fentanes  <cit.> use this model for planning paths that take into account the temporal periodicity of changes in the environment.
In contrast to that, we use a probabilistic approach to learn a time-agnostic model of the traversability changes on the edges of a topological representation of the environment.

Representing the full joint probability of the edge traversability is intractable even for relatively small environments.
In the context of SLAM, FAB-MAP <cit.> uses the Chow-Liu approximation for modeling the joint distribution of a set of visual features.
For traffic prediction, Furtlehner  <cit.> uses a factor graph representation to predict the road traffic from a probe vehicle.
We also investigate spatial patterns and consider an approximation based on factor graphs <cit.> which is able to capture the correlation between traversability changes in the environment and exploits this correlation to make probabilistic predictions.

Planning paths in the belief space defined by the predictions can be formulated as a Partially Observable Markov Decision Process (POMDP). 
However, POMPDs are in practice intractable for real-world environments <cit.>.

The Reactive Planning Problem <cit.> consider a set of possible configurations and plans policies that guarantee the robot to reach the goal.
Murphy  <cit.> samples the edge costs from a probabilistic costmap, generates a list of paths using A^*, and selects the most frequent path.
RAG search <cit.> plans risk-aware paths in a graph where the edge costs are unknown by trading off exploration and exploitation. 

In the Canadian Traveler Problem (CTP) <cit.>, an agent aims at traveling along the shortest path in a road network where some roads, unknown to the agent, are blocked.
Lim  <cit.> introduce a variant of the CTP in which the roads' traversability may be correlated.
The CTP is a PSPACE-complete problem <cit.>, however different approximations have been proposed.
Nikolova and Karger <cit.> approximate the CTP by considering graphs that consist only of disjoint paths.
Whereas, CTP-UCT <cit.> is a Monte-Carlo search algorithm that computes policies by taking the uncertainty of the predictions into account.
We extend this approach for planning in our problem paths that lead the robot to collect informative observations for improving the model of the environment traversability and its predictions. 

Krause  <cit.> introduce an approach for planning informative paths by selecting the locations that maximize the mutual information <cit.>.
We use the mutual information for planning paths that trade off the exploration of informative locations and
the exploitation of the traversability predictions to navigate along short paths.
























































§ PROBLEM DEFINITION AND ASSUMPTIONS



We consider a robot that navigates in indoor environments where the traversability changes according to repeating patterns.
Initially, these patterns are completely unknown to the robot.
It can however observe which passages are frequently blocked at the same time while navigating.

Our robot navigation system relies on a topological map = (,  ) of the environment, where  are the edges representing possible paths and  are the set of nodes representing their intersections. 

We refer to each navigation task performed by the robot as run.
At every run, the traversability of the environment changes. 
We represent the traversability of an edge _i at a run t as the binary random variable ^t_i that is 0 if the edge is be blocked or 1 if the edge is free.

We refer to the state of all the edges of the topology during run t as the environment configuration ^t = {^t_1, …, ^t_||}.

In this work, we make the following key assumption:


  * when a new run starts, the robot has no knowledge about the current environment configuration except from its previous observations;

  * during each run, the environment configuration does not change. This means that we account only for the low frequency dynamic changes in the environment;

  * the environment configuration is independent on the temporal order of the runs, i.e. the configuration at run t has the same degree of dependence to the configuration at run t+1 than the one at t+k;

  * when reaching one of the nodes ∈, the robot observes all the adjacent edges to .














§ ESTIMATING PATTERNS IN TRAVERSABILITY



To navigate in changing environments, we aim at learning a model of the patterns of change to make predictions about the traversability at unknown locations.
We use the robot's observations during traversal to incrementally learn a probabilistic model that captures the correlation among the edge traversabilities and that exploits these correlation to make predictions during navigation.





 §.§ Modeling Environment Configurations





During navigation at run t, the environment presents a configuration ^t of which the robot only observes a subset of edges ^t ⊆ ^t.
To make informed decisions for navigation, we aim at predicting the current environment configuration ^t and, in particular, the traversability of the unobserved edges ^t, with ^t = ^t ∪ ^t.
Therefore, we look for the probability of the unobserved edges ^t to be traversable 
conditioned on the partial observation of the environment configuration ^t:



    (^t | ^t)=   (^t, ^t)(^t),   def. cond. prob.
    
        =   η (^t),    ^t=^t∪^t

where η is a normalizer given the current observations ^t and (^t) is the joint probability distribution over the traversability of the edges in the environment.

The distribution (^t) defines a probability function over the space of possible configurations and captures the correlation among the traversability of edges.



This is in general a distribution without a special structure and, thus, the space required to represent it is exponential in the number of edges.
Therefore, representing the joint distribution over the edges becomes quickly intractable.

Different approaches exist to approximate joint probability distributions.
The simplest one is to consider each edge to be independent from all others, as illustrated in fig:example:naive for the example topology in fig:example:topo.
This representation is efficient to store and compute, but it is not able to capture the correlation among the edges.
A more advanced approach is the Chow-Liu approximation <cit.> that represents the joint probability using a tree-structured Bayesian Network as the one depicted in fig:example:chowliu.
Chow-Liu representation requires quadratic space and is able to capture some correlation among edges.
However, it requires training data and typically does not deal with incremental and partial data which characterize our problem.
To deal with our requirements, we propose to approximate the joint probability distribution over the edge traversability by using a flexible but bounded factor graph representation.






















 §.§ Our Factor Graph Model



A factor graph is a probabilistic graphical model that allows for representing a general factorization of a function.
It is structured as an undirected graph with two kinds of nodes: the variable nodes that correspond to the random variables and the factor nodes that represent local functions of the adjacent variable nodes.

We use a factor graph representation in which the variable nodes are the edges  of the topology.
We model the correlation among the traversability of the edges by defining 

one unary factor node ϕ for each edge and of one binary factor node ψ for each pair of topology edges, as in the factor graph illustrated in fig:example:fg.


Considering this representation, we assume that we can approximate the joint distribution over the edges as:

    (^t)   ≈   p_ϕψ(^t) = ∏_iϕ_i ∏_jψ_ij.



This factor graph representation allows for approximating the probability over the environment configuration
by capturing some of the correlation between the edge traversability while storing only ||(||-1)/2+|| low-dimensional factors.
Therefore, our representation requires only quadratic space in the number of edges rather than exponential as in the case of the full joint probability distribution.






















 §.§ Learning Factors From Observations



Given our factor graph representation, we need to provide a definition for the factors ϕ and ψ such that
the model corresponds to the robot's observations collected in the previous runs, 
and we can efficiently update the model in an incremental manner as the robot acquires new observations.

The belief propagation algorithm (BP) <cit.> allows for performing inference on factor graphs.
We use the BP algorithm to make predictions from current data (see sec:fg:predictions) but also to estimate the model parameters from robot's observations.
Furtlehner  <cit.> introduce an approach to estimate the factor nodes from the marginal probabilities by using the fixed points of BP algorithm.
We use this approach to define the unary factors and binary factors of our factor graph as:

    ϕ_i    =   (_i), 
       
    ψ_ij   =   (_i, _j)/(_i) (_j),

where (_i) and (_i,_j) are respectively the unary and binary joint probabilities of the edges to be traversable or blocked. 
Note that (<ref>) has an analogy with the mutual information between _i and _j that is non-zero for (_i, _j)≠(_i) (_j).
This gives an intuition that such definition of the factors allows for modeling the correlation between edges.

This definition of factor nodes allows for computing the approximated joint probability distribution in (<ref>) as:

    p_ϕψ(^t)    =   ∏_i^|| (_i) ∏_j^|| (_i, _j)/(_i) (_j).



We compute the unary and binary joint probabilities, (_i) and (_i, _j), from the robot's observations in the previous runs ^1:t-1. 
We achieve this by maintaining a counter of the number of observed occurrences of each unary and binary configurations.
To prevent probabilities to take extreme values of 0 or 1 on a single observation, we initialize them with a uniform prior by assigning to each configuration an equal positive number of occurrences.
After each run, we update the counters based on the robot's observations and recompute the probabilities.
Dealing with unary and binary joint probabilities allows us to update only the probabilities corresponding to the observed edges.
Using this procedure, we can incrementally and efficiently compute the model's parameters from robot's observations.
The other key advantage of this procedure is that it also allows for easily incorporating partial observations of the environment.
For instance, in the example illustrated in fig:example:topo, if the robot observes the edges a and b but not c and d, we update (a), (b) and (a, b) but not (a, c) and (b, d).






























 §.§ Predicting Edge Traversability



Our factor graph model maintains a tractable approximation of the joint probability distribution of the edge traversability, but also provides us a tool for predicting the traversability of currently unknown edges.
The BP algorithm implements a message passing procedure in the graph to estimate the MAP environment configuration and the marginal probabilities of each edge to be traversable.

We predict the traversability of the unobserved edges ^t at run t by fixing the observed edges ^t to the observed values in the factor graph and by performing belief propagation. 
This procedure allows for computing a belief about the environment configuration that approximates ^t^t.

We perform inference on factor graphs by using the implementation of approximate loopy belief propagation of libDAI <cit.>.


















































§ PLANNING EXPLOITING PREDICTIONS



We aim at exploiting the predictions of the edge traversability provided by our factor graph model to plan anticipatory behaviors that lead the robot to encountering a reduced number of unforeseen obstacles during navigation in the long run.

To achieve this, we explore the belief space of possible environment configurations and plan paths that trade off travel distance and information gain to improve the edge traversability model.







 §.§ Estimating Travel Distance From Predictions










We estimate the travel distance to reach the goal in a partially observed environment by exploring the belief space of the possible environment configurations defined by the predictions. 
We search the belief space by using an approach based on CTP-UCT <cit.>.
CTP-UCT is a Monte-Carlo search algorithm based on the Upper Confidence tree algorithm (UCT) <cit.> that allows for computing approximate solutions for the Canadian Traveler Problem.




Given a prediction of the environment configuration, we approximate the belief space of possible configurations by performing a sequence of rollouts. 
A rollout randomly samples a configuration according to the current belief and simulates robot navigation based on this configuration.
The robot has no initial knowledge about the sampled configuration but it can make observations during traversal.
To explore the belief space, the robot selects locations for navigation that led to the goal through short paths and have been selected less often in the previous rollouts.
To this end, at each step of the rollouts, we consider a state composed by the robot's current location, the set of known traversable and blocked edges, and the set of unknown edges.
Let ρ = {s_0, s_1, …, s} be the current sequence of states at the k-th rollout, we select the next state s' that maximizes the UCT formula:

    s' = _s'B√(log R^k-1(ρ)/R^k-1(ρ')) - dist(s,s') - C^k-1(ρ'),

where ρ'={ρ,s'} is the new sequence of states, 
B>0 is a parameter that biases the exploration in the belief space,
dist(s, s') is the travel distance to move from s to s', 
R^k-1(ρ) is the number of previous rollouts that start with ρ, and
C^k-1(ρ) is the average travel distance to the goal in the previous rollouts that start with ρ.


After performing a number of rollouts, CTP-UCT selects the path 𝒫 that minimizes:

    cost_ CTP-UCT (𝒫)   =   length(𝒫),

where length(𝒫) is the average travel distance to the goal during the rollouts.
We extend this cost function to plan paths that in the initial runs lead the robot to collect information about the traversability in the environment for improving the model and the predictions in the subsequent runs.















 §.§ Collecting Informative Observations











A common approach to collect information about the environment is to make observations at locations that maximize the mutual information about the non-observed regions.
The mutual information, also called information gain, between two discrete random variables A and B is defined as:

    (A, B)     =   (A) - (A | B), 
       =   ∑_a∈A, b∈B (a, b) log(a, b)/(a) (b),

where (·) and (·|·) are respectively the entropy and the conditional entropy.

Given the current set of unobserved edges ^t, we bias the robot's behavior to collect informative observations by selecting paths that maximize the information gain:

    (P, ^t)    =   (P) - (P | ^t),

where P are the edges along the path 𝒫.

Computing the entropy over P requires the knowledge of their joint probability but our factor graph model does not provide direct access to it.
Therefore, we approximate (P,^t) with the sum of the pairwise mutual information between the edges along 𝒫 and the unobserved edges ^t:

    (P, ^t) ≈ (P, ^t) = ∑_U^t∈^t max_P∈P (P, U^t),

where (P, U^t) is computed using (<ref>) and involves only unary and binary joint probabilities that are directly available from our factor graph model.
We make sure that the mutual information for the same edge is not counted multiple times by considering the maximum mutual information for each unobserved edge U^t.





 §.§ Exploration Vs. Exploitation

We aim at exploring the environment configurations while minimizing the travel distance.
To this end, we select paths according to a cost function that trades off the the path length and the information gain along it:



    cost(𝒫) = length(𝒫) - γ^#runs [ζ (P, ^t) ],

where P are the edges along 𝒫, length(𝒫) is the average travel distance to reach the goal following 𝒫 in the rollouts, γ∈ [0,1] is a parameter that controls the exploration term, and ζ is a constant that normalizes the information gain with respect to the travel distance.




The exploratory behavior of the robot is determined by the parameter γ that decays exponentially with the number of runs performed by the robot.
Initially, when few observations are available, γ leads the robot to favor exploratory behaviors for improving the model of the traversability of the edges.
As the robot performs a number of runs and acquires several observations of the environment, the model and its ability to make predictions improve and the exploration becomes less and less prominent.
When the learning process of the model converges, our problem becomes similar to a CTP in which the traversability of the edges is correlated. 
At this point, the exploratory term in (<ref>) is weighted low and, thus, the cost function becomes similar to the original CTP-UCT.





§ EXPERIMENTAL EVALUATION



The main focus of this work is on robot navigation over extended periods of time in environments where the traversability changes by following repeating patterns and on how the knowledge about such patterns can be obtained and exploited.

Our experiments are designed to illustrate that our approach is able to

(i) model and make predictions about the traversability changes in the environment from the robot's incremental observations;

(ii) plan paths that exploit the predictions to make informed decisions for navigation;

(iii) navigate along paths that are on average shorter than following greedy shortest path strategies.



In our experiments, we consider different topologies defined over real-world environments.
There exist many approaches to build topological maps for example Kuipers  <cit.>.
On these topologies, we simulate repeating patterns in the traversability changes.
First, we sample M independent template configurations for each environment.
Then, we generate a set of N, N ≫ M configurations by sampling uniformly one of the M templates and applying random noise on edge traversability.



 §.§ Predicting Environment Configurations




We designed the first experiment to show the capabilities of our approach to model the traversability changes in the environment and to make predictions about the environment configurations.
In this experiment, we consider a relatively small topology composed by 9 nodes and 13 edges and assume that the robot observes at each run the whole environment.
These assumptions allows for computing the ground truth joint distribution over the edge traversability despite its exponential space complexity to use for comparison.
We additionally compare our approach with a model that assumes each edge to be independent from all the others as the one illustrated in fig:example:naive.
To investigate the performance of our approach, we consider two different cases: one in which the environment configurations are highly correlated, and one in which the correlation is low.

We compare the capabilities of each model to predict the edge traversability for 10000 partial configurations after the robot observed 10, 100, 1000, and 10000 configurations.
In fig:exp:predictions, we illustrate the RMS error of the predictions to the true configurations for the three approaches.
In cases in which the configurations are highly correlated (solid lines), our approach (blue) is able to provide good predictions already after few observations.
The predictions improve incrementally as the robot makes more observations, similarly as if using the full joint probability distribution (green).
Instead, assuming the edges to be independent (red) cannot capture the correlation between edges and leads to worse predictions.
In cases in which the configurations have low correlation (dotted line), all of the three approaches provide similar predictions. 
Therefore, also in situations with low correlation, our approach does not reveal worse performance than the model assuming independence among edges.



 §.§ Navigation Exploiting Predictions



The second experiment is designed to show that our approach is able to exploit the predictions of the environment configurations to plan informed strategies that lead the robot to navigate along shorter paths over time.
In this experiment, we consider four different environments described in tab:environments.
To evaluate the improvement over time, we repeat a fixed sequence of navigation of 25 tasks and configurations for a total of 500 runs in each environment.
We compare our approach to the theoretical optimal path computed in the ground truth environment configuration (unknown to the robot) and to an optimistic shortest path policy called SPO.
This strategy plan paths using A^* by assuming that every edge of the environment is traversable unless the robot observes the opposite and re-plans.
SPO does not take into account the predictions of the environment configuration and thus, independent of the number of runs, makes the same decisions as if navigating for the very first time.
In our approach, we perform 50 rollouts per decision and set the parameter that regulates the exploratory term of the cost function to γ = 0.95.





The performance of our approach over the number of runs in the four environments are illustrated in fig:exp:costnav.
We evaluate the average difference in the travel distance to the theoretical optimum with ground truth environment knowledge available (0.0) normalized with respect to the SPO solution (1.0).

Initially, when the robot collected little information about the environment, the predictions of the edge traversability are weak and the robot following our approach performs similarly as following SPO.
After 100 runs, the robot starts discovering patterns in the traversability changes and our approach is able to plan paths leading it to the goal along shorter paths.
Over time, when the robot collects more and more observations about the environment, the learning process of the edge traversability model converges and, after 500 runs, the robot is able to navigate along paths that are on average about 30% shorter than following an optimistic shortest path strategy.























 §.§ Planning Performance Comparison




Besides the baselines discussed in the previous section, we compare the performance of our approach to other planners.
For comparison, we consider the original CTP-UCT <cit.> that searches for the shortest path in the predicted belief as described in sec:uct.
We compare our approach also to a strategy inspired by Lim  <cit.> called SPD.  
SPD makes a most likely assumption on the belief about the edge traversabilities and plans the shortest path using A^* on the determinized environment configuration.
When the robot makes an observation incompatible with the current determinized configuration, SPD computes a new prediction and re-plans.

It is important to note that CTP-UCT and SPD do not provide an approach to model and make predictions of the edge traversability in the environment.
Therefore, we consider the predictions provided by our factor graph model also for these approaches.

The average performance of the approaches after 50, 250 and 500 runs for navigating in the environments introduced in the previous section are illustrated in fig:exp:predsnav.

SPO (orange) reveals a constant trend over time as it does not take the predictions into account.

Considering the predictions of the edge traversability, SPD (green) leads the robot to navigate along shorter paths over time.
However, the determinization of the predicted configurations may cause the robot to follow paths that are distant from the optimal ones.

CTP-UCT (red) consider a weaker approximation of the belief defined by the predictions by performing rollouts.
Thus, it is able to make more informed decisions than SPD that lead the robot along shorter paths.

Our approach (blue) extends CTP-UCT by considering an exploratory term that allows the robot to collect informative observations that explicitly improve the model and so the predictions of the edge traversabilities.
The exploratory behavior leads initially to slightly longer travel distances than CTP-UCT but, in the long run, it allows the robot to navigate along shorter paths.

































































§ CONCLUSION



In this paper, we investigate robot navigation over extended periods of time in environments where the traversability changes according to repeating patterns.
We present an approach that learns a probabilistic model of the traversability changes from robot's incremental observations during navigation. 
Our model predicts the traversability at unknown locations exploiting the estimated correlation between the traversability in the environment.
We exploit these predictions to make informed decisions that lead the robot to navigate by encountering a reduced number of unforeseen obstacles.


Although our approach presents a higher complexity in comparison to traditional planning systems,
in environments where the traversability changes following certain patterns, it has the potential to lead robots to automatically navigate along shorter paths over time increasing the efficiency of their operations.


































plain







