代码求解拉格朗日方程组时卡顿挂起，求解决方法

问题描述 #

编写了一段求解陀螺运动的Python代码，使用SymPy推导拉格朗日方程后，调用smp.solve([LE1, LE2, LE3], (the_dd, phi_dd, psi_dd))求解线性方程组时出现卡顿挂起。代码能正常输出solving lagrangian及之前的日志，但此后完全停滞。完整代码如下：

import numpy as np
import sympy as smp
import matplotlib.pyplot as plt
from scipy.integrate import odeint
import plotly.graph_objects as go
from IPython.display import HTML
import pandas as pd
print("code started")

#to find the inertia tensor of the object we are going to try to draw out the shape of the top as a function of theta, for the purposes of testing this shape is going to be a cube
#defining the function for against the z axis in the domin pi/4 to pi/2

#now we want numpy to generate points of that function in our first domain for theta
#this test is for a cube and we have aldready computed to MOI tensor by hand in book so
# defining M and r 
M = 0.1
r = 5
scale = M * r**2
#I = scale * np.array([
#    [2/3, -1/4, -1/4],
#    [-1/4, 2/3, -1/4],
#    [-1/4, -1/4, 2/3]
#])
#here we have defined our inertia tensor, now to move on to the motion itself
t, h, g = smp.symbols('t h g', real=True)
#the above defines time height of center of mass and gravity as symbols and says that they are real
the, phi, psi = smp.symbols(r'\theta \phi \psi', cls=smp.Function) 
#this defines the euler angles as functions of time
print("defining functions")
the = the(t)
phi = phi(t)
psi = psi(t)
#now we can define their derivatives and Second derivatives w.r.t time
print("finding derivatives")
the_d = smp.diff(the,t)
phi_d = smp.diff(phi,t)
psi_d = smp.diff(psi,t)
the_dd = smp.diff(the_d,t)
phi_dd = smp.diff(phi_d,t)
psi_dd = smp.diff(psi_d,t)
#now we define the transformation matrices of the rotation to be able to transform the vectors to the cartesian coordinate system in terms of z y and x
#since the vectors themselves are being rotatend instead of the coordinate axes the sign convention for sin is opposite
print("defining transformation matrices")
R3 = smp.Matrix([[smp.cos(psi), -smp.sin(psi), 0],
                 [smp.sin(psi), smp.cos(psi), 0],
                 [0, 0, 1]])
R2 = smp.Matrix([[1,0,0],
                 [0,smp.cos(the),-smp.sin(the)],
                 [0,smp.sin(the),smp.cos(the)]])

R1 = smp.Matrix([[smp.cos(phi),-smp.sin(phi),0],
                 [smp.sin(phi),smp.cos(phi),0],
                 [0,0,1]])

# and then we find the full rotation matrix as R(product of all)
R = R1*R2*R3
#now we define angular momentum
print("defining angular momentum")
omega = smp.Matrix([phi_d*smp.sin(the)*smp.sin(psi)+the_d*smp.cos(psi),
                    phi_d*smp.sin(the)*smp.cos(psi)-the_d*smp.sin(psi),
                    phi_d*smp.cos(the)+psi_d])
#although we have the numerical tensor now we need to define symbolic elements to perform symbolic integration
Ixx, Iyy, Izz, Ixy, Iyz, Ixz = smp.symbols('I_{xx}, I_{yy}, I_{zz}, I_{xy}, I_{yz}, I_{xz}', real=True)
I = smp.Matrix([[Ixx,Ixy,Ixz],[Ixy,Iyy,Iyz],[Ixz,Iyz,Izz]])
#now we move on to defining the lagrangian using the simplified one where we divide by mL^2 sicne if you multiply the lagrangian by a constant the equations remain the same
print("defining lagrangian")
T = smp.Rational(1,2)*omega.T.dot(I*omega).simplify()
V = g*h*smp.cos(the)
L = T-V
#now to obtain the lagrangian equations for all three angles
print("defining lagrangian equatiosn")
LE1 = smp.diff(L, the) - smp.diff(smp.diff(L, the_d),t)
LE1 = LE1.simplify()
LE2 = smp.diff(L, phi) - smp.diff(smp.diff(L, phi_d),t)
LE2 = LE2.simplify()
LE3 = smp.diff(L, psi) - smp.diff(smp.diff(L, psi_d),t)
LE3 = LE3.simplify()
#now since all of their secod derivatives are linear we can solve for them using the LEs
print("solving lagrangian ")
sols=smp.solve([LE1, LE2, LE3], (the_dd, phi_dd, psi_dd),
            simplify=False, rational=False)
#however since python can only work with first order derivatives we need to change our variable definitions  and we change the sympy symbolic functions into numerical python functions
print("Converting to numerical functions")
dz1dt_f = smp.lambdify((g, h, Ixx,Iyy,Izz,Ixy,Iyz,Ixz, the, phi, psi, the_d, phi_d, psi_d), sols[the_dd])
dthedt_f = smp.lambdify(the_d, the_d)
dz2dt_f = smp.lambdify((g, h, Ixx,Iyy,Izz,Ixy,Iyz,Ixz, the, phi, psi, the_d, phi_d, psi_d), sols[phi_dd])
dphidt_f = smp.lambdify(phi_d, phi_d)
dz3dt_f = smp.lambdify((g, h, Ixx,Iyy,Izz,Ixy,Iyz,Ixz, the, psi, phi, the_d, phi_d, psi_d), sols[psi_dd])

dpsidt_f = smp.lambdify(psi_d, psi_d)
# now given the arguments we will be able to get the vales of the second order differential equations
# now we write as a system of equations that takes in all the angles and their first derivatives and returns their time derivative
def dSdt(s, t):
    the, z1, phi, z2, psi, z3 = s
    return [
        dthedt_f(z1),
        dz1dt_f(g, h, Ixx,Iyy,Izz,Ixy,Iyz,Ixz,the,phi,psi,z1,z2,z3),
        dphidt_f(z2),
        dz2dt_f(g, h, Ixx,Iyy,Izz,Ixy,Iyz,Ixz,the,phi,psi,z1,z2,z3),
        dpsidt_f(z3),
        dz3dt_f(g, h, Ixx,Iyy,Izz,Ixy,Iyz,Ixz,the,phi,psi,z1,z2,z3),
    ]
Ixx = smp.Rational(2,3) * scale
Iyy = smp.Rational(2,3) * scale
Izz = smp.Rational(2,3) * scale
Ixy = smp.Rational(-1,4) * scale
Iyz = smp.Rational(-1,4) * scale
Ixz = smp.Rational(-1,4) * scale
#I = np.array([[Ixx, Ixy, Ixz],[Ixy, Iyy, Iyz],[Ixz, Iyz, Izz]])
g = 9.8/0.05
#in our current case 2.5
h = 2.5
#now we define our initial conditions and solve the differential equations for hte first 2 seconds
t = np.linspace(0, 2, 10000)
print("Starting integration...")
ans = odeint(dSdt, y0=[np.pi/4, 0, 0, 10, 0, 120*np.pi], t=t)
print("Integration done!")

print(ans.T)

问题原因 #

1. 符号表达式复杂度极高：由于惯性张量包含非对角元，加上欧拉角的三角函数项，生成的拉格朗日方程表达式嵌套层级深、项数极多，SymPy的通用solve函数处理这类线性系统时会陷入大量符号运算，导致计算资源耗尽。
2. 不必要的提前简化：代码中对LE1/LE2/LE3执行了.simplify()操作，SymPy的简化算法可能会将表达式转化为更复杂的形式（比如引入分式、嵌套三角函数），进一步增加求解难度。
3. 通用求解函数效率低下：smp.solve是通用求解器，针对线性方程组场景，不如直接用线性代数方法（如矩阵求逆）高效，因为通用求解器会额外处理非线性情况的分支逻辑。

解决建议 #

1. 移除拉格朗日方程的提前简化 #

删除LE1 = LE1.simplify()、LE2 = LE2.simplify()、LE3 = LE3.simplify()这三行代码，保留原始的微分表达式，减少SymPy的计算负担。

2. 改用线性代数方法求解 #

由于待求解的是线性方程组，可以将方程整理为A * [the_dd, phi_dd, psi_dd]^T = b的形式，直接通过矩阵求逆或线性系统求解器得到结果，效率远高于通用solve函数。修改后的求解代码如下：

print("solving lagrangian ")
# 将方程组整理为线性形式：A * x = b，x = [the_dd, phi_dd, psi_dd]
x = smp.Matrix([the_dd, phi_dd, psi_dd])
# 提取系数矩阵A和常数项向量b
A, b = smp.linear_eq_to_matrix([LE1, LE2, LE3], x)
# 求解线性方程组
sols_matrix = A.inv() * b
# 转换为字典形式，保持后续代码兼容
sols = {
    the_dd: sols_matrix[0],
    phi_dd: sols_matrix[1],
    psi_dd: sols_matrix[2]
}

3. 提前代入惯性张量的数值 #

如果不需要保留惯性张量的符号形式，可以在生成拉格朗日方程之前就代入Ixx/Iyy等的具体值，大幅减少符号变量数量，降低计算复杂度。修改示例：

# 先定义数值化的惯性张量参数
scale = M * r**2
Ixx_val = smp.Rational(2,3) * scale
Iyy_val = smp.Rational(2,3) * scale
Izz_val = smp.Rational(2,3) * scale
Ixy_val = smp.Rational(-1,4) * scale
Iyz_val = smp.Rational(-1,4) * scale
Ixz_val = smp.Rational(-1,4) * scale

# 定义惯性张量时直接代入数值
I = smp.Matrix([
    [Ixx_val, Ixy_val, Ixz_val],
    [Ixy_val, Iyy_val, Iyz_val],
    [Ixz_val, Iyz_val, Izz_val]
])

# 后续生成拉格朗日方程时就无需处理符号形式的惯性张量参数

4. 优化omega表达式的结构 #

检查omega的定义是否符合欧拉角的旋转顺序（当前代码用的是Z-Y-X顺序），确保表达式正确的前提下，可以尝试拆分表达式、合并同类项，减少后续微分运算的复杂度。

修改后代码运行效果 #

采用上述修改后，线性方程组求解的速度会大幅提升，能够顺利完成符号推导并进入数值积分阶段。

内容的提问来源于stack exchange，提问作者V_V_V_