1 0 10 g u =[ τ ,τ ] torque θ θ x =[ θ ,θ , θ , θ ] θ θ ˙ ˙ ˙ ˙ m m l l m m Maple Matlab symbolictoolbox • debugging Matlab controltoolboxes • •

(1)

mkqn libxz

zebefa yibdl ozip

:zeillkzexrd

ewae` ziteqdaeyza wtzqdloi` .mini`zn miaeyigaellnazeaeyzd lk z`xiaqdlyi

•

.xaqdk

,(

debugging

îkxevlôaenkôzip)^dybda

Matlab

^ly

control toolboxes

^d^jezn^ze
ewtd^ynzydl^oi`

•

.zxg`yxetnaoiievokm``l`

zipkhze earjeqglzpnlr(zeye

Maple

^e`)

Matlab

^ly

symbolic toolbox

â^xfridlûlneneôzip

•

.zerbiin

m ₁ m ₂

l

₂

l

₁

x

₁

x

₂

g

ixqg migiyw miwlg ipy zlra rexfd .i nin e xeyina drpy rexf ly zkxrna weqrp df libxza

xe`izl ozipzkxrnd avn .

m 2

^e

m 1

^zeiz
ewp ^zeqn^ze`vnp ^mdizevwa ^xy`

l 2

^e

l 1

^mkxe`y ^dqn

zkxrnd .

x = [θ 1 , θ 2 , ˙ θ 1 , ˙ θ 2 ] ^T

^:avn^xehweea ^onqp^xy`

˙θ 2

^e

˙θ 1

^zeiziefd ^zeiexidnde

θ 2

^e

θ 1

^zeiefd ^i"r

g

^dkiynd ^gek ^.

u = [τ 1 , τ 2 ] ^T

^epnqp ^.drepzd^ixiv ^lr⁽

torque

⁾ ^lezit^hpnen ^zlrtd ^i"r^dxwal ^zpzip

.zakey`idyk

0

^e`^z
ner ^zkxrndyk

10

^ekxry^xhnxt^`ed

:zkxrnazegekd of`nz`zex`znd drepzdze`eeyn

(2)

u = M (θ ¹ , θ 2 )

θ ¨ 1

θ ¨ 2

+ v(θ ¹ , θ 2 , ˙ θ 1 , ˙ θ 2 ) + g(θ ¹ , θ 2 )

θ ¨ ¹ θ ¨ 2

= M(θ) ⁻¹ (−u + v(θ) + g(θ))

M =

l ² 2 m 2 + 2l 1 l 2 m 2 cos(θ 2 ) + l 1 ² (m 1 + m 2 ) l 2 ² m 2 + l 1 l 2 m 2 cos(θ 2 ) l ² 2 m ² + l ¹ l ² m ² cos(θ ² ) l 2 ² m ²

v =

−m 2 l 1 l 2 sin(θ 2 ) ˙θ ² 2 − 2m 2 l 1 l 2 sin(θ 2 ) ˙θ 1 ˙θ 2

m 2 l 1 l 2 sin(θ ² ) ˙θ ² 1

g =

m 2 l 2 g cos(θ ¹ + θ ² ) + (m ¹ + m ² )l ¹ g cos(θ ¹ ) m 2 l 2 g cos(θ 1 + θ 2 )

zegeke milbetxhpvzegek ly xehwe `ed

v

^.(

positive definite

^`id^z`fkke) ^dqn ^zvixhn ^dpekn

M

.(

g = 0

^yk^qt`zn^df ^xehwe)^dkiynd ^gekn^miraepd^zegek^ly^xehwe ^df

g

^e^qileixew

:(

control project files.zip

^uaewa)^ze`ad

Matlab

^zeivwpet^mkzeyxl^z
ner"d arn"-azkxrndzpigajxevl

zxfra zixnep divxbhpi`) sivx onfa zkxrnd ly divleniq zrvan

two link arm control •

oezp izlgzd avnn (

Matlab

^a

ode45

^ziivwpet ^,dpzyn ^l
eba ^onf ^rve ⁴ ^x
qn

Runge Kutta

.

u(t, x)

^dxwa^ziivwpet^mrôezpônfôelga

m ewnksivxonfazkxrndly g` rvlydivleniqzrvan

arm noisy discrete control step •

zitvz dxifgn divwpetd .(dxwa yrx `ed

η

⁾

u + η

^reaw ^dxwa^ze` ^dxwa ^ziivwpet ^mr ^j`

.(awrnjxevlmipezpdx`yz`dxifgndivwpetdsqepa)divleniqd rvseqaavndly(zyrex)

.divleniqzvixlyzil`efie dbvdl

show 2 link arm simulation •

.l"pdzeivwpetayeniyl ze`nbe

arm usage example1/2 •

(onlwopqnyenina yeniyl)ziadlibxzjezn

get kf P and K •

open loop

^a^dxwa ^.1 ^wlg

aygpjk myl .dve`z menipinaexyiewa repirexfddvwy jkrexfdlydrepz xviildvxp dfwlga

.miwxtndlrievx dxwaze`leze`xinpeifhxwdagxnaievxdlelqndz`

`l iteqonfa)iteq mewine,

x (0) = [x ¹ (0), x ² (0), ] = x ⁰

îfhxwdâgxna îzlgzd ^mewinôezp ^.1

:d`adxignd ziivwpetz`xrfnny(ihilp` iehia)

x (t)

^lelqn ^`evnl^jilr^.

x (t f ) = x f

^(re
i

J = 1 2 t ² _f + 1

2 Z t

f

0 ||¨ x(t)|| ² dt

ew lr `ed lawznd lelqnd ik gipdl ozip .0 `id eteqaelelqnd zligza zexidndy dgpda

.xyi

agxnarexfddvwmewinnietind)

inverse kinematics

^-d^z`^zepzep^zeàd^zeèeynd îk^gked ^.2

: 1

(miwxtndzeieflifhxwd

zeaygzdjez

x

^e

y

^od^eizerlvy^zief^xyi^yleyna^xzil

x

^d^xivn^ziefd^z`^dxifgn

Atan(y, x) : R

²

→ [−π, π]

^divwpetd¹

tan(y/x) : R

²

→ [−

^π₂

,

^π₂

]

^l^ebipa^z`f^,

y

^e

x

^ly^mipniqa

(3)

θ 2 = Atan2(s ² , c 2 ) s 2 = sin(θ ² ) = ±

q 1 − c ² 2

c 2 = cos(θ 2 ) = x ² 1 + x ² 2 − l ² 1 − l ² 2

2l 1 l 2

(a)

θ ¹ = Atan2(x ² , x ¹ ) − Atan2(k ² , k ¹ ) k 1 = l 1 + l 2 cos(θ 2 )

k ² = l ² sin(θ ² )

oezpyk 1 dl`y z`miniiwny

t = [0 : 0.01 : t f ]

^mipnfa

x(t)

^ikxr^ly ^xehwe ⁽

Matlab

^a) ^xev ^.3

zixnepxefb .mini`zndzeiefd ixehwez`2dl`yzxfra ,`vn.

x f = [−0.5, 1]

^e

x 0 = [1, −1] ^T

ik oezp .zeizieif zeve`z lye zeiexidn ly xehwe zlawl miinrt zlaiwy zeiefd ixehwe z`

drepzdze`eeyn t"r(

open loop

^,
i
a ^onf)^dxwa^xehwe ^`vn

g = 0

^e

l 1 = l ² = m ¹ = m ² = 1

zevixhnmr)

arm noisy discrete control step

^a^ynzyd^.dxwad^ze`^z`^sxba^bvd^.zkxrnd^ly

dfz`eievxdlelqndz`sxbabvd.l"pddxwadze`mrrexfdlydlrtdzen li k(qt`yrx

?eppevxkle bdxwaze`yx perevialj`1dl`yzeyix z`miiwnylelqnyim`d.lawzdy

.avndly zeyrex zeitvz jezn i a onfadxwa .2 wlg

H

K +

η

+ v

+ -

H y

L

x

₀

+

u

-

+ x

ix`pildaexiwdxeardzpapydxwamr (zix`pil-i`d)zkxrnd

jildz yrxsqeezi okenk .(zebx nziivwpet) i adxwaze` i"rzkxrnd lydxwarvapdfwlga

.(izin`davnlziaihi `ztqezk)zitvzyrxe(ievxddxwadze`lzi`xw`ziaihi `ztqezzxeva)

certainty equivalence

^d^oexwra^xfrpe^d
i
a^zix`pil^zkxrnl^zkxrnd ^ly^divfix`pil^rvap^jk^jxevl

z`xtyl zpnlr .

steady state Kalman gain

^d ^zxfra âvn^jexrye î
a ônfa

LQR

^zxwa^oia^alyl

.ix`pilote`amewnazixnepdivxbhpi` i"rjxreynd avndz`m wp avndjexry

l 1 = l 2 = m 1 = m 2 = 1

^e

g = 10

^ik^egipd^3-8^zel`ya

(

˙x = f (x, u)

^dxevdn) ^zkxrnd^ly^dwinpi
d^ze`eeyn ^z`^aezk ^.1

zix`pilzkxrnzlawl

u 0 = [0, 0]

^,

x 0 = [ ^π 2 , 0, 0, 0] ^T

^d
ewpd^aiaq^zkxrnd^ly^divfix`pil^rva ^.2

.

˙x = Ax + Bu

(4)

x 0

^ikxr^z`e

l ¹ = l ² = m ¹ = m ² = 1

^e

g = 10

^mikxrd^z`^avd^,zepekp^jze`vezy^`
eel ^zpn^lr

:zeidlmixen`zevixhndikxr.l"pd

u 0

^e

A =







0 0 1 0

0 0 0 1

10 −10 0 0

−10 30 0 0







B =







0 0

1 −2

−2 5







zebx n ziivwpet `ed dxwad ze`y gpd :zn ewd dl`ya z`vny zix`pild zkxrnd xear .3

.

∆

^ol
eby

:dxevdnonfazihxwqi ,dlewyzkxrn`vn (`)

x(t + ∆) = Fx(t) + Gu(t)

?

poles

^d^okid

∆ = 0.1sec

^avd ^(a)

?daivizkxrnd m`d (b)

G

^ly ^zipnid^d
enrdy ^,xnelk ^.wtxnd ^lr ^zegek ^lirtdl ^zlekid ^z` ^ep
ai`y^zrk ^gipp

qt` `id

u 2 = 0

^y^dgpdae^,zn
ewd^dl`yay^zihxwi
d^zkxrnd^xear ^.4

z`xrfnn

u(t) = −L ^T x (t)

^y^jk^(hehxy ^d`x)

L

^,

gain

^xehwe^`vn ^(`)

∞

X

i=0

x ^T (i∆)Qx(i∆) + u ² (i∆)

Q =







1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1







zpnlr .

debugging

^jxevl ^`l` ⁽

dlqr

^oebk)

control toolbox

^d ^ly ^zeivwpeta ^ynzydl ^oi`

drbdl rdni`znddbiqpdzgqep z`ayglozipdni`znd

Riccati

^d^z`eeyn^z`^xeztl

(m ewzia libxzamzyx pyitk)

steady state

^l

?aeyndmrzkxrnd ly

poles

^d^md^dn ^(a)

?daivizkxrnd m`d (b)

zix`pild)zkxrndm`d .

y(t) = θ 1

^xnelk^,

θ 1

^z`^wx^ze`xl^epizlekia^ik^(
ala^df^sirqa)^gipp ^.5

?

observable

^`id^(zihxwqi
d

covariance

^zvixhn^mr

η

^ilnxep^yrx^,

u ^′ (t)

^dxwa^ze`^lkl^sqep^miwxtna^yrxn^d`vezk^ik^gipp ^.6

:y jk

σI

^zipeqkl`

u(t) = u(t) + η η ∼ N (0, σI)

lr ezrtydl ddfote`a zix`pil`ld zkxrnd lr drityn

u

^l ^iaihi
`d^yrxd ^zrtyd^ik ^gpd

xnelk.3 dl`yaz`vnyzix`pildzkxrnd

x(t + ∆) = Fx(t) + G(u(t) + η)

= Fx(t) + Gu(t) + w

w ∼ N (0, W)

?

W

^`ed ^dn

(5)

yjk

ǫI variance

^mr^ilnxep^zitvz^yrx ^mb^epyi^zn
ewd^dl`yay^jildzd ^yrxl^sqepa ^.7

y(t) = x(t) + v

v ∼ N (0, ǫI)

zyrexdzihxwqi dzix`pildzkxrndly(hehxya

K

⁾

steady state Kalman Gain

^d^z`^`vn

-peta ynzydl ozip jk jxevl .(lirl zitvz yrxe zn ewd dl`ya mz`vny jildz yrx mr)

z`lawli k.(dflibxzlsxevnoexztd)m ewzialibxzamzazkxy`

get kf P and K

^divw

:mi`admipezpdmr.(

500

^-d^onf^rva)

K(500)

^zlawl^zipkzd^z`^evixd

steady state

^d^zvixhn

σ = 0.1

^yk⁶^dl`ya^mz`vny ^itk^`ed^jildzd ^yrx

•

.(dlrnivg)

ǫ = 360 ^π

^,ef^dl`ya ^oezpy^itk^zitvzd^yrx

•

.(yrx`ll)

x 0 = [ ^π 2 , 0, 0, 0] ^T

^izlgzd^avn

• ||A|| ² _F = P

i,j A ² _i,j

⁾

i = {1, . . . 500}

^xear

||K(i) − K(i − 1)|| ² _F

^z` ^d`xny^hehxy ^bvd ^(`)

.qpkzdjildzdy ew a.(

Forbenius Norm

^d ^`id

?

K(500)

^`id^dn ^(a)

?aeh zeidlietvavndjexrym`d?mzlaiwy

P(500)

^zernyn^dn ^(b)

oelykl re`zeipy20jynl4dl`yndxwaze`mrzyrexdzkxrnd lyzeivleniq100uxd .8

:zih xphqddgqepaaivpavndlyxzeiwiie njexrylawlzpnlr.(

4π

ⁿ^dle
b^zeiefd^zg`)

¯

x t = ¯ x _t|t−1 + K ⁵⁰⁰ (y t − C¯ x _t|t−1 )

:z`

¯

x _t|t−1 = ¯ x _t−1|t−1 + Z ^t

t−1

f (x τ , u t−1 )dτ

zpnlrzix`pil`ldzkxrnd lydivleniqaynzyp,xnelk.

x ¯ _t|t−1 = Fx t−1 + Gu t−1

^mewna

zpn lr .zaxewnd zix`pild zkxrna ynzydl mewna (re i epi`y yrxd `ll)

x

^z` ^m
wl

`ll

arm noisy discrete control step

^divwpeta^ynzydl ôzip^yex
d îxnepdâeyigd ^z`^rval

.dreaw dxwa ziivwpet mr

two link arm control

^divweta ^e` ⁽

0

^qp`ix`ew ^zevixhn) ^miyrx

.`nbe ldvixx`zndsxbybd .elawzdydivleniqdipnflydnxbehqidbvd

1 0 10 g u =[ τ ,τ ] torque θ θ x =[ θ ,θ , θ , θ ] θ θ ˙ ˙ ˙ ˙ m m l l m m Maple Matlab symbolictoolbox • debugging Matlab controltoolboxes • •

•

debugging

Matlab

control toolboxes

•

Maple

Matlab

symbolic toolbox

•

m 1 m 2

l

l

x

x

g

m 2

m 1

l 2

l 1

x = [θ 1 , θ 2 , ˙ θ 1 , ˙ θ 2 ] T

˙θ 2

˙θ 1

θ 2

θ 1

g

u = [τ 1 , τ 2 ] T

torque

0

10

u = M (θ 1 , θ 2 )

 θ ¨ 1

θ ¨ 2



+ v(θ 1 , θ 2 , ˙ θ 1 , ˙ θ 2 ) + g(θ 1 , θ 2 )

 θ ¨ 1 θ ¨ 2



= M(θ) −1 (−u + v(θ) + g(θ))

M =

 l 2 2 m 2 + 2l 1 l 2 m 2 cos(θ 2 ) + l 1 2 (m 1 + m 2 ) l 2 2 m 2 + l 1 l 2 m 2 cos(θ 2 ) l 2 2 m 2 + l 1 l 2 m 2 cos(θ 2 ) l 2 2 m 2



v =

 −m 2 l 1 l 2 sin(θ 2 ) ˙θ 2 2 − 2m 2 l 1 l 2 sin(θ 2 ) ˙θ 1 ˙θ 2

m 2 l 1 l 2 sin(θ 2 ) ˙θ 2 1



g =

 m 2 l 2 g cos(θ 1 + θ 2 ) + (m 1 + m 2 )l 1 g cos(θ 1 ) m 2 l 2 g cos(θ 1 + θ 2 )



v

positive definite

M

g = 0

g

control project files.zip

Matlab

two link arm control •

Matlab

ode45

Runge Kutta

u(t, x)

arm noisy discrete control step •

η

u + η

show 2 link arm simulation •

arm usage example1/2 •

get kf P and K •

open loop

x (0) = [x 1 (0), x 2 (0), ] = x 0

x (t)

x (t f ) = x f

J = 1 2 t 2 f + 1

2 Z t

0

||¨ x(t)|| 2 dt

inverse kinematics

x

y

x

Atan(y, x) : R

→ [−π, π]

m ₁ m ₂

x = [θ 1 , θ 2 , ˙ θ 1 , ˙ θ 2 ] ^T

u = [τ 1 , τ 2 ] ^T

u = M (θ ¹ , θ 2 )

θ ¨ 1

+ v(θ ¹ , θ 2 , ˙ θ 1 , ˙ θ 2 ) + g(θ ¹ , θ 2 )

θ ¨ ¹ θ ¨ 2

= M(θ) ⁻¹ (−u + v(θ) + g(θ))

l ² 2 m 2 + 2l 1 l 2 m 2 cos(θ 2 ) + l 1 ² (m 1 + m 2 ) l 2 ² m 2 + l 1 l 2 m 2 cos(θ 2 ) l ² 2 m ² + l ¹ l ² m ² cos(θ ² ) l 2 ² m ²

−m 2 l 1 l 2 sin(θ 2 ) ˙θ ² 2 − 2m 2 l 1 l 2 sin(θ 2 ) ˙θ 1 ˙θ 2

m 2 l 1 l 2 sin(θ ² ) ˙θ ² 1

m 2 l 2 g cos(θ ¹ + θ ² ) + (m ¹ + m ² )l ¹ g cos(θ ¹ ) m 2 l 2 g cos(θ 1 + θ 2 )

x (0) = [x ¹ (0), x ² (0), ] = x ⁰

J = 1 2 t ² _f + 1

||¨ x(t)|| ² dt

θ 2 = Atan2(s ² , c 2 ) s 2 = sin(θ ² ) = ±

q 1 − c ² 2

c 2 = cos(θ 2 ) = x ² 1 + x ² 2 − l ² 1 − l ² 2

θ ¹ = Atan2(x ² , x ¹ ) − Atan2(k ² , k ¹ ) k 1 = l 1 + l 2 cos(θ 2 )

k ² = l ² sin(θ ² )

x 0 = [1, −1] ^T

l 1 = l ² = m ¹ = m ² = 1

x 0 = [ ^π 2 , 0, 0, 0] ^T

l ¹ = l ² = m ¹ = m ² = 1