Footholds optimization for legged robots walking on complex terrain

Abstract

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.

Graphical abstract

Keywords

footholds optimization / legged robot / complex terrain adapting / hexapod robot / locomotion control

References

[1] Raibert M H, Tello E R. Legged robots that balance. IEEE Expert, 1986, 1(4): 89

[2] BledtG, Powell M J, KatzB, Di CarloJ, Wensing P M, KimS. MIT Cheetah 3: design and control of a robust, dynamic quadruped robot. In: Proceedings of 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems. Madrid: IEEE, 2018

[3] VernazaP, Likhachev M, BhattacharyaS, ChittaS, Kushleyev A, Lee D D. Search-based planning for a legged robot over rough terrain. In: Proceedings of 2009 IEEE International Conference on Robotics & Automation. Kobe: IEEE, 2009, 2380–2387

[4] KalakrishnanM, BuchliJ, PastorP, MistryM, Schaal S. Fast, robust quadruped locomotion over challenging terrain. In: Proceedings of 2010 IEEE International Conference on Robotics & Automation. Anchorage: IEEE, 2010, 2665–2670

[5] Semini C, Tsagarakis N G, Guglielmino E, Focchi M, Cannella F, Caldwell D G. Design of HyQ—a hydraulically and electrically actuated quadruped robot. Proceedings of the Institution of Mechanical Engineers, Part I: Journal of Systems and Control Engineering, 2011, 225(6): 831–849

[6] WinklerA, Havoutis I, BazeilleS, OrtizJ, Focchi M, Dillmann R, Caldwell D, SeminiC. Path planning with force-based foothold adaptation and virtual model control for torque controlled quadruped robots. In: Proceedings of 2014 IEEE International Conference on Robotics & Automation (ICRA). Hong Kong: IEEE, 2014, 6476–6482

[7] HutterM, Gehring C, JudD, LauberA, Bellicoso C D, TsounisV, HwangboJ, BodieK, FankhauserP, Bloesch M, DiethelmR, BachmannS, MelzerA, HoepflingerM. ANYmal—a highly mobile and dynamic quadrupedal robot. In: Proceedings of 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Daejeon: IEEE, 2016, 38–44

[8] Winkler A W, Bellicoso C D, Hutter M, Buchli J. Gait and trajectory optimization for legged systems through phase-based end-effector parameterization. IEEE Robotics and Automation Letters, 2018, 3(3): 1560–1567

[9] MastalliC, Focchi M, HavoutisI, RadulescuA, Calinon S, Buchli J, Caldwell D G, SeminiC. Trajectory and foothold optimization using low-dimensional models for rough terrain locomotion. In: Proceedings of 2017 IEEE International Conference on Robotics & Automation (ICRA). Singapore: IEEE, 2017, 1096–1103

[10] Mastalli C, Havoutis I, Focchi M, Caldwell D G, Semini C. Motion planning for quadrupedal locomotion: coupled planning, terrain mapping and whole-body control. IEEE Transactions on Robotics, 2020, 36(6): 1635–1648

[11] MastalliC, Havoutis I, WinklerA W, CaldwellD G, SeminiC. On-line and on-board planning and perception for quadrupedal locomotion. In: Proceedings of 2015 IEEE International Conference on Technologies for Practical Robot Applications (TePRA). Woburn: IEEE, 2019, 1–7

[12] FankhauserP, Bjelonic M, BellicosoC D, MikiT, HutterM. Robust rough-terrain locomotion with a quadrupedal robot. In: Proceedings of 2018 IEEE International Conference on Robotics and Automation (ICRA). Brisbane: IEEE, 2018, 5761–5768

[13] Jenelten F, Miki T, Vijayan A E, Bjelonic M, Hutter M. Perceptive locomotion in rough terrain—online foothold optimization. IEEE Robotics and Automation Letters, 2020, 5(4): 5370–5376

[14] ChaiX, Gao F, XuY L. Perception-based gait planning for a hexapod robot walking on typical structured terrain. In: Zhang X M, Wang N F, Huang Y J, eds. Mechanism and Machine Science. Singapore: Springer, 2017, 169–181

[15] KimD, Carballo D, Di CarloJ, KatzB, Bledt G, Lim B, Kim S. Vision aided dynamic exploration of unstructured terrain with a small-scale quadruped robot. In: Proceedings of 2020 IEEE International Conference on Robotics and Automation (ICRA). Paris: IEEE, 2020, 2464–2470

[16] Mao L H, Tian Y, Gao F, Zhao Y. Novel method of gait switching in six-legged robot walking on continuous-nondifferentiable terrain by utilizing stability and interference criteria. Science China Technological Sciences, 2020, 63(12): 2527–2540

[17] Mao L H, Gao F, Tian Y, Zhao Y. Novel method for preventing shin-collisions in six-legged robots by utilising a robot–terrain interference model. Mechanism and Machine Theory, 2020, 151: 103897

[18] ZhaoY, Gao F, YinY P. Obstacle avoidance and terrain identification for a hexapod robot. Research Square, 2020, preprint

[19] Magaña O A V, Barasuol V, Camurri M, Franceschi L, Focchi M, Pontil M, Caldwell D G, Semini C. Fast and continuous foothold adaptation for dynamic locomotion through CNNs. IEEE Robotics and Automation Letters, 2019, 4(2): 2140–2147

[20] Miki T, Lee J, Hwangbo J, Wellhausen L, Koltun V, Hutter M. Learning robust perceptive locomotion for quadrupedal robots in the wild. Science Robotics, 2022, 7(62): eabk2822

[21] Tsounis V, Alge M, Lee J, Farshidian F, Hutter M. DeepGait: planning and control of quadrupedal gaits using deep reinforcement learning. IEEE Robotics and Automation Letters, 2020, 5(2): 3699–3706

[22] GehringC, Fankhauser P, IslerL, DiethelmR, Bachmann S, PotzM, GerstenbergL, HutterM. ANYmal in the field: solving industrial inspection of an offshore HVDC platform with a quadrupedal robot. In: Ishigami G, Yoshida K, eds. Field and Service Robotics. Singapore: Springer, 2021, 247–260

[23] LiuW, GaoY, GaoF, LiS Y. Trajectory adaptation and safety control via control barrier functions for legged robots. In: Proceedings of 2021 China Automation Congress (CAC). Beijing: IEEE, 2021, 5571–5576

[24] Park H W, Wensing P M, Kim S. Jumping over obstacles with MIT Cheetah 2. Robotics and Autonomous Systems, 2021, 136: 103703

[25] QiS H, Lin W C, HongZ J, ChenH, ZhangW. Perceptive autonomous stair climbing for quadrupedal robots. In: Proceedings of 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Prague: IEEE, 2021, 2313–2320

[26] GerumP. Xenomai—Implementing a RTOS emulation framework on GNU/Linux. White Paper, 2004, 81

[27] HintjensP. ZeroMQ: Messaging for Many Applications. O’Reilly Media, 2013

[28] Sun Q, Gao F, Chen X B. Towards dynamic alternating tripod trotting of a pony-sized hexapod robot for disaster rescuing based on multi-modal impedance control. Robotica, 2018, 36(7): 1048–1076

[29] EkstromM P. Digital Image Processing Techniques. 2nd ed. Academic Press, 2012

[30] Ranganathan A.. The Levenberg‒Marquardt algorithm. Tutoral on LM Algorithm, 2004, 11(1): 101–110

[31] BlöschMHutterMHöpflinger M ALeuteneggerSGehringCRemyC D SiegwartR Y. State estimation for legged robots—consistent fusion of leg kinematics and IMU. In: Proceedings of Robotics: Science and Systems Conference (RSS 2012). Sidney: Robotics: Science and Systems Conference, 2012

[32] Focchi M, Del Prete A, Havoutis I, Featherstone R, Caldwell D G, Semini C. High-slope terrain locomotion for torque-controlled quadruped robots. Autonomous Robots, 2017, 41(1): 259–272

Nomenclature

Abbreviations
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
Variables
A	Merged matrix for CBC
b, f	Merged vectors for CBC
ci, 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
fi	GRF of the ith leg
{}_{}{}^{\mathrm{L}}\mathit{f}{}_i^{\ast }	Virtual feedforward force of the ith leg in the LCS
flim	Friction cone and upper and lower bound constraints for GRF
{\mathit{f}}_{\mathrm{s}\mathrm{t}}^{\ast }	Virtual force of the supporting legs
{\mathit{f}}_{\mathrm{s}\mathrm{t},i}^{\ast }	Virtual force of the ith supporting leg
{}_{}^{}L{\mathit{f}}_{\mathrm{s}\mathrm{t},i}^{\ast }, {}_{}^{}\mathrm{L}{\mathit{f}}_{\mathrm{s}\mathrm{w},i}^{\ast }	Virtual feedforward forces of the ith supporting leg and swing leg in the LCS, respectivley
g	Local gravitational acceleration
hz	Step height that needs to be raised in the middle of the swing process
h	Merged vector of hi
h*	Horizontal component of optimized footholds
hi	Horizontal component of pi
{\mathit{h}}_i^{\ast }	Optimized horizontal coordinate of the foothold
hpre	Merged vector of hpre,i
hpre,i	Horizontal component of preplanned foothold
Δh	Intermediate variable of the difference between the optimized footholds and the preplanned footholds
Δhlim	Maximum amount of adjustment Δpi allowed
nΔh	nth element of Δh
i	Index for the item, such as the ith leg, or the (i, j) pixel
BI	Inertia matrix of the body in the BCS
j	Index for the item, such as the (i, j) pixel
Ji	Jacobian matrix of the ith leg
J (hi)	Terrain cost at the position of hi
J (ph)	Cost function for NDCI
\mathrm{\nabla }J\left({\mathit{p}}_{\mathrm{h}}\right)	Partial derivative of the cost function for NDCI
{\mathit{K}}_{\mathrm{D},\mathrm{j}}, {\mathit{K}}_{\mathrm{P},\mathrm{j}}	Joint damping and stiffness matrices, respectively
{\mathit{K}}_{\mathrm{D},\mathrm{p}}, {\mathit{K}}_{\mathrm{P},\mathrm{p}}	Damping and stiffness of body position, respectively
{\mathit{K}}_{\mathrm{D},\mathrm{a}}, {\mathit{K}}_{\mathrm{P},\mathrm{a}}	Damping and stiffness of body attitude, respectively
{}_{}^{}\mathrm{L}{\mathit{K}}_{\mathrm{D}}, {}_{}^{}\mathrm{L} {\mathit{K}}_{\mathrm{P}}	Damping and stiffness of the leg, respectively
Bli	Tip position in the BCS
{}_{}^{}\mathrm{L}{\mathit{l}}_i, {}_{}^{}\mathrm{L}{\dot{\mathit{l}}}_i	Position and velocity of the ith leg in the LCS, respectively
{}_{}^{}\mathrm{L}{\mathit{l}}_i^{\ast }, {}_{}^{}\mathrm{L} {\dot{\mathit{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
{\mathit{p}}_{\mathrm{b}}	Position of the robot body
{\mathit{p}}_{\mathrm{b}}^{\ast }, {\dot{\mathit{p}}}_{\mathrm{b}}^{\ast }, {\ddot{\mathit{p}}}_{\mathrm{b}}^{\ast }	Target position, velocity, and acceleration of the robot’s CoM, respectively
ph	Horizontal coordinate of the position of the selected point on the whole map
{\mathit{p}}_i	Foothold point of the ith leg
p_{i,x}, p_{i,y}, p_{i,\mathit{\text{z}}}	x, y, and z components of the foothold {\mathit{p}}_i, respectively
{\mathit{p}}_i^{\ast }	Optimized footholds
{\mathit{p}}_{i,\mathit{\text{z}}}^{\ast }	Height components at the position {\mathit{p}}_i^{\ast } on the heightmap
{\mathit{p}}_{\mathrm{p}\mathrm{r}\mathrm{e},i}	Preplanning of the target foothold
{\mathit{p}}_{{\xi }_i}	Swing trajectory of the ith leg
{\hat{\mathit{p}}}_{\mathrm{b}}, {\widehat{\dot{\mathit{p}}}}_{\mathrm{b}}	Estimated position and velocity of the robot, respectively
\mathrm{\Delta }{\mathit{p}}_{\mathrm{b}}^{\ast }	Correction amount on the original body trajectory
Δpi	Foothold adjustment for the ith leg
Bphip,i	Position of the ith leg’s hip joint in the BCS
{\mathit{q}}_i, {\dot{\mathit{q}}}_i	Current angle and angular velocity read by the ith encoder, respectively
{\mathit{q}}_i^{\ast }, {\dot{\mathit{q}}}_i^{\ast }	Target angle and angular velocity of the ith joint, respectively
R	Current attitude matrix
{\mathit{R}}^{\ast }	Desired attitude of the body
\hat{\mathit{R}}	Estimated current attitude of the robot
\mathbb{R}	Set of real number
S	Selection matrix for CBC
tsw,i	Time when the ith leg enters the swing phase
Tst, Tsw	Duration of the standing and swing phase, respectively
v	Velocity of the body
vd	Desired velocity of the body
\mathrm{\Delta }{\mathit{v}}_{\mathrm{b}}^{\ast }	Speed correction
xh	x component of the horizontal coordinate of the position of the selected point on the heightmap
xi,j	x component of the center of the pixel (i, j) on the map
yh	y component of the horizontal coordinate of the position of the selected point on the heightmap
yi,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
{\mathit{\tau }}_i^{\ast }	Control torque of the ith joint
ξi	Swing phase
{\widehat{\mathbf{\text{ω}}}}_{\mathrm{b}}	Angular velocity of the body
{\text{ω}}_{\mathrm{b}}^{\ast }, {\dot{\text{ω}}}_{\mathrm{b}}^{\ast }	Target angular velocity and acceleration of the robot, respectively
*	Symbol maps from a vector ({\mathbb{R}}^3) to an antisymmetric matrix ({\mathbb{R}}^{3\times 3})

Acknowledgements

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

RIGHTS & PERMISSIONS

2023 Higher Education Press