倒立振子の制御 運動方程式 (Newton-Euler) 運動方程式 その1 運動

運動方程式 (Newton-Euler)
倒立振子の制御
力 u [N] を操作して, 台車の位置 x = 0 [m] , 振り子の傾きθ= 0 [rad] で
静止させたい
θ
2l
m
y
x
Cθ
振り子(均質な棒) 長さ 2l [m], 質量 m [kg] 軸の粘性摩擦係数 C [kg m2/s]
u
M
台車 質量 M [kg] 粘性摩擦係数 B [kg/s]
Bx
V
Cθ
y
x
H
Mg
(1-3)
J = ml 2 3
台車
M x = u − B x − H
VF
x θ
と について解くと
(1-4)
(
)
(
(
)
)
x = { J + ml 2 ⋅ u − B J + ml 2 ⋅ x
+ Cml cosθ ⋅θ + J + ml 2 ml sin θ ⋅θ 2
( M + m ) x + ml cos θ ⋅θ − mlθ 2 sin θ + Bx = u
ml cos θ ⋅ x + ( J + ml 2 )θ − ml g sin θ + Cθ = 0
− m 2l 2 g sin θ cosθ }/ α
(2-1)
θ = {−ml cosθ ⋅ u + Bml cosθ ⋅ x
J = ml 3
2
ml cos θ ⎤ ⎡ x ⎤ ⎡ − mlθ 2 sin θ ⎤ ⎡
0
⎤ ⎡ Bx ⎤ ⎡u ⎤
+⎢
⎥+⎢
⎥ + ⎢Cθ ⎥ = ⎢ 0 ⎥
J + ml 2 ⎥⎦ ⎢⎣θ⎥⎦ ⎣
ml
g
sin
θ
−
0
⎦ ⎣ ⎦ ⎣ ⎦
⎦ ⎣
M (q )q + h(q, q ) + g (q ) + d (q ) = τ
− C (m + M ) ⋅θ − m 2l 2 sin θ cosθ ⋅θ 2
ただし
(
(2-2)
α = J (m + M ) + Mml + m l sin θ
2 2
2
の形
状態方程式
)
K = Mx + Jθ 2 + mX 2 + mY 2 2 (3-1)
2
X = x + l sin θ , Y = l cos θ
U = mgl cosθ
ラグランジュ関数 L = K − U
2
2
損失エネルギー D = B x + Cθ 2
d ⎛ ∂ L ⎞ ∂ L ∂D
運動方程式
+
=u
⎜
⎟−
d t ⎝ ∂ x ⎠ ∂ x ∂ x
位置エネルギー
(
+ (m + M ) ml g sin θ } / α
2
運動方程式 (Lagrange)
ただし,
慣性モーメント
u
V
(1-2)
運動方程式 その2
式(1-1)~(1-4) から,H と V を消去する
運動エネルギー
m
H
運動方程式 その1
⎡ M +m
⎢ ml cos θ
⎣
d2
(x + l sin θ ) = H
dt2
d2
m 2 (l cosθ ) = V − mg
dt
mg
Bx
ただし
振り子
Jθ = Vl sin θ − Hl cosθ − Cθ (1-1)
θ
)
d ⎛ ∂ L ⎞ ∂ L ∂D
+
=0
⎜
⎟−
d t ⎝ ∂θ ⎠ ∂θ ∂θ
式(3-1)~(3-6) を整理すると,式(2-1),(2-2) となる
(3-2)
(3-3)
(3-4)
(3-5)
状態変数ベクトル
T
x = [x1 x 2 x 3 x 4 ] = x θ
状態方程式
[
]
T
(4-1)
⎡ x3
⎤
⎢ x4
⎥
⎥
x = ⎢
⎢ a 32 sin x 2 cos x 2 + a 33 x 3 + a 34 x 4 cos x 2 + a 35 x 4 2 sin x 2 + b3u 1 + α sin 2 x 2
⎥
⎢
⎥
2
2
⎣⎢ a 42 sin x 2 + a 43 x3 cos x 2 + a 44 x 4 + a 45 x 4 sin x 2 cos x 2 + b 4u cos x 2 1 + α sin x 2 ⎦⎥
(
(
ただし,
(3-6)
x θ
)(
)(
)
)
(4-2)
α 0 = J (m + M ) + Mml 2 , α = m 2l 2 /α 0,
J = ml 2 3
a 32 = − m 2l 2 g/α 0,
a 33 = − B (J + ml 2 ) /α 0, a 34 = Cml/α 0,
a 35 = (J + ml 2 ) ml/α 0
a 42 = (m + M ) ml g/α 0,
a 43 = Bml/α 0,
a 44 = −C (m + M ) /α 0, a 45 = − m 2l 2 /α 0
b3 = (J + ml 2 ) /α 0,
b 4 = − ml/α 0
1