MODERN ROBOTICS MECHANICS, PLANNING, AND CONTROL

Preview 1 2 Configuration Space 11 2.1 Degrees of Freedom of a Rigid Body . . . . . . . . . . . . . . . . 12 2.2 Degrees of Freedom of a Robot . . . . . . . . . . . . . . . . . . . 15 2.2.1 Robot Joints . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.2.2 Gr¨ubler’s Formula . . . . . . . . . . . . . . . . . . . . . . 17 2.3 Configuration Space: Topology and Representation . . . . . . . . 23 2.3.1 Configuration Space Topology . . . . . . . . . . . . . . . . 23 2.3.2 Configuration Space Representation . . . . . . . . . . . . 25 2.4 Configuration and Velocity Constraints . . . . . . . . . . . . . . . 29 2.5 Task Space and Workspace . . . . . . . . . . . . . . . . . . . . . 32 2.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36 2.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 38 2.8 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 3 Rigid-Body Motions 59 3.1 Rigid-Body Motions in the Plane . . . . . . . . . . . . . . . . . . 62 3.2 Rotations and Angular Velocities . . . . . . . . . . . . . . . . . . 68 3.2.1 Rotation Matrices . . . . . . . . . . . . . . . . . . . . . . 68 3.2.2 Angular Velocities . . . . . . . . . . . . . . . . . . . . . . 76 3.2.3 Exponential Coordinate Representation of Rotation . . . 79 3.3 Rigid-Body Motions and Twists . . . . . . . . . . . . . . . . . . . 89 i ii Contents 3.3.1 Homogeneous Transformation Matrices . . . . . . . . . . 89 3.3.2 Twists . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 97 3.3.3 Exponential Coordinate Representation of Rigid-Body Motions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 104 3.4 Wrenches . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108 3.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111 3.6 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 113 3.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 115 3.8 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 116 4 Forward Kinematics 137 4.1 Product of Exponentials Formula . . . . . . . . . . . . . . . . . . 140 4.1.1 First Formulation: Screw Axes in the Base Frame . . . . 141 4.1.2 Examples . . . . . . . . . . . . . . . . . . . . . . . . . . . 143 4.1.3 Second Formulation: Screw Axes in the End-Effector Frame148 4.2 The Universal Robot Description Format . . . . . . . . . . . . . 152 4.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158 4.4 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159 4.5 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 160 4.6 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 160 5 Velocity Kinematics and Statics 171 5.1 Manipulator Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 178 5.1.1 Space Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 178 5.1.2 Body Jacobian . . . . . . . . . . . . . . . . . . . . . . . . 183 5.1.3 Visualizing the Space and Body Jacobian . . . . . . . . . 185 5.1.4 Relationship between the Space and Body Jacobian . . . 187 5.1.5 Alternative Notions of the Jacobian . . . . . . . . . . . . 187 5.1.6 Looking Ahead to Inverse Velocity Kinematics . . . . . . 189 5.2 Statics of Open Chains . . . . . . . . . . . . . . . . . . . . . . . . 190 5.3 Singularity Analysis . . . . . . . . . . . . . . . . . . . . . . . . . 191 5.4 Manipulability . . . . . . . . . . . . . . . . . . . . . . . . . . . . 196 5.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 200 5.6 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201 5.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 201 5.8 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 202 6 Inverse Kinematics 219 6.1 Analytic Inverse Kinematics . . . . . . . . . . . . . . . . . . . . . 221 6.1.1 6R PUMA-Type Arm . . . . . . . . . . . . . . . . . . . . 221 6.1.2 Stanford-Type Arms . . . . . . . . . . . . . . . . . . . . . 225 May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. Contents iii 6.2 Numerical Inverse Kinematics . . . . . . . . . . . . . . . . . . . . 226 6.2.1 Newton–Raphson Method . . . . . . . . . . . . . . . . . . 227 6.2.2 Numerical Inverse Kinematics Algorithm . . . . . . . . . 227 6.3 Inverse Velocity Kinematics . . . . . . . . . . . . . . . . . . . . . 232 6.4 A Note on Closed Loops . . . . . . . . . . . . . . . . . . . . . . . 234 6.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 235 6.6 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 235 6.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 236 6.8 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 236 7 Kinematics of Closed Chains 245 7.1 Inverse and Forward Kinematics . . . . . . . . . . . . . . . . . . 247 7.1.1 3×RPR Planar Parallel Mechanism . . . . . . . . . . . . . 247 7.1.2 Stewart–Gough Platform . . . . . . . . . . . . . . . . . . 249 7.1.3 General Parallel Mechanisms . . . . . . . . . . . . . . . . 251 7.2 Differential Kinematics . . . . . . . . . . . . . . . . . . . . . . . . 252 7.2.1 Stewart–Gough Platform . . . . . . . . . . . . . . . . . . 252 7.2.2 General Parallel Mechanisms . . . . . . . . . . . . . . . . 254 7.3 Singularities . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 256 7.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 261 7.5 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 262 7.6 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 263 8 Dynamics of Open Chains 271 8.1 Lagrangian Formulation . . . . . . . . . . . . . . . . . . . . . . . 272 8.1.1 Basic Concepts and Motivating Examples . . . . . . . . . 272 8.1.2 General Formulation . . . . . . . . . . . . . . . . . . . . . 277 8.1.3 Understanding the Mass Matrix . . . . . . . . . . . . . . 279 8.1.4 Lagrangian Dynamics vs. Newton–Euler Dynamics . . . . 281 8.2 Dynamics of a Single Rigid Body . . . . . . . . . . . . . . . . . . 283 8.2.1 Classical Formulation . . . . . . . . . . . . . . . . . . . . 283 8.2.2 Twist–Wrench Formulation . . . . . . . . . . . . . . . . . 288 8.2.3 Dynamics in Other Frames . . . . . . . . . . . . . . . . . 290 8.3 Newton–Euler Inverse Dynamics . . . . . . . . . . . . . . . . . . 291 8.3.1 Derivation . . . . . . . . . . . . . . . . . . . . . . . . . . . 291 8.3.2 Newton-Euler Inverse Dynamics Algorithm . . . . . . . . 294 8.4 Dynamic Equations in Closed Form . . . . . . . . . . . . . . . . . 294 8.5 Forward Dynamics of Open Chains . . . . . . . . . . . . . . . . . 298 8.6 Dynamics in the Task Space . . . . . . . . . . . . . . . . . . . . . 300 8.7 Constrained Dynamics . . . . . . . . . . . . . . . . . . . . . . . . 301 May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. iv Contents 8.8 Robot Dynamics in the URDF . . . . . . . . . . . . . . . . . . . 303 8.9 Actuation, Gearing, and Friction . . . . . . . . . . . . . . . . . . 303 8.9.1 DC Motors and Gearing . . . . . . . . . . . . . . . . . . . 305 8.9.2 Apparent Inertia . . . . . . . . . . . . . . . . . . . . . . . 310 8.9.3 Newton–Euler Inverse Dynamics Algorithm Accounting for Motor Inertias and Gearing . . . . . . . . . . . . . . . 312 8.9.4 Friction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 314 8.9.5 Joint and Link Flexibility . . . . . . . . . . . . . . . . . . 314 8.10 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 315 8.11 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 319 8.12 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 320 8.13 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 321 9 Trajectory Generation 325 9.1 Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 325 9.2 Point-to-Point Trajectories . . . . . . . . . . . . . . . . . . . . . 326 9.2.1 Straight-Line Paths . . . . . . . . . . . . . . . . . . . . . 326 9.2.2 Time Scaling a Straight-Line Path . . . . . . . . . . . . . 328 9.3 Polynomial Via Point Trajectories . . . . . . . . . . . . . . . . . 334 9.4 Time-Optimal Time Scaling . . . . . . . . . . . . . . . . . . . . . 336 9.4.1 The (s, s˙) Phase Plane . . . . . . . . . . . . . . . . . . . . 339 9.4.2 The Time-Scaling Algorithm . . . . . . . . . . . . . . . . 341 9.4.3 A Variation on the Time-Scaling Algorithm . . . . . . . . 342 9.4.4 Assumptions and Caveats . . . . . . . . . . . . . . . . . . 344 9.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 345 9.6 Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 346 9.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 347 9.8 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 348 10 Motion Planning 353 10.1 Overview of Motion Planning . . . . . . . . . . . . . . . . . . . . 353 10.1.1 Types of Motion Planning Problems . . . . . . . . . . . . 354 10.1.2 Properties of Motion Planners . . . . . . . . . . . . . . . 355 10.1.3 Motion Planning Methods . . . . . . . . . . . . . . . . . . 356 10.2 Foundations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 358 10.2.1 Configuration Space Obstacles . . . . . . . . . . . . . . . 358 10.2.2 Distance to Obstacles and Collision Detection . . . . . . . 362 10.2.3 Graphs and Trees . . . . . . . . . . . . . . . . . . . . . . . 364 10.2.4 Graph Search . . . . . . . . . . . . . . . . . . . . . . . . . 365 10.3 Complete Path Planners . . . . . . . . . . . . . . . . . . . . . . . 368 May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. Contents v 10.4 Grid Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 369 10.4.1 Multi-Resolution Grid Representation . . . . . . . . . . . 372 10.4.2 Grid Methods with Motion Constraints . . . . . . . . . . 373 10.5 Sampling Methods . . . . . . . . . . . . . . . . . . . . . . . . . . 378 10.5.1 The RRT Algorithm . . . . . . . . . . . . . . . . . . . . . 379 10.5.2 The PRM Algorithm . . . . . . . . . . . . . . . . . . . . . 384 10.6 Virtual Potential Fields . . . . . . . . . . . . . . . . . . . . . . . 386 10.6.1 A Point in C-space . . . . . . . . . . . . . . . . . . . . . . 386 10.6.2 Navigation Functions . . . . . . . . . . . . . . . . . . . . . 389 10.6.3 Workspace Potential . . . . . . . . . . . . . . . . . . . . . 390 10.6.4 Wheeled Mobile Robots . . . . . . . . . . . . . . . . . . . 391 10.6.5 Use of Potential Fields in Planners . . . . . . . . . . . . . 392 10.7 Nonlinear Optimization . . . . . . . . . . . . . . . . . . . . . . . 392 10.8 Smoothing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 394 10.9 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 394 10.10Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 397 10.11Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 398 11 Robot Control 403 11.1 Control System Overview . . . . . . . . . . . . . . . . . . . . . . 404 11.2 Error Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 405 11.2.1 Error Response . . . . . . . . . . . . . . . . . . . . . . . . 406 11.2.2 Linear Error Dynamics . . . . . . . . . . . . . . . . . . . 406 11.3 Motion Control with Velocity Inputs . . . . . . . . . . . . . . . . 413 11.3.1 Motion Control of a Single Joint . . . . . . . . . . . . . . 414 11.3.2 Motion Control of a Multi-joint Robot . . . . . . . . . . . 418 11.3.3 Task-Space Motion Control . . . . . . . . . . . . . . . . . 419 11.4 Motion Control with Torque or Force Inputs . . . . . . . . . . . . 420 11.4.1 Motion Control of a Single Joint . . . . . . . . . . . . . . 421 11.4.2 Motion Control of a Multi-joint Robot . . . . . . . . . . . 429 11.4.3 Task-Space Motion Control . . . . . . . . . . . . . . . . . 433 11.5 Force Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 434 11.6 Hybrid Motion–Force Control . . . . . . . . . . . . . . . . . . . . 437 11.6.1 Natural and Artificial Constraints . . . . . . . . . . . . . 437 11.6.2 A Hybrid Motion–Force Controller . . . . . . . . . . . . . 439 11.7 Impedance Control . . . . . . . . . . . . . . . . . . . . . . . . . . 441 11.7.1 Impedance-Control Algorithm . . . . . . . . . . . . . . . . 443 11.7.2 Admittance-Control Algorithm . . . . . . . . . . . . . . . 444 11.8 Low-Level Joint Force/Torque Control . . . . . . . . . . . . . . . 445 11.9 Other Topics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 448 May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. vi Contents 11.10Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 449 11.11Software . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 451 11.12Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 452 11.13Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 453 12 Grasping and Manipulation 461 12.1 Contact Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . 463 12.1.1 First-Order Analysis of a Single Contact . . . . . . . . . . 463 12.1.2 Contact Types: Rolling, Sliding, and Breaking Free . . . . 465 12.1.3 Multiple Contacts . . . . . . . . . . . . . . . . . . . . . . 468 12.1.4 Collections of Bodies . . . . . . . . . . . . . . . . . . . . . 472 12.1.5 Other Types of Contacts . . . . . . . . . . . . . . . . . . . 472 12.1.6 Planar Graphical Methods . . . . . . . . . . . . . . . . . . 473 12.1.7 Form Closure . . . . . . . . . . . . . . . . . . . . . . . . . 478 12.2 Contact Forces and Friction . . . . . . . . . . . . . . . . . . . . . 484 12.2.1 Friction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 484 12.2.2 Planar Graphical Methods . . . . . . . . . . . . . . . . . . 487 12.2.3 Force Closure . . . . . . . . . . . . . . . . . . . . . . . . . 489 12.2.4 Duality of Force and Motion Freedoms . . . . . . . . . . . 494 12.3 Manipulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 494 12.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 501 12.5 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 503 12.6 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 504 13 Wheeled Mobile Robots 513 13.1 Types of Wheeled Mobile Robots . . . . . . . . . . . . . . . . . . 514 13.2 Omnidirectional Wheeled Mobile Robots . . . . . . . . . . . . . . 515 13.2.1 Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . 515 13.2.2 Motion Planning . . . . . . . . . . . . . . . . . . . . . . . 520 13.2.3 Feedback Control . . . . . . . . . . . . . . . . . . . . . . . 520 13.3 Nonholonomic Wheeled Mobile Robots . . . . . . . . . . . . . . . 520 13.3.1 Modeling . . . . . . . . . . . . . . . . . . . . . . . . . . . 521 13.3.2 Controllability . . . . . . . . . . . . . . . . . . . . . . . . 528 13.3.3 Motion Planning . . . . . . . . . . . . . . . . . . . . . . . 537 13.3.4 Feedback Control . . . . . . . . . . . . . . . . . . . . . . . 542 13.4 Odometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 546 13.5 Mobile Manipulation . . . . . . . . . . . . . . . . . . . . . . . . . 548 13.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 552 13.7 Notes and References . . . . . . . . . . . . . . . . . . . . . . . . . 554 13.8 Exercises . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 555 May 2017 preprint of Modern Robotics, Lynch and Park, Cambridge U. Press, 2017. Contents