Simulation, Collision Avoidance and Angular Momentum Tracking

171 Pages·2014·3.57 MB·English

by  Markus Schwienbacher

FakultätfürMaschinenwesen
LehrstuhlfürAngewandteMechanik
Efficient Algorithms for Biped Robots
Simulation,CollisionAvoidanceandAngularMomentumTracking
Dipl.-Ing.Univ.MarkusSchwienbacher
VollständigerAbdruckdervonderFakultätfürMaschinenwesender
TechnischenUniversitätMünchenzurErlangungdesakademischenGradeseines
Doktors-Ingenieurs(Dr.-Ing.)
genehmigtenDissertation.
Vorsitzender:Univ.-Prof.dr.ir.DanielRixen
PrüferderDissertation:
1. Univ.-Prof.Dr.-Ing.habil.HeinzUlbrich(i.R.)
2. Univ.-Prof.Dr.-Ing.CarloL.Bottasso
DieDissertationwurdeam24.10.2013beiderTechnischenUniversitätMünchen
eingereichtunddurchdieFakultätfürMaschinenwesenam22.5.2014angenommen.

iii
Abstract
This PhD thesis covers efficient algorithms for biped robots. A multibody simulation
extends the (n)-algorithm in order to cope with small kinematic loops inherent
O
in the modeling of mechatronic systems. Self-collision avoidance is based on the
efficient computation of distances between segments of the robot. Angular momentum
is typically not considered in the control of biped robots. A method for collision
avoidance and angular momentum tracking is presented which improves fast walking.
Simulations and experiments are shown with the biped robot Lola.
Keywords: Humanoid, Biped, Walking, Efficient Algorithms, Multibody Simulation,
Self-Collision Avoidance, Angular Momentum Tracking
Zusammenfassung
Thema der Arbeit sind effiziente Algorithmen für zweibeinige Roboter. Eine Mehr-
körpersimulation erweitert das (n)-Verfahren zur Behandlung kleiner kinematischer
O
Schleifen, die bei der Modellierung mechatronischer Systeme auftreten. Ein Verfah-
ren zur Vermeidung von Selbstkollisionen basiert auf der effizienten Berechnung der
Distanzen zwischen Robotersegmenten. Es wird ein Verfahren vorgestellt, das die
Kollisionsvermeidung mit einer Drallregelung integriert und so insbesondere schnelle
Laufbewegungen verbessert. Simulationen und Experimente mit dem zweibeinigen
Roboter Lola werden gezeigt.
Stichworte: Humanoide, Zweibeiner, effiziente Algorithmen, Mehrkörpersimulation,
Kollisionsvermeidung, Drallregelung

v
Acknowledgments
This thesis summarizes a large part of my research carried out at the Institute of
Applied Mechanics, Technische Universität München. Many people have supported me
during the past six years and made this work possible.
First, I would like to thank my supervisor Professor Heinz Ulbrich for giving me the
opportunity to work on this research topic and providing such an excellent research
environment. The given freedom combined with his guidance and support made this
work possible. I would also like to acknowledge Professor Carlo Bottasso and Professor
Daniel Rixen for serving on my thesis defense committee. After becoming the new
head of the institute, Professor Rixen gave me the opportunity to finish my research
for which I am thankful.
I am deeply grateful for having had the chance to work with a number of very
talented and highly motivated people. I am especially thankful to the research group
working on the robot Lola. While still being a student I learned a lot on mechanical
designfromDr.SebastianLohmeier,whowasresponsibleforLola’smechatronicsystem
architecture. Helaterencouragedmetojointheteamasaresearcherandwecontinued
discussing mechanical designs. I would like to thank Dr. Thomas Buschmann, who was
responsible for the simulation and control of Lola. I learned so much about program
design and walking control from him. His knowledge in robotics and guidance during
his time as a group leader was invaluable. I warmly thank Valerio Favot, who worked
onthedecentralizedcontrollersandcommunicationsystem. Hisskillsasanelectronics
wizard were greatly appreciated. Throughout the project we enjoyed good times but
also endured some hardship. I would like to thank the other robotics team members
Alexander Ewald, Robert Wittmann, Arne Hildebrand, Jörg Baur and Christoph Schütz
for the inspiring discussions on robotics research topics.
Quality hardware is crucial for experimental work. I would like to thank the insti-
tute’s electrical and mechanical workshops. I owe special thanks to Georg “Schorsch”
Mayr. His long experience with electronics on research projects enabled both smooth
experimental work with Lola and further developments on Lola. I would like to thank
Wilhelm Miller, Walter Wöß, Simon Gerer, Philip Schneider and Tobias Schmidt for
their terrific work in manufacturing parts for Lola. I am grateful to PD Dr. Thomas
Thümmel for managing project resources and his support throughout this thesis.
I would also like to thank my ex-colleagues Dr. Thomas Buschmann, Dr. Sebas-
tian Lohmeier and Robert Wittmann for proofreading this thesis and giving helpful
comments.
Finally, I would like to thank my family, my girlfriend Karina and my good friend Dr.
Reinhard Tschiesner for their continuous support and encouragement.
Munich, July 2014 Markus Schwienbacher

Contents
1 Introduction 1
1.1 ProblemStatement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 BackgroundandRelatedWork . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.3 OverviewofthisThesis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2 An (n)-formalismfortheSimulationofMBSwithSmallKinematicLoops 9
O
2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.1.1 BasicDynamicsEquation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.1.2 RelatedWork . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.2 RigidBodyKinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
2.3 RigidBodyDynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.4 RelativeKinematicsofRigidMultibodySystems . . . . . . . . . . . . . . . . . . . . 17
2.4.1 TopologyofMultibodySystems . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.4.2 MotionConstraintsandMinimalCoordinates . . . . . . . . . . . . . . . . . 19
2.4.3 RecursiveKinematicsCalculation . . . . . . . . . . . . . . . . . . . . . . . . . 20
2.4.4 RecursiveKinematicsusingSpatialVectorNotation. . . . . . . . . . . . . . 21
2.5 DynamicsofRigidMultibodySystems . . . . . . . . . . . . . . . . . . . . . . . . . . 25
2.6 Sub-Systems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
2.7 DetailedDerivationofthe (n)-AlgorithmwithSub-Systems . . . . . . . . . . . . 28
O
2.8 ResultingFormalism . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
2.9 AutomaticSub-SystemGeneration. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
2.10 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
2.10.1 Run-TimeComparisonsfortheDillExample . . . . . . . . . . . . . . . . . . 42
2.10.2 Run-TimeComparisonfortheLolaModel . . . . . . . . . . . . . . . . . . . 45
2.11 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
2.12 ChapterSummary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
3 Kinematics 55
3.1 HarmonicDriveGears . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
3.2 KneeJointDriveKinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
3.3 AnkleJointDriveKinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
3.4 CameraVergenceKinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60
3.5 KinematicsCalibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
3.6 ChapterSummary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
4 InverseKinematics 69
4.1 ProblemStatement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69
4.2 PositionBasedInverseKinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
vii
viii Contents
4.3 DifferentialInverseKinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
4.3.1 ResolvedMotionRateControlandRedundancyResolution . . . . . . . . . 72
4.3.2 JacobianTranspose . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
4.3.3 ResolvedAccelerationRateControl . . . . . . . . . . . . . . . . . . . . . . . 76
4.3.4 HierarchicalApproaches . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
4.4 SingularitiesandManipulability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
4.5 TaskDescriptionofLola . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
4.6 ChapterSummary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
5 Self-CollisionAvoidance 81
5.1 BackgroundandRelatedWork . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
5.2 Self-CollisionAvoidance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
5.3 ChapterSummary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
6 Real-TimeDistanceComputationusingSwept-Sphere-Volumes 89
6.1 BackgroundandRelatedWork . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89
6.2 FormalAspectsofDistanceComputation . . . . . . . . . . . . . . . . . . . . . . . . . 91
6.3 SSVPrimitives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92
6.3.1 Point-Swept-SphereVolume . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
6.3.2 Line-Swept-SphereVolume. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 93
6.3.3 Triangle-Swept-SphereVolume . . . . . . . . . . . . . . . . . . . . . . . . . . 94
6.4 DistanceCalculationbetweenSSVPrimitives . . . . . . . . . . . . . . . . . . . . . . 94
6.4.1 PointtoPointDistanceComputation . . . . . . . . . . . . . . . . . . . . . . . 96
6.4.2 PointtoLine-SegmentDistanceComputation . . . . . . . . . . . . . . . . . 97
6.4.3 PointtoTriangleDistanceComputation . . . . . . . . . . . . . . . . . . . . . 98
6.4.4 Line-SegmenttoLine-SegmentDistanceComputation . . . . . . . . . . . . 99
6.4.5 Line-SegmenttoTriangleDistanceComputation . . . . . . . . . . . . . . .105
6.4.6 TriangletoTriangleDistanceComputation . . . . . . . . . . . . . . . . . . .110
6.4.7 GeneralFrameworkofMinimizationUsingInequalityConstraints. . . . .110
6.5 ImplementationDetailsandRun-TimePerformance . . . . . . . . . . . . . . . . . .113
6.6 ModelingoftheRobotSegments. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .114
6.6.1 CompoundsofSSVsasRobotSegments . . . . . . . . . . . . . . . . . . . . .114
6.6.2 AVersatileModelingTool . . . . . . . . . . . . . . . . . . . . . . . . . . . . .114
6.7 IntegrationofBoundingBoxes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .117
6.8 SystemOverview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .118
6.9 ChapterSummary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .121
7 UseofAngularMomentuminWalkingControl 123
7.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .123
7.2 AngularMomentum . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .123
7.3 AngularMomentumCompensationviaNull-SpaceMotion . . . . . . . . . . . . . .124
7.3.1 ReferenceMethod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .124
7.3.2 ProposedMethod . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .125
7.3.3 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .126
7.3.4 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .126
Contents ix
7.4 AngularMomentumTrajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .127
7.5 AngularMomentumMinimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . .129
7.6 ChapterSummary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .131
8 ConclusionandOutlook 133
8.1 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .133
8.2 RecommendationsforFutureWork . . . . . . . . . . . . . . . . . . . . . . . . . . . .135
A MathematicalToolbox 137
A.1 NotationandOperators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .137
A.2 CoordinateTransformations. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .138
A.3 PartialDerivatives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .139
B Pseudo-CodeforDistanceCalculationAlgorithms 141
B.1 Line-SegmenttoLine-Segment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .141
C InverseKinematics:OrientationErrorinTaskSpace 145
ListofAbbreviations 149
Bibliography 151