Autonomous Navigation in Complex Indoor And Outdoor Environments with Micro Aerial Vehicles

187 Pages·2016·9.89 MB·English

AUTONOMOUSNAVIGATIONINCOMPLEXINDOORANDOUTDOOR
ENVIRONMENTSWITHMICROAERIALVEHICLES
ShaojieShen
ADISSERTATION
in
ElectricalandSystemsEngineering
PresentedtotheFacultiesoftheUniversityofPennsylvania
in
PartialFulfillmentoftheRequirementsforthe
DegreeofDoctorofPhilosophy
2014
VijayKumar,SupervisorofDissertation
UPSFoundationProfessorofMechanicalEngineeringandAppliedMechanics
NathanMichael,Co-SupervisorofDissertation
AssistantResearchProfessorofRobotics
SaswatiSarkar,GraduateGroupChairperson
ProfessorofElectricalandSystemsEngineering
DissertationCommittee:
DanielD.Lee,ProfessorofElectricalandSystemsEngineering
VijayKumar,ProfessorofMechanicalEngineeringandAppliedMechanics
NathanMichael,AssistantResearchProfessorofRobotics
DavideScaramuzza,AssistantProfessorofRobotics
Acknowledgments
This thesis will not be possible without the support of many people. I would first like to
thank my advisors Vijay Kumar and Nathan Michael, for all their support and guidance
throughout my graduate studies at Penn. I would also like to thank my thesis committee
members Daniel Lee and Davide Scaramuzza, for taking the time to serve on my com-
mittee. Additionally, I would like to thank fellow and past members of the Multi-Robot
Systems Lab (MRSL), as well as others in the GRASP lab, for many great technical
discussions and fun time. Specially, I would like to thank Kartik Mohta and Yash Mul-
gaonkarfortheirhelpinmakinganddebuggingmanyroboticssystems.
I am indebted to my parents for their love and support over years across continents
and oceans. Last but not least, I thank my wife, Shuangyu, who has been with me every
momentthroughallupsanddowns.
ii
ABSTRACT
AUTONOMOUSNAVIGATIONINCOMPLEXINDOORANDOUTDOOR
ENVIRONMENTSWITHMICROAERIALVEHICLES
ShaojieShen
VijayKumar
NathanMichael
Micro aerial vehicles (MAVs) are ideal platforms for surveillance and search and rescue
in confined indoor and outdoor environments due to their small size, superior mobil-
ity, and hover capability. In such missions, it is essential that the MAV is capable of
autonomous flight to minimize operator workload. Despite recent successes in commer-
cialization of GPS-based autonomous MAVs, autonomous navigation in complex and
possibly GPS-denied environments gives rise to challenging engineering problems that
requireanintegratedapproachtoperception,estimation,planning,control,andhighlevel
situationalawareness. Amongthese,stateestimationisthefirstandmostcriticalcompo-
nent for autonomous flight, especially because of the inherently fast dynamics of MAVs
and the possibly unknown environmental conditions. In this thesis, we present method-
ologies and system designs, with a focus on state estimation, that enable a light-weight
off-the-shelf quadrotor MAV to autonomously navigate complex unknown indoor and
outdoor environments using only onboard sensing and computation. We start by de-
veloping laser and vision-based state estimation methodologies for indoor autonomous
flight. Wetheninvestigatefusionfromheterogeneoussensorstoimproverobustnessand
enable operations in complex indoor and outdoor environments. We further propose es-
timation algorithms for on-the-fly initialization and online failure recovery. Finally, we
present planning, control, and environment coverage strategies for integrated high-level
autonomy behaviors. Extensive online experimental results are presented throughout the
thesis. Weconcludebyproposingfutureresearchopportunities.
iii
Contents
1 Introduction 1
1.1 ResearchProblems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.1.1 AutonomousFlightinGPS-deniedEnvironments . . . . . . . . . 3
1.1.2 Multi-SensorFusionforAutonomousFlight . . . . . . . . . . . . 3
1.1.3 EstimatorInitializationandFailureRecovery . . . . . . . . . . . 4
1.1.4 PlanningandControl . . . . . . . . . . . . . . . . . . . . . . . . 4
1.1.5 AutonomousEnvironmentCoverage . . . . . . . . . . . . . . . . 5
1.2 ThesisOverview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.3 OverviewofExperimentalPlatforms . . . . . . . . . . . . . . . . . . . . 8
1.4 ThesisContributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2 ScientificBackgroundandLiteratureReview 13
2.1 AutonomousFlightinGPS-deniedEnvironments . . . . . . . . . . . . . 13
2.2 IncrementalMotionEstimation . . . . . . . . . . . . . . . . . . . . . . . 14
2.3 SimultaneousLocalizationandMapping . . . . . . . . . . . . . . . . . . 15
2.4 Multi-SensorFusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
2.5 MonocularVisual-InertialStateEstimation . . . . . . . . . . . . . . . . 18
2.6 EstimatorInitializationandFailureRecovery . . . . . . . . . . . . . . . 19
2.7 AutonomousEnvironmentCoverage . . . . . . . . . . . . . . . . . . . . 21
3 Laser-BasedAutonomousIndoorNavigation 24
iv
3.1 PoseEstimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.1.1 Assumption . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.1.2 2DPoseEstimation . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.1.3 AltitudeEstimation . . . . . . . . . . . . . . . . . . . . . . . . . 27
3.2 EKF-basedSensorFusionforControl . . . . . . . . . . . . . . . . . . . 28
3.3 SimultaneousLocalizationandMapping . . . . . . . . . . . . . . . . . . 29
3.3.1 EnvironmentRepresentation . . . . . . . . . . . . . . . . . . . . 30
3.4 ExperimentalResults . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
3.4.1 EvaluatingEstimatorPerformance . . . . . . . . . . . . . . . . . 31
3.4.2 NavigationinConfinedMulti-FloorIndoorEnvironments . . . . 31
3.4.3 LargeScaleMappingAcrossMultipleFloors . . . . . . . . . . . 36
3.4.4 PublicDemonstration . . . . . . . . . . . . . . . . . . . . . . . 37
3.5 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
4 Vision-BasedStateEstimationandAutonomousFlight 40
4.1 FeatureDetectionandTracking . . . . . . . . . . . . . . . . . . . . . . . 42
4.2 PoseEstimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
4.2.1 OrientationEstimation . . . . . . . . . . . . . . . . . . . . . . . 43
4.2.2 PositionEstimation . . . . . . . . . . . . . . . . . . . . . . . . . 44
4.3 Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46
4.3.1 LocalMapUpdate . . . . . . . . . . . . . . . . . . . . . . . . . 47
4.3.2 ScaleRecovery . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
4.3.3 GlobalMapping . . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.4 UKF-BasedSensorFusion . . . . . . . . . . . . . . . . . . . . . . . . . 50
4.5 ExperimentalResults . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
4.5.1 AutonomousTrajectoryTrackingwithGroundTruth . . . . . . . 52
4.5.2 HighSpeedStraightLineTracking . . . . . . . . . . . . . . . . 52
4.5.3 NavigationofIndoorEnvironmentswithLargeLoops . . . . . . 54
4.5.4 AutonomousNavigationinComplexOutdoorEnvironments . . . 57
v
4.6 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
5 Multi-SensorFusionforIndoorandOutdoorOperations 60
5.1 Multi-SensorSystemModel . . . . . . . . . . . . . . . . . . . . . . . . 61
5.1.1 AbsoluteMeasurements . . . . . . . . . . . . . . . . . . . . . . 62
5.1.2 RelativeMeasurements . . . . . . . . . . . . . . . . . . . . . . . 62
5.2 UKF-basedMulti-SensorFusion . . . . . . . . . . . . . . . . . . . . . . 63
5.2.1 StateAugmentationforMultipleRelativeMeasurements . . . . . 64
5.2.2 StatisticalLinearizationforUKF . . . . . . . . . . . . . . . . . . 65
5.2.3 StatePropagation . . . . . . . . . . . . . . . . . . . . . . . . . . 66
5.2.4 MeasurementUpdate . . . . . . . . . . . . . . . . . . . . . . . . 67
5.2.5 DelayedandOut-of-OrderMeasurementUpdate . . . . . . . . . 68
5.3 HandlingGlobalPoseMeasurements . . . . . . . . . . . . . . . . . . . . 69
5.4 ImplementationDetails . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
5.4.1 AbsoluteMeasurements . . . . . . . . . . . . . . . . . . . . . . 72
5.4.2 RelativeMeasurement-LaserOdometry . . . . . . . . . . . . . 74
5.4.3 RelativeMeasurement-VisualOdometry . . . . . . . . . . . . . 75
5.5 ExperimentalResults . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
5.5.1 EvaluationofEstimatorPerformance . . . . . . . . . . . . . . . 76
5.5.2 AutonomousFlightinIndoorandOutdoorEnvironments . . . . . 76
5.5.3 AutonomousFlightinTree-LinedCampus . . . . . . . . . . . . 77
5.6 BenefitsandLimitations . . . . . . . . . . . . . . . . . . . . . . . . . . 80
5.7 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82
6 InitializationandFailureRecoveryforMonocularVisual-InertialSystems 84
6.1 LinearSlidingWindowVINSEstimator . . . . . . . . . . . . . . . . . . 85
6.1.1 Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 86
6.1.2 LinearRotationEstimation . . . . . . . . . . . . . . . . . . . . . 87
6.1.3 LinearSlidingWindowEstimator . . . . . . . . . . . . . . . . . 88
vi
6.1.4 IMUMeasurementModel . . . . . . . . . . . . . . . . . . . . . 89
6.1.5 CameraMeasurementModel . . . . . . . . . . . . . . . . . . . . 90
6.2 NonlinearOptimization . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
6.2.1 Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
6.2.2 IMUMeasurementModel . . . . . . . . . . . . . . . . . . . . . 93
6.2.3 CameraMeasurementModel . . . . . . . . . . . . . . . . . . . . 96
6.3 HandlingScaleAmbiguityviaTwo-WayMarginalization . . . . . . . . . 97
6.4 InitializationandFailureRecovery . . . . . . . . . . . . . . . . . . . . . 99
6.5 SimulationResults . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102
6.6 ExperimentalResults . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104
6.6.1 Real-TimeImplementation . . . . . . . . . . . . . . . . . . . . . 104
6.6.2 ImplementationDetailsandChoiceofParameters . . . . . . . . . 105
6.6.3 InitializationPerformance . . . . . . . . . . . . . . . . . . . . . 107
6.6.4 AutonomousHovering . . . . . . . . . . . . . . . . . . . . . . . 110
6.6.5 AutonomousTrajectoryTracking . . . . . . . . . . . . . . . . . 111
6.6.6 AutonomousFlightinIndoorEnvironments . . . . . . . . . . . . 116
6.6.7 StateEstimationinLarge-ScaleEnvironments . . . . . . . . . . . 120
6.7 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126
7 PlanningandControl 127
7.1 FeedbackControl . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 127
7.2 HighLevelPlanning . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128
7.3 MinimumJerkTrajectoryGeneration . . . . . . . . . . . . . . . . . . . 129
7.4 ExperimentalResults . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131
8 AutonomousThree-DimensionalEnvironmentCoverage 134
8.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135
8.2 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136
8.2.1 NotesonNotation . . . . . . . . . . . . . . . . . . . . . . . . . 136
vii
8.2.2 Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138
8.2.3 Assumptions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140
8.3 TheSDEEAlgorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140
8.3.1 Particle-basedRepresentationofFreeSpace . . . . . . . . . . . . 140
8.3.2 Resampling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141
8.3.3 ParticleDynamics . . . . . . . . . . . . . . . . . . . . . . . . . 142
8.3.4 FrontierExtraction . . . . . . . . . . . . . . . . . . . . . . . . . 144
8.3.5 GoalQueuingandAlgorithmTermination . . . . . . . . . . . . . 147
8.3.6 HeuristicsforImprovedPerformance . . . . . . . . . . . . . . . 147
8.4 Complexity . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148
8.5 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149
8.5.1 ComparisontoFrontier-basedExploration . . . . . . . . . . . . . 149
8.5.2 SimulationResults . . . . . . . . . . . . . . . . . . . . . . . . . 150
8.5.3 ExperimentalResults . . . . . . . . . . . . . . . . . . . . . . . . 153
8.6 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 157
9 ConclusionandFutureWork 158
9.1 SummaryofContributions . . . . . . . . . . . . . . . . . . . . . . . . . 159
9.2 FutureWork . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159
Bibliography 162
viii
List of Figures
1.1 Flowoftopicsandstructureofanavigationsystem . . . . . . . . . . . . 7
1.2 Listofplatforms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.1 Comparisonofstateestimationapproaches . . . . . . . . . . . . . . . . 20
3.1 Diagramoflaser-basedstateestimation . . . . . . . . . . . . . . . . . . 25
3.2 Laser-basedaltitudeestimation . . . . . . . . . . . . . . . . . . . . . . . 28
3.3 Laser-basedtrajectorytracking . . . . . . . . . . . . . . . . . . . . . . . 32
3.4 Laser-basedhovering . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
3.5 Mapsgeneratedduringlaser-basednavigation . . . . . . . . . . . . . . . 34
3.6 Imagesoflaser-basednavigation . . . . . . . . . . . . . . . . . . . . . . 35
3.7 Mapgenerationacrossthreefloorsofanindoorenvironment. . . . . . . . 36
3.8 Publicdemonstration . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
4.1 Diagramoftheproposedvision-basedstateestimator . . . . . . . . . . . 41
4.2 Camerageometrynotaion . . . . . . . . . . . . . . . . . . . . . . . . . . 42
4.3 Datastructureforfeaturestorage . . . . . . . . . . . . . . . . . . . . . . 45
4.4 Localizationerrordistribution . . . . . . . . . . . . . . . . . . . . . . . 46
4.5 Visualscalechangesduringflight . . . . . . . . . . . . . . . . . . . . . 49
4.6 Vision-basedtrajectorytracking . . . . . . . . . . . . . . . . . . . . . . 53
4.7 Snapshotsofvision-basedtrajectorytracking . . . . . . . . . . . . . . . 54
4.8 Vision-basedhighspeedlinetracking . . . . . . . . . . . . . . . . . . . 55
4.9 3Dmapfromindoorexperiment . . . . . . . . . . . . . . . . . . . . . . 56
ix
4.10 Snapshotofindoorenvironment . . . . . . . . . . . . . . . . . . . . . . 57
4.11 3Dmapfromoutdoorexperiment . . . . . . . . . . . . . . . . . . . . . 58
4.12 Onboardimagesfromoutdoorexperiment . . . . . . . . . . . . . . . . . 58
5.1 Delayedandout-of-ordermeasurementupdate . . . . . . . . . . . . . . . 69
5.2 DirectfusionofGPSmeasurements . . . . . . . . . . . . . . . . . . . . 71
5.3 IndirectfusionofGPSmeasurements . . . . . . . . . . . . . . . . . . . 71
5.4 Multi-sensorsetup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
5.5 Evaluationofmulti-sensorfusionwithgroundtruth . . . . . . . . . . . . 76
5.6 Imagesofautonomousnavigationinanindustrialcomplex . . . . . . . . 78
5.7 Vehicletrajectorywhilenavigatinginanindustrialcomplex . . . . . . . . 79
5.8 Sensoravailabilityduringautonomousnavigation . . . . . . . . . . . . . 79
5.9 CovariancechangesduringGPSoutage . . . . . . . . . . . . . . . . . . 80
5.10 Impactofsensoravailabilityonstateestimationuncertainty . . . . . . . . 81
5.11 Exampleimagesofatree-linedenvironment . . . . . . . . . . . . . . . . 81
5.12 Vehicletrajectorywhilenavigatinginatree-linedenvironment . . . . . . 82
6.1 Two-waymarginalization . . . . . . . . . . . . . . . . . . . . . . . . . . 98
6.2 Diagramforinitializationandfailurerecovery . . . . . . . . . . . . . . . 101
6.3 VINSsimulationenvironment . . . . . . . . . . . . . . . . . . . . . . . 103
6.4 VINSsimulationresults . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
6.5 Initializationwithoutdepthinformation . . . . . . . . . . . . . . . . . . 108
6.6 On-the-flyinitialization . . . . . . . . . . . . . . . . . . . . . . . . . . . 109
6.7 Convergenceofnonlinearoptimization . . . . . . . . . . . . . . . . . . . 109
6.8 Hoverperformance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112
6.9 Hovervelocitycomparison . . . . . . . . . . . . . . . . . . . . . . . . . 113
6.10 Trajectorytrackingperformance . . . . . . . . . . . . . . . . . . . . . . 114
6.11 Trajectorytrackingerror . . . . . . . . . . . . . . . . . . . . . . . . . . 115
6.12 Onboardimagesfromautonomousindoorflight . . . . . . . . . . . . . . 118
x