Probabilistic Robotics

Transcription

1 Probabilistic Robotics Sebastian Thrun Wolfram Burgard Dieter Fox The MIT Press Cambridge, Massachusetts London, England

2 Preface xvii Acknowledgments xix I Basics 1 1 Introduction Uncertainty in Robotics Probabilistic Robotics Implications Road Map Teaching Probabilistic Robotics Bibliographical Remarks 11 2 Recursive State Estimation Introduction Basic Concepts in Probability Robot Environment Interaction State Environment Interaction Probabilistic Generative Laws Belief Distributions Bayes Filters The Bayes Filter Algorithm Example Mathematical Derivation of the Bayes Filter The Markov Assumption 33

3 via 2.5 Representation and Computation Summary Bibliographical Remarks Exercises 36 3 Gaussian Filters Introduction The Kaiman Filter Linear Gaussian Systems The Kaiman Filter Algorithm Illustration Mathematical Derivation of the KF The Extended Kaiman Filter Why Linearize? Linearization Via Taylor Expansion The EKF Algorithm Mathematical Derivation of the EKF Practical Considerations The Unscented Kaiman Filter Linearization Via the Unscented Transform The UKF Algorithm The Information Filter Canonical Parameterization The Information Filter Algorithm Mathematical Derivation of the Information Filter The Extended Information Filter Algorithm Mathematical Derivation of the Extended Information Filter Practical Considerations Summary Bibliographical Remarks Exercises 81 4 Nonparametric Filters The Histogram Filter The Discrete Bayes Filter Algorithm Continuous State Mathematical Derivation of the Histogram Approximation 89

4 4.1.4 Decomposition Techniques Binary Bayes Filters with Static State The Particle Filter Basic Algorithm Importance Sampling Mathematical Derivation of the PF Practical Considerations and Properties of Particle Filters Summary Bibliographical Remarks Exercises Robot Motion Introduction Preliminaries Kinematic Configuration Probabilistic Kinematics Velocity Motion Model Closed Form Calculation Sampling Algorithm Mathematical Derivation of the Velocity Motion Model Odometry Motion Model Closed Form Calculation Sampling Algorithm Mathematical Derivation of the Odometry Motion Model Motion and Maps Summary Bibliographical Remarks Exercises Robot Perception Introduction Maps Beam Models of Range Finders The Basic Measurement Algorithm Adjusting the Intrinsic Model Parameters Mathematical Derivation of the Beam Model 162

5 6.3 A Practical Considerations Limitations of the Beam Model Likelihood Fields for Range Finders Basic Algorithm Extensions Correlation-Based Measurement Models Feature-Based Measurement Models Feature Extraction Landmark Measurements Sensor Model with Known Correspondence Sampling Poses Further Considerations Practical Considerations Summary Bibliographical Remarks Exercises 185 II Localization Mobile Robot Localization: Markov and Gaussian A Taxonomy of Localization Problems Markov Localization Illustration of Markov Localization EKF Localization Illustration The EKF Localization Algorithm Mathematical Derivation of EKF Localization Physical Implementation Estimating Correspondences EKF Localization with Unknown Correspondences Mathematical Derivation of the ML Data Association Multi-Hypothesis Tracking UKF Localization Mathematical Derivation of UKF Localization Illustration Practical Considerations 229

6 xi 7.9 Summary Bibliographical Remarks Exercises Mobile Robot Localization: Grid And Monte Carlo Introduction Grid Localization Basic Algorithm Grid Resolutions Computational Considerations Illustration Monte Carlo Localization Illustration The MCL Algorithm Physical Implementations Properties of MCL Random Particle MCL: Recovery from Failures Modifying the Proposal Distribution KLD-Sampling: Adapting the Size of Sample Sets Localization in Dynamic Environments Practical Considerations Summary Bibliographical Remarks Exercises 276 HI Mapping Occupancy Grid Mapping Introduction The Occupancy Grid Mapping Algorithm Multi-Sensor Fusion Learning Inverse Measurement Models Inverting the Measurement Model Sampling from the Forward Model The Error Function Examples and Further Considerations Maximum A Posteriori Occupancy Mapping The Case for Maintaining Dependencies 299

7 XU Occupancy Grid Mapping with Forward Models Summary Bibliographical Remarks Exercises Simultaneous Localization and Mapping Introduction SLAM with Extended Kaiman Filters Setup and Assumptions SLAM with Known Correspondence Mathematical Derivation of EKF SLAM EKF SLAM with Unknown Correspondences The General EKF SLAM Algorithm Examples Feature Selection and Map Management Summary Bibliographical Remarks Exercises The GraphSLAM Algorithm Introduction Intuitive Description Building Up the Graph Inference The GraphSLAM Algorithm Mathematical Derivation of GraphSLAM The Full SLAM Posterior The Negative Log Posterior Taylor Expansion Constructing the Information Form Reducing the Information Form Recovering the Path and the Map Data Association in GraphSLAM The GraphSLAM Algorithm with Unknown Correspondence Mathematical Derivation of the Correspondence Test Efficiency Consideration Empirical Implementation 370

8 xin 11.8 Alternative Optimization Techniques Summary Bibliographical Remarks Exercises The Sparse Extended Information Filter Introduction Intuitive Description The SEIF SLAM Algorithm Mathematical Derivation of the SEIF Motion Update Measurement Updates Sparsification General Idea Sparsification in SEIFs Mathematical Derivation of the Sparsification Amortized Approximate Map Recovery How Sparse Should SEIFs Be? Incremental Data Association Computing Incremental Data Association Probabilities Practical Considerations Branch-and-Bound Data Association Recursive Search Computing Arbitrary Data Association Probabilities Equivalence Constraints Practical Considerations Multi-Robot SLAM Integrating Maps Mathematical Derivation of Map Integration Establishing Correspondence Example Summary Bibliographical Remarks Exercises The FastSLAM Algorithm The Basic Algorithm 439

9 13.2 Factoring the SLAM Posterior Mathematical Derivation of the Factored SLAM Posterior FastSLAM with Known Data Association Improving the Proposal Distribution Extending the Path Posterior by Sampling a New Pose Updating the Observed Feature Estimate Calculating Importance Factors Unknown Data Association Map Management The FastSLAM Algorithms Efficient Implementation FastSLAM for Feature-Based Maps Empirical Insights Loop Closure Grid-based FastSLAM The Algorithm Empirical Insights Summary Bibliographical Remarks Exercises 482 IV Planning and Control Markov Decision Processes Motivation Uncertainty in Action Selection Value Iteration Goals and Payoff Finding Optimal Control Policies for the Fully Observable Case Computing the Value Function Application to Robot Control Summary Bibliographical Remarks Exercises 510

10 XV 15 Partially Observable Markov Decision Processes Motivation An Illustrative Example Setup Control Choice Sensing Prediction Deep Horizons and Pruning The Finite World POMDP Algorithm Mathematical Derivation of POMDPs Value Iteration in Belief Space Value Function Representation Calculating the Value Function Practical Considerations Summary Bibliographical Remarks Exercises Approximate POMDP Techniques Motivation QMDPs Augmented Markov Decision Processes The Augmented State Space The AMDP Algorithm Mathematical Derivation of AMDPs Application to Mobile Robot Navigation Monte Carlo POMDPs Using Particle Sets The MC-POMDP Algorithm Mathematical Derivation of MC-POMDPs Practical Considerations Summary Bibliographical Remarks Exercises Exploration Introduction Basic Exploration Algorithms Information Gain 571

11 xvi Greedy Techniques Monte Carlo Exploration Multi-Step Techniques Active Localization Exploration for Learning Occupancy Grid Maps Computing Information Gain Propagating Gain Extension to Multi-Robot Systems Exploration for SLAM Entropy Decomposition in SLAM Exploration in FastSLAM Empirical Characterization Summary Bibliographical Remarks Exercises 604 Bibliography 607 Index 639