Arduino Self-Balancing Robot

Halo semuanya!

Dalam instruksi ini, saya akan menunjukkan kepada Anda bagaimana membangun robot penyeimbang diri kecil yang dapat bergerak menghindari rintangan. Ini adalah robot kecil berukuran 4 inci lebar dan 4 inci tinggi dan didasarkan pada papan pengembangan Arduino Pro Mini dan modul accelerometer-gyroscope MPU6050.

Pada langkah-langkah berikut, kita akan melihat bagaimana antarmuka MPU6050 dengan Arduino, bagaimana mengukur sudut kemiringan robot, cara menggunakan PID untuk membuat robot tetap seimbang. Pengintai ultrasonik juga ditambahkan ke robot yang mencegahnya membentur rintangan saat berkeliaran.

Daftar Bagian

Saya membeli sebagian besar komponen ini dari aliexpress tetapi Anda dapat menemukannya di toko elektronik lain juga.

1. Arduino Pro Mini

2. Modul GY-521 dengan MPU-6050

3. DRV8833 Pengemudi motor Pololu

4. 2, 5V meningkatkan konverter

5. US-020 sensor jarak ultrasonik

6. baterai dan dudukan NCR18650

7. Pasang motor mikro metal gear (N20, 6V, 200 rpm) dan braket

8. Pasang roda 42x19mm

9. 3, prototipe PCB dua sisi (4cm x 6cm)

10. 8, 25cm Spacer nilon dan 4, mur nilon

Terlepas dari yang di atas, Anda akan memerlukan beberapa kabel, konektor berg dan satu sakelar on / off.

Langkah 1: Sedikit Teori

Mari kita mulai dengan beberapa dasar sebelum tangan kita kotor.

Robot penyeimbang sendiri mirip dengan pendulum terbalik. Tidak seperti pendulum normal yang terus mengayun setelah diberi dorongan, pendulum terbalik ini tidak bisa tetap seimbang dengan sendirinya. Itu hanya akan jatuh. Lalu bagaimana kita menyeimbangkannya? Pertimbangkan menyeimbangkan sapu pada jari telunjuk kami yang merupakan contoh klasik menyeimbangkan pendulum terbalik. Kami menggerakkan jari kami ke arah tongkat itu jatuh. Serupa halnya dengan robot yang menyeimbangkan diri sendiri, hanya saja robot itu akan jatuh ke depan atau ke belakang. Sama seperti bagaimana kita menyeimbangkan tongkat di jari kita, kita menyeimbangkan robot dengan menggerakkan rodanya ke arah jatuh. Apa yang kami coba lakukan di sini adalah untuk menjaga pusat gravitasi robot tepat di atas titik pivot.

Untuk menggerakkan motor kita memerlukan beberapa informasi tentang keadaan robot. Kita perlu mengetahui arah kemana robot itu jatuh, seberapa besar kemiringan robot dan kecepatan jatuhnya. Semua informasi ini dapat disimpulkan dari bacaan yang diperoleh dari MPU6050. Kami menggabungkan semua input ini dan menghasilkan sinyal yang menggerakkan motor dan menjaga keseimbangan robot.

Langkah 2: Ayo Mulai Membangun

Pertama-tama kita akan menyelesaikan sirkuit dan struktur robot. Robot ini dibangun di atas tiga lapisan perfboards yang berjarak 25mm terpisah menggunakan spacer nilon. Lapisan bawah berisi dua motor dan driver motor. Lapisan tengah memiliki controller, IMU, dan modul penguat boost 5V. Lapisan paling atas memiliki baterai, sakelar on / off dan sensor jarak ultrasonik (kami akan memasang ini menjelang akhir begitu robot mendapatkan keseimbangan).

Sebelum kita mulai membuat prototipe pada perfboard, kita harus memiliki gambaran yang jelas tentang di mana setiap bagian harus ditempatkan. Untuk membuat prototyping mudah, selalu lebih baik untuk menggambar tata letak fisik semua komponen dan menggunakan ini sebagai referensi untuk menempatkan komponen dan merutekan jumper di perfboard. Setelah semua bagian ditempatkan dan disolder, sambungkan ketiga papan menggunakan spacer nilon.

Anda mungkin telah memperhatikan bahwa saya telah menggunakan dua modul pengatur tegangan terpisah untuk menggerakkan motor dan pengontrol walaupun keduanya membutuhkan sumber 5V. Ini sangat penting. Dalam desain pertama saya, saya menggunakan penguat boost 5V tunggal untuk menyalakan controller dan juga motor. Ketika saya menghidupkan robot, program membeku sebentar-sebentar. Ini disebabkan oleh kebisingan yang dihasilkan dari sirkuit motor yang bekerja pada pengontrol dan IMU. Ini secara efektif dihilangkan dengan memisahkan regulator tegangan ke pengontrol dan motor dan menambahkan kapasitor 10uF di terminal catu daya motor.

Langkah 3: Mengukur Sudut Kemiringan Menggunakan Accelerometer

MPU6050 memiliki accelerometer 3-sumbu dan giroskop 3-sumbu. Akselerometer mengukur akselerasi sepanjang tiga sumbu dan giroskop mengukur laju sudut tentang ketiga sumbu tersebut. Untuk mengukur sudut kemiringan robot kita perlu nilai percepatan sepanjang sumbu y dan z. Fungsi atan2 (y, z) memberikan sudut dalam radian antara sumbu z positif dari sebuah pesawat dan titik yang diberikan oleh koordinat (z, y) pada bidang itu, dengan tanda positif untuk sudut berlawanan arah jarum jam (setengah kanan). pesawat, y> 0), dan tanda negatif untuk sudut searah jarum jam (setengah bidang kiri, y <0). Kami menggunakan pustaka ini yang ditulis oleh Jeff Rowberg untuk membaca data dari MPU6050. Unggah kode yang diberikan di bawah ini dan lihat bagaimana sudut kemiringannya bervariasi.

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

MPU6050 mpu;

int16_t accY, accZ; float accAngle;

membatalkan pengaturan () {mpu.initialize (); Serial.begin (9600); }

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

Coba gerakkan robot maju dan mundur sambil menjaganya tetap miring. Anda akan mengamati bahwa sudut yang ditunjukkan pada monitor serial Anda tiba-tiba berubah. Hal ini disebabkan komponen horizontal percepatan mengganggu nilai percepatan sumbu y dan z.

Langkah 4: Mengukur Sudut Kemiringan Menggunakan Giroskop

Giroskop 3-sumbu MPU6050 mengukur kecepatan sudut (kecepatan rotasi) di sepanjang tiga sumbu. Untuk robot self-balancing kami, kecepatan sudut sepanjang sumbu-x saja sudah cukup untuk mengukur tingkat kejatuhan robot.

Dalam kode yang diberikan di bawah ini, kita membaca nilai gyro tentang sumbu x, mengonversinya menjadi derajat per detik dan kemudian mengalikannya dengan waktu loop untuk mendapatkan perubahan sudut. Kami menambahkan ini ke sudut sebelumnya untuk mendapatkan sudut saat ini.

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

MPU6050 mpu;

int16_t gyroX, gyroRate; float gyroAngle = 0; currTime panjang yang tidak ditandatangani, prevTime = 0, loopTime;

membatalkan pengaturan () {mpu.initialize (); Serial.begin (9600); }

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

Posisi MPU6050 saat program mulai berjalan adalah titik inklinasi nol. Sudut kemiringan akan diukur sehubungan dengan titik ini.

Jaga agar robot tetap stabil pada sudut yang tetap dan Anda akan mengamati bahwa sudut tersebut secara bertahap akan bertambah atau berkurang. Itu tidak akan tetap stabil. Ini disebabkan oleh pergeseran yang melekat pada giroskop.

Dalam kode yang diberikan di atas, waktu loop dihitung menggunakan fungsi millis () yang dibangun ke dalam Arduino IDE. Pada langkah selanjutnya, kita akan menggunakan penghenti waktu untuk membuat interval pengambilan sampel yang tepat. Periode pengambilan sampel ini juga akan digunakan dalam menghasilkan output menggunakan pengontrol PID.

Langkah 5: Menggabungkan Hasil Dengan Filter Pelengkap

Google mendefinisikan pelengkap sebagai " menggabungkan sedemikian rupa untuk meningkatkan atau menekankan kualitas satu sama lain atau lain-lain ".

Kami memiliki dua pengukuran sudut dari dua sumber yang berbeda. Pengukuran dari accelerometer akan dipengaruhi oleh gerakan horisontal mendadak dan pengukuran dari giroskop secara bertahap menjauh dari nilai aktual. Dengan kata lain, pembacaan accelerometer akan dipengaruhi oleh sinyal durasi pendek dan pembacaan giroskop oleh sinyal durasi panjang. Bacaan-bacaan ini saling melengkapi satu sama lain. Gabungkan keduanya menggunakan Filter Pelengkap dan kami mendapatkan pengukuran sudut yang stabil dan akurat. Filter komplementer pada dasarnya adalah filter high-pass yang bekerja pada giroskop dan filter low-pass yang bekerja pada accelerometer untuk menyaring penyimpangan dan kebisingan dari pengukuran.

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

0, 9934 dan 0, 0066 adalah koefisien filter untuk konstanta waktu filter 0, 75s. Filter low pass memungkinkan setiap sinyal lebih lama dari durasi ini untuk melewatinya dan filter high pass memungkinkan sinyal apa pun lebih pendek dari durasi ini untuk melewati. Respons filter dapat diubah dengan memilih konstanta waktu yang tepat. Menurunkan konstanta waktu akan memungkinkan lebih banyak akselerasi horisontal untuk melewatinya.

Menghilangkan kesalahan offset accelerometer dan giroskop
Unduh dan jalankan kode yang diberikan di halaman ini untuk mengkalibrasi offset MPU6050. Kesalahan apa pun karena offset dapat dihilangkan dengan menentukan nilai offset dalam rutin setup () seperti yang ditunjukkan di bawah ini.

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

Langkah 6: Kontrol PID untuk Menghasilkan Output

PID adalah singkatan dari Proportional, Integral, dan Derivative. Masing-masing istilah ini memberikan respons unik terhadap robot penyeimbang diri kami.

Istilah proporsional, seperti namanya, menghasilkan respons yang sebanding dengan kesalahan. Untuk sistem kami, kesalahannya adalah sudut kemiringan robot.

Istilah integral menghasilkan respons berdasarkan akumulasi kesalahan. Ini pada dasarnya adalah jumlah dari semua kesalahan yang dikalikan dengan periode pengambilan sampel. Ini adalah respons berdasarkan perilaku sistem di masa lalu.

Istilah derivatif sebanding dengan turunan kesalahan. Ini adalah perbedaan antara kesalahan saat ini dan kesalahan sebelumnya dibagi dengan periode pengambilan sampel. Ini bertindak sebagai istilah prediksi yang merespons bagaimana robot berperilaku dalam loop pengambilan sampel berikutnya.

Mengalikan masing-masing istilah ini dengan konstanta terkait (yaitu, Kp, Ki dan Kd) dan menjumlahkan hasilnya, kami menghasilkan output yang kemudian dikirim sebagai perintah untuk menggerakkan motor.

Langkah 7: Menyetel Konstanta PID

1. Atur Ki dan Kd ke nol dan secara bertahap tingkatkan Kp sehingga robot mulai berosilasi tentang posisi nol.

2. Tingkatkan Ki sehingga respons robot lebih cepat ketika tidak seimbang. Ki harus cukup besar sehingga sudut kemiringan tidak bertambah. Robot harus kembali ke posisi nol jika condong.

3. Tingkatkan Kd untuk mengurangi osilasi. Overshoot juga harus dikurangi sekarang.

4. Ulangi langkah-langkah di atas dengan menyempurnakan setiap parameter untuk mencapai hasil terbaik.

Langkah 8: Menambahkan Sensor Jarak

Sensor jarak ultrasonik yang saya gunakan adalah US-020. Ini memiliki empat pin yaitu Vcc, Trig, Echo, dan Gnd. Ini didukung oleh sumber 5V. Pin pemicu dan gema masing-masing terhubung ke pin digital 9 dan 8 Arduino. Kami akan menggunakan pustaka NewPing untuk mendapatkan nilai jarak dari sensor. Kami akan membaca jarak sekali setiap 100 milidetik dan jika nilainya antara 0 dan 20cm, kami akan memerintahkan robot untuk melakukan rotasi. Ini harus cukup untuk menjauhkan robot dari rintangan.

Langkah 9: Kode Lengkap

 #sertakan "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; Sonar 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; jumlah byte yang mudah menguap = 0; jarak int Cm;

membatalkan 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); }}

membatalkan init_PID () // menginisialisasi Timer1 cli (); // nonaktifkan interupsi global TCCR1A = 0; // set seluruh register TCCR1A ke 0 TCCR1B = 0; // sama untuk TCCR1B // set bandingkan register pencocokan untuk menetapkan waktu sampel 5ms OCR1A = 9999; // nyalakan mode CTC TCCR1B

void setup () {// atur pin kontrol motor dan PWM ke mode keluaran pinMode (leftMotorPWMPin, OUTPUT); pinMode (leftMotorDirPin, OUTPUT); pinMode (rightMotorPWMPin, OUTPUT); pinMode (rightMotorDirPin, OUTPUT); // atur LED status ke mode keluaran pinMode (13, OUTPUT); // inisialisasi MPU6050 dan atur nilai offset mpu.initialize (); mpu.setYAccelOffset (1593); mpu.setZAccelOffset (963); mpu.setXGyroOffset (40); // inisialisasi loop pengambilan sampel PID init_PID (); }

void loop () {// baca nilai akselerasi dan giroskop accY = mpu.getAccelerationY (); accZ = mpu.getAccelerationZ (); gyroX = mpu.getRotationX (); // atur daya motor setelah membatasi itu motorPower = constrain (motorPower, -255, 255); setMotors (motorPower, motorPower); // ukur jarak setiap 100 milidetik jika ((hitung% 20) == 0) {distanceCm = sonar.ping_cm (); } if ((distanceCm <20) && (distanceCm! = 0)) {setMotors (-motorPower, motorPower); }} // ISR akan dipanggil setiap 5 milidetik ISR (TIMER1_COMPA_vect) {// menghitung sudut kemiringan accAngle = atan2 (accY, accZ) * RAD_TO_DEG; gyroRate = peta (gyroX, -32768, 32767, -250, 250); gyroAngle = (float) gyroRate * sampleTime; currentAngle = 0.9934 * (prevAngle + gyroAngle) + 0.0066 * (accAngle); error = currentAngle - targetAngle; errorSum = errorSum + error; errorSum = constrain (errorSum, -300, 300); // hitung output dari nilai P, I dan D motorPower = Kp * (error) + Ki * (errorSum) * sampleTime - Kd * (currentAngle-prevAngle) / sampleTime; prevAngle = currentAngle; // nyalakan led pada pin13 setiap detik ++; if (count == 200) {count = 0; digitalWrite (13, ! digitalRead (13)); }}

Langkah 10: Pikiran Final

Menghabiskan lebih banyak waktu untuk mengubah konstanta PID akan memberi kita hasil yang lebih baik. Ukuran robot kami juga membatasi tingkat stabilitas yang bisa kami raih. Lebih mudah membuat robot penyeimbang ukuran penuh daripada membuat robot kecil seperti kita. Namun, saya kira, robot kami melakukan pekerjaan yang cukup baik dalam menyeimbangkan berbagai permukaan seperti yang ditunjukkan dalam video.

Itu saja untuk sekarang.

Terima kasih atas waktunya. Jangan lupa untuk meninggalkan pemikiran Anda di bagian komentar.

Artikel Terkait