Arduino自己バランスロボット

みなさん、こんにちは!

このレッスンでは、障害物を避けて動き回ることができる小型の自己バランスロボットの構築方法を紹介します。 これは、幅4インチ、高さ4インチの小さなロボットで、Arduino Pro Mini開発ボードとMPU6050加速度計-ジャイロスコープモジュールに基づいています。

次の手順では、MPU6050とArduinoを接続する方法、ロボットの傾斜角を測定する方法、PIDを使用してロボットのバランスを保つ方法を説明します。 また、超音波距離計がロボットに追加され、歩き回るときに障害物にぶつかることを防ぎます。

パーツリスト

これらの部品のほとんどはaliexpressから購入しましたが、他の電気店でも見つけることができます。

1. Arduino Pro Mini

2. MPU-6050を備えたGY-521モジュール

3. DRV8833 Pololuモータードライバー

4. 2、5Vブーストコンバーター

5. US-020超音波距離センサー

6. NCR18650バッテリーとホルダー

7.マイクロメタルギアモーター(N20、6V、200 rpm)およびブラケットのペア

8. 42x19mmホイールのペア

9. 3、両面プロトタイプPCB(4cm x 6cm)

10. 8、25 cmのナイロンスペーサーと4、ナイロンナット

上記とは別に、いくつかのケーブル、bergコネクタ、1つのオン/オフスイッチが必要になります。

ステップ1:少しの理論

手を汚す前に、いくつかの基礎から始めましょう。

自己平衡ロボットは、逆さまの振り子に似ています。 ナッジが与えられると振れ続ける通常の振り子とは異なり、この倒立振子はそれ自体でバランスを保つことができません。 それは単に倒れます。 次に、どのようにバランスを取りますか? 人差し指でほうきの柄のバランスを取ることを検討してください。これは、倒立振子のバランスを取る典型的な例です。 スティックが落ちる方向に指を動かします。 自己平衡ロボットの場合も同様です。ただし、ロボットは前方または後方に落下します。 指の棒のバランスをとるのと同じように、落下する方向に車輪を動かしてロボットのバランスを取ります。 ここでやろうとしていることは、ロボットの重心をピボットポイントの真上に保つことです。

モーターを駆動するには、ロボットの状態に関する情報が必要です。 ロボットが落下する方向、ロボットがどれだけ傾いたか、そして落下する速度を知る必要があります。 これらの情報はすべて、MPU6050から取得した測定値から推測できます。 これらすべての入力を組み合わせて、モーターを駆動し、ロボットのバランスを保つ信号を生成します。

ステップ2:ビルドを始めましょう

まず、ロボットの回路と構造を完成させます。 ロボットは、ナイロンスペーサーを使用して25mm間隔で配置された3層のパフォーマンスボード上に構築されています。 最下層には、2つのモーターとモータードライバーが含まれています。 中間層には、コントローラー、IMU、および5Vブーストレギュレーターモジュールがあります。 最上層には、バッテリー、オン/オフスイッチ、および超音波距離センサーがあります(ロボットのバランスが取れたら、これを最後に取り付けます)。

Perfboardでプロトタイプを作成する前に、各部品を配置する場所について明確な画像を用意する必要があります。 プロトタイピングを簡単にするには、すべてのコンポーネントの物理レイアウトを描画し、これをリファレンスとして使用して、コンポーネントを配置し、ジャンパーをパフォーマンスボードに配線することをお勧めします。 すべての部品を配置してはんだ付けしたら、ナイロンスペーサーを使用して3つのボードを相互接続します。

モーターとコントローラーの両方に5Vの電源が必要な場合でも、モーターとコントローラーを駆動するために2つの個別の電圧レギュレーターモジュールを使用していることに気づいたかもしれません。 これはとても重要です。 最初の設計では、単一の5V昇圧レギュレーターを使用して、コントローラーとモーターの電源を入れました。 ロボットのスイッチを入れると、プログラムが断続的にフリーズします。 これは、コントローラーとIMUに作用するモーター回路から発生するノイズが原因でした。 これは、電圧レギュレータをコントローラーとモーターに分離し、モーターの電源端子に10uFのコンデンサを追加することにより、効果的に排除されました。

ステップ3:加速度計を使用して傾斜角を測定する

MPU6050には、3軸加速度計と3軸ジャイロスコープがあります。 加速度計は3つの軸に沿って加速度を測定し、ジャイロスコープは3つの軸の角速度を測定します。 ロボットの傾斜角を測定するには、y軸とz軸に沿った加速度値が必要です。 atan2 (y、z)関数は、平面の正のz軸とその平面上の座標(z、y)によって与えられる点との間の角度をラジアンで与えます。平面、y> 0)、時計回りの角度の負符号(左半平面、y <0)。 Jeff Rowbergによって作成されたこのライブラリを使用して、MPU6050からデータを読み取ります。 以下のコードをアップロードして、傾斜角がどのように変化するかを確認してください。

#include "Wire.h"
#include "I2Cdev.h" #include "MPU6050.h" #include "math.h"

MPU6050 mpu;

int16_t accY、accZ; float accAngle;

void setup(){mpu.initialize(); Serial.begin(9600); }

void loop(){accZ = mpu.getAccelerationZ(); accY = mpu.getAccelerationY(); accAngle = atan2(accY、accZ)* RAD_TO_DEG; if(isnan(accAngle)); else Serial.println(accAngle); }

ロボットを一定の角度に傾けたまま、前後に動かしてみてください。 シリアルモニターに表示される角度が突然変化することがわかります。 これは、y軸とz軸の加速度値を妨げる加速度の水平成分によるものです。

ステップ4:ジャイロスコープを使用した傾斜角の測定

MPU6050の3軸ジャイロスコープは、3つの軸に沿って角速度(回転速度)を測定します。 自己平衡ロボットの場合、ロボットの落下速度を測定するには、x軸に沿った角速度だけで十分です。

以下のコードでは、x軸に関するジャイロ値を読み取り、1秒あたりの角度に変換してから、ループ時間を掛けて角度の変化を取得します。 これを前の角度に追加して、現在の角度を取得します。

#include "Wire.h"
#include "I2Cdev.h" #include "MPU6050.h"

MPU6050 mpu;

int16_t gyroX、gyroRate; float gyroAngle = 0; 符号なしlong currTime、prevTime = 0、loopTime;

void setup(){mpu.initialize(); Serial.begin(9600); }

void loop(){currTime = millis(); loopTime = currTime-prevTime; prevTime = currTime; gyroX = mpu.getRotationX(); gyroRate = map(gyroX、-32768、32767、-250、250); gyroAngle = gyroAngle +(float)gyroRate * loopTime / 1000; Serial.println(gyroAngle); }

プログラムが実行を開始するときのMPU6050の位置は、ゼロ傾斜点です。 傾斜角は、この点に関して測定されます。

ロボットを一定の角度で安定させておくと、角度が徐々に増加または減少することが観察されます。 安定したままではありません。 これは、ジャイロスコープに固有のドリフトによるものです。

上記のコードでは、ループ時間はArduino IDEに組み込まれているmillis()関数を使用して計算されます。 後の手順では、タイマー割り込みを使用して、正確なサンプリング間隔を作成します。 このサンプリング周期は、PIDコントローラーを使用して出力を生成する際にも使用されます。

ステップ5:結果を補完フィルターと組み合わせる

Googleは、「 相互の品質を強化または強調する、または rに別の方法で結合することを補完的に定義しています。

2つの異なるソースからの角度の2つの測定値があります。 加速度計からの測定値は、突然の水平方向の動きの影響を受け、ジャイロスコープからの測定値は、実際の値から徐々にドリフトします。 言い換えれば、加速度計の読み取り値は、短時間の信号の影響を受け、ジャイロスコープの読み取り値は、長時間の信号の影響を受けます。 これらの測定値は、ある意味で、相互に補完的です。 相補フィルターを使用してそれらを組み合わせれば、角度の安定した正確な測定値が得られます。 補完的なフィルターは、本質的にジャイロスコープに作用するハイパスフィルターと加速度計に作用して測定からのドリフトとノイズを除去するローパスフィルターです。

currentAngle = 0.9934 *(previousAngle + gyroAngle)+ 0.0066 *(accAngle)

0.9934および0.0066は、0.75秒のフィルター時定数のフィルター係数です。 ローパスフィルターはこの期間より長い信号を通過させ、ハイパスフィルターはこの期間より短い信号を通過させます。 フィルターの応答は、正しい時定数を選択することで微調整できます。 時定数を小さくすると、より多くの水平加速度が通過できます。

加速度計とジャイロスコープのオフセットエラーの排除
このページにあるコードをダウンロードして実行し、MPU6050のオフセットを調整します。 以下に示すように、setup()ルーチンでオフセット値を定義することにより、オフセットによるエラーを排除できます。

mpu.setYAccelOffset(1593);
mpu.setZAccelOffset(963); mpu.setXGyroOffset(40);

ステップ6:出力を生成するためのPID制御

PIDは、比例、積分、および微分の略です。 これらの各用語は、自己バランスロボットに対する独自の応答を提供します。

比例項は、その名前が示すように、エラーに比例する応答を生成します。 私たちのシステムでは、エラーはロボットの傾斜角です。

積分項は、累積誤差に基づいて応答を生成します。 これは、本質的にすべてのエラーの合計にサンプリング周期を掛けたものです。 これは、過去のシステムの動作に基づいた応答です。

導関数項は誤差の導関数に比例します。 これは、現在のエラーと以前のエラーの差をサンプリング周期で割ったものです。 これは、次のサンプリングループでのロボットの動作に対応する予測用語として機能します。

これらの各項に対応する定数(Kp、Ki、Kdなど)を乗算し、結果を合計して、モーターを駆動するコマンドとして送信される出力を生成します。

ステップ7:PID定数の調整

1. KiとKdをゼロに設定し、Kpを徐々に増やして、ロボットがゼロ位置を中心に振動し始めるようにします。

2. Kiを増やして、バランスが崩れたときにロボットの応答が速くなるようにします。 Kiは、傾斜角が大きくならないように十分大きくする必要があります。 ロボットが傾いている場合、ロボットはゼロ位置に戻る必要があります。

3.振動を減らすためにKdを増やします。 オーバーシュートも今までに減らす必要があります。

4.各パラメーターを微調整して上記の手順を繰り返し、最良の結果を達成します。

ステップ8:距離センサーの追加

私が使用した超音波距離センサーはUS-020です。 Vcc、Trig、Echo、Gndの4つのピンがあります。 5Vの電源で動作します。 トリガーピンとエコーピンは、それぞれArduinoのデジタルピン9と8に接続されています。 NewPingライブラリを使用して、センサーから距離値を取得します。 100ミリ秒ごとに距離を読み取り、値が0〜20 cmの場合、ロボットに回転を実行するように命令します。 これは、ロボットを障害物から遠ざけるのに十分なはずです。

ステップ9:完全なコード

 #include "Wire.h" 

#include "I2Cdev.h" #include "MPU6050.h" #include "math.h" #include

#define leftMotorPWMPin 6 #define leftMotorDirPin 7 #define rightMotorPWMPin 5 #define rightMotorDirPin 4

#define TRIGGER_PIN 9 #define ECHO_PIN 8 #define MAX_DISTANCE 75

#define Kp 40 #define Kd 0.05 #define Ki 40 #define sampleTime 0.005 #define targetAngle -2.5

MPU6050 mpu; NewPingソナー(TRIGGER_PIN、ECHO_PIN、MAX_DISTANCE);

int16_t accY、accZ、gyroX; volatile int motorPower、gyroRate; volatile float accAngle、gyroAngle、currentAngle、prevAngle = 0、error、prevError = 0、errorSum = 0; 揮発性バイトカウント= 0; int distanceCm;

void setMotors(int leftMotorSpeed、int rightMotorSpeed){if(leftMotorSpeed> = 0){analogWrite(leftMotorPWMPin、leftMotorSpeed); digitalWrite(leftMotorDirPin、LOW); } else {analogWrite(leftMotorPWMPin、255 + leftMotorSpeed); digitalWrite(leftMotorDirPin、HIGH); } if(rightMotorSpeed> = 0){analogWrite(rightMotorPWMPin、rightMotorSpeed); digitalWrite(rightMotorDirPin、LOW); } else {analogWrite(rightMotorPWMPin、255 + rightMotorSpeed); digitalWrite(rightMotorDirPin、HIGH); }}

void init_PID()// Timer1を初期化cli(); //グローバル割り込みを無効にするTCCR1A = 0; // TCCR1Aレジスタ全体を0に設定しますTCCR1B = 0; // TCCR1Bでも同じ//比較時間レジスタを設定してサンプル時間5msを設定OCR1A = 9999; // CTCモードTCCR1Bをオンにします

void setup(){//モーター制御ピンとPWMピンを出力モードに設定pinMode(leftMotorPWMPin、OUTPUT); pinMode(leftMotorDirPin、OUTPUT); pinMode(rightMotorPWMPin、OUTPUT); pinMode(rightMotorDirPin、OUTPUT); //ステータスLEDを出力モードに設定pinMode(13、OUTPUT); // MPU6050を初期化し、オフセット値を設定しますmpu.initialize(); mpu.setYAccelOffset(1593); mpu.setZAccelOffset(963); mpu.setXGyroOffset(40); // PIDサンプリングループの初期化init_PID(); }

void loop(){//加速度とジャイロスコープの値を読み取りますaccY = mpu.getAccelerationY(); accZ = mpu.getAccelerationZ(); gyroX = mpu.getRotationX(); //制約後にモーターの出力を設定しますmotorPower = constrain(motorPower、-255、255); setMotors(motorPower、motorPower); // 100ミリ秒ごとに距離を測定if((count%20)== 0){distanceCm = sonar.ping_cm(); } if((distanceCm <20)&&(distanceCm!= 0)){setMotors(-motorPower、motorPower); }} // ISRは5ミリ秒ごとに呼び出されますISR(TIMER1_COMPA_vect){//傾斜角を計算accAngle = atan2(accY、accZ)* RAD_TO_DEG; gyroRate = map(gyroX、-32768、32767、-250、250); gyroAngle =(float)gyroRate * sampleTime; currentAngle = 0.9934 *(prevAngle + gyroAngle)+ 0.0066 *(accAngle); エラー= currentAngle-targetAngle; errorSum = errorSum +エラー; errorSum = constrain(errorSum、-300、300); // P、I、Dの値から出力を計算motorPower = Kp *(error)+ Ki *(errorSum)* sampleTime-Kd *(currentAngle-prevAngle)/ sampleTime; prevAngle = currentAngle; // count13秒ごとにpin13のledを切り替えます; if(count == 200){count = 0; digitalWrite(13、!digitalRead(13)); }}

ステップ10:最終的な考え

PID定数の調整にもう少し時間をかけると、より良い結果が得られます。 ロボットのサイズは、達成できる安定性のレベルも制限します。 私たちのような小さなロボットを構築するよりも、フルサイズのバランシングロボットを構築する方が簡単です。 それでも、私たちのロボットは、ビデオに示されているように、さまざまな表面でバランスを取るのにかなりまともな仕事をしていると思います。

今のところそれだけです。

御時間ありがとうございます。 コメント欄にあなたの考えを残すことを忘れないでください。

関連記事