防止旋转漂移

问题描述

我正在 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。

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

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

解决方法

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

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

def Rot_1(m):
    cs = math.cos(m)
    sn = math.sin(m)
    return np.array([
            [1,[0,-sn],sn,cs]])
        
def vector_to_matrix(Vector):
    Matrix = np.array([ 0,- Vector[2],Vector[1]],-Vector[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,1,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],t_step)
    return M_evolution

def Propagate_Rotation(Body_initial,Moment_initial,1/Inertia[2]])
    k_23 = I_inv[1]-I_inv[2]
    k_21 = I_inv[1]-I_inv[0]
    Moment_evolution = np.empty((3,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[:,t_step)
        Body_evolution[:,i+1] = Rotation_step(Moment_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,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()

请注意，角动量始终穿过始终位于 2D 球体上的红色曲线。曲线看起来是封闭的，没有任何一个坐标轴向外螺旋，这些坐标轴与身体固定框架中的惯性轴对齐。

physics physics rigid-bodies