Professional Documents
Culture Documents
"!#$&%')(+*-,.%'/
102!34%
56,87&%:9 56.%4!3;;&%<7=%
>?A@CBCDFE/GH?JILK >MHn N KPORQT[/SUSVWY?AXRWY@CXQtZ\sA?J[/SVQTOu]HOR?AQ@CB MP?A^ GRSUBvBC`V_UWY_@C@v[/Q WYX`a?J` MP[/b Q+WY?AXR]dSU{Yc\QTBC_ QTSUSUQ3SV?A[e]R?A}TSUQAKf~ WA`
YkE
gA+>
h'jAUi8kmrjAHl kmR> +l k eN#UJMfMporRq NY=sYQT]=XRQ3/ORX2HJfHWY]R}Tg Q+?AXPMPzzAqMwo+P orxYE/qzyAQTRSUqA{AR¡z¢Jc£fWY]z=`VWY+PBCMP_UJ|Q MP=| SVR?A]R}TL=QA~
†? † † ‡
†
‡
HYYPJJfYmL=
¼r¯½¤zÃ+´¥¯Èº+¦¨³§U°²ÄV©ªÂTɾ±
¥«±U¼¯¾ºT¬Âµ»+Ã+t´mÇr®+· ´²¯p4¶ Å+°²¯½±U±Ãr¯²³p®aÊR³²µËAÂT´³²'ÄU¶¯½°¸±
°²· ºT±µÅA¹®µÆ¿µ®Tºt±UºTÇr´»+¯½Ìµ±U¯ª±V´®+¼r¿V°²´V±ºt³±V¼µ»+ºt¶Í´L¯½®+¯£º+¯®+¶H¼J¯ªºTÎ+»r»+»T®T´m±Ï±¾»r· '®T®TºT±
´»Tºt°¸¿aÇ· ±Vµ»+·¯¯È¯½±À³±
±Uµ¯Å+¼T¯½±± Á ý+þ+ÿ
(2 , U 1
H , 1
&'
õ ýTõ þ þ
ų+Ѹ±mÌT·Ð¶ ±UÆ ºt¯ªÇr¯¯½»r±U¶´¿U¿UtV®a³³±V±U»+¯£´wÔ ¼r´ºt¼Jºt¯½ÑÀ¯½ºT®a³ªa¯¯»+´Ñ±U¯wÌt´Òµ°²±£t»TÓ
Å+±V»TÓ
¯È³L´VÁ
H Ls s sd(:
V - 1
WJ
%
)(q C2 O1 T W
õ ÿ ÿ =ýýTTþ+þ+ÿ ÿ
&
+
ý +
þ ÿ ÿ þ
êVë¾êLìzízîAïðñòJízóôïì
ż(t) = Aq z(t) + Bq u(t) ¯
¯
8; °
<°
Lõ ö÷P÷føpùmúwûüýTþ+ÿ ÿ
y(t) = Cq z(t)
Fj°
þ
! "
$# % q(t) = qi si ϕ(z) ∈ χi i = 1, ..., N ¯
&'! ( )" *
ý ýõ ÿ þ ý õ ÿ
+, -
. , / 0%
±
(∈R
1 z(t)
n×1
(
(
q ∈ {q1 , ..., qN }
þ
U
õ õ
k
&
õ
ÿ
1
2 334
.5 6879(:5;8<879(,5 ;=>79(:5 ;86879(:5 ;?79( 5 ;>@79( u(t) ∈ Rlu ×1 y(t) ∈ Rly ×1
&
N
0%
ý ÿ þ ýTþrÿ
5 ;A>79&BC D D
D
E4 Aq ∈ Rn×n , Bq ∈ Rn×lu , Cq ∈SRly ×n χi
ÿ
n ( 2 (
5 F>79(5 =>79(5 ;G>79(5 ;F>7
2 1
H )& χ χ= χi
I %
1
ú ý+þ+ÿ ÿ ý
i=1,...,N
õ
" J : + K L % 4 ?4°
¯
&M'3
1N
χi ∩ χj = ∆χij ∀i 6= j
ÿ
N "O
>PQ 3 %
ý+þ þ+ÿ þ ü õ ÿ õ þ
PQ
& +R: ( K S+ HT U2 + 1
V IW H W 0% 6°
½
∆χij = ∅
)|i−
j| > 1 ¯
1
! 2 , YX
∆χij = ∆χji 6= ∅
&$Z SD
I
H-[]\^>_`_QabdcEe4fgI
ÿ õ
h ae:i>&:' ( C ,j "O 1
J
ý
ÿ õÿ
k, dC "H
3l
∆χ ¸ · ¸ + ¶
ýÿ
W V CJj C >% χ´ +³ µ
i) m (>, Y n 1
k T k 2 k
õÿ
&>'Y $ o 1 !, d" dn
þõ χ² ∆χ ¾ »)
−
¼½ »
ý
kJj k n
1
n
I;& χ º −¹
+
ý þ õÿ
pd : 1
U
1
1
J
# >%
ii) m&
ú +
ý þ ý
1
C
q
I
n
¿ z 8
jÀd
3Á*~uQÂu0n8{ uYz { { 8 v`w`x1v`z 8u *
8 E u0~ u0Ã8{ u
úý
JI # T Vr,& χi
O Jj
L &k'
þ ý+þ+õ ÿ
T LW IW )&,Z
2
õ ÿÿ ¾ý+þ+ÿ
Ds sd(Y W &Y' " ϕ : Rn → χ
ÿ
" ) T ** "3
2
M
% jF °Y
?4°$ 2 2 1
;8°
1
&kBC
Jj ,¯ X
¯1
"
2 ¯
i i + 1
&
ý õÿ ù
k
T !Jj ! % ϕ(z) ∈ ∆χi,i+1
õ þ ýTþ+ÿ
Ä Å
1 # ème I )&
õ ÿ
( j2 n T W
Z)p : qi 1
kÆ O#/
?
âUà²áÍÐÝÞÈå
t:u$v`w`x0ydx]z {x|Qv`|Y}>~>x]~80|q>x]w*{ x)|0dz d~Ut:u0~v`wud
C! 0z { { x1v`z d~ w`x]8z 8u v`w 8w98u 8u*{ x ` w x]0u*8u*0dk
Ý µÛ
J2 N :Æl
q = qi
(Y#
I (k 2 +
þ ýTþ+ÿ
þ
I
)
( N
1
%
v9x1v`z d~jdndz ~ dwv, u){ uY d ~8u*z ~}>~8z v`|)8u)0dn 9v x1v`z d~ >x]~
1
1)ème
þ ÿ ú
~z ~v`u0wydx]{ { uq8u$v`u0n }>~8z
I V 1
&
qi−1 qi+1
þ +ý þ+ÿ õ
2 + 1
V
1
&
U
Ç * I 1.2
õÿ pý+þ+ÿ ý þ õ
Z X ,W
2 > % 1
ÿ
32 ;8°d(:± 2 I 0%9
0.8
T U ¯
0.6
0.4
x2
0.2
=j° −0.2
−0.6
õÿ ÿ
± !
*Jj * ) % −0.8
−1.5 −1 −0.5 0 0.5 1 1.5
L&,qZ*(q
= 1, ...,2 N
)
N
þ õ
x1
3 e = z − ẑ ¿ z 8
)í
ÈîxI0 wÃuWu0~+0d~v`z ~ uWwu08w| u0~v`uV{ xWv`w`x]ïu0Qv`dz wuVjð |Qv9x1v3
1 ñ v`0nuÀY{ xQ wÃ>uu0~dz ~v`z { { |0uYwu08w| u0~v`u0u0{ { uY ñ v`0nuYí
(A1 − L&& 1 C1 )e(t)
ϕ(z) ∈ χ1
@°
ė(t) = & ¯
1
ÿ ý
Y I
q C2 C 2 >%
x2
Lq
þ
2 , & 0
,Aq−
L(qCI " U ( U q%
ú õ
q
õÿ ÿ
−0.5
N o 1 C, 1 1
þ
2 3J 3 @°d& −1.5 −1 −0.5 0
õõ þ ý+þ+ÿ ÿ
x1
V I ¿ z 8
>ò
ów`x]ïu0Qv`dz wuq ñ v`0nuY ñ Ã8wz 8u
. T L IW È L
W4 É
þ þ ÿ þ õ ý+ÿ +þ ÿ
1
. (C %Q#>%Q (n2 È I
ª i>e_ ª ^>_`c ªô ^>cE_Qa ª ë ^>å i>b ª å ä i ©dª abd«>^>_Qa¬bp\4 © bdc®aöõ ÷1øö^>«>a][Èå a ª
ÿÿ
N 1
I V: J + 1
ÿþ þ
1
& V :k
(
2 ), 3k: m %Q @° n
õõ þ
I
WH[]\^>_`_QabdcEe4f( " W C ¯ ;8<°
ýý
Lq = P −1 Xq , q = 1, ..., N ¯
I
& ^ ª]ª ¬b1aCå ä ^ ª >çê,_Qi>_`cì¬a ª _Q^ © cEå cE_Qè3®aå ä abdb a¬b3®)ä i ©dª abd«>^>_`ci>eÆõ]ù0ø
þ
ÊË)ÌÍÆÎ$ÏÐÑ,Ò)Ó C U 1
( ì¬aå ì¬a ª i>cE_å ^NçN^>ecúb1aV®aI[1i>çVçV¬_Q^>_`ci>e §
ė(t) = Aq(t) e(t)
õþ ÿ ÿ þ ý+þ+ÿ õ
û Ý8àü*ýàâZ*p 1
1
1
õ
· ¸ · ¸
Aj° @°d() 1
I " T d%
LVZ I¯ W: V 0%
0 −1 0 1
þ õ +ý þ+ÿ ÿ õ þ
A1 = , A2 = ¯
1
1
(42 M ,j 3 1
1 −2 −1 −2
1
õ þõ ÿ
@°d& Ó ,j W2 "
T +Z IW ¯ (
1 V (e) := eT P e
q(0) = 1
− ;Fj°
° (Aq − Lq Cq )T P + P (Aq − Lq Cq ) < 0, q = 1, ..., N ¯
½
q(t) =
1
σ − = 2 ∧ t > δk ¯Ô
2 σ = 1 ∧ t > δk 1 T (
2 ;Fj°W,
, " ¯ ( q ∈ Q
ÿ
& Ó
± −
O (q P, Lq q = 1, ..., N
ý ÿ õ
n J (
2 ;Fj°):
,q IW q &8δBk ! 1
ÿ ý
k Xq = P L q ¯
k
õ
+ : q = 1, ..., N
δk
;?4°
ATq P + P Aq − Xq Cq − CqT XqT < 0, q = 1, ..., N ¯
þ
δ0 = 1 O
2 T þ 1
p
;Gj° N
ÿ
2(k + 1) Z*ÿ.s0n &
δk = δk−1 + ¯ P, X1 , ..., XN
k(k + 2) ' D 0%Q (k H ,j - %
ý õ ý+þ ý ý ý
Z* Ç J Õ< PQ Ö× "Ø 0% L,
V
I
IV J(,
þ þ þ ý+þ+ÿ ÿ õ ý ý õ
1
( 1
T I
%
L;K 2 K1 ,
K+ J
2 W
O
I
IW &
JK IX Kr, Ç J NFj°d( J
2
ý þ
"Æ 0%Q 1
U ¯ 2 &!BC § i>çêi>bd_QaçNae_þ®)ä \4 ª _9èb è ª c ª ®a å ä i ©dª abd«>^>_Qa¬b×\4 © bdc®a
ý þ ÿ
,j *
2 ! !
% çNi®>c éè
ö õÿ ý ÿ
I5 ?79(*2 Æ o 1 I, Ç K3 # 1
ÿ þ þ Tý þ+ÿ ý
1
2 U 1
@°d&
V H
I
++ JL # T Or,(
ÙÚ)Û8ÜÝ8ÞßÆà¤á,âãkä cEåYa1æc ª _QaW¬e:açN^>_`bdc[]aN®è`éYecaêi ª cE_`cE«>a ¯
õÿ ý
k Ç ,j
&jZ)3 J
P ∈
õ
a_k®a ª çN^>_`bdc[1a ª n×ly ë ë _Qaå!ì¬aå a ª W
2 W+ I V
Rn×n
ü ÿ
X ∈ R q = 1, ..., N 0%9 # . &
cEe:a`fj^>å cE_Qè]a ª çN^>_`bdc[cEå aåå a ª
q
õ õÿ
N OR: ( C W J . >%
&)Z E
õÿ
;;8° i K
$
ATq P + P Aq − Xq Cq − CqT XqT < 0, q = 1, ..., N ¯ i i
q
∆χ
+ +
q= i + 2
∆χ
+
Ei −1 Ei Ei +1 q=i+ 1
∆χ
−
χ + ∆χ + +
q=i
q=i-1 q=i q=i+ 1 χ ∆χ
+
q=i-1
Ei Ei +1 Ei + 2
χ
− ∆χ
− χ
x Ã
¿ z 8
8
x Âu0n8{ uq8uqv`wdz n8u 8u{ð dà u0wydx1v`u wY ñ Ã8wz 8ud
à { xn x]w`x]Qv`|0w`z v`z uk ñ v`|0w| z 8u{ð dà u0wydx1v`u w04uQv${ xkv`w`x]ïu0Qv`dz wuk8u{ð |Qv9x1v
8z 0wuQv
6° Ei = 1
( J
J
U
¯
Ç J êêêVë.- ì 0/£ó21ñ 3ð / ñ=ì ì=ó54rñ76í +3íz/ óôï#ì îAï Yï=ízó21ñ 3ð
3 M¢ ) ¤d$ M *
C¢ ¢
) *
þ õ þ ÿ
∗
Z*p 2 É 1
2 O-4
i = 1] ∧ [ϕ ∈ χi ]
ÿ
Ei = 1
[E O Ç J L=j°N L
¯ N
ÿ þ
Ei = 0 p /H 2
£
∗−
Ei∗ = 1
¤
õÿ þ ÿ
£(ϕ ∈ χi − ∆χi,i+1 ) ∧ (Ei−1
∗−
= 1)¤ ∨ PQ
&:Z* 2 W ème
(ϕ ∈ χi − ∆χi−1,i ) ∧ (Ei+1 = 1) i
∗
Ei = 0
;86°
4 ¯
;>@°
õ
± ∗ U - J " V U Mi (Θi )Θ̈i + Vi (Θi , Θ̇i )Θ̇i + Gi (Θi ) + JiT (Θi )Fi = τi ¯
Ei8; 6°d& ∗ N
I
þ
I
+ 1
õ
¯ ECi 11JU ±W, ( $ k %
ý+þ+ÿ
&,Z)W J ∗− i Mi ∈ Rp×p
i . 1
Ei E4 ( p×p
þ Tý þrÿ
+ J ∗ & Ó
V i∈ R T J(
ú
Ei C UJ 1
(
9 8
Gi ∈ Rp×1
ÿ ý ÿ
V
( V H H
& "$% # " J
1
;: s&
Ji ∈ Rs×p
ÿ õ
∆χ
s×1 k
'
+
!! '( ( p×1 C C C &
∆χ q=i χ+
& "" % # i
Z*p τi∈ R2 OJ O 1
L 1
õ ÿ þ ÿú ÿ þ ÿ þ þ
+
χ
+ ∆χ +
χ PQ CN q q 2 q >%
& 2 3
3 5 ;?7 k
¿ z 8
*)8
î,ð ñ v`|0w| z uQv*{ u ) 8 Eu0~ u0Ã8{ u 8uY0dn v9x1v`z d~ ;Aj°
Mr (Θ)Θ̈ + Vr (Θ, Θ̇)Θ̇ + Gr (Θ) + JrT (Θ)Fr = τr ¯
±
õ
N - " (
I I# q¤ qi+1
õÿ
i
,%
L . W (:2 $
, qiHl
M1 && 0 V1 && 0
V
( T ∆t ; °
õ õ ÿ
Vn IW4 Mr =
& ; Vr =
& ¯ Ô
n 0%9 &
ϕ
T pJ
Æ ∆χ 0 Mk 0 Vk
õÿ 'ý+þ+ÿ
Z* i,i+1
Ç
2
C, Wi ©dª abd«>^>_Qa¬b3\4 © bdc®a,+I\4 ª _Qèb1è ª c ª 4
J1T
G& 1 && 0 F& 1
&& T & &&
Gr = ; Jr = ; Fr =
N
ẑ˙ = JkT
P
[(Ai − Li Ci )ẑ + Bi u + Li y]Ei Gk 0 Fk
;=j° <Gj°
i=1
N ¯ ¯
τ& 1 Θ& 1
P
ŷ = Ci ẑEi
& &
<;8°
τr = & ; Θ = &
÷ý+þ õÿ
" (i=1
# ¯
1
K I
1
Ç J L? j°d(n 4% τk Θk
.# ¯
þ ýTþ+ÿ õ
,j I2 . 1
T £ ¤&
t
õ ÿ
( H
(M ( Z* Θ i =
1θ i1 . PQ C#C
, . . . , θip
q(t) = i
ÿ
i
H2 K , 1 Ei = 1
i
C 4
V
õÿ
4
ÿ
q = i−1 q = i+1
ÿ
J "
, Ç %
J 3? E° i−1 Ei+1 ¯
ö ÿ õ ÿ zý+þ+ÿ þ
°d& <<°
¯
*
* ( ! ) Mo (X)Ẍ + Vo (X, Ẋ)Ẋ + Go (X) − G(X)Fr = 0 ¯
þ ÿ
H
I 2 V 1
± s×1 &qBC I2 V: 1 V" V
1
)2 1 %
1 )& X ∈nR
k < <°d&
¯
AB AB
ý+þ+ÿ
(θ , τ )
U IX
2
þ
2 H
;8°d(!± W O Ç J - 1
H
(θ A A , τ A A ) ¯ + ϕ(z)
U
AB AB
(θ , τ ) ?@ ?@
(θ , τ ) CC CC
(θ , τ )
(θ ? @ , τ ? @ ) < °
(θ A A ,τ A A ) => =>
(θ , τ )
BA BA
(θ , τ ) y
(θ > = , τ > = )
(θ = > , τ = > ) êONÏë.PH43436Tð óôMò T+ízî óôïeì ð ×Uð6TóñRQSîtí / ì=ì&ó54òYñ7ï6 +ï0í 4 ñ=î î +ízóô&ïì ð ñRQ
*
¢ p£ *
M¥ * E,
ϕ(z) = ϕ(Θ) = Θ
C¢
M¥
¢ M )W£ *
¥ :¢
¯ Ô
(θ C = , τ C = )
x
(θ > = , τ > = ) (θ < < ,τ < < )
y z
x << <<
(θ ,τ )
Paramètres
(a) (b )
l4 l2
¿ z 8
$D
Âu0n8{ u 8u ñ v`0nu 8u!Cx]~8z { x1v`z d~ x , x 8{ x]~>x]z wud à θ 22 θ12 m = 2 kg
x Fò E l = 0.6 m B ra s i
lc 4
d1
lc 2
V
l = 0.3 m (i = 1 , …, 4 )
ý
,
K I5 687
ý+þ þ
N q# Y 2 (
l3
θ 21
lc 3 lc1
θ11
l1
W
d = 0. 2 m
X
d = 0.5 m
d2
m = 0.5 kg Y
<Fj°
· ¸
Θ̇
−GT ¯ ¿ z 8
Z
[E!u ÂCx]~8z { x1v`u w u0~0d|0w`x1v9z d~3uQv!{ u w >x]w`x]nQv`wu
£ ¤
Jr =0
Ẋ
HG8j IG GW W
ÿ
±
õÿ þ
Z
1
2
# + 1
U >%
JPQr G>( G : &
õÿ'U2 q () U 3 2 %
õÿ õÿ
L ,j
I " C # "O%
ý ÿ ÿ þ
J
1
( k K PQ &Z)3 1
þ ÿ ýTþ
,j
# Æ 1
2 N )&)Z 8%
ÿ
1
Ç J L@4&Z)
V , 1
! I
1
%
I V3Jj 3
1 U
ý
K
I;& FGj°
ΘT = [θ11 , θ12 , θ21 , θ22 ] ¯
¦§KJ i®úå a\4 © bdc®aN®>¬ ª ª _QúçNaV®açN^>ec ê,¬å ^>_`ci>e
þ þ
Z*V 2 J
U3 1
3 ;Aj°d(
¯ (
1
! CML # ( (
õ ÿ ýTþ+ÿ ý+þ+ÿ þ ýTþ ÿ õ
Z #
\8j
Mr (Θ) ∈ R4×4 V (Θ, Θ̇) ∈ R4×4 Gr (Θ) ∈ R4×1
O ]
O )&'N
K k
ÿ
C J r ∈
RT
4×4 &)Z)3
UN I
&,Z) d%
ÿ ú þ ý+þ+ÿ +ý þ+ÿ ÿ
U W % T
2 &
T k τr = [τ 11 , τ12 , τ21 , τ22 ]
õ
+ W % T
1 D 1
K
KI J
Fr
F;8°
ý ÿ
, ( q
FrT = [F1x F1y F2x F2y ] ¯
N pi i ∈ {1, .., N }
õ
U &
ÿ
'
1
H PQ J
õ
Z)
W4 <<° ;Aj°n + J O, 1 J Ç J 3@°d&BC q %
5 ;?7 ¯ ¯
m PQ o R: 1 + d 1 ¯ M&$Z 2
õ ÿõ ÿ õ õ
pq ,j V2
þ
2 U PQ : k U Ç #
<>?4°
ż = Aq z + Bq τr + fq ¯
Fj<°
± Mo (X)Ẍ + Go (X) − G(X)Fr = 0 ¯
¤T <6° BC
1%
2
¯
£
z= ΘT XT Θ̇T Ẋ T
÷ þ
, T kU & U 1
3 %
)(M U 3 "L: N U "
0
õÿ õ
<=j° M * PQ &Z) ) %
· ¸
0 I 0
Aq = ; Bq = ¯
1 , C T
−Wk −Wb p Mr−1
q
0 p q
FFj°
K = diag(K1 , K2 ) = diag(k1 Id , k2 Id ) ¯
F?4°
¯
¸−1 ·
JrT B = diag(B1 , B2 ) = diag(b1 Id , b2 Id )
· ¸
Mr 0
−GT
£ ¤
Wk = K Jr
0 Mo −G ± 2×2
3
V
1
&
3
·
Mr 0
¸−1 ·
JrT
¸ I dI∈
Rk +
1
2 & k1 , k2 , b1 , b2
õ ý
−GT Z K + 2 4
£ ¤
Wb = B Jr
0 Mo −G
ÿý
6
J ( 2
J T JU π &
BC Ç (χ J i ) nW 2 6 rad
þý
)(
<j@°
ÿ
T
¯ V Ç J &!Z)+ 1
+ %
£ ¤
fq = −Aq zpi − Bq Jr Frpi
.:j1
25
pi
O W ) 1
(MW2
<Aj°
ÿ
IN Ç J Ç I# Ç J Ç J
¤T
zpi = ΘTpi XpTi 0 ¯
£
0
16
1
¯
õ þ õ ý+þ+ÿ
* ) 1 A j° °d&YZM
K B
Ç J ¯ UA
ÿ
1%9 2 N4 T &*Z)H 1
°d&
¯
Initializatio n
]^
^ ]
] `] ]_ ]b ]b ]_ ]b ]a ]b ]b ]a r
] `
r q r q
r
^ _^ `^ ` `b ]_ ] ]_ ]` ]_ ]` ]a ]a
r r r r
q
^ _ _ ^_ `a a b b b c b c ] ] ] ] ^ ] _] ] _] `] `] ^ ] ` ] a ] `
`
q q r r q
r r r r
r r r r
r
a _ a db aa b cc d c c] ] ce ]] ^ ]] ^e ] ^ ] ^ ] `
r q r r q q
q
r r r r
r r r r
q q r
a d d d d f c ] ef c ] e ] e ] e ] ^
r q
r r r
r
r r
r q r
q
fd f ]ef
r r
r q r
x Ã
¿ z 8
g
x î4u dz ~v 8u){ z ~8| x]wz 1x v`z d~ u0{ u0Qv`z d~8~8| uQv:{ u w 0 d~}> w`x1v`z d~ x ` 0z |0u { uM~8dÃ8wu)8u)0d~}> w`x1v9z d~ : )
u w|0 z vRhÀiD$0d~}> w`x]v`z d~
u0h${ð u >x]0u*8uMv`w`x0ydx]z {z ~88z | ` w{ x*}> wud
à î,ð x v`dC x1v`u!x ` Qz |*x n80{ uM ñ Ã8wz 8u* kñ v`0nu)8u)Cx]~8z { x1v`z d~j rij u v,{ x$0d~88z v`z d~
8u$v`w`x]~ z v`z d~3 n8u i yu0w { u$n 8u j
ý+þ+ÿ þ
Z)
W 1
V O
Z)
N L /
]
Ç J
õÿ
>< ?4°d(: I
2 I (
¯ ( I
3
A3q
∈ R14×14 , Bq ∈ R14×1
(
± Ô e = (θ11 − θ̂11 )
( 1 &
fq ∈ R 14×1
y = Cz e2 = (θ12 − θ̂12 ) e13 = (θ21 − θ̂21 ) e4 = (θ22 − θ̂22 )
ù ÿ õ õÿ
Ö2 ö ¤
1 D
Fj6° >" 4
C = [ Id(7×7) 0(7×7) ] (7 × 14) ¯ 2.10−3 rad
°d&Z* Ç J 1 -;G( I 2.61rad
õ õ
I >%
þ
K 1
&q' È 1
+ >%
Id ¯ n0.076%
n>" &Z* Ç J ;;
p N " ( O Ç J
1 !J x !y k 2 1 )&>Z*
þ
1
U +# K
(, CX
Ç J ;8< J I
ý õÿ
ÿ õ
T W ) W j &
ý+þ+ÿ ý+þ
U# 1
&
'CU T + IW K
3
F=j°
ϕ(z) = ϕ(Θ1 ) = Θ1 ⇒ ϕ(Θ) ¯ 5
x 10
−3
Z)× I
1 2 4
Erreur
Erreur
d’observation,
d’observation,
robot
robot
1,
1,
sortie
sortie
1
2
k>=%
&Z)K Erreur
Erreur
d’observation,
d’observation,
robot
robot
2,
2,
sortie
sortie
1
2
diag(200, 200, 100, 100) B = diag(50, 50, 25, 25)
õ ý+þ
3
I
1
2 ,# ◦ °d&!Z)∆χ UJj=
constatnte = 0.0873 rad
õÿ \ýTþrÿ
erreurs d’observation [rad]
E4 2
¯
5
;8<°d()± ;;8°d( 1
1 ¯ Xq P ¯ 0
F4@° −1
· ¸
P1 P2
P := ∈ R14×14 ¯
P2T P3 −2
−3
−4
78.12 −4.60 0.01 −0.34 1.45 −1.02 −2.25
84.75 1.25 0.67 0.76
−4.60 −0.52 −0.96 −5
0.01 77.98 0.42 0.08 0 5 10 15 20
−0.52 −2.49 −0.74
P1 = 1.25 79.72 0.00 Temps [sec]
−0.34 −2.49 −0.36 −0.67
1.45 0.67 73.86 0.05 1.12
−0.74 −0.36
¿ z 8
kj
0.76 0.42 0.00 0.05 74.75
wwu w jð u v`z Cx1v`z d~8u x1v`wu dwv`z u x]wv`z { x]z wu
−1.02 −1.43
−2.25 −0.96 0.08 −0.67 1.12 −1.43 80.19
0.03
67.55 18.38 −0.66 1.57 0.10 0.28 −1.17
18.38 37.37 2.42 −5.16 0.17 −1.51 −1.35
2.42 72.48 7.43 0.02
−0.66 −0.35 −0.17 −0.32
Erreurs d’observation sur x et y [m]
þõÿ
Z
1
W
Y
Y q Y
−1.17 −1.35 −0.32 −0.07 −4.17
þ ý ÿ õ ÿ
1
O È #-
. I K
%Q
. I
1 −0.01
8ÿ ÿ
J8°d& Ó ¤ 1 L- −0.02
õÿ
2 q3, qC W ,j
q
ÿ
&4Z* PQ C
T
−0.03
õÿ
J 1
PQ
−0.04
x, y 0 2 4 6 8 10
Temps [s]
12 14 16 18 20
2
v2 vitesse angulaire v4, robot 2, articulation 2. JiT (θ)(2, 1) = −l2 sin(θi1 + θi2 )
1 v1 JiT (θ)(2, 2) = l2 cos(θi1 + θi2 )
−1
q¥3r4¥ î ìò
s ÀutwvV
x
zy:w`x]~8z |{ ñ nv { v`z 8{ ukî ñ x] ~81yW ~8Qv`z d~ x]~8I]v`8u0wkx]~>x]{ ñ z
M ,,
v`d{ dw ~} z v`98u0x]~8I ñ Ã8wz ñ v`u0
z nn ów`x]~
3 v`dCx1v`z
v3
s íOtwEt:d
$~y:v` wd {v9kx1v ò*
$9y:8u0j~>
kx]{zZ;x])1~85 gd8í*
k !$
8yMwz x]{wiÀÃjF]jFv gI
u!à v:v`8u à u0wydx]Ã8z { z v ñ
−2 v4
} ñ
s òOt[]xM
dt:>z u0x]0z u ÃjOz Eu
;y:E ~>v9x]x1nv ; z x]O
{zy:x u0~>x]v`u0{ z ; 8O
]k;
*yMdx]{ 0w Ã]>v íF* lF
l; 8Ã
u0wydx]Ã8z { z v ñ ]8v`8u
−3
2 4 6 8 10 12 14 16 18
ñ
E!z 0wuQv`uxv9x1v`u: dMw E ñ ~>x]nz x]${ z u00u } z 7u ñ Ã8wz x ñ v`u0
]F d w~>x]{
Temps [s]
¿ z 8
jÀdÀd
`v z Cx1v`z d~8u yz v`u ` u x]~8 { x]z wu ]*d~8{ z ~8u x]w3!~>x]{ ñ z M ñ Ã8wz x ñ v`u0 x]~8!88{ z x1v`z d~ O,d{
FDdò
s ;t[*x
d8t:
>>òx]>z 8ÃjjO *
EO
ídy:ò15 Fò v9gx1v ; Fí lFlF;
)y:
u0~>x]{ zO 8O
O
yMx]wÃ]v *
à u0wyu0Mw E!u z d~Y dw
xUt:{ x ` ]x } z v`98u0x ñ v`u0 t:dnnd~î ñ x] ~8 y ¿> ~8Qv`z d~\!
18
8wx]9I
]$ d w~>x]{] *d~8{ z ~8u x]M w !~>x]{ ñ z ) ñ Ã8wz x ñ v`u0 x]~8 !
17
8{ z x1v`z d~ nz >x]~ { u x]8wunjð ~8uz ~yz v9x1v`z dH ~ hE~v`u0w~>x]v`z d~>x]{
16
t:d~ u0wu0~80u], ñ Ã8wz 8u ñ v`u0 x]~8K!88{ z x1v`z d~ ó)8uÁ*~8z yu0w z v ñ
The discete state q
15
s )Ot
4t:d{ udI I
` 4x]~8\ x
xz
Á xxx v`* w vñ R x
E ídñ í1~>x]Fí nDz Fí lFQlFD~
v`wd{,d { z 8z ~8
]{ z z x]~>x8îx1x ñ uQvv`udIî >
14
ñ
à ñ wdÃ]vk>x]~8 dwwu0dw`x 8z ~8k
]n nn ów`xd~ xd9v`z d~N~N)dÃ]v`z
13
12
s DOtw8*
*v`du C >x1x]v`~8z d>~jxk yEd
{
î4gz Ãk u0*~w 80d
j~jÀdk >8j*
*
x*
í1v32 )ddíw ud* u0y!ÃW*
iÀ EjFjd
í
$
!~88u0w d~j8ó)8]
11
0 2 4 6 8 10
Time [s]
12 14 16 18 20 ¿
¿ z 8
jÀ í
î,ð |Qv9x1v!8z 0wuQv
Cx x
y:wz ~ nu x]j ¿ w`x]~*{ ñ 8uy:w ñ ~8ud3
v { v`z 8{ uYn8u0{4x]>x]v`z yu
0d~v`wd{$>x]wvWí x } z v`98z ~8k
]
E~v
8
*]C)dà 8 vWx]~8U *d~8{ z ~8u x]w
Ïë ïìòn6Tñ ¸óôïì
q
ÿ õÿ ì=ðõ ópQ õÿ
1 K N o 1
,j
& ñ v`u0 C xî4z uQx]{ du0Ã8w`x]z C0d~88z v`z dI~
]n x ñ v`u0 t:d~v`wd{)î4uQvv z ,d{
ò Z>8j
ÀdOÀ Z 9À ídí4iÀ jFjFj
PH434 s À íOtóY
8
dóx]w~j
y:uï20 ñ x]~8
; ~jk
E!u z d~k] ñ ~>x]nz M0d~v`wd{
])v } 30du0w`x]v`z ~8wdÃ]vkx]w 0{ u0V9>x]z ~V dw { x1v`z dI~
]z wd
nn E~v
$t:d~
))dÃ]v`z x]~8o v`dCx1v`z d~j)*x]{ u0z jM8j
Z 9À ò
úfý ÿ
2 ,
j1
C "IJ
Y 1
(
À jFgZ
i
s À òOt
0x
7v3dw ud
ix u0wyz dw ñ t:d~v`wd{q] ¿ x]nz { z u ]î4z ~8u x]w[xuQv
ý
I
kW 2
(i = 1, 2)
dz ~v* t:d~v`wd{ { u0w M x]wvYÀ $ Â8x]Q0v vUx1v`9 8z ~8k
]$ nn ów`x]~ x]Qv`z d~
s À;t~Y
vV
v`vdC x1ww`v`xz ñ t:d
~î4v`wzdu{>x8
xj
*
4xÀ x 8 À v`ò1w 9ñ À I
uò+Àd Cx1Qv`v8iÀu0CjFjFx1Dv`
z x]{z ~v`w Qv`z d~
dw
s iÀ )Otwv`YEwI
dÃw`]x1v`vz *v`z C98x]z ~80z 8 {x]x1~8v`. z dI~
]
y:y:z 0 9xY8z*n x1
v`E dñ ~j~> x]n{ dz w z >!x~>x]tM{ ñ) \t z ]wR u v38dÃ8iÀ jFz { j;z v 8ñ
Mi (θ)(1, 1) = 2
m1 lc1 + Iz1 + m2 (l12 + lc22 ¿ `
+2l1 lc2 cos(θi2 )) + Iz2 x]~8w!w`x >x]Ã8z { z v ñ ]3!u0~8u0w`x]{IvUx]~8z { x1v`z d~x ñ v`u0
] nn ów`x]~
2 x]Qv`z d~.d~.)dÃ]v`z Wx]~8o v`dCx1v`z d~j ,d{
iÀ 8R *>
íV8j
*Fí )À Fí )Fg
À jFjFg
i
Mi (θ)(1, 2) = m2 (lc2 + l1 lc2 cos(θi2 )) + Iz2
Mi (θ)(2, 1) 2
= m2 (lc2 + l1 lc2 cos(θi2 )) + Iz2 s ÀiDOtóY
, 8z {dx } xqx]~8
*>8u0~88I
t:dw8z ~>x1v`u0 ñ ~>x]nz ñ Ã8wz C]
z v`z d~ dw0u$0d~v`wd{ dw: { v`z 8{ u*wdÃ]v)Cx]~8z { x1v`dw >x]~88{ z ~8d~8u
0d~ v`w`x]z ~8u0dÃ8ïu0QOv
]* wd$
nn t:d~
d~)dÃ]v`z x]~8 v`dCx1
2
Mi (θ)(2, 2) = m2 lc2 + Iz2
v`z d~j8yd{
>í88j
4ÀdOÀ Z;g19ÀdiÀ gdòiÀ jFjFl
s ÀOZitwvV
qÁ*98z ñ x]Cx
} x x } xOx]~8
!x;{ddndwz
ñ Ã8wz Æ z
v`z d~ dw0uW0d~v`w{Y dw0dw8z ~>x1v`z d~.]kxVv } ]x]w×wdÃ]Ov
] wd
nn E~v
*t:d~
:)dÃ]v`z x]~89
Vi (θ, θ̇)(1, 1) = −2m2 l1 lc2 sin(θi2 )θ̇i2
v`dCx1v`z d~jM*x]{ u0z dj,8j
!À ;í í1
s ÀigOtwÀ ;í8
jZóY4
iÀ WjFgu0Z~W
x]~8.
$wu v2E!u0{ x]8Fd
v3]v`z d~Wx]~8 wQu dw$ {
Vi (θ, θ̇)(1, 2) = −m2 l1 lc2 sin(θi2 )θ̇i2
v`z 8{ u!wdÃ]v`z YCx]~8z { x1v`dw
]$ v`dCx1v`z x$ ,d{
Fí gk *8
88j
Z]Fí j1
Vi (θ, θ̇)(2, 1) = m2 l1 lc2 sin(θi2 )θ̇i1
Vi (θ, θ̇)(2, 2) = 0 ZOò4iÀ jFjdí
Gi (θ)(1) = g(m1 cos(θi1 )lc1 + m2 (l1 cos(θi1 )
+lc2 cos(θi1 + θi2 )))
Gi (θ)(2) = g(m2 lc2 cos(θi1 + θi2 ))