Постоянные значения при интеграции системы ODE - PullRequest
Новая
       10

Постоянные значения при интеграции системы ODE

0 голосов
/ 02 июня 2019

Мне нужно решить систему ОДУ, состоящую из 13 уравнений (фактически 4 дифференциальных уравнения в форме матрицы), определить так: 

def rigidBoomrang(att_vec, t, I, invI, m, f, torque):

    q = att_vec[0:4]
    w = att_vec[4:7]
    v = att_vec[7:10]
    p = att_vec[10:13]

    qp = 0.5*np.dot(np.array([[0,w[2], -w[1], w[0]], [-w[2], 0, w[0], w[1]], [w[1], -w[0], 0, w[2]], [-w[0], -w[2], -w[2], 0]]), q)
    wp = np.dot(invI,(torque-np.cross(w, np.dot(I, w))))
    vp = f/m
    pp = v

    qp = np.array(qp)
    wp = np.array(wp)
    vp = np.array(vp)
    pp = np.array(p)


    return np.concatenate((qp, wp, vp, pp))

Дело в том, что два параметра (f икрутящий момент, который моделировал силу и ... крутящий момент) зависит от переменных, участвующих в системе ODES: сила и крутящий момент, используемые для интегрирования од в момент времени t + dt, зависят от значений w и v в момент временит, а интеграция q, w, v, p требует f и крутящего момента.

Это то, что я пробовал до сих пор (полный код): 

import numpy as np
from pyquaternion import Quaternion
import scipy.integrate as spi

def MatRotX3d(theta):
    return np.array([[1,0,0], [0, np.cos(theta), -np.sin(theta)], [0, np.sin(theta), np.cos(theta)]])

def MatRotY3d(theta):
    return np.array([[np.cos(theta), 0, -np.sin(theta)], [0, 1, 0], [np.sin(theta), 0, np.cos(theta)]])

def MatRotZ3d(theta):
    return np.array([[np.cos(theta), -np.sin(theta), 0], [np.sin(theta), np.cos(theta), 0], [0,0,1]])

def ezxzrmx(euler_angles):
    """Transformer angles d'une rotation Z-X-Z en une matrice de rotation"""
    return np.dot(np.dot(MatRotZ3d(euler_angles[2]), MatRotX3d(euler_angles[1])), MatRotZ3d(euler_angles[0]))

def rigidBoomrang(att_vec, t, I, invI, m, f, torque):

    q = att_vec[0:4]
    w = att_vec[4:7]
    v = att_vec[7:10]
    p = att_vec[10:13]

    qp = 0.5*np.dot(np.array([[0,w[2], -w[1], w[0]], [-w[2], 0, w[0], w[1]], [w[1], -w[0], 0, w[2]], [-w[0], -w[2], -w[2], 0]]), q)
    wp = np.dot(invI,(torque-np.cross(w, np.dot(I, w))))
    vp = f/m
    pp = v

    qp = np.array(qp)
    wp = np.array(wp)
    vp = np.array(vp)
    pp = np.array(p)


    return np.concatenate((qp, wp, vp, pp))

#paramètres du boomerang

L = 0.4
l = 0.035
b = 0.015
e = 0.005
Sz = L*l
Sc = l**2
S = 2*Sz+Sc
m = 0.075
sigma = m/S
I = np.array([[14,8,0], [8,14,0], [0,0,27]])*10**-4
invI = np.linalg.inv(I)

#coefficients aérodynamiques

roAir = 1.2
Caero = 0.5*roAir*S
Cz = 1.5

#entrées

betha0 = 180*np.pi/180
incl0 = 80*np.pi/180
theta0 = 90*np.pi/180

RZbetha = MatRotZ3d(betha0)
RXi = MatRotX3d(incl0)
RZtheta = MatRotZ3d(theta0)
Rot = np.dot(np.dot(RZbetha, RXi), RZtheta)

##CONDITIONS INITIALES

#angles initiaux 

eulzxz = np.array([betha0, incl0, theta0])

#quaternion initial

quat1 = Quaternion(matrix=ezxzrmx(eulzxz))
quat = quat1.elements

#vitesse angulaire initale

w_ang = [0,0,10*2*np.pi] 

#Vitesse initiale

V0 = 25
angle0 = 20*np.pi/180
v = V0*np.array([np.cos(angle0), 0, np.sin(angle0)])

#angle d'attaque

alpha = 0

#position initale

h0 = 2 #hauteur de lancer = 2m
p = [0,0,h0]


#matrice Rbi

Rbi = quat1.rotation_matrix
Rib = np.linalg.inv(Rbi)

#données relatives au temps

tstart = 0
tstep = 0.1 #pas d'intégration
tstop = 15
temps = np.linspace(tstart, tstop, int((tstop-tstart)/tstep))



#vecteurs initiaux

# ~ t = tstart
euler = eulzxz*180/np.pi
omeg = w_ang
vit = v
pos = p

attack = alpha #angle d'incidence, 0 a priori
momentang = I*omeg

#force de gravite

g = [0,0,-9.81*m]

#valeurs successives

T = []
VAL = []




##INTEGRATION 

for t in temps:

    if p[2] > 0: #si le boomerang ne touche pas le sol

        #matrice de rotation

        Rbi = quat1.rotation_matrix
        Rib = np.linalg.inv(Rbi)

        #forces aerodynamiques dans le plan du boomerang

        vb = np.dot(Rbi, v)
        Vx = vb[0]
        Vy = vb[1]

        f1 = np.array([0,0, Caero*Cz*(((w_ang[2]*(L-b) + Vy)**3 - Vy**3)/(3*w_ang[2]))])
        f2 = np.array([0,0, Caero*Cz*(((w_ang[2]*(L-b) + Vx)**3 - Vx**3)/(3*w_ang[2]))])

        fb = f1+f2

        fRi = np.dot(Rib,fb) #force dans le référentiel initial

        f = fRi+g #résultantes totales

        #couples issus des forces aérodynamiques

        P1 = [3*(L-b)/4, -b-l/2, 0] #pt d'application de f1
        P2 = [-b-l/2, 3*(L-b)/4, 0] #pt d'application de f2

        torque = np.cross(P1, f1)+np.cross(P2,f2)

        #vecteur d'attitude inital


        att_vec = np.concatenate((quat, w_ang, v, p))

        #paramètres de résolution du système d'ODE

        tspan = [t]

        ode = spi.odeint(rigidBoomrang, att_vec, tspan, args=(I, invI, m, f, torque))

        quat = np.array(ode[0][0:4])
        w_ang = np.array(ode[0][4:7])
        v = np.array(ode[0][7:10])
        p = np.array(ode[0][10:13])

Когда я отображаюПри каждой интеграции я вижу, что ничего не было интегрировано: каждая переменная сохраняет свое начальное значение.Я думаю, это потому, что параметры не являются постоянными.Тем не менее, я не могу найти обходного пути к этому.Есть ли простой способ справиться с этим?

Заранее спасибо

...