Edited at

倒立振子を作ってみた。

More than 1 year has passed since last update.


Arduinoと3Dプリンターの練習に倒立振子を作ってみた

2016年3月7日

この記事とかを見て以外と簡単に作れそうだと思い、やってみたらいろいろ苦労して自分のスキルの低さを痛感した。なんとかバランスをとって自立するようになったけれど、安定性は低い。



YouTubeで見る。

世界にはすごく安定性の高いロボットもあって、自分と世界とのレベルの違いを思い知らされる。。。

作る前は「とりあえず立つようになったら、壁との距離を測って自立走行するようにしよう」とか妄想して、超音波センサーも付けたけれど、実際のところ立つのに精一杯でそれどころじゃなかった。


やってみたこと


  1. センサーはジャイロ (L3GD20)だけ。一応立ったが、数秒から運が良くて数十秒しか立たない。


  2. 長持ちしないのはジャイロのドリフトが原因だと考えて、ソフトで対策してみたがうまくいかない。


  3. センサーをジャイロ+加速度 (MPU-6050)に変更してカルマンフィルタでドリフト対策。ドリフトは解消できたものの安定度は期待はずれだった。


  4. 構造を低重心化したらまあまあ安定した。 ←いまここ



少し詳しく


  1. まずはじめにブレッドボードでジャイロセンサーをテストした。出力値(角速度)を積算することで、傾き(角度)がわかる。なるほどすごい。しかしけっこうドリフトする。数秒で1度ぐらいずれていく感じ。

    そこで起動時に静止状態でドリフト量を測定しておき、動作時にこれを打ち消す処理を入れる。これはそこそこ効果があって、だいたい数十秒で1度のずれに収まった。

    こうして得た角度をもとにPID制御でモーターを制御して自立させることができた。


  2. ドリフトが大きくなるとだんだん一方向に移動(傾き)しはじめ、速度が上がり、追いつかなくなって倒れる。数秒にわたり移動が一方に偏る場合はドリフトと見なして補正するようにした。しかし効果はいまいちだった。


  3. ドリフト対策について調べたところ、ジャイロ+加速度センサーでカルマンフィルタを使うのが良いらしいのでやってみた。結果、ドリフトは解消したが実際の安定度はいまいち。


  4. 目先を変えて低重心化をしてみたところ安定した。なーんだ。しかし起動時のバランスは神経質だし、よく倒れるし、左右によろよろするし、見るからに安定感がない。


項目
本機
優れたロボット

3Dセンサー
6軸
9軸

モーター
非力
強そう

ロータリーエンコーダ
なし
あり

制御速度
10msec
1msec

制御ソフト
立つだけ
自由に移動

いろいろと違いすぎる。

でも、倒立振子はこれで終わりにする。


ジャイロ/加速度センサーについて

ジャイロは角速度がわかる。しかし絶対角度(垂直かどうか)は分からない。だからたとえば起動時が垂直と仮定してそこを保つように制御する。装置をきっちり垂直にして電源ON(or リセット)しないと平衡感覚が悪くて倒れる。

加速度センサーは加速度または重力がわかる。静止時には重力だけかかるので、重力に対する角度(垂直かどうか)がわかる。静止時に加速度センサーで垂直検知し、動き出したらジャイロで追いかければ良い。

なお今回は、「センサーの取り付け角度」と「装置の重心的な垂直」は正確に合わせていないため、垂直検知は手動調整(リセットから2秒後の状態を垂直とみなす)で行う。


買い物リスト

品名
個数
価格
入手先

Arduino UNO互換品
1
500
aitendo

 +部品
1
250

 +発振子
1
60

 +AVRマイコン
1
380

ユニバーサル基板
1
120

3軸加速度/ジャイロ (MPU-6050)
1
475

モータードライバーIC (L298N)
1
250

ピンヘッダ 2.54mm/単列/40P
1
25

ピンソケット 2.54mm/単列/4P
2
20

ソケット付きワイヤ
2
150

ギヤ付きDCモーター
2
390

タイヤセット
1
400
秋月

電池ボックス 単3×4
1
40

電池ボックス 006P
1
50

トグルスイッチ
1
80

DCプラグ
1
30

熱収縮チューブ 1.5mm
1
40

六角穴付ボルト M3×8
4
539
モノタロウ

六角穴付ボルト M3×12
8
539


回路図



超音波センサー (HC-SR04) は使ってないので無視してください。


3Dプリントデータ

stlファイルをダウンロードできます。

http://www.thingiverse.com/thing:1399573


プログラム(スケッチ)



  1. balancer2.ino メイン


  2. KalmanFilter.h カルマンフィルタのクラス定義


  3. KalmanFilter.cpp カルマンフィルタのクラス実装

参考リンク:カルマンフィルタの計算式


balancer2.ino

// 倒立振子、加速度/ジャイロセンサー(MPU-6050)版

// 2016/3/4

#include <Wire.h>
#include "KalmanFilter.h"

// モータードライバの制御定数。
#define MOTOR_L_1 4 // L298 INPUT 1
#define MOTOR_L_2 5 // L298 INPUT 2
#define MOTOR_L_E 6 // L298 ENABLE A
#define MOTOR_R_1 7 // L298 INPUT 3
#define MOTOR_R_2 8 // L298 INPUT 4
#define MOTOR_R_E 9 // L298 ENABLE B
#define V_MIN 30 // モーター駆動電圧(0~255)の最低値。これより低いと回らない。
#define V_MAX 255 // モーター駆動電圧(0~255)の最高値。

// 加速度/ジャイロセンサーの制御定数。
#define MPU6050_ADDR 0x68 // MPU-6050 device address
#define MPU6050_SMPLRT_DIV 0x19 // MPU-6050 register address
#define MPU6050_CONFIG 0x1a
#define MPU6050_GYRO_CONFIG 0x1b
#define MPU6050_ACCEL_CONFIG 0x1c
#define MPU6050_ACCEL_XOUT_H 0x3b
#define MPU6050_ACCEL_XOUT_L 0x3c
#define MPU6050_ACCEL_YOUT_H 0x3d
#define MPU6050_ACCEL_YOUT_L 0x3e
#define MPU6050_ACCEL_ZOUT_H 0x3f
#define MPU6050_ACCEL_ZOUT_L 0x40
#define MPU6050_GYRO_XOUT_H 0x43
#define MPU6050_GYRO_XOUT_L 0x44
#define MPU6050_GYRO_YOUT_H 0x45
#define MPU6050_GYRO_YOUT_L 0x46
#define MPU6050_GYRO_ZOUT_H 0x47
#define MPU6050_GYRO_ZOUT_L 0x48
#define MPU6050_PWR_MGMT_1 0x6b
#define MPU6050_WHO_AM_I 0x75

// 加速度/ジャイロセンサーの制御変数。
KalmanFilter gKfx, gKfy; // カルマンフィルタ。
float gCalibrateY; // 初期化時の角度。(=静止角とみなす)
long gPrevMicros; // loop()間隔の計測用。

// 倒立振子の制御変数。
float gPowerP, gPowerI, gPowerD; // 現在出力値とPID成分。

// 加速度/ジャイロセンサーへのコマンド送信。
void writeMPU6050(byte reg, byte data) {
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(reg);
Wire.write(data);
Wire.endTransmission();
}

// 加速度/ジャイロセンサーからのデータ読み込み。
byte readMPU6050(byte reg) {
Wire.beginTransmission(MPU6050_ADDR);
Wire.write(reg);
Wire.endTransmission(false);
Wire.requestFrom(MPU6050_ADDR, 1/*length*/, false);
byte data = Wire.read();
Wire.endTransmission(true);
return data;
}

void setup() {
Serial.begin(115200);

// モータードライバの初期化。
pinMode(MOTOR_L_1, OUTPUT);
pinMode(MOTOR_L_2, OUTPUT);
pinMode(MOTOR_L_E, OUTPUT);
pinMode(MOTOR_R_1, OUTPUT);
pinMode(MOTOR_R_2, OUTPUT);
pinMode(MOTOR_R_E, OUTPUT);

// 加速度/ジャイロセンサーの初期化。
Wire.begin();
if (readMPU6050(MPU6050_WHO_AM_I) != 0x68) {
Serial.println("\nWHO_AM_I error.");
while (true) ;
}
writeMPU6050(MPU6050_SMPLRT_DIV, 0x07); // sample rate: 8kHz/(7+1) = 1kHz
writeMPU6050(MPU6050_CONFIG, 0x00); // disable DLPF, gyro output rate = 8kHz
writeMPU6050(MPU6050_GYRO_CONFIG, 0x00); // gyro range: ±250dps
writeMPU6050(MPU6050_ACCEL_CONFIG, 0x00); // accel range: ±2g
writeMPU6050(MPU6050_PWR_MGMT_1, 0x01); // disable sleep mode, PLL with X gyro
delay(2000);

// 重力加速度から求めた角度をカルマンフィルタの初期値とする。
float ax = (readMPU6050(MPU6050_ACCEL_XOUT_H) << 8) | readMPU6050(MPU6050_ACCEL_XOUT_L);
float ay = (readMPU6050(MPU6050_ACCEL_YOUT_H) << 8) | readMPU6050(MPU6050_ACCEL_YOUT_L);
float az = (readMPU6050(MPU6050_ACCEL_ZOUT_H) << 8) | readMPU6050(MPU6050_ACCEL_ZOUT_L);
float degRoll = atan2(ay, az) * RAD_TO_DEG;
float degPitch = atan(-ax / sqrt(ay * ay + az * az)) * RAD_TO_DEG;
gKfx.setAngle(degRoll);
gKfy.setAngle(degPitch);
gCalibrateY = degPitch;
gPrevMicros = micros();
}

void loop() {

// 重力加速度から角度を求める。
float ax = (readMPU6050(MPU6050_ACCEL_XOUT_H) << 8) | readMPU6050(MPU6050_ACCEL_XOUT_L);
float ay = (readMPU6050(MPU6050_ACCEL_YOUT_H) << 8) | readMPU6050(MPU6050_ACCEL_YOUT_L);
float az = (readMPU6050(MPU6050_ACCEL_ZOUT_H) << 8) | readMPU6050(MPU6050_ACCEL_ZOUT_L);
float degRoll = atan2(ay, az) * RAD_TO_DEG;
float degPitch = atan(-ax / sqrt(ay * ay + az * az)) * RAD_TO_DEG;

// ジャイロで角速度を求める。
float gx = (readMPU6050(MPU6050_GYRO_XOUT_H) << 8) | readMPU6050(MPU6050_GYRO_XOUT_L);
float gy = (readMPU6050(MPU6050_GYRO_YOUT_H) << 8) | readMPU6050(MPU6050_GYRO_YOUT_L);
float gz = (readMPU6050(MPU6050_GYRO_ZOUT_H) << 8) | readMPU6050(MPU6050_GYRO_ZOUT_L);
float dpsX = gx / 131.0; // LSB sensitivity: 131 LSB/dps @ ±250dps
float dpsY = gy / 131.0;
float dpsZ = gz / 131.0;

// カルマンフィルタで角度(x,y)を計算する。
long curMicros = micros();
float dt = (float)(curMicros - gPrevMicros) / 1000000; // μsec -> sec
gPrevMicros = curMicros;
float degX = gKfx.calcAngle(degRoll, dpsX, dt);
float degY = gKfy.calcAngle(degPitch, dpsY, dt);
degY -= gCalibrateY;

// PID制御でモーター出力を計算。
gPowerP = degY / 90; // P成分:傾き-90~90度 → -1~1
gPowerI += gPowerP; // I成分:傾きの積算。
gPowerD = dpsY / 250; // D成分:角速度-250~250dps → -1~1
float power = gPowerP * 17.0 + // この数字は試行錯誤で調整。
gPowerI * 1.5 +
gPowerD * 2.0;
power = max(-1, min(1, power)); // → -1~1
float powerL = power;
float powerR = power;
// 方向転換させる場合はここでpowerL, Rに差分を与える。

// powerをモーター駆動電圧に変換。0~1 → V_MIN~V_MAX
int voltL = (int)((V_MAX - V_MIN) * abs(powerL) + V_MIN);
int voltR = (int)((V_MAX - V_MIN) * abs(powerR) + V_MIN);

// モーター駆動。
analogWrite(MOTOR_L_E, voltL);
digitalWrite(MOTOR_L_1, (0 <= powerL) ? HIGH : LOW);
digitalWrite(MOTOR_L_2, (0 <= powerL) ? LOW : HIGH);
analogWrite(MOTOR_R_E, voltR);
digitalWrite(MOTOR_R_1, (0 <= powerR) ? LOW : HIGH);
digitalWrite(MOTOR_R_2, (0 <= powerR) ? HIGH : LOW);

// 倒れたらモーター停止。
if (30 < abs(degY)) {
analogWrite(MOTOR_L_E, 0);
analogWrite(MOTOR_R_E, 0);
Serial.print("**** stop ");
Serial.print(degY);
while (1) delay(1000);
}

// デバッグ用。
static int ps;
if (++ps % 1 == 0) {
Serial.print("dt:"); Serial.print(dt);
Serial.print("\tdegY:"); Serial.print(degY);
Serial.print("\tP:"); Serial.print(gPowerP);
Serial.print("\tI:"); Serial.print(gPowerI);
Serial.print("\tD:"); Serial.print(gPowerD);
Serial.print("\tpwr:"); Serial.print(power);
Serial.println("");
}
delay(1);
}



KalmanFilter.h

// カルマンフィルタ。

// 2016/3/3

class KalmanFilter {
private:
float angle;
float bias;
float P[2][2];
public:
KalmanFilter();
void setAngle(float newAngle);
float calcAngle(float newAngle, float newRate, float dt);
};



KalmanFilter.cpp

// カルマンフィルタ。

// 2016/3/3
//
// 計算式の参考資料。
// http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

#include "KalmanFilter.h"

KalmanFilter::KalmanFilter() {
angle = 0.0;
bias = 0.0;
P[0][0] = 0.0;
P[0][1] = 0.0;
P[1][0] = 0.0;
P[1][1] = 0.0;
};

void KalmanFilter::setAngle(float newAngle) {
angle = newAngle;
};

float KalmanFilter::calcAngle(float newAngle, float newRate, float dt) {

// variances
float Q_angle = 0.001;
float Q_bias = 0.003;
float R_measure = 0.03;

// step 1
float rate = newRate - bias;
angle += dt * rate;

// step 2
P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;

// step 3
float y = newAngle - angle;

// step 4
float S = P[0][0] + R_measure;

// step 5
float K[2];
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;

// step 6
angle += K[0] * y;
bias += K[1] * y;

// step 7
float P00_temp = P[0][0];
float P01_temp = P[0][1];
P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;

return angle;
};