本記事では、先日アップロードした動画内で製作した倒立振子に使用した回路図とプログラムについて解説します。
動画を見ていない方は先にご覧いただくと､本記事の理解がより深まります｡
今回の倒立振子の構成（回路図）
概要
倒立振子の主なハードウェア構成部品はArduinoマイコンと、車体姿勢を計測するセンサ、モータの3つです。モータにロータリエンコーダを搭載して移動距離を取得する場合もありますが、今回は搭載せず純粋に車体姿勢のみで制御を行います。
今回はマイコンにはArduino Uno R3を採用しました。姿勢計測には、I2Cでマイコンと通信でき、3軸のジャイロセンサと3軸の加速度センサが一つのチップに入ったIMU (Inertial Measurement Unit/慣性計測装置)であるMPU6050を採用しました。また、モータを駆動するモータドライバICにはL298Nを採用しました。
電源について
動画でも紹介した1台目の試作機では、モータの駆動電圧に単3乾電池4本で6V、Arduinoの電源入力に006Pを1本使用していましたが、車体重量がかさんで直立させることができませんでした。モータとマイコンで電池を別にしていた理由は、同じ電池から電源を取った場合、モータに電流が流れたときに電圧が降下してマイコンにリセットがかかってしまったためです。
そのため、今回製作した2台目では、マイコンとモータを駆動する電源には直流安定化電源を使用し、そこから重さをほぼ無視できるUEW（ポリウレタン銅線）を空中配線することで、外部電源の供給によって動作するようにしました。
また、ArduinoのVinに繋がるVccは、モータドライバの電源入力にも繋がっています。本来、マブチFA-130モータの定格電圧は3Vですが、トルク不足を補うために電源電圧に定格を超えた6~7V程度の電圧をかけています。
モータについて
モータドライバL298NはIN1, IN2のどちらかをHighにすることで回転方向を決定し、Enableに入力するPWMで回転速度が決まります。そのため、モータドライバのIN1,IN2に接続されたLED二つで回転方向を表すことができます。
また、回路図に記載されていませんが、モータから発生するノイズによる影響を低減するため、モータの+,-端子とモータのボディそれぞれを1μF程度のセラミックコンデンサで繋いでいます。これにより、モータから発生するノイズでマイコンがリセットしてしまう現象を防ぐことができます。
Arduinoのプログラム（スケッチ）
概要
倒立振子で使用したプログラムを以下に示します。
動作にはMPU6050とMadgwickフィルタのライブラリが必要になります。参考にした文献・記事などはコード内に記載しています。
/*
参考
https://shizenkarasuzon.hatenablog.com/entry/2019/02/16/162647
https://qiita.com/MuAuan/items/8dacc75b2e94fc644798
https://qiita.com/mshr299/items/8015044269ba3d5fffee
https://qiita.com/coppercele/items/e4d71537a386966338d0
*/
#include <Wire.h>
#include <MPU6050.h>
#include <MadgwickAHRS.h>
#define DEBUG 0
#define MOTOR_FWD 9
#define MOTOR_REV 8
#define ANALOG_1 5
#define PIN_PWM 3
#define MPU6050_PWR_MGMT_1 0x6B // Read and Write
#define MPU_ADDRESS 0x68
#define V_MIN 20 // モータ駆動PWM(0～255)の最低値。これより低いと回らない。
#define V_MAX 255 // モータ駆動PWM(0～255)の最高値。
const int pidArraySize = 3; // 配列の要素数
Madgwick MadgwickFilter;
MPU6050 mpu;
//float pitch = 0;
float roll = 0;
//loat yaw = 0;
float prevRoll = 0;
float Kp = 700; // Pゲイン
float Ki = 50; // Iゲイン
float Kd = 75; // Dゲイン
float target = -99.2; // 目標値。モジュールが横置きなら0前後、縦置きなら90前後
float dt, preTime;
float P, I, D, U, preP;
float power = 0; // モータの出力(PID計算結果)
int pwm; //pwmデューティ比0~255
int stoptheta = 40; // 倒れすぎたらモータを止める角度
void processSerialData() {
char inputBuffer[20]; // シリアルからの入力を格納するバッファ
int currentIndex = 0; // バッファの現在の位置
float values[pidArraySize]; // 分割された値を格納する配列
for (int i=0; i<3; i++){
currentIndex = 0;
while(Serial.available()){
char receivedChar = Serial.read();
//Serial.println(receivedChar);
if (receivedChar == ',' || receivedChar == '\0') { // カンマを区切り文字として処理
inputBuffer[currentIndex] = '\0'; // 文字列を終端する
break;
} else {
inputBuffer[currentIndex] = receivedChar; // バッファに文字を追加
}
currentIndex++;
}
values[i] = atof(inputBuffer); // 文字列を整数に変換して配列に格納
/*if (currentIndex >= pidArraySize) {
// 配列がすでにいっぱいの場合、何か処理を行うか、エラーメッセージを送信できます
currentIndex = 0; // バッファをリセット
}*/
}
while(Serial.available()){
Serial.read();
}
Kp = values[0];
Ki = values[1];
Kd = values[2];
}
void setup()
{
Wire.begin();
Serial.begin(115200);
// 動作モードの読み出し
Wire.beginTransmission(MPU_ADDRESS);
Wire.write(MPU6050_PWR_MGMT_1);
Wire.write(0x00);
Wire.endTransmission();
MadgwickFilter.begin(100); //100Hz
pinMode(MOTOR_FWD,OUTPUT);
pinMode(MOTOR_REV,OUTPUT);
}
void loop()
{
if (Serial.available() > 0) {
processSerialData(); // シリアルに入力があったら処理を呼ぶ
}
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(0x68, 14);
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;
MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
roll = MadgwickFilter.getRoll();//角度取得
dt = (micros() - preTime) * 0.000001; // 処理時間を求める
preTime = micros(); // 処理時間を記録
// PID制御
// 目標角度から現在の角度を引いて偏差を求める
P = (target - roll) / 90; // -90~90を取るので180で割って-1.0~1.0にする
I += P * dt; // 偏差を積分する
D = (P - preP) / dt; // 偏差を微分する
preP = P; // 偏差を記録する
// 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
if (150 < abs(I * Ki))I = 0;
#if DEBUG
Serial.print("roll =\t"); Serial.print(roll); Serial.print(",\t");
Serial.print("KP*P =\t"); Serial.print(Kp*P,4); Serial.print(",\t");
Serial.print("KI*I =\t"); Serial.print(Ki*I,4); Serial.print(",\t");
Serial.print("KD*D =\t"); Serial.print(Kd*D,4); Serial.print(",\t");
// Serialに表示
Serial.print("P =\t"); Serial.print(P,4); Serial.print(",\t");
Serial.print("I =\t"); Serial.print(I,4); Serial.print(",\t");
Serial.print("D =\t"); Serial.print(D,4); Serial.print(", \t");
#endif
// 角度を検知してモータを動作させる。(倒立振子の主動作)
// 出力を計算する
power = (int)(Kp * P + Ki * I + Kd * D);
pwm = (int)(constrain((int)abs(power), V_MIN, V_MAX)); //255に制限 飽和する
#if DEBUG
Serial.print("power =\t"); Serial.print(power); Serial.print(",\t");
Serial.print("pwm =\t"); Serial.print(pwm); //Serial.print("\n");
#endif
if (roll < -stoptheta + target || stoptheta + target < roll) {
// 倒れすぎたら停止
digitalWrite(MOTOR_FWD,LOW); //停止（惰性回転）
digitalWrite(MOTOR_REV,LOW);
P = 0;
I = 0;
D = 0;
}else{
if (power < 0) {
digitalWrite(MOTOR_FWD,LOW);
digitalWrite(MOTOR_REV,HIGH); //逆転
analogWrite(PIN_PWM,pwm);
#if DEBUG
Serial.print(" r");
#endif
}else if (0 < power) {
digitalWrite(MOTOR_FWD,HIGH);
digitalWrite(MOTOR_REV,LOW); //正転
analogWrite(PIN_PWM,pwm);
#if DEBUG
Serial.print(" f");
#endif
}
}
}姿勢取得部
本プログラムでは、IMUが返す6軸の角速度と重力加速度の値をMadgwickフィルタに渡し、平滑化やドリフトの補正などを施したうえでセンサ自体の角度として取得しています。Madgwickフィルタの利点は、従来のカルマンフィルタなどより計算が軽く、それでいて精度も良い点です。
loop関数内の111～144行目でI2Cから値を取得し、6軸のそれぞれの値をライブラリに渡してセンサの地面に対する角度を取得しています。
Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(0x68, 14);
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;
MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
roll = MadgwickFilter.getRoll();//角度取得MadgwickFilterが便利なのは処理の軽さに加え、6軸の値を渡したうえでgetRoll, getPitch, getYawなどのメソッドから簡単に度数法で角度を取得できる点です。弧度法で取得するgetRollRadians()などもあります。
現状、setup関数内でMadgwickフィルタのオブジェクトに対してサンプルレートを100Hzに設定していますが、さらに高速化することも可能と思われます。
PID制御部
145～176行目がPID制御を行っている箇所です。
dt = (micros() - preTime) * 0.000001; // 処理時間を求める
preTime = micros(); // 処理時間を記録
// PID制御
// 目標角度から現在の角度を引いて偏差を求める
P = (target - roll) / 90; // -90~90を取るので180で割って-1.0~1.0にする
I += P * dt; // 偏差を積分する
D = (P - preP) / dt; // 偏差を微分する
preP = P; // 偏差を記録する
// 積分部分が大きくなりすぎると出力が飽和するので大きくなり過ぎたら0に戻す(アンチワインドアップ)
if (150 < abs(I * Ki))I = 0;
#if DEBUG
Serial.print("roll =\t"); Serial.print(roll); Serial.print(",\t");
Serial.print("KP*P =\t"); Serial.print(Kp*P,4); Serial.print(",\t");
Serial.print("KI*I =\t"); Serial.print(Ki*I,4); Serial.print(",\t");
Serial.print("KD*D =\t"); Serial.print(Kd*D,4); Serial.print(",\t");
// Serialに表示
Serial.print("P =\t"); Serial.print(P,4); Serial.print(",\t");
Serial.print("I =\t"); Serial.print(I,4); Serial.print(",\t");
Serial.print("D =\t"); Serial.print(D,4); Serial.print(", \t");
#endif
// 角度を検知してモータを動作させる。(倒立振子の主動作)
// 出力を計算する
power = (int)(Kp * P + Ki * I + Kd * D);
pwm = (int)(constrain((int)abs(power), V_MIN, V_MAX)); //255に制限 飽和する
前回ループ時からの時間差をマイクロ秒単位で取得し、正規化された車体の角度と時間差から微分、積分成分を求めます。その後、175行目でそれぞれの成分に事前に定義したゲインを掛けて、モータの出力を決定します。
動画内でも説明した通り
P（Proportional / 比例）制御は車体の傾いた角度に比例して、それを元に戻す方向に(負帰還)フィードバックがかかります。
I（Integral / 積分）制御は角度の時間積分で、車体が倒れ続けようとして傾いた角度の積算で制御がかかります。外乱によって横に押された場合などに有効です。
D（Differential / 微分）制御は角度の時間微分で、車体に急激な動きがあった場合のブレーキの役割をします。例えば負方向に倒れそうになっていた車体がP,I制御によって正方向に起き上がりだしたとき、微分成分は正方向に大きくなります。これを抑える方向に制御がはたらき、起き上がってきた車体が目標角(直立位置)を通り過ぎてしまうオーバーシュートを防ぐ役割があります。
なお、ArduinoのPWMでは値の入力範囲が0～255であるため、constrain関数で値の範囲を制限します。出力レベルが飽和してしまうとそれ以上の速度での制御が不可能になり、ただモータが全力で回転するだけになりますが、モータを全力で回す必要があるところまで車体が倒れてしまうと姿勢を回復することが不可能である場合がほとんどなので、特に問題はありません。
また、159行目の処理は、一方向に傾いた状態が続いて積分成分が蓄積しすぎると、積分成分が再び0に戻ってくるまで時間がかかるので、ある程度のところでリセットするためのアンチワインドアップ処理です。
モータ駆動部
182～206行目はモータ駆動処理を行う箇所です。PID制御によって算出されたモータの出力の正負によって回転方向を決定し、PWMの値によって出力を決定します。
if (roll < -stoptheta + target || stoptheta + target < roll) {
// 倒れすぎたら停止
digitalWrite(MOTOR_FWD,LOW); //停止（惰性回転）
digitalWrite(MOTOR_REV,LOW);
P = 0;
I = 0;
D = 0;
}else{
if (power < 0) {
digitalWrite(MOTOR_FWD,LOW);
digitalWrite(MOTOR_REV,HIGH); //逆転
analogWrite(PIN_PWM,pwm);
#if DEBUG
Serial.print(" r");
#endif
}else if (0 < power) {
digitalWrite(MOTOR_FWD,HIGH);
digitalWrite(MOTOR_REV,LOW); //正転
analogWrite(PIN_PWM,pwm);
#if DEBUG
Serial.print(" f");
#endif
}
}もし倒立振子が転倒した際にモータが回り続けると危険なため、一定以上の角度車体が倒れたらモータを止める処理が入っています。
デバッグ用処理
刻々と変わる姿勢を常に把握して制御するためにはある程度の処理速度が必要になりますが、ここで問題になってくるのがデバッグ用のシリアル通信です。USBでPCに接続したままデバッグを行うためシリアルに現在のセンサの値などを表示させていると、そのせいで制御に遅延が発生することがあります。
本プログラムではシリアルの通信速度を115200[bps]に設定しています。そのため、単純計算で1[bit]送信するのに8.68[μs]かかります。もしデバッグ表示のために半角10文字、つまり80[bit]を送ると694.4[μs]も貴重な時間を消費してしまうことになります。そのため、デバッグ用シリアル通信は必要なとき以外は無効にできる処理を入れておくとよいでしょう。
本プログラムでは、ソースコード冒頭のマクロ#define DEBUG 0と、プログラム内の
#if DEBUG
Serial.print("roll =\t"); Serial.print(roll); Serial.print(",\t");
Serial.print("KP*P =\t"); Serial.print(Kp*P,4); Serial.print(",\t");
Serial.print("KI*I =\t"); Serial.print(Ki*I,4); Serial.print(",\t");
Serial.print("KD*D =\t"); Serial.print(Kd*D,4); Serial.print(",\t");
// Serialに表示
Serial.print("P =\t"); Serial.print(P,4); Serial.print(",\t");
Serial.print("I =\t"); Serial.print(I,4); Serial.print(",\t");
Serial.print("D =\t"); Serial.print(D,4); Serial.print(", \t");
#endifなどのようにディレクティブで囲った処理で、不要なときはシリアル通信処理をコンパイル時点でスキップできるようにしています。
シリアル通信は悪い面だけではありません。PID制御というのはパラメータの微調整を必要とします。いちいちプログラムを編集してコンパイルしなおし、Arduinoにダウンロードするのは大変な手間です。そのため、PCにUSB接続したマイコンに対して、ターミナルからシリアルを介してPIDゲインのパラメータを送信できるプログラムを加えました。
void processSerialData() {
char inputBuffer[20]; // シリアルからの入力を格納するバッファ
int currentIndex = 0; // バッファの現在の位置
float values[pidArraySize]; // 分割された値を格納する配列
for (int i=0; i<3; i++){
currentIndex = 0;
while(Serial.available()){
char receivedChar = Serial.read();
//Serial.println(receivedChar);
if (receivedChar == ',' || receivedChar == '\0') { // カンマを区切り文字として処理
inputBuffer[currentIndex] = '\0'; // 文字列を終端する
break;
} else {
inputBuffer[currentIndex] = receivedChar; // バッファに文字を追加
}
currentIndex++;
}
values[i] = atof(inputBuffer); // 文字列を整数に変換して配列に格納
/*if (currentIndex >= pidArraySize) {
// 配列がすでにいっぱいの場合、何か処理を行うか、エラーメッセージを送信できます
currentIndex = 0; // バッファをリセット
}*/
}
while(Serial.available()){
Serial.read();
}
Kp = values[0];
Ki = values[1];
Kd = values[2];
}loop関数内の
if (Serial.available() > 0) {
processSerialData(); // シリアルに入力があったら処理を呼ぶ
}の条件分岐で呼ばれる処理で、シリアルに入力されたカンマ区切りの三つの数値をそれぞれP, I, Dゲインのパラメータに上書きします。
まとめ
いかがでしたでしょうか。「制御工学」などと聞くと難しそうな印象がありますが、高校数学の微分・積分の概念さえ頭に入っていればPID制御の理解はそう難しくないかと思います。つまり、ある連続して変化する値がどのくらい目標値から外れているか、その値が今現在上がっているか下がっているか、その値の目標値との誤差の積算はどうか、それらのパラメータによって制御を行い、値を目標値に近づけていくことがPID制御です。
実際にソースコードを読み解いてみると、単純にセンサの値の変化量を経過時間で割るかかけるかの処理をしているだけの非常にシンプルな制御であることがお分かり頂けると思います。
今回製作した倒立振子では、直立状態で完全に静止させるまでには至りませんでした。これには動画でも解説した通り、ギアボックスが持つバックラッシなどハードウェアの要因が考えられます。なお、本記事で紹介したソースコードは動画で紹介したものからさらに改善を重ね、安定性が増しています。皆さんも是非倒立振子に挑戦してみてください。
PID制御は、エアコンや温度調節機能付きのヒータ、自動車のクルーズコントロールなど、あなたが普段目にする様々なものに使われています。この記事があなたの世界を広げる一助になれば幸いです。
2024/05/28 ソースコード修正
コメント
コメント一覧 （6件）
ＡＥ－ＡＴＭＥＧＡ３２８－ＭＩＮＩ （Ａｒｄｕｉｎｏ Ｐｒｏ Ｍｉｎｉ上位互換に変え、コイン電池のようなバッテリーを積む方法を考えているのですが、イチケンさん記載の上記のプログラムでうまく動かせるものでしょうか?
コメントありがとうございます!
5V以上の電圧を供給すれば動作はするかと思いますが､ 電源供給がコイン電池程度の小電力のものですと倒立させるのは難しいかと思います｡
モータを十分に駆動できる電源を使っていただくと良いかと思います｡
イチケン 様
お返事ありがとうございます。
下記ページを見つけました。
https://www.hirotakaster.com/weblog/%E5%80%92%E7%AB%8B%E6%8C%AF%E5%AD%90%E3%82%92%E3%82%B5%E3%82%AF%E3%83%83%E3%81%A8%E4%BD%9C%E3%82%8B/
イチケンさんのおっしゃるバッテリー搭載とバランス感覚が修正されたモデルと感じました。
ピン配置を確認すれば同じプログラムでいけるでしょうか?
何度もすみません。
いつも楽しく動画拝見してます！たまには今回みたいな電子工作もいいですね(勉強になります)。しかもプログラムまで公開とは….(助かります)。
いずれは村田製作所のムラタセイサククンやJAXAの三軸制御https://www.kenkai.jaxa.jp/research/innovation/triaxial.html
などにも挑戦してもらいたいですね(要望)。プログラム公開付きで….
いつも有用な動画をアップしていただきありがとうございます。
私も動画に刺激を受けて、倒立振子にチャレンジしてみました。かなり手こずりましたが、ほぼ静止状態で安定して自立させることができました。制御プログラムは基本的に、イチケンさんと同じものですが、ハードはタミヤのダブルギアボックスを使い、２モーターとしています。単三４本と006P電池を使うと、かなり重くなるのでモーターのパワーはやはり必要なようです。PIDのパラメタはKpはかなり大きくなります、Kiはあまり効かず、Kdは有効でした。あと、当たり前かもしれませんが、うまく倒立させるには前後方向のバランスが重要のようで、電池は前後に分けて配置しました。「ヘボなハードに高級なソフト」より、まずはハードをしっかりということですね。
有益な動画をいつもありがとうございます。スケッチをコンパイルしたところ、206行目の}に対して、「Compilation error: expected ‘}’ at end of input」というメッセージが出ました。教えていただけますか？