Segwayをマイコンで実験♪（慣性センサ応用）

前回に引き続き、Segwayに関して．　

久しぶりに時間ができたので、前回試したシミュレーションをマイコンを使って本当に１輪だけで倒れずに制御することができるのか？
試してみました．も～好奇心でしかありません（笑）
Segwayの基本となる倒立振子を制御してみます．



基本的にPID制御ならばそこそこいい加減な制御パラメータでもうまくいっちゃうことが多々あるので（限界感度法でざっくり推定しても可）とりあえず？的なノリで挑戦してみました．ま～大人版　『夏休みの工作♪』


素材はTAMIYAの工作シリーズを使ってみました．２０年ぶりか？？？　 ギアボックス、シャーシ、タイヤ、電池BOXもAmazonでそろえてみました．その他に、マイコン／センサ／配線を少々でOK．世の中に見られるような物々しい倒立振子ではありません．お手軽に実現する！のも今回の目標でもあります．１ブレッドボード！

シングルギヤボックス	スリムタイヤセット	ユニバーサルプレート

簡単そーなギアBOX．でも実際組み立てようとすると子供の頃のようにサクサクと進みませんね（＾＾；　結構複雑で、説明書をじっくりと見てしまいました・・・．小さい頃は説明書なんてあまり見なかった気がするなぁ．

さて、このSegway（倒立振子）、制御の考え方は簡単です．シャーシの角度をセンシングして倒れないようにモータに推力指令を出してバランスを取るだけ！！　です．ハイ．

モータに出力させたい推力は、やんわりと動かすために弱く出力したり、瞬間的に大推力を出したり、といった具合に変化させたいのでPWMで制御します．マイコンでは大電流を扱えないので、モータドライバを適当に購入しました．あとで、Segway同様にステアリング機能を持たせたいので、２個のモータを駆動できるモノにしてます．

能書きはさておき、完成した動画がこちらです．部品は動画の通り、たったこれだけ！想像以上に簡単にバランスしてくれました！　理論的に成立することはわかりきっているのですが、実際にひょこひょこと机の上でバランスとってる姿を見ると、なんだかかわいらしくもあり、感動さえも覚えます（＾ー＾ｏ


【大画面で見る】

Bluetoothでリアルタイムに、パラメータ読出／書込／データ出力もできるようにプログラムしてみました．
スマホで、操作することも難しくないかな． 通信ソフトはフリーソフト等でOK．動画では9600bpsですが、データ項目を増やすと送信が間に合わず制御に影響を与えていたので、115200bpsに変更しました．データ項目を増やして100Hzで取得しても制御に全く影響を与えません．バッチリです．


【大画面で見る】

角度検出センサとして加速度計とジャイロを採用してみました．なぜ２種類のセンサを使うかというと、検出した値が様々な要因でドリフト（何もしないのに値が変かしていく）してしまうのを、それぞれの得意な周波数帯の合わせ技で補正するために２個のセンサを搭載しています． しかしながら、それぞれの観測値にはノイズを含んでいるのでカルマンフィルタを使って真の値を推定してみたのが下のグラフです．意図的に手で揺らしてみました．
※ ノイズの共分散Q，Rは適当に定めました．

ちなみに、フィルタせずに制御するとじゃじゃ馬になります・・・．
カルマンフィルタは予測と更新を繰り返して推定していきます．ＭＡやＦＩＲのように過去にいくつもさかのぼってデータを保持しなくてもいいのもこのフィルタの特徴です．



こうなると、人が乗れるSegwayもどきを作ってみたくなりますね♪　本物みたいに坂道もすいすい走るにはちょっと工夫が必要そうです．斜面で倒れずにバランスさせるためには、前傾姿勢で踏ん張らなければなりません．つまり、制御ゲインを上げなければならないのです．まーこれは、人間が操縦するときに自ずと前傾に操作するので、それにあわせてフィードフォーワードゲインをあてればよいでしょう．
動画はUPしてませんが、坂道でのバランスにも成功しました（＾＾

ちょっとしたマイコンプログラムで、パラメータと挙動の関係が実際に見て取れておもしろいですね／　
制御のお勉強にもなります． DIY倒立振子で有名なのは、制御の専門家が運営するこちらのHP→【半日で作る倒立振子】．制御を知らない人でもで現代制御”風”に倒立振子をDIYできる内容を紹介しています．

同じではつまらないので、黒っ９は部品点数を極力減らしてPIDで制御してみました．
↓部品はたったこれだけ．