みなさん，こんにちは
おかしょです．
ここ5年程でドローンの需要が急拡大しました．農薬の散布や災害地の調査，配送など広範囲での有効性が期待できるので，技術的進歩も革新的に進んでいます．
私の所属する研究室でも制御対象にクアッドコプターを選ぶ人が増えています．と言うより，固定翼機を扱う人は私だけで他の人はみんなクアッドコプターを対象としています・
そこで，この記事ではクアッドコプターの数値シミュレーションをするために必要な運動方程式を導出します．
運動方程式の導出方法にはニュートン法とラグランジュ法があります．この記事ではニュートン法での導出方法を解説するので，ラグランジュ法での導出法を知りたい方はそちらの記事を読んでみてください．
この記事を読むと以下のようなことがわかる・できるようになります．
- クアッドコプターの運動方程式
- 運動方程式の導出方法
- ニュートン法による運動方程式の求め方
この記事を読む前に
よくニュースなどでドローンが取り上げられることがありますが，それらは正式にはマルチコプターと呼ばれるものなのでご注意ください．
以下の記事で，ドローンとマルチコプターの違いなどを解説しているので読んでいない方は読んでみてください．
クアッドコプターの座標軸
この記事では4つのプロペラを持つ，クアッドコプターを対象とします．
また，座標軸の関係は以下のようになっています．
航空機の座標系で一般的に用いられるものと同じように，機体固定座標系はz軸を鉛直下向き，慣性座標系ではz軸は鉛直上向きとします．また，回転の方向はすべて右ネジの法則に従うとしています．
各モーターによって生じる推力は\(f\)で，モーターの反動トルクは\(\tau\)で表されています．\(\tau\)に関してはプロペラが回転する反動によって生じるので右ネジの法則には則っていないことに注意してください．これはプロペラの回転を逆にすることで，推力が一定の時は反動トルクを打ち消すために回転方向が違っています． また，当たり前ですがプロペラの回転は反動トルクとは逆向きであることにも注意してください．
運動方程式の型
運動方程式にはある程度の型があります．運動方程式を求めたいときは，この型に当てはめて考えていくと簡単に導出することができます．
この運動方程式の型は並進運動と回転運動の二種類があります．
並進運動の方程式の型は以下のようになります．
\[
m\ddot{x}=F \tag{1}
\]
この時，\(x\)は機体の位置，\(m\)は質量，\(F\)は機体に働く力を表しています．
回転の運動方程式の型は以下のようになります．
\[
J\ddot{\theta}=M \tag{2}
\]
この時，\(\theta\)は機体の姿勢角，\(J\)は慣性モーメント，\(M\)は機体に働くモーメントを表しています．
クアッドコプターの運動方程式は，この型にあわせて計算することで求めることができます．
並進運動方程式
まずは並進運動方程式を求めます．並進運動方程式を先ほどの式(1)の型にあわせて求めると以下のようになります．
\begin{eqnarray}
m\begin{bmatrix}
\ddot{x}\\ \ddot{y}\\ \ddot{z}
\end{bmatrix}=\begin{bmatrix}
0\\ 0\\ -mg
\end{bmatrix} +C^{I/B}F_{T}+F_{D}
\tag{3}
\end{eqnarray}
この式は慣性座標系で表されています．
右辺第1項は重力に関する項です．第2項は推力，第3項は空気抵抗力に関する項です．
ここで，\(C^{I/B}\)は機体固定座標系から慣性座標系に変換するための回転行列となっていて，以下の式で表されます．
\begin{eqnarray}
C^{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}
\tag{4}
\end{eqnarray}
機体に働く推力\(F_{T}\)は4つのモーターの回転数によって決まります．
\begin{eqnarray}
F_{T}=\begin{bmatrix}
0\\ 0\\ C_{L}
\displaystyle \sum_{i=1}^{4} \omega_{i}^{2}
\end{bmatrix}
\tag{5}
\end{eqnarray}
上式において\(C_{L}\)はプロペラによって生じる揚力係数を表しています．\(\omega_{i}\)は4つあるモータの回転数を意味します．4つのプロペラによって生じる揚力の総和が機体に働く推力となります．
式(3)の右辺第3項の空気抵抗力\(F_{D}\)は以下のように求めることができます．
\begin{eqnarray}
F_{D}=-\mu \begin{bmatrix}
\dot{x}\\ \dot{y}\\ \dot{z}
\end{bmatrix}
\tag{6}
\end{eqnarray}
ここで\(\mu\)は空気摩擦係数を表します．
以上のようにしてクアッドコプターの並進運動方程式を求めることができます．
回転運動方程式
回転の運動方程式も式(2)にある回転の型にあわせて求めていくと以下のようになります．
\begin{eqnarray}
J\dot{\omega}+\omega \times (J\omega)=\tau-\tau_{D} \tag{7}
\end{eqnarray}
ここで，\(\omega\)は機体の姿勢角速度を表しています．プロペラの角速度の文字と似ていますが，プロペラの角速度は各プロペラごとに添え字で番号が振られていますので注意してください．
先ほどの並進運動方程式は慣性座標系で表されていましたが，回転運動方程式は機体固定座標系で表されています．
機体固定座標系のように座標系が回転するときは基底ベクトルも時間変化するので，基底ベクトルも微分することができます．そのため，左辺の第2項が現れてきます．
右辺第１項の\(\tau\)はプロペラによって生じるトルクを表していて，各軸回りで以下のようにして求められます．
\begin{eqnarray}
\tau_{x}&=&LC_{L} \sin \beta (\omega_{1}^{2}+\omega_{2}^{2}-\omega_{3}^{2}-\omega_{4}^{2}) \tag{8}\\
\tau_{y}&=&LC_{L} \cos \beta (-\omega_{1}^{2}+\omega_{2}^{2}+\omega_{3}^{2}-\omega_{4}^{2}) \tag{9}\\
\tau_{z}&=&C_{D}(-\omega_{1}^{2}+\omega_{2}^{2}-\omega_{3}^{2}+\omega_{4}^{2}) \tag{10}
\end{eqnarray}
ここで，\(L\)はクアッドコプターの重心からモーターまでの距離，\(C_{L}\)はプロペラ揚力係数，\(C_{D}\)はプロペラ抗力係数を表します．
右辺第２項の\(\tau_{D}\)は空気の粘性によって生じる抗力で機体の角速度に比例し，以下のように求めることができます．
\begin{eqnarray}
\tau_{D}=-\mu \begin{bmatrix}
\omega_{x}\\ \omega_{y}\\ \omega_{z}
\end{bmatrix}
\tag{11}
\end{eqnarray}
式(7)において，\(J\)は慣性モーメントを表しているのですが，クアッドコプターは前後左右対称なので慣性モーメントは以下のような対角行列になります．
\begin{eqnarray}
J=\begin{bmatrix}
J_{xx} & 0 & 0\\
0 & J_{yy} & 0\\
0 & 0 & J_{zz}
\end{bmatrix} \tag{12}
\end{eqnarray}
したがって，式(7)は以下のように展開できます．
\begin{eqnarray}
\begin{bmatrix}
J_{xx} & 0 & 0\\
0 & J_{yy} & 0\\
0 & 0 & J_{zz}
\end{bmatrix}
\begin{bmatrix}
\dot{\omega_{x}}\\
\dot{\omega_{y}}\\
\dot{\omega_{z}}
\end{bmatrix}
+\begin{bmatrix}
\omega_{x}\\
\omega_{y}\\
\omega_{z}
\end{bmatrix} \times \left(\begin{bmatrix}
J_{xx} & 0 & 0\\
0 & J_{yy} & 0\\
0 & 0 & J_{zz}
\end{bmatrix}
\begin{bmatrix}
\omega_{x}\\
\omega_{y}\\
\omega_{z}
\end{bmatrix}\right)&=&\begin{bmatrix}
\tau_{x}\\
\tau_{y}\\
\tau_{z}
\end{bmatrix}
-\mu \begin{bmatrix}
\omega_{x}\\ \omega_{y}\\ \omega_{z}
\end{bmatrix}\\
\begin{bmatrix}
J_{xx} & 0 & 0\\
0 & J_{yy} & 0\\
0 & 0 & J_{zz}
\end{bmatrix}
\begin{bmatrix}
\dot{\omega_{x}}\\
\dot{\omega_{y}}\\
\dot{\omega_{z}}
\end{bmatrix}
+\begin{bmatrix}
\omega_{x}\\
\omega_{y}\\
\omega_{z}
\end{bmatrix} \times \begin{bmatrix}
J_{xx}\omega_{x}\\
J_{yy}\omega_{y}\\
J_{zz}\omega_{z}
\end{bmatrix}&=&\begin{bmatrix}
\tau_{x}\\
\tau_{y}\\
\tau_{z}
\end{bmatrix}
-\mu \begin{bmatrix}
\omega_{x}\\ \omega_{y}\\ \omega_{z}
\end{bmatrix}\\
\begin{bmatrix}
J_{xx} & 0 & 0\\
0 & J_{yy} & 0\\
0 & 0 & J_{zz}
\end{bmatrix}
\begin{bmatrix}
\dot{\omega_{x}}\\
\dot{\omega_{y}}\\
\dot{\omega_{z}}
\end{bmatrix}
+\begin{bmatrix}
(J_{zz}-J_{yy})\omega_{y}\omega_{z}\\
(J_{xx}-J_{zz})\omega_{x}\omega_{z}\\
(J_{yy}-J_{xx})\omega_{x}\omega_{y}
\end{bmatrix}&=&\begin{bmatrix}
\tau_{x}\\
\tau_{y}\\
\tau_{z}
\end{bmatrix}
-\mu \begin{bmatrix}
\omega_{x}\\ \omega_{y}\\ \omega_{z}
\end{bmatrix}\\
\begin{bmatrix}
J_{xx} & 0 & 0\\
0 & J_{yy} & 0\\
0 & 0 & J_{zz}
\end{bmatrix}
\begin{bmatrix}
\dot{\omega_{x}}\\
\dot{\omega_{y}}\\
\dot{\omega_{z}}
\end{bmatrix}
&=&\begin{bmatrix}
\tau_{x}\\
\tau_{y}\\
\tau_{z}
\end{bmatrix}
-\mu \begin{bmatrix}
\omega_{x}\\ \omega_{y}\\ \omega_{z}
\end{bmatrix}
+\begin{bmatrix}
(J_{yy}-J_{zz})\omega_{y}\omega_{z}\\
(J_{zz}-J_{xx})\omega_{x}\omega_{z}\\
(J_{xx}-J_{yy})\omega_{x}\omega_{y}
\end{bmatrix} \tag{13}
\end{eqnarray}
このようにして回転の運動方程式をもとめることができます．
キネマティクス方程式
以上のようにして運動方程式を求めることができますが，回転の運動方程式は機体固定座標系なので慣性座標系の姿勢角\((\phi ,\ \theta ,\ \psi)\)を求めることができません．そこで，機体固定座標系の角速度\(\omega\)を慣性座標系に変換することで，機体のオイラー角を求めます．
変換するには以下のような式を用います．
\begin{eqnarray}
\begin{bmatrix}
\dot{\phi}\\ \dot{\theta}\\ \dot{\psi}
\end{bmatrix}=\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} \begin{bmatrix}
\omega_{x}\\ \omega_{y}\\ \omega_{z}
\end{bmatrix} \tag{14}
\end{eqnarray}
このような姿勢角を求める式をキネマティクス方程式と言います．
航法方程式
並進運動方程式も機体固定座標系で表されている場合は，機体の位置を求めるために慣性座標系に変換する必要がありますが，式(3)は慣性座標系で表されているのでその必要はありません．したがって，機体の位置は式(3)を積分することによって求めることができます．
ちなみに，並進運動方程式が機体固定座標系で表されていた場合は慣性座標系に変換する式のことを航法方程式と言います．
モーターの表現
上で求めたような方程式を利用することで数値シミュレーションを行うことができますが，上の式のままでは制御入力がプロペラの回転数\((\omega_{1},\ \omega_{2},\ \omega_{3},\ \omega_{4})\)となっています．
数値シミュレーション上はこれでも良いかもしれませんが，実際のシステムには入力するのは回転数ではなく，モーターに与える電圧になります．そのため，数値シミュレーションを実際のシステムに近づけるには制御入力をモーターに与える電圧にする必要があります．
この時，電圧とプロペラの回転数の関係によって表現方法はさまざまありますが，ここではプロペラの回転数は電圧に対して以下のような関係にあるとします．
\begin{eqnarray}
\omega_{i}^{2}=c_{v}v_{i}^{2} \tag{15}
\end{eqnarray}
まとめ
クアッドコプターの運動方程式を導出しました．
この記事で求めた運動方程式をまとめると以下のようになります．
ポイント
\begin{eqnarray}
m\begin{bmatrix}
\ddot{x}\\ \ddot{y}\\ \ddot{z}
\end{bmatrix}&=&\begin{bmatrix}
0\\ 0\\ -mg
\end{bmatrix} +C^{I/B}F_{T}+F_{D}\\
\begin{bmatrix}
J_{xx} & 0 & 0\\
0 & J_{yy} & 0\\
0 & 0 & J_{zz}
\end{bmatrix}
\begin{bmatrix}
\dot{\omega_{x}}\\
\dot{\omega_{y}}\\
\dot{\omega_{z}}
\end{bmatrix}
&=&\begin{bmatrix}
\tau_{x}\\
\tau_{y}\\
\tau_{z}
\end{bmatrix}
-\mu \begin{bmatrix}
\omega_{x}\\ \omega_{y}\\ \omega_{z}
\end{bmatrix}
+\begin{bmatrix}
(J_{yy}-J_{zz})\omega_{y}\omega_{z}\\
(J_{zz}-J_{xx})\omega_{x}\omega_{z}\\
(J_{xx}-J_{yy})\omega_{x}\omega_{y}
\end{bmatrix}
\end{eqnarray}
この運動方程式に加えて，式(14)のキネマティクス方程式を用いることで機体の全状態量を求めることができます．
続けて読む
この記事ではモーターと電圧の関係を式(14)のように比例関係で表しましたが，より実際のシステムに近づけるためには1次遅れ系で表します．
1次遅れ系のシステムのことに関しては以下の記事で解説しているので参考にしてみてください．
Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください．
それでは最後まで読んでいただきありがとうございました．
コメント