Pythonでフライトシミュレーションをする第2回

前回は縦の運動を扱ったので，その続きとして横の運動を実装します．

そもそも何をやっているかというと，航空機の運動が釣り合い飛行からの微小擾乱であると仮定して，航空機の運動を互いに連成しないピッチ運動(機首上げ下げ)とロール&ヨー運動の2つのグループに分けています．これは線形化の恩恵で，非線形のままだとこれらが互いに連成してしまいます．

前回，短周期モード，フゴイドモードの話を最後にしたので，補足しておくと，航空機の運動は数学的には以下の5つのモードを用いると表現できます．

縦の運動

短周期モード
長周期モード(フゴイドモード)

横の運動

ダッチロールモード
ロールモード
スパイラルモード

古典力学の連成振動の振動モードみたいにこれらの5つのモードを重ね合わせればいかなる運動も表現できるということです（すげえ）．最終回でこれらのモードをシミュレーションで確かめるかもです．

パイロットはこの運動モードを熟知していないと，運動や振動を修正しようとしてもかえってこれらの運動を増大させるような操縦をしてしまうことがあって，PIO(Pilot Induced Oscillation)と呼ばれています．実際にPIOが事故につながった例もいくつかあるようです．

1. 線形化された横の運動方程式

縦と同様です．

状態量を $\mathbf{y}(t)=\begin{bmatrix}\beta & p & r & \phi & \psi \end{bmatrix}^{T}$

制御量を $\mathbf{u}=\begin{bmatrix}\delta_{a} & \delta_{r} \end{bmatrix}^{T}$

とおくと，

$\dot{\mathbf{y}}=\mathbf{A}\mathbf{y}+\mathbf{B}\mathbf{u}$

$\mathbf{A}=\begin{bmatrix} Y_{\beta}/U_{0} &Y_{p}/U_{0} & (Y_{r}/U_{0}-1) & g/U_{0} & 0 \\ L'_{\beta} & L'_{p} & L'_{r} & 0 & 0 \\ N'_{\beta} & N'_{p} & N'_{r} & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \end{bmatrix}, \mathbf{B}=\begin{bmatrix} 0 & Y_{\delta_{r}}/U_{0} \\ L'_{\delta_{a}} & L'_{\delta_{r}}\\ N'_{\delta_{a}} & N'_{\delta_{r}}\\ 0 & 0 \\ 0 & 0 \end{bmatrix}$

と記述できます．

2. 実装！

import numpy as np
import matplotlib.pyplot as plt
from math import *

#横の有次元安定微係数
Yb=-45.4
Lb=-1.71
Nb=0.986
Yp=0.716
Lp=-0.962
Np=-0.0632
Yr=2.66
Lr=0.271
Nr=-0.215
Y_deltaa=0
L_deltaa=1.72
N_deltaa=-0.0436
Y_deltar=9.17
L_deltar=0.244
N_deltar=-0.666

#安定軸での水平釣り合い飛行条件での飛行速度などは以下の通り
U0=293.8
W0=0
g=9.80
theta0=0.0

#時間の刻み幅
dt=0.1
#時間の計算範囲
Tmax=300
T= np.arange(0.0,Tmax,dt)
N = len(T)

#横の微分計算 縦と同様
#y(t)=[beta,p,r,phi,psi]
#操縦項 u2=[delta_a,delta_r]

def deriv_lateral(y,u2):
    dbeta=Yb/U0*y[0]+Yp/U0*y[1]+(Yr/U0-1)*y[2]+g/U0*y[3]+Y_deltar/U0*u2[1]
    dp=Lb*y[0]+Lp*y[1]+Lr*y[2]+L_deltaa*u2[0]+L_deltar*u2[1]
    dr=Nb*y[0]+Np*y[1]+Nr*y[2]+N_deltaa*u2[0]+N_deltar*u2[1]
    dphi=y[1]
    dpsi=y[2]
    dy=np.array([dbeta,dp,dr,dphi,dpsi])
    return dy

Y = np.zeros((N,5))
#微小擾乱の初期値
beta0=0
p0=0
r0=0
phi0=0
psi0=0

#操縦項は定数　操縦桿固定
u2=[0.01,0]

#状態変数を格納する行列
Y[0,:]=np.array([beta0,p0,r0,phi0,psi0])

#4次のルンゲクッタ法
for i in range(1,N):
    k1 = deriv_lateral(Y[i-1,:],u2)
    k2 = deriv_lateral(Y[i-1,:]+0.5*dt*k1,u2)
    k3 = deriv_lateral(Y[i-1,:]+0.5*dt*k2,u2)
    k4 = deriv_lateral(Y[i-1,:]+dt*k3,u2)
    Y[i,:] = Y[i-1,:] + dt/6 * (k1+2*k2+2*k3+k4)

#グラフ表示
plt.plot(T,Y[:,0])
plt.xlabel('time [s]')
plt.ylabel('beta [rad]')
plt.grid()
plt.show()

plt.plot(T,Y[:,1])
plt.xlabel('time [s]')
plt.ylabel('p [rad/s]')
plt.grid()
plt.show()

plt.plot(T,Y[:,2])
plt.xlabel('time [s]')
plt.ylabel('r [rad/s]')
plt.grid()
plt.show()

plt.plot(T,Y[:,3])
plt.xlabel('time [s]')
plt.ylabel('phi [rad]')
plt.grid()
plt.show()

plt.plot(T,Y[:,4])
plt.xlabel('time [s]')
plt.ylabel('psi [rad]')
plt.grid()
plt.show()

これを実行すると，

f:id:kai-aeroastro:20200819023717p:plain — 横滑り角β

f:id:kai-aeroastro:20200819023935p:plain — ロール角速度p

f:id:kai-aeroastro:20200819024008p:plain — ヨー角速度r

f:id:kai-aeroastro:20200819024042p:plain — ロール角φ

f:id:kai-aeroastro:20200819024112p:plain — ヨー角ψ

となり，強そうなグラフが得られます．教科書の解析例とそこまで変わらないので正しくできてそうです．

やっぱり自分で手を動かすと理解も深まるし，イイですね．無限にパラメータ変えて遊んでられます．高校時代の数学の先生が「自分で作った味噌汁は美味しいので，自分で導出した公式は忘れない」って言ってたのを思い出しました．自分で書いたコードは美味しい(？)

3. 飛行経路の計算

これまでの計算で機体固定座標系での各時刻における状態量はすべて求まりました．次に地面固定座標系での飛行経路を計算します．

流れとしては

機体固定座標系での速度 $(U,V,W)$ を地面固定座標系での速度 $(\dot{x}_e,\dot{y}_e,\dot{z}_e)$ に変換し，それを積分することで飛行経路 $(x(t),y(t),z(t))$ を求める．

と言った感じです．線形代数がここで役に立ちます．

数学的には回転写像 $E^{-1}(\varPhi,\varTheta,\varPsi)$ を $\begin{bmatrix}U & V &W \end{bmatrix}^{T}$ に作用させて座標変換を行います．

詳細な話はテキストを見ていただくことにして先に進みます．

$E^{-1}(\varPhi,\varTheta,\varPsi)=\begin{bmatrix} \cos{\varTheta}\cos{\varPsi} & \sin{\varPhi}\sin{\varTheta}\cos{\varPsi}-\cos{\varPhi}\sin{\varPsi} &\cos{\varPhi}\sin{\varTheta}\cos{\varPsi}+\sin{\varPhi}\sin{\varPsi}\\ \cos{\varTheta}\sin{\varPsi} & \sin{\varPhi}\sin{\varTheta}\sin{\varPsi}+\cos{\varPhi}\cos{\varPsi} &\cos{\varPhi}\sin{\varTheta}\sin{\varPsi}-\sin{\varPhi}\cos{\varPsi}\\ -\sin{\varTheta} & \sin{\varPhi}\cos{\varTheta} & \cos{\varPhi}\cos{\varTheta} \end{bmatrix}$

と表されるので，地面固定座標系での速度は

$\begin{bmatrix}\dot{x}_e \\ \dot{y}_e\\ \dot{z}_e \end{bmatrix}=E^{-1}(\varPhi,\varTheta,\varPsi)\begin{bmatrix}U \\ V\\ W \end{bmatrix}$

で計算できます．

ここで迎角 $\alpha$ と横滑り角 $\beta$ の定義から

$U=U_{0}+u$

$V=V_{c}\sin{\beta}\approx (U_{0}+u)\beta$

$W = U\tan{\alpha}\approx (U_{0}+u)\alpha$

が成り立つことを利用すると，以下のように飛行経路が計算できます．

import numpy as np
import matplotlib.pyplot as plt
from math import *
from mpl_toolkits.mplot3d import Axes3D

#回転写像
def rotation(theta,phi,psi,U,V,W):
    A=np.array([[np.cos(theta)*np.cos(psi), np.sin(phi)*np.sin(theta)*np.cos(psi)-np.cos(phi)*np.sin(psi), np.cos(phi)*np.sin(theta)*np.cos(psi)+np.sin(phi)*np.sin(psi)],
               [np.cos(theta)*np.sin(psi), np.sin(phi)*np.sin(theta)*np.sin(psi)+np.cos(phi)*np.cos(psi), np.cos(phi)*np.sin(theta)*np.sin(psi)-np.sin(phi)*np.cos(psi)],
               [-np.sin(theta), np.sin(phi)*np.cos(theta), np.cos(phi)*np.cos(theta)]
               ])
    velocity=np.array([U,V,W])
    return np.dot(A,velocity)



#飛行経路を格納する行列
x_e = np.zeros((N,3))
#初期位置は高度1000ftとする
x_e[0,:]=np.array([0,0,-1000])

for i in range(1,N):
    U=U0+X[i-1,0]
    V=(U0+X[i-1,0])*Y[i-1,0]
    W=(U0+X[i-1,0])*X[i-1,1]
    #地面固定座標系での速度dx_e=(x_e,y_e,z_e)を計算
    dx_e=rotation(theta0+X[i-1,2],Y[i-1,3],Y[i-1,4],U,V,W)
    #オイラー法で積分
    x_e[i,:]=x_e[i-1,:]+dx_e*dt

%matplotlib notebook
# Figureを追加
fig = plt.figure(figsize = (8, 8))

# 3DAxesを追加
ax = fig.add_subplot(111, projection='3d')

# Axesのタイトルを設定
ax.set_title("Flight Path", size = 20)


# 軸ラベルを設定
ax.set_xlabel("x", size = 14)
ax.set_ylabel("y", size = 14)
ax.set_zlabel("z", size = 14)
#データをプロット
plt.plot(x_e[:,0],x_e[:,1],x_e[:,2])

ax.set_zlim(-1000,1000)
plt.show()