ROBOTICA 03CFIOR 03CFIOR Basilio Bona DAUIN Politecnico di - - PowerPoint PPT Presentation

ROBOTICA 03CFIOR 03CFIOR

Basilio Bona DAUIN – Politecnico di Torino

Basilio Bona ROBOTICA 03CFIOR 1

Control – Part 1

Introduction to robot control

The 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 generalized actuating torques, such that the TCP motion generalized actuating torques, such that the TCP motion follows a specified task in the cartesian space, fulfilling the specifications on transient and steady-state response requirements

Basilio Bona 3 ROBOTICA 03CFIOR

Tasks

Two types of tasks can be defined:

• 1. Tasks that do not require an interaction with the environment

(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 task requirements

Sometimes it is sufficient to move the joints from a specified value

( ) q t

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

• 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 of task

Basilio Bona 4 ROBOTICA 03CFIOR

( )

i

q t ( )

i f

q t

Motion control

In particular the 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 response requirements Control schemes can be developed for:

Joint space control Task space control

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

Basilio Bona 5 ROBOTICA 03CFIOR

Joint space control

The Inverse Kinematics block transforms the desired task space positions

Basilio Bona 6 ROBOTICA 03CFIOR

The Inverse Kinematics block transforms the desired task space positions and velocities into desired joint space reference values. The Transducer measures 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

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

Controller Actuator Gearbox Robot Transducer

d

p p

Basilio Bona 7 ROBOTICA 03CFIOR

In this case, the Transducer must 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; the most used ones are digital camera sensors (vision-based control) or other types of exteroceptive 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

Two main joint space control architectures are possible Decentralized control or independent joint control: each i-th joint motor has a local 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 The 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,

( )

i

q t ( )

i

q t ɺ

Basilio Bona 8 ROBOTICA 03CFIOR

This scheme is very common in industrial robots, due to its simplicity, modularity and robustness The 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

Decentralized control

Joint space

controller 2 joint 1 reference joint 2 reference

Decentralized Joint Control

1( )

q t

Task space

controller 1

2( )

q t

Basilio Bona 9 ROBOTICA 03CFIOR

…

controller 6 joint 6 reference

6( )

q t

Decentralized control

DLV-11J EPROM

Disk Teach pendant Other Terminal

µG

D/A Amplifier Encoder Motor 1 T=0.875 ms

PUMA Control

Basilio Bona 10 ROBOTICA 03CFIOR

EPROM RAM CPU

Interface

COMPUTER ROBOT CONTROL

D/A Amplifier Encoder Motor 6 Reference angles T=28 ms T=0.875 ms

µG

Motor and gearbox model (rigid body assumptions)

Robot Gearbox Friction Inertia

,

m r

N; ω τ

m

τ

′ N r = N

Gearbox = Geartrain

Basilio Bona 11 ROBOTICA 03CFIOR

Motor Inertia Friction

, ′ ′ ′

m r

N ; ω τ

′

m

τ

m

τ

r m

τ ω ′ ′

r m

τ ω

Output power Input power

Motor and gearbox model

a

i i

a

L

a

R N

b

Γ

b

β

m

τ

m p

= +

m r

ω τ τ τ

Basilio Bona 12 ROBOTICA 03CFIOR

a

v

e

i E

m

Γ

m

β ′ N ′

m

τ

m p

′ ′ ′ ′ = −

m r

ω τ τ τ

p

′ τ

p

τ

Losses in geared motor

Armature circuit Motor inertia Gearbox Joint inertia

a

Ei

m r

ω τ ′ ′ d L i R i +

a a

v i

m r

ω τ

m m

ω τ

Motor side Joint side

Basilio Bona 13 ROBOTICA 03CFIOR

d d

a a a a

L i R i t + d d

m m m m

t Γ ω β ω ′ ′ +

voltage drop

p

τ′ η

gearbox efficiency

d d

b m b m

t Γ ω β ω +

p

τ

Gearbox model

Joint side

θ ρ

ρ θ ρθ ρ ω ρω ′ ′ = ′ ′ = N r N ρ ρ = = ′ ′

m r

ω τ

Ideal gearbox:

1 η =

GEARBOX Power in Power out

Basilio Bona 14 ROBOTICA 03CFIOR

Motor side

θ′ ρ′

m r

ω τ ′ ′

GEARBOX

m r

ω τ ′ ′

m r m r

ω τ ηω τ ′ ′ =

r r m m

r r τ τ ω ω ′ = ′ =

CC motor equations – 1 d d

a a a a a e m m

L i v R i E K i E k K t

φ ω

ω ω φ φ = − − = ′ ′ = = k k K K ′ ≈ ⇒ ≈

MOTOR SIDE

Basilio Bona 15 ROBOTICA 03CFIOR

m m m a a m a p m m m m m m m m r m p

k i K i i Kτ

ω τ

ω ω τ τ τ β ω β φ φ τ Γ θ ω τ τ θ Γ ′ ′ = = ′ = ′ ′ ′ = + = + ′ ′ ′ = − ɺɺ ɺ ɺ k k K K

ω τ

′ ≈ ⇒ ≈

CC motor equations – 2

JOINT SIDE

( ) ( ) ( )

r r m p m m m m m m m m m m

r r r r r r τ τ τ τ τ ω β ω τ ω β ω Γ Γ ′ = ′ ′ −   ′ ′ ′ = − −       ′ = − −     = ɺ ɺ 1 1 ( ) 1

r r m p m b m b m

r r r τ τ τ τ τ ω β ω Γ ′ =   =     = + + + ɺ

MOTOR SIDE

Basilio Bona 16 ROBOTICA 03CFIOR

2(

)

m m m m m m m m m m

r r Γ τ β ω ω     ′ = − + ɺ

2

1 1 1 1 1 ( )

m b m b m m b m b m m b m b m

r r r r r r τ ω β ω τ ω β ω Γ Γ Γ τ ω β ω   =             ′ ′ = +   + +            +      ′ ′ =   +   + ɺ ɺ ɺ

Control equations

( ) ( , ) ( ) ( )

T e

+ + + + = H q q C q q q B q q g q J F τ ɺɺ ɺ ɺ ɺ ( ) ) ) ( (

n n n ij j ijk j k bi i i fi ri

H q q h g q q β τ τ + + + + =

∑ ∑∑

q q q ɺ ɺɺ ɺ

component-wise joint torques

Basilio Bona 17 ROBOTICA 03CFIOR

1 1 1

( ) ) ) ( (

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

H q q h g q q β τ τ

= = =

+ + + + =

∑ ∑∑

q q q ɺ ɺɺ ɺ

1 1

( ) ( ) ( ) ( )

r n n n i ij j ijk j k bi i i fi j i i j i k i

q H q H q q h g q β τ

≠ = =

+ = + + + +

∑ ∑∑

q q q q τ ɺɺ ɺɺ ɺ ɺ ɺ

Inertial torques Coriolis & centripetal torques Friction, gravity & external torques

Control equations

1 1

( ) ( ) ( ) ( )

n n n ij j ijk j k ii i bi i fi j i j k ri i

H q h q q g H q q τ τ β

≠ = =

+ = + + + +

∑ ∑∑

q q q q ɺɺ ɺ ɺ ɺ ɺɺ

Basilio Bona 18 ROBOTICA 03CFIOR

( )

M i ci gi f r ii i bi i i i

H q q τ τ τ τ β τ + + + + + = q ɺɺ ɺ

Modelled torques “Disturbance” torques

From single motor model to robot control equation

mi mi mi i i i i i i

q q r r q r θ ω ω = = = ′ ′ ′ ɺ ɺ ɺ ɺ

Gearbox transformation

n ri ii i bi i ij j Mi ci gi fi

H q H q q τ β τ τ τ τ = + + + + + +∑ ɺɺ ɺɺ ɺ

Basilio Bona 19 ROBOTICA 03CFIOR

Equation seen at the joint side Structured disturbance

ri ii i bi i ij j Mi ci gi fi j i mi mi bi bi Mi ci gi fi i i mi mi bi bi di i i

H q H q r r r r q τ β τ τ τ τ ω β τ τ τ τ ω β Γ ω Γ τ ω

≠

= + + + + + ′ + + + + ′ = ′ + + = ′ + +

∑

ɺɺ ɺɺ ɺ ɺ ɺ

From single motor model to robot control equation

Equation seen at the motor side since

( )

2

1 1 1

ri ri bi mi bi mi di i i i

r r r Γ β ω τ τ ω τ ′ ′ + + ′ = = ɺ

( )

ri mi pi mi mi mi mi mi

τ τ τ τ Γ β ω ω ′ ′ ′ ′ = = + ′ ′ − − ɺ

and

Basilio Bona 20 ROBOTICA 03CFIOR

2 2

1 1 1

mi ri pi bi mi mi bi mi mi di i i i

r r r τ τ τ β β τ Γ ω Γ ω           ′ ′ ′ ′ = + =  + + + +               ′ ɺ

we obtain Total inertia Total friction

From single motor model to robot control equation

mi pi di ti mi ti mi di

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

p d t m t m m d

′ ′ ′ ′ ′ ′ ′ ′ = + = + + B τ τ τ Γ ω ω τ ɺ

Motor side

Basilio Bona 21 ROBOTICA 03CFIOR

= + +

t′

Γ

t′

B

2

1

bi mi i

r Γ Γ       +       

2

1

bi mi i

r β β       +       

From single motor model to robot control equation

Joint side

mi pi di ti mi ti mi di

τ τ τ ω β ω Γ τ = + = + + ɺ

p d t m t m d m =

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

Basilio Bona 22 ROBOTICA 03CFIOR

= + +

t

Γ

t

B

( )

2 bi i mi

r Γ Γ +

( )

2 bi i mi

r β β +

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

For 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 have

( ) ( ) ( ) ( )

t t m m d m a

s s s s K i

τ

β ω τ τ τ Γ + = − = 1 ( ) ( ) s s θ ω =

Basilio Bona 23 ROBOTICA 03CFIOR

+ – + 1

a a

R sL + 1

t t

s β Γ + Kω Kτ 1 s

a

v

a

i

m

τ

d

τ

m

ω

m

θ – E

r

τ

1 ( ) ( )

m m

s s s θ ω =

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

The armature circuit inductance is small and usually can be neglected

a a a a m

v R L i Kωω − = ≈ ⇒

m a

i Kτ τ τ =

Basilio Bona 24 ROBOTICA 03CFIOR

m a a m m d t m t m a a a a d t t m

v R K K R R R v s K K K K

ω τ ω τ τ τ

τ ω τ τ ω β ω τ β ω Γ Γ − = = + +          − = + +              ɺ

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

a t a t

R K K K K R K

ω ω ω τ

β β ω ω ′ = + ≈ ≪

since

Basilio Bona 25 ROBOTICA 03CFIOR

• friction torque

armature losses armature e.m.f. torque m a t m m t m a a m a

R K K R i K K i

ω τ ω τ τ

β ω ω β ω ω ≪ ≪

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

d

τ

d

K

1 1

a t a m a d

R R s v K K K K K

τ ω ω τ ω

Γ ω τ      + = −    ′ ′ ′    

a t a d

R T K K R K K

τ ω

Γ = ′ =

where

Basilio Bona 26 ROBOTICA 03CFIOR

– +

( )

G s

ω

1 s

a

v

m

ω

m

θ

d

K

d

Kτ

( ) ( )

1 1 G s K sT

ω ω

= ′ +

Matrix formulation (joint side)

( ) ( , ) ( ) ( ) ( )

T e r m m p

+ + + + = ≡ = − H q q C q q q Bq g q J q F R τ τ τ τ τ ɺɺ ɺ ɺ ɺ

where

2 m m m a a m ω

= + R R K v R K q τ ɺ

2

( ) ( )

m p m m m m m m m m

= + = + R R q B q R q B q τ Γ Γ ɺɺ ɺ ɺɺ ɺ

Lagrange Equation

Basilio Bona 27 ROBOTICA 03CFIOR

0 ; 0 ;

i m i a ai a i i i

R R K r K Kτ

τ ω ω

                      = = =                             R K K ⋱ ⋱ ⋱ ⋱ ⋱ ⋱

m p m m m m m m m m

Motor side Joint side Motor side Joint side and

Matrix formulation (joint side)

Then, we have

( )

( )

( )

2 2

( ) ( , ) ( ) ( )

m m m m e m a a ω

+ + + + + + + + = H q R q C q q B R K B q g q J q F R K v Γ ɺɺ ɺ ɺ

T

( ) M q F

Mass matrix Friction matrix

Basilio Bona 28 ROBOTICA 03CFIOR

( ) ( )

e m a a

+ + = g q J q F R K v

c

u

Command input

( )

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

Often we use this symbol to indicate the velocity dependent terms Gravity Interaction

Matrix formulation (joint side)

( ) ( , ) ( ) ( )

T e c

+ + + = M q q h q q g q J q F u ɺɺ ɺ ( ) ( , ) ( )

c

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

Gravity and interaction No interaction

Basilio Bona 29 ROBOTICA 03CFIOR

( ) ( , )

c

+ = M q q h q q u ɺɺ ɺ

No gravity, no interaction Control Design Problem

...?

c =

u

Decentralized joint control

( ) ( , )

c

+ = M q q h q q u ɺɺ ɺ

Assuming, for simplicity If …

( )

diagonal

2 2

( )

m m m m m

⇒ + ≈ R I H q R R Γ Γ ≫

Basilio Bona 30 ROBOTICA 03CFIOR

small

( , ) C q q q ɺ ɺ

Then …

t d c

+ + = q Fq u Γ τ ɺɺ ɺ

2 t m m

= R Γ Γ

with

( )

t c

s + = F u Γ ω

disturbance The model is diagonal, i.e., naturally decoupled Each joint can be controlled by local controllers

Decentralized joint control

–

d

τ ω

d

K

This is the proportional velocity controller

• r velocity compensator

reference voltage

Basilio Bona 31 ROBOTICA 03CFIOR

– +

( )

G s

ω

1 s

a

v

m

ω

m

θ

D

K

( )

t

K s

– +

r

v e

Transducer T.F. (tachimetric sensor)

( )

t t

K s K ≈

Open loop vs Closed loop

( ) ( ) ( ) (1 )

m D r

s K G s v s K s T

ω ω

ω α α ′ = = ′ + ( ) 1 ( ) ( ) (1 )

m a

s G s v s K sT

ω ω

ω = = ′ + ( ) ( ) ( ) ( ) (1 )

m d d

s T G s K G s s sT

ω

ω τ Γ = = − = − +

Basilio Bona 32 ROBOTICA 03CFIOR

( ) ( ) ( ) ( ) (1 ) (1 )

m d d d d D t

s K K T G s G s s K s T K s T

ω ω

ω α α α α τ Γ ′ ′ = = − = − = − ′ + + 1

D t

K K K K

ω ω

α ′ = < ′ + ( ) (1 )

d t

s sT τ Γ +

The closed-loop system

d

τ

d

K K

– +

( )

G s

ω

1 s

a

v

d

τ

m

ω

m

θ

d

K

Open loop

Basilio Bona 33 ROBOTICA 03CFIOR

– +

( )

G s

ω

′ 1 s

r

v

m

ω

m

θ

D

K

Closed loop

The closed-loop system

Time constant is reduced

T T α <

Disturbance gain is reduced

d d D

K K K →

Design parameter

1 K =

d

τ

Basilio Bona 34 ROBOTICA 03CFIOR

when

1

t

K =

– +

( )

G s

ω

1 s

a

v

m

ω

m

θ

d

K

D

K

– +

r

v e

Position compensator

–

( )

G s 1

a

v

m

ω

m

θ K

+

e K

+

r

θ

d

τ

d

K

Controller 1

Basilio Bona 35 ROBOTICA 03CFIOR

+

( )

G s

ω

1 s

D

K

– +

P

K

–

1 K θ ≈ K θ

t

K 1

t

K ≈

Position compensator

1 2

( ) ( ) ( )

m r

s K G s s s s T K θ θ α = = + +

2 2

( ) 1 ( ) ( ) ( )

m d t

s G s s s s T K τ Γ θ α = = − + +

D P D P

K K K K K K

τ

= =

where

Basilio Bona 36 ROBOTICA 03CFIOR

D P D P a t

K TK R

τ ω

Γ = = ′

Second-order TF with

1 1 · 2 2 1 ·

a D P t D P n a t

K K T K R K K K K K K K R

τ ω τ τ

α α ζ Γ ω Γ ′ = = = =

Configuration dependent

Design parameters

( )

2

1

t b m

r Γ Γ Γ     = +        The damping coefficient and the natural frequency are inversely proportional to the square root of the inertia moment, that may vary in time when the angles vary

Basilio Bona 37 ROBOTICA 03CFIOR

( )

( )

b ii

H t Γ   = q Since 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 in such a way that the damping ratio is satisfactory, e.g., no overshoot appears in the step response

,max t

Γ ζ ,

P D

K K

– +

( )

G s

ω

1 s

a

v

m

ω

m

θ

P D

K sK ′ ′ +

– +

r

θ e

d

τ

d

K

An alternative

Controller 2

( ) ( ) ( )

a P D

v t K e t K e t ′ ′ = + ɺ

3 2

( ) / ( ) ( )

m D P D r a t a t P

s K K s K K G s s R s s T K K R

τ τ

Γ α θ Γ θ ′ ′ ′ + = = ′ + +

4 2

( ) 1 1 ( ) ( )

m d t P a t

s G s s s s T K K R

τ

τ Γ α θ Γ       = = −     ′  + +    

A zero appears

Basilio Bona 38 ROBOTICA 03CFIOR

– +

( )

G s

ω

1 s

a

v

m

ω

m

θ

P D

K sK ′ ′ +

– +

r

θ e

d

τ

d

K

Another alternative

– +

D

K

Controller 3 –

Basilio Bona 39 ROBOTICA 03CFIOR

Comparison

Controller 2 Controller 1

( ) ( ) ( )

a D P D m

v t K K e t K t ω = − ( ) ( ) ( ) ( )

a P D r D m

v t K e t K t K t ω ω ′ = + −

Controller 3

( )

( ) ( ) ( ) 1 1 ( )

a D P D D r D D D m

v t K K e t K K t K K K t ω ω ′ ′ ′ ′ = + − +

Basilio Bona 40 ROBOTICA 03CFIOR

Practical Issues

• 1. Saturating actuators
• 2. Elasticity of the structure
• 3. Nonlinear friction at joints
• 3. Nonlinear friction at joints
• 4. Sensors or amplifiers with finite band

Basilio Bona 41 ROBOTICA 03CFIOR

Saturating Actuators

saturation

( ) u t ( ) y t

( ) y t ( ) u t

It is a nonlinear effect, difficult to be considered a-priori Linearity saturation

max max min max min min

( ) ( ) ( ) ( ) ( ) y u t u y t ku t u u t u y u t u           > = ≤ ≤  < IF IF IF

Basilio Bona 42 ROBOTICA 03CFIOR

Elasticity of the structure

Although we have considered rigid bodies, the elasticity is a phenomenon that limits the closed loop band We cannot design controllers that are “too fast” without taking explicitly into consideration some sort of elastic model. Recall that when we use a simplified model

( ) ( )

t m t m

t k t Γ θ θ + = ɺɺ

the proper structural resonance (or natural) frequency is

t r t

k ω Γ =

Basilio Bona 43 ROBOTICA 03CFIOR

Elasticity of the structure

Basilio Bona 44 ROBOTICA 03CFIOR

Nonlinear friction

total

( ) v t ( ) f t

( ) f t ( ) v t

A nonlinear effect between velocity and friction force

viscous coulomb stiction

( ) v t

Basilio Bona 45 ROBOTICA 03CFIOR

Nonlinear friction

Nonlinear static friction models include:

Coulomb + viscous Static model that includes stiction and “Stribeck effect”:

sign

c

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

Basilio Bona 46 ROBOTICA 03CFIOR

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

( )

sign

s s

v v c s c

F(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 real These 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.

47 ROBOTICA 03CFIOR Basilio Bona