二車輪型倒立振子の姿勢制御

2009SE136
:
1
Vstone
BeatoBalancerDuo(
BBD) .
2
BBD
,
.
1
BeautoBalancerDuo
,
, [1]. BBD 2
, 1.2[V]
.
BBD , 1
.
2.1
2.2
. L .
L = (T 1 + T 2 ) − U (1)
, T 1 , 2
, U
.
BeautoBalancerDuo
.
1
R m 0.6818 [Ω]
K b 0.0014 [Vs/rad]
K t 0.0012 [Nm/A]
4.9825×10 −7 [ ]
f m
f w
0 [ ]
g 9.8100 [m/s 2 ]
R w 0.0207 [m]
M w 0.0053 [g]
M m 0.0075 [g]
M b 0.1453 [kg]
J w 2.2710×10 −6 [kgm 2 ]
J m 3.8274×10 −6 [kgm 2 ]
J b 6.6336×10 −4 [kgm 2 ]
L 0.0520 [m]
l
0.0217 [m]
G r 20.7 [ ]
T 1 = 1 2 M b(ẋ 2 b + żb 2 ) + 2( 1 2 M m(ẋ 2 m + żm))
2
+2( 1 2 M w(ẋ 2 w + żw)) 2 (2)
T 2 = 1 2 J ˙ψ b 2 + 2( 1 2 J mG 2 r( ˙ψ − ˙θ) 2 ) + 2( 1 2 J ˙θ w 2 ) (3)
U = M b gz b + 2(M m gz m ) + 2(M w gz w ) (4)
F θ F ψ
,
.
F θ = ((M b + M m + M w )Rw 2 + J m G 2 r + J w )¨θ
+((M b L + M m l)R w − J m G 2 r) ¨ψ (5)
F ψ = ((M b L + 2M m l)R w − 2J m G 2 r)¨θ
+(M b L 2 + 2M m l 2 + J b + 2J m G 2 r) ¨ψ
−(M b L + 2M m l)gψ (6)
F θ F ψ
,
.
F θ = 2G r (K t i − f m G r ( ˙ψ − ˙θ) + f w ˙θ) (7)

F ψ = −2G r (K t i − f m G r ( ˙ψ − ˙θ)) (8)
.
v = L m˙i + R m i + K b G r ( ˙ψ − ˙θ) (9)
L m
, L m = 0 .
(9), (10), (11)
F θ , F ψ .
4
, x = [2π 0 0 0]
,
2, 3, 4 .
10[s] .
F θ = 2G 2 r( K tK b
R m
+ f m )( ˙θ − ˙ψ) + 2G r
2K t
R m
v (10)
F ψ = −2G 2 r( K tK b
R m
2.3
+ f m )( ˙θ − ˙ψ) − 2G r
2K t
R m
v (11)
(5), (6), (10), (11) (12)
E
[ ] ¨θ
+ F
¨ψ
[ ] [ ˙θ θ
+ G = Hv (12)
˙ψ ψ]
2
, E, F, G, H .
[ ]
e11 e 12
E =
e 21 e 22
e 11 = ((M b + 2M m + 2M w )Rw 2 + 2(J m G 2 r + J w ))
e 12 = e 21 = ((M b L + 2M m l)R w − 2J m G 2 r)
e 22 = (M
[ b L 2 + 2M m l 2 + J b + 2J m G 2 r)
]
2(fm + K tK b
R
F =
m
)G 2 r −2( K tK b
R m
+ f m )G 2 r
−2( K tK b
R m
+ f m )G 2 r 2(f m + K tK b
R m
)G 2 r
[ ] [ ]
0 0
2Kt
R
G =
, H = m
0 −(M b L + M m l)g − 2K t
R m
(12) (13)
A, B, C, x .
{
ẋ = Ax + Bu
y = Cx
[ ]
O2×2 I 2×2
A =
, B =
−E −1 G −E −1 F
[
C = I 2×2 , x = θ ψ ˙θ
]
˙ψ
3
[
O2×1
E −1 H
]
(13)
,
[2].
K
Q,
[
R .
]
K = −0.1000 −39.6322 −0.1905 −5.2626
[
]
Q = diag 1 1 0.7 1 , R = 100
3
4
5
BBD .
BBD
.
.
.
[1] :H8
, (2009)
[2] :MATLB/Simulink ,
, (2011)