Home Made Garbage

M5StickC で倒立振子 PID制御編 ー倒立振子への道 3ー

ホーム
電子工作
Raspberry Pi
IOT
WEB

さて前回まででカルマン・フィルタを使用したM5StickC傾斜計を作りました。
いよいよM5StickCによる倒立振子を作っていきます。

この道シリーズではトランジスタ技術 2019年 7月号を参考に倒立振子の実現を目指しています。

トラ技の倒立振子はカルマンでの傾斜推測に加えてロータリエンコーダでタイヤの角度、角速度検出して倒立振子自体のモデルで更にカルマン・フィルタかまして安定度の高い倒立振子を実現しています。モータへの電圧供給フィードバックもなんか難しい関数で算出されています。

しかし、今回はM5SickCを用いたカジュアルな倒立振子の実現を目指します。ロータリエンコーダは使わず、さらにモータへの供給電圧は難しい関数は使用せずに馴染みのあるPID制御で算出しました。

 

 

まずは作ってみた

とりあえず手を動かさないことには始まらないということで、倒立振子を作ってみた。

 

やっぱ何も考えずに適当につくった倒立振子はどうあがいても全然倒立しませんでした。。。

頑張って改良した

いろいろ工夫してついに立ちました!!見てください!

 

立った!立った!立ったー!やりぃー!キャッホー!!うれCY!!

 

さていかにして、この倒立振子が実現されたかを以下に記します。

 

構成

I2CモータドライバDRV8830をM5StickCで制御してモータを駆動します。

部品

マシン

以下が自慢のマシン詳細です。

 
いろいろ試して気づいたのですがマシン自体のバランスが非常に重要です(あたりまえか)。底面に鉄板貼り付けてバランス調整しています。

PID制御

今回作成した倒立振子ではM5StickCの傾きθと角速度ω ($\dot{θ}$)をカルマン・フィルタをかまして推定値を算出して、その値を元にモータ供給電圧を決定して姿勢を床に対して垂直に保つようにします。

ここではPID制御を用いて供給電圧を決定します。

PはProportional(比例)、IはIntegral(積分)、DはDifferential(微分)です。

$$操作量 = Kp × 誤差 + Ki × 誤差の積算値 + Kd × (前回の誤差と今回の誤差の差)$$

上式でモータ供給電圧を決定します。誤差は目標値と現在の傾きの差で今回は垂直(0°)を目指しますので誤差は傾きθ(θ – 0°)となりますので

$$モータ供給電圧 = Kp × θ + Ki × \sum{θ} + Kd × ω$$

係数Kp, Ki, Kdを調整して、過度な供給や供給不足のない適切な電圧の供給を実現します。今回はBluetoothでM5StickCと通信してスマホで係数を送れるようして地道に手動で調整しました。

参考

6軸センサ SH200Q調整

比較的 高速で繊細な観測が要求されますのでM5StickCに搭載されている6軸センサ SH200Qの設定値を調整しました。

SH200Q用のクラスを修正して以下を変更しました。

  • 加速度センサ出力周期:256Hz → 1024Hz 
  • ジャイロセンサ出力周期:500Hz → 1000Hz 
  • ジャイロセンサLPF遮断周波数:50Hz → 250Hz
  • 加速度センサレンジ:±8G → ±4G
  • ジャイロセンサレンジ:±2000°/sec → ±250°/sec
     

また出力レンジの変更に伴いヘッダファイルも修正しました。

 

Blynk設定

PID制御用の係数Kp, Ki, Kdとモータ供給電圧の最小値Vminをスマホから送れるようにします。

スマホとM5StickCはスマホアプリのBlynkを用いてBluetooth通信させます。Blynkアプリのバージョンは2.27.6です。

新規プロジェクトを作成します。HARDWRE MODELはESP32 Dev Boardを選択。CONNECTION TYPEはBluetoothを選択。AUTH TOKENはArduinoコード生成時に使用します(アカウント登録したメールに送信されます)。

 

ウィジェットとしてBluetoothとNumeric Inputウェジットを4つ配置します。Numeric Inputウェジットは±ボタンを押して値を増減させて送信できます。
 

Numeric Inputウェジットの設定は最小値と最大値、ステップ数を指定してヴァーチャルピンに出力させます。以下のように設定しました。

  • Kp:-1000~1000、0.1ステップ、出力ヴァーチャルピンV0
  • Ki:-1000~1000、0.001ステップ、出力ヴァーチャルピンV1
  • Kd:-1000~1000、0.01ステップ、出力ヴァーチャルピンV2
  • Vmin:0~255、1ステップ、出力ヴァーチャルピンV3

I2Cモータードライバ DRV8830

DRV8830はI2C入力によって、モータ供給電圧(スピード)と供給電圧方向(回転方向)を制御するものです。つまり正負の電圧を生成できるということです。

I2Cアドレス設定

本モジュールは基板上のジャンパA1, A0のステートによって以下の表のようにアドレスを指定することができます。

表のアドレス末尾xは読み込み時には1、書き込み時は0をしてします。ここでは書き込みのみ使用します。両方Openのアドレス:0x64としました。

書き込みI2Cデータ

アドレス0x00に8bitの情報を書き込みます。
各ビットの設定は以下の通り。上位6ビットで電圧を設定し、下位2ビットで正負を指定できます。

Arduino IDEプログラム

以下のBlynkのArduino用ライブラリを使用してプログラムしました。バージョンは0.6.1。
 https://github.com/blynkkk/blynk-library

ここではBluetoothを使用してM5StickCと通信します。ライブラリのコード例のESP32_BT.ino を参考にプログラムしました。

カルマン・フィルタによる傾斜角と角速度の誤差の推定値を元にPID制御によってモータに電圧を供給して姿勢を保ちます。

タイマ割り込み(2.5msec毎)でカルマン・フィルタ介して傾斜角と角速度の誤差を算出します。角速度に角速度の誤差を足して角速度の推定値としています。

10msec毎に供給電圧を計算してモータ駆動します。

初期設定時にセンサの測定値のオフセットを測って差し引くようにしてます。

 

参考

倒立振子によるPID制御のコードは以下を参考にさせていただきました。

動作

調整中の様子。

 

起動すると初期設定(LED点灯中)がはじまり終了すると、倒立動作が開始します。BluetoothでBlynkとつながるとディスプレイが赤から緑にかわり各種設定値を送信できます。

動きをみながら地道に調整を進めます。。。

上記コードでVMIN = 1、Kp = 0.4、Ki = 0.1、Kd = 0.2でだいたい安定しました。外乱には非常に弱いですが。。。

とりあえず倒立するマシンができたので次はカット&エラーではなくトラ技の制御法を取り入れて
もっと安定性の高い倒立振子を目指します!

Related Posts

「M5StickC で倒立振子 PID制御編 ー倒立振子への道 3ー」への5件のフィードバック

  1. お見事な倒立振り子です!
    私もこの一か月ほど。センサーの設定からモータードライバ、PIDと悪戦苦闘しています。
    まずは、ある程度自立させないことには、PIDの最適化もできませんが、PIDを最適化しないことには自立しません。この矛盾。
    鉄の重りで、重心を下にして安定させる裏技、なるほどです。(若干、反則技かとも思いますが)(笑)
    当方、重心をかなり上にもってきておりますが、かなかな難しい状況です。
    よろしければ、ブログを除いてやってください。励みになります(笑)
    あと2-3日で、最新の記事をアップする予定です。

  2. コメントありがとうございます。

    私も色々形や定数を変えて試行錯誤してやっとやっと立ったという経緯です。
    重心を上にすると比較的ゆっくりとした制御でいいのですが距離が大きくなるという
    経験的印象があり重心を下にしてさらに機体自体のバランスを取りました。
    おかげで微小な制御での安定が実現できましたが外乱には弱い倒立振子となっております。。。

    かなり奥深いですよね。。。理論でガッツリ設計するべきなのでしょうが、まだまだ勉強中です。

    ブログ楽しみにしております! 

  3. こんにちは
    お陰様で、とりあえず少しの間「立ち続ける」所までこぎつけました。
    ジャイロセンサーのドリフトで、徐々に安定位置がずれていき、最後には「激突」となります(笑)
    加速度センサーとジャイロセンサーの「相補フィルター」を試しているところですが、加速度からの角度と、角速度からの角度の整合をとることが難しく、はまっています。
    お時間ありましたら、今の「倒立ロボット」の雄姿、ご笑覧ください。

    1. ご案内ありがとうございます。
      重心が高いためか外乱にも強いようですね。

      ジャイロは素子の温度とかでもドリフトするようですね。。

      進捗を楽しみにしております!

コメントはこちらから

メールアドレスが公開されることはありません。コメントのみでもOKです。

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください