# 状態方程式の導出


```julia
using Symbolics
using Latexify
```


```julia
@variables t M m l g D_θ D_x u
@variables x(t) θ(t)

Dt = Differential(t)

v = Dt(x)
ω = Dt(θ)


Dx = Differential(x)
Dv = Differential(v)
Dθ = Differential(θ)
Dω = Differential(ω)
```


    (::Differential) (generic function with 2 methods)


## エネルギー
運動エネルギー，ポテンシャルエネルギー，損失エネルギーは次である．  
<br>  


運動エネルギーを定義


```julia
K = 2//3*m*l^2*ω^2 + m*l*v*ω*cos(θ) + 1//2*(M+m)*v^2
```


\begin{equation}
\left( \frac{dx(t)}{dt} \right)^{2} \left( \frac{1}{2} M + \frac{1}{2} m \right) + \frac{2}{3} \left( \frac{d\theta(t)}{dt} \right)^{2} l^{2} m + l m \frac{dx(t)}{dt} \frac{d\theta(t)}{dt} \cos\left( \theta\left( t \right) \right)
\end{equation}



ポテンシャルエネルギーを定義


```julia
U = m*g*l*cos(θ)
```


\begin{equation}
g l m \cos\left( \theta\left( t \right) \right)
\end{equation}



ポテンシャルエネルギーを定義


```julia
D = 1//2*D_θ*ω^2 + 1//2*D_x*v^2
```


\begin{equation}
\frac{1}{2} \left( \frac{dx(t)}{dt} \right)^{2} D_{x} + \frac{1}{2} \left( \frac{d\theta(t)}{dt} \right)^{2} D_{\theta}
\end{equation}



## ラグランジュの運動方程式に代入
ラグランジュの運動方程式は次式．  
<br>

<br>

### x方向
運動エネルギーの速度偏微分の時間微分


```julia
dKdv_dot = K |> (Dv * Dt) |> expand_derivatives
```


\begin{equation}
2 \mathrm{\frac{d}{d t}}\left( \frac{dx(t)}{dt} \right) \left( \frac{1}{2} M + \frac{1}{2} m \right) + l m \cos\left( \theta\left( t \right) \right) \mathrm{\frac{d}{d t}}\left( \frac{d\theta(t)}{dt} \right) - \left( \frac{d\theta(t)}{dt} \right)^{2} l m \sin\left( \theta\left( t \right) \right)
\end{equation}



運動エネルギーの位置偏微分


```julia
dKdx = K |> Dx |> expand_derivatives
```


\begin{equation}
0
\end{equation}



ポテンシャルエネルギーの位置偏微分


```julia
dUdx = U |> Dx |> expand_derivatives
```


\begin{equation}
0
\end{equation}



損失エネルギーの速度偏微分


```julia
dDdv = D |> Dv |> expand_derivatives
```


\begin{equation}
D_{x} \frac{dx(t)}{dt}
\end{equation}



x方向の入力


```julia
u_x = u
```


\begin{equation}
u
\end{equation}



### theta方向
運動エネルギーの角速度偏微分の時間微分


```julia
dKdω_dot = K |> (Dω * Dt) |> expand_derivatives
```


\begin{equation}
l m \cos\left( \theta\left( t \right) \right) \mathrm{\frac{d}{d t}}\left( \frac{dx(t)}{dt} \right) + \frac{4}{3} l^{2} m \mathrm{\frac{d}{d t}}\left( \frac{d\theta(t)}{dt} \right) - 2 l m \frac{dx(t)}{dt} \frac{d\theta(t)}{dt} \sin\left( \theta\left( t \right) \right)
\end{equation}



運動エネルギーの角度偏微分


```julia
dKdθ = K |> Dθ |> expand_derivatives
```


\begin{equation}
 - l m \frac{dx(t)}{dt} \frac{d\theta(t)}{dt} \sin\left( \theta\left( t \right) \right)
\end{equation}



ポテンシャルエネルギーの角度偏微分


```julia
dUdθ = U |> Dθ |> expand_derivatives
```


\begin{equation}
 - g l m \sin\left( \theta\left( t \right) \right)
\end{equation}



損失エネルギーの角速度偏微分


```julia
dDdω = D |> Dω |> expand_derivatives
```


\begin{equation}
D_{\theta} \frac{d\theta(t)}{dt}
\end{equation}



回転方向の入力


```julia
u_θ = 0
```


    0


## 加速度を求める
これまでの演算から運動方程式が得られた．  
x方向の運動方程式


```julia
f_x = dKdv_dot - dKdx + dUdx + dDdv - u_x
```


\begin{equation}
D_{x} \frac{dx(t)}{dt} - u + 2 \mathrm{\frac{d}{d t}}\left( \frac{dx(t)}{dt} \right) \left( \frac{1}{2} M + \frac{1}{2} m \right) + l m \cos\left( \theta\left( t \right) \right) \mathrm{\frac{d}{d t}}\left( \frac{d\theta(t)}{dt} \right) - \left( \frac{d\theta(t)}{dt} \right)^{2} l m \sin\left( \theta\left( t \right) \right)
\end{equation}



theta方向の運動方程式


```julia
f_θ = dKdω_dot - dKdθ + dUdθ + dDdω - u_θ
```


\begin{equation}
D_{\theta} \frac{d\theta(t)}{dt} + l m \cos\left( \theta\left( t \right) \right) \mathrm{\frac{d}{d t}}\left( \frac{dx(t)}{dt} \right) + \frac{4}{3} l^{2} m \mathrm{\frac{d}{d t}}\left( \frac{d\theta(t)}{dt} \right) - g l m \sin\left( \theta\left( t \right) \right) - l m \frac{dx(t)}{dt} \frac{d\theta(t)}{dt} \sin\left( \theta\left( t \right) \right)
\end{equation}



状態方程式を求めるために，加速度を求める．  
運動方程式は加速度と角加速度の連立方程式になっているため，`sy.solve`を使って解く．  
ついでに式を簡略化．  


```julia
@variables x_dot, θ_dot, x_ddot, θ_ddot, h, f
f_x = substitute(
    f_x,
    Dict([
        expand_derivatives(Dt(v)) => x_ddot,
        expand_derivatives(Dt(ω)) => θ_ddot,
        expand_derivatives(Dt(x)) => x_dot,
        expand_derivatives(Dt(θ)) => θ_dot,
        x => h,
        θ => f,
    ]))
f_θ = substitute(
    f_θ,
    Dict([
        expand_derivatives(Dt(v)) => x_ddot,
        expand_derivatives(Dt(ω)) => θ_ddot,
        expand_derivatives(Dt(x)) => x_dot,
        expand_derivatives(Dt(θ)) => θ_dot,
        x => h,
        θ => f,
    ]));
```


```julia
f_x
```


\begin{equation}
D_{x} x_{dot} - u + 2 x_{ddot} \left( \frac{1}{2} M + \frac{1}{2} m \right) + l m \theta_{ddot} \cos\left( f \right) - \theta_{dot}^{2} l m \sin\left( f \right)
\end{equation}




```julia
f_θ
```


\begin{equation}
D_{\theta} \theta_{dot} + l m x_{ddot} \cos\left( f \right) + \frac{4}{3} l^{2} m \theta_{ddot} - g l m \sin\left( f \right) - l m x_{dot} \theta_{dot} \sin\left( f \right)
\end{equation}




```julia
Symbolics.solve_for([f_x~0, f_θ~0], [x_ddot, θ_ddot])
```

e5033f0ce6419c88f5f8a49e43fb6b85a6916b1d
ipynb
Jupyter Notebook
excercise/julia_src/derivation.ipynb
YoshimitsuMatsutaIe/abc_2022
9c6fb487c7ec22fdc57cc1eb0abec4c9786ad995
MIT
excercise/julia_src/derivation.ipynb
YoshimitsuMatsutaIe/abc_2022
9c6fb487c7ec22fdc57cc1eb0abec4c9786ad995
MIT
excercise/julia_src/derivation.ipynb
YoshimitsuMatsutaIe/abc_2022
9c6fb487c7ec22fdc57cc1eb0abec4c9786ad995
MIT
Qwen/Qwen-72B
1. YES
2. YES