倒立振子のモデル化と制御

1
, ,
, . ,
, ,
. ,
,
.
2
,
Beauto Balancer 1 [1].
1 Beauto Balancer
——
2009SE129
2
2 . ,
θ[rad], ψ[rad] . ,
1 .
.
L .
L = T 1 + T 2 − U
= 1 2 M(R2 ˙θ2 + 2R ˙θl ˙ψ cos ψ + l 2 ˙ψ 2 )
+ 1 2 mR2 ˙θ2 + 1 2 J θ ¨θ 2 + 1 2 J θ ¨θ 2
− mgR − MgR − Mgl cos ψ
(1)
L ,
θ = 0, ψ = 0 , ,
.
[ [ [ [ ] [ ¨θ¨ψ] ˙θ˙ψ]
0 0 θ
Kt
]
E + F +
= r
0 −Mgl]
ψ − K v (2)
t
r
, u ,
[ ]
MR
E =
2 + mR 2 + J θ MRl
MRl Ml 2 + J ψ
.
F =
[ Kt K b
r
+ f − K ]
tK b
r
− f
− f + f
− K tK b
r
K t K b
r
, v = u, x =
[
ẋ =
0 2×2 I 2×2
−E −1 G −E −1 F
(3)
(4)
[
θ ψ ˙θ
] T
˙ψ
ẋ = Ax + Bu (5)
] [ ]
0
2×1
x + u (6)
E −1 H
1
g 9.81[m/g 2 ]
M 0.177[kg]
m 0.005[kg]
R 0.021[m]
l 0.084[m]
J ψ 14.83 × 10 −5 [kgm 2 ]
J θ 1.136 × 10 −6 [kgm 2 ]
0
f 0
r 0.6[Ω]
K t 0.00204[Vs/rad]
K b 0.00204[Nm/A]
T 1 ,
T 2 , U
. ,
K .
3 .
3 K
3
K ,
.

A + BK .
, .
,
K .
, Beauto Balancer 3.3[V]
, .
.
(−30 −20 −4 −5) (7)
K
K = [−0.21 −14.85 −0.115 −0.78] (8)
(−1 −1 −1 −1) (9)
K
K = [0.00 −7.34 −0.002 −0.02] (10)
t[s], θ = 0, ψ = π/12, ˙θ = 0,
˙ψ = 0 .
.
, R
. ,
Q, R
. .
K
⎛
⎞
3 0 0 0
⎜0 1 0 0⎟
Q = ⎝
0 0 1.5 0
⎠ , R = 100 (12)
0 0 0 2
K = [−0.17 −20.14 −0.17 −1.17] (13)
K
⎛ ⎞
1 0 0 0
⎜0 1 0 0⎟
Q = ⎝
0 0 1 0
⎠ , R = 10000 (14)
0 0 0 1
K = [−0.01 −14.62 −0.025 −0.49] (15)
.
t[s], θ = 0, ψ = π/12, ˙θ = 0, ˙ψ = 0
.
u[V]
t [s]
[(a) ]
ψ[V]
t [s]
[(b) ]
4
,
. ,
K .
4
. [2] ,
J (8) .
J =
∫ ∞
0
(x(t) T Qx(t) + u(t) T Ru(t))dt (11)
, .
, 3.3[V]
u[V]
t [s]
[(a) ]
ψ[V]
t [s]
[(b) ]
5
,
.
5
, ,
.
, ,
. ,
.
[1] Beauto Balancer http://www.vstone.co.jp/robot/
[2] : MATLAB/Simulink ,