みなさん，こんにちは
おかしょです．
私は制御系の研究室に所属していて，主な制御対象は固定翼航空機です．
ここではシミュレーションなどで使用している固定翼航空機の運動方程式をラグランジュ法で導出したいと思います．
この記事を読むと以下のようなことがわかる・できるようになります．
- 固定翼航空機の運動方程式
- 運動方程式の導出方法
- ラグランジュ法とは
この記事を読む前に
この記事では運動方程式をラグランジュ法で導出しますが，ラグランジュ法の詳細な解説は省きます．簡単には説明しますが，そもそもラグランジュ法とは何なのかを知りたい方は以下の記事で詳細に解説しているので，そちらを参照してください．
座標系の定義
この記事で導出する運動方程式は以下の座標系に則っていますので，最初に確認してください．
上の画像を見るとわかるように機体固定座標系は左手系となっていて，z軸は鉛直下向きとしています．一方，慣性座標系は鉛直上向きとしました．航空機の座標系では一般的に上の図のような座標系が用いられるので，このようにしました．
運動方程式の導出
上の図のシステムに対してラグランジュ法を用いて運動方程式を導出する方法について解説していきます．
一般化座標
まずは一般化座標を以下のようにとります．
\begin{eqnarray}
q&=&
\begin{bmatrix}
x_B & y_B & z_B & \phi_B & \theta_B & \psi_B
\end{bmatrix}^T\\
&=&
\begin{bmatrix}
X_B & \Theta_B
\end{bmatrix}^T\tag{1}
\end{eqnarray}
このように一般化座標はすべて機体固定座標系で表される位置\(X_B\)と姿勢角\(\Theta_B\)としました．
一般化力
次に機体に働く力とモーメントを考えます．
まずは機体に働く力は空気力によって生じるので以下のようにおきます．
\begin{eqnarray}
F_a&=&
\begin{bmatrix}
X_a\\
Y_a\\
Z_a
\end{bmatrix}\tag{2}
\end{eqnarray}
次にモーメントも同様に空気力によって生じます．
\begin{eqnarray}
\tau_a&=&
\begin{bmatrix}
L_a\\
M_a\\
N_a
\end{bmatrix}\tag{3}
\end{eqnarray}
上の力とモーメントをまとめると，一般化力は以下のようになります．
\begin{eqnarray}
F =
\begin{bmatrix}
F_a^T&\tau_a ^T
\end{bmatrix}^T \tag{4}
\end{eqnarray}
エネルギー
続いて，機体のエネルギーを求めます．
運動エネルギー
運動エネルギーは以下のようになります．
\begin{eqnarray}
W = \frac{1}{2}mV_B^T V_B+\frac{1}{2}\omega_B^T J \omega_B \tag{5}
\end{eqnarray}
ここで，\(m\)は機体の質量，\(V_B=[u_B\ v_B\ w_B]^T\)は機体固定座標系で表された機体の速度ベクトル，\(J\)は機体の慣性モーメント行列で以下のように定義されます．
\begin{eqnarray}
J =
\begin{bmatrix}
J_{xx} & 0 & 0 \\
0 & J_{yy} & 0 \\
0 & 0 & J_{zz}
\end{bmatrix} \tag{6}
\end{eqnarray}
\(\omega_B=[p_B\ q_B\ r_B]^T\)は機体固定座標系で表された機体の角速度ベクトルを表します．
位置エネルギー
次に位置エネルギーは以下のようになります．
\begin{eqnarray}
U = mgX_I^T E_z \tag{7}
\end{eqnarray}
ここで，\(g\)は重力加速度，\(E_z\)はz軸の要素のみを取り出すための行列で以下のように定義されます．
\begin{eqnarray}
E_z =
\begin{bmatrix}
0 & 0 & 1
\end{bmatrix}^T \tag{8}
\end{eqnarray}
\(X_I=[x_I\ y_I\ z_I]\)は慣性座標系で表された機体の位置を表します．一般化座標を機体固定座標系で最初に定義したので，機体固定座標系に変換する必要があります．慣性座標系での位置ベクトル\(X_I\)は以下のようにして機体固定座標系に変換することができます．
\begin{eqnarray}
X_I &=&
\begin{bmatrix}
\cos \psi \cos \theta & \cos \psi \sin \theta \sin \phi-\sin \psi \cos \phi & \cos \psi \sin \theta \cos\phi+\sin \psi \sin \phi\\
\sin\psi \cos \theta & \sin\psi \sin \theta \sin \phi+\cos \psi \cos \phi & \sin \psi \sin \theta \cos\phi-\cos \psi \sin \phi\\
-\sin \theta & \cos \theta \sin \phi & \cos \theta \cos \phi
\end{bmatrix} X_B\\
&=& R_v^{I/B} X_B \tag{9}
\end{eqnarray}
したがって，位置エネルギーは以下のようになります．
\begin{eqnarray}
U &=& mg\left(R_v^{I/B} X_B\right)^T E_z\\
&=& mgX_B^T \left(R_v^{I/B}\right)^T E_z\\
&=& mgX_B^T R_v^{B/I} E_z \tag{10}
\end{eqnarray}
ラグランジアン
以上より，ラグランジアンは以下のようになります．
\begin{eqnarray}
L &=& W-U\\
&=& \frac{1}{2} m V_B^T V_B+\frac{1}{2} \omega_B^T J \omega_B -mgX_B^T R_v^{B/I} E_z \tag{11}
\end{eqnarray}
ラグランジュ方程式
運動方程式を求めるために以下のラグランジュ方程式を解きます．
\begin{eqnarray}
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q}} \right)-\frac{\partial L}{\partial q} = f \tag{12}
\end{eqnarray}
並進方向
まずは\(X_B\)に関して解きます．
\begin{eqnarray}
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{X}_B} \right)-\frac{\partial L}{\partial X_B} &=& F_B \\
\frac{d}{dt}\left(\frac{\partial L}{\partial V_B} \right)-\frac{\partial L}{\partial X_B} &=& F_B \\
\frac{d}{dt}\left(mV_B \right)+mgR_v^{B/I} E_z &=& F_B \tag{13}
\end{eqnarray}
ここで，左辺第1項の微分には注意が必要です．上式は機体固定座標系で表されていて，機体固定座標系は機体の姿勢とともに変化します．つまり座標系が時間変化をします．
したがって，機体の速度\(V_B\)だけでなく機体固定座標系も微分することができます．上式では簡略化のために，各変数の向きを表すベクトルを表記していませんでしたが，向きを表す基底ベクトルも書くと以下のようになります．
\begin{eqnarray}
\frac{d}{dt}\left(mV_B E_B \right)+mgR_v^{B/I} E_z E_B = F_B E_B \tag{14}
\end{eqnarray}
ここで，\(E_B=[i_B\ j_B\ k_B]\)は機体固定座標系の基底ベクトルを表します．この基底ベクトルというのは大きさが1の各軸の向きを持つベクトルのことです．この基底ベクトルは時間とともに変化するので，上式を解くと以下のようになります．
\begin{eqnarray}
m\dot{V}_B E_B + mV_B \dot{E}_B +mgR_v^{B/I} E_z E_B&=& F_B E_B \tag{15}
\end{eqnarray}
ここで，左辺大2項の\(V_B \dot{E}_B\)は以下のようになります．
\begin{eqnarray}
V_B \dot{E}_B &=& \tilde{\omega} V_B E_B\\
&=&
\begin{bmatrix}
0 & -r_B & q_B\\
r_B & 0 & -p_B\\
-q_B & p_B & 0
\end{bmatrix}
\begin{bmatrix}
u_B\\
v_B\\
w_B
\end{bmatrix}
\begin{bmatrix}
i_B & j_B & k_B
\end{bmatrix} \tag{16}
\end{eqnarray}
この導出は別の記事で行います．上式を用いると並進運動方程式は以下のようになります．
\begin{eqnarray}
m\dot{V}_B E_B + m\tilde{\omega} V_B E_B +mgR_v^{B/I} E_z E_B&=& F_B E_B \\
m\dot{V}_B E_B &=& – m\tilde{\omega} V_B E_B -mgR_v^{B/I} E_z E_B+ F_B E_B \\
\dot{V}_B E_B &=& – \tilde{\omega} V_B E_B -gR_v^{B/I} E_z E_B+ \frac{1}{m} F_B E_B \tag{17}
\end{eqnarray}
行列で表すと以下のようになります．
\begin{eqnarray}
\frac{d}{dt}
\begin{bmatrix}
u_B\\
v_B\\
w_B
\end{bmatrix}
&=& –
\begin{bmatrix}
0 & -r_B & q_B\\
r_B & 0 & -p_B\\
-q_B & p_B & 0
\end{bmatrix}
\begin{bmatrix}
u_B\\
v_B\\
w_B
\end{bmatrix}
-g
\begin{bmatrix}
\cos \psi \cos \theta & \cos \psi \sin \theta \sin \phi-\sin \psi \cos \phi & \cos \psi \sin \theta \cos\phi+\sin \psi \sin \phi\\
\sin\psi \cos \theta & \sin\psi \sin \theta \sin \phi+\cos \psi \cos \phi & \sin \psi \sin \theta \cos\phi-\cos \psi \sin \phi\\
-\sin \theta & \cos \theta \sin \phi & \cos \theta \cos \phi
\end{bmatrix}^T
\begin{bmatrix}
0\\
0\\
1
\end{bmatrix}
+\frac{1}{m}
\begin{bmatrix}
F_x\\
F_y\\
F_z
\end{bmatrix}\\
&=& –
\begin{bmatrix}
0 & -r_B & q_B\\
r_B & 0 & -p_B\\
-q_B & p_B & 0
\end{bmatrix}
\begin{bmatrix}
u_B\\
v_B\\
w_B
\end{bmatrix}
-g
\begin{bmatrix}
-\sin \theta \\
\cos \theta \sin \phi \\
\cos \theta \cos \phi
\end{bmatrix}
+\frac{1}{m}
\begin{bmatrix}
F_x\\
F_y\\
F_z
\end{bmatrix}\tag{18}
\end{eqnarray}
さらに，上式に対して以下の計算をすることで，クアッドコプターの慣性座標系での位置を求めることができます．
\begin{eqnarray}
\dot{X}_I = R_v^{I/B} V_B \tag{19}
\end{eqnarray}
回転方向
次に\(\Theta_B\)について解きます．
\begin{eqnarray}
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\Theta}_B} \right)-\frac{\partial L}{\partial \Theta_B} &=& \tau_B \\
\frac{d}{dt}\left(\frac{\partial L}{\partial \omega_B} \right)-\frac{\partial L}{\partial X_B} &=& \tau_B \\
\frac{d}{dt}\left(J\omega_B \right) &=& \tau_B \tag{20}
\end{eqnarray}
ここでも左辺の微分に注意してください．先ほどと同様に上式では基底ベクトルを省略していますが，機体固定座標系の場合は基底ベクトルも微分することができるので上式は以下のようになります．
\begin{eqnarray}
\frac{d}{dt}\left(J\omega_B E_B \right) &=& \tau_B E_B \\
\frac{d(J\omega_B)}{dt} E_B +(J\omega_B)\dot{E}_B&=& \tau_B E_B \\
J\dot{\omega}_B E_B +\tilde{\omega}J\omega_B E_B &=& \tau_B E_B \\
J\dot{\omega}_B E_B &=& -\tilde{\omega}J\omega_B E_B+ \tau_B E_B \\
\dot{\omega}_B E_B &=& J^{-1} (-\tilde{\omega} J\omega_B E_B + \tau_B E_B) \tag{21}
\end{eqnarray}
この式を行列で表すと以下のようになります．
\begin{eqnarray}
\frac{d}{dt}
\begin{bmatrix}
p_B\\
q_B\\
r_B
\end{bmatrix}
&=&
\begin{bmatrix}
J_xx & 0 & 0 \\
0 & J_yy & 0 \\
0 & 0 & J_zz
\end{bmatrix}^{-1} \left(-
\begin{bmatrix}
0 & -r_B & q_B\\
r_B & 0 & -p_B\\
-q_B & p_B & 0
\end{bmatrix}
\begin{bmatrix}
J_xx & 0 & 0 \\
0 & J_yy & 0 \\
0 & 0 & J_zz
\end{bmatrix}
\begin{bmatrix}
p_B\\
q_B\\
r_B
\end{bmatrix}
+\begin{bmatrix}
\tau_x\\
\tau_y\\
\tau_z
\end{bmatrix}\right)\\
&=&
\begin{bmatrix}
J_xx & 0 & 0 \\
0 & J_yy & 0 \\
0 & 0 & J_zz
\end{bmatrix}^{-1} \left(-
\begin{bmatrix}
0 & -J_{yy}r_B & J_{zz}q_B\\
J_{xx}r_B & 0 & -J_{zz}p_B\\
-J_{xx}q_B & J_{yy}p_B & 0
\end{bmatrix}
\begin{bmatrix}
p_B\\
q_B\\
r_B
\end{bmatrix}
+\begin{bmatrix}
\tau_x\\
\tau_y\\
\tau_z
\end{bmatrix}\right)\\
&=&
\begin{bmatrix}
J_xx & 0 & 0 \\
0 & J_yy & 0 \\
0 & 0 & J_zz
\end{bmatrix}^{-1} \left(-
\begin{bmatrix}
(J_{zz}-J_{yy})q_B r_B\\
(J_{xx}-J_{zz})p_B r_B\\
(J_{yy}-J_{xx})p_B q_B
\end{bmatrix}
+\begin{bmatrix}
\tau_x\\
\tau_y\\
\tau_z
\end{bmatrix}\right) \tag{22}
\end{eqnarray}
さらに，上式に対して以下の計算をすることでクアッドコプターの姿勢角を求めることができます．
\begin{eqnarray}
\dot{\Theta}_I = R_\omega^{I/B} \omega_B
\end{eqnarray}
\begin{eqnarray}
R_\omega^{I/B} =
\begin{bmatrix}
1 & \sin \phi \tan \theta & \cos \phi \tan \theta\\
0 & \cos \phi & -\sin \phi\\
0 & \sin \phi / \cos \theta & \cos \phi / \cos \theta
\end{bmatrix} \tag{23}
\end{eqnarray}
機体に働く空気力の計算方法
固定翼機に働く空気力の計算は非常に難しいです．
本来は実際に固定翼機を用意して風洞試験を行って，さまざまな迎え角に対する空気力のデータを取得する必要があります．
しっかりとした固定翼機を用意できて風洞試験も行う環境が整っている方は上記の方法で空気力のデータを取得して，数値シミュレーションに反映させれば良いのですがなかなかできないと思います．
この場合は線形近似された空気力を使用します．
風洞試験を用いる方法ではより精密な空気力を使用することができるので，より精巧なシミュレーションを行うことができます．しかし，線形近似された空気力では姿勢角が大きく乱れたときの空気力は実際の非線形の空気力とは差異が生じてしまいます．
このようなデメリットはありますが，風洞試験をせずにそれなりの精度のシミュレーションをすることができるので，線形近似された空気力を用いてシミュレーションを行っている方が多いです．
この空気力の計算方法については長くなるので，ほかの記事で解説します．
まとめ
最後に求められた運動方程式を以下にまとめておきます．並進運動方程式は以下のようになります．
\begin{eqnarray}
\frac{d}{dt}
\begin{bmatrix}
u_B\\
v_B\\
w_B
\end{bmatrix}
= –
\begin{bmatrix}
0 & -r_B & q_B\\
r_B & 0 & -p_B\\
-q_B & p_B & 0
\end{bmatrix}
\begin{bmatrix}
u_B\\
v_B\\
w_B
\end{bmatrix}
-g
\begin{bmatrix}
-\sin \theta \\
\cos \theta \sin \phi \\
\cos \theta \cos \phi
\end{bmatrix}
+\frac{1}{m}
\begin{bmatrix}
F_x\\
F_y\\
F_z
\end{bmatrix}
\end{eqnarray}
回転運動方程式は以下のようになります．
\begin{eqnarray}
\frac{d}{dt}
\begin{bmatrix}
p_B\\
q_B\\
r_B
\end{bmatrix}
=
\begin{bmatrix}
J_xx & 0 & 0 \\
0 & J_yy & 0 \\
0 & 0 & J_zz
\end{bmatrix}^{-1} \left(-
\begin{bmatrix}
(J_{zz}-J_{yy})q_B r_B\\
(J_{xx}-J_{zz})p_B r_B\\
(J_{yy}-J_{xx})p_B q_B
\end{bmatrix}
+\begin{bmatrix}
\tau_x\\
\tau_y\\
\tau_z
\end{bmatrix}\right)
\end{eqnarray}
機体の位置を求める航法方程式は以下のようになります．
\begin{eqnarray}
\dot{X}_I = I_z R_v^{I/B} V_B
\end{eqnarray}
\begin{eqnarray}
R_v^{I/B} =
\begin{bmatrix}
\cos \psi \cos \theta & \cos \psi \sin \theta \sin \phi-\sin \psi \cos \phi & \cos \psi \sin \theta \cos\phi+\sin \psi \sin \phi\\
\sin\psi \cos \theta & \sin\psi \sin \theta \sin \phi+\cos \psi \cos \phi & \sin \psi \sin \theta \cos\phi-\cos \psi \sin \phi\\
-\sin \theta & \cos \theta \sin \phi & \cos \theta \cos \phi
\end{bmatrix}
\end{eqnarray}
\begin{eqnarray}
I_z =
\begin{bmatrix}
1 & 0 & 0\\
0 & 1 & 0\\
0 & 0 & -1
\end{bmatrix}
\end{eqnarray}
慣性座標系はz軸が鉛直上向きとなっており，機体固定座標系とは異なるため\(I_z\)が必要になります．さらに，機体の姿勢角を求めるキネマティクス方程式は以下のようになります．
\begin{eqnarray}
\dot{\Theta}_I = R_\omega^{I/B} \omega_B
\end{eqnarray}
\begin{eqnarray}
R_\omega^{I/B} =
\begin{bmatrix}
1 & \sin \phi \tan \theta & \cos \phi \tan \theta\\
0 & \cos \phi & -\sin \phi\\
0 & \sin \phi / \cos \theta & \cos \phi / \cos \theta
\end{bmatrix}
\end{eqnarray}
続けて読む
この記事で導出した運動方程式と別記事で解説している空気力の算出を行えば，固定翼航空機の数値シミュレーションをすることができます．以下の記事では運動方程式からどのようにして数値シミュレーションを行うのかを解説しています．数値シミュレーションをやりたい方は参考にしてください．
Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください．
YouTubeでも制御工学や電子工作などの解説をしているので，ぜひのぞいてみてください．
それでは最後まで読んでいただきありがとうございました．
コメント