Abstract
The quality of visual feedback can vary significantly on a legged robot meant to traverse unknown and unstructured terrains. The map of the environment, acquired with online state-of-the-art algorithms, often degrades after a few steps, due to sensing inaccuracies, slippage and unexpected disturbances. If a locomotion algorithm is not designed to deal with this degradation, its planned trajectories might end-up to be inconsistent in reality. In this work, we propose a heuristic-based planning approach that enables a quadruped robot to successfully traverse a significantly rough terrain (e.g. stones up to 10 cm of diameter), in absence of visual feedback. When available, the approach allows also to leverage the visual feedback (e.g. to enhance the stepping strategy) in multiple ways, according to the quality of the 3D map. The proposed framework also includes reflexes, triggered in specific situations, and the possibility to estimate online an unknown time-varying disturbance and compensate for it. We demonstrate the effectiveness of the approach with experiments performed on our quadruped robot HyQ (85 kg), traversing different terrains, such as: ramps, rocks, bricks, pallets and stairs. We also demonstrate the capability to estimate and compensate for external disturbances by showing the robot walking up a ramp while pulling a cart attached to its back.
This is a preview of subscription content, log in via an institution to check access.
Access this chapter
Tax calculation will be finalised at checkout
Purchases are for personal use only
Similar content being viewed by others
Notes
- 1.
See accompanying video of rough terrain experiments: https://youtu.be/_7ud4zIt-Gw.
- 2.
LH, LF, RH and RF stands for Left-Hind, Left-Front, Right-Hind and Right-Front legs, respectively.
- 3.
Without any loss of generality, the same controller can be implemented at the foot level. However, to avoid non collocation problems due to leg compliance, it is safer to close the loop at the joint level.
- 4.
This is a reasonable choice because the robot is unlikely to reach singularity (i.e. 90\(^{\circ }\) pitch).
- 5.
Belonging to the same side of the body.
- 6.
We found experimentally that using the desired feet position instead of the actual one to compute the CoM target would make the robot’s height gradually decrease.
- 7.
Statically stable gaits are convenient for locomotion in dangerous environments (e.g. nuclear decommissioning missions) because the motion can be stopped at any time. However, without loss of generality, the final velocity can be set to any other arbitrary value other than zero.
- 8.
Note that we performed a simple inversion since in our robot we have point feet and 3 Degree of Freedom (DoF) per leg, thus the Jacobian matrix is squared.
- 9.
The horizontal frame \(\mathcal {H}\) is the reference frame that shares the same origin and yaw value with the base frame but is aligned (in pitch and roll) to the world frame, hence horizontal (see Fig. 4 (left)).
- 10.
As a matter of fact, if the support polygon shrinks, the robustness decreases. In particular, CoM tracking errors and external pushes can move the ZMP very close to the boundary of the support polygon. This would result in a situation where not all the contact feet are “pushing” on the ground (e.g. the robot starts tipping about the line connecting two feet).
- 11.
We call the swing plane the plane passing though the X-Z axes of the swing frame.
- 12.
The swing frame, in general, is aligned with the terrain frame unless a vision based stepping strategy is used. In this case, the swing frame is computed independently for each foot (as explained in Sect. 4.2) from the visual input.
- 13.
Being the map expressed in a (fixed) world frame, it is necessary to apply appropriate kinematic conversions to this frame before evaluating the map.
- 14.
Step reflex video: https://www.youtube.com/watch?v=_7ud4zIt-Gw&t=2m33s.
- 15.
Shin coll. video: https://www.youtube.com/watch?v=_7ud4zIt-Gw&t=4m04s.
- 16.
For a complete description of the sensor setup, see [41], Chap. 3.
- 17.
Rough terrain experiments: https://www.youtube.com/watch?v=_7ud4zIt-Gw&t=0m10s.
- 18.
Note that the normal vector [a, b, 1] should be normalized for the computation of \(\phi _t, \theta _t\).
- 19.
Terrain estimation video: https://www.youtube.com/watch?v=_7ud4zIt-Gw&t=5m01s.
- 20.
We adopt a “telescopic strut” strategy as in [45], which means that, on an inclined terrain, the vector between each stance foot and the corresponding hip aiming to be maintained parallel to gravity. On one hand, this improves the margin for a static equilibrium. On the other hand, for high stair inclinations, it can result in bigger joint motions, where the kinematic limits are most likely hit.
- 21.
As the one shown in Fig. 2 (left), where the joints are in the middle of their range of motion.
- 22.
This sequence is the one animals employ that reduces the backward motions [47].
- 23.
Stair climb video: https://www.youtube.com/watch?v=_7ud4zIt-Gw&t=7m13s.
- 24.
Henceforth, for simplicity, we talk about coordinate vectors and not spatial vectors, that is why \(W_{ext} \in \mathbb {R}^6\) rather than \(W_{ext} \in F^6\) [49].
- 25.
The twist (i.e. 6D spatial velocity) is usually computed by a state estimator, which merges Inertial Measurement Unit (imu), encoder and force/torque sensors [19].
- 26.
- 27.
Note that only gravity and the external wrench have an influence on \(\varDelta x_{{com}_{x,y}}\), because the resultant of the GRF passes through the ZMP point, by definition.
- 28.
Note that computing this equations as \(\varDelta x_{com}= [mg + f_{ext}]_{\times }\tau _{ext}\) where \([\cdot ]_{\times } \) is the skew symmetric operator associated to the cross product, returns inaccurate results because \([\cdot ]_{\times }\) is rank deficient.
- 29.
MBDO simulations: https://www.youtube.com/watch?v=_7ud4zIt-Gw&t=9m15s.
- 30.
MBDO experiments: https://www.youtube.com/watch?v=_7ud4zIt-Gw&t=10m27s.
- 31.
The geodesic distance is the length of the shortest curve lying on the sphere connecting the two points.
References
Bretl, T., Lall, S.: Testing static equilibrium for legged robots. IEEE Trans. Robot. 24, 794–807 (2008)
Pardo, D., Möller, L., Neunert, M., Winkler, A.W., Buchli, J.: Evaluating direct transcription and nonlinear optimization methods for robot motion planning. IEEE Robot. Autom. Lett. 1(2), 946–953 (2016)
Mastalli, C., Focchi, M., Havoutis, I., Radulescu, A., Calinon, S., Buchli, J., Caldwell, D.G., Semini, D.G.: Trajectory and foothold optimization using low-dimensional models for rough terrain locomotion. In: IEEE International Conference on Robotics and Automation (ICRA) (2017)
Winkler, A.W., Bellicoso, C.D., Hutter, M., Buchli, J.: Gait and trajectory optimization for legged systems through phase-based end-effector parameterization. IEEE Robot. Autom. Lett. 1–1 (2018)
Aceituno-Cabezas, B., Mastalli, C., Hongkai, D., Focchi, M., Radulescu, A., Caldwell, D.G., Cappelletto, J., Grieco, J.C., Fernando-Lopez, G., Semini, C.: Simultaneous contact, gait and motion planning for robust multi-legged locomotion via mixed-integer convex optimization. IEEE Robot. Autom. Lett. 1–8 (2018)
Mastalli, C., Winkler, A.W., Havoutis, I., Caldwell, D. G., Semini, C.: On-line and on-board planning and perception for quadrupedal locomotion. In: IEEE International Conference on Technologies for Practical Robot Applications (TEPRA) (2015)
Dai, H., Tedrake, R.: Planning robust walking motion on uneven terrain via convex optimization. In: 2016 IEEE-RAS International Conference on Humanoid Robots (2016)
Ponton, B., Herzog, A., Del Prete, A., Schaal, S., Righetti, L.: On Time Optimisation of Centroidal Momentum Dynamics. arXiv:1709.09265 (2017)
Stephens, B.J.: State estimation for force-controlled humanoid balance using simple models in the presence of modeling error. In: 2011 IEEE International Conference on Robotics and Automation, pp. 3994–3999 (2011)
Englsberger, J., Mesesan, G., Ott, C.: Smooth trajectory generation and push-recovery based on divergent component of motion. In: IEEE International Conference on Intelligent Robots and Systems (IROS), pp. 4560–4567 (2017)
Rotella, N.: Estimation-based control for humanoid robots. Ph.D. thesis, University of Southern California (USC) (2018)
Bledt, G., Wensing, P.M., Kim, S.: Policy-regularized model predictive control to stabilize diverse quadrupedal gaits for the MIT cheetah. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4102–4109 (2017)
Wieber, P-B.: Trajectory free linear model predictive control for stable walking in the presence of strong perturbations trajectory free linear model predictive control for stable walking in the presence of strong perturbations. In: IEEE-RAS International Conference on Humanoid Robots (2006)
Park, H-W., Wensing, P., Kim, S.: Online planning for autonomous running jumps over obstacles in high-speed quadrupeds. In: Robotics: Science and Systems XI (2015)
Bellicoso, C.D., Gehring, C., Hwangbo, J., Fankhauser, P., Hutter, M.: Perception-less terrain adaptation through whole body control and hierarchical optimization. In: 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids), pp. 558–564 (2016)
Bellicoso, C.D., Jenelten, F., Fankhauser, P., Gehring, C., Hwangbo, J., Hutter, M.: Dynamic locomotion and whole-body control for quadrupedal robots. In: IEEE/RSJ Intenational Conference on Intelligent Robots and Systems (IROS), pp. 3359–3365 (2017)
Fankhauser, P., Bjelonic, M., Bellicoso, C.D., Miki, T., Hutter, M.: Robust rough-terrain locomotion with a quadrupedal robot. In: IEEE International Conference on Robotics and Automation (ICRA) (2018)
Fallon, M., Marion, P., Deits, R., Whelan, T., Antone, M., McDonald, J., Tedrake R.: Continuous humanoid locomotion over uneven terrain using stereo fusion. In: 2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids), pp. 881–888 (2015)
Camurri, M., Fallon, M., Bazeille, S., Radulescu, A., Barasuol, V., Caldwell, D.G., Semini, C.: Probabilistic contact estimation and impact detection for state estimation of quadruped robots. IEEE Robot. Autom. Lett. 2, 1023–1030 (2017)
Bloesch, M., Hutter, M., Hoepflinger, M.H., Gehring, C., David Remy, C., Siegwart, R.: State estimation for legged robots: consistent fusion of leg kinematics and IMU. MIT Press J. 8, 17–24 (2013)
Xinjilefu, X., Feng, S., Huang, W., Atkeson, C.G.: Decoupled state estimation for humanoids using full-body dynamics. In: 2014 IEEE International Conference on Robotics and Automation (ICRA), pp. 195–201 (2014)
Nobili, S., Camurri, M., Barasuol, V., Focchi, M., Caldwell, D., Semini, C., Fallon, M.: Heterogeneous sensor fusion for accurate state estimation of dynamic legged robots. In: Robotics: Science and Systems XIII (2017)
Barasuol, V., Buchli, J., Semini, C., Frigerio, M., De Pieri, E.R., Caldwell, D.G.: A reactive controller framework for quadrupedal locomotion on challenging terrain. In: Proceedings—IEEE International Conference on Robotics and Automation, pp. 2554–2561 (2013)
Focchi, M., Barasuol, V., Frigerio, M., Caldwell, D.G., Semini, C.: Slip Detection and Recovery for Quadruped Robots. Springer Proceedings in Advanced Robotics, vol. 3, pp. 185–199. Cham (2018)
Barasuol, V., Camurri, M., Bazeille, S., Caldwell, D., Semini, C.: Reactive trotting with foot placement corrections through visual pattern classification. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (2015)
Belter, D., Skrzypczyński, P.: Rough terrain mapping and classification for foothold selection in a walking robot. J. Field Robot. 28(4), 497–528 (2011)
Mastalli, C., Havoutis, I., Focchi, M., Caldwell, D.G., Semini, C.: Motion planning for quadrupedal locomotion: coupled planning, terrain mapping and whole-body control. HAL id: hal-01673438v2 (2018)
Focchi, M., Del Prete, A., Havoutis, I., Featherstone, R., Caldwell, D.G., Semini, C.: High-slope terrain locomotion for torque-controlled quadruped robots. Auton. Robots 41(1), 259–272 (2017)
Focchi, M., Featherstone, R., Orsolino, R., Caldwell, D.G., Semini, C.: Viscosity-based height reflex for workspace augmentation for quadrupedal locomotion on rough terrain. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (2017)
Focchi, M., Barasuol, V., Havoutis, I., Buchli, J., Semini, C., Caldwell, D.G.: Local reflex generation for obstacle negotiation in quadrupedal locomotion. In: International Conference on Climbing and Walking Robots (CLAWAR) (2013)
Semini, C., Tsagarakis, N., Guglielmino, E., Focchi, M., Cannella, F., Caldwel, D.: Design of HyQ—a hydraulically and electrically actuated quadruped robot. J. Syst. Control Eng. (2011)
Frigerio, M., Buchli, J., Caldwell, D.G., Semini, C.: RobCoGen: a code generator for efficient kinematics and dynamics of articulated robots, based on Domain Specific Languages 7(1), 36–54 (2016)
Tar, A.S., Cserey, G.G., Veres, J.: Patent N. WO 2013072712 A1, May 2014
Gao, Y., Semini, C., Focchi, M., Caldwell, D.G.: Patent N. IT 102018000002407, Feb 2018
Winkler, A., Mastalli, C., Havoutis, I., Focchi, M., Caldwell, D.G., Semini, C.: Planning and execution of dynamic whole-body locomotion for a hydraulic quadruped on challenging terrain. In: IEEE International Conference on Robotics and Automation (ICRA) (2015)
Boaventura, T., Buchli, J., Semini, C., Caldwell, D.G.: Model-based hydraulic impedance control for dynamic robots. IEEE Trans. Robot. 31, 1324–1336 (2015)
Orsolino, R., Focchi, M., Mastalli, C., Dai, H., Caldwell, D. G., Semini, C.: Application of wrench based feasibility analysis to the online trajectory optimization of legged robots. IEEE Robot. Autom. Lett. (RA-L) (2018)
Audren, H., Kheddar, A.: 3D robust stability polyhedron in multicontact. Submitted to IEEE Trans. Robot. (TRO) (2017)
Alexander, R.M.: Principles of Animal Locomotion. Princeton University Press (2003)
Semini, C., Tsagarakis, N.G., Guglielmino, E., Focchi, M., Cannella, F., Caldwell, D.G.: Design of HyQ—a hydraulically and electrically actuated quadruped robot. IMechE Part I: J. Syst. Control Eng. 225(6), 831–849 (2011)
Camurri, M.: Multisensory State Estimation and Mapping on Dynamic Quadruperd Robots. Ph.D. thesis, Istituto Italiano di Tecnologia (IIT) and Universiy of Genoa. http://iit-dlslab.github.io/papers/camurri17phd.pdf (2017)
Fankhauser, P., Hutter, M.: a universal grid map library: implementation and use case for rough terrain navigation. In: Koubaa, A. (ed.) Robot Operating System (ROS)—The Complete Reference, vol. 1, ch. 5. Springer (2016)
Semini, C., Barasuol, V., Goldsmith, J., Frigerio, M., Focchi, M., Gao, Y., Caldwell, D.G.: Design of the hydraulically-actuated, torque-controlled quadruped robot HyQ2Max. IEEE/ASME Trans. Mechatron. PP(99), 1–1 (2016)
Gramkow, C.: On averaging rotations. J. Math. Imaging Vis. 15(1–2), 7–16 (2001)
Gehring, C., Coros, S., Hutler, M., Dario Bellicoso, C., Heijnen, H., Diethelm, R., Bloesch, M., Fankhauser, P., Hwangbo, J., Hoepflinger, M., Siegwart, R.: Practice makes perfect: an optimization-based approach to controlling agile motions for a quadruped robot. IEEE Robot. Autom. Mag. 23(1), 34–43 (2016)
Oßwald, S., Gutmann, J.S., Hornung, A., Bennewitz, M.: From 3D point clouds to climbing stairs: a comparison of plane segmentation approaches for humanoids. In: 2011 11th IEEE-RAS International Conference on Humanoid Robots, pp. 93–98 (2011)
Pongas, D., Mistry, M., Schaal, S.: A robust quadruped walking gait for traversing rough terrain. In: Proceedings, 2007 IEEE International Conference on Robotics and Automation, pp. 1474–1479 (2007)
Tournois, G., Focchi, M., Del Prete, A., Orsolino, R., Caldwell, D.G., Semini, C.: Online payload identification for quadruped robots. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (2017)
Featherstone, R.: Rigid Body Dynamics Algorithms. Springer US, Boston, MA (2008)
Orin, D.E., Goswami, A., Lee, S.-H.: Centroidal dynamics of a humanoid robot. Auton. Robots 35, 161–176 (2013)
Popovic, M.B., Goswami, A., Herr, H.: Ground reference points in legged locomotion: definitions, biological trajectories and control implications. Int. J. Robot. Res. 24(12), 1013–1032 (2005)
Acknowledgements
This work was supported by Istituto Italiano di Tecnologia (IIT), with additional funding from the European Union’s Seventh Framework Programme for research, technological development and demonstration under grant agreement no. 601116 as part of the ECHORD++ (The European Coordination Hub for Open Robotics Development) project under the experiment called HyQ-REAL.
Author information
Authors and Affiliations
Corresponding author
Editor information
Editors and Affiliations
Appendices
Appendix A
This appendix shows how to compute the weighted average of N orientation vectors.
Because finite rotations do not sum up as vectors do, it is not possible to apply ordinary laws of vector arithmetic to them. Specifically, the task of averaging orientations (described in the Algorithm 1 written in pseudo-code) is equivalent to average points on a sphere, where the line is replaced with a spherical geodesic (arc of a circle).Footnote 31
The iterative procedure illustrated above is generic and can be used to obtain the average of N directional vectors \(n_k\), even though at each iteration we are only able to directly compute the average of two. After the initialization, at each loop the actual average \(\bar{n}\) is updated with the next normal \(n_k\). To do so, we first compute the angle \(\theta _k\) between \(n_k\) and the actual average \(\bar{n}\). Then we scale this angle, according to the (accumulated) weight of \(\bar{n}\). Finally, \(n_k\) is rotated by the scaled angle \(\hat{\theta }\) toward \(\bar{n}\) and the result is assigned back to \(\bar{n}\).
Appendix B
List of the main symbols used throughout the paper:
Symbol | Description |
\(x_{com} \in \mathbb {R}^3\) | coordinates of the CoM of the robot |
\(x_{fi}, \dot{x}_{fi} \in \mathbb {R}^3\) | positions and velocities of the ith foot |
\(f_i \in \mathbb {R}^{3}\) | contact forces of the ith stance foot |
n | number of active joints of the robot |
\(q^d_j, \dot{q}^d_j \in \mathbb {R}^n\) | joint reference positions and velocities |
\(\tau _{ff}^d \in \mathbb {R}^n\) | feedforward torque command |
\(\tau _{pd}^d \in \mathbb {R}^n\) | impedance torque command |
\(\tau ^d \in \mathbb {R}^n\) | total reference torque command |
\(\phi \) | roll angle of the robot’s trunk |
\(\theta \) | pitch angle of the robot’s trunk |
\(\psi \) | yaw angle of the robot’s trunk |
\(\varPhi = [\phi , \theta ,\psi ]\) | actual orient. of the robot’s trunk |
\(\varPhi ^d = [\phi ^{d}, \theta ^{d},\psi ^{d}]\) | desired orient. of the robot’s trunk |
\(\varPhi ^d(0) = \varPhi \) | des. orient. at the start of the move base |
\(\varPhi ^{tg}\) | target orient. at the end of the move base |
\(x_{com}^{tg}\) | target CoM position of the trunk |
\(x_{{com}_p}^{tg}\) | projection of \(x_{com}^{tg}\) on the terrain plane |
\(h_r\) | robot’s height |
\(f_{ext}, \tau _{ext}\) | external disturbance force and torque |
Rights and permissions
Copyright information
© 2020 Springer Nature Switzerland AG
About this chapter
Cite this chapter
Focchi, M. et al. (2020). Heuristic Planning for Rough Terrain Locomotion in Presence of External Disturbances and Variable Perception Quality. In: Grau, A., Morel, Y., Puig-Pey, A., Cecchi, F. (eds) Advances in Robotics Research: From Lab to Market. Springer Tracts in Advanced Robotics, vol 132. Springer, Cham. https://doi.org/10.1007/978-3-030-22327-4_9
Download citation
DOI: https://doi.org/10.1007/978-3-030-22327-4_9
Published:
Publisher Name: Springer, Cham
Print ISBN: 978-3-030-22326-7
Online ISBN: 978-3-030-22327-4
eBook Packages: Intelligent Technologies and RoboticsIntelligent Technologies and Robotics (R0)