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

 ここで実行する数値シミュレーションモデルは、次の2階非線型常微分方程式で表される。

それは、次の式で表される。

$$\begin{equation}\frac{d^{2}\theta}{dt^{2}}=\frac{1}{L}\left(\frac{\alpha}{m}-g\cos \theta\right)-\frac{\beta \theta}{mL}+\frac{\gamma}{mL} \theta^{2} \end{equation}\label{eq11}$$

この式がどのように導出されるかについては、以下のページで詳しく説明しているので、参照いただきたい。

PID制御によるドローンの揺れに関する数値シミュレーション（１）：理論編

動きを確かめるのは、$\theta$のピッチ角である。

$t$は時間、$L$はドローン中心からモーター位置までの距離、$m$は、ドローンのモーター位置にかかる質量である。$g$は、重力加速度である。また、パラメーター$\alpha, \beta, \gamma$は次のように表される。

$$\begin{eqnarray*}\alpha&=&\delta V_{0}^{2}\\\beta&=&2pV_{0}\delta\\\gamma&=&p^{2}\delta\end{eqnarray*}$$

ここで、$\delta$はモーターの回転数の二乗を力(N)に変換するパラメーターでモーターのパワー水準を表すパラメータである。$V_{0}$は、ホバリング状態を可能にするプロペラ回転数、$p$は、PID制御のうちのP 制御のパラメータである。先のモデル解説でも述べたように、現時点でのモデルでは、P制御のみを組み込み、D（微分）制御は次の課題にしている。（I制御は、モデルの性質上不要と考えている。傾向的傾きを作る潜在要素が見当たらない）

シミュレーションは、常微分方程式に関する4次のルンゲ・クッタ法を用いる。ただし、この常微分方程式は、2階なので、1階の常微分方程式を連立させる方法で行う。そのためにやや複雑になることが避けられない。

$\eqref{eq11}$の係数を$a, b, c, d$にまとめて、見やすくし、次のように表す。

$$\begin{equation}\frac{d^{2}\theta}{dt^{2}}=a-b\cos \theta-c\theta+d\theta^{2}\end{equation}\label{eq12}$$

また、初期値は、$\theta$の初期値$\theta_{0}$と角速度$\frac{d\theta}{dt}=\omega$の初期値$\omega_{0}$が与えられているものとする（シミュレーションでは、妥当な数値を適当に与えればよい）。

この時、上記の二階常微分方程式は、次の連立１階常微分方程式として表される。

$$\begin{eqnarray*}\frac{d\theta}{dt}&=&w\\\frac{d\omega}{dt}&=&a-b\cos \theta-c\theta+d\theta^{2}\end{eqnarray*}$$

この二つの式に、4次のルンゲ・クッタ法を用いる。（4次のルンゲ・クッタ法については、『微分方程式による計算科学入門』三井、小藤、斎藤著、「常微分方程式の数値計算法」山本昌志など、参照）

計算式は、次のような10本の式で表される。以下の、$n: (0,1,2,\cdots)$は時間を表す。

$$\begin{eqnarray*}k_{1}^{\theta}&=&h\omega_{n}\\k_{1}^{\omega}&=&h(\alpha-b\cos(\theta_{n})-c \theta_{n}+d \theta_{n}^{2})\\k_{2}^{\theta}&=&h(\omega_{n}+\frac{k_{1}^{\omega}}{2})\\k_{2}^{\omega}&=&h(a-b\cos(\theta_{n}+\frac{k_{1}^{\theta}}{2})-c( \theta_{n}+\frac{k_{1}^{\theta}}{2})+d (\theta_{n}+\frac{k_{1}^{\theta}}{2})^{2})\\k_{3}^{\theta}&=&h(\omega_{n}+\frac{k_{2}^{\omega}}{2})\\k_{3}^{\omega}&=&h(a-b\cos(\theta_{n}+\frac{k_{2}^{\theta}}{2})-c( \theta_{n}+\frac{k_{2}^{\theta}}{2})+d (\theta_{n}+\frac{k_{2}^{\theta}}{2})^{2})\\k_{4}^{\theta}&=&h(\omega_{n}+k_{3}^{\omega})\\k_{4}^{\omega}&=&h(a-b\cos(\theta_{n}+k_{3}^{\theta})-c( \theta_{n}+k_{3}^{\theta})+d (\theta_{n}+k_{3}^{\theta})^{2})\\\theta_{n+1}&=&\theta_{n}+\frac{1}{6}(k_{1}^{\theta}+2k_{2}^{\theta}+2k_{3}^{\theta}+k_{4}^{\theta})\\\omega_{n+1}&=&\omega_{n}+\frac{1}{6}(k_{1}^{\omega}+2k_{2}^{\omega}+2k_{3}^{\omega}+k_{4}^{\omega})\end{eqnarray*}$$

これがシミュレーションモデルとなる。初期の$\theta_{0}, \omega_{0}$が与えられると、上記の式の上から順にこの値を与えていけば、次期の$\theta_{1}, \omega_{1}$を得ることができる。これを繰り返せば$\theta_{0}, \theta_{1}, \theta_{2}, \theta_{3},\cdots$および$\omega_{0},\omega_{1}, \omega_{2}, \omega_{3},\cdots$を得ることができる。

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