Bruno Siciliano • Lorenzo Sciavicco Luigi Villani • Giuseppe Oriolo Contents
Robotics Modelling, Planning and Control 1 Introduction … … … … … … … … … … … … … … … . . 1 1.1 Robotics … … … … … … … … … … … … … … … . 1 1.2 Robot Mechanical Structure … … … … … … … … … . . 3 1.2.1 Robot Manipulators … … … … … … … … … … 4 1.2.2 Mobile Robots … … … … … … … … … … … . . 10 1.3 Industrial Robotics … … … … … … … … … … … … . 15 1.4 Advanced Robotics … … … … … … … … … … … … . 25 1.4.1 Field Robots … … … … … … … … … … … … 26 1.4.2 Service Robots … … … … … … … … … … … . . 27 1.5 Robot Modelling, Planning and Control … … … … … … . 29 1.5.1 Modelling … … … … … … … … … … … … … 30 1.5.2 Planning … … … … … … … … … … … … … . 32 1.5.3 Control … … … … … … … … … … … … … . . 32 Bibliography … … … … … … … … … … … … … … . 33
2 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
2.1 Pose of a Rigid Body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
2.2 Rotation Matrix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40
2.2.1 Elementary Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41
2.2.2 Representation of a Vector . . . . . . . . . . . . . . . . . . . . . . . . 42
2.2.3 Rotation of a Vector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
2.3 Composition of Rotation Matrices . . . . . . . . . . . . . . . . . . . . . . . . 45
2.4 Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
2.4.1 ZYZ Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 49
2.4.2 RPY Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51
2.5 Angle and Axis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
2.6 Unit Quaternion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
2.7 Homogeneous Transformations . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
123 2.8 Direct Kinematics … … … … … … … … … … … … . . 58 2.8.1 Open Chain … … … … … … … … … … … … . 60 2.8.2 Denavit–Hartenberg Convention … … … … … … . 61 xviii Contents Contents xix
2.8.3 Closed Chain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65 3.7.3 Orientation Error . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137
2.9 Kinematics of Typical Manipulator Structures . . . . . . . . . . . . . 68 3.7.4 Second-order Algorithms . . . . . . . . . . . . . . . . . . . . . . . . . . 141
2.9.1 Three-link Planar Arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69 3.7.5 Comparison Among Inverse Kinematics Algorithms . . . 143
2.9.2 Parallelogram Arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70 3.8 Statics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147
2.9.3 Spherical Arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72 3.8.1 Kineto-Statics Duality . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148
2.9.4 Anthropomorphic Arm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73 3.8.2 Velocity and Force Transformation . . . . . . . . . . . . . . . . . 149
2.9.5 Spherical Wrist . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75 3.8.3 Closed Chain . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151
2.9.6 Stanford Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76 3.9 Manipulability Ellipsoids . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152
2.9.7 Anthropomorphic Arm with Spherical Wrist . . . . . . . . . 77 Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158
2.9.8 DLR Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79 Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 159
2.9.9 Humanoid Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
2.10 Joint Space and Operational Space . . . . . . . . . . . . . . . . . . . . . . . 83 4 Trajectory Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161
2.10.1 Workspace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 85 4.1 Path and Trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161
2.10.2 Kinematic Redundancy . . . . . . . . . . . . . . . . . . . . . . . . . . . 87 4.2 Joint Space Trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162
2.11 Kinematic Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88 4.2.1 Point-to-Point Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163
2.12 Inverse Kinematics Problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90 4.2.2 Motion Through a Sequence of Points . . . . . . . . . . . . . . 168
2.12.1 Solution of Three-link Planar Arm . . . . . . . . . . . . . . . . . 91 4.3 Operational Space Trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . 179
2.12.2 Solution of Manipulators with Spherical Wrist . . . . . . . 94 4.3.1 Path Primitives . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181
2.12.3 Solution of Spherical Arm . . . . . . . . . . . . . . . . . . . . . . . . . 95 4.3.2 Position . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184
2.12.4 Solution of Anthropomorphic Arm . . . . . . . . . . . . . . . . . 96 4.3.3 Orientation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 187
2.12.5 Solution of Spherical Wrist . . . . . . . . . . . . . . . . . . . . . . . . 99 Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 188
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100 Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 189
Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100
5 Actuators and Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 191
3 Differential Kinematics and Statics … … … … … … … … 105 5.1 Joint Actuating System … … … … … … … … … … … 191 3.1 Geometric Jacobian … … … … … … … … … … … … 105 5.1.1 Transmissions … … … … … … … … … … … . . 192 3.1.1 Derivative of a Rotation Matrix … … … … … … . . 106 5.1.2 Servomotors … … … … … … … … … … … … . 193 3.1.2 Link Velocities … … … … … … … … … … … . . 108 5.1.3 Power Amplifiers … … … … … … … … … … … 197 3.1.3 Jacobian Computation … … … … … … … … … . 111 5.1.4 Power Supply … … … … … … … … … … … … 198 3.2 Jacobian of Typical Manipulator Structures … … … … … 113 5.2 Drives … … … … … … … … … … … … … … … … 198 3.2.1 Three-link Planar Arm … … … … … … … … … . 113 5.2.1 Electric Drives … … … … … … … … … … … . . 198 3.2.2 Anthropomorphic Arm … … … … … … … … … . 114 5.2.2 Hydraulic Drives … … … … … … … … … … … 202 3.2.3 Stanford Manipulator … … … … … … … … … . . 115 5.2.3 Transmission Effects … … … … … … … … … … 204 3.3 Kinematic Singularities … … … … … … … … … … … 116 5.2.4 Position Control … … … … … … … … … … … 206 3.3.1 Singularity Decoupling … … … … … … … … … . 117 5.3 Proprioceptive Sensors … … … … … … … … … … … . 209 3.3.2 Wrist Singularities … … … … … … … … … … . 119 5.3.1 Position Transducers … … … … … … … … … . . 210 3.3.3 Arm Singularities … … … … … … … … … … . . 119 5.3.2 Velocity Transducers … … … … … … … … … . . 214 3.4 Analysis of Redundancy … … … … … … … … … … … 121 5.4 Exteroceptive Sensors … … … … … … … … … … … . . 215 3.5 Inverse Differential Kinematics … … … … … … … … … 123 5.4.1 Force Sensors … … … … … … … … … … … … 215 3.5.1 Redundant Manipulators … … … … … … … … . . 124 5.4.2 Range Sensors … … … … … … … … … … … . . 219 3.5.2 Kinematic Singularities … … … … … … … … … 127 5.4.3 Vision Sensors … … … … … … … … … … … . . 225 3.6 Analytical Jacobian … … … … … … … … … … … … 128 Bibliography … … … … … … … … … … … … … … . 230 3.7 Inverse Kinematics Algorithms … … … … … … … … … 132 Problems … … … … … … … … … … … … … … … . 230 3.7.1 Jacobian (Pseudo-)inverse … … … … … … … … . 133 3.7.2 Jacobian Transpose … … … … … … … … … … . 134 xx Contents Contents xxi
6 Control Architecture … … … … … … … … … … … … . . 233 8.6 Operational Space Control … … … … … … … … … … 343 6.1 Functional Architecture … … … … … … … … … … … 233 8.6.1 General Schemes … … … … … … … … … … … 344 6.2 Programming Environment … … … … … … … … … … 238 8.6.2 PD Control with Gravity Compensation … … … … 345 6.2.1 Teaching-by-Showing … … … … … … … … … . . 240 8.6.3 Inverse Dynamics Control … … … … … … … … . 347 6.2.2 Robot-oriented Programming … … … … … … … . 241 8.7 Comparison Among Various Control Schemes … … … … . . 349 6.3 Hardware Architecture … … … … … … … … … … … . 242 Bibliography … … … … … … … … … … … … … … . 359 Bibliography … … … … … … … … … … … … … … . 245 Problems … … … … … … … … … … … … … … … . 360 Problems … … … … … … … … … … … … … … … . 245 9 Force Control … … … … … … … … … … … … … … … . 363 7 Dynamics … … … … … … … … … … … … … … … … . . 247 9.1 Manipulator Interaction with Environment … … … … … . 363 7.1 Lagrange Formulation … … … … … … … … … … … . 247 9.2 Compliance Control … … … … … … … … … … … … 364 7.1.1 Computation of Kinetic Energy … … … … … … . . 249 9.2.1 Passive Compliance … … … … … … … … … … 366 7.1.2 Computation of Potential Energy … … … … … … 255 9.2.2 Active Compliance … … … … … … … … … … . 367 7.1.3 Equations of Motion … … … … … … … … … … 255 9.3 Impedance Control … … … … … … … … … … … … . 372 7.2 Notable Properties of Dynamic Model … … … … … … . . 257 9.4 Force Control … … … … … … … … … … … … … … 378 7.2.1 Skew-symmetry of Matrix Ḃ − 2C … … … … … . . 257 9.4.1 Force Control with Inner Position Loop … … … … . 379 7.2.2 Linearity in the Dynamic Parameters … … … … … 259 9.4.2 Force Control with Inner Velocity Loop … … … … . 380 7.3 Dynamic Model of Simple Manipulator Structures … … … . 264 9.4.3 Parallel Force/Position Control … … … … … … . . 381 7.3.1 Two-link Cartesian Arm … … … … … … … … . . 264 9.5 Constrained Motion … … … … … … … … … … … … 384 7.3.2 Two-link Planar Arm … … … … … … … … … . . 265 9.5.1 Rigid Environment … … … … … … … … … … . 385 7.3.3 Parallelogram Arm … … … … … … … … … … . 277 9.5.2 Compliant Environment … … … … … … … … … 389 7.4 Dynamic Parameter Identification … … … … … … … … 280 9.6 Natural and Artificial Constraints … … … … … … … … 391 7.5 Newton–Euler Formulation … … … … … … … … … … 282 9.6.1 Analysis of Tasks … … … … … … … … … … . . 392 7.5.1 Link Accelerations … … … … … … … … … … . 285 9.7 Hybrid Force/Motion Control … … … … … … … … … . 396 7.5.2 Recursive Algorithm … … … … … … … … … … 286 9.7.1 Compliant Environment … … … … … … … … … 397 7.5.3 Example … … … … … … … … … … … … … . 289 9.7.2 Rigid Environment … … … … … … … … … … . 401 7.6 Direct Dynamics and Inverse Dynamics … … … … … … . 292 Bibliography … … … … … … … … … … … … … … . 403 7.7 Dynamic Scaling of Trajectories … … … … … … … … . . 294 Problems … … … … … … … … … … … … … … … . 404 7.8 Operational Space Dynamic Model … … … … … … … . . 296 7.9 Dynamic Manipulability Ellipsoid … … … … … … … … 299 10 Visual Servoing … … … … … … … … … … … … … … . . 407 Bibliography … … … … … … … … … … … … … … . 301 10.1 Vision for Control … … … … … … … … … … … … . . 407 Problems … … … … … … … … … … … … … … … . 301 10.1.1 Configuration of the Visual System … … … … … . . 409 10.2 Image Processing … … … … … … … … … … … … … 410 8 Motion Control … … … … … … … … … … … … … … . . 303 10.2.1 Image Segmentation … … … … … … … … … … 411 8.1 The Control Problem … … … … … … … … … … … . . 303 10.2.2 Image Interpretation … … … … … … … … … … 416 8.2 Joint Space Control … … … … … … … … … … … … 305 10.3 Pose Estimation … … … … … … … … … … … … … . 418 8.3 Decentralized Control … … … … … … … … … … … . . 309 10.3.1 Analytic Solution … … … … … … … … … … . . 419 8.3.1 Independent Joint Control … … … … … … … … 311 10.3.2 Interaction Matrix … … … … … … … … … … . 424 8.3.2 Decentralized Feedforward Compensation … … … . . 319 10.3.3 Algorithmic Solution … … … … … … … … … . . 427 8.4 Computed Torque Feedforward Control … … … … … … . 324 10.4 Stereo Vision … … … … … … … … … … … … … … 433 8.5 Centralized Control … … … … … … … … … … … … . 327 10.4.1 Epipolar Geometry … … … … … … … … … … . 433 8.5.1 PD Control with Gravity Compensation … … … … 328 10.4.2 Triangulation … … … … … … … … … … … … 435 8.5.2 Inverse Dynamics Control … … … … … … … … . 330 10.4.3 Absolute Orientation … … … … … … … … … . . 436 8.5.3 Robust Control … … … … … … … … … … … . 333 10.4.4 3D Reconstruction from Planar Homography … … . . 438 8.5.4 Adaptive Control … … … … … … … … … … . . 338 10.5 Camera Calibration … … … … … … … … … … … … 440 xxii Contents Contents xxiii
10.6 The Visual Servoing Problem . . . . . . . . . . . . . . . . . . . . . . . . . . . . 443 12.5.2 Bidirectional RRT Method . . . . . . . . . . . . . . . . . . . . . . . . 543
10.7 Position-based Visual Servoing . . . . . . . . . . . . . . . . . . . . . . . . . . . 445 12.6 Planning via Artificial Potentials . . . . . . . . . . . . . . . . . . . . . . . . . 546
10.7.1 PD Control with Gravity Compensation . . . . . . . . . . . . 446 12.6.1 Attractive Potential . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 546
10.7.2 Resolved-velocity Control . . . . . . . . . . . . . . . . . . . . . . . . . 447 12.6.2 Repulsive Potential . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 547
10.8 Image-based Visual Servoing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 449 12.6.3 Total Potential . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 549
10.8.1 PD Control with Gravity Compensation . . . . . . . . . . . . 449 12.6.4 Planning Techniques . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 550
10.8.2 Resolved-velocity Control . . . . . . . . . . . . . . . . . . . . . . . . . 451 12.6.5 The Local Minima Problem . . . . . . . . . . . . . . . . . . . . . . . 551
10.9 Comparison Among Various Control Schemes . . . . . . . . . . . . . . 453 12.7 The Robot Manipulator Case . . . . . . . . . . . . . . . . . . . . . . . . . . . . 554
10.10 Hybrid Visual Servoing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 460 Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 557
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 465 Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 557
Problems . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 466
11 Mobile Robots … … … … … … … … … … … … … … … 469 Appendices 11.1 Nonholonomic Constraints … … … … … … … … … … . 469 11.1.1 Integrability Conditions … … … … … … … … … 473 A Linear Algebra … … … … … … … … … … … … … … . . 563 11.2 Kinematic Model … … … … … … … … … … … … … 476 A.1 Definitions … … … … … … … … … … … … … … . . 563 11.2.1 Unicycle … … … … … … … … … … … … … . 478 A.2 Matrix Operations … … … … … … … … … … … … . 565 11.2.2 Bicycle … … … … … … … … … … … … … . . 479 A.3 Vector Operations … … … … … … … … … … … … . . 569 11.3 Chained Form … … … … … … … … … … … … … . . 482 A.4 Linear Transformation … … … … … … … … … … … . 572 11.4 Dynamic Model … … … … … … … … … … … … … . 485 A.5 Eigenvalues and Eigenvectors … … … … … … … … … . 573 11.5 Planning … … … … … … … … … … … … … … … . 489 A.6 Bilinear Forms and Quadratic Forms … … … … … … … . 574 11.5.1 Path and Timing Law … … … … … … … … … . 489 A.7 Pseudo-inverse … … … … … … … … … … … … … . . 575 11.5.2 Flat Outputs … … … … … … … … … … … … 491 A.8 Singular Value Decomposition … … … … … … … … … 577 11.5.3 Path Planning … … … … … … … … … … … . . 492 Bibliography … … … … … … … … … … … … … … . 578 11.5.4 Trajectory Planning … … … … … … … … … … 498 11.5.5 Optimal Trajectories … … … … … … … … … . . 499 B Rigid-body Mechanics … … … … … … … … … … … … . 579 11.6 Motion Control … … … … … … … … … … … … … . 502 B.1 Kinematics … … … … … … … … … … … … … … . . 579 11.6.1 Trajectory Tracking … … … … … … … … … … 503 B.2 Dynamics … … … … … … … … … … … … … … … 581 11.6.2 Regulation … … … … … … … … … … … … . . 510 B.3 Work and Energy … … … … … … … … … … … … . . 584 11.7 Odometric Localization … … … … … … … … … … … 514 B.4 Constrained Systems … … … … … … … … … … … . . 585 Bibliography … … … … … … … … … … … … … … . 518 Bibliography … … … … … … … … … … … … … … . 588 Problems … … … … … … … … … … … … … … … . 518 C Feedback Control … … … … … … … … … … … … … … 589 12 Motion Planning … … … … … … … … … … … … … … 523 C.1 Control of Single-input/Single-output Linear Systems … … . 589 12.1 The Canonical Problem … … … … … … … … … … … 523 C.2 Control of Nonlinear Mechanical Systems … … … … … … 594 12.2 Configuration Space … … … … … … … … … … … … 525 C.3 Lyapunov Direct Method … … … … … … … … … … . . 596 12.2.1 Distance … … … … … … … … … … … … … . 527 Bibliography … … … … … … … … … … … … … … . 598 12.2.2 Obstacles … … … … … … … … … … … … … 527 12.2.3 Examples of Obstacles … … … … … … … … … . 528 D Differential Geometry … … … … … … … … … … … … . 599 12.3 Planning via Retraction … … … … … … … … … … … 532 D.1 Vector Fields and Lie Brackets … … … … … … … … … 599 12.4 Planning via Cell Decomposition … … … … … … … … . 536 D.2 Nonlinear Controllability … … … … … … … … … … . . 603 12.4.1 Exact Decomposition … … … … … … … … … . . 536 Bibliography … … … … … … … … … … … … … … . 604 12.4.2 Approximate Decomposition … … … … … … … . . 539 12.5 Probabilistic Planning … … … … … … … … … … … . 541 12.5.1 PRM Method … … … … … … … … … … … … 541 xxiv Contents
E Graph Search Algorithms … … … … … … … … … … … . 605 E.1 Complexity … … … … … … … … … … … … … … . . 605 E.2 Breadth-first and Depth-first Search … … … … … … … . 606 1 E.3 A Algorithm … … … … … … … … … … … … … … 607 Bibliography … … … … … … … … … … … … … … . 608 Introduction References … … … … … … … … … … … … … … … … … . . 609
Index … … … … … … … … … … … … … … … … … … … . 623
Robotics is concerned with the study of those machines that can replace hu-
man beings in the execution of a task, as regards both physical activity and
decision making. The goal of the introductory chapter is to point out the
problems related to the use of robots in industrial applications, as well as the
perspectives offered by advanced robotics. A classification of the most common
mechanical structures of robot manipulators and mobile robots is presented.
Topics of modelling, planning and control are introduced which will be ex-
amined in the following chapters. The chapter ends with a list of references
dealing with subjects both of specific interest and of related interest to those
covered by this textbook.
1.1 Robotics
Robotics has profound cultural roots. Over the course of centuries, human be-
ings have constantly attempted to seek substitutes that would be able to mimic
their behaviour in the various instances of interaction with the surrounding
environment. Several motivations have inspired this continuous search refer-
ring to philosophical, economic, social and scientific principles.
One of human beings’ greatest ambitions has been to give life to their
artifacts. The legend of the Titan Prometheus, who molded humankind from
clay, as well as that of the giant Talus, the bronze slave forged by Hephaestus,
testify how Greek mythology was influenced by that ambition, which has been
revisited in the tale of Frankenstein in modern times.
Just as the giant Talus was entrusted with the task of protecting the
island of Crete from invaders, in the Industrial Age a mechanical creature
(automaton) has been entrusted with the task of substituting a human being
in subordinate labor duties. This concept was introduced by the Czech play-
wright Karel Čapek who wrote the play Rossum’s Universal Robots (R.U.R.)
in 1920. On that occasion he coined the term robot — derived from the term
2 1 Introduction 1.2 Robot Mechanical Structure 3
robota that means executive labour in Slav languages — to denote the au- tomaton built by Rossum who ends up by rising up against humankind in the science fiction tale. In the subsequent years, in view of the development of science fiction, the behaviour conceived for the robot has often been conditioned by feelings. This has contributed to rendering the robot more and more similar to its creator. It is worth noticing how Rossum’s robots were represented as creatures made with organic material. The image of the robot as a mechanical artifact starts in the 1940s when the Russian Isaac Asimov, the well-known science fiction writer, conceived the robot as an automaton of human appearance but devoid of feelings. Its behaviour was dictated by a “positronic” brain pro- grammed by a human being in such a way as to satisfy certain rules of ethical Fig. 1.1. Components of a robotic system conduct. The term robotics was then introduced by Asimov as the science devoted to the study of robots which was based on the three fundamental laws: (locomotion apparatus). The realization of such a system refers to the context of design of articulated mechanical systems and choice of materials.
- A robot may not injure a human being or, through inaction, allow a human The capability to exert an action, both locomotion and manipulation, is being to come to harm. provided by an actuation system which animates the mechanical components
- A robot must obey the orders given by human beings, except when such of the robot. The concept of such a system refers to the context of motion orders would conflict with the first law. control , dealing with servomotors, drives and transmissions.
- A robot must protect its own existence, as long as such protection does The capability for perception is entrusted to a sensory system which can not conflict with the first or second law. acquire data on the internal status of the mechanical system (proprioceptive sensors, such as position transducers) as well as on the external status of These laws established rules of behaviour to consider as specifications for the environment (exteroceptive sensors, such as force sensors and cameras). the design of a robot, which since then has attained the connotation of an The realization of such a system refers to the context of materials properties, industrial product designed by engineers or specialized technicians. signal conditioning, data processing, and information retrieval. Science fiction has influenced the man and the woman in the street that The capability for connecting action to perception in an intelligent fash- continue to imagine the robot as a humanoid who can speak, walk, see, and ion is provided by a control system which can command the execution of the hear, with an appearance very much like that presented by the robots of the action in respect to the goals set by a task planning technique, as well as movie Metropolis, a precursor of modern cinematography on robots, with Star of the constraints imposed by the robot and the environment. The realiza- Wars and more recently with I, Robot inspired by Asimov’s novels. tion of such a system follows the same feedback principle devoted to control According to a scientific interpretation of the science-fiction scenario, the of human body functions, possibly exploiting the description of the robotic robot is seen as a machine that, independently of its exterior, is able to modify system’s components (modelling). The context is that of cybernetics, dealing the environment in which it operates. This is accomplished by carrying out with control and supervision of robot motions, artificial intelligence and expert actions that are conditioned by certain rules of behaviour intrinsic in the systems, the computational architecture and programming environment. machine as well as by some data the robot acquires on its status and on the Therefore, it can be recognized that robotics is an interdisciplinary subject environment. In fact, robotics is commonly defined as the science studying the concerning the cultural areas of mechanics, control , computers, and electron- intelligent connection between perception and action. ics. With reference to this definition, a robotic system is in reality a complex system, functionally represented by multiple subsystems (Fig. 1.1). The essential component of a robot is the mechanical system endowed, in general, with a locomotion apparatus (wheels, crawlers, mechanical legs) and 1.2 Robot Mechanical Structure a manipulation apparatus (mechanical arms, end-effectors, artificial hands). As an example, the mechanical system in Fig. 1.1 consists of two mechanical The key feature of a robot is its mechanical structure. Robots can be classified arms (manipulation apparatus), each of which is carried by a mobile vehicle as those with a fixed base, robot manipulators, and those with a mobile base, 4 1 Introduction 1.2 Robot Mechanical Structure 5
mobile robots. In the following, the geometrical features of the two classes are presented.
1.2.1 Robot Manipulators
The mechanical structure of a robot manipulator consists of a sequence of rigid bodies (links) interconnected by means of articulations (joints); a manipulator is characterized by an arm that ensures mobility, a wrist that confers dexterity, and an end-effector that performs the task required of the robot. The fundamental structure of a manipulator is the serial or open kinematic chain. From a topological viewpoint, a kinematic chain is termed open when there is only one sequence of links connecting the two ends of the chain. Al- ternatively, a manipulator contains a closed kinematic chain when a sequence Fig. 1.2. Cartesian manipulator and its workspace of links forms a loop. A manipulator’s mobility is ensured by the presence of joints. The artic- ulation between two consecutive links can be realized by means of either a prismatic or a revolute joint. In an open kinematic chain, each prismatic or revolute joint provides the structure with a single degree of freedom (DOF). A prismatic joint creates a relative translational motion between the two links, whereas a revolute joint creates a relative rotational motion between the two links. Revolute joints are usually preferred to prismatic joints in view of their compactness and reliability. On the other hand, in a closed kinematic chain, the number of DOFs is less than the number of joints in view of the constraints imposed by the loop. The degrees of freedom should be properly distributed along the mechan- ical structure in order to have a sufficient number to execute a given task. In the most general case of a task consisting of arbitrarily positioning and orienting an object in three-dimensional (3D) space, six DOFs are required, three for positioning a point on the object and three for orienting the object with respect to a reference coordinate frame. If more DOFs than task vari- ables are available, the manipulator is said to be redundant from a kinematic viewpoint. Fig. 1.3. Gantry manipulator The workspace represents that portion of the environment the manipula- tor’s end-effector can access. Its shape and volume depend on the manipulator (Fig. 1.2). As opposed to high accuracy, the structure has low dexterity since structure as well as on the presence of mechanical joint limits. all the joints are prismatic. The direction of approach in order to manipu- The task required of the arm is to position the wrist which then is required late an object is from the side. On the other hand, if it is desired to ap- to orient the end-effector. The type and sequence of the arm’s DOFs, start- proach an object from the top, the Cartesian manipulator can be realized by ing from the base joint, allows a classification of manipulators as Cartesian, a gantry structure as illustrated in Fig. 1.3. Such a structure makes available cylindrical , spherical , SCARA, and anthropomorphic. a workspace with a large volume and enables the manipulation of objects of Cartesian geometry is realized by three prismatic joints whose axes typ- large dimensions and heavy weight. Cartesian manipulators are employed for ically are mutually orthogonal (Fig. 1.2). In view of the simple geometry, material handling and assembly. The motors actuating the joints of a Carte- each DOF corresponds to a Cartesian space variable and thus it is natu- sian manipulator are typically electric and occasionally pneumatic. ral to perform straight motions in space. The Cartesian structure offers very Cylindrical geometry differs from Cartesian in that the first prismatic joint good mechanical stiffness. Wrist positioning accuracy is constant everywhere is replaced with a revolute joint (Fig. 1.4). If the task is described in cylindri- in the workspace. This is the volume enclosed by a rectangular parallel-piped 6 1 Introduction 1.2 Robot Mechanical Structure 7
Fig. 1.4. Cylindrical manipulator and its workspace
Fig. 1.6. SCARA manipulator and its workspace
Fig. 1.5. Spherical manipulator and its workspace
Fig. 1.7. Anthropomorphic manipulator and its workspace
cal coordinates, in this case each DOF also corresponds to a Cartesian space variable. The cylindrical structure offers good mechanical stiffness. Wrist posi- tioning accuracy decreases as the horizontal stroke increases. The workspace is it can allow manipulation of objects on the floor. Spherical manipulators are a portion of a hollow cylinder (Fig. 1.4). The horizontal prismatic joint makes mainly employed for machining. Electric motors are typically used to actuate the wrist of a cylindrical manipulator suitable to access horizontal cavities. the joints. Cylindrical manipulators are mainly employed for carrying objects even of A special geometry is SCARA geometry that can be realized by disposing large dimensions; in such a case the use of hydraulic motors is to be preferred two revolute joints and one prismatic joint in such a way that all the axes to that of electric motors. of motion are parallel (Fig. 1.6). The acronym SCARA stands for Selective Spherical geometry differs from cylindrical in that the second prismatic Compliance Assembly Robot Arm and characterizes the mechanical features joint is replaced with a revolute joint (Fig. 1.5). Each DOF corresponds to a of a structure offering high stiffness to vertical loads and compliance to hori- Cartesian space variable provided that the task is described in spherical coor- zontal loads. As such, the SCARA structure is well-suited to vertical assembly dinates. Mechanical stiffness is lower than the above two geometries and me- tasks. The correspondence between the DOFs and Cartesian space variables chanical construction is more complex. Wrist positioning accuracy decreases is maintained only for the vertical component of a task described in Carte- as the radial stroke increases. The workspace is a portion of a hollow sphere sian coordinates. Wrist positioning accuracy decreases as the distance of the (Fig. 1.5); it can also include the supporting base of the manipulator and thus wrist from the first joint axis increases. The typical workspace is illustrated 8 1 Introduction 1.2 Robot Mechanical Structure 9
Fig. 1.8. Manipulator with parallelogram
Fig. 1.10. Hybrid parallel-serial manipulator
According to the latest report by the International Federation of Robotics
(IFR), up to 2005, 59% of installed robot manipulators worldwide has an-
thropomorphic geometry, 20% has Cartesian geometry, 12% has cylindrical
geometry, and 8% has SCARA geometry.
All the previous manipulators have an open kinematic chain. Whenever
larger payloads are required, the mechanical structure will have higher stiffness
to guarantee comparable positioning accuracy. In such a case, resorting to
a closed kinematic chain is advised. For instance, for an anthropomorphic
structure, parallelogram geometry between the shoulder and elbow joints can
be adopted, so as to create a closed kinematic chain (Fig. 1.8).
Fig. 1.9. Parallel manipulator An interesting closed-chain geometry is parallel geometry (Fig. 1.9) which
has multiple kinematic chains connecting the base to the end-effector. The
fundamental advantage is seen in the high structural stiffness, with respect to
in Fig. 1.6. The SCARA manipulator is suitable for manipulation of small open-chain manipulators, and thus the possibility to achieve high operational objects; joints are actuated by electric motors. speeds; the drawback is that of having a reduced workspace. Anthropomorphic geometry is realized by three revolute joints; the revolute The geometry illustrated in Fig. 1.10 is of hybrid type, since it consists axis of the first joint is orthogonal to the axes of the other two which are of a parallel arm and a serial kinematic chain. This structure is suitable for parallel (Fig. 1.7). By virtue of its similarity with the human arm, the second the execution of manipulation tasks requiring large values of force along the joint is called the shoulder joint and the third joint the elbow joint since vertical direction. it connects the “arm” with the “forearm.” The anthropomorphic structure The manipulator structures presented above are required to position the is the most dexterous one, since all the joints are revolute. On the other wrist which is then required to orient the manipulator’s end-effector. If arbi- hand, the correspondence between the DOFs and the Cartesian space variables trary orientation in 3D space is desired, the wrist must possess at least three is lost, and wrist positioning accuracy varies inside the workspace. This is DOFs provided by revolute joints. Since the wrist constitutes the terminal approximately a portion of a sphere (Fig. 1.7) and its volume is large compared part of the manipulator, it has to be compact; this often complicates its me- to manipulator encumbrance. Joints are typically actuated by electric motors. chanical design. Without entering into construction details, the realization The range of industrial applications of anthropomorphic manipulators is wide. endowing the wrist with the highest dexterity is one where the three revolute 10 1 Introduction 1.2 Robot Mechanical Structure 11
Fig. 1.11. Spherical wrist
axes intersect at a single point. In such a case, the wrist is called a spherical wrist, as represented in Fig. 1.11. The key feature of a spherical wrist is the Fig. 1.12. The three types of conventional wheels with their respective icons decoupling between position and orientation of the end-effector; the arm is en- trusted with the task of positioning the above point of intersection, whereas Other rigid bodies (trailers), also equipped with wheels, may be connected the wrist determines the end-effector orientation. Those realizations where the to the base by means of revolute joints. wrist is not spherical are simpler from a mechanical viewpoint, but position • Legged mobile robots are made of multiple rigid bodies, interconnected by and orientation are coupled, and this complicates the coordination between prismatic joints or, more often, by revolute joints. Some of these bodies the motion of the arm and that of the wrist to perform a given task. form lower limbs, whose extremities (feet) periodically come in contact The end-effector is specified according to the task the robot should ex- with the ground to realize locomotion. There is a large variety of mechan- ecute. For material handling tasks, the end-effector consists of a gripper ical structures in this class, whose design is often inspired by the study of of proper shape and dimensions determined by the object to be grasped living organisms (biomimetic robotics): they range from biped humanoids (Fig. 1.11). For machining and assembly tasks, the end-effector is a tool or to hexapod robots aimed at replicating the biomechanical efficiency of a specialized device, e.g., a welding torch, a spray gun, a mill, a drill, or a insects. screwdriver. The versatility and flexibility of a robot manipulator should not induce Only wheeled vehicles are considered in the following, as they represent the conviction that all mechanical structures are equivalent for the execution the vast majority of mobile robots actually used in applications. The basic of a given task. The choice of a robot is indeed conditioned by the application mechanical element of such robots is indeed the wheel. Three types of con- which sets constraints on the workspace dimensions and shape, the maximum ventional wheels exist, which are shown in Fig. 1.12 together with the icons payload, positioning accuracy, and dynamic performance of the manipulator. that will be used to represent them:
• The fixed wheel can rotate about an axis that goes through the center
1.2.2 Mobile Robots of the wheel and is orthogonal to the wheel plane. The wheel is rigidly The main feature of mobile robots is the presence of a mobile base which attached to the chassis, whose orientation with respect to the wheel is allows the robot to move freely in the environment. Unlike manipulators, such therefore constant. robots are mostly used in service applications, where extensive, autonomous • The steerable wheel has two axes of rotation. The first is the same as a motion capabilities are required. From a mechanical viewpoint, a mobile robot fixed wheel, while the second is vertical and goes through the center of the consists of one or more rigid bodies equipped with a locomotion system. This wheel. This allows the wheel to change its orientation with respect to the description includes the following two main classes of mobile robots:1 chassis. • The caster wheel has two axes of rotation, but the vertical axis does not • Wheeled mobile robots typically consist of a rigid body (base or chassis) pass through the center of the wheel, from which it is displaced by a con- and a system of wheels which provide motion with respect to the ground. stant offset. Such an arrangement causes the wheel to swivel automatically, 1 Other types of mechanical locomotion systems are not considered here. Among rapidly aligning with the direction of motion of the chassis. This type of these, it is worth mentioning tracked locomotion, very effective on uneven terrain, wheel is therefore introduced to provide a supporting point for static bal- and undulatory locomotion, inspired by snake gaits, which can be achieved with- ance without affecting the mobility of the base; for instance, caster wheels out specific devices. There also exist types of locomotion that are not constrained are commonly used in shopping carts as well as in chairs with wheels. to the ground, such as flying and navigation. 12 1 Introduction 1.2 Robot Mechanical Structure 13
Fig. 1.13. A differential-drive mobile robot Fig. 1.15. A tricycle mobile robot
Fig. 1.16. A car-like mobile robot
Fig. 1.14. A synchro-drive mobile robot
motor which controls their traction,2 while the steerable wheel is driven by
The variety of kinematic structures that can be obtained by combining another motor which changes its orientation, acting then as a steering device.
the three conventional wheels is wide. In the following, the most relevant Alternatively, the two rear wheels may be passive and the front wheel may arrangements are briefly examined. provide traction as well as steering. In a differential-drive vehicle there are two fixed wheels with a common A car-like vehicle has two fixed wheels mounted on a rear axle and two axis of rotation, and one or more caster wheels, typically smaller, whose func- steerable wheels mounted on a front axle, as shown in Fig. 1.16. As in the tion is to keep the robot statically balanced (Fig. 1.13). The two fixed wheels previous case, one motor provides (front or rear) traction while the other are separately controlled, in that different values of angular velocity may be changes the orientation of the front wheels with respect to the vehicle. It is arbitrarily imposed, while the caster wheel is passive. Such a robot can rotate worth pointing out that, to avoid slippage, the two front wheels must have a on the spot (i.e., without moving the midpoint between the wheels), provided different orientation when the vehicle moves along a curve; in particular, the that the angular velocities of the two wheels are equal and opposite. internal wheel is slightly more steered with respect to the external one. This A vehicle with similar mobility is obtained using a synchro-drive kinematic is guaranteed by the use of a specific device called Ackermann steering. arrangement (Fig. 1.14). This robot has three aligned steerable wheels which Finally, consider the robot in Fig. 1.17, which has three caster wheels are synchronously driven by only two motors through a mechanical coupling, usually arranged in a symmetric pattern. The traction velocities of the three e.g., a chain or a transmission belt. The first motor controls the rotation of the wheels are independently driven. Unlike the previous cases, this vehicle is om- wheels around the horizontal axis, thus providing the driving force (traction) nidirectional : in fact, it can move instantaneously in any Cartesian direction, to the vehicle. The second motor controls the rotation of the wheels around as well as re-orient itself on the spot. the vertical axis, hence affecting their orientation. Note that the heading of In addition to the above conventional wheels, there exist other special the chassis does not change during the motion. Often, a third motor is used types of wheels, among which is notably the Mecanum (or Swedish) wheel , in this type of robot to rotate independently the upper part of the chassis (a shown in Fig. 1.18. This is a fixed wheel with passive rollers placed along the turret) with respect to the lower part. This may be useful to orient arbitrarily external rim; the axis of rotation of each roller is typically inclined by 45◦ with a directional sensor (e.g., a camera) or in any case to recover an orientation respect to the plane of the wheel. A vehicle equipped with four such wheels error. mounted in pairs on two parallel axles is also omnidirectional. In a tricycle vehicle (Fig. 1.15) there are two fixed wheels mounted on a rear axle and a steerable wheel in front. The fixed wheels are driven by a single 2 The distribution of the traction torque on the two wheels must take into account the fact that in general they move with different speeds. The mechanism which equally distributes traction is the differential . 14 1 Introduction 1.3 Industrial Robotics 15
Fig. 1.17. An omnidirectional mobile robot with three independently driven caster wheels
Fig. 1.18. A Mecanum (or Swedish) wheel
Fig. 1.19. A mobile manipulator obtained by mounting an anthropomorphic arm
on a differential-drive vehicle
In the design of a wheeled robot, the mechanical balance of the structure
does not represent a problem in general. In particular, a three-wheel robot is and dynamic mechanical balance of the robot, as well as to the actuation of statically balanced as long as its center of mass falls inside the support triangle, the two systems. which is defined by the contact points between the wheels and ground. Robots with more than three wheels have a support polygon, and thus it is typically easier to guarantee the above balance condition. It should be noted, however, 1.3 Industrial Robotics that when the robot moves on uneven terrain a suspension system is needed to maintain the contact between each wheel and the ground. Industrial robotics is the discipline concerning robot design, control and ap- Unlike the case of manipulators, the workspace of a mobile robot (defined plications in industry, and its products have by now reached the level of a as the portion of the surrounding environment that the robot can access) is po- mature technology. The connotation of a robot for industrial applications is tentially unlimited. Nevertheless, the local mobility of a non-omnidirectional that of operating in a structured environment whose geometrical or physical mobile robot is always reduced; for instance, the tricycle robot in Fig. 1.15 characteristics are mostly known a priori. Hence, limited autonomy is required. cannot move instantaneously in a direction parallel to the rear wheel axle. The early industrial robots were developed in the 1960s, at the confluence Despite this fact, the tricycle can be manoeuvered so as to obtain, at the end of two technologies: numerical control machines for precise manufacturing, of the motion, a net displacement in that direction. In other words, many and teleoperators for remote radioactive material handling. Compared to its mobile robots are subject to constraints on the admissible instantaneous mo- precursors, the first robot manipulators were characterized by: tions, without actually preventing the possibility of attaining any position and orientation in the workspace. This also implies that the number of DOFs of • versatility, in view of the employment of different end-effectors at the tip the robot (meant as the number of admissible instantaneous motions) is lower of the manipulator, than the number of its configuration variables. • adaptability to a priori unknown situations, in view of the use of sensors, It is obviously possible to merge the mechanical structure of a manipulator • positioning accuracy, in view of the adoption of feedback control tech- with that of a mobile vehicle by mounting the former on the latter. Such niques, a robot is called a mobile manipulator and combines the dexterity of the • execution repeatability, in view of the programmability of various opera- articulated arm with the unlimited mobility of the base. An example of such tions. a mechanical structure is shown in Fig. 1.19. However, the design of a mobile During the subsequent decades, industrial robots have gained a wide popu- manipulator involves additional difficulties related, for instance, to the static larity as essential components for the realization of automated manufacturing 16 1 Introduction 1.3 Industrial Robotics 17
140,000
127 Automotive parts
120,000 112 Motor vehicles
99 97
100,000 Chemical, rubber and plastics
82 79 81 Electrical/electronics
80,000 77 78
Units
69 69 69
Metal products
60,000 53 55 2005 2006
Machinery
(industrial and consumer)
40,000
Food
20,000
Communication
0 Precision and optical products
1993 1994 1995 1996 1997 1998 1999 2000 2001 2002 2003 2004 2005 2006
0 5,000 10,000 15,000 20,000 25,000 30,000
Fig. 1.20. Yearly installations of industrial robots worldwide Units
Fig. 1.21. Yearly supply of industrial robots by main industries
systems. The main factors having determined the spread of robotics tech- nology in an increasingly wider range of applications in the manufacturing industry are reduction of manufacturing costs, increase of productivity, im- Flexible automation represents the evolution of programmable automation. provement of product quality standards and, last but not least, the possibility Its goal is to allow manufacturing of variable batches of different products by of eliminating harmful or off-putting tasks for the human operator in a man- minimizing the time lost for reprogramming the sequence of operations and ufacturing system. the machines employed to pass from one batch to the next. The realization of a By its usual meaning, the term automation denotes a technology aimed at flexible manufacturing system (FMS) demands strong integration of computer replacing human beings with machines in a manufacturing process, as regards technology with industrial technology. not only the execution of physical operations but also the intelligent processing The industrial robot is a machine with significant characteristics of versa- of information on the status of the process. Automation is then the synthesis tility and flexibility. According to the widely accepted definition of the Robot of industrial technologies typical of the manufacturing process and computer Institute of America, a robot is a reprogrammable multifunctional manipulator technology allowing information management. The three levels of automation designed to move materials, parts, tools or specialized devices through variable one may refer to are rigid automation, programmable automation, and flexible programmed motions for the performance of a variety of tasks. Such a defini- automation. tion, dating back to 1980, reflects the current status of robotics technology. Rigid automation deals with a factory context oriented to the mass manu- By virtue of its programmability, the industrial robot is a typical com- facture of products of the same type. The need to manufacture large numbers ponent of programmable automated systems. Nonetheless, robots can be en- of parts with high productivity and quality standards demands the use of trusted with tasks in both rigid and flexible automated systems. fixed operational sequences to be executed on the workpiece by special pur- According to the above-mentioned IFR report, up to 2006 nearly one mil- pose machines. lion industrial robots are in use worldwide, half of which are in Asia, one third Programmable automation deals with a factory context oriented to the in Europe, and 16% in North America. The four countries with the largest manufacture of low-to-medium batches of products of different types. A pro- number of robots are Japan, Germany, United States and Italy. The figures grammable automated system permits changing easy the sequence of opera- for robot installations in the last 15 years are summarized in the graph in tions to be executed on the workpieces in order to vary the range of products. Fig. 1.20; by the end of 2007, an increase of 10% in sales with respect to the The machines employed are more versatile and are capable of manufacturing previous year is foreseen, with milder increase rates in the following years, different objects belonging to the same group technology. The majority of the reaching a worldwide figure of 1,200,000 units at work by the end of 2010. products available on the market today are manufactured by programmable In the same report it is shown how the average service life of an industrial automated systems. robot is about 12 years, which may increase to 15 in a few years from now. An interesting statistic is robot density based on the total number of persons employed: this ranges from 349 robots in operation per 10,000 workers to 18 1 Introduction 1.3 Industrial Robotics 19
Handling
Welding
Assembly
2005 2006
Dispensing
Processing
Others
0 2,000 4,000 6,000 8,000 10,000 12,000 14,000 16,000 18,000
Fig. 1.22. Examples of AGVs for material handling (courtesy of E&K Automation Units GmbH) Fig. 1.23. Yearly supply of industrial robots in Europe for manufacturing operations
187 in Korea, 186 in Germany, and 13 in Italy. The United States has just from one manufacturing cell to the next (Fig. 1.22). As compared to the tra- 99 robots per 10,000 workers. The average cost of a 6-axis industrial robot, ditional fixed guide paths for vehicles (inductive guide wire, magnetic tape, including the control unit and development software, ranges from 20,000 to or optical visible line), modern AGVs utilize high-tech systems with onboard 60,000 euros, depending on the size and applications. microprocessors and sensors (laser, odometry, GPS) which allow their local- The automotive industry is still the predominant user of industrial robots. ization within the plant layout, and manage their work flow and functions, The graph in Fig. 1.21 referring to 2005 and 2006, however, reveals how both allowing their complete integration in the FMS. The mobile robots employed the chemical industry and the electrical/electronics industry are gaining in im- in advanced applications can be considered as the natural evolution of the portance, and new industrial applications, such as metal products, constitute AGV systems, as far as enhanced autonomy is concerned. an area with a high potential investment. Manufacturing consists of transforming objects from raw material into Industrial robots present three fundamental capacities that make them finished products; during this process, the part either changes its own physical useful for a manufacturing process: material handling, manipulation, and mea- characteristics as a result of machining, or loses its identity as a result of an surement. assembly of more parts. The robot’s capability to manipulate both objects and In a manufacturing process, each object has to be transferred from one tools make it suitable to be employed in manufacturing. Typical applications location in the factory to another in order to be stored, manufactured, assem- include: bled, and packed. During transfer, the physical characteristics of the object do not undergo any alteration. The robot’s capability to pick up an object, move • arc and spot welding, it in space on predefined paths and release it makes the robot itself an ideal • painting and coating, candidate for material handling operations. Typical applications include: • gluing and sealing, • laser and water jet cutting, • palletizing (placing objects on a pallet in an ordered way), • milling and drilling, • warehouse loading and unloading, • casting and die spraying, • mill and machine tool tending, • deburring and grinding, • part sorting, • screwing, wiring and fastening, • packaging. • assembly of mechanical and electrical groups, In these applications, besides robots, Automated Guided Vehicles (AGV) • assembly of electronic boards. are utilized which ensure handling of parts and tools around the shop floor 20 1 Introduction 1.3 Industrial Robotics 21
Fig. 1.24. The AdeptOne XL robot (courtesy of Adept Technology Inc) Fig. 1.25. The COMAU Smart NS robot (courtesy of COMAU SpA Robotica)
Besides material handling and manipulation, in a manufacturing process
it is necessary to perform measurements to test product quality. The robot’s capability to explore 3D space together with the availability of measurements on the manipulator’s status allow a robot to be used as a measuring device. Typical applications include:
• object inspection, • contour finding, • detection of manufacturing imperfections.
The graph in Fig. 1.23 reports the number of robots employed in Europe
in 2005 and 2006 for various operations, which reveals how material handling requires twice as many robots employed for welding, whereas a limited number of robots is still employed for assembly. In the following some industrial robots are illustrated in terms of their features and application fields. Fig. 1.26. The ABB IRB 4400 robot (courtesy of ABB Robotics) The AdeptOne XL robot in Fig. 1.24 has a four-joint SCARA structure. Direct drive motors are employed. The maximum reach is 800 mm, with a The Comau SMART NS robot in Fig. 1.25 has a six-joint anthropomorphic repeatability of 0.025 mm horizontally and 0.038 mm vertically. Maximum structure with spherical wrist. In its four versions, the outreach ranges from speeds are 1200 mm/s for the prismatic joint, while they range from to 650 1650 and 1850 mm horizontally, with a repeatability of 0.05 mm. Maximum to 3300 deg/s for the three revolute joints. The maximum payload3 is 12 kg. speeds range from 155 to 170 deg/s for the inner three joints, and from 350 Typical industrial applications include small-parts material handling, assem- to 550 deg/s for the outer three joints. The maximum payload is 16 kg. Both bly and packaging. floor and ceiling mounting positions are allowed. Typical industrial applica- tions include arc welding, light handling, assembly and technological processes. 3 Repeatability and payload are classical parameters found in industrial robot data The ABB IRB 4400 robot in Fig. 1.26 also has a six-joint anthropomor- sheets. The former gives a measure of the manipulator’s ability to return to a phic structure, but unlike the previous open-chain structure, it possesses a previously reached position, while the latter indicates the average load to be closed chain of parallelogram type between the shoulder and elbow joints. carried at the robot’s end-effector. The outreach ranges from 1960 to 2550 mm for the various versions, with a 22 1 Introduction 1.3 Industrial Robotics 23
Fig. 1.27. The KUKA KR 60 Jet robot (courtesy of KUKA Roboter GmbH) Fig. 1.28. The ABB IRB 340 FlexPicker robot (courtesy of ABB Robotics)
repeatability from 0.07 to 0.1 mm. The maximum speed at the end-effector is 2200 mm/s. The maximum payload is 60 kg. Floor or shelf-mounting is available. Typical industrial applications include material handling, machine tending, grinding, gluing, casting, die spraying and assembly. The KUKA KR 60 Jet robot in Fig. 1.27 is composed of a five-axis struc- ture, mounted on a sliding track with a gantry-type installation; the upright installation is also available. The linear unit has a stroke from a minimum of 400 mm to a maximum of 20 m (depending on customer’s request), and a maximum speed of 3200 mm/s. On the other hand, the robot has a payload of 60 kg, an outreach of 820 mm and a repeatability of 0.15 mm. Maximum speeds are 120 deg/s and 166 deg/s for the first two joints, while they range from 260 to 322 deg/s for the outer three joints. Typical industrial applications include machine tending, arc welding, deburring, coating, sealing, plasma and waterjet cutting. The ABB IRB340 FlexPicker robot in Fig. 1.28 adopts a parallel geometry Fig. 1.29. The Fanuc M-16iB robot (courtesy of Fanuc Ltd) with four axes; in view of its reduced weight and floor mounting, the robot can transport 150 objects a minute (cycle time of just 0.4 s), reaching record speeds of 10 m/s and accelerations of 100 m/s2 , for a payload of 1 kg, with The robot is utilized for handling arbitrarily located objects, deburring, seal- a repeatability of 0.1 mm. In its ‘clean’ aluminum version, it is particularly ing and waterjet cutting. suitable for packaging in the food and pharmaceutical industries. The Light Weight Robot (LWR) in Fig. 1.30 with a seven-axis structure The Fanuc M-16iB robot in Fig. 1.29 has a six-joint anthropomorphic was introduced in 2006 as the outcome of technology transfer from DLR (the structure with a spherical wrist. In its two versions, the outreach varies German Aerospace Agency) to KUKA. In view of the adoption of lightweight from 1667 to 1885 mm horizontally, with a repeatability of 0.1 mm. Maximum materials, as well as the adoption of torque sensors at the joints, the robot speeds range from 165 to 175 deg/s for the inner three joints, and from 340 can manipulate a payload of 7 to 14 kg, in the face of a weight of the structure to 520 deg/s for the outer three joints. Payload varies from 10 to 20 kg. The of just 15 kg. The horizontal outreach is 868 mm, with joint speeds ranging peculiarity of this robot consists of the integrated sensors in the control unit, from 110 to 210 deg/s. On the other hand, the presence of the seventh axis of including a servoing system based on 3D vision and a six-axis force sensor. motion confers kinematic redundancy to the robot, which can then be recon- figured into more dexterous postures for the execution of given tasks. Such 24 1 Introduction 1.4 Advanced Robotics 25
Fig. 1.31. The BarrettHand (courtesy of Barrett Technology Inc)
Fig. 1.30. The KUKA LWR robot (courtesy of KUKA Roboter GmbH)
a manipulator represents one of the most advanced industrial products and, in view of its lightweight feature, it offers interesting performance for interac- tion with the environment, ensuring an inherent safety in case of contact with human beings. Fig. 1.32. The SCHUNK Anthropomorphic Hand (courtesy of SCHUNK Intec Ltd) In most industrial applications requiring object manipulation, typical grip- pers are utilized as end-effectors. Nevertheless, whenever enhanced manipula- bility and dexterity is desired, multifingered robot hands are available. lightweight robot, the hands and the humanoid manipulator presented above The BarrettHand (Fig. 1.31), endowed with a fixed finger and two mobile are to be considered at the transition from traditional industrial robotics sys- fingers around the base of the palm, allows the manipulation of objects of tems toward those innovative systems of advanced robotics. different dimension, shape and orientation. The SCHUNK Antropomorphic Hand (SAH) in Fig. 1.32 is the outcome of technology transfer from DLR and Harbin Institute of Technology (China) 1.4 Advanced Robotics to SCHUNK. Characterized by three independent aligned fingers and an op- The expression advanced robotics usually refers to the science studying robots posing finger which is analogous to the human thumb. The finger joints are with marked characteristics of autonomy, operating in scarcely structured endowed with magnetic angular sensors and torque sensors. This hand offers or unstructured environments, whose geometrical or physical characteristics good dexterity and approaches the characteristics of the human hand. would not be known a priori. LWR technology has been employed for the realization of the two arms Nowadays, advanced robotics is still in its youth. It has indeed featured of Justin, a humanoid manipulator made by DLR, composed of a three-joint the realization of prototypes only, because the associated technology is not torso with an anthropomorphic structure, two seven-axis arms and a sen- yet mature. There are many motivations which strongly encourage advances sorized head. The robot is illustrated in Fig. 1.33 in the execution of a biman- in knowledge within this field. They range from the need for automata when- ual manipulation task; the hands employed are previous versions of the SAH ever human operators are not available or are not safe (field robots), to the anthropomorphic hand. opportunity of developing products for potentially wide markets which are The applications listed describe the current employment of robots as com- aimed at improving quality of life (service robots). ponents of industrial automation systems. They all refer to strongly structured The graph in Fig. 1.34 reports the number of robots in stock for non- working environments and thus do not exhaust all the possible utilizations of industrial applications at the end of 2006 and the forecast to 2010. Such robots for industrial applications. Whenever it is desired to tackle problems applications are characterized by the complexity level, the uncertainty and requiring the adaptation of the robot to a changeable working environment, variability of the environment with which the robot interacts, as shown in the the fall-out of advanced robotics products are of concern. In this regard, the following examples. 26 1 Introduction 1.4 Advanced Robotics 27
14,000
12,000
Stock at the end of 2006
New installations 2007í2010
10,000
8,000
Units
6,000
4,000
2,000
0
Underwater Defense, rescue, Hostile fields Medical Others
security
Cleaning Mobile platforms Logistics
Construction and demolition
Fig. 1.34. Robots on stock for non-industrial applications
Fig. 1.33. The Justin humanoid robot manipulator (courtesy of DLR)
1.4.1 Field Robots
The context is that of deploying robots in areas where human beings could not survive or be exposed to unsustainable risks. Such robots should carry out exploration tasks and report useful data on the environment to a remote operator, using suitable onboard sensors. Typical scenarios are the explo- Fig. 1.35. The Sojourner rover was deployed by the Pathfinder lander and explored ration of a volcano, the intervention in areas contaminated by poisonous gas 250 m2 of Martian soil in 1997 (courtesy of NASA) or radiation, or the exploration of the deep ocean or space. As is well known, NASA succeeded in delivering some mobile robots (rovers) to Mars (Fig. 1.35) which navigated on the Martian soil, across rocks, hills and crevasses. Such 1.4.2 Service Robots rovers were partially teleoperated from earth and have successfully explored the environment with sufficient autonomy. Some mini-robots were deployed Autonomous vehicles are also employed for civil applications, i.e., for mass on September 11, 2001 at Ground Zero after the collapse of the Twin Towers transit systems (Fig. 1.37), thus contributing to the reduction of pollution in New York, to penetrate the debris in the search for survivors. levels. Such vehicles are part of the so-called Intelligent Transportation Sys- A similar scenario is that of disasters caused by fires in tunnels or earth- tems (ITS) devoted to traffic management in urban areas. Another feasible quakes; in such occurrences, there is a danger of further explosions, escape of application where the adoption of mobile robots offers potential advantages harmful gases or collapse, and thus human rescue teams may cooperate with is museum guided tours (Fig. 1.38). robot rescue teams. Also in the military field, unmanned autonomous aircrafts Many countries are investing in establishing the new market of service and missiles are utilized, as well as teleoperated robots with onboard cameras robots which will co-habitat with human beings in everyday life. According to explore buildings. The ‘Grand Challenge’ of October 2005 (Fig. 1.36) was to the above-mentioned IFR report, up to 2005 1.9 million service robots for financially supported by the US Department of Defense (DARPA) with the domestic applications (Fig. 1.39) and 1 million toy robots have been sold. goal of developing autonomous vehicles to carry weapons and sensors, thus Technology is ready to transform into commercial products the prototypes reducing soldier employment. of robotic aids to enhance elderly and impaired people’s autonomy in everyday life; autonomous wheelchairs, mobility aid lifters, feeding aids and rehabilita- tion robots allowing tetraplegics to perform manual labor tasks are examples of such service devices. In perspective, other than an all-purpose robot waiter, 28 1 Introduction 1.5 Robot Modelling, Planning and Control 29
Fig. 1.38. Rhino, employing the synchro-drive mobile base B21 by Real World
Interface, was one of the first robots for museum guided tours (courtesy of Deutsches
Fig. 1.36. The unmanned car Stanley autonomously completed a path of 132 miles Museum Bonn) in the record time of 6 h and 53 min (courtesy of DARPA)
Fig. 1.39. The vacuum robot Roomba, employing a differential-drive kinematics,
autonomously sweeps and cleans floors (courtesy of I-Robot Corp)
Finally, in motor rehabilitation systems, a hemiplegic patient wears an
Fig. 1.37. The Cycab is an electrically-driven vehicle for autonomous transportation exoskeleton, which actively interacts, sustains and corrects the movements in urban environments (courtesy of INRIA) according to the physiotherapist’s programmed plan. Another wide market segment comes from entertainment, where robots assistance, and healthcare systems integrating robotic and telematic modules are used as toy companions for children, and life companions for the elderly, will be developed for home service management (domotics). such as humanoid robots (Fig. 1.41) and the pet robots (Fig. 1.42) being Several robotic systems are employed for medical applications. Surgery developed in Japan. It is reasonable to predict that service robots will be assistance systems exploit a robot’s high accuracy to position a tool, i.e., for naturally integrated into our society. Tomorrow, robots will be as pervasive hip prosthesis implant. Yet, in minimally-invasive surgery, i.e., cardiac surgery, and personal as today’s personal computers, or just as TV sets in the homes the surgeon operates while seated comfortably at a console viewing a 3D image of 20 years ago. Robotics will then become ubiquitous, a challenge under of the surgical field, and operating the surgical instruments remotely by means discussion within the scientific community. of a haptic interface (Fig. 1.40). Further, in diagnostic and endoscopic surgery systems, small teleoperated robots travels through the cavities of human body, i.e., in the gastrointestinal 1.5 Robot Modelling, Planning and Control system, bringing live images or intervening in situ for biopsy, dispensing drugs or removing neoplasms. In all robot applications, completion of a generic task requires the execution of a specific motion prescribed to the robot. The correct execution of such 30 1 Introduction 1.5 Robot Modelling, Planning and Control 31
Fig. 1.40. The da Vinci robotic system for laparoscopic surgery (courtesy of Intu- Fig. 1.41. The Asimo humanoid robot, launched in 1996, has been endowed with itive Surgical Inc) even more natural locomotion and human-robot interaction skills (courtesy of Honda Motor Company Ltd) motion is entrusted to the control system which should provide the robot’s actuators with the commands consistent with the desired motion. Motion control demands an accurate analysis of the characteristics of the mechanical structure, actuators, and sensors. The goal of such analysis is the derivation of the mathematical models describing the input/output relationship charac- terizing the robot components. Modelling a robot manipulator is therefore a necessary premise to finding motion control strategies. Significant topics in the study of modelling, planning and control of robots which constitute the subject of subsequent chapters are illustrated below. Fig. 1.42. The AIBO dog had been the most widely diffused entertainment robot in the recent years (courtesy of Sony Corp)
1.5.1 Modelling inverse problem; its solution is of fundamental importance to transform the Kinematic analysis of the mechanical structure of a robot concerns the de- desired motion, naturally prescribed to the end-effector in the workspace, into scription of the motion with respect to a fixed reference Cartesian frame the corresponding joint motion. by ignoring the forces and moments that cause motion of the structure. It The availability of a manipulator’s kinematic model is also useful to de- is meaningful to distinguish between kinematics and differential kinematics. termine the relationship between the forces and torques applied to the joints With reference to a robot manipulator, kinematics describes the analytical and the forces and moments applied to the end-effector in static equilibrium relationship between the joint positions and the end-effector position and ori- configurations. entation. Differential kinematics describes the analytical relationship between Chapter 2 is dedicated to the study of kinematics. Chapter 3 is dedicated to the joint motion and the end-effector motion in terms of velocities, through the study of differential kinematics and statics, whereas Appendix A provides the manipulator Jacobiann. a useful brush-up on linear algebra. The formulation of the kinematics relationship allows the study of two Kinematics of a manipulator represents the basis of a systematic, general key problems of robotics, namely, the direct kinematics problem and the in- derivation of its dynamics, i.e., the equations of motion of the manipulator verse kinematics problem. The former concerns the determination of a sys- as a function of the forces and moments acting on it. The availability of the tematic, general method to describe the end-effector motion as a function of dynamic model is very useful for mechanical design of the structure, choice the joint motion by means of linear algebra tools. The latter concerns the of actuators, determination of control strategies, and computer simulation of 32 1 Introduction Bibliography 33
manipulator motion. Chapter 7 is dedicated to the study of dynamics, whereas Chapter 6 is concerned with the hardware/software architecture of a Appendix B recalls some fundamentals on rigid body mechanics. robot’s control system which is in charge of implementation of control laws as Modelling of mobile robots requires a preliminary analysis of the kinematic well as of interface with the operator. constraints imposed by the presence of wheels. Depending on the mechanical The trajectories generated constitute the reference inputs to the motion structure, such constraints can be integrable or not; this has direct conse- control system of the mechanical structure. The problem of robot manipulator quence on a robot’s mobility. The kinematic model of a mobile robot is es- control is to find the time behaviour of the forces and torques to be delivered sentially the description of the admissible instantaneous motions in respect by the joint actuators so as to ensure the execution of the reference trajec- of the constraints. On the other hand, the dynamic model accounts for the tories. This problem is quite complex, since a manipulator is an articulated reaction forces and describes the relationship between the above motions and system and, as such, the motion of one link influences the motion of the oth- the generalized forces acting on the robot. These models can be expressed ers. Manipulator equations of motion indeed reveal the presence of coupling in a canonical form which is convenient for design of planning and control dynamic effects among the joints, except in the case of a Cartesian structure techniques. Kinematic and dynamic analysis of mobile robots is developed with mutually orthogonal axes. The synthesis of the joint forces and torques in Chap. 11, while Appendix D contains some useful concepts of differential cannot be made on the basis of the sole knowledge of the dynamic model, geometry. since this does not completely describe the real structure. Therefore, manip- ulator control is entrusted to the closure of feedback loops; by computing the 1.5.2 Planning deviation between the reference inputs and the data provided by the propri- oceptive sensors, a feedback control system is capable of satisfying accuracy With reference to the tasks assigned to a manipulator, the issue is whether requirements on the execution of the prescribed trajectories. to specify the motion at the joints or directly at the end-effector. In material Chapter 8 is dedicated to the presentation of motion control techniques, handling tasks, it is sufficient to assign only the pick-up and release locations whereas Appendix C illustrates the basic principles of feedback control . of an object (point-to-point motion), whereas, in machining tasks, the end- Control of a mobile robot substantially differs from the analogous problem effector has to follow a desired trajectory (path motion). The goal of trajectory for robot manipulators. This is due, in turn, to the availability of fewer control planning is to generate the timing laws for the relevant variables (joint or end- inputs than the robot has configuration variables. An important consequence effector) starting from a concise description of the desired motion. Chapter 4 is that the structure of a controller allowing a robot to follow a trajectory is dedicated to trajectory planning for robot manipulators. (tracking problem) is unavoidably different from that of a controller aimed at The motion planning problem for a mobile robot concerns the generation taking the robot to a given configuration (regulation problem). Further, since of trajectories to take the vehicle from a given initial configuration to a desired a mobile robot’s proprioceptive sensors do not yield any data on the vehicle’s final configuration. Such a problem is more complex than that of robot ma- configuration, it is necessary to develop localization methods for the robot nipulators, since trajectories have to be generated in respect of the kinematic in the environment. The control design problem for wheeled mobile robots is constraints imposed by the wheels. Some solution techniques are presented in treated in Chap. 11. Chap. 11, which exploit the specific differential structure of the mobile robots’ If a manipulation task requires interaction between the robot and the en- kinematic models. vironment, the control problem should account for the data provided by the Whenever obstacles are present in a mobile robot’s workspace, the planned exteroceptive sensors; the forces exchanged at the contact with the environ- motions must be safe, so as to avoid collisions. Such a problem, known as ment, and the objects’ position as detected by suitable cameras. Chapter 9 is motion planning, can be formulated in an effective fashion for both robot ma- dedicated to force control techniques for robot manipulators, while Chap. 10 nipulators and mobile robots utilizing the configuration space concept. The presents visual control techniques. solution techniques are essentially of algorithmic nature and include exact, probabilistic and heuristic methods. Chapter 12 is dedicated to motion plan- ning problem, while Appendix E provides some basic concepts on graph search algorithms. Bibliography
In the last 30 years, the robotics field has stimulated the interest of an increas-
1.5.3 Control ing number of scholars. A truly respectable international research community Realization of the motion specified by the control law requires the employment has been established. Literature production has been conspicuous, both in of actuators and sensors. The functional characteristics of the most commonly terms of textbooks and scientific monographs and in terms of journals dedi- used actuators and sensors for robots are described in Chap. 5. cated to robotics. Therefore, it seems appropriate to close this introduction 34 1 Introduction Bibliography 35
by offering a selection of bibliographical reference sources to those readers who • M.W. Spong, S. Hutchinson, M. Vidyasagar, Robot Modeling and Control , wish to make a thorough study of robotics. Wiley, New York, 2006. Besides indicating those basic textbooks sharing an affinity of contents • M. Vukobratović, Introduction to Robotics, Springer-Verlag, Berlin, Ger- with this one, the following lists include specialized books on related sub- many, 1989. jects, collections of contributions on the state of the art of research, scientific • T. Yoshikawa, Foundations of Robotics, MIT Press, Boston, MA, 1990. journals, and series of international conferences. Specialized books Basic textbooks Topics of related interest to robot modelling, planning and control are: • J. Angeles, Fundamentals of Robotic Mechanical Systems: Theory, Meth- ods, and Algorithms, Springer-Verlag, New York, 1997. • manipulator mechanical design, • H. Asada, J.-J.E. Slotine, Robot Analysis and Control , Wiley, New York, • manipulation tools, 1986. • manipulators with elastic members, • G.A. Bekey, Autonomous Robots, MIT Press, Cambridge, MA, 2005. • parallel robots, • C. Canudas de Wit, B. Siciliano, G. Bastin, (Eds.), Theory of Robot Con- • locomotion apparatus, trol , Springer-Verlag, London, 1996. • mobile robots, • J.J. Craig, Introduction to Robotics: Mechanics and Control , 3rd ed., Pear- • underwater and space robots, son Prentice Hall, Upper Saddle River, NJ, 2004. • control architectures • A.J. Critchlow, Introduction to Robotics, Macmillan, New York, 1985. • motion and force control, • J.F. Engelberger, Robotics in Practice, Amacom, New York, 1980. • robot vision, • J.F. Engelberger, Robotics in Service, MIT Press, Cambridge, MA, 1989. • multisensory data fusion, • K.S. Fu, R.C. Gonzalez, C.S.G. Lee, Robotics: Control, Sensing, Vision, • telerobotics, and Intelligence, McGraw-Hill, New York, 1987. • human-robot interaction. • W. Khalil, E. Dombre, Modeling, Identification and Control of Robots, The following books are dedicated to these topics: Hermes Penton Ltd, London, 2002. • A.J. Koivo, Fundamentals for Control of Robotic Manipulators, Wiley, • G. Antonelli, Underwater Robots: Motion and Force Control of Vehicle- New York, 1989. Manipulator Systems, 2nd ed., Springer, Heidelberg, Germany, 2006. • Y. Koren, Robotics for Engineers, McGraw-Hill, New York, 1985. • R.C. Arkin, Behavior-Based Robotics, MIT Press, Cambridge, MA, 1998. • F.L. Lewis, C.T. Abdallah, D.M. Dawson, Control of Robot Manipulators, • J. Baeten, J. De Schutter, Integrated Visual Servoing and Force Control: Macmillan, New York, 1993. The Task Frame Approach, Springer, Heidelberg, Germany, 2003. • P.J. McKerrow, Introduction to Robotics, Addison-Wesley, Sydney, Aus- • M. Buehler, K. Iagnemma, S. Singh, (Eds.), The 2005 DARPA Grand tralia, 1991. Challenge: The Great Robot Race, Springer, Heidelberg, Germany, 2007. • R.M. Murray, Z. Li, S.S. Sastry, A Mathematical Introduction to Robotic • J.F. Canny, The Complexity of Robot Motion Planning, MIT Press, Cam- Manipulation, CRC Press, Boca Raton, FL, 1994. bridge, MA, 1988. • S.B. Niku, Introduction to Robotics: Analysis, Systems, Applications, • H. Choset, K.M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L.E. Prentice-Hall, Upper Saddle River, NJ, 2001. Kavraki, S. Thrun, Principles of Robot Motion: Theory, Algorithms, and • R.P. Paul, Robot Manipulators: Mathematics, Programming, and Control Implementations, MIT Press, Cambridge, MA, 2005. MIT Press, Cambridge, MA, 1981. • P.I. Corke, Visual Control of Robots: High-Performance Visual Servoing, • R.J. Schilling, Fundamentals of Robotics: Analysis and Control , Prentice- Research Studies Press, Taunton, UK, 1996. Hall, Englewood Cliffs, NJ, 1990. • M.R. Cutkosky, Robotic Grasping and Fine Manipulation, Kluwer, Boston, • L. Sciavicco, B. Siciliano, Modelling and Control of Robot Manipulators, MA, 1985. 2nd ed., Springer, London, UK, 2000. • H.F. Durrant-Whyte, Integration, Coordination and Control of Multi- • W.E. Snyder, Industrial Robots: Computer Interfacing and Control , Pren- Sensor Robot Systems, Kluwer, Boston, MA, 1988. tice-Hall, Englewood Cliffs, NJ, 1985. • A. Ellery, An Introduction to Space Robotics, Springer-Verlag, London, UK, 2000. 36 1 Introduction Bibliography 37
• A.R. Fraser, R.W. Daniel, Perturbation Techniques for Flexible Manipu- • V.D. Hunt, Industrial Robotics Handbook , Industrial Press, New York, lators, Kluwer, Boston, MA, 1991. 1983. • B.K. Ghosh, N. Xi, T.-J. Tarn, (Eds.), Control in Robotics and Automa- • O. Khatib, J.J. Craig, T. Lozano-Pérez, (Eds.), The Robotics Review 1 , tion: Sensor-Based Integration, Academic Press, San Diego, CA, 1999. MIT Press, Cambridge, MA, 1989. • K. Goldberg, (Ed.), The Robot in the Garden: Telerobotics and Telepiste- • O. Khatib, J.J. Craig, T. Lozano-Pérez, (Eds.), The Robotics Review 2 , mology in the Age of the Internet, MIT Press, Cambridge, MA, 2000. MIT Press, Cambridge, MA., 1992. • S. Hirose, Biologically Inspired Robots, Oxford University Press, Oxford, • T.R. Kurfess, (Ed.), Robotics and Automation Handbook , CRC Press, Boca UK, 1993. Raton, FL, 2005. • B.K.P. Horn, Robot Vision, McGraw-Hill, New York, 1986. • B. Siciliano, O. Khatib, (Eds.), Springer Handbook of Robotics, Springer, • K. Iagnemma, S. Dubowsky, Mobile Robots in Rough Terrain Estimation: Heidelberg, Germany, 2008. Motion Planning, and Control with Application to Planetary Rovers Se- • C.S.G. Lee, R.C. Gonzalez, K.S. Fu, (Eds.), Tutorial on Robotics, 2nd ed., ries, Springer, Heidelberg, Germany, 2004. IEEE Computer Society Press, Silver Spring, MD, 1986. • R. Kelly, V. Santibañez, A. Lorı́a, Control of Robot Manipulators in Joint • M.W. Spong, F.L. Lewis, C.T. Abdallah, (Eds.), Robot Control: Dynamics, Space, Springer-Verlag, London, UK, 2005. Motion Planning, and Analysis, IEEE Press, New York, 1993. • J.-C. Latombe, Robot Motion Planning, Kluwer, Boston, MA, 1991. • M.T. Mason, Mechanics of Robotic Manipulation, MIT Press, Cambridge, Scientific journals MA, 2001. • M.T. Mason, J.K. Salisbury, Robot Hands and the Mechanics of Manipu- • Advanced Robotics lation, MIT Press, Cambridge, MA, 1985. • Autonomous Robots • J.-P. Merlet, Parallel Robots, 2nd ed., Springer, Dordrecht, The Nether- • IEEE Robotics and Automation Magazine lands, 2006. • IEEE Transactions on Robotics • R.R. Murphy, Introduction to AI Robotics, MIT Press, Cambridge, MA, • International Journal of Robotics Research 2000. • Journal of Field Robotics • C. Natale, Interaction Control of Robot Manipulators: Six-degrees-of- • Journal of Intelligent and Robotic Systems freedom Tasks, Springer, Heidelberg, Germany, 2003. • Robotica • M. Raibert, Legged Robots that Balance, MIT Press, Cambridge, MA, 1985. • Robotics and Autonomous Systems • E.I. Rivin, Mechanical Design of Robots, McGraw-Hill, New York, 1987. • B. Siciliano, L. Villani, Robot Force Control , Kluwer, Boston, MA, 2000. • R. Siegwart, Introduction to Autonomous Mobile Robots, MIT Press, Cam- Series of international scientific conferences bridge, MA, 2004. • IEEE International Conference on Robotics and Automation • S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics, MIT Press, Cam- • IEEE/RSJ International Conference on Intelligent Robots and Systems bridge, MA, 2005. • International Conference on Advanced Robotics • D.J. Todd, Walking Machines, an Introduction to Legged Robots, Chapman • International Symposium of Robotics Research Hall, London, UK, 1985. • International Symposium on Experimental Robotics • L.-W. Tsai, Robot Analysis: The Mechanics of Serial and Parallel Manip- • Robotics: Science and Systems ulators, Wiley, New York, 1999. The above journals and conferences represent the reference sources for the Edited collections on the state of the art of research international scientific community. Many other robotics journals and confer- ences exist which are devoted to specific topics, such as kinematics, control, vi- • M. Brady, (Ed.), Robotics Science, MIT Press, Cambridge, MA, 1989. sion, algorithms, haptics, industrial applications, space and underwater explo- • M. Brady, J.M. Hollerbach, T.L. Johnson, T. Lozano-Pérez, M.T. Mason, ration, humanoid robotics, and human-robot interaction. On the other hand, (Eds.), Robot Motion: Planning and Control , MIT Press, Cambridge, MA, several journals and prestigious conferences in other fields, such as mechan- 1982. ics, control, sensors, and artificial intelligence, offer generous space to robotics • R.C. Dorf, International Encyclopedia of Robotics, Wiley, New York, 1988. topics. 40 2 Kinematics
2 Kinematics
Fig. 2.1. Position and orientation of a rigid body
where ox , oy , oz denote the components of the vector o ∈ IR3 along the frame
A manipulator can be schematically represented from a mechanical viewpoint axes; the position of O can be compactly written as the (3 × 1) vector as a kinematic chain of rigid bodies (links) connected by means of revolute or prismatic joints. One end of the chain is constrained to a base, while an ⎡ ⎤ ox end-effector is mounted to the other end. The resulting motion of the struc- o = ⎣ oy ⎦ . (2.1) ture is obtained by composition of the elementary motions of each link with oz respect to the previous one. Therefore, in order to manipulate an object in space, it is necessary to describe the end-effector position and orientation. Vector o is a bound vector since its line of application and point of application This chapter is dedicated to the derivation of the direct kinematics equation are both prescribed, in addition to its direction and norm. through a systematic, general approach based on linear algebra. This allows In order to describe the rigid body orientation, it is convenient to consider the end-effector position and orientation (pose) to be expressed as a function an orthonormal frame attached to the body and express its unit vectors with of the joint variables of the mechanical structure with respect to a reference respect to the reference frame. Let then O –x y z be such a frame with origin frame. Both open-chain and closed-chain kinematic structures are considered. in O and x , y , z be the unit vectors of the frame axes. These vectors are With reference to a minimal representation of orientation, the concept of expressed with respect to the reference frame O–xyz by the equations: operational space is introduced and its relationship with the joint space is es- tablished. Furthermore, a calibration technique of the manipulator kinematic x = xx x + xy y + xz z parameters is presented. The chapter ends with the derivation of solutions to y = yx x + yy y + yz z (2.2) the inverse kinematics problem, which consists of the determination of the z = zx x + zy y + zz z. joint variables corresponding to a given end-effector pose. The components of each unit vector are the direction cosines of the axes of frame O –x y z with respect to the reference frame O–xyz. 2.1 Pose of a Rigid Body
A rigid body is completely described in space by its position and orientation 2.2 Rotation Matrix (in brief pose) with respect to a reference frame. As shown in Fig. 2.1, let O–xyz be the orthonormal reference frame and x, y, z be the unit vectors of By adopting a compact notation, the three unit vectors in (2.2) describing the the frame axes. body orientation with respect to the reference frame can be combined in the The position of a point O on the rigid body with respect to the coordinate (3 × 3) matrix frame O–xyz is expressed by the relation ⎡ ⎤ ⎡ ⎤ ⎡ T ⎤ xx yx zx x x y T x z T x o = ox x + oy y + oz z, R = ⎣ x y z ⎦ = ⎣ xy yy zy ⎦ = ⎣ xT y y T y z T y ⎦ , (2.3) T T T xz yz zz x z y z z z 2.2 Rotation Matrix 41 42 2 Kinematics
which is termed rotation matrix . It is worth noting that the column vectors of matrix R are mutually or- thogonal since they represent the unit vectors of an orthonormal frame, i.e.,
xT y = 0 y T z = 0 z T x = 0.
Also, they have unit norm
xT x = 1 y T y = 1 z T z = 1.
As a consequence, R is an orthogonal matrix meaning that
RT R = I 3 (2.4)
where I 3 denotes the (3 × 3) identity matrix. If both sides of (2.4) are postmultiplied by the inverse matrix R−1 , the Fig. 2.2. Rotation of frame O–xyz by an angle α about axis z useful result is obtained: RT = R−1 , (2.5) Hence, the rotation matrix of frame O–x y z with respect to frame O–xyz is ⎡ ⎤ that is, the transpose of the rotation matrix is equal to its inverse. Further, cos α −sin α 0 observe that det(R) = 1 if the frame is right-handed, while det(R) = −1 if Rz (α) = ⎣ sin α cos α 0 ⎦ . (2.6) the frame is left-handed. 0 0 1 The above-defined rotation matrix belongs to the special orthonormal group SO(m) of the real (m × m) matrices with othonormal columns and In a similar manner, it can be shown that the rotations by an angle β determinant equal to 1; in the case of spatial rotations it is m = 3, whereas about axis y and by an angle γ about axis x are respectively given by ⎡ ⎤ in the case of planar rotations it is m = 2. cos β 0 sin β Ry (β) = ⎣ 0 1 0 ⎦ (2.7) 2.2.1 Elementary Rotations −sin β 0 cos β ⎡ ⎤ 1 0 0 Consider the frames that can be obtained via elementary rotations of the Rx (γ) = ⎣ 0 cos γ −sin γ ⎦ . (2.8) reference frame about one of the coordinate axes. These rotations are positive 0 sin γ cos γ if they are made counter-clockwise about the relative axis. Suppose that the reference frame O–xyz is rotated by an angle α about These matrices will be useful to describe rotations about an arbitrary axis in axis z (Fig. 2.2), and let O–x y z be the rotated frame. The unit vectors of space. the new frame can be described in terms of their components with respect It is easy to verify that for the elementary rotation matrices in (2.6)–(2.8) to the reference frame. Consider the frames that can be obtained via elemen- the following property holds: tary rotations of the reference frame about one of the coordinate axes. These Rk (−ϑ) = RTk (ϑ) k = x, y, z. (2.9) rotations are positive if they are made counter-clockwise about the relative axis. In view of (2.6)–(2.8), the rotation matrix can be attributed a geometrical Suppose that the reference frame O–xyz is rotated by an angle α about meaning; namely, the matrix R describes the rotation about an axis in space axis z (Fig. 2.2), and let O–x y z be the rotated frame. The unit vectors of needed to align the axes of the reference frame with the corresponding axes the new frame can be described in terms of their components with respect to of the body frame. the reference frame, i.e., ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 2.2.2 Representation of a Vector cos α −sin α 0 x = ⎣ sin α ⎦ y = ⎣ cos α ⎦ z = ⎣ 0 ⎦ . In order to understand a further geometrical meaning of a rotation matrix, 0 0 1 consider the case when the origin of the body frame coincides with the origin 2.2 Rotation Matrix 43 44 2 Kinematics
Fig. 2.4. Representation of a point P in rotated frames
Fig. 2.3. Representation of a point P in two different coordinate frames Example 2.1
Consider two frames with common origin mutually rotated by an angle α about
of the reference frame (Fig. 2.3); it follows that o = 0, where 0 denotes the the axis z. Let p and p be the vectors of the coordinates of a point P , expressed (3 × 1) null vector. A point P in space can be represented either as in the frames O–xyz and O–x y z , respectively (Fig. 2.4). On the basis of simple geometry, the relationship between the coordinates of P in the two frames is ⎡ ⎤ px px = px cos α − py sin α p = ⎣ py ⎦ py = px sin α + py cos α pz pz = pz . with respect to frame O–xyz, or as Therefore, the matrix (2.6) represents not only the orientation of a frame with ⎡ ⎤ px respect to another frame, but it also describes the transformation of a vector from p = py ⎦ ⎣ a frame to another frame with the same origin. pz
with respect to frame O–x y z . Since p and p are representations of the same point P , it is 2.2.3 Rotation of a Vector ⎡ ⎤ A rotation matrix can be also interpreted as the matrix operator allowing p = px x + py y + pz z = ⎣ x y z ⎦ p rotation of a vector by a given angle about an arbitrary axis in space. In fact, let p be a vector in the reference frame O–xyz; in view of orthogonality of the matrix R, the product Rp yields a vector p with the same norm as that of p and, accounting for (2.3), it is but rotated with respect to p according to the matrix R. The norm equality p = Rp . (2.10) can be proved by observing that pT p = pT RT Rp and applying (2.4). This interpretation of the rotation matrix will be revisited later. The rotation matrix R represents the transformation matrix of the vector coordinates in frame O–x y z into the coordinates of the same vector in frame O–xyz. In view of the orthogonality property (2.4), the inverse transformation is simply given by p = RT p. (2.11) 2.3 Composition of Rotation Matrices 45 46 2 Kinematics
O–x1 y1 z1 , O–x2 y2 z2 be three frames with common origin O. The vector p
describing the position of a generic point in space can be expressed in each
of the above frames; let p0 , p1 , p2 denote the expressions of p in the three
frames.1
At first, consider the relationship between the expression p2 of the vector
p in Frame 2 and the expression p1 of the same vector in Frame 1. If Rji
denotes the rotation matrix of Frame i with respect to Frame j, it is
p1 = R12 p2 . (2.12)
Similarly, it turns out that
p0 = R01 p1 (2.13)
Fig. 2.5. Rotation of a vector
p 0
= R02 p2 . (2.14)
On the other hand, substituting (2.12) in (2.13) and using (2.14) gives
Example 2.2 R02 = R01 R12 . (2.15) Consider the vector p which is obtained by rotating a vector p in the plane xy by an angle α about axis z of the reference frame (Fig. 2.5). Let (px , py , pz ) be the The relationship in (2.15) can be interpreted as the composition of successive coordinates of the vector p . The vector p has components rotations. Consider a frame initially aligned with the frame O–x0 y0 z0 . The rotation expressed by matrix R02 can be regarded as obtained in two steps: px = px cos α − py sin α py = px sin α + py cos α • First rotate the given frame according to R01 , so as to align it with frame pz = pz . O–x1 y1 z1 . • Then rotate the frame, now aligned with frame O–x1 y1 z1 , according to It is easy to recognize that p can be expressed as R12 , so as to align it with frame O–x2 y2 z2 . p = Rz (α)p , Notice that the overall rotation can be expressed as a sequence of partial rotations; each rotation is defined with respect to the preceding one. The where Rz (α) is the same rotation matrix as in (2.6). frame with respect to which the rotation occurs is termed current frame. Composition of successive rotations is then obtained by postmultiplication of the rotation matrices following the given order of rotations, as in (2.15). With In sum, a rotation matrix attains three equivalent geometrical meanings: the adopted notation, in view of (2.5), it is • It describes the mutual orientation between two coordinate frames; its column vectors are the direction cosines of the axes of the rotated frame Rji = (Rij )−1 = (Rij )T . (2.16) with respect to the original frame. Successive rotations can be also specified by constantly referring them • It represents the coordinate transformation between the coordinates of a to the initial frame; in this case, the rotations are made with respect to a point expressed in two different frames (with common origin). fixed frame. Let R01 be the rotation matrix of frame O–x1 y1 z1 with respect • It is the operator that allows the rotation of a vector in the same coordinate 0 to the fixed frame O–x0 y0 z0 . Let then R̄2 denote the matrix characterizing frame. frame O–x2 y2 z2 with respect to Frame 0, which is obtained as a rotation of 1 Frame 1 according to the matrix R̄2 . Since (2.15) gives a composition rule of successive rotations about the axes of the current frame, the overall rotation 2.3 Composition of Rotation Matrices can be regarded as obtained in the following steps: In order to derive composition rules of rotation matrices, it is useful to consider 1 Hereafter, the superscript of a vector or a matrix denotes the frame in which its the expression of a vector in two different reference frames. Let then O–x0 y0 z0 , components are expressed. 2.3 Composition of Rotation Matrices 47 48 2 Kinematics
• First realign Frame 1 with Frame 0 by means of rotation R10 . 1 • Then make the rotation expressed by R̄2 with respect to the current frame. • Finally compensate for the rotation made for the realignment by means of the inverse rotation R01 .
Since the above rotations are described with respect to the current frame, the application of the composition rule (2.15) yields 0 1 R̄2 = R01 R10 R̄2 R01 .
In view of (2.16), it is 0 1 R̄2 = R̄2 R01 (2.17) 0 where the resulting R̄2 is different from the matrix R02 in (2.15). Hence, it can be stated that composition of successive rotations with respect to a fixed frame is obtained by premultiplication of the single rotation matrices in the order of the given sequence of rotations. By recalling the meaning of a rotation matrix in terms of the orientation of a current frame with respect to a fixed frame, it can be recognized that its Fig. 2.6. Successive rotations of an object about axes of current frame columns are the direction cosines of the axes of the current frame with respect to the fixed frame, while its rows (columns of its transpose and inverse) are the direction cosines of the axes of the fixed frame with respect to the current frame. An important issue of composition of rotations is that the matrix product is not commutative. In view of this, it can be concluded that two rotations in general do not commute and its composition depends on the order of the single rotations.
Example 2.3 Consider an object and a frame attached to it. Figure 2.6 shows the effects of two successive rotations of the object with respect to the current frame by changing the order of rotations. It is evident that the final object orientation is different in the two cases. Also in the case of rotations made with respect to the current frame, the final orientations differ (Fig. 2.7). It is interesting to note that the effects of the sequence of rotations with respect to the fixed frame are interchanged with the effects of the sequence of rotations with respect to the current frame. This can be explained by observing that the order of rotations in the fixed frame commutes with respect to Fig. 2.7. Successive rotations of an object about axes of fixed frame the order of rotations in the current frame. 2.4 Euler Angles Rotation matrices give a redundant description of frame orientation; in fact, they are characterized by nine elements which are not independent but related by six constraints due to the orthogonality conditions given in (2.4). This im- plies that three parameters are sufficient to describe orientation of a rigid body 2.4 Euler Angles 49 50 2 Kinematics
The resulting frame orientation is obtained by composition of rotations
with respect to current frames, and then it can be computed via postmulti-
plication of the matrices of elementary rotation, i.e.,2
R(φ) = Rz (ϕ)Ry (ϑ)Rz (ψ) (2.18)
⎡ ⎤
cϕ cϑ cψ − sϕ sψ −cϕ cϑ sψ − sϕ cψ cϕ sϑ
= ⎣ sϕ cϑ cψ + cϕ sψ −sϕ cϑ sψ + cϕ cψ sϕ sϑ ⎦ .
−sϑ cψ sϑ sψ cϑ
It is useful to solve the inverse problem, that is to determine the set of
Euler angles corresponding to a given rotation matrix
⎡ ⎤
Fig. 2.8. Representation of Euler angles ZYZ r11 r12 r13
R = ⎣ r21 r22 r23 ⎦ .
r31 r32 r33
in space. A representation of orientation in terms of three independent param- eters constitutes a minimal representation. In fact, a minimal representation Compare this expression with that of R(φ) in (2.18). By considering the of the special orthonormal group SO(m) requires m(m − 1)/2 parameters; elements [1, 3] and [2, 3], under the assumption that r13 = 0 and r23 = 0, it thus, three parameters are needed to parameterize SO(3), whereas only one follows that parameter is needed for a planar rotation SO(2). ϕ = Atan2(r23 , r13 ) A minimal representation of orientation can be obtained by using a set T of three angles φ = [ ϕ ϑ ψ ] . Consider the rotation matrix expressing where Atan2(y, x) is the arctangent function of two arguments3 . Then, squar- the elementary rotation about one of the coordinate axes as a function of a ing and summing the elements [1, 3] and [2, 3] and using the element [3, 3] single angle. Then, a generic rotation matrix can be obtained by composing a yields suitable sequence of three elementary rotations while guaranteeing that two ϑ = Atan2 2 + r2 , r r13 23 33 . successive rotations are not made about parallel axes. This implies that 12 distinct sets of angles are allowed out of all 27 possible combinations; each The choice of the positive sign for the term r13 2 + r 2 limits the range of 23 set represents a triplet of Euler angles. In the following, two sets of Euler feasible values of ϑ to (0, π). On this assumption, considering the elements angles are analyzed; namely, the ZYZ angles and the ZYX (or Roll–Pitch– [3, 1] and [3, 2] gives Yaw) angles. ψ = Atan2(r32 , −r31 ). In sum, the requested solution is 2.4.1 ZYZ Angles ϕ = Atan2(r23 , r13 ) The rotation described by ZYZ angles is obtained as composition of the fol- ϑ = Atan2 2 2 r13 + r23 , r33 (2.19) lowing elementary rotations (Fig. 2.8): • Rotate the reference frame by the angle ϕ about axis z; this rotation is ψ = Atan2(r32 , −r31 ). described by the matrix Rz (ϕ) which is formally defined in (2.6). • Rotate the current frame by the angle ϑ about axis y ; this rotation is It is possible to derive another solution which produces the same effects as described by the matrix Ry (ϑ) which is formally defined in (2.7). solution (2.19). Choosing ϑ in the range (−π, 0) leads to • Rotate the current frame by the angle ψ about axis z ; this rotation is ϕ = Atan2(−r23 , −r13 ) described by the matrix Rz (ψ) which is again formally defined in (2.6). 2 The notations cφ and sφ are the abbreviations for cos φ and sin φ, respectively; short-hand notations of this kind will be adopted often throughout the text. 3 The function Atan2(y, x) computes the arctangent of the ratio y/x but utilizes the sign of each argument to determine which quadrant the resulting angle belongs to; this allows the correct determination of an angle in a range of 2π. 2.4 Euler Angles 51 52 2 Kinematics
The resulting frame orientation is obtained by composition of rotations with
respect to the fixed frame, and then it can be computed via premultiplication
of the matrices of elementary rotation, i.e.,5
R(φ) = Rz (ϕ)Ry (ϑ)Rx (ψ) (2.21)
⎡ ⎤
cϕ cϑ cϕ sϑ sψ − sϕ cψ cϕ sϑ cψ + sϕ sψ
= ⎣ sϕ cϑ sϕ sϑ sψ + cϕ cψ sϕ sϑ cψ − cϕ sψ ⎦ .
−sϑ cϑ sψ cϑ cψ
As for the Euler angles ZYZ, the inverse solution to a given rotation matrix
⎡ ⎤
r11 r12 r13
R = ⎣ r21 r22 r23 ⎦ ,
r31 r32 r33
Fig. 2.9. Representation of Roll–Pitch–Yaw angles
can be obtained by comparing it with the expression of R(φ) in (2.21). The
2 + r2 , r
solution for ϑ in the range (−π/2, π/2) is
ϑ = Atan2 − r13 23 33 (2.20)
ϕ = Atan2(r21 , r11 )
ψ = Atan2(−r32 , r31 ).
ϑ = Atan2 −r31 , r32 2 + r2 (2.22)
33
Solutions (2.19), (2.20) degenerate when sϑ = 0; in this case, it is possible to determine only the sum or difference of ϕ and ψ. In fact, if ϑ = 0, π, ψ = Atan2(r32 , r33 ). the successive rotations of ϕ and ψ are made about axes of current frames which are parallel, thus giving equivalent contributions to the rotation; see The other equivalent solution for ϑ in the range (π/2, 3π/2) is Problem 2.2.4 ϕ = Atan2(−r21 , −r11 ) ϑ = Atan2 −r31 , − r32 2 + r2 (2.23) 2.4.2 RPY Angles 33
Another set of Euler angles originates from a representation of orientation in ψ = Atan2(−r32 , −r33 ). the (aero)nautical field. These are the ZYX angles, also called Roll–Pitch– Yaw angles, to denote the typical changes of attitude of an (air)craft. In this Solutions (2.22), (2.23) degenerate when cϑ = 0; in this case, it is possible to T case, the angles φ = [ ϕ ϑ ψ ] represent rotations defined with respect to determine only the sum or difference of ϕ and ψ. a fixed frame attached to the centre of mass of the craft (Fig. 2.9). The rotation resulting from Roll–Pitch–Yaw angles can be obtained as follows: 2.5 Angle and Axis • Rotate the reference frame by the angle ψ about axis x (yaw); this rotation A nonminimal representation of orientation can be obtained by resorting to is described by the matrix Rx (ψ) which is formally defined in (2.8). four parameters expressing a rotation of a given angle about an axis in space. • Rotate the reference frame by the angle ϑ about axis y (pitch); this rotation This can be advantageous in the problem of trajectory planning for a manip- is described by the matrix Ry (ϑ) which is formally defined in (2.7). ulator’s end-effector orientation. • Rotate the reference frame by the angle ϕ about axis z (roll); this rotation Let r = [ rx ry rz ]T be the unit vector of a rotation axis with respect is described by the matrix Rz (ϕ) which is formally defined in (2.6). to the reference frame O–xyz. In order to derive the rotation matrix R(ϑ, r) expressing the rotation of an angle ϑ about axis r, it is convenient to compose 4 5 In the following chapter, it will be seen that these configurations characterize the The ordered sequence of rotations XYZ about axes of the fixed frame is equivalent so-called representation singularities of the Euler angles. to the sequence ZYX about axes of the current frame. 2.5 Angle and Axis 53 54 2 Kinematics
Then, it can be found that the rotation matrix corresponding to a given angle
and axis is — see Problem 2.4 —
⎡ ⎤
rx2 (1 − cϑ ) + cϑ rx ry (1 − cϑ ) − rz sϑ rx rz (1 − cϑ ) + ry sϑ
⎢ ⎥
R(ϑ, r) = ⎣ rx ry (1 − cϑ ) + rz sϑ ry2 (1 − cϑ ) + cϑ ry rz (1 − cϑ ) − rx sϑ ⎦.
rx rz (1 − cϑ ) − ry sϑ ry rz (1 − cϑ ) + rx sϑ rz2 (1 − cϑ ) + cϑ
(2.25)
For this matrix, the following property holds:
R(−ϑ, −r) = R(ϑ, r), (2.26)
i.e., a rotation by −ϑ about −r cannot be distinguished from a rotation by ϑ
about r; hence, such representation is not unique.
If it is desired to solve the inverse problem to compute the axis and angle
corresponding to a given rotation matrix
Fig. 2.10. Rotation of an angle about an axis ⎡ ⎤
r11 r12 r13
R = ⎣ r21 r22 r23 ⎦ ,
elementary rotations about the coordinate axes of the reference frame. The r31 r32 r33 angle is taken to be positive if the rotation is made counter-clockwise about the following result is useful: axis r. As shown in Fig. 2.10, a possible solution is to rotate first r by the angles r11 + r22 + r33 − 1 ϑ = cos −1 (2.27) necessary to align it with axis z, then to rotate by ϑ about z and finally 2 ⎡ ⎤ to rotate by the angles necessary to align the unit vector with the initial r32 − r23 1 ⎣ direction. In detail, the sequence of rotations, to be made always with respect r= r13 − r31 ⎦ , (2.28) to axes of fixed frame, is the following: 2 sin ϑ r21 − r12 • Align r with z, which is obtained as the sequence of a rotation by −α for sin ϑ = 0. Notice that the expressions (2.27), (2.28) describe the rotation about z and a rotation by −β about y. in terms of four parameters; namely, the angle and the three components of • Rotate by ϑ about z. the axis unit vector. However, it can be observed that the three components • Realign with the initial direction of r, which is obtained as the sequence of r are not independent but are constrained by the condition of a rotation by β about y and a rotation by α about z. rx2 + ry2 + rz2 = 1. (2.29) In sum, the resulting rotation matrix is If sin ϑ = 0, the expressions (2.27), (2.28) become meaningless. To solve the R(ϑ, r) = Rz (α)Ry (β)Rz (ϑ)Ry (−β)Rz (−α). (2.24) inverse problem, it is necessary to directly refer to the particular expressions attained by the rotation matrix R and find the solving formulae in the two From the components of the unit vector r it is possible to extract the tran- cases ϑ = 0 and ϑ = π. Notice that, when ϑ = 0 (null rotation), the unit scendental functions needed to compute the rotation matrix in (2.24), so as vector r is arbitrary (singularity). See also Problem 2.5. to eliminate the dependence from α and β; in fact, it is ry rx 2.6 Unit Quaternion sin α = cos α = rx2 + ry2 rx2 + ry2 The drawbacks of the angle/axis representation can be overcome by a dif- ferent four-parameter representation; namely, the unit quaternion, viz. Euler sin β = rx2 + ry2 cos β = rz . parameters, defined as Q = {η, } where: ϑ η = cos (2.30) 2 2.6 Unit Quaternion 55 56 2 Kinematics
ϑ
= sin r; (2.31)
2
η is called the scalar part of the quaternion while = [ x y z ]T is called the vector part of the quaternion. They are constrained by the condition η 2 + 2x + 2y + 2z = 1, (2.32) hence, the name unit quaternion. It is worth remarking that, unlike the an- gle/axis representation, a rotation by −ϑ about −r gives the same quater- nion as that associated with a rotation by ϑ about r; this solves the above nonuniqueness problem. In view of (2.25), (2.30), (2.31), (2.32), the rotation matrix corresponding to a given quaternion takes on the form — see Prob- lem 2.6 — ⎡ ⎤ Fig. 2.11. Representation of a point P in different coordinate frames 2(η 2 + 2x ) − 1 2( x y − η z ) 2( x z + η y ) ⎢ ⎥ R(η, ) = ⎣ 2( x y + η z ) 2(η 2 + 2y ) − 1 2( y z − η x ) ⎦ . (2.33) 2.7 Homogeneous Transformations 2( x z − η y ) 2( y z + η x ) 2(η 2 + 2z ) − 1 If it is desired to solve the inverse problem to compute the quaternion As illustrated at the beginning of the chapter, the position of a rigid body in corresponding to a given rotation matrix space is expressed in terms of the position of a suitable point on the body with ⎡ ⎤ respect to a reference frame (translation), while its orientation is expressed in r11 r12 r13 terms of the components of the unit vectors of a frame attached to the body R = ⎣ r21 r22 r23 ⎦ , — with origin in the above point — with respect to the same reference frame r31 r32 r33 (rotation). the following result is useful: As shown in Fig. 2.11, consider an arbitrary point P in space. Let p0 1√ be the vector of coordinates of P with respect to the reference frame O0 – η= r11 + r22 + r33 + 1 (2.34) 2⎡ x0 y0 z0 . Consider then another frame in space O1 –x1 y1 z1 . Let o01 be the vector √ ⎤ sgn (r32 − r23 ) r11 − r22 − r33 + 1 describing the origin of Frame 1 with respect to Frame 0, and R01 be the 1⎢ √ ⎥ rotation matrix of Frame 1 with respect to Frame 0. Let also p1 be the vector = ⎣ sgn (r13 − r31 ) r22 − r33 − r11 + 1 ⎦ , (2.35) 2 √ of coordinates of P with respect to Frame 1. On the basis of simple geometry, sgn (r21 − r12 ) r33 − r11 − r22 + 1 the position of point P with respect to the reference frame can be expressed where conventionally sgn (x) = 1 for x ≥ 0 and sgn (x) = −1 for x < 0. Notice as that in (2.34) it has been implicitly assumed η ≥ 0; this corresponds to an p0 = o01 + R01 p1 . (2.38) angle ϑ ∈ [−π, π], and thus any rotation can be described. Also, compared to Hence, (2.38) represents the coordinate transformation (translation + rota- the inverse solution in (2.27), (2.28) for the angle and axis representation, no tion) of a bound vector between two frames. singularity occurs for (2.34), (2.35). See also Problem 2.8. The inverse transformation can be obtained by premultiplying both sides The quaternion extracted from R−1 = RT is denoted as Q−1 , and can be of (2.38) by R01 T ; in view of(2.4), it follows that computed as Q−1 = {η, −}. (2.36) p1 = −R01 T o01 + R01 T p0 (2.39) Let Q1 = {η1 , 1 } and Q2 = {η2 , 2 } denote the quaternions corresponding which, via (2.16), can be written as to the rotation matrices R1 and R2 , respectively. The quaternion correspond- ing to the product R1 R2 is given by p1 = −R10 o01 + R10 p0 . (2.40) Q1 ∗ Q2 = {η1 η2 − T1 2 , η1 2 + η2 1 + 1 × 2 } (2.37) In order to achieve a compact representation of the relationship between where the quaternion product operator “∗” has been formally introduced. It is the coordinates of the same point in two different frames, the homogeneous easy to see that if Q2 = Q−1 1 then the quaternion {1, 0} is obtained from (2.37) representation of a generic vector p can be introduced as the vector p̃ formed which is the identity element for the product. See also Problem 2.9. by adding a fourth unit component, i.e., 2.7 Homogeneous Transformations 57 58 2 Kinematics
⎡ ⎤
⎢p⎥
p = ⎣ ⎦. (2.41)
1
By adopting this representation for the vectors p0 and p1 in (2.38), the coor- dinate transformation can be written in terms of the (4 × 4) matrix ⎡ ⎤ ⎢ R01 o01 ⎥ Fig. 2.12. Conventional representations of joints A01 = ⎣ ⎦ (2.42) 0T 1 same origin, it reduces to the rotation matrix previously defined. Instead, if which, according to (2.41), is termed homogeneous transformation matrix . the frames have distinct origins, it allows the notation with superscripts and Since o01 ∈ IR3 e R01 ∈ SO(3), this matrix belongs to the special Euclidean subscripts to be kept which directly characterize the current frame and the group SE(3) = IR3 × SO(3). fixed frame. As can be easily seen from (2.42), the transformation of a vector from Analogously to what presented for the rotation matrices, it is easy to Frame 1 to Frame 0 is expressed by a single matrix containing the rotation verify that a sequence of coordinate transformations can be composed by the matrix of Frame 1 with respect to Frame 0 and the translation vector from product the origin of Frame 0 to the origin of Frame 1.6 Therefore, the coordinate p0 = A01 A12 … An−1 pn (2.47) n transformation (2.38) can be compactly rewritten as where Ai−1 i denotes the homogeneous transformation relating the description p0 = A01 p1 . (2.43) of a point in Frame i to the description of the same point in Frame i − 1. The coordinate transformation between Frame 0 and Frame 1 is described by the homogeneous transformation matrix A10 which satisfies the equation 2.8 Direct Kinematics −1 0 p1 = A10 p0 = A01 p . (2.44) A manipulator consists of a series of rigid bodies (links) connected by means of This matrix is expressed in a block-partitioned form as kinematic pairs or joints. Joints can be essentially of two types: revolute and ⎡ ⎤ ⎡ ⎤ prismatic; conventional representations of the two types of joints are sketched in Fig. 2.12. The whole structure forms a kinematic chain. One end of the ⎢ R01 T −R01 T o01 ⎥ ⎢ R10 −R10 o01 ⎥ A10 = ⎣ ⎦=⎣ ⎦, (2.45) chain is constrained to a base. An end-effector (gripper, tool) is connected to T T the other end allowing manipulation of objects in space. 0 1 0 1 From a topological viewpoint, the kinematic chain is termed open when which gives the homogeneous representation form of the result already estab- there is only one sequence of links connecting the two ends of the chain. Al- lished by (2.39), (2.40) — see Problem 2.10. ternatively, a manipulator contains a closed kinematic chain when a sequence Notice that for the homogeneous transformation matrix the orthogonality of links forms a loop. property does not hold; hence, in general, The mechanical structure of a manipulator is characterized by a number of degrees of freedom (DOFs) which uniquely determine its posture.7 Each DOF A−1 = AT . (2.46) is typically associated with a joint articulation and constitutes a joint variable. The aim of direct kinematics is to compute the pose of the end-effector as a In sum, a homogeneous transformation matrix expresses the coordinate function of the joint variables. transformation between two frames in a compact form. If the frames have the 6 7 It can be shown that in (2.42) non-null values of the first three elements of the The term posture of a kinematic chain denotes the pose of all the rigid bodies fourth row of A produce a perspective effect, while values other than unity for composing the chain. Whenever the kinematic chain reduces to a single rigid the fourth element give a scaling effect. body, then the posture coincides with the pose of the body. 2.8 Direct Kinematics 59 60 2 Kinematics
Fig. 2.14. Two-link planar arm
Example 2.4
Fig. 2.13. Description of the position and orientation of the end-effector frame Consider the two-link planar arm in Fig. 2.14. On the basis of simple trigonometry, the choice of the joint variables, the base frame, and the end-effector frame leads to8 It was previously illustrated that the pose of a body with respect to a ⎡ ⎤ ⎡ ⎤ reference frame is described by the position vector of the origin and the unit 0 s12 c12 a1 c1 + a2 c12 ⎢ nbe sbe abe pbe ⎥ ⎢0 −c12 s12 a1 s1 + a2 s12 ⎥ vectors of a frame attached to the body. Hence, with respect to a reference T be (q) = ⎣ ⎦ = ⎣1 ⎦. (2.49) 0 0 0 frame Ob –xb yb zb , the direct kinematics function is expressed by the homoge- 0 0 0 1 0 0 0 1 neous transformation matrix ⎡ ⎤ ⎢ nbe (q) sbe (q) abe (q) pbe (q) ⎥ It is not difficult to infer that the effectiveness of a geometric approach T be (q) = ⎣ ⎦, (2.48) to the direct kinematics problem is based first on a convenient choice of the 0 0 0 1 relevant quantities and then on the ability and geometric intuition of the prob- lem solver. Whenever the manipulator structure is complex and the number of where q is the (n × 1) vector of joint variables, ne , se , ae are the unit vectors joints increases, it is preferable to adopt a less direct solution, which, though, of a frame attached to the end-effector, and pe is the position vector of the is based on a systematic, general procedure. The problem becomes even more origin of such a frame with respect to the origin of the base frame Ob –xb yb zb complex when the manipulator contains one or more closed kinematic chains. (Fig. 2.13). Note that ne , se , ae and pe are a function of q. In such a case, as it will be discussed later, there is no guarantee to obtain an The frame Ob –xb yb zb is termed base frame. The frame attached to the end- analytical expression for the direct kinematics function in (2.48). effector is termed end-effector frame and is conveniently chosen according to the particular task geometry. If the end-effector is a gripper, the origin of the end-effector frame is located at the centre of the gripper, the unit vector ae 2.8.1 Open Chain is chosen in the approach direction to the object, the unit vector se is chosen Consider an open-chain manipulator constituted by n + 1 links connected by normal to ae in the sliding plane of the jaws, and the unit vector ne is chosen n joints, where Link 0 is conventionally fixed to the ground. It is assumed that normal to the other two so that the frame (ne , se , ae ) is right-handed. each joint provides the mechanical structure with a single DOF, corresponding A first way to compute direct kinematics is offered by a geometric analysis to the joint variable. of the structure of the given manipulator. The construction of an operating procedure for the computation of di- rect kinematics is naturally derived from the typical open kinematic chain of the manipulator structure. In fact, since each joint connects two consecutive 8 The notations si…j , ci…j denote respectively sin (qi + … + qj ), cos (qi + … + qj ). 2.8 Direct Kinematics 61 62 2 Kinematics
Fig. 2.16. Denavit–Hartenberg kinematic parameters
Fig. 2.15. Coordinate transformations in an open kinematic chain
method is to be derived to define the relative position and orientation of two
consecutive links; the problem is that to determine two frames attached to
links, it is reasonable to consider first the description of kinematic relationship the two links and compute the coordinate transformations between them. In between consecutive links and then to obtain the overall description of manip- general, the frames can be arbitrarily chosen as long as they are attached to ulator kinematics in a recursive fashion. To this purpose, it is worth defining the link they are referred to. Nevertheless, it is convenient to set some rules a coordinate frame attached to each link, from Link 0 to Link n. Then, the also for the definition of the link frames. coordinate transformation describing the position and orientation of Frame n With reference to Fig. 2.16, let Axis i denote the axis of the joint connect- with respect to Frame 0 (Fig. 2.15) is given by ing Link i − 1 to Link i; the so-called Denavit–Hartenberg convention (DH) is T 0n (q) = A01 (q1 )A12 (q2 ) … An−1 n (qn ). (2.50) adopted to define link Frame i:
As requested, the computation of the direct kinematics function is recursive • Choose axis zi along the axis of Joint i + 1. and is obtained in a systematic manner by simple products of the homogeneous • Locate the origin Oi at the intersection of axis zi with the common normal9 transformation matrices Ai−1 (qi ) (for i = 1, … , n), each of which is a function to axes zi−1 and zi . Also, locate Oi at the intersection of the common i of a single joint variable. normal with axis zi−1 . With reference to the direct kinematics equation in (2.49), the actual co- • Choose axis xi along the common normal to axes zi−1 and zi with direction ordinate transformation describing the position and orientation of the end- from Joint i to Joint i + 1. effector frame with respect to the base frame can be obtained as • Choose axis yi so as to complete a right-handed frame.
T be (q) = T b0 T 0n (q)T ne (2.51) The Denavit–Hartenberg convention gives a nonunique definition of the link
frame in the following cases:
where T b0 and T ne are two (typically) constant homogeneous transformations • For Frame 0, only the direction of axis z0 is specified; then O0 and x0 can describing the position and orientation of Frame 0 with respect to the base be arbitrarily chosen. frame, and of the end-effector frame with respect to Frame n, respectively. • For Frame n, since there is no Joint n + 1, zn is not uniquely defined while xn has to be normal to axis zn−1 . Typically, Joint n is revolute, and thus 2.8.2 Denavit–Hartenberg Convention zn is to be aligned with the direction of zn−1 . In order to compute the direct kinematics equation for an open-chain manip- 9 The common normal between two lines is the line containing the minimum dis- ulator according to the recursive expression in (2.50), a systematic, general tance segment between the two lines. 2.8 Direct Kinematics 63 64 2 Kinematics
• When two consecutive axes are parallel, the common normal between them • The resulting coordinate transformation is obtained by postmultiplication is not uniquely defined. of the single transformations as • When two consecutive axes intersect, the direction of xi is arbitrary. ⎡ ⎤ • When Joint i is prismatic, the direction of zi−1 is arbitrary. cϑi −sϑi cαi sϑi sαi ai cϑi i−1 i−1 i ⎢ sϑi cϑi cαi −cϑi sαi ai sϑi ⎥ Ai (qi ) = Ai Ai = ⎣ ⎦. (2.52) In all such cases, the indeterminacy can be exploited to simplify the procedure; 0 sαi cαi di for instance, the axes of consecutive frames can be made parallel. 0 0 0 1 Once the link frames have been established, the position and orientation of Frame i with respect to Frame i − 1 are completely specified by the following Notice that the transformation matrix from Frame i to Frame i−1 is a function parameters: only of the joint variable qi , that is, ϑi for a revolute joint or di for a prismatic joint. ai distance between Oi and Oi , To summarize, the Denavit–Hartenberg convention allows the construction di coordinate of Oi along zi−1 , of the direct kinematics function by composition of the individual coordinate αi angle between axes zi−1 and zi about axis xi to be taken positive when transformations expressed by (2.52) into one homogeneous transformation rotation is made counter-clockwise, matrix as in (2.50). The procedure can be applied to any open kinematic ϑi angle between axes xi−1 and xi about axis zi−1 to be taken positive when chain and can be easily rewritten in an operating form as follows. rotation is made counter-clockwise. 1. Find and number consecutively the joint axes; set the directions of axes Two of the four parameters (ai and αi ) are always constant and depend z0 , … , zn−1 . only on the geometry of connection between consecutive joints established 2. Choose Frame 0 by locating the origin on axis z0 ; axes x0 and y0 are by Link i. Of the remaining two parameters, only one is variable depending chosen so as to obtain a right-handed frame. If feasible, it is worth choosing on the type of joint that connects Link i − 1 to Link i. In particular: Frame 0 to coincide with the base frame. • if Joint i is revolute the variable is ϑi , Execute steps from 3 to 5 for i = 1, … , n − 1: • if Joint i is prismatic the variable is di . 3. Locate the origin Oi at the intersection of zi with the common normal to At this point, it is possible to express the coordinate transformation between axes zi−1 and zi . If axes zi−1 and zi are parallel and Joint i is revolute, Frame i and Frame i − 1 according to the following steps: then locate Oi so that di = 0; if Joint i is prismatic, locate Oi at a reference position for the joint range, e.g., a mechanical limit. • Choose a frame aligned with Frame i − 1. 4. Choose axis xi along the common normal to axes zi−1 and zi with direction • Translate the chosen frame by di along axis zi−1 and rotate it by ϑi about from Joint i to Joint i + 1. axis zi−1 ; this sequence aligns the current frame with Frame i and is 5. Choose axis yi so as to obtain a right-handed frame. described by the homogeneous transformation matrix ⎡ ⎤ To complete: cϑi −sϑi 0 0 i−1 ⎢ sϑi cϑi 0 0⎥ 6. Choose Frame n; if Joint n is revolute, then align zn with zn−1 , otherwise, Ai = ⎣ ⎦. 0 0 1 di if Joint n is prismatic, then choose zn arbitrarily. Axis xn is set according 0 0 0 1 to step 4. 7. For i = 1, … , n, form the table of parameters ai , di , αi , ϑi . • Translate the frame aligned with Frame i by ai along axis xi and rotate 8. On the basis of the parameters in 7, compute the homogeneous transfor- it by αi about axis xi ; this sequence aligns the current frame with Frame i mation matrices Ai−1 (qi ) for i = 1, … , n. i and is described by the homogeneous transformation matrix 9. Compute the homogeneous transformation T 0n (q) = A01 … An−1 n that ⎡ ⎤ yields the position and orientation of Frame n with respect to Frame 0. 1 0 0 ai 10.Given T b0 and T ne , compute the direct kinematics function as T be (q) = i ⎢0 cαi −sαi 0⎥ Ai = ⎣ ⎦. T b0 T 0n T ne that yields the position and orientation of the end-effector frame 0 sαi cαi 0 with respect to the base frame. 0 0 0 1 2.8 Direct Kinematics 65 66 2 Kinematics
Fig. 2.17. Connection of a single link in the chain with two links
For what concerns the computational aspects of direct kinematics, it can be Fig. 2.18. Coordinate transformations in a closed kinematic chain recognized that the heaviest load derives from the evaluation of transcenden- tal functions. On the other hand, by suitably factorizing the transformation equations and introducing local variables, the number of flops (additions + frames can be defined as in Fig. 2.18. Since Links 0 through i occur before multiplications) can be reduced. Finally, for computation of orientation it is the two branches of the tree, they are left out of the analysis. For the same convenient to evaluate the two unit vectors of the end-effector frame of sim- reason, Links j + 1 through n are left out as well. Notice that Frame i is to plest expression and derive the third one by vector product of the first two. be chosen with axis zi aligned with the axes of Joints i + 1 and i + 1 . It follows that the position and orientation of Frame j with respect to Frame i can be expressed by composing the homogeneous transformations as 2.8.3 Closed Chain Aij (q ) = Aii+1 (qi+1 ) … Aj−1 j (qj ) (2.53) The above direct kinematics method based on the DH convention exploits where q = [ qi+1 … qj ] . Likewise, the position and orientation of T the inherently recursive feature of an open-chain manipulator. Nevertheless, the method can be extended to the case of manipulators containing closed Frame k with respect to Frame i is given by kinematic chains according to the technique illustrated below. Consider a closed-chain manipulator constituted by n + 1 links. Because Aik (q ) = Aii+1 (qi+1 ) … Ak−1 k (qk ) (2.54) of the presence of a loop, the number of joints l must be greater than n; in where q = [ qi+1 … qk ] . T particular, it can be understood that the number of closed loops is equal to Since Links j and k are connected to each other through Joint j + 1, l − n. it is worth analyzing the mutual position and orientation between Frames j With reference to Fig. 2.17, Links 0 through i are connected successively and k, as illustrated in Fig. 2.19. Notice that, since Links j and k are connected through the first i joints as in an open kinematic chain. Then, Joint i + 1 to form a closed chain, axes zj and zk are aligned. Therefore, the following connects Link i with Link i + 1 while Joint i + 1 connects Link i with orientation constraint has to be imposed between Frames j and k: Link i + 1 ; the axes of Joints i + 1 and i + 1 are assumed to be aligned. Although not represented in the figure, Links i + 1 and i + 1 are members z ij (q ) = z ik (q ), (2.55) of the closed kinematic chain. In particular, Link i + 1 is further connected to Link i + 2 via Joint i + 2 and so forth, until Link j via Joint j. Likewise, where the unit vectors of the two axes have been conveniently referred to Link i + 1 is further connected to Link i + 2 via Joint i + 2 and so forth, Frame i. until Link k via Joint k. Finally, Links j and k are connected together at Moreover, if Joint j + 1 is prismatic, the angle ϑjk between axes xj and xk Joint j + 1 to form a closed chain. In general, j = k. is fixed; hence, in addition to (2.55), the following constraint is obtained: In order to attach frames to the various links and apply DH convention, xiT i j (q )xk (q ) = cos ϑjk . (2.56) one closed kinematic chain is taken into account. The closed chain can be virtually cut open at Joint j + 1, i.e., the joint between Link j and Link k. Obviously, there is no need to impose a similar constraint on axes yj and yk An equivalent tree-structured open kinematic chain is obtained, and thus link since that would be redundant. 2.8 Direct Kinematics 67 68 2 Kinematics
In either case, there are six equalities that must be satisfied. Those should be
solved for a reduced number of independent joint variables to be keenly chosen
among the components of q and q which characterize the DOFs of the closed
chain. These are the natural candidates to be the actuated joints, while the
other joints in the chain (including the cut joint) are typically not actuated.
Such independent variables, together with the remaining joint variables not
involved in the above analysis, constitute the joint vector q that allows the
direct kinematics equation to be computed as
T 0n (q) = A0i Aij Ajn , (2.61)
where the sequence of successive transformations after the closure of the chain
has been conventionally resumed from Frame j.
In general, there is no guarantee to solve the constraints in closed form
unless the manipulator has a simple kinematic structure. In other words, for
Fig. 2.19. Coordinate transformation at the cut joint
a given manipulator with a specific geometry, e.g., a planar structure, some of
the above equalities may become dependent. Hence, the number of indepen-
dent equalities is less than six and it should likely be easier to solve them.
Regarding the position constraint between Frames j and k, let pij and To conclude, it is worth sketching the operating form of the procedure to i compute the direct kinematics function for a closed-chain manipulator using pk respectively denote the positions of the origins of Frames j and k, when referred to Frame i. By projecting on Frame j the distance vector of the origin the Denavit–Hartenberg convention. of Frame k from Frame j, the following constraint has to be imposed: 1. In the closed chain, select one joint that is not actuated. Assume that the Rji (q ) pij (q ) − pik (q ) = [ 0 0 djk ] T joint is cut open so as to obtain an open chain in a tree structure. (2.57) 2. Compute the homogeneous transformations according to DH convention. where Rji = RiT j denotes the orientation of Frame i with respect to Frame j. 3. Find the equality constraints for the two frames connected by the cut joint. At this point, if Joint j + 1 is revolute, then djk is a fixed offset along axis zj ; 4. Solve the constraints for a reduced number of joint variables. hence, the three equalities of (2.57) fully describe the position constraint. If, 5. Express the homogeneous transformations in terms of the above joint vari- however, Joint j + 1 is prismatic, then djk varies. Consequently, only the first ables and compute the direct kinematics function by composing the various two equalities of (2.57) describe the position constraint, i.e., transformations from the base frame to the end-effector frame. iT xj (q ) 0 pi j (q ) − p i k (q ) = (2.58) y iT j (q ) 0 2.9 Kinematics of Typical Manipulator Structures where Rij = [ xij y ij z ij ]. This section contains several examples of computation of the direct kinemat- In summary, if Joint j + 1 is revolute the constraints are ics function for typical manipulator structures that are often encountered in j Ri (q ) pij (q ) − pik (q ) = [ 0 0 djk ] T industrial robots. (2.59) With reference to the schematic representation of the kinematic chain, z ij (q ) = z ik (q ), manipulators are usually illustrated in postures where the joint variables, de- whereas if Joint j + 1 is prismatic the constraints are fined according to the DH convention, are different from zero; such values might differ from the null references utilized for robot manipulator program- ⎧ iT ⎪ xj (q ) 0 ming. Hence, it will be necessary to sum constant contributions (offsets) to ⎪ ⎪ p i (q ) − pi (q ) = ⎨ y iT j (q ) j k 0 the values of the joint variables measured by the robot sensory system, so as (2.60) to match the references. ⎪ ⎪ z ij (q ) = z ik (q ) ⎪ ⎩ iT i xj (q )xk (q ) = cos ϑjk . 2.9 Kinematics of Typical Manipulator Structures 69 70 2 Kinematics
Fig. 2.21. Parallelogram arm
Fig. 2.20. Three-link planar arm
Computation of the direct kinematics function as in (2.50) yields
⎡ ⎤
2.9.1 Three-link Planar Arm c123 −s123 0 a1 c1 + a2 c12 + a3 c123 ⎢s c123 0 a1 s1 + a2 s12 + a3 s123 ⎥ T 03 (q) = A01 A12 A23 = ⎣ 123 ⎦ (2.63) Consider the three-link planar arm in Fig. 2.20, where the link frames have 0 0 1 0 been illustrated. Since the revolute axes are all parallel, the simplest choice 0 0 0 1 was made for all axes xi along the direction of the relative links (the direction where q = [ ϑ1 ϑ2 ϑ3 ]T . Notice that the unit vector z 03 of Frame 3 is aligned of x0 is arbitrary) and all lying in the plane (x0 , y0 ). In this way, all the with z 0 = [ 0 0 1 ]T , in view of the fact that all revolute joints are parallel parameters di are null and the angles between the axes xi directly provide the to axis z0 . Obviously, pz = 0 and all three joints concur to determine the joint variables. The DH parameters are specified in Table 2.1. end-effector position in the plane of the structure. It is worth pointing out that Frame 3 does not coincide with the end-effector frame (Fig. 2.13), since Table 2.1. DH parameters for the three-link planar arm the resulting approach unit vector is aligned with x03 and not with z 03 . Thus, assuming that the two frames have the same origin, the constant transforma- Link ai αi di ϑi tion ⎡ ⎤ 1 a1 0 0 ϑ1 0 0 1 0 2 a2 0 0 ϑ2 ⎢ 0 1 0 0⎥ 3 a3 0 0 ϑ3 T 3e = ⎣ ⎦. −1 0 0 0 0 0 0 1 Since all joints are revolute, the homogeneous transformation matrix de- is needed, having taken n aligned with z 0 . fined in (2.52) has the same structure for each joint, i.e., ⎡ ⎤ 2.9.2 Parallelogram Arm ci −si 0 ai ci i−1 ⎢ si ci 0 ai si ⎥ Consider the parallelogram arm in Fig. 2.21. A closed chain occurs where the Ai (ϑi ) = ⎣ ⎦ i = 1, 2, 3. (2.62) 0 0 1 0 first two joints connect Link 1 and Link 1 to Link 0, respectively. Joint 4 was 0 0 0 1 selected as the cut joint, and the link frames have been established accordingly. The DH parameters are specified in Table 2.2, where a1 = a3 and a2 = a1 in view of the parallelogram structure. Notice that the parameters for Link 4 are all constant. Since the joints are revolute, the homogeneous transformation matrix defined in (2.52) has 2.9 Kinematics of Typical Manipulator Structures 71 72 2 Kinematics T Table 2.2. DH parameters for the parallelogram arm Therefore, the vector of joint variables is q = [ ϑ1 ϑ1 ] . These joints are Link ai αi di ϑi natural candidates to be the actuated joints.10 Substituting the expressions 1 a1 0 0 ϑ 1 of ϑ2 and ϑ3 into the homogeneous transformation A03 and computing the 2 a2 0 0 ϑ 2 direct kinematics function as in (2.61) yields 3 a3 0 0 ϑ 3 ⎡ ⎤ 1 a1 0 0 ϑ1 −c1 s1 0 a1 c1 − a4 c1 4 a4 0 0 0 0 0 3 ⎢ −s1 −c1 0 a1 s1 − a4 s1 ⎥ T 4 (q) = A3 (q)A4 = ⎣ ⎦. (2.64) 0 0 1 0 0 0 0 1 the same structure for each joint, i.e., as in (2.62) for Joints 1 , 2 , 3 and 1 . Therefore, the coordinate transformations for the two branches of the tree are A comparison between (2.64) and (2.49) reveals that the parallelogram arm is respectively: kinematically equivalent to a two-link planar arm. The noticeable difference, ⎡ ⎤ though, is that the two actuated joints — providing the DOFs of the structure c1 2 3 −s1 2 3 0 a1 c1 + a2 c1 2 + a3 c1 2 3 — are located at the base. This will greatly simplify the dynamic model of ⎢s c1 2 3 0 a1 s1 + a2 s1 2 + a3 s1 2 3 ⎥ A03 (q ) = A01 A12 A23 = ⎣ 1 2 3 ⎦ the structure, as will be seen in Sect. 7.3.3. 0 0 1 0 0 0 0 1 2.9.3 Spherical Arm T where q = [ ϑ 1 ϑ 2 ϑ ] , and 3 Consider the spherical arm in Fig. 2.22, where the link frames have been ⎡ ⎤ c1 −s1 0 a1 c1 illustrated. Notice that the origin of Frame 0 was located at the intersection 0 ⎢ s1 c1 0 a1 s1 ⎥ of z0 with z1 so that d1 = 0; analogously, the origin of Frame 2 was located A1 (q ) = ⎣ ⎦ 0 0 1 0 at the intersection between z1 and z2 . The DH parameters are specified in 0 0 0 1 Table 2.3. where q = ϑ1 . To complete, the constant homogeneous transformation for the last link is ⎡ ⎤ Table 2.3. DH parameters for the spherical arm 1 0 0 a4 Link ai αi di ϑi ⎢0 1 0 0 ⎥ A34 = ⎣ ⎦. 1 0 −π/2 0 ϑ1 0 0 1 0 2 0 π/2 d2 ϑ2 0 0 0 1 3 0 0 d3 0 With reference to (2.59), the position constraints are (d3 1 = 0) ⎡ ⎤ 0 The homogeneous transformation matrices defined in (2.52) are for the R30 (q ) p03 (q ) − p01 (q ) = ⎣ 0 ⎦ single joints: 0 ⎡ ⎤ ⎡ ⎤ c1 0 −s1 0 c2 0 s2 0 while the orientation constraints are satisfied independently of q and q . Since ⎢ s1 0 c1 0⎥ ⎢ s2 0 −c2 0 ⎥ 0 1 a1 = a3 and a2 = a1 , two independent constraints can be extracted, i.e., A1 (ϑ1 ) = ⎣ ⎦ A2 (ϑ2 ) = ⎣ ⎦ 0 −1 0 0 0 1 0 d2 0 0 0 1 0 0 0 1 a1 (c1 + c1 2 3 ) + a1 (c1 2 − c1 ) = 0 ⎡ ⎤ 1 0 0 0 a1 (s1 + s1 2 3 ) + a1 (s1 2 − s1 ) = 0. ⎢0 1 0 0 ⎥ A23 (d3 ) = ⎣ ⎦. 0 0 1 d3 In order to satisfy them for any choice of a1 and a1 , it must be 0 0 0 1 ϑ2 = ϑ1 − ϑ1 10 Notice that it is not possible to solve (2.64) for ϑ2 and ϑ3 since they are con- ϑ3 = π − ϑ2 = π − ϑ1 + ϑ1 strained by the condition ϑ2 + ϑ3 = π. 2.9 Kinematics of Typical Manipulator Structures 73 74 2 Kinematics
Fig. 2.23. Anthropomorphic arm
Fig. 2.22. Spherical arm
Table 2.4. DH parameters for the anthropomorphic arm
Computation of the direct kinematics function as in (2.50) yields Link ai αi di ϑi ⎡ ⎤ 1 0 π/2 0 ϑ1 c1 c2 −s1 c1 s2 c1 s2 d3 − s1 d2 2 a2 0 0 ϑ2 0 0 1 2 ⎢ s1 c2 c1 s1 s2 s1 s2 d3 + c1 d2 ⎥ 3 a3 0 0 ϑ3 T 3 (q) = A1 A2 A3 = ⎣ ⎦ (2.65) −s2 0 c2 c2 d3 0 0 0 1 The homogeneous transformation matrices defined in (2.52) are for the where q = [ ϑ1 ϑ2 d3 ]T . Notice that the third joint does not obviously single joints: influence the rotation matrix. Further, the orientation of the unit vector y 03 ⎡ ⎤ c1 0 s1 0 is uniquely determined by the first joint, since the revolute axis of the second ⎢ s 0 −c1 0 ⎥ A01 (ϑ1 ) = ⎣ 1 ⎦ joint z1 is parallel to axis y3 . Different from the previous structures, in this 0 1 0 0 case Frame 3 can represent an end-effector frame of unit vectors (ne , se , ae ), 0 0 0 1 i.e., T 3e = I 4 . ⎡ ⎤ ci −si 0 ai ci i−1 ⎢ si ci 0 ai si ⎥ Ai (ϑi ) = ⎣ ⎦ i = 2, 3. 2.9.4 Anthropomorphic Arm 0 0 1 0 0 0 0 1 Consider the anthropomorphic arm in Fig. 2.23. Notice how this arm corre- sponds to a two-link planar arm with an additional rotation about an axis Computation of the direct kinematics function as in (2.50) yields of the plane. In this respect, the parallelogram arm could be used in lieu of ⎡ ⎤ c1 c23 −c1 s23 s1 c1 (a2 c2 + a3 c23 ) the two-link planar arm, as found in some industrial robots with an anthro- ⎢ s1 c23 −s1 s23 −c1 s1 (a2 c2 + a3 c23 ) ⎥ 0 0 1 2 pomorphic structure. T 3 (q) = A1 A2 A3 = ⎣ ⎦ (2.66) s23 c23 0 a2 s2 + a3 s23 The link frames have been illustrated in the figure. As for the previous 0 0 0 1 structure, the origin of Frame 0 was chosen at the intersection of z0 with z1 (d1 = 0); further, z1 and z2 are parallel and the choice of axes x1 and x2 where q = [ ϑ1 ϑ2 ϑ3 ]T . Since z3 is aligned with z2 , Frame 3 does not coin- was made as for the two-link planar arm. The DH parameters are specified in cide with a possible end-effector frame as in Fig. 2.13, and a proper constant Table 2.4. transformation would be needed. 2.9 Kinematics of Typical Manipulator Structures 75 76 2 Kinematics
Fig. 2.24. Spherical wrist
2.9.5 Spherical Wrist
Consider a particular type of structure consisting just of the wrist of Fig. 2.24. Joint variables were numbered progressively starting from 4, since such a wrist is typically thought of as mounted on a three-DOF arm of a six-DOF manipulator. It is worth noticing that the wrist is spherical since all revolute axes intersect at a single point. Once z3 , z4 , z5 have been established, and x3 Fig. 2.25. Stanford manipulator has been chosen, there is an indeterminacy on the directions of x4 and x5 . With reference to the frames indicated in Fig. 2.24, the DH parameters are Computation of the direct kinematics function as in (2.50) yields specified in Table 2.5. ⎡ ⎤ c4 c5 c6 − s4 s6 −c4 c5 s6 − s4 c6 c4 s5 c4 s5 d6 3 3 4 5 ⎢ s4 c5 c6 + c4 s6 −s4 c5 s6 + c4 c6 s4 s5 d6 ⎥ s4 s5 Table 2.5. DH parameters for the spherical wrist T 6 (q) = A4 A5 A6 = ⎣ ⎦ −s5 c6 s5 s6 c5c5 d6 Link ai αi di ϑi 0 0 0 1 4 0 −π/2 0 ϑ4 (2.67) 5 0 π/2 0 ϑ5 where q = [ ϑ4 ϑ5 ϑ6 ]T . Notice that, as a consequence of the choice made 6 0 0 d6 ϑ6 for the coordinate frames, the block matrix R36 that can be extracted from T 36 coincides with the rotation matrix of Euler angles (2.18) previously derived, that is, ϑ4 , ϑ5 , ϑ6 constitute the set of ZYZ angles with respect to the reference The homogeneous transformation matrices defined in (2.52) are for the frame O3 –x3 y3 z3 . Moreover, the unit vectors of Frame 6 coincide with the unit single joints: vectors of a possible end-effector frame according to Fig. 2.13.
⎡ ⎤ ⎡ ⎤
c4 0 −s4 0 c5 0 s5 0 2.9.6 Stanford Manipulator
⎢ s 0 c4 0⎥ ⎢ s 0 −c5 0⎥
A34 (ϑ4 ) = ⎣ 4 ⎦ A45 (ϑ5 ) = ⎣ 5 ⎦
0 −1 0 0 0 1 0 0 The so-called Stanford manipulator is composed of a spherical arm and a
0 0 0 1 0 0 0 1 spherical wrist (Fig. 2.25). Since Frame 3 of the spherical arm coincides with
⎡ ⎤ Frame 3 of the spherical wrist, the direct kinematics function can be obtained
c6 −s6 0 0 via simple composition of the transformation matrices (2.65), (2.67) of the
5 ⎢ s6 c6 0 0 ⎥ previous examples, i.e.,
A6 (ϑ6 ) = ⎣ ⎦.
0 0 1 d6 ⎡ ⎤
0 0 0 1
⎢ n0 s0 a0 p0 ⎥
T 06 = T 03 T 36 = ⎣ ⎦.
0 0 0 1
2.9 Kinematics of Typical Manipulator Structures 77 78 2 Kinematics
Carrying out the products yields ⎡ ⎤ c1 s2 d3 − s1 d2 + c1 (c2 c4 s5 + s2 c5 ) − s1 s4 s5 d6 p06 = ⎣ s1 s2 d3 + c1 d2 + s1 (c2 c4 s5 + s2 c5 ) + c1 s4 s5 d6 ⎦ (2.68) c2 d3 + (−s2 c4 s5 + c2 c5 )d6 for the end-effector position, and ⎡ ⎤ c1 c2 (c4 c5 c6 − s4 s6 ) − s2 s5 c6 − s1 (s4 c5 c6 + c4 s6 ) n06 = ⎣ s1 c2 (c4 c5 c6 − s4 s6 ) − s2 s5 c6 + c1 (s4 c5 c6 + c4 s6 ) ⎦ −s2 (c4 c5 c6 − s4 s6 ) − c2 s5 c6 ⎡ ⎤ c1 −c2 (c4 c5 s6 + s4 c6 ) + s2 s5 s6 − s1 (−s4 c5 s6 + c4 c6 ) s06 = ⎣ s1 −c2 (c4 c5 s6 + s4 c6 ) + s2 s5 s6 + c1 (−s4 c5 s6 + c4 c6 ) ⎦ (2.69) s2 (c4 c5 s6 + s4 c6 ) + c2 s5 s6 ⎡ ⎤ c1 (c2 c4 s5 + s2 c5 ) − s1 s4 s5 a06 = ⎣ s1 (c2 c4 s5 + s2 c5 ) + c1 s4 s5 ⎦ Fig. 2.26. Anthropomorphic arm with spherical wrist −s2 c4 s5 + c2 c5 Table 2.6. DH parameters for the anthropomorphic arm with spherical wrist for the end-effector orientation. A comparison of the vector p06 in (2.68) with the vector p03 in (2.65) relative Link ai αi di ϑi to the sole spherical arm reveals the presence of additional contributions due 1 0 π/2 0 ϑ1 to the choice of the origin of the end-effector frame at a distance d6 from 2 a2 0 0 ϑ2 3 0 π/2 0 ϑ3 the origin of Frame 3 along the direction of a06 . In other words, if it were 4 0 −π/2 d4 ϑ4 d6 = 0, the position vector would be the same. This feature is of fundamental 5 0 π/2 0 ϑ5 importance for the solution of the inverse kinematics for this manipulator, as 6 0 0 d6 ϑ6 will be seen later.
2.9.7 Anthropomorphic Arm with Spherical Wrist while the other transformation matrices remain the same. Computation of the direct kinematics function leads to expressing the position and orientation of A comparison between Fig. 2.23 and Fig. 2.24 reveals that the direct kinemat- the end-effector frame as: ics function cannot be obtained by multiplying the transformation matrices ⎡ ⎤ T 03 and T 36 , since Frame 3 of the anthropomorphic arm cannot coincide with a2 c1 c2 + d4 c1 s23 + d6 c1 (c23 c4 s5 + s23 c5 ) + s1 s4 s5 Frame 3 of the spherical wrist. p06 = ⎣ a2 s1 c2 + d4 s1 s23 + d6 s1 (c23 c4 s5 + s23 c5 ) − c1 s4 s5 ⎦ (2.70) Direct kinematics of the entire structure can be obtained in two ways. a2 s2 − d4 c23 + d6 (s23 c4 s5 − c23 c5 ) One consists of interposing a constant transformation matrix between T 03 and T 36 which allows the alignment of the two frames. The other refers to the and Denavit–Hartenberg operating procedure with the frame assignment for the ⎡ ⎤ c1 c23 (c4 c5 c6 − s4 s6 ) − s23 s5 c6 + s1 (s4 c5 c6 + c4 s6 ) entire structure illustrated in Fig. 2.26. The DH parameters are specified in n06 = ⎣ s1 c23 (c4 c5 c6 − s4 s6 ) − s23 s5 c6 − c1 (s4 c5 c6 + c4 s6 ) ⎦ Table 2.6. s23 (c4 c5 c6 − s4 s6 ) + c23 s5 c6 Since Rows 3 and 4 differ from the corresponding rows of the tables for ⎡ ⎤ the two single structures, the relative homogeneous transformation matrices c1 −c23 (c4 c5 s6 + s4 c6 ) + s23 s5 s6 + s1 (−s4 c5 s6 + c4 c6 ) A23 and A34 have to be modified into s06 = ⎣ s1 −c23 (c4 c5 s6 + s4 c6 ) + s23 s5 s6 − c1 (−s4 c5 s6 + c4 c6 ) ⎦ (2.71) ⎡ ⎤ ⎡ ⎤ −s23 (c4 c5 s6 + s4 c6 ) − c23 s5 s6 c3 0 s3 0 c4 0 −s4 0 ⎡ ⎤ ⎢ s 0 −c3 0 ⎥ ⎢s 0 c4 0 ⎥ c1 (c23 c4 s5 + s23 c5 ) + s1 s4 s5 A23 (ϑ3 ) = ⎣ 3 ⎦ A34 (ϑ4 ) = ⎣ 4 ⎦ a06 = ⎣ s1 (c23 c4 s5 + s23 c5 ) − c1 s4 s5 ⎦ . 0 1 0 0 0 −1 0 d4 0 0 0 1 0 0 0 1 s23 c4 s5 − c23 c5 2.9 Kinematics of Typical Manipulator Structures 79 80 2 Kinematics
The generic homogeneous transformation matrix defined in (2.52) is (αi =
π/2)
⎡ ⎤
ci 0 si 0
⎢ si 0 −ci 0 ⎥
Ai−1 =⎢
⎣0 1
⎥ i = 1, . . . , 6 (2.72)
i
0 di ⎦
0 0 0 1
while, since α7 = 0, it is
⎡ ⎤
c7 −s7 0 0
⎢ s7 c7 0 0 ⎥
A7 = ⎢
6
⎣0
⎥. (2.73)
0 1 d7 ⎦
0 0 0 1
The direct kinematics function, computed as in (2.50), leads to the following
Fig. 2.27. DLR manipulator
expressions for the end-effector frame
⎡ ⎤
By setting d6 = 0, the position of the wrist axes intersection is obtained. In d3 xd3 + d5 xd5 + d7 xd7 that case, the vector p0 in (2.70) corresponds to the vector p03 for the sole p07 = ⎣ d3 yd3 + d5 yd5 + d7 yd7 ⎦ (2.74) anthropomorphic arm in (2.66), because d4 gives the length of the forearm d3 zd3 + d5 zd5 + d7 zd7 (a3 ) and axis x3 in Fig. 2.26 is rotated by π/2 with respect to axis x3 in with Fig. 2.23. xd3 = c1 s2 2.9.8 DLR Manipulator xd5 = c1 (c2 c3 s4 − s2 c4 ) + s1 s3 s4 xd7 = c1 (c2 k1 + s2 k2 ) + s1 k3 Consider the DLR manipulator, whose development is at the basis of the real- ization of the robot in Fig. 1.30; it is characterized by seven DOFs and as such yd3 = s1 s2 it is inherently redundant. This manipulator has two possible configurations yd5 = s1 (c2 c3 s4 − s2 c4 ) − c1 s3 s4 for the outer three joints (wrist). With reference to a spherical wrist similar to yd7 = s1 (c2 k1 + s2 k2 ) − c1 k3 that introduced in Sect. 2.9.5, the resulting kinematic structure is illustrated zd3 = −c2 in Fig. 2.27, where the frames attached to the links are evidenced. As in the case of the spherical arm, notice that the origin of Frame 0 has zd5 = c2 c4 + s2 c3 s4 been chosen so as to zero d1 . The DH parameters are specified in Table 2.7. zd7 = s2 (c3 (c4 c5 s6 − s4 c6 ) + s3 s5 s6 ) − c2 k2 ,
where
Table 2.7. DH parameters for the DLR manipulator
Link ai αi di ϑi k1 = c3 (c4 c5 s6 − s4 c6 ) + s3 s5 s6 1 0 π/2 0 ϑ1 k2 = s4 c5 s6 + c4 c6 2 0 π/2 0 ϑ2 k3 = s3 (c4 c5 s6 − s4 c6 ) − c3 s5 s6 . 3 0 π/2 d3 ϑ3 4 0 π/2 0 ϑ4 Furthermore, the end-effector frame orientation can be derived as 5 0 π/2 d5 ϑ5 ⎡ ⎤ 6 0 π/2 0 ϑ6 ((xa c5 + xc s5 )c6 + xb s6 )c7 + (xa s5 − xc c5 )s7 7 0 0 d7 ϑ7 n7 = ⎣ ((ya c5 + yc s5 )c6 + yb s6 )c7 + (ya s5 − yc c5 )s7 ⎦ 0
(za c6 + zc s6 )c7 + zb s7
2.9 Kinematics of Typical Manipulator Structures 81 82 2 Kinematics ⎡ ⎤ −((xa c5 + xc s5 )c6 + xb s6 )s7 + (xa s5 − xc c5 )c7 s07 = ⎣ −((ya c5 + yc s5 )c6 + yb s6 )s7 + (ya s5 − yc c5 )c7 ⎦ (2.75) −(za c6 + zc s6 )s7 + zb c7 ⎡ ⎤ (xa c5 + xc s5 )s6 − xb c6 a7 = ⎣ (ya c5 + yc s5 )s6 − yb c6 ⎦ , 0
za s6 − zc c6
where
xa = (c1 c2 c3 + s1 s3 )c4 + c1 s2 s4
xb = (c1 c2 c3 + s1 s3 )s4 − c1 s2 c4
xc = c1 c2 s3 − s1 c3
ya = (s1 c2 c3 − c1 s3 )c4 + s1 s2 s4
yb = (s1 c2 c3 − c1 s3 )s4 − s1 s2 c4
yc = s1 c2 s3 + c1 c3
za = (s2 c3 c4 − c2 s4 )c5 + s2 s3 s5
zb = (s2 c3 s4 + c2 c4 )s5 − s2 s3 c5
zc = s2 c3 s4 + c2 c4 . Fig. 2.28. Humanoid manipulator
(2.76)
As in the case of the anthropomorphic arm with spherical wrist, it occurs that of the human body upper part: torso, arms, end-effectors similar to hu-
that Frame 4 cannot coincide with the base frame of the wrist. man hands and a ‘head’ which, eventually, includes an artificial vision system Finally, consider the possibility to mount a different type of spherical wrist, — see Chap. 10. where Joint 7 is so that α7 = π/2. In such a case, the computation of the For the humanoid manipulator in Fig. 1.33, it is worth noticing the pres- direct kinematics function changes, since the seventh row of the kinematic ence of two end-effectors (where the ‘hands’ are mounted), while the arms parameters table changes. In particular, notice that, since d7 = 0, a7 = 0, consist of two DLR manipulators, introduced in the previous section, each then ⎡ ⎤ with seven DOFs. In particular, consider the configuration where the last c7 0 s7 a7 c7 joint is so that α7 = π/2. ⎢ s7 0 −c7 a7 s7 ⎥ To simplify, the kinematic structure allowing the articulation of the robot’s A67 = ⎢ ⎣0 0 ⎥. (2.77) 1 0 ⎦ head in Fig. 1.33. The torso can be modelled as an anthropomorphic arm 0 0 0 1 (three DOFs), for a total of seventeen DOFs. Further, a connecting device exists between the end-effector of the anthro- It follows, however, that Frame 7 does not coincide with the end-effector pomorphic torso and the base frames of the two manipulators. Such device frame, as already discussed for the three-link planar arm, since the approach permits keeping the ‘chest’ of the humanoid manipulator always orthogonal to unit vector a07 is aligned with x7 . the ground. With reference to Fig. 2.28, this device is represented by a further joint, located at the end of the torso. Hence, the corresponding parameter ϑ4 2.9.9 Humanoid Manipulator does not constitute a DOF, yet it varies so as to compensate Joints 2 and 3 rotations of the anthropomorphic torso. The term humanoid refers to a robot showing a kinematic structure similar to To compute the direct kinematics function, it is possible to resort to a DH that of the human body. It is commonly thought that the most relevant fea- parameters table for each of the two tree kinematic structures, which can be ture of humanoid robots is biped locomotion. However, in detail, a humanoid identified from the base of the manipulator to each of the two end-effectors. manipulator refers to an articulated structure with a kinematics analogous to Similarly to the case of mounting a spherical wrist onto an anthropomorphic arm, this implies the change of some rows of the transformation matrices of 2.10 Joint Space and Operational Space 83 84 2 Kinematics
those manipulators, described in the previous sections, constituting the torso The problem of describing end-effector orientation admits a natural so- and the arms. lution if one of the above minimal representations is adopted. In this case, Alternatively, it is possible to consider intermediate transformation matri- indeed, a motion trajectory can be assigned to the set of angles chosen to ces between the relevant structures. In detail, as illustrated in Fig. 2.28, if t represent orientation. denotes the frame attached to the torso, r and l the base frames, respectively, Therefore, the position can be given by a minimal number of coordinates of the right arm and the left arm, and rh and lh the frames attached to the with regard to the geometry of the structure, and the orientation can be two hands (end-effectors), it is possible to compute for the right arm and the specified in terms of a minimal representation (Euler angles) describing the left arm, respectively: rotation of the end-effector frame with respect to the base frame. In this way, it is possible to describe the end-effector pose by means of the (m × 1) vector, T 0rh = T 03 T 3t T tr T rrh (2.78) with m ≤ n, T 0lh = T 03 T 3t T tl T llh (2.79) xe = pe (2.80) φe where the matrix T 3t describes the transformation imposed by the motion of where pe describes the end-effector position and φe its orientation. Joint 4 (dashed line in Fig. 2.28), located at the end-effector of the torso. This representation of position and orientation allows the description of an Frame 4 coincides with Frame t in Fig. 2.27. In view of the property of pa- end-effector task in terms of a number of inherently independent parameters. rameter ϑ4 , it is ϑ4 = −ϑ2 − ϑ3 , and thus The vector xe is defined in the space in which the manipulator task is specified; ⎡ ⎤ hence, this space is typically called operational space. On the other hand, the c23 s23 0 0 3 ⎢ −s23 c23 0 0⎥ joint space (configuration space) denotes the space in which the (n × 1) vector Tt = ⎣ ⎦. of joint variables 0 0 1 0 ⎡ ⎤ 0 0 0 1 q1 ⎢ .. ⎥ q = ⎣ . ⎦, (2.81) The matrix T 03 is given by (2.66), whereas the matrices T tr and T tl relating qn the torso end-effector frame to the base frames of the two manipulators have constant values. With reference to Fig. 2.28, the elements of these matrices is defined; it is qi = ϑi for a revolute joint and qi = di for a prismatic depend on the angle β and on the distances between the origin of Frame t joint. Accounting for the dependence of position and orientation from the and the origins of Frames r and l. Finally, the expressions of the matrices T rrh joint variables, the direct kinematics equation can be written in a form other and T llh must be computed by considering the change in the seventh row of than (2.50), i.e., the DH parameters table of the DLR manipulator, so as to account for the xe = k(q). (2.82) different kinematic structure of the wrist (see Problem 2.14). The (m × 1) vector function k(·) — nonlinear in general — allows computa- tion of the operational space variables from the knowledge of the joint space variables. 2.10 Joint Space and Operational Space It is worth noticing that the dependence of the orientation components of the function k(q) in (2.82) on the joint variables is not easy to express As described in the previous sections, the direct kinematics equation of a except for simple cases. In fact, in the most general case of a six-dimensional manipulator allows the position and orientation of the end-effector frame to operational space (m = 6), the computation of the three components of the be expressed as a function of the joint variables with respect to the base frame. function φe (q) cannot be performed in closed form but goes through the If a task is to be specified for the end-effector, it is necessary to assign the computation of the elements of the rotation matrix, i.e., ne (q), se (q), ae (q). end-effector position and orientation, eventually as a function of time (tra- The equations that allow the determination of the Euler angles from the triplet jectory). This is quite easy for the position. On the other hand, specifying of unit vectors ne , se , ae were given in Sect. 2.4. the orientation through the unit vector triplet (ne , se , ae )11 is quite difficult, since their nine components must be guaranteed to satisfy the orthonormal- ity constraints imposed by (2.4) at each time instant. This problem will be resumed in Chap. 4. 11 To simplify, the indication of the reference frame in the superscript is omitted. 2.10 Joint Space and Operational Space 85 86 2 Kinematics
Example 2.5 Consider again the three-link planar arm in Fig. 2.20. The geometry of the structure suggests that the end-effector position is determined by the two coordinates px and py , while its orientation is determined by the angle φ formed by the end-effector with the axis x0 . Expressing these operational variables as a function of the joint variables, the two position coordinates are given by the first two elements of the fourth column of the homogeneous transformation matrix (2.63), while the orientation angle is simply given by the sum of joint variables. In sum, the direct kinematics equation can be written in the form px a1 c1 + a2 c12 + a3 c123 xe = py = k(q) = a1 s1 + a2 s12 + a3 s123 . (2.83) φ ϑ1 + ϑ2 + ϑ3 This expression shows that three joint space variables allow specification of at most three independent operational space variables. On the other hand, if orientation is of no concern, it is xe = [ px py ]T and there is kinematic redundancy of DOFs Fig. 2.29. Region of admissible configurations for a two-link arm with respect to a pure positioning end-effector task; this concept will be dealt with in detail afterwards. (without end-effector) is reported in the data sheet given by the robot manu- facturer in terms of a top view and a side view. It represents a basic element to evaluate robot performance for a desired application.
2.10.1 Workspace
With reference to the operational space, an index of robot performance is Example 2.6 the so-called workspace; this is the region described by the origin of the end- effector frame when all the manipulator joints execute all possible motions. It Consider the simple two-link planar arm. If the mechanical joint limits are known, the arm can attain all the joint space configurations corresponding to the points in is often customary to distinguish between reachable workspace and dexterous the rectangle in Fig. 2.29. workspace. The latter is the region that the origin of the end-effector frame The reachable workspace can be derived via a graphical construction of the can describe while attaining different orientations, while the former is the image of the rectangle perimeter in the plane of the arm. To this purpose, it is region that the origin of the end-effector frame can reach with at least one worth considering the images of the segments ab, bc, cd, da, ae, ef , f d. Along the orientation. Obviously, the dexterous workspace is a subspace of the reachable segments ab, bc, cd, ae, ef , f d a loss of mobility occurs due to a joint limit; a workspace. A manipulator with less than six DOFs cannot take any arbitrary loss of mobility occurs also along the segment ad because the arm and forearm are position and orientation in space. aligned.12 Further, a change of the arm posture occurs at points a and d: for q2 > 0 The workspace is characterized by the manipulator geometry and the me- the elbow-down posture is obtained, while for q2 < 0 the arm is in the elbow-up chanical joint limits. For an n-DOF manipulator, the reachable workspace is posture. the geometric locus of the points that can be achieved by considering the In the plane of the arm, start drawing the arm in configuration A corresponding direct kinematics equation for the sole position part, i.e., to q1m and q2 = 0 (a); then, the segment ab describing motion from q2 = 0 to q2M generates the arc AB; the subsequent arcs BC, CD, DA, AE, EF , F D are pe = pe (q) qim ≤ qi ≤ qiM i = 1, … , n, generated in a similar way (Fig. 2.30). The external contour of the area CDAEF HC delimits the requested workspace. Further, the area BCDAB is relative to elbow- where qim (qiM ) denotes the minimum (maximum) limit at Joint i. This vol- down postures while the area DAEF D is relative to elbow-up postures; hence, the points in the area BADHB are reachable by the end-effector with both postures. ume is finite, closed, connected — pe (q) is a continuous function — and thus is defined by its bordering surface. Since the joints are revolute or prismatic, it is easy to recognize that this surface is constituted by surface elements of 12 In the following chapter, it will be seen that this configuration characterizes a planar, spherical, toroidal and cylindrical type. The manipulator workspace kinematic singularity of the arm. 2.10 Joint Space and Operational Space 87 88 2 Kinematics
describe a given task. With reference to the above-defined spaces, a manipu-
lator is intrinsically redundant when the dimension of the operational space is
smaller than the dimension of the joint space (m < n). Redundancy is, any-
how, a concept relative to the task assigned to the manipulator; a manipulator
can be redundant with respect to a task and nonredundant with respect to
another. Even in the case of m = n, a manipulator can be functionally redun-
dant when only a number of r components of operational space are of concern
for the specific task, with r < m.
Consider again the three-DOF planar arm of Sect. 2.9.1. If only the end-
effector position (in the plane) is specified, that structure presents a functional
redundancy (n = m = 3, r = 2); this is lost when also the end-effector
orientation in the plane is specified (n = m = r = 3). On the other hand, a
four-DOF planar arm is intrinsically redundant (n = 4, m = 3).
Yet, take the typical industrial robot with six DOFs; such manipulator
is not intrinsically redundant (n = m = 6), but it can become functionally
Fig. 2.30. Workspace of a two-link planar arm redundant with regard to the task to execute. Thus, for instance, in a laser-
cutting task a functional redundancy will occur since the end-effector rotation
In a real manipulator, for a given set of joint variables, the actual val- about the approach direction is irrelevant to completion of the task (r = 5).
ues of the operational space variables deviate from those computed via direct At this point, a question should arise spontaneously: Why to intentionally kinematics. The direct kinematics equation has indeed a dependence from the utilize a redundant manipulator? The answer is to recognize that redundancy DH parameters which is not explicit in (2.82). If the mechanical dimensions can provide the manipulator with dexterity and versatility in its motion. The of the structure differ from the corresponding parameter of the table because typical example is constituted by the human arm that has seven DOFs: three of mechanical tolerances, a deviation arises between the position reached in in the shoulder, one in the elbow and three in the wrist, without considering the assigned posture and the position computed via direct kinematics. Such a the DOFs in the fingers. This manipulator is intrinsically redundant; in fact, deviation is defined accuracy; this parameter attains typical values below one if the base and the hand position and orientation are both fixed — requiring millimeter and depends on the structure as well as on manipulator dimen- six DOFs — the elbow can be moved, thanks to the additional available DOF. sions. Accuracy varies with the end-effector position in the workspace and it Then, for instance, it is possible to avoid obstacles in the workspace. Further, is a relevant parameter when robot programming oriented environments are if a joint of a redundant manipulator reaches its mechanical limit, there might adopted, as will be seen in the last chapter. be other joints that allow execution of the prescribed end-effector motion. Another parameter that is usually listed in the performance data sheet of A formal treatment of redundancy will be presented in the following chap- an industrial robot is repeatability which gives a measure of the manipulator’s ter. ability to return to a previously reached position; this parameter is relevant for programming an industrial robot by the teaching–by–showing technique which will be presented in Chap. 6. Repeatability depends not only on the 2.11 Kinematic Calibration characteristics of the mechanical structure but also on the transducers and controller; it is expressed in metric units and is typically smaller than accuracy. The Denavit–Hartenberg parameters for direct kinematics need to be com- For instance, for a manipulator with a maximum reach of 1.5 m, accuracy puted as precisely as possible in order to improve manipulator accuracy. Kine- varies from 0.2 to 1 mm in the workspace, while repeatability varies from 0.02 matic calibration techniques are devoted to finding accurate estimates of DH to 0.2 mm. parameters from a series of measurements on the manipulator’s end-effector pose. Hence, they do not allow direct measurement of the geometric parame- ters of the structure. 2.10.2 Kinematic Redundancy Consider the direct kinematics equation in (2.82) which can be rewritten by emphasizing the dependence of the operational space variables on the fixed A manipulator is termed kinematically redundant when it has a number of DH parameters, besides the joint variables. Let a = [ a1 … an ]T , α = DOFs which is greater than the number of variables that are necessary to 2.11 Kinematic Calibration 89 90 2 Kinematics
[ α1 … αn ]T , d = [ d1 … dn ]T , and ϑ = [ θ1 … θn ]T denote the As regards the nominal values of the parameters needed for the computation vectors of DH parameters for the whole structure; then (2.82) becomes of the matrices Φi , it should be observed that the geometric parameters are constant whereas the joint variables depend on the manipulator configuration xe = k(a, α, d, ϑ). (2.84) at pose i. The manipulator’s end-effector pose should be measured with high precision In order to avoid ill-conditioning of matrix Φ̄, it is advisable to choose l for the effectiveness of the kinematic calibration procedure. To this purpose so that lm 4n and then solve (2.87) with a least-squares technique; in this a mechanical apparatus can be used that allows the end-effector to be con- case the solution is of the form strained at given poses with a priori known precision. Alternatively, direct Δζ = (Φ̄ Φ̄)−1 Φ̄ Δx̄ T T (2.88) measurement systems of object position and orientation in the Cartesian space T T can be used which employ triangulation techniques. where (Φ̄ Φ̄)−1 Φ̄ is the left pseudo-inverse matrix of Φ̄.14 By computing Φ̄ Let xm be the measured pose and xn the nominal pose that can be com- with the nominal values of the parameters ζ n , the first parameter estimate is puted via (2.84) with the nominal values of the parameters a, α, d, ϑ. The given by nominal values of the fixed parameters are set equal to the design data of the ζ = ζ n + Δζ. (2.89) mechanical structure, whereas the nominal values of the joint variables are set equal to the data provided by the position transducers at the given manipula- This is a nonlinear parameter estimate problem and, as such, the procedure tor posture. The deviation Δx = xm − xn gives a measure of accuracy at the should be iterated until Δζ converges within a given threshold. At each itera- given posture. On the assumption of small deviations, at first approximation, tion, the calibration matrix Φ̄ is to be updated with the parameter estimates it is possible to derive the following relation from (2.84): ζ obtained via (2.89) at the previous iteration. In a similar manner, the de- viation Δx̄ is to be computed as the difference between the measured values ∂k ∂k ∂k ∂k for the l end-effector poses and the corresponding poses computed by the di- Δx = Δa + Δα + Δd + Δϑ (2.85) ∂a ∂α ∂d ∂ϑ rect kinematics function with the values of the parameters at the previous where Δa, Δα, Δd, Δϑ denote the deviations between the values of the iteration. As a result of the kinematic calibration procedure, more accurate parameters of the real structure and the nominal ones. Moreover, ∂k/∂a, estimates of the real manipulator geometric parameters as well as possible ∂k/∂α, ∂k/∂d, ∂k/∂ϑ denote the (m × n) matrices whose elements are the corrections to make on the joint transducers measurements are obtained. partial derivatives of the components of the direct kinematics function with Kinematic calibration is an operation that is performed by the robot manu- respect to the single parameters.13 facturer to guarantee the accuracy reported in the data sheet. There is another Group the parameters in the (4n × 1) vector ζ = [ aT αT dT ϑT ]T . kind of calibration that is performed by the robot user which is needed for the Let Δζ = ζ m −ζ n denote the parameter variations with respect to the nominal measurement system start-up to guarantee that the position transducers data values, and Φ = [ ∂k/∂a ∂k/∂α ∂k/∂d ∂k/∂ϑ ] the (m × 4n) kinematic are consistent with the attained manipulator posture. For instance, in the calibration matrix computed for the nominal values of the parameters ζ n . case of incremental (nonabsolute) position transducers, such calibration con- Then (2.85) can be compactly rewritten as sists of taking the mechanical structure into a given reference posture (home) and initializing the position transducers with the values at that posture. Δx = Φ(ζ n )Δζ. (2.86) It is desired to compute Δζ starting from the knowledge of ζ n , xn and the 2.12 Inverse Kinematics Problem measurement of xm . Since (2.86) constitutes a system of m equations into 4n unknowns with m < 4n, a sufficient number of end-effector pose measure- The direct kinematics equation, either in the form (2.50) or in the form (2.82), ments has to be performed so as to obtain a system of at least 4n equations. establishes the functional relationship between the joint variables and the end- Therefore, if measurements are made for a number of l poses, (2.86) yields effector position and orientation. The inverse kinematics problem consists of ⎡ ⎤ ⎡ ⎤ the determination of the joint variables corresponding to a given end-effector Δx1 Φ1 . . position and orientation. The solution to this problem is of fundamental im- Δx̄ = ⎣ .. ⎦ = ⎣ .. ⎦ Δζ = Φ̄Δζ. (2.87) portance in order to transform the motion specifications, assigned to the end- Δxl Φl effector in the operational space, into the corresponding joint space motions 13 These matrices are the Jacobians of the transformations between the parameter that allow execution of the desired motion. 14 space and the operational space. See Sect. A.7 for the definition of the pseudo-inverse of a matrix. 2.12 Inverse Kinematics Problem 91 92 2 Kinematics
As regards the direct kinematics equation in (2.50), the end-effector po- As already pointed out, it is convenient to specify position and orientation
sition and rotation matrix are computed in a unique manner, once the joint in terms of a minimal number of parameters: the two coordinates px , py and variables are known15 . On the other hand, the inverse kinematics problem is the angle φ with axis x0 , in this case. Hence, it is possible to refer to the direct much more complex for the following reasons: kinematics equation in the form (2.83). A first algebraic solution technique is illustrated below. Having specified • The equations to solve are in general nonlinear, and thus it is not always the orientation, the relation possible to find a closed-form solution. • Multiple solutions may exist. φ = ϑ1 + ϑ 2 + ϑ 3 (2.90) • Infinite solutions may exist, e.g., in the case of a kinematically redundant manipulator. is one of the equations of the system to solve16 . From (2.63) the following • There might be no admissible solutions, in view of the manipulator kine- equations can be obtained: matic structure. pW x = px − a3 cφ = a1 c1 + a2 c12 (2.91) The existence of solutions is guaranteed only if the given end-effector position pW y = py − a3 sφ = a1 s1 + a2 s12 (2.92) and orientation belong to the manipulator dexterous workspace. On the other hand, the problem of multiple solutions depends not only on which describe the position of point W , i.e., the origin of Frame 2; this depends the number of DOFs but also on the number of non-null DH parameters; in only on the first two angles ϑ1 and ϑ2 . Squaring and summing (2.91), (2.92) general, the greater the number of non-null parameters, the greater the num- yields ber of admissible solutions. For a six-DOF manipulator without mechanical p2W x + p2W y = a21 + a22 + 2a1 a2 c2 joint limits, there are in general up to 16 admissible solutions. Such occur- from which rence demands some criterion to choose among admissible solutions (e.g., the p2W x + p2W y − a21 − a22 elbow-up/elbow-down case of Example 2.6). The existence of mechanical joint c2 = . 2a1 a2 limits may eventually reduce the number of admissible multiple solutions for The existence of a solution obviously imposes that −1 ≤ c2 ≤ 1, otherwise the real structure. the given point would be outside the arm reachable workspace. Then, set Computation of closed-form solutions requires either algebraic intuition to find those significant equations containing the unknowns or geometric intu- s2 = ± 1 − c22 , ition to find those significant points on the structure with respect to which it is convenient to express position and/or orientation as a function of a re- where the positive sign is relative to the elbow-down posture and the negative duced number of unknowns. The following examples will point out the ability sign to the elbow-up posture. Hence, the angle ϑ2 can be computed as required to an inverse kinematics problem solver. On the other hand, in all those cases when there are no — or it is difficult to find — closed-form so- ϑ2 = Atan2(s2 , c2 ). lutions, it might be appropriate to resort to numerical solution techniques; these clearly have the advantage of being applicable to any kinematic struc- Having determined ϑ2 , the angle ϑ1 can be found as follows. Substituting ture, but in general they do not allow computation of all admissible solutions. ϑ2 into (2.91), (2.92) yields an algebraic system of two equations in the two In the following chapter, it will be shown how suitable algorithms utilizing unknowns s1 and c1 , whose solution is the manipulator Jacobian can be employed to solve the inverse kinematics (a1 + a2 c2 )pW y − a2 s2 pW x problem. s1 = p2W x + p2W y (a1 + a2 c2 )pW x + a2 s2 pW y 2.12.1 Solution of Three-link Planar Arm c1 = . p2W x + p2W y Consider the arm shown in Fig. 2.20 whose direct kinematics was given In analogy to the above, it is in (2.63). It is desired to find the joint variables ϑ1 , ϑ2 , ϑ3 corresponding to a given end-effector position and orientation. ϑ1 = Atan2(s1 , c1 ). 15 16 In general, this cannot be said for (2.82) too, since the Euler angles are not If φ is not specified, then the arm is redundant and there exist infinite solutions uniquely defined. to the inverse kinematics problem. 2.12 Inverse Kinematics Problem 93 94 2 Kinematics
To compute β, applying again the cosine theorem yields
cβ p2W x + p2W y = a1 + a2 c2
and resorting to the expression of c2 given above leads to
⎛ ⎞
p2 + p2W y + a21 − a22
−1 ⎝ W x
β = cos ⎠
2a1 p2W x + p2W y
with β ∈ (0, π) so as to preserve the existence of triangles. Then, it is
Fig. 2.31. Admissible postures for a two-link planar arm ϑ1 = α ± β,
In the case when s2 = 0, it is obviously ϑ2 = 0, π; as will be shown in the where the positive sign holds for ϑ2 < 0 and the negative sign for ϑ2 > 0. following, in such a posture the manipulator is at a kinematic singularity. Yet, Finally, ϑ3 is computed from (2.90). the angle ϑ1 can be determined uniquely, unless a1 = a2 and it is required It is worth noticing that, in view of the substantial equivalence between pW x = pW y = 0. the two-link planar arm and the parallelogram arm, the above techniques can Finally, the angle ϑ3 is found from (2.90) as be formally applied to solve the inverse kinematics of the arm in Sect. 2.9.2.
ϑ3 = φ − ϑ1 − ϑ 2 . 2.12.2 Solution of Manipulators with Spherical Wrist
An alternative geometric solution technique is presented below. As above,
Most of the existing manipulators are kinematically simple, since they are
the orientation angle is given as in (2.90) and the coordinates of the origin typically formed by an arm, of the kind presented above, and a spherical wrist; of Frame 2 are computed as in (2.91), (2.92). The application of the cosine see the manipulators in Sects. 2.9.6–2.9.8. This choice is partly motivated by theorem to the triangle formed by links a1 , a2 and the segment connecting the difficulty to find solutions to the inverse kinematics problem in the general points W and O gives case. In particular, a six -DOF kinematic structure has closed-form inverse p2W x + p2W y = a21 + a22 − 2a1 a2 cos (π − ϑ2 ); kinematics solutions if:
the two admissible configurations of the triangle are shown in Fig. 2.31. Ob- • three consecutive revolute joint axes intersect at a common point, like for serving that cos (π − ϑ2 ) = −cos ϑ2 leads to the spherical wrist; • three consecutive revolute joint axes are parallel. p2W x + p2W y − a21 − a22 c2 = . In any case, algebraic or geometric intuition is required to obtain closed-form 2a1 a2 solutions. For the existence of the triangle, it must be p2W x + p2W y ≤ a1 + a2 . This Inspired by the previous solution to a three-link planar arm, a suitable point along the structure can be found whose position can be expressed both as condition is not satisfied when the given point is outside the arm reachable a function of the given end-effector position and orientation and as a function workspace. Then, under the assumption of admissible solutions, it is of a reduced number of joint variables. This is equivalent to articulating the ϑ2 = ±cos −1 (c2 ); inverse kinematics problem into two subproblems, since the solution for the position is decoupled from that for the orientation. the elbow-up posture is obtained for ϑ2 ∈ (−π, 0) while the elbow-down pos- For a manipulator with spherical wrist, the natural choice is to locate such ture is obtained for ϑ2 ∈ (0, π). point W at the intersection of the three terminal revolute axes (Fig. 2.32). In To find ϑ1 consider the angles α and β in Fig. 2.31. Notice that the deter- fact, once the end-effector position and orientation are specified in terms of mination of α depends on the sign of pW x and pW y ; then, it is necessary to pe and Re = [ ne se ae ], the wrist position can be found as compute α as α = Atan2(pW y , pW x ). pW = pe − d6 ae (2.93) 2.12 Inverse Kinematics Problem 95 96 2 Kinematics
Equating the first three elements of the fourth columns of the matrices on
both sides yields
⎡ ⎤ ⎡ ⎤
pW x c1 + pW y s1 d3 s2
p1W = ⎣ −pW z ⎦ = ⎣ −d3 c2 ⎦ (2.94)
−pW x s1 + pW y c1 d2
which depends only on ϑ2 and d3 . To solve this equation, set
ϑ1
t = tan
2
so that
1 − t2 2t
c1 = s1 = .
1 + t2 1 + t2
Fig. 2.32. Manipulator with spherical wrist Substituting this equation in the third component on the left-hand side
of (2.94) gives
which is a function of the sole joint variables that determine the arm posi- (d2 + pW y )t2 + 2pW x t + d2 − pW y = 0, tion17 . Hence, in the case of a (nonredundant) three-DOF arm, the inverse whose solution is kinematics can be solved according to the following steps: −pW x ± p2W x + p2W y − d22 t= . • Compute the wrist position pW (q1 , q2 , q3 ) as in (2.93). d 2 + pW y • Solve inverse kinematics for (q1 , q2 , q3 ). The two solutions correspond to two different postures. Hence, it is • Compute R03 (q1 , q2 , q3 ). • Compute R36 (ϑ4 , ϑ5 , ϑ6 ) = R03 T R. ϑ1 = 2Atan2 −pW x ± p2W x + p2W y − d22 , d2 + pW y . • Solve inverse kinematics for orientation (ϑ4 , ϑ5 , ϑ6 ).
Therefore, on the basis of this kinematic decoupling, it is possible to solve Once ϑ1 is known, squaring and summing the first two components of (2.94) the inverse kinematics for the arm separately from the inverse kinematics yields for the spherical wrist. Below are presented the solutions for two typical arms d3 = (pW x c1 + pW y s1 )2 + p2W z , (spherical and anthropomorphic) as well as the solution for the spherical wrist. where only the solution with d3 ≥ 0 has been considered. Note that the same value of d3 corresponds to both solutions for ϑ1 . Finally, if d3 = 0, from the 2.12.3 Solution of Spherical Arm first two components of (2.94) it is Consider the spherical arm shown in Fig. 2.22, whose direct kinematics was pW x c1 + pW y s1 d3 s2 given in (2.65). It is desired to find the joint variables ϑ1 , ϑ2 , d3 corresponding = , −pW z −d3 c2 to a given end-effector position pW . In order to separate the variables on which pW depends, it is convenient to from which express the position of pW with respect to Frame 1; then, consider the matrix ϑ2 = Atan2(pW x c1 + pW y s1 , pW z ). equation Notice that, if d3 = 0, then ϑ2 cannot be uniquely determined. (A01 )−1 T 03 = A12 A23 .
2.12.4 Solution of Anthropomorphic Arm
17 Consider the anthropomorphic arm shown in Fig. 2.23. It is desired to find Note that the same reasoning was implicitly adopted in Sect. 2.12.1 for the three- link planar arm; pW described the one-DOF wrist position for the two-DOF arm the joint variables ϑ1 , ϑ2 , ϑ3 corresponding to a given end-effector position obtained by considering only the first two links. pW . Notice that the direct kinematics for pW is expressed by (2.66) which can 2.12 Inverse Kinematics Problem 97 98 2 Kinematics
be obtained from (2.70) by setting d6 = 0, d4 = a3 and replacing ϑ3 with the From (2.103), (2.104) it follows angle ϑ3 + π/2 because of the misalignment of the Frames 3 for the structures in Fig. 2.23 and in Fig. 2.26, respectively. Hence, it follows ϑ2 = Atan2(s2 , c2 )
pW x = c1 (a2 c2 + a3 c23 ) (2.95) which gives the four solutions for ϑ2 , according to the sign of s3 in (2.99):
pW y = s1 (a2 c2 + a3 c23 ) (2.96)
ϑ2,I = Atan2 (a2 + a3 c3 )pW z − a3 s+3 p2W x + p2W y ,
pW z = a2 s2 + a3 s23 . (2.97)
(a2 + a3 c3 ) p2W x + p2W y + a3 s+
3 pW z (2.105)
Proceeding as in the case of the two-link planar arm, it is worth squaring and summing (2.95)–(2.97) yielding ϑ2,II = Atan2 (a2 + a3 c3 )pW z + a3 s+3 p2W x + p2W y , p2W x + p2W y + p2W z = a22 + a23 + 2a2 a3 c3 −(a2 + a3 c3 ) p2W x + p2W y + a3 s+3 pW z (2.106) from which p2W x + p2W y + p2W z − a22 − a23 corresponding to s+ 3 = 1 − c23 , and c3 = (2.98) 2a2 a3 where the admissibility of the solution obviously requires that −1 ≤ c3 ≤ 1, ϑ2,III = Atan2 (a2 + a3 c3 )pW z − a3 s− 3 p2W x + p2W y , or equivalently |a2 − a3 | ≤ p2W x + p2W y + p2W z ≤ a2 + a3 , otherwise the wrist (a2 + a3 c3 ) p2W x + p2W y + a3 s− 3 pW z (2.107) point is outside the reachable workspace of the manipulator. Hence it is ϑ2,IV = Atan2 (a2 + a3 c3 )pW z + a3 s−3 p2W x + p2W y , s3 = ± 1 − c23 (2.99) −(a2 + a3 c3 ) p2W x + p2W y + a3 s−3 pW z (2.108) and thus ϑ3 = Atan2(s3 , c3 ) corresponding to s− 2 3 = − 1 − c3 . Finally, to compute ϑ1 , it is sufficient to rewrite (2.95), (2.96), using giving the two solutions, according to the sign of s3 , (2.102), as ϑ3,I ∈ [−π, π] (2.100) ϑ3,II = −ϑ3,I . (2.101) pW x = ±c1 p2W x + p2W y Having determined ϑ3 , it is possible to compute ϑ2 as follows. Squaring pW y = ±s1 p2W x + p2W y and summing (2.95), (2.96) gives which, once solved, gives the two solutions: p2W x + p2W y = (a2 c2 + a3 c23 )2 ϑ1,I = Atan2(pW y , pW x ) (2.109) from which ϑ1,II = Atan2(−pW y , −pW x ). (2.110) a2 c2 + a3 c23 = ± p2W x + p2W y . (2.102) Notice that (2.110) gives18 The system of the two Eqs. (2.102), (2.97), for each of the solutions (2.100), (2.101), admits the solutions: Atan2(pW y , pW x ) − π pW y ≥ 0 ϑ1,II = Atan2(pW y , pW x ) + π pW y < 0. ± p2W x + p2W y (a2 + a3 c3 ) + pW z a3 s3 18 c2 = (2.103) It is easy to show that Atan2(−y, −x) = −Atan2(y, −x) and a22 + a23 + 2a2 a3 c3 π − Atan2(y, x) y≥0 pW z (a2 + a3 c3 ) ∓ p2W x + p2W y a3 s3 Atan2(y, −x) = −π − Atan2(y, x) y < 0. s2 = . (2.104) a22 + a23 + 2a2 a3 c3 2.12 Inverse Kinematics Problem 99 100 2 Kinematics
from its expression in terms of the joint variables in (2.67), it is possible to
compute the solutions directly as in (2.19), (2.20), i.e.,
ϑ4 = Atan2(a3y , a3x )
ϑ5 = Atan2 (a3x )2 + (a3y )2 , a3z (2.111)
ϑ6 = Atan2(s3z , −n3z )
for ϑ5 ∈ (0, π), and
ϑ4 = Atan2(−a3y , −a3x )
ϑ5 = Atan2 − (a3x )2 + (a3y )2 , a3z (2.112)
Fig. 2.33. The four configurations of an anthropomorphic arm compatible with a ϑ6 = Atan2(−s3z , n3z ) given wrist position for ϑ5 ∈ (−π, 0). As can be recognized, there exist four solutions according to the values of ϑ3 in (2.100), (2.101), ϑ2 in (2.105)–(2.108) and ϑ1 in (2.109), (2.110): Bibliography (ϑ1,I , ϑ2,I , ϑ3,I ) (ϑ1,I , ϑ2,III , ϑ3,II ) (ϑ1,II , ϑ2,II , ϑ3,I ) (ϑ1,II , ϑ2,IV , ϑ3,II ), The treatment of kinematics of robot manipulators can be found in several which are illustrated in Fig. 2.33: shoulder–right/elbow–up, shoulder–left/elbow– classical robotics texts, such as [180, 10, 200, 217]. Specific texts are [23, 6, up, shoulder–right/elbow–down, shoulder–left/elbow–down; obviously, the fore- 151]. arm orientation is different for the two pairs of solutions. For the descriptions of the orientation of a rigid body, see [187]. Quaternion Notice finally how it is possible to find the solutions only if at least algebra can be found in [46]; see [204] for the extraction of quaternions from rotation matrices. pW x = 0 or pW y = 0. The Denavit–Hartenberg convention was first introduced in [60]. A modi- fied version is utilized in [53, 248, 111]. The use of homogeneous transformation In the case pW x = pW y = 0, an infinity of solutions is obtained, since it is possible to determine the joint variables ϑ2 and ϑ3 independently of the value matrices for the computation of open-chain manipulator direct kinematics is of ϑ1 ; in the following, it will be seen that the arm in such configuration is presented in [181], while in [183] sufficient conditions are given for the closed- form computation of the inverse kinematics problem. For kinematics of closed kinematically singular (see Problem 2.18). chains see [144, 111]. The design of the Stanford manipulator is due to [196]. The problem of kinematic calibration is considered in [188, 98]. Methods 2.12.5 Solution of Spherical Wrist which do not require the use of external sensors for direct measurement of end-effector position and orientation are proposed in [68]. Consider the spherical wrist shown in Fig. 2.24, whose direct kinematics was The kinematic decoupling deriving from the spherical wrist is utilized given in (2.67). It is desired to find the joint variables ϑ4 , ϑ5 , ϑ6 corresponding in [76, 99, 182]. Numerical methods for the solution of the inverse kinematics to a given end-effector orientation R36 . As previously pointed out, these angles problem based on iterative algorithms are proposed in [232, 86]. constitute a set of Euler angles ZYZ with respect to Frame 3. Hence, having computed the rotation matrix ⎡ 3 ⎤ Problems nx s3x a3x R36 = ⎣ n3y s3y a3y ⎦ , 2.1. Find the rotation matrix corresponding to the set of Euler angles ZXZ. n3z s3z a3z 2.2. Discuss the inverse solution for the Euler angles ZYZ in the case sϑ = 0. Problems 101 102 2 Kinematics
Fig. 2.34. Four-link closed-chain planar arm with prismatic joint
2.3. Discuss the inverse solution for the Roll–Pitch–Yaw angles in the case cϑ = 0. Fig. 2.35. Cylindrical arm 2.4. Verify that the rotation matrix corresponding to the rotation by an angle about an arbitrary axis is given by (2.25). 2.15. For the set of minimal representations of orientation φ, define the sum 2.5. Prove that the angle and the unit vector of the axis corresponding to a operation in terms of the composition of rotations. By means of an example, rotation matrix are given by (2.27), (2.28). Find inverse formulae in the case show that the commutative property does not hold for that operation. of sin ϑ = 0. 2.16. Consider the elementary rotations about coordinate axes given by in- 2.6. Verify that the rotation matrix corresponding to the unit quaternion is finitesimal angles. Show that the rotation resulting from any two elementary given by (2.33). rotations does not depend on the order of rotations. [Hint: for an infinitesimal angle dφ, approximate cos (dφ) ≈ 1 and sin (dφ) ≈ dφ … ]. Further, define 2.7. Prove that the unit quaternion is invariant with respect to the rotation R(dφx , dφy , dφz ) = Rx (dφx )Ry (dφy )Rz (dφz ); show that matrix and its transpose, i.e., R(η, ) = RT (η, ) = . R(dφx , dφy , dφz )R(dφx , dφy , dφz ) = R(dφx + dφx , dφy + dφy , dφz + dφz ). 2.8. Prove that the unit quaternion corresponding to a rotation matrix is given by (2.34), (2.35). 2.17. Draw the workspace of the three-link planar arm in Fig. 2.20 with the 2.9. Prove that the quaternion product is expressed by (2.37). data: a1 = 0.5 a2 = 0.3 a3 = 0.2 2.10. By applying the rules for inverting a block-partitioned matrix, prove −π/3 ≤ q1 ≤ π/3 − 2π/3 ≤ q2 ≤ 2π/3 − π/2 ≤ q3 ≤ π/2. that matrix A10 is given by (2.45). 2.18. With reference to the inverse kinematics of the anthropomorphic arm 2.11. Find the direct kinematics equation of the four-link closed-chain planar in Sect. 2.12.4, discuss the number of solutions in the singular cases of s3 = 0 arm in Fig. 2.34, where the two links connected by the prismatic joint are and pW x = pW y = 0. orthogonal to each other 2.19. Solve the inverse kinematics for the cylindrical arm in Fig. 2.35. 2.12. Find the direct kinematics equation for the cylindrical arm in Fig. 2.35. 2.20. Solve the inverse kinematics for the SCARA manipulator in Fig. 2.36. 2.13. Find the direct kinematics equation for the SCARA manipulator in Fig. 2.36.
2.14. Find the complete direct kinematics equation for the humanoid manip- ulator in Fig. 2.28. Problems 103
3
Differential Kinematics and Statics
Fig. 2.36. SCARA manipulator In the previous chapter, direct and inverse kinematics equations establishing the relationship between the joint variables and the end-effector pose were derived. In this chapter, differential kinematics is presented which gives the relationship between the joint velocities and the corresponding end-effector linear and angular velocity. This mapping is described by a matrix, termed geometric Jacobian, which depends on the manipulator configuration. Alter- natively, if the end-effector pose is expressed with reference to a minimal representation in the operational space, it is possible to compute the Jaco- bian matrix via differentiation of the direct kinematics function with respect to the joint variables. The resulting Jacobian, termed analytical Jacobian, in general differs from the geometric one. The Jacobian constitutes one of the most important tools for manipulator characterization; in fact, it is useful for finding singularities, analyzing redundancy, determining inverse kinematics algorithms, describing the mapping between forces applied to the end-effector and resulting torques at the joints (statics) and, as will be seen in the follow- ing chapters, deriving dynamic equations of motion and designing operational space control schemes. Finally, the kineto-statics duality concept is illustrated, which is at the basis of the definition of velocity and force manipulability el- lipsoids.
3.1 Geometric Jacobian
Consider an n-DOF manipulator. The direct kinematics equation can be writ-
ten in the form ⎡ ⎤
⎢ Re (q) pe (q) ⎥
T e (q) = ⎣ ⎦ (3.1)
T
0 1
T
where q = [ q1 . . . qn ] is the vector of joint variables. Both end-effector
position and orientation vary as q varies.
106 3 Differential Kinematics and Statics 3.1 Geometric Jacobian 107
The goal of the differential kinematics is to find the relationship between the (3 × 3) matrix S is skew-symmetric since
the joint velocities and the end-effector linear and angular velocities. In other words, it is desired to express the end-effector linear velocity ṗe and angular S(t) + S T (t) = O. (3.7) velocity ω e as a function of the joint velocities q̇. As will be seen afterwards, the sought relations are both linear in the joint velocities, i.e., Postmultiplying both sides of (3.6) by R(t) gives
ṗe = J P (q)q̇ (3.2) Ṙ(t) = S(t)R(t) (3.8)
that allows the time derivative of R(t) to be expressed as a function of R(t)
ω e = J O (q)q̇. (3.3) itself.
Equation (3.8) relates the rotation matrix R to its derivative by means
In (3.2) J P is the (3 × n) matrix relating the contribution of the joint veloc- of the skew-symmetric operator S and has a meaningful physical interpreta- ities q̇ to the end-effector linear velocity ṗe , while in (3.3) J O is the (3 × n) tion. Consider a constant vector p and the vector p(t) = R(t)p . The time matrix relating the contribution of the joint velocities q̇ to the end-effector derivative of p(t) is angular velocity ω e . In compact form, (3.2), (3.3) can be written as ṗ(t) = Ṙ(t)p , ṗe which, in view of (3.8), can be written as ve = = J (q)q̇ (3.4) ωe ṗ(t) = S(t)R(t)p . which represents the manipulator differential kinematics equation. The (6×n) matrix J is the manipulator geometric Jacobian If the vector ω(t) denotes the angular velocity of frame R(t) with respect to the reference frame at time t, it is known from mechanics that JP J= , (3.5) JO ṗ(t) = ω(t) × R(t)p . which in general is a function of the joint variables. Therefore, the matrix operator S(t) describes the vector product between the In order to compute the geometric Jacobian, it is worth recalling a number vector ω and the vector R(t)p . The matrix S(t) is so that its symmetric of properties of rotation matrices and some important results of rigid body elements with respect to the main diagonal represent the components of the kinematics. vector ω(t) = [ ωx ωy ωz ]T in the form ⎡ ⎤ 3.1.1 Derivative of a Rotation Matrix 0 −ωz ωy S = ⎣ ωz 0 −ωx ⎦ , (3.9) The manipulator direct kinematics equation in (3.1) describes the end-effector −ωy ωx 0 pose, as a function of the joint variables, in terms of a position vector and a rotation matrix. Since the aim is to characterize the end-effector linear and which justifies the expression S(t) = S(ω(t)). Hence, (3.8) can be rewritten angular velocities, it is worth considering first the derivative of a rotation as matrix with respect to time. Ṙ = S(ω)R. (3.10) Consider a time-varying rotation matrix R = R(t). In view of the orthog- Furthermore, if R denotes a rotation matrix, it can be shown that the onality of R, one has the relation following relation holds: R(t)RT (t) = I RS(ω)RT = S(Rω) (3.11) which, differentiated with respect to time, gives the identity which will be useful later (see Problem 3.1). T T Ṙ(t)R (t) + R(t)Ṙ (t) = O.
Set S(t) = Ṙ(t)RT (t); (3.6) 108 3 Differential Kinematics and Statics 3.1 Geometric Jacobian 109
Example 3.1 Consider the elementary rotation matrix about axis z given in (2.6). If α is a function of time, by computing the time derivative of Rz (α(t)), (3.6) becomes −α̇ sin α −α̇ cos α 0 cos α sin α 0 S(t) = α̇ cos α −α̇ sin α 0 −sin α cos α 0 0 0 0 0 0 1 0 −α̇ 0 = α̇ 0 0 = S(ω(t)). 0 0 0
According to (3.9), it is ω = [0 0 α̇ ]T that expresses the angular velocity of the frame about axis z. Fig. 3.1. Characterization of generic Link i of a manipulator
and has origin along Joint i + 1 axis, while Frame i − 1 has origin along Joint i
With reference to Fig. 2.11, consider the coordinate transformation of a axis (Fig. 3.1). point P from Frame 1 to Frame 0; in view of (2.38), this is given by Let pi−1 and pi be the position vectors of the origins of Frames i − 1 and i, respectively. Also, let r i−1 i−1,i denote the position of the origin of Frame i with p0 = o01 + R01 p1 . (3.12) respect to Frame i − 1 expressed in Frame i − 1. According to the coordinate Differentiating (3.12) with respect to time gives transformation (3.10), one can write1
0 pi = pi−1 + Ri−1 r i−1
i−1,i .
ṗ0 = ȯ01 + R01 ṗ1 + Ṙ1 p1 ; (3.13)
Then, by virtue of (3.14), it is
utilizing the expression of the derivative of a rotation matrix (3.8) and speci- i−1,i + ω i−1 × Ri−1 r i−1,i = ṗi−1 + v i−1,i + ω i−1 × r i−1,i ṗi = ṗi−1 + Ri−1 ṙ i−1 i−1 fying the dependence on the angular velocity gives (3.16) ṗ0 = ȯ01 + R01 ṗ1 + S(ω 01 )R01 p1 . which gives the expression of the linear velocity of Link i as a function of the translational and rotational velocities of Link i − 1. Note that v i−1,i denotes Further, denoting the vector R01 p1 by r 01 , it is the velocity of the origin of Frame i with respect to the origin of Frame i − 1. Concerning link angular velocity, it is worth starting from the rotation ṗ0 = ȯ01 + R01 ṗ1 + ω 01 × r 01 (3.14) composition Ri = Ri−1 Ri−1i ; which is the known form of the velocity composition rule. Notice that, if p1 is fixed in Frame 1, then it is from (3.8), its time derivative can be written as i−1 S(ω i )Ri = S(ω i−1 )Ri + Ri−1 S(ω i−1 i−1,i )Ri (3.17) ṗ0 = ȯ01 + ω 01 × r 01 (3.15) where ω i−1 i−1,i denotes the angular velocity of Frame i with respect to Frame since ṗ1 = 0. i − 1 expressed in Frame i − 1. From (2.4), the second term on the right-hand side of (3.17) can be rewritten as 3.1.2 Link Velocities i−1 T i−1 Ri−1 S(ω i−1 i−1,i )Ri = Ri−1 S(ω i−1 i−1,i )Ri−1 Ri−1 Ri ; Consider the generic Link i of a manipulator with an open kinematic chain. 1 Hereafter, the indication of superscript ‘0’ is omitted for quantities referred to According to the Denavit–Hartenberg convention adopted in the previous Frame 0. Also, without loss of generality, Frame 0 and Frame n are taken as the chapter, Link i connects Joints i and i + 1; Frame i is attached to Link i base frame and the end-effector frame, respectively. 110 3 Differential Kinematics and Statics 3.1 Geometric Jacobian 111
in view of property (3.11), it is i−1 Ri−1 S(ω i−1 i−1,i )Ri = S(Ri−1 ω i−1 i−1,i )Ri .
Then, (3.17) becomes S(ω i )Ri = S(ω i−1 )Ri + S(Ri−1 ω i−1 i−1,i )Ri
leading to the result ω i = ω i−1 + Ri−1 ω i−1 i−1,i = ω i−1 + ω i−1,i , (3.18) which gives the expression of the angular velocity of Link i as a function of the angular velocities of Link i − 1 and of Link i with respect to Link i − 1. The relations (3.16), (3.18) attain different expressions depending on the Fig. 3.2. Representation of vectors needed for the computation of the velocity type of Joint i (prismatic or revolute). contribution of a revolute joint to the end-effector linear velocity
Prismatic joint 3.1.3 Jacobian Computation Since orientation of Frame i with respect to Frame i − 1 does not vary by moving Joint i, it is In order to compute the Jacobian, it is convenient to proceed separately for ω i−1,i = 0. (3.19) the linear velocity and the angular velocity. For the contribution to the linear velocity, the time derivative of pe (q) can Further, the linear velocity is be written as v i−1,i = d˙i z i−1 (3.20) n ∂pe n ṗe = q̇i = jP i q̇i . (3.27) ∂qi where z i−1 is the unit vector of Joint i axis. Hence, the expressions of angular i=1 i=1 velocity (3.18) and linear velocity (3.16) respectively become This expression shows how ṗe can be obtained as the sum of the terms q̇i jP i . ω i = ω i−1 (3.21) Each term represents the contribution of the velocity of single Joint i to the end-effector linear velocity when all the other joints are still. ṗi = ṗi−1 + d˙i z i−1 + ω i × r i−1,i , (3.22) Therefore, by distinguishing the case of a prismatic joint (qi = di ) from where the relation ω i = ω i−1 has been exploited to derive (3.22). the case of a revolute joint (qi = ϑi ), it is:
• If Joint i is prismatic, from (3.20) it is
Revolute joint q̇i jP i = d˙i z i−1 For the angular velocity it is obviously ω i−1,i = ϑ̇i z i−1 , (3.23) and then jP i = z i−1 . while for the linear velocity it is • If Joint i is revolute, observing that the contribution to the linear velocity v i−1,i = ω i−1,i × r i−1,i (3.24) is to be computed with reference to the origin of the end-effector frame due to the rotation of Frame i with respect to Frame i − 1 induced by the (Fig. 3.2), it is motion of Joint i. Hence, the expressions of angular velocity (3.18) and linear q̇i jP i = ω i−1,i × r i−1,e = ϑ̇i z i−1 × (pe − pi−1 ) velocity (3.16) respectively become ω i = ω i−1 + ϑ̇i z i−1 (3.25) and then jP i = z i−1 × (pe − pi−1 ). ṗi = ṗi−1 + ω i × r i−1,i , (3.26) where (3.18) has been exploited to derive (3.26). 112 3 Differential Kinematics and Statics 3.2 Jacobian of Typical Manipulator Structures 113
For the contribution to the angular velocity, in view of (3.18), it is • pi−1 is given by the first three elements of the fourth column of the trans-
formation matrix T 0i−1 , i.e., it can be extracted from
n
n
ωe = ωn = ω i−1,i = jOi q̇i , (3.28)
i=1 i=1 pi−1 = A01 (q1 ) . . . Ai−2
i−1 (qi−1 )p0 . (3.33)
where (3.19) and (3.23) have been utilized to characterize the terms q̇i jOi , The above equations can be conveniently used to compute the translational and thus in detail: and rotational velocities of any point along the manipulator structure, as long as the direct kinematics functions relative to that point are known. • If Joint i is prismatic, from (3.19) it is Finally, notice that the Jacobian matrix depends on the frame in which q̇i jOi = 0 the end-effector velocity is expressed. The above equations allow computation of the geometric Jacobian with respect to the base frame. If it is desired to and then represent the Jacobian in a different Frame u, it is sufficient to know the jOi = 0. relative rotation matrix Ru . The relationship between velocities in the two • If Joint i is revolute, from (3.23) it is frames is u u ṗe R O ṗe = , ω ue O Ru ωe q̇i jOi = ϑ̇i z i−1 and then which, substituted in (3.4), gives jOi = z i−1 . u u ṗe R O = J q̇ In summary, the Jacobian in (3.5) can be partitioned into the (3 × 1) ω ue O Ru column vectors jP i and jOi as ⎡ ⎤ and then jP 1 jP n Ru O Ju = u J, (3.34) J =⎣ … ⎦, (3.29) O R jO1 jOn where J u denotes the geometric Jacobian in Frame u, which has been assumed where to be time-invariant. ⎧ ⎪ ⎪ z i−1 ⎨ 0 for a prismatic joint jP i 3.2 Jacobian of Typical Manipulator Structures = (3.30) ⎪ ⎩ z i−1 × (pe − pi−1 ) jOi ⎪ for a revolute joint. z i−1 In the following, the Jacobian is computed for some of the typical manipulator structures presented in the previous chapter. The expressions in (3.30) allow Jacobian computation in a simple, systematic way on the basis of direct kinematics relations. In fact, the vectors z i−1 , pe and pi−1 are all functions of the joint variables. In particular: 3.2.1 Three-link Planar Arm
• z i−1 is given by the third column of the rotation matrix R0i−1 , i.e., In this case, from (3.30) the Jacobian is z i−1 = R01 (q1 ) … Ri−2 i−1 (qi−1 )z 0 (3.31) z 0 × (p3 − p0 ) z 1 × (p3 − p1 ) z 2 × (p3 − p2 ) J (q) = . z0 z1 z2 where z 0 = [ 0 0 1 ]T allows the selection of the third column. • pe is given by the first three elements of the fourth column of the trans- Computation of the position vectors of the various links gives formation matrix T 0e , i.e., by expressing pe in the (4 × 1) homogeneous ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ form 0 a1 c1 a1 c1 + a2 c12 pe = A01 (q1 ) … An−1 n (qn )p0 (3.32) p0 = ⎣ 0 ⎦ p1 = ⎣ a1 s1 ⎦ p2 = ⎣ a1 s1 + a2 s12 ⎦ where p0 = [ 0 0 0 1 ]T allows the selection of the fourth column. 0 0 0 114 3 Differential Kinematics and Statics 3.2 Jacobian of Typical Manipulator Structures 115 ⎡ ⎤ a1 c1 + a2 c12 + a3 c123 while computation of the unit vectors of revolute joint axes gives p3 = ⎣ a1 s1 + a2 s12 + a3 s123 ⎦ ⎡ ⎤ ⎡ ⎤ 0 0 s1 z 0 = ⎣ 0 ⎦ z 1 = z 2 = ⎣ −c1 ⎦ . while computation of the unit vectors of revolute joint axes gives 1 0 ⎡ ⎤ 0 From (3.29) it is z0 = z1 = z2 = ⎣ 0 ⎦ 1 ⎡ ⎤ −s1 (a2 c2 + a3 c23 ) −c1 (a2 s2 + a3 s23 ) −a3 c1 s23 ⎢ c1 (a2 c2 + a3 c23 ) −s1 (a2 s2 + a3 s23 ) −a3 s1 s23 ⎥ since they are all parallel to axis z0 . From (3.29) it is ⎢ ⎥ ⎢ 0 a2 c2 + a3 c23 a3 c23 ⎥ ⎡ ⎤ J =⎢ ⎥. (3.37) −a1 s1 − a2 s12 − a3 s123 −a2 s12 − a3 s123 −a3 s123 ⎢ 0 s1 s1 ⎥ ⎣ ⎦ ⎢ a1 c1 + a2 c12 + a3 c123 a2 c12 + a3 c123 a3 c123 ⎥ 0 −c1 −c1 ⎢ ⎥ 1 0 0 ⎢ 0 0 0 ⎥ J =⎢ ⎥. (3.35) ⎢ 0 0 0 ⎥ ⎣ ⎦ Only three of the six rows of the Jacobian (3.37) are linearly independent. 0 0 0 Having 3 DOFs only, it is worth considering the upper (3 × 3) block of the 1 1 1 Jacobian In the Jacobian (3.35), only the three non-null rows are relevant (the rank of ⎡ ⎤ −s1 (a2 c2 + a3 c23 ) −c1 (a2 s2 + a3 s23 ) −a3 c1 s23 the matrix is at most 3); these refer to the two components of linear velocity J P = ⎣ c1 (a2 c2 + a3 c23 ) −s1 (a2 s2 + a3 s23 ) −a3 s1 s23 ⎦ (3.38) along axes x0 , y0 and the component of angular velocity about axis z0 . This 0 a2 c2 + a3 c23 a3 c23 result can be derived by observing that three DOFs allow specification of at most three end-effector variables; vz , ωx , ωy are always null for this kinematic that describes the relationship between the joint velocities and the end-effector structure. If orientation is of no concern, the (2×3) Jacobian for the positional linear velocity. This structure does not allow an arbitrary angular velocity ω part can be derived by considering just the first two rows, i.e., to be obtained; in fact, the two components ωx and ωy are not independent (s1 ωy = −c1 ωx ). −a1 s1 − a2 s12 − a3 s123 −a2 s12 − a3 s123 −a3 s123 JP = . (3.36) a1 c1 + a2 c12 + a3 c123 a2 c12 + a3 c123 a3 c123 3.2.3 Stanford Manipulator 3.2.2 Anthropomorphic Arm In this case, from (3.30) it is In this case, from (3.30) the Jacobian is z 0 × (p6 − p0 ) z 1 × (p6 − p1 ) z2 J= z0 z1 0 z 0 × (p3 − p0 ) z 1 × (p3 − p1 ) z 2 × (p3 − p2 ) J= . z 3 × (p6 − p3 ) z 4 × (p6 − p4 ) z 5 × (p6 − p5 ) z0 z1 z2 . z3 z4 z5 Computation of the position vectors of the various links gives Computation of the position vectors of the various links gives ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 0 a2 c1 c2 0 c1 s2 d3 − s1 d2 p0 = p1 = ⎣ 0 ⎦ p2 = ⎣ a2 s1 c2 ⎦ p0 = p1 = ⎣ 0 ⎦ p3 = p4 = p5 = ⎣ s1 s2 d3 + c1 d2 ⎦ 0 a2 s2 0 c2 d3 ⎡ ⎤ ⎡ ⎤ c1 (a2 c2 + a3 c23 ) c1 s2 d3 − s1 d2 + c1 (c2 c4 s5 + s2 c5 ) − s1 s4 s5 d6 p3 = ⎣ s1 (a2 c2 + a3 c23 ) ⎦ p6 = ⎣ s1 s2 d3 + c1 d2 + s1 (c2 c4 s5 + s2 c5 ) + c1 s4 s5 d6 ⎦ , a2 s2 + a3 s23 c2 d3 + (−s2 c4 s5 + c2 c5 )d6 116 3 Differential Kinematics and Statics 3.3 Kinematic Singularities 117
while computation of the unit vectors of joint axes gives ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ 0 −s1 c1 s2 z 0 = ⎣ 0 ⎦ z 1 = ⎣ c1 ⎦ z 2 = z 3 = ⎣ s1 s2 ⎦ 1 0 c2 ⎡ ⎤ ⎡ ⎤ −c1 c2 s4 − s1 c4 c1 (c2 c4 s5 + s2 c5 ) − s1 s4 s5 z 4 = ⎣ −s1 c2 s4 + c1 c4 ⎦ z 5 = ⎣ s1 (c2 c4 s5 + s2 c5 ) + c1 s4 s5 ⎦ . s2 s4 −s2 c4 s5 + c2 c5 The sought Jacobian can be obtained by developing the computations as Fig. 3.3. Two-link planar arm at a boundary singularity in (3.29), leading to expressing end-effector linear and angular velocity as a function of joint velocities.
Example 3.2
3.3 Kinematic Singularities To illustrate the behaviour of a manipulator at a singularity, consider a two-link planar arm. In this case, it is worth considering only the components ṗx and ṗy of The Jacobian in the differential kinematics equation of a manipulator defines the linear velocity in the plane. Thus, the Jacobian is the (2 × 2) matrix a linear mapping v e = J (q)q̇ (3.39) −a1 s1 − a2 s12 −a2 s12 J= . (3.40) a1 c1 + a2 c12 a2 c12 between the vector q̇ of joint velocities and the vector v e = [ ṗTe ω Te ]T of end- effector velocity. The Jacobian is, in general, a function of the configuration To analyze matrix rank, consider its determinant given by q; those configurations at which J is rank-deficient are termed kinematic det(J ) = a1 a2 s2 . (3.41) singularities. To find the singularities of a manipulator is of great interest for the following reasons: For a1 , a2 = 0, it is easy to find that the determinant in (3.41) vanishes whenever
a) Singularities represent configurations at which mobility of the structure ϑ2 = 0 ϑ2 = π, is reduced, i.e., it is not possible to impose an arbitrary motion to the ϑ1 being irrelevant for the determination of singular configurations. These occur end-effector. when the arm tip is located either on the outer (ϑ2 = 0) or on the inner (ϑ2 = π) b) When the structure is at a singularity, infinite solutions to the inverse boundary of the reachable workspace. Figure 3.3 illustrates the arm posture for kinematics problem may exist. ϑ2 = 0. c) In the neighbourhood of a singularity, small velocities in the operational By analyzing the differential motion of the structure in such configuration, it space may cause large velocities in the joint space. can be observed that the two column vectors [ −(a1 + a2 )s1 (a1 + a2 )c1 ]T and [ −a2 s1 a2 c1 ]T of the Jacobian become parallel, and thus the Jacobian rank be- Singularities can be classified into: comes one; this means that the tip velocity components are not independent (see • Boundary singularities that occur when the manipulator is either out- point a) above). stretched or retracted. It may be understood that these singularities do not represent a true drawback, since they can be avoided on condition that the manipulator is not driven to the boundaries of its reachable workspace. • Internal singularities that occur inside the reachable workspace and are 3.3.1 Singularity Decoupling generally caused by the alignment of two or more axes of motion, or else by the attainment of particular end-effector configurations. Unlike the above, Computation of internal singularities via the Jacobian determinant may be these singularities constitute a serious problem, as they can be encountered tedious and of no easy solution for complex structures. For manipulators hav- anywhere in the reachable workspace for a planned path in the operational ing a spherical wrist, by analogy with what has already been seen for inverse space. kinematics, it is possible to split the problem of singularity computation into two separate problems: 118 3 Differential Kinematics and Statics 3.3 Kinematic Singularities 119
Fig. 3.4. Spherical wrist at a singularity
Fig. 3.5. Anthropomorphic arm at an elbow singularity
• computation of arm singularities resulting from the motion of the first 3 or more links, • computation of wrist singularities resulting from the motion of the wrist leads to determining the arm singularities, while the condition joints. det(J 22 ) = 0 For the sake of simplicity, consider the case n = 6; the Jacobian can be leads to determining the wrist singularities. partitioned into (3 × 3) blocks as follows: Notice, however, that this form of Jacobian does not provide the relation- ship between the joint velocities and the end-effector velocity, but it leads to J 11 J 12 J= (3.42) simplifying singularity computation. Below the two types of singularities are J 21 J 22 analyzed in detail. where, since the outer 3 joints are all revolute, the expressions of the two right blocks are respectively 3.3.2 Wrist Singularities ! J 12 = z 3 × (pe − p3 ) z 4 × (pe − p4 ) z 5 × (pe − p5 ) On the basis of the above singularity decoupling, wrist singularities can be ! determined by inspecting the block J 22 in (3.43). It can be recognized that the J 22 = z 3 z 4 z 5 . (3.43) wrist is at a singular configuration whenever the unit vectors z 3 , z 4 , z 5 are As singularities are typical of the mechanical structure and do not depend on linearly dependent. The wrist kinematic structure reveals that a singularity the frames chosen to describe kinematics, it is convenient to choose the origin occurs when z 3 and z 5 are aligned, i.e., whenever of the end-effector frame at the intersection of the wrist axes (see Fig. 2.32). ϑ5 = 0 ϑ5 = π. The choice p = pW leads to ! Taking into consideration only the first configuration (Fig. 3.4), the loss of J 12 = 0 0 0 , mobility is caused by the fact that rotations of equal magnitude about opposite directions on ϑ4 and ϑ6 do not produce any end-effector rotation. Further, the since all vectors pW − pi are parallel to the unit vectors z i , for i = 3, 4, 5, no wrist is not allowed to rotate about the axis orthogonal to z 4 and z 3 , (see matter how Frames 3, 4, 5 are chosen according to DH convention. In view of point a) above). This singularity is naturally described in the joint space and this choice, the overall Jacobian becomes a block lower-triangular matrix. In can be encountered anywhere inside the manipulator reachable workspace; as this case, computation of the determinant is greatly simplified, as this is given a consequence, special care is to be taken in programming an end-effector by the product of the determinants of the two blocks on the diagonal, i.e., motion. det(J ) = det(J 11 )det(J 22 ). (3.44) 3.3.3 Arm Singularities In turn, a true singularity decoupling has been achieved; the condition Arm singularities are characteristic of a specific manipulator structure; to det(J 11 ) = 0 illustrate their determination, consider the anthropomorphic arm (Fig. 2.23), 120 3 Differential Kinematics and Statics 3.4 Analysis of Redundancy 121
determinant leads to finding the same singular configurations, which are rela-
tive to different values of the third joint variables, though — compare (2.66)
and (2.70).
Finally, it is important to remark that, unlike the wrist singularities, the
arm singularities are well identified in the operational space, and thus they
can be suitably avoided in the end-effector trajectory planning stage.
3.4 Analysis of Redundancy
The concept of kinematic redundancy has been introduced in Sect. 2.10.2;
redundancy is related to the number n of DOFs of the structure, the number m
of operational space variables, and the number r of operational space variables
necessary to specify a given task.
Fig. 3.6. Anthropomorphic arm at a shoulder singularity In order to perform a systematic analysis of redundancy, it is worth con-
sidering differential kinematics in lieu of direct kinematics (2.82). To this end,
(3.39) is to be interpreted as the differential kinematics mapping relating the
whose Jacobian for the linear velocity part is given by (3.38). Its determinant n components of the joint velocity vector to the r ≤ m components of the ve- is locity vector v e of concern for the specific task. To clarify this point, consider det(J P ) = −a2 a3 s3 (a2 c2 + a3 c23 ). the case of a 3-link planar arm; that is not intrinsically redundant (n = m = 3) Like in the case of the planar arm of Example 3.2, the determinant does not and its Jacobian (3.35) has 3 null rows accordingly. If the task does not spec- depend on the first joint variable. ify ωz (r = 2), the arm becomes functionally redundant and the Jacobian to For a2 , a3 = 0, the determinant vanishes if s3 = 0 and/or (a2 c2 + a3 c23 ) = consider for redundancy analysis is the one in (3.36). 0. The first situation occurs whenever A different case is that of the anthropomorphic arm for which only posi- tion variables are of concern (n = m = 3). The relevant Jacobian is the one ϑ3 = 0 ϑ3 = π in (3.38). The arm is neither intrinsically redundant nor can become function- ally redundant if it is assigned a planar task; in that case, indeed, the task meaning that the elbow is outstretched (Fig. 3.5) or retracted, and is termed would set constraints on the 3 components of end-effector linear velocity. elbow singularity. Notice that this type of singularity is conceptually equiva- Therefore, the differential kinematics equation to consider can be formally lent to the singularity found for the two-link planar arm. written as in (3.39), i.e., By recalling the direct kinematics equation in (2.66), it can be observed v e = J (q)q̇, (3.45) that the second situation occurs when the wrist point lies on axis z0 (Fig. 3.6); where now v e is meant to be the (r × 1) vector of end-effector velocity of it is thus characterized by concern for the specific task and J is the corresponding (r × n) Jacobian p x = py = 0 matrix that can be extracted from the geometric Jacobian; q̇ is the (n × 1) and is termed shoulder singularity. vector of joint velocities. If r < n, the manipulator is kinematically redundant Notice that the whole axis z0 describes a continuum of singular configu- and there exist (n − r) redundant DOFs. rations; a rotation of ϑ1 does not cause any translation of the wrist position The Jacobian describes the linear mapping from the joint velocity space to (the first column of J P is always null at a shoulder singularity), and then the end-effector velocity space. In general, it is a function of the configuration. the kinematics equation admits infinite solutions; moreover, motions starting In the context of differential kinematics, however, the Jacobian has to be from the singular configuration that take the wrist along the z1 direction are regarded as a constant matrix, since the instantaneous velocity mapping is not allowed (see point b) above). of interest for a given posture. The mapping is schematically illustrated in If a spherical wrist is connected to an anthropomorphic arm (Fig. 2.26), Fig. 3.7 with a typical notation from set theory. the arm direct kinematics is different. In this case the Jacobian to consider represents the block J 11 of the Jacobian in (3.42) with p = pW . Analyzing its 122 3 Differential Kinematics and Statics 3.5 Inverse Differential Kinematics 123
since J P q̇ 0 = 0 for any q̇ 0 . This result is of fundamental importance for
redundancy resolution; a solution of the kind (3.46) points out the possibility
of choosing the vector of arbitrary joint velocities q̇ 0 so as to exploit advanta-
geously the redundant DOFs. In fact, the effect of q̇ 0 is to generate internal
motions of the structure that do not change the end-effector position and ori-
entation but may allow, for instance, manipulator reconfiguration into more
dexterous postures for execution of a given task.
3.5 Inverse Differential Kinematics
In Sect. 2.12 it was shown how the inverse kinematics problem admits closed-
Fig. 3.7. Mapping between the joint velocity space and the end-effector velocity form solutions only for manipulators having a simple kinematic structure. space Problems arise whenever the end-effector attains a particular position and/or orientation in the operational space, or the structure is complex and it is not The differential kinematics equation in (3.45) can be characterized in terms possible to relate the end-effector pose to different sets of joint variables, or of the range and null spaces of the mapping;2 specifically, one has that: else the manipulator is redundant. These limitations are caused by the highly nonlinear relationship between joint space variables and operational space • The range space of J is the subspace R(J ) in IRr of the end-effector veloc- variables. ities that can be generated by the joint velocities, in the given manipulator On the other hand, the differential kinematics equation represents a linear posture. mapping between the joint velocity space and the operational velocity space, • The null space of J is the subspace N (J ) in IRn of joint velocities that do although it varies with the current configuration. This fact suggests the pos- not produce any end-effector velocity, in the given manipulator posture. sibility to utilize the differential kinematics equation to tackle the inverse If the Jacobian has full rank , one has kinematics problem. Suppose that a motion trajectory is assigned to the end-effector in terms dim R(J ) = r dim N (J ) = n − r of v e and the initial conditions on position and orientation. The aim is to and the range of J spans the entire space IRr . Instead, if the Jacobian degen- determine a feasible joint trajectory (q(t), q̇(t)) that reproduces the given erates at a singularity, the dimension of the range space decreases while the trajectory. dimension of the null space increases, since the following relation holds: By considering (3.45) with n = r, the joint velocities can be obtained via simple inversion of the Jacobian matrix dim R(J ) + dim N (J ) = n q̇ = J −1 (q)v e . (3.47) independently of the rank of the matrix J . The existence of a subspace N (J ) = ∅ for a redundant manipulator allows If the initial manipulator posture q(0) is known, joint positions can be com- determination of systematic techniques for handling redundant DOFs. To this puted by integrating velocities over time, i.e., end, if q̇ ∗ denotes a solution to (3.45) and P is an (n × n) matrix so that ” t R(P ) ≡ N (J ), q(t) = q̇(ς)dς + q(0). 0 the joint velocity vector q̇ = q̇ ∗ + P q̇ 0 , (3.46) The integration can be performed in discrete time by resorting to numerical techniques. The simplest technique is based on the Euler integration method; with arbitrary q̇ 0 , is also a solution to (3.45). In fact, premultiplying both given an integration interval Δt, if the joint positions and velocities at time sides of (3.46) by J yields tk are known, the joint positions at time tk+1 = tk + Δt can be computed as J q̇ = J q̇ ∗ + J P q̇ 0 = J q̇ ∗ = v e q(tk+1 ) = q(tk ) + q̇(tk )Δt. (3.48) 2 See Sect. A.4 for the linear mappings. 124 3 Differential Kinematics and Statics 3.5 Inverse Differential Kinematics 125
This technique for inverting kinematics is independent of the solvability which, substituted into (3.49), gives the sought optimal solution
of the kinematic structure. Nonetheless, it is necessary that the Jacobian be square and of full rank ; this demands further insight into the cases of redun- q̇ = W −1 J T (J W −1 J T )−1 v e . (3.50) dant manipulators and kinematic singularity occurrence. Premultiplying both sides of (3.50) by J , it is easy to verify that this solution satisfies the differential kinematics equation in (3.45). 3.5.1 Redundant Manipulators A particular case occurs when the weighting matrix W is the identity When the manipulator is redundant (r < n), the Jacobian matrix has more matrix I and the solution simplifies into columns than rows and infinite solutions exist to (3.45). A viable solution q̇ = J † v e ; (3.51) method is to formulate the problem as a constrained linear optimization prob- lem. the matrix In detail, once the end-effector velocity v e and Jacobian J are given (for J † = J T (J J T )−1 (3.52) a given configuration q), it is desired to find the solutions q̇ that satisfy the 4 linear equation in (3.45) and minimize the quadratic cost functional of joint is the right pseudo-inverse of J . The obtained solution locally minimizes the velocities3 norm of joint velocities. 1 It was pointed out above that if q̇ ∗ is a solution to (3.45), q̇ ∗ +P q̇ 0 is also a g(q̇) = q̇ T W q̇ solution, where q̇ 0 is a vector of arbitrary joint velocities and P is a projector 2 where W is a suitable (n × n) symmetric positive definite weighting matrix. in the null space of J . Therefore, in view of the presence of redundant DOFs, This problem can be solved with the method of Lagrange multipliers. Con- the solution (3.51) can be modified by the introduction of another term of sider the modified cost functional the kind P q̇ 0 . In particular, q̇ 0 can be specified so as to satisfy an additional constraint to the problem. 1 T g(q̇, λ) = q̇ W q̇ + λT (v e − J q̇), In that case, it is necessary to consider a new cost functional in the form 2 1 where λ is an (r × 1) vector of unknown multipliers that allows the incorpo- g (q̇) = (q̇ − q̇ 0 )T (q̇ − q̇ 0 ); ration of the constraint (3.45) in the functional to minimize. The requested 2 solution has to satisfy the necessary conditions: this choice is aimed at minimizing the norm of vector q̇ − q̇ 0 ; in other words, T T solutions are sought which satisfy the constraint (3.45) and are as close as pos- ∂g ∂g sible to q̇ 0 . In this way, the objective specified through q̇ 0 becomes unavoid- =0 = 0. ∂ q̇ ∂λ ably a secondary objective to satisfy with respect to the primary objective specified by the constraint (3.45). From the first one, it is W q̇ − J T λ = 0 and thus Proceeding in a way similar to the above yields q̇ = W −1 J T λ (3.49) 1 g (q̇, λ) = (q̇ − q̇ 0 )T (q̇ − q̇ 0 ) + λT (v e − J q̇); where the inverse of W exists. Notice that the solution (3.49) is a minimum, 2 since ∂ 2 g/∂ q̇ 2 = W is positive definite. From the second condition above, the from the first necessary condition it is constraint v e = J q̇ q̇ = J T λ + q̇ 0 (3.53) is recovered. Combining the two conditions gives which, substituted into (3.45), gives v e = J W −1 J T λ; λ = (J J T )−1 (v e − J q̇ 0 ). −1 under the assumption that J has full rank, J W J is an (r × r) square T
matrix of rank r and thus can be inverted. Solving for λ yields Finally, substituting λ back in (3.53) gives
λ = (J W −1 J T )−1 v e q̇ = J † v e + (I n − J † J )q̇ 0 . (3.54)
3 4 Quadratic forms and the relative operations are recalled in Sect. A.6. See Sect. A.7 for the definition of the pseudo-inverse of a matrix. 126 3 Differential Kinematics and Statics 3.5 Inverse Differential Kinematics 127
As can be easily recognized, the obtained solution is composed of two terms. position vector of a generic point along the structure; thus, by maximizing The first is relative to minimum norm joint velocities. The second, termed this distance, redundancy is exploited to avoid collision of the manipulator homogeneous solution, attempts to satisfy the additional constraint to specify with an obstacle (see also Problem 3.9).7 via q̇ 0 ;5 the matrix (I − J † J ) is one of those matrices P introduced in (3.46) which allows the projection of the vector q̇ 0 in the null space of J , so as 3.5.2 Kinematic Singularities not to violate the constraint (3.45). A direct consequence is that, in the case v e = 0, is is possible to generate internal motions described by (I − J † J )q̇ 0 Both solutions (3.47) and (3.51) can be computed only when the Jacobian that reconfigure the manipulator structure without changing the end-effector has full rank. Hence, they become meaningless when the manipulator is at a position and orientation. singular configuration; in such a case, the system v e = J q̇ contains linearly Finally, it is worth discussing the way to specify the vector q̇ 0 for a con- dependent equations. venient utilization of redundant DOFs. A typical choice is It is possible to find a solution q̇ by extracting all the linearly independent T equations only if v e ∈ R(J ). The occurrence of this situation means that the ∂w(q) assigned path is physically executable by the manipulator, even though it is q̇ 0 = k0 (3.55) ∂q at a singular configuration. If instead v e ∈ / R(J ), the system of equations has no solution; this means that the operational space path cannot be executed where k0 > 0 and w(q) is a (secondary) objective function of the joint vari- by the manipulator at the given posture. ables. Since the solution moves along the direction of the gradient of the ob- It is important to underline that the inversion of the Jacobian can represent jective function, it attempts to maximize it locally compatible to the primary a serious inconvenience not only at a singularity but also in the neighbourhood objective (kinematic constraint). Typical objective functions are: of a singularity. For instance, for the Jacobian inverse it is well known that its • The manipulability measure, defined as computation requires the computation of the determinant; in the neighbour- hood of a singularity, the determinant takes on a relatively small value which can cause large joint velocities (see point c) in Sect. 3.3). Consider again the w(q) = det J (q)J T (q) (3.56) above example of the shoulder singularity for the anthropomorphic arm. If a which vanishes at a singular configuration; thus, by maximizing this mea- path is assigned to the end-effector which passes nearby the base rotation axis sure, redundancy is exploited to move away from singularities.6 (geometric locus of singular configurations), the base joint is forced to make • The distance from mechanical joint limits, defined as a rotation of about π in a relatively short time to allow the end-effector to 2 keep tracking the imposed trajectory. 1 n qi − q̄i A more rigorous analysis of the solution features in the neighbourhood of w(q) = − (3.57) 2n i=1 qiM − qim singular configurations can be developed by resorting to the singular value decomposition (SVD) of matrix J .8 where qiM (qim ) denotes the maximum (minimum) joint limit and q̄i the An alternative solution overcoming the problem of inverting differential middle value of the joint range; thus, by maximizing this distance, redun- kinematics in the neighbourhood of a singularity is provided by the so-called dancy is exploited to keep the joint variables as close as possible to the damped least-squares (DLS) inverse centre of their ranges. • The distance from an obstacle, defined as J = J T (J J T + k 2 I)−1 (3.59)
w(q) = min p(q) − o (3.58) where k is a damping factor that renders the inversion better conditioned
p ,o from a numerical viewpoint. It can be shown that such a solution can be
where o is the position vector of a suitable point on the obstacle (its
centre, for instance, if the obstacle is modelled as a sphere) and p is the
7
If an obstacle occurs along the end-effector path, it is opportune to invert the
5 It should be recalled that the additional constraint has secondary priority with order of priority between the kinematic constraint and the additional constraint; respect to the primary kinematic constraint. in this way the obstacle may be avoided, but one gives up tracking the desired 6 The manipulability measure is given by the product of the singular values of the path. 8 Jacobian (see Problem 3.8). See Sect. A.8. 128 3 Differential Kinematics and Statics 3.6 Analytical Jacobian 129
obtained by reformulating the problem in terms of the minimization of the cost functional 1 1 g (q̇) = (v e − J q̇)T (v e − J q̇) + k 2 q̇ T q̇, 2 2 where the introduction of the first term allows a finite inversion error to be tolerated, with the advantage of norm-bounded velocities. The factor k es- tablishes the relative weight between the two objectives, and there exist tech- niques for selecting optimal values for the damping factor (see Problem 3.10).
Fig. 3.8. Rotational velocities of Euler angles ZYZ in current frame
3.6 Analytical Jacobian The above sections have shown the way to compute the end-effector velocity in terms of the velocity of the end-effector frame. The Jacobian is computed according to a geometric technique in which the contributions of each joint velocity to the components of end-effector linear and angular velocity are determined. If the end-effector pose is specified in terms of a minimal number of pa- rameters in the operational space as in (2.80), it is natural to ask whether it is possible to compute the Jacobian via differentiation of the direct kine- matics function with respect to the joint variables. To this end, an analytical technique is presented below to compute the Jacobian, and the existing rela- tionship between the two Jacobians is found. The translational velocity of the end-effector frame can be expressed as the time derivative of vector pe , representing the origin of the end-effector frame with respect to the base frame, i.e., Fig. 3.9. Composition of elementary rotational velocities for computing angular ∂pe velocity ṗe = q̇ = J P (q)q̇. (3.60) ∂q For what concerns the rotational velocity of the end-effector frame, the where the analytical Jacobian minimal representation of orientation in terms of three variables φe can be ∂k(q) considered. Its time derivative φ̇e in general differs from the angular velocity J A (q) = (3.63) ∂q vector defined above. In any case, once the function φe (q) is known, it is formally correct to consider the Jacobian obtained as is different from the geometric Jacobian J , since the end-effector angular velocity ω e with respect to the base frame is not given by φ̇e . ∂φe φ̇e = q̇ = J φ (q)q̇. (3.61) It is possible to find the relationship between the angular velocity ω e and ∂q the rotational velocity φ̇e for a given set of orientation angles. For instance, Computing the Jacobian J φ (q) as ∂φe /∂q is not straightforward, since the consider the Euler angles ZYZ defined in Sect. 2.4.1; in Fig. 3.8, the vectors function φe (q) is not usually available in direct form, but requires computation corresponding to the rotational velocities ϕ̇, ϑ̇, ψ̇ have been represented with of the elements of the relative rotation matrix. reference to the current frame. Figure 3.9 illustrates how to compute the Upon these premises, the differential kinematics equation can be obtained contributions of each rotational velocity to the components of angular velocity as the time derivative of the direct kinematics equation in (2.82), i.e., about the axes of the reference frame: ṗe J P (q) • as a result of ϕ̇: [ ωx ωy ωz ]T = ϕ̇ [ 0 0 1 ]T ẋe = = q̇ = J A (q)q̇ (3.62) • as a result of ϑ̇: [ ωx ωy ωz ]T = ϑ̇ [ −sϕ cϕ 0 ]T φ̇e J φ (q) 130 3 Differential Kinematics and Statics 3.6 Analytical Jacobian 131
• as a result of ψ̇: [ ωx ωy ωz ]T = ψ̇ [ cϕ sϑ sϕ sϑ cϑ ]T ,
and then the equation relating the angular velocity ω e to the time derivative of the Euler angles φ̇e is9 ω e = T (φe )φ̇e , (3.64) where, in this case, ⎡ ⎤ 0 −sϕ cϕ sϑ T = ⎣0 cϕ sϕ sϑ ⎦ . 1 0 cϑ The determinant of matrix T is −sϑ , which implies that the relationship cannot be inverted for ϑ = 0, π. This means that, even though all rotational velocities of the end-effector frame can be expressed by means of a suitable angular velocity vector ω e , there exist angular velocities which cannot be expressed by means of φ̇e when the orientation of the end-effector frame causes sϑ = 0.10 In fact, in this situation, the angular velocities that can be described by φ̇e should have linearly dependent components in the directions orthogonal to axis z (ωx2 + ωy2 = ϑ̇2 ). An orientation for which the determinant of the transformation matrix vanishes is termed representation singularity of φe . From a physical viewpoint, the meaning of ω e is more intuitive than that of φ̇e . The three components of ω e represent the components of angular veloc- Fig. 3.10. Nonuniqueness of orientation computed as the integral of angular velocity ity with respect to the base frame. Instead, the three elements of φ̇e represent nonorthogonal components of angular velocity defined with respect to the axes of a frame that varies as the end-effector orientation varies. On the other Once the transformation T between ω e and φ̇e is given, the analytical hand, while the integral of φ̇e over time gives φe , the integral of ω e does not Jacobian can be related to the geometric Jacobian as admit a clear physical interpretation, as can be seen in the following example. I O ve = ẋe = T A (φe )ẋe (3.65) O T (φe )
Example 3.3 which, in view of (3.4), (3.62), yields Consider an object whose orientation with respect to a reference frame is known at J = T A (φ)J A . (3.66) time t = 0. Assign the following time profiles to ω: • ω = [ π/2 0 0 ]T 0 ≤ t ≤ 1 ω = [ 0 π/2 0 ]T 1 < t ≤ 2, This relationship shows that J and J A , in general, differ. Regarding the use T • ω = [ 0 π/2 0 ] 0≤t≤1 ω = [ π/2 0 0 ]T 1 < t ≤ 2. of either one or the other in all those problems where the influence of the The integral of ω gives the same result in the two cases Jacobian matters, it is anticipated that the geometric Jacobian will be adopted ” 2 whenever it is necessary to refer to quantities of clear physical meaning, while ωdt = [ π/2 π/2 0 ]T the analytical Jacobian will be adopted whenever it is necessary to refer to 0 differential quantities of variables defined in the operational space. but the final object orientation corresponding to the second timing law is clearly For certain manipulator geometries, it is possible to establish a substantial different from the one obtained with the first timing law (Fig. 3.10). equivalence between J and J A . In fact, when the DOFs cause rotations of the end-effector all about the same fixed axis in space, the two Jacobians 9 This relation can also be obtained from the rotation matrix associated with the are essentially the same. This is the case of the above three-link planar arm. three angles (see Problem 3.11). Its geometric Jacobian (3.35) reveals that only rotations about axis z0 are 10 In Sect. 2.4.1, it was shown that for this orientation the inverse solution of the permitted. The (3 × 3) analytical Jacobian that can be derived by considering Euler angles degenerates. the end-effector position components in the plane of the structure and defining 132 3 Differential Kinematics and Statics 3.7 Inverse Kinematics Algorithms 133
the end-effector orientation as φ = ϑ1 + ϑ2 + ϑ3 coincides with the matrix that is obtained by eliminating the three null rows of the geometric Jacobian.
3.7 Inverse Kinematics Algorithms
In Sect. 3.5 it was shown how to invert kinematics by using the differential kinematics equation. In the numerical implementation of (3.48), computation of joint velocities is obtained by using the inverse of the Jacobian evaluated with the joint variables at the previous instant of time Fig. 3.11. Inverse kinematics algorithm with Jacobian inverse −1 q(tk+1 ) = q(tk ) + J (q(tk ))v e (tk )Δt. 3.7.1 Jacobian (Pseudo-)inverse It follows that the computed joint velocities q̇ do not coincide with those satisfying (3.47) in the continuous time. Therefore, reconstruction of joint On the assumption that matrix J A is square and nonsingular, the choice variables q is entrusted to a numerical integration which involves drift phe- nomena of the solution; as a consequence, the end-effector pose corresponding q̇ = J −1 A (q)(ẋd + Ke) (3.70) to the computed joint variables differs from the desired one. This inconvenience can be overcome by resorting to a solution scheme that leads to the equivalent linear system accounts for the operational space error between the desired and the actual end-effector position and orientation. Let ė + Ke = 0. (3.71)
e = xd − x e (3.67) If K is a positive definite (usually diagonal) matrix, the system (3.71) is
asymptotically stable. The error tends to zero along the trajectory with a
be the expression of such error. convergence rate that depends on the eigenvalues of matrix K;11 the larger Consider the time derivative of (3.67), i.e., the eigenvalues, the faster the convergence. Since the scheme is practically implemented as a discrete-time system, it is reasonable to predict that an ė = ẋd − ẋe (3.68) upper bound exists on the eigenvalues; depending on the sampling time, there will be a limit for the maximum eigenvalue of K under which asymptotic which, according to differential kinematics (3.62), can be written as stability of the error system is guaranteed. The block scheme corresponding to the inverse kinematics algorithm ė = ẋd − J A (q)q̇. (3.69) in (3.70) is illustrated in Fig. 3.11, where k(·) indicates the direct kinematics function in (2.82). This scheme can be revisited in terms of the usual feedback Notice in (3.69) that the use of operational space quantities has naturally control schemes. Specifically, it can observed that the nonlinear block k(·) is lead to using the analytical Jacobian in lieu of the geometric Jacobian. For needed to compute x and thus the tracking error e, while the block J −1 A (q) this equation to lead to an inverse kinematics algorithm, it is worth relating has been introduced to compensate for J A (q) and making the system linear. the computed joint velocity vector q̇ to the error e so that (3.69) gives a The block scheme shows the presence of a string of integrators on the forward differential equation describing error evolution over time. Nonetheless, it is loop and then, for a constant reference (ẋd = 0), guarantees a null steady- necessary to choose a relationship between q̇ and e that ensures convergence state error. Further, the feedforward action provided by ẋd for a time-varying of the error to zero. reference ensures that the error is kept to zero (in the case e(0) = 0) along Having formulated inverse kinematics in algorithmic terms implies that the whole trajectory, independently of the type of desired reference xd (t). the joint variables q corresponding to a given end-effector pose xd are ac- Finally, notice that (3.70), for ẋd = 0, corresponds to the Newton method curately computed only when the error xd − k(q) is reduced within a given for solving a system of nonlinear equations. Given a constant end-effector threshold; such settling time depends on the dynamic characteristics of the pose xd , the algorithm can be keenly applied to compute one of the admissible error differential equation. The choice of q̇ as a function of e permits finding inverse kinematics algorithms with different features. 11 See Sect. A.5. 134 3 Differential Kinematics and Statics 3.7 Inverse Kinematics Algorithms 135
nonlinear differential equation. The Lyapunov direct method can be utilized
to determine a dependence q̇(e) that ensures asymptotic stability of the error
system. Choose as Lyapunov function candidate the positive definite quadratic
form12
1
V (e) = eT Ke, (3.73)
2
where K is a symmetric positive definite matrix. This function is so that
V (e) > 0 ∀e = 0, V (0) = 0.
Fig. 3.12. Block scheme of the inverse kinematics algorithm with Jacobian trans- pose Differentiating (3.73) with respect to time and accounting for (3.68) gives
V̇ = eT K ẋd − eT K ẋe . (3.74)
solutions to the inverse kinematics problem, whenever that does not admit closed-form solutions, as discussed in Sect. 2.12. Such a method is also useful In view of (3.62), it is in practice at the start-up of the manipulator for a given task, to compute the corresponding joint configuration. V̇ = eT K ẋd − eT KJ A (q)q̇. (3.75) In the case of a redundant manipulator , solution (3.70) can be generalized into At this point, the choice of joint velocities as q̇ = J †A (ẋd + Ke) + (I n − J †A J A )q̇ 0 , (3.72) q̇ = J TA (q)Ke (3.76) which represents the algorithmic version of solution (3.54). The structure of the inverse kinematics algorithm can be conceptually leads to adopted for a simple robot control technique, known under the name of kine- V̇ = eT K ẋd − eT KJ A (q)J TA (q)Ke. (3.77) matic control . As will be seen in Chap. 7, a manipulator is actually an electro- Consider the case of a constant reference (ẋd = 0). The function in (3.77) is mechanical system actuated by motor torques, while in Chaps. 8–10 dynamic negative definite, under the assumption of full rank for J A (q). The condition control techniques will be presented which will properly account for the non- V̇ < 0 with V > 0 implies that the system trajectories uniformly converge linear and coupling effects of the dynamic model. to e = 0, i.e., the system is asymptotically stable. When N (J TA ) = ∅, the At first approximation, however, it is possible to consider a kinematic function in (3.77) is only negative semi-definite, since V̇ = 0 for e = 0 with command as system input, typically a velocity. This is possible in view of Ke ∈ N (J TA ). In this case, the algorithm can get stuck at q̇ = 0 with e = 0. the presence of a low-level control loop, which ‘ideally’ imposes any specified However, the example that follows will show that this situation occurs only if reference velocity. On the other hand, such a loop already exists in a ‘closed’ the assigned end-effector position is not actually reachable from the current control unit, where the user can also intervene with kinematic commands. configuration. In other words, the scheme in Fig. 3.11 can implement a kinematic control, The resulting block scheme is illustrated in Fig. 3.12, which shows the no- provided that the integrator is regarded as a simplified model of the robot, table feature of the algorithm to require computation only of direct kinematics thanks to the presence of single joint local servos, which ensure a more or functions k(q), J TA (q). less accurate reproduction of the velocity commands. Nevertheless, it is worth It can be recognized that (3.76) corresponds to the gradient method for underlining that such a kinematic control technique yields satisfactory perfor- the solution of a system on nonlinear equations. As in the case of the Jaco- mance only when one does not require too fast motions or rapid accelerations. bian inverse solution, for a given constant end-effector pose xd , the Jacobian The performance of the independent joint control will be analyzed in Sect. 8.3. transpose algorithm can be keenly employed to solve the inverse kinemat- ics problem, or more simply to initialize the values of the manipulator joint 3.7.2 Jacobian Transpose variables. The case when xd is a time-varying function (ẋd = 0) deserves a separate A computationally simpler algorithm can be derived by finding a relationship analysis. In order to obtain V̇ < 0 also in this case, it would be sufficient to between q̇ and e that ensures error convergence to zero, without requiring choose a q̇ that depends on the (pseudo-)inverse of the Jacobian as in (3.70), linearization of (3.69). As a consequence, the error dynamics is governed by a 12 See Sect. C.3 for the presentation of the Lyapunov direct method. 136 3 Differential Kinematics and Statics 3.7 Inverse Kinematics Algorithms 137
implying that the direction of N (J TP ) coincides with the direction orthogonal to the
plane of the structure (Fig. 3.13). The Jacobian transpose algorithm gets stuck if,
with K diagonal and having all equal elements, the desired position is along the line
normal to the plane of the structure at the intersection with the wrist point. On the
other hand, the end-effector cannot physically move from the singular configuration
along such a line. Instead, if the prescribed path has a non-null component in the
plane of the structure at the singularity, algorithm convergence is ensured, since in
that case Ke ∈ / N (J TP ).
In summary, the algorithm based on the computation of the Jacobian
transpose provides a computationally efficient inverse kinematics method that
can be utilized also for paths crossing kinematic singularities.
3.7.3 Orientation Error
Fig. 3.13. Characterization of the anthropomorphic arm at a shoulder singularity for the admissible solutions of the Jacobian transpose algorithm The inverse kinematics algorithms presented in the above sections utilize the analytical Jacobian since they operate on error variables (position and orien- tation) that are defined in the operational space. recovering the asymptotic stability result derived above.13 For the inversion For what concerns the position error, it is obvious that its expression is scheme based on the transpose, the first term on the right-hand side of (3.77) given by is not cancelled any more and nothing can be said about its sign. This im- eP = pd − pe (q) (3.78) plies that asymptotic stability along the trajectory cannot be achieved. The tracking error e(t) is, anyhow, norm-bounded; the larger the norm of K, the where pd and pe denote respectively the desired and computed end-effector smaller the norm of e.14 In practice, since the inversion scheme is to be im- positions. Further, its time derivative is plemented in discrete-time, there is an upper bound on the norm of K with reference to the adopted sampling time. ėP = ṗd − ṗe . (3.79)
On the other hand, for what concerns the orientation error , its expression
depends on the particular representation of end-effector orientation, namely,
Example 3.4 Euler angles, angle and axis, and unit quaternion. Consider the anthropomorphic arm; a shoulder singularity occurs whenever a2 c2 + a3 c23 = 0 (Fig. 3.6). In this configuration, the transpose of the Jacobian in (3.38) is Euler angles 0 0 0 J TP = −c1 (a2 s2 + a3 s23 ) −s1 (a2 s2 + a3 s23 ) 0 . The orientation error is chosen according to an expression formally analogous −a3 c1 s23 −a3 s1 s23 a3 c23 to (3.78), i.e., By computing the null space of J TP , if νx , νy and νz denote the components of vector eO = φd − φe (q) (3.80) ν along the axes of the base frame, one has the result where φd and φe denote respectively the desired and computed set of Euler νy 1 angles. Further, its time derivative is =− νz = 0, νx tan ϑ1 13 Notice that, anyhow, in case of kinematic singularities, it is necessary to resort ėO = φ̇d − φ̇e . (3.81) to an inverse kinematics scheme that does not require inversion of the Jacobian. 14 Notice that the negative definite term is a quadratic function of the error, while Therefore, assuming that neither kinematic nor representation singularities the other term is a linear function of the error. Therefore, for an error of very occur, the Jacobian inverse solution for a nonredundant manipulator is derived small norm, the linear term prevails over the quadratic term, and the norm of K from (3.70), i.e., should be increased to reduce the norm of e as much as possible. 138 3 Differential Kinematics and Statics 3.7 Inverse Kinematics Algorithms 139
Angle and axis
ṗd + K P eP
q̇ = J −1
A (q) (3.82)
φ̇d + K O eO If Rd = [ nd sd ad ] denotes the desired rotation matrix of the end-effector
frame and Re = [ ne se ae ] the rotation matrix that can be computed
where K P and K O are positive definite matrices. from the joint variables, the orientation error between the two frames can be As already pointed out in Sect. 2.10 for computation of the direct kinemat- expressed as ics function in the form (2.82), the determination of the orientation variables eO = r sin ϑ (3.83) from the joint variables is not easy except for simple cases (see Example 2.5). To this end, it is worth recalling that computation of the angles φe , in a where ϑ and r identify the angle and axis of the equivalent rotation that can minimal representation of orientation, requires computation of the rotation be deduced from the matrix matrix Re = [ ne se ae ]; in fact, only the dependence of Re on q is known in closed form, but not that of φe on q. Further, the use of inverse func- R(ϑ, r) = Rd RTe (q), (3.84) tions (Atan2) in (2.19), (2.22) involves a non-negligible complexity in the describing the rotation needed to align R with Rd . Notice that (3.83) gives a computation of the analytical Jacobian, and the occurrence of representation unique relationship for −π/2 < ϑ < π/2. The angle ϑ represents the magni- singularities constitutes another drawback for the orientation error based on tude of an orientation error, and thus the above limitation is not restrictive Euler angles. since the tracking error is typically small for an inverse kinematics algorithm. Different kinds of remarks are to be made about the way to assign a time By comparing the off-diagonal terms of the expression of R(ϑ, r) in (2.25) profile for the reference variables φd chosen to represent end-effector orienta- with the corresponding terms resulting on the right-hand side of (3.84), it can tion. The most intuitive way to specify end-effector orientation is to refer to be found that a functional expression of the orientation error in (3.83) is (see the orientation of the end-effector frame (nd , sd , ad ) with respect to the base Problem 3.16) frame. Given the limitations pointed out in Sect. 2.10 about guaranteeing or- thonormality of the unit vectors along time, it is necessary first to compute 1 the Euler angles corresponding to the initial and final orientation of the end- eO = (ne (q) × nd + se (q) × sd + ae (q) × ad ); (3.85) 2 effector frame via (2.19), (2.22); only then a time evolution can be generated. Such solutions will be presented in Chap. 4. the limitation on ϑ is transformed in the condition nTe nd ≥ 0, sTe sd ≥ 0, A radical simplification of the problem at issue can be obtained for manip- aTe ad ≥ 0. ulators having a spherical wrist. Section 2.12.2 pointed out the possibility to Differentiating (3.85) with respect to time and accounting for the expres- solve the inverse kinematics problem for the position part separately from that sion of the columns of the derivative of a rotation matrix in (3.8) gives (see for the orientation part. This result also has an impact at algorithmic level. In Problem 3.19) fact, the implementation of an inverse kinematics algorithm for determining ėO = LT ω d − Lω e (3.86) the joint variables influencing the wrist position allows the computation of where the time evolution of the wrist frame RW (t). Hence, once the desired time 1 L=− S(nd )S(ne ) + S(sd )S(se ) + S(ad )S(ae ) . (3.87) evolution of the end-effector frame Rd (t) is given, it is sufficient to compute 2 the Euler angles ZYZ from the matrix RTW Rd by applying (2.19). As shown At this point, by exploiting the relations (3.2), (3.3) of the geometric Jacobian in Sect. 2.12.5, these angles are directly the joint variables of the spherical expressing ṗe and ω e as a function of q̇, (3.79), (3.86) become wrist. See also Problem 3.14. The above considerations show that the inverse kinematics algorithms ėP ṗd − J P (q)q̇ ṗd I O ė = = = − J q̇. (3.88) based on the analytical Jacobian are effective for kinematic structures having ėO L ω d − LJ O (q)q̇ T T L ωd O L a spherical wrist which are of significant interest. For manipulator structures which cannot be reduced to that class, it may be appropriate to reformulate The expression in (3.88) suggests the possibility of devising inverse kinematics the inverse kinematics problem on the basis of a different definition of the algorithms analogous to the ones derived above, but using the geometric Ja- orientation error. cobian in place of the analytical Jacobian. For instance, the Jacobian inverse solution for a nonredundant nonsingular manipulator is −1 ṗd + K P eP q̇ = J (q) . (3.89) L−1 LT ω d + K O eO 140 3 Differential Kinematics and Statics 3.7 Inverse Kinematics Algorithms 141
It is worth remarking that the inverse kinematics solution based on (3.89) To study stability of system (3.93), consider the positive definite Lyapunov
is expected to perform better than the solution based on (3.82) since it uses function candidate the geometric Jacobian in lieu of the analytical Jacobian, thus avoiding the occurrence of representation singularities. V = (ηd − ηe )2 + (d − e )T (d − e ). (3.96)
In view of (3.94), (3.95), differentiating (3.96) with respect to time and ac-
Unit quaternion counting for (3.93) yields (see Problem 3.20) In order to devise an inverse kinematics algorithm based on the unit quater- V̇ = −eTO K O eO (3.97) nion, a suitable orientation error should be defined. Let Qd = {ηd , d } and Qe = {ηe , e } represent the quaternions associated with Rd and Re , re- which is negative definite, implying that eO converges to zero. spectively. The orientation error can be described by the rotation matrix In summary, the inverse kinematics solution based on (3.92) uses the geo- Rd RTe and, in view of (2.37), can be expressed in terms of the quaternion metric Jacobian as the solution based on (3.89) but is computationally lighter. ΔQ = {Δη, Δ} where ΔQ = Qd ∗ Q−1 e . (3.90) 3.7.4 Second-order Algorithms It can be recognized that ΔQ = {1, 0} if and only if Re and Rd are aligned. Hence, it is sufficient to define the orientation error as The above inverse kinematics algorithms can be defined as first-order algo- rithms, in that they allow the inversion of a motion trajectory, specified at eO = Δ = ηe (q)d − ηd e (q) − S(d )e (q), (3.91) the end-effector in terms of of position and orientation, into the equivalent joint positions and velocities. where the skew-symmetric operator S(·) has been used. Notice, however, that Nevertheless, as will be seen in Chap. 8, for control purposes it may be the explicit computation of ηe and e from the joint variables is not possible necessary to invert a motion trajectory specified in terms of position, velocity but it requires the intermediate computation of the rotation matrix Re that and acceleration. On the other hand, the manipulator is inherently a second- is available from the manipulator direct kinematics; then, the quaternion can order mechanical system, as will be revealed by the dynamic model to be be extracted using (2.34). derived in Chap. 7. At this point, a Jacobian inverse solution can be computed as The time differentiation of the differential kinematics equation (3.62) leads ṗd + K P eP to q̇ = J −1 (q) (3.92) ẍe = J A (q)q̈ + J̇ A (q, q̇)q̇ (3.98) ω d + K O eO which gives the relationship between the joint space accelerations and the where noticeably the geometric Jacobian has been used. Substituting (3.92) operational space accelerations. into (3.4) gives (3.79) and Under the assumption of a square and non-singular matrix J A , the second- ω d − ω e + K O eO = 0. (3.93) order differential kinematics (3.98) can be inverted in terms of the joint ac- celerations It should be observed that now the orientation error equation is nonlinear q̈ = J −1 A (q) ẍe − J̇ A (q, q̇)q̇ . (3.99) in eO since it contains the end-effector angular velocity error instead of the time derivative of the orientation error. To this end, it is worth considering The numerical integration of (3.99) to reconstruct the joint velocities and the relationship between the time derivative of the quaternion Qe and the positions would unavoidably lead to a drift of the solution; therefore, similarly angular velocity ω e . This can be found to be (see Problem 3.19) to the inverse kinematics algorithm with the Jacobian inverse, it is worth considering the error defined in (3.68) along with its derivative 1 η̇e = − Te ω e (3.94) 2 ë = ẍd − ẍe (3.100) 1 ˙ e = (ηe I 3 − S(e )) ω e (3.95) which, in view of (3.98), yields 2 which is the so-called quaternion propagation. A similar relationship holds ë = ẍd − J A (q)q̈ − J̇ A (q, q̇)q̇. (3.101) between the time derivative of Qd and ω d . 142 3 Differential Kinematics and Statics 3.7 Inverse Kinematics Algorithms 143
3.7.5 Comparison Among Inverse Kinematics Algorithms
In order to make a comparison of performance among the inverse kinematics
algorithms presented above, consider the 3-link planar arm in Fig. 2.20 whose
link lengths are a1 = a2 = a3 = 0.5 m. The direct kinematics for this arm is
given by (2.83), while its Jacobian can be found from (3.35) by considering
the 3 non-null rows of interest for the operational space.
Let the arm be at the initial posture q = [ π −π/2 −π/2 ]T rad, corre-
sponding to the end-effector pose: p = [ 0 0.5 ]T m, φ = 0 rad. A circular path
of radius 0.25 m and centre at (0.25, 0.5) m is assigned to the end-effector. Let
the motion trajectory be
0.25(1 − cos πt)
pd (t) = 0 ≤ t ≤ 4;
0.25(2 + sin πt)
i.e., the end-effector has to make two complete circles in a time of 2 s per
circle. As regards end-effector orientation, initially it is required to follow the
trajectory
π
φd (t) = sin t 0 ≤ t ≤ 4;
Fig. 3.14. Block scheme of the second-order inverse kinematics algorithm with 24 Jacobian inverse i.e., the end-effector has to attain a different orientation (φd = 0.5 rad) at the end of the two circles. At this point, it is advisable to choose the joint acceleration vector as The inverse kinematics algorithms were implemented on a computer by adopting the Euler numerical integration scheme (3.48) with an integration q̈ = J −1 A (q) ẍd + K D ė + K P e − J̇ A (q, q̇)q̇ (3.102) time Δt = 1 ms. At first, the inverse kinematics along the given trajectory has been per- where K D and K P are positive definite (typically diagonal) matrices. Sub- formed by using (3.47). The results obtained in Fig. 3.15 show that the norm stituting (3.102) into (3.101) leads to the equivalent linear error system of the position error along the whole trajectory is bounded; at steady state, after t = 4, the error sets to a constant value in view of the typical drift of ë + K D ė + K P e = 0 (3.103) open-loop schemes. A similar drift can be observed for the orientation error. Next, the inverse kinematics algorithm based on (3.70) using the Jacobian which is asymptotically stable: the error tends to zero along the trajectory with inverse has been used, with the matrix gain K = diag{500, 500, 100}. The a convergence speed depending on the choice of the matrices K P e K D . The resulting joint positions and velocities as well as the tracking errors are shown second-order inverse kinematics algorithm is illustrated in the block scheme in Fig. 3.16. The norm of the position error is radically decreased and con- of Fig. 3.14. verges to zero at steady state, thanks to the closed-loop feature of the scheme; In the case of a redundant manipulator , the generalization of (3.102) leads the orientation error, too, is decreased and tends to zero at steady state. to an algorithmic solution based on the Jacobian pseudo-inverse of the kind On the other hand, if the end-effector orientation is not constrained, the operational space becomes two-dimensional and is characterized by the first q̈ = J †A ẍd + K D ė + K P e − J̇ A (q, q̇)q̇ + (I n − J †A J A )q̈ 0 (3.104) two rows of the direct kinematics in (2.83) as well as by the Jacobian in (3.36); a redundant DOF is then available. Hence, the inverse kinematics algorithm where the vector q̈ 0 represents arbitrary joint accelerations which can be cho- based on (3.72) using the Jacobian pseudo-inverse has been used with K = sen so as to (locally) optimize an objective function like those considered in diag{500, 500}. If redundancy is not exploited (q̇ 0 = 0), the results in Fig. 3.17 Sect. 3.5.1. reveal that position tracking remains satisfactory and, of course, the end- As for the first-order inverse kinematics algorithms, it is possible to con- effector orientation freely varies along the given trajectory. sider other expressions for the orientation error which, unlike the Euler angles, With reference to the previous situation, the use of the Jacobian transpose refer to an angle and axis description, else to the unit quaternion. algorithm based on (3.76) with K = diag{500, 500} gives rise to a tracking 144 3 Differential Kinematics and Statics 3.7 Inverse Kinematics Algorithms 145
Ŧ3 pos error norm Ŧ5 orien error Ŧ6 pos error norm orien
x 10 x 10 x 10
2 0 5 0.5
Ŧ0.2 4
1.5
0
Ŧ0.4 3
[rad]
[rad]
[m]
[m]
1
Ŧ0.6 2
Ŧ0.5
0.5
Ŧ0.8 1
0 Ŧ1 0 Ŧ1
0 1 2 3 4 5 0 1 2 3 4 5 0 1 2 3 4 5 0 1 2 3 4 5
[s] [s] [s] [s]
Fig. 3.15. Time history of the norm of end-effector position error and orientation Fig. 3.17. Time history of the norm of end-effector position error and orientation error with the open-loop inverse Jacobian algorithm with the Jacobian pseudo-inverse algorithm
joint pos joint vel pos error norm orien
5 10 0.01 0.5
1 0.008
5 1
2 0
0.006
[rad/s]
[rad]
[rad]
[m]
0 0
3 0.004
Ŧ0.5
Ŧ5 3
2 0.002
Ŧ5 Ŧ10 0 Ŧ1
0 1 2 3 4 5 0 1 2 3 4 5 0 1 2 3 4 5 0 1 2 3 4 5
[s] [s] [s] [s]
Ŧ5
x 10 pos error norm x 10
Ŧ8 orien error Fig. 3.18. Time history of the norm of end-effector position error and orientation
1 0
with the Jacobian transpose algorithm
0.8 Ŧ1
0.6 Ŧ2 have been considered concerning an objective function to locally maximize
[rad]
[m]
0.4 Ŧ3 according to the choice (3.55). The first function is
1 2
0.2 Ŧ4
w(ϑ2 , ϑ3 ) = (s + s23 )
2 2
0 Ŧ5
0 1 2 3 4 5 0 1 2 3 4 5
[s] [s] that provides a manipulability measure. Notice that such a function is compu-
tationally simpler than the function in (3.56), but it still describes a distance
Fig. 3.16. Time history of the joint positions and velocities, and of the norm of end- from kinematic singularities in an effective way. The gain in (3.55)) has been effector position error and orientation error with the closed-loop inverse Jacobian set to k0 = 50. In Fig. 3.19, the joint trajectories are reported for the two algorithm cases with and without (k0 = 0) constraint. The addition of the constraint leads to having coincident trajectories for Joints 2 and 3. The manipulability error (Fig. 3.18) which is anyhow bounded and rapidly tends to zero at steady measure in the constrained case (continuous line) attains larger values along state. the trajectory compared to the unconstrained case (dashed line). It is worth In order to show the capability of handling the degree of redundancy, the underlining that the tracking position error is practically the same in the two algorithm based on (3.72) with q̇ 0 = 0 has been used; two types of constraints cases (Fig. 3.17), since the additional joint velocity contribution is projected in the null space of the Jacobian so as not to alter the performance of the end-effector position task. Finally, it is worth noticing that in the constrained case the resulting joint trajectories are cyclic, i.e., they take on the same values after a period of 146 3 Differential Kinematics and Statics 3.8 Statics 147
joint pos joint pos joint 1 pos joint 2 pos
5 5
1 6 6
1
4 4
2 2
[rad/s]
[rad]
[rad]
[rad]
0 3 0 0 0
2
Ŧ2 Ŧ2
2 3 Ŧ4 Ŧ4
Ŧ6 Ŧ6
Ŧ5 Ŧ5
0 1 2 3 4 5 0 1 2 3 4 5 0 1 2 3 4 5 0 1 2 3 4 5
[s] [s] [s] [s]
Ŧ6 pos error norm manip joint 3 pos Ŧ4 pos error norm
x 10 x 10
5 1 5 2
4
1.5
0.95
3
[rad]
[rad]
[m]
[m]
0 1
2
0.9
0.5
1
0 0.85 Ŧ5 0
0 1 2 3 4 5 0 1 2 3 4 5 0 1 2 3 4 5 0 1 2 3 4 5
[s] [s] [s] [s]
Fig. 3.19. Time history of the joint positions, the norm of end-effector position Fig. 3.20. Time history of the joint positions and the norm of end-effector position error, and the manipulability measure with the Jacobian pseudo-inverse algorithm error with the Jacobian pseudo-inverse algorithm and joint limit constraint (joint and manipulability constraint; upper left: with the unconstrained solution, upper limits are denoted by dashed lines) right: with the constrained solution
3.8 Statics
the circular path. This does not happen for the unconstrained case, since the internal motion of the structure causes the arm to be in a different posture The goal of statics is to determine the relationship between the generalized after one circle. forces applied to the end-effector and the generalized forces applied to the The second objective function considered is the distance from mechanical joints — forces for prismatic joints, torques for revolute joints — with the joint limits in (3.57). Specifically, it is assumed what follows: the first joint manipulator at an equilibrium configuration. does not have limits (q1m = −2π, q1M = 2π), the second joint has limits q2m = Let τ denote the (n × 1) vector of joint torques and γ the (r × 1) vector −π/2, q2M = π/2, and the third joint has limits q3m = −3π/2, q3M = −π/2. of end-effector forces15 where r is the dimension of the operational space of It is not difficult to verify that, in the unconstrained case, the trajectories of interest. Joints 2 and 3 in Fig. 3.19 violate the respective limits. The gain in (3.55) The application of the principle of virtual work allows the determination has been set to k0 = 250. The results in Fig. 3.20 show the effectiveness of of the required relationship. The mechanical manipulators considered are sys- the technique with utilization of redundancy, since both Joints 2 and 3 tend tems with time-invariant, holonomic constraints, and thus their configurations to invert their motion — with respect to the unconstrained trajectories in depend only on the joint variables q and not explicitly on time. This implies Fig. 3.19 — and keep far from the minimum limit for Joint 2 and the maximum that virtual displacements coincide with elementary displacements. limit for Joint 3, respectively. Such an effort does not appreciably affect the Consider the elementary works performed by the two force systems. As for position tracking error, whose norm is bounded anyhow within acceptable the joint torques, the elementary work associated with them is values. dWτ = τ T dq. (3.105) 15 Hereafter, generalized forces at the joints are often called torques, while general- ized forces at the end-effector are often called forces. 148 3 Differential Kinematics and Statics 3.8 Statics 149
As for the end-effector forces γ, if the force contributions f e are separated by the moment contributions μe , the elementary work associated with them is
dWγ = f Te dpe + μTe ω e dt, (3.106)
where dpe is the linear displacement and ω e dt is the angular displacement16 By accounting for the differential kinematics relationship in (3.4), (3.5), the relation (3.106) can be rewritten as
dWγ = f Te J P (q)dq + μTe J O (q)dq (3.107)
= γ Te J (q)dq
where γ e = [ f Te μTe ]T . Since virtual and elementary displacements coincide, Fig. 3.21. Mapping between the end-effector force space and the joint torque space the virtual works associated with the two force systems are
δWτ = τ T δq (3.108) • The null space of J T is the subspace N (J T ) in IRr of the end-effector forces
δWγ = γ Te J (q)δq, (3.109) that do not require any balancing joint torques, in the given manipulator
posture.
where δ is the usual symbol to indicate virtual quantities. According to the principle of virtual work, the manipulator is at static It is worth remarking that the end-effector forces γ e ∈ N (J T ) are entirely equilibrium if and only if absorbed by the structure in that the mechanical constraint reaction forces can balance them exactly. Hence, a manipulator at a singular configuration δWτ = δWγ ∀δq, (3.110) remains in the given posture whatever end-effector force γ e is applied so that γ e ∈ N (J T ). i.e., the difference between the virtual work of the joint torques and the virtual The relations between the two subspaces are established by work of the end-effector forces must be null for all joint displacements. From (3.109), notice that the virtual work of the end-effector forces is N (J ) ≡ R⊥ (J T ) R(J ) ≡ N ⊥ (J T ) null for any displacement in the null space of J . This implies that the joint torques associated with such displacements must be null at static equilibrium. and then, once the manipulator Jacobian is known, it is possible to charac- Substituting (3.108), (3.109) into (3.110) leads to the notable result terize completely differential kinematics and statics in terms of the range and null spaces of the Jacobian and its transpose. τ = J T (q)γ e (3.111) On the basis of the above duality, the inverse kinematics scheme with the Jacobian transpose in Fig. 3.12 admits an interesting physical interpretation. stating that the relationship between the end-effector forces and the joint Consider a manipulator with ideal dynamics τ = q̇ (null masses and unit torques is established by the transpose of the manipulator geometric Jacobian. viscous friction coefficients); the algorithm update law q̇ = J T Ke plays the role of a generalized spring of stiffness constant K generating a force Ke that 3.8.1 Kineto-Statics Duality pulls the end-effector towards the desired posture in the operational space. If this manipulator is allowed to move, e.g., in the case Ke ∈ / N (J T ), the The statics relationship in (3.111), combined with the differential kinematics end-effector attains the desired posture and the corresponding joint variables equation in (3.45), points out a property of kineto-statics duality. In fact, by are determined. adopting a representation similar to that of Fig. 3.7 for differential kinematics, one has that (Fig. 3.21): 3.8.2 Velocity and Force Transformation • The range space of J T is the subspace R(J T ) in IRn of the joint torques The kineto-statics duality concept presented above can be useful to character- that can balance the end-effector forces, in the given manipulator posture. ize the transformation of velocities and forces between two coordinate frames. 16 Consider a reference coordinate frame O0 –x0 y0 z0 and a rigid body moving The angular displacement has been indicated by ω e dt in view of the problems of integrability of ω e discussed in Sect. 3.6. with respect to such a frame. Then let O1 –x1 y1 z1 and O2 –x2 y2 z2 be two 150 3 Differential Kinematics and Statics 3.8 Statics 151
giving the sought general relationship of velocity transformation between two
frames.
It may be observed that the transformation matrix in (3.113) plays the
role of a true Jacobian, since it characterizes a velocity transformation, and
thus (3.113) may be shortly written as
v 22 = J 21 v 11 . (3.114)
At this point, by virtue of the kineto-statics duality, the force transformation
between two frames can be directly derived in the form
γ 11 = J 21 T γ 22 (3.115)
which can be detailed into18
1
Fig. 3.22. Representation of linear and angular velocities in different coordinate f1 R12 O f 22 frames on the same rigid body = . (3.116) μ11 S(r 112 )R12 R12 μ22
coordinate frames attached to the body (Fig. 3.22). The relationships between Finally, notice that the above analysis is instantaneous in that, if a coordinate translational and rotational velocities of the two frames with respect to the frame varies with respect to the other, it is necessary to recompute the Jaco- reference frame are given by bian of the transformation through the computation of the related rotation matrix of one frame with respect to the other. ω2 = ω1 ṗ2 = ṗ1 + ω 1 × r 12 . 3.8.3 Closed Chain By exploiting the skew-symmetric operator S(·) in (3.9), the above relations As discussed in Sect. 2.8.3, whenever the manipulator contains a closed chain, can be compactly written as there is a functional relationship between the joint variables. In particular, ṗ2 I −S(r 12 ) ṗ1 the closed chain structure is transformed into a tree-structured open chain by = . (3.112) ω2 O I ω1 virtually cutting the loop at a joint. It is worth choosing such a cut joint as one of the unactuated joints. Then, the constraints (2.59) or (2.60) should be All vectors in (3.112) are meant to be referred to the reference frame O0 – solved for a reduced number of joint variables, corresponding to the DOFs of x0 y0 z0 . On the other hand, if vectors are referred to their own frames, it the chain. Therefore, it is reasonable to assume that at least such independent is joints are actuated, while the others may or may not be actuated. Let q o = r 12 = R1 r 112 T [ q Ta q Tu ] denote the vector of joint variables of the tree-structured open and also chain, where q a and q u are the vectors of actuated and unactuated joint ṗ1 = R1 ṗ11 ṗ2 = R2 ṗ22 = R1 R12 ṗ22 variables, respectively. Assume that from the above constraints it is possible ω 1 = R1 ω 11 ω 2 = R2 ω 22 = R1 R12 ω 22 . to determine a functional expression
Accounting for (3.112) and (3.11) gives q u = q u (q a ). (3.117) R1 R12 ṗ22 = R1 ṗ11 − R1 S(r 112 )RT1 R1 ω 11 Time differentiation of (3.117) gives the relationship between joint velocities R1 R12 ω 22 = R1 ω 11 . in the form q̇ o = Υ q̇ a (3.118) Eliminating the dependence on R1 , which is premultiplied to each term on both sides of the previous relations, yields17 where ⎡ ⎤ 2 2 1 I ṗ2 R1 −R21 S(r 112 ) ṗ1 Υ = ⎣ ∂q u ⎦ (3.119) 2 = 2 (3.113) ω2 O R1 ω 11 ∂q a 17 Recall that RT R = I, as in (2.4). 18 The skew-symmetry property S + S T = O is utilized. 152 3 Differential Kinematics and Statics 3.9 Manipulability Ellipsoids 153
is the transformation matrix between the two vectors of joint velocities, which be generated by the given set of joint velocities, with the manipulator in a in turn plays the role of a Jacobian. given posture. To this end, one can utilize the differential kinematics equation At this point, according to an intuitive kineto-statics duality concept, it is in (3.45) solved for the joint velocities; in the general case of a redundant ma- possible to describe the transformation between the corresponding vectors of nipulator (r < n) at a nonsingular configuration, the minimum-norm solution joint torques in the form q̇ = J † (q)v e can be considered which, substituted into (3.122), yields τa = ΥTτo (3.120) T v Te J †T (q)J † (q) v e = 1. where τ o = [ τ Ta τ Tu ] , with obvious meaning of the quantities. Accounting for the expression of the pseudo-inverse of J in (3.52) gives −1 v Te J (q)J T(q) v e = 1, (3.123) Example 3.5 Consider the parallelogram arm of Sect. 2.9.2. On the assumption to actuate the which is the equation of the points on the surface of an ellipsoid in the end- two Joints 1 and 1 at the base, it is q a = [ ϑ1 ϑ1 ]T and q u = [ ϑ2 ϑ3 ]T . effector velocity space. Then, using (2.64), the transformation matrix in (3.119) is The choice of the minimum-norm solution rules out the presence of internal ⎡ ⎤ motions for the redundant structure. If the general solution (3.54) is used for 1 0 q̇, the points satisfying (3.122) are mapped into points inside the ellipsoid ⎢ 0 1 ⎥ Υ =⎣ 1 ⎦ . whose surface is described by (3.123). −1 1 −1 For a nonredundant manipulator, the differential kinematics solution (3.47) is used to derive (3.123); in this case the points on the surface of the sphere in Hence, in view of (3.120), the torque vector of the actuated joints is the joint velocity space are mapped into points on the surface of the ellipsoid in the end-effector velocity space. τ 1 − τ 2 + τ 3 τa = τ1 + τ2 − τ3 (3.121) Along the direction of the major axis of the ellipsoid, the end-effector can move at large velocity, while along the direction of the minor axis small end- while obviously τ u = [ 0 0 ]T in agreement with the fact that both Joints 2 and 3 effector velocities are obtained. Further, the closer the ellipsoid is to a sphere are unactuated. — unit eccentricity — the better the end-effector can move isotropically along all directions of the operational space. Hence, it can be understood why this ellipsoid is an index characterizing manipulation ability of the structure in terms of velocities. As can be recognized from (3.123), the shape and orientation of the ellip- 3.9 Manipulability Ellipsoids soid are determined by the core of its quadratic form and then by the matrix J J T which is in general a function of the manipulator configuration. The The differential kinematics equation in (3.45) and the statics equation in directions of the principal axes of the ellipsoid are determined by the eigen- (3.111), together with the duality property, allow the definition of indices for vectors ui , for i = 1, … , r, of the matrix J J T , while the dimensions of the the evaluation of manipulator performance. Such indices can be helpful both axes are given by the singular values of J , σi = λi (J J T ), for i = 1, … , r, for mechanical manipulator design and for determining suitable manipulator where λi (J J ) denotes the generic eigenvalue of J J T . T postures to execute a given task in the current configuration. A global representative measure of manipulation ability can be obtained First, it is desired to represent the attitude of a manipulator to arbitrarily by considering the volume of the ellipsoid. This volume is proportional to the change end-effector position and orientation. This capability is described in quantity an effective manner by the velocity manipulability ellipsoid. Consider the set of joint velocities of constant (unit) norm w(q) = det J (q)J T (q)
q̇ T q̇ = 1; (3.122) which is the manipulability measure already introduced in (3.56). In the case
of a nonredundant manipulator (r = n), w reduces to
this equation describes the points on the surface of a sphere in the joint ve- # # locity space. It is desired to describe the operational space velocities that can w(q) = det J (q) # . (3.124) 154 3 Differential Kinematics and Statics 3.9 Manipulability Ellipsoids 155
2.5
1 max
2
0.5
1.5
[m]
[m]
0
1
Ŧ0.5 min
0.5
Ŧ1
0
0 0.5 1 1.5 2 0 0.5 1 1.5 2
[m] [m]
Fig. 3.23. Velocity manipulability ellipses for a two-link planar arm in different Fig. 3.24. Minimum and maximum singular values of J for a two-link planar arm postures as a function of the arm posture
It is easy to recognize that it is always w > 0, except for a manipulator at a singular configuration when w = 0. For this reason, this measure is usually 1
adopted as a distance of the manipulator from singular configurations. 0.5
[m]
0
Ŧ0.5
Example 3.6 Ŧ1 Consider the two-link planar arm. From the expression in (3.41), the manipulability measure is in this case 0 0.5 1 1.5 2 [m] w = |det(J )| = a1 a2 |s2 |. Therefore, as a function of the arm postures, the manipulability is maximum for Fig. 3.25. Force manipulability ellipses for a two-link planar arm in different pos- ϑ2 = ±π/2. On the other hand, for a given constant reach a1 + a2 , the structure tures offering the maximum manipulability, independently of ϑ1 and ϑ2 , is the one with a 1 = a2 . The manipulability measure w has the advantage of being easy to compute, These results have a biomimetic interpretation in the human arm, if that is regarded as a two-link arm (arm + forearm). The condition a1 = a2 is satisfied with through the determinant of matrix J J T . However, its numerical value does good approximation. Further, the elbow angle ϑ2 is usually in the neighbourhood of not constitute an absolute measure of the actual closeness of the manipulator π/2 in the execution of several tasks, such as that of writing. Hence, the human being to a singularity. It is enough to consider the above example and take two tends to dispose the arm in the most dexterous configuration from a manipulability arms of identical structure, one with links of 1 m and the other with links of viewpoint. 1 cm. Two different values of manipulability are obtained which differ by four Figure 3.23 illustrates the velocity manipulability ellipses for a certain number of orders of magnitude. Hence, in that case it is convenient to consider only |s2 | postures with the tip along the horizontal axis and a1 = a2 = 1. It can be seen that — eventually |ϑ2 | — as the manipulability measure. In more general cases when the arm is outstretched the ellipsoid is very thin along the vertical direction. when it is not easy to find a simple, meaningful index, one can consider the Hence, one recovers the result anticipated in the study of singularities that the arm ratio between the minimum and maximum singular values of the Jacobian in this posture can generate tip velocities preferably along the vertical direction. In σr /σ1 which is equivalent to the inverse of the condition number of matrix J . Fig. 3.24, moreover, the behaviour of the minimum and maximum singular values of This ratio gives not only a measure of the distance from a singularity (σr = 0), the matrix J is illustrated as a function of tip position along axis x; it can be verified that the minimum singular value is null when the manipulator is at a singularity but also a direct measure of eccentricity of the ellipsoid. The disadvantage in (retracted or outstretched). utilizing this index is its computational complexity; it is practically impossible Therefore, with reference to the postures, manipulability has a maximum for to compute it in symbolic form, i.e., as a function of the joint configuration, ϑ2 = ±π/2. On the other hand, for a given total extension a1 + a2 , the structure except for matrices of reduced dimension. which, independently of ϑ1 and ϑ2 , offers the largest manipulability is that with On the basis of the existing duality between differential kinematics and a 1 = a2 . statics, it is possible to describe the manipulability of a structure not only 156 3 Differential Kinematics and Statics 3.9 Manipulability Ellipsoids 157
with reference to velocities, but also with reference to forces. To be specific, one can consider the sphere in the space of joint torques
τTτ = 1 (3.125) velocity
which, accounting for (3.111), is mapped into the ellipsoid in the space of end-effector forces γ Te J (q)J T(q) γ e = 1 (3.126) writing plane force which is defined as the force manipulability ellipsoid. This ellipsoid character- izes the end-effector forces that can be generated with the given set of joint torques, with the manipulator in a given posture. Fig. 3.26. Velocity and force manipulability ellipses for a 3-link planar arm in a As can be easily recognized from (3.126), the core of the quadratic form is typical configuration for a task of controlling force and velocity constituted by the inverse of the matrix core of the velocity ellipsoid in (3.123). This feature leads to the notable result that the principal axes of the force and for the velocity manipulability ellipsoid as manipulability ellipsoid coincide with the principal axes of the velocity manip- ulability ellipsoid, while the dimensions of the respective axes are in inverse −1/2 −1 proportion. Therefore, according to the concept of force/velocity duality, a β(q) = uT J (q)J T (q) u . (3.128) direction along which good velocity manipulability is obtained is a direction along which poor force manipulability is obtained, and vice versa. The manipulability ellipsoids can be conveniently utilized not only for an- In Fig. 3.25, the manipulability ellipses for the same postures as those alyzing manipulability of the structure along different directions of the opera- of the example in Fig. 3.23 are illustrated. A comparison of the shape and tional space, but also for determining compatibility of the structure to execute orientation of the ellipses confirms the force/velocity duality effect on the a task assigned along a direction. To this end, it is useful to distinguish be- manipulability along different directions. tween actuation tasks and control tasks of velocity and force. In terms of the It is worth pointing out that these manipulability ellipsoids can be repre- relative ellipsoid, the task of actuating a velocity (force) requires preferably sented geometrically in all cases of an operational space of dimension at most a large transformation ratio along the task direction, since for a given set of 3. Therefore, if it is desired to analyze manipulability in a space of greater joint velocities (forces) at the joints it is possible to generate a large velocity dimension, it is worth separating the components of linear velocity (force) (force) at the end-effector. On the other hand, for a control task it is impor- from those of angular velocity (moment), also avoiding problems due to non- tant to have a small transformation ratio so as to gain good sensitivity to homogeneous dimensions of the relevant quantities (e.g., m/s vs rad/s). For errors that may occur along the given direction. instance, for a manipulator with a spherical wrist, the manipulability analysis Revisiting once again the duality between velocity manipulability ellipsoid is naturally prone to a decoupling between arm and wrist. and force manipulability ellipsoid, it can be found that an optimal direction to An effective interpretation of the above results can be achieved by regard- actuate a velocity is also an optimal direction to control a force. Analogously, ing the manipulator as a mechanical transformer of velocities and forces from a good direction to actuate a force is also a good direction to control a velocity. the joint space to the operational space. Conservation of energy dictates that To have a tangible example of the above concept, consider the typical task an amplification in the velocity transformation is necessarily accompanied by of writing on a horizontal surface for the human arm; this time, the arm is re- a reduction in the force transformation, and vice versa. The transformation garded as a 3-link planar arm: arm + forearm + hand. Restricting the analysis ratio along a given direction is determined by the intersection of the vector to a two-dimensional task space (the direction vertical to the surface and the along that direction with the surface of the ellipsoid. Once a unit vector u direction of the line of writing), one has to achieve fine control of the vertical along a direction has been assigned, it is possible to compute the transforma- force (the pressure of the pen on the paper) and of the horizontal velocity (to tion ratio for the force manipulability ellipsoid as write in good calligraphy). As a consequence, the force manipulability ellipse −1/2 tends to be oriented horizontally for correct task execution. Correspondingly, α(q) = uT J (q)J T (q)u (3.127) the velocity manipulability ellipse tends to be oriented vertically in perfect agreement with the task requirement. In this case, from Fig. 3.26 the typical configuration of the human arm when writing can be recognized. 158 3 Differential Kinematics and Statics Problems 159
is presented in [163]; other approaches based on the concept of augmented
task space are presented in [14, 69, 199, 203, 194, 37]. For global redundancy
resolutions see [162]. A complete treatment of redundant manipulators can be
force found in [160] while a tutorial is [206].
The extension of inverse kinematics to the second order has been proposed
velocity
in [207], while the symbolic differentiation of the solutions in terms of joint
velocities to obtain stable acceleration solutions can be found in [208]. Further
throwing direction
details about redundancy resolution are in [59].
The concepts of kineto-statics duality are discussed in [191]. The manipu-
lability ellipsoids are proposed in [245, 248] and employed in [44] for posture
Fig. 3.27. Velocity and force manipulability ellipses for a 3-link planar arm in a dexterity analysis with regard to manipulation tasks. typical configuration for a task of actuating force and velocity
An opposite example to the previous one is that of the human arm when Problems
throwing a weight in the horizontal direction. In fact, now it is necessary to actuate a large vertical force (to sustain the weight) and a large horizontal 3.1. Prove (3.11). velocity (to throw the load for a considerable distance). Unlike the above, the 3.2. Compute the Jacobian of the cylindrical arm in Fig. 2.35. force (velocity) manipulability ellipse tends to be oriented vertically (horizon- tally) to successfully execute the task. The relative configuration in Fig. 3.27 3.3. Compute the Jacobian of the SCARA manipulator in Fig. 2.36. is representative of the typical attitude of the human arm when, for instance, releasing the ball in a bowling game. 3.4. Find the singularities of the 3-link planar arm in Fig. 2.20. In the above two examples, it is worth pointing out that the presence of a 3.5. Find the singularities of the spherical arm in Fig. 2.22. two-dimensional operational space is certainly advantageous to try reconfig- uring the structure in the best configuration compatible with the given task. 3.6. Find the singularities of the cylindrical arm in Fig. 2.35. In fact, the transformation ratios defined in (3.127) and (3.128) are scalar functions of the manipulator configurations that can be optimized locally ac- 3.7. Find the singularities of the SCARA manipulator in Fig. 2.36. cording to the technique for exploiting redundant DOFs previously illustrated. 3.8. Show that the manipulability measure defined in (3.56) is given by the product of the singular values of the Jacobian matrix. Bibliography 3.9. For the 3-link planar arm in Fig. 2.20, find an expression of the distance of the arm from a circular obstacle of given radius and coordinates. The concept of geometric Jacobian was originally introduced in [240] and the problem of its computationally efficient determination is considered in [173]. 3.10. Find the solution to the differential kinematics equation with the The concept of analytical Jacobian is presented in [114] with reference to damped least-square inverse in (3.59). operational space control. Inverse differential kinematics dates back to [240] under the name of re- 3.11. Prove (3.64) in an alternative way, i.e., by computing S(ω e ) as in (3.6) solved rate control. The use of the Jacobian pseudo-inverse is due to [118]. starting from R(φ) in (2.18). The adoption of the damped least-squares inverse has been independently 3.12. With reference to (3.64), find the transformation matrix T (φe ) in the proposed by [161] and [238]; a tutorial on the topic is [42]. The inverse case of RPY angles. kinematics algorithm based on the Jacobian transpose has been originally proposed in [198, 16]. Further details about the orientation error are found 3.13. With reference to (3.64), find the triplet of Euler angles for which in [142, 250, 132, 41]. T (0) = I. The utilization of the joint velocities in the null space of the Jacobian for redundancy resolution is proposed in [129] and further refined in [147] regard- 3.14. Show how the inverse kinematics scheme of Fig. 3.11 can be simplified ing the choice of the objective functions. The approach based on task priority in the case of a manipulator having a spherical wrist. 160 3 Differential Kinematics and Statics
3.15. Find an expression of the upper bound on the norm of e for the solu- tion (3.76) in the case ẋd = 0. 4 3.16. Prove (3.81).
3.17. Prove (3.86), (3.87). Trajectory Planning 3.18. Prove that the equation relating the angular velocity to the time deriva- tive of the quaternion is given by
ω = 2S()˙ + 2η ˙ − 2η̇.
[Hint: Start by showing that (2.33) can be rewritten as R(η, ) = (2η 2 − 1)I + 2T + 2ηS()].
3.19. Prove (3.94), (3.95). For the execution of a specific robot task, it is worth considering the main 3.20. Prove that the time derivative of the Lyapunov function in (3.96) is features of motion planning algorithms. The goal of trajectory planning is to given by (3.97). generate the reference inputs to the motion control system which ensures that the manipulator executes the planned trajectories. The user typically specifies 3.21. Consider the 3-link planar arm in Fig. 2.20, whose link lengths are a number of parameters to describe the desired trajectory. Planning consists of respectively 0.5 m, 0.3 m, 0.3 m. Perform a computer implementation of the generating a time sequence of the values attained by an interpolating function inverse kinematics algorithm using the Jacobian pseudo-inverse along the op- (typically a polynomial) of the desired trajectory. This chapter presents some erational space path given by a straight line connecting the points of coordi- techniques for trajectory generation, both in the case when the initial and nates (0.8, 0.2) m and (0.8, −0.2) m. Add a constraint aimed at avoiding link final point of the path are assigned (point-to-point motion), and in the case collision with a circular object located at ø = [ 0.3 0 ]T m of radius 0.1 m. The when a finite sequence of points are assigned along the path (motion through initial arm configuration is chosen so that pe (0) = pd (0). The final time is a sequence of points). First, the problem of trajectory planning in the joint 2 s. Use sinusoidal motion timing laws. Adopt the Euler numerical integration space is considered, and then the basic concepts of trajectory planning in scheme (3.48) with an integration time Δt = 1 ms. the operational space are illustrated. The treatment of the motion planning problem for mobile robots is deferred to Chap. 12. 3.22. Consider the SCARA manipulator in Fig. 2.36, whose links both have a length of 0.5 m and are located at a height of 1 m from the supporting plane. Perform a computer implementation of the inverse kinematics algorithms with 4.1 Path and Trajectory both Jacobian inverse and Jacobian transpose along the operational space path whose position is given by a straight line connecting the points of co- The minimal requirement for a manipulator is the capability to move from ordinates (0.7, 0, 0) m and (0, 0.8, 0.5) m, and whose orientation is given by an initial posture to a final assigned posture. The transition should be char- a rotation from 0 rad to π/2 rad. The initial arm configuration is chosen so acterized by motion laws requiring the actuators to exert joint generalized that xe (0) = xd (0). The final time is 2 s. Use sinusoidal motion timing laws. forces which do not violate the saturation limits and do not excite the typi- Adopt the Euler numerical integration scheme (3.48) with an integration time cally modelled resonant modes of the structure. It is then necessary to devise Δt = 1 ms. planning algorithms that generate suitably smooth trajectories. In order to avoid confusion between terms often used as synonyms, the 3.23. Prove that the directions of the principal axes of the force and velocity difference between a path and a trajectory is to be explained. A path denotes manipulability ellipsoids coincide while their dimensions are in inverse pro- the locus of points in the joint space, or in the operational space, which the portion. manipulator has to follow in the execution of the assigned motion; a path is then a pure geometric description of motion. On the other hand, a trajectory is a path on which a timing law is specified, for instance in terms of velocities and/or accelerations at each point. 162 4 Trajectory Planning 4.2 Joint Space Trajectories 163
In principle, it can be conceived that the inputs to a trajectory planning The planning algorithm generates a function q(t) interpolating the given
algorithm are the path description, the path constraints, and the constraints vectors of joint variables at each point, in respect of the imposed constraints. imposed by manipulator dynamics, whereas the outputs are the end-effector In general, a joint space trajectory planning algorithm is required to have trajectories in terms of a time sequence of the values attained by position, the following features: velocity and acceleration. • the generated trajectories should be not very demanding from a compu- A geometric path cannot be fully specified by the user for obvious com- tational viewpoint, plexity reasons. Typically, a reduced number of parameters is specified such • the joint positions and velocities should be continuous functions of time as extremal points, possible intermediate points, and geometric primitives in- (continuity of accelerations may be imposed, too), terpolating the points. Also, the motion timing law is not typically specified • undesirable effects should be minimized, e.g., nonsmooth trajectories in- at each point of the geometric path, but rather it regards the total trajectory terpolating a sequence of points on a path. time, the constraints on the maximum velocities and accelerations, and even- tually the assignment of velocity and acceleration at points of particular inter- At first, the case is examined when only the initial and final points on est. On the basis of the above information, the trajectory planning algorithm the path and the traveling time are specified (point-to-point); the results are generates a time sequence of variables that describe end-effector position and then generalized to the case when also intermediate points along the path are orientation over time in respect of the imposed constraints. Since the control specified (motion through a sequence of points). Without loss of generality, action on the manipulator is carried out in the joint space, a suitable inverse the single joint variable q(t) is considered. kinematics algorithm is to be used to reconstruct the time sequence of joint variables corresponding to the above sequence in the operational space. 4.2.1 Point-to-Point Motion Trajectory planning in the operational space naturally allows the presence of path constraints to be accounted; these are due to regions of workspace In point-to-point motion, the manipulator has to move from an initial to a which are forbidden to the manipulator, e.g., due to the presence of obstacles. final joint configuration in a given time tf . In this case, the actual end-effector In fact, such constraints are typically better described in the operational space, path is of no concern. The algorithm should generate a trajectory which, in since their corresponding points in the joint space are difficult to compute. respect to the above general requirements, is also capable of optimizing some With regard to motion in the neighbourhood of singular configurations and performance index when the joint is moved from one position to another. presence of redundant DOFs, trajectory planning in the operational space may A suggestion for choosing the motion primitive may stem from the analysis involve problems difficult to solve. In such cases, it may be advisable to specify of an incremental motion problem. Let I be the moment of inertia of a rigid the path in the joint space, still in terms of a reduced number of parameters. body about its rotation axis. It is required to take the angle q from an initial Hence, a time sequence of joint variables has to be generated which satisfy value qi to a final value qf in a time tf . It is obvious that infinite solutions the constraints imposed on the trajectory. exist to this problem. Assumed that rotation is executed through a torque τ For the sake of clarity, in the following, the case of joint space trajectory supplied by a motor, a solution can be found which minimizes the energy dis- planning is treated first. The results will then be extended to the case of sipated in the motor. This optimization problem can be formalized as follows. trajectories in the operational space. Having set q̇ = ω, determine the solution to the differential equation I ω̇ = τ 4.2 Joint Space Trajectories subject to the condition ” tf A manipulator motion is typically assigned in the operational space in terms ω(t)dt = qf − qi of trajectory parameters such as the initial and final end-effector pose, possi- o ble intermediate poses, and travelling time along particular geometric paths. so as to minimize the performance index If it is desired to plan a trajectory in the joint space, the values of the joint ” tf variables have to be determined first from the end-effector position and ori- τ 2 (t)dt. entation specified by the user. It is then necessary to resort to an inverse 0 kinematics algorithm, if planning is done off-line, or to directly measure the It can be shown that the resulting solution is of the type above variables, if planning is done by the teaching-by-showing technique (see Chap. 6). ω(t) = at2 + bt + c. 164 4 Trajectory Planning 4.2 Joint Space Trajectories 165
Even though the joint dynamics cannot be described in the above simple pos manner,1 the choice of a third-order polynomial function to generate a joint trajectory represents a valid solution for the problem at issue. 3
Therefore, to determine a joint motion, the cubic polynomial
2
[rad]
3 2
q(t) = a3 t + a2 t + a1 t + a0 (4.1) 1
can be chosen, resulting into a parabolic velocity profile 0
q̇(t) = 3a3 t2 + 2a2 t + a1 0 0.2 0.4 0.6 0.8 1
[s]
and a linear acceleration profile vel 5
q̈(t) = 6a3 t + 2a2 . 4
3
Since four coefficients are available, it is possible to impose, besides the initial
[rad/s]
and final joint position values qi and qf , also the initial and final joint velocity 2
values q̇i and q̇f which are usually set to zero. Determination of a specific 1 trajectory is given by the solution to the following system of equations: 0 0 0.2 0.4 0.6 0.8 1 a0 = qi [s] a1 = q̇i acc a3 t3f + a2 t2f + a1 tf + a0 = qf 20 3a3 t2f + 2a2 tf + a1 = q̇f , 10
[rad/s^2]
2
that allows the computation of the coefficients of the polynomial in (4.1). 0 Figure 4.1 illustrates the timing law obtained with the following data: qi = 0, Ŧ10 qf = π, tf = 1, and q̇i = q̇f = 0. As anticipated, velocity has a parabolic pro- file, while acceleration has a linear profile with initial and final discontinuity. Ŧ20 If it is desired to assign also the initial and final values of acceleration, six 0 0.2 0.4 0.6 0.8 1 constraints have to be satisfied and then a polynomial of at least fifth order [s] is needed. The motion timing law for the generic joint is then given by Fig. 4.1. Time history of position, velocity and acceleration with a cubic polynomial 5 4 3 2 timing law q(t) = a5 t + a4 t + a3 t + a2 t + a1 t + a0 , (4.2)
whose coefficients can be computed, as for the previous case, by imposing the of whether the resulting velocities and accelerations can be supported by the conditions for t = 0 and t = tf on the joint variable q(t) and on its first physical mechanical manipulator. two derivatives. With the choice (4.2), one obviously gives up minimizing the In this case, a trapezoidal velocity profile is assigned, which imposes a above performance index. constant acceleration in the start phase, a cruise velocity, and a constant An alternative approach with timing laws of blended polynomial type is deceleration in the arrival phase. The resulting trajectory is formed by a linear frequently adopted in industrial practice, which allows a direct verification segment connected by two parabolic segments to the initial and final positions. In the following, the problem is formulated by assuming that the final time 1 In fact, recall that the moment of inertia about the joint axis is a function of of trajectory duration has been assigned. However, in industrial practice, the manipulator configuration. user is offered the option to specify the velocity percentage with respect to the 2 Notice that it is possible to normalize the computation of the coefficients, so as maximum allowable velocity; this choice is aimed at avoiding occurrences when to be independent both on the final time tf and on the path length |qf − qi |. 166 4 Trajectory Planning 4.2 Joint Space Trajectories 167
Usually, q̈c is specified with the constraint that sgn q̈c = sgn (qf − qi ); hence,
for given tf , qi and qf , the solution for tc is computed from (4.5) as (tc ≤ tf /2)
$
tf 1 t2f q̈c − 4(qf − qi )
tc = − . (4.6)
2 2 q̈c
Acceleration is then subject to the constraint
4|qf − qi |
|q̈c | ≥ . (4.7)
t2f
When the acceleration q̈c is chosen so as to satisfy (4.7) with the equality
sign, the resulting trajectory does not feature the constant velocity segment
any more and has only the acceleration and deceleration segments (triangular
profile).
Given qi , qf and tf , and thus also an average transition velocity, the con-
straint in (4.7) allows the imposition of a value of acceleration consistent with
the trajectory. Then, tc is computed from (4.6), and the following sequence of
polynomials is generated:
Fig. 4.2. Characterization of a timing law with trapezoidal velocity profile in terms ⎧ 1 2 ⎪ ⎨ qi + 2 q̈c t 0 ≤ t ≤ tc of position, velocity and acceleration q(t) = qi + q̈c tc (t − tc /2) tc < t ≤ t f − tc (4.8) ⎪ ⎩ qf − 12 q̈c (tf − t)2 tf − tc < t ≤ tf . the specification of a much too short motion duration would involve much too large values of velocities and/or accelerations, beyond those achievable by the Figure 4.3 illustrates a representation of the motion timing law obtained by manipulator. imposing the data: qi = 0, qf = π, tf = 1, and |q̈c | = 6π. As can be seen from the velocity profiles in Fig. 4.2, it is assumed that both Specifying acceleration in the parabolic segment is not the only way to initial and final velocities are null and the segments with constant accelerations determine trajectories with trapezoidal velocity profile. Besides qi , qf and tf , have the same time duration; this implies an equal magnitude q̈c in the two one can specify also the cruise velocity q̇c which is subject to the constraint segments. Notice also that the above choice leads to a symmetric trajectory |qf − qi | 2|qf − qi | with respect to the average point qm = (qf + qi )/2 at tm = tf /2. < |q̇c | ≤ . (4.9) tf tf The trajectory has to satisfy some constraints to ensure the transition from qi to qf in a time tf . The velocity at the end of the parabolic segment By recognizing that q̇c = q̈c tc , (4.5) allows the computation of tc as must be equal to the (constant) velocity of the linear segment, i.e., qi − qf + q̇c tf tc = , (4.10) qm − qc q̇c q̈c tc = (4.3) tm − tc and thus the resulting acceleration is where qc is the value attained by the joint variable at the end of the parabolic q̇c2 segment at time tc with constant acceleration q̈c (recall that q̇(0) = 0). It is q̈c = . (4.11) qi − qf + q̇c tf then 1 qc = qi + q̈c t2c . (4.4) The computed values of tc and q̈c as in (4.10), (4.11) allow the generation of 2 the sequence of polynomials expressed by (4.8). Combining (4.3), (4.4) gives The adoption of a trapezoidal velocity profile results in a worse perfor- mance index compared %t to the cubic polynomial. The decrease is, however, q̈c t2c − q̈c tf tc + qf − qi = 0. (4.5) limited; the term 0 f τ 2 dt increases by 12.5% with respect to the optimal case. 168 4 Trajectory Planning 4.2 Joint Space Trajectories 169
pos
3
2
[rad]
1
0
0 0.2 0.4 0.6 0.8 1
[s]
vel
5
4
Fig. 4.4. Characterization of a trajectory on a given path obtained through inter-
3 polating polynomials
[rad/s]
2
1 or a high path curvature is expected. It should not be forgotten that the
0 corresponding joint variables have to be computed from the operational space
0 0.2 0.4 0.6 0.8 1 poses.
[s] Therefore, the problem is to generate a trajectory when N points, termed
acc path points, are specified and have to be reached by the manipulator at certain
20
instants of time. For each joint variable there are N constraints, and then one
might want to use an (N − 1)-order polynomial. This choice, however, has the
10 following disadvantages:
[rad/s^2]
0
• It is not possible to assign the initial and final velocities.
Ŧ10 • As the order of a polynomial increases, its oscillatory behaviour increases,
and this may lead to trajectories which are not natural for the manipulator.
Ŧ20
• Numerical accuracy for computation of polynomial coefficients decreases
0 0.2 0.4 0.6 0.8 1
[s]
as order increases.
• The resulting system of constraint equations is heavy to solve.
Fig. 4.3. Time history of position, velocity and acceleration with a trapezoidal • Polynomial coefficients depend on all the assigned points; thus, if it is velocity profile timing law desired to change a point, all of them have to be recomputed.
These drawbacks can be overcome if a suitable number of low-order inter-
4.2.2 Motion Through a Sequence of Points polating polynomials, continuous at the path points, are considered in place of a single high-order polynomial. In several applications, the path is described in terms of a number of points According to the previous section, the interpolating polynomial of lowest greater than two. For instance, even for the simple point-to-point motion order is the cubic polynomial, since it allows the imposition of continuity of of a pick-and-place task, it may be worth assigning two intermediate points velocities at the path points. With reference to the single joint variable, a between the initial point and the final point; suitable positions can be set for function q(t) is sought, formed by a sequence of N − 1 cubic polynomials lifting off and setting down the object, so that reduced velocities are obtained Πk (t), for k = 1, … , N − 1, continuous with continuous first derivatives. The with respect to direct transfer of the object. For more complex applications, function q(t) attains the values qk for t = tk (k = 1, … , N ), and q1 = qi , it may be convenient to assign a sequence of points so as to guarantee better t1 = 0, qN = qf , tN = tf ; the qk ’s represent the path points describing monitoring on the executed trajectories; the points are to be specified more densely in those segments of the path where obstacles have to be avoided 170 4 Trajectory Planning 4.2 Joint Space Trajectories 171 pos the desired trajectory at t = tk (Fig. 4.4). The following situations can be considered: 6 • Arbitrary values of q̇(t) are imposed at the path points. • The values of q̇(t) at the path points are assigned according to a certain 4
[rad]
criterion. 2 • The acceleration q̈(t) has to be continuous at the path points. 0 To simplify the problem, it is also possible to find interpolating polynomials of order less than three which determine trajectories passing nearby the path 0 2 4 6 [s] points at the given instants of time. vel
5
Interpolating polynomials with imposed velocities at path points
This solution requires the user to be able to specify the desired velocity at 0
[rad/s]
each path point; the solution does not possess any novelty with respect to the above concepts. Ŧ5 The system of equations allowing computation of the coefficients of the N − 1 cubic polynomials interpolating the N path points is obtained by im- posing the following conditions on the generic polynomial Πk (t) interpolating 0 2 4 6 [s] qk and qk+1 , for k = 1, … , N − 1: acc Πk (tk ) = qk 20 Πk (tk+1 ) = qk+1 Π̇k (tk ) = q̇k
[rad/s^2]
0
Π̇k (tk+1 ) = q̇k+1 .
Ŧ20
The result is N − 1 systems of four equations in the four unknown coefficients of the generic polynomial; these can be solved one independently of the other. Ŧ40 The initial and final velocities of the trajectory are typically set to zero (q̇1 = 0 2 4 6 [s] q̇N = 0) and continuity of velocity at the path points is ensured by setting Fig. 4.5. Time history of position, velocity and acceleration with a timing law of Π̇k (tk+1 ) = Π̇k+1 (tk+1 ) interpolating polynomials with velocity constraints at path points
for k = 1, … , N − 2. q̇1 = 0 Figure 4.5 illustrates the time history of position, velocity and acceleration obtained with the data: q1 = 0, q2 = 2π, q3 = π/2, q4 = π, t1 = 0, t2 = 2, t3 = 0 sgn (vk ) = sgn (vk+1 ) q̇k = 1 (4.12) 3, t4 = 5, q̇1 = 0, q̇2 = π, q̇3 = −π, q̇4 = 0. Notice the resulting discontinuity 2 (vk + vk+1 ) sgn (vk ) = sgn (vk+1 ) on the acceleration, since only continuity of velocity is guaranteed. q̇N = 0, where vk = (qk − qk−1 )/(tk − tk−1 ) gives the slope of the segment in the Interpolating polynomials with computed velocities at path points time interval [tk−1 , tk ]. With the above settings, the determination of the interpolating polynomials is reduced to the previous case. In this case, the joint velocity at a path point has to be computed according Figure 4.6 illustrates the time history of position, velocity and acceleration to a certain criterion. By interpolating the path points with linear segments, obtained with the following data: q1 = 0, q2 = 2π, q3 = π/2, q4 = π, t1 = 0, the relative velocities can be computed according to the following rules: t2 = 2, t3 = 3, t4 = 5, q̇1 = 0, q̇4 = 0. It is easy to recognize that the imposed 172 4 Trajectory Planning 4.2 Joint Space Trajectories 173
pos continuity of velocity and acceleration. The following equations have then to
be satisfied:
6
Πk−1 (tk ) = qk
4 Πk−1 (tk ) = Πk (tk )
[rad]
2 Π̇k−1 (tk ) = Π̇k (tk )
Π̈k−1 (tk ) = Π̈k (tk ).
0
The resulting system for the N path points, including the initial and final
0 2 4 6
[s] points, cannot be solved. In fact, it is formed by 4(N − 2) equations for the
vel intermediate points and 6 equations for the extremal points; the position
constraints for the polynomials Π0 (t1 ) = qi and ΠN (tf ) = qf have to be
5 excluded since they are not defined. Also, Π̇0 (t1 ), Π̈0 (t1 ), Π̇N (tf ), Π̈N (tf ) do
not have to be counted as polynomials since they are just the imposed values
of initial and final velocities and accelerations. In summary, one has 4N − 2
[rad/s]
0
equations in 4(N − 1) unknowns.
Ŧ5 The system can be solved only if one eliminates the two equations which
allow the arbitrary assignment of the initial and final acceleration values.
Fourth-order polynomials should be used to include this possibility for the
0 2 4 6
[s] first and last segment.
acc
On the other hand, if only third-order polynomials are to be used, the fol-
lowing deception can be operated. Two virtual points are introduced for which
30 continuity constraints on position, velocity and acceleration can be imposed,
20 without specifying the actual positions, though. It is worth remarking that the
10
[rad/s^2]
effective location of these points is irrelevant, since their position constraints
0
regard continuity only. Hence, the introduction of two virtual points implies
Ŧ10
the determination of N + 1 cubic polynomials.
Ŧ20
Consider N + 2 time instants tk , where t2 and tN +1 conventionally refer to
Ŧ30
the virtual points. The system of equations for determining the N + 1 cubic
0 2 4 6 polynomials can be found by taking the 4(N − 2) equations:
[s]
Fig. 4.6. Time history of position, velocity and acceleration with a timing law of Πk−1 (tk ) = qk (4.13) interpolating polynomials with computed velocities at path points Πk−1 (tk ) = Πk (tk ) (4.14) Π̇k−1 (tk ) = Π̇k (tk ) (4.15) sequence of path points leads to having zero velocity at the intermediate Π̈k−1 (tk ) = Π̈k (tk ) (4.16) points. for k = 3, … , N , written for the N − 2 intermediate path points, the 6 equa- tions: Interpolating polynomials with continuous accelerations at path points (splines) Π1 (t1 ) = qi (4.17) Both the above two solutions do not ensure continuity of accelerations at Π̇1 (t1 ) = q̇i (4.18) the path points. Given a sequence of N path points, the acceleration is also Π̈1 (t1 ) = q̈i , (4.19) continuous at each tk if four constraints are imposed, namely, two position constraints for each of the adjacent cubics and two constraints guaranteeing ΠN +1 (tN +2 ) = qf (4.20) 174 4 Trajectory Planning 4.2 Joint Space Trajectories 175
Π̇N +1 (tN +2 ) = q̇f (4.21) ..
.
Π̈N +1 (tN +2 ) = q̈f (4.22)
Π̇N (tN +1 ) = Π̇N +1 (tN +1 ).
written for the initial and final points, and the 6 equations: Time-differentiation of (4.27) gives both Π̇k (tk+1 ) and Π̇k+1 (tk+1 ) for k = Πk−1 (tk ) = Πk (tk ) (4.23) 1, … , N , and thus it is possible to write a system of linear equations of the kind Π̇k−1 (tk ) = Π̇k (tk ) (4.24) T A [ Π̈2 (t2 ) … Π̈N +1 (tN +1 ) ] = b (4.28) Π̈k−1 (tk ) = Π̈k (tk ) (4.25) which presents a vector b of known terms and a nonsingular coefficient matrix for k = 2, N + 1, written for the two virtual points. The resulting system A; the solution to this system always exists and is unique. It can be shown has 4(N + 1) equations in 4(N + 1) unknowns, that are the coefficients of the that the matrix A has a tridiagonal band structure of the type N + 1 cubic polynomials. ⎡ ⎤ a11 a12 … 0 0 The solution to the system is computationally demanding, even for low ⎢ a21 a22 … 0 0 ⎥ values of N . Nonetheless, the problem can be cast in a suitable form so as ⎢ … .. ⎥ to solve the resulting system of equations with a computationally efficient A=⎢ ⎢ … … ⎥, ⎥ ⎣ 0 0 … aN −1,N −1 aN −1,N ⎦ algorithm. Since the generic polynomial Πk (t) is a cubic, its second derivative must be a linear function of time which then can be written as 0 0 … aN,N −1 aN N
Π̈k (tk ) Π̈k (tk+1 ) which simplifies the solution to the system (see Problem 4.4). This matrix
Π̈k (t) = (tk+1 − t) + (t − tk ) k = 1, … , N + 1, (4.26) Δtk Δtk is the same for all joints, since it depends only on the time intervals Δtk specified. where Δtk = tk+1 − tk indicates the time interval to reach qk+1 from qk . By An efficient solution algorithm exists for the above system which is given integrating (4.26) twice over time, the generic polynomial can be written as by a forward computation followed by a backward computation. From the first equation, Π̈2 (t2 ) can be computed as a function of Π̈3 (t3 ) and then Π̈k (tk ) Π̈k (tk+1 ) Πk (t) = (tk+1 − t)3 + (t − tk )3 (4.27) substituted in the second equation, which then becomes an equation in the 6Δtk 6Δtk unknowns Π̈3 (t3 ) and Π̈4 (t4 ). This is carried out forward by transforming all & ’ Πk (tk+1 ) Δtk Π̈k (tk+1 ) the equations in equations with two unknowns, except the last one which will + − (t − tk ) have Π̈N +1 (tN +1 ) only as unknown. At this point, all the unknowns can be Δtk 6 & ’ determined step by step through a backward computation. Πk (tk ) Δtk Π̈k (tk ) The above sequence of cubic polynomials is termed spline to indicate + − (tk+1 − t) k = 1, … , N + 1, Δtk 6 smooth functions that interpolate a sequence of given points ensuring con- tinuity of the function and its derivatives. which depends on the 4 unknowns: Πk (tk ), Πk (tk+1 ), Π̈k (tk ), Π̈k (tk+1 ). Figure 4.7 illustrates the time history of position, velocity and acceleration Notice that the N variables qk for k = 2, N + 1 are given via (4.13), while obtained with the data: q1 = 0, q3 = 2π, q4 = π/2, q6 = π, t1 = 0, t3 = 2, continuity is imposed for q2 and qN +1 via (4.23). By using (4.14), (4.17), t4 = 3, t6 = 5, q̇1 = 0, q̇6 = 0. Two different pairs of virtual points were (4.20), the unknowns in the N + 1 equations in (4.27) reduce to 2(N + 2). considered at the time instants: t2 = 0.5, t5 = 4.5 (solid line in the figure), By observing that the equations in (4.18), (4.21) depend on q2 and qN +1 , and and t2 = 1.5, t5 = 3.5 (dashed line in the figure), respectively. Notice the that q̇i and q̇f are given, q2 and qN +1 can be computed as a function of Π̈1 (t1 ) parabolic velocity profile and the linear acceleration profile. Further, for the and Π̈N +1 (tN +2 ), respectively. Thus, a number of 2(N + 1) unknowns are left. second pair, larger values of acceleration are obtained, since the relative time By accounting for (4.16), (4.25), and noticing that in ((4.19), (4.22) q̈i and instants are closer to those of the two intermediate points. q̈f are given, the unknowns reduce to N . At this point, (4.15), (4.24) can be utilized to write the system of N Interpolating linear polynomials with parabolic blends equations in N unknowns: A simplification in trajectory planning can be achieved as follows. Consider Π̇1 (t2 ) = Π̇2 (t2 ) the case when it is desired to interpolate N path points q1 , … , qN at time 176 4 Trajectory Planning 4.2 Joint Space Trajectories 177
pos
6
4
[rad]
2
0
0 1 2 3 4 5
[s]
vel
5
Fig. 4.8. Characterization of a trajectory with interpolating linear polynomials with
parabolic blends
[rad/s]
0
Ŧ5 are assumed to be given. Velocity and acceleration for the intermediate points
are computed as
0 1 2 3 4 5
[s] qk − qk−1
q̇k−1,k = (4.29)
acc Δtk−1
30 q̇k,k+1 − q̇k−1,k
q̈k = ; (4.30)
20 Δtk
10
[rad/s^2]
0 these equations are straightforward.
Ŧ10 The first and last segments deserve special care. In fact, if it is desired to
Ŧ20 maintain the coincidence of the trajectory with the first and last segments,
Ŧ30 at least for a portion of time, the resulting trajectory has a longer duration
given by tN − t1 + (Δt1 + ΔtN )/2, where q̇0,1 = q̇N,N +1 = 0 has been imposed
0 1 2 3 4 5
[s]
for computing initial and final accelerations.
Notice that q(t) reaches none of the path points qk but passes nearby
Fig. 4.7. Time history of position, velocity and acceleration with a timing law of (Fig. 4.8). In this situation, the path points are more appropriately termed cubic splines for two different pairs of virtual points via points; the larger the blending acceleration, the closer the passage to a via point. instants t1 , … , tN with linear segments. To avoid discontinuity problems on On the basis of the given qk , Δtk and Δtk , the values of q̇k−1,k and q̈k the first derivative at the time instants tk , the function q(t) must have a are computed via (4.29), (4.30) and a sequence of linear polynomials with parabolic profile (blend ) around tk ; as a consequence, the entire trajectory is parabolic blends is generated. Their expressions as a function of time are not composed of a sequence of linear and quadratic polynomials, which in turn derived here to avoid further loading of the analytic presentation. implies that a discontinuity on q̈(t) is tolerated. Figure 4.9 illustrates the time history of position, velocity and acceleration Then let Δtk = tk+1 − tk be the time distance between qk and qk+1 , and obtained with the data: q1 = 0, q2 = 2π, q3 = π/2, q4 = π, t1 = 0, t2 = 2, Δtk,k+1 be the time interval during which the trajectory interpolating qk and t3 = 3, t4 = 5, q̇1 = 0, q̇4 = 0. Two different values for the blend times have qk+1 is a linear function of time. Also let q̇k,k+1 be the constant velocity and been considered: Δtk = 0.2 (solid line in the figure) and Δtk = 0.6 (dashed q̈k be the acceleration in the parabolic blend whose duration is Δtk . The line in the figure), for k = 1, … , 4, respectively. Notice that in the first case resulting trajectory is illustrated in Fig. 4.8. The values of qk , Δtk , and Δtk the passage of q(t) is closer to the via points, though at the expense of higher acceleration values. 178 4 Trajectory Planning 4.3 Operational Space Trajectories 179
pos the intermediate point, but it would be forced to stop there, before continuing
the motion towards the final point. A keen alternative is to start generating
6 the second segment ahead of time with respect to the end of the first segment,
using the sum of velocities (or positions) as a reference. In this way, the joint
4
is guaranteed to reach the final position; crossing of the intermediate point at
[rad]
2 the specified instant of time is not guaranteed, though.
Figure 4.10 illustrates the time history of position, velocity and accelera-
0 tion obtained with the data: qi = 0, qf = 3π/2, ti = 0, tf = 2. The interme-
0 1 2 3 4 5
diate point is located at q = π with t = 1, the maximum acceleration values
[s] in the two segments are respectively |q̈c | = 6π and |q̈c | = 3π, and the time
vel anticipation is 0.18. As predicted, with time anticipation, the assigned inter-
4 mediate position becomes a via point with the advantage of an overall shorter
time duration. Notice, also, that velocity does not vanish at the intermediate
2
point.
[rad/s]
0
Ŧ2
Ŧ4
4.3 Operational Space Trajectories
Ŧ6 A joint space trajectory planning algorithm generates a time sequence of val-
0 1 2 3 4 5
[s] ues for the joint variables q(t) so that the manipulator is taken from the
acc initial to the final configuration, eventually by moving through a sequence of
40 intermediate configurations. The resulting end-effector motion is not easily
predictable, in view of the nonlinear effects introduced by direct kinematics.
20
Whenever it is desired that the end-effector motion follows a geometrically
[rad/s^2]
0 specified path in the operational space, it is necessary to plan trajectory exe-
cution directly in the same space. Planning can be done either by interpolating
Ŧ20
a sequence of prescribed path points or by generating the analytical motion
Ŧ40 primitive and the relative trajectory in a punctual way.
In both cases, the time sequence of the values attained by the operational
0 1 2 3 4 5
[s] space variables is utilized in real time to obtain the corresponding sequence
of values of the joint space variables, via an inverse kinematics algorithm. In
Fig. 4.9. Time history of position, velocity and acceleration with a timing law of this regard, the computational complexity induced by trajectory generation interpolating linear polynomials with parabolic blends in the operational space and related kinematic inversion sets an upper limit on the maximum sampling rate to generate the above sequences. Since these The technique presented above turns out to be an application of the trape- sequences constitute the reference inputs to the motion control system, a zoidal velocity profile law to the interpolation problem. If one gives up a tra- linear microinterpolation is typically carried out. In this way, the frequency jectory passing near a via point at a prescribed instant of time, the use of at which reference inputs are updated is increased so as to enhance dynamic trapezoidal velocity profiles allows the development of a trajectory planning performance of the system. algorithm which is attractive for its simplicity. Whenever the path is not to be followed exactly, its characterization can In particular, consider the case of one intermediate point only, and suppose be performed through the assignment of N points specifying the values of the that trapezoidal velocity profiles are considered as motion primitives with variables xe chosen to describe the end-effector pose in the operational space the possibility to specify the initial and final point and the duration of the at given time instants tk , for k = 1, … , N . Similar to what was presented motion only; it is assumed that q̇i = q̇f = 0. If two segments with trapezoidal in the above sections, the trajectory is generated by determining a smooth velocity profiles were generated, the manipulator joint would certainly reach interpolating vector function between the various path points. Such a function 180 4 Trajectory Planning 4.3 Operational Space Trajectories 181
pos to refer to motion primitives defining the geometric features of the path and
5 time primitives defining the timing law on the path itself.
4
3 4.3.1 Path Primitives
[rad]
2 For the definition of path primitives it is convenient to refer to the parametric
1 description of paths in space. Then let p be a (3 × 1) vector and f (σ) a con-
0 tinuous vector function defined in the interval [σi , σf ]. Consider the equation
0 0.5 1 1.5 2 p = f (σ); (4.31)
[s]
vel with reference to its geometric description, the sequence of values of p with
σ varying in [σi , σf ] is termed path in space. The equation in (4.31) defines
4 the parametric representation of the path Γ and the scalar σ is called pa-
3 rameter. As σ increases, the point p moves on the path in a given direction.
[rad/s]
2
This direction is said to be the direction induced on Γ by the parametric
representation (4.31). A path is closed when p(σf ) = p(σi ); otherwise it is
1 open.
0 Let pi be a point on the open path Γ on which a direction has been fixed.
The arc length s of the generic point p is the length of the arc of Γ with
0 0.5 1 1.5 2
[s] extremes p and pi if p follows pi , the opposite of this length if p precedes pi .
acc The point pi is said to be the origin of the arc length (s = 0).
From the above presentation it follows that to each value of s a well-
20 determined path point corresponds, and then the arc length can be used as a
10 parameter in a different parametric representation of the path Γ :
[rad/s^2]
0 p = f (s); (4.32)
Ŧ10 the range of variation of the parameter s will be the sequence of arc lengths
Ŧ20 associated with the points of Γ .
Consider a path Γ represented by (4.32). Let p be a point corresponding
0 0.5 1 1.5 2
[s] to the arc length s. Except for special cases, p allows the definition of three
unit vectors characterizing the path. The orientation of such vectors depends
Fig. 4.10. Time history of position, velocity and acceleration with a timing law of exclusively on the path geometry, while their direction depends also on the interpolating linear polynomials with parabolic blends obtained by anticipating the direction induced by (4.32) on the path. generation of the second segment of trajectory The first of such unit vectors is the tangent unit vector denoted by t. This vector is oriented along the direction induced on the path by s. can be computed by applying to each component of xe any of the interpolation The second unit vector is the normal unit vector denoted by n. This vector techniques illustrated in Sect. 4.2.2 for the single joint variable. is oriented along the line intersecting p at a right angle with t and lies in the Therefore, for given path (or via) points xe (tk ), the corresponding com- so-called osculating plane O (Fig. 4.11); such plane is the limit position of the ponents xei (tk ), for i = 1, … r (where r is the dimension of the operational plane containing the unit vector t and a point p ∈ Γ when p tends to p along space of interest) can be interpolated with a sequence of cubic polynomials, a the path. The direction of n is so that the path Γ , in the neighbourhood of p sequence of linear polynomials with parabolic blends, and so on. with respect to the plane containing t and normal to n, lies on the same side On the other hand, if the end-effector motion has to follow a prescribed of n. trajectory of motion, this must be expressed analytically. It is then necessary The third unit vector is the binormal unit vector denoted by b. This vector is so that the frame (t, n, b) is right-handed (Fig. 4.11). Notice that it is not always possible to define uniquely such a frame. 182 4 Trajectory Planning 4.3 Operational Space Trajectories 183
Fig. 4.11. Parametric representation of a path in space
Fig. 4.12. Parametric representation of a circle in space
It can be shown that the above three unit vectors are related by simple
relations to the path representation Γ as a function of the arc length. In Circular path particular, it is dp Consider a circle Γ in space. Before deriving its parametric representation, it t= is necessary to introduce its significant parameters. Suppose that the circle is ds specified by assigning (Fig. 4.12): 1 d2 p n= ( 2 ( 2 (4.33) ( d p ( ds • the unit vector of the circle axis r, ( ( ( ds2 ( • the position vector d of a point along the circle axis, b = t × n. • the position vector pi of a point on the circle.
Typical path parametric representations are reported below which are useful With these parameters, the position vector c of the centre of the circle can for trajectory generation in the operational space. be found. Let δ = pi − d; for pi not to be on the axis, i.e., for the circle not to degenerate into a point, it must be Rectilinear path |δ T r| < δ ; Consider the linear segment connecting point pi to point pf . The parametric representation of this path is in this case it is s c = d + (δ T r)r. (4.37) p(s) = pi + (pf − pi ). (4.34) pf − pi It is now desired to find a parametric representation of the circle as a function of the arc length. Notice that this representation is very simple for a suitable Notice that p(0) = pi and p( pf − pi ) = pf . Hence, the direction induced choice of the reference frame. To see this, consider the frame O –x y z , where on Γ by the parametric representation (4.34) is that going from pi to pf . O coincides with the centre of the circle, axis x is oriented along the direction Differentiating (4.34) with respect to s gives of the vector pi − c, axis z is oriented along r and axis y is chosen so as to dp 1 complete a right-handed frame. When expressed in this reference frame, the = (pf − pi ) (4.35) ds pf − pi parametric representation of the circle is d2 p ⎡ ⎤ = 0. (4.36) ρ cos (s/ρ) ds2 p (s) = ⎣ ρ sin (s/ρ) ⎦ , (4.38) In this case it is not possible to define the frame (t, n, b) uniquely. 0 184 4 Trajectory Planning 4.3 Operational Space Trajectories 185
where ρ = pi − c is the radius of the circle and the point pi has been where t is the tangent vector to the path at point p in (4.33). Then, ṡ rep- assumed as the origin of the arc length. For a different reference frame, the resents the magnitude of the velocity vector relative to point p, taken with path representation becomes the positive or negative sign depending on the direction of ṗ along t. The magnitude of ṗ starts from zero at t = 0, then it varies with a parabolic or p(s) = c + Rp (s), (4.39) trapezoidal profile as per either of the above choices for s(t), and finally it returns to zero at t = tf . where c is expressed in the frame O–xyz and R is the rotation matrix of As a first example, consider the segment connecting point pi with point pf . frame O – x y z with respect to frame O–xyz which, in view of (2.3), can be The parametric representation of this path is given by (4.34). Velocity and ac- written as celeration of pe can be easily computed by recalling the rule of differentiation R = [ x y z ]; of compound functions, i.e., x , y , z indicate the unit vectors of the frame expressed in the frame O–xyz. ṡ Differentiating (4.39) with respect to s gives ṗe = (pf − pi ) = ṡt (4.42) pf − pi ⎡ ⎤ −sin (s/ρ) s̈ dp p̈e = (pf − pi ) = s̈t. (4.43) = R ⎣ cos (s/ρ) ⎦ (4.40) pf − pi ds 0 ⎡ ⎤ As a further example, consider a circle Γ in space. From the parametric −cos (s/ρ)/ρ d2 p representation derived above, in view of (4.40), (4.41), velocity and accelera- = R ⎣ −sin (s/ρ)/ρ ⎦ . (4.41) ds2 tion of point pe on the circle are 0 ⎡ ⎤ −ṡ sin (s/ρ) 4.3.2 Position ṗe = R ⎣ ṡ cos (s/ρ) ⎦ (4.44) 0 Let xe be the vector of operational space variables expressing the pose of ⎡ 2 ⎤ −ṡ cos (s/ρ)/ρ − s̈ sin (s/ρ) the manipulator’s end-effector as in (2.80). Generating a trajectory in the p̈e = R ⎣ −ṡ2 sin (s/ρ)/ρ + s̈ cos (s/ρ) ⎦ . (4.45) operational space means to determine a function xe (t) taking the end-effector 0 frame from the initial to the final pose in a time tf along a given path with a specific motion timing law. First, consider end-effector position. Orientation Notice that the velocity vector is aligned with t, and the acceleration vector will follow. is given by two contributions: the first is aligned with n and represents the Let pe = f (s) be the (3 × 1) vector of the parametric representation of the centripetal acceleration, while the second is aligned with t and represents the path Γ as a function of the arc length s; the origin of the end-effector frame tangential acceleration. moves from pi to pf in a time tf . For simplicity, suppose that the origin of Finally, consider the path consisting of a sequence of N + 1 points, the arc length is at pi and the direction induced on Γ is that going from pi p0 , p1 , … , pN , connected by N segments. A feasible parametric representa- to pf . The arc length then goes from the value s = 0 at t = 0 to the value tion of the overall path is the following: s = sf (path length) at t = tf . The timing law along the path is described by the function s(t). N sj In order to find an analytic expression for s(t), any of the above techniques pe = p0 + (pj − pj−1 ), (4.46) pj − pj−1 for joint trajectory generation can be employed. In particular, either a cubic j=1 polynomial or a sequence of linear segments with parabolic blends can be chosen for s(t). with j = 1, … , N . In (4.46) sj is the arc length associated with the j-th It is worth making some remarks on the time evolution of pe on Γ , for a segment of the path, connecting point pj−1 to point pj , defined as given timing law s(t). The velocity of point pe is given by the time derivative ⎧ ⎪ ⎨0 0 ≤ t ≤ tj−1 of pe dp sj (t) = sj (t) tj−1 < t < tj (4.47) ṗe = ṡ e = ṡt, ⎪ ⎩ p −p ds j j−1 t j ≤ t ≤ t f , 186 4 Trajectory Planning 4.3 Operational Space Trajectories 187
where t0 = 0 and tN = tf are respectively the initial and final time instants of 4.3.3 Orientation the trajectory, tj is the time instant corresponding to point pj and sj (t) can be an analytical function of cubic polynomial type, linear type with parabolic Consider now end-effector orientation. Typically, this is specified in terms of blends, and so forth, which varies continuously from the value sj = 0 at the rotation matrix of the (time-varying) end-effector frame with respect to t = tj−1 to the value sj = pj − pj−1 at t = tj . the base frame. As is well known, the three columns of the rotation matrix The velocity and acceleration of pe can be easily found by differentiat- represent the three unit vectors of the end-effector frame with respect to the ing (4.46): base frame. To generate a trajectory, however, a linear interpolation on the unit vectors ne , se , ae describing the initial and final orientation does not N ṡj N guarantee orthonormality of the above vectors at each instant of time. ṗe = (pj − pj−1 ) = ṡj tj (4.48) j=1 pj − pj−1 j=1 Euler angles N s̈j N p̈e = (pj − pj−1 ) = s̈j tj , (4.49) j=1 pj − pj−1 j=1 In view of the above difficulty, for trajectory generation purposes, orientation is often described in terms of the Euler angles triplet φe = (ϕ, ϑ, ψ) for which where tj is the tangent unit vector of the j-th segment. a timing law can be specified. Usually, φe moves along the segment connecting Because of the discontinuity of the first derivative at the path points be- its initial value φi to its final value φf . Also in this case, it is convenient to tween two non-aligned segments, the manipulator will have to stop and then choose a cubic polynomial or a linear segment with parabolic blends timing go along the direction of the following segment. Assumed a relaxation of the law. In this way, in fact, the angular velocity ω e of the time-varying frame, constraint to pass through the path points, it is possible to avoid a manipu- which is related to φ̇e by the linear relationship (3.64), will have continuous lator stop by connecting the segments near the above points, which will then magnitude. be named operational space via points so as to guarantee, at least, continuity Therefore, for given φi and φf and timing law, the position, velocity and of the first derivative. acceleration profiles are As already illustrated for planning of interpolating linear polynomials with s parabolic blends passing by the via points in the joint space, the use of trape- φe = φi + (φf − φi ) φf − φi zoidal velocity profiles for the arc lengths allows the development of a rather ṡ simple planning algorithm φ̇e = (φf − φi ) (4.51) In detail, it will be sufficient to properly anticipate the generation of the φf − φi single segments, before the preceding segment has been completed. This leads s̈ φ̈e = (φf − φi ); to modifying (4.47) as follows: φf − φi ⎧ ⎪ ⎨0 0 ≤ t ≤ tj−1 − Δtj where the timing law for s(t) has to be specified. The three unit vectors of the sj (t) = sj (t + Δtj ) tj−1 − Δtj < t < tj − Δtj (4.50) end-effector frame can be computed — with reference to Euler angles ZYZ ⎪ ⎩ p −p — as in (2.18), the end-effector frame angular velocity as in (3.64), and the j j−1 t j − Δt j ≤ t ≤ t f − Δt N , angular acceleration by differentiating (3.64) itself. where Δtj is the time advance at which the j-th segment is generated, which can be recursively evaluated as Angle and axis
Δtj = Δtj−1 + δtj , An alternative way to generate a trajectory for orientation of clearer inter-
pretation in the Cartesian space can be derived by resorting to the the angle
with j = 1, … , N e Δt0 = 0. Notice that this time advance is given by the and axis description presented in Sect. 2.5. Given two coordinate frames in sum of two contributions: the former, Δtj−1 , accounts for the sum of the time the Cartesian space with the same origin and different orientation, it is always advances at which the preceding segments have been generated, while the possible to determine a unit vector so that the second frame can be obtained latter, δtj , is the time advance at which the generation of the current segment from the first frame by a rotation of a proper angle about the axis of such starts. unit vector. 188 4 Trajectory Planning Problems 189
Let Ri and Rf denote respectively the rotation matrices of the initial The generation of motion trajectories through sequences of points in the frame Oi –xi yi zi and the final frame Of –xf yf zf , both with respect to the joint space using splines is due to [131]. Alternative formulations for this base frame. The rotation matrix between the two frames can be computed by problem are found in [56]. For a complete treatment of splines, including recalling that Rf = Ri Rif ; the expression in (2.5) leads to geometric properties and computational aspects, see [54]. In [155] a survey ⎡ ⎤ on the functions employed for trajectory planning of a single motion axis r11 r12 r13 is given, which accounts for performance indices and effects of unmodelled Rf = Ri Rf = ⎣ r21 r22 r23 ⎦ . i T flexible dynamics. r31 r32 r33 Cartesian space trajectory planning and the associated motion control If the matrix Ri (t) is defined to describe the transition from Ri to Rf , it problem have been originally treated in [179]. The systematic management must be Ri (0) = I and Ri (tf ) = Rif . Hence, the matrix Rif can be expressed of the motion by the via points using interpolating linear polynomials with as the rotation matrix about a fixed axis in space; the unit vector r i of the parabolic blends has been proposed in [229]. A detailed presentation of the axis and the angle of rotation ϑf can be computed by using (2.27): general aspects of the geometric primitives that can be utilized in robotics to define Cartesian space paths can be found in the computer graphics text [73]. r11 + r22 + r33 − 1 ϑf = cos −1 (4.52) 2 ⎡ ⎤ r32 − r23 Problems 1 ⎣ r13 − r31 ⎦ r= (4.53) 2 sin ϑf r21 − r12 4.1. Compute the joint trajectory from q(0) = 1 to q(2) = 4 with null initial and final velocities and accelerations. for sin ϑf = 0. The matrix Ri (t) can be interpreted as a matrix Ri (ϑ(t), r i ) and computed 4.2. Compute the timing law q(t) for a joint trajectory with velocity profile via (2.25); it is then sufficient to assign a timing law to ϑ, of the type of those of the type q̇(t) = k(1 − cos (at)) from q(0) = 0 to q(2) = 3. presented for the single joint with ϑ(0) = 0 and ϑ(tf ) = ϑf , and compute the components of r i from (4.52). Since r i is constant, the resulting velocity and 4.3. Given the values for the joint variable: q(0) = 0, q(2) = 2, and q(4) = 3, acceleration are respectively compute the two fifth-order interpolating polynomials with continuous veloc- ities and accelerations. ω i = ϑ̇ r i (4.54) ω̇ i = ϑ̈ r i . (4.55) 4.4. Show that the matrix A in (4.28) has a tridiagonal band structure. Finally, in order to characterize the end-effector orientation trajectory with 4.5. Given the values for the joint variable: q(0) = 0, q(2) = 2, and q(4) = 3, respect to the base frame, the following transformations are needed: compute the cubic interpolating spline with null initial and final velocities and Re (t) = Ri Ri (t) accelerations. ω e (t) = Ri ω i (t) 4.6. Given the values for the joint variable: q(0) = 0, q(2) = 2, and q(4) = 3, ω̇ e (t) = Ri ω̇ i (t). find the interpolating polynomial with linear segments and parabolic blends with null initial and final velocities. Once a path and a trajectory have been specified in the operational space in terms of pe (t) and φe (t) or Re (t), inverse kinematics techniques can be 4.7. Find the timing law p(t) for a Cartesian space rectilinear path with trape- used to find the corresponding trajectories in the joint space q(t). zoidal velocity profile from p(0) = [ 0 0.5 0 ]T to p(2) = [ 1 −0.5 0 ]T .
4.8. Find the timing law p(t) for a Cartesian space circular path with trape-
Bibliography zoidal velocity profile from p(0) = [ 0 0.5 1 ]T to p(2) = [ 0 −0.5 1 ]T ; the circle is located in the plane x = 0 with centre at c = [ 0 0 1 ]T and Trajectory planning for robot manipulators has been addressed since the first radius ρ = 0.5, and is executed clockwise for an observer aligned with x. works in the field of robotics [178]. The formulation of the interpolation prob- lem of the path points by means of different classes of functions has been suggested in [26]. 248 7 Dynamics
Lagrangian of the mechanical system can be defined as a function of the
generalized coordinates:
7 L=T −U (7.1) where T and U respectively denote the total kinetic energy and potential Dynamics energy of the system. The Lagrange equations are expressed by d ∂L ∂L − = ξi i = 1, … , n (7.2) dt ∂ q̇i ∂qi where ξi is the generalized force associated with the generalized coordinate qi . Equations (7.2) can be written in compact form as T T d ∂L ∂L − =ξ (7.3) Derivation of the dynamic model of a manipulator plays an important role dt ∂ q̇ ∂q for simulation of motion, analysis of manipulator structures, and design of where, for a manipulator with an open kinematic chain, the generalized coor- control algorithms. Simulating manipulator motion allows control strategies dinates are gathered in the vector of joint variables q. The contributions to and motion planning techniques to be tested without the need to use a phys- the generalized forces are given by the nonconservative forces, i.e., the joint ically available system. The analysis of the dynamic model can be helpful for actuator torques, the joint friction torques, as well as the joint torques induced mechanical design of prototype arms. Computation of the forces and torques by end-effector forces at the contact with the environment.1 required for the execution of typical motions provides useful information for The equations in (7.2) establish the relations existing between the gener- designing joints, transmissions and actuators. The goal of this chapter is to alized forces applied to the manipulator and the joint positions, velocities and present two methods for derivation of the equations of motion of a manipula- accelerations. Hence, they allow the derivation of the dynamic model of the tor in the joint space. The first method is based on the Lagrange formulation manipulator starting from the determination of kinetic energy and potential and is conceptually simple and systematic. The second method is based on the energy of the mechanical system. Newton–Euler formulation and yields the model in a recursive form; it is com- putationally more efficient since it exploits the typically open structure of the manipulator kinematic chain. Then, a technique for dynamic parameter iden- tification is presented. Further, the problems of direct dynamics and inverse Example 7.1 dynamics are formalized, and a technique for trajectory dynamic scaling is in- troduced, which adapts trajectory planning to the dynamic characteristics of In order to understand the Lagrange formulation technique for deriving the dynamic model, consider again the simple case of the pendulum in Example 5.1. With ref- the manipulator. The chapter ends with the derivation of the dynamic model erence to Fig. 5.8, let ϑ denote the angle with respect to the reference position of of a manipulator in the operational space and the definition of the dynamic the body hanging down (ϑ = 0). By choosing ϑ as the generalized coordinate, the manipulability ellipsoid. kinetic energy of the system is given by 1 2 1 T = I ϑ̇ + Im kr2 ϑ̇2 . 2 2 7.1 Lagrange Formulation The system potential energy, defined at less than a constant, is expressed by The dynamic model of a manipulator provides a description of the relationship U = mg(1 − cos ϑ). between the joint actuator torques and the motion of the structure. With Lagrange formulation, the equations of motion can be derived in Therefore, the Lagrangian of the system is a systematic way independently of the reference coordinate frame. Once a 1 2 1 set of variables qi , i = 1, … , n, termed generalized coordinates, are chosen L= I ϑ̇ + Im kr2 ϑ̇2 − mg(1 − cos ϑ). 2 2 which effectively describe the link positions of an n-DOF manipulator, the 1 The term torque is used as a synonym of joint generalized force. 7.1 Lagrange Formulation 249 250 7 Dynamics
Substituting this expression in the Lagrange equation in (7.2) yields
(I + Im kr2 )ϑ̈ + mg sin ϑ = ξ.
The generalized force ξ is given by the contributions of the actuation torque τ at the joint and of the viscous friction torques −F ϑ̇ and −Fm kr2 ϑ, where the latter has been reported to the joint side. Hence, it is
ξ = τ − F ϑ̇ − Fm kr2 ϑ
leading to the complete dynamic model of the system as the second-order differential equation (I + Im kr2 )ϑ̈ + (F + Fm kr2 )ϑ̇ + mg sin ϑ = τ . It is easy to verify how this equation is equivalent to (5.25) when reported to the joint side.
Fig. 7.1. Kinematic description of Link i for Lagrange formulation
7.1.1 Computation of Kinetic Energy
Consider a manipulator with n rigid links. The total kinetic energy is given where m i is the link mass. As a consequence, the link point velocity can be by the sum of the contributions relative to the motion of each link and the expressed as contributions relative to the motion of each joint actuator:2 ṗ∗i = ṗ i + ω i × r i (7.8) n = ṗ i + S(ω i )r i , T = (T i + Tmi ), (7.4) i=1 where ṗ i is the linear velocity of the centre of mass and ω i is the angular velocity of the link (Fig. 7.1). where T i is the kinetic energy of Link i and Tmi is the kinetic energy of the By substituting the velocity expression (7.8) into (7.5), it can be recognized motor actuating Joint i. that the kinetic energy of each link is formed by the following contributions. The kinetic energy contribution of Link i is given by ” Translational 1 Ti= ṗ∗ T ṗ∗ ρdV , (7.5) 2 Vi i i The contribution is ” 1 1 where ṗ∗i denotes the linear velocity vector and ρ is the density of the elemen- ṗTi ṗ i ρdV = m ṗT ṗ . (7.9) 2 Vi 2 i i i tary particle of volume dV ; V i is the volume of Link i. Consider the position vector p∗i of the elementary particle and the position vector pCi of the link centre of mass, both expressed in the base frame. One Mutual has The contribution is r i = [ rix riy riz ]T = p∗i − p i (7.6) & ” ’ & ’ ” 1 1 T ∗ with ” 2 T ṗ S(ω i )r i ρdV = 2 ṗ S(ω i ) (pi − p i )ρdV = 0 1 2 Vi i 2 i pi= p∗i ρdV (7.7) Vi mi Vi since, by virtue of (7.7), it is ” ” p∗i ρdV = p i ρdV . 2 Vi Vi Link 0 is fixed and thus gives no contribution. 7.1 Lagrange Formulation 251 252 7 Dynamics
Rotational By summing the translational and rotational contributions (7.9) and (7.10), the kinetic energy of Link i is The contribution is ” &” ’ 1 1 1 1 T Ti= m ṗT ṗ + ω T Ri I i i RTi ω i . (7.13) T T r S (ω i )S(ω i )r i ρdV = ω i T S (r i )S(r i )ρdV ω i 2 i i i 2 i 2 Vi i 2 Vi At this point, it is necessary to express the kinetic energy as a function where the property S(ω i )r i = −S(r i )ω i has been exploited. In view of the of the generalized coordinates of the system, that are the joint variables. To expression of the matrix operator S(·) this end, the geometric method for Jacobian computation can be applied to the intermediate link other than the end-effector, yielding ⎡ ⎤ 0 −riz riy ( ) ( ) ( ) S(r i ) = ⎣ riz 0 −rix ⎦ , ṗ i = jP 1i q̇1 + … + jP ii q̇i = J P i q̇ (7.14) −riy rix 0 ( ) ( ) ( ) ω i = jO1i q̇1 + … + jOii q̇i = J O i q̇, (7.15) it is ” where the contributions of the Jacobian columns relative to the joint velocities 1 1 T r Ti S T (ω i )S(ω i )r i ρdV = ω I ωi . (7.10) have been taken into account up to current Link i. The Jacobians to consider 2 Vi 2 i i are then: The matrix ! ( ) ⎡% 2 2 % % ⎤ J P i = j(P 1i ) … j(P ii ) 0 … 0 (7.16) (riy + riz )ρdV − rix riy ρdV − rix riz ρdV ! ⎢ % 2 2 % ⎥ ( i) ( i) ( i) J O = jO1 … jOi 0 … 0 ; (7.17) I i =⎣ ∗ (rix + riz )ρdV − riy riz ρdV ⎦ (7.11) % 2 2 ∗ ∗ (rix + riy )ρdV the columns of the matrices in (7.16) and (7.17) can be computed according ⎡ ⎤ to (3.30), giving I i xx −I i xy −I i xz ⎢ ⎥ =⎣ ∗ I i yy −I i yz ⎦ . ( i) z j−1 for a prismatic joint jP j = (7.18) ∗ ∗ I i zz z j−1 × (p i − pj−1 ) for a revolute joint is symmetric3 and represents the inertia tensor relative to the centre of mass ( ) jOji = 0 for a prismatic joint (7.19) of Link i when expressed in the base frame. Notice that the position of Link i z j−1 for a revolute joint. depends on the manipulator configuration and thus the inertia tensor, when where pj−1 is the position vector of the origin of Frame j − 1 and z j−1 is the expressed in the base frame, is configuration-dependent. If the angular velocity unit vector of axis z of Frame j − 1. It follows that the kinetic energy of Link of Link i is expressed with reference to a frame attached to the link (as in the i in (7.13) can be written as Denavit–Hartenberg convention), it is 1 ( )T ( ) 1 ( )T ( ) ω ii = RTi ω i Ti= m q̇ T J P i J P i q̇ + q̇ T J O i Ri I i i RTi J O i q̇. (7.20) 2 i 2 where Ri is the rotation matrix from Link i frame to the base frame. When The kinetic energy contribution of the motor of Joint i can be computed referred to the link frame, the inertia tensor is constant. Let I i i denote such in a formally analogous way to that of the link. Consider the typical case of tensor; then it is easy to verify the following relation: rotary electric motors (that can actuate both revolute and prismatic joints by means of suitable transmissions). It can be assumed that the contribution of I i = Ri I i i RTi . (7.12) the fixed part (stator) is included in that of the link on which such motor is located, and thus the sole contribution of the rotor is to be computed. If the axes of Link i frame coincide with the central axes of inertia, then the With reference to Fig. 7.2, the motor of Joint i is assumed to be located inertia products are null and the inertia tensor relative to the centre of mass on Link i − 1. In practice, in the design of the mechanical structure of an open is a diagonal matrix. kinematic chain manipulator one attempts to locate the motors as close as 3 The symbol ‘∗’ has been used to avoid rewriting the symmetric elements. possible to the base of the manipulator so as to lighten the dynamic load of 7.1 Lagrange Formulation 253 254 7 Dynamics
To express the rotor kinetic energy as a function of the joint variables, it
is worth expressing the linear velocity of the rotor centre of mass — similarly
to (7.14) — as
(m )
ṗmi = J P i q̇. (7.24)
The Jacobian to compute is then
(m ) !
J P i = j(m
P1
i)
...
(m )
i
jP,i−1 0 ... 0 (7.25)
whose columns are given by
(mi ) z j−1 for a prismatic joint
jP j = (7.26)
z j−1 × (pmi − pj−1 ) for a revolute joint
where pj−1 is the position vector of the origin of Frame j − 1. Notice that
(m )
jP i i = 0 in (7.25), since the centre of mass of the rotor has been taken along
its axis of rotation.
The angular velocity in (7.23) can be expressed as a function of the joint
variables, i.e.,
Fig. 7.2. Kinematic description of Motor i (m )
ω mi = J O i q̇. (7.27)
The Jacobian to compute is then
the first joints of the chain. The joint actuator torques are delivered by the (m ) ! motors by means of mechanical transmissions (gears).4 The contribution of J O i = j(m O1 i) … (m ) i jO,i−1 (m ) jOi i 0 … 0 (7.28) the gears to the kinetic energy can be suitably included in that of the motor. whose columns, in view of (7.23), (7.15), are respectively given by It is assumed that no induced motion occurs, i.e., the motion of Joint i does ( ) not actuate the motion of other joints. (mi ) jOji j = 1, … , i − 1 jOj = (7.29) The kinetic energy of Rotor i can be written as kri z mi j = i. 1 1 To compute the second relation in (7.29), it is sufficient to know the compo- Tmi = mmi ṗTmi ṗmi + ω Tmi I mi ω mi , (7.21) 2 2 nents of the unit vector of the rotor rotation axis z mi with respect to the base where mmi is the mass of the rotor, ṗmi denotes the linear velocity of the frame. Hence, the kinetic energy of Rotor i can be written as centre of mass of the rotor, I mi is the inertia tensor of the rotor relative to 1 (m )T (m ) 1 (m )T (mi ) its centre of mass, and ω mi denotes the angular velocity of the rotor. Tmi = mmi q̇ T J P i J P i q̇ + q̇ T J O i Rmi I m i T mi R mi J O q̇. (7.30) 2 2 Let ϑmi denote the angular position of the rotor. On the assumption of a Finally, by summing the various contributions relative to the single links rigid transmission, one has (7.20) and single rotors (7.30) as in (7.4), the total kinetic energy of the kri q̇i = ϑ̇mi (7.22) manipulator with actuators is given by the quadratic form where kri is the gear reduction ratio. Notice that, in the case of actuation of 1 n n a prismatic joint, the gear reduction ratio is a dimensional quantity. 1 T = bij (q)q̇i q̇j = q̇ T B(q)q̇ (7.31) According to the angular velocity composition rule (3.18) and the rela- 2 i=1 j=1 2 tion (7.22), the total angular velocity of the rotor is where ω mi = ω i−1 + kri q̇i z mi (7.23) n ( )T ( ) ( )T ( ) B(q) = m i J P i J P i + J O i Ri I i i RTi J O i (7.32) where ω i−1 is the angular velocity of Link i − 1 on which the motor is located, i=1 and z mi denotes the unit vector along the rotor axis. (m )T (m ) (m )T (m ) +mmi J P i J P i + J O i Rmi I m i T mi R mi J O i
4 Alternatively, the joints may be actuated by torque motors directly coupled to the rotation axis without gears. is the (n × n) inertia matrix which is: 7.1 Lagrange Formulation 255 256 7 Dynamics
• symmetric, where T T • positive definite, 1 ∂ ∂U(q) n(q, q̇) = Ḃ(q)q̇ − q̇ T B(q)q̇ + . • (in general) configuration-dependent. 2 ∂q ∂q In detail, noticing that U in (7.36) does not depend on q̇ and accounting 7.1.2 Computation of Potential Energy for (7.31) yields n n As done for kinetic energy, the potential energy stored in the manipulator is d ∂L d ∂T dbij (q) given by the sum of the contributions relative to each link as well as to each = = bij (q)q̈j + q̇j dt ∂ q̇i dt ∂ q̇i j=1 j=1 dt rotor: n n n n ∂bij (q) U= (U i + Umi ). (7.33) = bij (q)q̈j + q̇k q̇j i=1 j=1 j=1 k=1 ∂qk On the assumption of rigid links, the contribution due only to gravitational and forces5 is expressed by ∂T 1 ∂bjk (q) n n ” = q̇k q̇j ∂qi 2 j=1 ∂qi Ui =− g T0 p∗i ρdV = −m i g T0 p i (7.34) k=1 Vi where the indices of summation have been conveniently switched. Further, in view of (7.14), (7.24), it is where g 0 is the gravity acceleration vector in the base frame (e.g., g 0 = [ 0 0 −g ]T if z is the vertical axis), and (7.7) has been utilized for the n ∂U ∂p j ∂pmj coordinates of the centre of mass of Link i. As regards the contribution of =− m j g T0 + mmj g T0 (7.39) ∂qi ∂qi ∂qi Rotor i, similarly to (7.34), one has j=1 n ( ) (m ) Umi = −mmi g T0 pmi . (7.35) =− m j g T0 jP ij (q) + mmj g T0 jP i j (q) = gi (q) j=1 By substituting (7.34), (7.35) into (7.33), the potential energy is given by where, again, the index of summation has been changed. n As a result, the equations of motion are U =− (m i g T0 p i + mmi g T0 pmi ) (7.36) i=1 n n n bij (q)q̈j + hijk (q)q̇k q̇j + gi (q) = ξi i = 1, … , n. (7.40) which reveals that potential energy, through the vectors p i and pmi is a j=1 j=1 k=1 function only of the joint variables q, and not of the joint velocities q̇. where ∂bij 1 ∂bjk 7.1.3 Equations of Motion hijk = − . (7.41) ∂qk 2 ∂qi Having computed the total kinetic and potential energy of the system as A physical interpretation of (7.40) reveals that: in (7.31), (7.36), the Lagrangian (7.1) for the manipulator can be written as • For the acceleration terms: L(q, q̇) = T (q, q̇) − U(q). (7.37) – The coefficient bii represents the moment of inertia at Joint i axis, in the current manipulator configuration, when the other joints are Taking the derivatives required by Lagrange equations in (7.3) and recalling blocked. that U does not depend on q̇ yields – The coefficient bij accounts for the effect of acceleration of Joint j on Joint j. B(q)q̈ + n(q, q̇) = ξ (7.38) • For the quadratic velocity terms: 5 In the case of link flexibility, one would have an additional contribution due to – The term hijj q̇j2 represents the centrifugal effect induced on Joint i by elastic forces. velocity of Joint j; notice that hiii = 0, since ∂bii /∂qi = 0. 7.2 Notable Properties of Dynamic Model 257 258 7 Dynamics
– The term hijk q̇j q̇k represents the Coriolis effect induced on Joint i by elaborating the term on the right-hand side of (7.43) and accounting for the velocities of Joints j and k. expressions of the coefficients hijk in (7.41). To this end, one has • For the configuration-dependent terms: n n n – The term gi represents the moment generated at Joint i axis of the cij q̇j = hijk q̇k q̇j manipulator, in the current configuration, by the presence of gravity. j=1 j=1 k=1 n n ∂bij 1 ∂bjk Some joint dynamic couplings, e.g., coefficients bij and hijk , may be re- = − q̇k q̇j . duced or zeroed when designing the structure, so as to simplify the control j=1 k=1 ∂qk 2 ∂qi problem. Regarding the nonconservative forces doing work at the manipulator joints, Splitting the first term on the right-hand side by an opportune switch of these are given by the actuation torques τ minus the viscous friction torques summation between j and k yields F v q̇ and the static friction torques f s (q, q̇): F v denotes the (n × n) diagonal n 1 ∂bij 1 ∂bik n n n n ∂bjk matrix of viscous friction coefficients. As a simplified model of static friction cij q̇j = q̇k q̇j + − q̇k q̇j . 2 j=1 ∂qk 2 j=1 ∂qj ∂qi torques, one may consider the Coulomb friction torques F s sgn (q̇), where F s j=1 k=1 k=1 is an (n × n) diagonal matrix and sgn (q̇) denotes the (n × 1) vector whose As a consequence, the generic element of C is components are given by the sign functions of the single joint velocities. If the manipulator’s end-effector is in contact with an environment, a n cij = cijk q̇k (7.44) portion of the actuation torques is used to balance the torques induced at k=1 the joints by the contact forces. According to a relation formally analogous to (3.111), such torques are given by J T (q)he where he denotes the vector of where the coefficients force and moment exerted by the end-effector on the environment. 1 ∂bij ∂bik ∂bjk In summary, the equations of motion (7.38) can be rewritten in the com- cijk = + − (7.45) 2 ∂qk ∂qj ∂qi pact matrix form which represents the joint space dynamic model : are termed Christoffel symbols of the first type. Notice that, in view of the B(q)q̈ + C(q, q̇)q̇ + F v q̇ + F s sgn (q̇) + g(q) = τ − J T (q)he (7.42) symmetry of B, it is cijk = cikj . (7.46) where C is a suitable (n × n) matrix such that its elements cij satisfy the This choice for the matrix C leads to deriving the following notable prop- equation n n n erty of the equations of motion (7.42). The matrix cij q̇j = hijk q̇k q̇j . (7.43) N (q, q̇) = Ḃ(q) − 2C(q, q̇) (7.47) j=1 j=1 k=1 is skew-symmetric; that is, given any (n × 1) vector w, the following relation holds: 7.2 Notable Properties of Dynamic Model wT N (q, q̇)w = 0. (7.48) In fact, substituting the coefficient (7.45) into (7.44) gives In the following, two notable properties of the dynamic model are presented n 1 ∂bij 1 ∂bik which will be useful for dynamic parameter identification as well as for deriving n ∂bjk control algorithms. cij = q̇k + − q̇k 2 ∂qk 2 ∂qj ∂qi k=1 k=1 n 7.2.1 Skew-symmetry of Matrix Ḃ − 2C 1 1 ∂bik ∂bjk = ḃij + − q̇k 2 2 ∂qj ∂qi k=1 The choice of the matrix C is not unique, since there exist several matri- ces C whose elements satisfy (7.43). A particular choice can be obtained by and then the expression of the generic element of the matrix N in (7.47) is n ∂bjk ∂bik nij = ḃij − 2cij = − q̇k . ∂qi ∂qj k=1 7.2 Notable Properties of Dynamic Model 259 260 7 Dynamics
The result follows by observing that where 1 1 Ti= m i ṗTi ṗ i + ω Ti I i ω i (7.53) nij = −nji . 2 2 and An interesting property which is a direct implication of the skew-symmetry 1 1 Tmi+1 = mmi+1 ṗTmi+1 ṗmi+1 + ω Tmi+1 I mi+1 ω mi+1 . (7.54) of N (q, q̇) is that, by setting w = q̇, 2 2 With reference to the centre of mass of the augmented link, the linear velocities q̇ T N (q, q̇)q̇ = 0; (7.49) of the link and rotor can be expressed according to (3.26) as notice that (7.49) does not imply (7.48), since N is a function of q̇, too. ṗ i = ṗCi + ω i × r Ci , i (7.55) It can be shown that (7.49) holds for any choice of the matrix C, since it is a result of the principle of conservation of energy (Hamilton). By virtue of ṗmi+1 = ṗCi + ω i × r Ci ,mi+1 (7.56) this principle, the total time derivative of kinetic energy is balanced by the with power generated by all the forces acting on the manipulator joints. For the mechanical system at issue, one may write r Ci , i = p i − pCi (7.57) 1 d T r Ci ,mi+1 = pmi+1 − pCi , (7.58) q̇ B(q)q̇ = q̇ T τ − F v q̇ − F s sgn (q̇) − g(q) − J T (q)he . (7.50) 2 dt where pCi denotes the position vector of the centre of mass of augmented Taking the derivative on the left-hand side of (7.50) gives Link i. Substituting (7.55) into (7.53) gives 1 T q̇ Ḃ(q)q̇ + q̇ T B(q)q̈ 2 1 Ti= m i ṗTCi ṗCi + ṗTCi S(ω i )m i r Ci , i (7.59) 2 and substituting the expression of B(q)q̈ in (7.42) yields 1 1 + m i ω Ti S T (r Ci , i )S(r Ci , i )ω i + ω Ti I i ω i . 1 d T 1 2 2 q̇ B(q)q̇ = q̇ T Ḃ(q) − 2C(q, q̇) q̇ (7.51) 2 dt 2 By virtue of Steiner theorem, the matrix +q̇ T τ − F v q̇ − F s sgn (q̇) − g(q) − J T (q)he . Ī i = I i + m i S T (r Ci , i )S(r Ci , i ) (7.60) A direct comparison of the right-hand sides of (7.50) and (7.51) leads to the result established by (7.49). represents the inertia tensor relative to the overall centre of mass pCi , which To summarize, the relation (7.49) holds for any choice of the matrix C, contains an additional contribution due to the translation of the pole with since it is a direct consequence of the physical properties of the system, respect to which the tensor is evaluated, as in (7.57). Therefore, (7.59) can be whereas the relation (7.48) holds only for the particular choice of the ele- written as ments of C as in (7.44), (7.45). 1 1 Ti= m i ṗTCi ṗCi + ṗTCi S(ω i )m i r Ci , i + ω Ti Ī i ω i . (7.61) 2 2 7.2.2 Linearity in the Dynamic Parameters In a similar fashion, substituting (7.56) into (7.54) and exploiting (7.23) An important property of the dynamic model is the linearity with respect to yields the dynamic parameters characterizing the manipulator links and rotors. 1 1 In order to determine such parameters, it is worth associating the kinetic Tmi+1 = mmi+1 ṗTCi ṗCi + ṗTCi S(ω i )mmi+1 r Ci ,mi+1 + ω Ti Ī mi+1 ω i (7.62) 2 2 and potential energy contributions of each rotor with those of the link on 1 2 2 which it is located. Hence, by considering the union of Link i and Rotor i + 1 +kr,i+1 q̇i+1 z Tmi+1 I mi+1 ω i + kr,i+1 q̇i+1 z Tmi+1 I mi+1 z mi+1 , 2 (augmented Link i), the kinetic energy contribution is given by where Ti = T i + Tmi+1 (7.52) Ī mi+1 = I mi+1 + mmi+1 S T (r Ci ,mi+1 )S(r Ci ,mi+1 ). (7.63) 7.2 Notable Properties of Dynamic Model 261 262 7 Dynamics
Summing the contributions in (7.61), (7.62) as in (7.52) gives the expres- where all the vectors have been referred to Frame i; note that r ii,Ci is fixed in
sion of the kinetic energy of augmented Link i in the form such a frame. Substituting (7.69) into (7.68) gives 1 1 1 1 iT )i i Ti = mi ṗTCi ṗCi + ω Ti Ī i ω i + kr,i+1 q̇i+1 z Tmi+1 I mi+1 ω i (7.64) Ti = mi ṗiT i iT i i i ṗi + ṗi S(ω i )mi r i,Ci + ω i I i ω i (7.70) 2 2 2 2 1 2 2 1 2 2 + kr,i+1 q̇i+1 z Tmi+1 I mi+1 z mi+1 , +kr,i+1 q̇i+1 Imi+1 z iT i mi+1 ω i + kr,i+1 q̇i+1 Imi+1 , 2 2 where mi = m i + mmi+1 and Ī i = Ī i + Ī mi+1 are respectively the overall where i I)i = Ī i + mi S T (r ii,Ci )S(r ii,Ci ) i mass and inertia tensor. In deriving (7.64), the relations in (7.57), (7.58) have (7.71) been utilized as well as the following relation between the positions of the represents the inertia tensor with respect to the origin of Frame i according centres of mass: to Steiner theorem. m i p i + mmi+1 pmi+1 = mi pCi . (7.65) Let r ii,Ci = [ Ci x Ci y Ci z ]T . The first moment of inertia is Notice that the first two terms on the right-hand side of (7.64) represent ⎡ ⎤ the kinetic energy contribution of the rotor when this is still, whereas the mi Ci x remaining two terms account for the rotor’s own motion. mi r ii,Ci = ⎣ mi Ci y ⎦ . (7.72) On the assumption that the rotor has a symmetric mass distribution about mi Ci z its axis of rotation, its inertia tensor expressed in a frame Rmi with origin at the centre of mass and axis zmi aligned with the rotation axis can be written From (7.71) the inertia tensor of augmented Link i is as ⎡ ⎤ ⎡¯ ⎤ Imi xx 0 0 Iixx + mi (2Ci y + 2Ci z ) −I¯ixy − mi Ci x Ci y −I¯ixz − mi Ci x Ci z i ⎢ ⎥ Im i mi = ⎣ 0 Imi yy 0 ⎦ (7.66) I)i = ⎣ ∗ I¯iyy + mi (2Ci x + 2Ci z ) −I¯iyz − mi Ci y Ci z ⎦ 0 0 Imi zz ∗ ∗ I¯izz + mi (2 + 2 ) Ci x Ci y where Imi yy = Imi xx . As a consequence, the inertia tensor is invariant with ⎡) ⎤ respect to any rotation about axis zmi and is, anyhow, constant when referred Iixx −I)ixy −I)ixz ⎢ ⎥ to any frame attached to Link i − 1. =⎣ ∗ I)iyy −I)iyz ⎦ . (7.73) Since the aim is to determine a set of dynamic parameters independent of ∗ ∗ I)izz the manipulator joint configuration, it is worth referring the inertia tensor of the link Ī i to frame Ri attached to the link and the inertia tensor I mi+1 to Therefore, the kinetic energy of the augmented link is linear with respect to frame Rmi+1 so that it is diagonal. In view of (7.66) one has the dynamic parameters, namely, the mass, the three components of the first moment of inertia in (7.72), the six components of the inertia tensor in (7.73), I mi+1 z mi+1 = Rmi+1 I m i+1 T mi+1 Rmi+1 z mi+1 = Imi+1 z mi+1 (7.67) and the moment of inertia of the rotor . As regards potential energy, it is worth referring to the centre of mass of where Imi+1 = Imi+1 zz denotes the constant scalar moment of inertia of the augmented Link i defined as in (7.65), and thus the single contribution of rotor about its rotation axis. potential energy can be written as Therefore, the kinetic energy (7.64) becomes Ui = −mi g iT i 0 p Ci (7.74) 1 1 iT i i Ti = mi ṗiT i iT i Ci ṗCi + ω i Ī i ω i + kr,i+1 q̇i+1 Imi+1 z mi+1 ω i (7.68) 2 2 where the vectors have been referred to Frame i. According to the relation 1 2 2 + kr,i+1 q̇i+1 Imi+1 . 2 piCi = pii + r ii,Ci . According to the linear velocity composition rule for Link i in (3.15), one The expression in (7.74) can be rewritten as may write ṗiCi = ṗii + ω ii × r ii,Ci , (7.69) Ui = −g iT i i 0 (mi pi + mi r i,Ci ) (7.75) 7.2 Notable Properties of Dynamic Model 263 264 7 Dynamics
that is, the potential energy of the augmented link is linear with respect to the mass and the three components of the first moment of inertia in (7.72). By summing the contributions of kinetic energy and potential energy for all augmented links, the Lagrangian of the system (7.1) can be expressed in the form n L= (β TT i − β TU i )π i (7.76) i=1
where π i is the (11 × 1) vector of dynamic parameters
π i = [ mi mi Ci x mi Ci y mi Ci z I)ixx I)ixy I)ixz I)iyy I)iyz I)izz Imi ]T , (7.77) in which the moment of inertia of Rotor i has been associated with the pa- Fig. 7.3. Two-link Cartesian arm rameters of Link i so as to simplify the notation. In (7.76), β T i and β U i are two (11 × 1) vectors that allow the La- grangian to be written as a function of π i . Such vectors are a function where π is a (p × 1) vector of constant parameters and Y is an (n × p) matrix of the generalized coordinates of the mechanical system (and also of their which is a function of joint positions, velocities and accelerations; this matrix derivatives as regards β T i ). In particular, it can be shown that β T i = is usually called regressor . Regarding the dimension of the parameter vector, β T i (q1 , q2 , … , qi , q̇1 , q̇2 , … , q̇i ) and β U i = β U i (q1 , q2 , … , qi ), i.e., they do not notice that p ≤ 13n since not all the thirteen parameters for each joint may depend on the variables of the joints subsequent to Link i. explicitly appear in (7.81). At this point, it should be observed how the derivations required by the Lagrange equations in (7.2) do not alter the property of linearity in the pa- rameters, and then the generalized force at Joint i can be written as 7.3 Dynamic Model of Simple Manipulator Structures n ξi = y Tij π j (7.78) In the following, three examples of dynamic model computation are illustrated j=1 for simple two-DOF manipulator structures. Two DOFs, in fact, are enough to understand the physical meaning of all dynamic terms, especially the joint where coupling terms. On the other hand, dynamic model computation for manip- d ∂β T j ∂β T j ∂β U j y ij = − + . (7.79) ulators with more DOFs would be quite tedious and prone to errors, when dt ∂ q̇i ∂qi ∂qi carried out by paper and pencil. In those cases, it is advisable to perform it Since the partial derivatives of β T j and β U j appearing in (7.79) vanish for with the aid of a symbolic programming software package. j < i, the following notable result is obtained: ⎡ ⎤ ⎡ y T11 y T12 … y T1n ⎤ ⎡ π 1 ⎤ 7.3.1 Two-link Cartesian Arm ξ1 ⎢ ξ2 ⎥ ⎢ 0T y T22 … y T2n ⎥ ⎢ ⎥ ⎥ ⎢ π2 ⎥ ⎢ . ⎥=⎢ ⎢ ⎥ ⎢ ⎥ (7.80) Consider the two-link Cartesian arm in Fig. 7.3, for which the vector of gen- ⎣ . ⎦ ⎣ … … . .. ⎦ ⎣ .. ⎦ eralized coordinates is q = [ d1 d2 ]T . Let m 1 , m 2 be the masses of the two … ξn links, and mm1 , mm2 the masses of the rotors of the two joint motors. Also let 0T 0T … y Tnn πn Im1 , Im2 be the moments of inertia with respect to the axes of the two rotors. which yields the property of linearity of the model of a manipulator with It is assumed that pmi = pi−1 and z mi = z i−1 , for i = 1, 2, i.e., the motors respect to a suitable set of dynamic parameters. are located on the joint axes with centres of mass located at the origins of the In the simple case of no contact forces (he = 0), it may be worth including respective frames. the viscous friction coefficient Fvi and Coulomb friction coefficient Fsi in the With the chosen coordinate frames, computation of the Jacobians in (7.16), parameters of the vector π i , thus leading to a total number of 13 parameters (7.18) yields ⎡ ⎤ ⎡ ⎤ per joint. In summary, (7.80) can be compactly written as 0 0 0 1 ( 1) ( ) JP = ⎣ 0 0 ⎦ JP = ⎣ 0 0 ⎦ . 2
τ = Y (q, q̇, q̈)π (7.81) 1 0 1 0
7.3 Dynamic Model of Simple Manipulator Structures 265 266 7 Dynamics
Obviously, in this case there are no angular velocity contributions for both links. Computation of the Jacobians in (7.25), (7.26) e (7.28), (7.29) yields ⎡ ⎤ ⎡ ⎤ 0 0 0 0 (m ) (m ) JP 1 = ⎣ 0 0 ⎦ JP 2 = ⎣ 0 0 ⎦ 0 0 1 0 ⎡ ⎤ ⎡ ⎤ 0 0 0 kr2 (m ) (m ) JO 1 = ⎣ 0 0 ⎦ JO 2 = ⎣ 0 0 ⎦ kr1 0 0 0 Fig. 7.4. Two-link planar arm where kri is the gear reduction ratio of Motor i. It is obvious to see that z 1 = [ 1 0 0 ]T , which greatly simplifies computation of the second term centres of mass of the two links, respectively. It is assumed that pmi = pi−1 in (4.34). and z mi = z i−1 , for i = 1, 2, i.e., the motors are located on the joint axes From (7.32), the inertia matrix is with centres of mass located at the origins of the respective frames. 2 With the chosen coordinate frames, computation of the Jacobians in (7.16), m 1 + mm2 + kr1 Im1 + m 2 0 B= 2 . (7.18) yields 0 m 2 + kr2 Im2 ⎡ ⎤ ⎡ ⎤ It is worth observing that B is constant, i.e., it does not depend on the arm −1 s1 0 −a1 s1 − 2 s12 −2 s12 ( 1) ( ) configuration. This implies also that C = O, i.e., there are no contributions J P = ⎣ 1 c1 0 ⎦ J P = ⎣ a1 c1 + 2 c12 2 2 c12 ⎦ , of centrifugal and Coriolis forces. As for the gravitational terms, since g 0 = 0 0 0 0 [ 0 0 −g ]T (g is gravity acceleration), (7.39) with the above Jacobians gives whereas computation of the Jacobians in (7.17), (7.19) yields g1 = (m 1 + mm2 + m 2 )g g2 = 0. ⎡ ⎤ ⎡ ⎤ 0 0 0 0 ( ) ( ) In the absence of friction and tip contact forces, the resulting equations of J O1 = ⎣ 0 0 ⎦ J O2 = ⎣ 0 0 ⎦ . motion are 1 0 1 1
2
(m 1 + mm2 + kr1 Im1 + m 2 )d¨1 + (m 1 + mm2 + m 2 )g = τ1 Notice that ω i , for i = 1, 2, is aligned with z0 , and thus Ri has no effect. It
is then possible to refer to the scalar moments of inertia I i .
(m + k 2 Im )d¨2 = τ2
2 r2 2 Computation of the Jacobians in (7.25), (7.26) yields
where τ1 and τ2 denote the forces applied to the two joints. Notice that a ⎡ ⎤ ⎡ ⎤ 0 0 −a1 s1 0 (m1 ) (m ) completely decoupled dynamics has been obtained. This is a consequence not JP = ⎣0 0⎦ JP 2 = ⎣ a1 c1 0 ⎦ , only of the Cartesian structures but also of the particular geometry; in other 0 0 0 0 words, if the second joint axis were not at a right angle with the first joint axis, the resulting inertia matrix would not be diagonal (see Problem 7.1). whereas computation of the Jacobians in (7.28), (7.29) yields ⎡ ⎤ ⎡ ⎤ 0 0 0 0 7.3.2 Two-link Planar Arm (m ) (m ) JO 1 = ⎣ 0 0 ⎦ JO 2 = ⎣ 0 0 ⎦ Consider the two-link planar arm in Fig. 7.4, for which the vector of general- kr1 0 1 kr2 ized coordinates is q = [ ϑ1 ϑ2 ]T . Let 1 , 2 be the distances of the centres where kri is the gear reduction ratio of Motor i. of mass of the two links from the respective joint axes. Also let m 1 , m 2 be From (7.32), the inertia matrix is the masses of the two links, and mm1 , mm2 the masses of the rotors of the two joint motors. Finally, let Im1 , Im2 be the moments of inertia with respect to b (ϑ ) b12 (ϑ2 ) the axes of the two rotors, and I 1 , I 2 the moments of inertia relative to the B(q) = 11 2 b21 (ϑ2 ) b22 7.3 Dynamic Model of Simple Manipulator Structures 267 268 7 Dynamics
b11 = I 1 + m 1 21 + kr1
2
Im1 + I 2 + m 2 (a21 + 22 + 2a1 2 c2 ) As for the gravitational terms, since g 0 = [ 0 −g 0 ]T , (7.39) with the
+Im2 + mm2 a21 above Jacobians gives
b12 = b21 = I 2 + m 2 (22 + a1 2 c2 ) + kr2 Im2 g1 = (m 1 1 + mm2 a1 + m 2 a1 )gc1 + m 2 2 gc12
b22 = I 2 + m 2 22 + kr2
2
Im2 . g2 = m 2 2 gc12 .
Compared to the previous example, the inertia matrix is now configuration- In the absence of friction and tip contact forces, the resulting equations of dependent. Notice that the term kr2 Im2 in the off-diagonal term of the inertia motion are matrix derives from having considered the rotational part of the motor ki- netic energy as due to the total angular velocity, i.e., its own angular velocity I 1 + m 1 21 + kr1 2 Im1 + I 2 + m 2 (a21 + 22 + 2a1 2 c2 ) + Im2 + mm2 a21 ϑ̈1 and that of the preceding link in the kinematic chain. At first approximation, especially in the case of high values of the gear reduction ratio, this contribu- + I 2 + m 2 (22 + a1 2 c2 ) + kr2 Im2 ϑ̈2 tion could be neglected; in the resulting reduced model, motor inertias would −2m 2 a1 2 s2 ϑ̇1 ϑ̇2 − m 2 a1 2 s2 ϑ̇22 appear uniquely in the elements on the diagonal of the inertia matrix with +(m 1 1 + mm2 a1 + m 2 a1 )gc1 + m 2 2 gc12 = τ1 (7.82) 2 terms of the type kri Imi . I 2 + m 2 (22 + a1 2 c2 ) + kr2 Im2 ϑ̈1 + I 2 + m 2 22 + kr2 2 Im2 ϑ̈2 The computation of Christoffel symbols as in (7.45) gives +m 2 a1 2 s2 ϑ̇21 + m 2 2 gc12 = τ2 1 ∂b11 c111 = =0 2 ∂q1 where τ1 and τ2 denote the torques applied to the joints. 1 ∂b11 Finally, it is wished to derive a parameterization of the dynamic model c112 = c121 = = −m 2 a1 2 s2 = h (7.82) according to the relation (7.81). By direct inspection of the expressions 2 ∂q2 ∂b12 1 ∂b22 of the joint torques, it is possible to find the following parameter vector: c122 = − =h ∂q2 2 ∂q1 π = [ π1 π2 π3 π4 π5 π6 π7 π8 ]T (7.83) ∂b21 1 ∂b11 c211 = − = −h ∂q1 2 ∂q2 π1 = m1 = m 1 + mm2 1 ∂b22 π2 = m1 C1 = m 1 (1 − a1 ) c212 = c221 = =0 2 ∂q1 π3 = I)1 = I + m (1 − a1 )2 + Im 1 1 2 1 ∂b22 c222 = = 0, π4 = Im1 2 ∂q2 π5 = m 2 = m 2 leading to the matrix π6 = m2 C2 = m 2 (2 − a2 ) hϑ̇2 h(ϑ̇1 + ϑ̇2 ) π7 = I)2 = I + m (2 − a2 )2 2 2 C(q, q̇) = . −hϑ̇1 0 π8 = Im2 , Computing the matrix N in (7.47) gives where the parameters for the augmented links have been found according to (7.77). It can be recognized that the number of non-null parameters is less N (q, q̇) = Ḃ(q) − 2C(q, q̇) than the maximum number of twenty-two parameters allowed in this case.6 2hϑ̇2 hϑ̇2 hϑ̇2 h(ϑ̇1 + ϑ̇2 ) The regressor in (7.81) is = −2 hϑ̇2 0 −hϑ̇1 0 y y12 y13 y14 y15 y16 y17 y18 0 −2hϑ̇1 − hϑ̇2 Y = 11 (7.84) = y21 y22 y23 y24 y25 y26 y27 y28 2hϑ̇1 + hϑ̇2 0 6 The number of parameters can be further reduced by resorting to a more accurate that allows the verification of the skew-symmetry property expressed by (7.48). inspection, which leads to finding a minimum number of five parameters; those See also Problem 7.2. turn out to be a linear combination of the parameters in (7.83) (see Problem 7.4). 7.3 Dynamic Model of Simple Manipulator Structures 269 270 7 Dynamics
y11 = a21 ϑ̈1 + a1 gc1 joint 1 pos joint 2 pos
y12 = 2a1 ϑ̈1 + gc1 4 4
y13 = ϑ̈1 3 3
2 2 2
y14 = kr1 ϑ̈1
[rad]
[rad]
y15 = (a1 + 2a1 a2 c2 + a22 )ϑ̈1 + (a1 a2 c2 + a22 )ϑ̈2 − 2a1 a2 s2 ϑ̇1 ϑ̇2
2 1 1
0 0
−a1 a2 s2 ϑ̇22 + a1 gc1 + a2 gc12 Ŧ1 Ŧ1
y16 = (2a1 c2 + 2a2 )ϑ̈1 + (a1 c2 + 2a2 )ϑ̈2 − 2a1 s2 ϑ̇1 ϑ̇2 − a1 s2 ϑ̇22 Ŧ2 Ŧ2
0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5
+gc12 [s] [s]
y17 = ϑ̈1 + ϑ̈2 joint 1 vel joint 2 vel
y18 = kr2 ϑ̈2 6 6
y21 = 0 4 4
[rad/s]
[rad/s]
y22 = 0
2 2
y23 = 0
y24 = 0 0 0
y25 = (a1 a2 c2 + a22 )ϑ̈1 + a22 ϑ̈2 + a1 a2 s2 ϑ̇21 + a2 gc12 0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5
[s] [s]
y26 = (a1 c2 + 2a2 )ϑ̈1 + 2a2 ϑ̈2 + a1 s2 ϑ̇21 + gc12
joint 1 acc joint 2 acc
y27 = ϑ̈1 + ϑ̈2 30 30
2 20 20
y28 = kr2 ϑ̈1 + kr2 ϑ̈2 .
10 10
[rad/s^2]
[rad/s^2]
0 0
Ŧ10 Ŧ10
Example 7.2 Ŧ20 Ŧ20
Ŧ30 Ŧ30
In order to understand the relative weight of the various torque contributions in the 0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 dynamic model (7.82), consider a two-link planar arm with the following data: [s] [s] joint 1 torque joint 2 torque a1 = a 2 = 1 m 1 = 2 = 0.5 m m1 = m2 = 50 kg I1 = I2 = 10 kg·m2 6000 6000 kr1 = kr2 = 100 mm1 = mm2 = 5 kg Im1 = Im2 = 0.01 kg·m2 . 4000 4000
The two links have been chosen equal to illustrate better the dynamic interaction 2000 2000
[Nm]
[Nm]
between the two joints. 0 0 Figure 7.5 shows the time history of positions, velocities, accelerations and Ŧ2000 Ŧ2000 torques resulting from joint trajectories with typical triangular velocity profile and Ŧ4000 Ŧ4000 equal time duration. The initial arm configuration is so that the tip is located at the Ŧ6000 Ŧ6000 point (0.2, 0) m with a lower elbow posture. Both joints make a rotation of π/2 rad 0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 [s] [s] in a time of 0.5 s. From the time history of the single torque contributions in Fig. 7.6 it can be Fig. 7.5. Time history of positions, velocities, accelerations and torques with joint recognized that: trajectories of equal duration • The inertia torque at Joint 1 due to Joint 1 acceleration follows the time history of the acceleration. • The inertia torque at Joint 2 due to Joint 2 acceleration is piecewise constant, since the inertia moment at Joint 2 axis is constant. 7.3 Dynamic Model of Simple Manipulator Structures 271 272 7 Dynamics
inert_11 inert_22 joint 1 pos joint 2 pos
5000 5000 4 4
3 3
2 2
[Nm]
[Nm]
[rad]
[rad]
0 0 1 1
0 0
Ŧ1 Ŧ1
Ŧ5000 Ŧ5000 Ŧ2 Ŧ2
0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 0 0.2 0.4 0.6 0 0.2 0.4 0.6
[s] [s] [s] [s]
inert_12 inert_21 joint 1 vel joint 2 vel
100 100 6 6
0 0 4 4
Ŧ100 Ŧ100 2 2
[rad/s]
[rad/s]
[Nm]
[Nm]
Ŧ200 Ŧ200 0 0
Ŧ300 Ŧ300 Ŧ2 Ŧ2
Ŧ400 Ŧ400 Ŧ4 Ŧ4
Ŧ500 Ŧ500 Ŧ6 Ŧ6
0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 0 0.2 0.4 0.6 0 0.2 0.4 0.6
[s] [s] [s] [s]
centrif_2 + coriol_12 centrif_1 joint 1 acc joint 2 acc
40 40
1000 1000
12 20 20
[rad/s^2]
[rad/s^2]
500 500
[Nm]
[Nm]
2 0 0
0 0
Ŧ20 Ŧ20
Ŧ500 Ŧ500
Ŧ40 Ŧ40
0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 0 0.2 0.4 0.6 0 0.2 0.4 0.6
[s] [s] [s] [s]
grav_1 grav_2 joint 1 torque joint 2 torque
800 800
5000 5000
600 600
400 400
[Nm]
[Nm]
[Nm]
[Nm]
0 0
200 200
0 0
Ŧ5000 Ŧ5000
Ŧ200 Ŧ200
0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 0 0.2 0.4 0.6 0 0.2 0.4 0.6
[s] [s] [s] [s]
Fig. 7.6. Time history of torque contributions with joint trajectories of equal du- Fig. 7.7. Time history of positions, velocities, accelerations and torques with joint ration trajectories of different duration 7.3 Dynamic Model of Simple Manipulator Structures 273 274 7 Dynamics
inert_11 inert_22 tip pos
6000 6000 2
4000 4000
1.5
2000 2000
[Nm]
[Nm]
0 0
[m]
1
Ŧ2000 Ŧ2000
Ŧ4000 Ŧ4000 0.5
Ŧ6000 Ŧ6000
0
0 0.2 0.4 0.6 0 0.2 0.4 0.6 0 0.1 0.2 0.3 0.4 0.5
[s] [s] [s]
inert_12 inert_21 tip vel
1500 1500
5
1000 1000
4
500 500
[m/s]
[Nm]
[Nm]
3
0 0 2
Ŧ500 Ŧ500 1
0
0 0.2 0.4 0.6 0 0.2 0.4 0.6 0 0.1 0.2 0.3 0.4 0.5
[s] [s] [s]
centrif_2 + coriol_12 centrif_1 tip acc
40
1000 1000
12 20
500 500
[m/s^2]
[Nm]
[Nm]
0
0 0
2 Ŧ20
Ŧ500 Ŧ500
Ŧ40
0 0.2 0.4 0.6 0 0.2 0.4 0.6 0 0.1 0.2 0.3 0.4 0.5
[s] [s] [s]
grav_1 grav_2
1000 1000
Fig. 7.9. Time history of tip position, velocity and acceleration with a straight line
tip trajectory along the horizontal axis
800 800
600 600
•
[Nm]
[Nm]
The inertia torques at each joint due to acceleration of the other joint confirm
400 400 the symmetry of the inertia matrix, since the acceleration profiles are the same
200 200 for both joints.
• The Coriolis effect is present only at Joint 1, since the arm tip moves with respect
0 0
to the mobile frame attached to Link 1 but is fixed with respect to the frame
0 0.2 0.4 0.6 0 0.2 0.4 0.6 attached to Link 2.
[s] [s]
• The centrifugal and Coriolis torques reflect the above symmetry.
Fig. 7.8. Time history of torque contributions with joint trajectories of different Figure 7.7 shows the time history of positions, velocities, accelerations and duration torques resulting from joint trajectories with typical trapezoidal velocity profile and different time duration. The initial configuration is the same as in the previous case. The two joints make a rotation so as to take the tip to the point (1.8, 0) m. The acceleration time is 0.15 s and the maximum velocity is 5 rad/s for both joints. 7.3 Dynamic Model of Simple Manipulator Structures 275 276 7 Dynamics
joint 1 pos joint 2 pos x 10
4 inert_11 x 10
4 inert_22
4 4 1 1
3 3
0.5 0.5
2 2
[Nm]
[Nm]
[rad]
[rad]
1 1 0 0
0 0
Ŧ0.5 Ŧ0.5
Ŧ1 Ŧ1
Ŧ2 Ŧ2 Ŧ1 Ŧ1
0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5
[s] [s] [s] [s]
joint 1 vel joint 2 vel inert_12 inert_21
3000 3000
4 4
2 2 2000 2000
0 0
[rad/s]
[rad/s]
[Nm]
[Nm]
1000 1000
Ŧ2 Ŧ2
Ŧ4 Ŧ4 0 0
Ŧ6 Ŧ6 Ŧ1000 Ŧ1000
Ŧ8 Ŧ8
0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5
[s] [s] [s] [s]
joint 1 acc joint 2 acc centrif_2 + coriol_12 centrif_1
80 80
1000 1000
60 60
500 12 500
40 40
[rad/s^2]
[rad/s^2]
[Nm]
[Nm]
20 20 0 0
0 0
Ŧ500 2 Ŧ500
Ŧ20 Ŧ20
Ŧ1000 Ŧ1000
Ŧ40 Ŧ40
0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5
[s] [s] [s] [s]
joint 1 torque joint 2 torque grav_1 grav_2
1000 1000
800 800
5000 5000
600 600
[Nm]
[Nm]
[Nm]
[Nm]
400 400
0 0
200 200
Ŧ5000 Ŧ5000 0 0
0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5 0 0.1 0.2 0.3 0.4 0.5
[s] [s] [s] [s]
Fig. 7.10. Time history of joint positions, velocities, accelerations, and torques with Fig. 7.11. Time history of joint torque contributions with a straight line tip tra- a straight line tip trajectory along the horizontal axis jectory along the horizontal axis 7.3 Dynamic Model of Simple Manipulator Structures 277 278 7 Dynamics
From the time history of the single torque contributions in Fig. 7.8 it can be
recognized that: • The inertia torque at Joint 1 due to Joint 2 acceleration is opposite to that at Joint 2 due to Joint 1 acceleration in that portion of trajectory when the two accelerations have the same magnitude but opposite sign. • The different velocity profiles imply that the centrifugal effect induced at Joint 1 by Joint 2 velocity dies out later than the centrifugal effect induced at Joint 2 by Joint 1 velocity. • The gravitational torque at Joint 2 is practically constant in the first portion of the trajectory, since Link 2 is almost kept in the same posture. As for the gravitational torque at Joint 1, instead, the centre of mass of the articulated system moves away from the origin of the axes. Finally, Fig. 7.9 shows the time history of tip position, velocity and acceleration for a trajectory with a trapezoidal velocity profile. Starting from the same initial posture as above, the arm tip makes a translation of 1.6 m along the horizontal axis; the acceleration time is 0.15 s and the maximum velocity is 5 m/s. Fig. 7.12. Parallelogram arm As a result of an inverse kinematics procedure, the time history of joint positions, velocities and accelerations have been computed which are illustrated in Fig. 7.10, and ⎡ ⎤ together with the joint torques that are needed to execute the assigned trajectory. −1 s1 ( ) It can be noticed that the time history of the represented quantities differs from J P 1 = ⎣ 1 c1 ⎦, the corresponding ones in the operational space, in view of the nonlinear effects 0 introduced by kinematic relations. For what concerns the time history of the individual torque contributions in whereas computation of the Jacobians in (7.17), (7.19) yields Fig. 7.11, it is possible to make a number of remarks similar to those made above ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ for trajectories assigned directly in the joint space. 0 0 0 0 0 0 0 0 0 ( 1 ) ( ) ( ) JO = ⎣ 0 0 0 ⎦ J O2 = ⎣ 0 0 0 ⎦ J O3 = ⎣ 0 0 0⎦ 1 0 0 1 1 0 1 1 1 and ⎡ ⎤ 7.3.3 Parallelogram Arm 0 ( ) J O 1 = ⎣ 0 ⎦ . Consider the parallelogram arm in Fig. 7.12. Because of the presence of the 1 closed chain, the equivalent tree-structured open-chain arm is initially taken From (7.32), the inertia matrix of the virtual arm composed of joints ϑ1 , into account. Let 1 , 2 , 3 and 1 be the distances of the centres of mass ϑ2 , ϑ3 is of the three links along one branch of the tree, and of the single link along ⎡ ⎤ the other branch, from the respective joint axes. Also let m 1 , m 2 , m 3 and b1 1 (ϑ2 , ϑ3 ) b1 2 (ϑ2 , ϑ3 ) b1 3 (ϑ2 , ϑ3 ) m 1 be the masses of the respective links, and I 1 , I 2 , I 3 and I 1 the B (q ) = ⎣ b2 1 (ϑ2 , ϑ3 ) b2 2 (ϑ3 ) b2 3 (ϑ3 ) ⎦ moments of inertia relative to the centres of mass of the respective links. For b3 1 (ϑ2 , ϑ3 ) b3 2 (ϑ3 ) b3 3 the sake of simplicity, the contributions of the motors are neglected. With the chosen coordinate frames, computation of the Jacobians in (7.16) b1 1 = I 1 + m 1 21 + I 2 + m 2 (a21 + 22 + 2a1 2 c2 ) + I 3 (7.18) yields +m 3 (a21 + a22 + 23 + 2a1 a2 c2 + 2a1 3 c2 3 + 2a2 3 c3 ) ⎡ ⎤ ⎡ ⎤ b1 2 = b2 1 = I 2 + m 2 (22 + a1 2 c2 ) + I 3 −1 s1 0 0 −a1 s1 − 2 s1 2 −2 s1 2 0 ( 1 ) J P = ⎣ 1 c1 0 0⎦ ( ) J P 2 = ⎣ a1 c1 + 2 c1 2 2 c1 2 0⎦ +m 3 (a22 + 23 + a1 a2 c2 + a1 3 c2 3 + 2a2 3 c3 ) 0 0 0 0 0 0 b1 3 = b31 = I 3 + m 3 (23 + a1 3 c2 3 + a2 3 c3 ) ⎡ ⎤ b2 2 = I 2 + m 2 22 + I 3 + m 3 (a22 + 23 + 2a2 3 c3 ) −a1 s1 − a2 s1 2 − 3 s1 2 3 −a2 s1 2 − 3 s1 2 3 −3 s1 2 3 ( 3 ) J P = ⎣ a1 c1 + a2 c1 2 + 3 c1 2 3 a2 c1 2 + 3 c1 2 3 3 c1 2 3 ⎦ b2 3 = I 3 + m 3 (23 + a2 3 c3 ) 0 0 0 b3 3 = I 3 + m 3 23 7.3 Dynamic Model of Simple Manipulator Structures 279 280 7 Dynamics
while the moment of inertia of the virtual arm composed of just joint ϑ1 is As for the gravitational terms, since g 0 = [ 0 −g 0 ]T , (7.39) with the above Jacobians gives b1 1 = I 1 + m 1 21 . g1 (m 1 1 + m 2 a1 + m 3 a1 )gc1 + (m 2 2 + m 3 a2 )gc1 2 Therefore, the inertial torque contributions of the two virtual arms are re- +m 3 3 gc1 2 3 spectively: 3 g2 (m 2 2 + m 3 a2 )gc1 2 + m 3 3 gc1 2 3 τi = bi j ϑ̈j τ1 = b1 1 ϑ̈1 . g3 m 3 3 gc1 2 3 j =1 and At this point, in view of (2.64) and (3.121), the inertial torque contribu- g1 = m 1 1 gc1 . tions at the actuated joints for the closed-chain arm turn out to be Composing the various contributions as done above yields τ a = B a q̈ a (m 1 1 + m 2 a1 − m 3 ¯3 )gc1 ga = where q a = [ ϑ1 T ϑ1 ] , τ a = [ τa1 T τa2 ] and (m 1 1 + m 2 2 + m 3 a1 )gc1 which, together with the inertial torques, completes the derivation of the b ba12 B a = a11 sought dynamic model. ba21 ba22 A final comment is in order. In spite of its kinematic equivalence with the ba11 = I 1 + m 1 21 + m 2 a21 + I 3 + m 3 23 + m 3 a21 two-link planar arm, the dynamic model of the parallelogram is remarkably lighter. This property is quite advantageous for trajectory planning and con- −2a1 m 3 3 trol purposes. For this reason, apart from obvious considerations related to ba12 = ba21 = a1 m 2 2 + a1 m 3 (a1 − 3 ) cos (ϑ1 − ϑ1 ) manipulation of heavy payloads, the adoption of closed kinematic chains in ba22 = I 1 + m 1 21 + I 2 + m 2 22 + m 3 a21 . the design of industrial robots has received a great deal of attention.
This expression reveals the possibility of obtaining a configuration-independent and decoupled inertia matrix; to this end it is sufficient to design the four links 7.4 Dynamic Parameter Identification of the parallelogram so that The use of the dynamic model for solving simulation and control problems de- m 3 ¯3 a1 mands the knowledge of the values of dynamic parameters of the manipulator = m 2 2 a1 model. Computing such parameters from the design data of the mechanical struc- where ¯3 = 3 − a1 is the distance of the centre of mass of Link 3 from the ture is not simple. CAD modelling techniques can be adopted which allow the axis of Joint 4. If this condition is satisfied, then the inertia matrix is diagonal computation of the values of the inertial parameters of the various components (ba12 = ba21 = 0) with (links, actuators and transmissions) on the basis of their geometry and type of materials employed. Nevertheless, the estimates obtained by such techniques 2 2 2 ¯3 ba11 = I 1 + m 1 1 + m 2 a1 1 + + I 3 are inaccurate because of the simplification typically introduced by geometric a1 a1 modelling; moreover, complex dynamic effects, such as joint friction, cannot a1 a1 be taken into account. ba22 = I 1 + m 1 21 + I 2 + m 2 22 1 + . 2 ¯3 A heuristic approach could be to dismantle the various components of the manipulator and perform a series of measurements to evaluate the inertial As a consequence, no contributions of Coriolis and centrifugal torques are parameters. Such technique is not easy to implement and may be troublesome obtained. Such a result could not be achieved with the previous two-link to measure the relevant quantities. planar arm, no matter how the design parameters were chosen. In order to find accurate estimates of dynamic parameters, it is worth resorting to identification techniques which conveniently exploit the property of linearity (7.81) of the manipulator model with respect to a suitable set of 7.4 Dynamic Parameter Identification 281 282 7 Dynamics
dynamic parameters. Such techniques allow the computation of the parameter of manipulator links and joints, are non-identifiable, since for any trajectory vector π from the measurements of joint torques τ and of relevant quantities assigned to the structure they do not contribute to the equations of motion. A for the evaluation of the matrix Y , when suitable motion trajectories are direct consequence is that the columns of the matrix Y in (7.80) correspond- imposed to the manipulator. ing to such parameters are null and thus they have to be removed from the On the assumption that the kinematic parameters in the matrix Y are matrix itself; e.g., the resulting (2 × 8) matrix in (7.84). known with good accuracy, e.g., as a result of a kinematic calibration, mea- Another issue to consider about determination of the effective number surements of joint positions q, velocities q̇ and accelerations q̈ are required. of parameters that can be identified by (7.86) is that some parameters can Joint positions and velocities can be actually measured while numerical recon- be identified in linear combinations whenever they do not appear isolated in struction of accelerations is needed; this can be performed on the basis of the the equations. In such a case, it is necessary, for each linear combination, to position and velocity values recorded during the execution of the trajectories. remove as many columns of the matrix Y as the number of parameters in the The reconstructing filter does not work in real time and thus it can also be linear combination minus one. anti-causal, allowing an accurate reconstruction of the accelerations. For the determination of the minimum number of identifiable parameters As regards joint torques, in the unusual case of torque sensors at the that allow direct application of the least-squares technique based on (7.86), joint, these can be measured directly. Otherwise, they can be evaluated from it is possible to inspect directly the equations of the dynamic model, as long either wrist force measurements or current measurements in the case of electric as the manipulator has few joints. Otherwise, numerical techniques based on actuators. singular value decomposition of matrix Ȳ have to be used. If the matrix Ȳ If measurements of joint torques, positions, velocities and accelerations resulting from a series of measurements is not full-rank, one has to resort to have been obtained at given time instants t1 , … , tN along a given trajectory, a damped least-squares inverse of Ȳ where solution accuracy depends on the one may write ⎡ ⎤ ⎡ ⎤ weight of the damping factor. τ (t1 ) Y (t1 ) In the above discussion, the type of trajectory imposed to the manipulator ⎢ ⎥ ⎢ ⎥ τ̄ = ⎣ … ⎦ = ⎣ … ⎦ π = Ȳ π. (7.85) joints has not been explicitly addressed. It can be generally ascertained that the choice should be oriented in favor of polynomial type trajectories which are τ (tN ) Y (tN ) sufficiently rich to allow an accurate evaluation of the identifiable parameters. The number of time instants sets the number of measurements to perform T This corresponds to achieving a low condition number of the matrix Ȳ Ȳ and should be large enough (typically N n p) so as to avoid ill-conditioning along the trajectory. On the other hand, such trajectories should not excite of matrix Ȳ . Solving (7.85) by a least-squares technique leads to the solution any unmodelled dynamic effects such as joint elasticity or link flexibility that in the form would naturally lead to unreliable estimates of the dynamic parameters to π = (Ȳ Ȳ )−1 Ȳ τ̄ T T (7.86) identify. T T Finally, it is worth observing that the technique presented above can also where (Ȳ Ȳ )−1 Ȳ is the left pseudo-inverse matrix of Ȳ . be extended to the identification of the dynamic parameters of an unknown It should be noticed that, in view of the block triangular structure of payload at the manipulator’s end-effector. In such a case, the payload can be matrix Y in (7.80), computation of parameter estimates could be simplified regarded as a structural modification of the last link and one may proceed to by resorting to a sequential procedure. Take the equation τn = y Tnn π n and identify the dynamic parameters of the modified link. To this end, if a force solve it for π n by specifying τn and y Tnn for a given trajectory on Joint n. sensor is available at the manipulator’s wrist, it is possible to characterize By iterating the procedure, the manipulator parameters can be identified on directly the dynamic parameters of the payload starting from force sensor the basis of measurements performed joint by joint from the outer link to the measurements. base. Such procedure, however, may have the inconvenience to accumulate any error due to ill-conditioning of the matrices involved step by step. It may then be worth operating with a global procedure by imposing motions on all manipulator joints at the same time. 7.5 Newton–Euler Formulation Regarding the rank of matrix Ȳ , it is possible to identify only the dynamic parameters of the manipulator that contribute to the dynamic model. Exam- In the Lagrange formulation, the manipulator dynamic model is derived start- ple 7.2 has indeed shown that for the two-link planar arm considered, only ing from the total Lagrangian of the system. On the other hand, the Newton– 8 out of the 22 possible dynamic parameters appear in the dynamic model. Euler formulation is based on a balance of all the forces acting on the generic Hence, there exist some dynamic parameters which, in view of the disposition link of the manipulator. This leads to a set of equations whose structure allows a recursive type of solution; a forward recursion is performed for propagating 7.5 Newton–Euler Formulation 283 284 7 Dynamics
• μi moment exerted by Link i − 1 on Link i with respect to origin of
Frame i − 1,
• −μi+1 moment exerted by Link i + 1 on Link i with respect to origin of
Frame i.
Initially, all the vectors and matrices are assumed to be expressed with
reference to the base frame.
As already anticipated, the Newton–Euler formulation describes the mo-
tion of the link in terms of a balance of forces and moments acting on it.
The Newton equation for the translational motion of the centre of mass
can be written as
f i − f i+1 + mi g 0 = mi p̈Ci . (7.87)
The Euler equation for the rotational motion of the link (referring mo-
ments to the centre of mass) can be written as
Fig. 7.13. Characterization of Link i for Newton–Euler formulation d
μi + f i × r i−1,Ci − μi+1 − f i+1 × r i,Ci = (Ī i ω i + kr,i+1 q̇i+1 Imi+1 z mi+1 ),
dt
(7.88)
link velocities and accelerations, followed by a backward recursion for propa- where (7.67) has been used for the angular momentum of the rotor. Notice gating forces. that the gravitational force mi g 0 does not generate any moment, since it is Consider the generic augmented Link i (Link i plus motor of Joint i + 1) of concentrated at the centre of mass. the manipulator kinematic chain (Fig. 7.13). According to what was presented As pointed out in the above Lagrange formulation, it is convenient to in Sect. 7.2.2, one can refer to the centre of mass Ci of the augmented link to express the inertia tensor in the current frame (constant tensor). Hence, ac- characterize the following parameters: i cording to (7.12), one has Ī i = Ri Ī i RTi , where Ri is the rotation matrix from • mi mass of augmented link, Frame i to the base frame. Substituting this relation in the first term on the • Ī i inertia tensor of augmented link, right-hand side of (7.88) yields • Imi moment of inertia of rotor, d i i T i • r i−1,Ci vector from origin of Frame (i − 1) to centre of mass Ci , (Ī i ω i ) = Ṙi Ī i RTi ω i + Ri Ī i Ṙi ω i + Ri Ī i RTi ω̇ i (7.89) • r i,Ci vector from origin of Frame i to centre of mass Ci , dt i T i T T i T • r i−1,i vector from origin of Frame (i − 1) to origin of Frame i. = S(ω i )Ri Ī i Ri ω i + Ri Ī i Ri S (ω i )ω i + Ri Ī i Ri ω̇ i = Ī i ω̇ i + ω i × (Ī i ω i ) The velocities and accelerations to be considered are: • ṗCi linear velocity of centre of mass Ci , where the second term represents the gyroscopic torque induced by the depen- • ṗi linear velocity of origin of Frame i, dence of Ī i on link orientation.7 Moreover, by observing that the unit vector • ω i angular velocity of link, z mi+1 rotates accordingly to Link i, the derivative needed in the second term • ω mi angular velocity of rotor, on the right-hand side of (7.88) is • p̈Ci linear acceleration of centre of mass Ci , d • p̈i linear acceleration of origin of Frame i, (q̇i+1 Imi+1 z mi+1 ) = q̈i+1 Imi+1 z mi+1 + q̇i+1 Imi+1 ω i × z mi+1 (7.90) dt • ω̇ i angular acceleration of link, • ω̇ mi angular acceleration of rotor, By substituting (7.89), (7.90) in (7.88), the resulting Euler equation is • g 0 gravity acceleration. μi + f i × r i−1,Ci −μi+1 − f i+1 × r i,Ci = Ī i ω̇ i + ω i × (Ī i ω i ) (7.91) The forces and moments to be considered are: +kr,i+1 q̈i+1 Imi+1 z mi+1 + kr,i+1 q̇i+1 Imi+1 ω i × z mi+1 . • f i force exerted by Link i − 1 on Link i, 7 In deriving (7.89), the operator S has been introduced to compute the derivative • −f i+1 force exerted by Link i + 1 on Link i, of Ri , as in (3.8); also, the property S T (ω i )ω i = 0 has been utilized. 7.5 Newton–Euler Formulation 285 286 7 Dynamics
The generalized force at Joint i can be computed by projecting the force In summary, the equations in (7.95), (7.96), (7.98), (7.99) can be compactly
f i for a prismatic joint, or the moment μi for a revolute joint, along the rewritten as joint axis. In addition, there is the contribution of the rotor inertia torque ω̇ i−1 for a prismatic joint kri Imi ω̇ Tmi z mi . Hence, the generalized force at Joint i is expressed by ω̇ i = (7.100) ω̇ i−1 + ϑ̈i z i−1 + ϑ̇i ω i−1 × z i−1 for a revolute joint T f i z i−1 + kri Imi ω̇ Tmi z mi for a prismatic joint and τi = (7.92) ⎧ μTi z i−1 + kri Imi ω̇ Tmi z mi for a revolute joint. ⎪ p̈i−1 + d¨i z i−1 + 2d˙i ω i × z i−1 for a prismatic joint ⎪ ⎪ ⎨ +ω̇ i × r i−1,i + ω i × (ω i × r i−1,i ) 7.5.1 Link Accelerations p̈= (7.101) i ⎪ ⎪ p̈i−1 + ω̇ i × r i−1,i for a revolute joint. ⎪ ⎩ The Newton–Euler equations in (7.87), (7.91) and the equation in (7.92) re- +ω i × (ω i × r i−1,i ) quire the computation of linear and angular acceleration of Link i and Rotor The acceleration of the centre of mass of Link i required by the Newton i. This computation can be carried out on the basis of the relations expressing equation in (7.87) can be derived from (3.15), since ṙ ii,Ci = 0; by differenti- the linear and angular velocities previously derived. The equations in (3.21), ating (3.15) with respect to time, the acceleration of the centre of mass Ci (3.22), (3.25), (3.26) can be briefly rewritten as can be expressed as a function of the velocity and acceleration of the origin of Frame i, i.e., ω i−1 for a prismatic joint ωi = (7.93) ω i−1 + ϑ̇i z i−1 for a revolute joint p̈Ci = p̈i + ω̇ i × r i,Ci + ω i × (ω i × r i,Ci ). (7.102) and Finally, the angular acceleration of the rotor can be obtained by time ṗi−1 + d˙i z i−1 + ω i × r i−1,i for a prismatic joint differentiation of (7.23), i.e., ṗi = (7.94) ṗi−1 + ω i × r i−1,i for a revolute joint. ω̇ mi = ω̇ i−1 + kri q̈i z mi + kri q̇i ω i−1 × z mi . (7.103) As for the angular acceleration of the link, it can be seen that, for a prismatic joint, differentiating (3.21) with respect to time gives 7.5.2 Recursive Algorithm ω̇ i = ω̇ i−1 , (7.95) It is worth remarking that the resulting Newton–Euler equations of motion are not in closed form, since the motion of a single link is coupled to the whereas, for a revolute joint, differentiating (3.25) with respect to time gives motion of the other links through the kinematic relationship for velocities ω̇ i = ω̇ i−1 + ϑ̈i z i−1 + ϑ̇i ω i−1 × z i−1 . (7.96) and accelerations. Once the joint positions, velocities and accelerations are known, one can As for the linear acceleration of the link, for a prismatic joint, differenti- compute the link velocities and accelerations, and the Newton–Euler equations ating (3.22) with respect to time gives can be utilized to find the forces and moments acting on each link in a recur- sive fashion, starting from the force and moment applied to the end-effector. p̈i = p̈i−1 + d¨i z i−1 + d˙i ω i−1 × z i−1 + ω̇ i × r i−1,i (7.97) On the other hand, also link and rotor velocities and accelerations can be +ω i × d˙i z i−1 + ω i × (ω i−1 × r i−1,i ) computed recursively starting from the velocity and acceleration of the base link. In summary, a computationally recursive algorithm can be constructed where the relation ṙ i−1,i = d˙i z i−1 + ω i−1 × r i−1,i has been used. Hence, in that features a forward recursion relative to the propagation of velocities and view of (3.21), the equation in (7.97) can be rewritten as accelerations and a backward recursion for the propagation of forces and mo- ments along the structure. p̈i = p̈i−1 + d¨i z i−1 + 2d˙i ω i × z i−1 + ω̇ i × r i−1,i + ω i × (ω i × r i−1,i ). (7.98) For the forward recursion, once q, q̇, q̈, and the velocity and acceleration of the base link ω 0 , p̈0 − g 0 , ω̇ 0 are specified, ω i , ω̇ i , p̈i , p̈Ci , ω̇ mi can be Also, for a revolute joint, differentiating (3.26) with respect to time gives computed using (7.93), (7.100), (7.101), (7.102), (7.103), respectively. Notice that the linear acceleration has been taken as p̈0 − g 0 so as to incorporate the p̈i = p̈i−1 + ω̇ i × r i−1,i + ω i × (ω i × r i−1,i ). (7.99) 7.5 Newton–Euler Formulation 287 288 7 Dynamics
term −g 0 in the computation of the acceleration of the centre of mass p̈Ci via (7.101), (7.102). Having computed the velocities and accelerations with the forward recur- sion from the base link to the end-effector, a backward recursion can be carried out for the forces. In detail, once he = [ f Tn+1 μTn+1 ]T is given (eventually he = 0), the Newton equation in (7.87) to be used for the recursion can be rewritten as f i = f i+1 + mi p̈Ci (7.104) since the contribution of gravity acceleration has already been included in p̈Ci . Further, the Euler equation gives μi = −f i × (r i−1,i + r i,Ci ) + μi+1 + f i+1 × r i,Ci + Ī i ω̇ i + ω i × (Ī i ω i ) +kr,i+1 q̈i+1 Imi+1 z mi+1 + kr,i+1 q̇i+1 Imi+1 ω i × z mi+1 (7.105) which derives from (7.91), where r i−1,Ci has been expressed as the sum of the two vectors appearing already in the forward recursion. Finally, the general- ized forces resulting at the joints can be computed from (7.92) as ⎧ T ⎪ ⎪ f i z i−1 + kri Imi ω̇ Tmi z mi ⎪ ⎨ +F d˙ + F sgn (d˙ ) vi i si i for a prismatic joint τi = (7.106) ⎪ ⎪ T T μi z i−1 + kri Imi ω̇ mi z mi ⎪ ⎩ +Fvi ϑ̇i + Fsi sgn (ϑ̇i ) for a revolute joint, where joint viscous and Coulomb friction torques have been included. Fig. 7.14. Computational structure of the Newton–Euler recursive algorithm In the above derivation, it has been assumed that all vectors were referred to the base frame. To simplify greatly computation, however, the recursion is computationally more efficient if all vectors are referred to the current frame p̈iCi = p̈ii + ω̇ ii × r ii,Ci + ω ii × (ω ii × r ii,Ci ) (7.110) on Link i. This implies that all vectors that need to be transformed from mi = ω̇ i−1 + kri q̈i z mi + kri q̇i ω i−1 × z mi ω̇ i−1 i−1 i−1 i−1 i−1 Frame i + 1 into Frame i have to be multiplied by the rotation matrix Rii+1 , (7.111) whereas all vectors that need to be transformed from Frame i − 1 into Frame i f ii = Rii+1 f i+1 i i+1 + mi p̈Ci (7.112) have to be multiplied by the rotation matrix Ri−1 i T . Therefore, the equations in (7.93), (7.100), (7.101), (7.102), (7.103), (7.104), (7.105), (7.106) can be μii = −f ii × (r ii−1,i +r ii,Ci ) + Rii+1 μi+1 i+1 + Ri+1 f i+1 × r i,Ci i i+1 i (7.113) rewritten as: i i i−1 T i−1 +Ī i ω̇ ii + ω ii × (Ī i ω ii ) Ri ω i−1 for a prismatic joint i ωi =i (7.107) +ω ii × (Ī i ω ii ) + kr,i+1 q̈i+1 Imi+1 z imi+1 + kr,i+1 q̇i+1 Imi+1 ω ii × z imi+1 Ri−1 i T (ω i−1 i−1 + ϑ̇i z 0 ) for a revolute joint ⎧ i T i−1 T i−1 T i−1 ⎪ ⎪ f i Ri z 0 + kri Imi ω̇ i−1 T i−1 mi z mi Ri ω̇ i−1 for a prismatic joint ⎪ ⎨ +Fvi d˙i + Fsi sgn (d˙i ) i for a prismatic joint ω̇ i = (7.108) τi = (7.114) Ri−1 T i−1 + ϑ̈i z 0 + ϑ̇i ω i−1 × z 0 ) for a revolute joint (ω̇ i−1 i−1 ⎪ ⎪ μi T Ri−1 T z 0 + kri Imi ω̇ i−1 T i−1 i ⎪ ⎩ i i mi z mi ⎧ i−1 +Fvi ϑ̇i + Fsi sgn (ϑ̇i ) for a revolute joint. ⎪ ⎪ Ri T (p̈i−1 ¨ ˙ i i−1 + di z 0 ) + 2di ω i × Ri i−1 T z0 ⎪ ⎪ ⎪ ⎨ +ω̇ ii × r ii−1,i + ω ii × (ω ii × r ii−1,i ) for a prismatic joint The above equations have the advantage that the quantities Ī i , r ii,Ci , z i−1 mi i
p̈ii = (7.109) are constant; further, it is z 0 = [ 0 0 1 ]T .
⎪
⎪ p̈i−1 + ω̇ i × r ii−1,i
i−1 T i−1 i
⎪
⎪
Ri To summarize, for given joint positions, velocities and accelerations, the
⎪
⎩
+ω ii × (ω ii × r ii−1,i ) for a revolute joint recursive algorithm is carried out in the following two phases:
7.5 Newton–Euler Formulation 289 290 7 Dynamics ⎡ ⎤ • With known initial conditions ω 00 , p̈00 − g 00 , and ω̇ 00 , use (7.107), (7.108), 0 ⎢ ⎥ (7.109), (7.110), (7.111), for i = 1, … , n, to compute ω ii , ω̇ ii , p̈ii , p̈iCi , ω̇ i−1 mi . ω̇ 11 = ⎣ 0 ⎦ • With known terminal conditions f n+1 n+1 and μ n+1 n+1 , use (7.112), (7.113), for ϑ̈1 i i i = n, … , 1, to compute f i , μi , and then (7.114) to compute τi . ⎡ ⎤ −a1 ϑ̇21 + gs1 The computational structure of the algorithm is schematically illustrated ⎢ ⎥ in Fig. 7.14. p̈11 = ⎣ a1 ϑ̈1 + gc1 ⎦ 0 7.5.3 Example ⎡ ⎤ −(C1 + a1 )ϑ̇21 + gs1 ⎢ ⎥ In the following, an example to illustrate the single steps of the Newton– p̈1C1 = ⎣ (C1 + a1 )ϑ̈1 + gc1 ⎦ Euler algorithm is developed. Consider the two-link planar arm whose dy- 0 namic model has already been derived in Example 7.2. Start by imposing the initial conditions for the velocities and accelerations: ⎡ ⎤ 0 ⎢ ⎥ p̈00 − g 00 = [ 0 g 0 ]T ω 00 = ω̇ 00 = 0, ω̇ 0m1 = ⎣ 0 ⎦. kr1 ϑ̈1 and the terminal conditions for the forces: • Forward recursion: Link 2 f 33 = 0 μ33 = 0. ⎡ ⎤ All quantities are referred to the current link frame. As a consequence, the 0 ⎢ ⎥ following constant vectors are obtained: ω 22 = ⎣ 0 ⎦ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ⎡ ⎤ ϑ̇1 + ϑ̇2 C1 a1 C2 a2 r 1,C1 = ⎣ 0 ⎦ r 0,1 = ⎣ 0 ⎦ r 2,C2 = ⎣ 0 ⎦ r 1,2 = ⎣ 0 ⎦ 1 1 2 2 ⎡ ⎤ 0 0 0 0 0 ⎢ ⎥ ω̇ 22 = ⎣ 0 ⎦ where C1 and C2 are both negative quantities. The rotation matrices needed ϑ̈1 + ϑ̈2 for vector transformation from one frame to another are ⎡ ⎤ ⎡ ⎤ ci −si 0 a1 s2 ϑ̈1 − a1 c2 ϑ̇21 − a2 (ϑ̇1 + ϑ̇2 )2 + gs12 ⎢ ⎥ Ri−1 i = ⎣ si ci 0 ⎦ i = 1, 2 R23 = I. p̈22 = ⎣ a1 c2 ϑ̈1 + a2 (ϑ̈1 + ϑ̈2 ) + a1 s2 ϑ̇21 + gc12 ⎦ 0 0 1 0 Further, it is assumed that the axes of rotation of the two rotors coincide with ⎡ ⎤ the respective joint axes, i.e., z i−1 a1 s2 ϑ̈1 − a1 c2 ϑ̇21 − (C2 + a2 )(ϑ̇1 + ϑ̇2 )2 + gs12 T mi = z 0 = [ 0 0 1 ] for i = 1, 2. According to (7.107)–(7.114), the Newton–Euler algorithm requires the ⎢ ⎥ p̈2C2 = ⎣ a1 c2 ϑ̈1 + (C2 + a2 )(ϑ̈1 + ϑ̈2 ) + a1 s2 ϑ̇21 + gc12 ⎦ execution of the following steps: 0 • Forward recursion: Link 1 ⎡ ⎤ 0 ⎡ ⎤ ⎢ ⎥ 0 ω̇ 1m2 = ⎣ 0 ⎦. ⎢ ⎥ ω 11 = ⎣ 0 ⎦ ϑ̈1 + kr2 ϑ̈2 ϑ̇1 7.5 Newton–Euler Formulation 291 292 7 Dynamics
• Backward recursion: Link 2 As for the moment components, those marked by the symbol ‘∗’ have not been computed, since they are not related to the joint torques τ2 and τ1 . ⎡ ⎤ Expressing the dynamic parameters in the above torques as a function of m2 a1 s2 ϑ̈1 − a1 c2 ϑ̇21 − (C2 + a2 )(ϑ̇1 + ϑ̇2 )2 + gs12 ⎢ ⎥ the link and rotor parameters as in (7.83) yields f 22 = ⎣ m2 a1 c2 ϑ̈1 + (C2 + a2 )(ϑ̈1 + ϑ̈2 ) + a1 s2 ϑ̇21 + gc12 ⎦ 0 m1 = m 1 + mm2 m1 C1 = m 1 (1 − a1 ) ⎡ ⎤ ∗ I¯1zz + m1 2 = I)1 = I + m (1 − a1 )2 + Im C1 1 1 2 ⎢ ∗ ⎥ ⎢ ⎥ m2 = m 2 μ22 = ⎢ ⎥ ⎣ I¯2zz (ϑ̈1 + ϑ̈2 ) + m2 (C2 + a2 )2 (ϑ̈1 + ϑ̈2 ) + m2 a1 (C2 + a2 )c2 ϑ̈1 ⎦ m2 C2 = m 2 (2 − a2 ) +m2 a1 (C2 + a2 )s2 ϑ̇21 + m2 (C2 + a2 )gc12 I¯2zz + m2 2C2 = I)2 = I 2 + m 2 (2 − a2 )2 . τ2 = I¯2zz + m2 (C2 + a2 )2 + a1 (C2 + a2 )c2 + kr2 Im2 ϑ̈1 On the basis of these relations, it can be verified that the resulting dynamic + I¯2zz + m2 (C2 + a2 )2 + kr2 2 Im2 ϑ̈2 model coincides with the model derived in (7.82) with Lagrange formulation. 2 +m2 a1 (C2 + a2 )s2 ϑ̇1 + m2 (C2 + a2 )gc12 .
• Backward recursion: Link 1 7.6 Direct Dynamics and Inverse Dynamics
⎡ ⎤ Both Lagrange formulation and Newton–Euler formulation allow the compu-
−m2 (C2 + a2 )s2 (ϑ̈1 + ϑ̈2 ) − m1 (C1 + a1 )ϑ̇21 − m2 a1 ϑ̇21
tation of the relationship between the joint torques — and, if present, the
⎢ −m2 (C2 + a2 )c2 (ϑ̇1 + ϑ̇2 )2 + (m1 + m2 )gs1 ⎥
⎢ ⎥ end-effector forces — and the motion of the structure. A comparison between
1 ⎢ ⎥
f 1 = ⎢ m1 (C1 + a1 )ϑ̈1 + m2 a1 ϑ̈1 + m2 (C2 + a2 )c2 (ϑ̈1 + ϑ̈2 ) ⎥ the two approaches reveals what follows. The Lagrange formulation has the
⎢ ⎥
⎣ −m2 (C2 + a2 )s2 (ϑ̇1 + ϑ̇2 )2 + (m1 + m2 )gc1 ⎦ following advantages:
0 • It is systematic and of immediate comprehension.
⎡ ⎤ • It provides the equations of motion in a compact analytical form containing
∗ the inertia matrix, the matrix in the centrifugal and Coriolis forces, and
⎢ ∗ ⎥ the vector of gravitational forces. Such a form is advantageous for control
⎢ ⎥
⎢ ⎥ design.
⎢ I¯1zz ϑ̈1 + m2 a21 ϑ̈1 + m1 (C + a1 )2 ϑ̈1 + m2 a1 (C + a2 )c2 ϑ̈1 ⎥
⎢ 1 2 ⎥ • It is effective if it is wished to include more complex mechanical effects
μ11 = ⎢
⎢ +I¯2zz (ϑ̈1 + ϑ̈2 ) + m2 a1 (C2 + a2 )c2 (ϑ̈1 + ϑ̈2 ) ⎥
⎥
⎢ ⎥ such as flexible link deformation.
⎢ 2 ⎥
+m2 (C2 + a2 ) (ϑ̈1 + ϑ̈2 ) + kr2 Im2 ϑ̈2
⎢ ⎥ The Newton–Euler formulation has the following fundamental advantage:
⎣ 2
+m2 a1 (C2 + a2 )s2 ϑ̇1 − m2 a1 (C2 + a2 )s2 (ϑ̇1 + ϑ̇2 )2 ⎦
+m1 (C1 + a1 )gc1 + m2 a1 gc1 + m2 (C2 + a2 )gc12 • It is an inherently recursive method that is computationally efficient.
In the study of dynamics, it is relevant to find a solution to two kinds of
τ1 = I¯1zz + m1 (C1 + a1 ) 2 2
+ kr1 Im1 + I¯2zz problems concerning computation of direct dynamics and inverse dynamics.
The direct dynamics problem consists of determining, for t > t0 , the joint
+m2 a21 + (C2 + a2 )2 + 2a1 (C2 + a2 )c2 ϑ̈1 accelerations q̈(t) (and thus q̇(t), q(t)) resulting from the given joint torques
τ (t) — and the possible end-effector forces he (t) — once the initial positions
+ I¯2zz + m2 (C2 + a2 )2 + a1 (C2 + a2 )c2 + kr2 Im2 ϑ̈2 q(t0 ) and velocities q̇(t0 ) are known (initial state of the system).
The inverse dynamics problem consists of determining the joint torques
−2m2 a1 (C2 + a2 )s2 ϑ̇1 ϑ̇2 − m2 a1 (C2 + a2 )s2 ϑ̇22 τ (t) which are needed to generate the motion specified by the joint accelera-
+ m1 (C1 + a1 ) + m2 a1 gc1 + m2 (C2 + a2 )gc12 . tions q̈(t), velocities q̇(t), and positions q(t) — once the possible end-effector
forces he (t) are known.
7.6 Direct Dynamics and Inverse Dynamics 293 294 7 Dynamics
Solving the direct dynamics problem is useful for manipulator simulation. • O(n2 ) for computing direct dynamics,
Direct dynamics allows the motion of the real physical system to be described • O(n) for computing inverse dynamics. in terms of the joint accelerations, when a set of assigned joint torques is applied to the manipulator; joint velocities and positions can be obtained by integrating the system of nonlinear differential equations. 7.7 Dynamic Scaling of Trajectories Since the equations of motion obtained with Lagrange formulation give the analytical relationship between the joint torques (and the end-effector forces) The existence of dynamic constraints to be taken into account for trajectory and the joint positions, velocities and accelerations, these can be computed generation has been mentioned in Sect. 4.1. In practice, with reference to the from (7.42) as given trajectory time or path shape (segments with high curvature), the tra- q̈ = B −1 (q)(τ − τ ) (7.115) jectories that can be obtained with any of the previously illustrated methods where may impose too severe dynamic performance for the manipulator. A typical case is that when the required torques to generate the motion are larger than τ (q, q̇) = C(q, q̇)q̇ + F v q̇ + F s sgn (q̇) + g(q) + J T (q)he (7.116) the maximum torques the actuators can supply. In this case, an infeasible trajectory has to be suitably time-scaled. denotes the torque contributions depending on joint positions and velocities. Suppose a trajectory has been generated for all the manipulator joints Therefore, for simulation of manipulator motion, once the state at the time as q(t), for t ∈ [0, tf ]. Computing inverse dynamics allows the evaluation of instant tk is known in terms of the position q(tk ) and velocity q̇(tk ), the accel- the time history of the torques τ (t) required for the execution of the given eration q̈(tk ) can be computed by (7.115). Then using a numerical integration motion. By comparing the obtained torques with the torque limits available method, e.g., Runge–Kutta, with integration step Δt, the velocity q̇(tk+1 ) and at the actuators, it is easy to check whether or not the trajectory is actually position q(tk+1 ) at the instant tk+1 = tk + Δt can be computed. executable. The problem is then to seek an automatic trajectory dynamic If the equations of motion are obtained with Newton–Euler formulation, scaling technique — avoiding inverse dynamics recomputation — so that the it is possible to compute direct dynamics by using a computationally more manipulator can execute the motion on the specified path with a proper timing efficient method. In fact, for given q and q̇, the torques τ (q, q̇) in (7.116) can law without exceeding the torque limits. be computed as the torques given by the algorithm of Fig. 7.14 with q̈ = 0. Consider the manipulator dynamic model as given in (7.42) with F v = Further, column bi of matrix B(q) can be computed as the torque vector O, F s = O and he = 0, for simplicity. The term C(q, q̇) accounting for given by the algorithm of Fig. 7.14 with g 0 = 0, q̇ = 0, q̈i = 1 and q̈j = 0 centrifugal and Coriolis forces has a quadratic dependence on joint velocities, for j = i; iterating this procedure for i = 1, … , n leads to constructing the and thus it can be formally rewritten as matrix B(q). Hence, from the current values of B(q) and τ (q, q̇), and the given τ , the equations in (7.115) can be integrated as illustrated above. C(q, q̇)q̇ = Γ (q)[q̇ q̇], (7.117) Solving the inverse dynamics problem is useful for manipulator trajectory planning and control algorithm implementation. Once a joint trajectory is where [q̇ q̇] indicates the symbolic notation of the (n(n + 1)/2 × 1) vector specified in terms of positions, velocities and accelerations (typically as a re- sult of an inverse kinematics procedure), and if the end-effector forces are [q̇ q̇] = [ q̇12 q̇1 q̇2 … q̇n−1 q̇n q̇n2 ]T ; known, inverse dynamics allows computation of the torques to be applied to the joints to obtain the desired motion. This computation turns out to be Γ (q) is a proper (n × n(n + 1)/2) matrix that satisfies (7.117). In view of such useful both for verifying feasibility of the imposed trajectory and for com- position, the manipulator dynamic model can be expressed as pensating nonlinear terms in the dynamic model of a manipulator. To this B(q(t))q̈(t) + Γ (q(t))[q̇(t)q̇(t)] + g(q(t)) = τ (t), (7.118) end, Newton–Euler formulation provides a computationally efficient recursive method for on-line computation of inverse dynamics. Nevertheless, it can be where the explicit dependence on time t has been shown. shown that also Lagrange formulation is liable to a computationally efficient Consider the new variable q̄(r(t)) satisfying the equation recursive implementation, though with a nonnegligible reformulation effort. For an n-joint manipulator the number of operations required is:8 q(t) = q̄(r(t)), (7.119)
where r(t) is a strictly increasing scalar function of time with r(0) = 0 and
8 r(tf ) = t̄f . See Sect. E.1 for the definition of computational complexity of an algorithm. 7.7 Dynamic Scaling of Trajectories 295 296 7 Dynamics
Differentiating (7.119) twice with respect to time provides the following torque contributions τ̄ s (ct) computed as in (7.125) are scaled by the factor c2
relations: with respect to the torque contributions τ s (t) required to execute the original trajectory q(t). q̇ = ṙq̄ (r) (7.120) With the use of a recursive algorithm for inverse dynamics computation, q̈ = ṙ2 q̄ (r) + r̈q̄ (r) (7.121) it is possible to check whether the torques exceed the allowed limits during trajectory execution; obviously, limit violation should not be caused by the where the prime denotes the derivative with respect to r. Substituting (7.120), sole gravity torques. It is necessary to find the joint for which the torque (7.121) into (7.118) yields has exceeded the limit more than the others, and to compute the torque contribution subject to scaling, which in turn determines the factor c2 . It ṙ2 B(q̄(r))q̄ (r) + Γ (q̄(r))[q̄ (r)q̄ (r)] + r̈B(q̄(r))q̄ (r) + g(q̄(r)) = τ . is then possible to compute the time-scaled trajectory as a function of the (7.122) new time variable r = ct which no longer exceeds torque limits. It should be In (7.118) it is possible to identify the term pointed out, however, that with this kind of linear scaling the entire trajectory may be penalized, even when a torque limit on a single joint is exceeded only τ s (t) = B(q(t))q̈(t) + Γ (q(t))[q̇(t)q̇(t)], (7.123) for a short interval of time.
representing the torque contribution that depends on velocities and accelera- tions. Correspondingly, in (7.122) one can set 7.8 Operational Space Dynamic Model τ s (t) = ṙ2 B(q̄(r))q̄ (r) + Γ (q̄(r))[q̄ (r)q̄ (r)] + r̈B(q̄(r))q̄ (r). (7.124) As an alternative to the joint space dynamic model, the equations of motion of the system can be expressed directly in the operational space; to this end it By analogy with (7.123), it can be written is necessary to find a dynamic model which describes the relationship between the generalized forces acting on the manipulator and the number of minimal τ̄ s (r) = B(q̄(r))q̄ (r) + Γ (q̄(r))[q̄ (r)q̄ (r)] (7.125) variables chosen to describe the end-effector position and orientation in the and then (7.124) becomes operational space. Similar to kinematic description of a manipulator in the operational space, τ s (t) = ṙ2 τ̄ s (r) + r̈B(q̄(r))q̄ (r). (7.126) the presence of redundant DOFs and/or kinematic and representation singu- larities deserves careful attention in the derivation of an operational space The expression in (7.126) gives the relationship between the torque contribu- dynamic model. tions depending on velocities and accelerations required by the manipulator The determination of the dynamic model with Lagrange formulation using when this is subject to motions having the same path but different timing operational space variables allows a complete description of the system motion laws, obtained through a time scaling of joint variables as in (7.119). only in the case of a nonredundant manipulator, when the above variables Gravitational torques have not been considered, since they are a function constitute a set of generalized coordinates in terms of which the kinetic energy, of the joint positions only, and thus their contribution is not influenced by the potential energy, and the nonconservative forces doing work on them can time scaling. be expressed. The simplest choice for the scaling function r(t) is certainly the linear This way of proceeding does not provide a complete description of dy- function namics for a redundant manipulator; in this case, in fact, it is reasonable to r(t) = ct expect the occurrence of internal motions of the structure caused by those with c a positive constant. In this case, (7.126) becomes joint generalized forces which do not affect the end-effector motion. To develop an operational space model which can be adopted for both τ s (t) = c2 τ̄ s (ct), redundant and nonredundant manipulators, it is then convenient to start from the joint space model which is in all general. In fact, solving (7.42) for the joint which reveals that a linear time scaling by c causes a scaling of the magnitude accelerations, and neglecting the joint friction torques for simplicity, yields of the torques by the coefficient c2 . Let c > 1: (7.119) shows that the trajectory described by q̄(r(t)), assuming r = ct as the independent variable, has a q̈ = −B −1 (q)C(q, q̇)q̇ − B −1 (q)g(q) + B −1 (q)J T (q)(γ e − he ), (7.127) duration t̄f > tf to cover the entire path specified by q. Correspondingly, the 7.8 Operational Space Dynamic Model 297 298 7 Dynamics
where the joint torques τ have been expressed in terms of the equivalent end- following derivation is meaningful for redundant manipulators; for a nonre- effector forces γ according to (3.111). It is worth noting that h represents the dundant manipulator, in fact, using (7.133) does not pose specific problems contribution of the end-effector forces due to contact with the environment, as long as J A is nonsingular ((7.134)–(7.136)). whereas γ expresses the contribution of the end-effector forces due to joint With reference to operational space, the direct dynamics problem consists actuation. of determining the resulting end-effector accelerations ẍe (t) (and thus ẋe (t), On the other hand, the second-order differential kinematics equation xe (t)) from the given joint torques τ (t) and end-effector forces he (t). For a in (3.98) describes the relationship between joint space and operational space redundant manipulator, (7.133) cannot be directly used, since (3.111) has a accelerations, i.e., solution in γ e only if τ ∈ R(J T ). It follows that for simulation purposes, ẍe = J A (q)q̈ + J̇ A (q, q̇)q̇. the solution to the problem is naturally obtained in the joint space; in fact, the expression in (7.42) allows the computation of q, q̇, q̈ which, substituted The solution in (7.127) features the geometric Jacobian J , whereas the analyt- into the direct kinematics equations in ((2.82), (3.62), (3.98), give xe , ẋe , ẍe , ical Jacobian J A appears in (3.98). For notation uniformity, in view of (3.66), respectively. one can set Formulation of an inverse dynamics problem in the operational space re- T TA (xe )γ e = γ A T TA (xe )he = hA (7.128) quires the determination of the joint torques τ (t) that are needed to generate where T A is the transformation matrix between the two Jacobians. Substi- a specific motion assigned in terms of ẍe (t), ẋe (t), xe (t), for given end-effector tuting (7.127) into (3.98) and accounting for (7.128) gives forces he (t). A possible way of solution is to solve a complete inverse kinemat- ics problem for (2.82), (3.62), (3.98), and then compute the required torques ẍe = −J A B −1 C q̇ − J A B −1 g + J̇ A q̇ + J A B −1 J TA (γ A − hA ). (7.129) with the joint space inverse dynamics as in (7.42). Hence, for redundant ma- nipulators, redundancy resolution is performed at kinematic level. where the dependence on q and q̇ has been omitted. With the positions An alternative solution to the inverse dynamics problem consists of com- puting γ A as in (7.133) and the joint torques τ as in (3.111). In this way, B A = (J A B −1 J TA )−1 (7.130) however, the presence of redundant DOFs is not exploited at all, since the C A ẋe = B A J A B −1 C q̇ − B A J̇ A q̇ (7.131) computed torques do not generate internal motions of the structure. g A = B A J A B −1 g, (7.132) If it is desired to find a formal solution that allows redundancy resolution at dynamic level, it is necessary to determine those torques corresponding to the expression in (7.129) can be rewritten as the equivalent end-effector forces computed as in (7.133). By analogy with the differential kinematics solution (3.54), the expression of the torques to be B A (xe )ẍe + C A (xe , ẋe )ẋe + g A (xe ) = γ A − hA , (7.133) determined will feature the presence of a minimum-norm term and a homoge- neous term. Since the joint torques have to be computed, it is convenient to which is formally analogous to the joint space dynamic model (7.42). Notice express the model (7.133) in terms of q, q̇, q̈. By recalling the positions (7.131), that the matrix J A B −1 J TA is invertible if and only if J A is full-rank, that is, (7.132), the expression in (7.133) becomes in the absence of both kinematic and representation singularities. For a nonredundant manipulator in a nonsingular configuration, the ex- B A (ẍe − J̇ A q̇) + B A J A B −1 C q̇ + B A J A B −1 g = γ A − hA pressions in (7.130)–(7.132) become: and, in view of (3.98), B A = J −TA BJ A −1 (7.134) B A J A q̈ + B A J A B −1 C q̇ + B A J A B −1 g = γ A − hA . (7.137) C A ẋe = J −T A C q̇ − B A J̇ A q̇ (7.135) By setting g A = J −T A g. (7.136) J̄ A (q) = B −1 (q)J TA (q)B A (q), (7.138) the expression in (7.137) becomes As anticipated above, the main feature of the obtained model is its formal validity also for a redundant manipulator, even though the variables xe do T J̄ A (Bq̈ + C q̇ + g) = γ A − hA . (7.139) not constitute a set of generalized coordinates for the system; in this case, the matrix B A is representative of a kinetic pseudo-energy. At this point, from the joint space dynamic model in (7.42), it is easy to In the following, the utility of the operational space dynamic model recognize that (7.139) can be written as in (7.133) for solving direct and inverse dynamics problems is investigated. The T J̄ A (τ − J TA hA ) = γ A − hA 7.9 Dynamic Manipulability Ellipsoid 299 300 7 Dynamics
from which T 1 J̄ A τ = γ A . (7.140) The general solution to (7.140) is of the form (see Problem 7.10) 0.5 Ŧ1 JB g
[m]
T
τ = J TA (q)γ A + I n − J TA (q)J̄ A (q) τ 0 , (7.141)
0
that can be derived by observing that J TA in (7.138) is a right pseudo-inverse of J̄ A weighted by the inverse of the inertia matrix B −1 . The (n × 1) vector of T Ŧ0.5 Ŧ0.5 0 0.5 1 arbitrary torques τ 0 in (7.141) does not contribute to the end-effector forces, [m] T since it is projected in the null space of J̄ A . Fig. 7.15. Effect of gravity on the dynamic manipulability ellipsoid for a three-link To summarize, for given xe , ẋe , ẍe and hA , the expression in (7.133) planar arm allows the computation of γ A . Then, (7.141) gives the torques τ which, besides executing the assigned end-effector motion, generate internal motions of the structure to be employed for handling redundancy at dynamic level through needed to derive the ellipsoid. In fact, substituting (7.145) into (7.142) gives a suitable choice of τ 0 . T B(q)J † (q)v̇ e + g(q) B(q)J † (q)v̇ e + g(q) = 1.
The vector on the right-hand side of (7.145) can be rewritten as
7.9 Dynamic Manipulability Ellipsoid BJ † v̇ e + g = B(J † v̇ e + B −1 g) (7.146) The availability of the dynamic model allows formulation of the dynamic ma- † −1 † −1 † −1 = B(J v̇ e + B g + J J B g − J J B g) nipulability ellipsoid which provides a useful tool for manipulator dynamic performance analysis. This can be used for mechanical structure design as = B J † v̇ e + J † J B −1 g + (I n − J † J )B −1 g , well as for seeking optimal manipulator configurations. where the dependence on q has been omitted. According to what was done Consider the set of joint torques of constant (unit) norm for solving (7.144), one can neglect the contribution of the accelerations given τTτ = 1 (7.142) by B −1 g which are in the null space of J and then produce no end-effector acceleration. Hence, (7.146) becomes describing the points on the surface of a sphere. It is desired to describe the operational space accelerations that can be generated by the given set of joint BJ † v̇ e + g = BJ † (v̇ e + J B −1 g) (7.147) torques. and the dynamic manipulability ellipsoid can be expressed in the form For studying dynamic manipulability, suppose to consider the case of a manipulator standing still (q̇ = 0), not in contact with the environment (he = (v̇ e + J B −1 g)T J †T B T BJ † (v̇ e + J B −1 g) = 1. (7.148) 0). The simplified model is The core of the quadratic form J † T B T BJ † depends on the geometrical and B(q)q̈ + g(q) = τ . (7.143) inertial characteristics of the manipulator and determines the volume and principal axes of the ellipsoid. The vector −J B −1 g, describing the contribu- The joint accelerations q̈ can be computed from the second-order differen- tion of gravity, produces a constant translation of the centre of the ellipsoid tial kinematics that can be obtained by differentiating (3.39), and imposing (for each manipulator configuration) with respect to the origin of the reference successively q̇ = 0, leading to frame; see the example in Fig. 7.15 for a three-link planar arm. The meaning of the dynamic manipulability ellipsoid is conceptually simi- v̇ e = J (q)q̈. (7.144) lar to that of the ellipsoids considered with reference to kineto-statics duality. Solving for minimum-norm accelerations only, for a nonsingular Jacobian, and In fact, the distance of a point on the surface of the ellipsoid from the end- substituting in (7.143) yields the expression of the torques effector gives a measure of the accelerations which can be imposed to the end-effector along the given direction, with respect to the constraint (7.142). τ = B(q)J † (q)v̇ e + g(q) (7.145) With reference to Fig. 7.15, it is worth noticing how the presence of gravity Problems 301 302 7 Dynamics
acceleration allows the execution of larger accelerations downward, as natural to predict. In the case of a nonredundant manipulator, the ellipsoid reduces to
(v̇ e + J B −1 g)T J −T B T BJ −1 (v̇ e + J B −1 g) = 1. (7.149)
Bibliography Fig. 7.16. Two-link planar arm with a prismatic joint and a revolute joint The derivation of the dynamic model for rigid manipulators can be found in several classical robotics texts, such as [180, 10, 248, 53, 217, 111]. 7.3. Find the dynamic model of the SCARA manipulator in Fig. 2.36. The first works on the computation of the dynamic model of open-chain manipulators based on the Lagrange formulation are [234, 19, 221, 236]. A 7.4. For the planar arm of Sect. 7.3.2, find a minimal parameterization of the computationally efficient formulation is presented in [96]. dynamic model in (7.82). Dynamic model computation for robotic systems having a closed-chain or a tree kinematic structure can be found in [11, 144] and [112], respectively. 7.5. Find the dynamic model of the two-link planar arm with a prismatic Joint friction models are analyzed in [9]. joint and a revolute joint in Fig. 7.16 with the Lagrange formulation. Then, The notable properties of the dynamic model deriving from the principle of consider the addition of a concentrated tip payload of mass mL , and express energy conservation are underlined in [213], on the basis of the work in [119]. the resulting model in a linear form with respect to a suitable set of dynamic Algorithms to find the parameterization of the dynamic model in terms of parameters as in (7.81). a minimum number of parameters are considered in [115], which utilizes the results in [166]. Methods for symbolic computation of those parameters are 7.6. For the two-link planar arm of Fig. 7.4, find the dynamic model with presented in [85] for open kinematic chains and [110] for closed kinematic the Lagrange formulation when the absolute angles with respect to the base chains. Parameter identification methods based on least-squares techniques frame are chosen as generalized coordinates. Discuss the result in view of a are given in [13]. comparison with the model derived in (7.82). The Newton–Euler formulation is proposed in [172], and a computationally 7.7. Compute the joint torques for the two-link planar arm of Fig. 7.4 with efficient version for inverse dynamics can be found in [142]; an analogous for- the data and along the trajectories of Example 7.2, in the case of tip forces mulation is employed for direct dynamics computation in [237]. The Lagrange f = [ 500 500 ]T N. and Newton–Euler formulations are compared by a computational viewpoint in [211], while they are utilized in [201] for dynamic model computation with 7.8. Find the dynamic model of the two-link planar arm with a prismatic inclusion of inertial and gyroscopic effects of actuators. Efficient algorithms joint and a revolute joint in Fig. 7.16 by using the recursive Newton–Euler for direct dynamics computation are given in [76, 77]. algorithm. The trajectory dynamic scaling technique is presented in [97]. The opera- tional space dynamic model is illustrated in [114] and the concept of weighted 7.9. Show that for the operational space dynamic model (7.133) a skew- pseudo-inverse of the inertia matrix is introduced in [78]. The manipulability symmetry property holds which is analogous to (7.48). ellipsoids are analyzed in [246, 38]. 7.10. Show how to obtain the general solution to (7.140) in the form (7.141).
7.11. For a nonredundant manipulator, compute the relationship between the
Problems dynamic manipulability measure that can be defined for the dynamic manip- 7.1. Find the dynamic model of a two-link Cartesian arm in the case when ulability ellipsoid and the manipulability measure defined in (3.56). the second joint axis forms an angle of π/4 with the first joint axis; compare the result with the model of the manipulator in Fig. 7.3. 7.2. For the two-link planar arm of Sect. 7.3.2, prove that with a different choice of the matrix C, (7.49) holds true while (7.48) does not. 304 8 Motion Control
8 Motion Control
Fig. 8.1. General scheme of joint space control
a Cartesian manipulator is substantially different from that of an anthropo-
morphic manipulator.
The driving system of the joints also has an effect on the type of control
strategy used. If a manipulator is actuated by electric motors with reduction
In Chap. 4, trajectory planning techniques have been presented which al- gears of high ratios, the presence of gears tends to linearize system dynam- low the generation of the reference inputs to the motion control system. The ics, and thus to decouple the joints in view of the reduction of nonlinearity problem of controlling a manipulator can be formulated as that to determine effects. The price to pay, however, is the occurrence of joint friction, elastic- the time history of the generalized forces (forces or torques) to be developed ity and backlash that may limit system performance more than it is due to by the joint actuators, so as to guarantee execution of the commanded task configuration-dependent inertia, Coriolis and centrifugal forces, and so forth. while satisfying given transient and steady-state requirements. The task may On the other hand, a robot actuated with direct drives eliminates the draw- regard either the execution of specified motions for a manipulator operating backs due to friction, elasticity and backlash, but the weight of nonlinearities in free space, or the execution of specified motions and contact forces for a and couplings between the joints becomes relevant. As a consequence, different manipulator whose end-effector is constrained by the environment. In view of control strategies have to be thought of to obtain high performance. problem complexity, the two aspects will be treated separately; first, motion Without any concern to the specific type of mechanical manipulator, it control in free space, and then control of the interaction with the environ- is worth remarking that task specification (end-effector motion and forces) is ment. The problem of motion control of a manipulator is the topic of this usually carried out in the operational space, whereas control actions (joint chapter. A number of joint space control techniques are presented. These can actuator generalized forces) are performed in the joint space. This fact nat- be distinguished between decentralized control schemes, i.e., when the single urally leads to considering two kinds of general control schemes, namely, a manipulator joint is controlled independently of the others, and centralized joint space control scheme (Fig. 8.1) and an operational space control scheme control schemes, i.e., when the dynamic interaction effects between the joints (Fig. 8.2). In both schemes, the control structure has closed loops to exploit are taken into account. Finally, as a premise to the interaction control prob- the good features provided by feedback, i.e., robustness to modelling uncer- lem, the basic features of operational space control schemes are illustrated. tainties and reduction of disturbance effects. In general terms, the following considerations should be made. The joint space control problem is actually articulated in two subprob- 8.1 The Control Problem lems. First, manipulator inverse kinematics is solved to transform the motion requirements xd from the operational space into the corresponding motion q d Several techniques can be employed for controlling a manipulator. The tech- in the joint space. Then, a joint space control scheme is designed that allows nique followed, as well as the way it is implemented, may have a significant the actual motion q to track the reference inputs. However, this solution has influence on the manipulator performance and then on the possible range of the drawback that a joint space control scheme does not influence the opera- applications. For instance, the need for trajectory tracking control in the op- tional space variables xe which are controlled in an open-loop fashion through erational space may lead to hardware/software implementations, which differ the manipulator mechanical structure. It is then clear that any uncertainty of from those allowing point-to-point control, where only reaching of the final the structure (construction tolerance, lack of calibration, gear backlash, elas- position is of concern. ticity) or any imprecision in the knowledge of the end-effector pose relative On the other hand, the manipulator mechanical design has an influence on the kind of control scheme utilized. For instance, the control problem of 8.2 Joint Space Control 305 306 8 Motion Control
Fig. 8.3. Block scheme of the manipulator and drives system as a voltage-controlled
system
Fig. 8.2. General scheme of operational space control
where K r is an (n × n) diagonal matrix, whose elements are defined in (7.22)
to an object to manipulate causes a loss of accuracy on the operational space and are much greater than unity.1 variables. In view of (8.2), if τ m denotes the vector of actuator driving torques, one The operational space control problem follows a global approach that re- can write quires a greater algorithmic complexity; notice that inverse kinematics is now τ m = K −1 r τ. (8.3) embedded into the feedback control loop. Its conceptual advantage regards the possibility of acting directly on operational space variables; this is somewhat With reference to (5.1)–(5.4), the n driving systems can be described in only a potential advantage, since measurement of operational space variables compact matrix form by the equations: is often performed not directly, but through the evaluation of direct kinematics functions starting from measured joint space variables. K −1 r τ = K t ia (8.4) On the above premises, in the following, joint space control schemes for v a = Ra ia + K v q̇ m (8.5) manipulator motion in the free space are presented first. In the sequel, op- v a = Gv v c . (8.6) erational space control schemes will be illustrated which are logically at the basis of control of the interaction with the environment. In (8.4), K t is the diagonal matrix of torque constants and ia is the vector of armature currents of the n motors; in (8.5), v a is the vector of armature voltages, Ra is the diagonal matrix of armature resistances,2 and K v is the 8.2 Joint Space Control diagonal matrix of voltage constants of the n motors; in (8.6), Gv is the diagonal matrix of gains of the n amplifiers and v c is the vector of control In Chap. 7, it was shown that the equations of motion of a manipulator in voltages of the n servomotors. the absence of external end-effector forces and, for simplicity, of static friction On reduction of (8.1), (8.2), (8.4), (8.5), (8.6), the dynamic model of the (difficult to model accurately) are described by system given by the manipulator and drives is described by B(q)q̈ + C(q, q̇)q̇ + F v q̇ + g(q) = τ (8.1) B(q)q̈ + C(q, q̇)q̇ + F q̇ + g(q) = u (8.7) with obvious meaning of the symbols. To control the motion of the manipula- tor in free space means to determine the n components of generalized forces — where the following positions have been made: torques for revolute joints, forces for prismatic joints — that allow execution F = F v + K r K t R−1 a KvKr (8.8) of a motion q(t) so that −1 q(t) = q d (t), u = K r K t R a Gv v c . (8.9) as closely as possible, where q d (t) denotes the vector of desired joint trajectory From (8.1), (8.7), (8.8), (8.9) it is variables. The generalized forces are supplied by the actuators through proper trans- K r K t R−1 −1 a Gv v c = τ + K r K t Ra K v K r q̇ (8.10) missions to transform the motion characteristics. Let q m denote the vector 1 of joint actuator displacements; the transmissions — assumed to be rigid and Assuming a diagonal K r leads to excluding the presence of kinematic couplings in the transmission, that is the motion of each actuator does not induce motion with no backlash — establish the following relationship: on a joint other than that actuated. 2 K r q = qm , (8.2) The contribution of the inductance has been neglected. 8.2 Joint Space Control 307 308 8 Motion Control
In this case, by resorting to an inverse dynamics technique, it is possible
to find the joint torques τ (t) needed to track any specified motion in terms of
the joint accelerations q̈(t), velocities q̇(t) and positions q(t). Obviously, this
solution requires the accurate knowledge of the manipulator dynamic model.
Fig. 8.4. Block scheme of the manipulator and drives system as a torque-controlled system The determination of the torques to be generated by the drive system can thus refer to a centralized control structure, since to compute the torque history at the i-th joint it is necessary to know the time evolution of the motion of all and thus the joints. By recalling that τ = K r K t R−1 a (Gv v c − K v K r q̇). (8.11) τ = K r K t ia , (8.14) The overall system is then voltage-controlled and the corresponding block scheme is illustrated in Fig. 8.3. If the following assumptions hold: to find a relationship between the torques τ and the control voltages v c , using (8.5), (8.6) leads to • the elements of matrix K r , characterizing the transmissions, are much greater than unity; τ = K r K t R−1 −1 a Gv v c − K r K t Ra K v K r q̇. (8.15) • the elements of matrix Ra are very small, which is typical in the case of high-efficiency servomotors; If the actuators have to provide torque contributions computed on the basis • the values of the torques τ required for the execution of the desired motions of the manipulator dynamic model, the control voltages — to be determined are not too large; according to (8.15) — depend on the torque values and also on the joint velocities; this relationship depends on the matrices K t , K v and R−1 a , whose then it can be assumed that elements are influenced by the operating conditions of the motors. To reduce Gv v c ≈ K v K r q̇. (8.12) sensitivity to parameter variations, it is worth considering driving systems characterized by a current control rather than by a voltage control. In this case The proportionality relationship obtained between q̇ and v c is independent the actuators behave as torque-controlled generators; the equation in (8.5) of the values attained by the manipulator parameters; the smaller the joint becomes meaningless and is replaced by velocities and accelerations, the more valid this assumption. Hence, velocity (or voltage) control shows an inherent robustness with respect to parameter ia = Gi v c , (8.16) variations of the manipulator model, which is enhanced by the values of the which gives a proportional relation between the armature currents ia (and gear reduction ratios. thus the torques τ ) and the control voltages v c established by the constant In this case, the scheme illustrated in Fig. 8.3 can be taken as the reference matrix Gi . As a consequence, (8.9) becomes structure for the design of the control system. Having assumed that τ = u = K r K t Gi v c (8.17) v c ≈ G−1 v K v K r q̇ (8.13) which shows a reduced dependence of u on the motor parameters. The overall implies that the velocity of the i-th joint depends only on the i-th control volt- system is now torque-controlled and the resulting block scheme is illustrated age, since the matrix G−1 v K v K r is diagonal. Therefore, the joint position in Fig. 8.4. control system can be designed according to a decentralized control structure, The above presentation suggests resorting for the decentralized structure since each joint can be controlled independently of the others. The results, — where the need for robustness prevails — to feedback control systems, while evaluated in the terms of the tracking accuracy of the joint variables with for the centralized structure — where the computation of inverse dynamics is respect to the desired trajectories, are improved in the case of higher gear re- needed — it is necessary to refer to control systems with feedforward actions. duction ratios and less demanding values of required speeds and accelerations. Nevertheless, it should be pointed out that centralized control still requires On the other hand, if the desired manipulator motion requires large joint the use of error contributions between the desired and the actual trajectory, speeds and/or accelerations, the approximation (8.12) no longer holds, in view no matter whether they are implemented in a feedback or in a feedforward of the magnitude of the required driving torques; this occurrence is even more fashion. This is a consequence of the fact that the considered dynamic model, evident for direct-drive actuation (K r = I). even though a quite complex one, is anyhow an idealization of reality which 8.3 Decentralized Control 309 310 8 Motion Control
does not include effects, such as joint Coulomb friction, gear backlash, di- mension tolerance, and the simplifying assumptions in the model, e.g., link rigidity, and so on. As already pointed out, the drive systems is anyhow inserted into a feed- back control system. In the case of decentralized control, the drive will be characterized by the model describing its behaviour as a velocity-controlled generator. Instead, in the case of centralized control, since the driving torque is to be computed on a complete or reduced manipulator dynamic model, the drive will be characterized as a torque-controlled generator.
8.3 Decentralized Control The simplest control strategy that can be thought of is one that regards the manipulator as formed by n independent systems (the n joints) and con- trols each joint axis as a single-input/single-output system. Coupling effects between joints due to varying configurations during motion are treated as disturbance inputs. In order to analyze various control schemes and their performance, it is worth considering the model of the system manipulator with drives in terms of mechanical quantities at the motor side; in view of (8.2), (8.3), it is Fig. 8.5. Block scheme of the system of manipulator with drives K −1 −1 −1 −1 −1 −1 −1 r B(q)K r q̈ m + K r C(q, q̇)K r q̇ m + K r F v K r + K r g(q) = τ m . (8.18) it accounts for all those nonlinear and coupling terms of manipulator joint By observing that the diagonal elements of B(q) are formed by constant terms dynamics. and configuration-dependent terms (functions of sine and cosine for revolute On the basis of the above scheme, several control algorithms can be derived joints), one can set with reference to the detail of knowledge of the dynamic model. The simplest B(q) = B̄ + ΔB(q) (8.19) approach that can be followed, in case of high-gear reduction ratios and/or where B̄ is the diagonal matrix whose constant elements represent the result- limited performance in terms of required velocities and accelerations, is to ing average inertia at each joint. Substituting (8.19) into (8.1) yields consider the component of the nonlinear interacting term d as a disturbance for the single joint servo. K −1 −1 r B̄K r q̈ m + F m q̇ m + d = τ m (8.20) The design of the control algorithm leads to a decentralized control struc- ture, since each joint is considered independently of the others. The joint where controller must guarantee good performance in terms of high disturbance re- F m = K −1 −1 r F vKr (8.21) jection and enhanced trajectory tracking capabilities. The resulting control represents the matrix of viscous friction coefficients about the motor axes, and structure is substantially based on the error between the desired and actual output, while the input control torque at actuator i depends only on the error d = K −1 −1 −1 −1 −1 r ΔB(q)K r q̈ m + K r C(q, q̇)K r q̇ m + K r g(q) (8.22) of output i. Therefore, the system to control is Joint i drive corresponding to the single- represents the contribution depending on the configuration. input/single-output system of the decoupled and linear part of the scheme in As illustrated by the block scheme of Fig. 8.5, the system of manipulator Fig. 8.5. The interaction with the other joints is described by component i of with drives is actually constituted by two subsystems; one has τ m as input the vector d in (8.22). and q m as output, the other has q m , q̇ m , q̈ m as inputs, and d as output. The former is linear and decoupled , since each component of τ m influences only the corresponding component of q m . The latter is nonlinear and coupled , since 8.3 Decentralized Control 311 312 8 Motion Control
be of PI type as in (8.23) so as to obtain zero error at steady state for a
constant disturbance. Further, kT P , kT V , kT A are the respective transducer
constants, and the amplifier gain Gv has been embedded in the gain of the
inmost controller. In the scheme of Fig. 8.6, notice that ϑr is the reference
input, which is related to the desired output ϑmd as
ϑr = kT P ϑmd .
Further, the disturbance torque D has been suitably transformed into a volt-
age by the factor Ra /kt .
In the following, a number of possible solutions that can be derived from
the general scheme of Fig. 8.6 are presented; at this stage, the issue arising
from possible lack of measurement of physical variables is not considered yet.
Fig. 8.6. Block scheme of general independent joint control Three case studies are considered which differ in the number of active feedback
loops.4
Assumed that the actuator is a rotary electric DC motor, the general Position feedback scheme of drive control is that in Fig. 5.9 where Im is the average inertia 2 3 reported to the motor axis (Imi = b̄ii /kri ). In this case, the control action is characterized by 1 + sTP 8.3.1 Independent Joint Control CP (s) = KP CV (s) = 1 CA (s) = 1 s To guide selection of the controller structure, start noticing that an effective kT V = kT A = 0. rejection of the disturbance d on the output ϑm is ensured by: With these positions, the structure of the control scheme in Fig. 8.6 leads to • a large value of the amplifier gain before the point of intervention of the the scheme illustrated in Fig. 5.10. From this scheme the transfer function of disturbance, the forward path is • the presence of an integral action in the controller so as to cancel the effect km KP (1 + sTP ) P (s) = , of the gravitational component on the output at steady state (constant s2 (1 + sTm ) ϑm ). while that of the return path is These requisites clearly suggest the use of a proportional-integral (PI) con- H(s) = kT P . trol action in the forward path whose transfer function is A root locus analysis can be performed as a function of the gain of the po- 1 + sTc sition loop km KP kT P TP /Tm . Three situations are illustrated for the poles C(s) = Kc ; (8.23) s of the closed-loop system with reference to the relation between TP and Tm (Fig. 8.7). Stability of the closed-loop feedback system imposes some con- this yields zero error at steady state for a constant disturbance, and the pres- straints on the choice of the parameters of the PI controller. If TP < Tm , ence of the real zero at s = −1/Tc offers a stabilizing action. To improve the system is inherently unstable (Fig. 8.7a). Then, it must be TP > Tm dynamic performance, it is worth choosing the controller as a cascade of ele- (Fig. 8.7b). As TP increases, the absolute value of the real part of the two mentary actions with local feedback loops closed around the disturbance. roots of the locus tending towards the asymptotes increases too, and the sys- Besides closure of a position feedback loop, the most general solution is tem has faster time response. Hence, it is convenient to render TP Tm obtained by closing inner loops on velocity and acceleration. This leads to (Fig. 8.7c). In any case, the real part of the dominant poles cannot be less the scheme in Fig. 8.6, where CP (s), CV (s), CA (s) respectively represent than −1/2Tm . position, velocity, acceleration controllers, and the inmost controller should 4 See Appendix C for a brief brush-up on control of linear single-input/single-output 3 Subscript i is to be dropped for notation compactness. systems. 8.3 Decentralized Control 313 314 8 Motion Control
The closed-loop disturbance/output transfer function is
sRa
Θm (s) kt KP kT P (1 + sTP )
=− , (8.25)
D(s) s2 (1 + sTm )
1+
km KP kT P (1 + sTP )
which shows that it is worth increasing KP to reduce the effect of disturbance
on the output during the transient. The function in (8.25) has two complex
poles (−ζωn , ±j 1 − ζ 2 ωn ), a real pole (−1/τ ), and a zero at the origin. The
zero is due to the PI controller and allows the cancellation of the effects of
gravity on the angular position when ϑm is a constant.
In (8.25), it can be recognized that the term KP kT P is the reduction
factor imposed by the feedback gain on the amplitude of the output due to
disturbance; hence, the quantity
XR = KP kT P (8.26)
can be interpreted as the disturbance rejection factor , which in turn is de-
termined by the gain KP . However, it is not advisable to increase KP too
much, because small damping ratios would result leading to unacceptable os-
cillations of the output. An estimate TR of the output recovery time needed
Fig. 8.7. Root loci for the position feedback control scheme by the control system to recover the effects of the disturbance on the angular
position can be evaluated by analyzing the modes of evolution of (8.25). Since
τ ≈ TP , such estimate is expressed by
The closed-loop input/output transfer function is * 1 1 TR = max TP , . (8.27) Θm (s) ζωn kT P = , (8.24) Θr (s) s2 (1 + sTm ) 1+ Position and velocity feedback km KP kT P (1 + sTP )
which can be expressed in the form In this case, the control action is characterized by
1 1 + sTV
(1 + sTP ) CP (s) = KP CV (s) = KV CA (s) = 1
kT P s
W (s) = ,
2ζs s2 kT A = 0;
1+ + 2 (1 + sτ )
ωn ωn
with these positions, the structure of the control scheme in Fig. 8.6 leads to
where ωn and ζ are respectively the natural frequency and damping ratio of scheme illustrated in Fig. 5.11. To carry out a root locus analysis as a function the pair of complex poles and −1/τ locates the real pole. These values are of the velocity feedback loop gain, it is worth reducing the velocity loop in assigned to define the joint drive dynamics as a function of the constant TP ; parallel to the position loop by following the usual rules for moving blocks. if TP > Tm , then 1/ζωn > TP > τ (Fig. 8.7b); if TP Tm (Fig. 8.7c), for From the scheme in Fig. 5.11 the transfer function of the forward path is large values of the loop gain, then ζωn > 1/τ ≈ 1/TP and the zero at −1/TP in the transfer function W (s) tends to cancel the effect of the real pole. km KP KV (1 + sTV ) P (s) = , s2 (1 + sTm ) 8.3 Decentralized Control 315 316 8 Motion Control
Fig. 8.8. Root locus for the position and velocity feedback control scheme
while that of the return path is Fig. 8.9. Block scheme of position, velocity and acceleration feedback control kT V H(s) = kT P 1 + s . KP kT P ωn2 KP kT P KV = . (8.31) km The zero of the controller at s = −1/TV can be chosen so as to cancel the effects of the real pole of the motor at s = −1/Tm . Then, by setting For given transducer constants kT P and kT V , once KV has been chosen to satisfy (8.30), the value of KP is obtained from (8.31). TV = Tm , The closed-loop disturbance/output transfer function is the poles of the closed-loop system move on the root locus as a function of the sRa loop gain km KV kT V , as shown in Fig. 8.8. By increasing the position feedback Θm (s) kt KP kT P KV (1 + sTm ) gain KP , it is possible to confine the closed-loop poles into a region of the =− , (8.32) complex plane with large absolute values of the real part. Then, the actual D(s) skT V s2 1+ + location can be established by a suitable choice of KV . KP kT P km KP kT P KV The closed-loop input/output transfer function is which shows that the disturbance rejection factor is 1 Θm (s) kT P XR = KP kT P KV (8.33) = 2 , (8.28) Θr (s) skT V s 1+ + and is fixed, once KP and KV have been chosen via (8.30), (8.31). Concerning KP kT P km KP kT P KV disturbance dynamics, the presence of a zero at the origin introduced by the which can be compared with the typical transfer function of a second-order PI, of a real pole at s = −1/Tm , and of a pair of complex poles having real system part −ζωn should be noticed. Hence, in this case, an estimate of the output 1 recovery time is given by the time constant W (s) = kT P . (8.29) * 2ζs s2 1 1+ + 2 TR = max Tm , ; (8.34) ωn ωn ζωn It can be recognized that, with a suitable choice of the gains, it is possible to which reveals an improvement with respect to the previous case in (8.27), obtain any value of natural frequency ωn and damping ratio ζ. Hence, if ωn since Tm TP and the real part of the dominant poles is not constrained by and ζ are given as design requirements, the following relations can be found: the inequality ζωn < 1/2Tm . 2ζωn KV kT V = (8.30) km 8.3 Decentralized Control 317 318 8 Motion Control
Fig. 8.11. Block scheme of a first-order filter
The two solutions are equivalent as regards dynamic performance of the con-
Fig. 8.10. Root locus for the position, velocity and acceleration feedback control trol system. In both cases, the poles of the closed-loop system are constrained scheme to move on the root locus as a function of the loop gain km KP KV KA /(1 + km KA kT A ) (Fig. 8.10). A close analogy with the previous scheme can be recognized, in that the resulting closed-loop system is again of second-order Position, velocity and acceleration feedback type. In this case, the control action is characterized by The closed-loop input/output transfer function is 1 1 + sTA Θm (s) CP (s) = KP CV (s) = KV CA (s) = KA . kT P s = , (8.35) Θr (s) skT V s2 (1 + km KA kT A ) 1+ + After some manipulation, the block scheme of Fig. 8.6 can be reduced to that KP kT P km KP kT P KV KA of Fig. 8.9 where G (s) indicates the following transfer function: while the closed-loop disturbance/output transfer function is km sRa G (s) = ⎛ ⎞. TA Θm (s) kt KP kT P KV KA (1 + sTA ) ⎜ sTm 1 + km KA kT A ⎟ =− . (8.36) (1 + km KA kT A ) ⎜ T m ⎟ D(s) skT V s2 (1 + km KA kT A ) ⎝1 + (1 + km KA kT A ) ⎠ 1+ + KP kT P km KP kT P KV KA The resulting disturbance rejection factor is given by The transfer function of the forward path is XR = KP kT P KV KA , (8.37) KP KV KA (1 + sTA ) while the output recovery time is given by the time constant P (s) = G (s), s2 * 1 while that of the return path is TR = max TA , (8.38) ζωn skT V where TA can be made less than Tm , as pointed out above. H(s) = kT P 1+ . KP kT P With reference to the transfer function in (8.29), the following relations can be established for design purposes, once ζ, ωn , XR have been specified: Also in this case, a suitable pole cancellation is worthy which can be achieved either by setting 2KP kT P ωn = (8.39) TA = T m , kT V ζ km XR or by making km KA kT A = −1 (8.40) ωn2 km KA kT A TA Tm km KA kT A 1. KP kT P KV KA = XR . (8.41) 8.3 Decentralized Control 319 320 8 Motion Control
For given kT P , kT V , kT A , KP is chosen to satisfy (8.39), KA is chosen to satisfy (8.40), and then KV is obtained from (8.41). Notice how admissible solutions for the controller typically require large values for the rejection fac- tor XR . Hence, in principle, not only does the acceleration feedback allow the achievement of any desired dynamic behaviour but, with respect to the pre- vious case, it also allows the prescription of the disturbance rejection factor as long as km XR /ωn2 > 1. In deriving the above control schemes, the issue of measurement of feed- back variables was not considered explicitly. With reference to the typical position control servos that are implemented in industrial practice, there is no problem of measuring position and velocity, while a direct measure- ment of acceleration, in general, either is not available or is too expensive to obtain. Therefore, for the scheme of Fig. 8.9, an indirect measurement can Fig. 8.12. Block scheme of position feedback control with decentralized feedforward be obtained by reconstructing acceleration from direct velocity measurement compensation through a first-order filter (Fig. 8.11). The filter is characterized by a band- width ω3f = kf . By choosing this bandwidth wide enough, the effects due to measurement lags are not appreciable, and then it is feasible to take the the reference inputs to the three control structures analyzed in the previous acceleration filter output as the quantity to feed back. Some problem may section can be respectively modified into occur concerning the noise superimposed on the filtered acceleration signal, s2 (1 + sTm ) though. Θr (s) = kT P + Θmd (s) (8.42) Resorting to a filtering technique may be useful when only the direct posi- km KP (1 + sTP ) tion measurement is available. In this case, by means of a second-order state skT V s2 Θr (s) = kT P + + Θmd (s) (8.43) variable filter, it is possible to reconstruct velocity and acceleration. However, KP km KP KV the greater lags induced by the use of a second-order filter typically degrade skT V s2 (1 + km KA kT A ) the performance with respect to the use of a first-order filter, because of lim- Θr (s) = kT P + + Θmd (s); (8.44) KP km KP KV KA itations imposed on the filter bandwidth by numerical implementation of the controller and filter. in this way, tracking of the desired joint position Θmd (s) is achieved, if not Notice that the above derivation is based on an ideal dynamic model, i.e., for the effect of disturbances. Notice that computing time derivatives of the when the effects of transmission elasticity as well as those of amplifier and desired trajectory is not a problem, once ϑmd (t) is known analytically. The motor electrical time constants are neglected. This implies that satisfaction tracking control schemes, resulting from simple manipulation of (8.42), (8.43), of design requirements imposing large values of feedback gains may not be (8.44) are reported respectively in Figs. 8.12, 8.13, 8.14, where M (s) indicates verified in practice, since the existence of unmodelled dynamics — such as the motor transfer function in (5.11), with km and Tm as in (5.12). electric dynamics, elastic dynamics due to non-perfectly rigid transmissions, All the solutions allow the input trajectory to be tracked within the range filter dynamics for the third scheme — might lead to degrading the system and of validity and linearity of the models employed. It is worth noticing that, as eventually driving it to instability. In summary, the above solutions constitute the number of nested feedback loops increases, a less accurate knowledge of design guidelines whose limits should be emphasized with regard to the specific the system model is required to perform feedforward compensation. In fact, application. Tm and km are required for the scheme of Fig. 8.12, only km is required for the scheme of Fig. 8.13, and km again — but with reduced weight — for the 8.3.2 Decentralized Feedforward Compensation scheme of Fig. 8.14. It is worth recalling that perfect tracking can be obtained only under the When the joint control servos are required to track reference trajectories with assumption of exact matching of the controller and feedforward compensation high values of speed and acceleration, the tracking capabilities of the scheme in parameters with the process parameters, as well as of exact modelling and Fig. 8.6 are unavoidably degraded. The adoption of a decentralized feedforward linearity of the physical system. Deviations from the ideal values cause a compensation allows a reduction of the tracking error. Therefore, in view performance degradation that should be analyzed case by case. of the closed-loop input/output transfer functions in (8.24), (8.28), (8.35), 8.3 Decentralized Control 321 322 8 Motion Control
Fig. 8.13. Block scheme of position and velocity feedback control with decentralized feedforward compensation Fig. 8.15. Equivalent control scheme of PI type
Fig. 8.14. Block scheme of position, velocity and acceleration feedback control with decentralized feedforward compensation
The presence of saturation blocks in the schemes of Figs. 8.12, 8.13, 8.14 Fig. 8.16. Equivalent control scheme of PID type
is to be intended as intentional nonlinearities whose function is to limit rele- vant physical quantities during transients; the greater the number of feedback However, tuning of regulator parameters is less straightforward, and the elim- loops, the greater the number of quantities that can be limited (velocity, ac- ination of inner feedback loops prevents the possibility of setting saturations celeration, and motor voltage). To this end, notice that trajectory tracking is on velocity and/or acceleration. The control structures equivalent to those obviously lost whenever any of the above quantities saturates. This situation of Figs. 8.12, 8.13, 8.14 are illustrated in Figs. 8.15, 8.16, 8.17, respectively; often occurs for industrial manipulators required to execute point-to-point control actions of PI, PID, PIDD2 type are illustrated which are respectively motions; in this case, there is less concern about the actual trajectories fol- equivalent to the cases of: position feedback; position and velocity feedback; lowed, and the actuators are intentionally taken to operate at the current position, velocity and acceleration feedback. limits so as to realize the fastest possible motions. It is worth noticing that the equivalent control structures in Figs. 8.15–8.17 After simple block reduction on the above schemes, it is possible to de- are characterized by the presence of the feedforward action (Tm /km )ϑ̈md + termine equivalent control structures that utilize position feedback only and (1/km )ϑ̇md . If the motor is current-controlled and not voltage-controlled, by regulators with standard actions. It should be emphasized that the two solu- recalling (5.13), the feedforward action is equal to (ki /kt )(Im ϑ̈md + Fm ϑ̇md ). tions are equivalent in terms of disturbance rejection and trajectory tracking. If ϑ̇m ≈ ϑ̇md , ϑ̈m ≈ ϑ̈md and the disturbance is negligible, the term Im ϑ̈d + 8.3 Decentralized Control 323 324 8 Motion Control
Fig. 8.18. Control scheme with current-controlled drive and current feedforward
action
all those factors that have not been modelled, such as implementation of
discrete-time controllers in lieu of the continuous-time controllers analyzed
in theory, presence of finite sampling time, neglected dynamic effects (e.g.,
joint elasticity, structural resonance, finite transducer bandwidth), and sensor
noise. In fact, the influence of such factors in the implementation of the above
Fig. 8.17. Equivalent control scheme of PIDD2 type controllers may cause a severe system performance degradation for much too
large values of feedback gains.
Fm ϑ̇d represents the driving torque providing the desired velocity and accel- eration, as indicated by (5.3). By setting 8.4 Computed Torque Feedforward Control 1 iad = (Im ϑ̈md + Fm ϑ̇md ), Define the tracking error e(t) = ϑmd (t) − ϑm (t). With reference to the most kt general scheme (Fig. 8.17), the output of the PIDD2 regulator can be written the feedforward action can be rewritten in the form ki iad . This shows that, in as ” t the case the drive is current-controlled, it is possible to replace the acceleration and velocity feedforward actions with a current and thus a torque feedforward a2 ë + a1 ė + a0 e + a−1 e(ς)dς action, which is to be properly computed with reference to the desired motion. which describes the time evolution of the error. The constant coefficients This equivalence is illustrated in Fig. 8.18, where M (s) has been replaced a2 , a1 , a0 , a−1 are determined by the particular solution adopted. Summing by the block scheme of an electric drive of Fig. 5.2, where the parameters of the contribution of the feedforward actions and of the disturbance to this the current loop are chosen so as to realize a torque-controlled generator. The expression yields feedforward action represents a reference for the motor current, which im- Tm 1 Ra poses the generation of the nominal torque to execute the desired motion; the ϑ̈md + ϑ̇md − d, km km kt presence of the position reference allows the closure of a feedback loop which, where in view of the adoption of a standard regulator with transfer function CR (s), Tm Im Ra 1 confers robustness to the presented control structure. In summary, the perfor- = km = . km kt kv mance that can be achieved with velocity and acceleration feedforward actions The input to the motor (Fig. 8.6) has then to satisfy the following equation: and voltage-controlled actuator can be achieved with a current-controlled ac- tuator and a desired torque feedforward action. ” t Tm 1 Ra Tm 1 The above schemes can incorporate the typical structure of the controllers a2 ë + a1 ė + a0 e + a−1 e(ς)dς + ϑ̈md + ϑ̇md − d= ϑ̈m + ϑ̇m . km km kt km km actually implemented in the control architectures of industrial robots. In these systems it is important to choose the largest possible gains so that model With a suitable change of coefficients, this can be rewritten as inaccuracy and coupling terms do not appreciably affect positions of the single ” t Ra joints. As pointed out above, the upper limit on the gains is imposed by a2 ë + a1 ė + a0 e + a−1 e(ς)dς = d. kt 8.4 Computed Torque Feedforward Control 325 326 8 Motion Control
disturbance expressed by (8.22) and in turn allows the control system to
operate in a better condition.
This solution is illustrated in the scheme of Fig. 8.19, which conceptually
describes the control system of a manipulator with computed torque control.
The feedback control system is representative of the n independent joint con-
trol servos; it is decentralized , since controller i elaborates references and mea-
surements that refer to single Joint i. The interactions between the various
joints, expressed by d, are compensated by a centralized action whose function
is to generate a feedforward action that depends on the joint references as well
as on the manipulator dynamic model. This action compensates the nonlinear
coupling terms due to inertial, Coriolis, centrifugal, and gravitational forces
that depend on the structure and, as such, vary during manipulator motion.
Although the residual disturbance term d = dd − d vanishes only in the
ideal case of perfect tracking (q = q d ) and exact dynamic modelling, d is
Fig. 8.19. Block scheme of computed torque feedforward control
representative of interaction disturbances of considerably reduced magnitude
with respect to d. Hence, the computed torque technique has the advantage to
This equation describes the error dynamics and shows that any physically alleviate the disturbance rejection task for the feedback control structure and executable trajectory is asymptotically tracked only if the disturbance term in turn allows limited gains. Notice that expression (8.45) in general imposes a d(t) = 0. With the term physically executable it is meant that the saturation computationally demanding burden on the centralized part of the controller. limits on the physical quantities, e.g., current and voltage in electric motors, Therefore, in those applications where the desired trajectory is generated in are not violated in the execution of the desired trajectory. real time with regard to exteroceptive sensory data and commands from higher The presence of the term d(t) causes a tracking error whose magnitude is hierarchical levels of the robot control architecture,5 on-line computation of reduced as much as the disturbance frequency content is located off to the left the centralized feedforward action may require too much time.6 of the lower limit of the bandwidth of the error system. The disturbance/error Since the actual controller is to be implemented on a computer with a transfer function is given by finite sampling time, torque computation has to be carried out during this interval of time; in order not to degrade dynamic system performance, typical Ra sampling times are of the order of the millisecond. s E(s) kt Therefore, it may be worth performing only a partial feedforward action = 3 , D(s) a2 s + a1 s2 + a0 s + a−1 so as to compensate those terms of (8.45) that give the most relevant con- tributions during manipulator motion. Since inertial and gravitational terms and thus the adoption of loop gains which are not realizable for the above dominate velocity-dependent terms (at operational joint speeds not greater discussed reasons is often required. than a few radians per second), a partial compensation can be achieved by Nevertheless, even if the term d(t) has been introduced as a disturbance, computing only the gravitational torques and the inertial torques due to the its expression is given by (8.22). It is then possible to add a further term to diagonal elements of the inertia matrix. In this way, only the terms depending the previous feedforward actions which is able to compensate the disturbance on the global manipulator configuration are compensated while those deriving itself rather than its effects. In other words, by taking advantage of model from motion interaction with the other joints are not. knowledge, the rejection effort of an independent joint control scheme can be Finally, it should be pointed out that, for repetitive trajectories, the above lightened with notable simplification from the implementation viewpoint. compensating contributions can be computed off-line and properly stored on Let q d (t) be the desired joint trajectory and q md (t) the corresponding the basis of a trade-off solution between memory capacity and computational actuator trajectory as in (8.2). By adopting an inverse model strategy, the requirements of the control architecture. feedforward action Ra K −1t dd can be introduced with
dd = K −1 −1 −1 −1 −1 r ΔB(q d )K r q̈ md + K r C(q d , q̇ d )K r q̇ md + K r g(q d ), (8.45) 5 See also Chap. 6. 6 In this regard, the problem of real-time computation of compensating torques can where Ra and K t denote the diagonal matrices of armature resistances and be solved by resorting to efficient recursive formulations of manipulator inverse torque constants of the actuators. This action tends to compensate the actual dynamics, such as the Newton–Euler algorithm presented in Chap. 7. 8.5 Centralized Control 327 328 8 Motion Control
8.5 Centralized Control 8.5.1 PD Control with Gravity Compensation
In the previous sections several techniques have been discussed that allow Let a constant equilibrium posture be assigned for the system as the vector of the design of independent joint controllers. These are based on a single- desired joint variables q d . It is desired to find the structure of the controller input/single-output approach, since interaction and coupling effects between which ensures global asymptotic stability of the above posture. the joints have been considered as disturbances acting on each single joint The determination of the control input which stabilizes the system around drive system. the equilibrium posture is based on the Lyapunov direct method. On the other hand, when large operational speeds are required or direct- Take the vector [ q T q̇ T ]T as the system state, where drive actuation is employed (K r = I), the nonlinear coupling terms strongly q = qd − q (8.46) influence system performance. Therefore, considering the effects of the com- ponents of d as a disturbance may generate large tracking errors. In this case, represents the error between the desired and the actual posture. Choose the it is advisable to design control algorithms that take advantage of a detailed following positive definite quadratic form as Lyapunov function candidate: knowledge of manipulator dynamics so as to compensate for the nonlinear coupling terms of the model. In other words, it is necessary to eliminate the 1 T 1 V (q̇, q) = q̇ B(q)q̇ + q T K P q > 0 ∀q̇, q = 0 (8.47) causes rather than to reduce the effects induced by them; that is, to generate 2 2 compensating torques for the nonlinear terms in (8.22). This leads to central- where K P is an (n × n) symmetric positive definite matrix. An energy-based ized control algorithms that are based on the (partial or complete) knowledge interpretation of (8.47) reveals a first term expressing the system kinetic en- of the manipulator dynamic model. ergy and a second term expressing the potential energy stored in the system Whenever the robot is endowed with the torque sensors at the joint motors of equivalent stiffness K P provided by the n position feedback loops. presented in Sect. 5.4.1, those measurements can be conveniently utilized to Differentiating (8.47) with respect to time, and recalling that q d is con- generate the compensation action, thus avoiding the on-line computation of stant, yields the terms of the dynamic model. 1 As shown by the dynamic model (8.1), the manipulator is not a set of V̇ = q̇ T B(q)q̈ + q̇ T Ḃ(q)q̇ − q̇ T K P q. (8.48) 2 n decoupled system but it is a multivariable system with n inputs (joint Solving (8.7) for Bq̈ and substituting it in (8.48) gives torques) and n outputs (joint positions) interacting between them by means of nonlinear relations.7 1 T In order to follow a methodological approach which is consistent with V̇ = q̇ Ḃ(q) − 2C(q, q̇) q̇ − q̇ T F q̇ + q̇ T u − g(q) − K P q . (8.49) 2 control design, it is necessary to treat the control problem in the context of nonlinear multivariable systems. This approach will obviously account for the The first term on the right-hand side is null since the matrix N = Ḃ − 2C manipulator dynamic model and lead to finding nonlinear centralized control satisfies (7.49). The second term is negative definite. Then, the choice laws, whose implementation is needed for high manipulator dynamic perfor- mance. On the other hand, the above computed torque control can be inter- u = g(q) + K P q, (8.50) preted in this framework, since it provides a model-based nonlinear control describing a controller with compensation of gravitational terms and a pro- term to enhance trajectory tracking performance. Notice, however, that this portional action, leads to a negative semi-definite V̇ since action is inherently performed off line, as it is computed on the time history of the desired trajectory and not of the actual one. V̇ = 0 q̇ = 0, ∀q. In the following, the problem of the determination of the control law u ensuring a given performance to the system of manipulator with drives is This result can be obtained also by taking the control law tackles. Since (8.17) can be considered as a proportional relationship between v c and u, the centralized control schemes below refer directly to the generation u = g(q) + K P q − K D q̇, (8.51) of control toques u. with K D positive definite, corresponding to a nonlinear compensation action of gravitational terms with a linear proportional-derivative (PD) action. In fact, substituting (8.51) into (8.49) gives 7 See Appendix C for the basic concepts on control of nonlinear mechanical systems. V̇ = −q̇ T (F + K D )q̇, (8.52) 8.5 Centralized Control 329 330 8 Motion Control
Fig. 8.21. Exact linearization performed by inverse dynamics control
Fig. 8.20. Block scheme of joint space PD control with gravity compensation 8.5.2 Inverse Dynamics Control
Consider now the problem of tracking a joint space trajectory. The reference
which reveals that the introduction of the derivative term causes an increase framework is that of control of nonlinear multivariable systems. The dynamic of the absolute values of V̇ along the system trajectories, and then it gives an model of an n-joint manipulator is expressed by (8.7) which can be rewritten improvement of system time response. Notice that the inclusion of a derivative as action in the controller, as in (8.51), is crucial when direct-drive manipulators B(q)q̈ + n(q, q̇) = u, (8.55) are considered. In that case, in fact, mechanical viscous damping is practi- where for simplicity it has been set cally null, and current control does not allow the exploitation of the electrical viscous damping provided by voltage-controlled actuators. n(q, q̇) = C(q, q̇)q̇ + F q̇ + g(q). (8.56) According to the above, the function candidate V decreases as long as q̇ = 0 for all system trajectories. It can be shown that the system reaches an The approach that follows is founded on the idea to find a control vector u, as equilibrium posture. To find such posture, notice that V̇ ≡ 0 only if q̇ ≡ 0. a function of the system state, which is capable of realizing an input/output System dynamics under control (8.51) is given by relationship of linear type; in other words, it is desired to perform not an approximate linearization but an exact linearization of system dynamics ob- B(q)q̈ + C(q, q̇)q̇ + F q̇ + g(q) = g(q) + K P q − K D q̇. (8.53) tained by means of a nonlinear state feedback . The possibility of finding such a linearizing controller is guaranteed by the particular form of system dynam- At the equilibrium (q̇ ≡ 0, q̈ ≡ 0) it is ics. In fact, the equation in (8.55) is linear in the control u and has a full-rank KP q = 0 (8.54) matrix B(q) which can be inverted for any manipulator configuration. Taking the control u as a function of the manipulator state in the form and then q = qd − q ≡ 0 u = B(q)y + n(q, q̇), (8.57)
is the sought equilibrium posture. The above derivation rigorously shows that leads to the system described by any manipulator equilibrium posture is globally asymptotically stable under a controller with a PD linear action and a nonlinear gravity compensating q̈ = y action. Stability is ensured for any choice of K P and K D , as long as these are positive definite matrices. The resulting block scheme is shown in Fig. 8.20. where y represents a new input vector whose expression is to be determined The control law requires the on-line computation of the term g(q). If com- yet; the resulting block scheme is shown in Fig. 8.21. The nonlinear control pensation is imperfect, the above discussion does not lead to the same result; law in (8.57) is termed inverse dynamics control since it is based on the com- this aspect will be revisited later with reference to robustness of controllers putation of manipulator inverse dynamics. The system under control (8.57) performing nonlinear compensation. is linear and decoupled with respect to the new input y. In other words, the component yi influences, with a double integrator relationship, only the joint variable qi , independently of the motion of the other joints. 8.5 Centralized Control 331 332 8 Motion Control
The resulting block scheme is illustrated in Fig. 8.22, in which two feed-
back loops are represented; an inner loop based on the manipulator dynamic
model, and an outer loop operating on the tracking error. The function of
the inner loop is to obtain a linear and decoupled input/output relationship,
whereas the outer loop is required to stabilize the overall system. The con-
troller design for the outer loop is simplified since it operates on a linear and
time-invariant system. Notice that the implementation of this control scheme
requires computation of the inertia matrix B(q) and of the vector of Coriolis,
centrifugal, gravitational, and damping terms n(q, q̇) in (8.56). Unlike com-
puted torque control, these terms must be computed on-line since control is
now based on nonlinear feedback of the current system state, and thus it is
not possible to precompute the terms off line as for the previous technique.
The above technique of nonlinear compensation and decoupling is very at-
Fig. 8.22. Block scheme of joint space inverse dynamics control tractive from a control viewpoint since the nonlinear and coupled manipulator
dynamics is replaced with n linear and decoupled second-order subsystems.
Nonetheless, this technique is based on the assumption of perfect cancellation
In view of the choice (8.57), the manipulator control problem is reduced
of dynamic terms, and then it is quite natural to raise questions about sensi-
to that of finding a stabilizing control law y. To this end, the choice tivity and robustness problems due to unavoidably imperfect compensation. y = −K P q − K D q̇ + r (8.58) Implementation of inverse dynamics control laws indeed requires that pa- rameters of the system dynamic model are accurately known and the complete leads to the system of second-order equations equations of motion are computed in real time. These conditions are difficult to verify in practice. On one hand, the model is usually known with a certain q̈ + K D q̇ + K P q = r (8.59) degree of uncertainty due to imperfect knowledge of manipulator mechani- cal parameters, existence of unmodelled dynamics, and model dependence on which, under the assumption of positive definite matrices K P and K D , is end-effector payloads not exactly known and thus not perfectly compensated. asymptotically stable. Choosing K P and K D as diagonal matrices of the On the other hand, inverse dynamics computation is to be performed at sam- type pling times of the order of a millisecond so as to ensure that the assumption 2 2 of operating in the continuous time domain is realistic. This may pose severe K P = diag{ωn1 , … , ωnn } K D = diag{2ζ1 ωn1 , … , 2ζn ωnn }, constraints on the hardware/software architecture of the control system. In such cases, it may be advisable to lighten the computation of inverse dynamics gives a decoupled system. The reference component ri influences only the joint and compute only the dominant terms. variable qi with a second-order input/output relationship characterized by a On the basis of the above remarks, from an implementation viewpoint, natural frequency ωni and a damping ratio ζi . compensation may be imperfect both for model uncertainty and for the ap- Given any desired trajectory q d (t), tracking of this trajectory for the out- proximations made in on-line computation of inverse dynamics. In the follow- put q(t) is ensured by choosing ing, two control techniques are presented which are aimed at counteracting r = q̈ d + K D q̇ d + K P q d . (8.60) the effects of imperfect compensation. The first consists of the introduction of an additional term to an inverse dynamics controller which provides robust- In fact, substituting (8.60) into (8.59) gives the homogeneous second-order ness to the control system by counteracting the effects of the approximations differential equation made in on-line computation of inverse dynamics. The second adapts the pa- ¨ + K D q˙ + K P q = 0 q (8.61) rameters of the model used for inverse dynamics computation to those of the true manipulator dynamic model. expressing the dynamics of position error (8.46) while tracking the given tra- ˙ jectory. Such error occurs only if q(0) and/or q(0) are different from zero and converges to zero with a speed depending on the matrices K P and K D chosen. 8.5 Centralized Control 333 334 8 Motion Control
8.5.3 Robust Control By taking q In the case of imperfect compensation, it is reasonable to assume in (8.55) a ξ= ˙ , (8.69) q control vector expressed by as the system state, the following first-order differential matrix equation is ) u = B(q)y ) (q, q̇) +n (8.62) obtained: ) and n ξ̇ = Hξ + D(q̈ d − y + η), (8.70) where B ) represent the adopted computational model in terms of es- timates of the terms in the dynamic model. The error on the estimates, i.e., where H and D are block matrices of dimensions (2n × 2n) and (2n × n), the uncertainty, is represented by respectively: O I O ) −B B=B ) −n n=n (8.63) H= O O D= I . (8.71)
and is due to imperfect model compensation as well as to intentional simplifi- Then, the problem of tracking a given trajectory can be regarded as the prob- cation in inverse dynamics computation. Notice that by setting B ) = B̄ (where lem of finding a control law y which stabilizes the nonlinear time-varying error ) = 0, the B̄ is the diagonal matrix of average inertia at the joint axes) and n system (8.70). above decentralized control scheme is recovered where the control action y Control design is based on the assumption that, even though the uncer- can be of the general PID type computed on the error. tainty η is unknown, an estimate on its range of variation is available. The Using (8.62) as a nonlinear control law gives sought control law y should guarantee asymptotic stability of (8.70) for any η varying in the above range. By recalling that η in (8.66) is a function of q, ) +n Bq̈ + n = By ) (8.64) q̇, q̈ d , the following assumptions are made: where functional dependence has been omitted. Since the inertia matrix B is supt≥0 q̈ d < QM < ∞ ∀q̈ d (8.72) invertible, it is ) − I)y + B −1 n = y − η ) I − B −1 (q)B(q) ≤ α ≤ 1 ∀q q̈ = y + (B −1 B (8.65) (8.73)
where n ≤Φ<∞ ∀q, q̇. (8.74) ) − B −1 n. η = (I − B −1 B)y (8.66) Taking as above Assumption (8.72) is practically satisfied since any planned trajectory cannot require infinite accelerations. y = q̈ d + K D (q̇ d − q̇) + K P (q d − q), Regarding assumption (8.73), since B is a positive definite matrix with upper and lower limited norms, the following inequality holds: leads to ¨ + K D q˙ + K P q = η. q (8.67) 0 < Bm ≤ B −1 (q) ≤ BM < ∞ ∀q, (8.75) The system described by (8.67) is still nonlinear and coupled, since η is a ˙ error convergence to zero is not ensured by the ) always exists which satisfies (8.73). In fact, by setting and then a choice for B nonlinear function of q and q; term on the left-hand side only. 2 To find control laws ensuring error convergence to zero while tracking a ) = B I, BM + Bm trajectory even in the face of uncertainties, a linear PD control is no longer sufficient. To this end, the Lyapunov direct method can be utilized again for from (8.73) it is the design of an outer feedback loop on the error which should be robust to the uncertainty η. ) −I ≤ BM − Bm B −1 B = α < 1. (8.76) Let the desired trajectory q d (t) be assigned in the joint space and let BM + Bm q = q d − q be the position error. Its first time-derivative is q˙ = q̇ d − q̇, while ) is a more accurate estimate of the inertia matrix, the inequality is satisfied If B its second time-derivative in view of (8.65) is with values of α that can be made arbitrarily small (in the limit, it is B ) =B ¨ = q̈ − y + η. q (8.68) and α = 0). d 8.5 Centralized Control 335 336 8 Motion Control
Finally, concerning assumption (8.74), observe that n is a function of q and
q̇. For revolute joints a periodical dependence on q is obtained, while for pris- matic joints a linear dependence is obtained, but the joint ranges are limited and then the above contribution is also limited. On the other hand, regarding the dependence on q̇, unbounded velocities for an unstable system may arise in the limit, but in reality saturations exist on the maximum velocities of the motors. In summary, assumption (8.74) can be realistically satisfied, too. With reference to (8.65), choose now
y = q̈ d + K D q˙ + K P q + w (8.77)
where the PD term ensures stabilization of the error dynamic system matrix, q̈ d provides a feedforward term, and the term w is to be chosen to guarantee robustness to the effects of uncertainty described by η in (8.66). Using (8.77) and setting K = [ K P K D ] yields - + D(η − w), ξ̇ = Hξ (8.78) Fig. 8.23. Block scheme of joint space robust control where - = (H − DK) = O I H The first term on the right-hand side of (8.82) is negative definite and then −K P −K D the solutions converge if ξ ∈ N (D T Q). If instead ξ ∈ N (D T Q), the control is a matrix whose eigenvalues all have negative real parts — K P and K D w must be chosen so as to render the second term in (8.82) less than or equal being positive definite — which allows the desired error system dynamics to to zero. By setting z = D T Qξ, the second term in (8.82) can be rewritten as 2 2 be prescribed. In fact, by choosing K P = diag{ωn1 , … , ωnn } and K D = z T (η − w). Adopting the control law diag{2ζ1 ωn1 , … , 2ζn ωnn }, n decoupled equations are obtained as regards the ρ linear part. If the uncertainty term vanishes, it is obviously w = 0 and the w= z ρ>0 (8.83) ) =B z above result with an exact inverse dynamics controller is recovered (B and n) = n). gives8 To determine w, consider the following positive definite quadratic form as ρ T Lyapunov function candidate: z T (η − w) = z T η − z z z V (ξ) = ξ T Qξ > 0 ∀ξ = 0, (8.79) ≤ z η −ρ z = z ( η − ρ). (8.84) where Q is a (2n × 2n) positive definite matrix. The derivative of V along the trajectories of the error system (8.78) is Then, if ρ is chosen so that T V̇ = ξ̇ Qξ + ξ Qξ̇T (8.80) ρ≥ η ∀q, q̇, q̈ d , (8.85) T - Q + QH)ξ = ξ T (H - + 2ξ T QD(η − w). the control (8.83) ensures that V̇ is less than zero along all error system trajectories. - has eigenvalues with all negative real parts, it is well-known that for Since H In order to satisfy (8.85), notice that, in view of the definition of η in (8.66) any symmetric positive definite matrix P , the equation and of assumptions (8.72)–(8.74), and being w = ρ, it is - Q + QH H T - = −P (8.81) η ≤ I − B −1 B ) q̈ d + K ξ + w + B −1 n 8 gives a unique solution Q which is symmetric positive definite as well. In view Notice that it is necessary to divide z by the norm of z so as to obtain a linear of this, (8.80) becomes dependence on z of the term containing the control z T w, and thus to effectively counteract, for z → 0, the term containing the uncertainty z T η which is linear V̇ = −ξ T P ξ + 2ξ T QD(η − w). (8.82) in z. 8.5 Centralized Control 337 338 8 Motion Control
≤ αQM + α K ξ + αρ + BM Φ. (8.86)
Therefore, setting 1 ρ≥ (αQM + α K ξ + BM Φ) (8.87) 1−α gives ρ V̇ = −ξ T P ξ + 2z T η− z <0 ∀ξ = 0. (8.88) z The resulting block scheme is illustrated in Fig. 8.23. To summarize, the presented approach has lead to finding a control law which is formed by three different contributions: • The term By+) ) ensures an approximate compensation of nonlinear effects n Fig. 8.24. Error trajectory with robust control and joint decoupling. • The term q̈ d + K D q˙ + K P q introduces a linear feedforward action (q̈ d + In order to provide an intuitive interpretation of this law, notice that (8.89) K D q̇ d +K P q d ) and linear feedback action (−K D q̇−K P q) which stabilizes gives a null control input when the error is in the null space of matrix D T Q. the error system dynamics. On the other hand, (8.83) has an equivalent gain tending to infinity when z • The term w = (ρ/ z )z represents the robust contribution that counter- tends to the null vector, thus generating a control input of limited magnitude. acts the indeterminacy B and n in computing the nonlinear terms that Since these inputs commute at an infinite frequency, they force the error depend on the manipulator state; the greater the uncertainty, the greater system dynamics to stay on the sliding subspace. With reference to the above the positive scalar ρ. The resulting control law is of the unit vector type, example, control law (8.89) gives rise to a hyperplane z = 0 which is no since it is described by a vector of magnitude ρ aligned with the unit vector longer attractive, and the error is allowed to vary within a boundary layer of z = D T Qξ, ∀ξ. whose thickness depends on (Fig. 8.25). The introduction of a contribution based on the computation of a suitable All the resulting trajectories under the above robust control reach the sub- linear combination of the generalized error confers robustness to a control space z = D T Qξ = 0 that depends on the matrix Q in the Lyapunov function scheme based on nonlinear compensation. Even if the manipulator is accu- V . On this attractive subspace, termed sliding subspace, the control w is ide- rately modeled, indeed, an exact nonlinear compensation may be computa- ally commuted at an infinite frequency and all error components tend to zero tionally demanding, and thus it may require either a sophisticated hardware with a transient depending on the matrices Q, K P , K D . A characterization architecture or an increase of the sampling time needed to compute the con- of an error trajectory in the two-dimensional case is given in Fig. 8.24. Notice trol law. The solution then becomes weak from an engineering viewpoint, due that in the case ξ(0) = 0, with ξ(0) ∈ N (D T Q), the trajectory is attracted either to infeasible costs of the control architecture, or to poor performance on the sliding hyperplane (a line) z = 0 and tends towards the origin of the at decreased sampling rates. Therefore, considering a partial knowledge of the error state space with a time evolution governed by ρ. manipulator dynamic model with an accurate, pondered estimate of uncer- In reality, the physical limits on the elements employed in the controller tainty may suggest robust control solutions of the kind presented above. It impose a control signal that commutes at a finite frequency, and the trajec- is understood that an estimate of the uncertainty should be found so as to tories oscillate around the sliding subspace with a magnitude as low as the impose control inputs which the mechanical structure can bear. frequency is high. Elimination of these high-frequency components (chattering) can be achie- ved by adopting a robust control law which, even if it does not guarantee error 8.5.4 Adaptive Control convergence to zero, ensures bounded-norm errors. A control law of this type is ⎧ ρ The computational model employed for computing inverse dynamics typically ⎪ ⎨ z per z ≥ has the same structure as that of the true manipulator dynamic model, but z parameter estimate uncertainty does exist. In this case, it is possible to devise w= (8.89) ⎪ ⎩ ρz solutions that allow an on-line adaptation of the computational model to the per z < . 8.5 Centralized Control 339 340 8 Motion Control
that depends on velocity, not only on the basis of the desired velocity but also
on the basis of the position tracking error. A similar argument also holds for
the acceleration contribution, where a term depending on the velocity tracking
error is considered besides the desired acceleration.
The term K D σ is equivalent to a PD action on the error if σ is taken as
σ = q̇ r − q̇ = q˙ + Λq. (8.93)
Substituting (8.91) into (8.90) and accounting for (8.93) yields
B(q)σ̇ + C(q, q̇)σ + F σ + K D σ = 0. (8.94)
Consider the Lyapunov function candidate
1 T 1
Fig. 8.25. Error trajectory with robust control and chattering elimination V (σ, q) = σ B(q)σ + q T M q > 0 ∀σ, q = 0, (8.95) 2 2
dynamic model , thus performing a control scheme of the inverse dynamics where M is an (n × n) symmetric positive definite matrix; the introduction type. of the second term in (8.95) is necessary to obtain a Lyapunov function of the The possibility of finding adaptive control laws is ensured by the property entire system state which vanishes for q = 0 and q˙ = 0. The time derivative of linearity in the parameters of the dynamic model of a manipulator. In of V along the trajectories of system (8.94) is fact, it is always possible to express the nonlinear equations of motion in a 1 linear form with respect to a suitable set of constant dynamic parameters as V̇ = σ T B(q)σ̇ + σ T Ḃ(q)σ + q T M q˙ 2 in (7.81). The equation in (8.7) can then be written as ˙ = −σ T (F + K D )σ + q T M q, (8.96) B(q)q̈ + C(q, q̇)q̇ + F q̇ + g(q) = Y (q, q̇, q̈)π = u, (8.90) where the skew-symmetry propertyv of the matrix N = Ḃ − 2C has been where π is a (p × 1) vector of constant parameters and Y is an (n × p) exploited. In view of the expression of σ in (8.93), with diagonal Λ and K D , matrix which is a function of joint positions, velocities and accelerations. This it is convenient to choose M = 2ΛK D ; this leads to property of linearity in the dynamic parameters is fundamental for deriving T adaptive control laws, among which the technique illustrated below is one of V̇ = −σ T F σ − q˙ K D q˙ − q T ΛK D Λq. (8.97) the simplest. At first, a control scheme which can be derived through a combined com- This expression shows that the time derivative is negative definite since it puted torque/inverse dynamics approach is illustrated. The computational vanishes only if q ≡ 0 and q˙ ≡ 0; thus, it follows that the origin of the state model is assumed to coincide with the dynamic model. space [ q T σ T ]T = 0 is globally asymptotically stable. It is worth noticing Consider the control law that, unlike the robust control case, the error trajectory tends to the subspace σ = 0 without the need of a high-frequency control. u = B(q)q̈ r + C(q, q̇)q̇ r + F q̇ r + g(q) + K D σ, (8.91) On the basis of this notable result, the control law can be made adaptive with respect to the vector of parameters π. with K D a positive definite matrix. The choice Suppose that the computational model has the same structure as that of the manipulator dynamic model, but its parameters are not known exactly. q̇ r = q̇ d + Λq ˙ q̈ r = q̈ d + Λq, (8.92) The control law (8.91) is then modified into with Λ a positive definite (usually diagonal) matrix, allows the nonlinear com- ) u = B(q)q̈ ) ) ) + KDσ r + C(q, q̇)q̇ r + F q̇ r + g (8.98) pensation and decoupling terms to be expressed as a function of the desired = Y (q, q̇, q̇ r , q̈ r )) π + K D σ, velocity and acceleration, corrected by the current state (q and q̇) of the ma- nipulator. In fact, notice that the term q̇ r = q̇ d + Λq weighs the contribution 8.5 Centralized Control 341 342 8 Motion Control
where π) represents the available estimate on the parameters and, accordingly, ) C, B, ) F ), g ) denote the estimated terms in the dynamic model. Substituting control (8.98) into (8.90) gives
B(q)σ̇ + C(q, q̇)σ + F σ + K D σ = −B(q)q̈ r − C(q, q̇)q̇ r − F q̇ r − g(q) = −Y (q, q̇, q̇ r , q̈ r )π, (8.99)
where the property of linearity in the error parameter vector
) −π
π=π (8.100)
has been conveniently exploited. In view of (8.63), the modelling error is characterized by Fig. 8.26. Block scheme of joint space adaptive control ) −B B=B ) −C C=C ) −F F =F ) − g. g=g (8.101)
It is worth remarking that, in view of position (8.92), the matrix Y does not and the parameter adaptive law depend on the actual joint accelerations but only on their desired values; this )˙ = K −1 π T ˙ π Y (q, q̇, q̇ r , q̈ r )(q + Λq), avoids problems due to direct measurement of acceleration. At this point, modify the Lyapunov function candidate in (8.95) into the globally asymptotically converge to σ = 0 and q = 0, which implies conver- form ˙ and boundedness of π gence to zero of q, q, ) . The equation in (8.99) shows 1 T 1 that asymptotically it is V (σ, q, π) = σ B(q)σ + q T ΛK D q + π T K π π > 0 ∀σ, q, π = 0, 2 2 (8.102) π − π) = 0. Y (q, q̇, q̇ r , q̈ r )() (8.105) which features an additional term accounting for the parameter error (8.100), with K π symmetric positive definite. The time derivative of V along the This equation does not imply that π ) tends to π; indeed, convergence of param- trajectories of system (8.99) is eters to their true values depends on the structure of the matrix Y (q, q̇, q̇ r , q̈ r ) and then on the desired and actual trajectories. Nonetheless, the followed ap- T V̇ = −σ T F σ − q˙ K D q˙ − q T ΛK D Λq + π T K π π˙ − Y T (q, q̇, q̇ r , q̈ r )σ . proach is aimed at solving a direct adaptive control problem, i.e., finding a (8.103) control law that ensures limited tracking errors, and not at determining the If the estimate of the parameter vector is updated as in the adaptive law actual parameters of the system (as in an indirect adaptive control problem). The resulting block scheme is illustrated in Fig. 8.26. To summarize, the above )˙ = K −1 π T π Y (q, q̇, q̇ r , q̈ r )σ, (8.104) control law is formed by three different contributions:
the expression in (8.103) becomes • The term Y π ) describes a control action of inverse dynamics type which ensures an approximate compensation of nonlinear effects and joint decou- T V̇ = −σ T F σ − q˙ K D q˙ − q T ΛK D Λq pling. • The term K D σ introduces a stabilizing linear control action of PD type )˙ = π˙ — π is constant. since π on the tracking error . By an argument similar to above, it is not difficult to show that the tra- • The vector of parameter estimates π ) is updated by an adaptive law of jectories of the manipulator described by the model gradient type so as to ensure asymptotic compensation of the terms in the manipulator dynamic model; the matrix K π determines the convergence B(q)q̈ + C(q, q̇)q̇ + F q̇ + g(q) = u, rate of parameters to their asymptotic values. under the control law Notice that, with σ ≈ 0, the control law (8.98) is equivalent to a pure inverse dynamics compensation of the computed torque type on the basis of π + K D (q˙ + Λq) u = Y (q, q̇, q̇ r , q̈ r )) 8.6 Operational Space Control 343 344 8 Motion Control
desired velocities and accelerations; this is made possible by the fact that Yπ ) ≈ Y π. The control law with parameter adaptation requires the availability of a complete computational model and it does not feature any action aimed at reducing the effects of external disturbances. Therefore, a performance degra- dation is expected whenever unmodelled dynamic effects, e.g., when a reduced computational model is used, or external disturbances occur. In both cases, Fig. 8.27. Block scheme of Jacobian inverse control the effects induced on the output variables are attributed by the controller to parameter estimate mismatching; as a consequence, the control law attempts to counteract those effects by acting on quantities that did not provoke them All operational space control schemes present considerable computational originally. requirements, in view of the necessity to perform a number of computations On the other hand, robust control techniques provide a natural rejection in the feedback loop which are somewhat representative of inverse kinematics to external disturbances, although they are sensitive to unmodelled dynamics; functions. With reference to a numerical implementation, the presence of a this rejection is provided by a high-frequency commuted control action that computationally demanding load requires sampling times that may lead to constrains the error trajectories to stay on the sliding subspace. The resulting degrading the performance of the overall control system. inputs to the mechanical structure may be unacceptable. This inconvenience In the face of the above limitations, it is worth presenting operational is in general not observed with the adoption of adaptive control techniques space control schemes, whose utilization becomes necessary when the prob- whose action has a naturally smooth time behaviour. lem of controlling interaction between the manipulator and the environment is of concern. In fact, joint space control schemes suffice only for motion con- trol in the free space. When the manipulator’s end-effector is constrained by 8.6 Operational Space Control the environment, e.g., in the case of end-effector in contact with an elastic environment, it is necessary to control both positions and contact forces and In all the above control schemes, it was always assumed that the desired tra- it is convenient to refer to operational space control schemes. Hence, below jectory is available in terms of the time sequence of the values of joint position, some solutions are presented; these are worked out for motion control, but velocity and acceleration. Accordingly, the error for the control schemes was they constitute the premise for the force/position control strategies that will expressed in the joint space. be illustrated in the next chapter. As often pointed out, motion specifications are usually assigned in the op- erational space, and then an inverse kinematics algorithm has to be utilized to 8.6.1 General Schemes transform operational space references into the corresponding joint space ref- erences. The process of kinematic inversion has an increasing computational As pointed out above, operational space control schemes are based on a direct load when, besides inversion of direct kinematics, inversion of first-order and comparison of the inputs, specifying operational space trajectories, with the second-order differential kinematics is also required to transform the desired measurements of the corresponding manipulator outputs. It follows that the time history of end-effector position, velocity and acceleration into the corre- control system should incorporate some actions that allow the transformation sponding quantities at the joint level. It is for this reason that current indus- from the operational space, in which the error is specified, to the joint space, trial robot control systems compute the joint positions through kinematics in which control generalized forces are developed. inversion, and then perform a numerical differentiation to compute velocities A possible control scheme that can be devised is the so-called Jacobian and accelerations. inverse control (Fig. 8.27). In this scheme, the end-effector pose in the op- A different approach consists of considering control schemes developed erational space xe is compared with the corresponding desired quantity xd , directly in the operational space. If the motion is specified in terms of opera- and then an operational space deviation Δx can be computed. Assumed that tional space variables, the measured joint space variables can be transformed this deviation is sufficiently small for a good control system, Δx can be trans- into the corresponding operational space variables through direct kinematics formed into a corresponding joint space deviation Δq through the inverse of relations. Comparing the desired input with the reconstructed variables allows the manipulator Jacobian. Then, the control input generalized forces can be the design of feedback control loops where trajectory inversion is replaced with computed on the basis of this deviation through a suitable feedback matrix a suitable coordinate transformation embedded in the feedback loop. gain. The result is a presumable reduction of Δq and in turn of Δx. In other words, the Jacobian inverse control leads to an overall system that intuitively 8.6 Operational Space Control 345 346 8 Motion Control
Fig. 8.28. Block scheme of Jacobian transpose control
behaves like a mechanical system with a generalized n-dimensional spring in the joint space, whose constant stiffness is determined by the feedback matrix gain. The role of such system is to take the deviation Δq to zero. If the matrix Fig. 8.29. Block scheme of operational space PD control with gravity compensation gain is diagonal, the generalized spring corresponds to n independent elastic elements, one for each joint. Since ẋd = 0, in view of (3.62) it is A conceptually analogous scheme is the so-called Jacobian transpose con- trol (Fig. 8.28). In this case, the operational space error is treated first through x˙ = −J A (q)q̇ a matrix gain. The output of this block can then be considered as the elas- tic force generated by a generalized spring whose function in the operational and then space is that to reduce or to cancel the position deviation Δx. In other words, 1 V̇ = q̇ T B(q)q̈ + q̇ T Ḃ(q)q̇ − q̇ T J TA (q)K P x. (8.108) the resulting force drives the end-effector along a direction so as to reduce Δx. 2 This operational space force has then to be transformed into the joint space By recalling the expression of the joint space manipulator dynamic model generalized forces, through the transpose of the Jacobian, so as to realize the in (8.7) and the property in (7.49), the expression in (8.108) becomes described behaviour. Both Jacobian inverse and transpose control schemes have been derived V̇ = −q̇ T F q̇ + q̇ T u − g(q) − J TA (q)K P x . (8.109) in an intuitive fashion. Hence, there is no guarantee that such schemes are effective in terms of stability and trajectory tracking accuracy. These problems This equation suggests the structure of the controller; in fact, by choosing can be faced by presenting two mathematical solutions below, which will be the control law shown to be substantially equivalent to the above schemes. u = g(q) + J TA (q)K P x − J TA (q)K D J A (q)q̇ (8.110)
8.6.2 PD Control with Gravity Compensation with K D positive definite, (8.109) becomes
By analogy with joint space stability analysis, given a constant end-effector V̇ = −q̇ T F q̇ − q̇ T J TA (q)K D J A (q)q̇. (8.111) pose xd , it is desired to find the control structure so that the operational space error As can be seen from Fig. 8.29, the resulting block scheme reveals an anal- x = xd − xe (8.106) ogy with the scheme of Fig. 8.28. Control law (8.110) performs a nonlin- ear compensating action of joint space gravitational forces and an operational tends asymptotically to zero. Choose the following positive definite quadratic space linear PD control action. The last term has been introduced to enhance form as a Lyapunov function candidate: system damping; in particular, if measurement of ẋ is deduced from that of 1 T 1 q̇, one can simply choose the derivative term as −K D q̇. V (q̇, x) = q̇ B(q)q̇ + xT K P x > 0 ∀q̇, x = 0, (8.107) The expression in (8.111) shows that, for any system trajectory, the Lya- 2 2 punov function decreases as long as q̇ = 0. The system then reaches an equi- with K P a symmetric positive definite matrix. Differentiating (8.107) with librium posture. By a stability argument similar to that in the joint space respect to time gives (see (8.52)–(8.54)) this posture is determined by 1 T V̇ = q̇ T B(q)q̈ + q̇ T Ḃ(q)q̇ + x˙ K P x. J TA (q)K P x = 0. (8.112) 2 8.6 Operational Space Control 347 348 8 Motion Control
suggests, for a nonredundant manipulator, the choice of the control law —
formally analogous to (3.102) —
y = J −1 ˙
A (q) ẍd + K D x + K P x − J̇ A (q, q̇)q̇ (8.114)
with K P and K D positive definite (diagonal) matrices. In fact, substitut-
ing (8.114) into (8.113) gives
¨ + K D x˙ + K P x = 0
x (8.115)
which describes the operational space error dynamics, with K P and K D
determining the error convergence rate to zero. The resulting inverse dynamics
control scheme is reported in Fig. 8.30, which confirms the anticipated analogy
with the scheme of Fig. 8.27. Again in this case, besides xe and ẋe , q and q̇ are
also to be measured. If measurements of xe and ẋe are indirect, the controller
must compute the direct kinematics functions k(q) and J A (q) on-line.
A critical analysis of the schemes in Figs. 8.29, 8.30 reveals that the design
Fig. 8.30. Block scheme of operational space inverse dynamics control
of an operational space controller always requires computation of manipulator
Jacobian. As a consequence, controlling a manipulator in the operational space
From (8.112) it can be recognized that, under the assumption of full-rank is in general more complex than controlling it in the joint space. In fact, the Jacobian, it is presence of singularities and/or redundancy influences the Jacobian, and the x = xd − xe = 0, induced effects are somewhat difficult to handle with an operational space i.e., the sought result. controller. For instance, if a singularity occurs for the scheme of Fig. 8.29 and If measurements of xe and ẋe are made directly in the operational space, the error enters the null space of the Jacobian, the manipulator gets stuck k(q) and J A (q) in the scheme of Fig. 8.45 are just indicative of direct kine- at a different configuration from the desired one. This problem is even more matics functions; it is, however, necessary to measure q to update both J TA (q) critical for the scheme of Fig. 8.30 which would require the computation of a and g(q) on-line. If measurements of operational space quantities are indirect, DLS inverse of the Jacobian. Yet, for a redundant manipulator, a joint space the controller has to compute the direct kinematics functions, too. control scheme is naturally transparent to this situation, since redundancy has already been solved by inverse kinematics, whereas an operational space control scheme should incorporate a redundancy handling technique inside 8.6.3 Inverse Dynamics Control the feedback loop. Consider now the problem of tracking an operational space trajectory. Recall As a final remark, the above operational space control schemes have been the manipulator dynamic model in the form (8.55) derived with reference to a minimal description of orientation in terms of Euler angles. It is understood that, similar to what is presented in Sect. 3.7.3 B(q)q̈ + n(q, q̇) = u, for inverse kinematics algorithms, it is possible to adopt different definitions where n is given by (8.56). As in (8.57), the choice of the inverse dynamics of orientation error, e.g., based on the angle and axis or the unit quaternion. linearizing control The advantage is the use of the geometric Jacobian in lieu of the analytical u = B(q)y + n(q, q̇) Jacobian. The price to pay, however, is a more complex analysis of the stability and convergence characteristics of the closed-loop system. Even the inverse leads to the system of double integrators dynamics control scheme will not lead to a homogeneous error equation, and q̈ = y. (8.113) a Lyapunov argument should be invoked to ascertain its stability. The new control input y is to be designed so as to yield tracking of a trajectory specified by xd (t). To this end, the second-order differential equation in the form (3.98) ẍe = J A (q)q̈ + J̇ A (q, q̇)q̇ 8.7 Comparison Among Various Control Schemes 349 350 8 Motion Control
8.7 Comparison Among Various Control Schemes F. Joint space PD control with gravity compensation (Fig. 8.20), modified by the addition of a feedforward velocity term K D q̇ d , with the following In order to make a comparison between the various control schemes presented, data: consider the two-link planar arm with the same data of Example 7.2: K P = 3750I 2 K D = 750I 2 . a1 = a2 = 1 m 1 = 2 = 0.5 m m 1 = m 2 = 50 kg I 1 = I 2 = 10 kg·m2 G. Joint space inverse dynamics control (Fig. 8.22) with the following data:
kr1 = kr2 = 100 mm1 = mm2 = 5 kg Im1 = Im2 = 0.01 kg·m2 . K P = 25I 2 K D = 5I 2 .
The arm is assumed to be driven by two equal actuators with the following H. Joint space robust control (Fig. 8.23), under the assumption of constant data: ) = B̄) and compensation of friction and gravity () inertia (B n = F v q̇ + g), Fm1 = Fm2 = 0.01 N·m·s/rad Ra1 = Ra2 = 10 ohm with the following data: kt1 = kt2 = 2 N·m/A kv1 = kv2 = 2 V·s/rad; K P = 25I 2 K D = 5I 2 P = I2 ρ = 70 = 0.004. it can be verified that Fmi kvi kti /Rai for i = 1, 2. The desired tip trajectories have a typical trapezoidal velocity profile, and I. As in case H with = 0.01. thus it is anticipated that sharp torque variations will be induced. The tip path J. Joint space adaptive control (Fig. 8.26) with a parameterization of the is a motion of 1.6 m along the horizontal axis, as in the path of Example 7.2. In arm dynamic model (7.82) as in (7.83), (7.84). The initial estimate of the the first case (fast trajectory), the acceleration time is 0.6 s and the maximum ) is computed on the basis of the nominal parameters. The arm vector π velocity is 1 m/s. In the second case (slow trajectory), the acceleration time is is supposed to carry a load which causes the following variations on the 0.6 s and the maximum velocity is 0.25 m/s. The motion of the controlled arm second link parameters: was simulated on a computer, by adopting a discrete-time implementation of the controller with a sampling time of 1 ms. Δm2 = 10 kg Δm2 C2 = 11 kg·m ΔI)2 = 12.12 kg·m2 . The following control schemes in the joint space and in the operational space have been utilized; an (analytic) inverse kinematics solution has been im- This information is obviously utilized only to update the simulated arm plemented to generate the reference inputs to the joint space control schemes: model. Further, the following data are set:
A. Independent joint control with position and velocity feedback (Fig. 5.11) Λ = 5I 2 K D = 750I 2 K π = 0.01I 8 . with the following data for each joint servo: K. Operational space PD control with gravity compensation (Fig. 8.29), mod- KP = 5 KV = 10 kT P = kT V = 1, ified by the addition of a feedforward velocity term K D ẋd , with the fol- lowing data: corresponding to ωn = 5 rad/s and ζ = 0.5. K P = 16250I 2 K D = 3250I 2 . B. Independent joint control with position, velocity and acceleration feedback (Fig. 8.9) with the following data for each joint servo: L. Operational space inverse dynamics control (Fig. 8.30) with the following data: KP = 5 KV = 10 KA = 2 kT P = kT V = kT A = 1, K P = 25I 2 K D = 5I 2 . corresponding to ωn = 5 rad/s, ζ = 0.5, XR = 100. To reconstruct accel- It is worth remarking that the adopted model of the dynamic system of arm eration, a first-order filter has been utilized (Fig. 8.11) characterized by with drives is that described by (8.7). In the decentralized control schemes A– ω3f = 100 rad/s. E, the joints have been voltage-controlled as in the block scheme of Fig. 8.3, C. As in scheme A with the addition of a decentralized feedforward action with unit amplifier gains (Gv = I). On the other hand, in the centralized (Fig. 8.13). control schemes F–L, the joints have been current-controlled as in the block D. As in scheme B with the addition of a decentralized feedforward action scheme of Fig. 8.4, with unit amplifier gains (Gi = I). (Fig. 8.14). Regarding the parameters of the various controllers, these have been cho- E. Joint space computed torque control (Fig. 8.19) with feedforward com- sen in such a way as to allow a significant comparison of the performance of pensation of the diagonal terms of the inertia matrix and of gravitational each scheme in response to congruent control actions. In particular, it can be terms, and decentralized feedback controllers as in scheme A. observed that: 8.7 Comparison Among Various Control Schemes 351 352 8 Motion Control
joint 1 pos joint 2 pos joint torques pos error norm
0 3 1500 0.015
Ŧ0.5 2.5
1000
Ŧ1 2 0.01
[Nm]
[rad]
[rad]
[m]
Ŧ1.5 1.5 500
Ŧ2 1 0.005
0
Ŧ2.5 0.5
Ŧ3 0 Ŧ500 0
0 1 2 3 4 0 1 2 3 4 0 1 2 3 4 0 1 2 3 4
[s] [s] [s] [s]
1500
joint torques pos errors
Fig. 8.33. Time history of the joint torques and of the norm of tip position error
for the fast trajectory with control scheme E
1000 0.2
joint 1 pos joint 2 pos
[Nm]
0 3
[m]
500 0.1
Ŧ0.5 2.5
0 0 Ŧ1 2
[rad]
[rad]
Ŧ1.5 1.5
Ŧ500 Ŧ0.1
0 1 2 3 4 0 1 2 3 4
Ŧ2 1
[s] [s]
Ŧ2.5 0.5
Fig. 8.31. Time history of the joint positions and torques and of the tip position Ŧ3 0 errors for the fast trajectory with control scheme A 0 1 2 3 4 0 1 2 3 4 [s] [s]
joint torques joint torques joint torques pos error norm
1500 1500 1500 0.1
0.08
1000 1000 1000
0.06
[Nm]
[Nm]
[Nm]
[m]
500 500 500
0.04
0 0 0
0.02
Ŧ500 Ŧ500 Ŧ500 0
0 1 2 3 4 0 1 2 3 4 0 1 2 3 4 0 1 2 3 4
[s] [s] [s] [s]
0.015
pos error norm
0.015
pos error norm
Fig. 8.34. Time history of the joint positions and torques and of the norm of tip
position error for the fast trajectory with control scheme F
0.01 0.01 joint torques Ŧ4 pos error norm
x 10
1500 4
[m]
[m]
0.005 0.005 1000 3
[Nm]
[m]
500 2
0 0
0 1 2 3 4 0 1 2 3 4
[s] [s] 0 1
Fig. 8.32. Time history of the joint torques and of the norm of tip position error Ŧ500 0 for the fast trajectory; left: with control scheme C, right: with control scheme D 0 1 2 3 4 0 1 2 3 4 [s] [s]
Fig. 8.35. Time history of the joint torques and of the norm of tip position error
for the fast trajectory with control scheme G
8.7 Comparison Among Various Control Schemes 353 354 8 Motion Control
joint torques joint torques pos error norm parameter error norm
1500 1500 0.01 17
0.008 16
1000 1000
15
0.006
[Nm]
[Nm]
[m]
[m]
500 500 14
0.004
13
0 0
0.002 12
Ŧ500 Ŧ500 0 11
0 1 2 3 4 0 1 2 3 4 0 1 2 3 4 0 1 2 3 4
[s] [s] [s] [s]
Fig. 8.37. Time history of the norm of tip position error and of the norm of pa-
Ŧ4 pos error norm Ŧ4 pos error norm
x 10 x 10
4 4
rameter error vector for the fast trajectory with control scheme J
3 3
joint torques pos error norm
1500 0.1
[m]
[m]
2 2
0.08
1000
1 1
0.06
[Nm]
[m]
500
0 0 0.04
0 1 2 3 4 0 1 2 3 4
[s] [s] 0
0.02
Fig. 8.36. Time history of the joint torques and of the norm of tip position error Ŧ500 0 for the fast trajectory; left: with control scheme H, right: with control scheme I 0 1 2 3 4 0 1 2 3 4 [s] [s]
Fig. 8.38. Time history of the joint torques and of the norm of tip position error
• The dynamic behaviour of the joints is the same for schemes A–E. for the fast trajectory with control scheme K • The gains of the PD actions in schemes G, H, I and L have been chosen so as to obtain the same natural frequency and damping ratios as those of joint torques Ŧ4 pos error norm x 10 schemes A–E. 1500 4
The results obtained with the various control schemes are illustrated in 1000 3
Figs. 8.31–8.39 for the fast trajectory and in Figs. 8.40–8.48 for the slow
[Nm]
[m]
trajectory, respectively. In the case of two quantities represented in the same 500 2 plot notice that: 0 1 • For the joint trajectories, the dashed line indicates the reference trajectory obtained from the tip trajectory via inverse kinematics, while the solid line Ŧ500 0 0 1 2 3 4 0 1 2 3 4 indicates the actual trajectory followed by the arm. [s] [s] • For the joint torques, the solid line refers to Joint 1 while the dashed line Fig. 8.39. Time history of the joint torques and of the norm of tip position error refers to Joint 2. for the fast trajectory with control scheme L • For the tip position error, the solid line indicates the error component along the horizontal axis while the dashed line indicates the error component along the vertical axis. Deviation of the actual joint trajectories from the desired ones shows that tracking performance of scheme A is quite poor (Fig. 8.31). It should be Finally, the representation scales have been made as uniform as possible noticed, however, that the largest contribution to the error is caused by a in order to allow a more direct comparison of the results. time lag of the actual trajectory behind the desired one, while the distance Regarding performance of the various control schemes for the fast trajec- tory, the obtained results lead to the following considerations. 8.7 Comparison Among Various Control Schemes 355 356 8 Motion Control
joint 1 pos joint 2 pos joint torques Ŧ3
x 10 pos error norm
0 3 1500 4
Ŧ0.5 2.5
1000 3
Ŧ1 2
[Nm]
[rad]
[rad]
[m]
Ŧ1.5 1.5 500 2
Ŧ2 1
0 1
Ŧ2.5 0.5
Ŧ3 0 Ŧ500 0
0 2 4 6 8 10 0 2 4 6 8 10 0 2 4 6 8 10 0 2 4 6 8 10
[s] [s] [s] [s]
1500
joint torques pos errors
Fig. 8.42. Time history of the joint torques and of the norm of tip position error
for the slow trajectory with control scheme E
1000
0.05 joint 1 pos joint 2 pos
[Nm]
0 3
[m]
500
Ŧ0.5 2.5
0
0 Ŧ1 2
[rad]
[rad]
Ŧ1.5 1.5
Ŧ500 Ŧ0.05
0 2 4 6 8 10 0 2 4 6 8 10
Ŧ2 1
[s] [s]
Ŧ2.5 0.5
Fig. 8.40. Time history of the joint positions and torques and of the tip position Ŧ3 0 errors for the slow trajectory with control scheme A 0 2 4 6 8 10 0 2 4 6 8 10 [s] [s]
joint torques joint torques joint torques pos error norm
1500 1500 1500 0.02
1000 1000 1000 0.015
[Nm]
[Nm]
[Nm]
[m]
500 500 500 0.01
0 0 0 0.005
Ŧ500 Ŧ500 Ŧ500 0
0 2 4 6 8 10 0 2 4 6 8 10 0 2 4 6 8 10 0 2 4 6 8 10
[s] [s] [s] [s]
Fig. 8.43. Time history of the joint positions and torques and of the norm of tip
Ŧ3 pos error norm Ŧ3 pos error norm
x 10 x 10
4 4
position error for the slow trajectory with control scheme F
3 3
joint torques Ŧ4 pos error norm
x 10
1500 1
[m]
[m]
2 2
0.8
1000
1 1
[Nm] 0.6
[m]
500
0 0 0.4
0 2 4 6 8 10 0 2 4 6 8 10
[s] [s] 0
0.2
Fig. 8.41. Time history of the joint torques and of the norm of tip position error Ŧ500 0 for the slow trajectory; left: with control scheme C, right: with control scheme D 0 2 4 6 8 10 0 2 4 6 8 10 [s] [s]
Fig. 8.44. Time history of the joint torques and of the norm of tip position error
for the slow trajectory with control scheme G
8.7 Comparison Among Various Control Schemes 357 358 8 Motion Control
Ŧ3 pos error norm parameter error norm
x 10
5 17
joint torques joint torques
1500 1500
4 16
1000 1000 3 15
[m]
[m]
[Nm]
[Nm]
2 14
500 500
1 13
0 0
0 12
0 2 4 6 8 10 0 2 4 6 8 10
Ŧ500 Ŧ500 [s] [s]
0 2 4 6 8 10 0 2 4 6 8 10
[s] [s]
Fig. 8.46. Time history of the norm of tip position error and of the norm of pa-
Ŧ4
x 10 pos error norm Ŧ4
x 10 pos error norm rameter error vector for the slow trajectory with control scheme J
1 1
0.8 0.8 joint torques pos error norm
1500 0.02
0.6 0.6
[m]
[m] 1000 0.015
0.4 0.4
[Nm]
[m]
0.2 0.2 500 0.01
0 0
0 2 4 6 8 10 0 2 4 6 8 10 0 0.005
[s] [s]
Ŧ500 0
Fig. 8.45. Time history of the joint torques and of the norm of tip position error 0 2 4 6 8 10 0 2 4 6 8 10 [s] [s] for the slow trajectory; left: with control scheme H, right: with control scheme I Fig. 8.47. Time history of the joint torques and of the norm of tip position error for the slow trajectory with control scheme K of the tip from the geometric path is quite contained. Similar results were obtained with scheme B, and then they have not been reported. joint torques Ŧ4 pos error norm x 10 With schemes C and D, an appreciable tracking accuracy improvement is 1500 1 observed (Fig. 8.32), with better performance for the second scheme, thanks 0.8 1000 to the outer acceleration feedback loop that allows a disturbance rejection 0.6 factor twice as much as for the first scheme. Notice that the feedforward
[Nm]
[m]
500
action yields a set of torques which are closer to the nominal ones required to 0.4 execute the desired trajectory; the torque time history has a discontinuity in 0 0.2 correspondence of the acceleration and deceleration fronts. The tracking error is further decreased with scheme E (Fig. 8.33), by virtue Ŧ500 0 2 4 6 8 10 0 0 2 4 6 8 10 of the additional nonlinear feedforward compensation. [s] [s] Scheme F guarantees stable convergence to the final arm posture with a Fig. 8.48. Time history of the joint torques and of the norm of tip position error tracking performance which is better than that of schemes A and B, thanks to for the slow trajectory with control scheme L the presence of a velocity feedforward action, but worse than that of schemes C–E, in view of lack of an acceleration feedforward action (Fig. 8.34). As would be logical to expect, the best results are observed with scheme G frequency components in Joint 1 torque (see the thick portions of the torque for which the tracking error is practically zero, and it is mainly due to numer- plot) at the advantage of a very limited tracking error. As the threshold value is ical discretization of the controller (Fig. 8.35). increased (scheme I), the torque assumes a smoother behaviour at the expense It is then worth comparing the performance of schemes H and I (Fig. 8.36). of a doubled norm of tracking error, though. In fact, the choice of a small threshold value for (scheme H) induces high- Bibliography 359 360 8 Motion Control
For scheme J, a lower tracking error than that of scheme F is observed, Problems
thanks to the effectiveness of the adaptive action on the parameters of the dynamic model. Nonetheless, the parameters do not converge to their nominal 8.1. With reference to the block scheme with position feedback in Fig. 5.10, values, as confirmed by the time history of the norm of the parameter error find the transfer functions of the forward path, the return path, and the vector that reaches a non-null steady-state value (Fig. 8.37). closed-loop system. Finally, the performance of schemes K and L is substantially comparable to that of corresponding schemes F and G (Figs. 8.38 and 8.39). 8.2. With reference to the block scheme with position and velocity feedback Performance of the various control schemes for the slow trajectory is glob- in Fig. 5.11, find the transfer functions of the forward path, the return path, ally better than that for the fast trajectory. Such improvement is particularly and the closed-loop system. evident for the decentralized control schemes (Figs. 8.40–8.42), whereas the 8.3. With reference to the block scheme with position, velocity and accelera- tracking error reduction for the centralized control schemes is less dramatic tion feedback in Fig. 8.9, find the transfer functions of the forward path, the (Figs. 8.43–8.48), in view of the small order of magnitude of the errors already return path, and the closed-loop system. obtained for the fast trajectory. In any case, as regards performance of each single scheme, it is possible to make a number of remarks analogous to those 8.4. For a single joint drive system with the data: I = 6 kg·m2 , Ra = 0.3 ohm, previously made. kt = 0.5 N·m/A, kv = 0.5 V·s/rad, Fm = 0.001 N·m·s/rad, find the parameters of the controller with position feedback (unit transducer constant) that yield a closed-loop response with damping ratio ζ ≥ 0.4. Discuss disturbance rejection Bibliography properties.
The independent joint control is analyzed in classical texts [180, 120, 200], and 8.5. For the drive system of Problem 8.4, find the parameters of the controller scientific articles [19, 127, 141, 101, 39]. Stability of PD control with gravity with position and velocity feedback (unit transducer constants) that yield compensation is proved in [7], on the basis of the notable properties of the a closed-loop response with damping ratio ζ ≥ 0.4 and natural frequency dynamic model in [226]. ωn = 20 rad/s. Discuss disturbance rejection properties. Computed torque control and inverse dynamics control were developed at 8.6. For the drive system of Problem 8.4, find the parameters of the controller the beginning of the 1970s. One of the first experimental works is [149]. Other with position, velocity and acceleration feedback (unit transducer constants) articles on the topic are [83, 4, 117, 121, 126, 227, 29]. that yield a closed-loop response with damping ratio ζ ≥ 0.4, natural fre- The main approaches of robust control are inspired to the work [50]. quency ωn = 20 rad/s and disturbance rejection factor XR = 400. Also, design Among them it is worth citing [212, 84, 130, 219, 205, 216]. Robust con- a first-order filter that allows acceleration measurement reconstruction. trollers based on the high gain concept are presented in [192, 222]. A survey on robust control is [1]. 8.7. Verify that the control schemes in Figs. 8.12, 8.13, 8.14 correspond to One of the first approaches to adaptive control, based on the assumption realizing (8.42), (8.43), (8.44), respectively. of decoupled joint dynamics, is presented in [67]. The first works on adaptive control accounting for the manipultor nonlinear dynamics are [15, 167, 100], 8.8. Verify that the standard regulation schemes in Figs. 8.15, 8.16, 8.17 are yet they exploit the notable properties of the dynamic model only to some equivalent to the schemes in Figs. 8.12, 8.13, 8.14, respectively. extent. The adaptive version of inverse dynamics control is analyzed in [52, 8.9. Prove inequality (8.76). 157]. The approach based on the energy properties of the dynamic model has been proposed in [214] and further analyzed in [218]. An interesting tutorial 8.10. For the two-link planar arm with the same data as in Sect. 8.7, design a on adaptive control is [175]. joint control of PD type with gravity compensation. By means of a computer Operational space control has been proposed in [114], on the basis of the simulation, verify stability for the following postures q = [ π/4 −π/2 ]T and resolved acceleration control concept [143]. Inverse dynamics control schemes q = [ −π −3π/4 ]T , respectively. Implement the control in discrete-time with in the operational space are given in [30]. For the extension to redundant a sampling time of 1 ms. manipulators see [102]. 8.11. For the two-link planar arm with the same data as in Sect. 8.7, under the assumption of a concentrated tip payload of mass mL = 10 kg, design an independent joint control with feedforward computed torque. Perform a Problems 361
computer simulation of the motion of the controlled arm along the joint space rectilinear path from q i = [ 0 π/4 ]T to q f = [ π/2 π/2 ]T with a trapezoidal velocity profile and a trajectory duration tf = 1 s. Implement the control in A discrete-time with a sampling time of 1 ms.
8.12. For the two-link planar arm of Problem 8.11, design an inverse dynamics Linear Algebra joint control. Perform a computer simulation of the motion of the controlled arm along the trajectory specified in Problem 8.11. Implement the control in discrete-time with a sampling time of 1 ms.
8.13. For the two-link planar arm of Problem 8.11, design a robust joint con- trol. Perform a computer simulation of the motion of the controlled arm along the trajectory specified in Problem 8.11. Implement the control in discrete- time with a sampling time of 1 ms.
8.14. For the two-link planar arm of Problem 8.11, design an adaptive joint Since modelling and control of robot manipulators requires an extensive use control, on the basis of a suitable parameterization of the arm dynamic model. of matrices and vectors as well as of matrix and vector operations, the goal Perform a computer simulation of the motion of the controlled arm along the of this appendix is to provide a brush-up of linear algebra. trajectory specified in Problem 8.11. Implement the control in discrete-time with a sampling time of 1 ms. A.1 Definitions 8.15. For the two-link planar of Problem 8.11, design a PD control in the operational space with gravity compensation. By means of a computer sim- A matrix of dimensions (m × n), with m and n positive integers, is an array ulation, verify stability for the following postures p = [ 0.5 0.5 ]T and of elements aij arranged into m rows and n columns: p = [ 0.6 −0.2 ]T , respectively. Implement the control in discrete-time with a sampling time of 1 ms. ⎡ ⎤ a11 a12 … a1n ⎢ a21 a22 … a2n ⎥ 8.16. For the two-link planar arm of Problem 8.11, design an inverse dynamics A = [aij ] i = 1, … , m = ⎢ ⎣ … .. .. ⎥ . (A.1) . ⎦ .. control in the operational space. Perform a computer simulation of the motion . . j = 1, … , n of the controlled arm along the operational space rectlinear path from p(0) = am1 am2 … amn [ 0.7 0.2 ]T to p(1) = [ 0.1 −0.6 ]T with a trapezoidal velocity profile and a If m = n, the matrix is said to be square; if m < n, the matrix has more trajectory duration tf = 1 s. Implement the control in discrete-time with a columns than rows; if m > n the matrix has more rows than columns. Further, sampling time of 1 ms. if n = 1, the notation (A.1) is used to represent a (column) vector a of dimensions (m × 1);1 the elements ai are said to be vector components. A square matrix A of dimensions (n × n) is said to be upper triangular if aij = 0 for i > j: ⎡ ⎤ a11 a12 … a1n ⎢ 0 a22 … a2n ⎥ A=⎢ ⎣ … .. .. ⎥ ; . ⎦ … 0 0 … ann the matrix is said to be lower triangular if aij = 0 for i < j.
1
According to standard mathematical notation, small boldface is used to denote
vectors while capital boldface is used to denote matrices. Scalars are denoted by
roman characters.
564 A Linear Algebra A Linear Algebra 565
An (n × n) square matrix A is said to be diagonal if aij = 0 for i = j, i.e., A partitioned matrix may be block-triangular or block-diagonal. Special par-
⎡ ⎤ titions of a matrix are that by columns
a11 0 ... 0
⎢ 0 a22 ... 0 ⎥ A = [ a1 a2 ... an ]
A=⎢
⎣ ... .. .. ⎥ = diag{a11 , a22 , . . . , ann }.
. ⎦
..
. . and that by rows
0 0 ... ann ⎡ aT ⎤
1
⎢ aT ⎥
If an (n × n) diagonal matrix has all unit elements on the diagonal (aii = 1), ⎢ 2 ⎥ the matrix is said to be identity and is denoted by I n .2 A matrix is said to be A=⎢ ⎥ ⎢ .. ⎥ . ⎣ . ⎦ null if all its elements are null and is denoted by O. The null column vector is denoted by 0. aTm The transpose AT of a matrix A of dimensions (m × n) is the matrix of Given a square matrix A of dimensions (n × n), the algebraic complement dimensions (n×m) which is obtained from the original matrix by interchanging A(ij) of element aij is the matrix of dimensions ((n − 1) × (n − 1)) which is its rows and columns: obtained by eliminating row i and column j of matrix A. ⎡ ⎤ a11 a21 … am1 ⎢ a12 a22 … am2 ⎥ AT = ⎢⎣ … .. .. ⎥ . (A.2) A.2 Matrix Operations . ⎦ … a1n a2n … amn The trace of an (n × n) square matrix A is the sum of the elements on the diagonal: The transpose of a column vector a is the row vector aT . n An (n × n) square matrix A is said to be symmetric if AT = A, and thus Tr(A) = aii . (A.3) aij = aji : i=1 ⎡ ⎤ a11 a12 … a1n Two matrices A and B of the same dimensions (m × n) are equal if aij = ⎢ a12 a22 … a2n ⎥ bij . If A and B are two matrices of the same dimensions, their sum is the A=⎢ ⎣ … .. .. ⎥ . . ⎦ .. matrix . . a1n a2n … ann C =A+B (A.4)
An (n × n) square matrix A is said to be skew-symmetric if AT = −A, and whose elements are given by cij = aij + bij . The following properties hold: thus aij = −aji for i = j and aii = 0, leading to A+O =A ⎡ ⎤ A+B =B+A 0 a12 … a1n ⎢ −a12 0 … a2n ⎥ (A + B) + C = A + (B + C). A=⎢ ⎣ … .. .. ⎥ . . ⎦ … Notice that two matrices of the same dimensions and partitioned in the same −a1n −a2n … 0 way can be summed formally by operating on the blocks in the same position A partitioned matrix is a matrix whose elements are matrices (blocks) of and treating them like elements. proper dimensions: The product of a scalar α by an (m × n) matrix A is the matrix αA whose elements are given by αaij . If A is an (n × n) diagonal matrix with all equal ⎡ ⎤ elements on the diagonal (aii = a), it follows that A = aI n . A11 A12 … A1n ⎢ A21 A22 … A2n ⎥ If A is a square matrix, one may write A=⎢ ⎣ … .. .. ⎥ . . ⎦ … A = As + Aa (A.5) Am1 Am2 … Amn where 1 2 Subscript n is usually omitted if the dimensions are clear from the context. As = (A + AT ) (A.6) 2 566 A Linear Algebra A Linear Algebra 567
is a symmetric matrix representing the symmetric part of A, and of matrix A. The minors obtained by taking the first k rows and columns of A are said to be principal minors. 1 Aa = (A − AT ) (A.7) If A and B are square matrices, then 2 is a skew-symmetric matrix representing the skew-symmetric part of A. det(AB) = det(A)det(B). (A.10) The row-by-column product of a matrix A of dimensions (m × p) by a If A is an (n × n) triangular matrix (in particular diagonal), then matrix B of dimensions (p × n) is the matrix of dimensions (m × n) / n C = AB (A.8) det(A) = aii . .p i=1 whose elements are given by cij = k=1 aik bkj . The following properties hold: More generally, if A is block-triangular with m blocks Aii on the diagonal, A = AI p = I m A then /m A(BC) = (AB)C det(A) = det(Aii ). A(B + C) = AB + AC i=1
(A + B)C = AC + BC A square matrix A is said to be singular when det(A) = 0.
(AB)T = B T AT . The rank (A) of a matrix A of dimensions (m × n) is the maximum
integer r so that at least a non-null minor of order r exists. The following
Notice that, in general, AB = BA, and AB = O does not imply that A = O properties hold: or B = O; further, notice that AC = BC does not imply that A = B. If an (m × p) matrix A and a (p × n) matrix B are partitioned in such a (A) ≤ min{m, n} way that the number of blocks for each row of A is equal to the number of (A) = (AT ) blocks for each column of B, and the blocks Aik and B kj have dimensions (AT A) = (A) compatible with product, the matrix product AB can be formally obtained by operating by rows and columns on the blocks of proper position and treating (AB) ≤ min{ (A), (B)}. them like elements. A matrix so that (A) = min{m, n} is said to be full-rank . For an (n × n) square matrix A, the determinant of A is the scalar given The adjoint of a square matrix A is the matrix by the following expression, which holds ∀i = 1, … , n: Adj A = [(−1)i+j det(A(ij) )]T . (A.11) n i = 1, … , n i+j det(A) = aij (−1) det A(ij) . (A.9) j = 1, … , n j=1 An (n × n) square matrix A is said to be invertible if a matrix A−1 exists, The determinant can be computed according to any row i as in (A.9); the termed inverse of A, so that same result is obtained by computing it according to any column j. If n = 1, then det(a11 ) = a11 . The following property holds: A−1 A = AA−1 = I n .
det(A) = det(AT ). Since (I n ) = n, an (n × n) square matrix A is invertible if and only if
(A) = n, i.e., det(A) = 0 (nonsingular matrix). The inverse of A can be
Moreover, interchanging two generic columns p and q of a matrix A yields computed as 1 det [ a1 … ap … aq … an ] = −det [ a1 … aq … ap … an ] . A−1 = Adj A. (A.12) det(A) As a consequence, if a matrix has two equal columns (rows), then its deter- The following properties hold: minant is null. Also, it is det(αA) = αn det(A). Given an (m × n) matrix A, the determinant of the square block obtained (A−1 )−1 = A by selecting an equal number k of rows and columns is said to be k-order minor (AT )−1 = (A−1 )T . 568 A Linear Algebra A Linear Algebra 569
If the inverse of a square matrix is equal to its transpose If an (n × n) square matrix A(t) is so that (A(t)) = n ∀t and its elements aij (t) are differentiable functions, then the derivative of the inverse of A(t) AT = A−1 (A.13) is given by d −1 then the matrix is said to be orthogonal ; in this case it is A (t) = −A−1 (t)Ȧ(t)A−1 (t). (A.19) dt AAT = AT A = I. (A.14) Given a scalar function f (x), endowed with partial derivatives with respect to the elements xi of the (n × 1) vector x, the gradient of function f with A square matrix A is said idempotent if respect to vector x is the (n × 1) column vector AA = A. (A.15) T T ∂f (x) ∂f (x) ∂f (x) ∂f (x) ∇x f (x) = = … . (A.20) If A and B are invertible square matrices of the same dimensions, then ∂x ∂x1 ∂x2 ∂xn
(AB)−1 = B −1 A−1 . (A.16) Further, if x(t) is a differentiable function with respect to t, then
Given n square matrices Aii all invertible, the following expression holds: d ∂f f˙(x) = f (x(t)) = ẋ = ∇Tx f (x)ẋ. (A.21) −1 dt ∂x diag{A11 , … , Ann } = diag{A−1 −1 11 , … , Ann }. Given a vector function g(x) of dimensions (m × 1), whose elements gi are where diag{A11 , … , Ann } denotes the block-diagonal matrix. differentiable with respect to the vector x of dimensions (n × 1), the Jacobian If A and C are invertible square matrices of proper dimensions, the fol- matrix (or simply Jacobian) of the function is defined as the (m × n) matrix lowing expression holds: ⎡ ∂g (x) ⎤ 1 (A + BCD)−1 = A−1 − A−1 B(DA−1 B + C −1 )−1 DA−1 , ⎢ ∂x ⎥ ⎢ ⎥ ⎢ ∂g2 (x) ⎥ where the matrix DA−1 B + C −1 must be invertible. ∂g(x) ⎢ ⎥ J g (x) = =⎢ ⎢ ∂x ⎥ ⎥. (A.22) If a block-partitioned matrix is invertible, then its inverse is given by the ∂x ⎢ .. ⎥ general expression ⎢ . ⎥ ⎣ ⎦ −1 −1 ∂gm (x) A D A + EΔ−1 F −EΔ−1 ∂x = (A.17) C B −Δ−1 F Δ−1 If x(t) is a differentiable function with respect to t, then where Δ = B − CA−1 D, E = A−1 D and F = CA−1 , under the assumption d ∂g that the inverses of matrices A and Δ exist. In the case of a block-triangular ġ(x) = g(x(t)) = ẋ = J g (x)ẋ. (A.23) matrix, invertibility of the matrix requires invertibility of the blocks on the dt ∂x diagonal. The following expressions hold: −1 A.3 Vector Operations A O A−1 O = C B −B −1 CA−1 B −1 −1 −1 Given n vectors xi of dimensions (m × 1), they are said to be linearly inde- A D A −A−1 DB −1 pendent if the expression = . O B O B −1 k1 x1 + k2 x2 + … + kn xn = 0 The derivative of an (m × n) matrix A(t), whose elements aij (t) are dif- ferentiable functions, is the matrix holds true only when all the constants ki vanish. A necessary and sufficient condition for the vectors x1 , x2 … , xn to be linearly independent is that the d d matrix Ȧ(t) = A(t) = aij (t) . (A.18) dt dt i = 1, … , m A = [ x1 x2 … xn ] j = 1, … , n 570 A Linear Algebra A Linear Algebra 571
has rank n; this implies that a necessary condition for linear independence It is possible to show that both the triangle inequality is that n ≤ m. If instead (A) = r < n, then only r vectors are linearly x+y ≤ x + y (A.28) independent and the remaining n − r vectors can be expressed as a linear combination of the previous ones. and the Schwarz inequality A system of vectors X is a vector space on the field of real numbers IR if |xT y| ≤ x y . (A.29) the operations of sum of two vectors of X and product of a scalar by a vector of X have values in X and the following properties hold: T hold. A unit vector x̂ is a vector whose norm is unity, i.e., x̂ x̂ = 1. Given a vector x, its unit vector is obtained by dividing each component by its norm: x+y =y+x ∀x, y ∈ X 1 (x + y) + z = x + (y + z) ∀x, y, z ∈ X )= x x. (A.30) x ∃0 ∈ X : x + 0 = x ∀x ∈ X A typical example of vector space is the Euclidean space whose dimension is ∀x ∈ X , ∃(−x) ∈ X : x + (−x) = 0 3; in this case a basis is constituted by the unit vectors of a coordinate frame. 1x = x ∀x ∈ X The vector product of two vectors x and y in the Euclidean space is the α(βx) = (αβ)x ∀α, β ∈ IR ∀x ∈ X vector ⎡ ⎤ x2 y3 − x3 y2 (α + β)x = αx + βx ∀α, β ∈ IR ∀x ∈ X x × y = ⎣ x3 y1 − x1 y3 ⎦ . (A.31) α(x + y) = αx + αy ∀α ∈ IR ∀x, y ∈ X . x1 y2 − x2 y1 The dimension of the space dim(X ) is the maximum number of linearly inde- The following properties hold: pendent vectors x in the space. A set {x1 , x2 , … , xn } of linearly independent x×x=0 vectors is a basis of vector space X , and each vector y in the space can be x × y = −y × x uniquely expressed as a linear combination of vectors from the basis x × (y + z) = x × y + x × z. y = c1 x1 + c2 x2 + … + cn xn , (A.24) The vector product of two vectors x and y can be expressed also as the where the constants c1 , c2 , … , cn are said to be the components of the vector product of a matrix operator S(x) by the vector y. In fact, by introducing y in the basis {x1 , x2 , … , xn }. the skew-symmetric matrix ⎡ ⎤ A subset Y of a vector space X is a subspace Y ⊆ X if it is a vector space 0 −x3 x2 with the operations of vector sum and product of a scalar by a vector, i.e., S(x) = ⎣ x3 0 −x1 ⎦ (A.32) −x2 x1 0 αx + βy ∈ Y ∀α, β ∈ IR ∀x, y ∈ Y. obtained with the components of vector x, the vector product x × y is given According to a geometric interpretation, a subspace is a hyperplane passing by by the origin (null element) of X . x × y = S(x)y = −S(y)x (A.33) The scalar product < x, y > of two vectors x and y of dimensions (m × as can be easily verified. Moreover, the following properties hold:
-
is the scalar that is obtained by summing the products of the respective components in a given basis S(x)x = S T (x)x = 0 S(αx + βy) = αS(x) + βS(y). < x, y >= x1 y1 + x2 y2 + … + xm ym = xT y = y T x. (A.25) Given three vectors x, y, z in the Euclidean space, the following expres- Two vectors are said to be orthogonal when their scalar product is null: sions hold for the scalar triple products:
xT y = 0. (A.26) xT (y × z) = y T (z × x) = z T (x × y). (A.34) If any two vectors of three are equal, then the scalar triple product is null;
The norm of a vector can be defined as e.g., √ x = xT x. (A.27) xT (x × y) = 0. 572 A Linear Algebra A Linear Algebra 573
A.4 Linear Transformation which can also be computed as
Consider a vector space X of dimension n and a vector space Y of dimension max Ax . x=1 m with m ≤ n. The linear transformation (or linear map) between the vectors x ∈ X and y ∈ Y can be defined as A direct consequence of (A.41) is the property y = Ax (A.35) AB ≤ A B . (A.43) in terms of the matrix A of dimensions (m × n). The range space (or simply A different norm of a matrix is the Frobenius norm defined as range) of the transformation is the subspace 1/2 R(A) = {y : y = Ax, x ∈ X } ⊆ Y, (A.36) A F = Tr(AT A) (A.44)
which is the subspace generated by the linearly independent columns of matrix A taken as a basis of Y. It is easy to recognize that A.5 Eigenvalues and Eigenvectors (A) = dim(R(A)). (A.37) Consider the linear transformation on a vector u established by an (n × n) On the other hand, the null space (or simply null) of the transformation is square matrix A. If the vector resulting from the transformation has the same the subspace direction of u (with u = 0), then N (A) = {x : Ax = 0, x ∈ X } ⊆ X . (A.38) Au = λu. (A.45) Given a matrix A of dimensions (m × n), the notable result holds: The equation in (A.45) can be rewritten in matrix form as (A) + dim(N (A)) = n. (A.39) (λI − A)u = 0. (A.46) Therefore, if (A) = r ≤ min{m, n}, then dim(R(A)) = r and dim(N (A)) = n − r. It follows that if m < n, then N (A) = ∅ independently of the rank of For the homogeneous system of equations in (A.46) to have a solution different A; if m = n, then N (A) = ∅ only in the case of (A) = r < m. from the trivial one u = 0, it must be If x ∈ N (A) and y ∈ R(AT ), then y T x = 0, i.e., the vectors in the null space of A are orthogonal to each vector in the range space of the transpose det(λI − A) = 0 (A.47) of A. It can be shown that the set of vectors orthogonal to each vector of the range space of AT coincides with the null space of A, whereas the set of which is termed a characteristic equation. Its solutions λ1 , … , λn are the vectors orthogonal to each vector in the null space of AT coincides with the eigenvalues of matrix A; they coincide with the eigenvalues of matrix AT . On range space of A. In symbols: the assumption of distinct eigenvalues, the n vectors ui satisfying the equation
N (A) ≡ R⊥ (AT ) R(A) ≡ N ⊥ (AT ) (A.40) (λi I − A)ui = 0 i = 1, . . . , n (A.48)
where ⊥ denotes the orthogonal complement of a subspace. are said to be the eigenvectors associated with the eigenvalues λi . If the matrix A in (A.35) is square and idempotent, the matrix represents The matrix U formed by the column vectors ui is invertible and constitutes the projection of space X into a subspace. a basis in the space of dimension n. Further, the similarity transformation A linear transformation allows the definition of the norm of a matrix A established by U induced by the norm defined for a vector x as follows. In view of the property Λ = U −1 AU (A.49) 0n Ax ≤ A x , (A.41) is so that Λ = diag{λ1 , … , λn }. It follows that det(A) = i=1 λi . If the matrix A is symmetric, its eigenvalues are real and Λ can be written the norm of A can be defined as as Ax Λ = U T AU ; (A.50) A = sup (A.42) x=0 x hence, the eigenvector matrix U is orthogonal. 574 A Linear Algebra A Linear Algebra 575
A.6 Bilinear Forms and Quadratic Forms An (n × n) square matrix A is said to be positive semi-definite if
A bilinear form in the variables xi and yj is the scalar xT Ax ≥ 0 ∀x. (A.56)
m
n
This definition implies that (A) = r < n, and thus r eigenvalues of A
B= aij xi yj are positive and n − r are null. Therefore, a positive semi-definite matrix A
i=1 j=1
has a null space of finite dimension, and specifically the form vanishes when
which can be written in matrix form x ∈ N (A). A typical example of a positive semi-definite matrix is the matrix A = H T H where H is an (m × n) matrix with m < n. In an analogous way, B(x, y) = xT Ay = y T AT x (A.51) a negative semi-definite matrix can be defined. Given the bilinear form in (A.51), the gradient of the form with respect where x = [ x1 x2 … xm ]T , y = [ y1 y2 … yn ]T , and A is the (m × to x is given by n) matrix of the coefficients aij representing the core of the form. T ∂B(x, y) A special case of bilinear form is the quadratic form ∇x B(x, y) = = Ay, (A.57) ∂x Q(x) = xT Ax (A.52) whereas the gradient of B with respect to y is given by
where A is an (n × n) square matrix. Hence, for computation of (A.52), the T ∂B(x, y) matrix A can be replaced with its symmetric part As given by (A.6). It follows ∇y B(x, y) = = AT x. (A.58) ∂y that if A is a skew-symmetric matrix, then Given the quadratic form in (A.52) with A symmetric, the gradient of the xT Ax = 0 ∀x. form with respect to x is given by The quadratic form (A.52) is said to be positive definite if T ∂Q(x) ∇x Q(x) = = 2Ax. (A.59) xT Ax > 0 ∀x = 0 xT Ax = 0 x = 0. (A.53) ∂x
The matrix A core of the form is also said to be positive definite. Analogously, Further, if x and A are differentiable functions of t, then a quadratic form is said to be negative definite if it can be written as −Q(x) = d −xT Ax where Q(x) is positive definite. Q̇(x) = Q(x(t)) = 2xT Aẋ + xT Ȧx; (A.60) A necessary condition for a square matrix to be positive definite is that dt its elements on the diagonal are strictly positive. Further, in view of (A.50), if A is constant, then the second term obviously vanishes. the eigenvalues of a positive definite matrix are all positive. If the eigenvalues are not known, a necessary and sufficient condition for a symmetric matrix to be positive definite is that its principal minors are strictly positive (Sylvester A.7 Pseudo-inverse criterion). It follows that a positive definite matrix is full-rank and thus it is always invertible. The inverse of a matrix can be defined only when the matrix is square and A symmetric positive definite matrix A can always be decomposed as nonsingular. The inverse operation can be extended to the case of non-square A = U T ΛU (A.54) matrices. Consider a matrix A of dimensions (m × n) with (A) = min{m, n} If m < n, a right inverse of A can be defined as the matrix Ar of dimen- where U is an orthogonal matrix of eigenvectors (U T U = I) and Λ is the sions (n × m) so that diagonal matrix of the eigenvalues of A. AAr = I m . Let λmin (A) and λmax (A) respectively denote the smallest and largest If instead m > n, a left inverse of A can be defined as the matrix Al of eigenvalues of a positive definite matrix A (λmin , λmax > 0). Then, the dimensions (n × m) so that quadratic form in (A.52) satisfies the following inequality: Al A = I n . λmin (A) x 2 ≤ xT Ax ≤ λmax (A) x 2 . (A.55) 576 A Linear Algebra A Linear Algebra 577
If A has more columns than rows (m < n) and has rank m, a special right A.8 Singular Value Decomposition inverse is the matrix A†r = AT (AAT )−1 (A.61) For a nonsquare matrix it is not possible to define eigenvalues. An extension of the eigenvalue concept can be obtained by singular values. Given a matrix which is termed right pseudo-inverse, since AA†r = I m . If W r is an (n × n) A of dimensions (m × n), the matrix AT A has n nonnegative eigenvalues positive definite matrix, a weighted right pseudo-inverse is given by λ1 ≥ λ2 ≥ … ≥ λn ≥ 0 (ordered from the largest to the smallest) which can A†r = W −1 T −1 T −1 r A (AW r A ) . (A.62) be expressed in the form If A has more rows than columns (m > n) and has rank n, a special left λi = σi2 σi ≥ 0. inverse is the matrix A†l = (AT A)−1 AT (A.63) The scalars σ1 ≥ σ2 ≥ … ≥ σn ≥ 0 are said to be the singular values of matrix A. The singular value decomposition (SVD) of matrix A is given by which is termed left pseudo-inverse, since A†l A = I n .3 If W l is an (m × m) positive definite matrix, a weighted left pseudo-inverse is given by A = U ΣV T (A.67) A†l = (AT W l A)−1 AT W l . (A.64) where U is an (m × m) orthogonal matrix The pseudo-inverse is very useful to invert a linear transformation y = Ax U = [ u1 u2 … um ] , (A.68) with A a full-rank matrix. If A is a square nonsingular matrix, then obviously x = A−1 y and then A†l = A†r = A−1 . V is an (n × n) orthogonal matrix If A has more columns than rows (m < n) and has rank m, then the V = [ v1 v2 … vn ] (A.69) solution x for a given y is not unique; it can be shown that the expression and Σ is an (m × n) matrix x = A† y + (I − A† A)k, (A.65) D O with k an arbitrary (n × 1) vector and A† as in (A.61), is a solution to the Σ= D = diag{σ1 , σ2 , … , σr } (A.70) O O system of linear equations established by (A.35). The term A† y ∈ N ⊥ (A) ≡ R(AT ) minimizes the norm of the solution x . The term (I − A† A)k is the where σ1 ≥ σ2 ≥ … ≥ σr > 0. The number of non-null singular values is projection of k in N (A) and is termed homogeneous solution; as k varies, equal to the rank r of matrix A. all the solutions to the homogeneous equation system Ax = 0 associated The columns of U are the eigenvectors of the matrix AAT , whereas the with (A.35) are generated. columns of V are the eigenvectors of the matrix AT A. In view of the partitions On the other hand, if A has more rows than columns (m > n), the equation of U and V in (A.68), (A.69), it is Av i = σi ui , for i = 1, … , r and Av i = 0, in (A.35) has no solution; it can be shown that an approximate solution is given for i = r + 1, … , n. by Singular value decomposition is useful for analysis of the linear transforma- x = A† y (A.66) tion y = Ax established in (A.35). According to a geometric interpretation, where A† as in (A.63) minimizes y − Ax . If instead y ∈ R(A), then (A.66) the matrix A transforms the unit sphere in IRn defined by x = 1 into the set is a real solution. of vectors y = Ax which define an ellipsoid of dimension r in IRm . The sin- Notice that the use of the weighted (left or right) pseudo-inverses in the gular values are the lengths of the various axes of the ellipsoid. The condition solution to the linear equation systems leads to analogous results where the number of the matrix σ1 minimized norms are weighted according to the metrics defined by matrices κ= σr W r and W l , respectively. is related to the eccentricity of the ellipsoid and provides a measure of The results of this section can be easily extended to the case of (square ill-conditioning (κ 1) for numerical solution of the system established or nonsquare) matrices A not having full-rank. In particular, the expres- by (A.35). sion (A.66) (with the pseudo-inverse computed by means of the singular value It is worth noticing that the numerical procedure of singular value de- decomposition of A) gives the minimum-norm vector among all those mini- composition is commonly adopted to compute the (right or left) pseudo- mizing y − Ax . inverse A† , even in the case of a matrix A not having full rank. In fact, 3 from (A.67), (A.70) it is Subscripts l and r are usually omitted whenever the use of a left or right pseudo- inverse is clear from the context. A† = V Σ † U T (A.71) 578 A Linear Algebra
with * D† O 1 1 1 Σ† = D † = diag , ,…, . (A.72) O O σ1 σ2 σr B
Bibliography Rigid-body Mechanics
A reference text on linear algebra is [169]. For matrix computation see [88]. The properties of pseudo-inverse matrices are discussed in [24].
The goal of this appendix is to recall some fundamental concepts of rigid body
mechanics which are preliminary to the study of manipulator kinematics,
statics and dynamics.
B.1 Kinematics
A rigid body is a system characterized by the constraint that the distance
between any two points is always constant.
Consider a rigid body B moving with respect to an orthonormal reference
frame O–xyz of unit vectors x, y, z, called fixed frame. The rigidity assump-
tion allows the introduction of an orthonormal frame O –x y z attached to
the body, called moving frame, with respect to which the position of any point
of B is independent of time. Let x (t), y (t), z (t) be the unit vectors of the
moving frame expressed in the fixed frame at time t.
The orientation of the moving frame O –x y z at time t with respect to
the fixed frame O–xyz can be expressed by means of the orthogonal (3 × 3)
matrix ⎡ T ⎤
x (t)x y T (t)x z T (t)x
R(t) = ⎣ xT (t)y y T (t)y z T (t)y ⎦ , (B.1)
xT (t)z y T (t)z z T (t)z
which is termed rotation matrix defined in the orthonormal special group
SO(3) of the (3 × 3) matrices with orthonormal columns and determinant
equal to 1. The columns of the matrix in (B.1) represent the components
of the unit vectors of the moving frame when expressed in the fixed frame,
whereas the rows represent the components of the unit vectors of the fixed
frame when expressed in the moving frame.
Let p be the constant position vector of a generic point P of B in the
moving frame O –x y z . The motion of P with respect to the fixed frame
O–xyz is described by the equation
p(t) = pO (t) + R(t)p , (B.2)
580 B Rigid-body Mechanics B Rigid-body Mechanics 581
where pO (t) is the position vector of origin O of the moving frame with Comparing this equation with the formal time derivative of (B.3) leads to the respect to the fixed frame. result Notice that a position vector is a bound vector since its line of application Ṙ = S(ω)R. (B.5) and point of application are both prescribed, in addition to its direction; the In view of (B.4), the elementary displacement of a point P of the rigid body point of application typically coincides with the origin of a reference frame. B in the time interval (t, t + dt) is Therefore, to transform a bound vector from a frame to another, both trans- lation and rotation between the two frames must be taken into account. dp = ṗdt = ṗQ + ω × (p − pQ ) dt (B.6) If the positions of the points of B in the moving frame are known, it follows from (B.2) that the motion of each point of B with respect to the fixed frame = dpQ + ωdt × (p − pQ ). is uniquely determined once the position of the origin and the orientation Differentiating (B.4) with respect to time yields the following expression of the moving frame with respect to the fixed frame are specified in time. for acceleration: The origin of the moving frame is determined by three scalar functions of time. Since the orthonormality conditions impose six constraints on the nine p̈ = p̈Q + ω̇ × (p − pQ ) + ω × ω × (p − pQ ) . (B.7) elements of matrix R(t), the orientation of the moving frame depends only on three independent scalar functions, three being the minimum number of parameters to represent SO(3).1 B.2 Dynamics Therefore, a rigid body motion is described by arbitrarily specifying six scalar functions of time, which describe the body pose (position + orientation). Let ρdV be the mass of an elementary particle of a rigid body B, where ρ The resulting rigid motions belong to the special Euclidean group SE(3) = denotes the density IR3 × SO(3). % of the particle of volume dV . Also let VB be the body volume and m = VB ρdV its total mass assumed to be constant. If p denotes The expression in (B.2) continues to hold if the position vector pO (t) of the position vector of the particle of mass ρdV in the frame O–xyz, the centre the origin of the moving frame is replaced with the position vector of any of mass of B is defined as the point C whose position vector is other point of B, i.e., ” 1 p(t) = pQ (t) + R(t)(p − pQ ) (B.3) pC = m VB pρdV . (B.8)
where pQ (t) and pQ are the position vectors of a point Q of B in the fixed In the case when B is the union of n distinct parts of mass m1 , … , mn and and moving frames, respectively. centres of mass pC1 … pCn , the centre of mass of B can be computed as In the following, for simplicity of notation, the dependence on the time 1 variable t will be dropped. n
Differentiating (B.3) with respect to time gives the known velocity com- pC = mi pCi m i=1 position rule ṗ = ṗQ + ω × (p − pQ ), (B.4) .n with m = i=1 mi . where ω is the angular velocity of rigid body B. Notice that ω is a free vector Let r be a line passing by O and d(p) the distance from r of the particle since its point of application is not prescribed. To transform a free vector from of B of mass ρdV and position vector p. The moment of inertia of body B a frame to another, only rotation between the two frames must be taken into with respect to line r is defined as the positive scalar account. ” By recalling the definition of the skew-symmetric operator S(·) in (A.32), Ir = d2 (p)ρdV . the expression in (B.4) can be rewritten as VB
ṗ = ṗQ + S(ω)(p − pQ ) Let r denote the unit vector of line r; then, the moment of inertia of B with
respect to line r can be expressed as
= ṗQ + S(ω)R(p − pQ ).
"
T T
1 The minimum number of parameters represent a special orthonormal Ir = r S (p)S(p)ρdV r = r T I O r, (B.9) VB group SO(m) is equal to m(m − 1)/2. 582 B Rigid-body Mechanics B Rigid-body Mechanics 583
where S(·) is the skew-symmetric operator in (A.31), and the symmetric, Let ṗ be the velocity of a particle of B of elementary mass ρdV in frame positive definite matrix O–xyz. The linear momentum of body B is defined as the vector ⎡% % % ⎤ ” (p2 + p2z )ρdV VB y − VB px py ρdV − VB px pz ρdV l= ṗρdV = mṗC . (B.13) ⎢ % % IO = ⎣ ∗ (p2 + p2z )ρdV VB x − VB py pz ρdV ⎥⎦ VB % 2 2 Let Ω be any point in space and pΩ its position vector in frame O–xyz; ∗ ∗ (p + py )ρdV VB x then, the angular momentum of body B relative to pole Ω is defined as the ⎡ ⎤ IOxx −IOxy −IOxz vector ” ⎢ ⎥ ṗ × (pΩ − p)ρdV . =⎣ ∗ IOyy −IOyz ⎦ (B.10) kΩ = VB ∗ ∗ IOzz The pole can be either fixed or moving with respect to the reference frame. is termed inertia tensor of body B relative to pole O.2 The (positive) elements The angular momentum of a rigid body has the following notable expression: IOxx , IOyy , IOzz are the inertia moments with respect to three coordinate axes kΩ = I C ω + mṗC × (pΩ − pC ), (B.14) of the reference frame, whereas the elements IOxy , IOxz , IOyz (of any sign) are said to be products of inertia. where I C is the inertia tensor relative to the centre of mass, when expressed The expression of the inertia tensor of a rigid body B depends both on the in a frame parallel to the reference frame with origin at the centre of mass. pole and the reference frame. If orientation of the reference frame with origin The forces acting on a generic system of material particles can be distin- at O is changed according to a rotation matrix R, the inertia tensor I O in guished into internal forces and external forces. the new frame is related to I O by the relationship The internal forces, exerted by one part of the system on another, have null linear and angular momentum and thus they do not influence rigid body I O = RI O RT . (B.11) motion. The external forces, exerted on the system by an agency outside the sys- The way an inertia tensor is transformed when the pole is changed can be tem, in the case of a rigid body B are distinguished into active forces and inferred by the following equation, also known as Steiner theorem or parallel reaction forces. axis theorem: The active forces can be either concentrated forces or body forces. The I O = I C + mS T (pC )S(pC ), (B.12) former are applied to specific points of B, whereas the latter act on all ele- where I C is the inertia tensor relative to the centre of mass of B, when ex- mentary particles of the body. An example of body force is the gravitational pressed in a frame parallel to the frame with origin at O and with origin at force which, for any elementary particle of mass ρdV , is equal to g 0 ρdV where the centre of mass C. g 0 is the gravity acceleration vector. Since the inertia tensor is a symmetric positive definite matrix, there al- The reaction forces are those exerted because of surface contact between ways exists a reference frame in which the inertia tensor attains a diagonal two or more bodies. Such forces can be distributed on the contact surfaces or form; such a frame is said to be a principal frame (relative to pole O) and they can be assumed to be concentrated. its coordinate axes are said to be principal axes. In the case when pole O For a rigid body B subject to gravitational force, as well as to active and coincides with the centre of mass, the frame is said to be a central frame and or reaction forces f 1 … f n concentrated at points p1 … pn , the resultant of its axes are said to be central axes. the external forces f and the resultant moment μΩ with respect to a pole Ω Notice that if the rigid body is moving with respect to the reference frame are respectively with origin at O, then the elements of the inertia tensor I O become a func- ” n n tion of time. With respect to a pole and a reference frame attached to the f = g 0 ρdV + f i = mg 0 + fi (B.15) body (moving frame), instead, the elements of the inertia tensor represent six VB i=1 i=1 structural constants of the body which are known once the pole and reference ” n
frame have been specified. μΩ = g 0 × (pΩ − p)ρdV + f i × (pΩ − pi ) VB i=1 n
2 = mg 0 × (pΩ − pC ) + f i × (pΩ − pi ). (B.16) The symbol ‘∗’ has been used to avoid rewriting the symmetric elements. i=1 584 B Rigid-body Mechanics B Rigid-body Mechanics 585
In the case when f and μΩ are known and it is desired to compute the The kinetic energy of a body B is defined as the scalar quantity resultant moment with respect to a point Ω other than Ω, the following ” 1 relation holds: T = ṗT ṗρdV μΩ = μΩ + f × (pΩ − pΩ ). (B.17) 2 VB
Consider now a generic system of material particles subject to external which, for a rigid body, takes on the notable expression
forces of resultant f and resultant moment μΩ . The motion of the system 1 1 in a frame O–xyz is established by the following fundamental principles of T = mṗTC ṗC + ω T I C ω (B.23) dynamics (Newton laws of motion): 2 2 where I C is the inertia tensor relative to the centre of mass expressed in a f = l̇ (B.18) frame parallel to the reference frame with origin at the centre of mass. μΩ = k̇Ω (B.19) A system of position forces, i.e., the forces depending only on the positions of the points of application, is said to be conservative if the work done by each where Ω is a pole fixed or coincident with the centre of mass C of the system. force is independent of the trajectory described by the point of application of These equations hold for any mechanical system and can be used even in the the force but it depends only on the initial and final positions of the point of case of variable mass. For a system with constant mass, computing the time application. In this case, the elementary work of the system of forces is equal derivative of the momentum in (B.18) gives Newton equations of motion in to minus the total differential of a scalar function termed potential energy, the form i.e., f = mp̈C , (B.20) dW = −dU. (B.24) where the quantity on the right-hand side represents the resultant of inertia An example of a conservative system of forces on a rigid body is the gravita- forces. tional force, with which is associated the potential energy If, besides the assumption of constant mass, the assumption of rigid system ” holds too, the expression in (B.14) of the angular momentum with (B.19) yield U =− g T0 pρdV = −mg T0 pC . (B.25) Euler equations of motion in the form VB
μΩ = I Ω ω̇ + ω × (I Ω ω), (B.21)
B.4 Constrained Systems
where the quantity on the right-hand side represents the resultant moment of inertia forces. Consider a system Br of r rigid bodies and assume that all the elements of Br For a system constituted by a set of rigid bodies, the external forces obvi- can reach any position in space. In order to find uniquely the position of all the ously do not include the reaction forces exerted between the bodies belonging points of the system, it is necessary to assign a vector x = [ x1 … xp ]T to the same system. of 6r = p parameters, termed configuration. These parameters are termed Lagrange or generalized coordinates of the unconstrained system Br , and p determines the number of degrees of freedom (DOFs). B.3 Work and Energy Any limitation on the mobility of the system Br is termed constraint. A constraint acting on Br is said to be holonomic if it is expressed by a system Given a force f i applied at a point of position pi with respect to frame O–xyz, of equations the elementary work of the force f i on the displacement dpi = ṗi dt is defined h(x, t) = 0, (B.26) as the scalar where h is a vector of dimensions (s × 1), with s < m. On the other hand, dWi = f Ti dpi . a constraint in the form h(x, ẋ, t) = 0 which is nonintegrable is said to For a rigid body B subject to a system of forces of resultant f and resultant be nonholonomic. For simplicity, only equality (or bilateral ) constraints are moment μQ with respect to any point Q of B, the elementary work on the considered. If the equations in (B.26) do not explicitly depend on time, the rigid displacement (B.6) is given by constraint is said to be scleronomic. On the assumption that h has continuous and continuously differentiable dW = (f T ṗQ + μTQ ω)dt = f T dpQ + μTQ ωdt. (B.22) components, and its Jacobian ∂h/∂x has full rank, the equations in (B.26) 586 B Rigid-body Mechanics B Rigid-body Mechanics 587
allow the elimination of s out of m coordinates of the system Br . With the In the case of frictionless equality constraints, reaction forces are exerted remaining n = m − s coordinates it is possible to determine uniquely the orthogonally to the contact surfaces and the virtual work is always null. Hence, configurations of Br satisfying the constraints (B.26). Such coordinates are (B.31) reduces to the Lagrange or generalized coordinates and n is the number of degrees of δWm + δWa = 0. (B.32) freedom of the unconstrained system Br .3 For a steady system, inertia forces are identically null. Then the condition The motion of a system Br with n DOFs and holonomic equality con- for the equilibrium of system Br is that the virtual work of the active forces straints can be described by equations of the form is identically null on any virtual displacement, which gives the fundamental x = x(q(t), t), (B.27) equation of statics of a constrained system
δWa = 0 (B.33)
where q(t) = [ q1 (t) … qn (t) ]T is a vector of Lagrange coordinates. The elementary displacement of system (B.27) relative to the interval (t, t+ known as principle of virtual work . Expressing (B.33) in terms of the incre- dt) is defined as ment δλ of generalized coordinates leads to ∂x(q, t) ∂x(q, t) dx = q̇dt + dt. (B.28) δWa = ζ T δq = 0 (B.34) ∂q ∂t The virtual displacement of system (B.27) at time t, relative to an increment where ζ denotes the (n × 1) vector of active generalized forces. δλ, is defined as the quantity In the dynamic case, it is worth distinguishing active forces into conserva- tive (that can be derived from a potential) and nonconservative. The virtual ∂x(q, t) work of conservative forces is given by δx = δq. (B.29) ∂q ∂U The difference between the elementary displacement and the virtual displace- δWc = − δq, (B.35) ∂q ment is that the former is relative to an actual motion of the system in an interval (t, t + dt) which is consistent with the constraints, while the latter is where U(λ) is the total potential energy of the system. The work of noncon- relative to an imaginary motion of the system when the constraints are made servative forces can be expressed in the form invariant and equal to those at time t. δWnc = ξ T δq, (B.36) For a system with time-invariant constraints, the equations of motion (B.27) become where ξ denotes the vector of nonconservative generalized forces. It follows x = x(q(t)), (B.30) that the vector of active generalized forces is and then, by setting δλ = dλ = λ̇dt, the virtual displacements (B.29) coincide T ∂U with the elementary displacements (B.28). ζ =ξ− . (B.37) To the concept of virtual displacement can be associated that of virtual ∂q work of a system of forces, by considering a virtual displacement instead of Moreover, the work of inertia forces can be computed from the total kinetic an elementary displacement. energy of system T as If external forces are distinguished into active forces and reaction forces, a direct consequence of the principles of dynamics (B.18), (B.19) applied to the ∂T d ∂T δWm = − δq. (B.38) system of rigid bodies Br is that, for each virtual displacement, the following ∂q dt ∂ q̇ relation holds: δWm + δWa + δWh = 0, (B.31) Substituting (B.35), (B.36), (B.38) into (B.32) and observing that (B.32) holds true for any increment δλ leads to Lagrange equations where δWm , δWa , δWh are the total virtual works done by the inertia, active, T T reaction forces, respectively. d ∂L ∂L − = ξ, (B.39) dt ∂ q̇ ∂q 3 In general, the Lagrange coordinates of a constrained system have a local validity; in certain cases, such as the joint variables of a manipulator, they can have a global where validity. L=T −U (B.40) 588 B Rigid-body Mechanics
is the Lagrangian function of the system. The equations in (B.39) completely describe the dynamic behaviour of an n-DOF system with holonomic equality constraints. C The sum of kinetic and potential energy of a system with time-invariant constraints is termed Hamiltonian function Feedback Control H = T + U. (B.41)
Conservation of energy dictates that the time derivative of the Hamiltonian must balance the power generated by the nonconservative forces acting on the system, i.e., dH = ξ T q̇. (B.42) dt In view of (B.37), (B.41), the equation in (B.42) becomes
dT As a premise to the study of manipulator decentralized control and centralized
= ζ T q̇. (B.43) control, the fundamental principles of feedback control of linear systems are
dt
recalled, and an approach to the determination of control laws for nonlinear
systems based on the use of Lyapunov functions is presented.
Bibliography The fundamental concepts of rigid-body mechanics and constrained systems C.1 Control of Single-input/Single-output Linear can be found in classical texts such as [87, 154, 224]. An authoritative reference Systems on rigid-body system dynamics is [187]. According to classical automatic control theory of linear time-invariant single- input/single-output systems, in order to servo the output y(t) of a system to a reference r(t), it is worth adopting a negative feedback control structure. This structure indeed allows the use of approximate mathematical models to describe the input/output relationship of the system to control, since negative feedback has a potential for reducing the effects of system parameter variations and nonmeasurable disturbance inputs d(t) on the output. This structure can be represented in the domain of complex variable s as in the block scheme of Fig. C.1, where G(s), H(s) and C(s) are the transfer func- tions of the system to control, the transducer and the controller, respectively. From this scheme it is easy to derive
Y (s) = W (s)R(s) + WD (s)D(s), (C.1)
where
C(s)G(s)
W (s) = (C.2)
1 + C(s)G(s)H(s)
is the closed-loop input/output transfer function and
G(s)
WD (s) = (C.3)
1 + C(s)G(s)H(s)
is the disturbance/output transfer function.
590 C Feedback Control C Feedback Control 591
If the closed-loop system is asymptotically stable, the steady-state response
to a sinusoidal input r(t), with d(t) = 0, is sinusoidal, too. In this case, the
function W (s), evaluated for s = jω, is termed frequency response function;
the frequency response function of a feedback system can be assimilated to
that of a low-pass filter with the possible occurrence of a resonance peak inside
its bandwidth.
As regards the transducer, this should be chosen so that its bandwidth
is much greater than the feedback system bandwidth, in order to ensure
Fig. C.1. Feedback control structure
a nearly instantaneous response for any value of ω inside the bandwidth
of W (jω). Therefore, setting H(jω) ≈ H0 and assuming that the loop gain
The goal of the controller design is to find a control structure C(s) ensuring |C(jω)G(jω)H0 | 1 in the same bandwidth, the expression in (C.1) for
that the output variable Y (s) tracks a reference input R(s). Further, the s = jω can be approximated as controller should guarantee that the effects of the disturbance input D(s) on R(jω) D(jω) the output variable are suitably reduced. The goal is then twofold, namely, Y (jω) ≈ + . H0 C(jω)H0 reference tracking and disturbance rejection. The basic problem for controller design consists of the determination of an Assuming R(jω) = H0 Yd (jω) leads to action C(s) which can make the system asymptotically stable. In the absence D(jω) of positive or null real part pole/zero and zero/pole cancellation in the open- Y (jω) ≈ Yd (jω) + ; (C.4) loop function F (s) = C(s)G(s)H(s), a necessary and sufficient condition for C(jω)H0 asymptotic stability is that the poles of W (s) and WD (s) have all negative i.e., the output tracks the desired output Yd (jω) and the frequency compo- real parts; such poles coincide with the zeros of the rational transfer function nents of the disturbance in the bandwidth of W (jω) produce an effect on the 1 + F (s). Testing for this condition can be performed by resorting to stability output which can be reduced by increasing |C(jω)H0 |. Furthermore, if the criteria, thus avoiding computation of the function zeros. disturbance input is a constant, the steady-state output is not influenced by Routh criterion allows the determination of the sign of the real parts of the disturbance as long as C(s) has at least a pole at the origin. the zeros of the function 1 + F (s) by constructing a table with the coefficients Therefore, a feedback control system is capable of establishing a propor- of the polynomial at the numerator of 1 + F (s) (characteristic polynomial). tional relationship between the desired output and the actual output, as evi- Routh criterion is easy to apply for testing stability of a feedback system, denced by (C.4). This equation, however, requires that the frequency content but it does not provide a direct relationship between the open-loop function of the input (desired output) be inside the frequency range for which the loop and stability of the closed-loop system. It is then worth resorting to Nyquist gain is much greater than unity. criterion which is based on the representation, in the complex plane, of the The previous considerations show the advantage of including a proportional open-loop transfer function F (s) evaluated in the domain of real angular fre- action and an integral action in the controller C(s), leading to the transfer quency (s = jω, −∞ < ω < +∞). function Drawing of Nyquist plot and computation of the number of circles made by 1 + sTI C(s) = KI (C.5) the vector representing the complex number 1 + F (jω) when ω continuously s varies from −∞ to +∞ allows a test on whether or not the closed-loop system of a proportional-integral controller (PI); TI is the time constant of the integral is asymptotically stable. It is also possible to determine the number of positive, action and the quantity KI TI is called proportional sensitivity. null and negative real part roots of the characteristic polynomial, similarly to The adoption of a PI controller is effective for low-frequency response of application of Routh criterion. Nonetheless, Nyquist criterion is based on the the system, but it may involve a reduction of stability margins and/or a reduc- plot of the open-loop transfer function, and thus it allows the determination of tion of closed-loop system bandwidth. To avoid these drawbacks, a derivative a direct relationship between this function and closed-loop system stability. It action can be added to the proportional and integral actions, leading to the is then possible from an examination of the Nyquist plot to draw suggestions transfer function on the controller structure C(s) which ensures closed-loop system asymptotic 1 + sTI + s2 TD TI C(s) = KI (C.6) stability. s of a proportional-integral-derivative controller (PID); TD denotes the time constant of the derivative action. Notice that physical realizability of (C.6) 592 C Feedback Control C Feedback Control 593
demands the introduction of a high-frequency pole which little influences the input/output relationship in the system bandwidth. The transfer function in (C.6) is characterized by the presence of two zeros which provide a stabi- lizing action and an enlargement of the closed-loop system bandwidth. Band- width enlargement implies shorter response time of the system, in terms of both variations of the reference signal and recovery action of the feedback system to output variations induced by the disturbance input. The parameters of the adopted control structure should be chosen so as to satisfy requirements on the system behaviour at steady state and during the transient. Classical tools to determine such parameters are the root locus in the domain of the complex variable s or the Nichols chart in the domain of the real angular frequency ω. The two tools are conceptually equivalent. Fig. C.2. Feedback control structure with feedforward compensation Their potential is different in that root locus allows a control law to be found which assigns the exact parameters of the closed-loop system time response, whereas Nichols chart allows a controller to be specified which confers good and of the 3 dB bandwidth transient and steady-state behaviour to the system response. A feedback system with strict requirements on the steady-state and tran- ω 3 = ωn 1 − 2ζ 2 + 2 − 4ζ 2 + 4ζ 4 . sient behaviour, typically, has a response that can be assimilated to that of a A step input is typically used to characterize the transient response in the second-order system. In fact, even for closed-loop functions of greater order, time domain. The influence of parameters ζ and ωn on the step response can it is possible to identify a pair of complex conjugate poles whose real part be evaluated in terms of the percentage of overshoot absolute value is smaller than the real part absolute values of the other poles. Such a pair of poles is dominant in that its contribution to the transient re- s% = 100 exp(−πζ/ 1 − ζ 2 ), sponse prevails over that of the other poles. It is then possible to approximate of the rise time the input/output relationship with the transfer function 1.8 tr ≈ ωn kW and of the settling time within 1% W (s) = (C.7) 2ζs s2 4.6 1+ + 2 ts = . ωn ωn ζωn which has to be realized by a proper choice of the controller. Regarding The adoption of a feedforward compensation action represents a feasible the values to assign to the parameters characterizing the transfer function solution both for tracking a time-varying reference input and for enhancing in (C.7), the following remarks are in order. The constant kW represents the rejection of the effects of a disturbance on the output. Consider the general input/output steady-state gain, which is equal to 1/H0 if C(s)G(s)H0 has at scheme in Fig. C.2. Let R(s) denote a given input reference and Dc (s) de- least a pole at the origin. The natural frequency ωn is the modulus of the note a computed estimate of the disturbance D(s); the introduction of the complex conjugate poles, whose real part is given by −ζωn where ζ is the feedforward action yields the input/output relationship damping ratio of the pair of poles. C(s)G(s) F (s)G(s) The influence of parameters ζ and ωn on the closed-loop frequency re- Y (s) = + R(s) (C.8) 1 + C(s)G(s)H(s) 1 + C(s)G(s)H(s) sponse can be evaluated in terms of the resonance peak magnitude G(s) 1 + D(s) − Dc (s) . Mr = , 1 + C(s)G(s)H(s) 2ζ 1 − ζ2 By assuming that the desired output is related to the reference input by a constant factor Kd and regarding the transducer as an instantaneous system occurring at the resonant frequency (H(s) ≈ H0 = 1/Kd ) for the current operating conditions, the choice ωr = ωn 1 − 2ζ 2 , Kd F (s) = (C.9) G(s) 594 C Feedback Control C Feedback Control 595
1
where H(x) )
and h(x) respectively denote the estimates of the terms H(x)
and h(x), computed on the basis of measures on the system state, and v is a
new control input to be defined later. In general, it is
1
H(x) = H(x) + ΔH(x) (C.13)
)
h(x, ẋ) = h(x, ẋ) + Δh(x, ẋ) (C.14)
Fig. C.3. Feedback control structure with inverse model technique
because of the unavoidable modelling approximations or as a consequence of
an intentional simplification in the compensating action. Substituting (C.12)
yields the input/output relationship into (C.11) and accounting for (C.13), (C.14) yields G(s) Y (s) = Yd (s) + D(s) − Dc (s) . (C.10) ẍ = v + z(x, ẋ, v) (C.15) 1 + C(s)G(s)H0 If |C(jω)G(jω)H0 | 1, the effect of the disturbance on the output is further where reduced by means of an accurate estimate of the disturbance. z(x, ẋ, v) = H −1 (x) ΔH(x)v + Δh(x, ẋ) . Feedforward compensation technique may lead to a solution, termed in- If tracking of a trajectory (xd (t), ẋd (t), ẍd (t)) is desired, the tracking error verse model control , illustrated in the scheme of Fig. C.3. It should be re- can be defined as marked, however, that such a solution is based on dynamics cancellation, xd − x e= (C.16) and thus it can be employed only for a minimum-phase system, i.e., a system ẋd − ẋ whose poles and zeros have all strictly negative real parts. Further, one should and it is necessary to derive the error dynamics equation to study convergence consider physical realizability issues as well as effects of parameter variations of the actual state to the desired one. To this end, the choice which prevent perfect cancellation. v = ẍd + w(e), (C.17)
C.2 Control of Nonlinear Mechanical Systems substituted into (C.15), leads to the error equation
If the system to control does not satisfy the linearity property, the control ė = F e − Gw(e) − Gz(e, xd , ẋd , ẍd ), (C.18) design problem becomes more complex. The fact that a system is qualified as nonlinear , whenever linearity does not hold, leads to understanding how where the (2n × 2n) and (2n × n) matrices, respectively, it is not possible to resort to general techniques for control design, but it is necessary to face the problem for each class of nonlinear systems which can O I O F = G= be defined through imposition of special properties. O O I On the above premise, the control design problem of nonlinear systems described by the dynamic model follow from the error definition in (C.16). Control law design consists of finding the error function w(e) which makes (C.18) globally asymptotically stable,1 H(x)ẍ + h(x, ẋ) = u (C.11) i.e., lim e(t) = 0. is considered, where [ xT ẋT ]T denotes the (2n × 1) state vector of the t→∞ system, u is the (n × 1) input vector, H(x) is an (n × n) positive definite In the case of perfect nonlinear compensation (z(·) = 0), the simplest choice (and thus invertible) matrix depending on x, and h(x, ẋ) is an (n × 1) vector of the control action is the linear one depending on state. Several mechanical systems can be reduced to this class, including manipulators with rigid links and joints. w(e) = −K P (xd − x) − K D (ẋd − ẋ) (C.19) The control law can be found through a nonlinear compensating action = [ −K P −K D ] e, obtained by choosing the following nonlinear state feedback law (inverse dy- namics control): 1 Global asymptotic stability is invoked to remark that the equilibrium state is 1 u = H(x)v ) + h(x, ẋ) (C.12) asymptotically stable for any perturbation. 596 C Feedback Control C Feedback Control 597
where asymptotic stability of the error equation is ensured by choosing positive V (e) = 0 e=0 definite matrices K P and K D . The error transient behaviour is determined V̇ (e) < 0 ∀e = 0 by the eigenvalues of the matrix V (e) → ∞ e → ∞. O I A= (C.20) The existence of such a function ensures global asymptotic stability of the equi- −K P −K D librium e = 0. In practice, the equilibrium e = 0 is globally asymptotically characterizing the error dynamics stable if a positive definite, radially unbounded function V (e) is found so that its time derivative along the system trajectories is negative definite. ė = Ae. (C.21) If positive definiteness of V (e) is realized by the adoption of a quadratic form, i.e., If compensation is imperfect, then z(·) cannot be neglected and the error V (e) = eT Qe (C.24) equation in (C.18) takes on the general form with Q a symmetric positive definite matrix, then in view of (C.22) it follows ė = f (e). (C.22) V̇ (e) = 2eT Qf (e). (C.25) It may be worth choosing the control law w(e) as the sum of a nonlinear term and a linear term of the kind in (C.19); in this case, the error equation can If f (e) is so as to render the function V̇ (e) negative definite, the function be written as V (e) is a Lyapunov function, since the choice (C.24) allows system global ė = Ae + k(e), (C.23) asymptotic stability to be proved. If V̇ (e) in (C.25) is not negative definite for the given V (e), nothing can be inferred on the stability of the system, where A is given by (C.20) and k(e) is available to make the system globally since the Lyapunov method gives only a sufficient condition. In such cases asymptotically stable. The equations in (C.22), (C.23) express nonlinear dif- one should resort to different choices of V (e) in order to find, if possible, a ferential equations of the error. To test for stability and obtain advise on the negative definite V̇ (e). choice of suitable control actions, one may resort to Lyapunov direct method In the case when the property of negative definiteness does not hold, but illustrated below. V̇ (e) is only negative semi-definite
V̇ (e) ≤ 0,
C.3 Lyapunov Direct Method global asymptotic stability of the equilibrium state is ensured if the only sys- The philosophy of the Lyapunov direct method is the same as that of most tem trajectory for which V̇ (e) is identically null (V̇ (e) ≡ 0) is the equilibrium methods used in control engineering to study stability, namely, testing for trajectory e ≡ 0 (a consequence of La Salle theorem). stability without solving the differential equations describing the dynamic Finally, consider the stability problem of the nonlinear system in the system. form (C.23); under the assumption that k(0) = 0, it is easy to verify that This method can be presented in short on the basis of the following rea- e = 0 is an equilibrium state for the system. The choice of a Lyapunov func- soning. If it is possible to associate an energy-based description with a (linear tion candidate as in (C.24) leads to the following expression for its derivative: or nonlinear) autonomous dynamic system and, for each system state with the exception of the equilibrium state, the time rate of such energy is negative, V̇ (e) = eT (AT Q + QA)e + 2eT Qk(e). (C.26) then energy decreases along any system trajectory until it attains its mini- By setting mum at the equilibrium state; this argument justifies an intuitive concept of AT Q + QA = −P , (C.27) stability. With reference to (C.22), by setting f (0) = 0, the equilibrium state is the expression in (C.26) becomes e = 0. A scalar function V (e) of the system state, continuous together with its first derivative, is defined a Lyapunov function if the following properties V̇ (e) = −eT P e + 2eT Qk(e). (C.28) hold: The matrix equation in (C.27) is said to be a Lyapunov equation; for any V (e) > 0 ∀e = 0 choice of a symmetric positive definite matrix P , the solution matrix Q exists 598 C Feedback Control
and is symmetric positive definite if and only if the eigenvalues of A have all negative real parts. Since matrix A in (C.20) verifies such condition, it is always possible to assign a positive definite matrix P and find a positive D definite matrix solution Q to (C.27). It follows that the first term on the right-hand side of (C.28) is negative definite and the stability problem is Differential Geometry reduced to searching a control law so that k(e) renders the total V̇ (e) negative (semi-)definite. It should be underlined that La Salle theorem does not hold for time- varying systems (also termed non-autonomous) in the form
ė = f (e, t).
In this case, a conceptually analogous result which might be useful is the following, typically referred to as Barbalat lemma — of which it is indeed a consequence. Given a scalar function V (e, t) so that The analysis of mechanical systems subject to nonholonomic constraints, such
- V (e, t) is lower bounded as wheeled mobile robots, requires some basic concepts of differential geometry
- V̇ (e, t) ≤ 0 and nonlinear controllability theory, that are briefly recalled in this appendix.
- V̇ (e, t) is uniformly continuous
then it is lim t→∞ V̇ (e, t) = 0. Conditions 1 and 2 imply that V (e, t) has a D.1 Vector Fields and Lie Brackets bounded limit for t → ∞. Since it is not easy to verify the property of uniform continuity from the definition, Condition 3 is usually replaced by For simplicity, the case of vectors x ∈ IRn is considered. The tangent space 3’. V̈ (e, t) is bounded at x (intuitively, the space of velocities of trajectories passing through x) is hence denoted by Tx (IRn ). The presented notions are however valid in the which is sufficient to guarantee validity of Condition 3. Barbalat lemma can more general case in which a differentiable manifold (i.e., a space that is obviously be used for time-invariant (autonomous) dynamic systems as an locally diffeomorphic to IRn ) is considered in place of a Euclidean space. alternative to La Salle theorem, with respect to which some conditions are A vector field g : IRn → Tx (IRn ) is a mapping that assigns to each point relaxed; in particular, V (e) needs not necessarily be positive definite. x ∈ IRn a tangent vector g(x) ∈ Tx (IRn ). In the following it is always assumed that vector fields are smooth, i.e., such that the associated mappings are of class C ∞ . Bibliography If the vector field g(x) is used to define a differential equation as in
Linear systems analysis can be found in classical texts such as [61]. For the ẋ = g(x), (D.1) control of these systems see [82, 171]. For the analysis of nonlinear systems g the flow φt (x) of g is the mapping that associates to each point x the value see [109]. Control of nonlinear mechanical systems is dealt with in [215]. at time t of the solution of (D.1) evolving from x at time 0, or
d g g
φ (x) = g(φt (x)). (D.2)
dt t
The family of mappings {φgt } is a one-parameter (i.e., t) group under the
composition operator
φgt1 ◦ φgt2 = φgt1 +t2 .
For example, for time-invariant linear systems it is g(x) = Ax and the flow
is the linear operator φgt = eAt .
600 D Differential Geometry D Differential Geometry 601
?n If g 1 and g 2 commute, the net displacement resulting from the input se-
quence (D.5) is zero.
The above expression shows that, at each point x, infinitesimal motion
¦ /1 of the driftless system (D.4) is possible not only in the directions belonging
1 > /d /1 H to the linear span of g 1 (x) and g 2 (x), but also in the direction of their Lie
?1 bracket [g 1 , g 2 ](x). It can be proven that more complicated input sequences
/d
/d /1 ¦ /d can be used to generate motion in the direction of higher-order Lie brackets,
/1 such as [g 1 , [g 1 , g 2 ]].
Similar constructive procedures can be given for systems with a drift 1
?d vector field, such as the following:
Fig. D.1. The net displacement of system (D.4) under the input sequence (D.5) is ẋ = f (x) + g 1 (x)u1 + g 2 (x)u2 . (D.6) directed as the Lie bracket of the two vector fields g 1 and g 2 Using appropriate input sequences, it is possible to generate motion in the direction of Lie brackets involving the vector field f as well as g j , j = 1, 2. Given two vector fields g 1 and g 2 , the composition of their flows is non- commutative in general: g g g g φt 1 ◦ φs 2 = φs 2 ◦ φt 1 . Example D.1 The vector field [g 1 , g 2 ] defined as For a single-input linear system ∂g 2 ∂g 1 g 1 , g 2 = g (x) − g (x) (D.3) ∂x 1 ∂x 2 ẋ = A x + b u, is called Lie bracket of g 1 and g 2 . The two vector field g 1 and g 2 commute if the drift and input vector fields are f (x) = Ax and g(x) = b, respectively. The [g 1 , g 2 ] = 0. following Lie brackets: The Lie bracket operation has an interesting interpretation. Consider the driftless dynamic system −[f , g] = Ab [f , [f , g]] = A2 b ẋ = g 1 (x)u1 + g 2 (x)u2 (D.4) − [f , [f , [f , g]]] = A3 b associated with the vector fields g 1 and g 2 . If the inputs u1 and u2 are never .. active simultaneously, the solution of the differential equation (D.4) can be . obtained by composing the flows of g 1 and g 2 . In particular, consider the following input sequence: represent well-known directions in which it is possible to move the system. ⎧ ⎪ u (t) = +1, u2 (t) = 0 t ∈ [0, ε) ⎨ 1 u1 (t) = 0, u2 (t) = +1 t ∈ [ε, 2ε) The Lie derivative of the scalar function α : IRn → IR along vector field g u(t) = (D.5) ⎩ u1 (t) = −1, u2 (t) = 0 t ∈ [2ε, 3ε) ⎪ is defined as u1 (t) = 0, u2 (t) = −1 t ∈ [3ε, 4ε), ∂α Lg α(x) = g(x). (D.7) where ε is an infinitesimal time interval. The solution of (D.4) at time t = 4ε ∂x can be obtained by following first the flow of g 1 , then of g 2 , then of −g 1 , and The following properties of Lie brackets are useful in computation: finally of −g 2 (see Fig. D.1). By computing x(ε) through a series expansion [f , g] = −[g, f ] (skew-symmetry) at x0 = x(0) along g 1 , then x(2ε) as a series expansion at x(ε) along g 2 , and [f , [g, h]] + [h, [f , g]] + [g, [h, f ]] = 0 (Jacobi identity) so on, one obtains [αf , βg] = αβ[f , g] + α(Lf β)g − β(Lg α)f (chain rule) −g 2 −g g g x(4ε) = φε ◦ φε 1 ◦ φε 2 ◦ φε 1 (x0 ) 1 ∂g 2 ∂g 1 This term emphasizes how the presence of f will in general force the system to = x0 + ε2 g 1 (x0 ) − g 2 (x0 ) + O(ε3 ). move (ẋ = 0) even in the absence of inputs. ∂x ∂x 602 D Differential Geometry D Differential Geometry 603
with α, β: IRn → IR. The vector space V(IRn ) of smooth vector fields on IRn , D.2 Nonlinear Controllability equipped with the Lie bracket operation, is a Lie algebra. The distribution Δ associated with the m vector fields {g 1 , … , g m } is the Consider a nonlinear dynamic system of the form mapping that assigns to each point x ∈ IRn the subspace of Tx (IRn ) defined m as ẋ = f (x) + g j (x)uj , (D.9) Δ(x) = span{g 1 (x), … , g m (x)}. (D.8) j=1
Often, a shorthand notation is used: that is called affine in the inputs uj . The state x takes values in IRn , while each component uj of the control input u ∈ IRm takes values in the class U Δ = span{g 1 , … , g m }. of piecewise-constant functions. The distribution Δ is nonsingular if dim Δ(x) = r, with r constant for all Denote by x(t, 0, x0 , u) the solution of (D.9) at time t ≥ 0, corresponding x. In this case, r is called the dimension of the distribution. Moreover, Δ is to an input u: [0, t] → U and an initial condition x(0) = x0 . Such a solution called involutive if it is closed under the Lie bracket operation: exists and is unique provided that the drift vector field f and the input vector fields g j are of class C ∞ . System (D.9) is said to be controllable if, for any [g i , g j ] ∈ Δ ∀ g i , g j ∈ Δ. choice of x1 , x2 in IRn , there exists a time instant T and an input u: [0, T ] → U such that x(T, 0, x1 , u) = x2 . The involutive closure Δ̄ of a distribution Δ is its closure under the Lie bracket The accessibility algebra A of system (D.9) is the smallest subalgebra of operation. Hence, Δ is involutive if and only if Δ̄ = Δ. Note that the distri- V(IRn ) that contains f , g 1 , … , g m . By definition, all the Lie brackets that can bution Δ = span{g} associated with a single vector field is always involutive, be generated using these vector fields belong to A. The accessibility distribu- because g, g = 0. tion ΔA of system (D.9) is defined as
ΔA = span{v|v ∈ A}. (D.10)
Example D.2 In other words, ΔA is the involutive closure of Δ = span{f , g 1 , … , g m }. The computation of ΔA may be organized as an iterative procedure The distribution & ‘2 ΔA = span {v|v ∈ Δi , ∀i ≥ 1} , cos x3 0 Δ = span{g 1 , g 2 } = span sin x3 , 0 with 0 1 Δ1 = Δ = span{f , g 1 , … , g m } is nonsingular and has dimension 2. It is not involutive, because the Lie bracket Δi = Δi−1 + span{[g, v]| g ∈ Δ1 , v ∈ Δi−1 }, i ≥ 2. sin x3 g 1 , g 2 = −cos x3 This procedure stops after κ steps, where κ is the smallest integer such that 0 Δκ+1 = Δκ = ΔA . This number is called the nonholonomy degree of the is always linearly independent of g 1 (x) and g 2 (x). Its involutive closure is therefore system and is related to the ‘level’ of Lie brackets that must be included in ΔA . Since dim ΔA ≤ n, it is κ ≤ n − m necessarily. Δ̄ = span{g 1 , g 2 , [g 1 , g 2 ]}. If system (D.9) is driftless m ẋ = g i (x)ui , (D.11) i=1
the accessibility distribution ΔA associated with vector fields g 1 , . . . , g m char-
acterizes its controllability. In particular, system (D.11) is controllable if and
only if the following accessibility rank condition holds:
dim ΔA (x) = n. (D.12)
604 D Differential Geometry
Note that for driftless systems the iterative procedure for building ΔA starts with Δ1 = Δ = span{g 1 , … , g m }, and therefore κ ≤ n − m + 1. For systems in the general form (D.9), condition (D.12) is only necessary for controllability. There are, however, two notable exceptions:
• If system (D.11) is controllable, the system with drift obtained by per- Index forming a dynamic extension of (D.11)
m
ẋ = g i (x)vi (D.13)
i=1
v̇i = ui , i = 1, . . . , m, (D.14)
i.e., by adding an integrator on each input channel, is also controllable. • For a linear system acceleration anthropomorphic, 73, 96, 114 m feedback, 317 anthropomorphic with spherical ẋ = Ax + bj uj = Ax + Bu gravity, 255, 583 wrist, 77 j=1 joint, 141, 256 parallelogram, 70 (D.12) becomes link, 285 singularity, 119 accessibility spherical, 72, 95 ([ B AB A2 B … An−1 B ]) = n, (D.15) loss, 471, 476 three-link planar, 69, 91, 113 rank condition, 477, 603 automation i.e., the well-known necessary and sufficient condition for controllability accuracy, 87 flexible, 17 actuator, 3, 191 industrial, 24 due to Kalman. algorithm programmable, 16 A , 607 rigid, 16 best-first, 552 axis Bibliography complete, 535 and angle, 52 complexity, 605 central, 582 The concepts briefly recalled in this appendix can be studied in detail in inverse kinematics, 132, 143 joint, 62 various tests of differential geometry [94, 20] and nonlinear control theory [104, pose estimation, 427 principal, 582 168, 195]. probabilistically complete, 543 Barbalat randomized best-first, 553 lemma, 507, 512, 513, 598 resolution complete, 540 bicycle search, 606 chained-form transformation, 485 steepest descent, 551 flat outputs, 491 sweep line, 536 front-wheel drive, 481 sweep plane, 539 rear-wheel drive, 481 wavefront expansion, 554 angle calibration and axis, 52, 139, 187 camera, 229, 440 Euler, 48 kinematic, 88 architecture matrix, 217 control, 233, 237 camera functional, 233 calibration, 440 hardware, 242 eye-in-hand, 409 arm eye-to-hand, 409 624 Index Index 625
fixed configuration, 409 architecture, 233, 237 degree pose, 58, 184 hybrid configuration, 409 centralized, 327 nonholonomy, 603 position, 184 mobile configuration, 409 comparison among schemes, 349, 453 of freedom, 4, 585 energy pan-tilt, 410 compliance, 364, 367 Denavit–Hartenberg conservation, 588 cell decomposition decentralized, 309 convention, 61 conservation principle, 259 approximate, 539 force, 378 parameters, 63, 69, 71, 72, 74, 75, 78, kinetic, 249 exact, 536 force with inner position loop, 379 79 potential, 255, 585 chained form, 482 force with inner velocity loop, 380 differential flatness, 491 environment flat outputs, 492 hybrid force/motion, 396 displacement compliant, 389, 397 transformation, 483 hybrid force/position, 403 elementary, 366, 368, 581, 586 interaction, 363 Christoffel hybrid force/velocity, 398, 402 virtual, 385, 586 programming, 238 symbols, 258 impedance, 372 distribution rigid, 385, 401 collision checking, 532 independent joint, 311 accessibility, 603 structured, 15 compensation interaction, 363 dimension, 602 unstructured, 25 decentralized feedforward, 319 inverse dynamics, 330, 347, 372, 487, involutive, 602 epipolar feedforward, 593 594 involutive closure, 602 geometry, 433 feedforward computed torque, 324 inverse model, 594 disturbance line, 435 gravity, 328, 345, 368, 446, 449 Jacobian inverse, 344 compensation, 325 error compliance Jacobian transpose, 345 rejection, 207, 376, 590 estimation, 430 active, 367 joint space, 305 drive force, 378 control, 364, 367 electric, 198 joint space, 328 kinematic, 134 matrix, 366 hydraulic, 202 operational space, 132, 345, 367, 445 linear systems, 589 passive, 366 with gear, 204 orientation, 137 motion, 303 configuration, 470, 525, 585 dynamic extension, 487 position, 137 operational space, 343, 364 configuration space dynamic model tracking, 324 parallel force/position, 381 2R manipulator, 526 constrained mechanical system, 486 estimation PD with gravity compensation, 328, as a manifold, 527 joint space, 257 pose, 427 345, 368 distance, 527 linearity in the parameters, 259 Euler PI, 311, 322, 380, 591 free, 528 notable properties, 257 angles, 48, 137, 187 PID, 322, 591 free path, 528 operational space, 296 obstacles, 527 PIDD2 , 322 feedback parallelogram arm, 277 connectivity graph, 536, 537 points, 555 parameter identification, 280 nonlinear, 594 constraint position, 206, 312, 314, 317 reduced order, 402 position, 312 artificial, 391 resolved-velocity, 448 skew-symmetry of matrix Ḃ − 2C, position and velocity, 314 bilateral, 386, 585 robust, 333 257 position, velocity and acceleration, epipolar, 434 system, 3 two-link Cartesian arm, 264 317 frame, 391 unit vector, 337 two-link planar arm, 265 flat outputs, 491 holonomic, 385, 470, 585 velocity, 134, 314, 317, 502 dynamics force Jacobian, 385 vision-based, 408 direct, 298 active, 583, 586 kinematic, 471 voltage, 199 fundamental principles, 584 centrifugal, 256 natural, 391 controllability inverse, 298, 330, 347 conservative, 585, 587 nonholonomic, 469, 585 and nonholonomy, 477 contact, 364 Pfaffian, 471 condition, 477 encoder control, 378 pure rolling, 472 system, 603 absolute, 210 controlled subspace, 387 scleronomic, 585 coordinate incremental, 212, 517 Coriolis, 257 unilateral, 386 generalized, 247, 296, 585 end-effector elementary work, 584 control homogeneous, 56, 418 force, 147 end-effector, 147 adaptive, 338 Lagrange, 585 frame, 59 error, 378 admittance, 377 transformation, 56 orientation, 187 external, 583, 584 626 Index Index 627
generalized, 248, 587 Hamilton anthropomorphic arm, 114 level gravity, 255, 583 principle of conservation of energy, computation, 111 action, 235 internal, 583 259 constraint, 385 gray, 410 nonconservative, 587 homography damped least-squares, 127 hierarchical, 234 reaction, 385, 583, 586 planar, 420, 438 geometric, 105 primitive, 236 resultant, 583 image, 424 servo, 236 transformation, 151 identification inverse, 133, 344 task, 235 form dynamic parameters, 280 pseudo-inverse, 133 Lie bilinear, 574 kinematic parameters, 88 Stanford manipulator, 115 bracket, 600 negative definite, 574 image three-link planar arm, 113 derivative, 601 positive definite, 574 binary, 412 transpose, 134, 345 link quadratic, 574, 597 centroid, 416 joint acceleration, 285 frame feature parameters, 410 acceleration, 141, 256 centre of mass, 249 attached, 40 interpretation, 416 actuating system, 191 inertia, 251 base, 59 Jacobian, 424 axis, 62 velocity, 108 central, 582 moment, 416 prismatic, 4 local compliant, 377 processing, 410 revolute, 4 minima, 550, 551 constraint, 391 segmentation, 411 space, 84 planner, 542 current, 46 impedance torque, 147, 248 Lyapunov fixed, 46, 579 active, 373 variable, 58, 248 direct method, 596 moving, 579 control, 372 equation, 597 principal, 582 mechanical, 373 kinematic chain function, 135, 328, 335, 340, 341, 345, rotation, 40 passive, 374 closed, 4, 65, 151 368, 431, 446, 449, 452, 506, 513, friction inertia open, 4, 60 596 Coulomb, 257 first moment, 262 kinematics electric, 200 matrix, 254 anthropomorphic arm, 73 manipulability viscous, 257 moment, 262, 581 anthropomorphic arm with spherical dynamic, 299 Frobenius product, 582 wrist, 77 ellipsoid, 152 norm, 421 tensor, 251, 582 differential, 105 measure, 126, 153 theorem, 476 integrability direct, 58 manipulability ellipsoid function multiple kinematic constraints, 475, DLR manipulator, 79 dynamic, 299 gradient, 569 477 humanoid manipulator, 81 force, 156 Hamiltonian, 588 single kinematic constraint, 473 inverse, 90 velocity, 153 Lagrangian, 588 interaction inverse differential, 123 manipulator Lyapunov, 596 control, 363 parallelogram arm, 70 anthropomorphic, 8 environment, 363 spherical arm, 72, 95 Cartesian, 4 gear matrix, 424 spherical wrist, 75 cylindrical, 5 reduction ratio, 205, 306 inverse kinematics Stanford manipulator, 76 DLR, 79 generator algorithm, 132 three-link planar arm, 69 end-effector, 4 torque-controlled, 200, 309 anthropomorphic arm, 96 kineto-statics duality, 148 humanoid, 81 velocity-controlled, 200, 309 comparison among algorithms, 143 joint, 58 graph search, 606 manipulator with spherical wrist, 94 La Salle joints, 4 A , 607 second-order algorithm, 141 theorem, 507, 597 link, 58 breadth-first, 606 spherical arm, 95 Lagrange links, 4 depth-first, 606 spherical wrist, 99 coordinates, 585 mechanical structure, 4 gravity three-link planar arm, 91 equations, 587 mobile, 14 acceleration, 255, 583 formulation, 247, 292 parallel, 9 compensation, 328, 345, 368, 446, 449 Jacobian function, 588 posture, 58 force, 255, 583 analytical, 128 multipliers, 124, 485 redundant, 4, 87, 124, 134, 142, 296 628 Index Index 629
SCARA, 7 symmetric, 251, 255, 564 hydraulic, 193 sequence, 170, 172, 175 spherical, 6 trace, 565 pneumatic, 193 Pontryagin Stanford, 76, 115 transpose, 564 minimum principle, 499 with spherical wrist, 94 triangular, 563 navigation function, 553 pose wrist, 4 mobile robot Newton–Euler estimation, 418 matrix car-like, 13, 482 equations, 584 regulation, 345 adjoint, 567 control, 502 formulation, 282, 292 rigid body, 39 algebraic complement, 565 differential drive, 12, 479 recursive algorithm, 286 position block-partitioned, 564 dynamic model, 486 nonholonomy, 469 control, 206, 312 calibration, 217, 229 kinematic model, 476 end-effector, 184 compliance, 366 legged, 11 octree, 541 feedback, 312, 314, 317 condition number, 577 mechanical structure, 10 odometric localization, 514 rigid body, 39 damped least-squares, 127 omnidirectional, 13 operational trajectory, 184 damped least-squares inverse, 282 path planning, 492 space, 84, 445 transducer, 210 derivative, 568 planning, 489 operator posture determinant, 566 second-order kinematic model, 488 Laplacian, 415 manipulator, 58 diagonal, 564 synchro drive, 12, 479 Roberts, 414 regulation, 328, 503, 512 eigenvalues, 573 trajectory planning, 498 Sobel, 414 potential eigenvectors, 573 tricycle-like, 12, 482 orientation artificial, 546 essential, 434 wheeled, 10, 469 absolute, 436 attractive, 546 homogeneous transformation, 56 moment end-effector, 187 repulsive, 547 idempotent, 568 image, 416 error, 137 total, 549 identity, 564 inertia, 262, 581 minimal representation, 49 power inertia, 254 inertia first, 262 rigid body, 40 amplifier, 197 interaction, 424 resultant, 583 trajectory, 187 supply, 198 inverse, 567 motion principle Jacobian, 569 constrained, 363, 384 parameters conservation of energy, 259 left pseudo-inverse, 90, 281, 386, 428, control, 303 Denavit–Hartenberg, 63 virtual work, 147, 385, 587 431, 452, 576 equations, 255 dynamic, 259 PRM (Probabilistic Roadmap), 541 minor, 566 internal, 296 extrinsic, 229, 440 programming negative definite, 574 planning, 523 intrinsic, 229, 440 environment, 238 negative semi-definite, 575 point-to-point, 163 uncertainty, 332, 444 language, 238 norm, 572 primitives, 545 path object-oriented, 242 null, 564 through a sequence of points, 168 circular, 183 robot-oriented, 241 operations, 565 motion planning geometrically admissible, 490 teaching-by-showing, 240 orthogonal, 568, 579 canonical problem, 523 minimum, 607 positive definite, 255, 574, 582 multiple-query, 535 primitive, 181 quadtree, 540 positive semi-definite, 575 off-line, 524 rectilinear, 182 product, 566 on-line, 524 plane range product of scalar by, 565 probabilistic, 541 epipolar, 435 sensor, 219 projection, 389, 572 query, 535 osculating, 181 reciprocity, 387 right pseudo-inverse, 125, 299, 576 reactive, 551 points redundancy rotation, 40, 579 sampling-based, 541 feature, 417 kinematic, 121 selection, 389 single-query, 543 path, 169 analysis, 121 singular value decomposition, 577 via artificial potentials, 546 via, 186, 539 kinematic, 87 skew-symmetric, 257, 564 via cell decomposition, 536 virtual, 173 resolution, 123, 298 square, 563 via retraction, 532 polynomial Reeds–Shepp stiffness, 366 motor cubic, 164, 169 curves, 501 sum, 565 electric, 193 interpolating, 169 regulation 630 Index Index 631
Cartesian, 511 image, 411 statics, 147, 587 dynamic model, 488 discontinuous and/or time-varying, sensor Steiner flat outputs, 491 514 exteroceptive, 3, 215, 517 theorem, 260, 582 kinematic model, 478 pose, 345 laser, 222 stiffness minimum-time trajectories, 500 posture, 328, 503, 512 proprioceptive, 3, 209, 516 matrix, 366 optimal trajectories, 499 Remote Centre of Compliance (RCC), range, 219 second-order kinematic model, 489 366 shaft torque, 216 tachometer, 214 unit quaternion, 54, 140 resolver, 213 sonar, 219 torque unit vector retraction, 534 vision, 225 actuating, 257 approach, 59 rigid body wrist force, 216 computed, 324 binormal, 181 angular momentum, 583 servomotor controlled generator, 200 control, 337 angular velocity, 580 brushless DC, 194 driving, 199, 203 normal, 59, 181 inertia moment, 581 electric, 193 friction, 257 sliding, 59 inertia product, 582 hydraulic, 195 joint, 147, 248 tangent, 181 inertia tensor, 582 permanent-magnet DC, 194 limit, 294 kinematics, 579 simulation reaction, 199 vector linear momentum, 583 force control, 382 sensor, 216 basis, 570 mass, 581 hybrid visual servoing, 464 tracking bound, 580 orientation, 40 impedance control, 376 error, 504 column, 563 pose, 39, 580 inverse dynamics, 269 reference, 590 components, 570 position, 39 inverse kinematics algorithms, 143 trajectory, 503, 595 feature, 418 potential energy, 585 motion control schemes, 349 via input/output linearization, 507 field, 599 roadmap, 532 pose estimation, 432 via linear control, 505 homogeneous representation, 56 robot regulation for mobile robots, 514 via nonlinear control, 506 linear independence, 569 applications, 18 trajectory tracking for mobile robots, trajectory norm, 570 field, 26 508 dynamic scaling, 294 null, 564 industrial, 17 visual control schemes, 453 joint space, 162 operations, 569 manipulator, 4 visual servoing, 453 operational space, 179 product, 571 mobile, 10 singularity orientation, 187 product of scalar by, 570 origin, 1 arm, 119 planning, 161, 179 representation, 42 service, 27 classification, 116 position, 184 rotation, 44 robotics decoupling, 117 tracking, 503 scalar product, 570 advanced, 25 kinematic, 116, 127 transducer scalar triple product, 571 definition, 2 representation, 130 position, 210 space, 570 fundamental laws, 2 wrist, 119 velocity, 214 subspace, 570 industrial, 15 space transformation sum, 570 rotation configuration, 470 coordinate, 56 unit, 571 elementary, 41 joint, 83, 84, 162 force, 151 velocity instantaneous centre, 480 null, 122, 149 homogeneous, 56 controlled generator, 200 matrix, 40, 579 operational, 83, 84, 296, 343 linear, 572 controlled subspace, 387 vector, 44 projection, 572 matrix, 56 feedback, 314, 317 rotation matrix range, 122, 149, 572 perspective, 227 link, 108 composition, 45 vector, 570 similarity, 573 transducer, 214 derivative, 106 work, 85 velocity, 149 transformation, 149 RRT (Rapidly-exploring Random Tree), special group transmission, 192 trapezoidal profile, 165 543 Euclidean, 57, 580 triangulation, 435 triangular profile, 167 orthonormal, 41, 49, 579 vision segmentation stability, 133, 135, 141, 328, 368, 446, unicycle sensor, 225 binary, 412 447, 452, 590, 595, 596 chained-form transformation, 484 stereo, 409, 433 632 Index
visual servoing fixed, 11 hybrid, 460 Mecanum, 13 image-based, 449 steerable, 11 PD with gravity compensation, 446, work 449 elementary, 584 position-based, 445 virtual, 147, 385, 586 resolved-velocity, 447, 451 workspace, 4, 14 Voronoi wrist generalized diagram, 533 force sensor, 216 wheel singularity, 119 caster, 11 spherical, 75, 99