Post on 22-Mar-2020
Robot control
Lecture 4Mikael Norrlöf
Substantial contribution by Stig Moberg
PhDCourse Robot modeling and controlLecture 4
Up till now
§ Lecture 1� Rigid body motion� Representation of rotation� Homogenous transformation
§ Lecture 2� Kinematics
• Position• Velocity via Jacobian• DH parameterization
§ Lecture 3� Dynamics� Lagrange’s equation� Newton Euler
Next lecture 15/110-12 Algoritmen
PhDCourse Robot modeling and controlLecture 4
Robot control - examples
§ Fanta challengehttp://www.youtube.com/watch?v=PSKdHsqtok0
§ Fanta challenge IIhttp://www.youtube.com/watch?v=SOESSCXGhFo
PhDCourse Robot modeling and controlLecture 4
Outline
§ Robot Motion Control Overview§ Current and Torque Control§ Control Methods for Rigid Robots§ Control Methods for Flexible Robots§ Interaction with the environment
Chapters 6, 8, 9
PhDCourse Robot modeling and controlLecture 4
Robot motion control
RobotModels
u(t)
Zd(t)
User Program
MoveL x1, v1MoveL x2, v2
x1 x2
x1 x2
TrajectoryInterpolator
Controllery(t)xi,vi
v1v2
v2
v1
Trajectory Interpolator• Path Zd(p)• Timing law p(t) gives Zd(t)• Time Optimal Trajectory• Limited d/dt (spd, acc, jerk, ...)
Models• Kinematic Model• Rigid Dynamic Model• Flexible Dynamic Model
Controller• Position Control (Tracking and Regulation)• Force and Compliance Control
PhDCourse Robot modeling and controlLecture 4
R
LV
I
ItKτ
RILdtdIV
=
+=
Simple Model of 3-Phase PM Motor
PI
RILdtdI
+
1tK -refτ
τ
measuredI
Simplified PI + FFW Control
Torqueactingon rotor
Torque and Current Control
PhDCourse Robot modeling and controlLecture 4
FeedForwardCalculation
Torqueto
Current
PIControl
Irefq
refτ Uref
q
d-qto
stator
mq&
0Irefd =
statortod-q
FeedForwardCalculation
PIControl
Uref
d
PWM
Imeasuredq
Imeasuredd
MeasuredCurrents
mq
Torque and Current Control
mq
PhDCourse Robot modeling and controlLecture 4
5 10 15 20 25 30 35 40 45 500
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
Motor Number
Torq
ueC
onst
ant[
Nm
/A]
0 1 2 3 4 5 611.96
11.97
11.98
11.99
12
12.01
12.02
12.03
12.04
12.05
Motor Angular Position [rad]
Torq
ue[N
m]
Reference TorqueMotor Torque
Torque Ripple Disturbances Measurement DisturbancesGain (Kt) uncertainty
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5-1
-0.5
0
0.5
1Differentiated deterministic disturbance
Spee
d[r
ad/s
]
0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5
-5
0
5
Differentiated stochastic disturbance
Time [s]
Spee
d[r
ad/s
]
Rotor SensorKt Gear LinkCurrent
MotorPositionTrend: Reduced Size => Lower Inertia
Torque and Current Control
PhDCourse Robot modeling and controlLecture 4
The Robot Control Problem
The user-specified path Zd must be followed with specified precision evenunder the influence of different uncertainties. These uncertainties are disturbances actingon the robot and on the measurements, as well as uncertainties in the models used bythe motion control system.
PhDCourse Robot modeling and controlLecture 4
The Robot Control Problem
-60 -40 -20 0 20 40 60
-90
-80
-70
-60
-50
-40
-30
-20
-10
0
X [mm]
Y[m
m]
Max Path Following Error 3.40 mm
Ref PosPosMax Error
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10
0.05
0.1
Pos
[rad]
Typical Requirements:q Settling Time 0.1 sq Path Error < 0.1 mm @ 20 mm/sq Path Error 1 mm @ 1000 mm/sq Speed Accuracy 5 %q Absolute Accuracy 1 mmq Repeatability < 0.1 mm
PhDCourse Robot modeling and controlLecture 4
Independent Joint PD Control
q Integral part added in most casesq Speed normally not measured, estimated from positionq D-part on reference can be removed (gives overshoot if I-part is added!)
)()( qqKqqKu dvdp && -+-=
PhDCourse Robot modeling and controlLecture 4
Cascade Controller
-vK Robot
dtd
q
ò iK
dq&
Inner Loop: Speed (PI) Controller
PhDCourse Robot modeling and controlLecture 4
Cascade Controller
Outer Loop: Position Controller (P) – Inner Loop: Speed (PI) Controller
-
dq
-vKpK Robot
q
dtd
ò iK
dq&
PhDCourse Robot modeling and controlLecture 4
Feedforward + PD Control
)(),()( dddddffw qgqqcqqMu ++= &&&
)()( qqKqqKuu dvdpffw && -+-+=
PhDCourse Robot modeling and controlLecture 4
)()( qqKqqKqv dvdpd &&&& -+-+=
Feedback Linearization + PD Control
)(),()( qgqqcvqMu ++= &
PhDCourse Robot modeling and controlLecture 4
Feedback Linearization + PD Control
Feedback Linearization is sometimes called ComputedTorque or Inverse Dynamics, so is Feedforward Control!
vs1qvq)()ˆ(
u(q)g)q(q,c(q)vM
ug(q))qc(q,qM(q)
2=Þ=Þ*=*
=++
=++
&&
&
&&&
Decoupledsystem ofdoubleintegrators!
PhDCourse Robot modeling and controlLecture 4
Simulation Example – Simple Robot
Js1
s1pK
vKqq&
-
-
dq
25Kv158Kp
1J
==
=
Closed LoopPoles -12.6, -12.6
i.e. damping = 1=> no overshoot
PhDCourse Robot modeling and controlLecture 4
Simulation Example – Simple Robot
Js1
s1pK
vKqq&
-
-
dq
Closed Loop Poles-12.6, -12.6Zero -6
dq&
25Kv158Kp
1J
==
=
PhDCourse Robot modeling and controlLecture 4
Simulation Example – Simple Robot
Js1
s1pK
vKqq&
-
-
dq
Closed LoopPoles -124, -1.3Zero -1.3
dq&
126Kv158Kp
1J
==
=
PhDCourse Robot modeling and controlLecture 4
Simulation Example – Simple Robot
Js1
s1pK
vKqq&
-
-
dq
dq&
25Kv158Kp
1J1J
==
==
ˆ
Jdq&&
”2-DOF” Controller:Tracking &Regulation/Robustness”decoupled”
PhDCourse Robot modeling and controlLecture 4
Simulation Example – Simple Robot
Js1
s1pK
vKqq&
-
-
dq
dq&
25Kv158Kp
1J0.7J
==
==
ˆ
Jdq&&
PhDCourse Robot modeling and controlLecture 4
Robustification of Feedback Linearization
)ee,η(v,q)()ˆ(
eKeKqqqe
u(q)g)q(q,c(q)M
ug(q))qc(q,qM(q)
vpd
d
&&&
&&&
&
&&&
+=Þ*¹*
++=-=
=++
=++
q
q
q
a
a
a
Robust outer loop design (Second Method of Lyapunov)
Worst case estimation of => discontinuous control termcan be added to outer loop control:
η aΔ
t),e(e,ΔeKeKq vpd &&&& aaq +++=
q High sample rate requiredq Must know max acceleration & worst case error in M, c, and gq Approximate continuous version
PhDCourse Robot modeling and controlLecture 4
ρ
Passivity Based Robust Control
q)Λ(qqqr)qqΛ(qva
q)Λ(qqvKr(q)g)vq(q,c(q)aMu
dd
dd
dd
-+-=-+==
-+=+++=
&&
&&&&&
&
&Control Input:
q K and are diagonal matrices.q Closed loop system is coupled and nonlinearq Linear parametrization of dynamic model is usedq Bound does not depend on trajectory or state
Λ
Krδθ)v)(θa,,qY(q,u 0 ++= &
ïïî
ïïí
ì
£
>=
εrY;ε
rYρ
εrY;rYrYρ
δθT
T
TT
T
ρθθ0 £-
PhDCourse Robot modeling and controlLecture 4
Passivity Based Adaptive Control
Control Input: Krθv)a,,qY(q,u += &
a)rv,,q(q,YΓθ T1 && --=Parameter update:
PhDCourse Robot modeling and controlLecture 4
Robustified Feedback Linearization (Lyap 2)
Js1
s1pK
vKqq&
-
-
dq
dq& J
dq&& aΔ
ïî
ïíì ++-
=0
ww)eeγ(γ
Δ22
11 &a
25Kv158Kp
1J
0.7J
==
=
=
epepw 2221 +=
0w;
0w;
=
¹
q p21, p22, , , computed from J uncertainty, Kv, Kp, andq Lyapunov equation => p21, p22
1γ 2γmaxdq&&
PhDCourse Robot modeling and controlLecture 4
Robustified Feedback Linearization (Lyap 2)
Discontinuous aΔ
PhDCourse Robot modeling and controlLecture 4
Robustified Feedback Linearization (Lyap 2)
Continuous aΔPhDCourse Robot modeling and controlLecture 4
Path Accuracy: Circle r = 50 mm @ 1 s
XY rigid robot with PD (fdb) P (ref) control
-60 -40 -20 0 20 40 60
-90
-80
-70
-60
-50
-40
-30
-20
-10
0
X [mm]
Y[m
m]
Max Error 44.36 mm, Bandwidth 1 Hz (x) 1 Hz (y)
Ref PosPosMax Error
PhDCourse Robot modeling and controlLecture 4
Path Accuracy: Circle r = 50 mm @ 1 s
XY rigid robot with PD (fdb) P (ref) control
-60 -40 -20 0 20 40 60
-90
-80
-70
-60
-50
-40
-30
-20
-10
0
X [mm]
Y[m
m]
Max Error 0.84 mm, Bandwidth 15 Hz (x) 15 Hz (y)
Ref PosPosMax Error
PhDCourse Robot modeling and controlLecture 4
Path Accuracy: Circle r = 50 mm @ 1 s
XY rigid robot with PD (fdb) P (ref) control
-60 -40 -20 0 20 40 60
-90
-80
-70
-60
-50
-40
-30
-20
-10
0
X [mm]
Y[m
m]
Max Error 2.01 mm, Bandwidth 15 Hz (x) 20 Hz (y)
Ref PosPosMax Error
PhDCourse Robot modeling and controlLecture 4
Path Accuracy: Circle r = 50 mm @ 1 s
XY rigid robot with PD (fdb) P (ref) control
-60 -40 -20 0 20 40 60
-90
-80
-70
-60
-50
-40
-30
-20
-10
0
X [mm]
Y[m
m]
Max Error 7.88 mm, Bandwidth 50 Hz (x) 10 Hz (y)
Ref PosPosMax Error
Bandwidth must be sufficientlyhigh relative frequency content oftrajectory
All channels should be equal!
PhDCourse Robot modeling and controlLecture 4
Flexible Joint Model
)qf()qqD()qK(q)q,(qcqSqMτ)qqD()qK(q)g(q)q,q,(qcqSq)(qM0
mmamaaa2mT
mm
mamaamaa1maaa
&&&&&&&&
&&&&&&&&
+----++=-+-++++=
For N links and N motors: 2N d.o.f.
)qf()qqD()qK(qqMτ)qqD()qK(q)g(q)q,c(qq)(qM0
mmamamm
mamaaaaaaa&&&&&
&&&&&
+----=-+-+++=
)qK(qqMτ)qK(q)g(q)q,c(qq)(qM0
mamm
maaaaaaa
--=-+++=
&&
&&&
Complete Model:
Simplified Model I (high gear ratio):
Simplified Model II (friction and damping low):
PhDCourse Robot modeling and controlLecture 4
Linear One Axis Model
mJ aJk
d
τaqmq
az kJ
12dς =
az J
kω =ma
map JJ
)Jk(Jω +=
( )
( ) ÷÷ø
öççè
æ+++
+=
÷÷ø
öççè
æ+++
++=
®
®
1ω
s2ςωsJJs
ksdG
1ω
s2ςωsJJs
1ω
s2ςωs
G
p
p2p
2
ma2
qτ
p
p2p
2
ma2
z
z2z
2
qτ
a
m
ma
map JkJ
JJ2dς +
=
PhDCourse Robot modeling and controlLecture 4
PD Control
mqmq&
pK
vK-
-
dq
Closed Loop Bandwidth:10 HzMechanical Resonance:6 Hz
PhDCourse Robot modeling and controlLecture 4
PD Control – Motor Feedback
mqmq&
pK
vK-
-
dq
Closed Loop Bandwidth:3.5 HzMechanical Resonance:6 Hz
PhDCourse Robot modeling and controlLecture 4
PD + FFW Control – Motor Feedback
mqmq&
pK
vK-
-
dqJdq&&
dq&
Same PD Tuning as onprevious slide(Poles @ 3.5 Hz)
PhDCourse Robot modeling and controlLecture 4
PD Control – Arm Feedback
aqaq&
pK
vK-
-
dq
Same PD Tuning as onprevious slide => unstable
PhDCourse Robot modeling and controlLecture 4
PD Control – Arm Feedback
aqaq&
pK
vK-
-
dq
Low Gain Controller
PhDCourse Robot modeling and controlLecture 4
PD Control – Arm Pos / Motor Speed Feedback
aqmq&
pK
vK-
-
dq
Motor Feedback preferredfor flexible joint robot if onlytwo states are measured
PhDCourse Robot modeling and controlLecture 4
LQ Control / State Feedback – Full State Measurement
[ ]amam qqqqx &&=
0L
L-dq
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 10
0.05
0.1Po
s[r
ad]
Ref PositionMotor PositionArm Position
0 0.05 0.1 0.15 0.2 0.250
0.01
0.02
0.03
0.04
0.05
0.06
0.07
0.08
0.09
0.1
Pos
[rad]
Ref PositionMotor PositionArm Position
PhDCourse Robot modeling and controlLecture 4
Control of a flexible robot arm: A benchmark problem
qWhat is the limit for control using motor measurements only?
q Design of a robust digital controller for optimal disturbance rejection
q One axis uncertain model of an industrial robot
PhDCourse Robot modeling and controlLecture 4
Swedish Open Championships in Robot Control 2005
www.robustcontrol.org
PhDCourse Robot modeling and controlLecture 4
Nonlinear MIMO Flexible Joint Control
(t)ZZ(t) d=Goal: Perfect tracking, i.e
PhDCourse Robot modeling and controlLecture 4
Suggested Control Methods
Feedback LinearizationPassivity-Based ControlBacksteppingAdaptive ControlNeural NetworksSingular PerturbationsComposite ControlInput ShapingRobust Control based on Lyaponov 2nd MethodSliding Mode ControlIterative Learning ControlFeedforward ControlLinear MIMO Control (Pole Placement, LQG, H infinity, …)Linear Diagonal Control (PID, Pole Placement, …)……
PhDCourse Robot modeling and controlLecture 4
-1.5 -1 -0.5 0 0.5 1 1.5-1000
-800
-600
-400
-200
0
200
400
600
800
1000
Torq
ue[N
m]
Deflection [arcmin]
-300 -200 -100 0 100 200 300-4
-3
-2
-1
0
1
2
3
4
Speed [rad/s]
Fric
tion
[Nm
]
Measured FrictionModel
Elasticity with hysteresis in rotational direction
Friction
Bearing ElasticityTorque Ripple
Compact Gear Transmission
PhDCourse Robot modeling and controlLecture 4
Nonlinear Flexibility
PhDCourse Robot modeling and controlLecture 4
Extended Flexible Joint Model
For N links, N motors and Munactuated joints: 2N + M d.o.f.
PhDCourse Robot modeling and controlLecture 4
Extended Flexible Joint Model
PhDCourse Robot modeling and controlLecture 4
Feedforward Control of Extended Flexible Joint
)Γ(q(t)Zvqvq
u)v,v,q,(qτvM)v,v,q,(qτ)g(q)v,c(qv)(qM
ad
mm
aa
mamammm
mamaaaaaaaa
===
+=++=
&
&
&
&
q Perfect tracking & Point-To-Pointq A High Index DAE must be solvedq Solution for M=2, N=3 & M=3, N=9q Non-minimum Phase?
InverseDynamics
PhDCourse Robot modeling and controlLecture 4
Nonlinear Simulation Model
§ 2 actuated DOF (N = 2)§ 1 unactuated DOF (M = 1)§ 5 DOF
PhDCourse Robot modeling and controlLecture 4
Simulation Model – The Equations
PhDCourse Robot modeling and controlLecture 4
Non-minimum Phase
0 0.05 0.1 0.15 0.2 0.25 0.3
-5000
0
5000
u1
u 1[N
m]
Time [s]
0 0.05 0.1 0.15 0.2 0.25 0.3-4000
-2000
0
2000
4000
u2
u 2[N
m]
Time [s]
• Extended Flexible Joint & Flexible Link is normally NMP forperfect tracking
• Example: 5 DOF 2 Axis Model:Non-causal solution, movement starts at t = 0.1 s
PhDCourse Robot modeling and controlLecture 4
Control when in contact with the environment
§ Position and speed controlnot enough
§ Why?
§ Sensor configuration� Wrist torque/force sensor� Joint torque/force sensor� Tactile
Figure 9.2 in Spong etal
PhDCourse Robot modeling and controlLecture 4
Force and torque when in contact with environment
Virtual displacement (again)
virtual work
qqJX dd )(=
qXFw TT dtdd -=
( ) qqJF TT dt-= )(
FqJ T )(=Þt
PhDCourse Robot modeling and controlLecture 4
Complete dynamics
uFqJqgqqqCqqM eT =+++ )()(),()( &&&&
Figure 9.2 in Spong etal
PhDCourse Robot modeling and controlLecture 4
Coordinate frames and constraints
Let the velocity (Twist) be defined
and the force (Wrenche)
Reciprocity condition
úû
ùêë
é=
wx
v
úû
ùêë
é=
nf
F
0=+= nfvF TTT wx
PhDCourse Robot modeling and controlLecture 4
Task constraints
Natural constraints. Constraints imposed by the task.
Example:
Natural constraints Artificial constraints
0=+++++= zzyyxxzzyyxxT nnnfvfvfvF wwwx
Figure 9.3 in Spong etal0
000
00
=
===
==
z
y
x
z
y
x
n
fvv
ww
0
00
00
=
===
==
z
y
x
dz
y
x
nn
vvff
w
PhDCourse Robot modeling and controlLecture 4
Impedance control
Let
The closed loop system is
FuxM -=&&
FmMu )1( +-=
FxmFFmMxM -=Þ++-= &&&& )1(
PhDCourse Robot modeling and controlLecture 4
Force Control and SoftMoveGeneral idea – mechanical compliance
§ Define compliant behaviour in one (orseveral) Cartesian directions
• Define soft direction relative to tool or workobject
» The robot is stiff in other directions
Unevensurface Pressure directionSoft direction
SoftMove
Movement direction
FC MachiningFC Assembly
Soft directions
Searchdirections
F/T sensor
PhDCourse Robot modeling and controlLecture 4
Force Control and SoftMoveCurrent important applications
§ Force Control� Machining
• Milling, grinding, fettling, polishing, etc.• FC SpeedChange functionality can be used to control path speed
� Assembly• Complex, multi-stage assembly operations
� Product testing
§ SoftMove� Ejector machines, extracting (die casting)� Handling workpiece variations
PhDCourse Robot modeling and controlLecture 4
Force Control Assembly
PhDCourse Robot modeling and controlLecture 4
Force Control Machining
PhDCourse Robot modeling and controlLecture 4
SoftMove
PhDCourse Robot modeling and controlLecture 4
Force Control technologyForce feedback
§ Speed reference is generated in the softdirections based on current deviation fromdesired force
• Damping and LP-filter (“inertia”) can beprogrammed
F/T sensor
Force
+ -
PhDCourse Robot modeling and controlLecture 4
SoftMove technologyServo controller modification
§ Make servo control “softer” in a chosen Cartesian direction� No force sensor needed
• Forces can not be controlled• More sensitive to friction
» Friction can be compensated by adding a force offset
• “Cartesian Soft Servo”» Compare ordinary (joint) Soft Servo functionality
Programmed point
ForcePhDCourse Robot modeling and controlLecture 4
Force Control and SoftMoveImportant aspects to remember
• Forces are unpredictableNo feed-forward or path planning to improve performance
• A robot in contact behaves very differently depending oncontact stiffness and geometry
PhDCourse Robot modeling and controlLecture 4
Summary
§ Robot Motion Control Overview� Inner/outer loop control architecture
§ Current and Torque Control§ Control Methods for Rigid Robots
� Computed torque� Feedback linearization
§ Control Methods for Flexible Robots� Feed-forward control� State feedback
§ Interaction with the environment