倒立振子ロボットを MonoBrick で作成する(C#編)

list このエントリーをはてなブックマークに追加

ここ2週間ほど倒立振子ロボットにハマっていたのがようやく形になって来たので、メモ代わりに残しておきます。初手は ET ロボコンの EV3Way用のサンプルプログラム なのですが、そのままだとうまく動かかなったので(今考えると車輪の径の設定等が必要だった)、もうちょっと簡単に組み立てて動くところから始められるように、BALANC3R にある LabVIEW のコードから C# に移植しています。

moonmile/MonoBalancer
https://github.com/moonmile/MonoBalancer

移植先

Robot Square – Tutorial: Building BALANC3R – Robot Square のコードを忠実に C# に移植しているつもりです。「つもり」というのは、コードの意味をいまいち把握していないからですね。いくつかパラメータの意味もわかっていないので、そのまま C# に移してパラメータもそのまま持ってきています。

よく知らなかったのですが、倒立振子ロボットは制御工学では定番の教材で色々なものがあります。ジャイロセンサや、モーターの特性(角速度、ギアボックスの比率、トルクなど)、重心位置、車輪の半径など、いろいろな要素が絡み合って倒立するように(倒れないように)する訳ですが、それらをモデル化してフィードバック回路にするところがミソです。複雑な要素をうまくモデル化して時間軸で定常値に近づけます…と今のところ理解しています(まだ、教科書でPID制御のあたりまでしか行ってないので)。

MonoBrick

LEGO Mindstorms EV3 は主に LabVIEW を使ってコードブロックを組み合わせてプログラミングをしていきます。が、そのままでは普通のプログラミングがしずらいので、MonoBrick.DK | Home of MonoBrick を使います。Linux に Mono を入れて C# でも動かそう(F#/VB等でも動きます)という方法です。EV3 への制御は MonoBrickFirmware ライブラリを通して使います。

MonoBrickFirmware のコードは、Larsjep/monoev3 にあります。豊富なサンプルコードがあるので色々な機能を動かすには苦労しないでしょう。MonoBrickFirmware を見ていくと、中間コード生成して EV3 に渡していることがわかります。ここの中間コードの形式などは、ダウンロード – レゴ®マインドストーム LEGO.com にある EV3 Firmware Developer Kit 等で公開されています。

ちょっとしたコードの解説

制御工学的な話は勉強中なので、ちょっとだけコードの解説をしておきます。

センサーとしては「ジャイロセンサー」と「モーターの角速度」だけを使います。倒立振子ロボットを作る際のセンサーは「ジャイロ」「ロータリーエンコーダ」「トルクセンサ」が必要なことが多いらしいのですが、EV3 の倒立振子ロボットは2つだけです。

double GAIN_ANGLE_VELOCITY = 1.3;
int GAIN_ANGLE = 25;
int GAIN_WHEEL_SPEED = 75 ;
int GAIN_WHEEL_POSITION = 350;

ba.Initialize(
    0,  // LEGO Jyro 
    42, // wheel 42 mm
    22);// sample time 22msec
ba.SetConstants(
    0.6,    // Kp
    14,     // Ki
    0.005,  // Kd
    GAIN_ANGLE_VELOCITY,    // Gain Angular Velocity 
    GAIN_ANGLE,     // Gain Angle    
    GAIN_WHEEL_SPEED,     // Gain Wheel speed 
    GAIN_WHEEL_POSITION);   // Gain Wheel position 

ba.Initialize で指定するのはタイヤの直径と、サンプリングタイムです。重心位置は寄与していないのかもしれません。計測間隔は 2,3msec と短いものが多いのですが、Baranc3R では 22msec(内部的に補正して 20msec)になっています。これでもバランスが取れているので、C# などで制御するときには非常に楽です。ちなみに .NET Framwork 等では Timer クラスを使うと 30msec 以上になってしまうので、ちょっと手を入れています。元の LabVIEW のコードで 2msec 補正しているのは、計算時間(2msec)+ 待ち間隔(20msec)という感じなんだと思います。

ba.SetConstants で指定しているのは PID パラメータと、ゲインです。この値は制御工学を紐解くのと実測値を調節しないと駄目なので、そのまま使います。通常は、モデル化→ ブロック線図 → 伝達回路 → ラプラス変換 → PID 制御 に直して定常値を計算する(らしい)のですが、このあたりはまだ勉強中。

double motorPosition = Position();
double robotPosition = 0.0;
double robotSpeed = 0.0;
ReadEncoders(out robotPosition, out robotSpeed);
double angle = 0.0;
double angleVelocity = 0.0;
ReadGyro(out angle, out angleVelocity);
double inputVal = CombineSensorValues(angleVelocity, angle, robotSpeed, robotPosition, motorPosition);
double pid = PID(_kp, _ki, _kd, _dt, 0.0, inputVal);
Errors(pid); // エラーの場合は例外が発生する
SetMotorPower(_steering, pid);

制御ループの中で、CombineSensorValues 関数が入力値を計算しているところです。参照値は、倒立させるために常に 0.0 です。ただし、リファレンス値を 0.0 にしてしまうと、ステアリングとロボットのスピードも殺されてしまうような気がするんですけど、どうなんでしょうね。私のコードの移植が変なのかもしれません。

PID 関数でモーターに与えるパワー(PWD値、-100%から100%の間)を計算します。このあたりの定数値は Arduino や Raspberry Pi で倒立振子ロボットを作ったときに異なってくるところです。まあ、そのあたりも理解して作りたいなと思っているので、やっぱり制御工学の知識は必要ですね。

あと、平均スピードの計算方法とかジャイロセンサーの取得方法とかが少し工夫してあります。ジャイロセンサーの揺れに過敏にならないように近傍の平均値を使っていますね。

数式に直すのと、モデル化に戻すのは後で。

参考先

姿勢制御のコードを理解してプログラムを書きたかったので、既存のプログラムを読み直している途中です。結局のところ、制御工学の教科書 はじめての制御工学 (KS理工学専門書) [Kindle版] に行きついています。

etroboEV3 / Wiki / Home
http://sourceforge.net/p/etroboev3/wiki/Home/
LEGO Mindstorms EV3でGyro Boyを動かしてみた。(その1) – robo8080のブログ
http://blog.goo.ne.jp/roboz80/e/455074cc58e99474d4daf6a24d5ebcc0
2輪倒立振子ロボット2号機作成。 – robo8080のブログ
http://blog.goo.ne.jp/roboz80/e/e485f7efa259e44baaa298fd6989c556
Robot Square – Tutorial: Building BALANC3R – Robot Square
http://robotsquare.com/2014/06/23/tutorial-building-balanc3r/
NXTway-GS (Self-Balancing Two-Wheeled Robot) Controller Design – File Exchange – MATLAB Central
http://www.mathworks.com/matlabcentral/fileexchange/19147-nxtway-gs–self-balancing-two-wheeled-robot–controller-design
倒立振子の研究
http://www.instructables.com/id/%E5%80%92%E7%AB%8B%E6%8C%AF%E5%AD%90%E3%81%AE%E7%A0%94%E7%A9%B6/
みのくらロボット
http://minokura.net/works/wheelpendulum.html

カテゴリー: C#, EV3, MonoBrick パーマリンク

倒立振子ロボットを MonoBrick で作成する(C#編) への1件のフィードバック

コメントは停止中です。