Rigid Manipulator Control - · PDF fileRigid Manipulator Control ... Li Ri t + η gearbox...

Rigid Manipulator Control

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

CY

The control problem consists in the design of control algorithms for the robot motors, such that the TCP motion follows a specified task in the cartesian space

Two types of task can be defined:

02

CFI

C Two types of task 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

CFI

DV

the manipulator moves its TCP following cartesian trajectories, with constraint on positions, velocities and accelerations due to the manipulator itself or the task requirements

CA

–0

1 Sometimes 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

OB

OTI

C 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

RO

We will consider only the first type of task

Th t l t k l t j i t l l (j i t t l) t t i l l

Basilio Bona – DAUIN – Politecnico di Torino 005/1

The control may take place at joint level (joint space control) or at cartesian level (task space control)

Joint Space controlC

Y0

2C

FIC

CFI

DV

C

A –

01

The transducer measures the value of the joint quantities (angles, displacements) and compares them with the desired ones obtained if necessary from the desired

OB

OTI

C and compares them with the desired ones, obtained, if necessary, from the desired cartesian quantities

The Inverse Kinematics block transforms the desired task space positions and

RO The Inverse Kinematics block transforms the desired task space positions and

velocities into desired joint space reference values


Task Space controlC

Y Controller Actuator Gearbox Robotdp p

02

CFI

CC

FID

V

Transducer

CA

–0

1

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

OB

OTI

C 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

RO most used ones are digital camera sensors (vision based control) or other types of

exteroceptiveexteroceptive sensors (infra-red, ultra-sonic, ...)


Joint Space Control ArchitecturesC

Y

Two main joint space control architectures are possible

Decentralized control or independent joint control: each i-th joint motor has a local

02

CFI

C 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

( )iq t

( )iq t

CFI

DV

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

CA

–0

1

This scheme is very common in industrial robots, due to its simplicity, modularity and robustness

OB

OTI

C

The classical PUMA robot architecture is shown in the following slide

C t li d t l th i l MIMO t ll th t t dRO Centralized control: there is only one MIMO controller that generates a command

vector for joint motors; it is based on the complete model of the manipulator and takes into account the entire vector of measured positions and velocities ( )tq ( )tq


Decentralized ControlC

Y

joint 1reference

Decentralized Joint Control1q (t)

controller 1

02

CFI

C

taskspace

jointspace

CFI

DV

controller 2

joint 2reference 2q (t)

p p

CA

–0

1O

BO

TIC

…

RO

controller 6

joint 6reference 6q (t)


Decentralized Control

T h

CY

Disk Teachpendant AccessoriesTerminal

PUMA Control

02

CFI

C

μG D/A Amplifier Motor 1

CFI

DV

DLV-11J

EPROM

EncoderT=0.875 ms

CA

–0

1 EPROM

RAM

InterfaceReference

anglesT=28 ms

OB

OTI

C

CPUμG D/A Amplifier Motor 6

RO / p

EncoderT=0.875 ms


COMPUTER ROBOT CONTROL

Motor and Gearbox Model

G b G t i

CY GearboxFriction

′N

r =

Gearbox = Geartrain

02

CFI

C ′N

CFI

DV

RobotInertiamN; ω rτ

τ

CA

–0

1

Frictionm

OB

OTI

C

Inertia ′ ′mN ; ω′

rτ

RO

Motor′mτ


r mτ ω′ ′r mτ ω

Motor and Gearbox ModelC

Y bβm r;ω τ

02

CFI

C

i aLaR N bΓ mτ

CFI

DV

aiaa N b m

CA

–0

1

avei E mΓ ′N

′τ

OB

OTI

C

mβ

mτ′ ′m r;ω τ

RO mβ


Losses in Geared MotorC

Y

Motorside

Jointside

02

CFI

C

Armature circuit

Motor inertia Gearbox Joint

inertiaaEi

m rω τ′ ′

a av i

m rω τ

m mω τ

CFI

DV

d

CA

–0

1 dda a a a

L i R it

+ ηgearboxefficiency

OB

OTI

C

ddm m m mt

Γ ω β ω′ ′+voltage drop

efficiency

dΓ ω β ω+

RO

pτ ′

db m b mtΓ ω β ω+


pτ

Gearbox ModelC

Y

ρ θ ρθρ ω ρω

′ ′ =′ ′

Nr

Nρρ

= =′ ′ω τ

02

CFI

C

Joint side ρ

ρ ω ρω= Nρm rω τ

Ideal gearbox: 1η =

CFI

DV

θGEARBOX

Power in Power out

CA

–0

1

M t id

θ′

′

GEARBOX

m rω τ′ ′

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

OB

OTI

C Motor side ρ′

m rω τ′ ′

rτ τ ′=

RO r r

m

rτ τω

ω′

=


m r

CC Motor Equations (1)C

Y

d

MOTOR SIDE

02

CFI

C dda a a a a

L i v R i E

K it

φ

= − −

CFI

DV

e

m m

K i

E k Kφ

ωω ωφ

φ=

′ ′= =k k K K′ ⇒

CA

–0

1 m m

m a ak i K i

ω

τττ

φ′ ′= =′

k k K Kω τ′≈ ⇒ ≈

OB

OTI

C

mai

Kτ

τ=

′ ′ ′RO

p m m m m m m m m

r m p

τ β ω βτΓ θ ω

τ τθ Γ′ ′ ′= + = +

′ ′ ′= −


r m p

CC Motor Equations (2)C

Y

GEARBOX SIDE

′1

MOTOR SIDE

02

CFI

C

( )r r

m p

r

r

τ ττ τ

′=′ ′−

⎡ ⎤=

1

1( )

r rrτ τ′ =

CFI

DV

( ) ( )m m m m m

m m m m m

r

r r r

τ ω β ω

τ ω β ω

Γ

Γ

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

( )

1m pr

Γ

τ τ

τ ω β ω

−

⎡ ⎤= − −⎢ ⎥⎣ ⎦

=

CA

–0

1 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Γτ ω β ω

τ ω β ωΓ

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

⎛ ⎞⎟⎜⎟⎢ ⎥⎜ ⎟

⎟⎜ ⎟⎜ ⎟

OB

OTI

C

2

1 1( )

m b m b m

m b m b m

r r rβ

τ ω ωΓ β

⎟⎢ ⎥⎜ ⎟⎝ ⎠⎣ ⎦

′ ′= − +

⎟⎜ ⎟⎝ ⎠

RO 2

( )m b m b mr r

β


Control EquationC

Y

( ) ( , ) ( ) ( ) Te

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

02

CFI

C

component-wise joint torques

CFI

DV

( ) )) ((n n n

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

CA

–0

1

1 1 1

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

j j k

q q gq qβ= = =

+ + + +∑ ∑∑q q q

OB

OTI

C

1 1

() ( ( ))( )n n n

i ij j ijk j k bi i i fij i j

riik

iqH gqH q h q q τβ τ

≠ = =

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

RO

I ti l t Coriolis & centripetal Friction, gravity


Inertial torques Co o s & ce t petatorques

ct o , g a ty& external torques

Control EquationC

Y ( ) ( ) ( )( )n n n

ij j ijk j kii i bi i fi riiH q h q q gH q q ττβ+ =+ + + +∑ ∑∑q q qq

02

CFI

C 1 1j i j k≠ = =

CFI

DV

( )Mi ci gi f rii i bi ii i

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

CA

–0

1O

BO

TIC

Modelled torques “Disturbance” torques

RO


From single motor model to robot control equation

θ′ ′ ′

CY

mi mi mii i i

i i i

q qr r

qr

θ ω ω= = =

′ ′ ′

02

CFI

C

Gearbox transformation

CFI

DV

Gearbox transformation

n

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

CA

–0

1

StructuredStructured

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

mi mi

H q H qqτ β τ τ τ τ

ωβ τ τ τ τΓ

ω≠

= + + + + +

′+ + +

+

′= + +

∑

OB

OTI

C StructuredStructureddisturbancedisturbancebi bi Mi ci gi fi

i i

mi mi

r rβ τ τ τ τ

ωβ

Γ

ωΓ

+ + +=

′

+ +

′

RO mi mi

bi bi dii ir r

βΓ τ= + +


Equation seen at the joint sidejoint side

From single motor model to robot control equation

Equation seen at the motor sidemotor side

CY

Equation seen at the motor sidemotor side

since( )1 1 1Γ β′ ′′

02

CFI

C ( )2ri ri bi mi bi mi dii iir rr

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

CFI

DV

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

and

CA

–0

1

1 1 1β βΓ Γ

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

we obtain

OB

OTI

C

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

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

RO

Total inertia Total friction


From single motor model to robot control equationC

Y

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

02

CFI

C

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

CFI

DV

p d t m t mm d= + = + +Bτ τ τ Γ ω ω τ Motor sideMotor side

CA

–0

1

= + +t′Γ

t′B

OB

OTI

C

⎛ ⎞ ⎛ ⎞RO

2

1bi mi

ir

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

2

1bi mi

ir

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


i⎝ ⎠ i⎝ ⎠

From single motor model to robot control equationC

Y

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

02

CFI

C

Joint sideJoint side= + = + +Bτ τ τ Γ ω ω τ

CFI

DV

Joint sideJoint sidep d t m t m dm

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

CA

–0

1

= + +t

ΓtB

OB

OTI

CR

O

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

bi i mirβ β+


Block Diagram of open-loop CC Motor (motor side)

For simplicity we drop the prime ′ symbol

CY

For simplicity we drop the prime ′ symbolfor the motor side quantities, and we considerthe generic i-th motor

Taking the Laplace transform of the involved variables, we have

02

CFI

C g p ,

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

s s s sβ ω τ τΓ + = −

CFI

DV

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

m aK iτ

βτ =

1( ) ( )s sθ ω

CA

–0

1

+dτ

( ) ( )m ms s

sθ ω=

OB

OTI

C +

– +1

a aR sL+

1

t tsβ Γ+

Kτ

1s

av

ai m

τ mω

mθ–

rτ

RO

Kω

E


ω


Th t i it i d t i ll d ll b l t d

CY

The armature circuit inductance is small and usually can be neglected

0 v RL i K ω− =≈ ⇒

02

CFI

C aa a a mω

miτ

CFI

DV

mai

K

R K

τ

τ

=

CA

–0

1 ma a mv R K

K ωτ

ω

τ τ ω β ωΓ

− =

= + +

OB

OTI

C m d t m t m

a a ad t t

R R Rv s K

τ τ ω β ω

τ β ω

Γ

Γ

= + +⎡ ⎤⎛ ⎞⎟⎜⎢ ⎥⎟− = + +⎜ ⎟⎢ ⎥⎜R

O a d t t mK K Kωτ τ τ

β+ + ⎟⎢ ⎥⎜ ⎟⎜⎝ ⎠⎢ ⎥⎣ ⎦


Block Diagram of open-loop CC Motor (motor side)C

Y a tR

K K KKω ω ω

β′ = + ≈

02

CFI

C Kω ω ωτ

βsince

CFI

DV

a tm m

RK

K ωτ

βω ω

CA

–0

1 friction torque

t mR i Kβ ω

ω

OB

OTI

C

armature losses armature e.m.f.torque m

a a ma

R i KK i ω

τ

τ

ω

RO



R RΓ⎛ ⎞

CY

11a t a

m a d

R Rs v

K K K K Kτ ω ω τ ω

Γω τ

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

02

CFI

C

dτ

a tR

TK K

Γ=

′

CFI

DV

d

dK

ad

K KR

KK

τ ω

=where

CA

–0

1

– θ

d Kτ

OB

OTI

C

+ ( )G sω

1s

av m

ωmθ

RO

( ) ( )1

G s =′


( ) ( )1K sTωω′ +

Matrix Formulation (joint side)

Lagrange

CY

( ) ( , ) ( ) ( )

( )

Te

r m m p

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

Rτ

τ τ τ τ

LagrangeEquation

02

CFI

C ( )r m m p

where

2= +R R K v R K qτ

CFI

DV

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τ Γ Γ

CA

–0

1

p

Motor side Joint sideMotor side Joint side

OB

OTI

C

0 0 0 00 0

KKK

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

and

RO

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 have

CY

Then, we have

Mass matrix Friction matrix

02

CFI

C

( ) ( )

( )M q ( )F q

CFI

DV

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

( ) ( )m m m m

Tω+ + + + + +

+ + =

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

g q J q F R K v

Γ

CA

–0

1 ( ) ( )e m a a

+ +g q J q F R K v

uGravity

Inte action

OB

OTI

C cu

Command input

Often we use this symbol to indicate

Interaction

RO

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

ythe velocity dependent terms


( )( ) ( ) ( )

Matrix Formulation (joint side)C

Y

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

Gravity and interaction

02

CFI

C ( ) ( , ) ( ) ( )Te c

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

No interaction

CFI

DV

( ) ( , ) ( )c

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

CA

–0

1

No gravity, no interaction

OB

OTI

C ( ) ( , )c

+ =M q q h q q u

RO

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


Decentralized Joint Control

A i f i li it

CY ( ) ( , )

c+ =M q q h q q u

Assuming, for simplicity

02

CFI

C ( ) ( , )c

q q q q

If …

CFI

DV

( ) diagonal 2 2( )m m m m

⇒ + ≈R I H q R RΓ Γ

CA

–0

1

small ( , )C q q q

Th

OB

OTI

C Then …

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

t m m= +R JΓwith

disturbance

RO

( )t cs + =F uΓ ω

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


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

Decentralized Joint Control – Local Controller

This is the proportional velocity controller

CY

dτ

This is the proportional velocity controlleror velocity compensator

02

CFI

C d

K

CFI

DV

dKreference voltage

CA

–0

1 –

+( )G sω

1s

av m

ωmθ

DK

–

+rv e

OB

OTI

C

( )KRO ( )t

K s

Transducer T.F. ( )K s K≈


(tachimetric sensor)( )t t

K s K≈

Open Loop vs Closed Loop C

Y

( )( )

( ) (1 )m Ds K

G sv s K s Tω

ωα

α′= =

′ +

( ) 1( )

( ) (1 )ms

G sv s K sTω

ω= =

′ +

02

CFI

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

av s K sTω +

CFI

DV

( )( ) ( )

( ) (1 )m

d d

s TG s K G s

s sTω

ωτ Γ

= = − = −+

CA

–0

1

( )( ) ( )m d d

s K K TG G

ω α α′ ′

( ) (1 )d ts sTτ Γ +

OB

OTI

C ( )( ) ( )

( ) (1 ) (1 )m d d

dd D t

G s G ss K s T K s Tω

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

′ + +

RO

1Kωα′

= <


1D t

K K Kω

α = <′ +

The closed-loop systemC

Y

dτ

K

02

CFI

C

dτ –

( )G s1a

v mω

mθ

dK

CFI

DV

dK

K

+( )G sω s

Open loop

CA

–0

1

DK

OB

OTI

C –

+( )G sω

′ 1s

rv

mω

mθ

RO s

Closed loop


The closed-loop systemC

Y

Time constant is reduced T Tα <

K

02

CFI

C

Disturbance gain is reduced d

dD

KK

K→

CFI

DV

Design parameterd

τ

CA

–0

1 when 1dK = dK

OB

OTI

C

–

( )G sω

1s

av m

ωmθ

DK

+rv e

RO

+( )

s–


Position CompensatorC

Y dτController 1

02

CFI

C

dK

CFI

DV

–

( )G1a

vm

ωmθ

K+e

K+rθ

CA

–0

1

+( )G sω sD

K

–PK

–

OB

OTI

C

tK

1tK ≈

RO

1K θ ≈

K θ


θ

Position Compensator

( )θ

CY

1 2

( )( )

( )m

r

s KG s

s s s T K

θθ α

= =+ +

02

CFI

C

2 2

( ) 1( )

( ) ( )ms

G ss s s T Kτ Γ

θ

α= = −

+ +

CFI

DV

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

D P D PK K K K K

K τ= =where

CA

–0

1

a t

KTK Rω Γ

= =′

Configuration dependent

OB

OTI

C

Second-order TF with1 1

·2 2

K K

T K R K K Kτ ω

α αζ

Γ

′= =

RO 2 2

1

a D P t

D P

T K R K K K

K K KK

τ

τ

α α Γ

ω = =


·n

a t

KR

ωΓ

= =

CY

⎛ ⎞

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

02

CFI

C

( )2

1t b mr

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

CFI

DV

( )( )b iiH tΓ = q

CA

–0

1 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

,maxtΓ

ζ,P DK K

OB

OTI

C j g g y p gsatisfactory, e.g., no overshoot appears in the step response

ζ,P D

RO


An alternativeC

Y

dτ

KController 2

02

CFI

C

+

dK

CFI

DV

–

+( )G sω

1s

av

mω

mθ

P DK sK′ ′+

–

+rθ e

CA

–0

1

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

OB

OTI

C

3 2

( ) /( )

( )m D P Ds K K s K K

G sR

τ

Γθθ

′ ′ ′+= =

′

A zero appears

RO 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 alternativeC

Y

dτ

KController 3

02

CFI

C

++

dK

CFI

DV

–+

( )G sω

1s

av

mω

mθ

P DK sK′ ′+

–

+rθ e

–+D

K

CA

–0

1O

BO

TIC

RO


ComparisonC

Y

Controller 1

( ) ( ) ( )t K K t K t

02

CFI

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

CFI

DV

Controller 2

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

CA

–0

1

Controller 3

OB

OTI

C Controller 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ω ω′ ′ ′ ′= + − +

RO


Practical IssuesC

Y0

2C

FIC

1.1. Saturating actuatorsSaturating actuators

CFI

DV

2.2. Elasticity of the structureElasticity of the structure

33 Nonlinear friction at jointsNonlinear friction at joints

CA

–0

1 3.3. Nonlinear friction at jointsNonlinear friction at joints

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

OB

OTI

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

RO


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

CY ( )y t

02

CFI

C

saturation

( )y t( )u t

CFI

DV

( )u t

( )y t( )u t

CA

–0

1

Li itsaturation

OB

OTI

C Linearity

( )⎧⎪RO if

if max max

min max

( )

( ) ( ) ( )

s u t u

y t ku t u u t u

⎧⎪⎪⎪⎪>

= ≤ ≤⎨⎪⎪


ifmin min

( )s u t u<⎪⎪⎪⎪⎩

Elasticity of the structureC

Y

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

d i ll h “ f ” i h ki li i l i

02

CFI

C We cannot design controllers that are “too fast” without taking explicitly into consideration some sort of elastic model.

R ll th t h i lifi d d l

CFI

DV

Recall that when we use a simplified model

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

CA

–0

1

the proper structural resonance (or natural) frequency is

OB

OTI

C

er

t

kω

Γ=

RO t


Elasticity of the structureC

Y0

2C

FIC

CFI

DV

C

A –

01

OB

OTI

CR

O


Nonlinear friction

It is a nonlinear effect between velocity and friction force

CY

It is a nonlinear effect between velocity and friction force

02

CFI

C

total( )f t

istiction

CFI

DV

( )

( )f t( )v tviscous

coulomb

stiction

CA

–0

1 ( )v t

OB

OTI

CR

O


Finite band in sensors and amplifiersC

Y

S d lifi ft d ll d i l i hil i th l ld th

02

CFI

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

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

CFI

DV

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

CA

–0

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

OB

OTI

CR

O


Inverse Dynamics Control

C id i th f ll i i lifi d d i d l f th b t

CY ( ) ( , )+ =M q q h q q u

Consider again the following simplified dynamic model of the robot

02

CFI

C ( ) ( , )c

+q q q q

CFI

DV

h

CA

–0

1

++

qrq

M O OO O

OB

OTI

C

+– qc

uM ROBOTROBOT

RO

cv

( )c r c

− +u M q v h


Approximate LinearizationC

Y

H

02

CFI

C Hence( ) ( , ) ( )

r c+ = − +M q q h q q M hq v

CFI

DV

( )1 1( ) ( ) ( , )( )r c

− −− −−=q M q M M q h q qq v h

CA

–0

1

( )( , ) ( , )Δ− =h q q h h q q1( ) ( )− = +M q M I E q

OB

OTI

C

( )1( ) ( )−= −E q M q M I

RO


ConclusionsC

Y

( ),) , ,(rc cr

= +−q vvq q q qη

02

CFI

C

If we can cancel this part, the system becomes linear and decoupled but unstable

CFI

DV

and decoupled, but unstable

( ) 1, , , ( ) ( )( ) ( , )rr c c

Δ−= −−qq q q v E q M q h qv qη

Structured disturbance

CA

–0

1

( ) ( ) ( )( ) ( )rr c c

Approximation in Approximation in

OB

OTI

C ppinertia modelling

Approximation in Coriolis modelling

RO


State Variable RepresentationC

Y0

2C

FIC

CFI

DV

C

A –

01

OB

OTI

CR

O


Error Variable RepresentationC

Y0

2C

FIC

CFI

DV

C

A –

01

OB

OTI

CR

O

This term is equivalent to the injection into the system


of a structured nonlinear disturbance that can make it unstable in spite of the control design

Controller DesignC

Y0

2C

FIC

CFI

DV

C

A –

01

OB

OTI

CR

O


Controller DesignC

Y

hInner Loop

02

CFI

C

+ qq

hROBOTROBOT

CFI

DV

+

+

–

q

qcu

rq

M ,M h

CA

–0

1

qcv

OB

OTI

CR

O

Outer Loop


Inner-Outer Loop (nonlinear linearizing control)C

Y0

2C

FIC

CFI

DV

C

A –

01

OB

OTI

CR

O


Exact LinearizationC

Y0

2C

FIC

CFI

DV

C

A –

01

OB

OTI

CR

O


Exact Linearization

( )h

CY q

( , )h q qROBOTROBOT

02

CFI

C

+

+

+

–

qq

cu

rq

,M h( )M q

CFI

DV

cv Inner Loop

CA

–0

1O

BO

TIC 1

s

1

s1q1

q+rq

RO

1

s

1

s

–

cv

nqn

q


c nn

Control design

PD Outer Loop Control DesignC

Y0

2C

FIC

CFI

DV

C

A –

01

OB

OTI

CR

O



Y0

2C

FIC

CFI

DV

C

A –

01

OB

OTI

CR

O



Y Inner Loop

( , )h q qROBOTROBOT

02

CFI

C

+

+

+

–

qq

cu

rq

Inner Loop

,M h( )M q

CFI

DV

cv

CA

–0

1

+qOuter Loop

OB

OTI

C

D−K

+

+

+

+

–q

rq

RO

P−K

+

–

rq


CY

02

CFI

CC

FID

V

CA

–0

1O

BO

TIC

RO


CY

h

q

02

CFI

C

ROBOTM+

+

+

–

q

qcurq

CFI

DV

cv

CA

–0

1O

BO

TIC

DK−

+

++

–

rq

K

+

RO

PK−+

–

rq IKs

−


CY

( )g q

02

CFI

C

ROBOT+

q

qcu

–

CFI

DV

cv

CA

–0

1O

BO

TIC

DK−

+

++

–

rq

RO

PK−+

–

rq

Outer Loop


CY q

02

CFI

C

ROBOT

q

qc c= −u v

CFI

DV

C

A –

01

++q

OB

OTI

C

DK−

+

+

+

+

–rq

rq

RO

PK−+

–

rq

Outer Loop


CY

( ),r rh q q

02

CFI

C

ROBOT( )rM q+

+

+q

qcurq

CFI

DV

–

cv Inner Loop

CA

–0

1O

BO

TIC

DK−

+

++

–rq

RO

PK−+

–

rq

Outer Loop


Rigid Manipulator Control - · PDF fileRigid Manipulator Control ... Li Ri t + η gearbox...

Documents

Transcript of Rigid Manipulator Control - · PDF fileRigid Manipulator Control ... Li Ri t + η gearbox...