Control

The City College of New York
1
Jizhong Xiao
Department of Electrical Engineering
City College of New York
jxiao@ccny.cuny.edu
Manipulator Control
Introduction to ROBOTICS
2
Outline

Homework Highlights
Robot Manipulator Control
Control Theory Review
Joint-level PD Control
Computed Torque Method
Non-linear Feedback Control
Midterm Exam Scope

3
Homework 2
+ s s
s s
+ s s
+ + s s
= =
y and x f or
y and x f or
y and x f or
y and x f or
x y a

0 90
90 180
180 90
90 0
) , ( 2 tan
u
u
u
u
u
2
1
sin = x
Joint variables ?
Find the forward kinematics, Roll-Pitch-Yaw
representation of orientation

Why use atan2 function?
Inverse trigonometric functions have multiple solutions:
? = x
2
3
cos = x
? = x

150 , 30 ) 2 / 1 ( sin
1
=

30 , 30 ) 2 / 3 ( cos
1
=

30 ) 2 / 3 , 2 / 1 tan( = a
) tan( ) tan( t k x x + = Limit x to [-180, 180] degree
4
Homework 3
Z1
x1
y1
x2
y2
Z2
m2
m1
1
u
2
u
L
L
(
(
(
(
+
+
= =
1 0 0 0
0 1 0 0
12 1 0 12 12
12 1 0 12 12
2
1
1
0
2
0
S S C S
C C S C
T T T
(
(
(
(
=
1 0 0 0
2
0
z z z z
y y y y
x x x x
p a s n
p a s n
p a s n
T
Find kinematics model of 2-link robot, Find the inverse kinematics solution
)
`
= +
= +
y
x
n
n
) sin(
) cos(
2 1
2 1
u u
u u
)
`
= + +
= + +
y
x
p
p
1 2 1
1 2 1
sin ) sin(
cos ) cos(
u u u
u u u
Inverse: know position (Px,Py,Pz) and
orientation (n, s, a), solve joint variables.
) , ( 2 tan
2 1 x y
n n a = +u u
) , ( 2 tan
1 x x y y
n p n p a = u
5
Homework 4
Z1
x1
y1
x2
y2
Z2
m2
m1
1
u
2
u
L
L
Find the dynamic model of 2-link
robot with mass equally distributed
) ( ) , ( ) ( q C q q H q q D + + = t
=
=
n
k i j
T
ji j jk ik
U J U Tr D
) , max(
) (
>
s
=
c
c
i j for
i j for T Q T
q
T
U
i
j j
j
j
i
ij
0
1
1
0 0
(
+
(
+
(
=
(
) (
) (
) , (
) , (
2
1
2
1
2
1
22 21
12 11
2
1
u
u
u u
u u
u
u
t
t
c
c
h
h
D D
D D

Calculate D, H, C terms directly
Physical meaning?
< <
> >
> >
=
c
c
k i or j i
k j i T Q T Q T
j k i T Q T Q T
U
q
U
i
j j
j
k k
k
i
k k
k
j j
j
ijk
k
ij
0
1
1
1
1
0
1
1
1
1
0
j
j ji
n
i j
j i
r U g m C

=
=
=
=
n
m k i j
T
ji j jkm ikm
U J U Tr h
) , , max(
) (
Interaction effects of motion of joints j & k on link i
6
Homework 4
Z1
x1
y1
x2
y2
Z2
m2
m1
1
u
2
u
L
L
Find the dynamic model of 2-link
robot with mass equally distributed
i
i i
q
L
q
L
dt
d
t =
c
c
c
c
) (
P K L =
) ( ) , ( ) ( q C q q H q q D + + = t
Derivation of L-E Formula
(
(
(
(
=
1
i
i
i
i
i
z
y
x
r
(
= =

}
}
= =
i
p
r p
T
ir
T
i
i
i
i ip
i
r
i i
q q U dm r r U Tr dK K
1 1
) (
2
1

= =
=
c
c
= =
i
j
i
i j ij
i
i
i
j
j
j
i
i i
i
r q U r q
q
T
r
dt
d
V V
1 1
0
0 0
) ( ) (
point at link i
Velocity of point
Kinetic energy of link i
i
J
Erroneous answer
(
(
(
(
=
1
0
0
L
r
i
i
7
Homework 4
X0
Y0
X1
Y1
1
u
L
m
(
(
(
(
=
1
0
0
1
1
l
r
Example: 1-link robot with point mass (m)
concentrated at the end of the arm.
Set up coordinate frame as in the figure
2
1
2
2
1
u
m l K =
1
8 . 9 u S l m P =
P K L =
1 1
2
1 1
8 . 9 ) ( u u
u u
t C l m m l
L L
dt
d
=
c
c
c
c
=

According to physical meaning:

8
Manipulator Control
9
Manipulator Dynamics Revisit
) ( ) , ( ) ( q C q q H q q D + + =

t
Dynamics Model of n-link Arm
The Acceleration-related Inertia term, Symmetric Matrix
The Coriolis and Centrifugal terms
The Gravity terms
(
(
(
=
n
t
t
t
1
Driving torque
applied on each link
Non-linear, highly coupled , second order differential equation
Joint torque Robot motion
10
Jacobian Matrix Revisit
4 4
6
0
1 0 0 0
=
p a s n
T
(
(
(
=
(
(
(
=
) (
) (
) (
3
2
1
q h
q h
q h
z
y
x
p
(
(
(
=
(
(
(
) (
) (
) (
) (
) (
) (
} , , {
6
5
4
q h
q h
q h
q
q
q
a s n
u
|
Forward Kinematics
(
(
(
(
= =
) (
) (
) (
) (
6
2
1
1 6
q h
q h
q h
q h Y
1 6 1 6
=
n n
q J Y
(
(
(
(
(
(
(
(
u
|
z
y
x
Y
1
2
1
6
) (
(
(
(
(
=
n
n
n
q
q
q
dq
q dh
n
n
n
n
n
q
h
q
h
q
h
q
h
q
h
q
h
q
h
q
h
q
h
dq
q dh
J
(
(
(
(
(
(
(
(
c
c
c
c
c
c
c
c
c
c
c
c
c
c
c
c
c
c
=
|
|
.
|
\
|
=
6
6
2
6
1
6
2
2
2
1
2
1
2
1
1
1
6
) (

11
Jacobian Matrix Revisit
Example: 2-DOF planar robot arm
Given l
1
, l
2
, Find: Jacobian
u
2
u
1
(x , y)
l
2

l
1

(
=
(
+ +
+ +
=
(
) , (
) , (
) sin( sin
) cos( cos
2 1 2
2 1 1
2 1 2 1 1
2 1 2 1 1
u u
u u
u u u
u u u
h
h
l l
l l
y
x
(
+ + +
+ +
=
(
(
(
(
c
c
c
c
c
c
c
c
=
) cos( ) cos( cos
) sin( ) sin( sin
2 1 2 2 1 2 1 1
2 1 2 2 1 2 1 1
2
2
1
2
2
1
1
1
u u u u u
u u u u u
u u
u u
l l l
l l l
h h
h h
J
(
=
(
=
2
1
u
u
J
y
x
Y
12
Joint Level Controller

=
= + +
) (
) ( ) , ( ) (
q h Y
q C q q H q q D t

Task Level Controller
Find a control input (tor),
t as q q
d
t as Y Y
d
0 = Y Y e
d
Find a control input (tor),
Robot Controller
_
tor
q q
d
q
d
q
d
q
Trajectory
Planner
q q e
d
=
e e
Task level
Planner
Robot
Dynamics
Controller
_
tor q q
d
Y
d
Y
Forward
Kinematics
Y Y
Y Y e
d
=
e e
Robot System:

13
Control Methods
Conventional Joint PID Control
Widely used in industry
Advanced Control Approaches
Computed torque approach
Nonlinear feedback
Adaptive control
Variable structure control
.

14
Control Theory Review (I)
actual
a
desired
d
V

Motor
actual
a
-
compute V using
PID feedback
d

a
Error signal e

PID controller: Proportional / Integral / Derivative control
e=
d

a

V = K
p
e + K
i
e dt + K
d
)
d e
dt
Closed Loop Feedback Control
Reference book: Modern Control Engineering, Katsuhiko Ogata, ISBN0-13-060907-2
15
Evaluating the response
How can we eliminate
the steady-state error?
steady-state error
settling time
rise time
overshoot
overshoot -- % of final value
exceeded at first oscillation
rise time -- time to span from
10% to 90% of the final value
settling time -- time to reach
within 2% of the final value
ss error -- difference from the
systems desired value
16
Control Performance, P-type
K
p
= 20
K
p
= 200
K
p
= 50
K
p
= 500
17
Control Performance, PI - type
K
p
= 100
K
i
= 50 K
i
= 200
18
Youve been integrated...
K
p
= 100
unstable &
oscillation
19
Control Performance, PID-type
K
p
= 100
K
i
= 200 K
d
= 2
K
d
= 10 K
d
= 20
K
d
= 5
20
PID final control
21
Control Theory Review (II)
Linear Control System
State space equation of a system

Example: a system:

Eigenvalue of A are the root of characteristic equation

Asymptotically stable all eigenvalues of A have negative
real part

Bu Ax x + =
=
=
u x
x x
2
2 1
u
x
x
x
x
(
+
(
=
(
1
0
0 0
1 0
2
1
2
1
0 = A I
0
0
1
2
= =
A I
(Equ. 1)
22
Control Theory Review (II)
Find a state feedback control such that the
closed loop system is asymptotically stable

Closed loop system becomes

Chose K, such that all eigenvalues of A=(A-BK) have
negative real parts

}
A
B
-K
x
x
u
x K u =
| |
(
=
2
1
2 1
x
x
k k u
x BK A x ) ( =
(Equ. 2)
0
1
'
1 2
2
2 1
= + + =
+
= k k
k k
A I

23
Control Theory Review (III)
Feedback linearization
Nonlinear system

Example:
U x G x f X ) ( ) ( + =
] ) ( ) ( ) ( [
1 1
V x G x f x G U

+ =
V X =
Dynamic
System
U
x
Nonlinear
Feedback
Linear System
V
U x x = +cos

V x U + = cos V x =

Original system:
Nonlinear feedback:
Linear system:
24
Robot Motion Control (I)
Joint level PID control
each joint is a servo-mechanism
adopted widely in industrial robot
neglect dynamic behavior of whole arm
degraded control performance especially in
high speed
performance depends on configuration
Robot Controller
_
tor
q q
d
q
d
q
d
q
Trajectory
Planner
q q e
d
=
e e
25
Robot Motion Control (II)
Computed torque method

Robot system:

Controller:

=
= + +
) (
) ( ) , ( ) (
q h Y
q C q q H q q D t
) ( ) , ( )] ( ) ( )[ ( q C q q H q q k q q k q q D tor
d
p
d
v
d
+ + + + =
0 ) ( ) ( ) ( = + + q q k q q k q q
d
p
d
v
d

0 = + + e k e k e
p v
Error dynamics
How to chose
Kp, Kv ?
Advantage: compensated for the dynamic effects
Condition: robot dynamic model is known
26
Robot Motion Control (II)
0 = + + e k e k e
p v

e x
e x
=
=
2
1
1 2 2
2 1
x k x k x
x x
p v
=
=
How to chose Kp, Kv

to make the system
stable?
Error dynamics
Define states:
AX
x
x
k k
x
x
v p
=
(

=
(
2
1
2
1
1 0
0
1
2
= + + =
+
=
p v
v p
k k
k k
A I
2
4
2
2 , 1
p v v
k k k
=
In matrix form:
Characteristic equation:
The eigenvalue of A matrix is:
Condition: have negative real part
0 >
v
k
0 >
p
k
One of a
selections:
27
Robot Motion Control (III)
=
= + +
) (
) ( ) , ( ) (
q h Y
q C q q H q q D t

q J q q h
dq
d
Y
= = )] ( [
q J q J Y

+ = ) (
1
q J Y J q

=

t = + +
) ( ) , ( ) ( ) (
1
q C q q H q J Y J q D

Task level
Planner
Robot
Dynamics
Linear
Controller
_
U
q
q
d
Y
d
Y
Forward
Kinematics
Y
Y
Y Y e
d
=
e
e Nonlinear
Feedback
tor
Linear System
Jocobian:
Robot System:
28
Robot Motion Control (III)
Task level
Planner
Robot
Dynamics
Linear
Controller
_
U
q
q
d
Y
d
Y
Forward
Kinematics
Y
Y
Y Y e
d
=
e
e Nonlinear
Feedback
tor
Linear System
) ( ) , ( ) ( ) (
1
q C q q H q J U J q D tor + + =

U J q D Y J q D
1 1
) ( ) (

=

Design the nonlinear feedback controller as:
Then the linearized dynamic model:
U Y =

Design the linear controller:
) ( ) ( Y Y k Y Y k Y U
d p d v d
+ + =

Error dynamic equation: 0 = + + e k e k e
p v

29
Project
Task level
Planner
Robot
Dynamics
Linear
Controller
_
U
q
q
d
Y
d
Y
Forward
Kinematics
Y
Y
Y Y e
d
=
e
e Nonlinear
Feedback
tor
Linear System
Simulation study of Non-linear Feedback Control
30
Thank you!
x
y
z
x
y
z
x
y
z
x
z
y
HWK 5 posted on the web, Due: Oct. 23 before class
Next Class (Oct. 23): Midterm Exam
Time: 6:30-9:00, Please on time!

Control

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Control

Uploaded by

Copyright:

Available Formats

The City College of New York

According to physical meaning:

The City College of New York

The City College of New York

How to chose Kp, Kv

You might also like