ドローン(クアッドコプター)の運動方程式を導出する方法を細かく解説

制御工学

みなさん,こんにちは
おかしょです.

ここ5年程でドローンの需要が急拡大しました.農薬の散布や災害地の調査,配送など広範囲での有効性が期待できるので,技術的進歩も革新的に進んでいます.

私の所属する研究室でも制御対象にクアッドコプターを選ぶ人が増えています.

そこで,この記事ではクアッドコプターの数値シミュレーションをするために必要な運動方程式を導出します.

この記事を読むと以下のようなことがわかる・できるようになります.

  • クアッドコプターの運動方程式
  • 運動方程式の導出方法

 

この記事を読む前に

よくニュースなどでドローンを取り上げられることがありますが,それらは正式にはマルチコプターと呼ばれるものなのでご注意ください.

以下の記事で,ドローンとマルチコプターの違いなどを解説しているので読んでいない方は読んでみてください.

この記事では4つのプロペラを持つ,クアッドコプターを対象とします.

また,座標軸の関係は以下のようになっています.

 

運動方程式の型

運動方程式にはある程度の型があります.

運動方程式を求めたいときは,この型に当てはめて考えていくと簡単に導出することができます.

この運動方程式の型は並進運動と回転運動の二種類があります.

並進運動の方程式の型は以下のようになります.

\[
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}=-C_{d}\begin{bmatrix}
\dot{x}\\ \dot{y}\\ \dot{z}
\end{bmatrix}
\tag{6}
\end{eqnarray}

ここで\(C_{D}\)は空気摩擦係数を表します.

以上のようにしてクアッドコプターの並進運動方程式を求めることができます.

 

回転運動方程式

回転の運動方程式も式(2)にある回転の型にあわせて求めていくと以下のようになります.

\begin{eqnarray}
J\dot{\omega}+\omega \times (J\omega)=\tau \tag{7}
\end{eqnarray}

ここで,\(\omega\)は機体の姿勢角速度を表しています.プロペラの角速度の文字と似ていますが,プロペラの角速度は各プロペラごとに添え字で番号が振られていますので注意してください.

先ほどの並進運動方程式は慣性座標系で表されていましたが,回転運動方程式は機体固定座標系で表されているのでご注意ください.

機体固定座標系のように座標系が回転するときは基底ベクトルも時間変化するので,基底ベクトルも微分することができます.そのため,左辺の第2項が現れてきます.

右辺の\(\tau\)はプロペラによって生じるトルクを表していて,各軸回りで以下のようにして求められます.

\begin{eqnarray}
\tau_{x}=lk(\omega_{1}^{2}-\omega_{3}^{2}) \tag{8}\\
\tau_{y}=lk(\omega_{2}^{2}-\omega_{4}^{2}) \tag{9}\\
\tau_{z}=b(\omega_{1}^{2}-\omega_{2}^{2}+\omega_{3}^{2}-\omega_{4}^{2}) \tag{10}
\end{eqnarray}

ここで,\(l\)はクアッドコプターの重心からモーターまでの距離,\(b\)はプロペラ抗力係数を表します.

式(7)において,\(J\)は慣性モーメントを表しているのですが,クアッドコプターは前後左右対称なので慣性モーメントは以下のような対角行列になります.

\begin{eqnarray}
J=\begin{bmatrix}
J_{xx} & 0 & 0\\
0 & J_{yy} & 0\\
0 & 0 & J_{zz}
\end{bmatrix} \tag{11}
\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}\\

\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}\\

\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}\\

\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}
+\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{12}
\end{eqnarray}

このようにして回転の運動方程式をもとめることができます.

 

キネマティクス方程式

以上のようにして運動方程式を求めることができますが,回転の運動方程式は機体固定座標系なので慣性座標系の姿勢角を求めることができません.

そこで,機体固定座標系の角速度\(\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{13}
\end{eqnarray}

このような姿勢角を求める式をキネマティクス方程式と言います.

 

航法方程式

並進運動方程式も機体固定座標系で表されている場合は,機体の位置を求めるために慣性座標系に変換する必要がありますが,式(3)は慣性座標系で表されているのでその必要はありません.

したがって,機体の位置は式(3)を積分することによって求めることができます.

ちなみに,並進運動方程式が機体固定座標系で表されていた場合は慣性座標系に変換する式のことを航法方程式と言います.

 

モーターの表現

上で求めたような方程式を利用することで数値シミュレーションを行うことができますが,上の式のままでは制御入力がプロペラの回転数\(\omega_{1},\ \omega_{2},\ \omega_{3},\ \omega_{4}\)となっています.

数値シミュレーション上はこれでも良いかもしれませんが,実際のシステムには入力するのは回転数ではなく,モーターに与える電圧になります.

そのため,数値シミュレーションを実際のシステムに近づけるには制御入力をモーターに与える電圧にする必要があります.

この時,電圧とプロペラの回転数の関係によって表現方法はさまざまありますが,ここではプロペラの回転数は電圧に対して比例関係にあるとします.

\begin{eqnarray}
\omega_{i}^{2}=c_{v}v_{i}^{2} \tag{14}
\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}
+\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}

この運動方程式に加えて,式(13)のキネマティクス方程式を用いることで機体の全状態量を求めることができます.

 

続けて読む

この記事ではモーターと電圧の関係を式(14)のように比例関係で表しましたが,より実際のシステムに近づけるためには1次遅れ系で表します.

1次遅れ系のシステムのことに関しては以下の記事で解説しているので参考にしてみてください.

Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください.

それでは最後まで読んでいただきありがとうございました.

コメント

タイトルとURLをコピーしました