Preface ix
I Introductory Material 1
1 Introduction 3
1.1 Planning to Plan . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2 Motivational Examples and Applications . . . . . . . . . . . . . . . 5
1.3 Basic Ingredients of Planning . . . . . . . . . . . . . . . . . . . . . 17
1.4 Algorithms, Planners, and Plans . . . . . . . . . . . . . . . . . . . . 19
1.5 Organization of the Book . . . . . . . . . . . . . . . . . . . . . . . . 24
2 Discrete Planning 27
2.1 Introduction to Discrete Feasible Planning . . . . . . . . . . . . . . 28
2.2 Searching for Feasible Plans . . . . . . . . . . . . . . . . . . . . . . 32
2.3 Discrete Optimal Planning . . . . . . . . . . . . . . . . . . . . . . . 43
2.4 Using Logic to Formulate Discrete Planning . . . . . . . . . . . . . 57
2.5 Logic-Based Planning Methods . . . . . . . . . . . . . . . . . . . . 63
II Motion Planning 77
3 Geometric Representations and Transformations 81
3.1 Geometric Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . 81
3.2 Rigid-Body Transformations . . . . . . . . . . . . . . . . . . . . . . 92
3.3 Transforming Kinematic Chains of Bodies . . . . . . . . . . . . . . 100
3.4 Transforming Kinematic Trees . . . . . . . . . . . . . . . . . . . . . 112
3.5 Nonrigid Transformations . . . . . . . . . . . . . . . . . . . . . . . 120
4 The Configuration Space 127
4.1 Basic Topological Concepts . . . . . . . . . . . . . . . . . . . . . . 127
4.2 Defining the Configuration Space . . . . . . . . . . . . . . . . . . . 145
4.3 Configuration Space Obstacles . . . . . . . . . . . . . . . . . . . . . 155
4.4 Closed Kinematic Chains . . . . . . . . . . . . . . . . . . . . . . . . 167
5 Sampling-Based Motion Planning 185
5.1 Distance and Volume in C-Space . . . . . . . . . . . . . . . . . . . 186
5.2 Sampling Theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . 195
5.3 Collision Detection . . . . . . . . . . . . . . . . . . . . . . . . . . . 209
5.4 Incremental Sampling and Searching . . . . . . . . . . . . . . . . . 217
5.5 Rapidly Exploring Dense Trees . . . . . . . . . . . . . . . . . . . . 228
5.6 Roadmap Methods for Multiple Queries . . . . . . . . . . . . . . . . 237
6 Combinatorial Motion Planning 249
6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 249
6.2 Polygonal Obstacle Regions . . . . . . . . . . . . . . . . . . . . . . 251
6.3 Cell Decompositions . . . . . . . . . . . . . . . . . . . . . . . . . . 264
6.4 Computational Algebraic Geometry . . . . . . . . . . . . . . . . . . 280
6.5 Complexity of Motion Planning . . . . . . . . . . . . . . . . . . . . 298
7 Extensions of Basic Motion Planning 311
7.1 Time-Varying Problems . . . . . . . . . . . . . . . . . . . . . . . . 311
7.2 Multiple Robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 318
7.3 Mixing Discrete and Continuous Spaces . . . . . . . . . . . . . . . . 327
7.4 Planning for Closed Kinematic Chains . . . . . . . . . . . . . . . . 337
7.5 Folding Problems in Robotics and Biology . . . . . . . . . . . . . . 347
7.6 Coverage Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . 354
7.7 Optimal Motion Planning . . . . . . . . . . . . . . . . . . . . . . . 357
8 Feedback Motion Planning 369
8.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 369
8.2 Discrete State Spaces . . . . . . . . . . . . . . . . . . . . . . . . . . 371
8.3 Vector Fields and Integral Curves . . . . . . . . . . . . . . . . . . . 381
8.4 Complete Methods for Continuous Spaces . . . . . . . . . . . . . . 398
8.5 Sampling-Based Methods for Continuous Spaces . . . . . . . . . . . 412
III Decision-Theoretic Planning 433
9 Basic Decision Theory 437
9.1 Preliminary Concepts . . . . . . . . . . . . . . . . . . . . . . . . . . 438
9.2 A Game Against Nature . . . . . . . . . . . . . . . . . . . . . . . . 446
9.3 Two-Player Zero-Sum Games . . . . . . . . . . . . . . . . . . . . . 459
9.4 Nonzero-Sum Games . . . . . . . . . . . . . . . . . . . . . . . . . . 468
9.5 Decision Theory Under Scrutiny . . . . . . . . . . . . . . . . . . . . 477
10 Sequential Decision Theory 495
10.1 Introducing Sequential Games Against Nature . . . . . . . . . . . . 496
10.2 Algorithms for Computing Feedback Plans . . . . . . . . . . . . . . 508
10.3 Infinite-Horizon Problems . . . . . . . . . . . . . . . . . . . . . . . 522
10.4 Reinforcement Learning . . . . . . . . . . . . . . . . . . . . . . . . 527
10.5 Sequential Game Theory . . . . . . . . . . . . . . . . . . . . . . . . 536
10.6 Continuous State Spaces . . . . . . . . . . . . . . . . . . . . . . . . 551
11 Sensors and Information Spaces 559
11.1 Discrete State Spaces . . . . . . . . . . . . . . . . . . . . . . . . . . 561
11.2 Derived Information Spaces . . . . . . . . . . . . . . . . . . . . . . 571
11.3 Examples for Discrete State Spaces . . . . . . . . . . . . . . . . . . 581
11.4 Continuous State Spaces . . . . . . . . . . . . . . . . . . . . . . . . 589
11.5 Examples for Continuous State Spaces . . . . . . . . . . . . . . . . 598
11.6 Computing Probabilistic Information States . . . . . . . . . . . . . 614
11.7 Information Spaces in Game Theory . . . . . . . . . . . . . . . . . 619
12 Planning Under Sensing Uncertainty 633
12.1 General Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . 634
12.2 Localization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 640
12.3 Environment Uncertainty and Mapping . . . . . . . . . . . . . . . . 655
12.4 Visibility-Based Pursuit-Evasion . . . . . . . . . . . . . . . . . . . . 684
12.5 Manipulation Planning with Sensing Uncertainty . . . . . . . . . . 691
IV Planning Under Differential Constraints 711
13 Differential Models 715
13.1 Velocity Constraints on the Configuration Space . . . . . . . . . . . 716
13.2 Phase Space Representation of Dynamical Systems . . . . . . . . . 735
13.3 Basic Newton-Euler Mechanics . . . . . . . . . . . . . . . . . . . . . 745
13.4 Advanced Mechanics Concepts . . . . . . . . . . . . . . . . . . . . . 762
13.5 Multiple Decision Makers . . . . . . . . . . . . . . . . . . . . . . . . 780
14 Sampling-Based Planning Under Differential Constraints 787
14.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 788
14.2 Reachability and Completeness . . . . . . . . . . . . . . . . . . . . 798
14.3 Sampling-Based Motion Planning Revisited . . . . . . . . . . . . . 810
14.4 Incremental Sampling and Searching Methods . . . . . . . . . . . . 820
14.5 Feedback Planning Under Differential Constraints . . . . . . . . . . 837
14.6 Decoupled Planning Approaches . . . . . . . . . . . . . . . . . . . . 841
14.7 Gradient-Based Trajectory Optimization . . . . . . . . . . . . . . . 855
15 System Theory and Analytical Techniques 861
15.1 Basic System Properties . . . . . . . . . . . . . . . . . . . . . . . . 862
15.2 Continuous-Time Dynamic Programming . . . . . . . . . . . . . . . 870
15.3 Optimal Paths for Some Wheeled Vehicles . . . . . . . . . . . . . . 880
15.4 Nonholonomic System Theory . . . . . . . . . . . . . . . . . . . . . 888
15.5 Steering Methods for Nonholonomic Systems . . . . . . . . . . . . . 910