我正在尝试求解微分方程:
m(t) = M(x)x'' + C(x, x') + B x'
where x and x'是具有 2 个条目的向量,表示动态系统中的角度和角速度。中号(x) 是一个 2x2 矩阵,它是 theta 分量的函数,C 是一个 2x1 向量,它是 theta 和 theta' 的函数,B 是一个 2x2 常量矩阵。 m(t) 是一个 2*1001 数组,包含在 1001 个时间步长处施加到两个关节中的每个关节的扭矩,我想计算角度的演变作为这 1001 个时间步长的函数。
我已将其转换为标准形式:
x'' = M(x)^-1 (m(t) - C(x, x') - B x')
然后代入y_1 = x and y_2 = x'给出一阶线性方程组:
y_2 = y_1'
y_2' = M(y_1)^-1 (m(t) - C(y_1, y_2) - B y_2)
(我在 x 和 y 的代码中使用了 theta 和 phi)
def joint_angles(theta_array, t, torques, B):
phi_1 = np.array([theta_array[0], theta_array[1]])
phi_2 = np.array([theta_array[2], theta_array[3]])
def M_func(phi):
M = np.array([[a_1+2.*a_2*np.cos(phi[1]), a_3+a_2*np.cos(phi[1])],[a_3+a_2*np.cos(phi[1]), a_3]])
return np.linalg.inv(M)
def C_func(phi, phi_dot):
return a_2 * np.sin(phi[1]) * np.array([-phi_dot[1] * (2. * phi_dot[0] + phi_dot[1]), phi_dot[0]**2])
dphi_2dt = M_func(phi_1) @ (torques[:, t] - C_func(phi_1, phi_2) - B @ phi_2)
return dphi_2dt, phi_2
t = np.linspace(0,1,1001)
initial = theta_init[0], theta_init[1], dtheta_init[0], dtheta_init[1]
x = odeint(joint_angles, initial, t, args = (torque_array, B))
我收到错误,无法使用 t 数组对扭矩进行索引,这非常有意义,但是我不确定如何让它在每个时间步使用扭矩的当前值。
我还尝试将 odeint 命令放入 for 循环中,并一次仅在一个时间步骤对其进行评估,使用函数的解作为下一个循环的初始条件,但是该函数只是返回初始条件,这意味着每个循环都是完全相同的。这让我怀疑我在实施标准表格时犯了一个错误,但我无法弄清楚它是什么。然而,最好不必每次都在 for 循环中调用 odeint 求解器,而是将其全部作为一个来完成。
如果有帮助的话,我的初始条件和常数值是:
theta_init = np.array([10*np.pi/180, 143.54*np.pi/180])
dtheta_init = np.array([0, 0])
L_1 = 0.3
L_2 = 0.33
I_1 = 0.025
I_2 = 0.045
M_1 = 1.4
M_2 = 1.0
D_2 = 0.16
a_1 = I_1+I_2+M_2*(L_1**2)
a_2 = M_2*L_1*D_2
a_3 = I_2
感谢您的帮助!