Scaling Local Control to Large-Scale Topological Navigation
Xiangyun Meng Nathan Ratliff Yu Xiang Dieter Fox





[
    Anamaria Savu
    August 1, 2016
==================




empty
empty



Visual topological navigation has been revitalized recently thanks to the advancement of deep learning that substantially improves robot perception.  However, the scalability and reliability issue remain challenging due to the complexity and ambiguity of real world images and mechanical constraints of real robots. We present an intuitive approach to show that by accurately measuring the capability of a local controller, large-scale visual topological navigation can be achieved while being scalable and robust. Our approach achieves state-of-the-art results in trajectory following and planning in large-scale environments. It also generalizes well to real robots and new environments without retraining or finetuning. 






§ INTRODUCTION




There has been an emergence of cognitive approaches <cit.> towards navigation thanks to the advancement of deep learning that substantially improves robot perception. Compared to the traditional mapping, localization and planning  approach (SLAM) <cit.> that builds a metric map, cognitive navigation uses a topological map. This eliminates the need of meticulously reconstructing an environment which requires expensive or bulky hardware such as a laser scanner or a high-resolution camera. Moreover, the fact that humans are able to navigate effortlessly in large-scale environments without a metric map is intriguing. By adding this cognitive spatial reasoning capability to robots, we could potentially lower the hardware cost (i.e., using low-resolution cameras), make them work more robustly in dynamic environments and bring insights to more complex tasks such as visual manipulation.

While cognitive navigation has drawn significant attention recently, the problem remains challenging because i) it does not scale well to the size of experiences ii) it is fragile due to actuation noise and dynamic obstacles and iii) it lacks probabilistic interpretation, making it difficult to plan with uncertainty. These problems are exacerbated when using a RGB camera in indoor environments, where partial observability makes it difficult to control a robot to follow a single path <cit.>.


In this paper, we present a simple and intuitive solution for topological navigation. We show that by accurately measuring the capability of a local controller, robust visual topological navigation can be achieved with sparse experiences (Fig.<ref>). In our approach, we do not assume the availability of a global coordinate system or robot poses, nor do we assume noise-free actuation or static environment. This minimalistic representation only has two components: a local controller and a reachability estimator. The controller is responsible for local reactive navigation, whereas the reachability estimator measures the capability of the controller for landmark selection and long-term probabilistic planning. To achieve this, we leverage the Riemannian Motion Policy (RMP) framework <cit.> for robust reactive control and deep learning for learning the capability of the controller from data. We show that with both components working in synergy, a robot can i) navigate robustly with the presence of nonholonomic constraints, actuation noise and obstacles; ii) build a compact spatial memory through adaptive experience sparsification and iii) plan in the topological space probabilistically, allowing robot to generalize to new navigation tasks.

We evaluate our approach in the Gibson simulation environment <cit.> and on a real RC car. Our test environments contain a diverse set of real-world indoor scenes with presence of strong symmetry and tight spaces. We show that our approach generalizes well to these unseen environments and surprisingly well to real robots without finetuning. Scalability-wise, our spatial memory grows only when new experiences are unseen, making the system space-efficient and compute-efficient.





§ RELATED WORK

Cognitive spatial reasoning has been extensively studied both in neuroscience <cit.>, and robotics <cit.>. The Spatial Semantic Hierarchy <cit.> divides the cognitive mapping process into four levels: control, causal, topological and metric. In our method, the local controller operates on the control level, whereas the reachability estimator reasons about causal and topological relationship between observations. We omit metric-level reasoning since we are not concerned about building a metric map.

Experience-driven navigation constructs a topological map for localization and mapping <cit.>. Unlike SLAM that assumes a static environment, the experience graph can also be used for dealing with long-term appearance changes <cit.>. This line of works mostly focus on appearance-based localization and ignore the control aspect of navigation, and assume that a robot can always follow experiences robustly. This does not usually hold in unstructured indoor environments, where it is crucial to design a good controller while considering its capability.

Semi-Parametric Topological Memory (SPTM) <cit.> is a recent work that adopts deep learning into topological navigation. Similar to SPTM, we build a topological map through past experiences. Unlike SPTM that uses image similarity as a proxy for reachability, we measure the reachability of a controller directly. This significantly improves robustness and opens opportunities for constructing sparse maps.

There have been recent works studying visual trajectory following that handles obstacles <cit.>, actuation noise <cit.>, or with self-supervision <cit.>. Our approach differs from them in that our trajectory follower extends seamlessly to probabilistic planning. Our method also handles obstacles and actuation noise well, thanks to the RMP controller that models local geometry and vehicle dynamics.

Recent works on cognitive planning <cit.> show that a neural planner can be learned from data. However, assumptions such as groundtruth camera poses are available with perfect self-localization are unrealistic. The use of grid map also limits its flexibility. Another line of research uses reinforcement learning to learn a latent map <cit.>, but it is data-inefficient and cannot be easily applied to real robots. In contrast, our planner is general and can adapt to new environments quickly. It bears a resemblance to feedback motion planning system such as LQR-Trees <cit.>, where planning is performed on the topological map connecting reachable state spaces with visual feedback control.







§ METHOD




 §.§ Overview

We consider the goal-directed navigation problem: a robot is asked to navigate to a goal G given an observation o_G taken at G. Robot does not have a map of the environment, but we assume it has collected a set of trajectories (e.g., via self-exploration or following language instructions) as its experiences. Each trajectory is a dense sequence of observations o_1, o_2, ..., o_N recorded by its on-board camera. Using its experiences, robot decides the next action to take in order to reach G. The action space is continuous (e.g., velocity and steering angle) and robot could be affected by actuation noise and unseen obstacles.

We approach this problem from a cognitive perspective. Robot first builds a topological map from its experiences. The map is a directed graph, with vertices as observations and edges encoding traversability. Then, given its current observation o_current and goal o_G, robot searches for a path on the graph and follows that path to reach G. Our setup is similar to that of SPTM <cit.>. The difference is that we design our system to make it generalize to real robots and scale to real environments.

For such a navigation system to work, we first need a target-conditioned Local Controller .  takes current observation and a target observation, and outputs an action a=(o_current, o_target) to drive robot towards the target. The action is executed for a small time step to get an updated o_current and the process is repeated until o_current matches o_target. Given a path (a sequence of observations) computed by a planner, robot uses  to follow the path progressively to reach its final destination.

In practice, robot's experience pool can be large and grow indefinitely, thus the key issue is to build a sparse and scalable representation of an environment given dense, unstructured trajectories. Clearly, adjacent observations in a trajectory is highly correlated and it would be wasteful to keep every observation. One ad-hoc approach to sparsify a trajectory is to take every nth observation. However, this assumes that target n steps away is always reachable, which is not necessarily true. For example, without occlusion, an observation far away can be confidently reached (e.g., in a straight hallway), whereas an observation nearby may be hidden (thus not reachable) if it is blocked by obstacles.  Moreover, motion constraints, sensor field of view, motor noise, etc. can all affect the reachability of a target. 

Our intuition is that the sparsification of a trajectory should adapt to the capability of the controller. 
We propose learning a Reachability Estimator  that predicts the probability of   successfully reaching a target: (o_current, o_target)=P(reach | o_current, o_target, ). We use  as a probabilistic metric throughout the system, illustrated by Fig. <ref>. Given a controller , we train a corresponding . The incoming trajectories are first sparsified by  and then interlinked to form a compact topological map. Given o_current and o_G, we leverage  to plan a probabilistic path and use  and  in synergy to follow the planned path robustly.



 §.§ Designing a Robust Local Controller



Real-world robots are subject to disturbances such as motor noise and moving obstacles, which can cause a robot to deviate from planned path and fail. Hence our first objective is to design a sufficiently robust local controller. Contrary to directly predicting low-level controls, we split our controller into two stages: high-level waypoint prediction and low-level reactive control. The high-level controller _WP predicts a waypoint x, y (in robot's local coordinate system) for the low-level controller. The waypoint needs not be precise, but only serves as a hint for the low-level controller to make progress. Hence _WP is agnostic to robot dynamics (e.g., can be trained with A* waypoints as supervision) and absorbs the effects of actuation noise. For the low-level controller, we adopt the RMP representation <cit.> as a principled way for obstacle avoidance and vehicle control. Hence we have (o_current, o_target)=_RMP(_WP(o_current, o_target)). Note that this allows the same _WP to be applied to different robots by replacing the low-level controller.

Fig. <ref> illustrates the design of _WP. The robot state is represented by its current observation o_current. Denote ith observation in a trajectory as o_i. We represent the corresponding o_target at o_i as a sequence of neighbor observations centered at o_i: 
    o_i-kΔ T, o_i-(k-1)Δ T, ..., o_i,..., o_i+(k-1)Δ T, o_i+kΔ T,
 where k controls  context length and Δ T (set to 3) is the gap between two observations. The past frames expand the field of view of o_i which helps controller to do visual closed-loop control. The future frames encode intention at o_i, allowing a controller to adjust its waypoint in advance in order to follow subsequent targets smoothly and reliably.




Technically, we extract a feature vector by feeding stacked [o_current, o_i-kΔ T, o_current - o_i-kΔ T]  into a sequence of convolutions, followed by combining the 2k + 1 feature vectors through one convolution and multiple fully-connected layers to predict a waypoint x, y. We find this design works much better than featurizing each image or stacking all images together. Additionally, the network predicts the heading difference between o_current and o_i to help the network anchor the target image in the sequence. Finally, in order to reason about proximity to a target (Sec.<ref>), _WP predicts mutual image overlap. Image overlap is a ratio that represents the percentage of content in one image that is visible in another image. Hence mutual image overlap is a pair of ratios (R_current→target, R_target→current). 





















We train _WP in a supervised fashion (Sec.<ref>). To evaluate our design, we randomly sample o_current from a trajectory and o_target being -1.0m behind to 3.0m ahead of o_current in 4 unseen environments, and run each controller to see if robot reaches the target. Table <ref> compares the success rate of each controller. Directly predicting low-level controls (forward acceleration and steering velocity) results in much lower success rate than our two-stage design. Compared with directly mapping images to low-level actions, we find it more robust to map images to higher-level abstractions such as waypoints, and then map waypoints to low-level controls using a representation (e.g., RMP) that explicitly models environmental geometry and robot dynamics.









 §.§ Learning the Reachability Estimator

Table <ref> suggests that controller design and parametrization can heavily affect target reachability. Unlike <cit.> that uses image similarity as a proxy, we learn reachability by explicitly predicting the execution outcome of . During training, o_current and o_target are randomly sampled from demonstration trajectories (Sec. <ref>) and  is used to drive the robot from o_current to o_target to get a binary outcome. The criteria for success is that robot reaches the target within time limit defined as t_max = A*(o_current, o_target) / v_min, where A*(·, ·) computes the A* path length and v_min is the minimum velocity. Hence  measures the probability of   reaching the target efficiently, which is independent of the temporal and physical distance between o_current and o_target. This idea has an interesting connection to feedback motion planning systems <cit.>, as  can be seen as estimating visual funnels that are locally stable.



The design of  is almost identical to , except that it predicts a single probability and is trained with a binary classification loss.









 §.§ Sparsifying a Trajectory

For any observation o_i in a dense trajectory, if (o_i, o_i+1), ..., (o_i, o_i+k+1) are sufficiently high, we could confidently discard o_i+1, ..., o_i+k because  does not need them to reach o_i+k+1. Hence a greedy approach to choose the next anchor is

    max    j 
    s.t.    (o_i, o_k) > p_sparsify, ∀ k, i < k ≤ j

where i is previous anchor's position and p_sparsify is the probability threshold that controls sparsity. Hence a dense trajectory is converted to a sequence of contextified anchor observations ô_1, ..., ô_m. One may argue that contextification reduces the effective sparsification ratio. Since the time and space complexity is a function of the number of anchors, in practice it significantly saves computation during planning and following a trajectory, allowing our system to run on a robot in real time.





 §.§ Building a Compact Probabilistic Topological Map

Our topological map is a weighted directed graph (Fig. <ref>a). Vertices are anchor observations and edge weight from ô_̂î to ô_j is -log(ô_̂î, ô_̂ĵ). Construction is incremental: for an incoming trajectory, we create pairwise edges between every vertex in the graph and every anchor in the new trajectory.


Compared to a graph constructed with dense observations, a graph built from sparsified observations has less than 1/10 of the vertices and 1/100 of the edges. To further improve scalability, we propose the following two optimizations to make the graph grow sublinearly to the number of raw observations, and eventually the size of the graph converges:

Edge pruning. Low-probability edges with (ô_̂î, ô_̂ĵ) < p_edge are discarded since they contribute little to successful navigation (Fig. <ref>b).

Vertex reuse. It is common for two trajectories to be partially overlapped and storing this overlapping part repeatedly is unnecessary. Hence when adding anchor ô_i into a graph, we check if there exists a vertex ô such that (ô_i-1, ô) > p_reuse and (ô, ô_i+1) > p_reuse. If the condition holds, we discard ô_i and add edges ô_i-1→ô and ô→ô_i+1, as illustrated in Fig. <ref>c.

The graph will converge because for any static environment of finite size, there is a maximum density of anchors. Any additional anchor will pass the vertex reuse check and be discarded. Practically however, an environment may change over time. The solution is to timestamp every observation and discard outdated observations using . We leave the handling of long-term appearance change as future work.



 §.§ Planning

We add an edge (weighted by its negative log probability) from o_current to every vertex in the graph, and from every vertex in the graph to o_G. The weighted Dijkstra algorithm computes the path with the lowest negative log probability (i.e., the path that robot is most confident). Robot then decides whether the probability is high enough and may run the trajectory follower proposed in Section <ref>.





 §.§ Mitigating Perceptual Aliasing


Practically, o_current may correspond to different locations of similar appearances. Traditional approaches usually formulate this as a POMDP problem <cit.> and try to resolve the ambiguity by maintaining beliefs over states. This requires having a unique state (e.g., global pose) associated with each observation which is difficult to implement since we do not have any metric information.

We use two techniques to resolve ambiguity. The first is to match a sequence of anchors during search and graph construction. In practice the probability of two segments having similar appearances is much lower than two single observations. Additionally we let robot re-plan a new path if it detects discrepancy (entering Dead reckoning state for too long) while following the previous path. The intuition is that the location where robot detects the discrepancy is likely distinct. See Sec. <ref> for an example. In the worst case where such distinctive anchor is absent, robot might follow a cycle of anchors without making progress. The solution is to count how many times the robot has visited an anchor (i.e., by collecting statistics from  last visited anchor). Cyclic behavior can be detected so that the robot can break the loop by biasing its choice in future planning. We leave the handling of this extreme case as future work.



 §.§ Following a Trajectory


Our trajectory follower constantly updates and tracks an active anchor to make progress, while performing dead reckoning to counter local disturbances. Specifically, given a sequence of anchor observations ô_1, ô_2, ..., ô_m, the trajectory follower acts as a state machine:

Search: robot searches for the best anchor: ô^* = _o∈{ô_1, ..., ô_m}(o_current, o). If (o_current, ô^*) > p_search, it sets ô^* as current active anchor ô_active and enters Follow state, otherwise it gives up and stops.

Follow: robot computes the next waypoint x, y = _WP(o_current, ô_active) and uses it to drive _RMP. Meanwhile it tracks and updates the following two values:

    
  * last visited anchor. Robot uses the predicted mutual image overlap to measure the proximity between o_current and anchors close to ô_active. The closest anchor is set as o_lastvisited. This is a form of approximate localization. 
    
  * active anchor. If (o_current, ô_active+1) > p_follow and is within proximity, it advances ô_active to ô_active + 1, otherwise ô_active = o_lastvisited + 1. The intuition is to choose an ô_active that is neither too close nor too far away.

Normally robot stays in Follow state. But if moving obstacles or actuation noise cause (o_current, ô_active) < p_follow, it enters Dead reckoning state.

Dead reckoning: robot tracks the last waypoint computed in the Follow state and uses the waypoint to drive _RMP. The assumption is that disturbances are transient which the robot could escape by following the last successfully computed waypoint. Waypoint tracking can be done by an odometer and needs not be very accurate. While in this state, robot keeps checking if (o_current, ô_active) >  p_follow and returns to  Follow state if possible.











§ EXPERIMENTS


We trained _WP,  and all baselines in 12 Gibson environments. 100k training trajectories were generated by running an A^* planner (used to provide waypoints) with a laser RMP controller similar to <cit.>. Simulation step size is 0.1. We use the laser RMP controller as _RMP mostly for efficiency, but in practice an image-based RMP controller can also be used <cit.>. _WP was trained by randomly sampling two images on the same trajectory with certain visual overlap, with the A^* waypoint as supervision. After _WP was trained, we trained  by sampling two images that either belong to the same trajectory (prob 0.6) or different trajectories (prob 0.4), and ran a rollout with  to get a binary outcome. Image size is 64× 64 with 120^∘ horizontal field of view. We augmented the dataset by jittering robot's starting location and orientation to improve generalization. About 1.5M samples were used to train _WP and . Our training setup models a real vehicle similar to <cit.>, so that the same model can be used for real experiments.

We present quantitative results in 5 unseen Gibson environments with diverse appearances. Our baseline is based on SPTM. Since SPTM is designed for small synthetic mazes with discrete action space, its original version would perform poorly in our setting. For a fair comparison, we let SPTM use the same controller and trajectory following logic as ours. The main differences between SPTM and ours are thus: i) how reachability is learned and ii) how graph is constructed and used. Our ablation study will thus be in the form of evaluating trajectory following and planning performance in the following sections.




 §.§ Trajectory Sparsification


Fig. <ref> compares sparsification results of three controllers. The two visual controllers _k=2, _k=5 differ in their context length. To show that our model is general, we also trained a laser-based controller _laser by modifying the input layer in Fig. <ref> to take 64-point 240^∘ 1D depth as input. 

Fig. <ref> shows placement of anchors with p_sparsify=0.99. Comparing with _k=5, _k=2 requires denser anchors. Since _k=2 uses a shorter context, it is more “local” and has to keep more anchors to follow a path robustly. Nonetheless, anchors are more densely distributed in tight spaces and corners for both controllers, indicating that our sparsification strategy adapts well to environmental geometry. Interestingly, _laser shows a more uniform distribution pattern. Since laser scans have a much wider field of view and measures geometry directly, it is not heavily affected by tight spaces and large viewpoint change.











 §.§ Trajectory Following


We randomly generated 500 trajectories in the test environments (Fig. <ref>) with an average length of 15 m. When following a trajectory, we stop the robot when it diverges from the path or collides with obstacles. We report the cover rate, the percentage of total length of trajectories successfully followed by robot. For our trajectory followers, p_search=p_follow=0.92.


Sparsity is the average ratio of number of images in a sparsified trajectory to the number of images in the corresponding dense trajectory. To change sparsity, we vary p_sparsify for our models. For SPTM we select every nth frame and vary n. Fig. <ref> plots cover rates for varying sparsity conditions. Controllers with contexts (*-k2, *-k5) achieve higher than 95% cover rate, better than controllers without context (at most 90%). This indicates that having contextual frames can improve robustness. But since contextual frames are used, more observations have to be kept so storage-wise it is not as efficient as (*-k0). 




SPTM performs comparably to ours when using a strong controller (*-k5), but for all controllers it starts to degrade before ours as sparsity lowers. Due to its fixed-interval subsampling, it does not adapt to controllers' capability well, as can be seen by the increasing gap between ours and the SPTM counterparts when less contextual frames are used (*-k2, *-k0).







We also evaluated performance under noisy actuation by multiplying a random scaling factor s∼𝒩(1.0, 0.33) to the control output. No noticeable difference was found. This is expected because the local controller runs at a high frequency (10 Hz) and uses visual feedback for closed-loop control.






 §.§ Planning



  §.§.§ Navigation between Places




We built one topological map for each environment (Fig. <ref>a). A map is constructed from 90 trajectories connecting 10 locations in a pairwise fashion. The locations are selected to make the trajectories cover most of the reachable area.


Robot starts at one of the locations (with jittered position and orientation) and is given an goal image taken at one of the other 9 destinations. Robot has no prior knowledge of its initial location. We re-implemented SPTM's planner and uses the best trajectory follower SPTM-k5 (Sec<ref>) to make it a competitive baseline. We set the sub-sampling ratio to 20 and Δ T_l=1 to prevent the graph from getting too large. We performed a hyperparameter search to set s_shortcut=0.99. 
For our method, we set p_reuse=p_edge=0.99.



Table <ref> presents the success rate for each environment compared to SPTM. Our method outperforms SPTM with much sparser maps. Graphs built by SPTM have unweighted edges and do not reuse vertices. SPTM also does explicit localization which sometimes causes planning failure. This results in worse scalability and reliability compared with our approach. Note that the slightly lower success rates in space8 and house75 are mostly caused by strong symmetry and rendering artefacts.  









  §.§.§ Comparing Trajectory Probabilities to Empirical Success Rate

To show that path probability is a reasonable indicator of empirical outcome, we let a robot start at a random location (anywhere in a map), plan a path to one of the 10 destinations, and follow the path. 1,000 trajectories were collected in each environment. Fig. <ref> shows that path probability strongly corresponds to empirical success rate. This allows a robot to assess the risk before executing a plan, and ask for help if necessary. Note that SPTM does not provide any uncertainty measure.





  §.§.§ Generalizing to New Navigation Tasks

To test the generalizability of our planner, we randomly pruned the graphs to contain only a subset of the trajectories, and repeated the experiment in <ref>. Fig. <ref> shows that with only 60% of the trajectories, robot already performs close to its peak success rate. In other words, robot is able to combine existing trajectories to solve novel navigation tasks. Fig. <ref> shows an example. 












  §.§.§ Resolving Ambiguity


Fig. <ref> illustrates how perceptual aliasing is resolved in environments with strong symmetry. Robot initially starts at an ambiguous location (marked “1”) and plans a wrong path (red path). While following this path, robot detects the discrepancy at “2” by realizing what is expected to be an office room is actually a hallway. As a result, it plans a new path (green) whereby it successfully reaches the goal.











  §.§.§ Scalability


























L0.4
    

    
    < g r a p h i c s >

    Dotted lines show 10x and 20x temporal subsampling for reference.
    

Fig. <ref> shows that map sizes grow sublinearly to the number of raw observations, making our approach  scalable to large environments. It also shows that map size is adaptive to an environment. Since house75 has more complex geometry and exhibits more rendering artefacts, denser samples are kept to stay conservative.





 §.§ Testing in a Real Environment


Our model trained in simulation generalizes well to real images without finetuning. To map a real environment, we manually drove the RC car to collect 7 trajectories, totalling 3,200 images. The final map contains 206 vertices and 215 edges (Fig.<ref>b). The car is able to plan novel paths between locations and follow the path while avoiding obstacles not seen during mapping (Fig.<ref>). We refer the interested reader to the video supplementary material for more examples.



§ CONCLUSION

In this work, we show that by learning the capability of a local controller, robust and scalable visual topological navigation can be achieved. Due to the simplicity and flexibility of our framework, it can be extended to support non-visual sensors and applied to other robotics problems. Future works include combining multiple sensors to improve the controller, developing better algorithms to resolve ambiguity, improving generalization, and extending to manipulation tasks.

The hyperparameters in our approach are mostly probability thresholds, which are easy to interpret and tune. One important scenario our approach does not handle is when robot deviates too much from all vertices in the navigation graph, where it would fail to find a plausible path. A self-exploratory model can help here, and it can also be used for autonomous map construction. 



§ ACKNOWLEDGEMENTS

This work was funded in part by ONR grant 63-6094 and by the Honda Curious Minded Sponsored Research Agreement. We thank NVIDIA for generously providing a DGX used for this research via the NVIDIA Robotics Lab and the UW NVIDIA AI Lab (NVAIL).

IEEEtran


