Manipulator Control
description
Transcript of Manipulator Control
1The City College of New York
Jizhong XiaoDepartment of Electrical Engineering
City College of New [email protected]
Manipulator Control
Introduction to ROBOTICS
2The City College of New York
Outline
• Homework Highlights• Robot Manipulator Control
– Control Theory Review– Joint-level PD Control– Computed Torque Method– Non-linear Feedback Control
• Midterm Exam Scope
3The City College of New York
Homework 2
yandxforyandxforyandxforyandxfor
xya
09090180
18090900
),(2tan
21sin x
Joint variables ?
Find the forward kinematics, Roll-Pitch-Yaw representation of orientation
Why use atan2 function?
Inverse trigonometric functions have multiple solutions:
?x23cos 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
4The City College of New York
Homework 3
Z1
x1y1
x2
y2
Z2
m 2
m 1
1
2
LL
10000100
1210121212101212
21
10
20
SSCSCCSC
TTT
1000
20
zzzz
yyyy
xxxx
pasnpasnpasn
T
Find kinematics model of 2-link robot, Find the inverse kinematics solution
y
x
nn
)sin()cos(
21
21
y
x
pp
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
5The City College of New York
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(
)(
ijforijforTQT
qTU
ijj
j
j
i
ij 01
100
)()(
),(),(
2
1
2
1
2
1
2221
1211
2
1
cc
hh
DDDD
• Calculate D, H, C terms directly
Physical meaning?
kiorjikjiTQTQTjkiTQTQT
UqU i
jjjkk
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
6The City College of New York
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 qL
qL
dtd
)(
PKL
)(),()( qCqqHqqD
• Derivation of L-E Formula
1i
i
i
ii z
yx
r
i
prp
Tir
Tii
iiip
i
rii qqUdmrrUTrdKK
1 1
)(21
i
j
iijij
ii
i
jj
j
iii
i rqUrqqTr
dtdVV
11
000 )()(
point at link i
Velocity of point
Kinetic energy of link iiJ
Erroneous answer
100L
r ii
7The City College of New York
Homework 4
X0
Y0
X1Y1
1
L
m
1001
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
21
2
21 mlK
18.9 SlmP
PKL
112
11
8.9)(
ClmmlLLdtd
According to physical meaning:
8The City College of New York
Manipulator Control
9The City College of New York
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
10The City College of New York
Jacobian Matrix Revisit
44
60 1000
pasnT
)()()(
3
2
1
qhqhqh
zyx
p
)()()(
)()()(
},,{
6
5
4
qhqhqh
qqq
asn
Forward Kinematics
)(
)()(
)(
6
2
1
16
qh
qhqh
qhY
1616 nnqJY
zyx
Y
1
2
1
6
)(
nn
n
q
dqqdh
nn
n
n
n
qh
qh
qh
qh
qh
qh
qh
qh
qh
dqqdhJ
6
6
2
6
1
6
2
2
2
1
2
1
2
1
1
1
6
)(
11The City College of New York
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
hh
llll
yx
)cos()cos(cos)sin()sin(sin
21221211
21221211
2
2
1
2
2
1
1
1
llllll
hh
hh
J
2
1
Jyx
Y
12The City College of New York
Robot Manipulator Control
• Joint Level Controller
)()(),()(
qhYqCqqHqqD
• Task Level Controller
Find a control input (tor), tasqq d
tasYY d 0 YYe d Find a control input (tor),
R obotC ontro lle r_
tor qqdq dq
dqTra jectory
P lanner
qqe d e e
Task leve lP lanner
R obotD ynam icsC ontro ller
_
tor qqdY dY Forw ard
K inem aticsY Y
YYe d ee
• Robot System:
13The City College of New York
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• ….
14The City College of New York
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 controle= 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
15The City College of New York
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
16The City College of New York
Control Performance, P-type
Kp = 20
Kp = 200
Kp = 50
Kp = 500
17The City College of New York
Control Performance, PI - type
Kp = 100
Ki = 50 Ki = 200
18The City College of New York
You’ve been integrated...
Kp = 100
unstable &
oscillation
19The City College of New York
Control Performance, PID-typeKp = 100 Ki = 200 Kd = 2
Kd = 10 Kd = 20
Kd = 5
20The City College of New York
PID final control
21The City College of New York
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
uxxx
2
21
uxx
xx
10
0010
2
1
2
1
0 AI 00
1 2
AI
(Equ. 1)
22The City College of New York
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 xx
kku
xBKAx )(
(Equ. 2)
01
' 122
21
kkkk
AI
23The City College of New York
Control Theory Review (III)• Feedback linearization
– Nonlinear system
– Example:
UxGxfX )()(
])()()([ 11 VxGxfxGU
VX
Dynam icSystem
U xNonlinearFeedback
Linear System
V
Uxx cos
VxU cos Vx
Original system:
Nonlinear feedback:
Linear system:
24The City College of New York
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
RobotContro ller_
tor qqdq dq
dqTrajectory
P lanner
qqe d e e
25The City College of New York
Robot Motion Control (II)• Computed torque method
– Robot system:
– Controller:
)()(),()(
qhYqCqqHqqD
)(),()]()()[( 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
26The City College of New York
Robot Motion Control (II)
0 ekeke pv
exex
2
1
122
21
xkxkxxx
pv
How to chose Kp, Kv to make the system
stable?Error dynamics
Define states:
AXxx
kkxx
vp
2
1
2
1 10
01 2
pvvp
kkkk
AI
2
42
2,1pvv kkk
In matrix form:
Characteristic equation:
The eigenvalue of A matrix is:
Condition: have negative real part0vk0pk
One of a selections:
27The City College of New York
Robot Motion Control (III)
)()(),()(
qhYqCqqHqqD
qJqqhdqdY )]([
• Non-linear Feedback Control
qJqJY )(1 qJYJq
)(),()()( 1 qCqqHqJYJqD
Task levelP lanner
RobotD ynam ics
LinearContro ller_
U
qqdY
dYForw ard
K inem aticsYY
YYe d
ee Nonlinear
Feedbacktor
L inear System
Jocobian:
Robot System:
28The City College of New York
Robot Motion Control (III)
Task leve lP lanner
RobotD ynam ics
LinearController_
U
qqdY
dYForw ard
K inem aticsYY
YYe d
ee Nonlinear
Feedbacktor
Linear 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
29The City College of New York
Project
Task levelP lanner
RobotDynam ics
LinearControlle r_
U
qqdY
dYForw ard
K inem aticsYY
YYe d
ee Nonlinear
Feedbacktor
Linear System
• Simulation study of Non-linear Feedback Control
30The City College of New York
Thank you!
x
yz
x
yz
x
yz
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!