防止旋转漂移

Question

我正在 3D 中进行刚体模拟。现在我有一个旋转的精度问题。最终，物体的方向（没有外力）收敛到转动惯量最小的轴周围。降低dt 会有所帮助，但不会太多。有没有办法减少这种漂移？

这是我目前的算法：

给定：

• 角动量L
• 在局部空间中围绕主轴的转动惯量I
• 模拟时间步长dt
• 初始方向矩阵O

步骤：

• 根据角动量计算角速度W。 L = I * W => W = Inverse(I) * L。因为只知道局部转动惯量，所以实际公式为W = O * Inverse(I) * Inverse(O) * L。
• 计算并应用旋转变化dO - 关于归一化的旋转矩阵 W 带角度Length(W) * dt。

为了防止正交性问题，方向用四元数来表示，但算法在其他方面保持不变。

我的想法是可能有一种方法可以利用能量守恒。在分析我的一个测试示例时，我发现旋转能量随着身体旋转而漂移（增加）。由于我使用动量守恒来计算角速度，因此使用能量守恒可以将所有数值误差推向其他“维度”。我想那将是时间，我认为观看起来不那么痛苦。但我什至不知道从哪里开始。

Answer 1

我可以分享我的算法。它是一种辛算法，基于将惯性矩阵拆分为两个惯性矩阵之和，每个惯性矩阵有两个相等的轴，这导致将角动量微分方程（所谓的欧拉方程）的原始系统拆分为两个系统具有两个相等惯性轴的物体的微分方程。每个系统都可以显式求解，然后将两个系统的相流以跳跃式的方式组合起来。因此，该算法与原系统一样保持角动量，准能量守恒，即能量几乎守恒，能量不耗散，因此不存在角速度漂移。这是因为该算法在角动量球面上保留了所谓的辛结构，这在几何上意味着该算法保留了该球面上的面积。

import math
import numpy as np

def Rot_3(m):
    cs = math.cos(m)
    sn = math.sin(m)
    return np.array([
            [cs, -sn, 0],
            [sn,  cs, 0],
            [ 0,   0, 1]])

def Rot_1(m):
    cs = math.cos(m)
    sn = math.sin(m)
    return np.array([
            [1,  0,   0],
            [0, cs, -sn],
            [0, sn,  cs]])
        
def vector_to_matrix(Vector):
    Matrix = np.array([ 0, - Vector[2],  Vector[1]],
                      [ 0,           0, -Vector[0]],
                      [ 0,           0,         0 ])
    return Matrix - Matrix.T

def Angular_Momentum_step(M_input, k_23, k_21, t_step):
    M_step = Rot_3(t_step*k_23 * M_input[2]/2).dot(M_input)
    M_step = Rot_1(t_step*k_21 * M_step[0]).dot(M_step)
    M_step = Rot_3(t_step*k_23 * M_step[2]/2).dot(M_step)
    return M_step

def Rotation_step(M, I_inv, t_step):
    O = I_inv*M
    angle = math.sqrt(O.dot(O))
    O = O / angle
    angle = t_step*angle
    O = vector_to_matrix(O)
    U = np.array([[1,0,0],[0,1,0],[0,0,1]])
    U = U + math.sin(angle)*O + (1-math.cos(angle))*(O.dot(O))
    return U 

def Propagate_Angular_Momentum(M_initial, Inertia, n_iterations, t_step):
    I_inv = np.array([1/Inertia[0], 1/Inertia[1], 1/Inertia[2]])
    k_23 = I_inv[1]-I_inv[2]
    k_21 = I_inv[1]-I_inv[0]
    M_evolution = np.empty((3, n_iterations), dtype=float)
    M_evolution[:,0] = M_initial.copy()
    for i in range((n_iterations-1)):
        M_evolution[:,i+1] = Angular_Momentum_step(M_evolution[:,i], k_23, k_21, t_step)
    return M_evolution

def Propagate_Rotation(Body_initial, Moment_initial, Inertia, n_iterations, t_step):
    I_inv = np.array([1/Inertia[0], 1/Inertia[1], 1/Inertia[2]])
    k_23 = I_inv[1]-I_inv[2]
    k_21 = I_inv[1]-I_inv[0]
    Moment_evolution = np.empty((3, n_iterations), dtype=float)
    Moment_evolution[:,0] = Moment_initial.copy()
    Body_evolution = np.empty((3, n_iterations+1), dtype=float)
    Body_evolution[:,0] = Body_initial.dopy()
    for i in range((n_iterations-1)):
        Moment_evolution[:,i+1] = Angular_Momentum_step(Moment_evolution[:,i], k_23, k_21, t_step)
        Body_evolution[:,i+1] = Rotation_step(Moment_evolution[:,i], I_inv, t_step)
        Body_evolution[:,i+1] = Body_evolution[:,i+1].dot(Body_evolution[:,i])
    return Body_evolution, Moment_evolution

# a test example, set up the initial angular velocity and the 
#  
I1 = 2.35
I2 = 2.0
I3 = 1.0
I = np.array([I1, I2, I3])
O = np.array([0, 2, 0.95])
O = Rot_3(-math.pi*(30)/180).dot(O)
M = I*O
dt = 0.3
n_iter=500

# propagate the system
Momenta = Propagate_Angular_Momentum(M, I, n_iter, dt)

# plot
fig = plt.figure()
ax = fig.add_subplot(1,2,1,projection='3d')
ax.set_xlim((-4, 4))
ax.set_ylim((-4, 4))
ax.set_zlim((-4, 4))

ax.plot(Momenta[0,:], Momenta[1,:], Momenta[2,:], 'r-')


plt.show()

观察角动量总是穿过总是在二维球面上的红色曲线。曲线看起来是封闭的，没有任何坐标轴向外盘旋，与车身固定框架中的惯性轴对齐。