2022年1月31日月曜日

PID制御によるドローンの揺れに関する数値シミュレーション(10):ジャイロ効果シミュレーション

前の(9)では、遅延制御による問題を回避するためにジャイロ効果を利用する理論的モデルを示した。ここでは二階の非線型常微分方程式と数値解用の漸化式で表されたモデルを、計算で解いた結果を示す。

これまでの記事は以下のリンクから見ることができる。


シミュレーションで使用したc++のプログラムは、次のgithubのレポジトリから取得できる。gyroeffect-1.cppという名前のファイルである。

https://github.com/toyowa/pidsimulation/tree/main 

シミュレーション結果を示す。まず、試行錯誤の末に到達した最もよい結果から示す。制御の遅延レベルは、実機の実態を反映した200msに統一している。

1。遅延20期(200ms)、σ = 1.0、P=5000. D=1000

青い線が、機体のピッチ角である。オレンジの線は、モータの回転rpmである。ほぼ迷いなくホバリングの水平姿勢に向け一直線に姿勢を正している。腕の長さや重量などを実機のままに、遅延20期は、これまで全て飛行破綻状態だったが、初めて、遅延のない制御とほぼ同じ結果がもたらされた。おろどくべきことだ。

2。遅延20期(200ms)、σ = 0.5、P=5000. D=1000

他は同じで、ジャイロ効果を半分まで小さくすると、初期の揺れののちにホバリング姿勢に収束していく。
これよりも小さく、例えばシグマを0.25とすると、飛行はすぐに破綻する。すなわち、ジャイロ効果のない事態に近づいていくのである。

逆にシグマを増加させてみよう。

3。遅延20期(200ms)、σ = 2.0、P=5000. D=1000

シグマが1.0の時よりも、滑らかに変化しているが、収束スピードは逆に弱まっている。

PIDのスケールを変えたものも行ったが、ここでのPDの値で、シグマが1.0あたりが一番、綺麗に制御されているように思う。

いずれにしても、ジャイロ効果は、PIDの制御遅延の問題を解決する上で、大きな効果と意味があることがわかった。

PID制御によるドローンの揺れに関する数値シミュレーション(9):ジャイロ効果

これまでの記事は以下のリンクから見ることができる。

これまでのシミュレーションで、フライトコントロールシステムに制御遅延が生じていると、PID制御は強く悪影響を受けて、振動やフライトの破綻が引き起こされることを見た。そして、その対策として、機体の慣性モーメントを増加させることによって、制御遅延の問題を吸収可能であることを示した。しかし、その方法としての、モーターまでの腕の長さを長くしたり、機体をあえて重くしたりすることでは、現実的な解決にはならない。

そこで、機体の中でジャイロ効果を発生させることによって慣性モーメントを大きくすることと同じ効果をもたらすかどうかを確かめることにする。

ジャイロ効果とは、回転体の回転によって回転軸方向を維持する力が発生したり、回転軸の歳差運動などを引き起こす効果を指す。コマが倒れなかったり、歳差運動を引き起こすのがその典型的な例である。

ヘリコプターはその大きなローターによってジャイロ効果を発生させ、それを機体の制御に利用していると言われている。一方ドローンは、ジャイロ効果に依存して制御するのではなく、PID制御など情報処理システムによって機体の姿勢制御を行なっていると言われている。

しかし、ドローンもまたプロペラという明確な回転体を持っているので、ジャイロ効果を発生させていることは確実だ。実際、ある程度の制御の遅延は避けられないはずなのに、市販のメーカードローンは、綺麗に機体の制御が行われている。これについて、ある程度は、プロペラのジャイロ効果が役割を果たしているのではないかと私は考えている。

しかし、DJIのプロペラなどを見ていると、あえてプロペラが発生するジャイロ効果を低減させているのではないかと思えなくもない。それはなぜか。プロペラのジャイロ効果が存在するということは、機敏な姿勢制御ができないことを意味していて、都合が悪いからではないか。優れたレスポンスのモーターさえあれば、プロペラのジャイロ効果に依存しなくても、PID制御は適切に機能するので、それで問題ないはずだからである。

ここでは、これまでの制御遅延が含まれたモデルに、ジャイロ効果を導入し、その効果を確かめる。今回は、モデルの理論的な側面を明らかにし、次回にシミュレーション結果を示す。

まず、プロペラなどの形で(明示的なフライホイール型のジャイロを組み込む場合もある)ジャイロ効果が導入されているとしよう。するとそれは、ピッチ角やロール角の与えられたレベルに対してジャイロ効果が働くのではない。変化に対する効力として働くのであるから、ここでは機体角度の変化分に対してそれを打ち消す方向に力が働くとしよう。

理論的には、この力はジャイロモーメントと言われるもので、方向性を無視すると回転の核速度に比例するので、角度ではなく角度の変化の速さが復元力になるものと考えればよい(これについては、(11)でより詳しい説明を加えているので、そちらを参考にしていただきたい)。方向も交えて説明すると、機体の回転の角運動量を$\vec{L}$で表すとその方向はベクトルの方向である。機体に加わる揺れの角速度をベクトルで$\vec{\Omega}$で表すと、ベクトルの外積で、

$$\vec{T}_{g}=\vec{L}\times\vec{\Omega}$$

のジャイロモーメント$\vec{T}_{g}$を発生させるということである。この方向は、加えられた揺れとは方向が異なったものであり(それをどう処理するかは別問題として)、その揺れを吸収させる力と考えればよい。すなわち、方向は別に、復元力は角度の微分値を使えばよいということである。

この時、(6)で導入された遅延制御モデルの運動方程式は次のように変形される。

\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}-\sigma \frac{d\theta_{t}}{dt}\end{eqnarray*}

最後に$-\sigma \frac{d\theta_{t}}{dt}$がジャイロ効果の項で、$\sigma$はパラメータである。

これを4次のルンゲ・クッタ法で解くために、この二階の非線型微分方程式を一階の連立微分方程式に変換する。

$$\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}+\xi \omega_{t}\end{eqnarray*}$$

ラグ関数$lag(\theta_{t-\tau}, \omega_{t-\tau})$は、パラメータの定義も含め(6)で導入したものと同じである。(パラメータの定義は元々(4)で行われている)

これを解くための漸化式は次のようになる。

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

漸化式は、最後にジャイロ効果の項が加わっているだけで大きく変化していない。また、漸化式の解き方も、これまでの遅延制御モデルと同じである。

次回にその結果を示そう。

2022年1月30日日曜日

P制御とD制御

 数値シミュレーションで、P 制御にD制御が加わると、姿勢安定化への傾向が一挙に強まることわかった。そのことについて、少し考えてみる。

まず、サインカーブとコサインカーブを示しておこう。


青がサインで、赤がコサインだ。いま、例えばドローンの揺れ(ピッチあるいはロール)がサインカーブのように発生していると使用。P制御はその揺れを抑止する形で制御をかける。すなわち、サインカーブを水平軸で裏返したような動きの指示を与える。

これまでみたように、それでは揺れは治らない。また別の揺れを作り出すものだ。ところが、その揺れを微分した制御を加えるのが、D制御だが、 サインの微分はコサインなので、図のコサインカーブの逆向きの制御をかける。それが決定的に重要なのは、振動の位相が90度ずれているということである。

実際の機体が揺れている時の、ロールのP制御、とロールのD制御の値を示している。ただし、激しく細かく揺れ過ぎているので、20期(1期10msなので、200ms)の移動平均を取った値である。よくみると、D制御のピークがP制御のピークから少し早く始まっている。


制御における、位相をずらすことがD制御の大事な役割なのである。微分制御だから急激な変化を弱めるということも言われるが、より大事なのは、この位相をずらした制御だと思っている。

2022年1月29日土曜日

プロペラ用タコメーター(回転数 rpm 計測器)

 ESC信号に対してどれほど正しくモーターが反応しているかを確かめたかった。市販の非接触タコメーターも持っていて、飛行していない時の回転数はそれで計測できたが、飛行中、すなわちPID制御に対するモーターの反応が、信号と並列に捉えられない限り、その辺りが、どうしてもブラックボックスになっていて、精密なシミュレーションをする気にもならなかったが、なんとか自作のプロペラタコメーターをドローンにつなぐことができたので、色々わかってきた。

ここでは、また、もう一度作る時のために、メモがわりにここに書いておく。

反射型光センサーについては、以下のサイトを参考にさせていただいた。

http://nanoappli.com/blog/archives/5051

タコメータは、プロペラ直下に以下のように置かれている。




左側の黒いのが反射型の光センサーRPR220である。右の3ピンコネクタが、上からグラウンド、3.3V電源、信号の順になっている。真ん中の赤いものがLEDで、プロペラが直上に来ると、光る。白いプロペラはカウントできるが、黒いものはカウントできないので、反射シートなどをプロペラに貼る必要がある。

ここでは、小さい基盤に回路を組んで、モーターベースに強力接着テープで貼り付けている。

回路図は、次のようなものである。書き方のルールは特に意識していないが、簡単なので理解いただけると思う。

10Kオームの可変抵抗をつけているが、大事な役割を果たす。というのは、上に来た時の電圧レベルとそうでない時の電圧レベルの絶対水準を変更するもので、プログラム上でその境界電圧を設定するときに、調整して、4つのタコメータが同じになるようにしておかなければ、ややこしくなるからである。

私の場合は、プロペラがない時の信号電圧を1.0V、プロペラがある時のそれを1.9Vに調整し、1.5Vを超えたときにプロペラが通過したと判定するようにしている。この電圧は、RPR220とプロペラの距離によっても変わる。6ミリ程度離れているのが一番感度がいいと書かれていたが、私も、再接近で5mmくらいになるようにタコメーターを設置している。

信号は、SPIのシリアル通信で、ADコンバーター経由で、raspberrypiに送られる。距離センサーの電圧信号をデジタル信号に変換するのは、8チャンネルの、MCP3208を使った(MCP3204は、4チャンネルで、それでもいいのだが)。当初はI2CのADS1115を使っていたが、信号速度の遅さが気になって、圧倒的に早いSPI通信に変えた。

RaspberrypiのSPI通信は、一つのチャンネルが解放されていて、CS0とCS1が使える。CS0が超音波距離センサーのために塞がっているので、CS1にぶら下げた。他のバスは共通に繋いでおけば良い。もし、それ以上のデバイスをぶら下げる場合はRspberryPiは、もう一つSPIチャンネルが潜在的に利用可能になっている(いくつか処理をしなければならないが)。

MCP3208のRaspberryPI用のプログラムは、数多く公開されている。

それらをもとに、基準電圧を超えた回数をカウントするようにすれば、プロペラタコメータになる。

ESC信号とそれに対するモーターの回転数が、例えば次のように捉えられるようになる。

青い線がESCが与えた回転指示で、オレンジの線がそれに対する回転数だ。D制御の細かい揺れは回転数に反映することはできない。

PID制御によるドローンの揺れに関する数値シミュレーション(8):慣性モーメント

 遅延シミュレーション(7)では、制御に遅延がある場合に、深刻な困難が発生することを示した。

問題をどう克服するか。どこで遅延が発生するのかをきちんと把握することが必要である。モーターのパワーあるいは反応特性の可能性は高いと思っているが、制御コンピュータ(私の場合はRaspberryPI 4)からPWMモジュール、そしてESCの間の信号の流れの中で発生している可能性も否定できない。このような問題の所在を突き止めることはまた次の課題にして、ここでは、信号遅延があることを前提に、可能な解決の道を少し探りたいと思う。

これまでの記事については、以下を見ていただきたい。


先の記事で示した、フライトの破綻は、機体の動きに対する必要な制御が遅れることによって、加速度的に事態を悪化させている状況と見ることができる。そこで、機体の反応をゆっくりにして、信号の遅れを許容するようにすることが考えられる。それは、言い換えれば、機体の慣性モーメントを大きくして、動きをゆったりとしたものにすることである。

(1)腕の長さ1m、遅延200ms
中心からモーター位置までの距離を長くすると慣性モーメントは大きくなる。ここではL=1.0メートルにしよう。期待幅が2メートルを超えるドローンになるので、現実にはありえないが、何しろシミュレーションなので簡単である(笑)

プログラムは遅延シミュレーション(7)にリンク先を書いておいたので、そちらからダウンロードできる。そのパラメータLを1.0に変更したわけである。

ピッチ角とモーター回転数の結果は次のようになる。
(7)の結果と決定的に違うのは、加速度的破綻ではなく、一応、振動に留まっていることである。ただ、モーターの回転数が瞬間的に負になっている、フライトが破綻しているという事態は変わりない。その一瞬、モーターが逆回りするようなシステムであれば対応可能だが、まあ、ありえない。P制御と、D制御のスケールは、それぞれ10000と4000であり、遅延がない状況では、一瞬で安定したホバリングが実現できる状況である((5)を参照)。

後半では、ほぼ単振動状態になり、振動の周期は、約1秒である。

(2)腕の長さ1m、遅延100ms
もし、他は上と同じ状況で、遅延が半分の100msにとどまったとしたら次のような結果になる。(タイトルに L=0.5とあるのは、L=1.0の間違い)
揺れは正常に収束する。前のシミュレーションでは100msの遅延でも200msの遅延と全く変わらず、一方的に破綻していたのに、こちらは正常に収束している点では、完成モーめんを大きくすることの効果は絶大と言ってもいい。

その意味では、少しでも遅延を小さくした方がいいのである。まさに、喫緊の課題なのだ。

(3)腕の長さ1m、遅延200ms、P制御10000、D制御3000
(1)の状況から、D制御を3000に小さくしてみた。
比較すると、振動の周期が傾向的に増大、(1)のような単身同状態にはならなくなる。D制御が効かなくなったことがネガティブな影響を与えている。

(4)腕の長さ1m、遅延200ms、P制御5000、D制御4000
(1)の状態から、P制御だけを半分にした。




遅延が200msなのに、かなり急速に安定化している。

(5)腕の長さ1m、質量=1.0Kg、遅延200ms、P制御10000、D制御4000
(1)の状態からモーターにかかる機体荷重が1Kgに増えたとしよう。これは4ロータードローンの場合、機体荷重が4Kgになるということである。

当然、モーター強度も強くならなければならない。モーターの強度は、このシミュレーションモデルの場合、$\delta$で表され、それはホバリングを実現するレベル、与えられた機体荷重のもとで、基準回転数で重力による力と浮力がバランスする値として、自動で計算されるようにしている。その時は、機体の加速度がゼロになっているということである。

結果は次のようになる。


(1)と比べてみると、一瞬でもモーター回転が負になる状況が存在しないという点では、パフォーマンスは改善しているが、単振動化の事実は変わらない。

それでも機体重量の増加による慣性モーメントの増大はポジティブな結果をもたらすことはわかった。

(5)腕の長さ0.8m、質量=1.0Kg、遅延200ms、P制御5000、D制御4000
最後に、一応、いいところを入れたものを一つ示しておく。実用性はないのだが。


慣性モーメント、特に腕の長さが、遅延に対して影響を与えることは確認できた。

記事のシリーズ一覧は以下です。

PID制御によるドローンの揺れに関する数値シミュレーション(7):遅延シミュレーション 

PID制御によるドローンの揺れに関する数値シミュレーション(8):慣性モーメント

PID制御によるドローンの揺れに関する数値シミュレーション(9):ジャイロ効果

PID制御によるドローンの揺れに関する数値シミュレーション(10):ジャイロ効果シミュレーション



2022年1月28日金曜日

PID制御によるドローンの揺れに関する数値シミュレーション(7):遅延シミュレーション

 前の記事で示した、ドローンのPID制御において、制御とプロペラの回転の間に遅延がある場合のモデルを実際に計算してみる。

本稿で不明なところは、以下のページを参考にしてください。

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


モデルは、(6)の記事に示しているもので、それをc++でプログラム化する。

これまでは、EXCELを用いて、結果が直ちにグラフ化されるようにしていたが、遅延を組み込むと実行のたびにモデルの構造を変える必要性が出てくるようなので(モジュールを使えばもっと簡単に行くのだろうが面倒なので)C++で、遅延構造も含めパラメータを変更したのちの結果の数字はすぐにファイルに保存されるようにした。それはcsv形式になっているので、EXCELにも直ちに組み込め、グラフ化できるはずである。

c++のプログラムは、次のgithubのレポジトリから取得できる。ファイル名、pidlag.cppを選んでください。。


(1)遅延が200msの場合(P=10000, D=100)
これまでと同様のP制御に、緩くD制御をかけている。

ピッチ角とモーター回転数のグラフである。

最初から20期分は初期状態として与えたもので、これまでと同様に30度ほどピッチが正になるように期待を傾けて200msど持って、静かに離すというものである。ピッチ角度(青線:目盛左)は0.5でモータスピード(オレンジ線:目盛右)は、ピッチが正に傾いているために、ホバリングよりも少ない8000rpmくらいからスタートしている。90期に近づくと、実際にはありえないが、モーター回転数が負の領域まで落ち込んでしまっている。つまり1秒もたたないうちに飛行が破綻していることになる。

(2)遅延が50msの場合(P=10000, D=100)
遅延を1/4にした。1サイクル10msなので、5サイクル遅れて制御がモーター回転数に伝わるとしている。

状況は基本的に変わっていない。ただし、増加から低下、低下から増加への周期が短くなっている。

(3)遅延が20msの場合(P=10000, D=100)
元の1/10の遅延である。

ついに振動が現れた。

(4)遅延が10msの場合(P=10000, D=100)
1期分の遅れであり、これまでの遅延のないモデルのD制御の弱い状況に大きく近づいている。

(5)遅延が200msの場合(P=10000, D=1000)

元の200msの遅れの状態で、D制御のスケールを10倍にしてみた。状況はほとんど変わっていない。

これらをまとめると、ESCからプロペラの回転までの遅延は、ホバリングの姿勢制御に決定的な影響を与えるということである。なんとも、単純で分かりやすい結論に至った。


PID制御によるドローンの揺れに関する数値シミュレーション(6):制御遅延モデル

 これまでのモデルは、制御が直ちにモーターの回転数に反映することを前提にしていた。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$は、常微分方程式の各係数に対応するもので、詳細は(4)に掲載されているもと同じなので、参照していただきたい。

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

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

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

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


2022年1月26日水曜日

ドローンPID制御の意味

 この間、今現に作製しているドローンの使用に近づけて、物理的な現象をより再現する形で、PID制御をシミュレーションするという作業をやってきた。それで気づいたことをメモがわりにここに書いておく。

まず何よりも、P制御では、一応揺れが収束はしていくものの、機体の安定を実現するのは困難なレベルだった。とろこが、D制御をしっかり入れると、揺れは、あるいは初期の傾きは一挙に解消して水平を実現してしまう。それは驚くべきほどのレベルでそうなった。

その時のモーター回転に対する、P制御とD制御のレベルをグラフに書かせてみた。最終的には、ほとんど揺れなく水平状態になるのだが、あえてD制御のスケールを少し抑え気味にした時の結果をグラフで示しておこう。

これは、D制御を500というスケールで入れた場合だった。オレンジの線が回転数という単位で見たD制御の調整量の推移である。これに対して、青の線は、同じく回転数で見たP制御のスケールである。回転数で見たとは、そのレベルの回転数の調整がモータに指示されるという意味である。このD制御のレベルを2000まで増加すると、揺れなく、ワンショットで、水平状態を達成してしまう。(ひとつ前の記事を見ればその図を見ることができる)

この状況ですら、P制御だけに比べてみれば、より急速に水平かを実現しているものとなっている。なぜD制御を入れるとそうなるのか。それは、見てもわかるように、調整周期のズレである。P制御は実際の機体の傾きを比例的に反映するが、D制御は、実際の傾きに対して周期が微妙に遅れている。その遅れによって、揺れを打ち消しているのである。

今まで、D制御はパルス的な反応に対して、それをまず打ち消す動きとして現れるというような認識でいた。パルス的な動きが重要だと思っていたのである。しかし、そのようなものは、このシミュレーションの動きの中では現れないし、たとえ現れたとしても姿勢安定化の上で大きな役割を果たさないだろう。

実機の飛行状態のモーターの回転数がログされるようになったので、わかってきたことは、そのようなパルス的なD制御の信号に対して、モータは反応できないということである。だから、D制御は、実機では重要な機能を持たせられないなと思い、P制御だけでなんとかならないかと思うようになっていた。そして、姿勢から周期を推測して、P制御のずれた形での制御信号を与えればなんとかなるのではと思っていたのだが、D 制御はまさにそれをやっていたのである。

sinカーブを微分するとcosカーブになる。つまり、1/4周期、微分は遅れるのである。上の図の揺れの1周期は約1.2秒であり、その1/4は300msである。この遅れこそが素早い収束の動きを実現する決定的要素だと考えられる。

やっぱり信号から回転数へのラグの問題を考えないといけないか。


2022年1月25日火曜日

PID制御によるドローンの揺れに関する数値シミュレーション(5):P制御+D制御の計算

 ドローン姿勢のPID制御のうち、P制御とD制御を行なったシミュレーション結果を示す。これまでの議論については、以下のページを参照してください。

PID制御によるドローンの揺れに関する数値シミュレーション(1):理論編

PID制御によるドローンの揺れに関する数値シミュレーション(2):シミュレーションモデル


以下で解説する、EXCELのシミュレーションファイルは、以下からダウンロードできる。(基本的にEXCELで開いてください。Googleのスプレッドシートなど代替ソフトで開くと、計算とグラフ表示に問題が生じる可能性があります。クリックして、Googleのスプレッドシートが開いてしまった場合は、メニューの「ファイル」から一旦ダウンロードしたのちにExcelで再実行してください

EXCELファイル(モジュールなし)のダウンロード(2022/01/28修正)

EXCELファイル(モジュール組み込み済み)のダウンロード(2022/01/28修正)

シミュレーションにはRKMETHODという自作の関数をEXCELのVBAで作成し使用している。モージュールなしを選んだ場合は、それがないというエラーが出るので、「ツール」→「マクロ」からVBAのエディタを開いて、「挿入」→「標準モジュール」をクリックして、エディタを開き、次の関数をコピペしてください。そして、PDパラメータなどを更新すると正しく表示されます。

(次から)

Function RKMETHOD(a As Double, b As Double, c As Double, d As Double, e As Double, j As Double, s As Double, omega As Double, theta As Double, k_omega As Double, k_theta As Double) As Double

    RKMETHOD = a + b * (theta + k_theta) + c * (theta + k_theta) ^ 2 + d * (omega + k_omega) + e * (theta + k_theta) * (omega + k_omega) + j * (omega + k_omega) ^ 2 + s * Cos(theta + k_theta)

End Function

(以上)

ファイルの使い方を説明する。


ファイルを開くと左側に、パラメーターの設定欄、右側にシミュレーションの実行画面が現れる。パラメータを変更すると自動的にシミュレーション結果も変更される。ただし、計算量が多いので、時間が1、2分かかる場合があるかもしれない。その時はそのまま待つしかない。

開いたEXCELシートの左側のパラメータは、私の自作ドローンの使用に沿ったものが書いてある。薄緑蘭のものは、任意に変更が可能である。薄赤の欄は、自動的に変更されるパラメータである。

パラメータの説明を加える。

スロットル(回転数:rpm)
これはドローンのホバリングするプロペラ回転数を書いてください。初期値としては、13000rpmが入力されていますが、小型のドローンでは数万rpmになるのではと思われる。
回転数を力に変換
これは、回転数を水力に変換するパラメータである。が、他が入力されると、機体角度が水平の時の回転数が、上で定義された回転数になるような、適切な値が、自動的に与えられることになっている。これは、理論のページで解説されているので参照して、自力で変更しても問題ない。
機体質量 Kg
これは、機体重量をプロペラの数で割ったものをKg単位で入力することが妥当だ。
中心とモーター間の距離 m
腕の長さである。機体の中心から一つのプロペラの軸までの長さをm単位で入力する。
制御パラメータP
Pパラメータのスケールを記入する。0にすると、P制御が働かない状態になる。
制御パラメータD
Dパラメータのスケールを記入する。0にすると、D制御は働かない。
Δt (時間間隔:秒)
シミュレーションのステップ間隔である。これを大きくすると、計算は速くなるが精度が悪くなる。逆は逆である。

以下、私の自作ドローンのパラメータで実行したシミュレーション結果を示しておこう。初期状態は、ピッチ角が0.5にしてあるので、ピッチが正の方向に30度ほど傾けた状態で、角速度の初期値はゼロにしているので、どの方向にも力を加えず、そのままただ手を離した状態だと考えればよい。
(1)P=10000, D=0
まず、微分制御を入れない、すなわちD=0の場合を調べよう。これは、前に実行したシミュレーションの再現になるはずである。




掲げた図は少し小さいが、ダウンロードしたファイルにP=10000, D=0を代入して実行すれば、このグラフも表示される。
0.8秒くらいの周期で期待が揺れる。自作機の揺れの周期とほぼ等しい感じだ。振幅は変わらない。
振幅は、これ以降もほとんど変わらない。単振動状態である。P制御だけでは姿勢制御は困難なことを意味している。

(2)P=10000, D=100
D制御をD=100のスケールで導入する。




先の場合と比べると、揺れが急速に小さくなっていくことが確認できる。
(3)P=10000, D=1000
さらに10倍のスケールでD制御を加える。




揺れとは言えないほど、ほんの僅かに機体角が負の側に振れただけで、ほぼホバリングを再現している。モーター速度も、一瞬、ホバリングの回転数を超えただけである。
(4)P=10000, D=2000
さらに、D制御のスケールを2倍にした。




ピッチ角は一直線にゼロに収束し、揺れはほとんどなく、一方的にホバリング状態を再現している。ピッチ角がゼロで安定するまでに、0.7秒しかかかっていない。
(5)P=10000, D=5000
さらにD制御を強めたらどうなるか。
モーター速度は、瞬間的に13000rpmを達成しているが、ピッチ角がゼロになるまでに、2秒以上の時間を要するようになり、実用的ではなくなってしまった。


以上の結果は、D制御が高い有効性を持っていることを示している。その一方で、過度に損は姿勢制御に悪影響を及ぼすことがわかる。



PID制御によるドローンの揺れに関する数値シミュレーション(4):P制御+D制御モデル

 これまでの記述については、以下のページを参考にしてください。

PID制御によるドローンの揺れに関する数値シミュレーション(1):理論編

PID制御によるドローンの揺れに関する数値シミュレーション(2):シミュレーションモデル


モデルについての基本的なコンセプトはこれまでの議論を踏襲する。これまでは、PID制御と言ってもP制御だけを組み込んだモデルを扱ってきた。確かに、P制御は基本的なものだが、結果として、機体の揺れは、収束には向かうものの時間がかかったり揺れの大きさでドローンを安定させるためには不十分なものだった。

そこで、ここではさらにD制御もモデルの中に組み込んでその効果を確認する。

すなわち、

$$V=V_{0}-P\theta-D\frac{d\theta}{dt}$$

とする。$V$はモーター回転数(rpm)で、$V_{0}$は、ホバリングを可能にする定常回転数である。$\theta$はピッチ角であり、$t$は時間である。$P$は制御係数でピッチ角に比例して、その揺れを抑える方向に回転数を調整する。$D$は、揺れの急激な変化に反応する微分項である。

モーターの推力$F$は、次のようになる(パラメータの意味および式の解説はこれまでの記事を参考にしていただきたい)。
$$\begin{eqnarray*}F & = & \delta V^{2} \\& = & \delta \left(V_{0}-P\theta-D\frac{d\theta}{dt}\right)^{2} \\& = &  \delta \left(V_{0}^{2}+P^{2}\theta^{2}+D^{2}\left(\frac{d\theta}{dt}\right)^{2}-2V_{0}P\theta+2P\theta D\frac{d\theta}{dt}-2V_{0}D\frac{d\theta}{dt}\right)\end{eqnarray*}$$

これを運動方程式

$$mL\frac{d^{2}\theta}{dt^{2}}=F-mg\cos \theta$$

に代入し変形すると、

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

となる。これまでと同様に、これは特殊な二階非線形常微分方程式で帯域的な解を求めることはできるのかもしれないが、一般に困難なものとなる。ここでの目的は、数値的解が目的なので、一般解にこだわる理由もない。

4次のルンゲ・クッタ法で解くためのモデルを示そう。

まず、この二階の常微分方程式を一階の連立常微分方程式に変換する。角速度を$\omega$とすると、

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

簡単化のため、この第2式を次のように表そう。

$$\frac{d\omega}{dt}=a+b\theta+c\theta^{2}+d\omega+e\theta\omega+j\omega^{2}+s\cos\theta$$

各係数は次のようなものである。

$$\begin{eqnarray*}a & = &  \frac{\delta V_{0}^{2}}{mL}\\b & = &  -\frac{2\delta V_{0}P}{mL} \\c & = &   \frac{\delta P^{2}}{mL}\\d & = &   -\frac{2D\delta V_{0}}{mL}\\e & = &   \frac{2DP\delta}{mL}\\j & = &   \frac{\delta D^{2}}{mL}\\s & = &  -\frac{g}{L}\end{eqnarray*}$$

モデルの記述をわかりやすくするために、ここで次のような関数を定義する。

$$\begin{eqnarray*}f(\omega_{n},\theta_{n},k^{\omega},k^{\theta})&=&a+b(\theta+k^{\theta})+c(\theta^{2}+k^{\theta})+d(\omega+k^{\omega})\\& &+e(\theta+k^{\theta})(\omega+k^{\omega})+j(\omega+k^{\omega})^{2}\\& &+s\cos(\theta+k^{\theta}))\end{eqnarray*}$$

これは、EXCELで計算するときに、VBAで定義しても使う。これを使うと、微分方程式を解くための漸化式は次のように定式化できる。$n = 0, 1, 2 \cdots$は、繰り返される時間、ステップを表している。

$$\begin{eqnarray*}k_{1}^{\theta}&=&h\omega_{n}\\k_{1}^{\omega}&=&hf(\omega_{n},\theta_{n},0,0)\\k_{2}^{\theta}&=&h(\omega_{n}+\frac{k_{1}^{\omega}}{2})\\k_{2}^{\omega}&=&hf(\omega_{n},\theta_{n},\frac{k_{1}^{\omega}}{2},\frac{k_{1}^{\theta}}{2})\\k_{3}^{\theta}&=&h(\omega_{n}+\frac{k_{2}^{\omega}}{2})\\k_{3}^{\omega}&=&hf(\omega_{n},\theta_{n},\frac{k_{2}^{\omega}}{2},\frac{k_{2}^{\theta}}{2})\\k_{4}^{\theta}&=&h(\omega_{n}+k_{3}^{\omega})\\k_{4}^{\omega}&=&f(\omega_{n},\theta_{n},k_{1}^{\omega},k_{1}^{\theta})\\\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}$が得られ、これを繰り返すことによって機体角とその加速度の系列を得ることができる。

EXCELによる実際の計算とその結果は次回示します。

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

2022年1月24日月曜日

PID制御によるドローンの揺れに関する数値シミュレーション(3):EXCELで実行

 ここで実行するシミュレーションモデルについては、以下の二つの記事で解説している。必要に応じてそちらを参照していただきたい。

PID制御によるドローンの揺れに関する数値シミュレーション(1):理論編

PID制御によるドローンの揺れに関する数値シミュレーション(2):シミュレーションモデル

以下で解説する、EXCELのシミュレーションファイルは、以下からダウンロードできる。(EXCELで開いてください。代替ソフトで開くと、データや式は問題ないと思いますが、グラフが正しく表示されない可能性があります。グラフを書き直せば大丈夫だと思いますが)

EXCELファイルのダウンロード(このファイルのダウンロードを中止しました。D制御も組み込んだより一般的なモデルのEXCELファイルをお使いください。その中で、この記事の結果が再現可能です【D制御パラメータをゼロにする】。以下のリンク先にある、このシリーズの(5)の記事からダウンロードできます。)


ファイルの使い方を説明する。

ファイルを開くと、緑の縦線の左側に設定すべきパラメータの欄があり、「パラメータ」の蘭のDeltaという項目以外を入力する必要がある。Deltaは、そのままにすると、自動的にホバリングレベルのプロペラ回転速度で、上昇力(スラスト)と重力加速度がバランスするように計算される。あえて違う値を入力する場合は、そうしてもよい。どうなるかはご自身で考えていただきたいが、モデルが壊れることはないので、自由にやられたらよいと思う。

ダウンロードした現状は、私の自作のドローンに関わる値がセットされている。

右側は、パラメータに基づいて、シミュレーションが実行される。左側に時間の推移が示されている。時間の刻みはパラメータのΔtの欄で指定することができる。現状では、10ms毎に実行される(私の自作システムのループタイムに一致させているが、それより長くても短くてもよい)。
その後各期のパラメータが続いて、最後から3列目に、機体角度の$\theta$およびその角速度$\omega$、さらにその外側に、プロペラの回転数の変化が現れる。

パラメータを変更すると、実行結果全てがそれに合わせて自動的に変わる。

私の自作ドローンのパラメータで実行した結果を少し解説しよう。まず、解そのものを見てみよう。

この図は小さいが、EXCELの中にオリジナルが表示されているはずで、それを見ていただきたい。青い線が機体の傾き、ピッチ角を表す解で、単位は左側の軸に示されている。ラジアンで、0.1前後を頂点とする揺れなので、この揺れの角度は、30度が、π/6だから、0.1は、6度くらいの傾きが現れている。角速度は、当然ずれていて、期待が水平状態の時、絶対値で最大になる。速度は、正負が現れる。

また、その周期は約1秒である。少し緩い揺れだと思うが、自分のドローン的には、結構いいところだと思っている。

P制御だけでは揺れの制御が全くできていないことがわかる。

モータの回転数とピッチ角を同じグラフで表すと次のようになる。


興味深いのは、モータの回転の変化から、実際の回転角の変化が現れるまで、800msくらいラグがあるということである(ちょっと大きすぎる)。ESC信号から、モータの回転までのラグが、200msくらいあることが気になっているが、それ以上に回転と推力の間のラグがあるといことだ。
プロペラの回転は、直ちに力を生み出し、遅れるっことなく機体の加速度を生み出す。しかし、その加速度が、一定の機体の回転回転角の変化につながるためには、時間がかかるということだ。加速度と速度の関係から明らかなのだが、改めてなるほどと感じさせられた。
 

PID制御によるドローンの揺れに関する数値シミュレーション(2):シミュレーションモデル

 ここで実行する数値シミュレーションモデルは、次の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制御によるドローンの揺れに関する数値シミュレーション(1):理論編

動きを確かめるのは、$\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}$が与えられているものとする(シミュレーションでは、妥当な数値を適当に与えればよい)。

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

$$\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制御によるドローンの揺れに関する数値シミュレーション(1):理論編

ドローンにPID制御をかけると、安定化は可能であるが同時に機体の揺れも発生させる。この状況を数値シミュレーションする。ただし、今回は、ESCに与える信号と、モーターの回転数の変化の間に遅れが発生しないことを前提にする。このラグのある場合の状況は次に試みる予定だ。

というよりも、もともとこの分析を始めるきっかけが、自作ドローンの信号とモーター回転数の間にあるラグがPID制御の不安定の原因だと思ったことだ。それを分析したいのだが、まず単純でピュアな状態を分析しなければ話にならないので、最初はその前提で進める。

モデルを示そう。



4ローターのドローンを想定しているが、問題の趣旨から、ロールないしはピッチの片方だけ考慮すればよい。上図のように、モーターの腕方向に機体座標が設定されているとする。



ピッチをイメージしよう。先頭のモーター番号を1後方を2とする。zが垂直方向で、xがピッチが現れる水平方向である。

機体が太黒線のように傾いた状態を考える。また、問題はさらに単純化できる。

今、二つのモーターが完全に対称に動くとすると、原点Oが固定されていて、一方のモータの動きだけで回転する状況を想定すればよい。今そのモータを2番のモーターだとしよう。以後、1番のモーターの動きは問題にしない(Oが固定されているので)。

接線方向の運動方程式を立てる。モーターによる推力を$F$とする。重力加速度を$g$、機体のモーターにかかる重さを$m$とする。これは全機体重の1/4と考えてよいかもしれない(深く考えると色々ありそうだが、無視する)。さらに、図のように、機体がピッチが正の方向に$\theta$(ラジアン単位)だけ傾いているとしよう。上向きの接線方向の速度を$v$とすると、運動方程式は次のようになる。

$$\begin{equation} m\frac{dv}{dt}=F-mg\cos \theta \end{equation}\label{eq1}$$

また、角速度を$\omega$とすると、次の式が成立する。

$$\begin{equation}v=L\frac{d\theta}{dt}=L\omega\end{equation}\label{eq2}$$

プロペラの1分あたり回転数を$V$(単位rpm)とする。一般に、推力$F$ は、$V$の二乗に比例すると考えられているので、次の式を考える。

$$\begin{equation}F=\delta V^{2}\end{equation}\label{eq3}$$

ここで、$\delta$は、ある与えられたプロペラの元でのモーターパワーを表すパラメータと考えれば良い。

PID制御のP制御(比例制御)を次の式で導入する。微分(D)制御も入れることは可能だが、以下計算がさらに複雑になるので、次の課題とする。ただし、D制御を入れないと安定化しない場合があるので、それは念頭に置いておかなければならない。(D制御を入れたモデルは、次の記事で行なっているPID制御によるドローンの揺れに関する数値シミュレーション(4):P制御+D制御モデル

$$\begin{equation}V=V_{0}-p\theta\end{equation}\label{eq4}$$

ここで、$V_{0}$は、ホバリングを実現可能なスロットルレベルに対応したモーターの回転数である。$p$は、P制御の水準である。すなわち、このシミュレーションモデルにある唯一の制御変数である。$\theta$はピッチ角なので、ピッチ角に比例してモーターの回転数を制御することになる。ピッチ角がゼロの時は、ホバリングレベルのモーター回転数で、ピッチが正になると回転数を落とす方向に、逆に負になると回転数を増やす方向に変化させる。

私が自作のドローンで問題視しているのは、この制御信号が回転数に変化する上で200m秒くらいタイムラグがあることだ。それを本来調べるためのこの数値シミュレーションをしようと思ったのだが、そのタイムラグを入れるのは次の課題としてて、まず、タイムラグなしにESC信号をモータは回転数に反映させるとここでは考えている。(制御の遅延を組み込んだモデルは、このシリーズの「(6)の記事」以降で解析している)

この制御式$\eqref{eq4}$を、回転数と力の関係$\eqref{eq3}$に入れる。

$$\begin{equation}F=\delta V^{2}=\delta (V_{0}-p\theta)^{2}=\delta V_{0}^{2}-2p\delta V_{0}\theta+p^{2}\delta \theta^{2}\end{equation}\label{eq5}$$

簡単化かのためにこの式を次のように表そう。

$$\begin{equation}F=\alpha-\beta\theta+\gamma\theta^{2}\end{equation}\label{eq6}$$

ここで、$\alpha, \beta, \gamma$の各パラメータは、$\eqref{eq5}$式の各パラメータを表している。この式を運動方程式$\eqref{eq5}$に代入すると、次の式を得る。

$$\begin{equation}mL\frac{d^{2}\theta}{dt^{2}}=\alpha-mg\cos \theta-\beta \theta+\gamma \theta^{2} \end{equation}\label{eq7}$$

両辺を$mL$で割ると、

$$\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{eq8}$$

を得る。これは、二階非線型常微分方程式である。これを満たす$\theta$の振る舞いについて、解析的を求める方法が有るかどうかはわからなかった。しかし、ここでおこなう数値シミュレーションはこのままの形で実行可能、実際、あとでその結果を示すことができる。

ただ、その前に、ここでは議論をわかりやすくするために、$\theta$の値が小さい範囲で成立する、

$$\cos\theta\simeq 1$$

という近似を用いて議論の見通しをよくする。この時、$\eqref{eq8}$は、次のように書き換えることができる。

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

これもまた、二階非線型常微分方程式であるが、これには解析解が存在している。(例えば. http://www6338.la.coocan.jp/mathematics/diff-equation.pdf 参照)しかし、それはワイエルシュトラスの$\wp$関数を用いて表現される楕円関数を使うもので、そこまで複雑なものは上手く扱えない。

そこで、さらに、この$\theta$の二乗の項(右辺最後の項)が存在しなかった場合を考えてみよう。すなわち、

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

となる。これは、高校物理でもお馴染みの、中心が原点以外のところにある単振動の式に他ならない。詳しくは教科書を見ていただきたいが、これは、周期$T$が、
$$T=2\pi\sqrt{\frac{mL}{2pV_{0}\delta}}$$
単振動の中心位置 $\theta_{0}$が、

$$\theta_{0}=\frac{1}{2p}\left(V_{0}-\frac{mg}{V_{0}\delta}\right)$$
となるものである。つまり、振動への傾向は、このモデルに内蔵されているということである。しかし、制御機能があるので、その振動は安定、すなわち一定の時間が経てば収束する可能性ももちろん持っている。

また、$\theta$が小さい範囲ということは、また、最終項の$\theta^{2}$も小さいと考えることもできるので、そうなると、このリアリティは重要だ。しかし、実際のドローンは、わずかな傾きでも、水平方向へ移動する。水平移動は、まだモデルに組み込んでいないので、このシミュレーションには現れない(いずれ組み込みたい)。このような振動の影響の大きさを考えると、この近似は避けるべきだ。

そこで、ここでは$\eqref{eq8}$そのものの数値シミュレーションを行うことにする。ここからは、次の記事(2)で解説する。

このシリーズの記事の目次は次のところにある。


2022年1月9日日曜日

ESCをメーカー品にしてみた:スロットル90%までの直線性

 今まで、安くて動けばそれでいいと思って、ノーブランドのESCを利用してきた。その結果が、先の投稿にも書いたように、肝心の離陸後のスロットル直線性が失われ、制御できない状態になっていた。

そこで、ESCを 「ブラシレスESC 40A アンプ SBEC 5V/3A RC飛行機固定翼空用スピードコントローラー ZTW Beatles 40A ESC 2-4S」というものを1個Amazonで買って(2500円、ちょっと高いが)、交換してみた。スロットルと回転との関係は、次のようなものになった。


驚くべき違いだ。スロットル90%まで、ほぼ直線的に上昇している。最大回転数も、前のESCが最大およそ13000だったので、30%増大している。

50%までは、前と同じに伸びている。その前あたりで離陸するので、スロットルで30%分の制御余地が出てきている。これだと、少なくとも駆動部分で制御の問題は現れないはずだ。

早速、残りの3個も購入した。明後日の朝、到着する。楽しみだ。

2022年1月7日金曜日

モーター駆動の線形性


 いくら制御信号をきちんと出力しても、それに正しくモーターが反応していなければ、姿勢制御はできない。

この間、そのあたりの問題が気になった。まず、スロットルレベル(%)とモーターの回転数を調べた。


こんな感じである。縦軸が回転数だが、プロペラ2枚の回転数を測っているので、半分にしなければならないはずだ。そうなると、モーターは、940Kvなので、回転数が少ない。1Vあたり940rpmであるから4sバッテリーで少し多めに15V(実際16Vの時もある)としても28200回転くらいなければならないのだが、最大でもその半分以下である。非接触回転系が、プロペラの数に無関係に回転数を測れるなら別だが、そんなことは考えられない。
そしてなによりも線形性だ。スロットルレベル40%あたりから徐々に線形性(厳密には直線性のこと)が崩れる。ということは、信号に対して比例的にモーターが反応していないということだ。そもそも、スロットルレベルが40%後半で期待は浮上するので、浮上した途端、制御信号に正しく反応しなくなることを意味している。これでは、姿勢が制御できるわけがない。

心配になって、PWMモジュールからESCに対して、正しく信号が送られているかが気になって、以前も調べたのだが、再度オシロスコープで調べ直した。

特に問題は見当たらなかった。

ということは、ESCか、モーターに問題があるが、ESCの可能性が高い。というのも、Kv値がおおきなレース用モーターのESCに、今使っているモーターを繋いで、回転数を測ったところ、少なくとも60%くらいまでは線形性を確保していたからだ(それ以上は、回転数が大きすぎて怖くて測らなかった)。

DJI MAVIC MINIのFPVライブ動画をiOSのVisionframeworkで顔認識させるという話

 iOSでは、他の方法もあるが、Visionframeworkは、いろんなことができるので、これでなんとか顔認識させようとここ2,3日没頭していた。ようやく、 こんな感じで、緑の枠で認識画像をリアルタイムで表示できるようになった。 (プログラムの全体は、最下段に掲げてある) 途中...