A Robot Architecture Using ContextSLAM to Find Products in Unknown Crowded Retail Environments

Abstract

Grocery shoppers must negotiate cluttered, crowded, and complex store layouts containing a vast variety of products to make their intended purchases. This complexity may prevent even experienced shoppers from finding their grocery items, consuming a lot of their time and resulting in monetary loss for the store. To address these issues, we present a generic grocery robot architecture for the autonomous search and localization of products in crowded dynamic unknown grocery store environments using a unique context Simultaneous Localization and Mapping (contextSLAM) method. The contextSLAM method uniquely creates contextually rich maps through the online fusion of optical character recognition and occupancy grid information to locate products and aid in robot localization in an environment. The novelty of our robot architecture is in its ability to intelligently use geometric and contextual information within the context map to direct robot exploration in order to localize products in unknown environments in the presence of dynamic people. Extensive experiments were conducted with a mobile robot to validate the overall architecture and contextSLAM, including in a real grocery store. The results of the experiments showed that our architecture was capable of searching for and localizing all products in various grocery lists in different unknown environments.

1. Introduction

Finding products in grocery stores is a challenging task requiring an understanding of a store’s layout, finding these products based on this understanding and negotiating crowded aisles. In general, customers have difficulty remembering the locations of products in aisles due to a lack of location cues and mnemonic aids [1]. Moreover, the layout of a store changes regularly due to several factors to improve sales such as: (1) seasons, and holidays; (2) periodic changes due to shopper behavior [2]; (3) product promotions, and (4) the daily introduction of new products [3]. The estimated 21,000 products [4] introduced yearly have a significant impact on store layouts due to the rearrangement of other products [5]. Therefore, shopping can be stressful for customers in such environments [6]. Even regular customers leave without buying all their desired items [7]. The inefficiency of the shopping experience reduces customer satisfaction [8] and the inability to make intended purchases represents significant lost revenue for stores [9].

It has been found that customer satisfaction is directly related to perceived employee knowledge [10]. Employee availability and lack of employee knowledge are exacerbated by turnover rates of grocery employees; a major contributor to the general decline in grocery store satisfaction [11].

Systems using multiple ceiling mounted cameras require modifying and incorporating sensors in the entire store to aid with product localization and server fees for data processing, which can be costly and invades customer privacy [12]. Existing smartphone apps rely on planograms of a store, that are manually created by staff, which is time-consuming and can contain product location errors. Compliance with planograms has been found to be less than 60% [13]. Furthermore, these apps are not widely accessible to all demographics including seniors (only 20% of seniors have access to a smartphone [14]). Our system aims to provide an autonomous robot for finding products on shelves within a grocery store without altering the environment and with minimal change to the shopping experience. Our robot has onboard sensing and computation capabilities and can be placed in any new environment to work immediately without prior knowledge, unlike existing camera systems that need to be installed and calibrated in each environment. Using robots to automate one-on-one customer assistance would aid in reducing customer stress thereby improving their shopping experience [6].

Service robots have been used to automate numerous tasks in human-centered environments such as searching for people using a preexisting map and their activity schedule to provide assistance [15], helping with cleaning/delivery [16,17], and surveillance [18]. With respect to grocery stores, robots have several advantages as they: (1) allow employees to perform complex tasks that cannot be easily automated (e.g., stocking shelves, rearranging products to improve revenue), and (2) lower operating costs by reducing employee turnover and salary requirements (e.g., the cost to replace an employee is between 75–150% of the position’s salary [19]). They have already been used in retail stores to improve overall customer experience. This includes showing product locations, and carrying products for customers [20,21]. Studies have shown that service robots in grocery stores have high satisfaction and intent to use rates. For example, in [21], 92% of customers would use a robot again as a shopping companion. However, such robots require full prior knowledge of their environment and cannot behave autonomously without human input to generate/obtain a representation of the environment [20,21,22]. Their reliance on expert human knowledge of the environment prevents these robots from autonomously adapting to regular changes to a store layout or product locations, and prevents their use in multiple stores. Given that there are approximately 38,000 grocery stores in the U.S. alone [23], regular manual mapping due to layout changes would represent a significant time cost for stores (e.g., it can take at least 75 min to manually map a grocery-like environment with 29 aisles with a robot moving at 0.5 m/s [21]).

To autonomously handle new layout changes, a robot must generate/update a map and identify vital landmarks in the environment, while simultaneously localizing itself in the map. This is known as the Simultaneous Localization and Mapping (SLAM) problem [24,25]. Current trends in SLAM focus on data association techniques to fuse sensor readings, dealing with uncertainty, or decreasing time complexity [26,27,28,29,30]. In a grocery store environment, SLAM strategies need to take advantage of the texture rich features that are available to aid in finding products in crowded aisles containing many products and dynamic people. The performance of SLAM methods relies heavily on the ability to associate previously seen unique features between consecutive sensing frames to build the environment map and define the robot’s path within this map [30,31]. As such, the choice of features and their repeatability impact the method’s performance. Furthermore, annotating the map with contextual information is vital to the decision making process for mobile robots and enhances a robot’s ability to interact with the environment [32]. For example, planning an optimal path to products of interest in a crowded store.

This paper presents the development of a generic grocery robot architecture for the autonomous search and localization of products in unknown dynamic (with unpredictable people) grocery stores. A unique context Simultaneous Localization and Mapping (contextSLAM) framework is developed that combines an Optical Character Recognition (OCR) system with laser range measurements using a Rao-Blackwellized particle filter to simultaneously generate an annotated map and localize a robot to address the grocery store product search problem. contextSLAM differs from existing methods in that: (1) it is the first to simultaneously combine contextual landmarks and occupancy grid information for online generation of an annotated map and robot localization, (2) it can uniquely update the context map to accommodate regularly changing grocery store layouts, and (3) it accounts for non-unique feature instances and rejects dynamic objects/people, while using landmarks pervasive in human environments such as text. The framework uses contextual information to aid robots with exploration and localization in dynamic environments, without the need to introduce artificial landmarks or sensors into the scene. Contextual information includes symbols on signs, objects, and posters that provide information for the localization of the robot and products of interest. It is the first work inspired by human exploration that considers common environmental cues to not only reason about the content of the environment, but also to aid in finding the item locations of interest. The advantage of considering scene text is that it is readily available in retail environments and thus can be incorporated as a robust source of localization information for robots.

2. Related Work

Herein, literature on: (1) retail robots, and (2) use of contextual evidence for mapping and robot localization is discussed.

2.1. Retail Robots

Robots have been designed to aid in a variety of tasks in stores including: (1) inventory management, or (2) assisting customers.

2.1.1. Inventory Management

Several systems have been designed for the automation of inventory management [22,33]. For example, in [22], a robot was used to create a semantic map of a retail environment. An operator manually moved the robot through the environment as GMapping generated a 2D occupancy grid map. Then a dynamic programming algorithm segmented the map into categories (e.g., oats, cans, etc.) based on the products observed while the robot autonomously scanned shelves to create a semantic map of product locations.

In [33], a robot was designed for autonomous oos detection, spacing, inventory compliance, and facing checks of products. An operator manually moved the robot through the environment to construct a 2D occupancy grid using GMapping. The robot then navigated identified aisles while taking depth and 2D images to detect shelves and the spacing between products.

2.1.2. Customer Service

Robots have been proposed for customer service tasks, such as finding products [34], and escorting people through a store [20,21]. In [20], a robot guided a user to desired products using an annotated map of product locations while a second carried their products. Localization and path planning were achieved using onboard laser scanners and external cameras in the store. In [21], a robot provided product location guidance, shopping companionship, and price checks. A teleoperated robot collected laser and sonar data for Map-Match SLAM [35] to generate a map. The map was labelled using data from the store’s internal map via a manual transform.

Our previous workshop paper [34] introduced an autonomous robot to guide users to desired products in a static unknown environment. While exploring the environment, a map was created using GMapping and annotated with text detected in the environment. Human-robot interaction experiments in a lab environment showed that participants found the robot helpful, motivating our current research.

While some of the aforementioned robots were able to provide customer assistance, the majority require a priori knowledge of the store layout or product locations [21,22,33], or external sensors in the environment [20]. Therefore, existing architectures cannot be directly applied to our problem, as there is no available/limited prior information to exploit in the search process as we consider scenarios where: (1) the robot is deployed for the first time in a new store(s), or (2) a store layout has changed. We address these limitations using a novel autonomous robot architecture for grocery stores that allows a robot to explore unknown crowded dynamic environments to find products of interest without any prior knowledge of product locations or layout of an environment. This work extends our preliminary research by developing a robust online approach using: (1) context detection via deep learning-based OCR, and (2) a fused mapping approach merging context and laser range data via our novel context Simultaneous Localization and Mapping (contextSLAM) method.

2.2. Mapping and Localization Using Contextual Information

Contextual features in the environment have been used for a variety of purposes, including: (1) localization [36,37,38], (2) map annotation [21,22,39,40,41,42,43], (3) SLAM using text [44,45], and (4) Semantic SLAM [32,46,47,48,49].

2.2.1. Localization

In general, the types of contextual features used for localization have included unique fiducial markers [36,38], or visual or range features [37]. In [36], a method used laser range and unique April tag based visual context measurements in a graph SLAM framework. Robot localization was then performed using only the April tags. In [37], a SLAM method used a reinforcement learning policy to switch between localizing with an occupancy grid or landmark map. The method maintained separate particle weights for both maps. The policy selected which map to use based on a handcrafted state representation. In [38], a method used color-based segmentation to extract regions of text from unique name plates in the environment to associate the strings to nodes in a topological map.

2.2.2. Map Annotation

Context, and feature-annotated maps have been used for SLAM purposes such as in [21,22,39,40,41,42,43]. In [39], a method for combining and correcting occupancy grids using feature measurements was presented. The features (e.g., object locations) were used for localization and to partition the environment into triangular regions, with local occupancy grids defined relative to these features. In [40], a modified tinySLAM method was presented. The tinySLAM method fused laser data, odometry, and RFID tags detected in the environment to create an occupancy grid.

Both [42,43] proposed methods to generate annotated occupancy grids of environments. In [43], an OCR system annotated a pre-existing occupancy grid with the text on door signs. Alternatively, in [42] an existing map of features was transformed to fit into an occupancy grid as it was generated using robot observations during navigation.

2.2.3. SLAM Using Text Features

In [44], OCR was used in SLAM by fusing measurements to the centroids of unique text instances with visual inertial odometry via incremental Smoothing and Mapping. In [45], SLAM was performed using the planar features of text instances in the environment by minimizing the projected photometric error of detected text boxes via bundle adjustment.

2.2.4. Semantic SLAM

More recently, SLAM methods have been detecting and classifying objects in the environment to semantically segment and label maps [32,46,47,48,49]. In [32], Mask-RCNN was used for the image segmentation task and combined with an RTAB-Map SLAM algorithm to generate semantic point clouds of the environment. In [46], a method that combined semantic segmentation information from PSPNet with 3D Point cloud data from ORB-SLAM was proposed to create a 3D semantic map and in [47], a SLAM method combining planar surfaces of semantically detected objects and visual inertial odometry was proposed.

In both [48,49], dynamic objects were considered. Namely, in [48], a SLAM approach used surfel-based mapping and semantic labels to filter out dynamic obstacles from 3D point cloud readings. The semantic segmentation resulted in point-wise labels for each point in the point cloud. In [49], optical flow was combined with MASK-RCNN SLAM to filter dynamic feature points. The Mask-RCNN network was used to detect and mask potentially moving objects.

The methods described above have shown that contextual information can be used for various robotic tasks. However, none of the methods can be used to solve our grocery store search problem. In particular, some methods require an existing map of the environment [21,22,42,43] or training on an existing map [37] prior to the creation of a context map. Others represent their occupancy grid based on the locations of distinct features [39]. If features are occluded, error accumulation occurs, resulting in an inconsistent map. Furthermore, some methods require the environment to be modified with artificial landmarks (i.e., April or RFID tags) [36,40], or require unique landmarks [38,44]. Lastly, it is infeasible to create an object detector for all items within a grocery store environment, as the number of classes required for a generic classifier may be on the order of 10⁵ [50,51]. As a result, semantic SLAM methods, which inherently use object classifiers, are limited in the number of products that they can accurately identify, which limits the robot’s ability to search for and navigate to a variety of different products [32,46,47,48,49]. Using text, on the other hand, results in a more accurate classification of products since all products in the environment have a text label, either on the shelf or the packaging themselves. In [45], text features were used for SLAM, however, only the planar surfaces of the detected text were utilized and the method did not annotate the map with the contextual information of the text strings found [45]. Therefore, these approaches cannot be used for our grocery search problem as: (1) real grocery environments contain repeated features (e.g., same text on signage, shelves, posters, etc.), and (2) introducing artificial landmarks requires each environment to be modified prior to the robot being used in the store.

In this paper, we propose a new online approach, contextSLAM, that simultaneously utilizes both the occupancy grid and observed non-unique context in the environment obtained via OCR to localize the robot and generate a contextually rich map. Our approach allows for online intelligent robot exploration of the environment by using the context map during navigation. Furthermore, we make use of scene text that is pervasive within a grocery store environment without the limitation that the contextual information be unique.

3. Grocery Robot System Architecture

The proposed grocery product search problem requires a mobile robot to search for and locate various products in an unknown environment which may contain other independent agents (e.g., people). The store contains a set of products ${\mathbb{P}}_{\mathrm{gr}}$. The task begins when a user provides a subset of products to locate, $\mathcal{Q}\subseteq {\mathbb{P}}_{\mathrm{gr}}$. A product is considered to be located when the robot detects it on a shelf.

The grocery environment contains an open area where the robot’s home location is. Aisles exist perpendicular to the edges of this open area and are formed by shelving units containing products on both sides. Aisles have openings at both ends and have a traversable width, $w_a$, and minimum length $l_{\min }$. Aisle signs are placed visibly within the aisle and display contextual information. Static text landmarks (e.g., on aisle signs) are present throughout the environment. Unique fiducial markers are placed beside products of interest on shelves, as the product identification problem is beyond the scope of this paper.

3.1. Architecture Overview

For a robot to autonomously search a grocery store for user desired products, we have developed the grocery robot architecture in Figure 1. A search is initiated by a user providing a search query. This triggers the Explore state within the Action Deliberation module that uses the Frontier Detection module to implement frontier-based exploration. Navigation goals are sent to the Navigation System which provides motion commands to the Low-Level Controller. Frontiers are determined using the map generated by the Context Mapping module. This map is a fusion of data from the Context Identification and Obstacle Detection modules and Odometry. Parallel to exploration, the Aisle Detection module finds aisles in the environment. When there is evidence of the presence of a product in the search query in an aisle, the Action Deliberation module transitions to the Aisle Found state and the Navigation System localizes the robot in front of the aisle. The Aisle Search state allows the robot to navigate a crowded aisle. The Product Detection module is used to detect products within images from the onboard Camera. When a product in the search query is found the user is notified. The search continues until all products are found, no frontiers remain, or the maximum number of search attempts has been reached. The following subsections detail the main modules of the architecture.

3.2. Context Identification

The Context Identification module uses 2D images of the environment to identify contextual information to be used to find the locations of products in $\mathcal{Q}$. It utilizes OCR to identify text that is pervasive within grocery stores (e.g., on aisle signs).

The product function, $P\left(\mathcal{S}\right),$ is used to determine whether a set of text indicates the presence of a product, where:

$$P\left(\mathcal{S}\right)=\left\{p_{\mathrm{gr}}\right|p_{\mathrm{gr}}\in {\mathbb{P}}_{\mathrm{gr}},\mathrm{Pr}(p_{\mathrm{gr}}|\mathcal{S})>{\varphi }_{min}\},$$

(1)

and ${\varphi }_{min}$ is a probability threshold. $\mathcal{S}$ is a set of strings and $\mathrm{Pr}\left(p_{\mathrm{gr}}|\mathcal{S}\right)$ is the probability that a product $p_{\mathrm{gr}}$ is present given a set of strings $\mathcal{S}$. Aisle signs have the property that the context detected on the signs $s_a$ is guaranteed to generate a non-empty set, i.e., $\left|P\left(\right\{s_a\})\right|>0$, based on this assumption we set the threshold ${\varphi }_{min}$ to 1 for our experiments in Section 5. In general, the threshold is a user-defined value and is set based on the desired precision and recall of the probabilistic model representing the association of scene text to the presence of products.

Our grocery OCR system combines convolution neural network detectors and trains them to find text in images of the environment. It begins with a RetinaNet single stage text object detector [52] with a ResNeXt-50 [53] backbone, which predicts which regions in an image contain text. Region proposals (formed by a quadrilateral) are placed into standard size containers using a homography transform. Text strings in the regions are identified with a character region neural network [54] and filtered using a dictionary. Detections are formed by a tuple containing the text strings, $s$, and the relative 3D world coordinate, $p_{xyz}$, of each region’s center. The 2D images are obtained from an RGB-D Camera, such that each pixel is associated with a depth measurement. The world coordinate is found by sampling a concurrently captured point cloud region as the text. The observation set, $o$ = $\left\{\left(s_i,p_{xyz,i}\right)\right\}$, is provided to the Context Mapping module.

3.3. Obstacle Detection

Grocery shoppers make unpredictable stops and change directions to complete their shopping goals. Dynamic people can introduce false obstacles within maps of the environment, which can lead to map misalignments and localization errors [55]. These map errors can prevent planners from finding valid navigation plans through an environment to goal locations. To detect people we adapted the leg detection method in [56] and incorporated a per laser beam score with respect to each beam intersecting a leg. Beams that are observing dynamic obstacles are clustered based on distance with a minimum of three members in a cluster, Figure 2. A random forest assigns a confidence to each cluster indicating the likelihood of containing a leg. A higher confidence is given to cluster members closer to the cluster center than the edges. The weighted scan is provided to the Context Mapping module to prevent the addition of dynamic obstacles to the map.

3.4. Context Mapping

The Context Mapping module uses the novel contextSLAM approach we have developed for the online creation of an annotated occupancy grid map of the environment. ContextSLAM incorporates sparse, non-unique, landmarks represented as environment text and laser scan information provided by the Obstacle Detection module within a Rao-Blackwellized particle filter (RBPF). The landmarks are used concurrently with the annotated context map to localize the robot in the environment. We expand the Rao-Blackwellized factorization in [57] to incorporate context observations $o_{1:t}$ = $\left[o_1,\dots ,o_t\right]$. This is achieved by introducing a new probabilistic model, and particle weights and robot pose estimation representations. The joint distribution of the context, context map $\mathbb{M}$, robot state trajectory $x_{1:t}$ = $\left[x_1,\dots ,x_t\right]$, 2D laser scan measurements $z_{1:t}=\left[z_1,\dots ,z_t\right]$, and odometry $y_{1:t-1}=\left[y_1,\dots ,y_{t-1}\right]$ is:

$$\mathrm{Pr}\left(x_{1:t},\mathbb{M}|o_{1:t},z_{1:t},y_{1:t-1}\right)=\mathrm{Pr}\left(\mathbb{M}|x_{1:t},o_{1:t},z_{1:t}\right)\mathrm{Pr}\left(x_{1:t}|o_{1:t},z_{1:t},y_{1:t-1}\right),$$

(2)

where $\mathrm{Pr}\left(\mathbb{M}|x_{1:t},o_{1:t},z_{1:t}\right)$ represents the probability of the current context map given the robot trajectory, measurements and odometry. $\mathrm{Pr}\left(x_{1:t}|o_{1:t},z_{1:t},y_{1:t-1}\right)$ represents the probability of the trajectory given the measurements, odometry, and context. We obtain these probabilities using a particle filter.

Particle weights within the particle filter, $W_t^{\left(i\right)}$, indexed by $i$, are calculated based on the following relationship:

$$W_t^{\left(i\right)}=\mathrm{Pr}\left(x_{1:t}^{\left(i\right)}|o_{1:t},z_{1:t},y_{1:t-1}\right)/\pi \left(x_{1:t}^{\left(i\right)}|o_{1:t},z_{1:t},y_{1:t-1}\right),$$

(3)

where $\mathrm{Pr}\left(x_{1:t}^{\left(i\right)}|o_{1:t},z_{1:t},y_{1:t-1}\right)$ is the posterior over the potential trajectories, and $\pi \left(x_{1:t}^{\left(i\right)}|o_{1:t},z_{1:t},y_{1:t-1}\right)$ is the proposal distribution. We obtain a recursive formulation for the weights by constraining the distributions of the current trajectory to be a product of the previous likelihoods:

$$\begin{array}{l}{W}_t^{\left(i\right)}=\frac{\mathrm{Pr}\left(x_t^{\left(i\right)}|x_{1:t-1}^{\left(i\right)}, o_{1:t},z_{1:t},y_{1:t-1}\right)\mathrm{Pr}\left(x_{1:t-1}^{\left(i\right)}|o_{1:t-1},z_{1:t-1},y_{1:t-2}\right)}{\pi \left(x_t^{\left(i\right)}|x_{1:t-1}^{\left(i\right)},o_{1:t},z_{1:t},y_{1:t-1}\right)\pi \left(x_{1:t-1}^{\left(i\right)}|o_{1:t-1},z_{1:t-1},y_{1:t-2}\right)}\\ =\frac{\mathrm{Pr}\left(x_t^{\left(i\right)}|x_{1:t-1}^{\left(i\right)}, o_{1:t},z_{1:t},y_{1:t-1}\right)}{\pi \left(x_t^{\left(i\right)}|x_{1:t-1}^{\left(i\right)},o_{1:t},z_{1:t},y_{1:t-1}\right)}{W}_{t-1}^{\left(i\right)}.\end{array}$$

(4)

Furthermore, we can incorporate the previous context, $o_{1:t-1},$ and laser scan measurements, $z_{1:t-1}$, into the previous map estimate, ${\mathbb{M}}_{t-1}^{\left(i\right)}.$ Given this decomposition, the one time-step of the proposal distribution incorporating the context and laser scan measurements is defined as: $\pi \left(x_t^{\left(i\right)}|x_{1:t-1}^{\left(i\right)},o_{1:t},z_{1:t},y_{1:t-1}\right)$ = $\mathrm{Pr}\left(x_t^{\left(i\right)}|{\mathbb{M}}_{t-1}^{\left(i\right)},x_{t-1}^{\left(i\right)},o_t,z_t,y_{t-1}\right).$

Using Bayes’ rule, the proposal distribution is:

$$\mathrm{Pr}\left(x_t^{\left(i\right)}|{\mathbb{M}}_{t-1}^{\left(i\right)},x_{t-1}^{\left(i\right)},o_t,z_t,y_{t-1}\right)=\frac{\mathrm{Pr}\left(z_t,o_t|{\mathbb{M}}_{t-1}^{\left(i\right)},x_t^{\left(i\right)}\right)\mathrm{Pr}\left(x_t^{\left(i\right)}|x_{t-1}^{\left(i\right)},y_{t-1}\right)}{{\displaystyle \int }\mathrm{Pr}\left(z_t,o_t|{\mathbb{M}}_{t-1}^{\left(i\right)},x\prime \right)\mathrm{Pr}\left(x\prime |x_{t-1}^{\left(i\right)},y_{t-1}\right)dx\prime }.$$

(5)

By substituting the proposal distribution Equation (5) into Equation (4), and applying Bayes’ Rule to the posterior distribution, we then obtain the following weight update:

$$\begin{array}{ll}{W}_t^{\left(i\right)}& =\frac{\eta \mathrm{Pr}\left(z_t,o_t|{\mathbb{M}}_{t-1}^{\left(i\right)},x_t^{\left(i\right)}\right)\mathrm{Pr}(x_t^{\left(i\right)}|x_{t-1}^{\left(i\right)},y_{t-1}}{\frac{\mathrm{Pr}\left(z_t,o_t|{\mathbb{M}}_{t-1}^{\left(i\right)},x_t^{\left(i\right)}\right)\mathrm{Pr}\left(x_t^{\left(i\right)}|x_{t-1}^{\left(i\right)},y_{t-1}\right)}{\int \mathrm{Pr}\left(z_t,o_t|{\mathbb{M}}_{t-1}^{\left(i\right)},x^{\prime }\right)\mathrm{Pr}\left(x^{\prime }|x_{t-1}^{\left(i\right)},y_{t-1}\right)d{x}^{\prime }}}{W}_{t-1}^{\left(i\right)}\\ & =W_{t-1}^{\left(i\right)}\eta ·\int \mathrm{Pr}\left(z_t,o_t|{\mathbb{M}}_{t-1}^{\left(i\right)},x^{\prime }\right)\mathrm{Pr}\left(x^{\prime }|x_{t-1}^{\left(i\right)},y_t\right)d{x}^{\prime }\\ & \propto W_{t-1}^{\left(i\right)}·{\int }^{}\mathrm{Pr}\left(z_t,o_t|{\mathbb{M}}_{t-1}^{\left(i\right)},x^{\prime }\right)\mathrm{Pr}\left(x^{\prime }|x_{t-1}^{\left(i\right)},y_t\right)d{x}^{\prime },\end{array}$$

(6)

where $\mathsf{\eta }$= $1/\mathrm{Pr}\left(z_t,o_t|z_{1:t-1},o_{1:t-1},y_{1:t-1}\right)$ is constant for all weights and is a normalization factor resulting from Bayes’ Rule. We use a Gaussian approximation of the proposal distribution in Equation (6):

$$\begin{array}{l}{W}_t^{\left(i\right)}=W_{t-1}^{\left(i\right)}\underset{}{\overset{}{{\displaystyle \int }}}\mathrm{Pr}\left(z_t,o_t|{\mathbb{M}}_{t-1}^{\left(i\right)},x^{\prime }\right)\mathrm{Pr}\left(x^{\prime }|x_{t-1}^{\left(i\right)},y_t\right)d{x}^{\prime }\\ \cong W_{t-1}^{\left(i\right)}{\mathsf{\Sigma }}_{j=1}^{n_s}\mathrm{Pr}\left(z_t,o_t|{\mathbb{M}}_{t-1}^{\left(i\right)},x_{s,j}\right)\mathrm{Pr}\left(x_{s,j}|x_{t-1}^{\left(i\right)},y_t\right),\end{array}$$

(7)

where $n_s$ is the number of sampled poses. States $x_{s,j}$ are sampled from $x_{s,j}$~ $\left\{x_{s,j}|\Vert x_{s,j}-x_t^{*\left(i\right)}\Vert <\delta \right\}$, where $\delta$ is a sampling radius [58]. To obtain the current most likely pose, $x_t^{*\left(i\right)},$ given a laser scan and context, we use:

$$x_t^{*\left(i\right)}=\underset{x}{\mathrm{argmax}}\mathrm{Pr}\left(x|{\mathbb{M}}_{t-1}^{\left(i\right)},x_t^{+\left(i\right)},z_t,o_t\right),$$

(8)

where $x_t^{+\left(i\right)}=x_{t-1}^{\left(i\right)}\oplus y_{t-1}$ is the predicted next state based on the odometry and motion model. To avoid particle saturation, we use the adaptive resampling method in [57].

As the 2D laser scans, $z_t$, detect the horizontal plane of obstacles for mapping, the beams used in $z_t$ are independent of the Context Identification module that localizes context, $o_t$, which are above this horizontal plane. Thus, $z_t$ and $o_t$ are conditionally independent given the map such that:

$$\mathrm{Pr}\left(z_t,o_t|{\mathbb{M}}_{t-1}^{\left(i\right)},x_t^{\left(i\right)}\right)=\mathrm{Pr}\left(z_t\right|{\mathbb{M}}_{oc{c}_{t-1}}^{\left(i\right)},x_t)\mathrm{Pr}(o_t|{\mathcal{K}}_{t-1}^{\left(i\right)},x_t),$$

(9)

where ${\mathbb{M}}_{\mathrm{occ}}$ is an occupancy grid and $\mathcal{K}$ is a set of Extended Kalman Filters (EKFs) that track each detected string and the probability density of its location in the environment. As a result, we arrive at our final form for the weight update:

$$W_t^{\left(i\right)}=W_{t-1}^{\left(i\right)}{\mathsf{\Sigma }}_{j=1}^{n_s}\mathrm{Pr}\left(z_t|{\mathbb{M}}_{oc{c}_{t-1}}^{\left(i\right)},x_{s,j}\right)\mathrm{Pr}\left(o_t|{\mathcal{K}}_{t-1}^{\left(i\right)},x_{s,j}\right)\mathrm{Pr}\left(x_{s,j}|x_{t-1}^{\left(i\right)},y_t\right).$$

(10)

The set of tuples $\left({\kappa }_k,s_k\right)\in \mathcal{K}$, in the map $\mathbb{M}$, contain an EKF, ${\kappa }_k$, and a string, $s_k$ for each context landmark detected in the environment. When contextSLAM receives a context detection, $\left(s_i,p_{xyz,i}\right)\in o$, we find the set of EKFs, ${\mathcal{K}}_{si},$ with matching strings. We then find the EKF within ${\mathcal{K}}_{si}$ that minimizes the Mahalanobis distance $d_m\left({\kappa }_k,p_{xyz,i}\right)$ between the context location and the EKF:

$$\left({\kappa }_{\min },p_{xyz,\min }\right)=\underset{\left({\kappa }_i,p_{xyz,i}\right)\in {\mathcal{K}}_{si}}{\mathrm{argmin}}{d}_m\left({\kappa }_i,p_{xyz,i}\right),$$

(11)

where the estimated covariance associated with each EKF, ${\kappa }_i,$ is used when computing $d_m$. If the minimum distance is below a threshold, then the EKF ${\kappa }_{\min }$ is updated using $p_{xyz,\min }$. Otherwise a new EKF is created using the measurement and appended to $\mathcal{K}$. Thresholding allows for non-unique context instances within the environment.

When updating an EKF, the predictions for the expected location and covariances of the string are simply the last estimates, since context observations are not expected to move in the map frame from one time step to another:

$${\widehat{o}}_{i,t|t-1}={\widehat{o}}_{i,t-1},$$

(12)

$${\widehat{\mathsf{\Sigma }}}_{i,t|t-1}={\widehat{\mathsf{\Sigma }}}_{i,t-1}.$$

(13)

The measurement model for each context, $i$, is computed from the a priori estimate, ${\widehat{o}}_{i,t|t-1}$, and the current measured pose, $x_{xy\theta }$, such that:

$$h\left({\widehat{o}}_{i,t|t-1 }, x_{xy\theta }\right)=\left[\begin{array}{c}{\left||{{\widehat{o}}_{i,t|t-1 }}_{xy}-x_{xy}|\right|}^2\\ atan\left({{\widehat{o}}_{i,t|t-1 }}_x-x_x, {{\widehat{o}}_{i,t|t-1 }}_y-x_y\right)- x_{\theta }\end{array} \right].$$

(14)

Then, the context’s 3D world coordinate, $p_{xyz,i}$, is projected into polar coordinates, ${\left[r_{i,t},{\theta }_{i,t}\right]}^T,$ where $r_{i,t}\mathrm{and}{\theta }_{i,t}$ are the distance and heading of detected context, $i$, in the robot’s frame. The projected point is then used to compute the residual error $y_t={\left[r_{i,t},{\theta }_{i,t}\right]}^T-h\left({\widehat{o}}_{i,t|t-1},x_{xy\theta }\right)$. The residual error is then used with the standard EKF correction update:

$$y_{i,t}=\left[\begin{array}{c}{r}_{i,t}\\ {\theta }_{i,t}\end{array}\right]-h\left({\widehat{o}}_{i,t|t-1},x_{xy\theta }\right),$$

(15)

$$Q_{i,t}=J_{i,t}{\widehat{\mathsf{\Sigma }}}_{i,t|t-1}{J}_{i,t}^T+R_{i,t},$$

(16)

$$K_{i,t}={\hat{\mathsf{\Sigma }}}_{i,t|t-1}{J}_{i,t}^T*Q_{i,t}^{-1},$$

(17)

$${\widehat{o}}_{i,t|t}={\widehat{o}}_{i,t|t-1}+K_{i,t}{y}_{i,t},$$

(18)

$${\widehat{\mathsf{\Sigma }}}_{i,t|t}=\left(I-K_{i,t}{J}_{i,t}\right){\widehat{\mathsf{\Sigma }}}_{i,t|t-1}+K_{i,t}{R}_{i,t}{K}_{i,t}^T,$$

(19)

where $R_t$ is the noise associated with range measurement ${\left[r,\theta \right]}^T$, $J_t$ is the Jacobian of $h\left({\widehat{o}}_{t|t-1},x_{xy\theta }\right)$ with respect to ${\widehat{o}}_{t|t-1}$, and $I$ is the identity matrix.

ContextSLAM provides the most confident particle and its corresponding context map to the other modules. An overview of the contextSLAM Algorithm is presented in Algorithm 1.

Algorithm 1: contextSLAM: RBPF method extension to include context.

Require: ${\mathsf{\Phi }}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)},$ the sample set of the previous time step; ${\mathrm{z}}_{\mathrm{t}},$ the current laser scan from Obstacle Detection; ${\mathrm{o}}_{\mathrm{t}},$ the current context observation from Context Identification; and ${\mathrm{y}}_{\mathrm{t}-1},$ the current odometry observation. Ensure: ${\mathsf{\Phi }}^{\mathrm{t}}=\left\{\right\}$ #The new sample set for ${\mathsf{\varphi }}_{t-1}^{\left(i\right)}\in {\mathsf{\Phi }}_{\mathrm{t}-1}^{}$do   (${\mathrm{x}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)},{\mathrm{W}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)},{\mathbb{M}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)}$)$={\mathsf{\varphi }}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)}$   $\left({\mathbb{M}}_{{\mathrm{occ}}_{\mathrm{t}-1}}^{\left(\mathrm{i}\right)},{\mathcal{K}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)}\right)={\mathbb{M}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)}$ #Expand context map into grid and context EKFs.   ${\mathrm{x}}_{\mathrm{t}}^{+\left(\mathrm{i}\right)}={\mathrm{x}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)}\oplus {\mathrm{y}}_{\mathrm{t}-1}$ #Motion model   ${\mathrm{x}}_{\mathrm{t}}^{*\left(\mathrm{i}\right)}=\underset{\mathrm{x}}{\mathrm{argmax}}\mathrm{Pr}(\mathrm{x}|{\mathbb{M}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)},{\mathrm{x}}_{\mathrm{t}}^{+\left(\mathrm{i}\right)},{\mathrm{z}}_{\mathrm{t}},{\mathrm{o}}_{\mathrm{t}})$ #Max probability state of ${\mathrm{x}}_{\mathrm{t}}^{\left(\mathrm{i}\right)}$.   If ${\mathrm{x}}_{\mathrm{t}}^{*\left(\mathrm{i}\right)}=\mathrm{failure}$ then       ${\mathrm{x}}_{\mathrm{t}}^{\left(\mathrm{i}\right)}~\mathrm{Pr}\left({\mathrm{x}}_{\mathrm{t}}^{\left(\mathrm{i}\right)}{|\mathrm{x}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)},{\mathrm{y}}_{\mathrm{t}-1}\right)$         ${\mathrm{W}}_{\mathrm{t}}^{\left(\mathrm{i}\right)}={\mathrm{W}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)}\mathrm{Pr}({\mathrm{z}}_{\mathrm{t}}|{\mathbb{M}}_{{\mathrm{occ}}_{\mathrm{t}-1}}^{\left(\mathrm{i}\right)},{\mathrm{x}}_{\mathrm{s},\mathrm{j}})\mathrm{Pr}({\mathrm{o}}_{\mathrm{t}}|{\mathcal{K}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)},{\mathrm{x}}_{\mathrm{s},\mathrm{j}})$ #Next particle weights.   Else    for $\mathrm{j}=1,\dots ,{\mathrm{n}}_{\mathrm{s}}$ do #Sample around the node       ${\mathrm{x}}_{\mathrm{s},\mathrm{j}}\sim \left\{{\mathrm{x}}_{\mathrm{s},\mathrm{j}}\right|\left|\left|{\mathrm{x}}_{\mathrm{s},\mathrm{j}}-{\mathrm{x}}_{\mathrm{t}}^{*\left(\mathrm{i}\right)}\right|\right|<\mathsf{\delta }\}$    end for    ${\mathsf{\mu }}_{\mathrm{t}}^{\left(\mathrm{i}\right)}={\left(0,0,0\right)}^{⊺}$ #Compute Gaussian proposal    $\mathsf{\Sigma }=0$     ${\mathrm{n}}_{\mathsf{\mu }}^{\left(\mathrm{i}\right)}=0$     for all ${\mathrm{x}}_{\mathrm{s},\mathrm{j}}\in \{{\mathrm{x}}_{\mathrm{s},1},\dots ,{\mathrm{x}}_{\mathrm{s},{\mathrm{n}}_{\mathrm{s}}}\}$ do       ${\mathsf{\mu }}_{\mathrm{t}}^{\left(\mathrm{i}\right)}\leftarrow {\mathsf{\mu }}_{\mathrm{t}}^{\left(\mathrm{i}\right)}+{\mathrm{x}}_{\mathrm{s},\mathrm{j}}\mathrm{Pr}({\mathrm{z}}_{\mathrm{t}}|{\mathbb{M}}_{{\mathrm{occ}}_{\mathrm{t}-1}}^{\left(\mathrm{i}\right)},{\mathrm{x}}_{\mathrm{s},\mathrm{j}})\mathrm{Pr}({\mathrm{o}}_{\mathrm{t}}|{\mathcal{K}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)},{\mathrm{x}}_{\mathrm{s},\mathrm{j}})\mathrm{Pr}({\mathrm{x}}_{\mathrm{s},\mathrm{j}}|{\mathrm{x}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)},{\mathrm{y}}_{\mathrm{t}})$       ${\mathrm{n}}_{\mathsf{\mu }}^{\left(\mathrm{i}\right)}\leftarrow {\mathrm{n}}_{\mathsf{\mu }}^{\left(\mathrm{i}\right)}+\mathrm{Pr}({\mathrm{z}}_{\mathrm{t}}|{\mathrm{M}}_{{\mathrm{occ}}_{\mathrm{t}-1}}^{\left(\mathrm{i}\right)},{\mathrm{x}}_{\mathrm{s},\mathrm{j}})\mathrm{Pr}({\mathrm{o}}_{\mathrm{t}}|{\mathcal{K}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)},{\mathrm{x}}_{\mathrm{s},\mathrm{j}})\mathrm{Pr}({\mathrm{x}}_{\mathrm{s},\mathrm{j}}|{\mathrm{x}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)},{\mathrm{y}}_{\mathrm{t}})$    end for     ${\mathsf{\mu }}_{\mathrm{t}}^{\left(\mathrm{i}\right)}\leftarrow {\mathsf{\mu }}_{\mathrm{t}}^{\left(\mathrm{i}\right)}/n_{\mathsf{\mu }}^{\left(\mathrm{i}\right)}$     for all ${\mathrm{x}}_{\mathrm{s},\mathrm{j}}\in \{{\mathrm{x}}_{\mathrm{s},1},\dots ,{\mathrm{x}}_{\mathrm{s},{\mathrm{n}}_{\mathrm{s}}}\}$ do       ${\mathsf{\Sigma }}_{\mathrm{t}}^{\left(\mathrm{i}\right)}\leftarrow {\mathsf{\Sigma }}_{\mathrm{t}}^{\left(\mathrm{i}\right)}+({\mathrm{x}}_{\mathrm{s},\mathrm{j}}-{\mathsf{\mu }}_{\mathrm{t}}^{\left(\mathrm{i}\right)}){({\mathrm{x}}_{\mathrm{s},\mathrm{j}}-{\mathsf{\mu }}_{\mathrm{t}}^{\left(\mathrm{i}\right)})}^{\mathsf{{\rm T}}}\cdot$       $\mathrm{Pr}({\mathrm{z}}_{\mathrm{t}}|{\mathrm{M}}_{{\mathrm{occ}}_{\mathrm{t}-1}}^{\left(\mathrm{i}\right)},{\mathrm{x}}_{\mathrm{s},\mathrm{j}})\mathrm{Pr}({\mathrm{o}}_{\mathrm{t}}|{\mathcal{K}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)},{\mathrm{x}}_{\mathrm{s},\mathrm{j}})\mathrm{Pr}({\mathrm{x}}_{\mathrm{s},\mathrm{j}}|{\mathrm{x}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)},{\mathrm{y}}_{\mathrm{t}})$     end for     ${\mathsf{\Sigma }}_{\mathrm{t}}^{\left(\mathrm{i}\right)}\leftarrow {\mathsf{\Sigma }}_{\mathrm{t}}^{\left(\mathrm{i}\right)}/{\mathrm{n}}_{\mathsf{\mu }}^{\left(\mathrm{i}\right)}$    ${\mathrm{x}}_{\mathrm{t}}^{\mathrm{i}}\sim \mathcal{N}\left({\mathsf{\mu }}_{\mathrm{t}}^{\left(\mathrm{i}\right)},{\mathsf{\Sigma }}_{\mathrm{t}}^{\left(\mathrm{i}\right)}\right)$#Sample new pose    ${\mathrm{W}}_{\mathrm{t}}^{\left(\mathrm{i}\right)}={\mathrm{W}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)}{\mathrm{n}}_{\mathsf{\mu }}^{\left(\mathrm{i}\right)}$ #Update particle weights   end if   ${\mathbb{M}}_{{\mathrm{occ}}_{\mathrm{t}}}^{\left(\mathrm{i}\right)}=\mathrm{integrateScan}\left({\mathbb{M}}_{{\mathrm{occ}}_{\mathrm{t}-1}}^{\left(\mathrm{i}\right)},{\mathrm{x}}_{\mathrm{t}}^{\left(\mathrm{i}\right)},{\mathrm{z}}_{\mathrm{t}}\right)$#Update occupancy grid   ${\mathcal{K}}_{\mathrm{t}}^{\left(\mathrm{i}\right)}=\mathrm{integrateText}\left({\mathcal{K}}_{\mathrm{t}-1}^{\left(\mathrm{i}\right)},{\mathrm{x}}_{\mathrm{t}}^{\left(\mathrm{i}\right)},{\mathrm{z}}_{\mathrm{t}}\right)$ #Update maps with context   ${\mathsf{\Phi }}_{\mathrm{t}}^{\left(\mathrm{i}\right)}={\mathsf{\Phi }}_{\mathrm{t}}^{\left(\mathrm{i}\right)}{{\displaystyle \cup }}^{}\left\{\left({\mathrm{x}}_{\mathrm{t}}^{\left(\mathrm{i}\right)},{\mathrm{W}}_{\mathrm{t}}^{\left(\mathrm{i}\right)},\left({\mathbb{M}}_{{\mathrm{occ}}_{\mathrm{t}}}^{\left(\mathrm{i}\right)},{\mathcal{K}}_{\mathrm{t}}^{\left(\mathrm{i}\right)}\right)\right)\right\}$ #Update sample set end for  ${\mathrm{N}}_{\mathrm{eff}}=1/{\mathsf{\Sigma }}_{\mathrm{i}=1}^{\left|{\mathsf{\Phi }}_{\mathrm{t}}\right|}{({\mathrm{W}}_{\mathrm{t}}^{\left(\mathrm{i}\right)}/{\mathsf{\Sigma }}_{\mathrm{j}=1}^{\left|{\mathsf{\Phi }}_{\mathrm{t}}\right|}{\mathrm{W}}_{\mathrm{t}}^{\left(\mathrm{j}\right)})}^2$ If ${\mathrm{N}}_{\mathrm{eff}}<\mathrm{T}$ then   ${\mathsf{\Phi }}_{\mathrm{t}}=\mathrm{resample}\left({\mathsf{\Phi }}_{\mathrm{t}}\right)$

end if

3.5. Aisle Detection

The Aisle Detection module identifies potential aisles within the context map generated by the Context Mapping module. We examine the contours detected in the context map to find a set of candidate aisles, $\mathcal{A},$ that satisfy the geometric constraints related to the minimum/maximum aisle dimensions, and wall parallelism. The set of context EKFs associated with aisle $a_k\mathrm{are}$:

$${\mathcal{A}}_{{\mathcal{C}}_{𝓀}}=\left\{s_m|\left({\mathsf{\kappa }}_m,s_m\right)\in \mathcal{K},d_c\left(a,{\mathsf{\kappa }}_{\mathrm{m},\mathsf{\mu }}\right)<w_{a,\min }/2\right\},$$

(20)

where $d_c\left(a_k,{\kappa }_{m,\mu }\right)$ is the minimum distance between an aisle and the point ${\kappa }_{m,\mu }$ which is the mean of ${\kappa }_m$. Aisle product and aisle geometry information is provided to the Action Deliberation module to aid in searching for products.

3.6. Action Deliberation

The Action Deliberation module controls the robot’s actions based on the information provided by the other modules. It uses a finite state machine that contains three main states, Figure 3. It is initiated by user input, leading to the Begin Exploration state transition. A copy of the search query, ${\mathcal{Q}}_{\mathrm{cp}}$= $\mathcal{Q}$, tracks the set of unfound products. The main states are presented below:

3.6.1. Explore

The robot explores an unknown environment by navigating to frontiers provided by Frontier Detection. Frontiers are detected by examining the context map provided by Context Mapping module using frontier exploration [58], which provides a set of points representing a boundary between explored and unexplored space in the context map. The point with the minimum estimated travel time is provided to the Navigation module. The Navigation module uses the ROS Move Base package [59], where costmaps are used to plan navigation paths around dynamic obstacles which are executed by the Low-Level Controller. In the Explore state, if an aisle paired with context is found, the robot transitions to the Aisle Found state.

3.6.2. Aisle Found

In the Aisle Found state the robot has found an aisle that may contain a product of interest. An aisle is of interest if a product of interest has a Levenshtein distance less than 2 with any context found within the aisle. It navigates to the entrance of that aisle and transitions into the Search Aisle state.

3.6.3. Search Aisle

In the Search Aisle state the robot searches the aisle, navigating from the entrance to the opposite end using the Navigation module to locate the products of interest associated with the current aisle ${\mathcal{P}}_{\mathrm{gr}}=\mathrm{P}\left({\mathcal{A}}_e\right)$. ${\mathcal{P}}_{\mathrm{gr}}$ and the map are updated as new observations become available, and replanning takes place. The Product Detection module detects the products with onboard cameras. We use April tags [60] due to their robustness in cluttered environments for this module, as the product identification problem is beyond the scope of this paper. Several papers have only focused on this specific recognition problem [61,62,63]. Furthermore, the use of April tags is also a common approach in exploration problems when the focus is not on object/product recognition [36,64,65]. When a product, $p_{\mathrm{gr}}$, is found the set of products is updated ${\mathcal{Q}}_{\mathrm{cp}}\leftarrow {\mathcal{Q}}_{\mathrm{cp}}\backslash {\mathrm{p}}_{\mathrm{gr}}$, ${\mathcal{P}}_{\mathrm{gr}}\leftarrow {\mathcal{P}}_{\mathrm{gr}}\backslash {\mathrm{p}}_{\mathrm{gr}}$. The robot continues searching the aisle until $\left|{\mathcal{P}}_{\mathrm{gr}}\right|$ = $0$ or it has searched an aisle $n$ times. If there are more products on the list, i.e., $\left|{\mathcal{Q}}_{\mathrm{cp}}\right|>0$, the robot exits the Search Aisle state and enters the Explore state. If $\left|{\mathcal{Q}}_{\mathrm{cp}}\right|=0$, it enters the Finish Search state.

3.6.4. Finish Search

In the Finish Search state the robot has finished searching for products and returns to its home location. As the primary robot task is product search, the environment does not have to be fully explored when the search is complete.

4. Blueberry Robot Implementation

The grocery robot architecture is integrated into our Blueberry platform (Figure 4). The head has an RGB and depth camera for context identification and localization. The torso contains RGB cameras for product detection. The lidar generates a 3D point cloud which is converted into a 2D laser scan used for mapping and obstacle detection.

5. Experiments

We conducted extensive experiments to determine: (1) the accuracy of contextSLAM in comparison to an existing popular SLAM approach, and (2) the success rate of the architecture searching unknown real environments for products.

5.1. Map Performance

The performance of contextSLAM is validated by the accuracy of the robot’s entire trajectory using its generated map. This accuracy is defined by the root mean square error (RMSE):

$$\mathrm{RMSE}={\left({\mathsf{\Sigma }}_{k=1}^t{\left(x_k-{\widehat{x}}_k\right)}^2/t\right)}^{0.5}.$$

(21)

We measure the accuracy of the estimated robot’s entire trajectory $x_{1:t}$ with respect to the ground truth trajectory, ${\widehat{x}}_{1:t}$. Experiments were performed using the Stage Simulator [66] on four 19.6 × 29.5 m² environments consisting of: (1) mixed: aisles, dead-ends, and closed paths, Figure 5a; (2) dead-ends: aisles having dead-ends, Figure 5b; (3) loops: many closed paths, Figure 5c; and (4) circles: circular spaces with closed paths and dead-ends, Figure 5d. The ground truth trajectory was obtained from the ground truth state at each timestep as reported by Stage.

To simulate crowded retail conditions, we incorporated: (1) scene text, and (2) dynamic obstacles. Unique text were placed in 30 random locations in each environment and were published directly to the Context Mapping module. Context observations were tuples containing a randomly generated text string and the associated 2D world coordinate.

To simulate limited visibility of the cameras, a context observation was considered visible when it was within 3 m and 1.0 rad of the robot’s heading. When the robot navigated the environment, and the true location of a context was within its 2D sensory range, the Context Mapping module would receive an array of all visible context observations, where the provided 2D world coordinate would have a simulated measurement error modelled by a Gaussian distribution. In each environment, 45 dynamic obstacles were moving in circular motion paths. Each obstacle’s path radius was uniformly sampled between (0,1] m with velocity (0,1] m/s.

We manually navigated the robot within each environment four times, with unique paths. Each path was tested 13 times with different measurement errors and each trial was repeated 3 times (39 trials in total). The measurement error was represented by a Gaussian distribution with $\left(0,{\mathsf{\sigma }}_{\mathrm{r}}^2\in \mathsf{\Xi }\right)$ and $\left(0,{\mathsf{\sigma }}_{\mathsf{\theta }}^2\in \mathsf{\Xi }\right)$ for the range and angular measurements, such that $\mathsf{\Xi }$= {0.005,0.015,0.025,0.035,0.045}. In each trial a map is generated using contextSLAM and robot location estimates are obtained. For comparison purposes, we also conduct the same experiments using the popular GMapping SLAM approach. The RMSE of the predicted trajectory using estimates from both methods is presented in the boxplot in Figure 6 and a visualization of the predicted trajectories is presented in Figure 7.

5.1.1. Trajectory Prediction Results

The results show that our contextSLAM method had a lower RMSE of 1.14 m, compared to a RMSE of 1.98 m for GMapping. Furthermore, the Wilcoxon Sign Rank non-parametric test showed that the RMSE is statistically significantly smaller for our contextSLAM approach $\left(Z=3150.0,p<0.001\right)$. In Figure 7, we see that there is a significant improvement on trajectory predictions, particularly in environments that are lacking in corners, such as aisles having dead-ends and circular spaces.

5.1.2. Map Generation Results

Figure 8 shows examples of maps generated by contextSLAM (Figure 8a) and GMapping (Figure 8b), with 0.015 m radial and 0.025 rad angular context detection error. The maps demonstrate that contextSLAM maps had fewer false walls when compared to GMapping. The presence of false walls shows that GMapping failed to recognize and accurately localize in previously seen areas. For example, in the dead-end environment, an extra corridor was mapped. The use of salient text features in the environment allowed contextSLAM to build the environment map more effectively in areas that otherwise lacked corner features such as the circle environment. As a result, the use of contextual features shows a significant improvement towards loop closures by providing additional salient landmarks to localize against.

5.2. Using the Grocery Robot Architecture to Find Products

We evaluated the overall performance of the grocery robot architecture using Blueberry in two environments: (1) a grocery store-like environment, and (2) a real grocery store. The experiments test the feasibility and robustness of the architecture and its application to real-world environments with significant amounts of contextual information and product variety in both a controlled and real store setting. A series of trials were performed where the robot was provided a list of three to four items to find that were distributed within aisles in the environment. The robot started in its home location and explored the unknown environment with a maximum speed of 0.4 m/s to find all the items and returned home again. We consider the worst-case scenarios where the robot is deployed within a previously unseen store (no map information is available) or after layout changes have occurred (requiring the generation of a new map). The experiments were approved by the University of Toronto Ethics Committee (protocol number 37011) and all participants gave their informed consent prior to participating in the experiments.

5.2.1. Store-Like Environment

The 7 × 10 m² environment consisted of an open area with the robot’s home location and three parallel aisles containing the products on the list (Figure 9a). The open area represented the front of a store and was 2 × 4 m². Each aisle was 1.8 × 9 m². Hanging over the middle of each aisle was a two-sided aisle sign containing six product categories in that aisle (Figure 9b). The signs were 0.9 × 0.6 m² and 2.7 m above the ground.

Product search queries were generated using all combinations of 3–4 products consisting of: tea, cereal, pasta and household products. Two trials were performed for each query, for a total of 20 trials. Half were conducted with just the robot in the environment and the other half with up to three dynamic people in the environment with their own goals.

Store-Like Environment Results and Discussions

Table 1. Architecture performance and total search time in store-like environment.

No People—Number of Attempts to Find a Product
	Product	1	2	3	4	5	6	7	8	9	10
Trial
Tea		1	1	N/A	N/A	1	1	1	1	1	1
Cereal		1	1	1	1	N/A	N/A	1	1	1	1
Pasta		2	1	1	1	2	1	N/A	N/A	1	1
Household		N/A	N/A	1	1	1	1	1	1	1	1
Total Time (s)		230	170	240	285	250	205	230	270	270	235
With Dynamic People—Number of Attempts to Find a Product
Tea		1	1	N/A	N/A	1	1	1	1	1	1
Cereal		1	1	1	2	N/A	N/A	2	2	1	1
Pasta		1	1	3	3	1	2	N/A	N/A	1	1
Household		N/A	N/A	1	1	3	2	1	1	1	1
Total Time (s)		225	200	835	519	390	346	282	320	360	303

presents the number of attempts taken by Blueberry to find each product on the list and the physical search time to navigate the scene to find all the products for each trial. The number of attempts defines the number of times the robot traveled down an aisle to find the specified product location. In trials 1–8, three products were requested for the search, and in trials 9 and 10, all four products were requested. In Table 1, the non-requested products are represented by N/A. Blueberry was able to find all the requested products in every trial. In the experiments without people, the mean time to find a product was 79.5s ($\sigma$ = 10.17), with a mean of 1.06 searches ($\sigma$ = 0.24). In the experiments with dynamic people the mean time to find a product was 120.5 s ($\sigma$ = 60.5), with a mean of 1.34 searches ($\sigma$ = 0.64). The combined average computation time for running the context identification, mapping and planning methods together was only 60 ms.

There were six trials with dynamic people that required two or three searches for individual products. The trials that required three aisle searches were 3–5. In trials 3 and 4, there were instances of two people standing close together in the aisle (Figure 9b). The leg detector could not distinguish their legs and detected them as static objects, and, thus, they were included in the map. The robot then selected an alternative path, for example when searching for pasta in trial 3, it went through aisle 1 to navigate around them and reach its goal pose in aisle 2. Once the robot obtained new measurements of the aisle, the map was updated, and Blueberry followed a direct path through the aisle. In trial 5, the robot could not find a safe path around a large number of people blocking the aisle. It replanned, and on its 3rd search it was able to find a safe path through the aisle around the people. In general, since our person detection technique detects dynamic clusters as legs, wearing different clothing with varying colors or patterns did not impact its performance.

Sample context maps generated by contextSLAM are shown in Figure 10 for trial 9. The green lines represent aisle locations, numbered in the order of detection. The number is displayed if the aisle was associated with any context. Clusters of black text in the middle of the aisles are detected text on aisle signs. Context that has been matched with an aisle is shown in red.

5.2.2. Grocery Store Environment

Similar experiments were performed in a real grocery store (Figure 11a), which had significantly more clutter and context due to posters and packaging.

The experiment was conducted in an 8 × 14 m² section of the store which contained an approximately 5 × 7 m² open area and three aisles. The aisles in the search area were approximately 2.5 × 12 m². Two-sided signs were over the middle of each aisle (Figure 11b) and used different fonts than in experiment 1. Each sign contained 3–5 product categories. Search queries were generated using combinations of three or four products. A total of 10 trials were conducted, five with just the robot, and five with two dynamic people randomly walking and looking at items in the aisles. The robot always started in the open area in front of the aisles (Figure 12a). A video of Blueberry searching for products using our grocery robot architecture in this environment is presented here on our lab’s YouTube channel: https://youtu.be/9RYUxPVIhkM.

Grocery Store Environment Results and Discussion

Table 2. In store experiments—attempts to find a product and total search time.

		No People					Dynamic People
	Product	1	2	3	4	5	1	2	3	4	5
Trial
Crackers		1	N/A	1	1	1	2	N/A	2	1	1
Cereal		1	1	N/A	1	1	2	1	N/A	2	2
Granola		1	2	1	N/A	1	1	2	2	N/A	1
Honey		N/A	1	1	1	1	N/A	1	1	1	1
Total Time (s)		400	364	405	290	424	396	390	240	395	420

shows the number of search attempts needed to find each product and the time to find all products for each trial. The robot found all requested products in each trial. Trials without people, had a mean of 1.06 searches ($\sigma$ = 0.24). Trials with people had a mean of 1.44 searches ($\sigma$ = 0.50).

It is interesting to note that in some cases, the robot completed the trials with dynamic people in less time. For example, in trial 3 without people Blueberry searched the full environment to find all the desired products. The frontier exploration approach resulted in the robot taking a longer path by searching all aisles. However, in the dynamic people condition, all the products were found without having to search one of the aisles. In this case Blueberry chose to navigate the adjacent aisle, thus resulting in a shorter trial time.

The context maps generated in trial 5 are shown in Figure 12. The maps highlight the significant increase in text, observed in the grocery store compared to the store-like environment. Even with increased detections, the robot was able to efficiently find the products by associating text with their respective aisles, allowing for the successful completion of each of the queries.

6. Conclusions

In this paper we present a novel grocery robot architecture for searching for products in unknown cluttered grocery environments. The architecture uniquely combines an OCR system with a new context Simultaneous Localization and Mapping framework (contextSLAM). The contextSLAM method builds a map of the environment using context in the store such as aisle signs for which the robot can use to find products of interest. Experiments showed that a robot using the architecture can find multiple products in different environments with unpredictable dynamic people. Future work will include the investigation and integration of a product detection system such as SKU or few-shot object detection methods, and human-robot interaction studies with shoppers in a grocery store.