Coupling Vision and Proprioception for Navigation of Legged Robots

Introduction

Gibson has famously remarked, “we see in order to move and we move in order to see.” Although, it would be more accurate to say that we see and feel in order to move. Vision and proprioception are complementary senses. Vision is a distance sense, which allows us to avoid static and dynamic obstacles. However, vision is slow and cannot directly sense physical properties of terrains such as softness vs. hardness, smooth vs. rough. Proprioception (knowledge of agent’s own body like joint angles, body orientation, foot contacts, etc.) is fast and gives a direct measurement of physical environment characteristics. In this paper, we will focus on exploiting the complementary strengths of vision and proprioception for navigation of legged robots. The goal is to train a legged robot by developing both low-level control of its motor joints to walk on terrains (i.e., locomotion) as well as high-level path planning to reach certain goal locations by autonomously avoiding any obstacles along the way (i.e., navigation).

Locomotion and Navigation: Traditionally, locomotion and navigation are studied as separate problems and then put together on a robot as individual modules . However, to truly support dynamic goal reaching in complex terrains, the planner should know about the walking ability of the robot in different terrains. For instance, a robot navigating to a goal through a slippery patch may either lower its walking speed or walk around it altogether depending on its locomotion ability. To facilitate such communication between high-level and low-level, prior works generally infer a cost map for the planner from an onboard vision sensor which is only capable of detecting clearly visible obstacles and regions that are hard to traverse, e.g. steps and ramps . However, it is extremely challenging to predict several other terrain properties from vision like how slippery, uneven, granular or deformable the surface is. These directly affect the walking robot’s ability to follow the plan. Furthermore, the environment could also contain obstacles that are invisible to a vision-only planner as shown in Figure 1 and Figure 5, e.g., glass walls or uneven bumps on ground — things which a robot can readily feel as it walks through them.

Proprioceptive Feedback: Our insight is to leverage this robot’s on-ground feeling as observed via proprioception to bridge the gap and continually update the high-level navigation plan in accordance with low-level locomotion. Furthermore, this coupling of locomotion with navigation improves locomotion efficiency as well. For instance, a planner aware of locomotion ability can direct the robot to switch low-level gaits (walking $\rightarrow$ trotting $\rightarrow$ galloping) for increasing its speed whenever the path is straight and switch other way round to decrease speed on winding paths. We posit that the adaptation of navigational plan from vision and proprioception must occur online in real time. But, how?

Coupled Vision and Proprioception: We show a high-level illustration of our overall system VP-Nav (Vision and Proprioception for Navigation) in Figure 2. It consists of three subsystems: a velocity-conditioned walking policy, a safety advisor module, and the planning module which together make synergistic use of vision and proprioception for navigation of legged robots. At the lowest level, our velocity-conditioned locomotion controller is trained via reinforcement learning to allow the robot to walk at different speeds and in different directions. It takes the commanded linear and angular velocity as input along with the robot’s proprioception state to predict the target joint angles directly without using any hand-engineered control primitives. We train this base controller in simulation via energy-based reward to allow for seamless gait switching at different speeds and then transfer to the real world via rapid motor adaptation that estimates environment extrinsics using an adaptation module trained in simulation. Once we have learned the walking policy, which includes the base policy and the adaptation module, in simulation, we freeze it and train a Safety Advisor (SA) Module, also in simulation, which learns to estimate the safety constraints of the walking policy. It uses proprioception to estimate(1) if the robot is in collision to a visually undetected object such as glass walls (2) what is a safe velocity limit for the robot to walk in the current terrain which could be soft, slippery, bumpy, etc. During deployment, the walking policy (base policy and adaptation module) and the SA (safety advisor) module are kept frozen and interact with the planner as shown in Figure 2. The planner uses on board cameras to compute a navigation cost map for an input to the point goal and takes in the two bits of safety constraints from the safety advisor module to compute the target linear and angular velocity which is given to the walking policy to track. This planner also ensures that both the linear and angular commanded velocities are within the feasible range of the walking policy. The planning module continually updates the cost map and safety constraints to generate the target velocity for the walking velocity as the robot moves. All the modules run asynchronously onboard of the robot.

Simulation and Real-World Evaluation: We evaluate our system VP-Nav in challenging navigation settings (e.g., Figure 1) with difficult terrains, invisible glass obstacles, slippery surfaces, deformable ground and challenging outdoor scenarios. Please see videos at https://navigation-locomotion.github.io

In addition, we conduct a series of experiments in simulation. For this we import real-world Matterport 3D maps used in Habitat and Gibson into RaiSim to create a simulation benchmark for controlled study of joint navigation and legged locomotion. We find that the proposed system is 7% - 15% better than baselines with disjoint planning and control loop in different terrains and in settings with invisible obstacles. We find that minimizing time to goal can lead to more energy consuming behaviours which can be compensated for by the use of efficient locomotion policy with emergent gaits. We also additionally show the importance of legged systems over wheeled counterparts in traversing challenging terrains, and empirically demonstrate that continuous velocity-conditioned policy is more time efficient than its discrete counterpart.

Velocity-Conditioned Walking Policy

Our velocity-conditioned walking policy is an implementation of the approach in . We present a review here to make this paper self-contained. The walking policy contains a base policy which takes the command velocity and the robot state as input and predicts the target joint angles. It additionally takes the extrinsics vector as input which is estimated by the adaptation module and enables rapid online adaptation to varying environment conditions .

RL Reward: Reward encourages the policy to accurately track a commanded linear and angular velocity while penalizing a higher energy consumption . We denote the linear velocity as $v$, the orientation as $\theta$ and the angular velocity as $\omega$, all in the robot’s base frame. We additionally define the joint angles as $\bm{q}$, joint velocities as $\bm{\dot{q}}$, and joint torques as $\bm{\tau}$. The reward at time $r_{t}$ is defined as the sum of the following quantities (see supplementary for specifics):

Energy Consumption: $-{\bm{\tau}^{T}\bm{\dot{q}}}$

Training Scheme: Similar to , we train our agent on fractal terrains without any additional artificial rewards for foot clearance or external pushes. For target velocities, we sample from one of the two settings: jointly track linear and angular velocity (curve following), or turning in place. Turning in place is important to handle very cluttered environments. See supplementary for range details.

Adaptation Module: Since we don’t have the privileged environment information during deployment, we use RMA to train an adaptation module $\phi$ in simulation itself to estimate the extrinsics $z_{t}$ from proprioceptive state, which is available during deployment. Concretely, the adaptation module uses the recent history of robot’s states $x_{t-k:t-1}$ and actions $a_{t-k:t-1}$ to generate $\hat{z_{t}}$ which is an estimate of the true extrinsics vector $z_{t}$. This is trained via supervised learning because we have access to both proprioceptive history and the true extrinsics vector in simulation.

Safety Advisor Module

The safety advisor module captures the constraints which enable the robot to walk safely. For this, we train two safety advisors in simulation: (1) Collision Detector $M_{c}$ to detect collisions and (2) Fall Predictor $M_{f}$ to predict future falls, both from proprioception which includes the recent history of states ($x_{t-k:t-1}$) and actions ($a_{t-k:t-1}$) (analogous to ). During deployment, the safety advisor module uses the prediction of these two advisors to inform the planner of the safe operating constraints of the walking policy.

Collision Detector ($M_{c}$): The collision detector estimates the probability of whether the robot is currently in collision, using proprioception ($M_{c}(x_{t-k:t-1},a_{t-k:t-1})$). If a collision probability is above a threshold (0.5), the safety advisor module adds a fixed size patch of obstacle (9cm x 3cm, about the head size of A1), where the side with 3cm is in the current direction of robot, to the cost map in front of the current position of the robot to indicate an obstacle which may be missed by the vision system (e.g. glass walls).

Fall Predictor ($M_{f}$): The fall predictor makes a probability prediction of whether the walking policy is likely to fall within the next 1s using proprioception ($M_{f}(x_{t-k:t-1},a_{t-k:t-1})$). If a fall probability is above a threshold (0.5), the safety advisor module decreases the velocity limit ($v^{max}_{t}$) by 0.2 m/s, otherwise it increases the velocity limit by 0.05 m/s. The planner uses $v^{max}_{t}$ to generate the linear velocity command for the walking policy. This enables the planner to slow the robot down in dangerous settings like soft or slippery terrains, heavy payload, etc.

Module Training: We train both the safety advisors $M_{f}$ and $M_{c}$ in a self-supervised fashion in simulation. We collect data under randomly sampled environments and commands, and record the binary labels on (1) robot is currently in collision (2) if the policy results in a fall in the next 1s. We then train the safety advisors by minimizing binary cross-entropy loss. Details are in the supplementary.

Visual Planner

The visual planner uses the onboard cameras to generate a top down 2D cost map and uses it to plan a path to the goal. It additionally uses the safety constraints estimated by the safety advisor to generate the command velocities which are fed into the walking policy. Concretely, the visual planner consists of (1) a mapping module which generates a top down 2D occupancy map from onboard cameras, (2) cost map generation step using Fast Marching Method (FMM) and signed distance field, (3) PID based planner to use the cost map and safety constraints from the safety advisor module to generate linear and angular velocity commands for the walking policy.

We first generate a top down 2D visual occupancy map by incrementally accumulating point clouds from an onboard Intel RealSense D435 depth camera as the robot moves. The point clouds are transformed into the world reference frame using pose information from an onboard tracking camera (Intel RealSense T265). The transformed point clouds are capped by a maximum height of interest and then dynamically projected into a horizontal 2D frame to form an occupancy map where each grid has a value from 0 to 1 to indicate the probability of being free space. The occupancy map is binarized for the path planning using a threshold of 0.5. We use an open-sourced implementation from Intel RealSense to compute the visual occupancy map . We convert it to a configuration space by modeling the robot size as a square and dilating the occupancy map.

2 Cost Map Generation

The 2D cost map is a sum of goal distance map (geodesic distance to the goal) and obstacle distance map (to maintain a safety margin from obstacles). Following the direction of steepest descent from any starting point in this cost map gives an obstacle free path to the goal.

Here, $\alpha_{2}$ is a scaling factor to trade off the two costs. During deployment, the safety advisor module asynchronously adds an additional local obstacle to the cost map if the collision detector ($M_{c}$) predicts a collision.

3 Velocity Command Generation

Angular Velocity: We use a PD controller to compute the command angular velocity (3) which is then clipped to the feasible range (specified in supplementary):

Experimental Setup

Physical Hardware: We use the A1 robot from Unitree with 18-DoF (12 actuatable). Its proprioception sensors include joint motor encoders, roll and pitch from the IMU sensor and binarized foot contact indicators. We additionally mount Intel RealSense depth D435 and tracking T265 cameras. The deployed policy uses joint position control.

Locomotion Policy: For locomotion policy, we use similar architecture and training details as , and list the exact policy and training details in the supplementary.

Safety Advisor Module: Similar to the adaptation module, both the collision detector and fall predictor module share the same architecture and embed states and actions into a 32-dim vector using a linear layer. Then, we use 3 layers of 1D convolutions with input channels, output channels and strides $,,$. The flattened features are then passed through a 2-layer MLP with 8 hidden units to get 1 sigmoid output as the predicted probability value. We train the module in an online fashion by rollouts in environments with randomly sampled invisible obstacles, frictions, terrain roughness and payload values (see supplementary for ranges). At simulation test time, we run both the collision detector and fall predictor at 5Hz, whereas for deployment on robot we train a lightweight version using only the last $0.2$s of observation history and run it at 10Hz. More details are in the supplementary.

Simulation Environments: We generate top-down view room layouts from room scanning meshes using habitat-sim . The meshes are from gibson environment and matterport3D . We then select 200 challenging room layouts for navigation as our validation set. For each room layout, we sample 10 navigation goals and set the initial point to be the farthest point from the goal. We then convert the room layout to RaiSim simulation environment . The resolution is 0.1m per pixel. We show an example of the top-down layouts and the generated environment in figure 4.

To demonstrate our navigation system on complex terrains, we construct the following variations:

Flat: flat surface with coefficient of friction $\mu=0.8$.

RoughTerrain: we put eight patches of z-scale 0.05 and size 0.8m$\times$0.8m along the path from initial to the goal position. The rough terrain is constructed using the built-in terrain generator by RaiSim .

2x/4x/8x Inv-Obstacle: we put 2/4/8 0.2m$\times$0.2m obstacles that cannot be detected by the vision sensor.

Randomized: we put 8 rough and slippery patches along the path from initial to goal position. The rough patches are of z-scale 0.05. The coefficient of friction of slippery patches are sampled from {0.1, 0.3, 0.5, 0.7, 0.9}. An 8kg payload (A1 itself is 12kg) is placed on / removed from top of the robot every 5s.

Experimental Results

We test our approach both in simulation and in the real world.

In simulation we assume that the agent has access to the ground-truth occupancy map, and we only vary the terrains and the navigation strategy. The purpose of our simulation experiments is to answer the following questions:

How much does proprioception feedback help?

Minimizing time to goal requires more aggressive walking and more energy. Can a varying gait policy compensate for some of the energy consumed?

We additionally evaluate the following broader questions:

Does legged locomotion improve goal reaching?

Is continuous velocity conditioning better than discrete?

Baseline and Metrics: We use the LoCoBot as our wheeled robot baseline, as it is widely used in visual navigation . We import the PyRobot URDF model . Both VP-Nav and the LoCoBot use a control frequency of 100Hz and a planning frequency of 10Hz. We evaluate our system using the following metrics: 1) Success Rate 2) Success weighted by (normalized inverse) Path Length (SPL) 3) Average time used to achieve the goal. If the agent fails to reach the goal, we add a constant timeout penalty (220s) for the failure episodes; 4) Average energy consumption over the successful episodes .

Improvements with Proprioceptive Coupling: We separately analyze the importance of the two safety advisors (collision detector and fall predictor).

Collision Detector: We uniformly place 2/4/8 0.2m$\times$0.2m obstacles along the path from initial and goal positions, and run VP-Nav with/without proprioceptive feedback. The obstacles are not marked in the top-down view map to simulate the glass or other objects that an imperfect vision sensor fails to capture. In Table 1, we note that adding invisible obstacles makes the navigation task very challenging as evident from the performance drop of all the methods. Using the proprioceptive collision detector module improves the success rate by 5.7 points over the baseline method which does not use it. The performance improvements are even larger when the environment becomes more challenging with up to 15 points improvement over baseline.

Fall Predictor: In Table 2, we show that learned fall predictor enables safe navigation in challenging environments involving a combination of slippery surfaces, rough terrains, and payload changes. We put eight 2.4m$\times$2.4m patches with uneven slippery surfaces along the path from initial and the goal positions. An 8kg payload is placed / removed to the robot every 5 second. Using the proprioceptive fall prediction to adjust the speed of the robot gives 7 points higher goal-reaching success rate over the baseline without proprioception.

Compensating for higher energy consumption induced by minimizing time to goal: Minimizing time to goal leads to aggressive locomotion behaviours and increased energy consumption. To compensate for some of the increase in energy consumption, we show that a policy with efficient gaits leads to a 10% lower energy consumption compared to a fixed gait trotting-only policy (Table 3). VP-Nav also has a slightly higher success rate because it switches to a more stable gait at low speeds when traversing complex settings, as compared to a fixed gait policy. VP-Nav automatically switches gaits to optimize for stability and energy at different speeds.

Legs vs. Wheels: We also compare VP-Nav with LoCoBot on visual navigation in Table 4. On flat terrains, LoCoBot has a slightly lower performance since LoCoBot is more prone to getting stuck in the local minima of the FMM map (see supplementary for details). Whereas adding rough terrain (5cm elevation) to the environment leads to a significant drop in goal-reaching performance of the LoCoBot. We additionally try the planning scheme which plans around rough terrains while assuming ground truth access to their locations. Although the success rate improves, the time cost is still significantly worse than our legged robot baseline, which is able to maintain similar success rate and time to goal because of its robust walking capabilities. In short, though energy efficient, wheeled robots struggle on uneven terrains, whereas legged robots are more terrain-agnostic.

Continuous Velocity Conditioning vs. Discrete: We compare our continuous planner to a discrete planner typically used in visual navigation . Our discrete planner only commands four actions: 1) forward with 0.6 m/s; 2) turn left with 0.8 rad/s; 3) turn right with 0.8 rad/s; 4) stop, whereas planning over the continuous range of linear and angular velocities enables smoother trajectory and shorter time to goal. In Table 5, we see that our system VP-Nav is $27\%$ more time efficient than a discrete planner as the robot can simultaneously turn and go forward.

2 Real-World Experiments

Invisible Obstacles: We tested the collision detector with invisible obstacles like glass doors, humans that abruptly walk into the robot’s path, walls and boxes without textures (Fig 5, 6). We find that feedback from the safety module gives higher success rate in all these settings. The glass wall, which is invisible to the onboard cameras is detected by the proprioceptive feedback once the robot collides with the door. The missed obstacle is then updated in the map at the place of the collision and the robot replans its path around it. Humans abruptly rushing into the robot’s path are similarly missed by the camera, and then later block the robot’s cameras to be detected by them (depth camera’s near distance is around 30cm). Such obstacles that suddenly appear from outside into the field-of-view render the trajectory prediction approaches useless . With proprioceptive collision detector, our robot can reason about these “invisible” objects and update its occupancy map to plan a new path.

Rough Slippery Terrains: We tested the fall detector with challenging terrains including movable planks scattered on the floor and slippery terrain, shown in Figure 6 and in the supplementary. On rough slippery terrain, the fall predictor uses proprioception to estimate the risk of falling and accordingly decreases the velocity to ensure safety.

Other Complex Indoor Navigation: We deploy VP-Nav in challenging settings and compare to baselines which use pure vision without fall prediction and collision detection from proprioceptive feedback, and evaluate for 5 trials in all settings (Figure 6). We find that using vision and proprioception for coupled navigation and locomotion gives a higher success rate in all these settings. In the left of Figure 6, we have 2 indoor tasks which require taking a detour with planks scattered on the floor and maneuvers through a cluttered narrow path. In both settings, there are objects that can easily be missed by the vision system, including white walls with no texture, transparent desktop side panels and large brown packaging boxes in dim light. With the proprioceptive safety advisor, our robot can reason about these “invisible” objects and update its occupancy map to replan for a new viable path, despite hitting the same number of obstacles. The robot also slows down on unstable planks that are scattered on the ground.

Related Work

Visual Navigation: Visual navigation is mainly studied on wheeled robots by chaining mapping, localizing, and planning. Once a 2D map is created, an optimal path to a goal can be found using graph search techniques , level-set methods or potential field methods among others. The map is constructed via simultaneous localization and mapping using classical or learned methods assuming access to nearly perfect low-level control. In our benchmark, we import maps from the common navigation datasets include Habitat , Gibson and Matterport3D .

Navigation of Legged Robots: Earlier works decouple locomotion and navigation which restricts the application only to simple terrains . This decoupled framework has been extended to include learned modules for cluttered environment navigation . describe a coupled navigation and locomotion framework by estimating foothold placements from an elevation map. Foothold scores can be estimated heuristically or learned . Other methods forgo explicit foothold optimization and learn traversibility maps . Several works complement vision-based state estimation by using contact information . Instead of relying only on vision, we combine navigation and locomotion via coupling vision and proprioception.

Legged Locomotion: This has conventionally been accomplished using control theory over handcrafted dynamics models. Recently, RL has been successfully used to learn such policies in simulation and in the real world with sim2real methods . Alternatively, a policy learned in simulation can be adapted at test-time to work well in real environments .

Conclusion and Limitations

The use of a legged robot instead of a wheeled one broadens the applicability of visual navigation to complex terrains and environments. In this paper, we combine low-level locomotion with high-level navigation planning to enable goal-reaching for a legged quadruped robot. Our approach, VP-Nav, tightly couples vision and proprioception to exploit their complementary strengths for robust navigation in the presence of disturbances, transparent obstacles and complex terrains which may not be detected by vision alone. VP-Nav is lightweight and only uses the modest onboard computation and storage of the low-cost A1 quadruped robot. One limitation of our system is that low-level locomotion module communicates with navigation planner via safety module, and is not conditioned on the vision directly. Due to this, the robot can walk around obstacles but can not climb or jump over them. We leave vision-guided locomotion for future.

Acknowledgement We thank Aravind Sivakumar, Kenny Shaw and Shivam Duggal for help in real-world experiments. This work is supported by DARPA Machine Common Sense program and in part by Good AI research award.

References

Appendix A Locomotion Policy Details

Adaptation Module Architecture: The adaptation module first embeds states and actions into 32-dim vector using a 2-layer MLP. Then, a 3-layer $1$-D CNN convolves the representations across the time dimension to capture temporal correlations in the input. The input channel number, output channel number, kernel size, and stride of each layer are $,,$. The flattened CNN output is linearly projected to estimate $\hat{z_{t}}$.

Learning the Walking Policy: We jointly train the base policy and the environment encoder network using PPO for $15,000$ iterations ($1.2$B sample, 24 hours) each of which uses batch size of $80,000$ split into $4$ mini-batches. We then train the adaptation module using supervised learning with on-policy data. We run the optimization process for $1000$ iterations ($80$M samples, 3 hours) and use Adam optimizer to minimize MSE loss. The batch size is $80,000$ split up into $4$ mini-batches.

Reward Function: The reward at time $r_{t}$ is defined as the sum of the following quantities:

Energy Consumption: $-{\bm{\tau}^{T}\bm{\dot{q}}}$

We list the ranges of command linear velocity and angular velocity in Supplementary Table 6. We re-sample the command velocities within a single episode with probability $0.004$.

Appendix B Safety Advisor Details

Hyperparameters: Velocity changes in Fall Predictor and the size of obstacles in Collision Detector are set by simple rules. For instance, (a) if a fall is predicted, the safety advisor module decreases the velocity limit by a large amount (we pick 0.2 m/s), so the robot can slow down quickly; (b) otherwise, it increases the velocity limit by a small amount (we pick 0.05 m/s) for conservative speed up; (c) the size of obstacles in Collision Detector (9cm x 3cm) is set to roughly be the size of the head of the robot. Additional real-world experiments [link] show that if the obstacle is set to be larger, the robot will take a more conservative path around the unexpected obstacle. If the obstacle is set to be smaller, the robot takes a shorter path but risks colliding legs with the unexpected obstacle.

Network Structure: Similar to the adaptation module, both the collision detector and fall predictor module share the same architecture and embed states and actions into a 32-dim vector using a linear layer. Then, we use 3 layers of 1D convolutions with input channels, output channels and strides $,,$. The output is a sigmoid scalar.

Training Data and Environments: The scalar sigmoid output predicts a probability value, indicating whether the robot collides with an obstacle in the Obstacle Detector, or if the robot falls at time $t+100$ and otherwise (note that one simulation time-step is 0.01s) in the Fall Predictor. We train both modules in an self-supervised fashion by collecting data from robot walking / colliding with the obstacles / falling down. Data are collected given random command linear/angular velocity commands in environments with randomly sampled frictions, terrain roughness and payload values from the following list:

Coefficient of Friction: $[0.1,0.6,1.1,1.6,2.1]$.

Rough Terrain z-scale: $[0.01,0.08,0.14,0.23]$ (m).

Angular Velocity: $[-0.4,0.0,0.4]$ (rad/s).

We train both Obstacle Detector and Fall Predictor for 145k iterations with a batch size of 1000. At simulation test time, we run both the collision detector and fall predictor at 5Hz whereas for deployment on robot we train a lightweight version using only the last 20 timesteps of observation history and run it at 10Hz.

Appendix C Visual Planner Details

We command the angular velocity for our robot and the baseline LoCoBot using the following equation:

Appendix D LoCoBot Baseline & Discrete Planner Details

We import the PyRobot URDF model . Both our method and the LoCoBot use a control frequency of 100Hz and a planning frequency of 10Hz. We follow to convert commanded linear and angular velocity to the angular speed of the left and right wheel of the LoCoBot. We set the forward action of the discrete planner at 0.6 m/s after we measured the average speed of the continuous planner in the same evaluation environment is around 0.6 m/s. The low level controller is also a PD controller with $K_{p}=10$, $K_{d}=0.05$. The controller gain is adjusted so that no obvious motion jerk happens during movement. Since the control of wheeled robot is simpler and more accurate, we do observe the LoCoBot being more likely to stuck in local minima in the cost map (an illustration is shown in Supplementary Figure 7). For our robot, since the locomotion policy is not perfect and the legged robot is harder to control compared with LoCoBot, it sometimes can get out of the local minima due to the noisy movement, which is the reason why we perform better in the perfect flat ground (Table 4 (a) and (b) in the main text). However, we want to emphasize again that our point here is not to show our robot performs slightly better than baseline in the flat ground. Instead, what we show is the ability to traverse and navigate over difficult terrains where LoCoBot easily fail (Table 4 (c), (d), and (e) in the main text).