Robotics 2011 09 Control 1 - · PDF fileLi Ri t + η gearbox d efficiency ... ij j ijk j k...

Industrial RobotsIndustrial Robots

ControlControl –– Part 1Part 1Control Control Part 1Part 1

Introduction to robot control

The motion control problem motion control problem consists in the design of control algorithms for the robot actuators

In particular it consists in generating the time functions of the li d i h h h TCP igeneralized actuating torques, such that the TCP motion

follows a specified task specified task in the cartesian space, fulfilling the specifications on transient and steady state responsespecifications on transient and steady‐state response requirements

Basilio Bona 2ROBOTICA 03CFIOR

Tasks

T f t kt k b d fi dTwo types of tasks tasks can be defined:1. Tasks that do not require an interaction with the environment

(f i ) h i l i TCP f ll i(free space motion); the manipulator moves its TCP following cartesian trajectories, with constraint on positions, velocities and accelerations due to the manipulator itself or the taskaccelerations due to the manipulator itself or the task requirements

Sometimes it is sufficient to move the joints from a specified value ( )q tSometimes it is sufficient to move the joints from a specified value to another specified value without following a specific geometric path

0( )iq t

( )i fq t

2. Tasks that require and interaction with the environment, i.e., where the TCP shall move in some cartesian subspace while subject to forces or torques from the environment

We will consider only the first type only the first type of task


Motion control

In particular the motion control problem motion control problem consists in generating the time functions of the generalized actuating torques, such that the TCP motion follows a specified task in the cartesian space, fulfilling the specifications on transient and steady‐state

iresponse requirements

Control schemes can be developed for:Joint space controlpTask space control

considering that the task description is usually specified in theconsidering that the task description is usually specified in the task space, while control actions are defined in the joint space


Joint space control

Th I Ki ti bl k t f th d i d t k itiThe Inverse Kinematics block transforms the desired task space positions and velocities into desired joint space reference values.

The Transducermeasures the value of the joint quantities (anglesThe Transducermeasures the value of the joint quantities (angles, displacements) and compares them with the desired ones, obtained, if necessary, from the desired cartesian quantities.

The Controller uses the error to generate a (low power) signal for the Actuator that transforms it in a (high power) torque (via the Gearbox) that


moves the robot joints

Task space control

Controller Actuator Gearbox Robotdp p

Transducer

In this case, the Transducermust measure the task space quantities in order to

Transducer

In this case, the Transducermust measure the task space quantities in order to compare them with desired ones.

Usually this is not an easy task, since it requires environment‐aware sensors; y y , q ;the most used ones are digital camera sensors (vision‐based control) or other types of exteroceptiveexteroceptive sensors (infra‐red, ultra‐sonic, ...).

Otherwise one uses the direct kinematics to estimate the task space pose

Torques are always applied to the joints, so Inverse Kinematics is hidden inside


the Controller block

Joint space control architectures

T i j i t t l hit tj i t t l hit t iblTwo main joint space control architectures joint space control architectures are possible

Decentralized control or independent joint control: each i‐th joint motor has a p j jlocal controller that takes into account only local variables, i.e., the joint position and velocity The control is of SISO type usually based on a P PD or PID architecture

( )iq t ( )

iq t

The control is of SISO type, usually based on a P, PD or PID architectureThe controller is designed considering only an approximated model of the i‐th joint. This scheme is very common in industrial robots, due to its simplicity, modularity and robustnessThe classical PUMA robot architecture is shown in the following slideThe classical PUMA robot architecture is shown in the following slide

Centralized control: there is only one MIMO controller that generates a command vector for each joint motor; it is based on the complete model of the manipulator and takes into account the entire vector of measured positions and velocities


and velocities

Decentralized control

joint 1reference

Decentralized Joint Control 1

( )q tcontroller 1

joint 2 ( )q tJoint spacecontroller 2

joint 2referenceTask space 2

( )q t

…

controller 6

joint 6reference 6

( )q t


Decentralized controlTeach

DiskTeach

pendant OtherTerminalPUMA Control

μG D/A Amplifier Motor 1

DLV-11J

EPROM

EncoderT=0.875 ms

EPROM

RAM

InterfaceReference

anglesT=28 ms

CPUD/A Amplifier Motor 6μG

EncoderT=0.875 ms


COMPUTER ROBOT CONTROL

Motor and gearbox model (rigid body assumptions)

Gearbox Geartrain

GearboxFriction

′N

r =

Gearbox = Geartrain

′N

RobotInertia,m rN; ω τ

τFriction

m

Inertia ,′ ′ ′m rN ; ω τ

Motor′mτ


r mτ ω′ ′r mτ ω Output powerInput power

Motor and gearbox model

bβ+

mω

i aLaR N bΓ τ

m p= +rτ τ τ

aiaa N b m

avei E mΓ ′N

′τ τ

mβ

mτ′mω

pτ

mβm p

′ ′ ′= −rτ τ τ

′


p′τ

Losses in geared motor

Motorside

Jointside

Armature circuit

Motor inertia Gearbox Joint

inertiaaEi

m rω τ′ ′

a av i

m rω τ

m mω τ

ddda a a a

L i R it

+ ηgearboxefficiency

ddm m m mt

Γ ω β ω′ ′+voltage drop

efficiency

dΓ ω β ω+

pτ ′

db m b mtΓ ω β ω+


pτ

Gearbox model

ρ θ ρθρ ω ρω

′ ′ =′ ′

Nr

Nρρ

= =′ ′ω τ

Joint side ρ

ρ ω ρω= Nρm rω τ

Ideal gearbox: 1η =

θGEARBOX

Power in Power out

M t id

θ′

′

GEARBOX

m rω τ′ ′

m r m rω τ ηω τ′ ′=

Motor side ρ′

m rω τ′ ′

rτ τ ′=r r

m

rτ τω

ω′

=


m r

CC motor equations – 1

d

MOTOR SIDE

dda a a a a

L i v R i E

K it

φ

= − −

e

m m

K i

E k Kφ

ωω ωφ

φ=

′ ′= =k k K K′ ⇒m m

m a ak i K i

ω

τττ

φ′ ′= =′

k k K Kω τ′≈ ⇒ ≈

mai

Kτ

τ=

′ ′ ′p m m m m m m m m

r m p

τ β ω βτΓ θ ω

τ τθ Γ′ ′ ′= + = +

′ ′ ′= −


r m p

CC motor equations – 2

GEARBOX SIDE

′1

MOTOR SIDE

( )r r

m p

r

r

τ ττ τ

′=′ ′−

⎡ ⎤=

1

1( )

r rrτ τ′ =

+

( ) ( )m m m m m

m m m m m

r

r r r

τ ω β ω

τ ω β ω

Γ

Γ

⎡ ⎤′ ′ ′= − −⎢ ⎥⎣ ⎦⎡ ⎤′= − −⎢ ⎥⎣ ⎦

( )

1m prτ τ

τ ω β ωΓ⎡ ⎤= ⎢ ⎥⎣ ⎦

= +

+ +2( )

m m m m m

m m m m mr r Γτ β ωω⎣ ⎦′= − +

1 1 1

m b m b m

m b m b m

rτ ω β ω

τ ω β ω

Γ

Γ

⎢ ⎥⎣ ⎦⎡ ⎤⎛ ⎞⎟⎜⎢ ⎥′ ′= +⎟⎜

+ +

⎛⎟⎢ ⎥⎜ ⎟

⎞⎟⎜+ ⎟⎜ ⎟⎜ ⎟

2

1 1( )

m b m b m

m b m b m

r r rβ

Γτ ω β ω

⎟⎢ ⎥⎜ ⎟⎝ ⎠ ⎟⎣ ⎦

′ ′=

⎜⎝

+

⎟⎠

+2( )

m b m b mr rβ


Control equations

( ) ( , ) ( ) ( ) Te

+ + + + =H q q C q q q B q q g q J F τ

component-wise joint torques

( ) )) ((n n n

ij j ijk j k bi i i fi iH q qh gq qβ τ τ+ + + + =∑ ∑∑q q q1 1 1

( ) )) ((ij j ijk j k bi i i fi ri

j j k

q q gq qβ= = =

+ + + +∑ ∑∑q q q

1 1

( ) ( ) ( )( )r

n n n

i ij j ijk j k bi i i fij i

ij

ik

iq H qH q qh gqβ τ

≠ = =

+ =+ + + +∑ ∑∑q q q q τ

I ti l t Coriolis & centripetal Friction, gravity


Inertial torques Co o s & ce t petatorques

ct o , g a ty& external torques

Control equations

( ) ( ) ( )( )n n n

H q h q q gH q q ττβ+ =+ + + +∑ ∑∑q q qq1 1

( ) ( ) ( )( )ij j ijk j kii i bi i fi

j i j krii

H q h q q gH q q ττβ≠ = =

+ =+ + + +∑ ∑∑q q qq

( )Mi i i fii i bi ii i

H q q τ τ ττβ τ+ + ++ + =q( )Mi ci gi f rii i bi ii i

q q τ τ ττβ τ+ + ++ +q

Modelled torques “Disturbance” torques


From single motor model to robot control equation

θ′ ′ ′mi mi mi

i i ii i i

q qr r

qr

θ ω ω= = =

′ ′ ′

Gearbox transformationGearbox transformation

n

H q H qqτ β τ τ τ τ= + + + + ++∑

StructuredStructured

ri ii i bi i ij j Mi ci gi fij i

mi mi

H q H qqτ β τ τ τ τ

ωβ τ τ τ τΓ

ω≠

= + + + + +

′+ + +

+

′= + +

∑

StructuredStructureddisturbancedisturbancebi bi Mi ci gi fi

i i

mi mi

r rβ τ τ τ τ

ωβ

Γ

ωΓ

+ + +=

′

+ +

′mi mi

bi bi dii ir r

βΓ τ= + +


Equation seen at the joint sidejoint side

From single motor model to robot control equationEquation seen at the motor sidemotor sideEquation seen at the motor sidemotor side

since

( )1 1 1Γ β′ ′′( )2ri ri bi mi bi mi di

i iir rr

Γ βωτ τ ω τ′ ′+ +′= =

( )ri mi pi mi mi mi mi miτ τ τ τ Γ β ωω′ ′ ′ ′= = +′ ′− −

and

1 1 1β βΓ Γ

⎛ ⎞ ⎛ ⎞⎟ ⎟⎜ ⎜⎟ ⎟′ ′ ′ ′+ ⎜ + +⎜ + +′

we obtain

2 2mi ri pi bi mi mi bi mi mi diii irr r

τ τ τ β β τΓ ωΓ ω⎟ ⎟′ ′ ′ ′= + = ⎜ + +⎜ + +⎟ ⎟⎜ ⎜⎟ ⎟⎟ ⎟⎜ ⎜⎝ ⎠ ⎝ ⎠′

Total inertia Total friction



mi pi di ti mi ti mi diτ τ τ ω β ω τΓ′ ′ ′ ′ ′ ′ ′= + = + +′

′ ′ ′ ′ ′ ′ ′ ′= + = + +Bτ τ τ Γ ω ω τ Motor sideMotor sidep d t m t mm d

= + = + +Bτ τ τ Γ ω ω τ Motor sideMotor side

= + +t′Γ

t′B

⎛ ⎞ ⎛ ⎞2

1bi mi

ir

Γ Γ⎛ ⎞⎟⎜ ⎟⎜ + ⎟⎜ ⎟⎟⎜⎝ ⎠

2

1bi mi

ir

β β⎛ ⎞⎟⎜ ⎟⎜ + ⎟⎜ ⎟⎟⎜⎝ ⎠


i⎝ ⎠ i⎝ ⎠


mi pi di ti mi ti mi diτ τ τ ω β ωΓ τ= + = + +

Joint sideJoint side= + = + +Bτ τ τ Γ ω ω τ Joint sideJoint sidep d t m t m dm

= + = + +Bτ τ τ Γ ω ω τ

= + +t

ΓtB

( )2bi i mirΓ Γ+ ( )2

bi i mirβ β+


Block diagram of open‐loop CC motor (motor side)For simplicity we drop the prime ′ symbolFor simplicity we drop the prime ′ symbol

for the motor side quantities, and we consider the generic i-th motor

Taking the Laplace transform of the involved variables, we haveg p ,

( ) ( ) ( ) ( )t t m m d

s s s sβ ω τ τΓ + = −( ) ( ) ( ) ( )t t m m d

m aK iτ

βτ =

1( ) ( )s sθ ω

+dτ

( ) ( )m ms s

sθ ω=

+

–+

1

a aR sL+

1

t tsβ Γ+

Kτ

1s

av

ai m

τ mω

mθ–

rτ

Kω

E


ω

Block diagram of open‐loop CC motor (motor side)

Th t i it i d t i ll d ll b l t d The armature circuit inductance is small and usually can be neglected

0 v RL i K ω=≈ ⇒0aa a a m

v RL i Kωω− =≈ ⇒

τm

ai

Kτ

τ=

ma a mv R K

K ωτ

τω− =

m d t m t m

a a aR R R

K

τ τ ω β ω

β

Γ

Γ

= + +⎡ ⎤⎛ ⎞⎟⎜⎢ ⎥a a a

a d t t mv s K

K K Kωτ τ τ

τ β ωΓ ⎟⎜⎢ ⎥⎟− = + +⎜ ⎟⎢ ⎥⎜ ⎟⎜⎝ ⎠⎢ ⎥⎣ ⎦



R βa tR

K K KKω ω ω

τ

β′ = + ≈

a tR

Kβ

since

friction torque

a tm m

KK ω

τ

ω ω

t ma a mR i K

K i ω

β ωω

armature losses armature e.m.f.torque m

aK iτ

τ



⎛ ⎞ 11a t a

m a d

R Rs v

K K K K Kτ ω ω τ ω

Γω τ

⎛ ⎞⎟⎜ ⎟+ = −⎜ ⎟⎜ ′ ′ ′⎟⎜⎝ ⎠

dτ

⎝ ⎠

a tR

TK K

Γ=

′dτ

Ka

d

K KR

KK

τ ω

=where

– θ

dK Kτ

+ ( )G sω

1s

av m

ωmθ

( ) ( )1

G s =


( ) ( )1G s

K sTωω′ +

Matrix formulation (joint side)Lagrange

( ) ( , ) ( ) ( )

( )

Te

r m m p

+ + + + =≡ = −H q q C q q q Bq g q J q F

Rτ

τ τ τ τ

LagrangeEquation

( )r m m p

where

2= +R R K v R K qτm m m a a m ω+R R R qτ

2( ) ( )m p m m m m m m m m

= + = +R R q B q R q B qτ Γ Γp

Motor side Joint sideMotor side Joint side

0 0 0 00 0

KKK

⎡ ⎤ ⎡ ⎤⎡ ⎤ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥

and

0 0 ; 0 0 ; 0 0

0 00 0 0 0

im i a

ai a

i i

iR R

Kr

KKτ τ ωω

⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥= = =⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ ⎦

R K K


0 00 0 0 0⎢ ⎥ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎢ ⎥ ⎢ ⎥⎣ ⎦ ⎣ ⎦

Matrix formulation (joint side)Then we haveThen, we have

Mass matrix Friction matrix

( ) ( )

( )M q F

( ) ( )( )2 2( ) ( , )

( ) ( )m m m mω+ + + + + +

+ + =

H q R q C q q B R K B q

g q J q F R K v

ΓT( ) ( )

e m a a+ +g q J q F R K v

uGravity

Interaction cu

Command input

Often we use this symbol to indicate

Interaction

( )( , ) ( , )= +h q q C q q F q

ythe velocity dependent terms


( )( ) ( )

Matrix formulation (joint side)

( ) ( ) ( ) ( )T+ + +M h J F

Gravity and interaction

( ) ( , ) ( ) ( )Te c

+ + + =M q q h q q g q J q F u

No interaction

( ) ( , ) ( )c

+ + =M q q h q q g q u

No gravity, no interaction

( ) ( , )c

+ =M q q h q q u

Control Design ProblemControl Design Problem ...?c=u


Decentralized joint control

A i f i li it

( ) ( , )c

+ =M q q h q q u

Assuming, for simplicity

( ) ( , )c

q q q q

If …

( ) diagonal 2 2( )m m m m m

⇒ + ≈R I H q R RΓ Γ

small ( , )C q q q

Th Then …

t d c+ + =q Fq uΓ τ 2

t m m=RΓ Γwith

disturbance

( )t cs + =F uΓ ω

The model is diagonal, i.e., naturally decoupled


g , , y pEach joint can be controlled by local controllers

Decentralized joint controlThis is the proportional velocity controller

dτ

This is the proportional velocity controlleror velocity compensator

d

KdKreference voltage

–

+( )G sω

1s

av m

ωmθ

DK

–

+rv e

( )K ( )tK s

Transducer T.F. ( )K s K≈


(tachimetric sensor)( )t t

K s K≈

Open loop vs Closed loop

( )( )

( ) (1 )m Ds K

G sv s K s Tω

ωα

α′= =

′ +

( ) 1( )

( ) (1 )ms

G sv s K sTω

ω= =

′ + ( ) (1 )rv s K s Tω α+( ) (1 )

av s K sTω +

( )( ) ( )

( ) (1 )m

d d

s TG s K G s

s sTω

ωτ Γ

= = − = −+

( )( ) ( )m d d

s K K TG G

ω α α′ ′

( ) (1 )d ts sTτ Γ +

( )( ) ( )

( ) (1 ) (1 )m d d

dd D t

G s G ss K s T K s Tω

ω α ατ Γ′ ′= = − = − = −

′ + +

1Kωα′

= <


1D t

K K Kω

α = <′ +

The closed‐loop system

dτ

K

dτ –

( )G s1a

v mω

mθ

dK

dK

K

+( )G sω s

Open loop

DK

–

+( )G sω

′ 1s

rv

mω

mθ

s

Closed loop


The closed‐loop system

Time constant is reduced T Tα <

KDisturbance gain is reduced

dd

D

KK

K→

Design parameterd

τ

when 1dK = dK

–

( )G sω

1s

av m

ωmθ

DK

+rv e

+( )

s–


Position compensator

dτController 1

dK

–

( )G1a

vm

ωmθ

K+e

K+rθ

+( )G sω sD

K

–

PK

–

tK

1tK ≈

1K θ ≈

K θ


θ

Position compensator

( )θ1 2

( )( )

( )m

r

s KG s

s s s T K

θθ α

= =+ +

2 2

( ) 1( )

( ) ( )ms

G ss s s T Kτ Γ

θ

α= = −

+ +( ) ( )d ts s s T Kτ Γ α+ +

D P D PK K K K K

K τ= =where

a t

KTK Rω Γ

= =′

Configuration dependent

Second-order TF with

1 1·

2 2

K K

T K R K K Kτ ω

α αζ

Γ

′= =

with 2 2

1

a D P t

D P

T K R K K K

K K KK

τ

τ

α α Γ

ω = =


·n

a t

KR

ωΓ

= =

Design parameters

The damping coefficient and the natural frequency are inversely proportional to the square root of the inertia moment, that may vary p p q , y yin time when the angles vary

2

1t b mr

Γ Γ Γ⎛ ⎞⎟⎜= + ⎟⎜ ⎟⎜ ⎟⎝ ⎠

( )( )b iiH tΓ⎝ ⎠

= q

Since the damping coefficient and the natural frequency are often usedSince the damping coefficient and the natural frequency are often used as control specifications, we can design a controller computing the maximum inertia moment and adjusting the two gains int

Γ ,P DK Kmaximum inertia moment and adjusting the two gains in

such a way that the damping ratio is satisfactory, e.g., no overshoot appears in the step response

,maxt

ζ,P D


An alternative

dτ

KController 2

+

dK

–

+( )G sω

1s

av

mω

mθ

P DK sK′ ′+

–

+rθ e

( ) ( ) ( )a P Dv t K e t K e t′ ′= +

3 2

( ) /( )

( )m D P Ds K K s K K

G sR

τ

Γθθ

′ ′ ′+= =

′

A zero appears

3 2( )

( )r a t a tPs R s s T K K Rτ

Γ α Γθ ′+ +

( ) 1 1( )m

sG

θ ⎛ ⎞⎟⎜ ⎟⎜ ⎟4 2( )

( )m

d t P a t

G ss s s T K K Rτ

τ Γ α Γ⎜= = − ⎟⎜ ⎟⎜ ′ ⎟+ + ⎟⎜⎝ ⎠


Another alternative

dτ

KController 3

++

dK

–+

( )G sω

1s

av

mω

mθ

P DK sK′ ′+

–

+rθ e

–+D

K


Comparison

Controller 1

( ) ( ) ( )t K K t K t( ) ( ) ( )a D P D mv t K K e t K tω= −

Controller 2

( ) ( ) ( ) ( )a P D r D mv t K e t K t K tω ω′= + −

Controller 3Controller 3

( )( ) ( ) ( ) 1 1 ( )a D P D D r D D D mv t K K e t K K t K K K tω ω′ ′ ′ ′= + − +


Practical Issues

1.1. Saturating actuatorsSaturating actuators

2.2. Elasticity of the structureElasticity of the structure

33 Nonlinear friction at jointsNonlinear friction at joints3.3. Nonlinear friction at jointsNonlinear friction at joints

4.4. Sensors or amplifiers with finite band Sensors or amplifiers with finite band 4.4. Sensors or amplifiers with finite band Sensors or amplifiers with finite band


Saturating ActuatorsIt is a nonlinear effect, difficult to be considered a-priori

( )y t

It is a nonlinear effect, difficult to be considered a priori

saturation

( )y t( )u t

( )u t

( )y t( )u t

Li it

saturation

Linearity

( )t⎧⎪ IF

max max

min max

( )

( ) ( ) ( )

y u t u

y t ku t u u t u

⎪⎪⎪⎪⎨⎪

>= ≤ ≤

IFIF

min min( )y u t u

⎪⎪⎪⎪⎩ <IFBasilio Bona 41ROBOTICA 03CFIOR

Elasticity of the structure

Although we have considered rigid bodies, the elasticity is a phenomenon that limits the closed loop bandphenomenon that limits the closed loop band

We cannot design controllers that are “too fast” without taking li itl i t id ti t f l ti d lexplicitly into consideration some sort of elastic model.

Recall that when we use a simplified model

( ) ( ) 0t m e mt k tθ θΓ + =

the proper structural resonance (or natural) frequency is

er

kω

Γ=

tΓ


Elasticity of the structure


Nonlinear friction

A li ff t b t l it d f i ti fA nonlinear effect between velocity and friction force

total( )f t

istiction

( )

( )f t( )v tviscous

coulomb

stiction

( )v t


Nonlinear friction

Nonlinear static friction models include:Coulomb + viscous

signc

F(v) F (v) vβ= +

Static model that includes stiction and “Stribeck effect”: friction decreases with increasing velocity for v < vs (Stribeckvelocity)

( ) i

s

s

vvF( ) F F F ( )

δ

β−

⎡ ⎤⎢ ⎥⎢ ⎥+ +( ) signs

c s cF(v) F F F e (v) vβ⎢ ⎥= + − +

⎢ ⎥⎢ ⎥⎣ ⎦


Finite pass‐band in sensors and amplifiers

Sensors and amplifiers are often modelled as simple gains, while in the real world they have a finite pass‐band, nonlinearities, saturations, etc.

These effects must be taken into account when the simulated and realThese effects must be taken into account when the simulated and real behaviours differ.

Fortunately, very often the band of sensors and amplifiers is much wider than the final closed loop band of the controlled system.

46ROBOTICA 03CFIORBasilio Bona

Robotics 2011 09 Control 1 - · PDF fileLi Ri t + η gearbox d efficiency ... ij j ijk j k...

Documents

Transcript of Robotics 2011 09 Control 1 - · PDF fileLi Ri t + η gearbox d efficiency ... ij j ijk j k...