みなさん，こんにちは
おかしょです．
最近空撮などで話題のドローン．おそらくみなさんが良く見かけるのはクアッドコプターと呼ばれるプロペラが４つついているものだと思います．この名称の違いなどについてはこちらで解説しているので気になる方は読んでみてください．
さて，このクアッドコプターは自動で飛ばすために多くの研究者が研究を重ねています．そのため，これまでに多くの制御則が開発されてきました．
このような研究をするには，まず数値シミュレーションを行う必要があります．
いきなり制御則をクアッドコプターに実装しても，おそらく成功しません．まずは数値シミュレーション上で開発した制御則を適用してみて，応答を確認してから実際の機体に実装するという流れになります．
この時，数値シミュレーションを行うにはクアッドコプターの運動方程式が必要になります．この記事では，その運動方程式の導出方法を解説します．運動方程式を導出する方法にはニュートン法とラグランジュ法がありますが，この記事ではラグランジュ法による導出方法を解説します．ニュートン法による導出方法を知りたい方はこちらの記事を読んでください．
この記事を読むと以下のようなことがわかる・できるようになります．
- クアッドコプターの運動方程式
- 運動方程式の導出方法
- ラグランジュ法とは
この記事を読む前に
この記事では運動方程式をラグランジュ法で導出しますが，ラグランジュ法の詳細な解説は省きます．簡単には説明しますが，そもそもラグランジュ法とは何なのかを知りたい方は以下の記事で詳細に解説しているので，そちらを参照してください．
座標系の定義
この記事で導出する運動方程式は以下の座標系に則っていますので，最初に確認してください．
上の画像を見るとわかるように機体固定座標系は左手系となっていて，z軸は鉛直下向きとしています．一方，慣性座標系は鉛直上向きとしました．航空機の座標系では一般的に上の図のような座標系が用いられるので，このようにしました．
ただ，クアッドコプターの論文の多くはこのような座標系とはなっていないので，論文を読む際は注意をしてください．なぜか論文では回転の方向が右ネジの法則に則っていません．すべての軸が則っていないのであればいいのですが，軸によってめちゃくちゃな論文ばかりです．そのような座標系は気持ちが悪いので，上記のような座標系を取って運動方程式の導出を行いました．
上の図でプロペラから伸びている赤い矢印はプロペラが回転することによって生じる推力\(f\)を表しています．その近くの円を描いた矢印はプロペラが回転することによって生じる反動トルク\(\tau\)です．それぞれのプロペラの回転する方向が異なっていますが，これは反動トルクによって機体の方位角\(\psi\)が変わってしまうことを防ぐためです．上のようにプロペラの回転方向を変えることによって，反動トルクが打ち消しあい，方位角が変化せずに済みます．
運動方程式の導出
上の図のシステムに対してラグランジュ法を用いて運動方程式を導出する方法について解説していきます．
一般化座標
まずは一般化座標を以下のようにとります．
\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_B&=&
\begin{bmatrix}
F_x\\
F_y\\
F_z
\end{bmatrix}\tag{2}
\end{eqnarray}
次にモーメントは以下のようにおきます．
\begin{eqnarray}
\tau_B&=&
\begin{bmatrix}
\tau_x\\
\tau_y\\
\tau_z
\end{bmatrix}\tag{3}
\end{eqnarray}
上の力とモーメントをまとめると，一般化力は以下のようになります．
\begin{eqnarray}
F =
\begin{bmatrix}
F_B ^T &\tau_B ^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}
D = \frac{1}{2} \mu_v V_B^T V_B+\frac{1}{2} \mu_\omega \omega_B^T \omega_B \tag{11}
\end{eqnarray}
\(\mu_v\)と\(\mu_\omega\)は抗力係数を表します．
ラグランジアン
以上より，ラグランジアンは以下のようになります．
\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{12}
\end{eqnarray}
ラグランジュ方程式
運動方程式を求めるために以下のラグランジュ方程式を解きます．
\begin{eqnarray}
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q}} \right)-\frac{\partial L}{\partial q} +\frac{\partial D}{\partial \dot{q}} = f \tag{13}
\end{eqnarray}
並進方向
まずは\(X_B\)に関して解きます．
\begin{eqnarray}
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{X}_B} \right)-\frac{\partial L}{\partial X_B} +\frac{\partial D}{\partial \dot{X}_B} &=& F_B \\
\frac{d}{dt}\left(\frac{\partial L}{\partial V_B} \right)-\frac{\partial L}{\partial X_B} +\frac{\partial D}{\partial V_B} &=& F_B \\
\frac{d}{dt}\left(mV_B \right)+mgR_v^{B/I} E_z +\mu_v V_B &=& F_B \tag{14}
\end{eqnarray}
ここで，左辺第1項の微分には注意が必要です．上式は機体固定座標系で表されていて，機体固定座標系は機体の姿勢とともに変化します．つまり座標系が時間変化をします．
したがって，機体の速度\(V_B\)だけでなく機体固定座標系も微分することができます．上式では簡略化のために，各変数の向きを表すベクトルを表記していませんでしたが，向きを表す基底ベクトルも書くと以下のようになります．
\begin{eqnarray}
\frac{d}{dt}\left(mV_B E_B \right)+mgR_v^{B/I} E_z E_B+\mu_v V_B E_B = F_B E_B \tag{15}
\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+\mu_v V_B E_B&=& F_B E_B \tag{16}
\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{17}
\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+\mu_v V_B 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-\mu_v V_B 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{\mu_v}{m} V_B E_B+ \frac{1}{m} F_B E_B \tag{18}
\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{\mu_v}{m}
\begin{bmatrix}
u_B\\
v_B\\
w_B
\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{\mu_v}{m}
\begin{bmatrix}
u_B\\
v_B\\
w_B
\end{bmatrix}
+\frac{1}{m}
\begin{bmatrix}
F_x\\
F_y\\
F_z
\end{bmatrix}\tag{19}
\end{eqnarray}
さらに，上式に対して以下の計算をすることで，クアッドコプターの慣性座標系での位置を求めることができます．
\begin{eqnarray}
\dot{X}_I = R_v^{I/B} V_B \tag{20}
\end{eqnarray}
回転方向
次に\(\Theta_B\)について解きます．
\begin{eqnarray}
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\Theta}_B} \right)-\frac{\partial L}{\partial \Theta_B} +\frac{\partial D}{\partial \dot{\Theta}_B} &=& \tau_B \\
\frac{d}{dt}\left(\frac{\partial L}{\partial \omega_B} \right)-\frac{\partial L}{\partial X_B} +\frac{\partial D}{\partial \omega_B} &=& \tau_B \\
\frac{d}{dt}\left(J\omega_B \right)+\mu_\omega \omega_B &=& \tau_B \tag{21}
\end{eqnarray}
ここでも左辺第１項の微分に注意してください．先ほどと同様に上式では基底ベクトルを省略していますが，機体固定座標系の場合は基底ベクトルも微分することができるので上式は以下のようになります．
\begin{eqnarray}
\frac{d}{dt}\left(J\omega_B E_B \right)+\mu_\omega \omega_B E_B &=& \tau_B E_B \\
\frac{d(J\omega_B)}{dt} E_B +(J\omega_B)\dot{E}_B+\mu_\omega \omega_B E_B &=& \tau_B E_B \\
J\dot{\omega}_B E_B +\tilde{\omega}J\omega_B E_B +\mu_\omega \omega_B E_B &=& \tau_B E_B \\
J\dot{\omega}_B E_B &=& -\tilde{\omega}J\omega_B E_B -\mu_\omega \omega_B E_B + \tau_B E_B \\
\dot{\omega}_B E_B &=& J^{-1} (-\tilde{\omega} J\omega_B E_B -\mu_\omega \omega_B E_B + \tau_B E_B) \tag{22}
\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}
-\mu_\omega
\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}
-\mu_\omega
\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}
-\mu_\omega
\begin{bmatrix}
p_B\\
q_B\\
r_B
\end{bmatrix}+
\begin{bmatrix}
\tau_x\\
\tau_y\\
\tau_z
\end{bmatrix}\right) \tag{23}
\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{24}
\end{eqnarray}
機体に働く力とモーメント
最後に，機体に働く力\(F_B\)とモーメント\(\tau_B\)を求めます．機体に働く力は各モーターによって生じる推力のみで表すことができる．クアッドコプターは推力が機体に対してz軸方向のみに働くので以下のように表されます．
\begin{eqnarray}
F_B =
\begin{bmatrix}
F_x\\
F_y\\
F_z
\end{bmatrix}=
\begin{bmatrix}
0\\
0\\
f_1+f_2+f_3+f_4
\end{bmatrix} \tag{25}
\end{eqnarray}
次にモーメントはロールとピッチに関しては推力の差によって生まれ，ヨーに関してはモーターの反動トルクの差によって以下のように表されます．
\begin{eqnarray}
\tau_B =
\begin{bmatrix}
\tau_x\\
\tau_y\\
\tau_z
\end{bmatrix}=
\begin{bmatrix}
L \sin \beta (f_1+f_2-f_3-f_4)\\
L \cos \beta (-f_1+f_2+f_3-f_4)\\
-\tau_1+\tau_2-\tau_3+\tau_4
\end{bmatrix} \tag{26}
\end{eqnarray}
ここで，\(L\)は重心からモーターまでの距離，\(\beta\)はアームの角度を表します．推力\(f_i\)はプロペラの回転数\(\omega_i\)によって以下のように求めることができます．
\begin{eqnarray}
f_i=C_L \omega_i^2 \tag{27}
\end{eqnarray}
ここで，\(C_L\)はプロペラの揚力係数を表しています．さらに，プロペラの回転数\(\omega_i\)はモーターに加わる電圧\(v_i\)によって以下のように表されます．
\begin{eqnarray}
\omega_i^2=c v_i^2 \tag{28}
\end{eqnarray}
ここで，\(c\)は電圧を回転数に変換する定数です．また，反動トルク\(\tau_i\)は推力\(f_i\)と同じようにプロペラの回転数によって以下のように表すことができます．
\begin{eqnarray}
\tau_i =C_D \omega_i^2 \tag{29}
\end{eqnarray}
ここで，\(C_D\)はプロペラの抗力係数を表す．
まとめ
最後に求められた運動方程式を以下にまとめておきます．並進運動方程式は以下のようになります．
\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{\mu_v}{m}
\begin{bmatrix}
u_B\\
v_B\\
w_B
\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}
-\mu_\omega
\begin{bmatrix}
p_B\\
q_B\\
r_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}
機体に働く力は以下のようになります．
\begin{eqnarray}
F_B =
\begin{bmatrix}
F_x\\
F_y\\
F_z
\end{bmatrix}=
\begin{bmatrix}
0\\
0\\
f_1+f_2+f_3+f_4
\end{bmatrix}
\end{eqnarray}
\begin{eqnarray}
f_i=C_L \omega_i^2
\end{eqnarray}
\begin{eqnarray}
\omega_i^2=c v_i^2
\end{eqnarray}
機体に働くモーメントは以下のようになります．
\begin{eqnarray}
\tau_B =
\begin{bmatrix}
\tau_x\\
\tau_y\\
\tau_z
\end{bmatrix}=
\begin{bmatrix}
L \sin \beta (f_1+f_2-f_3-f_4)\\
L \cos \beta (-f_1+f_2+f_3-f_4)\\
-\tau_1+\tau_2-\tau_3+\tau_4
\end{bmatrix}
\end{eqnarray}
\begin{eqnarray}
\tau_i =C_D \omega_i^2
\end{eqnarray}
続けて読む
この記事で導出した運動方程式を使えば，クアッドコプターの数値シミュレーションをすることができます．以下の記事では運動方程式からどのようにして数値シミュレーションを行うのかを解説しています．数値シミュレーションをやりたい方は参考にしてください．
Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください．
YouTubeでも制御工学や電子工作などの解説をしているので，ぜひのぞいてみてください．
それでは最後まで読んでいただきありがとうございました．
コメント