Robot Control Independent joint control Lyapunov Theory Multivariable PD control and Lyapunov Theory Inverse Dynamic Control Robust Inverse Dynamic Control.

Robot Control

• Independent joint control

• Lyapunov Theory

• Multivariable PD control and Lyapunov Theory

• Inverse Dynamic Control

• Robust Inverse Dynamic Control

Monday 12 Sept 2005

Feedforward Compensation – Independent Joint

)(sE

effeff BsJ 1

+

dr

ss

s1

vK+ -

d

ssKK DP

effeff sBJs 2

d

ddPDeffeff rreKKeKKBeJ )(

desired.for stands t superscrip

)()( where,

d

qqqcqqdr d

ji

d

j

d

i

d

ijkj

d

j

d

kj

d

d k

kj

d

dr

dt

dx

xf

dtdx

xf

dtdx

xf

dt

txtxtxdf n

n

n

2

2

1

1

21 ))(),(),((

Chain Rule:

.)0(,)0(,0

:ofSolution 00

01 eeeeeKeKe

.,

,

0

0

0

00

0

0

00

0

:ofSolution

0

0

1

0

0

1

1

0

0

1

1

1

1

11

11

nn

nn

e

e

e

e

e

e

e

e

e

e

k

k

e

e

k

k

e

e

nn

nnn

point. mequilibriu theisin origin the

casesmost In system. theofpoint mequilibriu thecalled is

and that suppose and on field vector a is )(

)(

n

n

0

x

0xx

xx

e

e )f(f

f

such that 0 exists there

ifonly and if stableally asymptotic is )(solution null The

:Definition

δ

tx

)(

)(

)(

)(; 2

1

2

1

x

x

x

xx

nn f

f

f

f

x

x

x

ttδt as 0)()( 0 xx

Time Simulation

Time (s)

Angle (rad)

0 10 20 30 40 50 60-0.4

-0.2

0.0

0.2

0.4

0.6

0.8

1.0

Time Simulation

Time (s)

Speed (rad/s)

0 10 20 30 40 50 60-0.20

-0.15

-0.10

-0.05

0.00

0.05

0.10

Phase Portrait

Angle (rad)

Speed (rad/s)

-0.4 -0.2 0.0 0.2 0.4 0.6 0.8 1.0-0.20

-0.15

-0.10

-0.05

0.00

0.05

0.10

;0

1)0(

;2.0sin1.0 21

2

2

1

x

xx

x

x

x

;1

1)0(

;2.0sin1.0 21

2

2

1

x

xx

x

x

x

Time Simulation

Time (s)

Angle (rad)

0 10 20 30 40 50 601

2

3

4

5

6

7

Time Simulation

Time (s)

Speed (rad/s)

0 10 20 30 40 50 60-0.2

0.0

0.2

0.4

0.6

0.8

1.0

Phase Portrait

Angle (rad)

Speed (rad/s)

1 2 3 4 5 6 7-0.2

0.0

0.2

0.4

0.6

0.8

1.0

form. quadratic a be tosaid is

)V(

functionscalar the}{matrix symmetric aGiven

:Definition

1,

n

jijiij

T

ij

xxpP

pP

xxx

).0)( that (note 0for 0)( 0xx VV

)(xV

, will be positive definite if and only if the matrix P is positive definite matrix, that is, has all eigenvalues positive.

, equivalently the quadratic form, is said to be positive definite if and only if

)(xV

1x

2x

cxxV ),( 21

),( 11

21xx),( 22

21xx

21

2

22

1

11

21

then,),( and ),(Let

),( definite positiveFor

2121

ccc

cxxVcxxV

xxV

Positive Definite Functions

The null solution of

is asymptotically stable if there exists a Lyapunov function candidate V such that is

negative definite along solution trajectories, that is,

Theorem:

)(xx f

V

0

)(

)(

)(

2

1

21

2

2

1

1

x

x

x

n

n

n

n

f

f

f

xV

xV

xV

dtdx

xV

dtdx

xV

dtdx

xV

V

stable? system theIs ).( Find

21

)(;Let 23

xV

xxVxx

stable? system theIs ).( Find

21

)(;Let 22

xV

xxVxx

1x

2x

ctxV ))(( 0

A Non-rigorous Proof

?0 (b) and 0 (a) when )( is where

figure, above In the

))(( and ))((let

))(()()(let ),(

1

1100

001

VVtx

txVV ctxVV

ttftxtxf xxx

12

2

12

21

:as drepresente be system dynamical aLet

xxxx

xx

.0)0,0( and

)0,0(,for 0,21

21

,

2121

2

2

2

121

V

xxxxV

xxxxV

Example

.0)0,0( and

)0,0(,for 0,

)(

,

2121

2

2

2

1

212

2

112

221121

V

xxxxV

xx

xxxxxx

xxxxxxV

212

211

22

3

:is system dynamicalA

xxx

xxx

stable? system theIs ., Find21

21

,

21

2

2

2

121

xxV

xxxxV

Example

stable? system theIs ., Find21

21

,

21

2

221

2

121

xxV

xxxxxxV

212

211

22

3

:is system dynamicalA

xxx

xxx

Example

stable? system theIs ., Find47

21

,

21

2

221

2

121

xxV

xxxxxxV

cos

:is system dynamicalA

mglBm

Example

system? thestabilise chosen to be Can

., Find21

21

,

21

2

2

2

121

xxV

xxxxV

m

l

Given the system

suppose a Lyapunov function candidate V such that along solution trajectories

Then the above system is asymptotically stable if does not vanish identically along any solution of the above system other than the null solution, that is, the system is asymptotically stable if the only solution of the system satisfying

is the null solution.

LaSalle’s Theorem:

)(xx f

.0

V

V

0

V

3

12

21

2xxx

xx

0)even when (zero 0

)(

,

21

21

,

1

4

2

21

3

212

221121

2

2

2

121

x

x

xxxxx

xxxxxxV

xxxxVExample:

V-dot is zero when x2 is zero. When x2 is zero (in steady-state) then its rate of change also must be zero so x1 has to be identically zero from the second equation. So finally the rate of change of x1 should be zero from the first equation hence V is identically zero only along the equilibrium solution (0,0) of the system. Hence according to LaSalle’s Theorem the system is asymptotically stable.

.,2,1,)()(,

nkqqqcqqd kkjji

iijkj

jkj

kk lksk

th qk , actuator, For

.,2,1),())()((

)())((

,

2

2

nktvR

Krqqqcqqdr

R

KKBqqdrJq

k

k

mk

kjji

iijkj

jkjk

k

mb

mkkkkmk

k

kj

kk

kk

From a previous slide (30 Aug 2005 Lecture)

uqgqBqqqCqJqD

)(),())((

}/

,,/

diag{ },,,diag{ where22

1

1

22

1

1111

n

nmbmmbm

n

mm

r

RKKB

r

RKKBB

r

J

r

JJ nnnn

T

n

n

mm vrR

Kv

rR

Ku n

2

1

11

,,1

Independent joint PD-control Scheme

gains. positive of matrices diagonal are and Matrices

error)for used also is )(q~ symbol (the

)(

DP

d

D

d

P

KK

qq

qKqqKu

qKqqJqDqV P

TT ~~21

))((21

Function Lyapunov

qKqqqDqqJqDqV P

TTT ~)(21

))((21

)(),())((Remember qgqBqqqCuqJqD

Simplify V-dot. Is this sign definite?

Theorem 6.3.1 (p. 143 of the textbook)

Define the matrix

Then is skew symmetric, that is, the components

),(2)(),(N qqCqDqq

),(N qq

.satisfy N of kjjkjk nnn

Proof:

i

n

i i

kj

kj qq

dd

1

) (therefore

2

11

1

i

n

i j

ik

k

ji

jki

n

i k

ij

j

ki

i

n

i k

ij

j

ki

i

kj

i

kj

kjkjkj

qqd

q

dnq

q

d

qd

qq

d

qd

q

d

q

dcdn

Since the inertia matrix D(q) is symmetric, it is not difficult to see that N is skew symmetric.

Independent joint PD-control Scheme

))((

)~)(~(

)),(2)((21

)~)((

)(21

))(),((

~)(21

))((

qgqBqKq

qKqgqBqKqKq

qqqCqDqqKqgqBuq

qqDqqgqBqqqCuq

qKqqqDqqJqDqV

D

T

PDP

T

T

P

T

TT

P

TTT

1. We neglect the gravity terms or add an extra term g(q) in the input u.

2. We choose KD such that KD +B is positive definite.

3. This makes V-dot negative semi-definite.

))(( qKqqKu D

d

P

When V-dot is identically equal to zero then it implies that q-dot and q-double dot are both identically equal to zero. But this doesn’t imply that the error is zero, i.e., q-desired may not equal q. But from

we see that when v-dot is zero we must have

LaSalle’s Theorem then implies that the system is asymptotically stable.

To account for the gravity torque we can either have the position gains Kp very large or modify the input expression as

qKqKqqqCqJqD DP ~),())(

qKP~0

g(q) )(

qKqqKu D

d

P

Inverse Dynamics or Computed Torque

uqgqBqqqCqJqD

)(),())((

)(),(),( and ))(()( where

),()(

:asequation above thecan write wehere purposesour For

qgqBqqqCqqhJqDqM

uqqhqqM

)invertible is that (Note

),()(),()(

get we),,()(

:Choosing

M(q)

vq

qqhvqMqqhqqM

qqhvqMu

rqKqKq

rqKqKv

01

10 becomes system integrator double the,

:Set

)()()(

choose )),(),((

:y trajectordesired Given the

01 tqKtqKtqr

tqtqtddd

dd

0

giving ),()()(

:gives This

01

0101

e(t)K(t)eK(t)e

tqKtqKtqqKqKq ddd

}2,,diag{2

},,diag{

:is and for choice goodA

11

22

10

10

n

n

K

K

KK

Robust compensator

Trajectory planner

Nonlinear interface

Plant

ddd qqq ,,

Inner loop

Outer loop

Nominally linear system

uq

q

v

),()( qqhvqMu

)()( 01 qqKqqKqv ddd

)()(

sinIu

law control dynamic Inverse

01

ddd kkv

MgLv

Example 8.4.1 (page 226)

uMgLI sin

;5;5I

10; gL 5 10; 5

MgL

MI

)sin))()((Isin(1

,

:tionrepresenta space State

1102112

21

21

xMgLxkxkxMgLI

x

xx

xx

ddd

Trajectory Interpolation

0)(;0)(;1)(;1)(;0)(

s 1 s, 5.0 s, 0

)(

:as defined bey trajectorLet the

22210

210

432

ttttt

ttt

etdtctbtat

0

0

1

1

0

)(

)(

)(

)(

)(

0

0

1

1

1

2

2

2

1

0

4

2

3

2

2

22

4

2

3

2

2

22

4

2

3

2

2

22

4

1

3

1

2

11

4

0

3

0

2

00

t

t

t

t

t

e

d

c

b

a

tttt

tttt

tttt

tttt

tttt

Solve for (a,b,c,d,e).

2

7

9

5

0

e

d

c

b

a

3

32

432

244218)(

821185)(

27950)(

ttt

tttt

ttttt

d

d

d

Theta

Time (s)

Angle (rad)

0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.00.0

0.2

0.4

0.6

0.8

1.0

1.2

)()(

sinIu

law control dynamic Inverse

01

ddd kkv

MgLv

Example 8.4.1 (page 226)

uMgLI sin

;5.7;5.7I

10; gL 5 10; 5

MgL

MI

)sin))()((Isin(1

,

:tionrepresenta space State

1102112

21

21

xMgLxkxkxMgLI

x

xx

xx

ddd

//(1) trajectory planning//(2) simulation of inverse dynamic control of one link manipulator//I*theta-double dot+MgL*sin(theta) = u//10 Sept 2005//Go to the directory where you keep this file and then to run it//exec('invdynpendulum.sce')clear t0 t1 t2 t I Ihat Mglhat k0 k1 dt abcde pos speed acc x1 x2 z m trajt0=0; t1=0.5; t2=1;I=5;Mgl=10;Ihat=5;Mglhat=5;k0=10;k1=10;dt=0.01;//theta=a+b*t+c*t^2+d*t^3+e*t^4traj=[0;1;1;0;0];//position at t=t0, t=t1, t=t2; speed at t=t2; acceleration at t=t2m=[1,t0,t0^2,t0^3,t0^4;1,t1,t1^2,t1^3,t1^4;1,t2,t2^2,t2^3,t2^4;0,1,2*t2,3*t2^2,4*t2^3;0,0,2,6*t2,12*t2^2];abcde=inv(m)*traj;t=[0:dt:1];pos=abcde'*[ones(size(t,1),size(t,2));t;t.*t;(t.*t).*t;(t.*t).*(t.*t)];speed=abcde'*[zeros(size(t,1),size(t,2));ones(size(t,1),size(t,2));2*t;3*(t.*t);4*(t.*t).*t];acc=abcde'*[zeros(size(t,1),size(t,2));zeros(size(t,1),size(t,2));... 2*ones(size(t,1),size(t,2));6*(t);12*t.*t];x1(1)=pos(1); x2(1) = speed(1);z=[x1(1);x2(1)]; //initial conditionsfor k = 2:size(t,2),z(1)=z(1)+z(2)*dt;z(2)=z(2)+(-Mgl*sin(z(1))+Ihat*(acc(k)+k1*(speed(k)-z(2))+k0*(pos(k)-z(1)))+Mglhat*sin(z(1)))*(dt/I);x1(k)=z(1);x2(k)=z(2);end;f1=scf(1);clf(f1,'reset');plot2d(t,pos,1);plot(t,x1,2);xgrid;xtitle('Theta', 'Time (s)','Angle (rad)');

blue)(in control dynamic Inverse black);(in 27950)( 432 tttttd

k0=1, k1=2 k0=10, k1=10

)sin))()((Isin(1

1102112

21

xMgLxkxkxMgLI

x

xx

ddd

Theta

Time (s)

Angle (rad)

0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.00.0

0.2

0.4

0.6

0.8

1.0

1.2

I = 5, MgL = 10

Theta

Time (s)

Angle (rad)

0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.00.0

0.2

0.4

0.6

0.8

1.0

1.2

desireddesired

),()(u

:is law control dynamic inverse theso

,),( and )( estimates only their have but we

),( and )(

of knowledge theneed wecontrol dynamic inverseFor

),()(

:are equations dynamicr Manipulato

qqhvqM

qqhqM

qqhqM

uqqhqqM

Implementation and Robustness Issues

),()(),()(

:are equations loop-closed The

qqhvqMqqhqqM

)),(),(()(

)),(),((11

11

qqhqqhMvIMMv

qqhqqhMvMMq

hMEvqqv

qqhqqhh

IMME

1

1

),,(

),(),(

Define:

),,( qqvvq

)(),(

),,()()()(

)()(

21

01

01

dd

ddd

ddd

qqxqqx

qqvvqqKqqKqq

vqqKqqKqv

)),,((

),(

definite)-positive symmetric are and ( 21

21

),(

21211022211

22211121

2122211121

xxvvxKxKPxxPx

xPxxPxxxV

PPxPxxPxxxV

TT

TT

TT

),,( 2121102

21

xxvvxKxKx

xx

symmetric) are and ( 10 KK

)),,((),( 2121102221121 xxvvxKxKPxxPxxxV TT

We need to choose the control

such that

.0),( 21 xxV

10 and,, KKv

)),,(()(

21

)(21

0

)),,((),(

2122

2

1

12201

201

21

21222122220121121

xxvvPxx

x

KPPKP

PKPxx

xxvvPxxKPxxPKxxPxxxV

TTT

TTTTT

0 )(

21

)(21

0

:such that matrix definite-positive a choose Can we

2

1

21

12201

201

x

xQxxQ

KPPKP

PKP

Q

TT

This is not likely because of the zero elements of the upper left quarter of the matrix. This means we have to try another Lyapunov function candidate.

definite)-positive and symmetric s (

),(

2

1

21

2

1

2221

1211

21

222212122121111121

iP

x

xPxx

x

x

PP

PPxx

xPxxPxxPxxPxxxV

TTTT

TTTT

Find ).,( 21 xxV

)),,((

)),,((2

22),(

2121102222212

2121101212111

222212122121111121

xxvvxKxKPxxPx

xxvvxKxKPxxPx

xPxxPxxPxxPxxxV

TT

TT

TTTT

)),,((

21

21

),(

21

22

12

21

2

1

1222111211

11211012

2121

xxvvP

Pxx

x

x

KPPKPP

KPPKPxxxxV

TT

TT

0

21

21

:such that matrix definite-positive a choose Can we

2

1

21

1222111211

11211012

x

xQxxQ

KPPKPP

KPPKP

Q

TT

This should be possible. So choose the P matrix and K0 and K1 such that Q is negative definite.

)),,((

: of choiceproper awith

definite-negative termfollowing themake tohave weNow

21

22

12

21 xxvvP

Pxx

v

TT

)),,((

: thatsure make toneed weNow

212xxvvPx TT

later. see willwhich we

assumption more some need wecondition issatisfy th To

.on dependent not is ),,(function that theNote

),,(),,(

assume sLet'

it. into lumped iesuncertaint model theall has ),,( vector The

21

2121

21

vtxx

txxxxv

xxv

)),,((

:as written becan expression above Then the

and

:Define

21

2212221

2xxvvPx

PPPxxx

TT

TTT

0 if0

0 if),,(

:Choose

)),,((: thatsure make toneed weNow

2

2

2

221

212

xP

xPxPxP

txxv

xxvvPx TT

0. when happens what see sLet'

definite.-negativeclearly is 0,When

2

2

xP

VxP

)),,(),,(

)),,(),,(

)),,(),,((

21212

2121

2

2

21

2

221

2

2

2

2

xxvPxtxxxP

xxvPxtxxxP

xPPx

xxvxPxP

txxPx

TT

TT

TT

TT

.0)),,(),,(

making

),,(),,(

:is termsecond theof magnitude the

)),,(),,(

In

21212

21221

21212

2

2

2

xxvPxtxxxP

txxxPxxvPx

xxvPxtxxxP

TT

TT

TT

.0),(

Hence

21 xxV

by t. bounded function known afor ),( 3. Assumption

. allfor , somefor 1 2. Assumption

.sup 1. Assumption

21

1

10

xxh

qIMME

Qqd

t

).,,(),,(

have topossibleit making sAssumption

2121 txxxxv

hMvqqKqqKqEqqv ddd 1

01 ))()((),,(

),(),,( 21

1

1021 xxMxKxKqEvEqqv d

. allfor

and ),,(vlet Also1

21

qM(q)MM

txx-

),,(:

),()(),,(),,(

21

211021121

txx

xxMxKxKQtxxqqv

),(1

1),,(

:Choose

211021121 xxMxKxKQtxx

hMvqqKqqKqEqqv ddd 1

01 ))()((),,(

),()(),,( 21

1

1021 xxMxKxKEqEvEqqv d

0 if0

0 if),,(

:Remember

2

2

2

221

xP

xPxPxP

txxv

vkkv

MgLvddd

)()(

sinIu

law control dynamic Inverse

01

Example 8.4.1 (page 226)

uMgLI sin

;5.7;5.7I

10; gL 5 10; 5

MgL

MI

dd

dd

xMgLvxkxkxMgLI

x

xx

xx

)sin))(Isin(1

,

:tionrepresenta space State

1102112

21

21

;5.7;5.7I

10; gL and 5 for simulate sLet'

MgL

MI

5.2),(sinsin 3. Assumption

5.0155.7

1ˆ 2. Assumption

3744.0sup 1. Assumption

21

1

10

xxMgLMgLh

IIE

Qqd

t

0.2.M and 1.0M 2.01.0 1 -I

0 if0

0 if)(

),,(),,(

:Choose

21

2

221

dd

dd

dd

dd

txxxPxP

txxv

)5.2*2.0)()(2*5.03744.0*5.0(2

),()()(1

1),,(

:Choose

2101121

dd

dd xxMkkQtxx

.010

01

21

21

:1 2, 1, Choose

1222111211

11211012

2221121110

kppkpp

kppkp

ppppkk

//(1) trajectory planning

//(2) simulation of inverse dynamic control of one link manipulator

//I*theta-double dot+MgL*sin(theta) = u

//10 Sept 2005

//Go to the directory where you keep this file and then to run it

//exec('invdynpendulum.sce')

clear t0 t1 t2 t I Ihat Mglhat k0 k1 dt abcde pos speed acc x1 x2 z m traj Q1

t0=0; t1=0.5; t2=1;

I=5;Mgl=10;Ihat=7.5;Mglhat=7.5;

k0=1;k1=2;dt=0.01;

alpha=0.5;Mbar=0.2; phi=2.5;p11=1;p12=1;p21=11;p22=1;

//theta=a+b*t+c*t^2+d*t^3+e*t^4

traj=[0;1;1;0;0];//position at t=t0, t=t1, t=t2; speed at t=t2; acceleration at t=t2

m=[1,t0,t0^2,t0^3,t0^4;1,t1,t1^2,t1^3,t1^4;1,t2,t2^2,t2^3,t2^4;0,1,2*t2,3*t2^2,4*t2^3;0,0,2,6*t2,12*t2^2];

abcde=inv(m)*traj;

t=[0:dt:1];

pos=abcde'*[ones(size(t,1),size(t,2));t;t.*t;(t.*t).*t;(t.*t).*(t.*t)];

speed=abcde'*[zeros(size(t,1),size(t,2));ones(size(t,1),size(t,2));2*t;3*(t.*t);4*(t.*t).*t];

acc=abcde'*[zeros(size(t,1),size(t,2));zeros(size(t,1),size(t,2));...

2*ones(size(t,1),size(t,2));6*(t);12*t.*t];

Q1=max(acc);

x1(1)=pos(1); x2(1) = speed(1);z=[x1(1);x2(1)]; //initial conditionsfor k = 2:size(t,2),etheta=z(1)-pos(k);espeed=z(2)-speed(k);rho=(1/(1-alpha))*(alpha*Q1+alpha*sqrt(k0*k0+k1*k1)*sqrt(etheta*etheta+espeed*espeed)+Mbar*phi);if abs(z(2)+z(1)) > 0.0000000001 then delv=-rho*(etheta+espeed)/abs((etheta+espeed));else delv=0;end;z(1)=z(1)+z(2)*dt;z(2)=z(2)+(-Mgl*sin(z(1))+Ihat*(acc(k)+k1*(speed(k)-z(2))+k0*(pos(k)-z(1))+delv)+Mglhat*sin(z(1)))*(dt/I);x1(k)=z(1);x2(k)=z(2);end;f1=scf(1);clf(f1,'reset');plot2d(t,pos,1);plot(t,x1,2);xgrid;xtitle('Theta', 'Time (s)','Angle (rad)');

Theta

Time (s)

Angle (rad)

0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.00.0

0.2

0.4

0.6

0.8

1.0

1.2

Theta

Time (s)

Angle (rad)

0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.00.0

0.2

0.4

0.6

0.8

1.0

1.2

blue)(in control dynamic Inverse black);(in 27950)( 432 tttttd

k0=1, k1=2 I = 5, MgL = 10

0v algorithm controlrobust thefrom v

desired

desired

dd xMgLvxkxkxMgLI

x

xx

)sin))(Isin(1

1102112

21