On Terrain Aware Locomotion for Legged Robots

Shamel Fahmi

Doctoral Thesis Defense

April, 2021

 Supervisors: Victor Barasuol, Michele Focchi, Andreea Radulescu, and Claudio Semini

Legged Robots are moving out from research labs into the real world, with one main goal in mind.

Is there anything common in these applications?

The terrains that legged robots traverse are never the same.

  • Dynamic: They can change or move.

  • Unexplored: The robot has not traversed it before.

  • Uncertain: In scenarios where vision is denied, or when the robot's perception is noisy.

The terrains introduces a large amount of uncertainty.

How to deal with this uncertainty?

Legged Robots Have to be Terrain Aware.

Terrain Aware Locomotion (TAL) means that the robot is able to take decisions based on the surrounding terrain using its perception.

Terrain Aware Locomotion (TAL)

  • Proprioceptive TAL (PTAL)

  • Exteroceptive TAL (ETAL)

Relies on the internal robot measurements to acquire the terrain information.

​Relies on directly acquiring the terrain information mainly using the robot's vision.

7

Proprioceptive TAL (PTAL)

Barasuol 2019

Focchi 2013

M. Focchi et al., Local Reflex Generation for Obstacle Negotiation in Quadrupedal Locomotion, CLAWAR, 2013.
V. Barasuol et al., On the detection and localization of shin collisions and reactive actions in quadruped robots, CLAWAR, 2019.

8

Exteroceptive TAL (ETAL)

Barasuol 2015

Villarreal 2019

V. Barasuol et al., Reactive Trotting with Foot Placement Corrections through Visual Pattern Classification, IROS, 2015.
O. Villarreal et al., Fast and Continuous Foothold Adaptation for Dynamic Locomotion through CNNs, RA-L, 2019.

9

Proprioceptive TAL vs. Exteroceptive TAL

PTAL is crucial when

  • Visual feedback is denied (smoky areas, or areas with thick vegetation). 

  • Terrain map is unreliable.

 

 

But

  • Limited to corrective actions (step reflexes).

  • Do not act on what is ahead of the robot.

10

Research Statement

Let legged robots take decisions based on the surrounding terrain using their perception.

decisions

terrain

perception

Let legged robots take decisions based on the surrounding terrain using their perception.

Passive Whole-Body Control (pWBC)

STANCE: Locomotion Adaption over Soft Terrain

On State Estimation for Legged Robots over Soft Terrain

ViTAL: Vision-based Terrain-Aware Locomotion

Perception

  • Control

  • State Estimation

  • Planning

Terrain

  • Proprioception

  • Vision

Decisions

  • Physical Properties

    • Friction

    • Impedance

  • Geometry

Using Optimization and Machine Learning

Outline

  • Part I: Proprioceptive Terrain-Aware Locomotion

    • Passive Whole-Body Control (pWBC)

    • STANCE: Locomotion Adaption over Soft Terrain

    • On State Estimation for Legged Robots over Soft Terrain

 

  • Part II: Exteroceptive Terrain-Aware Locomotion

    • ViTAL: Vision-base Terrain-Aware Locomotion

  • Part I: Proprioceptive Terrain-Aware Locomotion

    • Passive Whole-Body Control (pWBC)

    • STANCE: Locomotion Adaption over Soft Terrain

    • On State Estimation for Legged Robots over Soft Terrain

 

  • Part II: Exteroceptive Terrain-Aware Locomotion

    • ViTAL: Vision-base Terrain-Aware Locomotion

Whole-Body Control (WBC)

  • To achieve dynamic locomotion, the robot should:

    • Track the desired reference trajectories.

    • Maintain balance.

    • Reason about the robot's (full) dynamics and limits.

 

  • To achieve TAL, the robot should reason about the contact interaction such as the terrain's inclination and frictional properties, etc.

 

  • How to deal with all these objectives in an optimal way?

 

  • A Whole-Body Control (WBC) Framework.

14

Whole-Body Control (WBC) Framework

  • Locomotion Planner [1]-[3]

  • State Estimator [4]

  • WBC

  • Low-level Control [5]

[1] B. Aceituno-Cabezas et al., Simultaneous Contact, Gait and Motion Planning for Robust Multi-Legged Locomotion via Mixed-Integer Convex Optimization, RA-L 2017.
[2] C. Mastalli et al., Trajectory and Foothold Optimization using Low-Dimensional Models for Rough Terrain Locomotion, ICRA, 2017.
[3] M. Focchi et al., Heuristic planning for rough terrain locomotion in presence of external disturbances and variable perception quality, STAR, 2020.
[4] S. Nobili et al., Heterogeneous sensor fusion for accurate state estimation of dynamic legged robots, RSS, 2017.
[5] T. Boaventura et al., Modelbased hydraulic impedance control for dynamic robots," T-RO, 2015.

15

  • Execute the planned trajectories of the body and swinging leg (control tasks).

  • Keep the robot balanced.

  • Respect the un-actuated part of the dynamics. 

  • Respect the robot's actuated part of the dynamics, and the joint & torque limits.

  • Respect the contact constraints due to the interaction with the terrain.

  • Maintain contact consistency.

Whole-Body Control (WBC) Objectives

  • Cast these objectives as an optimization problem via Quadratic Programming (QP).

  • Map the solution to joint torques using inverse dynamics.

Objectives:

1. Control Tasks Tracking

2. Physical Consistency

3. Joint & Torque Limits

4. Friction Constraints

5. Unilaterality Constraints

6. Stance Task

16

WBOpt Problem in the pWBC

\text{Torque Mapping/Inverse Dynamics:~~} \tau^{*} = M_j\ddot{q}_j^* + h_j - J_{\mathrm{st},j}^T F_{\mathrm{grf}}^*

Objectives:

1. Control Tasks Tracking

2. Physical Consistency

3. Joint & Torque Limits

4. Friction Constraints

5. Unilaterality Constraints

6. Stance Task

17

Results

B. Aceituno-Cabezas et al., Simultaneous Contact, Gait and Motion Planning for Robust Multi-Legged Locomotion via Mixed-Integer Convex Optimization, RA-L 2017.
C. Mastalli et al., Trajectory and Foothold Optimization using Low-Dimensional Models for Rough Terrain Locomotion, ICRA, 2017.

18

Results

pWBC versus a quasi-static WBC from [1]

[1] M. Focchi et al., High-slope terrain locomotion for torque-controlled quadruped robots, AuRo, 2017.

19

Results

Torque Limits

Kinematic Limits

Hip Adduction-Abduction (HAA)

Hip Flexion-Extension (HFE)

Knee Flexion-Extension (KFE)

20

Conclusions: pWBC

  • The pWBC is aware of the terrain's geometry and friction.

 

  • The pWBC can achieve dynamic locomotion while compliantly balancing the robot's trunk.

 

  • The pWBC is formulated as a QP that considers:

    • The robot's full dynamics.

    • Joint and torque limits.

    • The interaction with the rigid terrain.

21

  • We showed the capabilities of the WBC under:

    • Different gaits.

    • Rigid terrain conditions.

    • Any sudden change in the torque and joint limits.

 

  • We compared the pWBC against a quasi-static WBC.

 

  • Now, is the WBC terrain aware?

 

  • Not really, not over soft terrain. 

Conclusions: pWBC

22

Published In:

 

S. Fahmi, C. Mastalli, M. Focchi, and C. Semini, “Passive Whole-Body Control for Quadruped Robots: Experimental Validation over Challenging Terrain,”

IEEE Robot. Automat. Lett. (RA-L), vol. 4, no. 3, pp. 2553–2560, Jul. 2019,

doi: 10.1109/LRA.2019.2908502.

Outline

  • Part I: Proprioceptive Terrain-Aware Locomotion

    • Passive Whole-Body Control (pWBC)

    • STANCE: Locomotion Adaption over Soft Terrain

    • On State Estimation for Regged Robots over Soft Terrain

 

  • Part II: Exteroceptive Terrain-Aware Locomotion

    • ViTAL: Vision-base Terrain-Aware Locomotion

  • Most of WBCs are not terrain-aware. They fail to generalize beyond rigid terrain.

  • Locomotion over soft terrain by itself is challenging because of the induced contact dynamics.

What is the problem?

  • Mismatch between what the WBC assumes vs. what is actually happening.

26

  • Drop the assumption of the rigid terrain. Use a more generic model that accounts for the terrain impedance.

  • But that’s not enough, how do you know the impedance parameters of the terrain. 

  • Can we, estimate it?

How to solve it?

27

sWBC

Passive WBC (pWBC) = Standard WBC (sWBC)

C3WBC

Reformulate the sWBC to account for the contact dynamics

C3 = compliant contact consistent

C3WBC

How will the new WBC know about the current terrain impedance parameters?

C3 = compliant contact consistent

?

C3WBC

Feedback the terrain impedance parameters using a Terrain Compliance Estimator (TCE)

C3 = compliant contact consistent

TCE

Simultaneously, at every control loop

C3WBC

TCE

C3WBC

TCE

Soft Terrain Adaptation aNd Compliance Estimation
(STANCE)

  • Over soft terrain, the stance feet are non-stationary.

  • The stance task does not hold anymore.

  • We use the KV model.

C3 Whole-Body Control (C3WBC)

  • C3 Stance Task

  • The stance task of the sWBC.

  • To be compliant contact consistent (C3), we need to model the soft contact dynamics. 

34

WBOpt Problem in C3WBC

35

  • Now, the WBC is C3, but it still needs the terrain impedance parameters.


  • The goal of the TCE is to estimate the terrain parameters online based on the current measurements (states), and feed them back to the C3WBC.


  • The TCE is decoupled from the WBC but uses the same contact model (KV model).

WBC Framework

36

Terrain Compliance Estimation (TCE)

We start with the contact model.

We need to estimate these parameters.

So we first need to measure or estimate these variables. 

Then, we can solve for the model's parameters using linear regression.

37

Terrain Compliance Estimation (TCE)

  • The TCE estimates the GRFs and feet penetration based on the current robot measurements. 

 

  • We used a MCS to estimate the base states.

38

Results

39

Results

40

Results

41

Results

42

  • Unlike the state of the art WBC, we do not assume that the ground is rigid.

 

  • Adapt online to any type of terrain compliance.

 

  • C3WBC

    • ​Extends the sWBC but incorporating soft contact dynamics.

    • C3WBC can adapt to the given terrain compliance.

 

  • ​TCE

    • ​Estimate the terrain compliance online.

    • Not computationally expensive.

Conclusions: STANCE

43

  • With STANCE, HyQ can traverse and transition between multiple terrains with different compliance without pre-tuning.

  • STANCE makes the locomotion strategy C3. It allowed HyQ to remain consistent with ground, and thus more robust in challenging and scenarios.

  • Each leg independently senses and adapts to changes in terrain compliance.

Conclusions: STANCE

44

STANCE's Limitations

  • The TCE estimates the GRFs and feet penetration based on the current robot measurements. 

 

  • We used a MCS to estimate the base states.

45

Published In:

 

† S. Fahmi, M. Focchi, A. Radulescu, G. Fink, V. Barasuol, and C. Semini,

“STANCE: Locomotion Adaptation over Soft Terrain,” IEEE Trans. Robot. (T-RO), vol. 36, no. 2, pp. 443–457, Apr. 2020, doi: 10.1109/TRO.2019.2954670.

† Selected as a finalist for the IEEE RAS Italian Chapter Young Author Best Paper Award 2020, and for the IEEE RAS Technical Committee on Model-Based Optimization for Robotics Best Paper Award 2020.

Outline

  • Part I: Proprioceptive Terrain-Aware Locomotion

    • Passive Whole-Body Control (pWBC)

    • STANCE: Locomotion Adaption over Soft Terrain

    • On State Estimation for Legged Robots over Soft Terrain

 

  • Part II: Exteroceptive Terrain-Aware Locomotion

    • ViTAL: Vision-base Terrain-Aware Locomotion

State Estimation for Legged Robots

  • Most of the current state estimators are based on filters that fuse multiple sensor modalities.

    • High frequency: IMU, kinematic measurements (leg odometry), etc.

    • Low frequency: cameras & lidars. 

 

  • Similar to WBCs, state estimators are shown to be reliable over stiff terrains. But over soft terrain, the performance starts to decline.

 

 

  • Experimental analysis and formal study on the effects of soft terrain on state estimation, and the reasons behind these effects.

Why?

48

State Estimator

G. Fink et al., Proprioceptive Sensor Fusion for Quadruped Robot State Estimation, IROS, 2020.

Sensors:

  • Base acceleration: IMU

  • Joint Angles: absolute & relative encoders

  • Joint Torques: load cells & one torque sensor​

Modules:

  • Attitude Observer: NLO + XKF

  • Sensor Fusion: Kalman Filter

  • Leg Odometry: GRFs + LO

49

Experimental Setup

  • Soft and Rigid Terrain Trotting Experiments.

  • Using MCS for the ground truth.

50

Results

  • Specific force  (acc. + gravity) in the gravity aligned frame

  • GRFs of the LH leg in the gravity aligned frame

  • Linear position & velocity of the base

  • High acceleration peaks over rigid terrain.

  • Smoother GRFs over soft terrain. Contact is no longer binary.

  • Larger leg odometry errors over soft terrain.

  • Larger position drift over soft terrain.

Conclusions: State Estimation over Soft Terrain

  • Over soft terrain, there is a larger error in the velocity estimates which resulted in a larger drift in position estimates.

 

  • The contact with the terrain is no longer binary: it chatters rapidly between contact and non-contact.

 

  • Many states are affected besides the robot pose, including the contact state and the GRFs.

52

Published In:

 

S. Fahmi, G. Fink, and C. Semini, “On State Estimation for Legged Locomotion over Soft Terrain,” IEEE Sensors Lett. (L-SENS), vol. 5, no. 1, pp. 1–4, Jan. 2021, doi: 10.1109/LSENS.2021.3049954.

Outline

  • Part I: Proprioceptive Terrain-Aware Locomotion

    • Passive Whole-Body Control (pWBC)

    • STANCE: Locomotion Adaption over Soft Terrain

    • On State Estimation for Regged Robots over Soft Terrain

 

  • Part II: Exteroceptive Terrain-Aware Locomotion

    • ViTAL: Vision-base Terrain-Aware Locomotion

Locomotion Planning

Two ways to deal with locomotion planning:

1. Coupled Planning:

  • Plan the feet & base states simultaneously.

2. Decoupled planning:

  • Separate the plan into feet & base plans.

  • Planning the feet motion ⇒ foothold selection

  • Planning the body motion ⇒ pose adaptation

55

  • Planning the feet motion ⇒ foothold selection

Vision-Based Foothold Adaptation (VFA)

Esteban 2020

  • Better Evaluation Criteria

  • Improved CNNs

Villarreal 2019

  • Better Evaluation Criteria

  • Self-supervised via CNNs

Barasuol 2015

  • Simple Evaluation Criteria

  • Expert demonstrations

Foothold Selection

V. Barasuol et al., Reactive Trotting with Foot Placement Corrections through Visual Pattern Classification, IROS, 2015.
O. Villarreal et al., Fast and Continuous Foothold Adaptation for Dynamic Locomotion through CNNs, RA-L, 2019.
D. Esteban et al., On the Influence of Body Velocity in Foothold Adaptations for Dynamic Legged Locomotion via CNNs, CLAWAR, 2020.

56

Goal: Select footholds based on the terrain information and the robot's capabilities. 

  • Despite the work done on vision-based strategies for foothold selection, pose adaptation strategies lag behind.

 

 

  • Pose adaptation is important because to reach a certain foothold, the robot's pose has to move simultaneously with the legs.

What is the problem?

57

Pose Adaptation

  • The goal is to find the optimal robot's pose that can:

    • Maximize the reachablity of the selected footholds.

    • Avoid terrain collision and maintain static stability.

 

  • An pose optimizer to deal with these objectives.

M. Kalakrishnan et al., Learning, planning, and control for quadruped locomotion over challenging terrain, IJRR 2011.
P. Fankhauser et al., Robust Rough-Terrain Locomotion with a Quadrupedal Robot, IROS, 2018.

Kalakrishnan 2011

Fankhauser 2018

58

Goal: Adapt the robot's pose based on the terrain information and the robot's capabilities. 

The Problem with Pose Adaptation

  • The problem with current pose adaptation strategies is that they focus on finding one optimal solution based on given selected footholds.

 

  • First, the footholds are selected.

  • Then, the pose is optimized based on that.

 

  • What if the given footholds not reached (if the robot gets disturbed)?

 

  • The robot may end up in a state with no reachable safe footholds. 

59

  • Propose a different paradigm for pose adaptation.

 

  • Instead of finding body poses that are optimal w.r.t given footholds,

 

  • we find body poses that will maximize the chances of the robot to reach safe footholds.

 

  • Put the robot in a state that if it gets disturbed, it remains around a set of solutions that are still safe & reachable.

How to Solve This Problem?

60

Vision-Based Terrain Aware Locomotion (ViTAL)

  • ViTAL is an online vision-based locomotion planning strategy.

 

  • ViTAL selects footholds based on the robot's skills

 

  • Also, find the robot pose that maximizes the chances of success in reaching these footholds.

 

  • The notion of safety & success emerges from skills that characterize the robot's capabilities.

Vision-Based Foothold Adaptation (VFA)

Vision-Based Pose Adaptation (VPA)

Foothold Evaluation Criteria (FEC)

What the robot is capable of doing?

  • Avoid edges, corners, gaps, etc.
  • Remain with the workspace of the legs.
  • Avoid colliding with the terrain.

Foothold Evaluation Criteria (FEC)

The FEC is a way to evaluate footholds given a heightmap, the leg's hip height, & other robot states.

When the criteria are evaluated, a boolean matrix is given as an output.

Each pixel in the heightmap represents the terrain height corresponding to the pixel's location in the terrain map.

The heightmap is a 2D representation of the terrain.

Each pixel is a candidate foothold for the robot.

The boolean matrix characterizes which pixel (location) in the heightmap is feasible (safe) or not based on the robot's capabilities (the criteria). 

62

Foothold Evaluation Criteria (FEC)

Rejects footholds that cause foot trajectory collision.

- Foot Trajectory Collision (FTC)

Rejects footholds that are outside the workspace during the entire gait phase.

-  Kinematic Feasibility (KF)

Rejects footholds that cause leg collision with the terrain.

-  Leg Collision (LC)

-  Terrain Roughness (TR)

Rejects footholds that are near holes, spikes, edges, and corners.

63

Vision-Based Foothold Adaptation (VFA)

64

From the VFA to the VPA

VFA

VPA

65

Vision-Based Pose Adaptation (VPA)

First, we extract heightmaps that are centered around the projection of the leg hip location.

66

Vision-Based Pose Adaptation (VPA)

Then, the pose evaluation stage will evaluate the criteria for all the samples of the hip heights. 

The pose evaluation stage will output samples of number of safe footholds.

67

Vision-Based Pose Adaptation (VPA)

The output of the function approximation is

\hat{\mathcal{F}}_l

These samples are then used to approximate a continuous function of the number of safe footholds at the function approximation stage. 

68

Vision-Based Pose Adaptation (VPA)

Finally, the pose optimization stage finds the optimal pose that maximizes            for all of the legs

\hat{\mathcal{F}}_l

69

Pose Optimization

Find the optimal robot's trunk pose that maximizes the number of safe footholds for all the legs

Height

Roll

Pitch

70

Pose Optimization

71

Results: Detailed Explanation

hind hips rising up

hind hips lowering down

front hips rising up

front hips lowering down

72

Baseline: Terrain-Based Reference (TBR)

O. Villarreal et al., MPC-based Controller with Terrain Insight for Dynamic Legged Locomotion, ICRA, 2020.

73

Results: Comparison with the baseline

74

Results: Comparison with another approach

75

Results: Performance with Different Velocities

76

Preliminary Experiments

78

Preliminary Experiments

78

Conclusions: ViTAL

  • We presented ViTAL, a decoupled visual planning strategy

 

  • ViTAL consisted of :

  • VFA for foothold selection

  • VPA for pose adaptation

    • Introduced a different paradigm in pose adaptation

    • Finds the body poses that maximize the chances of the legs to succeed in reaching safe footholds

  • FEC (notion of safety) that characterizes the robot's capabilities

79

Conclusion: ViTAL

  • ViTAL allows HyQ and HyQReal to traverse a wide variety of terrains under various forward velocities.

    • These terrains included stairs, gaps, etc.

    • Forward velocities varied from 0.2m/s to 0.75 m/s.

 

  • We showed that the VPA resulted in body poses that are aware of the terrain, and of what the robot and its legs can do.

 

  • The VPA puts the robot in a pose that provides the feet with higher number of safe footholds which allows the robot to succeed unlike the baseline that fails.

80

To be Published:

 

S. Fahmi, D. Esteban, O. Villarreal, C. Semini, and V. Barasuol, “ViTAL: Terrain-Aware Locomotion Planning for Legged Robots,”  May. 2021.

Outline

  • Part I: Proprioceptive Terrain-Aware Locomotion

    • Passive Whole-Body Control (pWBC)

    • STANCE: Locomotion Adaption over Soft Terrain

    • On State Estimation for Regged Robots over Soft Terrain

 

  • Part II: Exteroceptive Terrain-Aware Locomotion

    • ViTAL: Vision-base Terrain-Aware Locomotion

Let legged robots take decisions based on the surrounding terrain using their perception.

Passive Whole-Body Control (pWBC)

STANCE: Locomotion Adaption over Soft Terrain

On State Estimation for Legged Robots over Soft Terrain

ViTAL: Vision-based Terrain-Aware Locomotion

Perception

  • Control

  • State Estimation

  • Planning

Terrain

  • Proprioception

  • Vision

Decisions

  • Physical Properties

    • Friction

    • Impedance

  • Geometry

Using Optimization and Machine Learning

Future Work

  • Whole-Body Control

    • Relies on inverse dynamics, can we learn these dynamics or contact interaction

 

  • Locomotion over Soft Terrain

    • STANCE made the WBC c3, can we have a c3 MPC-based controller?

    • Improving the state estimator, contact dynamics in legged odometry?

 

  • Vision-based Terrain Aware Locomotion

    • Experiments

    • Analysis of the CNNs

84

List of Publications

  • S. Fahmi, D. Esteban, O. Villarreal, C. Semini, and V. Barasuol, "ViTAL: Vision-based Terrain-Aware Locomotion Planning for Legged Robots," to be published, May 2021.

 

  • S. Fahmi, G. Fink, and C. Semini, "On state estimation for legged locomotion over soft terrain," IEEE Sensors Letters (L-SENS), vol. 5, no. 1, pp. 1-4, Jan. 2021.

 

  • D. Esteban, O. Villarreal, S. Fahmi, C. Semini, and V. Barasuol, "On the influence of body velocity in foothold adaptation for dynamic legged locomotion via cnns," in Proc.  International Conference on Climbing and Walking Robots (CLAWAR), Moscow, Russia, Aug. 2020, pp. 353-360.

 

  • † S. Fahmi, M. Focchi, A. Radulescu, G. Fink, V. Barasuol, and C. Semini, "STANCE: Locomotion adaptation over soft terrain," IEEE Transactions on Robotics (T-RO), vol. 36, no. 2, pp. 443-457, Apr. 2020.

 

  • C. Semini, V. Barasuol, M. Focchi, C. Boelens, M. Emara, S. Casella, O. Villarreal, R. Orsolino, G. Fink, S. Fahmi, G. Medrano-Cerda, and D. G. Caldwell, "Brief introduction to the quadruped robot HyQReal," in Italian Conference on Robotics and Intelligent Machines (I-RIM), Rome, Oct. 2019, pp. 1-2.

 

  • S. Fahmi, C. Mastalli, M. Focchi, and C. Semini, "Passive whole-body control for quadruped robots: Experimental validation over challenging terrain," IEEE Robotics and Automation  Letters (RA-L), vol. 4, no. 3, pp. 2553-2560, Jul. 2019.

† This work has been selected as a finalist for the IEEE RAS Italian Chapter Young Author Best Paper Award 2020, and for the IEEE RAS Technical Committee on Model-Based Optimization for Robotics Best Paper Award 2020.

85

On Terrain Aware Locomotion for 

Legged Robots

- SHAMEL FAHMI -

On Terrain Aware Locomotion

for Legged Robots

Shamel Fahmi

Doctoral Thesis Defense

April, 2021

Made with Slides.com