Академический Документы
Профессиональный Документы
Культура Документы
1
Introduction
In this Chapter the following concepts will be studied:
• The linear and angular velocity of a rigid body to analyze the
motion of the manipulator
• Forces that act on a rigid body (application on static forces of
a manipulator)
Jacobian matrix relations
2
Linear & Angular Velocities - Frames
– Computed Frame
This is the frame in which the velocity is measured
(differentiate the position).
3
Frame - Velocity
Represented
(Reference Frame)
A
dB
( VQ )
A B
PQ
dt
Computed
(Measured)
4
Differentiation of position vectors
Derivative of a vector:
d B
Q (t t ) B
Q(t ) Q
B
VQ Q lim
B
dt t 0 t
V dt
A
A B dB
Q Q
We may write it:
V
A B
Q
A
B
B
R VQ .
6 vC UVCORG
Example 5.1:
100 mph
30 mph
U
dU
PCORG U VCORG vC 30 Xˆ .
dt
( VTORG )C vT UCRvT UC R(100 Xˆ )CU R 1100 Xˆ .
C U
C T
( VCORG )TCRT VCORG UCRUT RT VCORG CU R 1UT R70 Xˆ .
7
Angular velocity vector:
8
Angular velocity vector:
A
B describes the rotation of frame {B} relative to {A}
Direction of B
A
Magnitude of B A
APQ
The Magnitude of the
differential change is A
PQ sin
A
PQ (t )
A
PQ (t t )
PQ
A A
B t A
PQ sin
Using a vector cross product
we get
APQ
AVQ A B APQ
t
12
Angular Velocity - Rigid Body
In the general case, the vector Q may also be changing with respect to
the frame {B}. Adding this component we get.
VQ
A
V P
A B
Q
A
B
A
Q
Using the rotation matrix to remove the dual-superscript, and since the
A
description of A
PQ at any instance is B R BPQ we get
13
Simultaneous Linear and Rotational Velocity
The final results for the derivative of a vector in a moving frame
Vector Form B
PQ
A
B
A
VQ AVBORG BAR BVQ AB BAR BPQ
Matrix Form
A
VQ AVBORG BAR BVQ BAR BAR BPQ
14
Definitions - Angular Velocity
Angular Velocity: The instantaneous rate of change in the
orientation of one frame relative to another.
Point - 1D
Linear Velocity
Plane - 2D / Body - 3D
15
Definitions - Angular Velocity
Just as there are many ways to represent orientation (Euler Angles, Roll-Pitch-
Yaw Angles, Rotation Matrices, etc.) there are also many ways to represent the
rate of change in orientation.
Angular Velocity
Representation
The angular velocity vector is convenient to use because it has an easy to grasp
x
A
B y
z
Specifically, the columns of BA R are the unit vectors of {B} represented in {A}.
A
A
B R P
BP
x
B
y
B
Pz
If we look at the derivative of the rotation matrix, the columns will be the velocity of
18
Definitions - Angular Velocity - Matrix
A
The relationship between the rotation matrix B R and the derivative of
the rotation matrix A
B R can be expressed as follows:
A
B R BAR BA R
A A
V
BV
x
B
y
B
Vz BAR
P
BP
x
B
y
B
Pz
where
A
B R is defined as the angular velocity matrix
0 z y x
B y
A
R
B z 0 x A
y x 0 z
19
Angular Velocity - Matrix & Vector Forms
Matrix Form Vector Form
0 z y x
Definition A
R z 0 x A
B y
B
y x 0 z
Multiply by Constant k
A
B R k
A
B
x x
Multiply by Vector A
B R y
A
B y
z z
Multiply by Matrix R
s
t R
A
B R
s
t
T
R
s
t
A
B
20
Simultaneous Linear and Rotational Velocity -
Vector Versus Matrix Representation
Vector Form Matrix Form
A
VQ AVBORG BAR BVQ AB BAR BPQ A
VQ AVBORG BAR BVQ BAR BAR BPQ
i j k
P x y z i ( y Pz z Py ) j ( x Pz z Px ) k ( x Py y Px )
Px Py Pz
0 z y Px z Py y Pz
R P z 0 x Py z Px x Pz
y x 0 Pz y Px x Py
21
Changing Frame of Representation - Linear Velocity
Recall that
R
A
P
A
BT
A B B org
0 0 0 1
so that the derivative is
A d
R
A
P
A
R
A
P
A
A
R BA R V
A
BT
B B org B B org B B org
dt
0 0 0 1 0 0 0 0 0 0 0 0
22
Summary – Changing Frame of Representation
Linear and Rotational Velocity
A
B
Vector Form
A
VQ AVBORG BAR BVQ AB BAR BPQ
B
PQ
Matrix Form
A
VQ AVBORG BAR BVQ BAR BAR BPQ
Angular Velocity
Vector Form
A
C A B BAR B C
Matrix Form A
C R BAR BAR CBR BA RT
23
Position Propagation
24
Motion of the Link of a Robot
• In considering the motion of a robot link we will always use link
frame {0} as the reference.
Where: vi- is the linear velocity of the origin of link frame (i)
with respect to frame {0} (Computed AND Represented)
i - is the angular velocity of the origin of link frame (i)
with respect to frame {0} (Computed AND Represented)
25
Velocities - Frame & Notation
Expressing the velocity of a frame {i} (associated with link i ) relative
to the robot base (frame {0}) using our previous notation is defined as
follows: 0
vi 0Vi 0Vi
0 0 0
i i i
26
Velocity Propagation
Given: A manipulator - A chain of rigid bodies each one capable of
moving relative to its neighbor
Problem: Calculate the linear and angular velocities of the link of a
robot
Solution (Concept): Due to the robot structure (serial mechanism)
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 components were added by joint i+1
27
Velocity of Adjacent Links - Angular Velocity
i 1
By pre-multiplying both sides of the equation by R ,we
0
can convert the frame of reference for the base {0} to frame
28
{i+1}
Velocity of Adjacent Links - Angular Velocity
i 1
0 R 0 i 1 i 01R 0 i i 01R 0i Ri i 1
i 1 i 1i i i1R i i 1
i 1
i 1
i 1 - Angular velocity of frame {i+1} measured relative to the
robot base, and expressed in frame {i+1}
-
i 1
i - - Angular velocity of frame {i} measured relative to the robot
base, and expressed in frame {i+1}
i 1
i R i i 1 - Angular velocity of frame {i+1} measured relative to frame
{i} and expressed in frame {i+1}
29
Velocity of Adjacent Links - Angular Velocity
i 1 i 1i i i1R i i 1
i 1
i i i1Rii
i 1
30
Velocity of Adjacent Links - Angular Velocity
i 1 i 1i i i1R i i 1
i 1
0
i 1 i
R 0
i+1 i i 1
i i 1
31
Velocity of Adjacent Links - Angular Velocity
The result is a recursive equation that shows the angular
velocity of one link in terms of the angular velocity of the
previous link plus the relative motion of the two links.
0
i 1
i 1 i i1Rii 0
i 1
32
Velocity of Adjacent Links – Linear Velocity
From the relationship developed previously (matrix form)
VQ AVBORG BAR BVQ AB BAR BPQ
A
A
VQ AVBORG BAR BVQ BAR BAR BPQ
we re-assign link frames for adjacent links (i and i +1) with the velocity
computed relative to the robot base frame {0}
A0
Bi
C i 1
Vi 1 0i R 0i R iPi 1 0Vi 0i R iVi 1
0
i 1
By pre-multiplying both sides of the equation by ,we can 0 R
convert the frame of reference for the left side to frame {i+1}
33
Velocity of Adjacent Links – Linear Velocity
i 1
0 R 0Vi 1 i 01R 0i R 0i R iPi 1 i 01R 0Vi i 01R 0i R iVi 1
Which simplifies to
i 1
0 R 0Vi 1 i 01R 0i R 0i R iPi 1 i 01R 0Vi i i1R iVi 1
i 1
Factoring out i R from the left side of the first two terms
i 1
0 R 0Vi 1 i i1R 0i R 0i R 0i R iPi 1 0iR 0Vi i i1R iVi 1
34
Velocity of Adjacent Links – Linear Velocity
i 1
0 R 0Vi 1 i i1R 0i R 0i R 0i R iPi 1 0iR 0Vi i i1R iVi 1
i 1
i R iVi 1- Linear velocity of frame {i+1} measured relative to frame {i}
and expressed in frame {i+1}
0
i 1
R iVi 1 0
i
di 1
35
Velocity of Adjacent Links – Linear Velocity
0
i 1 0
0 R Vi 1 i 1
i R i 0 0 i
R R
0 i i R P
i 1 0
i 0
R Vi 0
di 1
0
i
R 0i R 0i R 0iR 0i R 0i RT 0iR 0 i 0iRi ii
i 1
0 R 0Vi 1 i 1vi 1 0
i
R 0Vi i vi
36
Velocity of Adjacent Links – Linear Velocity
0
i 1
vi 1 i i1R i i i Pi 1 i vi 0
di 1
37
Velocity of Adjacent Links – Summary
Angular Velocity
0 - Prismatic Joint
0
i 1
i 1 i i1Rii 0
i 1
Linear Velocity
0 - Revolute Joint
0
i 1
vi 1 i i1R ii i Pi 1 i vi 0
di 1
38
Angular and Linear Velocities – 2links Robot - Example
c1 s1 0 0
c 2 s 2 0 l1
s1 c1 0 s2 c2
1T
0
0
0 0
2T
1
0 0 1 0 0
0 1 0
0 0 0 1
0 0 0 1
1 0 0 l 2
0 1 0 0
3T
2
0 0 1 0
0 0 0 1
39
Angular and Linear Velocities – 2links Robot - Example
0
i 1
i 1 i i1Rii 0
i 1
For i=0
0 c1 s1 0 0 0 0
1
1 01R 00 0 s1 c1 0 0 0 0
1 0 0 1 0 1 1
For i= 1
0 c2 s 2 0 0 0 0
2
2 12 R 11 0 s 2 c 2 0 0 0 0
2 0 0 1 1 2 1 2
40
Angular and Linear Velocities – 2links Robot - Example
For i=2
1 0 0 0 0
3
3 23 R 22 0 0 1 0 0 0
0 0 1 1 2 1 2
0
i 1
vi 1 i i1R ii i Pi 1 i vi 0
di 1
For i=0
For i=2
1 0 0 0 l 2 l1s 21
3
v3 23 R 22 2 P3 2 v2 0 1 0 0 0 l1c 21
0 0 1 1 2 0 0
42
Angular and Linear Velocities – 2links Robot - Example
c1 s1 0 c 2 s 2 0 c1c 2 s1s 2 c1s 2 s1c2 0
c1 0 s 2 c 2 0 s1c 2 c1s 2 s1s 2 c1c 2 0
2 R s1
0
0 0 1 0 0 1 0 0 1
c12 s12 0
s12 c12 0 30 R
0 0 1
43
c i s i 0 ai 1
s c c i ci 1 si 1 si 1d i
i 1
iT
i i 1
s i si 1 c i si 1 ci 1 ci 1d
0 0 0 1
c1 s1 0 0 c2 s 2 0 L1
s1 c1 0 0 0 0 1 0
1T
0 2T
1
0 0 1 0 s2 c2 0 0
0 0 0 1 0 0 0 1
c3 s 3 0 L 2
s3 c3 0 0
3T
2
0 0 1 0
0 0 0 1
i i 1 ai 1 di i
1 0 0 0 1
2 90 L1 0 2
3 0 L2 0 3
4 0 L3 0 0
44
From the DH parameter table, we can specify the homogeneous
transform matrix for each adjacent link pair:
c i s i 0 ai 1
s c c i ci 1 si 1 si 1d i
i 1
iT
i i 1
s i si 1 c i si 1 ci 1 ci 1d
0 0 0 1
c1 s1 0 0 c2 s 2 0 L1
s1 c1 0 0
0
0 1 0
1T
0
2T
1
0 0 1 0 s2 c2 0 0
0 0 0 1
0 0 0 1
c3 s 3 0 L 2
s3 c3 0 0
3T
2
0 0 1 0
0 0 0 1
45
Compute the angular velocity of the end effector frame relative to the
base frame expressed at the end effector frame.
0
i 1
i 1 i i1Rii 0
i 1
For i=0
0 c1 s1 0 0 0 0
1
1 01R 00 0 s1 c1 0 0 0 0
1 0 0 1 0 1 1
46
For i=1
0 c2 0 s 2 0 0 s 21
2
2 12R11 0 s 2 0 c 2 0 0 c 21
2 0 1 0 1 2 2
For i=2
0 c3 s3 0 s 21 0 s 231
3
3 2 R 2 0 s3 c3 0 c21 0 c231
3 2
3 0 0 1 2 3 2 3
For i=3
0 1 0 0 s 231 0 s 231
4
4 3 R 3 0 0 1 0 c231 0 c231
4 3
Note 0 0 0 1 2 3 0 2 3
47 3 4 4
3
Compute the linear velocity of the end effector frame relative to
the base frame expressed at the end effector frame.
Note that the term involving the prismatic joint has been dropped
from the equation (it is equal to zero).
0
0
i 1
vi 1 i i1Ri i Pi 1 ivi 0
di 1
48
For i=0
c2 0 s 2 0 L1 0 0
v2 12R111P2 1v1 s 2 0 c 2 0 0 0 0
2
49
For i=3
c3 s3 0 s 21 L 2 0
3
v3 2 R 2 P3 v2 s3 c30 0 c 21 0 0
3 2 2 2
0 0 1 2 0 L11
c3 s 3 0 0 L 2s32
s 3 c3 0 L 21 L 2c3 2
0 0 1 L 2c 21 L11 ( L1 L 2c 2)1
50
51
Kinematics Relations - Joint & Cartesian Spaces
Joint Space
1 {N}
2
N
Cartesian Space
R
0 0
PN
0 PN
X 0
T
0 N
N
0 1 rN
52
Kinematics Relations – Forward & Inverse
X FK ( )
1
2
0 PN
X 0
rN
N
IK ( X )
Tip Location in Tip Location in
Joint Space Cartesian Space
53
Kinematics Relations – Forward & Inverse
vx
1 v
y
d
[ ] 2 d
v v
X [ X ] N z
dt
dt
N x
N y
z
Tip Velocity in Tip velocity in
Joint Space Cartesian Space
54
Jacobian Matrix - Introduction
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 )
55
Jacobian Matrix - Introduction
If we wish to calculate the differential of yi as a function of the
differential xi we use the chain rule to get
f1 f f
y1 x1 1 x2 1 x6
x1 x2 x6
f 2 f f
y 2 x1 2 x2 2 x6
x1 x2 x6
f 6 f f
y 6 x1 6 x2 6 x6
x1 x2 x6
F
Y X
X
56
Jacobian Matrix - Introduction
The 6x6 matrix of partial derivative is defined as the Jacobian
matrix
F
Y X J ( X )X
X
Y J ( X ) X
57
Jacobian Matrix - Introduction
x J
J 1 x
58
Jacobian Matrix - Introduction
This expression can be expanded to:
x 1
y
2
z J
x
y
z N
6x1 6xN Nx1
Where:
x is a 6x1 vector of the end effector linear and angular velocities
J is a 6xN Jacobian matrix
N is a Nx1 vector of the manipulator joint velocities
N is the number of joints
59
Jacobian Matrix - Introduction
In addition to the velocity
relationship, we are also interested
in developing a relationship
between the robot joint torques ( )
F
and the forces and moments ( F )
at the robot end effector (Static
Conditions). This relationship is
given by:
J T F
60
Jacobian Matrix - Introduction
This expression can be expanded to:
1
T
Fx
F
2 y
J Fz
M x
M y
N M z
6x1 6xN Nx1
Where:
is a 6x1 vector of the robot joint torques
J is a 6xN Transposed Jacobian matrix
T
F is a Nx1 vector of the forces and moments at the robot end
effector
61 N is the number of joints