如何使用solve_ivp求解含时变角速度参数的姿态运动学ODE初值问题

我来帮你搞定这个问题！首先咱们先分析一下你遇到的错误原因：你之前在fun2里直接调用solve_ivp求解角速度的ODE，得到的omegax/omegay/omegaz是整个时间序列的数组（形状是(3,1000000)），但solve_ivp在求解姿态ODE的时候，是**逐时刻调用fun2**的——每次调用只传入单个time值和当前的euler状态，这时候你拿整个时间序列的omega去和单个时刻的欧拉角计算，自然会出现维度不匹配的广播错误。

正确的思路是：先提前把角速度的时变解做成插值函数，这样在姿态ODE的求解过程中，每到一个时刻，就能快速拿到对应时刻的omega值，而不用重复求解整个角速度ODE。具体步骤如下：

步骤1：预计算角速度的插值函数 #

首先求解角速度的ODE，利用solve_ivp的dense_output=True参数，它会返回一个可调用的插值对象（sol属性），能在任意时刻t返回对应的omega值：

import numpy as np
from scipy import integrate

# 先定义角速度的ODE函数（替换成你实际的omega微分方程）
def fun(time, omega):
    # 示例：这里写你原来求解omega的微分逻辑
    d_omega = ...  
    return d_omega

# 求解角速度ODE，得到插值函数
tspan = [0, 10]  # 替换成你的实际时间区间
omega0 = [omega0_x, omega0_y, omega0_z]  # 你的角速度初始值
t = np.linspace(tspan[0], tspan[1], 1000000)  # 你的时间采样点

sol_omega = integrate.solve_ivp(fun, tspan, omega0, t_eval=t, method='RK45', dense_output=True, rtol=1e-13, atol=1e-22)
# sol_omega.sol 就是可以在任意时刻调用的插值函数

步骤2：修改姿态ODE的函数，用插值函数获取对应时刻的omega #

现在修改fun2，让它接收这个插值函数，在每个时刻time调用插值函数得到当前的omega分量，再计算欧拉角的导数：

def fun2(time, euler, sol_omega):
    # 获取当前时刻的omega值：sol_omega.sol(time)返回形状为(3,)的数组
    omegax, omegay, omegaz = sol_omega.sol(time)
    
    # 处理欧拉角奇点问题（避免sin(theta)为0导致除零错误）
    sin_theta = np.sin(euler[1])
    if np.abs(sin_theta) < 1e-12:
        sin_theta = 1e-12  # 设置极小值避免除零，也可以根据需求调整
    
    # 计算欧拉角的导数
    dot1 = (omegax*np.sin(euler[2]) + omegay*np.cos(euler[2])) / sin_theta
    dot2 = omegax*np.cos(euler[2]) - omegay*np.sin(euler[2])
    dot3 = omegaz - (omegax*np.sin(euler[2]) + omegay*np.cos(euler[2])) / np.tan(euler[1])
    
    return np.array([dot1, dot2, dot3])

步骤3：求解姿态ODE #

调用solve_ivp时，用args参数把插值函数传递给fun2：

euler0 = [phi0, theta0, psi0]  # 你的欧拉角初始值
angles = integrate.solve_ivp(
    fun2, 
    tspan, 
    euler0, 
    t_eval=t, 
    method='RK45', 
    dense_output=True, 
    rtol=1e-13, 
    atol=1e-22,
    args=(sol_omega,)  # 将插值函数作为参数传入fun2
)

关键点说明 #

• 用dense_output得到的插值函数是高效的，它基于求解ODE时的内部节点做插值，比手动线性插值精度更高、速度更快。
• 欧拉角存在奇点问题（比如俯仰角θ=0或π时，sinθ=0），代码中加入了简单的防除零处理，你可以根据实际场景调整逻辑。
• 之前的错误本质是混淆了“整个时间序列的解”和“单个时刻的解”，现在通过插值函数，每个时刻只取对应点的omega值，维度完全匹配。

备注：内容来源于stack exchange，提问作者Mr Robot