Footholds optimization for legged robots walking on complex terrain

This paper proposes a novel continuous footholds optimization method for legged robots to expand their walking ability on complex terrains. The algorithm can efficiently run onboard and online by using terrain perception information to protect the robot against slipping or tripping on the edge of obstacles, and to improve its stability and safety when walking on complex terrain. By relying on the depth camera installed on the robot and obtaining the terrain heightmap, the algorithm converts the discrete grid heightmap into a continuous costmap. Then, it constructs an optimization function combined with the robot’s state information to select the next footholds and generate the motion trajectory to control the robot’s locomotion. Compared with most existing footholds selection algorithms that rely on discrete enumeration search, as far as we know, the proposed algorithm is the first to use a continuous optimization method. We successfully implemented the algorithm on a hexapod robot, and verified its feasibility in a walking experiment on a complex terrain.

BCS:

Body coordinate system

CBC:

Centroid balance control

CI:

Convolution interpolation

CNN:

Convolutional neural network

CoM:

Center of mass

GRF:

Ground reaction force

IMU:

Inertial measurement unit

LCS:

Leg coordinate system

NDCI:

Normalized derivable convolution interpolation

PCI:

Pyramid convolution interpolation

RGBD:

Red, green, blue, depth

SLIP:

Spring-loaded inverted pendulum

WCS:

World coordinate system

A :

Merged matrix for CBC

b, f :

Merged vectors for CBC

c _i,j :

Center of the pixel (i, j) on the map

C :

Matrix of friction cone and upper and lower bound constraints for GRF

E :

Identity matrix

f _i :

GRF of the ith leg

^L f ^* _i :

Virtual feedforward force of the ith leg in the LCS

f _lim :

Friction cone and upper and lower bound constraints for GRF

f ^*_st :

Virtual force of the supporting legs

f ^*_st,i :

Virtual force of the ith supporting leg

^L f ^*_st,i , ^L f ^*_sw,i :

Virtual feedforward forces of the ith supporting leg and swing leg in the LCS, respectivley

g :

Local gravitational acceleration

h _z :

Step height that needs to be raised in the middle of the swing process

h :

Merged vector of h_i

h * :

Horizontal component of optimized footholds

h _i :

Horizontal component of p_i

h ^* _i :

Optimized horizontal coordinate of the foothold

h _pre :

Merged vector of hpre,i

h _pre,i :

Horizontal component of preplanned foothold

Δh :

Intermediate variable of the difference between the optimized footholds and the preplanned footholds

Δh _lim :

Maximum amount of adjustment Δp_i allowed

_nΔh :

nth element of Δh

i :

Index for the item, such as the ith leg, or the (i, j) pixel

B _I :

Inertia matrix of the body in the BCS

j :

Index for the item, such as the (i, j) pixel

J _i :

Jacobian matrix of the ith leg

J (h _i):

Terrain cost at the position of h_i

J (p _h):

Cost function for NDCI

∇ J(p _h):

Partial derivative of the cost function for NDCI

K _D,j, K _P,j :

Joint damping and stiffness matrices, respectively

K _D,p, K _P,p :

Damping and stiffness of body position, respectively

K _Da, K _Pa :

Damping and stiffness of body attitude, respectively

^L K _Dj, ^L K _P :

Damping and stiffness of the leg, respectively

^B l _i :

Tip position in the BCS

\(^{\rm{L}}{{\boldsymbol{l}}_i}{,^{\rm{L}}}{{{\boldsymbol{\dot l}}}_i}\) :

Position and velocity of the ith leg in the LCS, respectively

\(^{\rm{L}}{\boldsymbol{l}}_i^ \ast {,^{\rm{L}}}{\boldsymbol{\dot l}}_i^ \ast \) :

Target position and velocity of the ith leg in the LCS, respectively

m, n :

Set numbers of pixels that need to be involved

p _b :

Position of the robot body

\({\boldsymbol{p}}_{\rm{b}}^ \ast,\,\,{\boldsymbol{\dot p}}_{\rm{b}}^ \ast,\,\,{\boldsymbol{\ddot p}}_{\rm{b}}^ \ast \) :

Target position, velocity, and acceleration of the robot’s CoM, respectively

p _h :

Horizontal coordinate of the position of the selected point on the whole map

p _i :

Foothold point of the th leg

p _i,x, p _i,y, p _i,z :

x, y, and z components of the foothold p_i, respectively

p ^* _i :

Optimized footholds

p ^*_iz :

Height components at the position p ^*_i on the heightmap

p _pre,i :

Preplanning of the target foothold

p _ξi :

Swing trajectory of the th leg

\({{{\boldsymbol{\hat p}}}_{\rm{b}}},\,\,{{{\boldsymbol{\hat \dot p}}}_{\rm{b}}}\) :

Estimated position and velocity of the robot, respectively

Δp ^*_b :

Correction amount on the original body trajectory

Δp _i :

Foothold adjustment for the ith leg

^B p _hip,i :

Position of the ith leg’s hip joint in the BCS

\({{\boldsymbol{q}}_i},\,{{{\boldsymbol{\dot q}}}_i}\) :

Current angle and angular velocity read by the ith encoder, respectively

\({\boldsymbol{q}}_i^ \ast,\,{\boldsymbol{\dot q}}_i^ \ast \) :

Target angle and angular velocity of the ith joint, respectively

R :

Current attitude matrix

R*:

Desired attitude of the body

\({{\boldsymbol{\hat R}}}\) :

Estimated current attitude of the robot

ℝ:

Set of real number

S :

Selection matrix for CBC

t _sw,i :

Time when the ith leg enters the swing phase

T _st, T _sw :

Duration of the standing and swing phase, respectively

v :

Velocity of the body

v _d :

Desired velocity of the body

Δv ^*_b :

Speed correction

x _h :

x component of the horizontal coordinate of the position of the selected point on the heightmap

x _i,j :

x component of the center of the pixel (i, j) on the map

y _h :

y component of the horizontal coordinate of the position of the selected point on the heightmap

y _i,j :

y component of the center of the pixel (i, j) on the map

W, D, Q :

Weighting matrices for footholds optimization

α, β, γ :

Weighting factors for footholds optimization

δ :

Weighting factor for CBC

σ _x, σ _y :

Smoothing factor in x and y directions, respectively

ϕ θ, ψ :

Euler angle roll, pitch, and yaw measured by the IMU

μ :

Number of legs in contact

τ ^*_i :

Control torque of the ith joint

ξ _i :

Swing phase

\({{{\boldsymbol{\hat \omega}}}_{\rm{b}}}\) :

Angular velocity of the body

\({\boldsymbol{\omega}}_{\rm{b}}^ \ast,\,\,{\boldsymbol{\dot \omega}}_{\rm{b}}^ \ast \) :

Target angular velocity and acceleration of the robot, respectively

*:

Symbol maps from a vector (ℝ³) to an antisymmetric matrix (ℝ^3×3)

This work was supported by the National Key R&D Program of China (Grant No. 2021YFF0306202).

Authors and Affiliations

1. State Key Laboratory of Mechanical System and Vibration, School of Mechanical Engineering, Shanghai Jiao Tong University, Shanghai, 200240, China

   Yunpeng Yin, Yuguang Xiao & Feng Gao

2. AI Institute, School of Electronic Information and Electrical Engineering, Shanghai Jiao Tong University, Shanghai, 200240, China

   Yue Zhao

Corresponding author

Correspondence to Feng Gao.

Reprints and permissions