automatically tracking and calibrating articulated robots using slam techniques

94 Pages·2016·24.8 MB·English

A T
UTOMATICALLY RACKING
C
AND ALIBRATING
A R
RTICULATED OBOTS
SLAM T
USING ECHNIQUES
Matthew Klingensmith
CMU-RI-TR-16-36
July 15, 2016
TheRoboticsInstitute
SchoolofComputerScience
CarnegieMellonUniversity
Pittsburgh,PA15213
ThesisCommittee:
SiddharthaS.Srinivasa
MichaelKaess
GeorgeKantor
AndrewDavison
Submittedinpartialfulfillmentoftherequirements
forthedegreeofDoctorofPhilosophy.
Copyright(cid:13)c MatthewKlingensmith
WorkinthisdissertationwassupportedinpartbyToyotaUSAgrantNo. 1011344theU.SOfficeofNavalResearch
grantNo. N000141210613andNSFgrantNo. IIS-1426703
Keywords: SLAM,Calibration,Tracking,Kinematics,Manipulation,VisualServoing,Com-
puterVision,3DMapping
Formyparents,whoshowedmetheway;
Sarah,wholitupmylife;
andDavid,whostillhauntstheshadows.
iv
Abstract
Robots still struggle with everyday manipulation tasks. An overriding problem
with robotic manipulation is uncertainty in the robot’s state and calibration param-
eters. Small amounts of uncertainty can lead to complete task failure. This thesis
explores ways of tracking and calibrating noisy robot arms using computer vision,
with an aim toward making them more robust. We consider three systems with in-
creasing complexity: a noisy robot arm tracked by an external depth camera (chap-
ter 2), a noisy arm that localizes itself using a hand-mounted depth sensor looking
at an unstructured word (chapter 3), and a noisy arm that only has a single hand-
mountedmonocularRGBcameraestimatingitsstatewhilesimultaneouslycalibrat-
ing its camera extrinsics (chapter 4). Using techniques taken from dense object
tracking, fully dense SLAM and sparse general SLAM, we are able to automati-
cally track the robot and extract its calibration parameters. We also provide analy-
sis linking these problems together, while exploring the fundamental limitations of
SLAM-basedapproachesforcalibratingrobotarms.
vi
Acknowledgments
Writing a thesis is a lot like producing a film, so I have decided to write these
acknowledgmentsasfilmcredits:
Advisor ..............................SiddharthaS.Srinivasa
Co-Advisor ..................................... MichaelKaess
CommitteeMember1 ..................................... GeorgeKantor
CommitteeMember2 ................................... AndrewDavison
ForeignAdvisor1 ................................... AndrewDavison
ForeignAdvisor2 .................................StefanLeutenegger
EmotionalSupport ......................................SarahHoskins
Misc. Services ......................................SarahHoskins
TheoryHelp1 ..................................... MichaelKoval
TheoryHelp2 ..................................ChristopherDellin
ExternalSupport ..................................AndrewJ.Bagnell
ExternalSupport ......................................NancyPollard
ExternalSupport ................................ GoogleTangoTeam
Producer1 ...................................... LeslieAnsley
Producer2 .................................KerryKlingensmith
Extra ..............................RebeccaKlingensmith
ThreeAmigos ............ YueranYuan,WilsonPei,StevenBlessing
This thesis work was completed in three locations: at the Carnegie Mellon Per-
sonalRoboticsLabinPittsburghPA,theDysonRoboticsLabatImperialCollegein
LondonUK.andattheRowanUniversityLibraryinGlassboroNJ.
It would not have been possible without the help of my many mentors and col-
leagues at Carnegie Mellon, especially my advisers Sidd Srinivasa and Michael
Kaess. The research spanned three projects at Carnegie Mellon: ARM-S, HERB
andADA.
MyworkonProjectTangounderJoelHeschwithhelpfromIvanDryanovskiand
SimonLynenintroducedmetosomeofthecoreconceptsinthisthesisincluding3D
mapping and SLAM.During my 5-month internship in London, my conversations
with Andrew Davison and Stefan Leutenegger greatly improved my understanding
ofvisual SLAM.Prof. Davisonwas kindenoughto letme workon mythesis docu-
mentwhiledevelopingtheSLAM/calibrationalgorithminhislab.
The emotional support of my parents, wife, and friends were also critical to
completingthisthesis. SomeoftheseedsofmyideascamefromSundayBSsessions
attheChinesebuffetonForbesavenue.
viii
Contents
1 Introduction 1
1.1 SourcesofError . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 MeasuringandAccountingforError . . . . . . . . . . . . . . . . . . . . . . . . 3
1.3 RobotArmTrackingandCalibrationasSLAM . . . . . . . . . . . . . . . . . . 4
2 DenseTrackingusinganExternalDepthSensor 5
2.1 ProblemDefinition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.1.1 DepthImagePosterior . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.1.2 JointEncoderPosterior . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.2 AlgorithmImplementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.2.1 Matchingtothedepthimage . . . . . . . . . . . . . . . . . . . . . . . . 8
2.2.2 Stochasticgradientdescent . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.3 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.4 DiscussionandLimitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
3 DenseSLAMusingaHand-MountedDepthSensor 13
3.1 ProblemDefinition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.1.1 MappingtheScene . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.1.2 TrackingtheArm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.2 AlgorithmImplementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
3.2.1 TSDFMapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.2.2 StochasticGradientDescent . . . . . . . . . . . . . . . . . . . . . . . . 18
3.3 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.3.1 2DSimulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.3.2 3DSimulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
3.3.3 BookshelfScanning . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.4 DiscussionandLimitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
4 SparseGeneralSLAMusingaHand-MountedMonocularCamera 25
4.1 ProblemDefinition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
4.2 GraphicalModel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
4.2.1 LandmarkObservationFactors . . . . . . . . . . . . . . . . . . . . . . . 27
4.2.2 JointEncoderFactors . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
4.2.3 RobotDynamicsFactors . . . . . . . . . . . . . . . . . . . . . . . . . . 30
ix
4.2.4 ExtrinsicPrior . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
4.3 SLAMBack-endSystemImplementation . . . . . . . . . . . . . . . . . . . . . 31
4.4 LandmarkFront-endSystemImplementation . . . . . . . . . . . . . . . . . . . 33
4.4.1 AprilTagFront-endImplementation . . . . . . . . . . . . . . . . . . . . 33
4.4.2 NaturalImageFeatureFront-endImplementation . . . . . . . . . . . . . 33
4.5 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
4.5.1 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
4.5.2 Performance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34
4.5.3 Simulatedgroundtruth . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
4.5.4 ModifyingtheRobot’sKinematicStructure . . . . . . . . . . . . . . . . 38
4.5.5 ComparisonwithOtherSLAMSystems . . . . . . . . . . . . . . . . . . 40
4.6 DiscussionandLimitations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
4.7 PreliminaryWork: MovingRobotBase . . . . . . . . . . . . . . . . . . . . . . 49
4.7.1 PlanarHolonomic3-DOFBase . . . . . . . . . . . . . . . . . . . . . . 49
4.7.2 Free-floating6-DOFBase . . . . . . . . . . . . . . . . . . . . . . . . . 50
5 RelatedWorks 53
5.1 Hand-eyeCalibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
5.2 TrackingArticulatedBodies . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
5.3 SimultaneousLocalizationandMapping(SLAM) . . . . . . . . . . . . . . . . . 55
5.3.1 DenseSLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
5.3.2 SparseSLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5.4 CalibratingwithSLAM . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57
6 Conclusion 59
A AppendixandBackground 61
A.1 Geometryof3DSpace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
A.1.1 DifferentialGeometryof3DSpace . . . . . . . . . . . . . . . . . . . . 62
A.2 ArticulatedLinkagesandRobotArms . . . . . . . . . . . . . . . . . . . . . . . 63
A.2.1 ForwardandInverseKinematics . . . . . . . . . . . . . . . . . . . . . . 63
A.2.2 TheRobotJacobian . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
A.2.3 TopologyoftheConfigurationSpace . . . . . . . . . . . . . . . . . . . 64
A.2.4 Encoders . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
A.3 DataSynchronizationandInterpolation . . . . . . . . . . . . . . . . . . . . . . 66
A.4 GeometricComputerVision . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
A.4.1 PinholeCameras . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66
A.4.2 StereoTriangulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
A.4.3 RGB-DCameras . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
A.5 FactorGraphs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
A.6 Robustm-Estimators . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
A.7 2DImageFeaturesandAssociation . . . . . . . . . . . . . . . . . . . . . . . . 69
A.7.1 BRISKImageFeatures . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
A.7.2 LandmarkAssociation . . . . . . . . . . . . . . . . . . . . . . . . . . . 70
x