The City College of New York
1
Jizhong Xiao
Department of Electrical Engineering
City College of New York
Manipulator Control
Introduction to ROBOTICS
The City College of New York
2
Outline
• Homework Highlights
• Robot Manipulator Control– Control Theory Review– Joint-level PD Control– Computed Torque Method– Non-linear Feedback Control
• Midterm Exam Scope
The City College of New York
3
Homework 2
yandxfor
yandxfor
yandxfor
yandxfor
xya
090
90180
18090
900
),(2tan
2
1sin x
Joint variables ?
Find the forward kinematics, Roll-Pitch-Yaw representation of orientation
Why use atan2 function?
Inverse trigonometric functions have multiple solutions:
?x2
3cos x?x
150,30)2/1(sin 1
30,30)2/3(cos 1
30)2/3,2/1tan( a
)tan()tan( kxx Limit x to [-180, 180] degree
The City College of New York
4
Homework 3
Z1
x1y1
x2
y2
Z2
m 2
m 1
1
2
LL
1000
0100
12101212
12101212
21
10
20
SSCS
CCSC
TTT
1000
20
zzzz
yyyy
xxxx
pasn
pasn
pasn
T
Find kinematics model of 2-link robot, Find the inverse kinematics solution
y
x
n
n
)sin(
)cos(
21
21
y
x
p
p
121
121
sin)sin(
cos)cos(
Inverse: know position (Px,Py,Pz) and orientation (n, s, a), solve joint variables.
),(2tan21 xy nna
),(2tan1 xxyy npnpa
The City College of New York
5
Homework 4
Z1
x1y1
x2
y2
Z2
m 2
m 1
1
2
LLFind the dynamic model of 2-link
robot with mass equally distributed
)(),()( qCqqHqqD
n
kij
Tjijjkik UJUTrD
),max(
)(
ijfor
ijforTQT
q
TU
ijj
j
j
i
ij0
11
00
)(
)(
),(
),(
2
1
2
1
2
1
2221
1211
2
1
c
c
h
h
DD
DD
• Calculate D, H, C terms directly
Physical meaning?
kiorji
kjiTQTQT
jkiTQTQT
Uq
U ijj
jkk
k
ikk
kjj
j
ijkk
ij
01
11
10
11
11
0
jjji
n
ijji rUgmC
n
mkij
Tjijjkmikm UJUTrh
),,max(
)(
Interaction effects of motion of joints j & k on link i
The City College of New York
6
Homework 4
Z1
x1y1
x2
y2
Z2
m 2
m 1
1
2
LLFind the dynamic model of 2-link
robot with mass equally distributed
iii q
L
q
L
dt
d
)(
PKL
)(),()( qCqqHqqD
• Derivation of L-E Formula
1i
i
i
ii z
y
x
r
i
prp
Tir
Tii
iiip
i
rii qqUdmrrUTrdKK
1 1
)(2
1
i
j
iijij
ii
i
jj
j
iii
i rqUrqq
Tr
dt
dVV
11
000 )()(
point at link i
Velocity of point
Kinetic energy of link iiJ
Erroneous answer
1
0
0
L
r ii
The City College of New York
7
Homework 4
X0
Y0
X1Y1
1
L
m
1
0
011
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
21
2
2
1 mlK
18.9 SlmP
PKL
112
11
8.9)(
ClmmlLL
dt
d
According to physical meaning:
The City College of New York
8
Manipulator Control
The City College of New York
9
Manipulator Dynamics Revisit
)(),()( qCqqHqqD • Dynamics Model of n-link Arm
The Acceleration-related Inertia term, Symmetric Matrix
The Coriolis and Centrifugal terms
The Gravity terms
n
1 Driving torque applied on each link
Non-linear, highly coupled , second order differential equation
Joint torque Robot motion
The City College of New York
10
Jacobian Matrix Revisit
44
60 1000
pasnT
)(
)(
)(
3
2
1
qh
qh
qh
z
y
x
p
)(
)(
)(
)(
)(
)(
},,{
6
5
4
qh
qh
qh
q
q
q
asn
Forward Kinematics
)(
)(
)(
)(
6
2
1
16
qh
qh
qh
qhY
1616 nnqJY
z
y
x
Y
1
2
1
6
)(
nn
n
q
q
q
dq
qdh
nn
n
n
n
q
h
q
h
q
h
q
h
q
h
q
hq
h
q
h
q
h
dq
qdhJ
6
6
2
6
1
6
2
2
2
1
2
1
2
1
1
1
6
)(
The City College of New York
11
Jacobian Matrix Revisit• Example: 2-DOF planar robot arm
– Given l1, l2 , Find: Jacobian2
1
(x , y)
l2
l1
),(
),(
)sin(sin
)cos(cos
212
211
21211
21211
h
h
ll
ll
y
x
)cos()cos(cos
)sin()sin(sin
21221211
21221211
2
2
1
2
2
1
1
1
lll
lllhh
hh
J
2
1
Jy
xY
The City College of New York
12
Robot Manipulator Control
• Joint Level Controller
)(
)(),()(
qhY
qCqqHqqD
• Task Level Controller
Find a control input (tor), tasqq d
tasYY d 0 YYe d Find a control input (tor),
R obotC ontro ller_
tor qqdq dq
dqTrajectory
P lanner
qqe d e e
Task levelP lanner
R obotD ynam ics
C ontro ller_
tor qqdY dY Forw ard
K inem atics
Y YYYe d
ee
• Robot System:
The City College of New York
13
Robot Manipulator Control
• Control Methods– Conventional Joint PID Control
• Widely used in industry
– Advanced Control Approaches• Computed torque approach• Nonlinear feedback • Adaptive control• Variable structure control• ….
The City College of New York
14
Control Theory Review (I)
actual adesired 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 = Kp • e + Ki ∫ e dt + Kd )d e dt
Closed Loop Feedback Control
Reference book: Modern Control Engineering, Katsuhiko Ogata, ISBN0-13-060907-2
The City College of New York
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 system’s desired value
The City College of New York
16
Control Performance, P-type
Kp = 20
Kp = 200
Kp = 50
Kp = 500
The City College of New York
17
Control Performance, PI - type
Kp = 100
Ki = 50 Ki = 200
The City College of New York
18
You’ve been integrated...
Kp = 100
unstable &
oscillation
The City College of New York
19
Control Performance, PID-typeKp = 100 Ki = 200 Kd = 2
Kd = 10 Kd = 20
Kd = 5
The City College of New York
20
PID final control
The City College of New York
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
BuAxx
ux
xx
2
21
ux
x
x
x
1
0
00
10
2
1
2
1
0 AI 00
1 2
AI
(Equ. 1)
The City College of New York
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 xu
xKu
2
121 x
xkku
xBKAx )(
(Equ. 2)
01
' 122
21
kkkk
AI
The City College of New York
23
Control Theory Review (III)• Feedback linearization
– Nonlinear system
– Example:
UxGxfX )()(
])()()([ 11 VxGxfxGU
VX
D ynam icSystem
U xN onlinearFeedback
Linear System
V
Uxx cos
VxU cos Vx
Original system:
Nonlinear feedback:
Linear system:
The City College of New York
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
R obotC ontro ller_
tor qqdq dq
dqTrajectory
P lanner
qqe d e e
The City College of New York
25
Robot Motion Control (II)• Computed torque method
– Robot system:
– Controller:
)(
)(),()(
qhY
qCqqHqqD
)(),()]()()[( qCqqHqqkqqkqqDtor dp
dv
d
0)()()( qqkqqkqq dp
dv
d
0 ekeke pv Error dynamics
How to chose Kp,
Kv ?
Advantage: compensated for the dynamic effects
Condition: robot dynamic model is known
The City College of New York
26
Robot Motion Control (II)
0 ekeke pv
ex
ex
2
1
122
21
xkxkx
xx
pv
How to chose Kp, Kv to make the system
stable?Error dynamics
Define states:
AXx
x
kkx
x
vp
2
1
2
1 10
01
2
pvvp
kkkk
AI
2
42
2,1
pvv kkk
In matrix form:
Characteristic equation:
The eigenvalue of A matrix is:
Condition: have negative real part0vk0pk
One of a selections:
The City College of New York
27
Robot Motion Control (III)
)(
)(),()(
qhY
qCqqHqqD
qJqqhdq
dY )]([
• Non-linear Feedback Control
qJqJY )(1 qJYJq
)(),()()( 1 qCqqHqJYJqD
Task levelP lanner
R obotD ynam ics
LinearC ontro ller
_
U
q
qdY
dYForw ard
K inem aticsY
Y
YYe d
ee N onlinear
Feedback
tor
L inear System
Jocobian:
Robot System:
The City College of New York
28
Robot Motion Control (III)
Task levelP lanner
R obotD ynam ics
LinearC ontro ller
_
U
q
qdY
dYForw ard
K inem aticsY
Y
YYe d
ee N onlinear
Feedback
tor
L inear System
)(),()()( 1 qCqqHqJUJqDtor
UJqDYJqD 11 )()(
Design the nonlinear feedback controller as:
• Non-linear Feedback Control
Then the linearized dynamic model:
UY
Design the linear controller: )()( YYkYYkYU dpdvd
Error dynamic equation: 0 ekeke pv
The City College of New York
29
Midterm Exam Scopes
The City College of New York
30
Thank you!
x
yz
x
yz
x
yz
x
z
y
HWK 5 posted on the web,
Next Class: Midterm Exam
Time: 6:30-9:00, Please on time!