Planning Algorithms

Half-title......Page 3
Title......Page 5
Copyright......Page 6
Dedication......Page 7
Contents......Page 9
Who is the intended audience?......Page 13
Computer graphics......Page 14
Suggested use......Page 15
Acknowledgments......Page 17
PART I Introductory Material......Page 19
1.1 Planning to plan......Page 21
1.2 Motivational examples and applications......Page 22
A motion planning puzzle......Page 23
Sealing cracks in automotive assembly......Page 24
Navigating mobile robots......Page 25
Playing hide and seek......Page 27
Virtual humans and humanoid robots......Page 28
Parking cars and trailers......Page 30
Flying through the air or in space......Page 31
State......Page 32
A criterion......Page 33
1.4.1 Algorithms......Page 34
Execution......Page 36
Hierarchical inclusion......Page 37
PART II: Motion Planning......Page 38
PART III: Decision-Theoretic Planning......Page 39
PART IV: Planning Under Differential Constraints......Page 40
2 Discrete Planning......Page 41
2.1.1 Problem formulation......Page 42
2.2 Searching for feasible plans......Page 45
2.2.1 General forward search......Page 46
Breadth first......Page 47
Dijkstra’s algorithm......Page 48
A-star......Page 49
Iterative deepening......Page 50
Backward search......Page 51
Bidirectional search......Page 52
2.2.4 A unified view of the search methods......Page 53
2.3 Discrete optimal planning......Page 54
2.3.1.1 Backward value iteration......Page 56
2.3.1.2 Forward value iteration......Page 58
2.3.2 Optimal plans of unspecified lengths......Page 60
2.3.3 Dijkstra revisited......Page 64
2.4.1 A STRIPS-like representation......Page 66
2.4.2 Converting to the state-space representation......Page 69
2.5.1 Searching in a space of partial plans......Page 71
2.5.2 Building a planning graph......Page 72
Mutex conditions......Page 73
Plan extraction......Page 75
2.5.3 Planning as satisfiability......Page 76
Exercises......Page 78
PART II Motion Planning......Page 81
2. Continuous discrete......Page 82
3.1 Geometric modeling......Page 84
Convex polygons......Page 85
Nonconvex polygons......Page 86
Defining a logical predicate......Page 87
Polyhedral models......Page 88
3.1.2 Semi-algebraic models......Page 89
Nonconvex polygons and polyhedra......Page 91
Nonuniform rational B-splines (NURBS)......Page 92
Generalized cylinders......Page 93
Transforming the robot model......Page 94
Translation......Page 95
Rotation......Page 96
Combining translation and rotation......Page 97
Yaw, pitch, and roll rotations......Page 98
Determining yaw, pitch, and roll from a rotation matrix......Page 99
The homogeneous transformation matrix for 3D bodies......Page 100
Attaching bodies......Page 101
Homogeneous transformation matrices for 2D chains......Page 102
3.3.2 A 3D kinematic chain......Page 104
Two screws......Page 106
The homogeneous transformation matrix......Page 107
Motivation......Page 111
Junctions with more than two rotation axes......Page 112
Constraining parameters......Page 113
What if there are loops?......Page 115
Linear transformations......Page 117
Flexible materials......Page 118
Exercises......Page 119
Implementations......Page 121
4.1.1 Topological spaces......Page 123
Special points......Page 124
Some examples......Page 125
Continuous functions......Page 126
Homeomorphism: Making a donut into a coffee cup......Page 127
4.1.2 Manifolds......Page 128
Cartesian products......Page 129
Identifications......Page 130
2D manifolds......Page 131
Higher dimensional manifolds......Page 132
Connected vs. path connected......Page 133
Simply connected......Page 134
The fundamental group......Page 135
Matrix groups......Page 138
Interpreting the C-space......Page 140
Using complex numbers to represent SO(2)......Page 141
Quaternions......Page 142
Using quaternion multiplication......Page 144
Special Euclidean group......Page 145
4.2.3 Chains and trees of bodies......Page 146
Obstacle region for multiple bodies......Page 147
Definition of basic motion planning......Page 148
4.3.2 Explicitly modeling Cobs: The translational case......Page 149
A polygonal C-space obstacle......Page 150
Computing the boundary of Cobs......Page 151
A polyhedral C-space obstacle......Page 153
4.3.3 Explicitly modeling Cobs: The general case......Page 154
Chains and trees of bodies......Page 156
Fields......Page 157
Polynomials......Page 158
Varieties......Page 159
Two links......Page 160
A one-dimensional variety......Page 161
Three links......Page 163
4.4.3 Defining the variety for general linkages......Page 165
Further reading......Page 167
Exercises......Page 168
Implementations......Page 170
Notions of completeness......Page 171
5.1.1 Metric spaces......Page 172
Cartesian products of metric spaces......Page 173
5.1.2 Important metric spaces for motion planning......Page 174
5.1.3 Basic measure theory definitions......Page 176
5.1.4 Using the correct measure......Page 178
Denseness......Page 179
The van der Corput sequence......Page 180
Generating a random element of SO(3)......Page 182
Pseudorandom number generation......Page 183
Testing for randomness......Page 184
Dispersion definition......Page 185
Making good grids......Page 186
Infinite grid sequences......Page 187
5.2.4 Low-discrepancy sampling......Page 188
Low-discrepancy sampling methods......Page 189
5.3 Collision detection......Page 191
5.3.2 Hierarchical methods......Page 192
5.3.3 Incremental methods......Page 194
5.3.4 Checking a path segment......Page 195
5.4.1 The general framework......Page 198
5.4.2 Adapting discrete search algorithms......Page 200
Neighborhoods......Page 201
Obtaining a discrete planning problem......Page 202
Grid resolution issues......Page 203
5.4.3 Randomized potential fields......Page 204
Ariadne’s clew algorithm......Page 206
5.5 Rapidly exploring dense trees......Page 207
5.5.1 The exploration algorithm......Page 208
Exact solutions......Page 210
Approximate solutions......Page 211
Single-tree search......Page 212
Balanced, bidirectional search......Page 213
5.6.1 The basic method......Page 214
Generic preprocessing phase......Page 215
Query phase......Page 216
Some analysis......Page 217
5.6.3 Heuristics for improving roadmaps......Page 218
Gaussian sampling [134]......Page 219
Further reading......Page 220
Exercises......Page 221
Reasons to study combinatorial methods......Page 224
Roadmaps......Page 225
6.2.1 Representation......Page 226
6.2.2 Vertical cell decomposition......Page 227
Defining the vertical decomposition......Page 228
Solving a query......Page 229
Algorithm execution......Page 231
6.2.3 Maximum-clearance roadmaps......Page 233
6.2.4 Shortest-path roadmaps......Page 234
6.3 Cell decompositions......Page 236
Simplicial complex......Page 237
Singular complex......Page 238
6.3.2 2D decompositions......Page 239
Triangulation......Page 240
Cylindrical decomposition......Page 241
6.3.3 3D vertical decomposition......Page 242
6.3.4 A decomposition for a line-segment robot......Page 243
An approximate solution......Page 244
Radar maps......Page 245
Critical changes in cells......Page 246
Constructing the roadmap......Page 248
6.4 Computational algebraic geometry......Page 250
Tarski sentences......Page 251
The quantifier-elimination problem......Page 252
Semi-algebraic decomposition......Page 253
6.4.2 Cylindrical algebraic decomposition......Page 254
Real algebraic numbers......Page 255
One-dimensional decomposition......Page 256
The inductive step to higher dimensions......Page 257
Solving a motion planning problem......Page 260
6.4.3 Canny’s roadmap algorithm......Page 261
Languages......Page 265
Hardness and completeness......Page 266
Lower bounds for motion planning......Page 267
6.5.2 Davenport-Schinzel sequences......Page 268
General algorithms......Page 270
Specialized algorithms......Page 271
Further reading......Page 272
Exercises......Page 273
Implementations......Page 274
7.1.1 Problem formulation......Page 275
Sampling-based methods......Page 277
Bounded speed......Page 278
7.1.3 The velocity-tuning method......Page 280
7.2.1 Problem formulation......Page 281
An ordinary motion planning problem?......Page 282
Assembly planning......Page 283
Prioritized planning......Page 284
Fixed-path coordination......Page 285
Fixed-roadmap coordination......Page 286
Cube complex......Page 287
7.3.1 Hybrid systems framework......Page 288
Admissible configurations......Page 292
Manipulation graph......Page 294
Multiple parts......Page 295
7.4.1 Adaptation of motion planning algorithms......Page 297
Closure constraints......Page 298
Sampling-based methods......Page 299
Stepping along Cclo......Page 300
7.4.2 Active-passive link decompositions......Page 301
Active and passive variables......Page 302
Loop generator......Page 303
Computing bounds on joint angles......Page 304
Carton folding......Page 305
Drug design......Page 307
Boustrophedon decomposition......Page 310
Spanning tree covering......Page 311
Euclidean shortest paths......Page 313
General optimality criteria......Page 314
7.7.2 Multiple-robot optimality......Page 317
Scalarization......Page 318
Further reading......Page 319
Exercises......Page 320
Implementations......Page 321
8.1 Motivation......Page 322
8.2.1 Defining a feedback plan......Page 324
Feasibility and optimality......Page 325
Navigation functions......Page 326
Computing navigation functions......Page 328
Wavefront propagation algorithms......Page 329
Maximum clearance......Page 330
Other extensions......Page 331
Vector spaces......Page 332
Vector fields......Page 334
An integral curve......Page 336
Piecewise-smooth vector fields......Page 337
8.3.2 Smooth manifolds......Page 339
Coordinates and parameterizations......Page 340
Tangent spaces on manifolds......Page 343
8.4.1 Feedback motion planning definitions......Page 346
8.4.1.1 Solution concepts......Page 347
8.4.1.2 Navigation functions......Page 348
8.4.2 Vector fields over cell complexes......Page 350
8.4.3 Optimal navigation functions......Page 352
8.4.4 A step toward considering dynamics......Page 353
8.4.4.1 An acceleration-based control model......Page 354
8.4.4.2 Velocity and acceleration constraints......Page 355
8.4.4.3 Navigation function in the sense of Rimon-Koditschek......Page 356
8.4.4.4 Harmonic potential functions......Page 357
8.5.1 Computing a composition of funnels......Page 358
8.5.1.1 An approximate cover......Page 359
8.5.1.2 Defining a feedback plan over a cover......Page 360
8.5.1.3 A sampling-based approach......Page 361
Defining new neighborhoods......Page 362
Termination......Page 363
8.5.2.1 Using interpolation for continuous state spaces......Page 364
8.5.2.2 The connection to feedback motion planning......Page 366
Obtaining a state transition equation......Page 367
Handling obstacles......Page 368
8.5.2.3 Obtaining Dijkstra-like algorithms......Page 369
Exercises......Page 372
Implementations......Page 373
PART III Decision-Theoretic Planning......Page 375
Uncertainty in predictability......Page 376
Uncertainty in sensing: The information space......Page 377
9 Basic Decision Theory......Page 378
9.1.1.1 Optimizing a single objective......Page 379
9.1.1.2 Multiobjective optimization......Page 380
9.1.2 Probability theory review......Page 381
Conditional probability......Page 382
Random variables......Page 383
9.1.3 Randomized strategies......Page 384
9.2.1 Modeling nature......Page 386
9.2.2 Nondeterministic vs. probabilistic models......Page 387
9.2.3 Making use of observations......Page 389
Optimal strategies......Page 390
Nature acts twice......Page 391
9.2.4 Examples of optimal decision making......Page 392
A Bayesian classifier......Page 393
9.2.4.2 Parameter estimation......Page 395
9.3.1 Game formulation......Page 396
9.3.2 Deterministic strategies......Page 397
Saddle points......Page 399
9.3.3 Randomized strategies......Page 400
9.3.3.1 Extending the formulation......Page 401
9.3.3.2 Computation of randomized saddle points......Page 402
9.4.1 Two-player games......Page 404
9.4.1.1 Dealing with multiple Nash equilibria......Page 406
9.4.1.2 Randomized Nash equilibria......Page 408
Summary of possible solutions......Page 409
9.4.2 More than two players......Page 410
9.5.1 Utility theory and rationality......Page 411
9.5.1.1 Comparing rewards......Page 412
9.5.1.3 Constructing a utility function......Page 415
9.5.2 Concerns regarding the probabilistic model......Page 416
9.5.2.2 The source of prior distributions......Page 417
9.5.2.3 Incorrect assumptions on conditional distributions......Page 419
9.5.3 Concerns regarding the nondeterministic model......Page 420
9.5.4 Concerns regarding game theory......Page 421
Further reading......Page 422
Exercises......Page 423
Implementations......Page 425
10.1 Introducing sequential games against nature......Page 426
10.1.1 Model definition......Page 427
Nondeterministic forward projections......Page 431
Probabilistic forward projections......Page 432
Backprojections......Page 433
Defining a plan......Page 434
Graph representations of a plan......Page 435
The cost of a feedback plan......Page 436
Nondeterministic case......Page 437
Probabilistic case......Page 438
Convergence issues......Page 439
Using the plan......Page 441
10.2.2 Policy iteration......Page 442
Backward search with backprojections......Page 445
Nondeterministic Dijkstra......Page 446
Probabilistic Dijkstra......Page 447
10.3.1 Problem formulation......Page 448
Discounted cost......Page 449
10.3.2 Solution techniques......Page 450
Value iteration for discounted cost......Page 451
Solutions for the average cost-per-stage model......Page 452
Terminology......Page 453
The general framework......Page 454
10.4.2 Evaluating a plan via simulation......Page 455
Temporal differences......Page 456
Value iteration......Page 459
10.5.1 Game trees......Page 460
10.5.1.1 Determining a security plan......Page 463
10.5.1.2 Computing a saddle point......Page 465
10.5.1.3 Converting the tree to a single-stage game......Page 466
10.5.2 Sequential games on state spaces......Page 467
Saddle points in a sequential game......Page 468
Value iteration......Page 469
Nash equilibria in sequential games......Page 470
Introducing nature......Page 471
Introducing more players......Page 472
10.6.0.1 Extending the value-iteration method......Page 473
10.6.0.2 Motion planning with nature......Page 474
Exercises......Page 477
Implementations......Page 478
11 Sensors and Information Spaces......Page 480
11.1.1 Sensors......Page 481
History......Page 485
The history information space......Page 486
11.1.3 Defining a planning problem......Page 487
The cost of a plan......Page 488
The information space is just another state space......Page 489
Making smaller information-feedback plans......Page 490
Constructing a derived information transition equation......Page 491
11.2.2 Nondeterministic information spaces......Page 493
11.2.3 Probabilistic information spaces......Page 495
Previous i stages......Page 497
11.3.1 Basic nondeterministic examples......Page 498
11.3.2 Nondeterministic finite automata......Page 501
11.3.3 The probabilistic case: POMDPs......Page 504
11.4.1 Discrete-stage information spaces......Page 505
11.4.2 Continuous-time information spaces......Page 506
11.4.3.1 Nondeterministic and probabilistic I-spaces for discrete stages......Page 507
Conservative approximations......Page 508
Moment-based approximations......Page 510
Linear sensing models......Page 512
Simple projection sensors......Page 514
Landmark sensors......Page 515
Depth-mapping sensors......Page 517
11.5.2 Simple projection examples......Page 518
11.5.3 Examples with nature sensing actions......Page 521
11.5.4 Gaining information without sensors......Page 524
11.6 Computing probabilistic information states......Page 525
11.6.1 Kalman filtering......Page 526
A grid-based approach......Page 528
Particle filtering......Page 529
11.7.1 Information States in Game Trees......Page 530
11.7.2 Information spaces for games on state spaces......Page 533
Further reading......Page 536
Exercises......Page 537
Implementations......Page 539
12 Planning Under Sensing Uncertainty......Page 540
12.1.1 The information space as a big state space......Page 541
Value iteration......Page 543
12.1.3 Algorithms for probabilistic I-spaces (POMDPs)......Page 544
The sensorless case......Page 545
12.2.1 Discrete localization......Page 546
Solving the passive localization problem......Page 548
Solving the active localization problem......Page 549
Other information models......Page 551
12.2.2 Combinatorial methods for continuous localization......Page 552
12.2.3 Probabilistic methods for localization......Page 555
Discrete problems......Page 556
Continuous problems......Page 557
12.3.1 Grid problems......Page 558
Algorithms for navigation......Page 562
Algorithms for maze searching......Page 563
12.3.2 Stentz’s algorithm (D*)......Page 565
Interpretation in terms of I-spaces......Page 567
12.3.3 Planning in unknown continuous environments......Page 568
The Bug1 strategy......Page 569
The Bug2 strategy......Page 570
Using range data......Page 571
Competitive ratios......Page 572
12.3.4 Optimal navigation without a geometric model......Page 573
Critical gap events......Page 574
I-space interpretation......Page 578
12.3.5 Probabilistic localization and mapping......Page 579
The EM algorithm......Page 580
12.4.1 Problem formulation......Page 582
12.4.2 A complete algorithm......Page 585
12.4.3 Other variations......Page 587
12.5 Manipulation planning with sensing uncertainty......Page 588
Compliant motions......Page 589
Sources of uncertainty......Page 590
Planning with motion commands......Page 591
Backprojections......Page 592
Computing backprojections......Page 594
12.5.2 Nonprehensile manipulation......Page 595
Squeezing parts......Page 596
Further reading......Page 599
Exercises......Page 600
Implementations......Page 602
PART IV Planning Under Differential Constraints......Page 605
Overview of Part IV: Planning Under Differential Constraints......Page 606
13.1 Velocity constraints on the configuration space......Page 608
The planar case......Page 609
General configuration spaces......Page 610
13.1.1.2 Parametric constraints......Page 611
13.1.1.3 Conversion from implicit to parametric form......Page 612
13.1.2.1 A simple car......Page 614
13.1.2.2 A differential drive......Page 617
13.1.2.3 A simple unicycle......Page 619
13.1.2.4 A car pulling trailers......Page 620
13.1.3.1 Pushing a box......Page 621
13.1.3.2 Flying an airplane......Page 622
13.1.3.4 Trapped on a surface......Page 623
13.2 Phase space representation of dynamical systems......Page 624
13.2.1.1 The scalar case......Page 625
13.2.1.3 Higher order differential constraints......Page 627
13.2.2 Linear systems......Page 628
13.2.3 Nonlinear systems......Page 629
13.2.4 Extending models by adding integrators......Page 630
13.2.4.2 A continuous-steering car......Page 631
13.2.4.3 Smooth differential drive......Page 632
13.3.1 The Newtonian model......Page 633
Newton’s laws......Page 634
13.3.2.1 Motion of a single particle......Page 635
13.3.2.2 Motion of a set of particles......Page 639
13.3.3 Motion of a rigid body......Page 640
The rotational part......Page 641
Differential rotations......Page 642
Inertia matrix......Page 643
Simplifying the inertia matrix......Page 644
Completing the state transition equation......Page 645
The 2D case......Page 646
A car with tire skidding......Page 647
13.4.1.1 Calculus of variations......Page 648
13.4.1.2 Hamilton’s principle of least action......Page 651
13.4.1.4 Procedure for deriving the state transition equation......Page 653
13.4.2 General lagrangian expressions......Page 654
13.4.2.1 Conversion to a phase transition equation......Page 655
13.4.3 Extensions of the Euler-Lagrange equations......Page 657
13.4.3.1 Incorporating velocity constraints......Page 658
13.4.3.2 Nonconservative forces......Page 660
13.4.4 Hamiltonian mechanics......Page 661
13.5.1 Differential decision making......Page 663
13.5.2 Differential game theory......Page 664
Further reading......Page 666
Exercises......Page 667
Implementations......Page 668
14 Sampling-Based Planning Under Differential Constraints......Page 669
14.1.1 Problem formulation......Page 670
Nonholonomic planning......Page 672
14.1.2.2 Terms from control theory......Page 673
Symmetric systems......Page 674
14.1.3.1 Additional constraints on phase variables......Page 675
14.1.3.2 The region of inevitable collision......Page 676
14.2.1.1 Reachable set......Page 678
14.2.1.2 Time-limited reachable set......Page 679
14.2.1.3 Backward reachable sets......Page 680
14.2.2 The discrete-time model......Page 681
14.2.2.1 Reachability graph......Page 682
14.2.2.2 Resolution completeness for x = u......Page 683
14.2.2.3 Resolution completeness for x = f(x, u)......Page 684
14.2.3 Motion primitives......Page 686
Riemannian metrics......Page 688
14.3.1.2 Sampling theory......Page 689
14.3.2 System simulator......Page 690
Euler method......Page 691
Multistep methods......Page 692
14.3.3 Local planning......Page 693
14.3.4 General framework under differential constraints......Page 694
14.4.1 Searching on a lattice......Page 696
14.4.1.1 A double-integrator lattice......Page 697
Alternative lattices for the double integrator......Page 699
Unconstrained mechanical systems......Page 700
Approximation algorithm framework......Page 701
Underactuated and nonholonomic systems......Page 702
Searching......Page 703
Resolution issues......Page 705
14.4.3 RDT-based methods......Page 706
Tree-based planners......Page 708
Ensuring resolution completeness......Page 709
Other tree-based planners......Page 710
14.5.1 Problem definition......Page 711
14.5.2 Dynamic programming with interpolation......Page 712
14.6.1 Different ways to decouple the big problem......Page 714
14.6.2 Plan and transform......Page 715
14.6.3.1 Expressing systems in terms of s, s,and s......Page 718
14.6.3.2 Determining the allowable accelerations......Page 719
14.6.3.3 The path-constrained phase space......Page 720
14.6.3.4 Computing optimal solutions via dynamic programming......Page 722
14.6.3.5 A bang-bang approach for time optimality......Page 724
14.7 Gradient-based trajectory optimization......Page 725
Further reading......Page 727
Exercises......Page 728
Implementations......Page 729
15.1 Basic system properties......Page 730
Equilibrium points and Lyapunov stability......Page 731
Asymptotic stability......Page 732
15.1.2 Lyapunov functions......Page 733
Determining stability......Page 734
Classical controllability......Page 735
STLC: Controllability that handles obstacles......Page 736
15.2.1.1 The discrete case......Page 738
15.2.1.2 The continuous case......Page 739
15.2.1.3 Variants of the HJB equation......Page 740
15.2.2 Linear-quadratic problems......Page 741
15.2.3 Pontryagin’s minimum principle......Page 742
Time optimality......Page 745
15.3 Optimal paths for some wheeled vehicles......Page 746
15.3.1 Dubins curves......Page 747
15.3.2 Reeds-Shepp curves......Page 750
15.3.3 Balkcom-Mason curves......Page 752
15.4.1 Control-affine systems......Page 754
15.4.2.1 Completely integrable or nonholonomic?......Page 757
15.4.2.2 Distributions......Page 758
15.4.2.3 Lie brackets......Page 760
15.4.3.1 The Lie algebra......Page 766
15.4.3.2 Lie algebra of the system distribution......Page 768
15.4.3.3 Philip Hall basis of a Lie algebra......Page 769
15.4.3.5 Handling control-affine systems with drift......Page 770
15.5 Steering methods for nonholonomic systems......Page 771
15.5.1 Using the P. Hall basis......Page 772
The exponential map......Page 773
The Chen-Fliess-Sussmann equation......Page 775
15.5.2 Using sinusoidal action trajectories......Page 777
15.5.2.1 Steering the nonholonomic integrator......Page 778
15.5.2.2 First-order controllable systems......Page 779
15.5.2.3 Chained-form systems......Page 780
Decoupling vector fields......Page 781
Further reading......Page 782
Exercises......Page 783
Implementations......Page 784
Bibliography......Page 785
Index......Page 829