Embed Size (px)
Citation preview
Etude et mise en place de stratégies de commande de structures flexibles :commande de structures flexibles : Application à un robot d’imagerie médicale
Jordi Batlle Porto - ATSI MASTERJordi Batlle Porto ATSI MASTER GE Healthcare Tutor: Carlos Martinez FerreiraSupélec Tutor: Emmanuel Godoy
Plan:•Introduction•Modélisation•Stratégies de Contrôle•Stratégies de Contrôle •Modèle de l’Accéléromètre•Mesures Expérimentales•Analyse par logiciel•Analyse par logiciel•Implémentation numérique•Conclusions
IntroductionL’imagerie médicale regroupe l’ensemble des techniques utilisées par lamédecine pour le diagnostic mais aussi le traitement d’un grand nombrede pathologies Elle a révolutionné la médecine en donnant un accès àde pathologies. Elle a révolutionné la médecine en donnant un accès àdes informations jusqu’alors «invisibles».
Context du stage :Context du stage :•Innova est un robot d´imagerie vasculaire, développait par GE. Pourpositionner la chaine d’image, le robot est constitué d’une structure quié t l t d d ib t i ê t à iprésente une souplesse causant de modes vibratoires gênants à niveau
de la chaine d’image.
Stage Objectives:•Le but de ce stage était de présenter de nouveaux techniques de controlcommande de façon à diminuer les vibrations, en ajoutant au robot d’unç , jcapteur qui fournissait à la boucle de contrôle l’information correspondantau comportement de cette souplesse .
Modélisation: Système Masse-Ressort-Amortisseur
•L’axe du pivot a été pris pour faire le première étude mono-axe.•Un modèle masse-ressort-amortisseur a été utilisé:
•Souplesse de la structure, supposé sur l’axe, est représenté par la raideur k.•Frottements représentés par l’amortissement d.•Toute l’inertie a été ramenée au niveau de l’axe pivot (Jpvt).
Raideur k
mθ PVTθrN
Jm JPVT
Amortissement d
Chaine Moteur
Modélisation: Système Masse-Ressort-Amortisseur
Transfert électrique:Currentu Kc mΓ
rΓ
0
La partie électrique du modèle peut être considéré comme un gain, à condition que la dynamique de la partie mécanique soit beaucoup plus lente que celle de la partie électrique:
KcG
JKcKeGp
pKcGpup
CLp
m
CL
CLm ≅⎯→⎯+
⋅≅
Γ ↑
)()(
Loop
E Ke
Kc
pJf m+1 ⎯⎯⎯ →⎯ ≅
≅≡Γ
CLGCLfr
00
⎥⎤
⎢⎡ Γ
=⎥⎤
⎢⎡
⎥⎤
⎢⎡ −
+⎥⎤
⎢⎡
⎥⎤
⎢⎡
+⎥⎤
⎢⎡
⎥⎤
⎢⎡ mmmmm kkdJ θθθ &&& 00
Transfer mécanique:m
m
kJkw =2
m
mm
JdJk
Jd
//2
/=ξ
⎥⎦
⎢⎣Γ
=⎥⎦
⎢⎣⋅⎥⎦
⎢⎣−
+⎥⎥⎦⎢
⎢⎣⋅⎥⎦
⎢⎣
+⎥⎥⎦⎢
⎢⎣⋅⎥⎦
⎢⎣ pvtpvtpvtpvtpvt kkdJ θθθ &&& 00
pvtJkw =2
0pvt
pvt
JkJd/2
/0 =ξ
⎟⎠⎞
⎜⎝⎛ +Γ⋅= pvtm
mm p
kwp θ
ξθ )(1
2)( 22
2
⎟⎠⎞
⎜⎝⎛ +Γ⋅= mpvtpvt pk
wp θξ
θ )(12
)( 22
20
⎠⎝+⋅+ pvtmmmm
m pkwpwp
pξ
)(2
)( 22
•Transmission irréversible: deux expressions selon si le couple gravitationnel et le couple moteur ont le même signe
( ) ⎟⎠⎞
⎜⎝⎛ +Γ⋅
+⋅+=→≥Γ⋅Δ+ )(1
2)(0 22
2
pkwpwp
wpkd pvtmmmm
mmmm θ
ξθθθ&
⎠⎝+⋅+ mpvtpvt kwpwp ξ2 2000
2
couple gravitationnel et le couple moteur ont le même signe ou le signe inversé: Hypothèses: Seulement premier cas ( ) ⎟
⎠⎞
⎜⎝⎛ Γ⋅
+⋅+=→≤Γ⋅Δ+ )(1
2)(0 22
2
pkwpwp
wpkd mmmm
mmmm ξθθθ&
pwp θθ →⎟⎞
⎜⎛ +Γ
20 )(1)(
•Simplification: Compensation du couple gravitationnel:( )mpvt
mpvtpvt
wpwpwp
pkwpwp
p
θξ
θ
θξ
θ
⋅+⋅+
=
→⎟⎠
⎜⎝
+Γ⋅+⋅+
=
2000
2
20
2000
20
2)(
)(2
)(
Modélisation: Système Masse-Ressort-AmortisseurFonction de transfert :
pvtmm
m pk
wp θξ
θ ⎟⎠⎞
⎜⎝⎛ +Γ⋅= 22
2
)(12
)(
( )( )( )222222
222
22
2
)()(
mmmooomo
ooom
m
m
wpwpwpwpww
wpwpkw
pp
+⋅++⋅+−
+⋅+=
Γ ξξ
ξθ
( )222
2)( ooom
mpwpwpk
wp +⋅+ ξθ&
( )mpvt
pvtmmmm
m
wpwpwp
pkwpwp
p
θξ
θ
ξ
⋅+⋅+
=
⎟⎠
⎜⎝+⋅+
2000
2
20
22
2)(
)(2
)(
( )( )222222
22
22)()(
mmmooomo
mo
m
pvt
wpwpwpwpwwk
ww
pp
+⋅++⋅+−=
Γ ξξθ
( )( )222222 22)()(
mmmooomom
m
wpwpwpwpwwk
pp
+⋅++⋅+−=
Γ ξξ
( )( )222222
22
22)()(
mmmooomo
mo
m
pvt
wpwpwpwpww
pkww
pp
+⋅++⋅+−
⎟⎠⎞
⎜⎝⎛
=Γ ξξθ&
⎤⎡⎤⎡⎤⎡
Représentation d’état:⎤⎡⎤⎡ θ
uJKcGJkJdJk mCL
pvt
m
m
mmm
pvt
m
m
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
+
⎥⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎢⎡
⋅
⎥⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎢⎡
−−=
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
0/
0
10000///0010
θθθ
θθθ
&
&
&&
&
&&
&
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
⋅
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
=pvt
m
m
NrNr
Yθθθ
&
&
0100000000
JdJkJk pvtpvtpvtpvtpvt⎥⎦
⎢⎣⎥
⎥⎦⎢
⎢⎣⎥
⎥⎦⎢
⎢⎣ −−⎥
⎥⎦⎢
⎢⎣ 0//0/ θθ
Diagramme de blocks:⎥⎥⎦⎢
⎢⎣
⎥⎦
⎢⎣ pvtθ1000
mθ&& PVTθ&&mθ k PVTθ& PVTθmΓ+
+ −−
−+mθ&
p1
p1
p1
p1
pvtJ1
mJ1
d+
d
Modélisation: Système Masse-Ressort-AmortisseurValeurs expérimentales:
Raideur k = 5.9770e+004 ωm= 24.3159 (rad/s) )(pmθ )(pmθ&Amortissement d = 98.32Jm= 186.8184 (kg.m2)Jpvt = 101.0892 (kg.m2)
ωo= 17.8868 (rad/s)ξ m=0.02ξ o =0.0147
)(pu )(pu
( )( )( )2.9118159.06831.0
3.5919726.09869.2)()(
2
2
+++++
=pppp
pppupmθ
( )( )( )2.9118159.06831.0
3.5919726.09869.2)()(
2
2
+++++
=pppp
ppppupmθ&
( )( )291181590683101649.3
)()(
2=ppvtθ
Antirésonance : 24.3 rad/sRésonance : 30.2 rad/s
( )( )2.9118159.06831.0)( 2 +++ pppppu
( )( )2.9118159.06831.01649.3
)()(
2 +++=
ppppp
puppvtθ& )(
)(puppvtθ
)()(
puppvtθ&
⎪
⎪⎬
⎫
⎪
⎪⎨
⎧==→==→±
/ 0 ;1- 0 /2.30 0.0135; 30.18i 0.4079-
sradsrad
ωξωξ
⎪⎭
⎪⎩ ==→ /68.0 ;1 0.6831- sradωξ
Résonance : 30.2 rad/s
Modélisation: Système Masse-Ressort-Amortisseur
⎤⎡ C
Analyse d’Observabilité :
Cv ⇔=≠⇔ vAvoù 0 observable Pôle λNumérique:Symbolique: PBH (Popov-Belevitch-Hautus) test du rang :
⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢
⎣
⎡
=
−1
2),(
nCA
CACAC
ACObsM
( )),( ACObssvd ( ) nC
ApIpRoRang d =⎥
⎦
⎤⎢⎣
⎡ −=
=
)(mode(i)p
mode(i)
325.7591394.56568.7938⎦⎣
( ) 4),( =ACObsRang
681.0030.1826i 0.4079- 0.39890.5840
222.5105;
0.00000.70710.0000
;
0.87900.0291
265.4433
−±
→⋅vC
Résultats:Identification de la paire de pôles complexes comme ceux avec
Numérique:Symbolique:
Identification de la paire de pôles complexes comme ceux avec la moindre observabilité (Les premiers en perdre le rang )
PBH (Popov Belevitch Hautus) test du rang
Analyse de contrôlabilité:Numérique:Symbolique: PBH (Popov-Belevitch-Hautus) test du rang
:
[ ]BABAABBBACrtl
n 12
),(−
=
L
2
( ) [ ] nBApIpRcRankwAwBw
d
TTT
=−−=⇔=≠⇔
= )(où 0 eContrôlabl Pôle
mode(i)pmode(i)
λ( )),( BACrtlsvd
( ) 4),(
),( 242
=
=
BACrtlrangJJ
kBACrtlpvtm
Résultats:
681.0030.1826i 0.4079-0.0039 ;0.0046 ;004-1.2533e
−±→⋅BwT
Résultats:Identification de la de la paire de pôles complexes comme ceux avec la moindre contrôlabilité (Les premiers en perdre le rang )
Stratégies de Commande• Stratégie actuelle (MCB):
• Régulation en cascade avec mesures du moteur• Régulation en cascade avec mesures du moteur
• Stratégies proposées refusées:• Régulation en cascade avec mesures du pivot• Retour de l’accélération dans la boucle de courant
• Stratégies proposées étudies:• (A) MCB + Retour d’état de Δdθ/dt • (B) Retour d’état par commande LQ
• (A ou B) + Filtre coup-bande (A ou B) + Observateur• (A ou B) + Observateur
Stratégies de Commande Régulation en cascade avec mesures du moteur
Stiffness k
mθ PVTθ&&
θR dI− + u Γ−
mθ& Nr
θ&R
Jm
JPVT
Damping d
θR driveI+ −
+ u mΓ+ θPI θ&PI currPI
currM
)(pM Kc
La stratégie de commande actuelle est une régulation en cascade avec:•Boucle de courant (PI) Boucle de Vitesse (PI) Boucle de position (PI)•Boucle de courant (PI), Boucle de Vitesse (PI), Boucle de position (PI)•Anticipation sur la consigne de vitesse.
Le système de commande reçoit uniquement l’information du comportement duLe système de commande reçoit uniquement l information du comportement du pivot à travers de la raideur, dans le cas réversible.
Stratégies de Commande Régulation en cascade avec mesures du pivot
mθ θ&&θ& Nr
PVTθ& PVTθ
)()(ppvtθ&
Jm JPVT
k
m
d
PVTθ
θR driveI+ −
− + − mΓ+
−
mθ
θControl θ&Contol currPI
currM
)(pM Kc
NrAccéléromètre
θ&R
)(pu
Après la résonance (30 2 rad/s) la phase descends jusqu’à –270° donc pour obtenir
dcurr
-270°
Après la résonance (30.2 rad/s), la phase descends jusqu à –270 donc pour obtenirstabilité 2 actions dérivatives sont nécessaires. Comme 2 actions dérivatives ne sont pasphysiquement réalisables (le bruit est trop amplifié), c’est stratégie était écarté.
Retour de l’accélération dans la boucle de courantRetour de l accélération dans la boucle de courant
Les essaies sur Matlab ont montréθ &&&
θ&&Contol
Les essaies sur Matlab ont montréque l’ajout de accélération à traversd’un gain où d’un PI dans la bouclede courant, n’améliorait pas leJm JPVT
k
mθ PVTθ&&
θR driveI+ −
− + − mΓ+
−
mθ
θControl θ&Contol currPI
M
)(pM Kc
NrAccéléromètre
&R comportement du système.dcurrMθR
Stratégies de Commande Cascade control + state feedback of Δdθ/dt
optKmPVT θθθ &&& −=Δ
k
mθ PVTθ&&
R Iθθθ &&&Δ Γ−
mθ& NrAccéléromètre
PVTθ&
− k
d
θR driveI+ u
mPVTθθθ −=Δ
mθ& u mΓθPI θ&PI currPI
currM
)( pM Kc mJ pvtJ
θ&R
Idée intuitive:Fermer la boucle asservissement au niveau du pivot:Est-ce que maintenir Δvel 0 va aider à réduire les vibrations?
↑K
Diagramme de bode de la boucle fermé de la position pivot⎟⎟⎠
⎞⎜⎜⎝
⎛
)()(
pp
stepref
pvt
θθ&
&
Stratégies de Commande Contrôle Optimal La commande LQ (Commande linéaire par optimisation quadratique) est une méthode qui permetde calculer le gain d'une commande par retour d'état qui minimise une fonction de coût du selonle comportement systèmele comportement système.Le choix de la matrice de pondération est un procès itératif, dans le quelle on fait agrandir lesvaleurs jusqu’à que le curant demandé soit au même niveau que celle demandé pour l’actuelstratégie.
⎟⎟⎟⎟⎞
⎜⎜⎜⎜⎛
⎥⎥⎥⎤
⎢⎢⎢⎡
−Γ⎥⎥⎥⎤
⎢⎢⎢⎡
+⎥⎥⎥⎤
⎢⎢⎢⎡
⋅⎥⎥⎥⎤
⎢⎢⎢⎡
−−=⎥
⎥⎥⎤
⎢⎢⎢⎡
pvt
m
mmm
m
mmmm
m
LJJkJdJk
θθθ
θθθ
θθθ
&
&&
&
&
&
&&
&
0/10
10000///0010•(A,B) doit être stabilizable
•(H,A) doit être détectable
⎟⎟
⎠⎜⎜
⎝ ⎥⎥⎥
⎦⎢⎢⎢
⎣⎥⎥
⎦⎢⎢
⎣⎥⎥⎥
⎦⎢⎢⎢
⎣⎥⎥⎥
⎦⎢⎢⎢
⎣ −−⎥⎥⎥
⎦⎢⎢⎢
⎣ pvt
m
pvt
pvt
pvtpvtpvtpvt
m
JdJkJk θθ
θθ
θθ
&&&&& 00
//0/1000
( )∫∞
⋅⋅+⋅⋅=0
)()()()( dttuRtutxQtxJ TT
⎥⎥⎤
⎢⎢⎡ 1
000000
K
⎥⎥⎥
⎦⎢⎢⎢
⎣
==
4
3
2
000000000
qHHQ T λ=R
⎪⎧ ⋅+⋅−=)( yMxLtu ref k
mθPVTθ&&mθ& Nr
Accéléromètr
PVTθ& PVTθoptK
−
⎪⎪⎩
⎪⎪⎨
⎧
−−=
=+−+=
=
−−
−
−
11
1
1
))((0
)(
BBLACMQPBPBRPAPAP
PBRLy
TT
Tref k
d
θR driveI−
u mΓθM currPI
currM
)( pM Kce
mJ pvtJ+
Stratégies de Commande PROBLEME:Il n’y a pas d’action intégral sur l’erreur de la −+
+∫
Contrôle Optimal
Il n y a pas d action intégral sur l erreur de laposition.
SOLUTION:Ajouter dans la représentation d’état un
Accéléromètremθ PVTθ&&mθ& Nr
PVTθ& PVTθoptKPVTm θθθ −=Δ
PVTm θθθ &&& −=Δ
−−+
Ajouter dans la représentation d état unnouveau état correspondant à l’intégral del’erreur de position.
IDÉE SUPPLÉMENTAIRE:
θ&R
k
d
θR motorI−
u mΓcurrPI
currM
)( pM Kc mJ pvtJ−
IDÉE SUPPLÉMENTAIRE:Modifier la représentation d’état de manièreà faire plus intuitif le choix de la matrice depondération.
( )∫∞
⋅⋅+⋅⋅=0
)()()()( dttuRtutxQtxJ TT
⎤⎡ 0000q
⎟⎟⎞
⎜⎜⎛
⎥⎥⎤
⎢⎢⎡Δ⎥
⎥⎤
⎢⎢⎡
⎥⎥⎤
⎢⎢⎡Δ
⎥⎥⎥⎤
⎢⎢⎢⎡
−⎥⎥⎤
⎢⎢⎡
Δ dkpvtpvtpvt
θθ
θθ
θθ&
&
0001000
00100
⎥⎥⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎢⎢⎡
==
4
3
2
1
00000000000000000000
HHQ T
⎟⎟⎟⎟⎟⎟
⎠⎜⎜⎜⎜⎜⎜
⎝⎥⎥⎥⎥⎥
⎦⎢⎢⎢⎢⎢
⎣
Δ
Δ
−Γ⋅
⎥⎥⎥⎥⎥
⎦⎢⎢⎢⎢⎢
⎣
−+
⎥⎥⎥⎥⎥
⎦⎢⎢⎢⎢⎢
⎣
Δ
Δ
⋅
⎥⎥⎥⎥⎥
⎢⎢⎢⎢⎢
−−−−−
−=
⎥⎥⎥⎥⎥
⎦⎢⎢⎢⎢⎢
⎣
Δ
Δ
I
LJ
IJJd
Jd
JJk
Jd
Jk
I
pvtm
m
pvt
pvtmmpvtm
pvtpvtpvt
θθ
θθθ
θθθ
θθθ
&
&
&
&
&
&&
&&
0/100
00)11()11(0
000
( )⎪
⎪⎪⎪
⎨
⎧
=+−+=
=⋅−=
−
−
TT
T
QPBPBRPAPAPPBRLxLtu
0
)(
1
1
⎥⎦⎢⎣ 50000 q
⎟⎠
⎜⎝
⎥⎦⎢⎣⎥⎦⎢⎣⎥⎦⎢⎣⎥⎥⎦⎢
⎢⎣
⎦⎣ pvtpvtpvtIII pvtmmpvtm θθθ 0
00001 ( )( )⎪
⎪⎪
⎩ −→
−→
pvtrefpvt
pvtrefpvt
θθθ
θθθ&&&
Stratégies de Commande Diagramme de Bode de la Boucle Fermé
Diagramme de Bode de la boucle fermé de la position pivot
MCBMCB + Δvel
LQ
⎟⎟⎠
⎞⎜⎜⎝
⎛
)()(
pp
Boderef
pvt
θθ
sradDi
sradDi
540%3.10%587.0309.066.0
3.24%94%0197.0024.0999.0
===±
===±
ωξ
ωξ
radDis
radDi
503%57%176043508050
22%71%108.0021.0997.0
±
===±
ωξ
ωξ
sradDi 503%57%176.0435.0805.0 ===± ωξ
sradDi
sradDi
6.71%4.5%67.005.0951.0
1.22%2.11%57.0017.0987.0
===±
===±
ωξ
ωξ
Stratégies de CommandeIl est réellement nécessaire un accéléromètre?Estimation du vecteur d’état avec un Observateur
θ
⎞⎛ ⎤⎡⎤⎡⎤⎡ ˆˆˆ
Contrôleur Modèlemθ
Observateurmθ&
mθ
mθ&
Jpvtθ
Jpvtθ&
Idée: Estimer la position et vitesse Pivot à partir des mesures de la position et vitesse Moteur
⎟⎟⎟⎟⎟⎟⎞
⎜⎜⎜⎜⎜⎜⎛
⎥⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎢⎡
⋅⎥⎦
⎤⎢⎣
⎡−+Γ
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
+
⎥⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎢⎡
⋅
⎥⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎢⎡
−−=
⎥⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎢⎡
pvt
m
m
m
pvt
m
m
mmm
m
m
m
NrNr
YKJ
JdJkJk
JkJdJkθθθ
θθθ
θθθ
ˆˆ
ˆˆ
000000
00/10
ˆˆ
ˆˆ
//0/10000///0010
ˆˆˆˆ
&
&
&
&
&
&&
&( ) ( ) ( ) ( ) ( )
( ) ( )txKCAxxx
tKytButxKCAtx~ˆ~
ˆˆ
−=−=
++−=&&&
&
Valeurs singulières de (A-KC)⎟⎟⎠
⎜⎜⎝
⎥⎦⎢⎣⎥⎦
⎢⎣⎥⎦⎢⎣⎥
⎥⎦⎢
⎢⎣ −−⎥
⎦⎢⎣ pvtpvtpvtpvtpvt
pvtJdJkJk θθθ 0//0/ &&&&
Choix de K de façon a faire la dynamique de reconstruction ~2,3 fois plus rapide que la dynamique de régulation. (C,A) doit être observable
Valeurs singulières de (A KC) Dynamique de reconstruction
( ) [ ]( )∏ −⋅++=−− 21)(deti
ipKCApI iid ξξω
⎪⎪⎪⎪⎧
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
⋅0.00120.06360.00160.02760.00000.0010
103
⎪⎪⎫
⎪⎪⎧
⎟⎟⎞
⎜⎜⎛
⎤⎡⎥⎤
⎢⎡
⎥⎤
⎢⎡
0000///0010 3130 KK
JkJdJk
⎪⎪⎪⎪
⎪⎪
⎨
⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
⋅
⎥⎦
⎢⎣=→
0.00100.04800.00140.02330.00000.0009
10
0.08657.9019
3
AK=
⎪⎪
⎭
⎪⎪
⎬
⎪⎪
⎩
⎪⎪
⎨
⎟⎟⎟⎟
⎠⎜⎜⎜⎜
⎝
⎥⎦
⎤⎢⎣
⎡
⎥⎥⎥⎥
⎦⎢⎢⎢⎢
⎣
−
⎥⎥⎥⎥⎥
⎦⎢⎢⎢⎢⎢
⎣ −−
−−−
000000
//0/10000///
det
0100
1110
2120
NrNr
KKKKKK
JdJkJk
JkJdJkpId
pvtpvtpvt
mmm
⎪⎪
⎩⎥⎦
⎢⎣ 0.06025.3331 BProblèmes:
1. Observabilité dans le cas de la transmission irréversible 2. Robustesse (Voir après)
Stratégies de Commande
θ θ
Estimation du vecteur d’état avec un Observateur en utilisant les mesures fournies par l’accéléromètre
Idée: Estimer la position et vitesse Pivot à partir des mesures de la position vitesse Moteur et l’ accélération du Pivot
Contrôleur
mθ
mθ&mθ
mθ&
Jpvtθ
Jθ&
Acc
pvtθ&&ObservateurModèle
( ) ( ) ( ) ( ) ( )( ) ( )txKCAxxx
tKytButxKCAtx~ˆ~
ˆˆ
−=−=
++−=&&&
&
. Jpvθcél
Valeurs singulières de (A-KC) Dynamique de reconstruction
⎟⎟⎟⎞
⎜⎜⎜⎛
⎥⎥⎥⎤
⎢⎢⎢⎡
⋅⎥⎥⎥⎤
⎢⎢⎢⎡
−+Γ⎥⎥⎥⎤
⎢⎢⎢⎡
+⎥⎥⎥⎤
⎢⎢⎢⎡
⋅⎥⎥⎥⎤
⎢⎢⎢⎡
−−=⎥⎥⎥⎤
⎢⎢⎢⎡
m
m
mm
m
mmmm
m
NrNr
YKJJkJdJk θ
θθθ
θ
θ
ˆ
ˆˆ
000000
/10
ˆ
ˆˆ
0///0010
ˆˆˆ
&&&&
&
⎟⎟⎟⎟
⎠⎜⎜⎜⎜
⎝⎥⎥⎥
⎦⎢⎢⎢
⎣⎥⎥⎥
⎦⎢⎢⎢
⎣
−−+Γ
⎥⎥
⎦⎢⎢
⎣
+
⎥⎥⎥
⎦⎢⎢⎢
⎣⎥⎥⎥
⎦⎢⎢⎢
⎣ −−⎥⎥⎥
⎦⎢⎢⎢
⎣ pvt
pvt
pvtpvtpvtpvt
pvt
pvtpvtpvtpvt
m
Jd
Jk
Jk
NrYK
JdJkJk θ
θ
θ
θ
θθ
ˆˆ
0000
00
ˆˆ
//0/1000
ˆˆ
&&&&
&( )
[ ]( )∏ −⋅++
=−−21
)(det
iip
KCApI
ii
d
ξξω
⎫⎧ ⎞⎛ ⎤⎡⎤⎡⎤⎡
=
⎪⎪
⎪⎪
⎬
⎫
⎪⎪
⎪⎪
⎨
⎧
⎟⎟⎟⎟⎟⎞
⎜⎜⎜⎜⎜⎛
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡
−−⋅
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
−
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡−−
− mmm
Jd
Jk
Jk
NrNr
KKKK
KKKKKKKK
JdJkJk
JkJdJkpId
0000000
//0/10000///0010
det32
32
32
1110
2120
3130
⎪⎭
⎪⎩
⎟⎠
⎜⎝ ⎥⎦⎢⎣⎦⎣⎥⎦⎢⎣ −−
pvtpvtpvtpvtpvtpvt JJJKKKJdJkJk //0/ 320100
Stratégies de Commande• Techniques de filtrage : Filtre coup-bande:
Le filtre coup-bande rejette une fréquence sélectionnée , qui peut être choisie comme la fréquence de résonance du système pour atténuer les vibrations.fréquence de résonance du système pour atténuer les vibrations.
mθ PVTθ&&
θR driveI+ u
mPVTθθθ &&& −=Δ
mθ& u mΓ−
mθ&
θPI θ&PI currPI
currM
)(pM Kc
Nr
PVTθ&
mJ pvtJ−
)()(
pp
ref
notch
θθ
mθ PVTθ&&
θR driveI+ u
mPVTθθθ &&& −=Δ
mθ& u mΓ−
mθ&
θPI
PI
currPI
currM
)(pM Kc
Nr
PVTθ&
mJ pvtJ
optK
−
mPVT θθθ &&& −=Δ
)()(
pp
ref
notch
θθ MCB
MCB + Δvelθ&PI
pvtRθ&
mθ PVTθ&&
motorI−
u mΓ
mθ&
currPI
currM
)( pM Kc
Nr
PVTθ& PVTθ
mJ pvtJ
optK
−
mPVT θθθ −=Δ
mPVT θθθ &&& −=Δ
−−++∫
pvtRθ
)()(
pp
ref
notch
θθ
Optimal Control
+++
= wpw2 p
wp)()( 2
02
2
pp
o
o
ref
notch
ξθθ
p
⎪
⎪⎪⎪
⎨
⎧
+++
→Δ+
+++
→
4848 84484p )/22(
3.591p 9.727 p 591.3p )/3.24(
2
2
2
2
sradMCB
sradMCB
θ& Conclusions:•Intéressant pour la stratégie actuelle
⎪⎪⎪
⎩ +++
→
++
488.41p 8.84 p 488.41p )/1.22(
484p 8.84 p
2
2
2
sradLQ
•Intéressant pour la stratégie actuelle •Intéressant pour MCB+ Δvit (réduire K)•Pas trop intéressant pour LQ
Analyse de RobustesseRobustesse de stabilité avec la valeur singulière structurée:Une étude de la robustesse de stabilité est faite avec la technique de la valeursingulière structurée par rapport à la raideur (k) et à l’inertie Pivot (Jpvt) { Ils sontsingulière structurée par rapport à la raideur (k) et à l inertie Pivot (Jpvt) { Ils sontles deux paramètres qui varient le plus}.
)( pΔ ( ) ( ) ( ){ } εδεεδδ ;;oit 111 ℑ∈ℜ∈ΔΔ=Δ IIIIppDiagpS)( pΔ
)( pHe z
( ) ( ) ( ){ }
αδαϖμ
εδεεδδ
1 alors ,))((t
;;,,,,,,,,oit 11 11q1
≤↔≤
ℑ∈ℜ∈ΔΔΔ
Δ StabilitéjHe
IIIIppDiagpS iicccrrr crKKK
1111o
J
k
JJJ
kkk
pvt⎪⎩
⎪⎨
⎧
+=
+=
δ
δ
&& &1k kδ 1
1Jpvt Jpvtδ
11:%50du Variation
1o pvtpvtpvt JJJ⎪⎩mθ&& PVTθ&&1/Jm 1/
p
mθ0k PVTθ& PVTθ
1/p 1/pmΓ
d
++ −−
−+1/
p
mθ&
d
−+
0
1Jpvt
;2/11 ;2/
;11 ;
1
oo
pvtpvt
JJkk
JJkk
==
==
modèle avec des incertitudes1 pvtpvt JJmodèle avec des incertitudes
Analyse de RobustesseRobustesse par rapport a la raideur (k) et inertie pivot (Jpvt)
( ) 122)( dBjH ϖμ
( ) 11)( zep kδ=→Δ
( )( )( ) −=
=
=
Δ
Δ+Δ
Δ
41.2)(02.7)(
1.22)(
max
max
dBjHdBjH
dBjH
Optimal
MCB
MCB
ϖμϖμϖμ
θ&
( )
⎪
⎪⎨
⎧≤Δ+
≤↔
Δ
445.0078.0
)(max
MCBMCB
Stability
j
k
Optimal
θδ
μ
&))(( ϖμ jHΔ
⎪⎩ ≤ 31.1OptimalMesures Idéales du Accéléromètre
( ) 11)( zep Jpvtδ=→Δ ( ) 11)( p Jpvt
( )( ) =
=
Δ+Δ
Δ
1.7)(1.22)(
max
max
dBjHdBjH
MCB
MCB
ϖμϖμ
θ&
( )
⎪⎧ ≤
−=Δ
078.0
11.2)(max
max
MCB
dBjH Optimalϖμ
))(( jH⎪⎩
⎪⎨
≤≤Δ+↔27.1
441.0OptimalMCBStability Jpvt θδ &))(( ϖμ jHΔ
Mesures Idéales du Accéléromètre
Analyse de Robustesse( ) 11)( zep kδ=→Δ
( ) 2)( dBjH
Robustesse par rapport a la raideur (k) et inertie pivot (Jpvt)
( )( )( ) =
−=
−=
Δ
Δ
Δ
23.0)(03.2)(
2)(
max
max
dBjHdBjH
dBjH
OldObs
NewObs
IdealM
ϖμϖμϖμ
( )
⎪
⎪⎨
⎧≤≤
↔
Δ
263.1258.1
.)(max
NewObsIdealM
Stability
j
k
OldObs
δ
ϖμ
))(( ϖμ jHObs+AccObsIdealM
( ) 11)( zep Jpvtδ=→Δ
⎪⎩ ≤ 973.0OldObs
))(( ϖμ jHΔ
( )( ) −=
−=
Δ
Δ
93.2)(7.1)(
max
max
dBjHdBjH
NewObs
IdealM
ϖμϖμ
( )Jp
( )
⎪⎨
⎧≤≤
−=Δ
4011216.1
35.1)(max
N ObIdealM
S bili
dBjH OldObs
δ
ϖμ
))(( ϖμ jHΔ ObsIdealM
⎪⎩
⎪⎨
≤≤↔
168.1401.1
OldObsNewObsStability Jpvtδ
))((μ jΔObs+AccObs
Analyse de RobustesseRobustesse du filtre coup-bande
Evaluation de la robustesse à partir des effets vues sur le di d b d d l i ti d l’i ti d i t
MCB MCB + 50%Jpvt
MCBΔVel MCBΔVel + 50%Jpvt
diagramme de bode de la variation de l’inertie du pivot
MCB – 50% Jpvtp
MCBΔVel – 50% JpvtMCBΔVel + 50%Jpvt
Résultats de la Simulation Génération de la consigne
La consigne est obtenue à partir du calcul d’un profil d'accélération qui satisfait :Une vitesse initiale du Tube de 20 °/s• Une vitesse initiale du Tube de 20 °/s
• Le Tube doit bouger à 10 millimètres maximums depuis le signal d'arrêt jusqu'à ce qu'il s'arrête complètement (spécification de distance d’arrêt)
Braking start point2 Valeurs considérés dans les spécifications des vibrations :
Fi t P k S d P k
spécifications des vibrations :Première Pic : (Spécification <1 mm)•Amplitude maximale après au moins une
Tube Braking distance
First Peak
1s
Second Peakinversion de signe
Deuxième Pic : (Spécification <0.1mm)•Amplitude maximale une seconde après le Tube Braking distance
specification: 10mm•Amplitude maximale une seconde après le premier pic (Instant où l’image est prise)
Résultats de la Simulation
Résultats de la SimulationAvec Filtre Coup-bandeSans Filtre Coup-bande
Réduction des vibrations
Résultats de la SimulationConclusions
+ •Pas besoin de la Position du Pivot
MCB + Δdθ/dt - •Pic de courant très forte
•Moins performante (+vibration)+ •Plus performante
Retour d’état LQ
+ Plus performante
•Robustesse
•Facilité d’implémentation- •Besoin de l’état complet
Filtre Coup+ •Intéressant si difficile d’obtenir l’état complet
Filtre Coup-Bande - •Robustesse
•Retard ajouté (Modification de la consigne)+ •Pas besoin de capteurs supplémentaires
Observateur
+ Pas besoin de capteurs supplémentaires
- •Robustesse
•Retard ajouté
•Observabilité dans le cas de transmission irréversible
•Implémentation numérique
Modèle de l’ accéléromètreTypes d’Accéléromètres
Circuit fermé (à asservissement ): Ici, l'accélérationest mesurée à la sortie d'une boucle à contre-réaction.
Boucle ouverte (non asservis) : une masseest à l'intérieur de la boîte raccordé via raideurs, es esu ée à a so e d u e bouc e à co e éac o
Quand la masse essaie de déplacer (accélération), ilapparaît un courant électrique pour garder la masse àça position initiale.
est à l intérieur de la boîte raccordé via raideurs,la position nulle signifie l'accélération zéro etquand l'accélération arrive, le déplacement estproportionnel à la force appliquée :
0)(
=Δ+Δ+→−⋅=
xcxkxmgamF
&&&1)( px
p mc p
1)()(
2
ckpF
px
++=
Types:•à détection piézoélectrique•à détection piézorésistive
Applications :•Les chocsL'accélération vibratoire
Utilisé:
•à détection piézorésistive•à jauge de contrainte•à détection capacitive•à détection inductive
•L'accélération vibratoire•L'accélération de mobiles
à détection inductive•à détection optique
MEMS (Micro Electro Mechanical Systems)
Modèle de l’ accéléromètreSources d'erreur du capteur
•Biais fixés (Bf)•Erreurs de facteur d'échelle (Sx)•Les erreurs du couplage parmi les axes (Mon, Mz)
( ) ( ) xfaxiszzaxisyyaxisxxaxisx NBAccMAccMAccSccA +++++= −−−− 1~•Bruit (Nx)
Dans un système électromécanique comme l'accéléromètre il y a trois sources différentes de bruit:
B it é i i d t B i)(,, edunconsiderfTKzyx ⋅⋅=σ
•Bruit mécanique en raison du mouvement Brownien:
•Bruit électronique présenté par le circuit d'interface (bruit blanc):
)(*2,, GaussianBWPSDzyx =σ
•Bruit électronique présenté par le circuit d interface (bruit blanc):
•Bruit de quantification de la conversion analogique – numérique:
))(/(212
1,, UniformgVSensV
Nbitszyx ⋅Δ
⋅=σ
q g q q
Résultats ExpérimentauxMontage Expérimental
Zigbee+RS-232Zigbee+RS-232
MCB (Axis motion control card)MCB (Axis motion control card)
Characteristic Min Type Max SymbolBias 2.7 3.3 3.6 VScale factor 740 800 860 mV/gBandwidth 300 Hz
Characteristic Min Type Max SymbolBias 2.25 2.5 2.75 VScale factor 1140 1200 1290 mV/gBandwidth 40 50 60 Hz
Dynamic range +-1.5 gNoise density 300 μg/√HZ
Spécifications pour l’accéléromètre du X-AXIS Y-AXIS
Bandwidth 40 50 60 HzDynamic range +-1.5 gNoise density 500 μg/√HZ
Spécifications pour l’accéléromètre du Z-AXIS
Résultats Expérimentaux Essais en une règleProblèmes :•Biais : La double intégration de l’accélération donne pour une petite erreur une grande incertitude de positionBiais : La double intégration de l accélération donne pour une petite erreur une grande incertitude de position•Intégration d’un bruit gaussienne -> Mouvement brownienne•Besoin de connaître le vecteur de gravité en chaque instant pour pouvoir l’extraire de la mesure.
Solutions : •Utiliser le modèle de géométrie direct pour obtenir le vecteur de gravité
Acc Vel Pos
•Utiliser le modèle de géométrie direct pour obtenir le vecteur de gravité.•Etalonnage initial pour rejeter les sources d'erreur constantes.•Utiliser un filtre passe-haut pour éliminer les erreurs presque constants.•Utiliser un filtre passe-bas pour rejeter le bruit électronique.
Acc Vel Pos
Résultats ExpérimentauxMesures sur Innova:
Résultats ExpérimentauxMesures en Innova: Mouvement C-Arc à 0 Pivot 0->90 + Enable Release
Résultats ExpérimentauxMesures en Innova: Mouvement: C-Arc à 0 Pivot 0->90 + Emergency Stop
Analyse des MesuresIdentification du modèle:
Fréquence de résonance:
PSD: Power Spectral Density
4.67Hz
( )0
ln where10
ttBA
eCBeCA tt⎟⎠⎞
⎜⎝⎛
=⎯→⎯=⋅=⋅= −−
ϖξξϖλλλAmortissement
: ( )0100 tt −ϖξξ
ζ ~0.0267.420 ⋅⋅= πω A B t0 t1 ξ
Worst case 0.266 0.1205 7.905 11.88 0.0068 Best case 0.677 0.235 7.05 8.12 0.0337
Analyse des MesuresEtalonnage: Etalonnage automatique: Ici, l'idée est d'utiliser la force de gravité comme une accélérationstatique connu sur les 3 axes accéléromètre pour un capteur sur lequel il n’y a pas destatique connu sur les 3 axes accéléromètre pour un capteur sur lequel il n y a pas decomposante dynamique appliquée. S'il est stationnaire (pas d'accélération externes) l'équationsuivante est valable:
1222r ( )owovou y222
⎟⎞
⎜⎛ −⎟
⎞⎜⎛ −
⎟⎞
⎜⎛ −1222 =++= zyxxr ( )wvux
sow
sov
sou
z
z
y
y
x
x ,,où 1 ==⎟⎟⎠
⎞⎜⎜⎝
⎛+⎟
⎟⎠
⎞⎜⎜⎝
⎛+⎟⎟
⎠
⎞⎜⎜⎝
⎛ r
Une variante de cette dernière méthode est utilisée pour l'estimation numérique desparamètres de calibration Pour minimiser l'effet du bruit dans l'estimation des paramètres
y-axis Pivot:(+/-)90°Pivot:(+/-)90°
paramètres de calibration. Pour minimiser l effet du bruit dans l estimation des paramètres,l'étalonnage se fait en utilisant toutes les mesures le long d'une trajectoire (avec uneaccélération presque nulle) :
y
oz
oysyReal g
1g
( )( )
z-axis
sz
Measured gxacc
yaccxyxacc
yacc
xacc
yaccxy
zaccz zacczaccz
Analyse des MesuresEtalonnage: Algorithme d’étalonnage:
• Placez le robot à C -arc- = 45 °. Déplacez le pivot de (+/-) 90 °.
Y
ZX
45°
Y
ZX
Y
ZX
Y
ZX
45°45°
+/-90°
X
YZX
X
YZX
XX
YZX
YZX
zacc
• Utilisez la fonction matlab planefit,qui fournis les angles du vecteur normal.yacc
xacc
α
γβγ
Analyse des MesuresEtalonnage: Algorithme d’étalonnage: • Rotation vectorielle de sorte que le demi-cercle s'inscrit dans le plan
du mouvementdu mouvement.
• Utiliser la fonction matlab elipsefit , qui fournis les offsets et les facteurs échelle :
Analyse des MesuresFiltrage numérique:
•Fréquence d'intérêt est ~ 4.67Hz .
•La fréquence de coupure du LPF =10Hz .
•La fréquence de coupure du HPF =2Hz .
•La fréquence d’échantillonnage =1000 Hz, (même que MCB).q g ( q )
4-3-2-1-
-4-3-2-1-51
0.8486z3.5305z-5.5208z3.8358z-1)0.0898z0.3594zz5391.00.3594z (0.0898*10)(
++++++
=−zHLPF
4-3-2-1-
-4-3-2-11
0.9837z3.9510z-5.9509z3.9836z-10.9918z3.9673z-5.9509z3.9673z- 0.9918)(
++++
=−zH HPF
Implémentation sur DSPIntroduction: L’implémentation dans un DSP, a impliqué la prise en compte de:•Application de facteurs d’échelle pour obtenir la meilleur résolutionApplication de facteurs d échelle pour obtenir la meilleur résolution•Inexistence de la virgule flottant•Limitation du temps de calcul pour satisfaire la contrainte de temps réel
• Gains des Accéléromètres :• Cos/Sin: X,Y Axe: 3V*(1.0/1.0)(Resistance divisor) -> 3V/2^8 --> 1Bit = 11.7mV Sensibitity3 3V=800mV/g; Sensibitity3V= 800mV/g*(3/3.3)= 727.27 mV/g
->1bit = 16.1mg
Implémenté à partir d’une LUT (Look-Up y3.3V g; y3V g ( ) g
Z Axe: 5V*(1.5/2.5)(Resistance divisor) -> 3V/2^8 --> 1Bit = 11.7mV Sensibitity5V=1200mV/g; Sensibitity3V = 1200mV/g*(3/5)= 720 mV/g
->1bit = 16.3mg
Table) de 8bits avec un ¼ de sinus
• Filtre du bus CAN :• Interpolation:
Implémentation sur DSPEtalonnage:
Du aux limitations mentionnées, une simplification est réalisée:1. Rotation selon la position de C-arc plan du mouvement de pivot.p p p2. Appliquer les facteurs d’échelle + offsets (valeurs équivalents en 16bits).3. Extraire la gravité aux mesures.4. Composant Y accélération du pivot.5 Diviser par le rayon pour la ramener a l’axe5. Diviser par le rayon, pour la ramener a l axe.
Y-axe du Acc. du Détecteur
Y-axe du Acc. duTube
Implémentation sur DSPEtalonnage:
Validation des paramètres obtenus: • Angle du vecteur gravité = Angle du pivot Comparaison de l’angle fourni par
Accéléromètre du Tube
Angle du vecteur gravité Angle du pivot .Comparaison de l angle fourni par l’encodeur du moteur avec l’angle du vecteur gravité fourni par accéléromètre.
Accéléromètre du Tube
X Axis composant de la PositionX-Axis composant de la Position
Y-Axis composant de la Position
Implémentation sur DSPFiltrage Numérique:
Premiers essais avec des filtres IIR ont montré:Au lieu de cela, des filtres de Moyenne mobile ontété utilisées. Ils ont pas besoin de calcul en virgule
[ ]∑ −=M
ikxky 1)(
•Erreurs d’arrondissement dues au fait de travailler avec de valeurs entières dans un nombre limité de bits .•Erreurs numériques propagées à chaque itération.
Les filtres IIR ne sont pas le meilleur choix pour
flottante et il n’y a pas de propagation des erreurs.
[ ]∑=iM
y0
)(
( )→=
)sin(sin)(
fMfMfHπ
π
Les filtres IIR ne sont pas le meilleur choix pour la mise en œuvre numérique!
Le filtre passe basse est implémenté avec une moyenne mobile de 32 points:
Le filtre passe haut est implémenté par la substruction de la CC (avec un filtrage multi-étape de MA):
Implémentation sur DSPRésultats: Après avoir fait l'étalonnage et l'algorithme de filtrage, on obtient finalement un i l 16 bit i é t l' élé ti d l'signal propre en 16-bits qui représente l'accélération dans l'axe
Mesure fournie par l’ élé ètl’accéléromètre
Mesure calibrée
M Fil éMesure Filtrée
PerspectivesVitesse et position : Retour d’état :
Premier essai d’un retour d’état des mesures de vitesse position et intégral de l’erreur de
Vitesse et position obtenus par double intégration des mesures de l’accéléromètre: de vitesse position et intégral de l erreur de
position du moteur. Trajectoire sinusoïdal.intégration des mesures de l accéléromètre:•Erreurs d’arrondissement.•Tendance aléatoire
Conclusions:• La commande par retour d’état complet a représenté en
simulation une amélioration considérable des performances desimulation une amélioration considérable des performances del’asservissement de position.
• Les accéléromètres sont des capteurs bien adaptés pour donnerLes accéléromètres sont des capteurs bien adaptés pour donnerl’information sur l’accélération, ils ne sont pas très bienappropriés pour fournir la position et la vitesse.
• En faisant l’étude de l'état de l'art des accéléromètres, il a étéconstaté que les gyroscopes ont de meilleures performances etqu'ils donnent directement la vitesse angulaireq g
• Les observateurs présentent une bonne opportunité decontourner le besoin de nouveaux capteurs, s’il n’y a pas delimite d’implémentation sur une carte numérique.
Merci deMerci de t tt tivotre attention
