👻
pythonでドローンシミュレーションやってみた
ドローンシミュレーションで遊ぼう
昔はドローンの研究していました。いざ，実機使おうとしたら，ドローン規制が入って，全然実機での研究ができなくなったのではいい思い出です。
正直，教授はドローンを1mmも知らない人だったので，学習は自己流なので，本物には敵わないと思いますが，色々試して理解を深めてみてください。
ドローンでのすき家は配達が始まるとのことで，昔の教材を引っ張り出してきました。遊んでみてください，
ドローンの力学(簡略版)
ドローンはドローンの機体力学を勉強しないと成立ちませんし，制御工学の知識も必要です。
ですが，今回は導入ですので，概念だけ説明して簡単に進みます。
詳しくは以下を参照してください。ですが，これを理解するには制御工学の知識が必要です。（ジレンマ）
以下の力学を簡単に説明します。
上記の図で，U1,U2,U3,U4は各プロペラ（Rotor）の推力，
φ(ピッチ)，θ（ロール）ψ（ヨー）は回転を表します。要はロール回転させたら前後（X座標）に，ピッチ回転させたら左右（Y座標）に，ヨー回転させたら機体の向きが変わりますよって話です。
これは一番簡単な設定で，もう少し複雑なのもあるので，気になった人は他の文献を読んでみてください。
本当はダイアグラムで綺麗に書きたいのですが，綺麗に出ないので図を分けます。すごい簡単にいうと以下のように設計されてます。要は，理想と現実の差分から推力を算出して，ドローンを理想の軌道に乗せるよって，話を難しく書いたものです。
制御の概念
コントローラーらへん
環境構築(python)
初期セットアップ
環境構築
まず，ファイルをダウンロードするフォルダー(私はpython-drone-sampleです。)を作ってください。
pythonの環境を作ります。
$ python3 -m venv env
環境を使用する時は，以下のコードを使用してください。
$ source env/bin/activate
ライブラリのダウンロード
以下のライブラリをダウンロードしていきます。
Numpy
$ pip install numpy
matplotlib
$ pip3 install matplotlib
次のコードが本題です。ドローンの軌道追従制御のしみゅれーしょんをSimulation_MPC_drone，色々なパラメータやコントローラーの再現をdrone_calcとして分けています。
Simulation_MPC_drone
MAIN_MPC_drone
import platform
print("Python " + platform.python_version())
import numpy as np
print("Numpy " + np.__version__)
import matplotlib
print("Matplotlib " + matplotlib.__version__)
import matplotlib.pyplot as plt
import drone_calc as sfd
import matplotlib.gridspec as gridspec
import matplotlib.animation as animation
# mpl_toolkits.mplot3d から Axes3D をインポート
from mpl_toolkits import mplot3d
# サポート関数のオブジェクトを作成します。
support=sfd.SupportFilesDrone()
constants=support.constants
# メインファイルに必要な定数値をロードします。
Ts=constants['Ts']
controlled_states=constants['controlled_states'] # 出力数
innerDyn_length=constants['innerDyn_length'] # 内部制御ループの反復回数
pos_x_y=constants['pos_x_y']
if pos_x_y==1:
extension=2.5
elif pos_x_y==0:
extension=0
else:
print("サポート ファイルの初期関数で pos_x_y 変数を 1 または 0 にしてください (すべての初期定数が存在します)。")
exit()
sub_loop=constants['sub_loop']
sim_version=constants['sim_version']
if sim_version==1:
sim_version=1
elif sim_version==2:
sim_version=2
else:
print("入力関数の変数「sim_version」には 1 または 2 のみを代入してください。")
exit()
# 参照軌道の信号を生成する
t=np.arange(0,100+Ts*innerDyn_length,Ts*innerDyn_length) # 時間0～100秒、サンプル時間(Ts=0.4秒)
t_angles=np.arange(0,t[-1]+Ts,Ts)
t_ani=np.arange(0,t[-1]+Ts/sub_loop,Ts/sub_loop)
X_ref,X_dot_ref,X_dot_dot_ref,Y_ref,Y_dot_ref,Y_dot_dot_ref,Z_ref,Z_dot_ref,Z_dot_dot_ref,psi_ref=support.trajectory_generator(t)
plotl=len(t) # 外部制御ループの反復数
# 初期状態ベクトルをロードします
ut=0
vt=0
wt=0
pt=0
qt=0
rt=0
xt=0
yt=-1
zt=0
phit=0
thetat=0
psit=psi_ref[0]
states=np.array([ut,vt,wt,pt,qt,rt,xt,yt,zt,phit,thetat,psit])
statesTotal=[states] # 制御のすべての状態を保管。
# statesTotal2=[states]
statesTotal_ani=[states[6:len(states)]]
# 最初の Phi_ref、Theta_ref、Psi_ref が最初の phit、thetat、psit と等しいと仮定します。
ref_angles_total=np.array([[phit,thetat,psit]])
velocityXYZ_total=np.array([[0,0,0]])
# ドローンのプロペラの初期状態
omega1=110*np.pi/3 # rad/s at t=-Ts s (今より Ts 秒前)
omega2=110*np.pi/3 # rad/s at t=-Ts s (今より Ts 秒前)
omega3=110*np.pi/3 # rad/s at t=-Ts s (今より Ts 秒前)
omega4=110*np.pi/3 # rad/s at t=-Ts s (今より Ts 秒前)
omega_total=omega1-omega2+omega3-omega4
ct=constants['ct']
cq=constants['cq']
l=constants['l']
U1=ct*(omega1**2+omega2**2+omega3**2+omega4**2) # t = -Ts [s]での入力
U2=ct*l*(omega2**2-omega4**2) # t = -Ts [s]での入力
U3=ct*l*(omega3**2-omega1**2) # t = -Ts [s]での入力
U4=cq*(-omega1**2+omega2**2-omega3**2+omega4**2) # t = -Ts [s]での入力
UTotal=np.array([[U1,U2,U3,U4]]) # 推力4 inputs
omegas_bundle=np.array([[omega1,omega2,omega3,omega4]])
UTotal_ani=UTotal
########## Start the global controller #################################
for i_global in range(0,plotl-1):
# 位置コントローラーの実装 (線形状態フィードバック)
phi_ref, theta_ref, U1=support.pos_controller(X_ref[i_global+1],X_dot_ref[i_global+1],X_dot_dot_ref[i_global+1],Y_ref[i_global+1],Y_dot_ref[i_global+1],Y_dot_dot_ref[i_global+1],Z_ref[i_global+1],Z_dot_ref[i_global+1],Z_dot_dot_ref[i_global+1],psi_ref[i_global+1],states)
Phi_ref=np.transpose([phi_ref*np.ones(innerDyn_length+1)])
Theta_ref=np.transpose([theta_ref*np.ones(innerDyn_length+1)])
# Psi_ref を外側のループごとに線形に連続的に増加させる
Psi_ref=np.transpose([np.zeros(innerDyn_length+1)])
for yaw_step in range(0, innerDyn_length+1):
Psi_ref[yaw_step]=psi_ref[i_global]+(psi_ref[i_global+1]-psi_ref[i_global])/(Ts*innerDyn_length)*Ts*yaw_step
temp_angles=np.concatenate((Phi_ref[1:len(Phi_ref)],Theta_ref[1:len(Theta_ref)],Psi_ref[1:len(Psi_ref)]),axis=1)
ref_angles_total=np.concatenate((ref_angles_total,temp_angles),axis=0)
# 参照ベクトルを作成する
refSignals=np.zeros(len(Phi_ref)*controlled_states)
# 参照信号ベクトルを構築します。
# refSignal = [Phi_ref_0, Theta_ref_0, Psi_ref_0, Phi_ref_1, Theta_ref_2, Psi_ref_2, ... etc.]
k=0
for i in range(0,len(refSignals),controlled_states):
refSignals[i]=Phi_ref[k]
refSignals[i+1]=Theta_ref[k]
refSignals[i+2]=Psi_ref[k]
k=k+1
# コントローラーの開始 - シミュレーション ループ
hz=support.constants['hz'] # 水平時間
k=0 # 参照信号の読み取り用
# statesTotal2=np.concatenate((statesTotal2,[states]),axis=0)
for i in range(0,innerDyn_length):
# 離散状態空間行列を生成する
Ad,Bd,Cd,Dd,x_dot,y_dot,z_dot,phi,phi_dot,theta,theta_dot,psi,psi_dot=support.LPV_cont_discrete(states, omega_total)
x_dot=np.transpose([x_dot])
y_dot=np.transpose([y_dot])
z_dot=np.transpose([z_dot])
temp_velocityXYZ=np.concatenate(([[x_dot],[y_dot],[z_dot]]),axis=1)
velocityXYZ_total=np.concatenate((velocityXYZ_total,temp_velocityXYZ),axis=0)
# 拡張された現在の状態と参照ベクトルを生成する
x_aug_t=np.transpose([np.concatenate(([phi,phi_dot,theta,theta_dot,psi,psi_dot],[U2,U3,U4]),axis=0)])
# Ts=0.1 s
# refSignals ベクトルから、[現在のサンプル (NOW) + Ts] から [NOW + 水平時間 (hz)] までの基準値のみを抽出します。
# 例: t_now は 3 秒、hz = 15 サンプルであるため、refSignals ベクトルから要素をベクトル r に移動します。
# r=[Phi_ref_3.1, Theta_ref_3.1, Psi_ref_3.1, Phi_ref_3.2, ... , Phi_ref_4.5, Theta_ref_4.5, Psi_ref_4.5]
# Ts=0.1 秒であるため、ループごとにすべて 0.1 秒ずつシフトします
k=k+controlled_states
if k+controlled_states*hz<=len(refSignals):
r=refSignals[k:k+controlled_states*hz]
else:
r=refSignals[k:len(refSignals)]
hz=hz-1
# コスト関数のコンパクトな単純化行列を生成する
Hdb,Fdbt,Cdb,Adc=support.mpc_simplification(Ad,Bd,Cd,Dd,hz)
ft=np.matmul(np.concatenate((np.transpose(x_aug_t)[0][0:len(x_aug_t)],r),axis=0),Fdbt)
du=-np.matmul(np.linalg.inv(Hdb),np.transpose([ft]))
# 入力値の更新
U2=U2+du[0][0]
U3=U3+du[1][0]
U4=U4+du[2][0]
# 入力を追跡する
UTotal=np.concatenate((UTotal,np.array([[U1,U2,U3,U4]])),axis=0)
# print(UTotal)
# 更新したUに基づいて新しいOmegaを計算します。
U1C=U1/ct
U2C=U2/(ct*l)
U3C=U3/(ct*l)
U4C=U4/cq
UC_vector=np.zeros((4,1))
UC_vector[0,0]=U1C
UC_vector[1,0]=U2C
UC_vector[2,0]=U3C
UC_vector[3,0]=U4C
omega_Matrix=np.zeros((4,4))
omega_Matrix[0,0]=1
omega_Matrix[0,1]=1
omega_Matrix[0,2]=1
omega_Matrix[0,3]=1
omega_Matrix[1,1]=1
omega_Matrix[1,3]=-1
omega_Matrix[2,0]=-1
omega_Matrix[2,2]=1
omega_Matrix[3,0]=-1
omega_Matrix[3,1]=1
omega_Matrix[3,2]=-1
omega_Matrix[3,3]=1
omega_Matrix_inverse=np.linalg.inv(omega_Matrix)
omegas_vector=np.matmul(omega_Matrix_inverse,UC_vector)
omega1P2=omegas_vector[0,0]
omega2P2=omegas_vector[1,0]
omega3P2=omegas_vector[2,0]
omega4P2=omegas_vector[3,0]
if omega1P2<=0 or omega2P2<=0 or omega3P2<=0 or omega4P2<=0:
print("負の数の平方根は取れません")
print("軌道が混沌としすぎているか、大きな不連続なジャンプがあることが問題である可能性があります")
print("不連続なジャンプを行わずに、より滑らかな軌道を作成してみてください")
print("その他に考えられる原因は、Ts、hz、innerDyn_length、px、py、pz などの変数の値である可能性があります")
print("問題が発生した場合は、ファイルを再度ダウンロードし、デフォルト設定を使用し、値を 1 つずつ変更してみてください。")
exit()
else:
omega1=np.sqrt(omega1P2)
omega2=np.sqrt(omega2P2)
omega3=np.sqrt(omega3P2)
omega4=np.sqrt(omega4P2)
omegas_bundle=np.concatenate((omegas_bundle,np.array([[omega1,omega2,omega3,omega4]])),axis=0)
# 新しいtotal omegaを計算します
omega_total=omega1-omega2+omega3-omega4
# 開ループ システムの新しい状態を計算します (間隔: Ts/10)
states,states_ani,U_ani=support.open_loop_new_states(states,omega_total,U1,U2,U3,U4)
# print(states)
# print(statesTotal)
statesTotal=np.concatenate((statesTotal,[states]),axis=0)
# print(statesTotal)
statesTotal_ani=np.concatenate((statesTotal_ani,states_ani),axis=0)
UTotal_ani=np.concatenate((UTotal_ani,U_ani),axis=0)
################################ ANIMATION LOOP ###############################
if max(Y_ref)>=max(X_ref):
max_ref=max(Y_ref)
else:
max_ref=max(X_ref)
if min(Y_ref)<=min(X_ref):
min_ref=min(Y_ref)
else:
min_ref=min(X_ref)
statesTotal_x=statesTotal_ani[:,0]
statesTotal_y=statesTotal_ani[:,1]
statesTotal_z=statesTotal_ani[:,2]
statesTotal_phi=statesTotal_ani[:,3]
statesTotal_theta=statesTotal_ani[:,4]
statesTotal_psi=statesTotal_ani[:,5]
UTotal_U1=UTotal_ani[:,0]
UTotal_U2=UTotal_ani[:,1]
UTotal_U3=UTotal_ani[:,2]
UTotal_U4=UTotal_ani[:,3]
frame_amount=int(len(statesTotal_x))
length_x=max_ref*0.15 # UAV の x 方向の半分の長さ (アニメーション目的のみ)
length_y=max_ref*0.15 # UAV の x 方向の半分の長さ (アニメーション目的のみ)
def update_plot(num):
# print("メートル単位の位置")
# print(statesTotal_x[num])
# print(statesTotal_y[num])
# print(statesTotal_z[num])
# print("Orientation in degrees")
# print(statesTotal_phi[num]*180/np.pi)
# print(statesTotal_theta[num]*180/np.pi)
# print(statesTotal_psi[num]*180/np.pi)
# u、v、w と x_dot、y_dot、z_dot に関連する回転行列
R_x=np.array([[1, 0, 0],[0, np.cos(statesTotal_phi[num]), -np.sin(statesTotal_phi[num])],[0, np.sin(statesTotal_phi[num]), np.cos(statesTotal_phi[num])]])
R_y=np.array([[np.cos(statesTotal_theta[num]),0,np.sin(statesTotal_theta[num])],[0,1,0],[-np.sin(statesTotal_theta[num]),0,np.cos(statesTotal_theta[num])]])
R_z=np.array([[np.cos(statesTotal_psi[num]),-np.sin(statesTotal_psi[num]),0],[np.sin(statesTotal_psi[num]),np.cos(statesTotal_psi[num]),0],[0,0,1]])
R_matrix=np.matmul(R_z,np.matmul(R_y,R_x))
drone_pos_body_x=np.array([[length_x+extension],[0],[0]])
drone_pos_inertial_x=np.matmul(R_matrix,drone_pos_body_x)
drone_pos_body_x_neg=np.array([[-length_x],[0],[0]])
drone_pos_inertial_x_neg=np.matmul(R_matrix,drone_pos_body_x_neg)
drone_pos_body_y=np.array([[0],[length_y+extension],[0]])
drone_pos_inertial_y=np.matmul(R_matrix,drone_pos_body_y)
drone_pos_body_y_neg=np.array([[0],[-length_y],[0]])
drone_pos_inertial_y_neg=np.matmul(R_matrix,drone_pos_body_y_neg)
drone_body_x.set_xdata([statesTotal_x[num]+drone_pos_inertial_x_neg[0][0],statesTotal_x[num]+drone_pos_inertial_x[0][0]])
drone_body_x.set_ydata([statesTotal_y[num]+drone_pos_inertial_x_neg[1][0],statesTotal_y[num]+drone_pos_inertial_x[1][0]])
drone_body_y.set_xdata([statesTotal_x[num]+drone_pos_inertial_y_neg[0][0],statesTotal_x[num]+drone_pos_inertial_y[0][0]])
drone_body_y.set_ydata([statesTotal_y[num]+drone_pos_inertial_y_neg[1][0],statesTotal_y[num]+drone_pos_inertial_y[1][0]])
real_trajectory.set_xdata(statesTotal_x[0:num])
real_trajectory.set_ydata(statesTotal_y[0:num])
real_trajectory.set_3d_properties(statesTotal_z[0:num])
drone_body_x.set_3d_properties([statesTotal_z[num]+drone_pos_inertial_x_neg[2][0],statesTotal_z[num]+drone_pos_inertial_x[2][0]])
drone_body_y.set_3d_properties([statesTotal_z[num]+drone_pos_inertial_y_neg[2][0],statesTotal_z[num]+drone_pos_inertial_y[2][0]])
if sim_version==1:
drone_body_phi.set_data([-length_y*0.9*0.9,length_y*0.9*0.9],[drone_pos_inertial_y_neg[2][0],drone_pos_inertial_y[2][0]])
drone_body_theta.set_data([length_x*0.9*0.9,-length_x*0.9*0.9],[drone_pos_inertial_x[2][0],drone_pos_inertial_x_neg[2][0]])
U1_function.set_data(t_ani[0:num],UTotal_U1[0:num])
U2_function.set_data(t_ani[0:num],UTotal_U2[0:num])
U3_function.set_data(t_ani[0:num],UTotal_U3[0:num])
U4_function.set_data(t_ani[0:num],UTotal_U4[0:num])
return drone_body_x, drone_body_y, real_trajectory,\
drone_body_phi, drone_body_theta, U1_function, U2_function, U3_function, U4_function
else:
drone_position_x.set_data(t_ani[0:num],statesTotal_x[0:num])
drone_position_y.set_data(t_ani[0:num],statesTotal_y[0:num])
drone_position_z.set_data(t_ani[0:num],statesTotal_z[0:num])
drone_orientation_phi.set_data(t_ani[0:num],statesTotal_phi[0:num])
drone_orientation_theta.set_data(t_ani[0:num],statesTotal_theta[0:num])
drone_orientation_psi.set_data(t_ani[0:num],statesTotal_psi[0:num])
return drone_body_x, drone_body_y, real_trajectory,\
drone_position_x, drone_position_y, drone_position_z,\
drone_orientation_phi, drone_orientation_theta, drone_orientation_psi
# 図の準備
fig_x=16
fig_y=9
fig=plt.figure(figsize=(fig_x,fig_y),dpi=120,facecolor=(0.8,0.8,0.8))
n=4
m=3
gs=gridspec.GridSpec(n,m)
# Drone motion
# ドローン用のオブジェクトを作成する
ax0=fig.add_subplot(gs[0:3,0:2],projection='3d',facecolor=(0.9,0.9,0.9))
# 参照軌道のプロット
ref_trajectory=ax0.plot(X_ref,Y_ref,Z_ref,'b',linewidth=1,label='reference')
real_trajectory,=ax0.plot([],[],[],'r',linewidth=1,label='trajectory')
drone_body_x,=ax0.plot([],[],[],'r',linewidth=5,label='drone_x')
drone_body_y,=ax0.plot([],[],[],'g',linewidth=5,label='drone_y')
ax0.set_xlim(min_ref,max_ref)
ax0.set_ylim(min_ref,max_ref)
ax0.set_zlim(0,max(Z_ref))
ax0.set_xlabel('X [m]')
ax0.set_ylabel('Y [m]')
ax0.set_zlabel('Z [m]')
ax0.legend(loc='upper left')
if sim_version==1:
# VERSION 1
# ドローンの向き (phi - around x axis) - zoomed:
ax1=fig.add_subplot(gs[3,0],facecolor=(0.9,0.9,0.9))
drone_body_phi,=ax1.plot([],[],'--g',linewidth=2,label='drone_y (+: Z-up,Y-right,phi-CCW)')
ax1.set_xlim(-length_y*0.9,length_y*0.9)
ax1.set_ylim(-length_y*1.1*0.01,length_y*1.1*0.01)
ax1.legend(loc='upper left',fontsize='small')
plt.grid(True)
# ドローンの向き (theta - around y axis) - zoomed:
ax2=fig.add_subplot(gs[3,1],facecolor=(0.9,0.9,0.9))
drone_body_theta,=ax2.plot([],[],'--r',linewidth=2,label='drone_x (+: Z-up,X-left,theta-CCW)')
ax2.set_xlim(length_x*0.9,-length_x*0.9)
ax2.set_ylim(-length_x*1.1*0.01,length_x*1.1*0.01)
ax2.legend(loc='upper left',fontsize='small')
plt.grid(True)
# U1 の出力関数
ax3=fig.add_subplot(gs[0,2],facecolor=(0.9,0.9,0.9))
U1_function,=ax3.plot([],[],'b',linewidth=1,label='Thrust (U1) [N]')
plt.xlim(0,t_ani[-1])
plt.ylim(np.min(UTotal_U1)-0.01,np.max(UTotal_U1)+0.01)
plt.grid(True)
plt.legend(loc='upper right',fontsize='small')
# U2 の出力関数
ax4=fig.add_subplot(gs[1,2],facecolor=(0.9,0.9,0.9))
U2_function,=ax4.plot([],[],'b',linewidth=1,label='Roll (U2) [Nm]')
plt.xlim(0,t_ani[-1])
plt.ylim(np.min(UTotal_U2)-0.01,np.max(UTotal_U2)+0.01)
plt.grid(True)
plt.legend(loc='upper right',fontsize='small')
# U3 の出力関数
ax5=fig.add_subplot(gs[2,2],facecolor=(0.9,0.9,0.9))
U3_function,=ax5.plot([],[],'b',linewidth=1,label='Pitch (U3) [Nm]')
plt.xlim(0,t_ani[-1])
plt.ylim(np.min(UTotal_U3)-0.01,np.max(UTotal_U3)+0.01)
plt.grid(True)
plt.legend(loc='upper right',fontsize='small')
# U4 の出力関数
ax6=fig.add_subplot(gs[3,2],facecolor=(0.9,0.9,0.9))
U4_function,=ax6.plot([],[],'b',linewidth=1,label='Yaw (U4) [Nm]')
plt.xlim(0,t_ani[-1])
plt.ylim(np.min(UTotal_U4)-0.01,np.max(UTotal_U4)+0.01)
plt.grid(True)
plt.legend(loc='upper right',fontsize='small')
plt.xlabel('t-time [s]',fontsize=15)
else:
# VERSION 2
# ドローンの位置： X
ax1=fig.add_subplot(gs[3,0],facecolor=(0.9,0.9,0.9))
ax1.plot(t,X_ref,'b',linewidth=1,label='X_ref [m]')
drone_position_x,=ax1.plot([],[],'r',linewidth=1,label='X [m]')
ax1.set_xlim(0,t_ani[-1])
ax1.set_ylim(np.min(statesTotal_x)-0.01,np.max(statesTotal_x)+0.01)
ax1.legend(loc='lower right',fontsize='small')
plt.grid(True)
plt.xlabel('t-time [s]',fontsize=15)
# ドローンの位置: Y
ax2=fig.add_subplot(gs[3,1],facecolor=(0.9,0.9,0.9))
ax2.plot(t,Y_ref,'b',linewidth=1,label='Y_ref [m]')
drone_position_y,=ax2.plot([],[],'r',linewidth=1,label='Y [m]')
ax2.set_xlim(0,t_ani[-1])
ax2.set_ylim(np.min(statesTotal_y)-0.01,np.max(statesTotal_y)+0.01)
ax2.legend(loc='lower right',fontsize='small')
plt.grid(True)
plt.xlabel('t-time [s]',fontsize=15)
# ドローンの位置: Z
ax3=fig.add_subplot(gs[3,2],facecolor=(0.9,0.9,0.9))
ax3.plot(t,Z_ref,'b',linewidth=1,label='Z_ref [m]')
drone_position_z,=ax3.plot([],[],'r',linewidth=1,label='Z [m]')
plt.xlim(0,t_ani[-1])
plt.ylim(np.min(statesTotal_z)-0.01,np.max(statesTotal_z)+0.01)
plt.grid(True)
plt.legend(loc='lower right',fontsize='small')
plt.xlabel('t-time [s]',fontsize=15)
# Phi軌道の図
ax4=fig.add_subplot(gs[0,2],facecolor=(0.9,0.9,0.9))
ax4.plot(t_angles,ref_angles_total[:,0],'b',linewidth=1,label='Phi_ref [rad]')
drone_orientation_phi,=ax4.plot([],[],'r',linewidth=1,label='Phi [rad]')
plt.xlim(0,t_ani[-1])
plt.ylim(np.min(statesTotal_phi)-0.01,np.max(statesTotal_phi)+0.01)
plt.grid(True)
plt.legend(loc='lower right',fontsize='small')
# Theta軌道の図
ax5=fig.add_subplot(gs[1,2],facecolor=(0.9,0.9,0.9))
ax5.plot(t_angles,ref_angles_total[:,1],'b',linewidth=1,label='Theta_ref [rad]')
drone_orientation_theta,=ax5.plot([],[],'r',linewidth=1,label='Theta [rad]')
plt.xlim(0,t_ani[-1])
plt.ylim(np.min(statesTotal_theta)-0.01,np.max(statesTotal_theta)+0.01)
plt.grid(True)
plt.legend(loc='lower right',fontsize='small')
# Psi軌道の図
ax6=fig.add_subplot(gs[2,2],facecolor=(0.9,0.9,0.9))
ax6.plot(t_angles,ref_angles_total[:,2],'b',linewidth=1,label='Psi_ref [rad]')
drone_orientation_psi,=ax6.plot([],[],'r',linewidth=1,label='Psi [rad]')
plt.xlim(0,t_ani[-1])
plt.ylim(np.min(statesTotal_psi)-0.01,np.max(statesTotal_psi)+0.01)
plt.grid(True)
plt.legend(loc='lower right',fontsize='small')
drone_ani=animation.FuncAnimation(fig, update_plot,
frames=frame_amount,interval=20,repeat=True,blit=True)
plt.show()
##################### END OF THE ANIMATION ############################
no_plots=support.constants['no_plots']
if no_plots==1:
exit()
else:
# 3次元空間のプロット
ax=plt.axes(projection='3d')
ax.plot(X_ref,Y_ref,Z_ref,'b',label='reference')
ax.plot(statesTotal_x,statesTotal_y,statesTotal_z,'r',label='trajectory')
ax.set_xlabel('X [m]')
ax.set_ylabel('Y [m]')
ax.set_zlabel('Z [m]')
ax.legend()
plt.show()
# 位置と速度のプロット
plt.subplot(2,1,1)
plt.plot(t,X_ref,'b',linewidth=1,label='X_ref')
plt.plot(t_ani,statesTotal_x,'r',linewidth=1,label='X')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('X [m]',fontsize=15)
plt.grid(True)
plt.legend(loc='center right',fontsize='small')
plt.subplot(2,1,2)
plt.plot(t,X_dot_ref,'b',linewidth=1,label='X_dot_ref')
plt.plot(t,velocityXYZ_total[0:len(velocityXYZ_total):innerDyn_length,0],'r',linewidth=1,label='X_dot')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('X_dot [m/s]',fontsize=15)
plt.grid(True)
plt.legend(loc='center right',fontsize='small')
plt.show()
# print(Y_dot_ref[0]-velocityXYZ_total[0,1])
plt.subplot(2,1,1)
plt.plot(t,Y_ref,'b',linewidth=1,label='Y_ref')
plt.plot(t_ani,statesTotal_y,'r',linewidth=1,label='Y')
# plt.plot(t,Y_ref-statesTotal2[:,7],'g',linewidth=1,label='D')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('Y [m]',fontsize=15)
plt.grid(True)
plt.legend(loc='center right',fontsize='small')
plt.subplot(2,1,2)
plt.plot(t,Y_dot_ref,'b',linewidth=1,label='Y_dot_ref')
plt.plot(t,velocityXYZ_total[0:len(velocityXYZ_total):innerDyn_length,1],'r',linewidth=1,label='Y_dot')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('Y_dot [m/s]',fontsize=15)
plt.grid(True)
plt.legend(loc='center right',fontsize='small')
plt.show()
plt.subplot(2,1,1)
plt.plot(t,Z_ref,'b',linewidth=1,label='Z_ref')
plt.plot(t_ani,statesTotal_z,'r',linewidth=1,label='Z')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('Z [m]',fontsize=15)
plt.grid(True)
plt.legend(loc='center right',fontsize='small')
plt.subplot(2,1,2)
plt.plot(t,Z_dot_ref,'b',linewidth=1,label='Z_dot_ref')
plt.plot(t,velocityXYZ_total[0:len(velocityXYZ_total):innerDyn_length,2],'r',linewidth=1,label='Z_dot')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('Z_dot [m/s]',fontsize=15)
plt.grid(True)
plt.legend(loc='center right',fontsize='small')
plt.show()
# 角度と角速度のプロット
plt.subplot(3,1,1)
plt.plot(t_angles,ref_angles_total[:,0],'b',linewidth=1,label='Phi_ref')
plt.plot(t_ani,statesTotal_phi,'r',linewidth=1,label='Phi')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('Phi [rad]',fontsize=15)
plt.grid(True)
plt.legend(loc='lower right',fontsize='small')
plt.subplot(3,1,2)
plt.plot(t_angles,ref_angles_total[:,1],'b',linewidth=1,label='Theta_ref')
plt.plot(t_ani,statesTotal_theta,'r',linewidth=1,label='Theta')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('Theta [rad]',fontsize=15)
plt.grid(True)
plt.legend(loc='lower right',fontsize='small')
plt.subplot(3,1,3)
plt.plot(t_angles,ref_angles_total[:,2],'b',linewidth=1,label='Psi_ref')
plt.plot(t_ani,statesTotal_psi,'r',linewidth=1,label='Psi')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('Psi [rad]',fontsize=15)
plt.grid(True)
plt.legend(loc='lower right',fontsize='small')
plt.show()
# コントローラーの入力値のプロット
plt.subplot(4,2,1)
plt.plot(t_angles,UTotal[0:len(UTotal),0],'b',linewidth=1,label='U1')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('U1 [N]',fontsize=15)
plt.grid(True)
plt.legend(loc='upper right',fontsize='small')
plt.subplot(4,2,3)
plt.plot(t_angles,UTotal[0:len(UTotal),1],'b',linewidth=1,label='U2')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('U2 [Nm]',fontsize=15)
plt.grid(True)
plt.legend(loc='upper right',fontsize='small')
plt.subplot(4,2,5)
plt.plot(t_angles,UTotal[0:len(UTotal),2],'b',linewidth=1,label='U3')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('U3 [Nm]',fontsize=15)
plt.grid(True)
plt.legend(loc='upper right',fontsize='small')
plt.subplot(4,2,7)
plt.plot(t_angles,UTotal[0:len(UTotal),3],'b',linewidth=1,label='U4')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('U4 [Nm]',fontsize=15)
plt.grid(True)
plt.legend(loc='upper right',fontsize='small')
plt.subplot(4,2,2)
plt.plot(t_angles,omegas_bundle[0:len(omegas_bundle),0],'b',linewidth=1,label='omega 1')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('omega 1 [rad/s]',fontsize=15)
plt.grid(True)
plt.legend(loc='upper right',fontsize='small')
plt.subplot(4,2,4)
plt.plot(t_angles,omegas_bundle[0:len(omegas_bundle),1],'b',linewidth=1,label='omega 2')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('omega 2 [rad/s]',fontsize=15)
plt.grid(True)
plt.legend(loc='upper right',fontsize='small')
plt.subplot(4,2,6)
plt.plot(t_angles,omegas_bundle[0:len(omegas_bundle),2],'b',linewidth=1,label='omega 3')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('omega 3 [rad/s]',fontsize=15)
plt.grid(True)
plt.legend(loc='upper right',fontsize='small')
plt.subplot(4,2,8)
plt.plot(t_angles,omegas_bundle[0:len(omegas_bundle),3],'b',linewidth=1,label='omega 4')
plt.xlabel('t-time [s]',fontsize=15)
plt.ylabel('omega 4 [rad/s]',fontsize=15)
plt.grid(True)
plt.legend(loc='upper right',fontsize='small')
plt.show()
drone_calc
drone_calc
import numpy as np
import matplotlib.pyplot as plt
class SupportFilesDrone:
''' 次の関数はメインファイルと関連しています。'''
def __init__(self):
''' 定数をロードします'''
# 機体の情報 (Astec Hummingbird)
Ix = 0.0034 # kg*m^2
Iy = 0.0034 # kg*m^2
Iz = 0.006 # kg*m^2
m = 0.698 # kg
g = 9.81 # m/s^2
Jtp=1.302*10**(-6) # N*m*s^2=kg*m^2
Ts=0.1 # s
# コスト関数の行列の重み (対角である必要があります)
Q=np.matrix('10 0 0;0 10 0;0 0 10') # 出力の重み (最後のサンプルを除くすべてのサンプル)
S=np.matrix('20 0 0;0 20 0;0 0 20') # 最終ホライズン期間出力の重み
R=np.matrix('10 0 0;0 10 0;0 0 10') # 入力の重み
ct = 7.6184*10**(-8)*(60/(2*np.pi))**2 # N*s^2
cq = 2.6839*10**(-9)*(60/(2*np.pi))**2 # N*m*s^2
l = 0.171 # m
controlled_states=3 # 姿勢の出力: Phi, Theta, Psi
hz = 4 # horizon period
innerDyn_length=4 # 内部制御ループの反復数
# 軸の位置
px=np.array([-1,-2])
py=np.array([-1,-2])
pz=np.array([-1,-2])
# # 複素数の極
# px=np.array([-0.1+0.3j,-0.1-0.3j])
# py=np.array([-0.1+0.3j,-0.1-0.3j])
# pz=np.array([-1+1.3j,-1-1.3j])
r=2
f=0.025
height_i=5
height_f=25
pos_x_y=0 # デフォルト: 0。視覚的な目的のために、正の x と y を長くします (1-はい、0-いいえ)。 UAV のダイナミクスには影響しません。
sub_loop=5 # アニメーション目的のため
sim_version=1 # 1 または 2 のみです。それに応じて、アニメーションで異なるグラフが表示されます。
# 空気の抗力:
drag_switch=0 # 0 または 1 のいずれかである必要があります (0 - 抗力オフ、1 - 抗力オン)
# 空気の抗力係数[-]:
C_D_u=1.5
C_D_v=1.5
C_D_w=2.0
# 抗力断面積 [m^2]
A_u=2*l*0.01+0.05**2
A_v=2*l*0.01+0.05**2
A_w=2*2*l*0.01+0.05**2
# 空気密度
rho=1.225 # [kg/m^3]
trajectory=7 # 参照軌道を選択: 1 ～ 9 のみ
no_plots=0 # 0 - プロットが表示されます。 1 - プロットはスキップします (アニメーションのみ)
self.constants={'Ix':Ix, 'Iy':Iy, 'Iz':Iz, 'm':m, 'g':g, 'Jtp':Jtp, 'Ts':Ts,\
'Q':Q, 'S':S, 'R':R, 'ct':ct, 'cq':cq, 'l':l, 'controlled_states':controlled_states,\
'hz':hz, 'innerDyn_length':innerDyn_length, 'px':px, 'py':py, 'pz':pz,\
'r':r, 'f':f, 'height_i':height_i, 'height_f':height_f, 'pos_x_y':pos_x_y,\
'sub_loop':sub_loop, 'sim_version':sim_version, 'drag_switch':drag_switch,\
'C_D_u':C_D_u, 'C_D_v':C_D_v, 'C_D_w':C_D_w, 'A_u':A_u, 'A_v':A_v, 'A_w':A_w,\
'rho':rho, 'trajectory':trajectory, 'no_plots':no_plots}
return None
def trajectory_generator(self,t):
'''ドローンがたどる参照軌道を作成します。'''
Ts=self.constants['Ts']
innerDyn_length=self.constants['innerDyn_length']
r=self.constants['r']
f=self.constants['f']
height_i=self.constants['height_i']
height_f=self.constants['height_f']
trajectory=self.constants['trajectory']
d_height=height_f-height_i
# ドローン軌道の x、y、z 次元を定義する
alpha=2*np.pi*f*t
if trajectory==1 or trajectory==2 or trajectory==3 or trajectory==4:
# Trajectory 1
x=r*np.cos(alpha)
y=r*np.sin(alpha)
z=height_i+d_height/(t[-1])*t
x_dot=-r*np.sin(alpha)*2*np.pi*f
y_dot=r*np.cos(alpha)*2*np.pi*f
z_dot=d_height/(t[-1])*np.ones(len(t))
x_dot_dot=-r*np.cos(alpha)*(2*np.pi*f)**2
y_dot_dot=-r*np.sin(alpha)*(2*np.pi*f)**2
z_dot_dot=0*np.ones(len(t))
if trajectory==2:
# Trajectory 2
# 軌道 1 との評価軌道
x[101:len(x)]=2*(t[101:len(t)]-t[100])/20+x[100]
y[101:len(y)]=2*(t[101:len(t)]-t[100])/20+y[100]
z[101:len(z)]=z[100]+d_height/t[-1]*(t[101:len(t)]-t[100])
x_dot[101:len(x_dot)]=1/10*np.ones(len(t[101:len(t)]))
y_dot[101:len(y_dot)]=1/10*np.ones(len(t[101:len(t)]))
z_dot[101:len(z_dot*(t/20))]=d_height/(t[-1])*np.ones(len(t[101:len(t)]))
x_dot_dot[101:len(x_dot_dot)]=0*np.ones(len(t[101:len(t)]))
y_dot_dot[101:len(y_dot_dot)]=0*np.ones(len(t[101:len(t)]))
z_dot_dot[101:len(z_dot_dot)]=0*np.ones(len(t[101:len(t)]))
elif trajectory==3:
# Trajectory 3
# 軌道 1 との評価軌道
x[101:len(x)]=2*(t[101:len(t)]-t[100])/20+x[100]
y[101:len(y)]=2*(t[101:len(t)]-t[100])/20+y[100]
z[101:len(z)]=z[100]+d_height/t[-1]*(t[101:len(t)]-t[100])**2
x_dot[101:len(x_dot)]=1/10*np.ones(len(t[101:len(t)]))
y_dot[101:len(y_dot)]=1/10*np.ones(len(t[101:len(t)]))
z_dot[101:len(z_dot)]=2*d_height/(t[-1])*(t[101:len(t)]-t[100])
x_dot_dot[101:len(x_dot_dot)]=0*np.ones(len(t[101:len(t)]))
y_dot_dot[101:len(y_dot_dot)]=0*np.ones(len(t[101:len(t)]))
z_dot_dot[101:len(z_dot_dot)]=2*d_height/t[-1]*np.ones(len(t[101:len(t)]))
elif trajectory==4:
# Trajectory 4
# 軌道 1 との評価軌道
x[101:len(x)]=2*(t[101:len(t)]-t[100])/20+x[100]
y[101:len(y)]=2*(t[101:len(t)]-t[100])/20+y[100]
z[101:len(z)]=z[100]+50*d_height/t[-1]*np.sin(0.1*(t[101:len(t)]-t[100]))
x_dot[101:len(x_dot)]=1/10*np.ones(len(t[101:len(t)]))
y_dot[101:len(y_dot)]=1/10*np.ones(len(t[101:len(t)]))
z_dot[101:len(z_dot)]=5*d_height/t[-1]*np.cos(0.1*(t[101:len(t)]-t[100]))
x_dot_dot[101:len(x_dot_dot)]=0*np.ones(len(t[101:len(t)]))
y_dot_dot[101:len(y_dot_dot)]=0*np.ones(len(t[101:len(t)]))
z_dot_dot[101:len(z_dot_dot)]=-0.5*d_height/t[-1]*np.sin(0.1*(t[101:len(t)]-t[100]))
elif trajectory==5 or trajectory==6:
if trajectory==5:
power=1
else:
power=2
if power == 1:
# Trajectory 5
r_2=r/15
x=(r_2*t**power+r)*np.cos(alpha)
y=(r_2*t**power+r)*np.sin(alpha)
z=height_i+d_height/t[-1]*t
x_dot=r_2*np.cos(alpha)-(r_2*t+r)*np.sin(alpha)*2*np.pi*f
y_dot=r_2*np.sin(alpha)+(r_2*t+r)*np.cos(alpha)*2*np.pi*f
z_dot=d_height/(t[-1])*np.ones(len(t))
x_dot_dot=-r_2*np.sin(alpha)*4*np.pi*f-(r_2*t+r)*np.cos(alpha)*(2*np.pi*f)**2
y_dot_dot=r_2*np.cos(alpha)*4*np.pi*f-(r_2*t+r)*np.sin(alpha)*(2*np.pi*f)**2
z_dot_dot=0*np.ones(len(t))
else:
# Trajectory 6
r_2=r/500
x=(r_2*t**power+r)*np.cos(alpha)
y=(r_2*t**power+r)*np.sin(alpha)
z=height_i+d_height/t[-1]*t
x_dot=power*r_2*t**(power-1)*np.cos(alpha)-(r_2*t**(power)+r)*np.sin(alpha)*2*np.pi*f
y_dot=power*r_2*t**(power-1)*np.sin(alpha)+(r_2*t**(power)+r)*np.cos(alpha)*2*np.pi*f
z_dot=d_height/(t[-1])*np.ones(len(t))
x_dot_dot=(power*(power-1)*r_2*t**(power-2)*np.cos(alpha)-power*r_2*t**(power-1)*np.sin(alpha)*2*np.pi*f)-(power*r_2*t**(power-1)*np.sin(alpha)*2*np.pi*f+(r_2*t**power+r_2)*np.cos(alpha)*(2*np.pi*f)**2)
y_dot_dot=(power*(power-1)*r_2*t**(power-2)*np.sin(alpha)+power*r_2*t**(power-1)*np.cos(alpha)*2*np.pi*f)+(power*r_2*t**(power-1)*np.cos(alpha)*2*np.pi*f-(r_2*t**power+r_2)*np.sin(alpha)*(2*np.pi*f)**2)
z_dot_dot=0*np.ones(len(t))
elif trajectory==7:
# Trajectory 7
x=2*t/20+1
y=2*t/20-2
z=height_i+d_height/t[-1]*t
x_dot=1/10*np.ones(len(t))
y_dot=1/10*np.ones(len(t))
z_dot=d_height/(t[-1])*np.ones(len(t))
x_dot_dot=0*np.ones(len(t))
y_dot_dot=0*np.ones(len(t))
z_dot_dot=0*np.ones(len(t))
elif trajectory==8:
# Trajectory 8
x=r/5*np.sin(alpha)+t/100
y=t/100-1
z=height_i+d_height/t[-1]*t
x_dot=r/5*np.cos(alpha)*2*np.pi*f+1/100
y_dot=1/100*np.ones(len(t))
z_dot=d_height/(t[-1])*np.ones(len(t))
x_dot_dot=-r/5*np.sin(alpha)*(2*np.pi*f)**2
y_dot_dot=0*np.ones(len(t))
z_dot_dot=0*np.ones(len(t))
elif trajectory==9:
# Trajectory 9
wave_w=1
x=r*np.cos(alpha)
y=r*np.sin(alpha)
z=height_i+7*d_height/t[-1]*np.sin(wave_w*t)
x_dot=-r*np.sin(alpha)*2*np.pi*f
y_dot=r*np.cos(alpha)*2*np.pi*f
z_dot=7*d_height/(t[-1])*np.cos(wave_w*t)*wave_w
x_dot_dot=-r*np.cos(alpha)*(2*np.pi*f)**2
y_dot_dot=-r*np.sin(alpha)*(2*np.pi*f)**2
z_dot_dot=-7*d_height/(t[-1])*np.sin(wave_w*t)*wave_w**2
else:
print(" 1 ～ 9 の参照軌道を選択してください。")
exit()
# x と y のベクトルはサンプル時間ごとに変化します
dx=x[1:len(x)]-x[0:len(x)-1]
dy=y[1:len(y)]-y[0:len(y)-1]
dz=z[1:len(z)]-z[0:len(z)-1]
dx=np.append(np.array(dx[0]),dx)
dy=np.append(np.array(dy[0]),dy)
dz=np.append(np.array(dz[0]),dz)
# 参照ヨー角を定義する
psi=np.zeros(len(x))
psiInt=psi
psi[0]=np.arctan2(y[0],x[0])+np.pi/2
psi[1:len(psi)]=np.arctan2(dy[1:len(dy)],dx[1:len(dx)])
# 回転量を追跡するためにヨー角が必要です
dpsi=psi[1:len(psi)]-psi[0:len(psi)-1]
psiInt[0]=psi[0]
for i in range(1,len(psiInt)):
if dpsi[i-1]<-np.pi:
psiInt[i]=psiInt[i-1]+(dpsi[i-1]+2*np.pi)
elif dpsi[i-1]>np.pi:
psiInt[i]=psiInt[i-1]+(dpsi[i-1]-2*np.pi)
else:
psiInt[i]=psiInt[i-1]+dpsi[i-1]
return x, x_dot, x_dot_dot, y, y_dot, y_dot_dot, z, z_dot, z_dot_dot, psiInt
def pos_controller(self,X_ref,X_dot_ref,X_dot_dot_ref,Y_ref,Y_dot_ref,Y_dot_dot_ref,Z_ref,Z_dot_ref,Z_dot_dot_ref,Psi_ref,states):
'''この関数は位置コントローラーです。開ループ システムに必要な U1、および MPC コントローラーのファイおよびシータ角度を計算します。'''
# 定数を取得
m=self.constants['m']
g=self.constants['g']
px=self.constants['px']
py=self.constants['py']
pz=self.constants['pz']
# 状態を割り振る
# States: [u,v,w,p,q,r,x,y,z,phi,theta,psi]
u = states[0]
v = states[1]
w = states[2]
x = states[6]
y = states[7]
z = states[8]
phi = states[9]
theta = states[10]
psi = states[11]
# u,v,w with x_dot,y_dot,z_dot の回転行列
R_x=np.array([[1, 0, 0],[0, np.cos(phi), -np.sin(phi)],[0, np.sin(phi), np.cos(phi)]])
R_y=np.array([[np.cos(theta),0,np.sin(theta)],[0,1,0],[-np.sin(theta),0,np.cos(theta)]])
R_z=np.array([[np.cos(psi),-np.sin(psi),0],[np.sin(psi),np.cos(psi),0],[0,0,1]])
R_matrix=np.matmul(R_z,np.matmul(R_y,R_x))
pos_vel_body=np.array([[u],[v],[w]])
pos_vel_fixed=np.matmul(R_matrix,pos_vel_body)
x_dot=pos_vel_fixed[0]
y_dot=pos_vel_fixed[1]
z_dot=pos_vel_fixed[2]
# 誤差を計算
ex=X_ref-x
ex_dot=X_dot_ref-x_dot
ey=Y_ref-y
ey_dot=Y_dot_ref-y_dot
ez=Z_ref-z
ez_dot=Z_dot_ref-z_dot
# フィードバック定数を計算する
kx1=(px[0]-(px[0]+px[1])/2)**2-(px[0]+px[1])**2/4
kx2=px[0]+px[1]
kx1=kx1.real
kx2=kx2.real
ky1=(py[0]-(py[0]+py[1])/2)**2-(py[0]+py[1])**2/4
ky2=py[0]+py[1]
ky1=ky1.real
ky2=ky2.real
kz1=(pz[0]-(pz[0]+pz[1])/2)**2-(pz[0]+pz[1])**2/4
kz2=pz[0]+pz[1]
kz1=kz1.real
kz2=kz2.real
# 位置コントローラーの値 vx、vy、vz を計算します。
ux=kx1*ex+kx2*ex_dot
uy=ky1*ey+ky2*ey_dot
uz=kz1*ez+kz2*ez_dot
vx=X_dot_dot_ref-ux[0]
vy=Y_dot_dot_ref-uy[0]
vz=Z_dot_dot_ref-uz[0]
# phi, theta, U1を計算する。
a=vx/(vz+g)
b=vy/(vz+g)
c=np.cos(Psi_ref)
d=np.sin(Psi_ref)
tan_theta=a*c+b*d
Theta_ref=np.arctan(tan_theta)
if Psi_ref>=0:
Psi_ref_singularity=Psi_ref-np.floor(abs(Psi_ref)/(2*np.pi))*2*np.pi
else:
Psi_ref_singularity=Psi_ref+np.floor(abs(Psi_ref)/(2*np.pi))*2*np.pi
if ((np.abs(Psi_ref_singularity)<np.pi/4 or np.abs(Psi_ref_singularity)>7*np.pi/4) or (np.abs(Psi_ref_singularity)>3*np.pi/4 and np.abs(Psi_ref_singularity)<5*np.pi/4)):
tan_phi=np.cos(Theta_ref)*(np.tan(Theta_ref)*d-b)/c
else:
tan_phi=np.cos(Theta_ref)*(a-np.tan(Theta_ref)*c)/d
Phi_ref=np.arctan(tan_phi)
U1=(vz+g)*m/(np.cos(Phi_ref)*np.cos(Theta_ref))
return Phi_ref, Theta_ref, U1
def LPV_cont_discrete(self,states,omega_total):
'''3つの回転軸に関するLPVモデルです。'''
# 必要な定数を取得する
Ix=self.constants['Ix'] # kg*m^2
Iy=self.constants['Iy'] # kg*m^2
Iz=self.constants['Iz'] # kg*m^2
Jtp=self.constants['Jtp'] #N*m*s^2=kg*m^2
Ts=self.constants['Ts'] #s
# 状態を割り当てる
# States: [u,v,w,p,q,r,x,y,z,phi,theta,psi]
u=states[0]
v=states[1]
w=states[2]
p=states[3]
q=states[4]
r=states[5]
phi=states[9]
theta=states[10]
psi=states[11]
# u,v,w with x_dot,y_dot,z_dot の回転行列
R_x=np.array([[1, 0, 0],[0, np.cos(phi), -np.sin(phi)],[0, np.sin(phi), np.cos(phi)]])
R_y=np.array([[np.cos(theta),0,np.sin(theta)],[0,1,0],[-np.sin(theta),0,np.cos(theta)]])
R_z=np.array([[np.cos(psi),-np.sin(psi),0],[np.sin(psi),np.cos(psi),0],[0,0,1]])
R_matrix=np.matmul(R_z,np.matmul(R_y,R_x))
pos_vel_body=np.array([[u],[v],[w]])
pos_vel_fixed=np.matmul(R_matrix,pos_vel_body)
x_dot=pos_vel_fixed[0]
y_dot=pos_vel_fixed[1]
z_dot=pos_vel_fixed[2]
x_dot=x_dot[0]
y_dot=y_dot[0]
z_dot=z_dot[0]
# phi_dot, theta_dot, psi_dot, のための転置行列(T行列)が必要です
# p、q、r と phi_dot、theta_dot、psi_dot を関連付ける変換行列
T_matrix=np.array([[1,np.sin(phi)*np.tan(theta),np.cos(phi)*np.tan(theta)],\
[0,np.cos(phi),-np.sin(phi)],\
[0,np.sin(phi)/np.cos(theta),np.cos(phi)/np.cos(theta)]])
rot_vel_body=np.array([[p],[q],[r]])
rot_vel_fixed=np.matmul(T_matrix,rot_vel_body)
phi_dot=rot_vel_fixed[0]
theta_dot=rot_vel_fixed[1]
psi_dot=rot_vel_fixed[2]
phi_dot=phi_dot[0]
theta_dot=theta_dot[0]
psi_dot=psi_dot[0]
# 連続 LPV A、B、C、D 行列を作成する
A01=1
A13=-omega_total*Jtp/Ix
A15=theta_dot*(Iy-Iz)/Ix
A23=1
A31=omega_total*Jtp/Iy
A35=phi_dot*(Iz-Ix)/Iy
A45=1
A51=(theta_dot/2)*(Ix-Iy)/Iz
A53=(phi_dot/2)*(Ix-Iy)/Iz
A=np.zeros((6,6))
B=np.zeros((6,3))
C=np.zeros((3,6))
D=0
A[0,1]=A01
A[1,3]=A13
A[1,5]=A15
A[2,3]=A23
A[3,1]=A31
A[3,5]=A35
A[4,5]=A45
A[5,1]=A51
A[5,3]=A53
B[1,0]=1/Ix
B[3,1]=1/Iy
B[5,2]=1/Iz
C[0,0]=1
C[1,2]=1
C[2,4]=1
D=np.zeros((3,3))
# システムの離散化 (フォワード オイラー法)
Ad=np.identity(np.size(A,1))+Ts*A
Bd=Ts*B
Cd=C
Dd=D
return Ad,Bd,Cd,Dd,x_dot,y_dot,z_dot,phi,phi_dot,theta,theta_dot,psi,psi_dot
def mpc_simplification(self, Ad, Bd, Cd, Dd, hz):
'''この関数は、モデル予測制御用のコンパクトな行列を作成します。'''
# db - double bar
# dbt - double bar transpose
# dc - double circumflex
A_aug=np.concatenate((Ad,Bd),axis=1)
temp1=np.zeros((np.size(Bd,1),np.size(Ad,1)))
temp2=np.identity(np.size(Bd,1))
temp=np.concatenate((temp1,temp2),axis=1)
A_aug=np.concatenate((A_aug,temp),axis=0)
B_aug=np.concatenate((Bd,np.identity(np.size(Bd,1))),axis=0)
C_aug=np.concatenate((Cd,np.zeros((np.size(Cd,0),np.size(Bd,1)))),axis=1)
D_aug=Dd
Q=self.constants['Q']
S=self.constants['S']
R=self.constants['R']
CQC=np.matmul(np.transpose(C_aug),Q)
CQC=np.matmul(CQC,C_aug)
CSC=np.matmul(np.transpose(C_aug),S)
CSC=np.matmul(CSC,C_aug)
QC=np.matmul(Q,C_aug)
SC=np.matmul(S,C_aug)
Qdb=np.zeros((np.size(CQC,0)*hz,np.size(CQC,1)*hz))
Tdb=np.zeros((np.size(QC,0)*hz,np.size(QC,1)*hz))
Rdb=np.zeros((np.size(R,0)*hz,np.size(R,1)*hz))
Cdb=np.zeros((np.size(B_aug,0)*hz,np.size(B_aug,1)*hz))
Adc=np.zeros((np.size(A_aug,0)*hz,np.size(A_aug,1)))
for i in range(0,hz):
if i == hz-1:
Qdb[np.size(CSC,0)*i:np.size(CSC,0)*i+CSC.shape[0],np.size(CSC,1)*i:np.size(CSC,1)*i+CSC.shape[1]]=CSC
Tdb[np.size(SC,0)*i:np.size(SC,0)*i+SC.shape[0],np.size(SC,1)*i:np.size(SC,1)*i+SC.shape[1]]=SC
else:
Qdb[np.size(CQC,0)*i:np.size(CQC,0)*i+CQC.shape[0],np.size(CQC,1)*i:np.size(CQC,1)*i+CQC.shape[1]]=CQC
Tdb[np.size(QC,0)*i:np.size(QC,0)*i+QC.shape[0],np.size(QC,1)*i:np.size(QC,1)*i+QC.shape[1]]=QC
Rdb[np.size(R,0)*i:np.size(R,0)*i+R.shape[0],np.size(R,1)*i:np.size(R,1)*i+R.shape[1]]=R
for j in range(0,hz):
if j<=i:
Cdb[np.size(B_aug,0)*i:np.size(B_aug,0)*i+B_aug.shape[0],np.size(B_aug,1)*j:np.size(B_aug,1)*j+B_aug.shape[1]]=np.matmul(np.linalg.matrix_power(A_aug,((i+1)-(j+1))),B_aug)
Adc[np.size(A_aug,0)*i:np.size(A_aug,0)*i+A_aug.shape[0],0:0+A_aug.shape[1]]=np.linalg.matrix_power(A_aug,i+1)
Hdb=np.matmul(np.transpose(Cdb),Qdb)
Hdb=np.matmul(Hdb,Cdb)+Rdb
temp=np.matmul(np.transpose(Adc),Qdb)
temp=np.matmul(temp,Cdb)
temp2=np.matmul(-Tdb,Cdb)
Fdbt=np.concatenate((temp,temp2),axis=0)
return Hdb,Fdbt,Cdb,Adc
def open_loop_new_states(self,states,omega_total,U1,U2,U3,U4):
'''この関数は、1 サンプル時間後の新しい状態ベクトルを計算します。'''
# 必要定数の取得
Ix=self.constants['Ix']
Iy=self.constants['Iy']
Iz=self.constants['Iz']
m=self.constants['m']
g=self.constants['g']
Jtp=self.constants['Jtp']
Ts=self.constants['Ts']
# States: [u,v,w,p,q,r,x,y,z,phi,theta,psi]
current_states=states
new_states=current_states
u = current_states[0]
v = current_states[1]
w = current_states[2]
p = current_states[3]
q = current_states[4]
r = current_states[5]
x = current_states[6]
y = current_states[7]
z = current_states[8]
phi = current_states[9]
theta = current_states[10]
psi = current_states[11]
sub_loop=self.constants['sub_loop'] # Ts/5
states_ani=np.zeros((sub_loop,6))
U_ani=np.zeros((sub_loop,4))
# 抗力:
drag_switch=self.constants['drag_switch']
C_D_u=self.constants['C_D_u']
C_D_v=self.constants['C_D_v']
C_D_w=self.constants['C_D_w']
A_u=self.constants['A_u']
A_v=self.constants['A_v']
A_w=self.constants['A_w']
rho=self.constants['rho']
# ルンゲ＝クッタ法
u_or=u
v_or=v
w_or=w
p_or=p
q_or=q
r_or=r
x_or=x
y_or=y
z_or=z
phi_or=phi
theta_or=theta
psi_or=psi
Ts_pos=2
for j in range(0,4):
if drag_switch==1:
Fd_u=0.5*C_D_u*rho*u**2*A_u
Fd_v=0.5*C_D_v*rho*v**2*A_v
Fd_w=0.5*C_D_w*rho*w**2*A_w
elif drag_switch==0:
Fd_u=0
Fd_v=0
Fd_w=0
else:
print("init 関数では、drag_switch 変数は 0 または 1 でなければなりません")
exit()
# print(u)
# print(v)
# 傾き k_x
u_dot=(v*r-w*q)+g*np.sin(theta)-Fd_u/m
v_dot=(w*p-u*r)-g*np.cos(theta)*np.sin(phi)-Fd_v/m
w_dot=(u*q-v*p)-g*np.cos(theta)*np.cos(phi)+U1/m-Fd_w/m
p_dot=q*r*(Iy-Iz)/Ix-Jtp/Ix*q*omega_total+U2/Ix
q_dot=p*r*(Iz-Ix)/Iy+Jtp/Iy*p*omega_total+U3/Iy
r_dot=p*q*(Ix-Iy)/Iz+U4/Iz
# 慣性系の状態を取得する
# u,v,w with x_dot,y_dot,z_dot の回転行列
R_x=np.array([[1, 0, 0],[0, np.cos(phi), -np.sin(phi)],[0, np.sin(phi), np.cos(phi)]])
R_y=np.array([[np.cos(theta),0,np.sin(theta)],[0,1,0],[-np.sin(theta),0,np.cos(theta)]])
R_z=np.array([[np.cos(psi),-np.sin(psi),0],[np.sin(psi),np.cos(psi),0],[0,0,1]])
R_matrix=np.matmul(R_z,np.matmul(R_y,R_x))
pos_vel_body=np.array([[u],[v],[w]])
pos_vel_fixed=np.matmul(R_matrix,pos_vel_body)
x_dot=pos_vel_fixed[0]
y_dot=pos_vel_fixed[1]
z_dot=pos_vel_fixed[2]
x_dot=x_dot[0]
y_dot=y_dot[0]
z_dot=z_dot[0]
# phi_dot、theta_dot、psi_dot を取得するには、転置行列が必要です
# p、q、r と phi_dot、theta_dot、psi_dot を関連付ける変換行列
T_matrix=np.array([[1,np.sin(phi)*np.tan(theta),np.cos(phi)*np.tan(theta)],\
[0,np.cos(phi),-np.sin(phi)],\
[0,np.sin(phi)/np.cos(theta),np.cos(phi)/np.cos(theta)]])
rot_vel_body=np.array([[p],[q],[r]])
rot_vel_fixed=np.matmul(T_matrix,rot_vel_body)
phi_dot=rot_vel_fixed[0]
theta_dot=rot_vel_fixed[1]
psi_dot=rot_vel_fixed[2]
phi_dot=phi_dot[0]
theta_dot=theta_dot[0]
psi_dot=psi_dot[0]
# 傾きを保存
if j == 0:
u_dot_k1=u_dot
v_dot_k1=v_dot
w_dot_k1=w_dot
p_dot_k1=p_dot
q_dot_k1=q_dot
r_dot_k1=r_dot
x_dot_k1=x_dot
y_dot_k1=y_dot
z_dot_k1=z_dot
phi_dot_k1=phi_dot
theta_dot_k1=theta_dot
psi_dot_k1=psi_dot
elif j == 1:
u_dot_k2=u_dot
v_dot_k2=v_dot
w_dot_k2=w_dot
p_dot_k2=p_dot
q_dot_k2=q_dot
r_dot_k2=r_dot
x_dot_k2=x_dot
y_dot_k2=y_dot
z_dot_k2=z_dot
phi_dot_k2=phi_dot
theta_dot_k2=theta_dot
psi_dot_k2=psi_dot
elif j == 2:
u_dot_k3=u_dot
v_dot_k3=v_dot
w_dot_k3=w_dot
p_dot_k3=p_dot
q_dot_k3=q_dot
r_dot_k3=r_dot
x_dot_k3=x_dot
y_dot_k3=y_dot
z_dot_k3=z_dot
phi_dot_k3=phi_dot
theta_dot_k3=theta_dot
psi_dot_k3=psi_dot
Ts_pos=1
else:
u_dot_k4=u_dot
v_dot_k4=v_dot
w_dot_k4=w_dot
p_dot_k4=p_dot
q_dot_k4=q_dot
r_dot_k4=r_dot
x_dot_k4=x_dot
y_dot_k4=y_dot
z_dot_k4=z_dot
phi_dot_k4=phi_dot
theta_dot_k4=theta_dot
psi_dot_k4=psi_dot
if j<3:
# k_x を使用した新しい状態
u=u_or+u_dot*Ts/Ts_pos
v=v_or+v_dot*Ts/Ts_pos
w=w_or+w_dot*Ts/Ts_pos
p=p_or+p_dot*Ts/Ts_pos
q=q_or+q_dot*Ts/Ts_pos
r=r_or+r_dot*Ts/Ts_pos
x=x_or+x_dot*Ts/Ts_pos
y=y_or+y_dot*Ts/Ts_pos
z=z_or+z_dot*Ts/Ts_pos
phi=phi_or+phi_dot*Ts/Ts_pos
theta=theta_or+theta_dot*Ts/Ts_pos
psi=psi_or+psi_dot*Ts/Ts_pos
else:
# 平均 k_x を使用した新しい状態
u=u_or+1/6*(u_dot_k1+2*u_dot_k2+2*u_dot_k3+u_dot_k4)*Ts
v=v_or+1/6*(v_dot_k1+2*v_dot_k2+2*v_dot_k3+v_dot_k4)*Ts
w=w_or+1/6*(w_dot_k1+2*w_dot_k2+2*w_dot_k3+w_dot_k4)*Ts
p=p_or+1/6*(p_dot_k1+2*p_dot_k2+2*p_dot_k3+p_dot_k4)*Ts
q=q_or+1/6*(q_dot_k1+2*q_dot_k2+2*q_dot_k3+q_dot_k4)*Ts
r=r_or+1/6*(r_dot_k1+2*r_dot_k2+2*r_dot_k3+r_dot_k4)*Ts
x=x_or+1/6*(x_dot_k1+2*x_dot_k2+2*x_dot_k3+x_dot_k4)*Ts
y=y_or+1/6*(y_dot_k1+2*y_dot_k2+2*y_dot_k3+y_dot_k4)*Ts
z=z_or+1/6*(z_dot_k1+2*z_dot_k2+2*z_dot_k3+z_dot_k4)*Ts
phi=phi_or+1/6*(phi_dot_k1+2*phi_dot_k2+2*phi_dot_k3+phi_dot_k4)*Ts
theta=theta_or+1/6*(theta_dot_k1+2*theta_dot_k2+2*theta_dot_k3+theta_dot_k4)*Ts
psi=psi_or+1/6*(psi_dot_k1+2*psi_dot_k2+2*psi_dot_k3+psi_dot_k4)*Ts
for k in range(0,sub_loop):
states_ani[k,0]=x_or+(x-x_or)/Ts*(k/(sub_loop-1))*Ts
states_ani[k,1]=y_or+(y-y_or)/Ts*(k/(sub_loop-1))*Ts
states_ani[k,2]=z_or+(z-z_or)/Ts*(k/(sub_loop-1))*Ts
states_ani[k,3]=phi_or+(phi-phi_or)/Ts*(k/(sub_loop-1))*Ts
states_ani[k,4]=theta_or+(theta-theta_or)/Ts*(k/(sub_loop-1))*Ts
states_ani[k,5]=psi_or+(psi-psi_or)/Ts*(k/(sub_loop-1))*Ts
U_ani[:,0]=U1
U_ani[:,1]=U2
U_ani[:,2]=U3
U_ani[:,3]=U4
# ルンゲ・クッタ法ここまで
# 次の状態を取得します
new_states[0]=u
new_states[1]=v
new_states[2]=w
new_states[3]=p
new_states[4]=q
new_states[5]=r
new_states[6]=x
new_states[7]=y
new_states[8]=z
new_states[9]=phi
new_states[10]=theta
new_states[11]=psi
return new_states, states_ani, U_ani
なお，以下にdrone_calcの簡単な説明をしておきます。主にドローンのシミュレーションに必要な力学関係のパラメータなので，必要なら読んでください。状況に応じて，パラメータをいじることで，よりリアルな環境に近づきます。
Ix, Iy, Iz：ドローンの慣性モーメント
m：ドローンの質量
g：重力加速度
Jtp：推力の慣性モーメント
Ts：サンプリング時間
Q, S, R：コスト関数の重み
ct, cq：推力とトルクの係数
l：ドローンのアームの長さ
controlled_states：制御される状態の数
hz：ホライゾン期間
innerDyn_length：内部制御ループの反復回数
px, py, pz：極座標
r, f, height_i, height_f：飛行経路のパラメータ
pos_x_y：xとyの位置を視覚的に長くするかどうか
sub_loop：アニメーションのためのサブループ
sim_version：シミュレーションのバージョン
drag_switch：抗力を考慮するかどうか
C_D_u, C_D_v, C_D_w：抗力係数
A_u, A_v, A_w：抗力の断面積
rho：空気密度
trajectory：選択する飛行経路
no_plots：プロットを表示するかどうか
trajectory_generator(self,t)
はドローンの参照軌道を生成するためのものです。軌道は、指定された時間tに対して、ドローンのx、y、z座標とその速度（一時微分）および加速度（二次微分）を計算します。また、ドローンがどの方向を向いているのかを決定するパラメータのヨー角（psi：）も計算します。
pos_controller(self,X_ref,X_dot_ref,X_dot_dot_ref,Y_ref,Y_dot_ref,Y_dot_dot_ref,Z_ref,Z_dot_ref,Z_dot_dot_ref,Psi_ref,states):
はドローンの位置制御コントローラーです。以下に簡略化したブロック図を書きます。
LPV_cont_discrete(self,states,omega_total)はLPV(線形パラメータモデル)というのを使用して，ドローンの動力学を再現しています。
mpc_simplification(self, Ad, Bd, Cd, Dd, hz)はモデル予測制御を使用して制御します。筆者はあまり理解していませんが，使えたのでヨシとします。
詳しくは
また，ドローンの本としては以下をお勧めします。
open_loop_new_states(self,states,omega_total,U1,U2,U3,U4)はドローンの動力学モデルです。このモデルは、ドローンの運動を6つの自由度（前後、左右、上下の移動、およびピッチ、ロール、ヨーの回転）で記述します。このモデルは、ドローンの運動を正確にシミュレートしています。
参照軌道の変更
参照軌道を9つ設定しています。（デフォルトは7）
drone_calc89行目の
drone_calc
- trajectory=self.constants['trajectory'] = 7 #(1~9の間で変更)
+ trajectory=self.constants['trajectory'] = 1 #(1~9の間で変更)
してみてください。
最後に
ドローンは航空力学等も入ってくるので，簡単に理解できないです。
段階としては
- 制御工学
- 文献の多い制御ロボット（倒立系やロボットアーム）
- 車
- 飛行機orヘリコプター
- ドローン
といくのが自然です。上の階層であれば，あるほど本があります。段階を踏んだほうがいいです。私のように初期でつまずかないようにしましょう。
Discussion