Escolar Documentos
Profissional Documentos
Cultura Documentos
=
=
=
(
=
+ + + =
Where
[J
ji
]-is a Jacobian
If j=n=6
Xj=f (
1
,
2
,
3
,
4
,
5
,
6
)
X1=f ( 1, 2, 3, 4, 5, 6) X2=f ( 1, 2, 3, 4, 5, 6)
X3=f ( 1, 2, 3, 4, 5, 6) X4=f ( 1, 2, 3, 4, 5, 6)
X5=f ( 1, 2, 3, 4, 5, 6) X6=f ( 1, 2, 3, 4, 5, 6)
JACOBIAN
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
=
(
(
(
(
(
(
(
(
=
(
(
(
(
(
(
(
(
(
6
*
5
*
4
*
3
*
2
*
1
*
1
3
1
2
3
1
2
1
1
1
*
6
*
5
*
4
*
3
*
2
1
*
.
. . . . . .
. . . . . .
. . . . . .
. . . . .
. . . . .
. . .
u
u
u
u
u
u
ou
o
ou
o
ou
o
ou
o
ou
o
o
o
o
f
f
f f f
z
y
x
dz
dy
dx
X
X
X
X
X
X
The jacobian (6X6 matrix) relates the hand
velocities to the joint velocities.
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
(
=
(
(
(
(
(
(
(
(
(
6
*
5
*
4
*
3
*
2
*
1
*
*
6
*
5
*
4
*
3
*
2
1
*
u
u
u
u
u
u
o
o
o
z
y
x
dz
dy
dx
X
X
X
X
X
X
Jacobian is a time varying quantity.
Jacobian matrix may not be a square matrix.
Example (cylindrical robot)
r
z
(x, y, z)
x= r cos
y= r sin
z=z
dx= cos.dr - r sin.d
dy= sin.dr - r cos.d
dz=dz
Jacobian
dx = cos - rsin 0 dr
dy = sin - rcos 0 d
dz = 0 0 1 dz
Jacobian
dr = cos sin 0 dx
d = -sin/r cos/r 0 dy
dz = 0 0 1 dz
Inverse of Jacobian
Inverse of Jacobian
Inverse jacobian
as function of x, y, z
dr = x/r y/r 0 dx
d = -y/r
2
x/r
2
0 dy
dz = 0 0 1 dz
Inverse of Jacobian
DYNAMICS
Mathematical formulation of motion
equations
Useful in motion simulation.
Useful in the design of control equations.
Useful to evaluate the kinematic
structure of robot arm.
Kinematics Control
Kinematics is the first step towards
robotic control.
Cartesian Space Joint Space Actuator Space
z
y
x
The Robot System
Control System
Sensors
Kinematics
Dynamics
Task Planning
Software
Hardware
Mechanical Design
Actuators
DYNAMICS
The dynamic model of the arm obtained
by 2 physical laws.
Laws of NEWTONIAN mechanics
Laws of LEGRANGIAN mechanics
DYNAMICS
Motion equations of arm are obtained from
Lagrange Euler Equations.
Newton Euler Equations.
Generalised DAlemberts Equations.
DYNAMICS
Forward Dynamics
Inverse Dynamics
Forces(or)
Torques
Accelerations
of joints
Velocities
of joints
Velocities
of joints
Accelerations
of joints
Forces(or)
Torques
Lagrange Euler Equations
n i
T
q
L
q
L
dt
d
i
i i
....... ,......... 3 , 2 , 1
* *
=
=
c
c
|
|
|
.
|
\
|
c
c
L Lagrangian function = KE PE
q
i
Generalised co-ordinate
T
i
generalised force/Torque applied to the system at joint i to drive the
link.
In case of Rotary Joint q
i
=
i
Prismatic Joint qi = d
i
Lagrange Euler Equations
y
0
x
0
z
0
i
r
i
| |
T
i
r
z y x
z
y
x
i 1
1
=
(
(
(
(
=
i
r
i
be the point fixed and at
rest in link i and expressed in
homogeneous co-ordinates
w.r.to the i
th
frame
0
r
i
be the same point
i
r
i
fixed and at rest in link
i and expressed in homogeneous co-ordinates
w.r.to the base frame
0
r
i
=
0
A
i
i
r
i
Where
0
A
i
=
0
A
1
1
A
2
2
A
3
..
(i-1)
A
i
(
(
(
(
=
=
1 0 0 0
cos sin 0
sin sin cos cos cos sin
cos sin sin cos sin cos
) , ( ) , 0 , 0 ( ) 0 , 0 , ( ) , (
) 1 (
1 ) 1 (
i i i
i i i i i i i
i i i i i i i
i i
n
i i i i i i i
d
a
a
A
Z Rot d Trans a Trans X Rot A
o o
u o u o u u
u o u o u u
u o
0
V
i
=
i
I
i
j
i
I
I
A Q
q
A
Q
1
1
0 0 0 0
0 0 0 0
0 0 0 1
0 0 1 0
=
(
(
c
c
(
(
(
(
=
The partial derivative
of
0
A
i
w.r.to. q
j
Can be
calculated by matrix Q
i
For revolute joint
For prismatic joint
Velocity of joint
( ) ( )
( )
( )
(
(
(
(
=
c
c
=
(
(
(
c
c
=
=
=
0 0 0 0
0 0 0 0
0 0 0 1
0 0 1 0
0
1
*
1
*
0
0 0
I
j
i
ij
i
j
i
i
j IJ
i
j
i
i
j
j
i
i
i
i i
Q
q
A
WhereU
r q U
r q
q
A
r A
dt
d
r
dt
d
Kinetic Energy
| |
( )
r p
T
ir
n
i
i
p
i
r
i ip i
r p
T
ir
i
p
T
i
i
i
i
i
r
ip i i
T
i i i
i
i
i
i
q q U J U K
ICENERGY totalKINET
q q U r dm r U dK K
dm V V dK
dm z y x dK
* *
1 1 1
* *
1 1
2
*
2
*
2
*
. .
2
1
. . . .
2
1
*
2
1
2
1
} } } }
= = =
= =
=
= =
=
(
+ + =
J
i
Potential Energy
=
=
=
=
n
i
i
i
i i
i i i
r A g m P
ENERGY TIAL totalPOTEN
n i
r g m P
1
0
0
. .
.......... 1 , 0
. .
= = = =
=
=
n
i
i
i
i i r p
T
ir
n
i
i
p
i
r
i ip i
r A g m q q U J U L
PE KE FunctionL Legrangian
1
0
* *
1 1 1
. . . .
2
1
= = = = = =
+ =
n
i
i
i
i i
T
ji
n
i j
j
k
j
m
j jkm k
T
ji
n
i j
j
k
j jk i
r A g m U J U q U J U T
1
0
1 1
*
*
1
. . .
Lagrangian Formulation
n i
T
q
L
q
L
dt
d
i
i i
....... ,......... 3 , 2 , 1
* *
=
=
c
c
|
|
|
.
|
\
|
c
c
Torque
Matrix form of Lagrangian Formulation
T(t)= D[q(t)] q(t) + h[q(t), q(t)] + C[q(t)]
n i
C q q h q D T
i
n
k
n
k
n
m
m k ikm k ik i
. .......... .......... .......... .......... .......... .......... 2 , 1
1 1 1
* * * *
=
+ + =
= = =
Newton Euler Equations.
Newtons Equation:
Force = mass X acceleration
Euler Equation
ocity AngularVel
sor InertiaTen I
I I N
c
c c
+ =
e
e e
2
*
.
moment
Newton-Euler equations
e e e
e
I I r p
I r p
I
+ = = = =
= =
l n m f
n f
l m
m
Equation s Euler' Equation Newtons
Torque Net Force Net
Momentum Angular Momentum Linear
Inertia Mass
Moment of Inertia- measure of mass
distribution of a rotated body with 1 DOF
Inertia Tensor - measure of mass
distribution of a rotated body with 3 DOF
Inertia Tensor - generalization of scalar
moment of inertia.
Inertia Tensor
Inertia Tensor
(
(
(
(
+
+
+
} } }
} } }
} } }
dm y x yzdm xzdm
yzdm dm z x xydm
xzdm xydm dm z y
I
) (
) (
) (
2 2
2 2
2 2
}
+ = dm z y I
xx
) (
2 2
I : Inertia Tensor:
Diagonal terms :
moments of inertia
Off-diagonal terms :
products of inertia
}
= dm xy I
xy
) (
To calculate the joint torques required
to cause the motion the following
parameters are required.
Position
Velocity
Acceleration
Mass distribution of robot arm
Newton Euler Equations.
Outward Iterations to compute
velocities and accelerations
Rotational velocity
Linear & rotational acceleration of centre of
mass of each link.
Newton Euler Equations.
Outward Iterations
Rotational velocity
) 1 int(
) 1 (
) ( . . ) 1 (
1 (
*
1
) 1 (
1
1
) 1 (
*
) 1 ( 1
1
+
+
+
+ =
+
+
+
+
+
+
+ +
+
i ocityOfJo angularvel
i e axisOf Fram Z Z
i To R W i ame VectorOf Fr Rotational R
ocity angularvel
Z R
i
i
i i
i
i
i
i
i
i i i
i
u
e
u e e
i
i
i i
i
i
i
i
i
i
i
i
i
i
i i
i
i
i i
i
i
R
C isPRISMATI i Jo if
Z Z R R
AL isROTATION i Jo f
*
) 1 (
1
*
1
1
1
) 1 (
* *
1
1
) 1 (
*
) 1 (
*
) 1 (
1
*
1
) 1 int(
) 1 int(
e e
u u e e e
+
+
+
+
+
+
+
+
+
+ +
+
+
=
+
+ + =
+
Outward Iterations
Angular acceleration of joint (i+1)
Robot Motion Planning
Path planning
Geometric path
Issues: obstacle avoidance, shortest
path
Trajectory planning,
interpolate or approximate the
desired path by a class of polynomial
functions and generates a sequence
of time-based control set points for
the control of manipulator from the
initial configuration to its destination.
Task Plan
Action Plan
Path Plan
Trajectory
Plan
Controller
Sensor
Robot
Tasks
Path of an object
= curve in the configuration space
represented either by:
Mathematical expression, or
Sequence of points
Trajectory
= Path + assignment of time to points along the path
Motion Planning (MP), a general term, either:
Path planning, or
Trajectory planning
Path planning
design of only geometric (kinematic) specifications of
the positions and orientations of robots
Trajectory planning
path planning + design of linear and angular velocities
Path planning < Trajectory planning
at path planning, dynamics of robots unimportant or
neglected
path planning also used as first step in design of trajectories
Trajectory/Path
The space curve along which the
manipulator moves from initial location
to final destination.
To plan the trajectory one must satisfy
the following two constraints
Obstacle constraint
Path constraint
Controlling the manipulator (so that it
follows the pre planned path) is divided
into two sub problems.
Trajectory Planning
Motion control
Trajectory Planning
Trajectory Planning
Trajectory Planning methods generally
interpolate (or) approximate the desired
path by a class of polynomial and
generates a sequence of time based
control set points for the control of the
manipulator from initial location to final
destination.
Trajectory Planning
Trajectory
Planning
Manipulator
Dynamic
constraints
Path
constraints
Path
specifications
*
) (t q ) (
* *
t q ) (t q
Trajectory planning
Path Profile
Velocity Profile
Acceleration Profile
t0 t1 t2 tf
Time
q(t0)
q(t1)
q(t2)
q(tf)
Initial
Lift-off
Set down
Final
Joint i
t0 t1 t2 tf Time
Speed
t0 t1 t2 tf Time
Acceleration
The boundary conditions
1) Initial position
2) Initial velocity
3) Initial acceleration
4) Lift-off position
5) Continuity in position at t
1
6) Continuity in velocity at t
1
7) Continuity in acceleration at t
1
8) Set-down position
9) Continuity in position at t
2
10) Continuity in velocity at t
2
11) Continuity in acceleration at t
2
12) Final position
13) Final velocity
14) Final acceleration
Requirements
Initial Position
Position (given)
Velocity (given, normally zero)
Acceleration (given, normally zero)
Final Position
Position (given)
Velocity (given, normally zero)
Acceleration (given, normally zero)
Requirements
Intermediate positions
set-down position (given)
set-down position (continuous with
previous trajectory segment)
Velocity (continuous with previous
trajectory segment)
Acceleration (continuous with previous
trajectory segment)
Requirements
Intermediate positions
Lift-off position (given)
Lift-off position (continuous with previous
trajectory segment)
Velocity (continuous with previous
trajectory segment)
Acceleration (continuous with previous
trajectory segment)
Trajectory Planning
n-th order polynomial, must satisfy 14
conditions,
13-th order polynomial
4-3-4 trajectory
3-5-3 trajectory
0
0 1
2
2
13
13
= + + + + a t a t a t a
0 2
2
2
3
3
4
4
20 21
2
22
3
23 2
10 12
2
12
3
13
4
14 1
) (
) (
) (
n n n n n n
a t a t a t a t a t h
a t a t a t a t h
a t a t a t a t a t h
+ + + + =
+ + + =
+ + + + =
t0t1, 5 unknow
t1t2, 4 unknow
t2tf, 5 unknow
ROBOT PROGRAMMING
Robots need to be taught what they
are expected to do and how they
should do it. The teachjing of the work
cycle to a robot is known as robot
programming.
Interface the control system to external
sensors