【Python】放物線経路に拘束された質点の運動シミュレーション

シミュ君

2024年11月20日 15:31

導出した経路に拘束された質点運動をシミュレーションするための計算アルゴリズムを用いた３つ目の例として放物線経路を取り上げるよ。計算に必要な経路に関する各種ベクトル（経路ベクトル・接線ベクトル・曲率ベクトル）の導出から始めるよ。

放物線経路の経路ベクトル・接線ベクトル・曲率ベクトル

放物線とは、重力場中の物体を斜方投射させたときの運動の軌跡を表す曲線だね。斜方投射の水平方向を$${x}$$軸、垂直方向を$${y}$$軸と定義した場合、$${y}$$の一般解は

$$
y=a(x-x_0)^2+b
$$

と$${x}$$の２次関数となるね。今回は図のような２次関数の頂点が原点となる場合を経路とするときの質点の運動シミュレーションを行うよ。まずはそのために必要な接線ベクトル、曲率ベクトルの導出を行うね。

放物線を表す媒介変数表示は次のとおりだよ。

放物線経路の媒介変数表示

$$
\left\{\begin{matrix}x(\theta )= \theta & \cr y(\theta ) =a \theta^2& \end{matrix}\right.
$$

媒介変数$${\theta}$$は$${-\infty<\theta<\infty}$$で定義されるね。接線ベクトルと曲率ベクトルに必要な$${\theta}$$の$${l}$$の１回微分と２回微分は

$$
\frac{d l}{d\theta } = \sqrt{1+4a^2\theta^2} \ \to \ \frac{d \theta}{dl } = \frac{1}{ \sqrt{1+4a^2\theta^2} }
$$

$$
\frac{d^2 \theta}{dl^2 } = \frac{d\theta}{dl}\,\frac{d }{d\theta}\left(\frac{1}{ \sqrt{1+4a^2\theta^2}}\right)=\frac{-4a^2\theta}{\left(1+4a^2\theta^2\right)^2}
$$

と与えられるので、放物線経路の接線ベクトルと曲率ベクトルは次のとおり得られるね。

放物線経路の接線ベクトルと曲率ベクトル

$$
\boldsymbol{t}_{\rm path} (\theta)= \frac{1}{\sqrt{1+4a^2\theta^2}} \,(1,2a\theta)
$$

$$
\boldsymbol{\chi} (\theta) = \frac{-2a}{\left(1+4a^2\theta^2\right)^2}\,(2a\theta,-1)
$$

なお、曲率ベクトルの大きさは

$$
\chi(\theta)=|\boldsymbol{\chi} (\theta)|= \frac{2|a|}{\left(1+4a^2\theta^2\right)^{3/2}}
$$

となることから、楕円と同様$${\theta}$$に依存します。曲率は$${\theta=0}$$のときに最大値$${2\left|a\right|}$$をとり、後は$${\theta}$$が大きくなるほど$${\theta }$$の２乗に反比例して小さくなるね。

ではさっそく計算結果を示すよ。

計算結果

Pythonプログラムソース

上記のシミュレーションを行うプログラムソースを公開するよ。もしよかったら試してみてください。これからも応援してください。よろしくお願いしまーす！

#################################################################
TITLE = "経路に拘束された運動（放物線経路）"
#################################################################
import os
import numpy as np
import scipy.integrate as integrate
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import colorsys
###############################
# 実験室グローバル変数
###############################
TIME_START = 0.0        #計算開始時刻
TIME_END   = 10.0       #計算終了時刻
DT = 0.01               #時間間隔
PLOT_FLAG = False        #グラフ描画
ANIMATION_FLAG = True  #アニメーション作成
ANIMATION_INTERVAL = 100 #アニメーション作成時の描画インターバル
OUTPUT_FLAG = False
OUTPUT_PASS = "./data_20241119/"
if( OUTPUT_FLAG and os.path.isdir(OUTPUT_PASS) == False) : os.mkdir(OUTPUT_PASS)
###############################
# 計算グローバル変数
###############################
#時刻
times = np.arange(TIME_START, TIME_END, DT)
#重力加速度
vec_g = np.array([0, 0, -10])
#質量
m = 1.0
#初速度の大きさ
#v0s = [ 5.0 ]
v0s = np.arange(10, 11.0+0.1, 0.02)
###############################
# 関数定義
###############################
#相対誤差
rtol = 1e-9
#絶対誤差                   
atol = 1e-9                   
#補正ばね弾性係数
compensationK = 0.004
#補正粘性抵抗係数
compensationGamma = 0.4

# 経路の設定
class Path:
	#コンストラクタ
	def __init__(self):
		#２次方程式のa係数
		self.a = 1.0
		#放物線の頂点位置ベクトル
		self.center = np.array([0.0, 0.0, 0.0])

	#経路の位置ベクトルを指定する媒介変数関数
	def position( self, theta ):
		x = theta + self.center[0]
		y = self.center[1]
		z = self.a * theta ** 2 + self.center[2]
		return np.array([x, y, z])

	#接線ベクトルを指定する媒介変数関数
	def tangent( self, theta ):
		K = 1.0 / np.sqrt( 1.0 + ( 2.0 * self.a * theta) ** 2 )
		x = K * 1.0 
		y = 0
		z =  K * 2.0 * self.a * theta
		return np.array([x, y, z])

	#曲率ベクトルを指定する媒介変数関数
	def curvature( self, theta ):
		K = ( 1.0 / ( 1.0 + ( 2.0 * self.a * theta)**2 ) )**2
		x = - K * 4.0 * self.a**2 * theta
		y = 0
		z = K * 2.0 * self.a
		return np.array([x, y, z])

	#媒介変数の取得
	def getTheta( self, r ):
		#相対位置ベクトル
		bar_r = r - self.center
		return bar_r[0]


#経路拘束力の微分方程式
def equations(t, X, path, compensationK, compensationGamma):
	vec_r =  np.array([ X[0], X[1], X[2] ])
	vec_v =  np.array([ X[3], X[4], X[5] ])

	#媒介変数の取得
	theta = path.getTheta ( vec_r )

	#媒介変数に対する位置ベクトル、接線ベクトル、曲率ベクトルの計算
	position = path.position( theta )
	tangent = path.tangent( theta )
	curvature = path.curvature( theta )

	#経路中心の位置ベクトル・速度ベクトル・加速度ベクトル
	vec_r_box = np.copy( path.center )
	vec_v_box = np.array([0.0, 0.0, 0.0])
	vec_a_box = np.array([0.0, 0.0, 0.0])

	#相対速度
	bar_v = vec_v - vec_v_box

	#微係数dl/dtとd^2l/dt^2を計算
	dl_dt = np.dot( bar_v, tangent )
	d2l_dt2 = np.dot( vec_g, tangent ) - np.dot( vec_a_box, tangent) 

	#加速度を計算
	vec_a = vec_a_box + d2l_dt2 * tangent + dl_dt * dl_dt * curvature

	#ズレ位置ベクトル
	bar_r = vec_r - position
	#補正ばね弾性力
	fk = -compensationK * bar_r
	#法線ベクトル
	n1 = curvature / np.sqrt(np.sum( curvature**2 ))
	n2 = np.cross(n1, tangent )
	#補正粘性抵抗力
	fgamma1 = -compensationGamma * np.dot( bar_v, n1 ) * n1 
	fgamma2 = -compensationGamma * np.dot( bar_v, n2 ) * n2
	#経路補正力を加える
	vec_a += (fk + fgamma1 + fgamma2)/m * np.abs(m*vec_a)

	return [vec_v[0], vec_v[1], vec_v[2], vec_a[0], vec_a[1], vec_a[2]]

# 初速度依存性を可視化
def calculation(vec_r0, vec_v0, path, compensationK, compensationGamma):

	solution = integrate.solve_ivp(
		equations,                     #常微分方程式
		method = 'RK45',                #計算方法
		args = (path, compensationK, compensationGamma),              #引数
		t_span=( times[0], times[-1]),  #積分区間
		rtol = rtol,                   #相対誤差
		atol = atol,                   #絶対誤差
		y0 = np.concatenate([vec_r0, vec_v0]), #初期値
		t_eval = times                  #結果を取得する時刻
	)
	#計算結果
	return solution.y[ 0 ], solution.y[ 1 ], solution.y[ 2 ], solution.y[ 3 ], solution.y[ 4 ], solution.y[ 5 ]

##########################################################
if __name__ == "__main__":

	#経路の生成
	path = Path()
	#描画用リスト
	xls = []
	yls = []
	zls = []
	vxls = []
	vyls = []
	vzls = []
	#初速度 による違い 
	for i in range(len(v0s)):
		print( i )

		#初期位置と速度
		vec_r0 = np.array([ 0, 0, 0])
		vec_v0 = np.array([ v0s[i], 0, 0])

		x, y, z, vx, vy, vz = calculation( vec_r0, vec_v0, path, compensationK, compensationGamma )
		xls.append( x )
		yls.append( y )
		zls.append( z )
		vxls.append( vx )
		vyls.append( vy )
		vzls.append( vz )

		vec_r = np.array([ x[-1], y[-1], z[-1] ])
		vec_v = np.array([ vx[-1],vy[-1],vz[-1] ])

		E_start = 1.0/2.0 * m * np.sum( vec_v0**2 ) - m * np.dot( vec_g, vec_r0 )
		E_end =   1.0/2.0 * m * np.sum( vec_v**2 )  - m * np.dot( vec_g, vec_r )
		DeltaE = np.abs(E_end - E_start)  
		print( E_start, E_end, " |ΔE/E(0)| =", DeltaE / E_start )

	if( OUTPUT_FLAG ):
		for i in range(len(times)):
			f_path = OUTPUT_PASS + str( i ) + '.txt'
			f = open(f_path, 'w')

			for j in range( len(xls) ):
				f.write( str(round( xls[ j ][ i ], 4)) + "\t" +  str(round( yls[ j ][ i ], 4)) + "\t" + str(round( zls[ j ][ i ], 4)) + "\t" +  str(round( vxls[ j ][ i ], 4)) + "\t" + str(round( vyls[ j ][ i ], 4)) + "\t" +  str(round( vzls[ j ][ i ], 4)) + "\n")

			f.close()

	if( PLOT_FLAG ):
		#################################################################
		# グラフの設定
		#################################################################
		fig = plt.figure( figsize=(12, 8) ) #画像サイズ
		plt.subplots_adjust(left = 0.085, right = 0.97, bottom = 0.10, top = 0.95) #グラフサイズ
		plt.rcParams['font.family'] = 'Times New Roman' #フォント
		plt.rcParams['font.size'] = 16 #フォントサイズ
		plt.rcParams["mathtext.fontset"] = 'cm' #数式用フォント
		#カラーリストの取得
		colors = plt.rcParams['axes.prop_cycle'].by_key()['color']
		#グラフタイトルと軸ラベルの設定 
		plt.title( TITLE, fontname="Yu Gothic", fontsize=20, fontweight=500 )
		plt.xlabel( r"$t \,[{\rm s}]$", fontname="Yu Gothic", fontsize=20, fontweight=500 )
		plt.ylabel( r"$z \,[{\rm m}]$" , fontname="Yu Gothic", fontsize=20, fontweight=500 )
		#罫線の描画
		plt.grid( which = "major", axis = "x", alpha = 0.7, linewidth = 1 )
		plt.grid( which = "major", axis = "y", alpha = 0.7, linewidth = 1 )

		#描画範囲を設定
		plt.xlim([0, TIME_END])
		plt.ylim([0, 2])

		#グラフの描画
		for i in range(len(v0s)):
			c = colorsys.hls_to_rgb( i/len(v0s) , 0.5 , 1)
			plt.plot(times, zls[i], linestyle='solid', linewidth = 4)#, color = c)

		#グラフの表示
		plt.show()

	if( ANIMATION_FLAG ):

		#楕円座標点配列の生成
		thetas = np.linspace(-np.pi, np.pi, 200)
		cirsx = thetas
		cirsy = path.a * thetas**2

		#################################################################
		# アニメーションの設定
		#################################################################
		fig = plt.figure(figsize=(9.6, 12)) #画像サイズ
		plt.subplots_adjust(left = 0.07, right = 0.98, bottom = 0.07, top = 0.95) #グラフサイズ
		plt.rcParams['font.family'] = 'Times New Roman' #フォント
		plt.rcParams['font.size'] = 16 #フォントサイズ
		plt.rcParams["mathtext.fontset"] = 'cm' #数式用フォント
		#カラーリストの取得
		colors = plt.rcParams['axes.prop_cycle'].by_key()['color']

		#アニメーション作成用
		imgs = []

		#アニメーション分割数
		AN = len( times )

		#軌跡リスト
		r_trajectorys = [ 0.0 ] * len( v0s )

		#アニメーションの各グラフを生成
		for an in range( AN ):
			t = DT * an

			if( an % ANIMATION_INTERVAL != 0): continue

			#軌跡の描画
			img = plt.plot(cirsx, cirsy, color = "black", linestyle='dotted', linewidth = 1 )

			for i in range( len( v0s ) ):
				c = colorsys.hls_to_rgb( i/len(v0s) , 0.5 , 1)
				if( i == 0 ): 
					img += plt.plot([xls[i][an]], [zls[i][an]], color = c, marker = 'o', markersize = 20)
				else:
					img += plt.plot([xls[i][an]], [zls[i][an]], color = c, marker = 'o', markersize = 20)

			
			#時刻をグラフに追加
			time = plt.text(13.5, 0.35, "t = " +  str("{:.2f}".format(t)), fontsize=18)
			img.append( time )
			
			imgs.append( img )

		#グラフタイトルと軸ラベルの設定 
		plt.title( TITLE, fontname="Yu Gothic", fontsize=20, fontweight=500 )
		#plt.xlabel( r"$x \,[{\rm m}]$", fontname="Yu Gothic", fontsize=20, fontweight=500 )
		#plt.ylabel( r"$z \,[{\rm m}]$" , fontname="Yu Gothic", fontsize=20, fontweight=500 )
		#罫線の描画
		plt.grid( which = "major", axis = "x", alpha = 0.7, linewidth = 1 )
		plt.grid( which = "major", axis = "y", alpha = 0.7, linewidth = 1 )

		#描画範囲を設定
		plt.xlim([-2.4, 2.4])
		plt.ylim([-0.2, 5.8])
		#アニメーションの生成
		ani = animation.ArtistAnimation(fig, imgs, interval=50)
		#アニメーションの保存
		#ani.save("output.html", writer=animation.HTMLWriter())
		#ani.save("output40.gif", writer="imagemagick")
		#ani.save("output40.mp4", writer="ffmpeg", dpi=360)
		#グラフの表示
		plt.show()