Question

我有一个求解微分方程组的函数。我想在系统求解后绘制结果变量之一（theta_i）。此变量来自等式：

np.arctan2(g1,g2)

，因此它的值假定在-pi和pi之间。如果在计算后立即将theta_i打印到函数中，则结果看起来正确（值介于-0.2和0.5之间）。但是当我将theta_i保存在向量u中后打印theta_i时，结果是完全错误的（它继续上升）。奇怪的是，我以相同的方式保存了来自函数的所有其他变量（如u中的数组），但是当我绘制它们时，它们的值看起来正确。我该如何解决？我是python的新手。我使用的是Python 2.7.12。预先谢谢你。

import numpy as np 
from scipy.integrate import odeint 
import matplotlib.pyplot as plt

added_mass_x = 0.03 # kg 
added_mass_y = 0.04 
mb = 0.3 # kg 
m1 = mb-added_mass_x 
m2 = mb-added_mass_y 
l1 = 0.07 # m 
l2 = 0.05 # m 
J = 0.00050797 # kgm^2 
Sa = 0.0110 # m^2 
Cd = 2.44 
Cl = 3.41 
Kd = 0.000655 # kgm^2 
r = 1000 # kg/m^3

c1 = 0.5*r*Sa*Cd 
c2 = 0.5*r*Sa*Cl 
c3 = 0.5*mb*(l1**2) 
c4 = Kd/J 
c5 = (1/(2*J))*(l1**2)*mb*l2 
c6 = (1/(3*J))*(l1**3)*mb

theta_0 = 10*(np.pi/180) # rad 
theta_A = 20*(np.pi/180) # rad 
f = 2 # Hz

t = np.linspace(0,100,8000) # s

def direct(u,t):
    vcx = u[0]
    vcy = u[1]
    wz = u[2]
    psi = u[3]
    x = u[4]
    y = u[5]
    vcx_i = u[6]
    vcy_i = u[7]
    psi_i = u[8]
    wz_i = u[9]
    theta_i = u[10]
    theta_deg_i = u[11]

    # Subsystem 1
    omega = 2*np.pi*f # rad/s
    theta = theta_0 + theta_A*np.sin(omega*t) # rad
    theta_deg = (theta*180)/np.pi # deg
    thetadotdot = -(omega**2)*theta_A*np.sin(omega*t) # rad/s^2

    # Subsystem 2
    vcxdot = (m2/m1)*vcy*wz-(c1/m1)*vcx*np.sqrt((vcx**2)+(vcy**2))+(c2/m1)*vcy*np.sqrt((vcx**2)+(vcy**2))*np.arctan2(vcy,vcx)-(c3/m1)*thetadotdot*np.sin(theta)
    vcydot = -(m1/m2)*vcx*wz-(c1/m2)*vcy*np.sqrt((vcx**2)+(vcy**2))-(c2/m2)*vcx*np.sqrt((vcx**2)+(vcy**2))*np.arctan2(vcy,vcx)+(c3/m2)*thetadotdot*np.cos(theta)
    wzdot = ((m1-m2)/J)*vcx*vcy-c4*wz*wz*np.sign(wz)-c5*thetadotdot*np.cos(theta)-c6*thetadotdot
    psidot = wz

    # Subsystem 3
    xdotdot = vcxdot*np.cos(psi)-vcx*np.sin(psi)*wz+vcydot*np.sin(psi)+vcy*np.cos(psi)*wz # m/s^2
    ydotdot = -vcxdot*np.sin(psi)-vcx*np.cos(psi)*wz+vcydot*np.cos(psi)-vcy*np.sin(psi)*wz # m/s^2
    xdot = vcx*np.cos(psi)+vcy*np.sin(psi) # m/s
    ydot = -vcx*np.sin(psi)+vcy*np.cos(psi) # m/s

    # Subsystem 4
    vcx_i = xdot*np.cos(psi_i)-ydot*np.sin(psi_i)
    vcy_i = ydot*np.cos(psi_i)+xdot*np.sin(psi_i)
    psidot_i = wz_i
    vcxdot_i = xdotdot*np.cos(psi_i)-xdot*np.sin(psi_i)*psidot_i-ydotdot*np.sin(psi_i)-ydot*np.cos(psi_i)*psidot_i
    vcydot_i = ydotdot*np.cos(psi_i)-ydot*np.sin(psi_i)*psidot_i+xdotdot*np.sin(psi_i)+xdot*np.cos(psi_i)*psidot_i

    g1 = -(m1/c3)*vcxdot_i+(m2/c3)*vcy_i*wz_i-(c1/c3)*vcx_i*np.sqrt((vcx_i**2)+(vcy_i**2))+(c2/c3)*vcy_i*np.sqrt((vcx_i**2)+(vcy_i**2))*np.arctan2(vcy_i,vcx_i)
    g2 = (m2/c3)*vcydot_i+(m1/c3)*vcx_i*wz_i+(c1/c3)*vcy_i*np.sqrt((vcx_i**2)+(vcy_i**2))+(c2/c3)*vcx_i*np.sqrt((vcx_i**2)+(vcy_i**2))*np.arctan2(vcy_i,vcx_i)

    A = 12*np.sin(2*np.pi*f*t+np.pi) # eksiswsi tail_frequency apo simulink
    if A>=0.1:
        wzdot_i = ((m1-m2)/J)*vcx_i*vcy_i-c4*wz_i**2*np.sign(wz_i)-c5*g2-c6*np.sqrt((g1**2)+(g2**2))
    elif A<-0.1:
        wzdot_i = ((m1-m2)/J)*vcx_i*vcy_i-c4*wz_i**2*np.sign(wz_i)-c5*g2+c6*np.sqrt((g1**2)+(g2**2))
    else:
        wzdot_i = ((m1-m2)/J)*vcx_i*vcy_i-c4*wz_i**2*np.sign(wz)-c5*g2


    if g2>0:
        theta_i = np.arctan2(g1,g2)
    elif g2<0 and g1>=0:
        theta_i = np.arctan2(g1,g2)-np.pi
    elif g2<0 and g1<0:
        theta_i = np.arctan2(g1,g2)+np.pi
    elif g2==0 and g1>0:
        theta_i = -np.pi/2
    elif g2==0 and g1<0:
        theta_i = np.pi/2
    elif g1==0 and g2==0:
        theta_i = 0
    theta_deg_i = (theta_i*180)/np.pi
    #print theta_deg_i


    return [vcxdot, vcydot, wzdot, psidot, xdot, ydot, vcxdot_i, vcydot_i, psidot_i, wzdot_i, theta_i, theta_deg_i]

# arxikes synthikes 
vcx_0 = 0.1257 
vcy_0 = 0 
wz_0 = 0 
psi_0 = 0 
x_0 = 0 
y_0 = 0 
vcx_i_0 = 0.1257 
vcy_i_0 = 0 
psi_i_0 = 0 
wz_i_0 = 0 
theta_i_0 = 0.1745 
theta_deg_i_0 = 9.866 
u0 = [vcx_0, vcy_0, wz_0, psi_0, x_0, y_0, vcx_i_0, vcy_i_0, psi_i_0, wz_i_0, theta_i_0, theta_deg_i_0]

u = odeint(direct, u0, t, tfirst=False)

vcx = u[:,0] 
vcy = u[:,1] 
wz = u[:,2] 
psi = u[:,3] 
x = u[:,4] 
y = u[:,5] 
vcx_i = u[:,6] 
vcy_i = u[:,7] 
psi_i = u[:,8] 
wz_i = u[:,9] 
theta_i = u[:,10] 
theta_deg_i = u[:,11] 
print theta_i

plt.figure(17) 
plt.plot(t,theta_i,'r-',linewidth=1,label='theta_i') 
plt.xlabel('t [s]') 
plt.title('theta_i [rad] (Main body CF)') 
plt.legend() 
plt.show()

Answer 1

结果对我来说似乎不错，只需确保您正确确定了差异的顺序（dY，dx）。点相对于原点的角度的例子。

b= np.array([[10,0], [10, 5], [10,10], [5, 10], [0,10]])
a = np.array([0,0])
ba = b - a
ang = np.degrees(np.arctan2(ba[:, 1], ba[:, 0]))
ang
  array([ 0.        , 26.56505118, 45.        , 63.43494882, 90.        ])

函数的输出未正确存储

1 个答案: