TOP-Nav: Legged Navigation Integrating Terrain, Obstacle and Proprioception Estimation
Junli Ren Yikai Liu Yingru Dai Guijin Wang



















































TOP-Nav: Legged Navigation Integrating Terrain, Obstacle and Proprioception Estimation
    Junli Ren^*, Yikai Liu^*, Yingru Dai, Guijin Wang

^* Equal Contribution.

Corresponding Author: Guijin Wang (wangguijin@tsinghua.edu.cn)

All authors are with Department of Electronic Engineering, Tsinghua University, Beijing 100084, China.
    April 29, 2024
=====================================================================================================================================================================================================================================================




Legged navigation is typically examined within open-world, off-road, and challenging environments. In these scenarios, estimating external disturbances requires a complex synthesis of multi-modal information. This underlines a major limitation in existing works that primarily focus on avoiding obstacles. In this work, we propose TOP-Nav, a novel legged navigation framework that integrates a comprehensive path planner with Terrain awareness, Obstacle avoidance and close-loop Proprioception. TOP-Nav underscores the synergies between vision and proprioception in both path and motion planning. Within the path planner, we present and integrate a terrain estimator that enables the robot to select waypoints on terrains with higher traversability while effectively avoiding obstacles. In the motion planning level, we not only implement a locomotion controller to track the navigation commands, but also construct a proprioception advisor to provide motion evaluations for the path planner. Based on the close-loop motion feedback, we make online corrections for the vision-based terrain and obstacle estimations. Consequently, TOP-Nav achieves open-world navigation that the robot can handle terrains or disturbances beyond the distribution of prior knowledge and overcomes constraints imposed by visual conditions. Building upon extensive experiments conducted in both simulation and real-world environments, TOP-Nav demonstrates superior performance in open-world navigation compared to existing methods. Project page at https://top-nav-legged.github.io/TOP-Nav-Legged-page/top-nav-legged.github.io.






§ INTRODUCTION




Imagine an outdoor hiking scenario where gait stability and efficient navigation are both critical. The target environment is often marked by intricate obstacles and hazardous terrains that cannot be fully observed through onboard vision. This challenge underscores the need for a comprehensive path planner integrating multi-modal observations. To achieve this, we humans employ experienced guidance and alternative perception modalities such as trekking poles and GPS devices. This example reveals a requirement of numerous trials and a substantial foundation of prior knowledge to accomplish the challenge navigation task.

Although recent advancements in legged locomotion have allowed the robots to navigate various terrains based on a simulation-learned strong controller <cit.>, the complexity of currently insimulable real-world factors makes it impossible for the robot to traverse all potential terrains encountered in reality. As a result, integrating the locomotion controller with only an open-loop path planner often restricts legged navigation to limited scenarios <cit.>.

An effective solution to overcome these limitations is equipping the robot with terrain awareness. A traversable path can be planned based on the robot's preferences on terrains <cit.> with an appropriate distribution of contact heights and forces <cit.>. Unlike obstacle estimation, the distinct terrain features are typically encoded as semantic information, where traditional methods collect sufficient data and train segmentation or classification models <cit.> to learn these features. Nevertheless, compiling an exhaustive catalogue of all conceivable terrains along with their corresponding walking preferences is impractical <cit.>. Compounding the issue, the dynamic real-world conditions, such as lighting, humidity, and temperature, may introduce inaccuracies in the correspondence between images and walking preferences, especially when relying exclusively on vision in this context <cit.>.


The mentioned challenges can be attributed to the reliance on vision and the ignorance of motion states in path planning. To address this, we complement the vision-only terrain estimator with online corrections from  motion evaluations. We construct a proprioception advisor to not only convey information about the traversability cost of novel terrains but also alert the robot to unexpected disturbances, such as invisible obstacles.

By integrating the Terrain estimator, Obstacle estimator, and Proprioception advisor, we formulate TOP-Nav: a hierarchical path and motion planning framework designed to navigate a quadruped robot through diverse and challenging terrains proficiently. TOP-Nav maintains four real-time robot-centric costmaps corresponding to: 1) goal approaching; 2) terrain traversibility; 3) obstacle occupancy; and 4) proprioception advice. The costmaps are synthesized with dynamically weighted factors to ensure a balanced consideration of safety and efficiency. We evaluate TOP-Nav both in simulation and on a physical robot, with a comparative analysis against existing approaches that address subsets of the factors (terrain, obstacle, proprioception). In simulation, we construct an environment featuring diverse terrains, including slopes, steps, and random textures, along with various obstacles. For real-world experiments, we deploy our approach in diverse off-road navigation scenarios encompassing common field terrains. Our main contributions are summarized below:


    
  * A comprehensive legged navigation framework Integrating multi-modal observations throughout a task and motion planner;
    
  * A terrain estimator trained from previously collected data to inform the robot of a vision-based terrain traversability;
    
  * Compensating proprioception to offer online corrections for the vision-based estimation of both terrain traversability and obstacles;
    
  * Successful implementation and quantitative validations of the proposed framework in both simulation and hardware.

    




§ RELATED WORK


The pivotal feature of TOP-Nav lies in its integration of the terrain estimator and proprioception advisor. To elucidate this aspect, we present an overview of the relevant literature. 



 §.§ Vision and Legged Proprioception Integration
 

Recent advancements in legged locomotion have showcased a synergistic mechanism for processing vision and proprioception within the context of “How to Walk" <cit.>. While heightmaps serve as crucial observations for a controller to generate dynamic motions across various terrains <cit.>, proprioception observations can be employed to reconstruct the heightmaps in visually degraded environments <cit.>. Different from the mentioned efforts focus on the depth channel, we propose a data-efficient solution to extract semantic information from the proprioceptive feedback. 



The insights of integrating vision and legged proprioception are further explored in guiding the robot in “Where to Walk". From the perspective of vision adied navigation, researchers have explored both hierarchical <cit.> and end-to-end <cit.> pipelines. These methods require substantial efforts to develop a robust perception module <cit.>, making it difficult and costly to transfer them to different hardwares. 

To provide a comprehensive task observation and reduce the reliance on vision systems, recent researches have introduced proprioception to improve task planning. A majority of these works learn proprioception representations along with visual features in simulation, and then implement the cross-modal features through end-to-end <cit.>, hybrid <cit.> or decoupled methods <cit.>. Despite the effectiveness demonstrated in these works, the high-dimensional representation space presents challenges for adaptation to novel scenarios and sim-to-real transfer. Alternatively, <cit.> introduced a hierarchical navigation framework that derives evaluation scores from motion states, yet it overlooks the integration of visual observations.

To mitigate these limitations, we propose a novel approach within TOP-Nav by maintaining a series of lightweight cost maps derived from multi-modal observations. This integration achieves a dynamic balance between vision and proprioception. Furthermore, we leverage the learning-based locomotion controller to derive motion evaluations from the value function, offering an efficient solution without additional training.



 §.§ Terrain Traversability Estimation
 

Terrain traversability is determined by factors such as terrain geometry, texture, and physical properties <cit.>. Estimating these features could be achieved by identifying the semantic class with a predefined static traversability score<cit.>. These solutions exhibit a notable dependency on large-scale datasets <cit.> or limited to structured environments like urban scenarios <cit.>.

In off-road navigation, the motion states involved in the dynamic interactions between the robot and the environment provide valuable metrics for assessing terrain traversability <cit.>. These insights have inspired methods that eliminate the need for manual annotation by autonomously deriving terrain traversability from proprioception through self-supervised learning <cit.>. Nevertheless, the performance of these studies is contingent upon the quality of the collected datasets <cit.>. To emphasize the challenges in unconstrained navigation, researchers have proposed various approaches to handling novel observations. For instance, <cit.> updated the traversability estimation network online with anomalies into consideration. <cit.> performs nearest-neighbor search in the proprioception space to align visually novel terrains with existing traversability.

Drawing inspiration from those works estimating traversability for novel terrains, we propose a prior-knowledge informed terrain estimator that employs the proprioception advisor as online corrections. Our method diverges from previous approaches primarily in two key aspects: 1) We employ an estimated value function from reinforcement learning to assess terrain traversability, providing a comprehensive evaluation of robot-terrain interactions. 2) By incorporating the proprioception based terrain traversability estimation as online corrections to vision-based predictions, we achieve a more data-efficient approach for estimating traversability on unknown terrains compared to online training.



§ BACKGROUND


The proposed proprioception advisor leverages recent progress in legged locomotion to provide online motion evaluations for the path planner. This section provides an essential background of this.

Learning-based legged locomotion controllers has been well developed through reinforcement learning, which is generally achieved by updating the policy π within the asymmetric actor-critic training:


    π_1{a_t=π_actor(o_t^p,o_t^i,o_t^e,o_t^h)
    
    c_t=π_critic(o_t^p,o_t^i,o_t^e,o_t^h)
    .,

the locomotion policy receives privileged observation o_t^p, proprioception observation o_t^i, scanned dots external observation o_t^e and historical observation o_t^h respectively. π_actor is modelled as a Gaussian policy and infers the optimized actions a_t to compute the joint positions q_des. c_t stands for the estimated value function from π_critic, which is updated through:



    L^critic_t=(c_t-c_t^targ)^2,

where 
    c_t^targ=∑_i=t^Tγ^i-tR(s_i),

s_i denotes the robot state, R(s_i) represents the rewards accrued at timestep i, which commonly includes guiding the robot to track a given velocity commands with stable gaits and attitude. A substantial efforts in reward engineering has enabled the value function c_t^targ to evaluate a comprehensive set of interactions between the robot and its environment. Therefore, when deploying the learning-based controller, c_t can be utilized to provide closed-loop feedback for the task planner.

The complete training paradigm will involve a second stage, training a depth encoder and a student network to reproducing o_t^p and o_t^e from real world accessible observations I_d (depth image), o_t^i and o_t^h. The motion controller will track the planned Δ_yaw and v_lin and the control action is represented by the 12 desired joint position q_des.





§ METHOD



 §.§ System Overview

As illustrated in Fig. <ref>, the proposed TOP-Nav pipeline connects a path and motion planner to tackle the task of legged navigation. To efficiently generate a collision-free path and stable motion states, we highlight a terrain estimator and a proprioception advisor inside the pipeline. In addition to combining these modules on the costmap (Fig. <ref>), the proprioception advisor provides online corrections for the terrain estimation. Such integration informs the robot of the traversability of unknown terrains and invisible obstacles, allowing TOP-Nav to handle open-world task without being constrained by visual conditions or prior knowledge.

To elucidate the task configuration, the robot is given a point goal with its location p_goal and has to approach the goal within a limited time. The location of the robot is denoted as (p_base,r_base). For external observation, we utilize a bio-channel perception module for both the task and motion planner. This module includes depth (I_d) and RGB (I_rgb) channels. The path planner computes the desired velocity command (v_lin,Δ_yaw) for the robot, which is tracked by the motion planner along with the appropraite joint position signal.



 §.§ Integrated Path Planner

A key feature within the path planner of TOP-Nav is the integrated cost map M_C which offers a comprehensive estimation that encompasses terrain traversability M_T, obstacles M_O, proprioception advice M_P and the goal approaching map M_G. This integration addresses both visible and unexpected external disturbances against the robot. 

To keep a balance between navigation efficiency and safety, we formulate the combination of M_C with dynamic scaling factors as follows:

    M_C=M_P+M_O+α_TM_T+α_GM_G,
    α_T=k_T_1/1+e^-k_T_2(d-d_0), α_G=k_G_1/1+e^-k_G_2(t-t_0)+k_G_0,

here M_i are 2-d matrix with a spatial resolution of 0.15m, d_0,t_0,k_T,k_G are hyperparameters, t denotes current time consuming and d denotes the distance from the point goal at the current step. We accord the highest priority to M_P and M_O since they represent non-traversable locations where the robot cannot pass through. The terrain traversability scale α_T decreases as the robot approaches the target. This design is made considering that, as the robot nears the target (d decreases), taking a detour to avoid a challenging yet traversable terrain would be an inefficient behaviour. Conversely, the goal approaching scale α_G increases with the duration t of the task.

With the integrated map M_C, we select the optimal waypoint p_way based on the lowest combined cost.

    p_way=_p∈E1/‖p_base-p‖∑_(x,y)=p_base^(x,y)=pM_C(x,y),

Here E denotes the set of points on the edge of M_C. For each point p in E, we calculate the average combined cost along the path from the robot location p_base to the edge point p. The optimal waypoint p_way is then chosen as the point with the lowest path cost.

After determining the optimized local target p_way, the target direction is calculated based on the relative position Δ_yaw=arctan(p_way-p_base)-r_base, here r_base denotes the yaw direction of the robot base. The linear velocity is constrained to mitigate the impact of high angular variation with v_lin=v_0e^-k Δ_yaw, thereby preventing abrupt and substantial turning at high speeds.

Obstacle Estimation and Localization:
In simulation, the robot has access to the ground truth location. For each point p=(x,y), M_G(x,y)=‖p_goal-p‖. In real world, we set a target direction r_goal to compute the goal map M_G(x,y) =‖ r_goal-arctan(p-p_base)‖. We construct the obstacle estimation based on the depth channel I_d. The perceived obstacles are converted into point clouds, and for each point in M_O, we compute the signed distance M_SDF to the closest obstacles within distance d_max, the cost of obstacles therefore can be computed as:

    M_O(x,y)=max(0,d_max-M_SDF(x,y))/d_max.




 §.§ Proprioception Advisor




We design the proprioception advisor to identify motion abnormalities that may arise from unexpected external disturbances. A straightforward approach is to use the velocity tracking error <cit.>, but its effectiveness can be hindered by the noisy velocity estimation on real hardware. Considering the interactions between the robot and its environment, the ideal metrics should be both observable and comprehensive. To address this, we utilize the value function c_t estimated from π_critic, which synthesizes multiple metrics and provides more stable results by incorporating historical observations.

The original π_critic requires privileged observations, we train another value function estimation network using observable motion states for the proprioception advisor to be deployed on hardware. The estimated motion evaluation c_t is normalized using a sigmoid function.  As demonstarted in Fig. <ref>, the motion evaluation will decline sharply when the robot encounters a locomotion failure, potentially due to transitioning onto challenging terrain. Implementation details for the reward design and normalization are provided in the appendix.

Within the path planner, we first directly utilize the motion evaluations to enhance the robot's awareness of unforeseen disturbances. This involves estimating collisions (M_P) beyond the obstacle estimator and integrating them into the combined cost map (M_C). Previous works generate fix-sized virtual obstalces when proprioception feedback reaches a certain threshold <cit.>, the design overlooks the variation of motion evaluations within each interval. We propose an continuously varied proprioception cost along the y-axis in M_P.

    M_P(x,y)=1-Norm(c_t)/e^k_P(‖ y_base-y ‖),

here k_P is hyperparameter. Since a lower motion evaluation indicates a potential challenging terrain or an invisible obstacle in the current direction of the robot, we allocate c_t to the centriod column in M_P and decrease it towards the edges. 



 §.§ Vision-based Terrain Estimation



The proposed terrain estimator incorporates terrain traversability into the path planner. Firstly, we construct a mapping from visual observations to reference terrain traversabilities using prior knowledge.

Given that terrains like stairs, slopes, or discrete small steps can be effectively identified from depth observations, to which our locomotion controller can respond appropriately, our task-level terrain estimation module focuses on semantic features such as color and texture. These features are highlighted by the RGB channel, specifically pertaining to terrains such as grass, snow, and cement. To correspond the RGB observation into real-world coordinates, we apply the perspective transformation on I_rgb to map the pixels to M_BEV, which has corresponding coordinates as M_T. In subsequent processing, we discretize M_BEV into patches (Fig. <ref>)  and assign the same difficulty within each patch.

To accomplish vision-based terrain estimation, the robot first identifies the observed terrain from M_BEV. We achieve this by training a terrain classification network π_terrain from offline collected data D_terrain. Considering that such inference falls short when faced with unfamiliar terrains, we calculate the predictive entropy to approximate the uncertainty U of the inference at current observation <cit.>. Since the approximation requires Monte Carlo simulation, we perform K stochastic forward inferences with different data augmentations at each timestep, with P_i^k denoting the predicted probability for label i at trail k. We estimate the expectation as the average of the predicted probability P_i=∑_kP_i^k/K. Then the predicted explicit terrain names T and corresponding uncertainty U for the current patch can be calculated as:

    U=-∑_iP_ilogP_i and  T =_i(P_i).


For safe navigation, terrain traversability signifies the robot's preference or stability in traversing a particular terrain, which has been explored from either self-supervision <cit.> or human demonstrations <cit.>. We leverage experience from these efforts to establish the terrain traversability (Fig. <ref>) as the operator preference. As we demonstrated in Fig. <ref>, such prior knowledge-based terrain traversability aligns with the proprioception evaluations.





 §.§ Online Terrain traversability Corrections


Through the seamless integration of vision and proprioception, we provide online corrections for the vision-based traversability estimation M_T_O without additional training (Fig. <ref>). This enables the robot with terrain awareness beyond the limitations of the collected data D_terrain.

We address the mechanism by which the robot recalls the traversability of a terrain once it has been seen and traversed within the navigation process. With the robot location (x_base,y_base), we record a duration of 1s proprioception advice to indicate the terrain traversability at the robot location:

    T_P(x_base,y_base)=1-Norm(c_t-k:t).


Besides T_P(x_base,y_base), we can access a latent feature L(x_base, y_base) extracted by the classifier π_terrain when the same patch was observed a few steps ago, we record both T_P and L at location (x_base,y_base) into an experienced list P_e. Now given a new observation at location (x_o,y_o), We can find a traversed and seen patch (x_r,y_r) that looks closest to (x_o, y_o) based on cosine similarity:

    S_(o,r)=_r ∈P_ecos(L(x_r, y_r),L(x_o, y_o)),


Note that we also have the historical traversability T_P(x_r,y_r) infered by the proprioception advisor at (x_r, y_r), we can compute a proprioception adapted traversability with the similarity S_(o,r) to access the historical proprioception correction M_T_P(x_o,y_o):

    M_T_P(x_o,y_o)=T_P(x_r,y_r)/1+e^-k_T_3(S_(o,r)-S_0),

here k_T_3 and S_0 are hyperparameters.

Due to the delay in estimation from the proprioception history, we intend for M_T_P to be utilized when encountering novel terrains where M_T_O becomes unreliable. Therefore, we normalize the visual uncertainty U(x_o,y_o) into confidence C(x_o,y_o) to adjust the contribution of M_T_O and M_T_P:

    M_T(x_o,y_o)=(M_T_O-M_T_P/1+e^-k_T_4(C-C_0)+M_T_P)(x_o,y_o),

here k_T_4 and C_0 are hyperparameters.

By integrating the adapted M_T as the terrain traversability into the path planner, our system demonstrates rapid adaptability to different terrain and visual conditions.



§ EVALUATIONS




 §.§ Experimental Setup


We assess TOP-Nav in both simulation and the real world. Considering the sim2real gap primarily lies in the RGB semantic information, we assume the robot has access to ground-truth terrain traversability in simulation. Therefore, our simulation experiments were designed to emphasize the following evaluations:


    
  * The improvements of Terrain awareness in navigating challenge environments.
    
  * The improvements of the proposed Proprioception advisor in navigating the challenge environments.
    
  * The improvements in locomotion by selecting waypoints with higher traversability.


Relatively, in addition to the evaluations mentioned above, our real-world experiments specifically focus on assessing the enhancements provided by the proposed terrain estimator.

Simulation Settings: Our simulation experiments are conducted within Nvidia Isaac Gym. We create a grid of 8 × 8 independent navigation cells. Each cell is 5m × 5m in size, featuring a robot assigned to a point goal navigation task. The robot and point goal is randomly generated, with a minimum initial distance of 5m within the cell. M_C is configured with dimensions (1.2m, 1.5m) centered around the robot`s location at p_base^map=(0.45m,0.75m). This design prioritizes a light-weight, real-time updated path planner and maintains consistency with the height map used in locomotion. The environments include randomly generated obstacles and terrains. Each cell is equipped with at least one wall obstacle, two column obstacles and the remaining space is divided into 1m × 1m sections. For terrain traversability assignments, we uniformly partition the obstacle-free space into difficulty levels [0, 0.25, 0.5, 0.75, 1] and generate terrain with various heights of steps and the intensity of irregular terrain corresponding to the difficulty levels. This difficulty serves as ground-truth walking preferences in simulation. The simulation experiments are conducted 25 times in each of the 64 navigation cells.



Real-World Settings: For real-world evaluations, we conduct outdoor navigation tasks in 5 different scenarios involving challenge obstacles and various terrains, with each scenario replicated 5 times for each method under investigation. To obtain a more comprehensive observation in the real world, we configure the cost map M_C with dimensions of (3m, 3m). The location of robot is at p_base^map=(0m, 1.5m) in real world M_C. The criteria for success include reaching the goal within the specified time constraints, consistent with the simulation setting.

Metrics: We evaluate TOP-Nav with the following metrics:

    
  * SR (Success Rate): The percentage of successful experiments. We define a success experiment as approaching the point goal into 0.5m within 20 seconds
    
  * TD (Terrain Difficulty): The percentage of average traversed terrain costs, TD provides the ground truth traversed terrain costs within each episode.
    
  * UT (Unstable Time): The percentage of unstable motion states (| roll |>0.15  or | pitch | > 0.15) in each episode.
    
  * VFT (Velocity Tracking Failure): The average percentage of velocity tracking failures (‖ v_lin-v_act‖>0.2) in each episode.
    
  * AEC (Average Energy Consumption): The average energy consumption (τq̇) <cit.> in each successful episode.

Among them, UT, VFT and AEC demonstrate walking states while completing navigation tasks in challenging terrains., TD provides a reference terrain traversable cost, measuring the improvements in terrain-aware path planning for gait stability. We calculate the variance across different navigation scenarios to assess the robustness of the proposed method.

Baselines: Beyond ablation study, we compare TOP-Nav against state-of-the-art legged navigation frameworks and segmentation based terrain estimators. VP-Nav <cit.> integrates vision and proprioception to develop a collision detector and a fall predictor within the navigation pipeline. GA-Nav <cit.> achieves terrain segmentation relying solely on vision.Sterling <cit.> learns terrain traversability in a self-supervised manner by assigning traversability to terrains based on those that share similar proprioception representations. However, this approach requires prior training for encountered terrains. 



 §.§ Simulation Evaluations




Improvements with Terrain Awareness: 
We illustrate a comprehensive terrain and obstacle map within one nav-cell in Fig. <ref>, exemplifying that the robot equipped with terrain awareness can plan a path with higher traversability. As demonstrated in Table <ref>, when integrated with terrain awareness, TOP-Nav surpasses the VP-Nav baseline by approximately 8% in success rate (SR). We observe that TOP-Nav and the wo/Proprioception methods achieve a TD of nearly 20%, which is half of the TD achieved in methods without terrain awareness. These results indicate that the proposed path planner empowers the robot to select terrains with higher traversability, leading to a significant improvement in the success rate of navigation.



Improvements with Proprioception Advisor:
The simulation environments are cluttered with various obstacles and difficult terrains, leading to potential locomotion failures such as getting stuck on irregular terrain surfaces or colliding with unseen obstacles during directional changes (Fig. <ref>). We demonstrate that the proposed proprioception advisor could address those challenges: In contrast to methods without proprioception, our approach exhibits an approximate 11% enhancement in SR. This is evident in the comparison between TOP-Nav and wo/Proprioception, as well as the comparison between wo/Terrain and Obstacle-Only. Such ablations also indicate that the terrain traversability integration and the proprioception advisor each contribute to performance improvement. Meanwhile, even without terrain awareness, the proposed system outperforms VP-Nav by 2.5%, signifying an improvement in our integration of the proprioception advisor compared to existing methods.
 
Advancements in Locomotion:
The metrics of UT and VFT evaluate the locomotion states. We observe that TOP-Nav achieves the lowest UT and VFT, indicating that selecting simpler terrains contributes to locomotion stability. In the absence of the proprioception advisor, wo/Proprioception and Obstacle-Only exhibit a significant increase in VFT due to the robot getting stuck by unexpected obstacles. We evaluate energy consumption with the assumption that when traversing simpler terrain, the robot should exhibit more natural gaits. As a result, TOP-Nav and wo/Proprioception exhibit a 10% reduction in energy consumption compared to methods without terrain awareness.



 §.§ Real World Evaluations


As shown in Fig. <ref>, TOP-Nav is evaluated in different environments featured with diverse terrains and obstacles. Among them, the terrains encountered in scenes 1-3 were provided in the terrain classifier, we assess the improvements introduced by the vision-based terrain estimator in these experiments. scenes 4-5 are designed to assess the effectiveness of the online correction module when encountering novel terrains. In scene 4, the terrain classifier does not include high-cost gravel. In scene 5, the robot encounters terrains with no prior knowledge, including a slippery detergent surface. The evaluations in scene 6 demonstrate that with the proposed proprioception advisor, the robot is able to avoid invisible obstacles.

Due to significant velocity estimation errors in the real robot, we exclude VFT in the real-world experiments. The Time metric measures the proportion of time the robot takes to complete the navigation task. ST in scenario 5 assesses the time taken by the robot from encountering a transparent object to completely avoiding the obstacle.

Improvements with the Visual Estimator:
The evaluation results for scenes 1-3 are presented in Table <ref>. TOP-Nav successfully completes all 15 trials with superior walking stability and minimal energy consumption. These results affirm the successful deployment of the proposed system on real hardware, allowing the robot to select waypoints on terrains with better traversability and thus execute more natural gaits. In contrast, GA-Nav, trained on RUGD <cit.>, demonstrates notable limitations when confronted with open-world navigation. We observe that the improvements of TOP-Nav over GA-Nav mainly stem from: 1) In scene 1,2, GA-Nav fails to identify the slabbed road as a low-cost terrain from the bush and gravel. 2) In scene 3, GA-Nav incorrectly identifies the lower part of the obstacle as a cement floor, which could be addressed by our integration of obstacles and terrain estimation. Nevertheless, GA-Nav surpasses VP-Nav and Obstacle-Only, which navigates without terrain awareness. With Obstacle-Only, the robot gets stuck by the stones or Grassroots, leading to a higher time consumption. While VP-Nav effectively alerts the robot to locomotion failures that could be brought by such terrains, the absence of terrain awareness prevents the robot from successfully navigating out of these challenging terrains, resulting in inferior locomotion performance.




Improvements with the Online Correction: To evaluate the efficacy of the online corrections for providing the robot with traversability on novel terrains, we conduct experiments in scene 4 using a terrain classifier trained without gravel. The degraded classifier exhibits an accuracy of 96.30% on the paved road with an average confidence of 0.87, whereas the inference on gravel has an average confidence of 0.43. We provide a navigation example in Fig. <ref> and divide it into four phases. In phase 1, despite M_P indicating that the robot is traversing a challenging terrain, the goal map M_G guides the robot to continue moving forward until this information is incorporated into M_T_P. In phase 2, an online correction is applied as the robot encounters similar gravel terrain, leading to an increased cost brought by M_T_P in the current direction. Consequently, the robot turns right to tap on the paved road. In phase 3, the robot avoids an observed obstacle. In phase 4, the historical proprioception continues to indicate the presence of high-cost gravel terrain on the left. The quantitative results are provided in Table <ref>. Despite GA-Nav possessing complete prior knowledge and correctly identifying the paved road in scene 4, our online corrections only lead to a 7% increase in UT, mainly due to the forward movement in phase 1. VP-Nav allows the robot to exit the gravel terrain with the safety advisor, but it can not retain this information. As a result, the target orientation prompts the robot to return to the gravel, causing increased time consumption and instability.



Evaluations on Challenge Novel Terrains: 
 In Scene 5, we present a more challenging scenario where the robot encounters terrains without any prior information. A black tarpaulin covered with slippery detergent is laid out in front of the robot, and the target direction is set in this facing direction. To mitigate the disturbances brought by reflections from detergents, the obstacle cost M_O is not considered in this experiment. As demonstrated in Fig. <ref>, the experiment could be divided into three phases. In phase 1, the robot moving forward along the goal direction, but the slippery terrain causes a decline in motion evaluations, resulting in a higher M_P. Simultaneously, the traversability cost of the slippery tarpaulin is recorded along with the visual features. In phase 2, the robot begins to turn aside to avoid the high-cost central area. In phase 3, the cost of M_G guides the robot to return to the original direction, where it can navigate on the wooden floor with better traversability to complete the task. The results presented in Table <ref> validate the effectiveness of our method in maintaining stability and improving the success rate by avoiding challenging terrain. Among the comparisons, Obstacle-Only keeps moving forward on the slippy surface and exhibit the worst motion performance. We include Sterling in this comparison to demonstrate the importance of online corrections when incorporating proprioception into terrain estimation. Without online corrections, Sterling behaves similarly to Obstacle-Only, unable to adapt to novel terrains and continuing forward. Compared with the result in scene 4, we demonstrate that TOP-Nav can handle novel terrains effectively without relying on operator preferences. This showcases the ability of the system to perform open-world navigation.






Invisible Obstacles: 
The proposed proprioception advisor provides close-loop feedback to alert the robot to invisible obstacles. We assess this capability in scene 6, where a glass wall is located along the planned path. In phase 1, the obstacle estimator is oblivious to the presence of the glass wall, and the robot continues moving forward. In phase 2, the robot collides with the glass wall, causing a rapid increase in M_P along the direction of movement. As a result, the robot adjusts its waypoint to steer clear of the high-cost area, successfully avoiding the glass wall. The quantitative results are provided in Table <ref>.



§ CONCLUSION

We present TOP-Nav, a legged navigation system that achieves closed-loop integration of visual and proprioception at both task and motion planning levels. Specifically, not only have we incorporated vision-based obstacle and terrain estimation into the navigation framework, but we have also utilized the proprioception advisor to make online corrections on these vision-only estimations. To validate the proposed system, we conducted extensive experiments in simulated environments featuring diverse terrains and obstacles. Furthermore, the navigation system was successfully deployed on a real robot, and quantitative experiments were conducted. These evaluation results underscore the success of our system in achieving open-world navigation, surpassing limitations posed by terrain, obstacles, or prior knowledge. Compared to existing approaches that couple vision and proprioception or navigate with terrain awareness, our system excels not only in achieving a higher success rate in challenge navigation, but also in demonstrating more stable gaits and lower energy consumption. In the future, we aim to extend the application of the proposed system to longer-distance navigation tasks through precise localization from radar. Furthermore, we plan to enhance the task planner with data-sufficient models such as Transformer.

plainnat






§ IMPLEMENTATION DETAILS



 §.§ Motion Planner Details

The motion planner is implemented following <cit.>. Both the actor and critic networks within the locomotion policy π have hidden layer sizes of [512, 256, 128]. The proprioception observation o_t^p includes angular velocity (3), Orientation (2), velocity commands (3), joint positions (12), joint velocities (12), and the last action (12). We store the last 10 steps of o_t^p into the history observation o_t^h. The depth encoder learns the exteroception latent features from onboard observation I_d using a conv-GRU structure. π is updated using PPO <cit.> for 20K iterations, the batch size is 160000 divided into 4 mini-batches. The depth encoder is optimized for 5k iterations. The rewards obtained at each step are the sum of the reward functions listed in Table <ref>. 





 §.§ Path Planner Details

For reference, we provide the default values of the hyperparameters used in the path planner in Table <ref>. Parameters d_0,t_0 adjust the weights of M_T and M_G as the robot approaches the target and as time progresses. k_G_0 keeps a minimum weight for the robot approaching the target. The listed values remain consistent in simulation; however, considering the various conditions in the real world, we make slight changes to these values in different real-world experiments for better validation of the contribution of this work. For instance, in outdoor navigation experiments, we assign a higher value to k_T_1 to expand the scale of the terrain estimator, which is effective for evaluating the accuracy of the proposed terrain estimator.

The default commanded velcoity v_0 is set to 0.5m/s in both simulation and the real world. The maximum distance d_max we considered for computing the obstacle map M_O is 0.3m.






 §.§ Proprioception Advisor Details

The proprioception advisor is trained with the locomotion policy π, with an exact same network architechture as π_critic. The input only includes proprioception observation o_t^p and historical observation o_t^h, updated through the MSE loss with c_t^targ. The estimated value c_t is normalized through sigmoid function:


    Norm(c_t)=1/1+e^2*(2.2-c_t)

In practice, and we set a minimum threshold c_th for the normalized critic value be considered in the path planner, in simulation c_th=0.8, while in the real world c_th=0.5, Norm(c_t) larger than c_th will be set to 1.

M_P is calculated with k_P=0.3, we demonstrate an example of M_P after the robot collides with a wall (with vision and obstacle detection disabled in this trial) in Fig <ref>. In this scenario, the normalized c_t returns 0.02, M_P has a width of 0.75m along the moving direction of the robot with costs greater than 0.5.



Generally, the proprioception advisor engages when locomotion states are compromised. To enable recovery from being obstructed by the unexpected terrains, we have designed the following recovery strategy in simulation: when Norm(c_t) falls below a specific threshold (0.5), we initially assign a backward velocity command of 0.5 m/s for 0.5 seconds. Next, to address potential terrain-related constraints that may prevent the robot from reaching planned waypoints at its default velocity, we reduce the frequency of the path planner to 0.5 Hz until the robot reaches the waypoint or makes a directional change of 0.3 radians. 



 §.§ Terrain Estimator Details


For prior data collection of the terrain estimator, we teleoperate the robot to walk for 5 minutes on each of the concerned terrains. We capture the motion evaluations c_t as well as the first-view observations during these demonstrations. We convert the RGB observations into BEV maps and dividing them into patches. The collected data is labelled based on the corresponding terrain from which it was acquired. The classification network is implemented using the MobileNet backbone and trained on a Nvidia GTX 2080 Ti for 6 hours. The proposed pipeline provides a light-weight inference model of 8 MB to be deployed for onboard computation. During deployment, at each step, we perform K=8 forward inferences of the terrain estimator to predict the vision-based terrain traversability and its uncertainty.

Based on the collected motion evaluations, we compute the average T_P observed during walks on each terrain. These walking experiences provide reference values for assigning the terrain costs defined by the operator. Table <ref> presents the validation accuracy of the terrain classifier, along with the reference terrain cost and the average collected T_P for each terrain. Since the terrains encountered in the real world did not exhibit significant differences in motion evaluations, we did not strictly set the reference cost based on linear correlation.


The hyperparameters used for online corrections are fine-tuned based on the distribution of the learned latent features L from the pre-collected data. Figure <ref> depicts a t-SNE visualization of the features inferred by the terrain classifier, involving data both within and outside of D_terrain. We observe that π_terrain can learn distinctive features for specific terrains, showcasing unique clustering in the latent space, even for previously unseen terrain types. The average cosine similarity S_(o,r) within each terrain class is 0.95, we set the middle point S_0 to be 0.85 and k_T_3 to be 15. This allows the terrain cost derived from historical experience to decrease rapidly when the similarity falls below 0.8.



To demonstrate the effectiveness of computing confidence in deciding whether the observed terrains is outside of D_terrain, we illustrate the distribution of samples across different confidence levels in Fig <ref>. After 200 steps of training, the confidence values for known terrains predominantly clustered beyond 0.9, while samples of novel terrains remained dispersed across the confidence axis. The confidence is used to adjust the weight of online correct ions in terrain estimation. We set k_T_4=20 and C_0=0.9 in the experiments.





 §.§ Hardware Details

We implement TOP-Nav on the Unitree-Go1 and Unitree-Go2 quadruped robot, with a weight of approximately 12 kg and dimensions of 645 mm × 280 mm. The robot is equipped with 12 brushless motors, each capable of producing a torque of 35.5 Nm. The perception module is equipped with a RealSense D435i camera, offering simultaneous depth and RGB channels. All computations are processed onboard with an NVIDIA Jetson NX asynchronously, i.e. the motion planner operates at a fixed frequency of 50Hz and receives the latest depth latent ℓ inference from the perception module. The path planner operates at a frequency of 3Hz. 




§ ADDITIONAL EXPERIMENTS




In this section, we provide additional experiments to justify the effectiveness of the key components in TOP-Nav.



 §.§ Evaluations on Proprioception Advisor

In this section, we discuss the relationship between the proposed proprioception advisor and terrain difficulties, along with ground truth reward terms such as velocity tracking and attitude stability.

 We conduct a series of straight navigation tasks in simulation, as demonstrated in Fig <ref>. Each robot's waypoint is set in a straight line, covering terrains of various difficulties D_l and types:


    
  * Irregular Surface: The height of each point in the terrain is randomly changed within the range of (0.01+0.04*D_l,0.07+0.04*D_l)m.
    
  * Discrete Steps: This terrain type consists of  12*D_l mall steps within a 1m × 1m area, where each small step has dimensions of (0.1,0.1,0.08)m.
    
  * Slope & Holes: Randomly switches between upper slopes and holes, where the maximum height/depth of each slope/hole is set as  1.2m*D_l.


We conduct 30k steps of navigation for each robot and compute the average motion evaluations estimated by the proprioception advisor. The results are shown in Table <ref>. We observe that as terrain ruggedness, irregularity, and the presence of obstacles increase, there is a corresponding decrease in the motion evaluations. Although this correlation is not strictly linear, the results provide supportive for us to estimate terrain traversability based on the proprioception advisor. 

For comparison, we train an independent motion evaluation function π_deq, which is decoupled from the training process of the motion controller. π_deq is updated using the MSE loss between the ground truth motion states and the network prediction c_ref. The ground truth walking states include velocity tracking, orientation penalty, and energy consumption:

    c^target_ref=min(v_act,v_lin)-(roll^2+pitch^2)-0.001*τq̇,

we train π_deq for 10k iterations, c^target_ref is normalized using a sigmoid function.  

As shown in Table <ref>, c_ref demonstrates a noticeable decline as terrain difficulty increases, yet it does not exhibit a significant advantage compared to the proposed proprioception advisor.

On the other hand, the same terrain may exhibit different traversability for robots with varying locomotion capabilities. Therefore, the evaluation of walking states represents another important metric for assessing terrain traversability. We demonstrate the correlation between the predicted motion evaluations c_t and the ground truth motion states c^target_ref in Fig <ref>. The perason correlation coefficient is 0.914, indicating a strong linear correlation between the estimated c_t and the actual motion states of the robot.







Building upon the discussion above, we validate that constructing the proprioception advisor with critic output enables efficient motion evaluation, including information on both the terrain difficulty and walking states of the robot. Moreover, the proposed advisor can be implemented without the need for additional training or sensors, making it a more appropriate approach to provide motion evaluations for robot path planning compared to existing methods.



 §.§ Different Velocities

We conducted quantitative experiments in simulation with different velocities (0.25, 0.5, 0.75, 1.0) (m/s), and the results demonstrate that proper velocity design does affects the performance of the navigation system. As demonstrated in Table <ref>, a slower velocity command (0.25) prevents the robot from reaching the goal on time, resulting in a lower success rate. Moreover, it can be concluded that lower the speed would help decrease the energy consumption.

On the other hand, setting a too large velocity command will introduce locomotion instability, as indicated by the results with a velocity of 1m/s. However, conducting a quantitative analysis of the influence of speed is challenging in complex terrains. For instance, in slopes, robots with higher velocities may navigate more effectively, whereas terrains with discrete steps could lead to locomotion failures at faster speeds. Consequently, experiments with velocity commands of 1m/s still achieve a high success rate.

The performances vary between 0.5m/s and 0.75m/s is not significant; therefore, we consider this range to be an appropriate commanded velocity range for deployment. In real-world experiments, we demonstrate the robot's velocity set at 0.75m/s in the supplementary video.



 §.§ Long Distance Navigation

To further investigate the open-world navigation ability of the proposed system, we conduct a longer outdoor navigation experiment covering a total distance of 190 meters, as depicted in Fig. <ref>.



We conducted the experiment in an off-road scenario with grass and gravel on both sides of the path. The robot was initially given a target direction straight ahead, relying on the terrain and obstacle estimator to keep navigating on the paved road. The path traversed by the robot is depicted in Fig. <ref>. We observe that in such scenarios, the challenging terrains help guide the robot in changing its moving direction, ensuring that the robot stays on the paved road automatically. In contrast, on the open flat terrain, we had to provide teleoperation to make the robot change its direction and move onto another narrowing path.

Since we do not have localization enabled in the real-world experiment, human teleoperation was used three times to change the target moving direction. This limitation was due to the onboard vision system's inability to perform accurate online localization, which we plan to address by integrating a mounted lidar in future work.




