3次元剛体の運動方程式の立て方1_3質点剛体

# 前回

https://qiita.com/GANTZ/items/8f03bccbe9aff6423674


# 問題設定
![2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/eda0d99f-7faa-63e0-2134-678c432c248e.png)

質点1,2,3は座標系Aに固定されている（逆に，点1,2,3に座標系Aを固定している，ともいえる）．
質点1,2,3同士はつながっており，内力が発生している．

質点1が受ける，質点2からの力を $\vec f_{12}$とする．
質点1が受ける，外力を　$\vec f_{1}$とする．
![3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/405484/4a21f786-7745-3df1-ce22-63bb860350da.png)

作用反作用の法則より

```math
\vec f_{12} = -1 * \vec f_{21}
```
という関係がある．

また，打ち消しあう内力に同士によるモーメントも打ち消しあうので

```math
\vec r_{A1} \times \vec f_{12} + \vec r_{A2} \times \vec f_{21} = 0
```
が成立する．打ち消されないとすると，内力によって回転することになってしまう．

### 位置代数ベクトル
$i=1,2,3$として

```math
r_{Oi}^O = r_{OA}^O + C_{OA} r_{Ai}^A
```

### 速度代数ベクトル
時間微分し

```math
v_{Oi}^O = v_{OA}^O + C_{OA} cross(\omega_{A}^A) r_{Ai}^A
```

この後，加速度ベクトルを計算する際ために，外積の順序を入れ替えます(外積オペレータ付きのものを微分するのはややこしくなるため)．入れ替えると符号が逆になるので，＋ーを逆に吊り合わせます．

```math
v_{Oi}^O = v_{OA}^O - C_{OA} cross(r_{Ai}^A) \omega_{A}^A
```


### 加速度代数ベクトル
さらに時間微分し

```math
\begin{eqnarray}
a_{Oi}^O &=& a_{OA}^O - (\frac{d}{dt}C_{OA}) cross(r_{Ai}^A) \omega_{A}^A - C_{OA} cross(r_{Ai}^A)  \{\frac{d}{dt}\omega_{A}^A \}\\
&=& a_{OA}^O - \{C_{OA}　cross(\omega_{A}^A)  \} cross(r_{Ai}^A) \omega_{A}^A - C_{OA} cross(r_{Ai}^A)  \{\frac{d}{dt}\omega_{A}^A \}\\
\end{eqnarray}
```

これにニュートンの運動方程式

```math
ma = f
```
を適用すると

```math
m_i \langle a_{OA}^O - \{C_{OA}　cross(\omega_{A}^A)  \} cross(r_{Ai}^A) \omega_{A}^A - C_{OA} cross(r_{Ai}^A)  \{\frac{d}{dt}\omega_{A}^A \}\rangle = f_i^O + f_{ij}^O
```
という式が得られます．
この式は，後々わかりますが，左辺から

* 第1項 : 重心の慣性力
* 第2項 : ジャイロ効果
* 第3項 : 回転の慣性力

を表しています．

上の式を$i=1,2,3$について全てを足すと

```math
\begin{eqnarray}
(m_1+m_2+m_3) a_{OA}^O \\
 - C_{OA}cross(\omega_{A}^A) \{ m_1 cross(r_{A1}^A) + m_2 cross(r_{A2}^A) +  m_3 cross(r_{A3}^A) \}\omega_{A}^A\\
- C_{OA} \{ m_1 cross(r_{A1}^A) + m_2 cross(r_{A2}^A) + m_3 cross(r_{A3}^A)  \} \{\frac{d}{dt}\omega_{A}^A\}\\
= f_1^O + f_2^O + f_3^O
\end{eqnarray}
```
となります．

ここで，

```math
\begin{eqnarray}
M_A &=& m_1+m_2+m_3\\
M_A r_{AG}^A &=& m_1 r_{A1}^A + m_2 r_{A2}^A + m_3 r_{A3}^A 
\end{eqnarray}
```
となるよう，$M_A,r_{AG}^A$を定義する．すると，次のようにまとめられる．

```math
\begin{eqnarray}
M_A  a_{OA}^O 
 - C_{OA}cross(\omega_{A}^A) M_A r_{AG}^A \omega_{A}^A
- C_{OA} M_A r_{AG}^A \{\frac{d}{dt}\omega_{A}^A\}
= f_1^O + f_2^O + f_3^O
\end{eqnarray}
```

力について，次のように$F_A^O,N_A^A$を定義する

```math
\begin{eqnarray}
F_A^O &=& f_1^O + f_2^O + f_3^O\\
N_A^A &=& cross(r_{A1}^A) C_{OA} f_1^O + cross(r_{A2}^A) C_{OA} f_2^O + cross(r_{A3}^A) C_{OA} f_3^O
\end{eqnarray}
```
$F_A^O$は質点1,2,3をまとめて考えた時の外力で,$N_A^A$は質点1,2,3をまとめて考えた時のトルクです．

先ほどの

```math
\begin{eqnarray}
m_i \langle a_{OA}^O - \{C_{OA}　cross(\omega_{A}^A)  \} cross(r_{Ai}^A) \omega_{A}^A
- C_{OA} cross(r_{Ai}^A)  \{\frac{d}{dt}\omega_{A}^A \}\rangle = f_i^O + f_{ij}^O
\end{eqnarray}
```
この式に，$cross(r_{Ai}^A) C_{OA}^T$を左からかけて，$i=1,2,3$について全てを足すと


```math
\begin{eqnarray}
\{m_1 cross(r_{A1}^A) + m_2 cross(r_{A2}^A) +m_3 cross(r_{A3}^A) \}C_{OA}^T a_{OA}^O\\
 - \{ m_1 cross(r_{A1}^A) cross(\omega_{A}^A) cross(r_{A1}^A) +m_2cross(r_{A2}^A) cross(\omega_{A}^A) cross(r_{A2}^A)  + m_3cross(r_{A3}^A) cross(\omega_{A}^A) cross(r_{A3}^A) \} \omega_{A}^A \\
 - \{ m_1 cross(r_{A1}^A)  cross(r_{A1}^A) +m_2cross(r_{A2}^A)  cross(r_{A2}^A)  + m_3cross(r_{A3}^A) cross(r_{A3}^A) \}  \{\frac{d}{dt}\omega_{A}^A\} \\
 = N_A^A
 \end{eqnarray}
```
にまとまります．
外積オペレータには次の性質がある

```math
cross(a) cross(b) cross(a) b = cross(b) cross(a) cross(a) b
```
これを用いて，

```math
 m_1 cross(r_{A1}^A) cross(\omega_{A}^A) cross(r_{A1}^A)\omega_{A}^A = m_1 cross(\omega_{A}^A)　cross(r_{A1}^A)  cross(r_{A1}^A)\omega_{A}^A 
```
よって，第二項は次のようにまとめられる

```math
 - \{ m_1 cross(r_{A1}^A) cross(\omega_{A}^A) cross(r_{A1}^A) +m_2cross(r_{A2}^A) cross(\omega_{A}^A) cross(r_{A2}^A)  + m_3cross(r_{A3}^A) cross(\omega_{A}^A) cross(r_{A3}^A) \} \omega_{A}^A \\
 = cross(\omega_{A}^A) \{ m_1 cross(r_{A1}^A)^T  cross(r_{A1}^A) +m_2cross(r_{A2}^A)^T  cross(r_{A2}^A)  + m_3cross(r_{A3}^A)^T  cross(r_{A3}^A) \}\omega_{A}^A
```

ここで，

```math
I_A^A = m_1 cross(r_{A1}^A)^T  cross(r_{A1}^A) +m_2cross(r_{A2}^A)^T  cross(r_{A2}^A)  + m_3cross(r_{A3}^A)^T  cross(r_{A3}^A)
```
とすると

```math
M_A cross(r_{AG}^A) C_{OA}^T a_{OA}^O + cross(\omega_{A}^A) I_A^A \omega_{A}^A + I_A^A \{\frac{d}{dt}\omega_{A}^A\} 
 = N_A^A
```
が得られます(符号は転置する際に反転した)．

### まとめ
ここまでをいったんまとめます．
力のつり合い

```math
M_A  a_{OA}^O 
 - C_{OA}cross(\omega_{A}^A) M_A cross(r_{AG}^A) \omega_{A}^A
- C_{OA} M_A r_{AG}^A \{\frac{d}{dt}\omega_{A}^A\}
= F_A^O
```

モーメントのつり合い

```math
M_A cross(r_{AG}^A) C_{OA}^T a_{OA}^O + cross(\omega_{A}^A) I_A^A \omega_{A}^A + I_A^A \{\frac{d}{dt}\omega_{A}^A\} 
 = N_A^A
```

定義

```math
\begin{eqnarray}
F_A^O &=& f_1^O + f_2^O + f_3^O\\
N_A^A &=& cross(r_{A1}^A) C_{OA} f_1^O + cross(r_{A2}^A) C_{OA} f_2^O + cross(r_{A3}^A) C_{OA} f_3^O\\
\\
M_A &=& m_1+m_2+m_3\\
M_A r_{AG}^A &=& m_1 r_{A1}^A + m_2 r_{A2}^A + m_3 r_{A3}^A \\
I_A^A &=& m_1 cross(r_{A1}^A)^T  cross(r_{A1}^A) +m_2cross(r_{A2}^A)^T  cross(r_{A2}^A)  + m_3cross(r_{A3}^A)^T  cross(r_{A3}^A)
\end{eqnarray}
```

# 行列形式で記述
力，モーメントの式で変数を$a_{OA}^O,\frac{d}{dt}\omega_{A}^A$ として行列形式でまとめると次のようになります．

```math
\left(
    \begin{array}{ccc}
     M_A &- C_{OA} M_A r_{AG}^A\\
     M_A cross(r_{AG}^A) C_{OA}^T &  I_A^A\\
    \end{array}
  \right)

\left(
    \begin{array}{ccc}
     a_{OA}^O\\
\frac{d}{dt}\omega_{A}^A\\
    \end{array}
  \right)
=
\left(
    \begin{array}{ccc}
      F_A^O +C_{OA}cross(\omega_{A}^A) M_A cross(r_{AG}^A) \omega_{A}^A\\
N_A^A - cross(\omega_{A}^A) I_A^A \omega_{A}^A \\
    \end{array}
  \right)

```

ここで，質点1,2,3を1つの剛体として見た時の重心位置$r_{AG}^A$が座標系Aの原点と一致していた場合

```math
r_{AG}^A = 0
```
が成立します．この時，上の行列方程式は次のようになります．

```math
\left(
    \begin{array}{ccc}
     M_A & 0\\
    0 &  I_A^A\\
    \end{array}
  \right)

\left(
    \begin{array}{ccc}
     a_{OA}^O\\
\frac{d}{dt}\omega_{A}^A\\
    \end{array}
  \right)
=
\left(
    \begin{array}{ccc}
      F_A^O \\
N_A^A - cross(\omega_{A}^A) I_A^A \omega_{A}^A \\
    \end{array}
  \right)

```

が得られます．
これが，3次元剛体のニュートン・オイラー方程式である！

```math
M_A a_{OA}^O = F_A^O\\
I_A^A \frac{d}{dt}\omega_{A}^A = N_A^A - cross(\omega_{A}^A) I_A^A \omega_{A}^A\\
```

文字が多く，ややこしくなりましたが，原理から一から理解していただければ2次元では現れない項$cross(\omega_{A}^A) I_A^A \omega_{A}^A$の出現理由が理解できたと思う．

次は，実際にシミュレーションを行う．

https://qiita.com/GANTZ/items/8a9d52c91cce902b44c9