Belajar Dynamic 2

7/29/2019 Belajar Dynamic 2

1/174

UNIVERSITY OF CALIFORNIA

IRVINE

Dynamic Motion Planning For Robot

Manipulators Using B-Splines

DISSERTATION

submitted in partial satisfaction of the requirements for the degree of

DOCTOR OF PHILOSOPHY

in Engineering

by

Chia-Yu Eric Wang

Dissertation Committee:

Professor James E. Bobrow, Chair

Professor Athanasios SiderisProfessor David J. Reinkensmeyer

2001


2/174

c2001 by Chia-Yu Eric Wang


3/174

The dissertation of Chia-Yu Eric Wang is approved,

and is acceptable in quality and form

for publication on microfilm:

Committee Chair

University of California, Irvine

2001

ii


4/174

In Memory Of My Best Friend,

Melissa Wang.

iii


5/174

Contents

List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . vii

List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xii

Acknowledgements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiii

Curriculum Vitae . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xv

Abstract of the Dissertation . . . . . . . . . . . . . . . . . . . . . . . . . . xvii

Chapter 1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.2.1 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21.2.2 Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.2.3 System Identification . . . . . . . . . . . . . . . . . . . . . . . 41.2.4 Dynamic Motion Planning for Robots . . . . . . . . . . . . . . 4

1.2.5 Robotic Body Weight Supported (BWS) Training . . . . . . . 61.3 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81.4 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

Chapter 2 Kinematics of Robot Manipulators . . . . . . . . . . . . 112.1 Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

2.1.1 Lie Groups and Lie Algebras . . . . . . . . . . . . . . . . . . . 122.1.2 Adjoint Operators . . . . . . . . . . . . . . . . . . . . . . . . 152.1.3 Forward Kinematics . . . . . . . . . . . . . . . . . . . . . . . 172.1.4 Derivatives of the Adjoint Mapping . . . . . . . . . . . . . . . 18

2.2 Differential Kinematics . . . . . . . . . . . . . . . . . . . . . . . . . . 192.2.1 The Manipulator Jacobian . . . . . . . . . . . . . . . . . . . . 20

2.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

Chapter 3 Dynamics of Robot Manipulators . . . . . . . . . . . . . 253.1 Open-Chained Systems . . . . . . . . . . . . . . . . . . . . . . . . . . 25

3.1.1 Inverse Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 263.1.2 Forward Dynamics . . . . . . . . . . . . . . . . . . . . . . . . 28

iv


6/174

3.1.3 Hybrid Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 303.2 Closed-Chained Systems . . . . . . . . . . . . . . . . . . . . . . . . . 32

3.2.1 Lathrops Algorithm . . . . . . . . . . . . . . . . . . . . . . . 353.3 Derivatives of Dynamics . . . . . . . . . . . . . . . . . . . . . . . . . 37

3.4 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

Chapter 4 B-Spline Curves . . . . . . . . . . . . . . . . . . . . . . . . 404.1 Construction of B-Splines . . . . . . . . . . . . . . . . . . . . . . . . 40

4.1.1 B-Spline Basis Functions . . . . . . . . . . . . . . . . . . . . . 424.2 Examples of B-Splines . . . . . . . . . . . . . . . . . . . . . . . . . . 454.3 Derivatives of B-Splines . . . . . . . . . . . . . . . . . . . . . . . . . 474.4 Uniform Quintic B-Spline Curves . . . . . . . . . . . . . . . . . . . . 49

Chapter 5 Dynamic Motion Planning for Open-Chained Robots . 535.1 Optimization Problems for Robots . . . . . . . . . . . . . . . . . . . 54

5.2 Identification of Mass Properties . . . . . . . . . . . . . . . . . . . . . 565.2.1 Application: PUMA 762 . . . . . . . . . . . . . . . . . . . . . 59

5.3 Weightlifting Motion Planning . . . . . . . . . . . . . . . . . . . . . . 625.3.1 Optimization Results . . . . . . . . . . . . . . . . . . . . . . . 655.3.2 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 695.3.3 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72

Chapter 6 Dynamic Motion Planning for the Design of Robotic GaitRehabilitation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74

6.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 746.2 Methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77

6.2.1 Human Model and Walking Motion . . . . . . . . . . . . . . . 776.2.2 Formulation of the Optimal Control Problem . . . . . . . . . . 816.2.3 Dynamic Motion Optimization via Direct Parameter Optimization 88

6.3 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 896.3.1 Case 1: Paralyzed Swing Leg with Motion Captured Stance Hip

Orientation (No Optimization) . . . . . . . . . . . . . . . . . 916.3.2 Case 2: Unimpaired Swing Leg with Effort Minimization of All

Joints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 926.3.3 Case 3: Paralyzed Swing Leg with Effort Minimization of the

Stance Hip Torques . . . . . . . . . . . . . . . . . . . . . . . . 96

6.3.4 Case 4: Paralyzed Swing Leg with Effort Minimization of theStance Hip Torques and Bounded Stance Hip Orientation . . . 97

6.3.5 Comparison of Cases . . . . . . . . . . . . . . . . . . . . . . . 1006.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105

Chapter 7 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . 1077.1 Research Summary and Conclusions . . . . . . . . . . . . . . . . . . . 1087.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 109

v


7/174

Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 110

Appendix A Transformation Between Joints and Motion CaptureMarkers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118

Appendix B Estimation of Dynamical Properties Using RegressionEquations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121

Appendix C Results for Gait Rehabilitation Case 1: ParalyzedSwing Leg with Motion Captured Stance Hip Orientation . . . . . . . 122

Appendix D Results for Gait Rehabilitation Case 2: UnimpairedSwing Leg with Effort Minimization of All Joints . . . . . . . . . . . . 126

Appendix E Results for Gait Rehabilitation Case 3: ParalyzedSwing Leg with Effort Minimization of the Stance Hip Torques . . . 136

Appendix F Results for Gait Rehabilitation Case 4: ParalyzedSwing Leg with Effort Minimization of the Stance Hip Torques andBounded Stance Hip Orientation . . . . . . . . . . . . . . . . . . . . . . . 146

vi


8/174

List of Figures

1.1 Body weight supported (BWS) step training following spinal cord in-jury. Two therapists assist in leg movement, while a third assists intorso movement. From Behrman and Harkema (2000). . . . . . . . . 7

4.1 A cubic B-spline curve (solid line) constructed on the basis functions(dashed lines). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

4.2 Local control of the B-spline curves. . . . . . . . . . . . . . . . . . . . 41

4.3 A quadratic B-spline produced from the recurrence. . . . . . . . . . . 444.4 A step-function B-spline curve with the control points {1.5, 1, 2, 1}

over the knots {2, 4, 5, 7.5, 9}. . . . . . . . . . . . . . . . . . . . . . . 454.5 A piecewise linear B-spline curve with the control points {2, 1, 3, 0, 1.5}

over the knots {1, 2.5, 3.5, 6, 6, 7.5, 10}. . . . . . . . . . . . . . . . . . 47

5.1 The home position of the PUMA with all the joint positions = 0. . . 595.2 The initial (left) and final (right) configurations of the puma. . . . . . 665.3 The motion, speed and torque of the 4th joint in the 1 DOF case.

(Limits: dotted lines) . . . . . . . . . . . . . . . . . . . . . . . . . . . 685.4 The motion of the wrist in the 3 DOF case. . . . . . . . . . . . . . . 69

5.5 The motion for the 6 DOF case. . . . . . . . . . . . . . . . . . . . . . 70

6.1 Body weight supported (BWS) step training following spinal cord in-jury. Two therapists assist in leg movement, while a third assists intorso movement. From Behrman and Harkema (2000). . . . . . . . . 75

6.2 One-half of the gait cycle (duration 0.467 sec). . . . . . . . . . . . . . 776.3 Muscle system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 826.4 One-half of the gait cycle (duration 0.433 sec). . . . . . . . . . . . . . 836.5 One-half of the gait cycle (duration 0.467 sec). . . . . . . . . . . . . . 836.6 One-half of the gait cycle (duration 0.5 sec). . . . . . . . . . . . . . . 84

6.7 One-half of the gait cycle (duration 0.567 sec). . . . . . . . . . . . . . 846.8 The resulting gait (duration 0.5 sec) for Case 1: Paralyzed Swing Legwith Motion Captured Stance Hip Orientation (No Optimization). Thesolid lines show the resulting gait and the dashed lines are the gaitrecorded from the motion capture system.) . . . . . . . . . . . . . . . 91

6.9 The resulting gait (duration 0.5 sec) for Case 2: Unimpaired SwingLeg. The solid lines show the resulting gait and the dashed lines arethe gait recorded from the motion capture system.) . . . . . . . . . . 93

vii


9/174

6.10 The resulting joint motions (duration 0.5 sec) for Case 2: UnimpairedSwing Leg. The solid lines show the resulting joint motions and thedashed lines are the joint motions recorded from the motion capturesystem.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94

6.11 The resulting joint torques (duration 0.5 sec) for Case 2: UnimpairedSwing Leg. The solid lines show the resulting joint torques and thedashed lines are the joint torques corresponding to the motion capturedata.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 95

6.12 The resulting gait (duration 0.5 sec) for Case 3: Paralyzed Swing Leg.The solid lines show the resulting gait and the dashed lines are the gaitrecorded from the motion capture system.) . . . . . . . . . . . . . . . 98

6.13 The resulting joint motions (duration 0.5 sec) for Case 3: ParalyzedSwing Leg. The solid lines show the resulting joint motions and thedashed lines are the joint motions recorded from the motion capturesystem.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99

6.14 The resulting joint torques (duration 0.5 sec) for Case 3: ParalyzedSwing Leg. The solid lines show the resulting joint torques and thedashed lines are the joint torques corresponding to the motion capturedata.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 100

6.15 The resulting gait (duration 0.5 sec) for Case 4: Paralyzed Swing Legwith Bounded Stance Hip Orientation. The solid lines show the result-ing gait and the dashed lines are the gait recorded from the motioncapture system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101

6.16 The resulting joint motions (duration 0.5 sec) for Case 4: ParalyzedSwing Leg with Bounded Stance Hip Orientation. The solid lines show

the resulting joint motions and the dashed lines are the joint motionsrecorded from the motion capture system.) . . . . . . . . . . . . . . . 102

6.17 The resulting joint torques (duration 0.5 sec) for Case 4: ParalyzedSwing Leg with Bounded Stance Hip Orientation. The solid lines showthe resulting joint torques and the dashed lines are the joint torquescorresponding to the motion capture data.) . . . . . . . . . . . . . . . 103

6.18 Applied effort for step duration 0.5 sec. (The , , and marks represent Case 1, 2, 3 and 4, respectively.) . . . . . . . . . . . 103

6.19 Root-Mean-Square of position error. (The , and marksrepresent Case 2, 3 and 4, respectively.) . . . . . . . . . . . . . . . . . 104

6.20 Final position error. (The , and marks represent Case 2,3 and 4, respectively.) . . . . . . . . . . . . . . . . . . . . . . . . . . . 104

C.1 The resulting gait (duration 0.433 sec) for Case 1: Paralyzed SwingLeg with Motion Captured Stance Hip Orientation (No Optimization).The solid lines show the resulting gait and the dashed lines are the gaitrecorded from the motion capture system.) . . . . . . . . . . . . . . . 123

viii


10/174



D.1 The resulting gait (duration 0.433 sec) for Case 2: Unimpaired SwingLeg. The solid lines show the resulting gait and the dashed lines arethe gait recorded from the motion capture system.) . . . . . . . . . . 127



D.4 The resulting joint motions (duration 0.433 sec) for Case 2: UnimpairedSwing Leg. The solid lines show the resulting joint motions and thedashed lines are the joint motions recorded from the motion capturesystem.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 130

D.5 The resulting joint torques (duration 0.433 sec) for Case 2: UnimpairedSwing Leg. The solid lines show the resulting joint torques and thedashed lines are the joint torques corresponding to the motion capture

data.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131D.6 The resulting joint motions (duration 0.467 sec) for Case 2: Unimpaired

Swing Leg. The solid lines show the resulting joint motions and thedashed lines are the joint motions recorded from the motion capturesystem.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132

D.7 The resulting joint torques (duration 0.467 sec) for Case 2: UnimpairedSwing Leg. The solid lines show the resulting joint torques and thedashed lines are the joint torques corresponding to the motion capturedata.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133

D.8 The resulting joint motions (duration 0.467 sec) for Case 2: UnimpairedSwing Leg. The solid lines show the resulting joint motions and thedashed lines are the joint motions recorded from the motion capturesystem.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134

D.9 The resulting joint torques (duration 0.567 sec) for Case 2: UnimpairedSwing Leg. The solid lines show the resulting joint torques and thedashed lines are the joint torques corresponding to the motion capturedata.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135

ix


11/174

E.1 The resulting gait (duration 0.433 sec) for Case 3: Paralyzed SwingLeg. The solid lines show the resulting gait and the dashed lines arethe gait recorded from the motion capture system.) . . . . . . . . . . 137

E.2 The resulting gait (duration 0.467 sec) for Case 3: Paralyzed Swing

Leg. The solid lines show the resulting gait and the dashed lines arethe gait recorded from the motion capture system.) . . . . . . . . . . 138E.3 The resulting gait (duration 0.567 sec) for Case 3: Paralyzed Swing

Leg. The solid lines show the resulting gait and the dashed lines arethe gait recorded from the motion capture system.) . . . . . . . . . . 139

E.4 The resulting joint motions (duration 0.433 sec) for Case 3: ParalyzedSwing Leg. The solid lines show the resulting joint motions and thedashed lines are the joint motions recorded from the motion capturesystem.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140

E.5 The resulting joint torques (duration 0.433 sec) for Case 3: ParalyzedSwing Leg. The solid lines show the resulting joint torques and thedashed lines are the joint torques corresponding to the motion capturedata.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141





F.1 The resulting gait (duration 0.433 sec) for Case 4: Paralyzed SwingLeg with Bounded Stance Hip Orientation. The solid lines show the re-sulting gait and the dashed lines are the gait recorded from the motioncapture system.) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147


x


12/174


F.4 The resulting joint motions (duration 0.433 sec) for Case 4: ParalyzedSwing Leg with Bounded Stance Hip Orientation. The solid lines showthe resulting joint motions and the dashed lines are the joint motionsrecorded from the motion capture system.) . . . . . . . . . . . . . . . 150

F.5 The resulting joint torques (duration 0.433 sec) for Case 4: ParalyzedSwing Leg with Bounded Stance Hip Orientation. The solid lines showthe resulting joint torques and the dashed lines are the joint torquescorresponding to the motion capture data.) . . . . . . . . . . . . . . . 151


F.7 The resulting joint torques (duration 0.467 sec) for Case 4: ParalyzedSwing Leg with Bounded Stance Hip Orientation. The solid lines showthe resulting joint torques and the dashed lines are the joint torquescorresponding to the motion capture data.) . . . . . . . . . . . . . . . 153


F.9 The resulting joint torques (duration 0.567 sec) for Case 4: Paralyzed

Swing Leg with Bounded Stance Hip Orientation. The solid lines showthe resulting joint torques and the dashed lines are the joint torquescorresponding to the motion capture data.) . . . . . . . . . . . . . . . 155

xi


13/174

List of Tables

5.1 The joint locations and associated rotation axes of the PUMA. . . . . 605.2 The physical properties without any prediction of mass and friction . 615.3 The physical properties with prediction of mass and friction . . . . . 625.4 The optimal results for three cases . . . . . . . . . . . . . . . . . . . 67

6.1 Link lengths . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 786.2 Dynamic properties of the human model (in the SI system of units) . 79

6.3 DOF of the human model. . . . . . . . . . . . . . . . . . . . . . . . . 856.4 Weighting coefficients in Case 2: Unimpaired Swing Leg . . . . . . . . 926.5 Weighting coefficients for swivel in Case 3: Paralyzed Swing Leg . . . 97

A.1 Joint width . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119

xii


14/174

Acknowledgements

At the moment I prepared the writing of this Acknowledgments I went through

my memory from the time I started my Masters to the time I completed my Ph.D. A

smile was on my face because of the pleasure during my graduate studies at University

of California, Irvine. I am grateful to many of those people who made the time of my

study so enjoyable.

First of all, I would like to thank my advisor, Professor James Bobrow, for his

guidance of my Ph.D. work. His knowledge in the physical sense of mathematical

formulas led my research to concentrate on practical applications using theoretical

concepts. Implementing robots to move with our algorithms is appealing to us and

has been the main stimulation for this work. I also thank him for reviewing this

dissertation and providing numerous valuable recommendations.

Professor David Reinkensmeyer has dedicated his research in the field of gait

rehabilitation. He was the person who came up with the idea of applying our algo-

rithms in the design of robots for gait rehabilitation. Much effort has been made and

many brilliant insights have been generated by him in this work. Without him this

work would not be done.

I would like to thank Professor Athanassios Sideris for serving on the committee

for my dissertation. I have taken several classes from him and his explicit instruction

helped me understand many fundamental concepts, especially in optimization. I

xiii


15/174

would also like to thank Professor Michael McCarthy for maintaining a unique and

wonderful environment for the people in the Robotics Lab and Professor William

Schmitendorf for introducing me to the study of research instead of the study of

course work.

Collaboration with the lab mates of the Robotics Lab, including Juanita Albro,

Butch Timoszyk, Craig Takahashi, Garett Sohl, Scott Ploen, Bryan Martin, Robert

Lienbach, Christian de Haro Carbonell, Eliot Shiosaki, Richard Srisamang, Tony

Nguyen, and David Pai, involved brainstorming and often resolved my confused

thoughts. I would like to express my thanks to Juanita Albro, Butch Timoszyk,Craig Takahashi, Garett Sohl and Alba Perez for their friendships especially in the

last three years of my Ph.D.

Many friends of mine have made my life beyond school cheerful by organizing

activities, including tennis-playing, golfing, swimming, hiking, and so on. Without

them, I would not be able to finish my Ph.D. in the first year (2001) of the new

millennium but the last year (2000) of the old millennium. It seemed that I might

graduate a year earlier. (I was just kidding.) Among them, I would like to express

my appreciation to Jie Yu, Ben Hsieh, Ying Chang and Yizhang Zhou for being my

close friends and for all they have done for me.

Finally, I would like to thank my parents, my brothers, and Mei-Ling Tao for

their love and support. This work is dedicated to them.

xiv


16/174

Curriculum Vitae

Chia-Yu Eric Wang

1989 B.S. in Mechanical Engineering,

National Cheng Kung University, Taiwan

1995 M.S. in Mechanical and Aerospace Engineering,


2001 Ph.D. in Engineering,


FIELD OF STUDY

Robot Control

PUBLICATIONS

1. V. Coimbatore, C-Y. E. Wang, R. Ganeshan, J. Heckel, A. Baskin, and S.

C-Y. Lu, CITYWORK: A Collaboration Maintenance Management Application,

International CIRP Design Seminar, California, Oct. 1997

2. C-Y. E. Wang, W. K. Timoszyk, and J. E. Bobrow, Weightlifting Motion Planning

for a Puma 762 Robot, IEEE International Conference on Robotics and Automation,

Michigan, May 1999

xv


17/174

3. R. Ganeshan, F. Grobler, C-Y. E. Wang, and V. Coimbatore, CITYWORK:

Application of Collaborative Technologies for Infrastructure Management, Journal

of Computing in Civil Engineering, v15-1, pp.74-80, Jan. 2001

4. C-Y. E. Wang, W. K. Timoszyk, and J. E. Bobrow, Payload Maximization for

Open Chained Manipulators: Finding Weightlifting Motions for a Puma 762 Robot,

IEEE Transactions on Robotics and Automation, v17-2, pp.218-224, Apr. 2001

5. C-Y. E. Wang, J. E. Bobrow, and D. J. Reinkensmeyer, Swinging From The Hip:

Use of Dynamic Motion Optimization in the Design of Robotic Gait Rehabilitation,

IEEE International Conference on Robotics and Automation, Korea, May 2001

EXPERIENCE

1992-1993 Mechanical Engineer,

San Shing Hardware Works Co., Ltd., Taiwan

1993-1994 Pro ject Leader,

Institute for Information Industry, Taiwan

1996-1997 Research Assistant,

Department of Mechanical Engineering,

University of Southern California

1994-2001 Research Assistant,

Department of Mechanical and Aerospace Engineering,


xvi


18/174

Abstract of the Dissertation

Dynamic Motion Planning For Robot Manipulators

Using B-Splines

by

Chia-Yu Eric Wang

Doctor of Philosophy in Engineering

University of California, Irvine, 2001

Professor James E. Bobrow, Chair

Although the dynamic equations of motion of open-chained robot systems are

well-known, they are seldom taken into account during the planning of motions. In

this work, we show that the dynamics of a robot can be taken into account in motion

planning. Two applications of dynamic motion planning are explored: 1) develop-

ment of point-to-point weightlifting motions for open-chained robots; and 2) design

of robotic gait rehabilitation. The governing optimal control problem is converted

into a direct, SQP parameter optimization in which the gradient is determined an-

alytically. The joint trajectories are defined by B-spline polynomials along with a

time-scale factor. In the first application, a weightlifting motion planner is applied

to a Puma 762 robot, with its physical limitations incorporated into the formula-

tion. The problem is formulated as an optimal control problem for a fully actuated

xvii


19/174

articulated chain. The torque limits are formulated as soft constraints added into

the objective function while the position and velocity limits are formulated as hard,

linear inequality constraints, on the parameters. The solutions obtained with the mo-

tion planner extend the robots payload capability while reducing the joint torques.

Interestingly, nearly all the trajectories found pass through singular configurations,

where large internal forces from the robot are applied to the payload and little torque

is needed from the motors. In the second application, we examine a method to con-

trol the stepping motion of a paralyzed person suspended on a treadmill using a

robot attached to the pelvis. A leg swing motion is created by moving the pelvis

without contact with the legs. The problem is formulated as an optimal control

problem for an underactuated articulated chain. Motion capture data recorded from

an unimpaired human subject is compared to the simulation results from the dy-

namic motion optimization. Our results suggest that it is feasible to create a gait

for a paralyzed person that is close to that of an unimpaired subject by control-

ling the pelvis with a robot. The resulting motions of the two applications can be

found at the web sites http://www.eng.uci.edu/chwang/project/puma762.html

and http://www.eng.uci.edu/chwang/project/stepper/stepper.html.

xviii


20/174

Chapter 1

Introduction

In this dissertation, we refer to dynamic motion planning as the study of robot

motion planning with the dynamics of the robot taken into account. The majority of

current robot motion planning algorithms, such as those found in well-known texts

(see, e.g. [32]), ignore the dynamics of the robot during the motion planning phase.

Our study demonstrates how the dynamics can be used in motion planning to improve

the performance and efficiency for robots and to design robots for gait rehabilitation.

1.1 Motivation

Robots have been used in automatic production lines for many decades. In

general, industrial robots are used to replace human workers in repetitive tasks. Most

industrial robots, such as the PUMA robot, have factory-established payload limits

which are small compared to their massive components. That draws our attention to

extending the payload capability of a robot to its maximum potential while taking

into account realistic constraints such as joint torque limits and velocity bounds. The

solution to the dynamic motion planning problem can also minimize joint torques for

a given load, thereby reducing wear on the robot.

1


21/174

As another important example of dynamic motion planning, we examine a

method to control the stepping motion of a paralyzed person suspended on a treadmill

using a robot attached to the pelvis. Clinical stepping motion training is currently

limited because the training is labor intensive. Multiple therapists are often required

to control the pelvis and legs. Implementing training with robotics is an attempt to

make the training less labor intensive, more consistent, and more widely accessible.

The objective of this study is to use dynamic motion planning to determine to what

extent repetitive stepping by a paralyzed patient can theoretically be generated by

assisting in pelvis motion alone.

1.2 Background

1.2.1 Kinematics

Kinematics of robot manipulators is the study of motion independent of theforces which cause it. Any robot can be described kinematically by giving the values

of four quantities for each link. These quantities are called the Denavit-Hartenberg

parameters [21]. Brockett [13] connects the theory of Lie groups and robot kinematics

by introducing the product-of-exponential (POE) equations for the forward kinemat-

ics. Although the matrix exponential needs six independent parameters to specify a

joint twist, it is more straight-forward to construct the joint twist directly by writing

down the direction of the joint axis. One of the most attractive features of the POE

is its use of two coordinate frames, the base frame and the tool frame. This property,

combined with the geometric significance of the twists, makes the POE representation

an attractive alternative to the use of Denavit-Hartenberg parameters.

2


22/174

1.2.2 Dynamics

Dynamics of robot manipulators is the study of the forces required to cause

motion. The problem of computing the forces required for a given motion is known

as the inverse dynamics problem. The Newton-Euler formulation is an efficient linear

recursive approach for inverse dynamics. The formulation is simplified by using a

spatial notation in Featherstone [23].

By reformulating the equations so that acceleration is computed as a function

of applied force, it becomes the forward dynamics. Perhaps the most familiar re-

cursive approach for open-chain forward dynamics is the articulated-body method

of Featherstone [23]. The concept of articulated-body inertias is also significant,

and it is the central feature of Featherstones method. The algorithm computes the

articulated-body inertia of each set of links of the robot manipulators in a recursive

manner from the tip to the base. A second recursion, starting at the base and working

out to the tip, calculates the applied force or acceleration at each joint. Featherstones

algorithm has also been implemented using the spatial operator algebra of Rodriguez

et al. [51, 52].

Linear recursive approaches for forward dynamics of closed chains have been

presented by Lathrop [31]. In [31], general endpoint constraints are propagated from

the tip down to the base, where two 6 6 inverses are necessary to match the propa-

gated tip constraints to the existing base constraints. A second recursion from base to

tip solves for the joint accelerations. By modifying Lathrops method, we developed

a formulation for hybrid dynamics to compute either acceleration or applied force for

each joint.

3


23/174

Using the POE, Park and Bobrow [41] and Park et al. [42] derive a geometric

version of the Newton-Euler recursive inverse dynamics similar to Featherstones, but

based on existing geometric concepts of Lie groups. Lie groups can also be used

to develop the recursive approaches for forward dynamics in Ploen [45] and hybrid

dynamics of open chains and closed chains in this dissertation.

1.2.3 System Identification

In this dissertation, we develop a method for estimating the mass, the location

of center of mass, and the moments of inertia of each link and the friction at each joint

of a robot during general manipulator movement. These dynamic system parameters

are factorized linearly using the geometric operators of Lie groups and Lie algebras.

The Lie group formulation of inverse dynamics is re-arranged in a linear matrix form

of the dynamic properties. The least squares method with a QR-decomposition is

used to identify the dynamic properties after exciting the robot and collecting the

data of joint positions, velocities, accelerations and applied forces.

1.2.4 Dynamic Motion Planning for Robots

Dynamic motion planning is a motion planning problem in which the dynam-

ics of the robot is taken into account. We assume that the desired motion can

be characterized as the solution to a general optimal control problem of the form

min(t) Jc = (q, q, tf) +tf0 L(q, q , , t)dt subject to the dynamic equations of motion

and bounds on position, velocity and torque. The specific cost functionals used in

this dissertation will be given in Chapter 5 and 6. The final time tf may be either

4


24/174

free or fixed in our formulation. Because of the importance of finding solutions to

optimal control problems of the above form, there are several numerical algorithms

and commercial software packages available to solve them. All of the competitive

approaches incorporate some form of Newton iteration on a parameterized version of

the problem. Because the problems are nonconvex, the Newton iteration will con-

verge to a local minimizer. See [10] for a review on progress made in this area since

the 1960s.

Algorithms used to solve optimal control problems are broadly classified as either

indirect methods or direct methods. Indirect methods explicitly solve the optimalityconditions stated in terms of the maximum principle [14], the adjoint equations, and

the transversality (boundary) conditions. These conditions can be derived using the

calculus of variations, where the first variation of the Hamiltonian function is set

to zero. A number of multiple shooting methods have been developed to solve the

two-point boundary value problem defined by the equations.

Although the indirect approach described above has been used successfully in

many applications, it has been replaced by direct methods in recent years. The

primary reasons are that the region of convergence is relatively small and that it is

difficult to incorporate path inequality constraints. We chose a direct approach for

the work reported in this dissertation. The basic idea is to parameterize the joint

trajectories, and to vary the parameters in a nonlinear optimization until a local

minimum which satisfies the constraints has been achieved. The joint trajectories

used in the motion planning problem can be represented in many ways. Among them

are C2splines and B-spline polynomials, which have been used by many robotics

researchers [15, 16, 1, 24, 35, 39, 44, 54]. B-spline polynomials provide local control

of the joint trajectory and they have the important convex hull property [6]. We

5


25/174

exploit the convex hull property by transforming the semi-infinite constraints that

arise in the problem formulation into finite dimensional ones. In order to vary the

total time taken for the motion, we have added a time-scale factor. This allows us

to solve a free-final-time problem in which the derivatives of the joint positions with

respect to the time-scale factor can be derived analytically.

By parameterizing the joint trajectories, the optimal control problem becomes a

parameter optimization one with the parameters being the time-scale factor and the

control points of the splines. In Chapter 5, we add another parameter that represents

the payload into the optimization in order to maximize it. Because the PUMA 762robot has bounded joint velocities, these semi-infinite constraints are included in our

formulation. Other researchers have included velocity constraints as penalty functions

[15, 39, 44], or have discretized them into a large number of inequality constraints

throughout the time interval [16], or have used a variable mesh approach [1] to handle

them. As mentioned above, our approach uses the convex hull property of B-splines

in order to transform these semi-infinite constraints into a set of linear inequalities

in the parameter space. This provides superior numerical stability since the semi-

infinite constraints on joint velocity are satisfied exactly with a small number linear

constraints on the parameters.

1.2.5 Robotic Body Weight Supported (BWS) Training

In the U.S. alone, over 700,000 people experience a stroke each year, and over

10,000 people experience a traumatic spinal cord injury. Impairment in walking ability

after such neurologic injuries is common. Recently, a new approach to locomotion

rehabilitation called body weight supported (BWS) training has shown promise in

6


26/174

Figure 1.1: Body weight supported (BWS) step training following spinal cord injury.Two therapists assist in leg movement, while a third assists in torso movement. FromBehrman and Harkema (2000).

improving locomotion after stroke and spinal cord injury [62]. The technique involves

suspending the patient in a harness above a treadmill in order to partially relieve the

weight of the body, and manually assisting the legs and pelvis in moving in a walking

pattern (Figure 1.1). Patients who receive this therapy can significantly increase their

independent walking ability and overground walking speed [5]. It is hypothesized

that the technique works in part by stimulating remaining force, position, and touch

sensors in the legs during stepping in a repetitive manner, and that residual circuits in

the nervous system learn from this sensor input to generate motor output appropriate

for stepping.

Several research groups are pursuing robotic implementations of BWS train-

ing in an attempt to make the training less labor intensive, more consistent, and

more widely accessible [8, 27, 50, 17]. Implementing BWS training with robotics is

7


27/174

also attractive because it could improve experimental control over the training, thus

providing a means to better understand and optimize its effects.

A difficulty in automating BWS training is that the required patterns of forces

at the pelvis and legs are unknown. For example, the relative importance of assisting

at the pelvis and legs is unclear. One approach toward determining the required

forces is to instrument the therapists hands with force and position transducers [8].

However, therapists are relatively limited in the forces that they can apply compared

to robots, and there is no guarantee that any given therapist has selected an optimal

solution. Current robotic devices for BWS have taken the simplified approach ofreplicating leg kinematics, and moving the torso in a gait, like up and down pattern

[17, 26].

In Chapter 6 we explore an alternate approach toward generating strategies

for assisting in gait training: dynamic motion planning. Dynamic motion planning

provides a formalized method for determining motions for underconstrained tasks,

and may reveal novel strategies for achieving the tasks. It has been used with success

to simulate human control over such activities as diving, jumping, and walking [2,

28, 40, 4]. We use dynamic motion planning to determine to what extent repetitive

stepping by a paralyzed person can be generated by assisting in pelvis motion alone.

1.3 Contributions

The following list summarizes the research contributions described in this dis-

sertation. Specifically, we:

8


28/174

Derived Lie group formulations of hybrid dynamics for both open-chained and

closed-chained robot manipulators.

Developed a system identification method, based on the Lie group formulation ofinverse dynamics, for estimating the dynamic properties of a robot manipulator.

Exploited the convex hull property of B-splines to transform the semi-infinite

constraints into a set of linear inequalities.

Solved the free-final time optimal problems by using a time factor with B-splines.

Derived a weightlifting motion planner for a PUMA 762 robot to extend its

payload capability beyond the limits established by the manufacturer.

Developed a motion planner for the design of a robot used in gait rehabilitation.

This motion planner can also generate leg swing motions similar to the human

gait during the swing phase.

1.4 Outline

This dissertation is organized as follows: in Chapter 2 and Chapter 3, we in-

troduce the essential concepts of kinematics, differential kinematics and dynamics,

applied particularly to robot manipulators. These concepts, incorporated with Lie

groups and Lie algebras, provide powerful tools for us to generate efficient mathemat-

ical model for robot dynamics.

A function approximation technique is addressed in Chapter 4. The technique

constructs curves using piecewise B-Spline basis functions multiplied by the corre-

sponding control points. Three important properties, 1) local control of curve shape,

9


29/174

2) invariance under affine transformation, and 3) convex hull, make the B-Spline curve

a great candidate to parameterize the joint trajectory in motion planning.

Two dynamic motion planning problems are explored in this dissertation: 1)

weightlifting motion planning for a PUMA 762 robot, and 2) motion planning in the

design of robotic gait rehabilitation. In Chapter 5, we develop a dynamic motion

planner for open-chained robots. An example of weightlifting motion planning for a

PUMA 762 robot is discussed. The robot is fully actuated and only inverse dynamics

is needed for computation of the joint torques. A system identification method is

explored in order to realize the dynamics information of a real robot.

The motion planner is used in Chapter 6 to assist in the design of a rehabilitation

robot that can generate gait motions for paralyzed persons. The system model is

underactuated with passive and active joints. The hybrid dynamics consisting inverse

dynamics and forward dynamics is required for the computation of the equations of

motion.

Conclusions and future works are described in Chapter 7.

10


30/174

Chapter 2

Kinematics of Robot Manipulators

Kinematics studies the motion without taking into account the forces which

cause it. In this chapter we will address the essential concepts of kinematics and

focus mainly on forward kinematics which computes the position and orientation of

the end-effector based on the configuration of the robot manipulator. Inverse kine-

matics, which finds the configuration to meet the given position and orientation of

the end-effector, will not be addressed. The product-of-exponential (POE) equations

will be introduced to connect the theory of Lie groups and robot forward kinemat-

ics. Differential kinematics will be discussed last which describes the relationship

between the velocity of the end-effector and the joint rates, and is represented as the

manipulator Jacobian.

2.1 Kinematics

A rigid body is a set of points with the property that the distance between

any two of them never varies. The robot manipulators studied in this dissertation

are defined as collections of rigid bodies, called rigid links, connected by joints. The

position of one link relative to another in a kinematic chain is defined mathemati-

cally by a coordinate transformation between reference frames attached to each body.

11


31/174

The Cartesian coordinates are used as natural coordinates for representing point lo-

cations in space, p = (x,y,z). This leads to the following formula for coordinate

transformation:

p = Rp + d (2.1)

where p is the position of a rigid point represented in one coordinate frame and

p is the position of the same point represented in another coordinate frame. The

transformation consists a rotation R and a translation d. Because (2.1) does not

satisfy f(p1 + p2) = f(p1) + f(p2), it is not a linear transformation. (It is an affine

transformation.)

A modification has been made to obtain a linear transformation. The modi-

fied transformation uses homogeneous coordinates, (p, 1) = (x,y,z, 1). Using these

coordinates yields a linear transformation (homogeneous transformation) on 4 p

1

=

R d

0 1

p

1

= T

p

1

. (2.2)

The matrix T is called homogeneous transformation matrix.

2.1.1 Lie Groups and Lie Algebras

A Lie Group is a differentiable manifold G such that the following two properties

are satisfied for A, B G:

Property 1: For the mapping f(A, B) = AB we require f(A, B) G and f

must be continuously differentiable.

Property 2: The inverse mapping A A1 must exist and be continuously

differentiable.

12


32/174

Rotation transformations in Euclidean space are accomplished by use of the

orthogonal 33 rotation matrix R. This matrix is an element of the special orthogonal

group on 3, SO(3), which is an example of Lie group and is defined as

SO(3) =

R|RRT = I, det(R) = +1

. (2.3)

The special Euclidean group, or SE(3), is a Lie group corresponding to the homoge-

neous transformation on 3:

SE(3) =

R d

0 1

|R SO(3), d 3

. (2.4)

Hence, in equation (2.2), T SE(3).

An important concept associated with each Lie group is the notion of a Lie

algebra. The tangent space at the identity element of G is the Lie algebra of G and

is denoted by a lowercase g. Associated with the Lie group SO(3) is the Lie algebra

so(3), which can be written in the 3 3 skew-symmetric matrix form

= () =

0 z y

z 0 x

y x 0

(2.5)

or in the 3 1 vector form

= () =

x

y

z

(2.6)

where the (wedge) operator and the (vee) operator are defined in Murray [38].

The Lie algebra associated with SE(3) is denoted by se(3), where g se(3) has the

4 4 matrix form

g = (g) =

g vg

0 0

(2.7)

13


33/174

or the 6 1 vector form

g = (g) =

g

vg

. (2.8)

The principal relationship between the Lie group SE(3) and its corresponding

Lie algebra se(3) is the exponential mapping, which maps se(3) onto SE(3), i.e. for

every G SE(3) there exists a g se(3) such that eg = G. The following explicit

formulas for the exponential mappings on so(3) and se(3) are derived in Park [43]

and Murray [38].

Lemma 2.1 Let so(3). Then

exp() = I+ +1

2!2 +

1

3!3 +

= I+sin

+

1 cos

22

is an element of SO(3), where = ||||.

Lemma 2.2 Let so(3) and v 3. Then

exp(

v0 0

) = exp() Av

0 1

is an element of SE(3), where

A = I +1 cos

2 +

sin

32 .

The matrix logarithm takes elements of the group and returns the associated element

of the algebra. It also has a closed-form expression:

Lemma 2.3 LetR SO(3) and d 3. Then

log(

R d

0 1

) =

A

1d

0 0

14


34/174

is an element of se(3), where

=

2sin (R RT)

A

1 = I 1

2 +2sin (1 + cos )

22 sin 2

and satisfies 1 2cos = T r(), || < . (Also, 2 = ||||2.)

A spatial velocity V is an element of se(3), which can be written either in a

vector from or in a matrix form, such as

V =

v

or V =

v

0 0

(2.9)

where is the angular velocity and v is the linear velocity. While spatial velocities are

regarded as elements ofse(3), spatial forces are regarded as inhabiting the associated

dual space se(3) and can be written as

F =

m

f

or F =

f m

0 0

(2.10)

where m is the moment and f is the force. The dual space se(3) is the space of

linear functionals on se(3), i.e. if V = (, v) se(3) and F = (m, f) se(3), then

F : se(3) is given by F(V) = FTV = mT + fTv.

2.1.2 Adjoint Operators

An element of a Lie group can be used as a linear mapping between its Lie

algebras via the adjoint representation. Given G = (R, d) SE(3) its adjoint map

AdG : se(3) se(3) acting on V = (, v) se(3) is given by

AdGV =

R 0

dR R

v

15


35/174

= (GV G1) =

R d

0 1

v

0 0

R

T RTd

0 1

. (2.11)

Physically, it describes how the spatial velocities transform under a change of reference

frame given by G. Similarly, an element of a Lie algebra can be applied as a linear

mapping between its Lie algebras via its adjoint representation, the Lie bracket. If

V1 = (1, v1) se(3), then its adjoint map adV1 : se(3) se(3) on V2 = (2, v2)

se(3) is defined as

adV1V2 =

1 0

v1 1

2

v2

= [V1, V2] = (V1V2 V2V1) (2.12)

which, physically, generalizes the standard cross product operation in se(3).

The dual operator AdG : se(3) se(3) is defined as follows:

AdGF =

R

T RTdT

0 RT

m

f

= (G1F G) =

R

T RTd

0 1

f m

0 0

R d

0 1

. (2.13)

Its matrix representation is given by the transpose of AdG. Physically, it describes

how the spatial forces transform under a change of reference frame given by G. The

dual operator adV : se(3) se(3) is defined as:

adVF =

T vT

0 T m

f

= (FV VF) . (2.14)

Similar to AdG, its matrix representation is given by the transpose ofadV. Physically,

it generalizes the standard cross product operation between se(3) and se(3) to se(3).

16


36/174

2.1.3 Forward Kinematics

If a reference frame is fixed at the tip of each link in an open kinematic chain,

then the element of SE(3) describing the position and orientation of frame i relative

to that of frame i 1 is Ti1,i = MieSiqi , where Mi SE(3), Si se(3), and qi

is the joint variable for link i. The frame attached at the end-effector is then related

to that of the base by the product

T0,n(q1, q2, , qn) = T0,1(q1)T1,2(q2) Tn1,n(qn)

= M1eSr1q1M2e

Sr2q2 MneSrnqn (2.15)

By a repeated application of the matrix identity MeSM1 = eMSM1

, one can obtain

T0,n(q1, q2, , qn) = eA1q1eA2q2 eAnqnM (2.16)

where A1 = M1S1M11 , A2 = (M1M2)S2(M1M2)

1, etc., and M = M1M2 Mn.

The configuration with qi = 0, i = 1 n, is called the home position. In the home

position, each joint screw Ai

is written in the base frame. The transformation M

relates the end-effector frame to the base frame as well. Alternatively, T0,n can also

be expressed as

T0,n(q1, q2, , qn) = MeB1q1eB2q2 eBnqn (2.17)

where Bi = M1AiM, i = 1 n, are the joint screws related to the end-effector

frame in the home position. Equations (2.16) and (2.17) are called the product-of-

exponentials (POE) kinematic equations in space and body coordinates, respectively.

The procedure for computing the forward kinematics of a robot manipulator in

space coordinates is described as follows:

Algorithm 2.1 Forward kinematics via POE in space coordinates:

17


37/174

1. Setq1 = q2 = = qn = 0 so that T0,n = M which is the home position.

2. In the home position identify the screw axis directionki and a point on the axis

pi.

3. Ai =

ki vi

0 0

where vi = pi ki + hiki; h is the pitch of the screw Ai.

4. T0,n = eA1q1eA2q2 eAnqnM.

In the next section, the derivatives of the adjoint mapping will be introduced.

It will be used for computing the derivatives of dynamics and, later, the analytical

gradient in optimization problems.

2.1.4 Derivatives of the Adjoint Mapping

The derivatives of the adjoint operators of Lie groups and Lie algebras are

described as follows:

Lemma 2.4 If the joint transformations are represented in local coordinates, Ti1,i =

MieSiqi, where Si is a screw in the coordinates of the local moving frame for link i,

for i1 i i2,

qiAdT1

i1,i2

= AdT1i,i2

adSiAdT1i1,i

. (2.18)

First, we see that

qiTi1,i2 = Ti1,i1Mie

SiqiSiTi,i2 = Ti1,iSiTi,i2 . (2.19)

Assume V se(3) and take derivative of the following equation with respect to qi:

AdT1i1,i2

V =

T1i1,i2V Ti1,i2

. (2.20)

18


38/174

One can obtain

qiAdT1

i1,i2

V =

T1i,i2 SiT1

i1,iV Ti1,i2 + T

1i1,i2

V Ti1,iSiTi,i2

=

T1i,i2(SiT1i1,iV Ti1,i T1i1,iV Ti1,iSi)Ti,i2

= AdT1i,i2

adSiAdT1i1,i

V . (2.21)

Then, writing the above equation in the 6 6 matrix form and removing V, equation

(2.18) is obtained. Similar calculation can be performed to get the following lemma:


MieSiqi, where Si is a screw in the coordinates of the local moving frame for link i,

for i1 i i2,

qiAd

T1i1,i2

= AdT1i1,i

adSiAd

T1i,i2

. (2.22)

The derivatives for the ad and ad operators can be easily obtained using the chain

rule and is therefore omitted.

2.2 Differential Kinematics

The mapping between the joint velocities and the end-effector velocities is de-

fined by the differential kinematics equation

Ve = e

ve

= Je(q)q (2.23)

where Ve is the spatial velocity of the end-effector; e and ve the angular and linear

velocities of the end-effector, respectively; q the generalized velocity of the robot

manipulator; and Je the Jacobian matrix. The matrix Je is called the geometric

19


39/174

Jacobian [19] of the manipulator since its computation follows a geometric procedure

that is based on computing the contribution of each joint velocity to the spatial

velocity of the end-effector.

Consider the forward kinematics map, T0,n : Q SE(3), in (2.15) for a robot

manipulator. A point p fixed in the end-effector can be represented in the base frame

as p

1

= T0,n

p

1

=

Re de

0 1

p

1

. (2.24)

The spatial velocity Vge of the end-effector written in the base frame can be mapped

from the joint velocities q by the geometric Jacobian Jge (q):

Vge =

ge

vge

=

(R

Te Re)

de

= Jge (q)q . (2.25)

Two variations of Jge can also be expressed in similar forms, depending on whether

the velocity of the point is measured in the base frame ( p) or in the end-effector

frame ( p).

2.2.1 The Manipulator Jacobian

In this section we introduce two Jacobian matrices, the spatial Jacobian and

the body Jacobian. They are similar to Jge , but are represented in different coordinate

frames. The spatial Jacobian is represented in the base frame while the body Jacobian

is represented in the end-effector frame.

20


40/174

The velocity of point p fixed in the end-effector, is obtained as follows by taking

the derivative of (2.24):

p

0

= T0,n p1

= T0,nT10,n p

1

=

Re de

0 0

R

Te R

Te de

0 1

p

1

=

ReR

Te de ReR

Te de

0 0

p

1

=

ge de

ge de

0 0

p

1

(2.26)

where T0,nT10,n is the map between the position and velocity of the end-effector in the

base frame. If the joints move along a path q(t) Q n, then the end-effector

traverses a path T0,n SE(3). The instantaneous spatial velocity of the end-effector

is given by the twist

Vse = (T0,nT10,n )

=

i

(T0,nqi

T10,n )qi

= Jse (q)q (2.27)

where Jse 6n is called the spatial Jacobian. The formulas for the derivative and

inverse of T0,n are:

T0,nqi

= M1eS1q1 Mie

SiqiSiMi+1eSi+1qi+1 Mne

Snqn (2.28)

T

10,n = e

SnqnM

1n e

Sn1qn1M

1n1 e

S1q1M

11 (2.29)

which produce the product

T0,nqi

T10,n = M1eS1q1 Mie

SiqiSieSiqiM1i e

S1q1M11

= T0,iSiT10,i =

AdT0,iSi

(2.30)

21


41/174

and the Jacobian can be expressed entirely in terms of the joint screws mapped into

the base frame

Jse (q) = [AdT0,1S1 AdT0,2S2 AdT0,nSn] . (2.31)

Each column of Jse (q) depends only on q1, q2, , qi1. In other words, the contribu-

tion of the ith joint velocity to the end-effector velocity is independent of the later

configuration in the manipulator.


MieSiqi , where Si is a screw in the coordinates of the local moving frame for link i,

then the spatial Jacobian is Jse = [AdT0,1S1 AdT0,2S2 AdT0,nSn] .

Rewrite (2.26) and represent the position and velocity in the end-effector frame.

One obtains the following equation:

T10,n

p

0

= T10,n T0,n

p

1

=

R

Te Re R

Te de

0 0

p

1

. (2.32)

The body Jacobian Jbe is defined by the relationship

Vbe = (T10,n T0,n)

=

i

(T10,nT0,n

qi)qi

= Jbe (q)q . (2.33)

A calculation similar to that performed previously yields

J

b

e (q) =

AdT1

1,nS1 AdT1

2,nS2 AdT1

n1,nSn

1 Sn

. (2.34)

The body Jacobian can be expressed entirely in terms of the joint screws mapped

into the end-effector frame. It maps the joint velocities into the corresponding spatial

velocity of the end-effector in the end-effector frame.

22


42/174


MieSiqi , where Si is a screw in the coordinates of the local moving frame for link i,

then the body Jacobian is Jbe = AdT11,nS1 AdT12,nS2 AdT1n1,nSn1 Sn .

The spatial and body Jacobians are related by the adjoint transformation:

Jse (q) = AdT0,nJbe (q) . (2.35)

Both the spatial and body Jacobians can be related to the geometric Jacobian

in (2.25) by a coordinate transformation. The 4 4 matrix form of Vge in (2.25) is

ge de

0 0

= I de

0 1

ge de

ge de

0 0

I de

0 1

= G1s T0,nT

10,n Gs =

AdGs1J

se q

. (2.36)

Therefore, the relation between Jge and Jse can be expressed as

Jge = AdGs1Jse . (2.37)

Similar procedure can be performed to obtain the the relation between Jge and Jbe :

Jge = AdGbJbe (2.38)

where

Gb =

Re 0

0 1

.

2.3 Summary

The concepts of both kinematics and differential kinematics are studied in this

chapter. Kinematics can be represented as joint motions in the special Euclidean

23


43/174

group, SE(3), which is a Lie group corresponding to homogeneous transformations,

while differential kinematics can be represented as se(3) which is the Lie algebra

associated with SE(3). The Lie algebra is the tangent space at the identity element

of the Lie group. Mapping with the adjoint operators, the Lie group and the Lie

algebra can be used to express not only the propagation of the spatial velocities and

accelerations but also the propagation of the spatial forces of a robot manipulator.

The propagation of the spatial velocities, accelerations and forces is referred to as the

recursive formulation of dynamics which will be addressed in the next chapter.

24


44/174

Chapter 3

Dynamics of Robot Manipulators

Dynamics of mechanisms is a field in which much research effort has been made.

In this chapter, the dynamics of robot manipulators will be studied, such as inverse

dynamics, forward dynamics and hybrid dynamics including inverse and forward dy-

namics. Formulations for both open-chained and closed-chained systems will be intro-

duced. These formulations, incorporated with Lie groups and Lie algebras, provide

powerful tools for us to generate efficient mathematical models of robot dynamics.

3.1 Open-Chained Systems

The dynamic equations of open-chained robot manipulators can be expressed

in the general form

H(q)q+ h(q, q) = (3.1)

which relates the applied joint forces to the joint positions q and their time deriva-

tives q and q. H(q) is the mass or inertia matrix and h(q, q) contains the centrifugal,Coriolis, gravitational and frictional forces.

The dynamics problem for robot manipulators is divided into two sub-problems,

inverse dynamics and forward dynamics. The inverse dynamics problem aims at

25


45/174

determining the motor or driving forces that produce a specific motion, as well as

the reactions that appear at each joint of the robot manipulator. It is necessary to

know the velocities and accelerations to be able to estimate the inertia forces which,

together with the weight and all the other known external forces, will provide the

basis to calculate the required actuating forces.

The forward dynamics problem determines the acceleration, given the applied

forces. These are used to find the motion of a robot manipulator over a given time

interval, given some initial conditions. The importance of the forward dynamic prob-

lem lies in the fact that it allows one to simulate and predict the systems actualbehavior; the motion is always the result of the forces that produce it.

3.1.1 Inverse Dynamics

The solution to the inverse dynamics has different applications. In the first place,

it determines the forces to which the system is subjected. Extremely important is thefact that the inverse dynamics yields the driving forces necessary to control a system

so that it follows a desired trajectory.

An efficient Lie group formulation given below supplies the solution to the in-

verse dynamics problem [42, 45]. It is of order O(n) where n is the number of the

joints.

Algorithm 3.1 The Newton-Euler recursive formulation of the inverse dynamics is:

26


46/174

Initialization

V0, V0, Fn+1

Forward recursion: i = 1 to n

Vi = AdT1i1,i

Vi1 + Siqi

Vi = AdT1i1,i

Vi1 + Siqi + adViSiqi

Backward recursion: i = n to 1

Fi = Ad

T1i,i+1

Fi+1 + JiVi ad

ViJiVi

i = STi Fi + fcisgn(qi) + fvi qi

In the algorithm, the index i represents the ith link frame counted from the base

frame (i = 0). The joint screw, spatial velocity, spatial acceleration and spatial force

are written as S, V, V and F se(3), respectively. Particularly, V0 and V0 represent

the spatial velocity and acceleration of the base, respectively, while Fn+1 represents

the external spatial force on the last link or end-effector. Ti1,i SE(3) denotes the

transformation from the i 1th link frame to the ith link frame. The joint velocity,

acceleration, force and the Coulomb and viscous frictions are written as q, q, , fc

and fv , respectively. And J is the spatial inertia matrix

Ji =

Ii mir

2i miri

miri mi 1

(3.2)

where mi and Ii are the mass and inertia of the ith link, respectively; ri is a vector

from the origin of the ith link frame to the center of mass of ith link; ri is the skew-

symmetric matrix formed by ri using the notation from the last chapter; and 1 is an

27


47/174

identity matrix. The spatial velocity and force are

Vi =

i

vi

and Fi =

mti

fti

(3.3)

where , v, mt andft are the angular velocity, linear velocity, moment and force,

respectively. This recursive formulation shows how the spatial velocity and accelera-

tion propagate forwards from the base to the end-effector and how the spatial force

propagates backwards from the end-effector to the base.

3.1.2 Forward Dynamics

The forward dynamics defines a system of nonlinear ordinary differential equa-

tions (initial value problem). The solution to these differential equations can be found

by numerically integrating them starting from the initial conditions. An important

characteristic of this mathematical problem is that it is computationally intensive.

Because of this, it is very important to choose the most efficient method for dealing

with and solving this problem. Featherstones [23] articulated-body method supplies

an efficient solution to the forward dynamics problem.

Featherstone has shown that the equations of motion of each link can be ex-

pressed as follows:

Fi = JiVi + Fi (3.4)

where Ji is the articulated-body inertia of link i, and Fi is the bias force associated

with link i. To this end, consider the articulated body consisting the last two links of

an n link chain. We assume that a spatial force Fn1 is applied to the handle of the

articulated body. From Algorithm 3.1, it follows the equations of motion of each link

Fn = JnVn ad

VnJnVn (3.5)

28


48/174

Fn1 = Ad

T1n1,nFn + Jn1Vn1 ad

Vn1Jn1Vn1 (3.6)

Note that the equations of motion for link n are already in the form of (3.4) with

Jn = Jn and

Fn = ad

VnJnVn. Referring to Algorithm 3.1, the spatial accelerationof the last link is given by

Vn = AdT1n1,nVn1 + Snqn + adVnSnqn (3.7)

Substituting it into (3.5) yields

Fn = JnAdT1n1,n

Vn1 + JnSnqn + JnadVnSnqn ad

VnJnVn (3.8)

Recalling that n = STn Fn it is straightforward to show that

qn =n S

Tn (JnAdT1n1,n

Vn1 + JnadVnSnqn ad

VnJnVn)

STn JnSn(3.9)

Substituting (3.9) back to (3.8), Fn can be written as a new equation

Fn = JnAdT1n1,nVn1 + JnadVnSnqn ad

VnJnVn +

JnSnn S

Tn (JnAdT1

n1,nVn1 + JnadVnSnqn ad

VnJnVn)

STn JnSn

(3.10)

in which only Vn1 of the spatial accelerations shows rather than Vn. Substituting

the new equation of Fn to (3.6) and rearranging it yields

Fn1 = Jn1Vn1 + Fn1 . (3.11)

Sequential computation from n to 1 confirms (3.4).

In general, the articulated-body method executes the forward computation of

the spatial velocities, the backward computation of the articulated-body inertias and

the bias forces, and then the forward computation of the joint accelerations and the

spatial accelerations. A general algorithm for the system including both inverse and

forward dynamics will be introduced in the next section.

29


49/174

3.1.3 Hybrid Dynamics

In this dissertation we define the hybrid dynamics problem as a system of equa-

tions for articulated bodies in which either the applied force or the joint acceleration

for each joint is known. For such a system, the computation of either the inverse

dynamics or the forward dynamics is needed for each joint depending on which one

of i and qi is specified for the joint.

The articulated-body method can be applied to solve not only the forward dy-

namics but also the inverse dynamics. A similar procedure to derive the formulation

for forward dynamics is used for the inverse dynamics. Consider the articulated body

in the previous section. Instead of the applied forces, the joint accelerations are spec-

ified. In this case, Fn can be calculated directly from (3.8) without any knowledge

of Vn. Substituting (3.8) into (3.6) and rearranging it confirms (3.11). Note that the

equations for computing Jn1 and Fn1 are different from those in the forward dy-

namics. Sequential computation from n to 1 confirms the articulated equation (3.4).

Compared to the Newton-Euler recursive formulation with two iterations, there are

three iterations needed for the articulated-body formulation. Although it is less effi-

cient than the Newton-Euler recursive formulation for the inverse dynamics case, the

articulated-body method supplies a powerful tool to deal with the complex dynamics

problem involving both inverse dynamics and forward dynamics.

Algorithm 3.2 The articulated-body recursive formulation of the hybrid dynamicsis:

Initialization

V0, V0, Fn+1, Fbn+1 = Fn+1, Jn+1 = 0, Jbn+1 = 0, bn+1 = 0

30


50/174


Vi = AdT1i1,i

Vi1 + Siqi


Ji = Ji + Ad

T1i,i+1

Jbi+1AdT1i,i+1

Fi = ad

ViJiVi + Ad

T1i,i+1

(Fbi+1 + Ji+1Si+1bi+1)

Fbi = Fi + JiadViSiqi

Jbi =

Ji i Ia

Ji JiSiSTi Ji/(STi JiSi) i Ip

bi =

qi i Ia

(i STi Fbi)/(S

Ti JiSi) i I

p


i = STi (Fbi + JiAdT1

i1,iVi1 + JiSibi) i I

a

qi

= bi

STi

JiAd

T1

i1,i

Vi

1/(ST

iJ

iS

i) i Ip

Vi = AdT1i1,i


The variables used here are the same as in Algorithm 3.1. The set of active joints

in which the forward dynamics is applied is represented as Ia; and the set of passive

joints in which the inverse dynamics is applied is represented as Ip. Furthermore, for

a branched-chained system, (3.4) can be extended to be

Fi =

(JiVi + Fi) . (3.12)

The formulation is very similar to Algorithm 3.2 and will not be addressed.

31


51/174

3.2 Closed-Chained Systems

So far the systems we have considered are open-chained robot manipulators. An

example of such a system is a robot arm moves around without any contact with the

environment. If there is any contact with the environment, it forms an closed-chained

system which can also be viewed as an open-chained system with the contact forces.

Lets consider the closed-chained system with the following holonomic constraint

equations, : n m:

(q(t)) = 0 (3.13)

during some time interval. The time derivatives of it can be simply derived as

= q q = 0 (3.14)

= q q+ q q = 0 (3.15)

where q can be computed in a similar way to the geometric Jacobian described in

Section 2.2. The torque, c, reflecting on each joint due to the contact between the

robot and the environment, can be obtained by

c = Tq Fc (3.16)

in which Fc is the contact force (or the Lagrange multiplier). Incorporating it with

the open-chained system (3.1) forms the following closed-chained system:

H(q)q+ h(q, q) = + Tq Fc . (3.17)

Together with (3.15) it can be written as

H

Tq

q 0

q

Fc

=

h

q q

(3.18)

32


52/174

which is a system of (n + m) equations with (n + m) unknowns, whose matrix is

symmetrical and, in general, non-positive definite, and also very sparse in many

practical cases.

Equations (3.18) can be used for the simultaneous solution of the accelerations

and the contact forces. Alternatively, (3.17) can be solved first to obtain an expression

for the accelerations:

q = H1( h + Tq Fc) (3.19)

which can only be used if the mass matrix is non-singular, as it will be in most cases.

By substituting (3.19) into (3.15) one obtains

Fc = (qH1Tq )

1(qH1(h ) q q) . (3.20)

In order to calculate the accelerations, it suffices to substitute Fc in (3.19).

Equation (3.18) uses the kinematic acceleration equations (3.15) obtained by

differentiating the constraint equations (3.13) twice with respect to time. This means

that when integrating the system dynamics, the following differential equation must

also be integrated with respect to time:

(q(t)) = qq+ qq = 0 . (3.21)

This system of differential equations has the following solution:

(t) = a1t + a2 (3.22)

where a1 and a2 are constant vectors that depend on the initial conditions. If the

initial position and velocity satisfy the constraint equations, both vectors a1 and a2 are

null. Theoretically, (3.22) guarantees that the constraint equations will be satisfied

at any time. In practice, this is not true.

33


53/174

Equation (3.21) is unstable since for any vector a1 different from zero the general

solution given by equation (3.22) is not bounded and tends to increase indefinitely

with time. Even though the initial conditions guarantee that a1 = 0, during the course

of the numerical integration, the round-off errors that appear during the integration

do not satisfy the constraint equations. The effects of these errors increase with time,

in accordance with expression (3.22).

The instability during the numeric integration of kinematic acceleration equa-

tions ensures that (3.18) may not be used directly to obtain the solution of the dy-

namic simulation problem. The Baumgarte stabilization method [7] is used to replacethe differential constraint equations by the following system:

+ k1 + k2 = q q+ q q+ k1q q+ k2 = 0 (3.23)

where k1 and k2 are appropriately chosen constants. The general solution to this

differential equation is

= a1es1t + a2e

s2t (3.24)

where a1 and a2 are constant vectors that depend on the initial conditions and s1 and

s2 are the roots of the characteristic equation, s2+k1s+k2 = 0. Ifk1 and k2 are positive

constants, the two roots have a real negative part which guarantees the stability of

the general solution (3.23) in contrast with that in (3.21). The initial condition of the

system should guarantee that the vectors a1 and a2 are zero. If numerical round-off

errors alter this condition, the real negative part of the exponential terms damps

out the possible errors occurring during the integration process. From our numerical

experiments, it appears that the behavior of the method does not significantly depend

on the value of k1 and k2.

34


54/174

By using (3.23) instead of (3.21) the closed-chained system (3.18) is transformed

into

H Tq

q 0

q

Fc

=

h

qq+ k1qq+ k2

(3.25)

which produces acceptable results. The Baumgarte stabilization approach is general,

simple and numerically efficient.

3.2.1 Lathrops Algorithm

An alternative recursive formulation of the dynamics of constrained robot sys-

tems was derived by Lathrop [31]. Two 6-vectors, n and 0, of unknown endpoint

degrees of freedom are introduced where an overbar indicates quantities related to

base constraints. The endpoint spatial acceleration, V0 and Vn, and force, F0 and

Fn+1, can be written as some known linear combination of 0 and n as shown in the

following algorithm where Ma0 , Mf0 , M

an , M

fn+1,

a0,

f0 ,

an, and

fn+1 are known. The

influence of n on the base is computed by a backward recursion which is related to0 to obtain 0. Then i and qi are solved with a forward computation.

Algorithm 3.3 The recursive formulation of the dynamics of the constrained robot

system is:

Initialization

V0

V0 = Ma0 0 +

a0 ; F0 = M

f0 0 +

f0

Vn = Man n +

an ; Fn+1 = M

fn+1n +

fn+1

Mfn = Ad

T1n,n+1Mfn+1 + JnM

an

35


55/174

fn = Ad

T1n,n+1fn+1 + Jn

an ad

VnJnVn

Fn = Mfn n +

fn


Vi = AdT1i1,i

Vi1 + Siqi


i Ia

Mai1 = AdTi1,iMai

ai1 = AdTi1,i(ai adViSiqi Siqi)

Mfi1 = Ad

T1i1,i

Mfi + Ji1Mai1

fi1 = Ad

T1i1,i

fi + Ji1ai1 ad

Vi1Ji1Vi1

i Ip

Mi = 166 ejSTi M

fi

[S

T

i M

f

i ]j

; i = eji S

Ti

fi

[S

T

i M

f

i ]jMai = Mai M

i ;

ai = Mai

i +

ai

Mfi

= Mfi Mi ;

fi

= Mfi i +

fi

Mai1 = AdTi1,i(Mai Sie

Tj )

ai1 = AdTi1,i(ai adViSiqi)

Mfi1 = Ad

T1i1,i

Mfi

+ Ji1Mai1

f

i1 = Ad

T1i1,if

i

+ Ji1a

i1 ad

Vi1Ji1Vi1

Solve 0 for:

V0 = Ma0 0 +

a0 ; F0 = M

f0 0 +

f0

V0 = Ma0 0 +

a0 ; F0 = M

f0 0 +

f0

36


56/174


i Ia

i = i1

i Ip

qi = eTj i1

i = i1 ej qi

i = Mi

i + i

Vi = AdT1i1,i


In the algorithm, [x]j denotes the jth element of x and ej is the j

th unit vector (all

the elements equal to zero except the jth element equal to one).

In the next section, formulations of derivatives of dynamics will be introduced.

The formulations will be applied in the next chapter to compute the analytical gra-

dient for a optimization problem.

3.3 Derivatives of Dynamics

Assume that the joint trajectories are parameterized as q = q(t, p). The follow-

ing formulation can be obtained by taking derivative of the inverse dynamics with

respect to p [35]. (Here we dont take into account friction, since the Coulomb friction

term is not continuously differentiable.)

37


57/174

Algorithm 3.4 The recursive formulation of the derivative of the inverse dynamics

is:

Initialization

V0p

,V0p

,Fn+1

p


Vip

= AdT1i1,i

Vi1p

+ Siqip

+ adViSiqip

Vi

p

= adAdT

1

i1,i

Vi1Si

qi

p

+ AdT1i1

,i

Vi1

p

+

Siqip

+ adVip

Siqi + adViSiqip


Fip

= AdT1i,i+1

Fi+1p

+ JiVip

adVip

JiVi

adViJiVip

AdT1i,i+1

adSi+1

qi+1p

Fi+1

ip

= STiFip

In the example of a PUMA robot described in the next chapter, the last link

holds the payload, which is the parameter pw in (6.1). Therefore Jn = Jn(pw), and

the derivative of the inverse dynamics with respect to pw is described below:

Algorithm 3.5 Replace Ji in Algorithm 3.1 by

Ji =

0 if i = 1 n 1

Jn/pw if i = n

38


58/174

The derivatives of the joint torques and/or accelerations in the hybrid dynamics

for both open-chained and closed-chained systems are more complicated but can be

obtained by a similar approach to that used for the derivative of the inverse dynamics.

These formulations are not addressed in this dissertation. The formulation for the

hybrid dynamics of the open-chained system can be found in Sohl and Bobrow [53].

These derivatives can be used to compute the analytical gradient of the dynamic

motion planning in the next two chapters.

3.4 Summary

In this chapter, the dynamics of robot manipulators is studied for both open-

chained and closed-chained syste

Belajar Dynamic 2

Documents

Transcript of Belajar Dynamic 2