Robot control - examples Outline · PhDCourse Robot modeling and control Lecture 4 Simulation Example – Simple Robot Js 1 s K p 1 K v q &q q d Kv 25 Kp 158 J 1 = = = Closed Loop

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


Robot control - examples

§ Fanta challengehttp://www.youtube.com/watch?v=PSKdHsqtok0

§ Fanta challenge IIhttp://www.youtube.com/watch?v=SOESSCXGhFo


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


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


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


FeedForwardCalculation

Torqueto

Current

PIControl

Irefq

refτ Uref

q

d-qto

stator

mq&

0Irefd =

statortod-q

FeedForwardCalculation

PIControl

Uref

d

PWM

Imeasuredq

Imeasuredd

MeasuredCurrents

mq


mq


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



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.


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


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 && -+-=


Cascade Controller

-vK Robot

dtd

q

ò iK

dq&

Inner Loop: Speed (PI) Controller


Cascade Controller

Outer Loop: Position Controller (P) – Inner Loop: Speed (PI) Controller

-

dq

-vKpK Robot

q

dtd

ò iK

dq&


Feedforward + PD Control

)(),()( dddddffw qgqqcqqMu ++= &&&

)()( qqKqqKuu dvdpffw && -+-+=


)()( qqKqqKqv dvdpd &&&& -+-+=

Feedback Linearization + PD Control

)(),()( qgqqcvqMu ++= &


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!


Simulation Example – Simple Robot

Js1

s1pK

vKqq&

-

-

dq

25Kv158Kp

1J

==

=

Closed LoopPoles -12.6, -12.6

i.e. damping = 1=> no overshoot



Js1

s1pK

vKqq&

-

-

dq

Closed Loop Poles-12.6, -12.6Zero -6

dq&

25Kv158Kp

1J

==

=



Js1

s1pK

vKqq&

-

-

dq

Closed LoopPoles -124, -1.3Zero -1.3

dq&

126Kv158Kp

1J

==

=



Js1

s1pK

vKqq&

-

-

dq

dq&

25Kv158Kp

1J1J

==

==

ˆ

Jdq&&

”2-DOF” Controller:Tracking &Regulation/Robustness”decoupled”



Js1

s1pK

vKqq&

-

-

dq

dq&

25Kv158Kp

1J0.7J

==

==

ˆ

Jdq&&


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


ρ

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 £-


Passivity Based Adaptive Control

Control Input: Krθv)a,,qY(q,u += &

a)rv,,q(q,YΓθ T1 && --=Parameter update:


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&&



Discontinuous aΔ



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




-60 -40 -20 0 20 40 60

-90

-80

-70

-60

-50

-40

-30

-20

-10

0

X [mm]

Y[m

m]


Ref PosPosMax Error




-60 -40 -20 0 20 40 60

-90

-80

-70

-60

-50

-40

-30

-20

-10

0

X [mm]

Y[m

m]


Ref PosPosMax Error




-60 -40 -20 0 20 40 60

-90

-80

-70

-60

-50

-40

-30

-20

-10

0

X [mm]

Y[m

m]


Ref PosPosMax Error

Bandwidth must be sufficientlyhigh relative frequency content oftrajectory

All channels should be equal!


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):


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ς +

=


PD Control

mqmq&

pK

vK-

-

dq

Closed Loop Bandwidth:10 HzMechanical Resonance:6 Hz


PD Control – Motor Feedback

mqmq&

pK

vK-

-

dq

Closed Loop Bandwidth:3.5 HzMechanical Resonance:6 Hz


PD + FFW Control – Motor Feedback

mqmq&

pK

vK-

-

dqJdq&&

dq&

Same PD Tuning as onprevious slide(Poles @ 3.5 Hz)


PD Control – Arm Feedback

aqaq&

pK

vK-

-

dq

Same PD Tuning as onprevious slide => unstable


PD Control – Arm Feedback

aqaq&

pK

vK-

-

dq

Low Gain Controller


PD Control – Arm Pos / Motor Speed Feedback

aqmq&

pK

vK-

-

dq

Motor Feedback preferredfor flexible joint robot if onlytwo states are measured


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


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


Swedish Open Championships in Robot Control 2005

www.robustcontrol.org


Nonlinear MIMO Flexible Joint Control

(t)ZZ(t) d=Goal: Perfect tracking, i.e


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, …)……


-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


Nonlinear Flexibility


Extended Flexible Joint Model

For N links, N motors and Munactuated joints: 2N + M d.o.f.


Extended Flexible Joint Model


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


Nonlinear Simulation Model

§ 2 actuated DOF (N = 2)§ 1 unactuated DOF (M = 1)§ 5 DOF


Simulation Model – The Equations


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


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


Force and torque when in contact with environment

Virtual displacement (again)

virtual work

qqJX dd )(=

qXFw TT dtdd -=

( ) qqJF TT dt-= )(

FqJ T )(=Þt


Complete dynamics

uFqJqgqqqCqqM eT =+++ )()(),()( &&&&

Figure 9.2 in Spong etal


Coordinate frames and constraints

Let the velocity (Twist) be defined

and the force (Wrenche)

Reciprocity condition

úû

ùêë

é=

wx

v

úû

ùêë

é=

nf

F

0=+= nfvF TTT wx


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


Impedance control

Let

The closed loop system is

FuxM -=&&

FmMu )1( +-=

FxmFFmMxM -=Þ++-= &&&& )1(


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


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


Force Control Assembly


Force Control Machining


SoftMove


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

+ -


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


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

Documents

Robot control - examples Outline · PhDCourse Robot modeling and control Lecture 4 Simulation Example – Simple Robot Js 1 s K p 1 K v q &q q d Kv 25 Kp 158 J 1 = = = Closed Loop