剛体力学のためのFEM：クォータニオンによる回転処理と慣性テンソル

剛体力学の基本方程式

流体-構造連成(FSI)解析や離散要素法(DEM)シミュレーションにおいて、剛体の運動を正確に記述することは不可欠です。剛体は変形のない物体であり、任意の2点間の距離が常に一定に保たれます。

剛体内の任意の粒子 $i$ の速度は、次のように分解されます。

$\mathbf{v}_i = \mathbf{v}_{cm} + \boldsymbol{\omega} \times \mathbf{r}_i$

ここで、$\mathbf{v}_{cm}$ は重心の速度、$\boldsymbol{\omega}$ は角速度、$\mathbf{r}_i$ は重心から粒子 $i$ までの変位ベクトルです。

線形運動量と角運動量

ニュートンの第2法則より、全体の線形運動量は次のようになります。

$\mathbf{P} = M \mathbf{v}_{cm}, \qquad \frac{d\mathbf{P}}{dt} = \mathbf{F}_{ext}$

ここで、$M = \sum_i m_i$ は全質量です。重心の加速度は、

$M \dot{\mathbf{v}}_{cm} = \mathbf{F}_{ext}$

全体の角運動量は、

$\mathbf{L} = \mathbf{I} \boldsymbol{\omega}, \qquad \frac{d\mathbf{L}}{dt} = \boldsymbol{\tau}$

ここで、$\mathbf{I}$ は慣性テンソル(inertia tensor)、$\boldsymbol{\tau}$ は全トルク(torque)です。

慣性テンソル

慣性テンソルは重心を基準として、

$I_{jk} = \sum_i m_i \left( |\mathbf{r}_i|^2 \delta_{jk} - r_{i,j} r_{i,k} \right)$

連続体へと拡張すると、

$\mathbf{I} = \int_V \rho \left( |\mathbf{r}|^2 \mathbf{E} - \mathbf{r} \otimes \mathbf{r} \right) dV$

ここで、$\mathbf{E}$ は単位テンソル、$\mathbf{r} \otimes \mathbf{r}$ は外積(outer product)です。

物体が回転すると、慣性テンソルは方位行列(orientation matrix) $\mathbf{R}$ によって変換されます。

$\mathbf{I}(t) = \mathbf{R}(t) \, \mathbf{I}_0 \, \mathbf{R}^T(t)$

ここで、$\mathbf{I}_0$ は物体座標系(body frame)における初期慣性テンソルであり、シミュレーション開始時に一度だけ計算すれば十分です。

クォータニオンを用いた回転表現

オイラー角(Euler angles)で回転を表現すると、ジンバルロック(gimbal lock)の問題が発生します。これを避けるために、**クォータニオン(quaternion)**を使用します。

クォータニオン $\mathbf{q} = (q_0, q_1, q_2, q_3)$ は、スカラー部分とベクトル部分で構成されます。

$\mathbf{q} = q_0 + q_1 \mathbf{i} + q_2 \mathbf{j} + q_3 \mathbf{k}$

単位軸 $\hat{\mathbf{n}}$ に対して角度 $\theta$ だけ回転させる場合、

$\mathbf{q} = \cos\frac{\theta}{2} + \sin\frac{\theta}{2}\left(n_x \mathbf{i} + n_y \mathbf{j} + n_z \mathbf{k}\right)$

2つのクォータニオン $\mathbf{q}_1$ と $\mathbf{q}_2$ の合成回転は、積として表現されます。

$\mathbf{q}_{12} = \mathbf{q}_2 \mathbf{q}_1$

これを利用すると、方位行列 $\mathbf{R}$ はクォータニオン成分によって明示的に表現されます。

クォータニオンの時間微分は、角速度 $\boldsymbol{\omega}$ と結びついています。

$\dot{\mathbf{q}} = \frac{1}{2} \begin{pmatrix} 0 \\ \boldsymbol{\omega} \end{pmatrix} \otimes \mathbf{q}$

FEM離散化と四面体形状関数#

FEMで剛体の変位場を離散化する場合、四面体要素の形状関数(shape function)は次のようになります。

$N_i(\xi, \eta, \zeta) = L_i, \quad i = 1, 2, 3, 4$

ここで、$L_i$ は無次元体積座標(barycentric coordinates)であり、$\sum_i L_i = 1$ の条件を満たします。

線形四面体要素における変位補間は、

$\mathbf{u}(\mathbf{x}) = \sum_{i=1}^{4} N_i(\mathbf{x}) \, \mathbf{u}_i$

剛体拘束条件を適用すると、自由度は6個（並進3 + 回転3）に減少します。

Python実装例#

import numpy as np
 
class RigidBody:
    def __init__(self, mass, I_body):
        self.M = mass
        self.I0 = I_body          # 物体座標系における慣性テンソル (3x3)
        self.x_cm = np.zeros(3)   # 重心の位置
        self.v_cm = np.zeros(3)   # 重心の速度
        self.q = np.array([1., 0., 0., 0.])  # クォータニオン (w, x, y, z)
        self.omega = np.zeros(3)  # 角速度
 
    def quaternion_to_rotation(self):
        w, x, y, z = self.q
        return np.array([
            [1-2*(y**2+z**2),  2*(x*y - w*z),  2*(x*z + w*y)],
            [2*(x*y + w*z),  1-2*(x**2+z**2),  2*(y*z - w*x)],
            [2*(x*z - w*y),    2*(y*z + w*x),  1-2*(x**2+y**2)]
        ])
 
    def inertia_world(self):
        R = self.quaternion_to_rotation()
        return R @ self.I0 @ R.T
 
    def step(self, F_ext, tau_ext, dt):
        # 並進運動の積分
        a_cm = F_ext / self.M
        self.v_cm += a_cm * dt
        self.x_cm += self.v_cm * dt
 
        # 回転運動の積分
        I_world = self.inertia_world()
        alpha = np.linalg.solve(I_world, tau_ext - np.cross(self.omega, I_world @ self.omega))
        self.omega += alpha * dt
 
        # クォータニオンのアップデート
        omega_q = np.concatenate([[0.], self.omega])
        q_dot = 0.5 * quaternion_multiply(omega_q, self.q)
        self.q += q_dot * dt
        self.q /= np.linalg.norm(self.q)  # 単位クォータニオンの正規化
 
def quaternion_multiply(q1, q2):
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    return np.array([
        w1*w2 - x1*x2 - y1*y2 - z1*z2,
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2
    ])

実務上の注意点

1. クォータニオンの正規化忘れ 数値積分が繰り返されると、 $|\mathbf{q}| \neq 1$ となり、回転行列が非直交(non-orthogonal)になります。毎タイムステップごとに q /= norm(q) を必ず実行する必要があります。

2. 慣性テンソルの座標系の混同 $\mathbf{I}_0$ は物体固定座標系(body frame)で定義されます。ワールド座標系(world frame)でトルクを計算した場合は、$\mathbf{I}_{world} = \mathbf{R} \mathbf{I}_0 \mathbf{R}^T$ に変換してから適用する必要があります。

3. タイムステップの選択 剛体の最高固有振動数 $\omega_{max}$ に対して、$\Delta t < 2 / \omega_{max}$ 条件（CFL類似条件）を満たすことで数値安定性が保証されます。高速回転する物体ほど、小さな $\Delta t$ が必要です。

4. クォータニオン補間 (SLERP) 2つの方位間を補間する場合は、線形補間(LERP)の代わりに球面線形補間(SLERP)を使用することで、等速回転が保証されます。

$\mathbf{q}(t) = \mathbf{q}_0 \left(\mathbf{q}_0^{-1} \mathbf{q}_1\right)^t$

剛体力学とFEMの結合は、FSI、DEM、マルチボディダイナミクスシミュレーションの基盤となります。クォータニオンベースの回転表現を正しく実装することで、ジンバルロックなしに任意の3次元回転を安定してシミュレーションできます。

回転軸と ω を変えてジンバルロックのない回転を確認しよう。