Autonomous Search of Semantic Objects in Unknown Environments

There is a significant amount of literature on simultaneous localization and mapping (SLAM) [1, 2] for robot mapping and navigation in an unknown environment based on perception, such as visual and odometry sensing [3]. SLAM methods model and reduce sensing uncertainties in mapping an unknown environment and localizing the robot in it at the same time. Semantic SLAM and active SLAM are particularly relevant.

Robot path and motion planning is one of the most studied areas in robotics. The basic objective is to find an optimal and collision-free path for a robot to navigate to some goals in an environment. Many traditional path-planning approaches assume a more or less known environment, i.e., the robot already has a map and models of objects [17]. On the other hand, real-time, sensing-based planning in an unknown environment remains largely a challenge [18].

Earlier work includes grid-based planning approaches such as D* [19] and D* Lite [20], sampling-based approaches such as ERRT[21] and DRRT [22], and adaptive approaches such as [23]. These approaches consider the environment dynamic and only partially known, but assume the goal position is known, disregard the uncertainties in sensing, the robot pose, and dynamics, and do not consider semantic information.

Recently, various techniques based on partially observable Markov decision processes (POMDPs) have been developed [24, 25, 26] to incorporate sensing and robot motion uncertainties into planning in partially observable environments. However, POMDP suffers from the curse of dimensionality and is computationally expensive, particularly when the state space is large. For the POMDP to scale, high-level abstraction must be made for the state space. For example, treat objects [25] or rooms[24] as state variables. The downside is that highly abstracted models can lose touch with reality. To bypass this problem, some researchers turn to deep learning to learn semantic priors and make predictions on the unobserved region (PONI[27], SemExp[28], L2M[29]). These methods tend to suffer from poor generalization.

Next-best view planning [30] is another highly related topic, designed for efficient visual exploration of unknown space. Unlike active SLAM, approaches for next-best view planning typically do not consider robot localization uncertainty. A next-best view planner starts by sampling a set of views in the environment, evaluates the estimated information gain for each view, and selects the view with the maximum information gain as the next view [31]. Different approaches differ in the sampling methods (uniform sampler, frontier-based coverage sampler [32]), information gain (path costs are incorporated in [33, 32]), and the selection of the next view (such as receding horizon scheme in [34] and Fixed Start Open Traveling Salesman Problem (FSOTSP) solver in [32]).

However, existing planning methods in unknown environments usually do not consider real-time results from SLAM with embedded and changing uncertainties, such as the robot’s pose, the metric map, and the semantic map (generated by semantic SLAM). Only the metric map was used by next-best view planning approaches [31, 32, 33].

We further discuss the limitations of the existing work when applied to the problem studied in this paper, summarized in Table I. Specifically, we evaluate each method for the essential capabilities identified in Section I. For example, SLAM approaches only have the SLAM capability. Active SLAM and next-best view planning methods do not employ semantic SLAM. They have exploration capability and limited adaption capability as plans can be changed based on the current map. These methods take explicit actions to reduce the map and pose uncertainty by creating and reaching a set of intermediate goals.

Traditional grid-based planning approaches such as D*, D* Lite, and sampling-based planning approaches such as ERRT and DRRT can also adapt to changes of objects in the map, but do not consider uncertainties. POMDP-based approaches are intended for probabilistic planning by taking into account uncertainties and maintaining belief about the mission status. However, these approaches do not incorporate SLAM, are compartmentalized, and miss essential capabilities (Section I) required for our overarching problem.

Several deep-learning based approaches exhibit more comprehensive capabilities. Nevertheless, PONI and SemExp only have mapping but not localization capabilities as they rely on ground-truth robot pose information. The uncertainty in the map, robot motion, and robot pose estimation are not treated when formulating plans to reach intermediate goals. No explicit action is taken to reduce map uncertainty. Although L2M improves upon those methods by taking uncertainty reduction into goal formulation, it still misses two capabilities either partially or entirely.

Geometric SLAM is run in parallel with semantic SLAM in our system. We employ the RTAB-MAP [35] algorithm for geometric SLAM. It generates a grid map $\mathbf{G}_{t}\in\{0,1,-1\}^{W\times H}$, where $W$ and $H$ are the width and height of the grid map. $0$, $1$, and $-1$ in the grid map represent free, occupied, and unknown space respectively, as shown in Fig. 2. The geometric SLAM module also estimates the robot pose $(\mathbf{\mu}_{p,t},\mathbf{\Sigma}_{p,t})$, where $\mathbf{\mu}_{p,t}$ and $\mathbf{\Sigma}_{p,t}$ are the mean and covariance of the robot pose at time instance $t$.

We adapt the system introduced in our previous work [6] for semantic SLAM. At time instance $t-1$, the position estimation $\mathbf{m}_{i,t-1}\in\mathbb{R}^{2}$ for the semantic object $o_{i}$ is:

Where $bel(\cdot)$ stands for the belief over a variable. Note that for simplicity, the subscript $i$ is dropped in (1), as in (2)–(4).

At time instance $t$, the robot pose $\mathbf{x}_{t}\in\mathbb{R}^{2}$ estimated by geometric SLAM is $bel(\mathbf{x}_{t})\sim\mathcal{N}(\mathbf{\mu}_{p,t},\mathbf{\Sigma}_{p,t})$.

If the semantic object $o_{i}$ is detected on the color image $\mathbf{I}_{t}$, range-bearing measurement $\mathbf{z}_{t}$ will be generated based on the depth information of $o_{i}$ from the depth image. The range-bearing measurement noise $\mathbf{\delta}_{t}$ is:

The covariance of the range-bearing measurement $\Sigma_{\delta}$ is assumed to be time-dependent. Then the posterior belief $bel(\mathbf{m}_{t})$ at time $t$ can be updated using Bayes’ theorem:

where $\eta$ is a normalizing term.

Substituting the probability density functions of $p(\mathbf{z}_{t}|\mathbf{m},\mathbf{x}_{t})$, $bel(\mathbf{x}_{t})$, and $bel(\mathbf{m}_{t-1})$ into (3), the final result after simplification suggests that the updated posterior belief $bel(\mathbf{m}_{t})$ can be approximated by a multivariate Gaussian distribution $bel(\mathbf{m}_{t})\sim\mathcal{N}(\mu_{t},\mathbf{\Sigma}_{t})$:

where $\Delta\mathbf{z}_{t}$ is the error between expected and actual range-bearing measurement,

The object class probability distribution $p_{t}(\cdot)$ is updated using Bayes’ rule:

where $\eta=1/p(\mathbf{L}_{t}|\mathbf{L}_{1:t-1})$ is a normalization constant, $\mathbf{L}_{t}\in\mathbb{R}^{\lvert\mathbb{C}\rvert}$ is the confidence level distribution of an object in different classes, returned by an object detector, such as YOLOv3 [36] at time $t$. $c\in\mathbb{C}$ is one of the possible object classes. $p(\mathbf{L}_{t}|c)$ is the object detector uncertainty model, representing the probability of object detector outputs $\mathbf{L}_{t}$ when the object class is $c$. We use the Dirichlet distribution ${\displaystyle\operatorname{Dir}({\boldsymbol{\alpha}}_{c})}$ to model this uncertainty, with a different parameter $\boldsymbol{\alpha}_{c}\in\mathbb{R}^{\lvert\mathbb{C}\rvert}$ for each object class $c$.

As the robot explores an environment, our system uses off-the-shelve tools [37] to segment the gradually built grid map into different geometric rooms: a room is defined as any space enclosed within a number of walls to which entry is possible only by a door or other dividing structure that connects it either to a hallway or to another room. Every grid on the grid map $\mathbf{G}_{t}$ is assigned with a corresponding room ID: $\mathbf{R}_{t}\in\mathbb{N}^{W\times H}$. An example is provided in Fig. 3.

Based on the locations of map objects, the corresponding geometric room IDs are assigned to the objects. Formally, a map object $o_{i}$ is represented as a 4-tuple $o_{i}=\langle\mu_{i},\Sigma_{i},p_{i},r_{i}\rangle$ with $\mu_{i}$ and $\Sigma_{i}$ the mean and covariance of the object $o_{i}$ pose, $p_{i}$ the object class distribution, and $r_{i}$ the room ID of $o_{i}$. The object map is the set of observed map objects $\mathbb{O}_{t}=\{o_{1},o_{2},\ldots,o_{n}\}$.

Finally, the fused map $E_{t}=\langle\mathbf{G}_{t},\mathbb{O}_{t},\mathbf{R}_{t}\rangle$ collects the grid map $\mathbf{G}_{t}$, object map $\mathbb{O}_{t}$, as well as the room information $\mathbf{R}_{t}$.

To account for the uncertainty in robot pose estimation, SLAM map, and robot motion, the robot is modeled as a Markov decision process (MDP) $\text{M}_{t}=\langle S,A,s_{0},P,R,F\rangle$ with the following components:
— $S$ is the discrete state space, representing the mean of the Gaussian distribution of the robot position. The mean of the robot’s position is discretized and clipped to the closest grid cell in the grid map $\mathbf{G}_{t}$ to avoid an infinite state space.

As the robot’s mission is to find a target object in an unknown environment, its goal of motion will be determined on the fly depending on the information provided by the fused map $E_{t}$ and the prediction made using semantic prior knowledge. The mission is accomplished if the target object is visible and identified as such.

There are several types of intermediate goals for the robot motion:

1. 1.

   If the target object is visible and identified as such, then the mission is accomplished and a stop signal is sent.

2. 2.

   If the target object is not included in the current map $E_{t}$, the robot chooses to explore where the target object likely is. This intermediate goal requires frontier detection.

3. 3.

   If an object in the map may be the target object (with a low probability ), the robot chooses to observe more of the object in its visibility region and reduce the object’s uncertainty.

At the beginning of each planning cycle, the robot evaluates the mission status and decides its intermediate goal. Next, the corresponding reward function of the robot MDP model $M_{t}$ is designed to drive the robot to a selected intermediate target state based on the intermediate goal. In the following, each type of intermediate goal and the corresponding reward function is described.

The MDP $\mathbf{M}_{t}$ and the selected reward function $R$ are fed into a planner based on the Real Time Dynamic Programming (RTDP) algorithm [40] to compute an optimal policy $\pi^{*}$ that maximizes the expected sum of rewards, i.e., value function $\mathcal{V}$. A value function $\mathcal{V}$ starting at state $s\in S$ following policy $\pi$ is defined as follows:

where $\pi\colon S\to A$ is a deterministic policy over $\mathbf{M}_{t}$ mapping the state into an action, and $\gamma\in[0,1)$ is a discounting factor. The optimal policy $\pi^{\ast}$ is computed as follows: for all $s\in S$,

The RTDP algorithm allows us to compute a semi-optimal policy in a short time¹¹1unlike a more traditional approach such as value iteration [41].. As the robot carries out the semi-optimal policy, the policy is continuously improved by the RTDP algorithm with the current robot mean position as the initial state $s_{0}$ and converges to the optimal policy.

The fused map $E_{t}$, the robot pose estimation $bel(\mathbf{x}_{t})$, frontier region $\mathbb{F}_{t}$, and visibility region $\mathbb{V}_{t}$ are updated at every time instance $t$ based on the ever-improving SLAM results. Consequently, once the robot reaches an intermediate target state, the MDP model $\text{M}_{t}$ must be updated. We call this process the adaptation of the MDP model. Next, the corresponding policy $\pi$ is also updated.

Specifically, the following components are adapted:

• •

  the discrete state space $S$ to match the changing grid map $\mathbf{G}_{t}$,

• •

  the initial state $s_{0}$ to match the changing robot pose estimation $bel(\mathbf{x}_{t})$,

• •

  the transition probability function $P$,

• •

  the reward function $R$ based on Eqs. (7) and (6), and

• •

  the set of intermediate target states $F$ as $\mathbb{F}_{t}$ and $\mathbb{V}_{t}$ change.

The RTDP planner takes the updated MDP model $\text{M}_{t}$ to generate a new policy.

In each experiment, the robot’s objective is to find any instance of the target with a confidence level greater than $1-\epsilon=0.99$. The robot’s linear and angular velocity actuation signal is continuous. A task is successful if the agent has identified the target object within a specific time budget (1K seconds) and stops its motion. The set of parameters used for virtual and real-world experiments are presented in Table II.

We performed experiments on the Matterport3D (MP3D) [42] dataset using the Habitat [43] simulator. MP3D dataset contains 3D scenes of a typical indoor environment, and the Habitat simulator allows the robot to navigate the virtual 3D scene. Two snapshots of the MP3D scene are given in Fig. 7. This particular scene is $6.4m\times 8.4m$ in size and has one level, ten rooms, and 187 objects, but it is unknown to the robot. The robot start positions and the target object instances are indicated in Fig. 8.

Random noise following a Dirichlet distribution is injected into the ground truth semantic labels that the Habitat-lab simulator provides to simulate the results from an object detector. Random Gaussian noise is also injected into the linear and angular velocity of the mobile robot to simulate the motion uncertainty. We used an AMD Ryzen^TM 3700X CPU with 3.60GHz, 16 GB of RAM, and an Nvidia GeForce RTX 3060Ti graphics card for all simulations. Fig. 9 presents several snapshots showing the robot starting from position 1 in the virtual environment at various stages of the task of finding towels.

We also performed real-world experiments using a TurtleBot2 mobile robot. We deployed our algorithm on the TurtleBot and tested it in two unknown environments – involving a cluttered laboratory and a kitchen.

In the laboratory, the robot’s task is to find a person. The laboratory is 6m × 6m in size, unknown to the robot. Several sample images from the laboratory environment were taken from the robot’s first-person view, showing the robot at the initial position, performing exploration tasks, performing uncertainty reduction tasks, and finally identifying the target object, as shown in Fig. 16. In Fig. 16b, the robot has detected a chair in this room and inferred that a person is likely to be in the same room. From Figs. 16e and 16f, we can see that before the uncertainty reduction operation, the confidence score for the person was 0.65. After the operation, the confidence score was 1.00, and the robot correctly determined that the task was completed. The success path the robot traversed is visualized in the blue line in Fig. 17.

We performed three runs in the laboratory, and all were successful. The average total time for the robot to complete the task was about 4.5 minutes, and the average time spent on planning alone was $67$ seconds. The average length of the robot path was 16.3m. We estimate the optimal path to be 6.5m, which gives a SPL of 0.41.

To highlight the importance of using semantic prior knowledge, we conducted another test by denying the robot planner access to semantic prior knowledge and using a uniform reward. Without semantic prior knowledge, the robot chose to exit the laboratory and failed to find the target, as visualized in the green line in Fig. 17.

For the kitchen scenario, the robot’s task is to find a microwave. The space is about 4.5m × 5.5m in size, unknown to the robot. Several robot first-person view images from the environment are presented in Fig. 18. In Fig. 18b, the robot has detected a trash bin and inferred that the unexplored room is a kitchen and probably contains a microwave. From Figs. 18c and 18d, we can see that after the uncertainty reduction operation, the confidence score for the microwave increased from 0.95 to 1.00, and the robot correctly determined that the task was completed. The success path the robot traversed was visualized in the blue line in Fig. 19.

Again, we performed three runs in this kitchen scenario, and all three were successful. The average total time for the robot to complete the task was 7.1 minutes, and the average time spent on planning was $4.3$ minutes. The average length of the robot path is 13.0m. We estimate the optimal path to be 3m, which gives a SPL of 0.24.

We also conducted a test denying the robot access to semantic prior knowledge. Without semantic prior knowledge, the robot did not enter the kitchen and failed to find the target, as visualized in the green line in Fig. 19.

The real-world tests show that our approach runs effectively on a physical robot in unknown real environments, among which the lab environment has many obstacles. It enabled the robot to find the target object nested among obstacles successfully.

We provide the semantic room labels inferred by the Bayesian network in Fig. 20. We can see that at the beginning, due to the lack of evidence, all four room types (“Kitchen”, “Bathroom”, “living room”, “office”) were equally likely. At the end of the task, the robot correctly concluded that in the laboratory environment, the scene most resembled an office, and in the kitchen environment, the scene was most likely a kitchen. For illustration purposes, we also show the Bayesian networks used for all four room types in Fig. 21 without displaying the associated conditional probabilities for brevity.

We now present the semantic SLAM results in the lab demo. Since we do not have the object ground truth information, for the semantic SLAM results, we focus on its uncertainty reduction capability. For all the detected map objects in the lab environment, the A-opt, D-opt, and E-opt of position covariance is calculated. Their evolution over time is plotted in Fig. 22. We can see that the position uncertainty of objects decreases with time and is kept at a low level in the end.

Fig. 23 gives a more intuitive presentation. Comparing the results at time instants t = 48s and t = 70s, we can see that at t = 254s, the peak of the bell (which shows the object position covariance) increases, and the bell’s base decreases, indicating a more certain estimation of the object’s position.

The entropy of the predicted object class is visualized in Fig. 24. The result suggests that as time progresses, the robot is more and more certain about the object class it predicted.

Please refer to the supplementary video for several recorded runs in the virtual and real-world environments.