面向楼梯场景的四足机器人运动规划与控制研究

与传统的轮式移动机器人相比，模仿自然界哺乳动物的四足机器人拥有优越 的地形适应能力，有望在货物运输、工厂巡检、安全巡逻、搜救以及家居服务方面 发挥重要作用，越来越受到学术界和工业界的青睐。为了实现这些应用场景，机 器人在复杂环境的移动能力至关重要。楼梯是复杂环境中的一个重要场景，基于 楼梯在实际应用场景中的普遍性和重要性，本文将整个问题缩小为研究四足机器 人在楼梯场景下的运动规划与控制器设计问题。具体包含以下两个方面的内容：

对于现有成熟的，采用串联驱动机构的点足式（point-feet，机器人的脚与地面 的接触方式为点接触）四足机器人：本文开发了一套视觉辅助爬楼梯框架。在这 个框架里，我们假设楼梯的尺寸是均一的，即楼梯的长宽高保持一致。基于这一 假设，我们的方法可以容易地实现机器人的定位和地形描述。同时，我们根据机 器人尺寸和楼梯信息开发了合适的前进速度以及落脚点规划方法，并结合模型预 测控制（model predictive control，MPC）技术实现四足自主动态爬楼梯效果。仿真 和实物实验验证了我们方法的有效性。

对于一种采用复杂并联机构驱动的新型四足机器人：本文开发了一种有效的 动力学建模与控制方法。我们的方法可以有效地建模复杂并联机构带来的影响，基 于所分析的动力学模型，我们开发了基于 QP（Quadratic Programming，二次规划）的 TSID（Task space inverse dynamics，任务空间逆动力学）WBC（whole-body control， 全身控制）控制器。考虑到模型的复杂性，同时为了满足实际控制的实时性要求， 我们进一步提出了一种简化的控制器。我们所提出的控制成功在 MuJoCo 仿真环 境实现了机器人的多步态动态行走，并且达到一定到抗冲击性能，在给定楼梯的 信息下，还能很好的实现上下楼梯，验证了我们控制器的有效性和鲁棒性。

With superior terrain adaptation capabilities compared to traditional wheeled mobile robots, quadrupedal robot that mimic natural mammals are expected to play an important role in package delivery, factory inspections, security patrols, search and rescue, and home services, and are increasingly favored by academia and industry. In order to realize these application scenarios, the robot’s ability to move in complex environments is crucial. This thesis investigates the problem of motion planning and controller design for quadruped robots in complex environments. Two specific aspects are included.

For existing mature, point-feet quadruped robots that driven by serial mechanism, this thesis develop a simple but effective vision-aided framework for autonomous stair climbing. In this framework, we assume that the dimensions of the stairs are homogeneous, i.e., the length, width, and height of the stairs remain the same. Based on this assumption, our approach can easily implement robot localization and terrain description. At the same time, we develop suitable forward speed as well as landing point planning methods based on robot dimensions and staircase information, and use popular model predictive control techniques to achieve quadrupedal autonomous dynamic stair climbing effects. Simulations and physical experiments validate the effectiveness of our method.

For a novel quadruped robot driven by complex parallel mechanism, this thesis develops an effective dynamics modeling and controller design method. Based on the analyzed dynamics model, we developed a QP-based (quadratic programming) TSID whole-body controller (WBC). Considering the complexity of the model, we further propose a simplified controller in order to meet the real time requirements in practice. Our proposed control successfully achieves several dynamic locomotion behaviors in the MuJoCo simulation environment, the robot is able to crawl, trot and recovery from a push and upstairs, which verifies the effectiveness and robustness of our proposed controllers.

[1] HIROSE S, KUNIEDA O. Generalized standard foot trajectory for a quadruped walking vehicle [J]. The International Journal of Robotics Research, 1991, 10(1): 3-12.

[2] KIMURA H, FUKUOKA Y, COHEN A H. Adaptive dynamic walking of a quadruped robot on natural ground based on biological concepts[J]. The International Journal of Robotics Research, 2007, 26(5): 475-490.

[3] BLEDT G, POWELL M J, KATZ B, et al. MIT Cheetah 3: Design and control of a robust, dynamic quadruped robot[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018: 2245-2252.

[4] LIU Y, WENSING P M, SCHMIEDELER J P, et al. Terrain-Blind Humanoid Walking Based on a 3-D Actuated Dual-SLIP Model[J]. IEEE Robotics and Automation Letters, 2016, 1(2): 1073-1080.

[5] KROTKOV E, SIMMONS R. Perception, planning, and control for autonomous walking with the ambler planetary rover[J]. The International journal of robotics research, 1996, 15(2): 155180.

[6] WOODEN D, MALCHANO M, BLANKESPOOR K, et al. Autonomous navigation for BigDog[C]//2010 IEEE international conference on robotics and automation. Ieee, 2010: 47364741.

[7] REBULA J R, NEUHAUS P D, BONNLANDER B V, et al. A controller for the littledog quadruped walking on rough terrain[C]//Proceedings 2007 IEEE International Conference on Robotics and Automation. IEEE, 2007: 1467-1473.

[8] KOLTER J Z, RODGERS M P, NG A Y. A control architecture for quadruped locomotion over rough terrain[C]//2008 IEEE International Conference on Robotics and Automation. 2008: 811-818.

[9] VERNAZA P, LIKHACHEV M, BHATTACHARYA S, et al. Search-based planning for a legged robot over rough terrain[C]//2009 IEEE International Conference on Robotics and Automation. 2009: 2380-2387.

[10] ZUCKER M, RATLIFF N, STOLLE M, et al. Optimization and learning for rough terrain legged locomotion[J]. The International Journal of Robotics Research, 2011, 30(2): 175-191.

[11] WINKLER A W, MASTALLI C, HAVOUTIS I, et al. Planning and execution of dynamic whole-body locomotion for a hydraulic quadruped on challenging terrain[C]//2015 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2015: 5148-5154.

[12] MASTALLI C, WINKLER A, HAVOUTIS I, et al. On-line and On-board Planning and Perception for Quadrupedal Locomotion[C]//IEEE International Conference on Technologies for Practical Robot Applications (TEPRA). 2015.

[13] FANKHAUSER P, BJELONIC M, BELLICOSO C D, et al. Robust Rough-Terrain Locomotion with a Quadrupedal Robot[C]//2018 IEEE International Conference on Robotics and Automation (ICRA). 2018: 5761-5768.

[14] JENELTEN F, MIKI T, VIJAYAN A E, et al. Perceptive Locomotion in Rough Terrain–Online Foothold Optimization[J]. IEEE Robotics and Automation Letters, 2020, 5(4): 5370-5376.

[15] Mastalli C, Havoutis I, Focchi M, et al. Motion Planning for Quadrupedal Locomotion: Coupled Planning, Terrain Mapping, and Whole-Body Control[J]. IEEE Transactions on Robotics, 2020: 1-14.

[16] KIM D, CARBALLO D, CARLO J D, et al. Vision Aided Dynamic Exploration of Unstructured Terrain with a Small-Scale Quadruped Robot[C]//2020 IEEE International Conference on Robotics and Automation (ICRA). 2020: 2464-2470.

[17] KALAKRISHNAN M, BUCHLI J, PASTOR P, et al. Learning locomotion over rough terrain using terrain templates[C]//2009 IEEE/RSJ International Conference on Intelligent Robots and Systems. 2009: 167-172.

[18] MAGAñA O A V, BARASUOL V, CAMURRI M, et al. Fast and Continuous Foothold Adaptation for Dynamic Locomotion Through CNNs[J]. IEEE Robotics and Automation Letters, 2019, 4(2): 2140-2147.

[19] VILLARREAL O, BARASUOL V, WENSING P M, et al. MPC-based Controller with Terrain Insight for Dynamic Legged Locomotion[C]//2020 IEEE International Conference on Robotics and Automation (ICRA). 2020: 2436-2442.

[20] DEITS R, TEDRAKE R. Footstep planning on uneven terrain with mixed-integer convex optimization[C]//2014 IEEE-RAS international conference on humanoid robots. IEEE, 2014: 279-286.

[21] ACEITUNO-CABEZAS B, DAI H, CAPPELLETTO J, et al. A mixed-integer convex optimization framework for robust multilegged robot locomotion planning over challenging terrain [C]//2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017: 4467-4472.

[22] ACEITUNO-CABEZAS B, MASTALLI C, DAI H, et al. Simultaneous contact, gait, and motion planning for robust multilegged locomotion via mixed-integer convex optimization[J]. IEEE Robotics and Automation Letters, 2017, 3(3): 2531-2538.

[23] GRIFFIN R J, WIEDEBACH G, MCCRORY S, et al. Footstep planning for autonomous walking over rough terrain[C]//2019 IEEE-RAS 19th international conference on humanoid robots (humanoids). IEEE, 2019: 9-16.

[24] SHAO X, YANG Y, WANG W. Obstacle crossing with stereo vision for a quadruped robot[C]// 2012 IEEE International Conference on Mechatronics and Automation. IEEE, 2012: 17381743.

[25] 张慧. 四足机器人环境感知、识别与领航员跟随算法研究[D]. 山东大学, 2016.

[26] 李兴东. 基于 TOF 相机的四足机器人地形感知及静步态规划研究[D]. 哈尔滨工业大学.

[27] LEE Y H, LEE Y H, LEE H, et al. Whole-Body Motion and Landing Force Control for Quadrupedal Stair Climbing[C]//2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 2019: 4746-4751.

[28] MIKI T, LEE J, HWANGBO J, et al. Learning robust perceptive locomotion for quadrupedal robots in the wild[J]. Science Robotics, 2022, 7(62): eabk2822.

[29] ”MINITAUR”. https://spectrum.ieee.org/ghost-robotics-minitaur-quadruped[J]. 2016.

[30] KLEMM V, MORRA A, SALZMANN C, et al. Ascento: A two-wheeled jumping robot[C]// 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019: 7515-7521.

[31] ”CASSIE”. https://spectrum.ieee.org/building-robots-that-can-go-where-we-go[J]. 2019.

[32] FEATHERSTONE R. Rigid body dynamics algorithms[M]. Springer, 2014.

[33] KLEMM V, MORRA A, GULICH L, et al. LQR-assisted whole-body control of a wheeled bipedal robot with kinematic loops[J]. IEEE Robotics and Automation Letters, 2020, 5(2): 3745-3752.

[34] REHER J, AMES A D. Inverse dynamics control of compliant hybrid zero dynamic walking [C]//2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021: 2040-2047.

[35] CARPENTIER J, BUDHIRAJA R, MANSARD N. Proximal and sparse resolution of constrained dynamic equations[C]//Robotics: Science and Systems 2021. 2021.

[36] DEL PRETE A, MANSARD N. Robustness to joint-torque-tracking errors in task-space inverse dynamics[J]. IEEE transactions on Robotics, 2016, 32(5): 1091-1105.

[37] WENSING P M, ORIN D E. Generation of dynamic humanoid behaviors through task-space control with conic optimization[C]//2013 IEEE International Conference on Robotics and Automation. IEEE, 2013: 3103-3109.

[38] SADEGHIAN H, VILLANI L, KESHMIRI M, et al. Task-space control of robot manipulators with null-space compliance[J]. IEEE Transactions on Robotics, 2013, 30(2): 493-506.

[39] SAAB L, RAMOS O E, KEITH F, et al. Dynamic whole-body motion generation under rigid contacts and other unilateral constraints[J]. IEEE Transactions on Robotics, 2013, 29(2): 346362.

[40] CARLO J D, WENSING P M, KATZ B, et al. Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control[C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 2018: 1-9.

[41] BOUSSEMA C, POWELL M J, BLEDT G, et al. Online gait transitions and disturbance recovery for legged robots via the feasible impulse set[J]. IEEE Robotics and automation letters, 2019, 4(2): 1611-1618.

[42] POSA M, CANTU C, TEDRAKE R. A direct method for trajectory optimization of rigid bodies through contact[J]. The International Journal of Robotics Research, 2014, 33(1): 69-81.

[43] NEUNERT M, FARSHIDIAN F, WINKLER A W, et al. Trajectory optimization through contacts and automatic gait discovery for quadrupeds[J]. IEEE Robotics and Automation Letters, 2017, 2(3): 1502-1509.

[44] NAVEAU M, KUDRUSS M, STASSE O, et al. A Reactive Walking Pattern Generator Based on Nonlinear Model Predictive Control[J]. IEEE Robotics and Automation Letters, 2017, 2(1): 10-17.

[45] NEUNERT M, STÄUBLE M, GIFTTHALER M, et al. Whole-body nonlinear model predictive control through contacts for quadrupeds[J]. IEEE Robotics and Automation Letters, 2018, 3(3): 1458-1465.

[46] ORIN D E, GOSWAMI A, LEE S H. Centroidal dynamics of a humanoid robot[J]. Autonomous robots, 2013, 35(2): 161-176.

[47] DAI H, VALENZUELA A, TEDRAKE R. Whole-body motion planning with centroidal dynamics and full kinematics[C]//2014 IEEE-RAS International Conference on Humanoid Robots. IEEE, 2014: 295-302.

[48] HERZOG A, ROTELLA N, SCHAAL S, et al. Trajectory generation for multi-contact momentum control[C]//2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids). 2015: 874-880.

[49] KUINDERSMA S, DEITS R, FALLON M, et al. Optimization-based locomotion planning, estimation, and control design for the atlas humanoid robot[J]. Autonomous robots, 2016, 40(3): 429-455.

[50] KAJITA S, KANEHIRO F, KANEKO K, et al. Biped walking pattern generation by using preview control of zero-moment point[C]//2003 IEEE International Conference on Robotics and Automation (Cat. No. 03CH37422): volume 2. IEEE, 2003: 1620-1626.

[51] KALAKRISHNAN M, BUCHLI J, PASTOR P, et al. Fast, robust quadruped locomotion over challenging terrain[C]//2010 IEEE International Conference on Robotics and Automation. 2010: 2665-2670.

[52] DARIO BELLICOSO C, JENELTEN F, FANKHAUSER P, et al. Dynamic locomotion and whole-body control for quadrupedal robots[C]//2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 2017: 3359-3365.

[53] FARSHIDIAN F, JELAVIC E, SATAPATHY A, et al. Real-time motion planning of legged robots: A model predictive control approach[C]//2017 IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids). IEEE, 2017: 577-584.

[54] BLEDT G, WENSING P M, KIM S. Policy-regularized model predictive control to stabilize diverse quadrupedal gaits for the MIT cheetah[C]//2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 2017: 4102-4109.

[55] WINKLER A W, BELLICOSO C D, HUTTER M, et al. Gait and Trajectory Optimization for Legged Systems Through Phase-Based End-Effector Parameterization[J]. IEEE Robotics and Automation Letters, 2018, 3(3): 1560-1567.

[56] BLEDT G, KIM S. Implementing Regularized Predictive Control for Simultaneous Real-Time Footstep and Ground Reaction Force Optimization[C]//2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 2019: 6316-6323.

[57] CORBèRES T, FLAYOLS T, LéZIART P A, et al. Comparison of predictive controllers for locomotion and balance recovery of quadruped robots[C]//2021 IEEE International Conference on Robotics and Automation (ICRA). 2021: 5021-5027.

[58] BJELONIC M, GRANDIA R, HARLEY O, et al. Whole-Body MPC and Online Gait Sequence Generation for Wheeled-Legged Robots[C]//2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). 2021: 8388-8395.

[59] KIM D, DI CARLO J, KATZ B, et al. Highly dynamic quadruped locomotion via whole-body impulse control and model predictive control[J]. arXiv preprint arXiv:1909.06586, 2019.

[60] SENTIS L, KHATIB O. A whole-body control framework for humanoids operating in human environments[C]//Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006. IEEE, 2006: 2641-2648.

[61] BELLICOSO C D, GEHRING C, HWANGBO J, et al. Perception-less terrain adaptation through whole body control and hierarchical optimization[C]//2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids). IEEE, 2016: 558-564.

[62] BJELONIC M, BELLICOSO C D, DE VIRAGH Y, et al. Keep Rollin’—Whole-Body Motion Control and Planning for Wheeled Quadrupedal Robots[J]. IEEE Robotics and Automation Letters, 2019, 4(2): 2116-2123.

[63] SLEIMAN J P, FARSHIDIAN F, MINNITI M V, et al. A Unified MPC Framework for WholeBody Dynamic Locomotion and Manipulation[J]. IEEE Robotics and Automation Letters, 2021, 6(3): 4688-4695.

[64] HOLZ D, HOLZER S, RUSU R B, et al. Real-Time Plane Segmentation Using RGB-D Cameras [C]//RoboCup 2011: Robot Soccer World Cup XV. 2012.

[65] Flayols T, Del Prete A, Wensing P, et al. Experimental evaluation of simple estimators for humanoid robots[C]//2017 IEEE-RAS 17th International Conference on Humanoid Robotics (Humanoids). 2017: 889-895.

[66] BLOESCH M, HUTTER M, HOEPFLINGER M A, et al. State estimation for legged robotsconsistent fusion of leg kinematics and IMU[J]. Robotics, 2013, 17: 17-24.

[67] ROTELLA N, BLOESCH M, RIGHETTI L, et al. State estimation for a humanoid robot[C]// 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2014: 952-958.

[68] BENALLEGUE M, LAMIRAUX F. Estimation and stabilization of humanoid flexibility deformation using only inertial measurement units and contact information[J]. International Journal of Humanoid Robotics, 2015, 12(03): 1550025.

[69] HARTLEY R, GHAFFARI M, EUSTICE R M, et al. Contact-aided invariant extended Kalman filtering for robot state estimation[J]. The International Journal of Robotics Research, 2020, 39(4): 402-430.

[70] UNITREE. https://github.com/unitreerobotics/unitree_ros/tree/master/robots/aliengo_descript ion[J].

[71] TODOROV E, EREZ T, TASSA Y. MuJoCo: A physics engine for model-based control[C]// 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. 2012: 5026-5033.

[72] RUSU R B, COUSINS S. 3D is here: Point Cloud Library (PCL)[C]//IEEE International Conference on Robotics and Automation (ICRA). Shanghai, China, 2011.

[73] LYNCH K M, PARK F C. Modern robotics[M]. Cambridge University Press, 2017.

[74] CARPENTIER J, SAUREL G, BUONDONNO G, et al. The Pinocchio C++ library : A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives [C]//2019 IEEE/SICE International Symposium on System Integration (SII). 2019: 614-619.

[75] RAIBERT M H. Legged robots that balance[M]. MIT press, 1986.