Lighthouses and Global Graph Stabilization: Active SLAM for Low-compute, Narrow-FoV Robots

Rule-based active SLAM strategies decompose the problem into (i) candidate action generation, (ii) utility computation, and (iii) action selection. For (i), random-sampling goal-selection strategies [11, 12] are very compute-efficient per candidate action but not exploration efficient. Frontier-based strategies [4, 13, 14, 15, 16, 17] use the insight that areas to explore in the map are at the boundary between known and unknown space; these approaches drive the robot directly to unexplored areas of the map but are more expensive since they require searching through the map. For (ii), traditional utility functions consider distance to the candidate goals[4, 15, 12]. More complex utility functions use information theory to approximate the expected reduction in uncertainty of the map at that candidate goal and balance that against the travel distance[18, 17, 11]. To estimate the uncertainty of the SLAM part of the map, some use various optimality criteria from the Theory of Experimental Design (TOED)[19, 20]. Some work[21, 22] creates “virtual” edges in a pose graph and then estimates the expected uncertainty along the path to the goal. For (iii), action selection performs some kind of optimization, the simplest being greedy selection, that minimizes either covariance or entropy[4, 15, 12, 17, 11].

Our approach fits into this set of strategies. Our candidate action generation is frontier-based and uses the frontiers to generate candidate viewpoints. We propose a novel compute-efficient utility computation accounting for the robot dynamics, and our greedy action selection works well in practice.

Instead of operating in a discrete space, belief-space strategies optimize the continuous trajectory and require a continuous utility function. These approaches incorporate an approximate uncertainty reduction term in the optimization to prefer trajectories that move towards the goal while reducing map uncertainty[23, 24].

Many deep reinforcement learning approaches [25, 26, 6, 27, 19] map sensor inputs, e.g., depth images and laser scans, perhaps along with some auxiliary information, into a fixed set of actions, e.g., go forward $0.2m$, turn left 8^∘, and turn right 8^∘; for these works, the reward functions consist of an extrinsic reward that performs collision avoidance and an intrinsic reward that encourages exploring new areas while minimizing uncertainty using information theory or TOED.

To construct the occupancy map, we use a frontier-based exploration strategy[4] to identify areas in the partially-explored map to reduce the occupancy map entropy, i.e., converting unknown cells into free or obstacle. A frontier is a contiguous set of unknown cells that are all directly adjacent to free cells. Using a wavefront search[13] over the occupancy map emanating from the robot pose and exploiting the contiguous property of frontier cells, we can find all frontiers in the partially-explored map. We produce a viewpoint for each frontier by computing a pose that obeys a number of constraints, e.g., away from obstacles but within sensor range. Since there are normally several frontiers, we assign a cost to each frontier viewpoint using Equation 1.

where $V$ represents the viewpoint-in-question; $R$ is the current pose of the robot; and $G$ is the previous viewpoint. $L(R,V)$ measures the path distance from the robot pose to the viewpoint $V$ using the A^⋆ global planner. For non-holonomic robots, we also have an angular penalty $A(R,V)$, along with a weight $\beta$, that discourages the robot from selecting viewpoints that require turning around since those are expensive operations for non-holonomic robots. The final term $\mathbb{I}(V\neq G)$, along with its penalty $\zeta$, is an indicator function that returns $1$ if the viewpoint-in-question $V$ and previous viewpoint $G$ are not the same. This prevents the robot from oscillating between two similarly-priced viewpoints by penalizing changing the viewpoint.

We take the greedy approach and travel to the frontier with the lowest cost. The nature of exploration constantly causes the frontier to be pushed back and deformed; furthermore, localization drift and loop closures can shift the map causing frontiers to change as well. For this reason, we re-evaluate all frontiers at a fixed rate to ensure we have the latest frontiers and viewpoints.

As the robot explores an environment, SLAM provides it with pose estimates that drift along the length of the trajectory. To correct this drift, we use purely vision-based loop closures. Since the creation of a loop closure constraint requires comparing the visual features of the current keyframe with a previous one, more correspondences help in both creation and disambiguation of loop closure constraints.

For a narrow-FoV robot, we can emulate a panoramic view by performing an in-place rotation. This is equivalent to having a ${360}^{\circ}$ view at the point where the robot stopped; intuitively, this is like “looking back” along the trajectory. But for narrow-FoV robots, this “looking back” requires extra motion and time so it would not be efficient to do this all across the map. Instead, while performing FE, we opportunistically and proactively identify a few key places to perform this in-place rotation. We monitor the incoming keyframes and, for each, we compute a view score that is the number of detected features in the keyframe. In addition to the panoramic view, the number of features also correlates to a higher likelihood of loop closure. If the view score is above a particular threshold, then we know that, if we were to perform an in-place rotation, then we have at least one view that is high in view score to encourage a likely loop closure. In the pose graph, this creates a special structure of keyframes that are both spatially and topologically clustered together, with a loop closure across all constituent keyframes. We call this cluster a lighthouse (Figure 2). If, while performing FE, we detect an incoming keyframe that satisfies the view score threshold, we pause FE and create a lighthouse which will be used by the LH-ALC planner for loop closure.

The purpose of ALC is to bound the pose uncertainty during FE by creating loop closure constraints in the pose graph. For keyframe-based vSLAM, this means traveling back to reacquire a view from some previous keyframe to trigger loop closure creation. However, for even smaller environments, vSLAM can produce hundreds, if not thousands, of keyframes that could be potential loop closure candidates. Furthermore, attempting to loop close at any one particular keyframe is very challenging since noise or drift may shift that keyframe making it difficult to reacquire.

To remedy these problems, instead of traveling back to a particular keyframe, we travel to a lighthouse and perform an in-place rotation. This greatly increases our likelihood of loop closure since the panoramic view detects many features that have a wider dispersion over the local environment than the features of a single keyframe. We call this flavor of ALC Lighthouse-based Active Loop Closure (LH-ALC).

There are several mechanisms to trigger LH-ALC such as (i) a periodic timer and (ii) computing the relative uncertainty of latest keyframe from the nearest lighthouse. For the former, we trigger LH-ALC first after some time $T_{0}$ and keep scaling the timer by a factor $\gamma$ for every time we trigger LH-ALC: $T_{0},\gamma T_{0},\gamma^{2}T_{0},\cdots$. This ensures we aren’t spatially bounded to some radius from any particular lighthouse. While this approach requires only constant-time evaluation, it can fail if the pose uncertainty has increased drastically between LH-ALC timers. The latter approach is more reactive to the pose uncertainty in that we only trigger LH-ALC when the relative uncertainty is high; however, computing the pose uncertainty of the latest keyframe is an expensive operation, requiring covariance estimation.

We unconditionally create the very first lighthouse, i.e., the home lighthouse, around the starting point of exploration. The home lighthouse is reliable because it is created near the origin so we collect additional panoramic views around the home lighthouse to ensure we can easily obtain a loop closure here. Note that since the home lighthouse is the starting point of exploration, a loop closure at the home lighthouse encompasses almost the entirety of the SLAM pose graph and is considered a global loop closure.

An additional graph strengthening measure we use is to connect each lighthouse to a nearby one. When we opportunistically create a lighthouse, instead of continuing FE immediately, we find the nearest lighthouse and travel to it to perform an in-place rotation. In the pose graph, this creates a loop that encompasses both lighthouses, thereby connecting them to each other and lowering both of their relative uncertainties. Using this strategy, we effectively connect each lighthouse to the home lighthouse transitively. By performing this sequence of connections, we create even more loop closure constraints in the pose graph that encompass other loops; this creates a very stable mesh-like structure that helps bound the SLAM uncertainty. After connecting each new lighthouse to a previously-created one, we continue FE.

The loop closures constraints created by the LH-ALC planner ensure local pose graph stability since we travel back to lighthouses or connect adjacent ones together. Global Graph Stability (GGS) is stability over the entire pose graph rather than just the subgraphs that encompass the adjacent lighthouse connections. The GGS planner creates an even more stable pose graph by analyzing regions of the map that are view-deficient and extending the trajectory along those regions in both directions and connecting them to the home lighthouse. Our lighthouse creation and connection strategy of SAE, paired with the greedy approach of FE, effectively creates a spoke-and-hub pattern in the keyframes across the occupancy map where the “spokes” aren’t necessarily connected. Furthermore, partial observability of the map during FE often interferes with the ability to connect the “spokes”.

We wait until FE is completed and we have a completed occupancy map and locally-stable pose graph around the lighthouses. Performing global uncertainty calculations on the pose graph itself is often very expensive, but we can exploit this “spoke-and-hub” pattern in a much cheaper way. By taking all of the keyframes in the pose graph and computing the convex hull via the QuickHull algorithm[29], the vertices become the endpoints of the “spokes”. The GGS planner will travel to each of these vertices both clockwise and anticlockwise along the hull to connect the spokes and collect both forward and backward views to connect to the home lighthouse. Figure 3 shows the convex hull and path the device would take to each vertex. GGS planning connects even more views of the environment to the home lighthouse to ensure global stability of the pose graph. This stability creates more loop closure constraints when performing subsequent navigation on the explored map.

SAE has multiple planners that interact with each other. Figure 4 illustrates our system design. Certain planners may preempt others or depend on them to finish operations. To ensure no contention between planners, we propose a system design based around planners that consume data from perception at a fixed rate, called the world snapshot, and produce plans to our motion planner. Furthermore, to enable GGS after FE, we define two phases of SAE: (i) discovery and (ii) refinement. For the former phase, we run two planners in order: LH-ALC and FE. LH-ALC starts by creating the home lighthouse. At the start of exploration, the SLAM uncertainty is near zero since the device starts at the origin so LH-ALC yields to FE immediately. During FE, if we notice a good view score, LH-ALC preempts FE to create the lighthouse and connect it to a previous lighthouse. If we detect SLAM uncertainty crosses a threshold or the timer triggers, we also preempt FE and travel to the closest lighthouse to create a loop closure constraint. The first phase finishes when FE finishes, and we travel back to the home lighthouse.

After the first phase, we have a completed occupancy map and the robot is at the home lighthouse. For the next refinement phase, we focus on the GGS planner and creating global stability for subsequent navigation. We exclusively run GGS planning during the second phase to ensure a globally stable pose graph for subsequent navigation. After GGS planning, we navigate the robot back to the home lighthouse for a final global correction.

A successful exploration is one where we can map the entire unknown environment without incurring so much localization drift that we cannot travel to unexplored regions or the home lighthouse. Since the quality of the map is dependent on the quality of localization[30] and ALC creates loop closures to stabilize the map, we performed an ablation study to measure the effect of the LH-ALC planner on overall exploration success. We ran exploration in 6 indoor home environments; in each environment, we ran ten exploration trials (half FE and half SAE) for a total of 60 samples. The robot started and ended in the same location in every environment.

Figure 5 shows the results averaged from several trials of each set. We see that SAE has a mean success rate of 93% compared to FE with 62%. On average, the SAE exploration time (without GGS) takes 1.75x longer than FE. The increase is due to the extra motion of actively creating more loop closures. Figure 6 shows example occupancy maps from two indoor home environments. We see map drifts with FE due to increasing pose uncertainty which causes paths to be blocked in the occupancy map. Exploration failures occur when the uncertainty of SLAM grows unbounded and distorts the occupancy map by placing obstacles in a way that blocks paths to prevent further exploration or returning to the home lighthouse. Such distortions can be found in the top right region in Figure 6(a) and bottom left region in Figure 6(c). Since frontiers are created at the boundary of free and unknown space, FE has no incentive to travel to known spaces. However, SLAM benefits from traveling to previously observed areas since they are the only places to obtain loop closures. SAE keeps the pose uncertainty bounded during exploration which mitigates drift and allows exploration to complete successfully.

Beyond exploration, subsequent navigation on the explored map should be successful as well and not produce drift. SAE has the GGS planner which globally stabilizes the pose graph to ensure the success of subsequent navigation. To understand the effectiveness of the GGS planner, we explore two real-world environments: a furnished and unfurnished home. We separately keep track of keyframes and loop closures during the discovery and refinement phases; since GGS is the only planner in the refinement stage, this becomes an ablation study assess how many additional keyframes and loop closures are created from the GGS planner to strengthen the pose graph.

Table I shows the additional keyframes and loop closures generated by the GGS planner. We see that in both cases, the GGS planner contributes significantly to additional keyframes being added to the pose graph that provide even more views for subsequent navigation. The furnished home produces a slighly larger number of keyframes than the unfurnished one since the furniture has visual features on it and also creates occlusions. We also see that almost half of the total loop closures in the exploration run come from the GGS planner which globally stablizes the pose graph and reduces drift.

We also compare the ability of the entire SAE against FE to improve subsequent navigation on the explored map. We explore the same environment twice, once with FE and another with SAE and save the maps. Then we travel through the same sequence of 50 fixed poses and measure the number of loop closures against the saved pose graph, which we call relocalizations instead of loop closures to distinguish the two.

Figure 7 shows the qualitative results comparing post-exploration navigation with FE and SAE. Quantitatively, the total number of SAE relocalizations was 315; FE achieved only 163 relocalization. There are almost twice the number of relocalizations for SAE. In general, observing more views of the environment increases the likelihood of relocalization which causes higher navigation success rates because the decreased pose uncertainty prevents occupancy map distortion. Views have directionality so observing views going in one direction of a path are different from views observed going in the opposite direction of the same path. The GGS planner directs the robot to observe views in both directions increase the overall likelihood of relocalization.