Technische Universit¨atM¨unchen Fakult¨atf¨urInformatik Lehrstuhl f¨urSensorbasierte Robotersysteme und Intelligente Assistenzsysteme Whole-Body Impedance Control of Wheeled Humanoid Robots Dipl.-Ing. (Univ.) Alexander Markus Dietrich Vollst¨andigerAbdruck der von der Fakult¨atf¨urInformatik der Technischen Universit¨atM¨unchen zur Erlangung des akademischen Grades eines Doktor-Ingenieurs (Dr.-Ing.) genehmigten Dissertation. Vorsitzende(r): Univ.-Prof. Dr.-Ing. habil. Alois Knoll Pr¨uferder Dissertation: 1. Univ.-Prof. Dr.-Ing. Alin Albu-Sch¨affer 2. Univ.-Prof. Dr.-Ing. habil. Boris Lohmann Diese Dissertation wurde am 28.1.2015 bei der Technischen Univer- sit¨atM¨unchen eingereicht und durch die Fakult¨atf¨urInformatik am 11.8.2015 angenommen. Preface This dissertation is based on research undertaken at the Institute of Robotics and Mecha- tronics of the German Aerospace Center (DLR) in Oberpfaffenhofen, Germany. It took five years (2010-2015) to accumulate the results which are reported this thesis. Nevertheless, it would have been impossible to finish the work without the help of others. Fortunately, I was blessed along the way. I would like to express my deep gratitude to my supervisor and mentor Prof. Alin Albu- Sch¨afferfor his guidance and the inspiring discussions we had throughout the course of this work. Furthermore, my special thanks go to Dr. Christian Ott, who supported me and introduced me to the exciting field of stability theory in robotics. Moreover, I wish to thank Daniel Leidner and Dr. Thomas Wimb¨ock, with whom I collaborated in a very productive way resulting in several valuable publications in the field of whole-body control. Without the excellent and continuous maintenance of the robot software and hardware by Florian Schmidt, Robert Burger, and Werner Friedl, the numerous experiments in this thesis would not have been possible at all. Moreover, I would like to thank my colleagues Dr. Florian Petit, Dominic Lakatos, Andreas Stemmer, and my former students Melanie Kimmel and Kristin Bussmann for the fruitful discussions and their support. My gratitude also goes to Prof. Gerd Hirzinger, who gave me the opportunity to work at the DLR and use the remarkable robotic systems for my research. Furthermore, I would like to thank Dr. Paul Kotyczka and Prof. Boris Lohmann for the great cooperation between DLR and TUM, which I am very glad to continue in the future. Special thanks go to my colleagues Jens Reinecke and Dr. Maxime Chalon, who sup- ported me in so many ways and proofread this thesis. Thanks and love to my parents Christine and Rainer, and my sister Kerstin, who have always encouraged me and helped me to find my way to become a scientist. Last, I thank my beloved wife Ann-Kristin. Without her patience and love, this work would have never been completed. Munich, November 2015 Alexander Markus Dietrich 3 Abstract The robotics research of the last years has created an increasing number of mobile hu- manoid robots. They can be employed in a great diversity of applications such as service robotics, the cooperation with humans in industry, or the autonomous operation in haz- ardous places where humans would be in danger. All of these use cases involve dynamic, unpredictable, and partially unstructured environments, where physical contacts are in- evitable and actually necessary for the task completion. The high requirements on the humanoid robots urge the designers to develop suitable whole-body control techniques in order to properly operate the systems. This thesis contributes to the field of whole-body control of mobile humanoid robots, focusing on the skills for soft contact interactions. New reactive methods in several crucial subdomains of this active research field are developed such as self-collision avoidance, singularity-free control of wheeled mobile platforms, or the efficient use of the robotic torso to increase the overall workspace of the system. The work addresses the interconnection of all these stand-alone methods. For this purpose, a hierarchy is established, so that the robot will execute the most important tasks with higher priority than the minor ones. The concept of hierarchy-based control is thoroughly investigated, and the classical state-of-the-art method for the task prioritization is extended by new features such as dynamic task hierarchies and the treatment of conflicting objectives. A fundamental requirement in robotics is stability, both theoretically proven and experimentally verified. The formal stability analysis for multi-task hierarchies developed in this thesis is the first one that demonstrates overall stability on the complete robot taking the various simultaneous control goals into account. The proof of stability is valid for a generic torque-controlled robot and it is furthermore extended to the particular case of systems with velocity-controlled wheeled platforms. All algorithms reported in this thesis are experimentally validated on a mobile humanoid robot. The work gives an outlook to the prospective use of the proposed controller as an essential component in an integrated framework. By interconnecting the low-level whole-body controller with a higher-level artificial intelligence, the high potential of the proposed approach for complex real-world applications becomes obvious. Several typical service robot tasks, such as autonomously wiping a window or sweeping the floor with a broom, are successfully performed on the robot. 5 Contents 1. Introduction 19 1.1. Motivation.................................... 19 1.2. Related Work................................... 21 1.3. Problem Statement................................ 22 1.4. Concept of Whole-Body Impedance....................... 23 1.5. Contributions and Overview........................... 25 2. Fundamentals 29 2.1. Robot Kinematics and Dynamics........................ 29 2.1.1. Forward Kinematics, Jacobian Matrices, and Power Ports...... 29 2.1.2. Derivation of the Equations of Motion................. 30 2.1.3. Rigid Body Dynamics.......................... 31 2.2. Compliant Motion Control of Robotic Systems................ 31 2.2.1. Impedance Control............................ 32 2.2.2. Admittance Control........................... 33 2.3. Humanoid Robot Rollin' Justin......................... 34 2.3.1. Design and Hardware.......................... 34 2.3.2. Modeling Assumptions.......................... 36 3. Control Tasks based on Artificial Potential Fields 37 3.1. Self-Collision Avoidance............................. 38 3.1.1. Geometric Collision Model....................... 39 3.1.2. Repulsive Potential............................ 40 3.1.3. Damping Design............................. 42 3.1.4. Control Design.............................. 44 3.1.5. Experiments............................... 45 3.2. Singularity Avoidance for Nonholonomic, Wheeled Platforms........ 46 3.2.1. Instantaneous Center of Rotation.................... 47 3.2.2. Controllability and Repulsion...................... 48 3.2.3. Effect on the Instantaneous Center of Rotation............ 49 3.2.4. Effect on the Wheel........................... 50 3.2.5. Control Design.............................. 51 3.2.6. Simulations and Experiments...................... 51 7 Contents 3.3. Posture Control for Kinematically Coupled Torso Structures......... 55 3.3.1. Model of the Torso of Rollin' Justin.................. 55 3.3.2. Kinematic Constraints.......................... 56 3.3.3. Dynamic Constraints........................... 56 3.3.4. Control Design.............................. 59 3.3.5. Experiments............................... 60 3.4. Classical Objectives in Reactive Control.................... 60 3.4.1. Cartesian Impedance........................... 61 3.4.2. Manipulator Singularity Avoidance................... 61 3.4.3. Avoidance of Mechanical End Stops.................. 61 3.5. Summary..................................... 62 4. Redundancy Resolution by Null Space Projections 63 4.1. Strictness of the Hierarchy............................ 64 4.1.1. Successive Projections.......................... 64 4.1.2. Augmented Projections......................... 65 4.2. Consistency of the Projections......................... 65 4.2.1. Static Consistency............................ 66 4.2.2. Dynamic Consistency.......................... 67 4.2.3. Stiffness Consistency........................... 70 4.3. Comparison of Null Space Projectors...................... 71 4.3.1. Simulations................................ 71 4.3.2. Experiments............................... 75 4.3.3. Discussion................................. 80 4.4. Unilateral Constraints in the Task Hierarchy................. 83 4.4.1. Basics................................... 84 4.4.2. Ensuring Continuity........................... 85 4.4.3. Simulations................................ 89 4.4.4. Experiments............................... 90 4.4.5. Discussion................................. 93 4.5. Summary..................................... 94 5. Stability Analysis 103 5.1. Whole-Body Impedance with Kinematically Controlled Platform...... 103 5.1.1. Subsystems................................ 104 5.1.2. Control Design.............................. 108 5.1.3. Proof of Stability............................. 109 5.1.4. Experiments............................... 111 5.1.5. Discussion................................. 115 5.2. Multi-Objective Compliance Control...................... 118 5.2.1. Problem Formulation.......................... 119 5.2.2. Hierarchical Dynamics Representation................. 121 5.2.3.