初投稿です。
タイトルにもある通り、Arduino nano(互換機)と、6軸のジャイロ・加速度センサのMPU6050(GY-521)、および連続回転サーボFS90Rで倒立振子を作ってみました。
外観
まずは外観をご覧ください。
特に線のごちゃつきを抑えようとか、基板に実装しようとかは考えていなかったので、大変お見苦しい画像となっています。
赤く光っているのがArduino nano、緑に光っているのがMPU6050モジュールGY-521です。
寝かせているとき
倒立しているとき
使用器具
以下の二つはAmazonで安く買いました。
Arduino nano(互換機)
GY-521(MPU6050モジュール) ※2020/8/6現在、在庫切れのようです。他メーカーのもので十分だと思います。
これのみ秋月電子通商で買いました。
連続回転サーボ FS90R
当然2つ必要です。
その他、9V角型電池(006P)と電池スナップ、単三電池4本とその電池ボックス、ブレッドボード、ジャンパ線、スライドスイッチ、マスキングテープ、M3のなべネジとナットなどを使いました。
タイヤはタミヤのスポーツタイヤセット(56mm径)です。
この記事にはありませんが、将来的な機能として赤外線リモコンで操作しようと考えているので、写真や動画には赤外線受光部VS1838が映り込んでいます(銀色のちっちゃい四角いLEDみたいなやつ)。今回は赤外線については触れません。うまくいってないから
ボディについて
今回制作した倒立振子、電池・ブレッドボード・モータを支えているパーツや、タイヤをサーボに嵌める白いパーツは3Dプリンタで印刷したものです。
これは政府の10万円の給付金を半分くらい使って購入した3Dプリンタを活用してみることも兼ねていました。
QIDI X-Smartという機種です。レビューの評価の高さと値段の手ごろさで選びました。かなりいい感じです。
ネジ2本でブレッドボードを抑えて固定している点は激ダサな上、単3電池4本はマステで仮止めのままです。この辺は追々改良したいと思っています・・・。
今回のボディの用途は、頑丈さは特に気にしておらず、もろもろのパーツがちゃんとひとかたまりに固定できるかのみで考えます。
なので実際に制作するときは、ボディはわざわざ3Dプリンタ造形物じゃなくても、アクリル板、アルミ切り抜き、レゴ、MDFやベニヤ板、タミヤのユニバーサルプレート、なんならプラ板や段ボールとか、そもそも接着剤や両面テープで全部まとめてくっつけるとかでもいいかと思います。もしこの記事を参考に倒立振子を制作するのであれば、ぜひ自分なりの工夫をしてみてください。タイヤもうまいことやれば3Dプリンタに頼らずともサーボに嵌められるかと。
暇があれば、あとQiitaの使い方が分かってくれば、どこかで3Dプリンタのデータを公開するかもしれません。
なぜ連続回転サーボなのか？
たまたま手元にあったからです。
・・・真面目な理由もあります。
ほかのサイトを見るとだいたいどの人も「遊びの少ないギヤードモータ」を使っていますね。
しかしDCモータの正転逆転・速度調整にはモータードライバが必要になります。
連続回転サーボなら、PWMでDuty比を変えれば速度調整は難しいものの正転逆転なら簡単にできます。つまりArduinoと連続回転サーボのみで済みます。部品点数の減少およびモータードライバを勉強する手間が省けます。
作ってみてから気づいたことですけどね・・・。
https://qiita.com/coppercele/items/527228e3f08c53597bd1 ←参考にさせていただいたこちらのページで紹介しているBalaCは連続回転サーボを使用しているようです。
ただし以下にあるように、連続回転サーボならではの落とし穴もありました。
引っかかったことなど
連続回転サーボの速度調整は難しい。
ふつうのサーボモータはwrite(angle)で0~180の角度を指定しますが、連続回転サーボは90で停止、90より大きいと正転、90より小さいと逆転、0および180で最大スピードという風に動作します。
write(angle) - Arduino 日本語リファレンス
本来であれば倒立振子は角度などだけではなく、モータの回転速度も含めたPID制御が理想的ですが、連続回転サーボだとそれが難しいです。
一応、自分の調べたところ、FS90Rは入力値が92~87で停止、93および86くらいの数値でゆっくりと回転し始め、106,71くらいで半分くらいの回転速度、120,57でほぼマックススピードという風に動作します(個体差も当然あると思います)。つまり速度調整できる範囲が±30くらいしかありません。おそらく狭いです。
以下にコードを載せていますが、PIDで計算したpowerを±50に制限しつつ、ちょうどいい塩梅になるようにPIDパラメータを調整している感じです。倒立はしたのでいいっちゃいいのですが、「なんだかわからんがとにかくヨシ！」感が否めません。
PIDパラメータと目標角度の調整が地獄のめんどくささ
多くの倒立振子挑戦者が苦しむところだと思います。そもそもPID制御の勉強から入らないとよくわからないし・・・。
PIDパラを調整して書き込み、動きから判断してさらに調整、書き込み・・・ずっとこの繰り返しでした。
https://qiita.com/coppercele/items/e4d71537a386966338d0 ←またまた参考にさせていただいたこの方のようにいちいち書き込みしなくてもいいシステムを作っちゃう、というのもクレバーな手かと思います。
また、PIDパラも重要ですが、機体の重心による目標角度も重要です。
機体が安定して倒立する目標角度の設定がそもそも甘いと、PIDパラをいくら調整しても一方向に動き続け、いずれ倒れます。
ソフトの方の調整も大事ですが、機体のバランスというハードの調整、それを加味した目標角度の調整も同じくらい大事です。当たり前か
バランスは、電池などの重いパーツをどう配置するかによるところが大きいと思います。自分はタイヤの軸のなるべく真上に単3電池4本が来るようにしています。
MPU6050から角度を出すのはMadgwickフィルタに任せればいい
カルマンフィルタという手もありますが、やっぱり新しいものにすがったほうが楽でいいです。
Arduino nanoを9V電池で動かすのはちょっと無理がある？
9V電池をVinピンに直で刺すことで電源としていますが、電池の減りが早いように感じます。別の手段を使いたいところですが、重量は増やしたくないんですよね・・・。
3Dプリンタでサーボの軸に嵌めるパーツを作ったけど精度が甘くてゆるゆる
thingiverseで見つけたデータをちょっとアレンジして連続回転サーボの軸に嵌めこむギザギザ穴のパーツを作りましたが、この細かいギザギザは3Dプリンタで再現するには難しいということが分かりました。もちろん嵌まりはしますし、倒立振子として動かしている分には外れることはないのですが、手で引っ張ると簡単に外れるくらいにはゆるいです。事故って転倒したり、落としたりすると外れたりします。
ギザギザ穴の周りに若干隙間が見えるのは、ギザギザ穴パーツ自体を八角形の別パーツにしているからです。このパーツを小さくして、3Dプリンタの設定で精度を高めにし、量産すれば、パーツ全体を高い精度の設定で印刷しなくて済む上(時短)、印刷時の個体差が出てもパーツを交換することで対応できると踏んだからです。八角形なのは、STLデータを軽くしつつ、普通の丸穴に嵌めこみやすいからです。
モータはめっちゃ電池食う
最初は9V電池1つでArduino nanoと連続回転サーボ2つをいっぺんに動かそうとしましたが、さすがにパワー不足でした。
電源を分けることで解決しましたが、重量バランスを考えないといけない因子が増えたのはちょっと厄介だと思います。
目標角度に近いときにモータを動かさないようにしたら倒立しなかった
最初はアソビというかアイドリングのつもりで、目標角度に近い(±3°)のときにモータの出力を止めるようにしたら、なかなか倒立しませんでした。これを±0°、つまり常にモータになんらかの出力を入れる、アソビなしの状態にしたら驚くほど安定するようになりました。±3°という数字が大きすぎたのかもしれません。でも、常にピクピク動いて安定させようとしているほうが、実際安定します。
連続回転サーボの個体差か、軸のブレか、回転するように動く
先述の通り速度調整には向かない連続回転サーボ、傾いたときにそれを相殺するために動きますが、左右で回転する速度が違うため、徐々に旋回するように動きます。これはPIDパラ、回転時のオフセット、そしてハード面で車軸が一直線上にないかもしれないことが原因として考えられます。下記の動画のように、踊るように回り続けます。一点・一直線上でのみ動くというような仕様にはならなかったのは悔いが残る結果かもしれません。
他にも、現在取り組んでいる赤外線リモコンでの操作はマジで難航しまくっていますが、ここでは特に言及しません。
コード
Arduinoに書き込むソースコードを公開します。この通りに制作しても、機体の重心やタイヤ径が違えば、当然倒立はしてくれないでしょう。その辺の調整を自分の手でやって、世界に一つだけの、自分だけの倒立振子を作り上げていくところも、倒立振子の醍醐味・・・と勝手に考えて納得しています。そう思わないとやってられない
PIDパラだけここにも併記しますが、自分の場合、Kp=150,Ki=2000,Kd=2.0で倒立しました。しかしこれも更なる調整が必須かと・・・。
touritusinsi_basic.ino(クリックして展開)
// 倒立振子 基本形
// https://shizenkarasuzon.hatenablog.com/entry/2019/02/16/181342
// https://qiita.com/coppercele/items/e4d71537a386966338d0
// https://qiita.com/coppercele/items/527228e3f08c53597bd1
// touritusinsi_test6をベースに清書
// ごく単純な倒立振子。操作等はできない。
// Arduino nano互換機、連続回転サーボ2個、GY-521(MPU6050モジュール)をブレッドボードに
// タイヤはタミヤのスポーツタイヤセット(56mm径)、ボディは3Dプリンタ
// Arduino nanoに9V角型電池(すぐなくなる)、モーターに単3電池4本
#include <Servo.h> // サーボモータを動かすライブラリ(ここでは連続回転サーボを使用)
Servo myservo1; // 右車輪を定義、ラベルはB
Servo myservo2; // 左車輪を定義、ラベルはA
#include <Wire.h> // I2C通信用ライブラリ
#include <MadgwickAHRS.h> // 角度と角速度にMadgwickフィルタをかけるライブラリ
Madgwick MadgwickFilter; // Madgwickフィルタのオブジェクトを設定
#define MPU6050_PWR_MGMT_1 0x6B // Read and Write
#define MPU_ADDRESS 0x68
int stoptheta = 65; // 倒れすぎたらモータを止める角度
float Kp = 150.0; // Pゲイン 全体的なモータのパワーに効く
float Ki = 2000.0; // Iゲイン バランスが崩れて全体が平行移動しだしたときに効く
float Kd = 2.0; // Dゲイン 激しく動いたときのブレーキの役割をする
float target = 83.0; // 目標値。モジュールが横置きなら0前後、縦置きなら90前後
// 上記のPIDパラと目標角は詰めが甘いかもしれない
// PIDパラは倒立しうる最小の値の組み合わせがベストらしい。これは大きいかもしれない。
// 目標角度は機体の前後の重量バランスによる。偏ると平行移動しがち。ある意味PIDパラより重要かも
float P, I, D, preP; // 偏差、その微分・積分、積分記録用
float dt, preTime; // 処理時間
float power = 0; // 連続回転サーボの出力(90からどれだけ動かすか)
void setup() {
Wire.begin();
Serial.begin(115200); //シリアル通信を115200bpsに設定
// 動作モードの読み出し
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(MPU6050_PWR_MGMT_1); // MPU6050_PWR_MGMT_1レジスタの設定
Wire.write(0x00);
Wire.endTransmission();
MadgwickFilter.begin(100); // Madgwickフィルタの周波数を100Hzに設定。
// ここを変えると角度の感度が変わる
// 100Hzだと妙に角度が暴れたりはしないけど、0°→90°になるまでちょっと計算に時間がかかるから、↓
// 起動したら少し待たないと倒立しない。かといってすぐ90°になるようにすると↓
// シリアル通信で見た限りは角度が暴れるから、うまく倒立しないかもしれない。
myservo1.attach(5); // 右車輪の信号線をD5番ピンに接続
myservo2.attach(6); // 左車輪の信号線をD6番ピンに接続
pinMode(13, OUTPUT); // オンボードLED（確認用）
}
void loop() {
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(0x68, 14, true);
while (Wire.available() < 14);
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, Temperature;
// センサーから加速度、角速度、温度を取得
axRaw = Wire.read() << 8 | Wire.read();
ayRaw = Wire.read() << 8 | Wire.read();
azRaw = Wire.read() << 8 | Wire.read();
Temperature = Wire.read() << 8 | Wire.read();
gxRaw = Wire.read() << 8 | Wire.read();
gyRaw = Wire.read() << 8 | Wire.read();
gzRaw = Wire.read() << 8 | Wire.read();
// 加速度値を分解能で割って加速度(G)に変換する
float acc_x = axRaw / 16384.0; //FS_SEL_0 16,384 LSB / g
float acc_y = ayRaw / 16384.0;
float acc_z = azRaw / 16384.0;
// 角速度値を分解能で割って角速度(degrees per sec)に変換する
float gyro_x = gxRaw / 131.0; // (度/s)
float gyro_y = gyRaw / 131.0;
float gyro_z = gzRaw / 131.0;
// Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
// rollの計算結果のみを取得する。pitchとyawは使わない。このrollが現在の角度になる。
// 前に傾いたら90+x°、後ろに傾いたら90-x°の値をとる。
float roll = MadgwickFilter.getRoll();
// Serialに表示
Serial.print("roll =\t"); Serial.print(roll); Serial.print(",\t");
dt = (micros() - preTime) / 1000000; // 処理時間を求める
preTime = micros(); // 処理時間を記録
// PID制御
// 目標角度から現在の角度を引いて偏差を求める
P = (target - roll) / 90; // -90~90を取るので180で割って-1.0~1.0にする
I += P * dt; // 偏差を積分する
D = (P - preP) / dt; // 偏差を微分する
preP = P; // 偏差を記録する
// Serialに表示
Serial.print("P =\t"); Serial.print(P); Serial.print(",\t");
Serial.print("I =\t"); Serial.print(I); Serial.print(",\t");
Serial.print("D =\t"); Serial.print(D); Serial.print(",\t");
// 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
if (100 < abs(I * Ki))I = 0;
// 出力を計算する
power = Kp * P + Ki * I + Kd * D;
power = constrain(power, -50, 50); // ±50に制限
// 後ろに傾くとpowerはプラス、前に傾くとマイナス
Serial.print("power =\t"); Serial.print(power); Serial.print("\n");
// 角度を検知してモータを動作させる。(倒立振子の主動作)
// 前に傾いたら前に進んで相殺
// 目標角83°<現在の角度(例:100°)<83+65=148°なら前に進んで83°に減らそうとする。
if ( target < roll && roll < stoptheta + target) {
// 連続回転サーボが左右でちょうどよく回転するようにオフセットを調整したいけど、↓
// 軸がぶれてたり個体差があったりするからどうしようもないかもしれない。
myservo1.write(92 + power); // 92で左前方に旋回、93,94,95で右前方に旋回
myservo2.write(90 - power);
}
// 後ろに傾いたら後ろに進んで相殺
// 83-65=18°<現在の角度(例:70°)<目標角83°なら後ろに進んで83°に増やそうとする。
if (-stoptheta + target < roll && roll < target) {
myservo1.write(90 + power);
myservo2.write(93 - power); // 92で右後方に旋回（早すぎ）、93で左後方に旋回
}
// 倒れすぎたら停止(倒れても動き続けるとうるさいし事故るから)
// 148°より大、もしくは18°より小ならモーターを止め、PIDパラもゼロにする。
if (roll < -stoptheta + target || stoptheta + target < roll) {
digitalWrite(13, LOW);
myservo1.write(90);
myservo2.write(90);
P = 0;
I = 0;
D = 0;
return;
}
}
フローチャート
draw.ioで作成しました。プログラムのフローチャートは授業の課題で作った以来なので、正しい書き方かどうかはわかりませんが、掲載します。
結果
倒立振子が完成した pic.twitter.com/BFIbe4JX9y
— まーくん (@ogawa_megane) August 2, 2020
倒立しました。10分くらい放置してても倒立し続けます。指で軽～く押したり、障害物に当たる程度の外乱であれば倒れません。
しかしバランスが悪いのか、10分くらい放置してると最初の位置から2メートルくらい移動しています。
その場で回り続ける倒立振子… pic.twitter.com/7sQxffCL3V
— まーくん (@ogawa_megane) August 3, 2020
左右の車輪の回転速度が違うせいか、踊るように回り続けます。
まぁ・・・これはこれでカワイイですけど。
今後の展望
- 赤外線リモコン操作を実装
- PIDパラのさらなる調整
- ボディの改良、顔や腕などの取り付け
- 基板へ実装
- 他のロボットへの応用
いろいろ考えてますが、コロナ禍で家にいる時間が増えたので、今のうちにいろいろチャレンジしたいと思います。