Arduino Self-Balancing Robot

Hai semua!

Dalam pengajaran ini, saya akan menunjukkan kepada anda bagaimana untuk membina sebuah robot yang mengimbangi diri yang kecil yang boleh bergerak mengelakkan halangan. Ini adalah robot kecil yang berukuran 4 inci lebar dan 4 inci tinggi dan berdasarkan papan pengembangan Arduino Pro Mini dan modul gyroscope accelerometer MPU6050.

Dalam langkah-langkah yang berikut, kita akan melihat bagaimana untuk mengawal MPU6050 dengan Arduino, bagaimana untuk mengukur sudut kecenderungan robot, bagaimana menggunakan PID untuk menjadikan robot tetap seimbang. Rangefinder ultrasonik juga ditambah kepada robot yang menghalangnya daripada terhantuk dalam halangan ketika ia mengembara sekitar.

Senarai Bahagian

Saya membeli kebanyakan bahagian ini dari aliexpress tetapi anda boleh menemui mereka di mana-mana kedai elektronik lain juga.

1. Arduino Pro Mini

2. Modul GY-521 dengan MPU-6050

3. DRV8833 Pololu pemandu motor

4. 2, 5V meningkatkan penukar

5. US-020 sensor jarak ultrasonik

6. bateri dan pemegang NCR18650

7. Sepasang motor gear logam mikro (N20, 6V, 200 rpm) dan kurungan

8. Sepasang roda 42x19mm

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

10. 8, 25cm Nylon spacers dan 4, kacang nilon

Selain itu, anda memerlukan beberapa kabel, penyambung berg dan satu suis hidup / mati.

Langkah 1: Satu Bit Teori

Mari kita mulakan dengan asas-asas sebelum mendapat tangan kita kotor.

Robot mengimbangi diri mirip dengan pendulum terbalik. Tidak seperti pendulum biasa yang terus berayun setelah diberi nudge, pendulum terbalik ini tidak dapat seimbang dengan sendirinya. Ia hanya akan jatuh. Kemudian bagaimana kita mengimbanginya? Pertimbangkan menyeimbangkan sapu di jari telunjuk kami yang merupakan contoh klasik menyeimbangkan pendulum yang terbalik. Kami menggerakkan jari kita ke arah mana batang itu jatuh. Begitu juga dengan robot pengimbang diri, hanya robot yang akan jatuh sama ada ke depan atau ke belakang. Sama seperti bagaimana kita mengimbangi tongkat pada jari kita, kita mengimbangi robot dengan memacu roda-rodanya ke arah yang mana ia jatuh. Apa yang kita cuba lakukan di sini adalah untuk menjaga pusat graviti robot betul-betul di atas titik pangsi.

Untuk memandu motor, kita memerlukan maklumat mengenai keadaan robot. Kita perlu tahu arah di mana robot itu jatuh, berapa robot yang miring dan kelajuan yang mana ia jatuh. Semua maklumat ini dapat disimpulkan dari bacaan yang diperolehi daripada MPU6050. Kami menggabungkan semua input ini dan menghasilkan isyarat yang memacu motor dan memastikan robot itu seimbang.

Langkah 2: Let's Start Building

Kami akan melengkapkan terlebih dahulu litar dan struktur robot. Robot ini dibina di atas tiga lapisan perfusi yang jaraknya 25mm dengan menggunakan spacer nilon. Lapisan bawah mengandungi dua motor dan pemandu motor. Lapisan tengah mempunyai pengawal, IMU, dan modul pengawal rangsangan 5V. Lapisan paling atas mempunyai bateri, suis on / off dan sensor jarak ultrasonik (kami akan memasang ini ke arah akhir sebaik sahaja kita dapat mengimbangi robot).

Sebelum kita memulakan prototaip pada perfboard kita harus mempunyai gambaran yang jelas tentang tempat setiap bahagian harus diletakkan. Untuk membuat prototaip mudah, adalah lebih baik untuk melukis susun atur fizikal semua komponen dan menggunakan ini sebagai rujukan untuk meletakkan komponen dan laluan jumper pada perfboard. Apabila semua bahagian diletakkan dan dipateri, sambungkan tiga papan menggunakan spacer nilon.

Anda mungkin perasan bahawa saya telah menggunakan dua modul pengawal voltan berasingan untuk memandu motor dan pengawal walaupun kedua-duanya memerlukan sumber 5V. Ini sangat penting. Dalam reka bentuk pertama saya, saya menggunakan pengatur rangsangan 5V tunggal untuk menyalakan pengawal serta motor. Apabila saya menghidupkan robot, program ini membeku secara berselang-seli. Ini disebabkan bunyi yang dihasilkan dari litar motor yang bertindak ke atas pengawal dan IMU. Ini berkesan dihapuskan dengan memisahkan pengatur voltan kepada pengawal dan motor dan menambah kapasitor 10uF di terminal bekalan kuasa motor.

Langkah 3: Mengukur Sudut Inklinasi Menggunakan Meterometer

MPU6050 mempunyai pecutan 3 paksi dan gyroscope 3 paksi. Akselerometer mengukur percepatan di sepanjang tiga paksi dan giroskop mengukur kadar sudut mengenai tiga paksi. Untuk mengukur sudut kecenderungan robot kita memerlukan nilai percepatan di sepanjang y dan z-paksi. Fungsi atan2 (y, z) memberikan sudut dalam radian antara paksi z positif satah dan titik yang diberikan oleh koordinat (z, y) pada satah itu, dengan tanda positif untuk sudut lawan arah jam (kanan separuh- satah, y> 0), dan tanda negatif untuk sudut mengikut arah jam (kiri satah satah, y <0). Kami menggunakan perpustakaan ini yang ditulis oleh Jeff Rowberg untuk membaca data dari MPU6050. Muat naik kod yang diberikan di bawah dan lihat bagaimana sudut kecenderungan berbeza-beza.

#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; jika (isnan (accAngle)); lain Serial.println (accAngle); }

Cuba gerakkan robot ke hadapan dan ke belakang sambil memastikan ia condong pada beberapa sudut tetap. Anda akan melihat bahawa sudut yang ditunjukkan dalam monitor bersiri anda tiba-tiba berubah. Ini disebabkan oleh komponen pecutan mendatar yang mengganggu nilai-nilai pecutan y dan axis-axes.

Langkah 4: Mengukur Sudut Inclination Menggunakan Gyroscope

Gyroscope 3 axis MPU6050 mengukur kadar sudut (putaran halaju) di sepanjang tiga paksi. Untuk robot pengimbang diri kita, halaju sudut di sepanjang paksi x sahaja cukup untuk mengukur kadar kejatuhan robot.

Dalam kod yang diberikan di bawah, kita membaca nilai gyro mengenai paksi-x, menukarnya kepada darjah per saat dan kemudian darab dengan masa gelung untuk mendapatkan perubahan dalam sudut. Kami menambah ini kepada sudut sebelumnya untuk mendapatkan sudut semasa.

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

MPU6050 mpu;

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

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

kekosongan gelung () {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); }

Kedudukan MPU6050 apabila program mula berjalan adalah titik kecenderungan sifar. Sudut kecenderungan akan diukur sehubungan dengan perkara ini.

Pastikan robot tetap mantap pada sudut yang tetap dan anda akan melihat bahawa sudut akan secara beransur-ansur meningkat atau berkurang. Ia tidak akan tetap mantap. Ini disebabkan oleh hanyutan yang wujud pada giroskop.

Dalam kod yang diberikan di atas, masa gelung dikira dengan menggunakan fungsi millis () yang dibina ke dalam Arduino IDE. Dalam langkah yang akan datang, kami akan menggunakan interupsi pemasa untuk membuat selang persampelan tepat. Tempoh pensampelan ini juga akan digunakan dalam menjana output menggunakan pengawal PID.

Langkah 5: Menggabungkan Keputusan Dengan Penapis Komplementari

Google mendefinisikan pelengkap sebagai " menggabungkan sedemikian rupa untuk meningkatkan atau menekankan mutu antara satu sama lain atau anothe ".

Kami mempunyai dua ukuran sudut dari dua sumber yang berlainan. Pengukuran dari pecutan dijangkiti oleh pergerakan melintang mendadak dan pengukuran dari giroskop secara beransur-ansur melayang jauh dari nilai sebenar. Dengan kata lain, bacaan pecutan akan terjejas oleh isyarat jangka pendek dan bacaan gyroscope dengan isyarat jangka panjang. Pembacaan ini, dengan cara, saling melengkapi antara satu sama lain. Campurkan kedua-duanya menggunakan Penapis Komplementari dan kami mendapat pengukuran yang stabil dan tepat pada sudut. Penapis komplementer pada dasarnya adalah penapis pas tinggi yang bertindak pada giroskop dan penapis pas rendah yang bertindak pada pecutan untuk menyaring drift dan bunyi dari pengukuran.

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

0.9934 dan 0.0066 adalah pekali penapis untuk penstabil masa penapis 0.75s. Penapis pas rendah membolehkan sebarang isyarat lebih lama daripada tempoh ini untuk melaluinya dan penapis pas tinggi membolehkan sebarang isyarat yang lebih pendek daripada tempoh ini untuk dilalui. Tanggapan penapis boleh ditapis dengan memilih pemalar masa yang betul. Mengurangkan pemalar masa akan membolehkan lebih banyak pecutan melintang untuk dilalui.

Menghapuskan pecutan dan giroskop kesilapan mengimbangi
Muat turun dan jalankan kod yang diberikan di halaman ini untuk menentukurkan offset MPU6050 itu. Sebarang kesilapan yang disebabkan oleh offset boleh dihapuskan dengan mendefinisikan nilai-nilai offset dalam rutin persediaan () seperti ditunjukkan di bawah.

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

Langkah 6: Kawalan PID untuk Menjana Keluaran

PID bermaksud Proportional, Integral, dan Derivatif. Setiap istilah ini memberi respons yang unik kepada robot pengimbang diri kita.

Istilah berkadar, seperti namanya, menghasilkan respons yang berkadar dengan ralat. Untuk sistem kami, ralat adalah sudut kecenderungan robot.

Istilah integral menjana respons berdasarkan kesilapan terkumpul. Ini pada asasnya jumlah kesilapan yang didarabkan oleh tempoh pensampelan. Ini adalah tindak balas berdasarkan tingkah laku sistem pada masa lalu.

Istilah derivatif adalah berkadar dengan derivatif kesilapan. Ini adalah perbezaan antara ralat semasa dan ralat sebelumnya yang dibahagikan dengan tempoh pensampelan. Ini bertindak sebagai istilah ramalan yang merespon bagaimana robot boleh bertindak dalam gelung sampling seterusnya.

Mengalikan setiap istilah ini dengan pemalar yang sepadan (iaitu, Kp, Ki dan Kd) dan hasilnya dijumlahkan, kita menghasilkan output yang kemudiannya dihantar sebagai arahan untuk memacu motor.

Langkah 7: Menala Peralihan PID

1. Tetapkan Ki dan Kd ke sifar dan secara beransur-ansur meningkatkan Kp supaya robot mula berayun tentang kedudukan sifar.

2. Meningkatkan Ki supaya respons robot lebih cepat apabila ia tidak seimbang. Ki harus cukup besar supaya sudut kecenderungan tidak meningkat. Robot harus kembali ke kedudukan sifar jika ia cenderung.

3. Meningkatkan Kd untuk mengurangkan ayunan. Penyebaran juga harus dikurangkan sekarang.

4. Ulangi langkah-langkah di atas dengan baik menala setiap parameter untuk mencapai hasil yang terbaik.

Langkah 8: Menambah Sensor Jarak

Sensor jarak ultrasonik yang saya gunakan adalah US-020. Ia mempunyai empat pin iaitu Vcc, Trig, Echo, dan Gnd. Ia dikuasakan oleh sumber 5V. Pemicu dan gema pin masing-masing disambungkan kepada pin digital 9 dan 8 Arduino. Kami akan menggunakan perpustakaan NewPing untuk mendapatkan nilai jarak dari sensor. Kami akan membaca jarak sekali setiap 100 milisaat dan jika nilai adalah antara 0 dan 20cm, kami akan memerintahkan robot untuk melakukan putaran. Ini sepatutnya mencukupi untuk mengarahkan robot dari halangan.

Langkah 9: Kod Lengkap

 #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 sonar (TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);

int16_t accY, accZ, gyroX; volatile int motorPower, gyroRate; float float accAngle, gyroAngle, currentAngle, prevAngle = 0, error, prevError = 0, errorSum = 0; kiraan byte yang tidak menentu = 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); } jika (rightMotorSpeed> = 0) {analogWrite (rightMotorPWMPin, rightMotorSpeed); digitalWrite (rightMotorDirPin, LOW); } else {analogWrite (rightMotorPWMPin, 255 + rightMotorSpeed); digitalWrite (rightMotorDirPin, HIGH); }}

void init_PID () // permulaan Timer1 cli (); / / melumpuhkan gangguan global TCCR1A = 0; // set keseluruhan TCCR1A mendaftar ke 0 TCCR1B = 0; // sama untuk TCCR1B / set membandingkan perbandingan padanan untuk menetapkan masa sampel 5ms OCR1A = 9999; // hidupkan mod CTC TCCR1B

void setup () {// set control motor and PWM pins to pinMode mode output (leftMotorPWMPin, OUTPUT); pinMode (leftMotorDirPin, OUTPUT); pinMode (rightMotorPWMPin, OUTPUT); pinMode (rightMotorDirPin, OUTPUT); // set status LED ke mod output pinMode (13, OUTPUT); // memulakan MPU6050 dan atur nilai offset mpu.initialize (); mpu.setYAccelOffset (1593); mpu.setZAccelOffset (963); mpu.setXGyroOffset (40); // Inisikan gelung persampelan PID init_PID (); }

void loop () {// read acceleration and gyroscope values ​​accY = mpu.getAccelerationY (); accZ = mpu.getAccelerationZ (); gyroX = mpu.getRotationX (); / // set motor kuasa selepas menghalangnya motorPower = menghalang (motorpower, -255, 255); setMotors (motor kuasa, motor kuasa); // Jarak jarak setiap 100 milisaat jika ((mengira% 20) == 0) {distanceCm = sonar.ping_cm (); } jika ((distanceCm <20) && (distanceCm! = 0)) {setMotors (-motorPower, motor power); }} // ISR akan dipanggil setiap 5 milisekon ISR (TIMER1_COMPA_vect) {// hitung angle inclination 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); ralat = currentAngle - targetAngle; errorSum = errorSum + error; errorSum = constrain (errorSum, -300, 300); / / menghitung output dari nilai P, I dan D motorPower = Kp * (ralat) + Ki * (errorSum) * sampleTime - Kd * (currentAngle-prevAngle) / sampleTime; prevAngle = currentAngle; // toggle yang diterajui pada pin13 setiap kiraan kedua ++; jika (count == 200) {count = 0; digitalWrite (13, ! digitalRead (13)); }}

Langkah 10: Pikiran Akhir

Membelanjakan sedikit masa untuk tweaking pemalar PID akan memberi kita hasil yang lebih baik. Saiz robot kami juga menghadkan tahap kestabilan yang dapat kami capai. Ia lebih mudah untuk membina robot pengimbang bersaiz penuh daripada membina kecil seperti kita. Namun, saya rasa, robot kami melakukan kerja yang cukup baik dalam mengimbangi pelbagai permukaan seperti yang ditunjukkan dalam video.

Itu untuk masa sekarang.

Terima kasih kerana meluangkan masa anda. Jangan lupa untuk meninggalkan pemikiran anda di bahagian komen.

Artikel Berkaitan