Sie sind auf Seite 1von 37

2017/10/15

Differential Motion and Velocity/Force


Propagation

Differential Motion and Jacobian

1
2017/10/15

Definitions of Differential Motion and Jacobian

Differential motion: A small movements of mechanism that can be used to


derive velocity relationships between different parts of the mechanism.

Jacobian: a representation of the geometry of the elements of a mechanism


in time.

Differential Relationship

The velocity of point B :

. ^ . ^ . . ^
VB V A V B/ A l1 1 sin 1 i l1 1 cos 1 j l2 ( 1 2 ) sin( 1 2 )i
. . ^
l2 ( 1 2 ) cos( 1 2 )j

V Bx l1 sin 1 l2 sin( 1 2 ) l2 sin( 1 2) 1

V By l1 cos 1 l2 cos( 1 2 ) l2 cos( 1 2 ) 2

(a) A two-degree-of-freedom planar


mechanism and (b) a Velocity diagram

2
2017/10/15

Velocity relationship of point B :

xB l2 cos 1 l2 cos( 1 2 )
y B l1 sin 1 l2 sin( 1 2 )

dx B l2 sin 1d 1 l2 sin( 1 2 )(d 1 d 2)


dy B l1 cos 1d 1 l2 cos( 1 2 )(d 1 d 2)

dxB l1 sin 1 l2 sin( 1 2 ) l2 sin( 1 2 ) d 1

dyB l1 cos 1 l2 cos( 1 2 ) l2 cos( 1 2 ) d 2

Differential Jacobian Differential


Motion of B Motion of Joint

Jacobian
Definition : Jacobian is a representation of the geometry of the
elements of a mechanism in time.

Formation : Jacobian is formed from the elements of the position


equations that were differentiated with respect to 1 and 2.

Assumption : A set of equations in terms of a set of variables

Yi f i ( x1 , x2 , x3 , , xj)
f1 f1 f1
Y1 x1 x2 x j
x1 x2 x j
f2 f2 f2
Y2 x1 x2 x j
x1 x2 x j

fi fi fi
Yi x1 x2 x j
x1 x2 x j

3
2017/10/15

Jacobian
f1 f1 f1
Y1 x1
x1 x2 x j
f2
Y 2
x1
x 2
Matrix Representation
fi
Yi xj
fi fi
xi
Yi x j
x1 x j

dx d 1
Differentiating the position
dy d 2
equations of a robot
dx Robot d 3
D J D
x Jacobian d 4

Differential motion y d 5
of the hand frame
z d 6 Differential motion of robot joints
Function of robots configuration(D-H
parameters)
and of its instantaneous position and orientation

Differential Motion of a Frame

The differential motion of a hand frame of the robot are caused by the
differential motions in each of the joints of the robot.

The differential motion of a frame:


Differential translations,
Differential rotations,
Differential transformations (translations and rotations).

(a) Differential motions of a frame and (b) differential motions of a frame


as related to the differential motions of a robot. (b) a Velocity diagram

4
2017/10/15

Differential Translations

Definition : A translation of a frame at differential values.

Representation : Trans(dx, dy, dz)


The frame has moved a differential amount along the 3 axes.

Differential Rotations

Definition : A small rotation of a frame at differential values.

Representation : Rot(k, d )
^
The frame has rotated an angle of d about an axis k

Differential rotation about x, y, z-axis is x, y, z, respectively.

1 0 0 0 1 0 y 0 1 z 0 0
0 1 x 0 0 1 0 0 z 1 0 0
Rot ( x, x) Rot ( y , y ) Rot ( z , z )
0 x 1 0 y 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
x

[1 0 0 0; Rot ( x, x) Rot ( y, y) Rot ( y, y) Rot ( x, x)

0 c@ -s@ 0

0 s@ c@ 0 10

0 0 0 1]

5
2017/10/15

Differential Rotation about a General Axis


^
A differential motion about a general axis k is composed of 3
differential motions about the 3 axes, in any order.

Rot (k , d ) Rot ( x, x ) Rot ( x, x ) Rot ( x, x)

1 0 0 0 1 0 y 0 1 z 0 0
0 1 x 0 0 1 0 0 z 1 0 0
0 x 1 0 y 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

1 z y 0 1 z y 0
Neglect the higher
x y z x y z 1 x 0 z 1 x 0 order differential motion
( x y, - x y z, x z, y z)
y x z x y z 1 0 y x 1 0
0 0 0 1 0 0 0 1

11

Differential Transformations of a Frame

Definition : A combination of differential translations and rotations.

: Differential Operator

Trans ( dx , dy , dz ) Rot ( k , d ) I

1 0 0 dx 1 z y 0 1 0 0 0
0 1 0 dy z 1 x 0 0 1 0 0
0 0 1 dz y x 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

0 z y dx
z 0 x dy
y x 0 dz
0 0 0 0

12

6
2017/10/15

Differential Change

Matrix dT represents the changes in a frame as a result of differential


motions.

Each element of the matrix represents the change in the corresponding


element of the frame.

dnx dox da x dp x
dn y do y da y dp y
dT
dnz doz da z dp z
0 0 0 0
The new frame is:

Tnew dT Told

13

delta

d s= (d=delta)
Differential Changes between Frames [0 -dz dy

dz 0 -dx
[ dT ] [ ][T ] [T ][T ] [T ] 1[ ][T ] [T ] 1[T ][T ] -dy dx 0]
T 1
[ ] [T ] [ ][T ]
S=S^T
nx ny nz p n 0 z y dx
1 ox oy oz p o z 0 x dy T
n
T x
ax ay az p a y x 0 dz T
y o
0 0 0 1 0 0 0 0 T
z a
T
0 T
z T
y T
dx dx n [( p) d ]
T
T
z 0 T
x dy T dy o [( p) d ]
[T 1 ][ ][T ] T
T T T T
y x 0 dz dz a [( p) d ]
0 0 0 0

14

7
2017/10/15

Differential Motions of a Robot and its Hand Frame

Relation between the differential motions of the joint of the robot and
the differential motions of the hand frame and dT.

It is a function of the robots configuration and design and its instantaneous


location and orientation.

dx d 1

dy d 2

dx Robot d 3
D J D
x Jacobian d 4

y d 5

z d 6

15

Calculation of Jacobian

Key point : Each element in the Jacobian is the derivative of a corresponding


kinematic equation with respect to one of the variables.

Example:

px
J11 S1 (C234a4 C23a3 C 2 a2 )
1

px
J12 C1 ( S 234a4 S 23a3 S 2 a2 )
px C1(C234a4 C23a3 C2a2 ) 2

px
J13 C1 ( S 234a4 S 23a3 )
px S1 (C234a4 C23a3 C2a2 ) 3

px
px S234a4 S23a3 S2a2 J14 C1 ( S 234a4 )
Taking the derivative of px 4

1 1 px
J15 0
5
The last column of the forward px
kinematic equation of the J16 0
robot 6

The first row of the Jacobian

16

8
2017/10/15

The velocity equation relative to the last frame

[T6 D] [T6 J ][ D ]

The differential motion relationship of Equation

T6 T6 T6
dx J11 T6
J12 J 16
d 1
T6 T6 T6
dy J 21 T6 J 22 J 26 d 2
T6 T6 T6
dz J 31 J 36 d 3
T6
x
T6
J 41 T6
J 46 d 4

T6
x
T6
J 51 T6
J 56 d 5

T6
x
T6
J 61 T6
J 66 d 6

17

Relation between Jacobian and Differential Operator

The differential motions of the robots joints are ultimately related to


the hand frame of the robot.

calculate [D] matrix


[D] contains differential motions of the hand,
dx, dy, dz, x, y, z.
calculate dT

calculate [T6D] matrix


[D] contains differential motions of the hand,
T6dx, T6dy, T6dz, T6 x, T6 y, T6 z.

calculate dT

18

9
2017/10/15

Inverse Jacobian

Inverse Jacobian used to calculate the differential motions needed at


the joints of the robot for a desired hand differential motion.

Inverse Jacobian calculates how fast each joint must move so that the
robots hand will yield a desired differential motion or velocity.

To make sure the robot follows a desired path, the joint velocities must
be calculated continuously in order to ensure that the robots hand
maintains a desired velocity.

[ D] [ J ][D ]
[ J ][D] [ J 1 ][ J ][D ]
1
[ D ] [ J 1 ][D]
[T6 J 1 ][T6 D] [T6 J 1 ][T6 J ][ D ] D [T6 J 1 ][T6 D]

19

Derivation of the Inverse Jacobian

Example: 6-axis robot first joint:

px S1 p y C1 0

1
py
1 tan and 1 1 180
px
px S1 p y C1
dpx S1 px C1d 1 dp y C1 p y S1d 1

d 1 ( px C1 p y S1 ) dpx S1 dp y C1
dpx S1 dp y C1
d 1
( px C1 p y S1 )

20

10
2017/10/15

Velocity/Force Propagation

21

Differentiation of position vectors

Derivative of a vector:

B B
B dB Q(t t) Q (t )
VQ Q lim
dt t 0 t

We are calculating the derivative of Q relative to frame B.

22

11
2017/10/15

Differentiation of position vectors

A velocity vector may be described in terms of any frame:

A
A B dB
VQ Q
We may write it: dt
A B A
VQ B R BVQ . Speed vector is a free vector

Special case: Velocity of the origin of a frame relative to some


understood universe reference frame
U
V C V CORG

23

Example 5.1
Both vehicles are 100 mph
heeding in X A fixed universal frame
direction of U

30 mph

U
dU
PCORG U VCORG vC 30 X .
dt
( VTORG ) C vT UCRvT UCR (100 X )
C U U
C R 1100 X .
C T
( VCORG ) C
T RT VCORG C
U RUT RT VCORG U
C R 1U
T R 70 X .

24

12
2017/10/15

Angular velocity vector:

Linear velocity attribute of a point

Angular velocity attribute of a body

Since we always attach a frame to a body we can


consider angular velocity as describing rational
motion of a frame.

25

Angular velocity vector:

A
B
describes the rotation of frame {B} relative to {A}
A
direction of B
indicates instantaneous axis
of rotation
A
Magnitude of B
indicates speed of rotation

In the case which there is


an understood reference
frame:
U
C C

26

13
2017/10/15

Linear velocity of a rigid body

We wish to describe motion


of {B} relative to frame {A}

A
If rotation B R
A A A
is not changing with time: VQ VBORG R BVQ .
B

27

Rotational velocity of a rigid body

Two frames with coincident origins


The orientation of B with
A {B} {A}
respect to A is changing in B B
Q
time.
Lets consider that vector
Q is constant as viewed
from B. B
VQ 0

28

14
2017/10/15

Rotational velocity of a rigid body

| Q| Is perpendicular to
A A
B and Q

Magnitude of differential
change is:

| Q| (| A Q | sin )(| A B | t)
A A A
VQ B Q

Vector cross product

29

Rotational velocity of a rigid body

A A B A A
In general case: VQ ( VQ ) B Q
A A
VQ B R BVQ A
B
A
B R BQ.

30

15
2017/10/15

Simultaneous linear and rotational velocity

A A A
VQ VBORG BR BVQ A
B
A
B R BQ.

We skip 5.4!

31

Motion of the Links of a Robot

Written in frame i

At any instant, each link of a robot in motion has some linear and
angular velocity.

32

16
2017/10/15

Velocity of a Link

Remember that linear velocity is


associated with a point and angular
velocity is associated with a body. Thus:

The velocity of a link means the linear


velocity of the origin of the link frame
and the rotational velocity of the link

33

Velocity Propagation From Link to Link

We can compute the velocities of each link in order


starting from the base.
The velocity of link i+1 will be that of link i, plus
whatever new velocity component added by joint
i+1.

34

17
2017/10/15

Rotational Velocity

Rotational velocities may be added when both


vectors are written with respect to the same frame.
Therefore the angular velocity of link i+1 is the
same as that of link i plus a new component caused
by rotational velocity at joint i+1.

35

Velocity Vectors of Neighboring Links

i
i 1
i
i
i
i 1 R i 1 Zi 1.
i 1

36

18
2017/10/15

Velocity Propagation From Link to Link


Note that:
i 1

i 1
0
i 1 Z i 1 0

i 1

i 1
By premultiplying both sides of previous equation
i R
to:
i 1
i Ri i 1
i 1
iRi i
i 1
i R i 1iR i 1
i 1
Z i 1.

i 1
i 1
i 1
i Ri i i 1
i 1
Z i 1.

37

Linear Velocity

The linear velocity of the origin of frame {i+1} is


the same as that of the origin of frame {i} plus a
new component caused by rotational velocity of link
i.

38

19
2017/10/15

Linear Velocity

Simultaneous linear and rotational velocity:


A A A
VQ VBORG BR BVQ A
B
A
B R BQ.
i i i i
vi 1 vi i Pi 1.
i 1
By premultiplying both sides of previous equation i R
to:
i 1
i R i vi 1
i 1
R ( i vi
i
i
i
i
Pi 1 ).
i 1 i 1
vi 1 i R ( i vi i
i
i
Pi 1 ).

39

Prismatic Joints Link

For the case that joint i+1 is prismatic:

i 1 i 1
i 1 i Ri i ,
i 1
vi 1
i 1
i R (i vi i
i
i
Pi 1 ) di 1 Z i 1.
i 1

40

20
2017/10/15

Velocity Propagation From Link to Link

Applying those previous equations successfully


from link to link, we can compute the rotational and
linear velocities of the last link.

41

Example 5.3

Calculate the velocity


of the tip of the arm as
a function of joint
rates?

A 2-link manipulator with rotational joints

42

21
2017/10/15

Example 5.3
Frame assignments for the two link manipulator

43

Example 5.3
We compute link transformations:

c1 s1 0 0 c2 s2 0 l1 1 0 0 l2
0 s1 c1 0 0 1 s2 c2 0 0 2 0 1 0 0
1T , T
2 , T
3 .
0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

44

22
2017/10/15

Example 5.3
Link to link transformation

0 0
1 1
1 0 , v1 0 ,
1 0
0 c2 s2 0 0 l1s2 1
2 2
2 0 , v2 s2 c2 0 l1 1 l1c2 1 ,
1 2 0 0 1 0 0
l1s2 1
3 2 3
l1s2 0 1
3 2 , v3 l1c2 1 l2 ( 1 2 ) .
l1c2 l2 l2 2
0

45

Example 5.3
Velocities with respect to non moving base

c12 s12 0
0 0 1 2
3 R 1 R 2 R 3 R s12 c12 0.
0 0 1
l1s1 1 l2 s12 ( 1 2 )
0 0
l1s1 l2 s12 l2 s12
v3 3 R 3v3 l1c1 1 l2 c12 ( 1 2 ) 1
.
l1c1 l2 c12 l2 c12 2
0

46

23
2017/10/15

Derivative of a Vector Function

If we have a vector function r which represents a


particles position as a function of time t:

r rx ry rz

dr drx dry drz


dt dt dt dt

47

Jacobian

A Jacobian is a vector derivative with respect to


another vector
If we have f(x), the Jacobian is a matrix of partial
derivatives- one partial derivative for each
combination of components of the vectors
The Jacobian is usually written as J(f,x), but you can
really just think of it as df/dx.
It is a differentiation of a vector function with
respect to a vector.

48

24
2017/10/15

Jacobian

f1 f1 f1
...
x1 x2 xN
f2 f2
... ...
J f,x x1 x2
... ... ... ...
fM fM
... ...
x1 xN

49

Partial Derivatives
The use of the symbol instead of d for partial
derivatives just implies that it is a single component
in a vector derivative.

50

25
2017/10/15

Jacobian
y1 f1 ( x1 , x2 , x3 , x4 , x5 , x6 ),
y2 f 2 ( x1 , x2 , x3 , x4 , x5 , x6 ),

y6 f 6 ( x1 , x2 , x3 , x4 , x5 , x6 ), Y F ( X ).
f1 f1 f1
y1 x1 x2 x6 ,
x1 x2 x6
f2 f2 f2
y2 x1 x2 x6 , Chain rule
x1 x2 x6 J(X)

f6 f6 f6 F
y6 x1 x2 x6 , Y X.
x1 x2 x6 X

51

Jacobian

In the field of robotics, we generally speak of


Jacobians which relate joint velocities to Cartesian
velocities of the tip of the arm.

Y J (X )X.
0
0 v 0
V 0
J( ) .

52

26
2017/10/15

Jacobian

.
For a 6 joint robot the Jacobian is 6x6,
is a 6x1 and v is 6x1.
The number of rows in Jacobian is equal to
number of degrees of freedom in Cartesian
space and the number of columns is equal
to the number of joints.

0
V 0J ( )

53

Jacobian

In example 5.3 we had:


l1s1 1 l2 s12 ( 1 2 )
0 0
l1s1 l2 s12 l2 s12
v3 3 R 3v3 l1c1 1 l2 c12 ( 1 2)
1
.
l1c1 l2 c12 l2 c12 2
0
Thus: l1s1 l2 s12 l2 s12
0
J( )
l1c1 l2 c12 l2 c12
And also:
3 l1s2 0
J( )
l1c2 l2 l2

54

27
2017/10/15

Jacobian
Jacobian might be found by directly
differentiating the kinematic
equations of the mechanism for linear
velocity, however there is no 3x1
orientation vector whose derivative is
rotational velocity. Thus we get
Jacobian using successive application
of:
i 1 i 1
vi 1 R ( i vi
i
i
i
i
Pi 1 )
i 1
i 1
i 1
Ri
i i i 1
i 1
Z i 1

55

56

28
2017/10/15

Geometric Jacobian

57

Derivative of a Rotation Matrix

58

29
2017/10/15

Singularities

Given a transformation relating joint velocity to


Cartesian velocity then
Is this matrix invertible? ( Is it non singular)
0
V 0J ( )
1
J ( )v
det[J ] 0 : singularity
det[J ] 0 : non singularity

59

Singularities
Singularities are categorized into two class:
Workspace boundary singularities:
Occur when the manipulator is fully starched
or folded back on itself.
Workspace interior singularities:
Are away from workspace boundary and are
caused by two or more joint axes lining up.

All manipulators have singularity at boundaries of their


workspace. In a singular configuration one or more degree of
freedom is lost. ( movement is impossible )

60

30
2017/10/15

Example 5.4

In example 5.3 we had:


3
l1s2 0
J( )
l1c2 l2 l2

l1s2 0
DET [ J ( )] | J ( ) | l1l2 s2 0.
l1c2 l2 l2
2 0 ,180 Workspace boundary singularities

61

Example 5.5

0 1 l2c12 l2 s12
J 1( ) .
l1l2 s2 l1c1 l2c12 l1s1 l2 s12
c12
1 ,
l1s2
c1 c12
2 .
l2 s2 l1s2

As the arm stretches out toward 2=0 both joint


rates go to infinity

62

31
2017/10/15

Static Forces in Manipulators


Force and moments propagation

To solve for joint


torques in static
equilibrium

fi force exerted on link i by link i-1

ni torque exerted on link i by link i-1

63

Static Forces in Manipulators

Solve for the joint torques which must be acting


to keep the system in static equilibrium.

Summing the force and


setting them equal to zero

i
f i ifi 1 0
Summing the torques about
the origin of frame i

i
ni i ni 1
i
Pi 1
i
fi 1 0

64

32
2017/10/15

Static Forces in Manipulators


i
f i if i 1 , Working down from last link to the base we
i i i i formulate the force moment expressions
ni ni 1 Pi 1 f i 1.

i i
fi i 1 R i 1f i 1 , Static force propagation
i i i 1 i i from link to link:
ni i 1 R ni 1 Pi 1 fi .

Important question: What torques i n Z i .


i Ti
i
are needed at the joint to balance
i Ti
reaction forces and moments acting i f Z.
i i
on the links?

65

Work-energy Principle
The change in the kinetic energy of an object is
equal to the net work done on the object.

66

33
2017/10/15

Principle of Virtual Work

External virtual work equals the internal virtual strain energy.

67

Jacobians in the Force Domain


Work is the dot product of a vector force or torque and a
vector displacement

F X
It can be written as: FT X T

The definition of jacobian is

X j
So we have FT J T
FT J T
.
J T F.
0 T0
J F.
68

34
2017/10/15

Cartesian Transformation of Velocities and Static Forces

General velocity of a body

v 3 x1 linear velocity
V
3 x1 angular velocity

General force of a body

F 3 x1 force vector
F
N 3 x1 moment vector

6 x 6 transformations map these quantities from one frame


to another.

69

Cartesian Transformation of Velocities and Static Forces

i 1
i 1
i 1
i Ri i i 1
i 1
Z i 1.

Since two frames are rigidly connected i 1 0


B B
vB B
A R RAPBORG
A
A
vA
B B A
wB 0 AR wA

Where the cross product is 0 pz py


the matrix operator P pz 0 px
py px 0

70

35
2017/10/15

Cartesian Transformation of Velocities and Static Forces

We use the term velocity transformation

B A A
vB T v
B v A

B B B
vB A R A R APBORG A
vA
B B A
B 0 AR A

Description of velocity in terms of A when given the quantities in B

A A A
vA B R PBORG BA R B
vB
A A B
A 0 BR B

A A
vA B Tv B v B

71

Cartesian Transformation of Velocities and Static Forces

A force-moment transformation

A A
FA B R 0 B
FB
A A A B
BR
A
NA PBORG B R NB

A A B
FA B Tf FB

With similarity to Jacobians

A A
B Tf B T vT

72

36
2017/10/15

Example 5.8

Frames of interest with a force sensor

T T
FT S T f S FS ,
T
T S R 0
S Tf T T T
.
PSORG S R S R
73

37