刚体动力学中的有限元法：基于四元数的旋转处理与惯性张量

刚体动力学的基本方程

在 FSI 或 DEM 里，刚体只要守住任意两点的距离。没有变形。数值上先垮掉的，通常是旋转的写法。

粒子 $i$ 的速度是质心平动加上转动。

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

$\mathbf{v}_{cm}$ 是质心速度，$\boldsymbol{\omega}$ 是角速度，$\mathbf{r}_i$ 是从质心到粒子的位移。

轴滑块和 ω 一动，角速度大小可以不变，转轴和转完一圈的时间会变。

把 ω 调大，姿态转得更快。把轴扳到体对角，看角度会不会锁死在某一根坐标轴上。

线动量与角动量

根据牛顿第二定律，总线动量为：

$\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)$

两个四元数 $\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}$

$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) 中离散刚体的位移场时，四面体单元的形函数 (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$

线性插值守不住这个球面。把转动自由度和单元变形混在一起数，约束会吃掉运动，和 薄壳的剪切锁定 里板越薄越僵是同一类账。刚体的自由度是 6，不是 12。