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

四面体の自由度は節点12個から剛体の6個へ落ちる。Δtの上限は2/ω_maxで、途中の姿勢を球面上に残すのはSLERPです。

剛体力学の基本方程式

FSI や DEM で剛体に求めるのは、任意の2点間距離が変わらないことです。変形はありません。その距離を数値で守るとき、先に崩れるのは回転の表し方です。

粒子 $i$ の速度は、重心の並進と回転の和です。

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

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

軸スライダーと ω を変えると、角速度の大きさが同じでも回転軸が変わり、一周にかかる時間が変わります。

axis x 0.40axis y 1.00axis z 0.20ω 1.2

주황 화살표가 회전축. 사원수 q를 직접 곱해 매 프레임 큐브를 회전시킨다 — 짐벌락 없이 안정.

ω を上げると姿勢が速く回ります。軸を体の対角に倒しても、角度が一つの軸に張りつかないかを見ます。

線形運動量と角運動量

ニュートンの第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}$ はクォータニオン成分によって明示的に表現されます。

\mathbf{R} = \begin{pmatrix} 1 - 2(q_2^2+q_3^2) & 2(q_1 q_2 - q_0 q_3) & 2(q_1 q_3 + q_0 q_2) \\ 2(q_1 q_2 + q_0 q_3) & 1 - 2(q_1^2+q_3^2) & 2(q_2 q_3 - q_0 q_1) \\ 2(q_1 q_3 - q_0 q_2) & 2(q_2 q_3 + q_0 q_1) & 1 - 2(q_1^2+q_2^2) \end{pmatrix}

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

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

$I_{jk}$ の対角は $m(|\mathbf{r}|^2-r_j^2)$ 、非対角は $-m r_j r_k$ です。質量が $x$ 軸上だけなら $r_y=r_z=0$ なので $I_{xx}=0$ 、 $I_{yy}=I_{zz}=m x^2$ です。その軸には回す腕がありません。 $\mathbf{I}_0$ はこの和を物体座標に一度だけ蓄えた値です。世界座標では $\mathbf{R}\mathbf{I}_0\mathbf{R}^T$ を作り直すだけで足ります。物体座標のテンソルに世界座標の角速度をそのまま掛けると、トルクの軸がずれます。座標を移すときの合同変換は、構成テンソルの座標変換で要素剛性にも同じ形で現れます。

$\theta=0$ なら $\cos(\theta/2)=1$ 、 $\sin(\theta/2)=0$ で $\mathbf{q}=(1,0,0,0)$ です。コードが入れる初期値と同じです。 $\theta=\pi$ なら $\cos(\pi/2)=0$ 、 $\sin(\pi/2)=1$ でスカラーが消え、ベクトル部は軸 $\hat{\mathbf{n}}$ だけになります。 $\theta=2\pi$ なら $\cos\pi=-1$ で $\mathbf{q}=(-1,0,0,0)$ です。回転は恒等と同じで、符号だけが逆です。一周をクォータニオンは二度覆います。 $\mathbf{R}$ の $(1,1)$ 成分は $1-2(q_2^2+q_3^2)$ なので、この恒等値では 1 になり、非対角は 0 です。 $\dot{\mathbf{q}}$ の前の $1/2$ は、角度の式の $\theta/2$ と同じ因子です。そのまま足すと $|\mathbf{q}|$ が 1 からずれます。線形四面体は節点 4、節点あたり変位 3 で 12 自由度です。剛体拘束は並進 3 と回転 3、合計 6 に減らします。 $\sum_i L_i=1$ は座標が残る条件であって、その 6 個を単独では作りません。

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
    ])

正規化を省くとRは回転ではなくなる#

毎ステップ $|\mathbf{q}|$ を 1 に戻さないと、 $\mathbf{R}$ の列が単位長さを失います。コードの q /= norm(q) がその割り算です。トルクを世界座標で求め、 $\mathbf{I}_0$ をそのまま掛けると $\boldsymbol{\alpha}$ の軸がずれます。世界座標のテンソルは $\mathbf{I}_{world} = \mathbf{R} \mathbf{I}_0 \mathbf{R}^T$ にしてから $\mathbf{L}=\mathbf{I}\boldsymbol{\omega}$ と揃います。陽的積分の上限は $\Delta t < 2 / \omega_{max}$ です。 $\omega_{max}$ が大きいほど間隔を先に縮めます。

二つの姿勢を直線でつなぐと、経路は単位球の内側に沈みます。 $t=0$ で $\mathbf{q}_0$ 、 $t=1$ で $\mathbf{q}_1$ に着くのが SLERP です。

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

線形補間はその球面を守りません。回転の自由度を要素の変形と一緒に数えると、薄いシェルのせん断ロッキングのように拘束が運動を飲み込みます。剛体なら自由度は 12 ではなく 6 です。