剛体としてのドローンのシミュレーション（２）：むだ時間ありモデル

 PID制御に信号と動作の間の遅延があった場合を分析する。この遅延は「むだ時間」と呼ばれている。

先のシミュレーションの目的もこのむだ時間があった場合の分析を目的としていた。そして、むだ時間があるとPID制御に大きな困難をもたらすことがわかった。その点が、モデルを剛体を考慮したものにしたときどうなるのかをここで調べる。

以前のシミュレーション記事のリストは以下にあるので、必要に応じて参照していただきたい。

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

剛体としてみたドローンモデルの運動式は、

$$I\frac{d\omega}{dt}=L(f_{1}-f_{2})$$

となり、前の記事と同じである。ESCからの信号とトルク発生の間にむだ時間が発生するとしよう。むだ時間を$\tau$で表す。これによって回転トルクを発生させる力$f_{1}, f_{2}$は次のように表される。ただし、$\omega$は角速度で、$\omega=d\theta/dt$である。

$$f_{1}=\delta\left(V_{0}-P\theta_{t-\tau}-D\left.\frac{d\theta}{dt}\right|_{t-\tau}\right)^{2}=\delta\left(V_{0}-P\theta_{t-\tau}-D\omega_{t-\tau}\right)^{2}$$

$$f_{2}=\delta\left(V_{0}+P\theta_{t-\tau}+D\left.\frac{d\theta}{dt}\right|_{t-\tau}\right)^{2}=\delta\left(V_{0}+P\theta_{t-\tau}+D\omega_{t-\tau}\right)^{2}$$

右辺は、$\tau$期前のPID制御の信号に基づいてプロペラの回転数が実現しトルクが発生していることを示している。この右辺をラグ関数として$lag(\theta, \omega)$としよう（システムが前のものとは簡単になったのであえて定義する必要がないかもしれないが）。すなわち、

$$lag(\theta, \omega)=-\frac{4L\delta V_{0}}{I}(P\theta+D\omega)$$

となる。そして、次のような、一階の連立微分方程式を考える。

$$\frac{d\theta_{t}}{dt}=\omega_{t}$$

$$\frac{d\omega_{t}}{dt}=lag(\theta_{t-\tau}, \omega_{t-\tau})$$

これに4次のルンゲ・クッタ法を適応する。（ルンゲ・クッタ法については、前のシミュレーションの（２）の記事などを参考にしていただきたい）

これは（６）の記事にある漸化式から$\cos \theta$に関わるものを除いた式と、形が一致するので、それを載せると次のようになる。

$$\begin{eqnarray*}k_{1}^{\theta}&=&h\omega_{n}\\k_{1}^{\omega}&=&h(lag(\theta_{n-\tau}, \omega_{n-\tau}))\\k_{2}^{\theta}&=&h(\omega_{n}+\frac{k_{1}^{\omega}}{2})\\k_{2}^{\omega}&=&h(lag(\theta_{n-\tau}, \omega_{n-\tau}))\\k_{3}^{\theta}&=&h(\omega_{n}+\frac{k_{2}^{\omega}}{2})\\k_{3}^{\omega}&=&h(lag(\theta_{n-\tau}, \omega_{n-\tau}))\\k_{4}^{\theta}&=&h(\omega_{n}+k_{3}^{\omega})\\k_{4}^{\omega}&=&h(lag(\theta_{n-\tau}, \omega_{n-\tau}))\\\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*}$$

これは機械的に変換したが、結果的に $k_{1}^{\omega}=k_{2}^{\omega}=k_{3}^{\omega}=k_{4}^{\omega}$ であるからこの値を$k^{\omega}$とおく。漸化式は次のようになる。

$$k^{\omega}=h\cdot lag(\theta_{n-\tau}, \omega_{n-\tau})$$

$$k_{1}^{\theta}=h\omega_{n}$$

$$k_{2}^{\theta}=h(\omega_{n}+\frac{k^{\omega}}{2})$$

$$k_{3}^{\theta}=h(\omega_{n}+\frac{k^{\omega}}{2})$$

$$k_{4}^{\theta}=h(\omega_{n}+k^{\omega})$$

$$\theta_{n+1}=\theta_{n}+\frac{h}{3}(2\omega_{n}+ lag(\theta_{n-\tau}, \omega_{n-\tau}))$$

$$\omega_{n+1}=\omega_{n}+\frac{2h}{3} \cdot lag(\theta_{n-\tau}, \omega_{n-\tau})$$

これによるシミュレーション結果は、次の記事で示す。