PID制御によるドローンの揺れに関する数値シミュレーション（６）：制御遅延モデル

 これまでのモデルは、制御が直ちにモーターの回転数に反映することを前提にしていた。ESCの信号に対して機敏に反応するモータであれば、これまでの分析通りに結果が出るはずである。しかし、私の場合は、開発資金の貧しさによって、1個数百円から千円程度の2212型、KV値が920のトルク重視で、プロペラ9インチ用のモータを使っていると、このような機敏さは期待できない。

実際、最近、光反射センサーによるタコメータを自作し、回転中の4つのプロペラ回転数をリアルタイムで把握して送ってくるシステムをつけたところ、ESCの信号に対して、波形で確認される限り200ms程度の遅延が発生していることがわかった。これが、どれほどの問題か、自作ドローンの安定したホバリングの困難性と、どれほどつながっているかを調べることが、このシリーズの分析を始めた動機だった。

つまり、これからが本番なのである（笑）

お決まりで、これまでの議論のページをリストアップしておく。

目次：PID制御によるドローンの揺れに関する数値シミュレーション

以下の解説で不明なところがあれば、これらを参考にしていただきたい。

まず、ここでは、制御遅延を含むモデルを示そう。

プロペラ回転数$V$に対するPID制御の式は次のようになる。

$$V_{t} = V_{0} -P\theta_{t-\tau}-D\left.\frac{\theta_{t}}{dt}\right|_{t-\tau}$$

ここで、$V$はプロペラ回転数で、$V_{0}$は、ホバリング可能なプロペラ回転数、$\theta$は、ドローンのピッチ角である。$I$制御は、組み込んでいない（傾向的傾斜継続の理由がないので必要性もない）。

すなわち$\tau$期以前のピッチ角に対してモータが反応している状態である。この時、力学的な運動方程式は次のようになる。

$$\begin{eqnarray*}\frac{d^{2}\theta_{t}}{dt^{2}}&=&\frac{\delta}{mL} \left(V_{0}^{2}+P^{2}\theta^{2}_{t-\tau}+D^{2}\left(\left.\frac{d\theta}{dt}\right|_{t-\tau}\right)^{2}-2V_{0}P\theta_{t-\tau}+2P\theta D\left.\frac{d\theta}{dt}\right|_{t-\tau}-2V_{0}D\left.\frac{d\theta}{dt}\right|_{t-\tau}\right)\\& &-\frac{g}{L}\cos \theta_{t}\end{eqnarray*}$$

これは遅延のある二階の非線型常微分方程式で、遅延が加わった分、これまでのもの以上に複雑な面を持っている。非線型上微分方程式の遅延については「微分方程式による計算科学入門」（斎藤、小藤、三井）を参考にしたが、こちらが二階の微分方程式であることもあり、以下の理論モデルに誤りがあるかもしれないので、気づいたらお知らせいただきたい。

まず、連立させて一階の常微分方程式に変換する。

それにあたって、次のようなラグ関数$lag(t)$を定義しておこう。

$$lag(\theta, \omega) = a+b\theta_{t}+c\theta^{2}_{t}+d\omega_{t}+e\theta_{t}\omega_{t}+j\omega_{t}^{2}$$ 

ここに現れているパラメータ$a,b,c,d,e,j$は、常微分方程式の各係数に対応するもので、詳細は（４）に掲載されているもと同じなので、参照していただきたい。

この時、連立微分方程式は次のようになる。

$$\begin{eqnarray*}\frac{d\theta_{t}}{dt} & = & \omega_{t} \\\frac{d\omega_{t}}{dt} & = & lag(\theta_{t-\tau}, \omega_{t-\tau})+s\cos\theta_{t}\end{eqnarray*}$$

ここで遅延に関わる項は、全て関数 $lag(\theta_{t-\tau}, \omega_{t-\tau})$に集約されていて、その関数には当期の変数が現れず、第二式については、当期の変数は最後の項$s\cos\theta_{t}$の中にだけ現れている。また、第一式は、当期の項しか含まれていない。したがって、問題は見通しが良いものとなっている。ラグ関数の項$lag(\theta_{t-\tau}, \omega_{t-\tau})$は、その木までに既に値が確定してしまっているので、当期の中では、与件として扱う、つまり過去の値から決定される定数項として処理すれば良い。

一方、$s\cos\theta_{t}$は、常微分方程式としての適切な処理が必要になる。

その意味では、この遅延付きの常微分方程式は、差分方程式が混じった常微分方程式と考えればよいのではないかと思う。

 $lag(\theta_{t-\tau}, \omega_{t-\tau})$をそのように位置付けて、この連立差分方程式に、これまでと同じように4次のルンゲ・クッタ法を適用し、漸化式を求め、数値計算を行う。

漸化式は次のようになる。

$$\begin{eqnarray*}k_{1}^{\theta}&=&h\omega_{n}\\k_{1}^{\omega}&=&h(lag(\theta_{n-\tau}, \omega_{n-\tau})+s\cos \theta_{n})\\k_{2}^{\theta}&=&h(\omega_{n}+\frac{k_{1}^{\omega}}{2})\\k_{2}^{\omega}&=&h(lag(\theta_{n-\tau}, \omega_{n-\tau})+s\cos(\theta_{n}+\frac{k_{1}^{\theta}}{2}))\\k_{3}^{\theta}&=&h(\omega_{n}+\frac{k_{2}^{\omega}}{2})\\k_{3}^{\omega}&=&h(lag(\theta_{n-\tau}, \omega_{n-\tau})+s\cos (\theta_{n}+\frac{k_{2}^{\theta}}{2}))\\k_{4}^{\theta}&=&h(\omega_{n}+k_{3}^{\omega})\\k_{4}^{\omega}&=&h(lag(\theta_{n-\tau}, \omega_{n-\tau})+s\cos( \theta_{n}+k_{3}^{\theta}))\\\theta_{n+1}&=&\theta_{n}+\frac{1}{6}(k_{1}^{\theta}+k_{2}^{\theta}+k_{3}^{\theta}+k_{4}^{\theta})\\\omega_{n+1}&=&\omega_{n}+\frac{1}{6}(k_{1}^{\omega}+k_{2}^{\omega}+k_{3}^{\omega}+k_{4}^{\omega})\end{eqnarray*}$$

なお、今までは、開始時の状態を初期状態として与えらばよかったが、遅延モデルの初期状態は、ラグの期数分だけ事前に与えなければならない。

これを次の記事（７）で、C++プログラムにして解いてみよう。

このシリーズの記事目次。

目次：PID制御によるドローンの揺れに関する数値シミュレーション