Modélisation et stabilisation LQG du mouvement

Modélisation et stabilisation LQG du mouvement

!
!"!
#%" !
" #$ % "&
%" ! )*+
! "#
$ % &'
(
,#" #
! / /
$
#$ % 0
%
/ $/
1
#
#
/#
4 ,
$$%
.
1
#
,
$ 3#
# 6 % 3# 2$$
/ %$ # %$ #$
# #
$
#
#
0 $
$
$ 2#
$
$
#/
$
$$
$% #
$
& # $ 7#
233
%
2//
"! !
8 $ /
$
#7 #
$
8 / / $
/
$$ $ # # #
,
# #7
# 3 $
9 #. 8
$ #7 5: #
. # 5 #
$ 3#
#7.
# $ $$ <
33
# $$ 3
0
)
"
! "#"(
!
%"
*+ ( ( '
,- # .
[email protected]
! #
.
'
!"- %#"(= #
/
"(
!
4
#
= #
$ >
4
#
$ >
#
%
$
/ $ ,
$
1 / / $% #
$3 $
# 2# # $ #
#
,
$ #
#
5. "#
1
%3% # . $ %$
$
$
#
.
.
9
#
# 3
#3
$
,
#
//
#$
# 3
#
$
$
#$
3 #
.
#
9
;,
.
#
$$ $ #
! #
.
.
!
= / $ ((
7
#
# - #? $
% %
3
#
$
#$
@ # $7 # $/ 7
$
$ $
# $$
$$ 3$.
7 % $ %$
$ #
,
7 #$
# $ / $ 2#$
# $$ $ 1 $
# A
$$ B
$C
$
#$/
$
/ $ #7
*
#
$ $ % %
7 % #
,
#
#7
3
/
$ %
$
$ # #
/
#
;
1 $/ ,
$
$4
% 1$ /
# #
$
3 $/ $% %4 /
$
$
$ 7 $
D E.
= / $ #
#7 #
&##%
$
$ %71 $ $ $ # / 7 $$
# %4 /% $
$/ $ 3$ / $$ 3$
3$
$%
%.
6
2
6 % $
$ $ # %4 /% $ 2 #
/ $
$2
2
7$2
2
$ $ 1 $ 2 . '# # # 4
&
$
7
/ $
$
#$ /
%
# 3 #
#$ & # #
/% ## . )
$ $ $ $1 $ / $
/ $$ # $ $ 4
2
') $$ $1 $ 2
3 # 7
$$ $1 $
#
,
#$ %
$ #.
) # #
F /%
/ $/
$
*
$B
$
$C $&
G # ,
#
#.!
* /
2
4 *$
/ $ / $4
H #$ $ #
%$ % 0 /
1 $.
%/
3 $
3 # 7 $
#$ #% *
$ $
6 $
7#
IBISC-CNRS Fre 3190 Lab. of Evry Val d’Essonne University, 91020 Evry Cedex, France.
#$ 2#$
%. = $ 2
2
7$2
2
#
%
//
# $
33
#
$
/ $/
$& / $
$
33 $ #
/
# 1$ $ # .
% $
# $$ $1 $ $
/ #
%/
/
$$ $ &
$
#. '
$& / $ /
$
#
# / # &$
#I # 3
#
/ %
#
% ,
#
%3 $
7# $
/ ## $ & # 3
#
# .
1
% ,
4
$ $1
$
#
/
#
% %
$
#$
# 3
#
#
$ $ / 1$ $ $ / 3
$@ #
/ %$ # 4 & / 3
#
% %.
$ &#
1
#
/ 3
#
#
#
# /
$$
$/ 3
# $ %$ % $
$4
/ % $ # /
% 6
$/
#$ /
$
6
%3% # @ &$
#
/
.!
#1 $ #
#
/
&% 4
& # 7
/ %$ #
#
*/
1 $
$ # 1$
#
%7 7 . & # 7
$
/ #$
#$ # %
# $
# $J
/
#
/
#/
# #
&%
%
$ $1 / $4
#
% 7 $
, # #$%4 #
/
#
/
# $
%.
1
# #
#
$
$
#4 /
$
$
#
#
$
/ 2
#$
/
#
/ 2
#$
2#7
2# # $ #
#
/
//
0
.
2//
$ # 1$
#
$$
$
%
$ $1 /
$
*
#37
#$ $
# $
K >%
#$
# $
/
#
K >%
#$
$
*
/
#
#7
2# # $ #
#
.
= 3 ? # 7%#%
#$ % 0
% $
# 0 $
$
# $ %
$
/
,
$ $
7 # $%
$
$
#
## #
3 / ?
$ %$
$ 2# %
2
#
7 4 4 $ $
// / %$
/
#
$ # %
%$.
= #$
$
#
#
$ / # 2#
1
#
4 /
$
% $ 4 $
$ $1
#
,
#
4
/#
4
$ / %$ # % .
= #$ $ $
#$
H 2//
$
#%
$
$
# $ / %$ # % . = $ # A
$
$ # $ # % $%$ #
$ #
#
5.
= $ %$
$
$
#$ 33 % $ $ # / %$ # %$ #$ / 7 / L. = $
#M
$
$ # *% % $ /
$
#$
# # &%4
# 7#
#
7 .= $
# $ #$ $ # 3 # $ #$ $
# N.
0
!%
"# %
% #
2
#
7
# 4 %4 / $
N(O
$
#$
$ #
#
# /% ,
#$ /
$ # 0
$$
#$
H(; I D E.
#
%7 ,
# 4
#$ HO $
#$
$
$
$
/ 4 %.
'#
4 #
# %
$ $
/ 4 % #$ &
#
#
$
$/ #$
#$ NO
$ $
%
$ #$ H(O
$ $. $ O $ # $ $ #
$0
2
%
/$
%
#
* %3 $
2#3 $
.
2
%
%
%
((
#$
1# 0 2$
#
N $
/
,
#
$ 0 2#
/
% #
#
# .!
#
/
% $
0
$ $
# $/ 3
# $ 2*%
#
#
0
$
# / %
#
2#
##
# . P# # % 4
#$ NHO
$
#$
#
#
#
#M
$
$ / # /
#
3 # 7
4 7 /
%
#7 .
10
#%" !
#$
%
= / $
1 7
* /$ / / $% #$ D E $
1 $/ $
6 $4 2
0$ /
/$ # % % %
%$ #$ D E.
/ * % % % 7%#%
#
$
/
/
2
%
2#
# #
% $ $
/
/ # # 2 %%
#
3 # 7
4 7 D H LE.
0 0
2" !
"
1 %
//%
#$ DN QE.
#
/$ 7
/
& #$
R)
# 3
R
1
6
$
#
R>
1
R)
$ /%
F
$T C.
!
1 04
/$ 2
2
>
1 2
2
% $
$
#
/$ 7 $ /
F
%
#$
/$ 2
2
$
/
1 0$
10
4%
3 #" ! !
"
/ *$
#
#
#% $,
# $/% 34
!0
$/ $
4
/$ 7 $ 6 # 3$
$ #$/ %
* / %$ # %$
#
/
$ /%
/$
#
. $4
2
2
! #
,
2
2$ $
$% #
# 7
#
#
$$ $
# #
#$ 2*
#$ $
S$$ $
/$ #3%
$
/$
#
/
$ /%
/$
#
B #
0$
#
$
/$ 7 $ #
1 2
2
. = #$
$
#2$ / $ / $$
2
2< $ #72
2
2
2$< #7 #7
2
2
.
#
$ / #
# 2# %
B#
*
$/
* / C %
2/,
# $ / # / $ 3#
# *
% #4
$
/$ $
$. ! $ / # / $ # ,
2# /
2
%4
$ 3 $ *%
$ 7 $$ # $
%
2
/
2
%4 ,
$
#$ #
4 $
%
/
//
*
# $ *%
$. $ $ #
$
$
* // 4 %$ , $$ $
$ $1 B
,! #
C.
U/
2//
# $ $ 3#
#
$
#
4 B $
< #C $ %4 ,
#$
# $ #
# $ #
# 2
%4
# %
# 2
%4
#
2
%4
#
$ 2
%4
#
/
#1
3
3#
# $
*
%4
#$
* #
3 $
/# $
#
1 .
$ #& #$ / $ # $ #
#$
# % #$
#
$ 33% # $ %4
#$
$ # $ / %$ #
#$ $
#
#1
$ # % 4
$ % % # $ $$ # $ $
1 $ # # #%
#%
.=
$ %4
#$ # $
%3# $$ #$ $
1 $ $ $ $ 1 $ # #, #%
#%
/ 1$ #% $
#.
/
# #
1 # # #%
$ %7 /
x = F ( x, u ) x ∈ R , u ∈ R
n
1
$ 2
%
#%
$
#
m
# %4
PV F $
/ 1$ #%
#
33% #
# 3#
$
3
# # #, #%
# #
$
#
.
$2
2
$%
$
8
2
2
Q
,
x = [v, φ , r , δ , φ , δ , F f , Fr ]T
PV v $
$$
#
φ
%
$$
φ 2#7
#
$
$
δ
&#7
#
%
$
#
1
#
/#
4 ,
$ # % $
# $ $ #
#
/
#
#
#
%
#C. ! $ # % $ $ # 7
/% $ #$
u1 = τ u 2 = [φr ,φr ,φr ]T
#%
$
#$ 3
#
∂Fi ( x, u )
∂x j
j =1
! 4
##
#
#
$$
Fr
Ff
$$% .
# % $ # %/ #
$
#
#
& #7
$ #
$3
$
# $
/ %$ # % $ /
B #7
$$
%,
u = [u1 , u 2 ] = [τ ,φr ,φr ,φr ]T
#
n
∆xi =
δ
$$
∆x j +
x 0 ,u 0
/ %$ #
$
/
1$ $ $
∂Fi ( x, u )
∂x j
j =1
p
# &%
$$
x 0 ,u 0
#
/ 3
$$
# $
∆u j
/
$ $1
#%
E.x = A.x + B.u
y = C.x + D.u
PV'
!
= W( $ #
$
$
6
x = [ν , φ , r , δ ,φ , δ , F f , Fr ]
10
3$#
4
#
!
4
/
$
#7
*
Fy $
7 $$
σ
f
u
σ
r
u
&#
$
#7
F
#
Fr = −
101 ,#2
4
/ %$ # % /
"
C
C
fvf
u
fvr
u
v−
v+
4%
$
#
% 3% /
# %4
σy
%
Ff = −
2
# #$ $
3 & 6
/ $
$
4 3 0 $
#$. = #$ D (E ) 6
;
# &
, 7#
# /
#
1
/
& #7
7 $$
# B#
#
#7
# %%
$%
$$
#$ D E. #$ /
%
/#
%
$3
#
!
%3
#
3
$
"!
$
x = [ν ,φ , r , δ ,φ , δ , F f , Fr ]T
T
$ $
H I$ 0
C
#
33% #
#7
fvf
.l f
u
C fvr .l r
u
ψ +
*
C
fvf
u
tt
δ
#
f
/
#7
%
*
.
%
*. )
$
%
#$
# %/ #$ 0
#C.
#
#
#$
,
Cy
#
7
%
#
− Ff
ψ − Fr
%
!
# /
$ 3
4 &0 $
$$ $
.!
1
$ #$
/
# 7
( I$.
$
% $ $
#
#
% $ /A $
$ $1 .
# $
? #$ $ /
#7
# $ /
$,
% #
$$
( I$. ! /
$ %
$ $ /A $ #
3#
# $
$$ $
1 $ #%
$$ #
#7
#
# $/
0 0
107
4%
#
$/
#%
# 7
$
#
"2 ! " # %% "
( I$ 0
$$
"25% "
6
!
7%#%
#
3
% $% /
%#
*1
$ #%/
3
#
τ P = 0.2.s
/
ζ n W (.N(N
3%4 #
#
/ $ # 4
$/ #
*
/
#
$1
0 2#7
$ $/
$ # $
33% #
! "" %
!
. (C. $
#$ # .
%0
V
#
#
4
/ #$
#
#3
/ $$ ,
$
#
# #
,
$
#
$
$
,
/
$$
#,
#
#$ / %
#$ #
$
$
#
/ %$ #
$
3
/ %,
7
#,
.
/ $ # $ #7 #$ /
$/ ?
/
1 $ # $ # / $ #$ # $
#
.!
#
2#7
$ %$ %
/ %$ # # 2
#/ /
##
#
,
//
0 2#7
$ %$ %. 3 #
4
$ # $
#
$/
$ #37
#$
1 $
%/
#
#/ /
##
0
φdes
2#7
φ
$ %
.!
/
#
$ // $ 4
#
$$
$
$
0
/
$ /%
$ #
/$ DQE.
#$ 4
/
1
#
#
/
B # / $C % $
$$
#7
# .
1 3#
/
, #
$
# # #
#
#$
1
/
#
ω n = 10rad / s .
τL
/$
$$ # $ %
#$ & 6 #
& #
$
$
#
$ #
/$. !
1 /
. .L.C. !
1
#
#
7
,
!
% # #$
/
#
7 7
4 /
#$ # /
/$
# $ &# # $ #
/
$ /%
$
$
#/
/
# B- 7
$
/ $ # $ DQE.
$2//
1 $
/ %$
/ $ #
/ %$ #
/
3%4 #
1
#
$
&# $ 7
# / $
/
.!
%
( I$ B37
#7
#
$ /%
#4 $
#
4
/$
3
#
#
φr .
#
# % #/
/
/
#
2# %
#
T
#
$
# 2#7
# #
# # 0
$
$
1
& #7
$
/
$
/
$ %$ %.
&#7
#
010
70
!$4"
& 6
#
8
"!
3
#
/
&%
&33
$
*/
&33
#
#$
$
$ $1
F
/
/
$$ #
J =
t1
1
lim
2 t1 → +∞
$
33
22% !
$
*
$
#
##
/
$.
/
1 4
#
0 &%
# / $ #
&$ 4
#
$
$ #
$ $ # #% $$
1
#
/
4 &# /
$
,
/ $
$# $ # / $
/
1
#
# $ 4 &#
D E. 8
##
#$
$
33% # 0
#
#%
4
% $/
&%
B 5:C 4 &$
1 4
/
/ $
%
4
. '#
$. P
5 $
#
#
!
$
"
2
/ # %
)
3
# %
$
$
$/ #
1
sup( u i )
$
$
$
#
#
33% # 0
#
4
/ # %
/ ,
#
#
,
.
4 B 5C #% $$
## $$ #
$
#
##
$/ $ 4
&#
#,
#$
$
1
. = #$
$ 0
#
5 $$
0 #3
X
# D+E.
!
%"
$
4 7 B #7
"
$
#/
/ $ #
1
%" !
#
##
4
6
#
&% "!
#4
R (t ) > 0
1
$
#
" "!4
$ @φ
$$ $ #7
#C
#
$
!-
!
r
0&
&#
δf .
# !
# $ # 7%#%
#
$ $
7 #
$ VD E
R = diag ( r1 , r2 ,..., rm )
Q = diag ( q 1 , q 2 ,..., q q )
ri =
$C
/ # %
#$
& #7
$
# &#
u opt (t ) = − K . x 4
#/
&#
;0 0 $ '
7 #
#
$ # 1$
.! / # #
# $$ # /
# 3 // 0
/
##
$
&%
&
$
3
t0
&%
;0
"! 0
B 5C $
#
/ $
!
4
# %
/
$ #
4
2
[ x T ( t ). Q ( t ). x ( t ) + u T ( t ). R ( t ). u ( t )] dt
Q (t ) ≥ 0
#
B
% $/
#
4
&/
#
# $
%" !
%" ! 9)*+:
#
#
"!
#%
# %0
33
4%
# #
5
# $ # / # # B>17
2
W
T
>
#$
$
$
7 /
$4
W
T 4
$ #C
qi =
1
sup( y i )
2
$ # % $,
0 ,#" %! !"
#
%!
#4
, #
#
#/
$
P# $ // $ 4
( I$
2#7
= #$ 4 $
# 37
B C.
#
#
#7
$
#
#.
%7
1
B 7#
#
/
#
4
$ $1
# $ %
$%
C 0 #
&#
#4
$$ 3*% 0
%
B #M
/
// 4 %
/
,
# C.
# /
%
%/ #$
$ 33% # $ % $
$ $1
%/ #$
$ $1
# #7
#
#
## $
# $%
%3% # .
#$
/
#
$ $
$ # #
#M
$
#
$ $1
4 $
#
(LQG Control)
80
Rider steering torque
60
Steering torque [N.m]
40
20
0
-20
-40
-60
0
5
10
15
20
Time[s]
070 < !#
(LQG Control)
0.8
Model
Observer
0.6
Roll rate [Deg/s]
0.4
0.2
0
-0.2
25
30
2%
15
20
10
10
0
Model
Observer
-5
-20
-30
-15
-0.8
-40
-20
20
Time[s]
40
(LQG Control)
30
20
Time[s]
-10
40
(LQG Control)
40
Steering angle [Deg]
10
0
-10
-20
Actual
Observer
0
20
Time [s]
40
20
10
0
-10
-20
Actual
Observer
-30
-40
0
20
Time [s]
0;0 #! !""! % "
" "!4
0
!%
20
Time[s]
20
0
-20
-40
-80
40
(LQG Control)
200
0
-200
-400
0
20
Time[s]
" "! # "
0
40
20
Time [s]
40
(LQG Control)
1000
Model
Observer
400
-600
40
40
-60
600
30
20
-30
0
Actual
Observer
60
0
-0.6
(LQG Control)
80
Model
Observer
5
-0.4
0
(LQG Control)
20
30
-10
40
!
(LQG Control)
40
35
Steering Rate [Deg/s]
-80
Lateral Speed [m/s]
$ $
3 1
#
0 9% .
#
$
$.H.
#
/
. P#
$
#
$
Roll angle [Deg]
$ 37
$
# $
$ %7
$
#
.
$ $
# %
#4
Rear Tire Lateral Force [N]
7
$
Yaw rate[Deg/s]
$
&#$
Front Tire Lateral Force [N]
P#
"
Actual
Observer
500
0
-500
-1000
0
20
Time [s]
40
=0
%"
= #$
4 4 $
/ %$ # %$ # #
#
/#
1
#
/
/$
#
$ 6
$
. "# / % %
#
$
#
5 $$ % 0 # $
# A
$ $ # % $% /
/
# 4
# A
$
$/
# %7
# 4
! / # # & $
$& 1 #% $$
3#
#7
# .
0> %
.
.
>.
>. .
$/
$ 7%#% * $ %
//
# $ 2#
1
$ #
4
$ $/ $
$
%$ #
$ %$. )
$
#
$ #
# 2# # $ #
/
$ /%
%
1 .!
#
#
/
# / $$ ,
$ # 1$
#
$ %
//% /
$
$
#
1
#
/
#
#.
%
,
X
# B 5:C $
$% /
$
$
#
$ .
$
&#
1
%3% # . $ %$
$
$
#
# % $% $$
# #$
$
#M
$ #
$. $ %$ ,
$/ 3
# $
#.
# &#
#$
%
$%4 #
## $ / 3
#
7 # # 3#
# $ # $
#
#
$$
$$
2$
/
! "#$%" &% %'(% %&$!$ . Y .
. '#7.
. BHC
LK + +N .
/ # =.Y. .
(% %&$!$ (%# '%&
! "# $%" &% " ! .
,
$
= #
$ LB C
K
(( .
. >. .
/
! %" &% "#
& ") & *%" %'(% %&$!$ . Z
$
= #
$
HB KHC + K Q. # [
#7
$$ .Z ## .
K
(( .
. >. .
/ . ' #7
# =.Y. .
. (*&%+ ( " "
(%# ") %'(% %&$!$
#!" ( $ . # '!!P
8
! #3 #
#
# $ #! /
#
= #
$ BY. .!.
\$ ' .C. $ #.
(( ,( +B!=,>P C (( .
H. >. .
/
!%'(% %&$!$ " $$ & %" "## $ & %". # #$ .
.'#7. ! #,
3 # )
#7$ # ] ; #7 3>
Z
$^. ').
# # HKH(@ +NL.
L. >. .
/
&
) %( &! $ + & & , * " %" '%& (% %&$!$ . # ) . 3
H
# ,
#
#
/ $
#
! #
B Z'! (((C. ##
. QHKH+ (((.
N. >. .
/
& %" (%# %'(% %&$!$ "#
& # )"* & ( & " + . # Z
Z
# > 3#
# )
# ! #3.
. '#7. )
#$
# #.
#7
(NK
++ .
Q.
. !.
Y. .
=.8. _ #7 . .
)" * & ( & ,#! %"
! "# * &$ *%" %'& #") $%('%& %'
$& $ (% %&$!$ ,"# & & # & "")
# $
+,NL
(( .
+.
.
#. % # $%,& # (
&
( %" . & )
%$
%". "#
$ %
&'
Z
&'$$ ## - # . ((N.
(. '. ;;
. .) 6
; #
.
# . " / & (%# /
" ** $ %" " + $ #!" ( $ ,# .
' / / B#.Q+((QNC +Q+. , NK
. ). =
#
4
%(( "# # ! 0(
"1 & . 1
%
#
7 #% '
#
1$ ) $ ++ , ++L.
. %
%
1
1$,& 1&%, 0& ".& "$
"# 2""1 3445 #%$,( " %"'& "6
) $ ((N.