第3章運動のモデリング | マルチコプタ制御入門

第2章までで、機体への入力——総推力 $F$ と 3 軸のトルク $\boldsymbol{\tau}$——を作れるようになりました。この章では、その入力を受け取った機体がどう動くのか、すなわち運動方程式を組み立てます。ホバリング近傍の近似で済ませることもできますが、それでは勿体ない。並進運動まで考えるなら、最初からフルのニュートン・オイラー運動方程式を書いてしまうのが結局は見通しがよいのです。以降の章で使う線形化やカスケード制御は、すべて「このフルモデルからの意図的な簡略化」として位置づけられます。

3.1 全体の見取り図

モデル全体は次の因果の連鎖です。この章が担当するのは後半の 2 ブロックです。

第2章指令 → ミキシング → 各モータ推力

第2章推力・反トルク → 合計の力 $F$ とトルク $\boldsymbol{\tau}$

この章剛体の運動方程式 → 角速度・速度

この章運動学 → 姿勢・位置

機体の状態は 12 個の変数で表されます: 位置 $(x, y, z)$、速度 $(v_x, v_y, v_z)$、姿勢（ロール $\phi$、ピッチ $\theta$、ヨー $\psi$）、角速度 $(p, q, r)$。運動方程式とは、この 12 変数の時間微分を入力と現在の状態で書き切ることです。

3.2 二つの座標系をつなぐ: 回転行列

推力は機体に固定された向き（機体 $z$ 軸の負方向）に出ますが、重力は地面に固定された向き（NED の $D$、下向き）に働きます。両者を同じ土俵に乗せるために、機体座標系のベクトルを慣性座標系（NED）へ変換する 回転行列 $R$ を使います。ZYX オイラー角（ヨー→ピッチ→ロールの順に回す）では

$$R = R_z(\psi)\,R_y(\theta)\,R_x(\phi) = \begin{pmatrix} c_\theta c_\psi & s_\phi s_\theta c_\psi - c_\phi s_\psi & c_\phi s_\theta c_\psi + s_\phi s_\psi \\ c_\theta s_\psi & s_\phi s_\theta s_\psi + c_\phi c_\psi & c_\phi s_\theta s_\psi - s_\phi c_\psi \\ -s_\theta & s_\phi c_\theta & c_\phi c_\theta \end{pmatrix}$$

$c_\theta = \cos\theta,\ s_\theta = \sin\theta$ などの略記。

3.3 並進運動 — ニュートンの運動方程式

慣性系で書いた並進の運動方程式は、重力・推力・空気抵抗の 3 つの力の和です:

$$m\,\dot{\boldsymbol{v}} = \underbrace{m g\,\hat{\boldsymbol{e}}_D}_{\text{重力}} + \underbrace{R \begin{pmatrix}0\\0\\-F\end{pmatrix}}_{\text{推力（機体z軸の負方向）}} - \underbrace{c\,\lVert\boldsymbol{v}\rVert\,\boldsymbol{v}}_{\text{空気抵抗（2乗則）}}$$

空気抵抗はオーソドックスに速度の 2 乗に比例する抗力（2乗則）を入れます。係数は $c = \frac{1}{2}\rho\,C_d\,A$。StampFly では概算で $A \approx 0.0815 \times 0.031 \approx 2.5\times10^{-3}\ \mathrm{m^2}$（前面投影面積）、 $C_d \approx 1$（鈍頭物体）とすると $c \approx 1.5\times10^{-3}\ \mathrm{N/(m/s)^2}$ です（未同定の概算値であることに注意）。

この式から、第1章で予告した「傾けて進む」が読み取れます。機体を傾けると $R\,(0,0,-F)^{\mathsf T}$ に水平成分が生まれ、機体は加速します。速度が上がると抗力が 2 乗で増え、やがて釣り合って一定速度になります。傾き $\theta$ で水平に巡航するときの釣り合い速度は

$$v_{eq} = \sqrt{\frac{mg\tan\theta}{c}} \qquad\text{（例: } \theta = 10^\circ \text{ で } v_{eq} \approx 6.4\ \mathrm{m/s}\text{）}$$

コラム — モデルは常に近似: 実は空気抵抗の導入は、研究の現場でも悩みどころです。機体は鈍頭物体で姿勢によって投影面積が変わり、プロペラの誘導流も抗力に影響します。「オーソドックスに 2 乗則」というのは割り切りであって、真実そのものではありません。モデリングとは「制御の目的に足りる精度で、できるだけ簡単に書く」営みだ、ということを覚えておいてください。

3.4 回転運動 — オイラーの運動方程式

回転は機体固定座標系で書くのが便利です（慣性テンソル $I$ が一定になるため）。機体の角運動量 $I\boldsymbol{\omega}$ に加えて、高速で回る 4 つのロータの角運動量 $\boldsymbol{h}$ も機体に乗っていることに注意すると:

$$I\,\dot{\boldsymbol{\omega}} + \underbrace{\boldsymbol{\omega}\times(I\boldsymbol{\omega})}_{\text{機体のジャイロ項}} + \underbrace{\boldsymbol{\omega}\times\boldsymbol{h}}_{\text{ロータのジャイロ項}} = \boldsymbol{\tau}, \qquad \boldsymbol{h} = J_{mp}\sum_i \mathrm{dir}_i\,\omega_i\ \hat{\boldsymbol{e}}_z$$

StampFly の慣性テンソルはほぼ対角で $I = \mathrm{diag}(9.16,\ 13.3,\ 20.4)\times10^{-6}\ \mathrm{kg\,m^2}$。 $\boldsymbol{\tau}$ は第2章のアロケーション行列の出力です。

ロータのジャイロ項には StampFly ならではの面白い構造があります。 CW と CCW のロータが同数・同回転数なら角運動量が相殺して $\boldsymbol{h} = 0$。つまりホバリング中はこの項は消えています。$\boldsymbol{h}$ が顔を出すのは ヨー指令で CW/CCW の回転数が割れたときで、その状態で機体がロールやピッチの角速度を持つと、$\boldsymbol{\omega}\times\boldsymbol{h}$ が「指令していない軸」にトルクを漏らします。

この項を入れる場合と入れない場合の差を、実際に数値で見積もった結果が次の表です。

ロータジャイロトルクの大きさ（StampFly、制御トルク上限 2.08 mN·m に対する比率）。ロータ慣性 $J_{mp}=1.375\times10^{-8}\ \mathrm{kg\,m^2}$ は写真デジタイズ・実測諸元・コーストダウン試験による2026年7月の確定値。
シナリオ	ジャイロトルク	制御トルク上限比
穏やか（ヨー割り 2.5%・レート 1 rad/s）	5 µN·m	0.2%
中間（ヨー割り 12.5%・レート 5 rad/s）	126 µN·m	6.1%
激しい（ヨー割り 12.5%・レート 10 rad/s）	252 µN·m	12.2%

さらに、強いヨーとロールを同時にかける機動（0.2 秒）をフルモデルで積分して比べると、ジャイロ項の有無で 0.4 秒後にピッチが約 10° 違う軌道になります。ホバリング付近の解析では無視してよいが、機動飛行やシミュレータでは入れるべき項—— それが数値からの答えです。下の教材で、項を ON/OFF して確かめられます。

3.5 姿勢の運動学 — 角速度と姿勢角は別物

最後のピースは、機体の角速度 $(p, q, r)$ とオイラー角の変化率 $(\dot\phi, \dot\theta, \dot\psi)$ の関係です。同じものに見えますが、 $(p,q,r)$ は機体軸回り、オイラー角はそれぞれ別の中間軸回りの回転なので、変換行列が挟まります:

$$\begin{pmatrix} \dot\phi \\ \dot\theta \\ \dot\psi \end{pmatrix} = \begin{pmatrix} 1 & \sin\phi\tan\theta & \cos\phi\tan\theta \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi/\cos\theta & \cos\phi/\cos\theta \end{pmatrix} \begin{pmatrix} p \\ q \\ r \end{pmatrix}$$

この行列には $\tan\theta$ と $1/\cos\theta$ が入っています。ピッチ $\theta \to \pm90^\circ$ で発散——これがジンバルロックです。機体が真上や真下を向くと、ロールとヨーの回転軸が同じ向きに重なってしまい、オイラー角では姿勢の変化を表しきれなくなります。宙返りをする機体やシミュレータの内部表現には、この特異点のない クォータニオン（4 つの数で回転を表す方法）を使います。本教材でも、直感はオイラー角で作り、実装の章でクォータニオンに乗り換えます（StampFly のファームウェアもクォータニオンで姿勢を持っています）。

3.6 体感する: フルモデルの解剖

この章の運動方程式をそのまま数値積分し、項ごとに ON/OFF できるようにしました。シナリオは 2 つ。「機動」は強いヨーとロールを同時にかける 0.5 秒（ロータジャイロ項の影響が見えます）。「巡航」は機体を傾けたまま飛ばし続ける 6 秒（空気抵抗が速度を頭打ちにする様子が見えます）。薄い線は全部入りのフルモデル。チェックを外すと、どの項がどれだけ効いていたかが分かります。

ロータジャイロ項 ω×h 機体ジャイロ項 ω×(Iω) 空気抵抗 c|v|v

機動シナリオ: ロールトルク 3×10⁻⁴ N·m とヨー割り 12.5% を 0〜0.2 s に同時印加、以降は無入力。巡航シナリオ: 傾き θ を保持したまま総推力 $mg/\cos\theta$ で高度維持。いずれも本文の式・StampFly の数値と同一（c は概算値）。

まとめ

機体の状態は 12 変数（位置・速度・姿勢・角速度）。運動方程式はその時間発展。
並進はニュートン: 重力 + 回転行列で傾けた推力 + 2乗則の空気抵抗（係数は概算、釣り合い速度 $v_{eq}=\sqrt{mg\tan\theta/c}$）。
回転はオイラー: $I\dot{\boldsymbol\omega} + \boldsymbol\omega\times(I\boldsymbol\omega) + \boldsymbol\omega\times\boldsymbol h = \boldsymbol\tau$。ロータジャイロ項はホバでは消えるが、機動では 0.4 秒でピッチ約 10° 分の差になる。
角速度と姿勢角の間には変換行列が挟まり、ピッチ ±90° で特異（ジンバルロック）。実装はクォータニオンで逃げる。
モデルは常に近似。「制御の目的に足りる精度で、できるだけ簡単に」が指針。

次章では、入力の源であるモータとプロペラそのものに潜り込みます。推力と反トルクの応答、モータの一次遅れ、そして第2章で予告した「積分器＋零点＋一次遅れ」のヨー軸モデルを導出します。

データの出所: 機体仕様（慣性テンソル・寸法・$J_{mp}$）、ジャイロ項の見積もりは notes/calc/gyro_effect.py（再現可能）。