【Python】飛行機の運動をシミュレートするプログラムをつくる その2 「リアルタイムシミュレーション」

はじめに

以下の記事の「その1」続きです。

本プログラムは、本プログラムは飛行機の運動がつり合い状態からの微小擾乱と考えた場合の運動をシミュレートするプログラムを、リアルタイムキーボード入力で動かせるように拡張したプログラムです。
『航空機力学入門　(加藤寛一郎・大屋昭男・柄沢研治 著　東京大学出版会)』の3章「微小擾乱の運動方程式」をベースに作成しています。

実行するとこんな感じです。

コードは以下にあります。

また、同様の解説も実行できませんがgooglecolabにも作ってあります。キーボードの入力をリアルタイムでは受け取れないため、Google Colab上では動きません。 ローカルで実行して下さい。

ライブラリのインストール

今回はキーボード入力をリアルタイムで監視するため、keyboardというライブラリを新たに使用するため、インストールが必要です。

import numpy as np
from scipy.integrate import odeint, solve_ivp
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import keyboard
import time
import mpl_toolkits.mplot3d.art3d as art3d

# --- 機体パラメータ ---
g = 9.81
# 初期条件
U0 = 293.8
W0 = 0
theta0 = 0.0

# 縦の有次元安定微係数: ロッキードP2V-7
Xu = -0.0215
Zu = -0.227
Mu = 0

Xa = 14.7
Za = -236
Ma = -3.78
Madot = -0.28

Xq = 0
Zq = -5.76
Mq = -0.992

X_deltat = 0.01#0
Z_deltae = -12.9
Z_deltat = 0
M_deltae = -2.48
M_deltat = 0


# 横・方向の有次元安定微係数: ロッキードP2V-7
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

運動方程式

前回と変わていません

# --- 縦の運動方程式 ---
# 行列A, Bの定義
A_long = np.array([[Xu, Xa, -W0, -g * np.cos(theta0)],
                   [Zu / U0, Za / U0, 1 + Zq / U0, (g / U0) * np.sin(theta0)],
                   [Mu + Madot * (Zu / U0), Ma + Madot * (Za / U0), Mq + Madot * (1 + Zq / U0),
                    (Madot * g / U0) * np.sin(theta0)],
                   [0, 0, 1, 0]])

B_long = np.array([[0, X_deltat],
                   [Z_deltae / U0, Z_deltat / U0],
                   [M_deltae + Madot * (Z_deltae / U0), M_deltat + Madot * (Z_deltat / U0)],
                   [0, 0]])


# --- 横・方向の運動方程式 ---
# 行列A, Bの定義
A_lat = np.array([[Yb / U0, (W0 + Yp) / U0, (Yr / U0 - 1), g * np.cos(theta0) / U0, 0],
                  [Lb, Lp, Lr, 0, 0],
                  [Nb, Np, Nr, 0, 0],
                  [0, 1, np.tan(theta0), 0, 0],
                  [0, 0, 1 / np.cos(theta0), 0, 0]])

B_lat = np.array([[0, Y_deltar / U0],
                  [L_deltaa, L_deltar],
                  [N_deltaa, N_deltar],
                  [0, 0],
                  [0, 0]])




# --- 回転行列の定義 ---
def rotation_matrix(psi, theta, phi):
  # z軸周り回転
  R_z = np.array([[np.cos(psi), -np.sin(psi), 0],
                   [np.sin(psi), np.cos(psi), 0],
                   [0, 0, 1]])
  # y軸周り回転
  R_y = np.array([[np.cos(theta), 0, np.sin(theta)],
                   [0, 1, 0],
                   [-np.sin(theta), 0, np.cos(theta)]])
  # x軸周り回転
  R_x = np.array([[1, 0, 0],
                   [0, np.cos(phi), -np.sin(phi)],
                   [0, np.sin(phi), np.cos(phi)]])
  # 全体の回転行列
  R = R_z @ R_y @ R_x
  return R





# --- 統合モデル ---

def aircraft_dynamics(t, x, A_long, B_long, A_lat, B_lat, u_long, u_lat):
  # 縦の運動方程式
  dxdt_long = A_long @ np.atleast_2d(x[:4]).T + B_long @ np.atleast_2d(u_long).T
  dxdt_long = dxdt_long.flatten()

  # 横・方向の運動方程式
  dxdt_lat = A_lat @ np.atleast_2d(x[4:9]).T + B_lat @ np.atleast_2d(u_lat).T
  dxdt_lat = dxdt_lat.flatten()

  # 機体座標系での速度ベクトル
  u_b = U0 + x[0]
  v_b = u_b * np.sin(x[4])
  w_b = u_b * np.tan(x[1])
  vel_b = np.array([u_b, v_b, w_b])

  # 全体座標系での速度ベクトル
  psi = x[8]
  theta = x[3]
  phi = x[7]
  vel_e = rotation_matrix(psi, theta, phi) @ np.atleast_2d(vel_b).T
  vel_e = vel_e.flatten()


  # 縦と横・方向の状態量の変化と位置の変化を結合
  dxdt = np.concatenate((dxdt_long, dxdt_lat, vel_e))
  return dxdt

キーボードの入力

今回は、キーボード入力に応じて舵面を動かすようなプログラムを作っていきます。

繰り返しますが、Google Colab上では動きません。 ローカルで実行して下さい。

プログラムとしては、keyboardというライブラリを使用して、指定したキーを入力したら舵面の角度を変化させていき、押されていないときはだんだん0に近づいていくようにしています。

テンキーを使うことを想定しており、

• 4,6がロール
• 8,5がエレベータ
• 7,9がエルロン
• +-がスロットル

に対応しています。

# --- キーボード入力を更新する関数 ---

# キーボード入力の初期化
elevator=0
throttle=0
aileron=0
rudder=0

def update_input():
    global elevator, throttle, aileron, rudder

    if keyboard.is_pressed('4'):
        aileron += 0.01
    elif keyboard.is_pressed('6'):
        aileron -= 0.01
    else:
        aileron *= 0.5  # 徐々に減衰


    if keyboard.is_pressed('9'):
        rudder += 0.01
    elif keyboard.is_pressed('7'):
        rudder -= 0.01
    else:
        rudder *= 0.5  # 徐々に減衰


    if keyboard.is_pressed('8'):
        elevator += 0.01
    elif keyboard.is_pressed('5'):
        elevator -= 0.01
    else:
        elevator *= 0.5  # 徐々に減衰


    if keyboard.is_pressed('+'):
        throttle += 1
    elif keyboard.is_pressed('-'):
        throttle -= 1
    else:
        throttle *= 0.5  # 徐々に減衰

紙飛行機の描写

こちらの関数も若干アップデートされています。

毎フレームごとに新しい紙飛行機を描写し続けると過去のものが邪魔になるので、一つの紙飛行機を移動させていくようにしています。

具体的には、triangle.set_verts(translated_rotated_points)という部分で紙飛行機を構成する三角形を移動しています。

# --- 紙飛行機を描写する ---
def plot_paper_airplane_update(triangles, x, y, z, phi, theta, psi, scale, ax):
    """
    3次元座標と角度が与えられたとき、その状態の紙飛行機のような図形をプロットする

    Args:
      x: x座標
      y: y座標
      z: z座標
      psi: ロール角 (ラジアン)
      theta : ピッチ角 (ラジアン)
      phi: ヨー角 (ラジアン)
      機体の大きさをいじりたければscaleを変える

    """

    #三角形を描く
    poly_left_wing = scale * np.array([[2, 0.0, 0],
                                      [-1, 1, 0],
                                      [-1, 0.1, 0]])
    poly_right_wing = poly_left_wing.copy()
    poly_right_wing[:,1] = -1 * poly_right_wing[:,1]

    poly_left_body = scale * np.array([[2, 0.0, 0.0],
                                      [-1, 0.0, +0.1],
                                      [-1, 0.1, 0.0]])
    poly_right_body = poly_left_body.copy()
    poly_right_body[:,1] = -1 * poly_right_body[:,1]



    R = rotation_matrix(psi, theta, phi) # yaw, pitch, roll


    for triangle, new_points in zip(triangles, [poly_left_wing, poly_left_body, poly_right_wing, poly_right_body]):
        # 紙飛行機の点を回転
        translated_rotated_points = (R @ new_points.T).T + np.array([x, y, z])
        #描写
        triangle.set_verts(translated_rotated_points)

実行と描写

今回大きく変わっている部分はここです。

リアルタイムで入力があるとはいえ、連続的に微分方程式を解くことは不可能で、時間を離散化して解いていく必要があります。

具体的には、

1. 今(t)の入力が一定であると仮定して、ちょっと未来(t+dt)まで微分方程式を解く
   2.その未来(t+dt)になったらまた入力を取得して微分方程式を解く

ということを繰り返していけば疑似的にリアルタイムシミュレーションができるようになります。

つまり以下のようなアルゴリズムになります。

時刻	t	→	→	t+dt
動作	①キーボードから入力を読み取り一定入力として[t, t+dt]における微分方程式を解き始める	②微分方程式を解き終わる	③待ち時間	④微分方程式のt+dtの結果を初期条件、t=t+dtとして①に戻る

もっと具体的に話すと、

1. まず0秒においてキーボードのにゅりょくによって増減したエルロンの舵角を取得します。
2. エルロンの舵角が一定という入力の元0秒から10秒までの微分方程式を解きます。
3. 方程式を解くのは1秒で終わるので、残りの9秒は何もしないで待ちます。
4. 10秒になったら、微分方程式の最後の結果（10秒における状態）を初期条件として、新たに舵角の入力をとってきて、今度10秒から20秒までの微分方程式を解きます。
5. 2～4を繰り返す

とすればいいです。

問題点

しかし、上記のアルゴリズムの問題点は、「0秒から10秒まで入力一定だけど実際は時間変化しているよね？」
です。

この入力量の時間変化を無視するためには、時間増分(dt)をできるだけ細かくしてやればいいのです。
細かくすれば細かくするほど滑らかな入力に近づいていきます。

新たな問題点

それではどこまで時間変化を細かくすればいいのでしょうか。
細かくしすぎると、今度は微分方程式を解き終わっていないのに次のt+dtに移ってしまうということが発生してしまいます。このようなことを、「オーバーラン」と呼ぶようです。

この対策としては、

1. dtを大きくする
2. 微分方程式を解くアルゴリズムを高速なものにする

などがあげられます。
参考：

今回は時間幅dtを0.1秒とした結果、微分方程式は平均で0.03秒で解き終わっていたのでオーバーランはありませんでした。

# --- 可視化(アニメーション with 紙飛行機) ---

# リアルタイムプロットの設定

plt.ion()
fig = plt.figure(dpi=100)
ax = Axes3D(fig)
fig.add_axes(ax)


#飛行の軌跡
line, = ax.plot([], [], [], 'b-')

#紙飛行機の描写
try_left_wing = art3d.Poly3DCollection([np.array([[2, 0.0, 0],[-1, 1, 0],[-1, 0.1, 0]])],facecolors='orangered', linewidths=1, edgecolors='k', alpha=0.6)
try_right_wing = art3d.Poly3DCollection([np.array([[2, 0.0, 0],[-1, 1, 0],[-1, 0.1, 0]])],facecolors='orangered', linewidths=1, edgecolors='k', alpha=0.6)
try_left_body = art3d.Poly3DCollection([np.array([[2, 0.0, 0],[-1, 1, 0],[-1, 0.1, 0]])],facecolors='lime', linewidths=1, edgecolors='k', alpha=0.6)
try_right_body = art3d.Poly3DCollection([np.array([[2, 0.0, 0],[-1, 1, 0],[-1, 0.1, 0]])],facecolors='lime', linewidths=1, edgecolors='k', alpha=0.6)

triangles = [try_left_wing, try_right_wing, try_left_body, try_right_body]
for triangle in triangles:
    ax.add_collection3d(triangle)


ax.set_xlim(0, 5000)
ax.set_ylim(-2500, 2500)
ax.set_zlim(1000, -1000)

ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")








# --- シミュレーション ---

# 初期状態 (縦と横・方向、位置を結合)
x0_long = np.array([0, 0, 0, 0])  # 例: 全て0で初期化
x0_lat = np.array([0, 0, 0, 0, 0])
x0_pos = np.array([0, 0, 0])
x0 = np.concatenate((x0_long, x0_lat, x0_pos))


# 初期制御入力 (縦と横・方向)
u0_long = np.array([0, 0])  # 例: elevator=0, throttle=0
u0_lat = np.array([0, 0])  # 例: aileron=0, rudder=0


# シミュレーションの設定

t_span = (0, 100)  # 開始時間と終了時間
dt = 0.1 #時間幅

t = 0 #現在時間
x = x0 #現在状態量

all_soly =  np.zeros((12, 1)) #変数をすべて保存する変数


# シミュレーションの実行
while t < t_span[1]:
    start_time = time.time()

    #キーボード入力を取得
    update_input() 
    u_long = [elevator, throttle]
    u_lat = [aileron, rudder]
    
    # t~t+dtまで微分方程式を解く (RK45メソッドを使用)
    sol = solve_ivp(aircraft_dynamics, [t, t+dt], x,
        args=(A_long, B_long, A_lat, B_lat, u_long, u_lat),
        method='RK45')

    #t~t+dtまでの結果を追加
    all_soly = np.append(all_soly, sol.y, axis=1)

    #次のt+dtにおける初期条件
    x = sol.y[:, -1] #現在の最終状態
    t += dt
    
    


    # プロットの更新
    line.set_data_3d(np.append(line.get_data_3d()[0], x[9]), np.append(line.get_data_3d()[1], x[10]), np.append(line.get_data_3d()[2], x[11]))

    #紙飛行機の更新
    plot_paper_airplane_update(triangles=triangles, x=x[9],  y=x[10], z=x[11], phi=x[7], theta=x[3], psi=x[8], scale=400, ax=ax)


    #軸の更新
    ax.set_xlim(min(min(all_soly[9]),0), max(max(all_soly[9]), 5000),)
    ax.set_ylim(min(min(all_soly[10]),-2500), max(max(all_soly[10]), 2500),)
    ax.set_zlim(max(max(all_soly[11]), 1000),min(min(all_soly[11]),-1000))
    fig.canvas.draw()
    fig.canvas.flush_events()
    
    elapsed_time = time.time() - start_time
    print("実行時間：", elapsed_time)
    time.sleep(max(0, dt - elapsed_time)) #実行時間を考慮

plt.ioff()
plt.show()

実行結果

実行すると、matplotlibのウィンドウが表示されます。
このウィンドウをクリックしてからテンキーで入力すると機体が動きます。
実際に動かしてみた結果が以下になります。
視点の変更など課題はまだ多そうですが、これくらいにしておきます。

実行と描写（2次元）

縦の運動だけ二次元上にプロットしたければ、以下のようにすればできます。

# --- 可視化(アニメーション) ---

# リアルタイムプロットの設定
plt.ion()
fig, ax = plt.subplots()
line, = ax.plot([], [], 'b-')
ax.set_xlim(0, 100)
ax.set_ylim(100, -100)
plt.xlabel('Time (s)')
plt.ylabel('Position (m)')







# --- シミュレーション ---

# 初期状態 (縦と横・方向、位置を結合)
x0_long = np.array([0, 0, 0, 0])  # 例: 全て0で初期化
x0_lat = np.array([0, 0, 0, 0, 0])
x0_pos = np.array([0, 0, 0])
x0 = np.concatenate((x0_long, x0_lat, x0_pos))


# 初期制御入力 (縦と横・方向)
u0_long = np.array([0, 0])  # 例: elevator=0, throttle=0
u0_lat = np.array([0, 0])  # 例: aileron=0, rudder=0


# シミュレーションの設定

t_span = (0, 100)  # 開始時間と終了時間
dt = 0.1 #時間幅

t = 0 #現在時間
x = x0 #現在状態量

all_soly =  np.zeros((12, 1)) #変数をすべて保存する変数


while t < t_span[1]:
    start_time = time.time()
    update_input()
    u_long = [elevator, throttle]
    u_lat = [aileron, rudder]
    
    # 微分方程式を解く (RK45メソッドを使用)
    sol = solve_ivp(aircraft_dynamics, [t, t+dt], x,
        args=(A_long, B_long, A_lat, B_lat, u_long, u_lat),
        method='RK45')

    all_soly = np.append(all_soly, sol.y, axis=1)
    
    
    x = sol.y[:, -1] #最終状態
    t += dt
    
    # プロットの更新
    line.set_xdata(np.append(line.get_xdata(), t))
    line.set_ydata(np.append(line.get_ydata(), x[11]))
    ax.set_xlim(0, t + 1)
    ax.set_ylim(max(all_soly[11]), min(all_soly[11]),)

    fig.canvas.draw()
    fig.canvas.flush_events()
    
    elapsed_time = time.time() - start_time
    print("実行時間：", elapsed_time)
    time.sleep(max(0, dt - elapsed_time)) #実行時間を考慮

plt.ioff()
plt.show()

最後に

今回は、飛行機をリアルタイムの入力に応じて動かすリアルタイムシミュレーションに拡張しました。

実際に作ってみると離散化の考え方や微分方程式の実行時間の問題などが理解でき、とてもいい経験になりました。

次回「その3」では、D学習を用いて自動で飛行してくれるようなAIを作りました。