HƯỚNG DẪN LÀM ROBOT TỰ CÂN BẰNG

 

Danh sách các chi tiết cần thiết

1. Arduino Pro Mini
2. Mô-đun GY-521 với MPU-6050
3. Trình điều khiển động cơ Pololu DRV8833
4. Bộ chuyển đổi 5V
5. Cảm biến khoảng cách siêu âm US-020
6. pin và nguồn NCR18650
7. Cặp động cơ bánh răng kim loại (N20, 6V, 200 vòng / phút)và các giá đỡ
8. Cặp bánh xe 42x19mm
9. 3 board PCB (4cm x 6cm)
10. 8 trụ đệm Nylon 25cm và 4, đai ốc nhựa

Ngoài ra, bạn cần một số dây nối và một nút công tắc.

Bước 1: Một chút lý thuyết

Robot tự cân bằng tương tự như một con lắc ngược. Không giống như một con lắc bình thường tiếp tục đong đưa khi được tác dụng lực, con lắc này không thể tự cân bằng được, nó sẽ bị ngã. Vậy làm thế nào để chúng ta cân bằng nó? Nó gần giống khi chúng ta cân bằng một cây viết trên ngón tay trỏ của chúng mình, đó là một ví dụ điển hình về việc cân bằng một con lắc ngược. Chúng ta di chuyển ngón tay theo hướng mà thanh rơi xuống. Tương tự như trường hợp với rô-bốt tự cân bằng, rô-bốt sẽ ngã về phía trước hoặc phía sau. Chúng ta cân bằng robot bằng cách điều khiển bánh xe theo hướng nó rơi xuống. Những gì chúng ta đang cố gắng làm ở đây là giữ cho trọng tâm của robot vuông góc với mặt đất.

Để điều khiển động cơ, chúng ta cần một số thông tin về trạng thái của robot. Chúng ta cần phải biết hướng mà robot đang ngã xuống, robot đã nghiêng bao nhiêu và tốc độ ngã xuống. Tất cả các thông tin này có thể được suy ra từ các số liệu thu được từ MPU6050. Chúng ta kết hợp tất cả các đầu vào này và tạo ra một tín hiệu điều khiển động cơ và giữ cho robot cân bằng.

Bước 2: Lắp ráp

 

 

 

 

 

Đầu tiên chúng ta sẽ hoàn thành mạch và cấu trúc của robot. Robot được lắp ráp trên ba khung được đặt cách nhau 25mm bằng cách sử dụng miếng đệm bằng nylon. Lớp dưới cùng chứa hai động cơ. Lớp giữa có bộ điều khiển, IMU và mô đun điều chỉnh tăng 5V. Lớp trên cùng chứa pin, một công tắc và cảm biến khoảng cách siêu âm.

Trước khi chúng ta bắt đầu thử nghiệm, chúng ta cần có một sơ đồ rõ ràng về vị trí đặt từng phần. Dựa vào sơ đồ lắp ráp các chi tiết và hàn, nối ba khung với nhau bằng cách sử dụng trụ đệm nylon.

Bạn có thể nhận thấy rằng tôi đã sử dụng hai mô-đun điều chỉnh điện áp riêng biệt để điều khiển động cơ và bộ điều khiển ngay cả khi cả hai đều yêu cầu nguồn 5V. Vì khi sử dụng một bộ điều chỉnh tăng áp 5V duy nhất để cấp nguồn cho bộ điều khiển cũng như các động cơ, chương trình sẽ gặp lỗi. Điều này là do tiếng ồn phát ra từ mạch động cơ tác động lên bộ điều khiển và IMU. Điều này đã được loại bỏ một cách hiệu quả bằng cách tách bộ điều chỉnh điện áp cho bộ điều khiển và động cơ và thêm một tụ 10uF tại các đầu nối nguồn với động cơ.

Bước 3: Đo góc nghiêng bằng Accelerometer

MPU6050 có gia tốc kế 3 trục và con quay hồi chuyển 3 trục. Gia tốc kế đo gia tốc dọc theo ba trục và con quay hồi chuyển đo tốc độ góc ba trục. Để đo góc nghiêng của robot, chúng ta cần các giá trị gia tốc dọc theo trục y và z Hàm atan2 (y, z) cho biết góc có đơn vị là radian giữa trục z dương của mặt phẳng và điểm được cho bởi tọa độ (z, y) trên mặt phẳng đó, với dấu dương cho góc ngược chiều kim đồng hồ (nửa mặt phẳng phải), y> 0) và dấu âm cho các góc theo chiều kim đồng hồ (nửa mặt phẳng trái, y <0). Chúng ta sử dụng thư viện này được viết bởi Jeff Rowberg để đọc dữ liệu từ MPU6050. Tải lên code được đưa ra dưới đây và xem cách góc nghiêng khác nhau.

#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);

}

Hãy thử di chuyển robot về phía trước và phía sau trong khi vẫn giữ nó nghiêng ở một góc cố định nào đó. Bạn sẽ thấy rằng góc hiển thị trong màn hình nối tiếp đột nhiên thay đổi. Điều này là do thành phần gia tốc nằm ngang gây nhiễu với các giá trị tăng tốc của trục y và z.

Bước 4: Đo góc nghiêng bằng Gyroscope

Con quay hồi chuyển 3 trục của MPU6050 đo tốc độ góc (vận tốc quay) dọc theo ba trục. Đối với robot tự cân bằng, vận tốc góc dọc theo trục x là đủ để đo tốc độ ngã của robot.

Trong code được đưa ra dưới đây, chúng ta đọc giá trị con quay hồi chuyển về trục x, chuyển đổi nó thành độ/ giây và sau đó nhân nó với thời gian vòng lặp để có được sự thay đổi về góc. Chúng ta cộng nó vào góc trước để có được góc hiện tại.

#include “Wire.h”

#include “I2Cdev.h”

#include “MPU6050.h”

 

MPU6050 mpu;

 

int16_t gyroX, gyroRate;

float gyroAngle=0;

unsigned 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);

}

Vị trí của MPU6050 khi chương trình bắt đầu chạy là điểm nghiêng bằng không. Góc nghiêng sẽ được đo đối với điểm này.

Giữ cho robot ổn định ở một góc cố định và bạn sẽ thấy rằng góc sẽ tăng hoặc giảm dần. Nó sẽ không ổn định. Điều này là do sự di chuyển vốn có của con quay hồi chuyển.

Trong đoạn code trên, thời gian vòng lặp được tính bằng hàm millis () được tích hợp vào IDE Arduino. Trong các bước sau, chúng ta sẽ sử dụng ngắt bộ hẹn giờ để tạo khoảng thời gian lấy mẫu chính xác. Khoảng thời gian lấy mẫu này cũng sẽ được sử dụng để tạo ra đầu ra bằng bộ điều khiển PID.

Bước 5: Kết hợp các kết quả bằng bộ lọc bổ sung

Chúng ta có hai phép đo góc từ hai nguồn khác nhau. Phép đo từ gia tốc bị ảnh hưởng bởi chuyển động ngang đột ngột và phép đo từ con quay hồi chuyển dần dần sai nhiều so với giá trị thực. Nói cách khác, việc đọc gia tốc bị ảnh hưởng bởi tín hiệu thời gian ngắn và con quay hồi chuyển đọc bằng tín hiệu thời gian dài. Những số liệu này, theo cách nào đó, bổ sung cho nhau. Kết hợp cả hai bằng cách sử dụng bộ lọc bổ sung và chúng ta nhận được phép đo góc ổn định, chính xác. Bộ lọc bổ sung về cơ bản là một bộ lọc thông cao tác động lên con quay hồi chuyển và bộ lọc thông thấp tác động lên gia tốc kế để lọc giá trị và nhiễu từ phép đo.

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

0.9934 và 0.0066 là hệ số lọc cho hằng số thời gian lọc là 0,75 giây. Bộ lọc thông thấp cho phép bất kỳ tín hiệu nào dài hơn khoảng thời gian này có thể đi qua nó và bộ lọc thông cao cho phép bất kỳ tín hiệu nào ngắn hơn khoảng thời gian này đi qua. Phản ứng của bộ lọc có thể được tinh chỉnh bằng cách chọn hằng số thời gian chính xác. Giảm hằng số thời gian sẽ cho phép tăng gần với giá trị chính xác.

Loại bỏ lỗi bù tốc độ và con quay hồi chuyển
Tải xuống và chạy code được đưa ra trong trang này để hiệu chỉnh bù trừ của MPU6050. Bất kỳ lỗi nào do offset có thể được loại bỏ bằng cách xác định các giá trị offset trong quy trình thiết lập () như hình dưới đây.

mpu.setYAccelOffset(1593);

mpu.setZAccelOffset(963);

mpu.setXGyroOffset(40);

Bước 6: Điều khiển PID để tạo đầu ra

 

PID là viết tắt của Proportional, Integral và Derivative. Mỗi thuật ngữ này cung cấp một hành động cho robot tự cân bằng.

Proportional tạo ra một phản ứng là tỷ lệ thuận với lỗi. Đối với hệ thống của chúng ta, lỗi là góc nghiêng của robot.

Integral tạo ra một phản ứng dựa trên các lỗi tích lũy. Về cơ bản, đây là tổng của tất cả các lỗi nhân với khoảng thời gian lấy mẫu. Đây là một phản ứng dựa trên hành vi của hệ thống trong quá khứ.

Derivative tỷ lệ với đạo hàm của lỗi. Đây là sự khác biệt giữa lỗi hiện tại và lỗi trước đó chia cho khoảng thời gian lấy mẫu. Điều này đóng vai trò như một thuật ngữ dự đoán giúp cách robot có thể hoạt động trong vòng lặp lấy mẫu tiếp theo.

Nhân mỗi thuật ngữ với các hằng số tương ứng của chúng (ví dụ, Kp, Ki và Kd) và lấy tổng kết quả, chúng ta tạo ra đầu ra được gửi như lệnh để điều khiển động cơ.

Bước 7: Điều chỉnh hằng số PID

1. Đặt Ki và Kd về 0 và tăng dần Kp sao cho robot bắt đầu dao động về vị trí 0.
2. Tăng Ki để phản xạ của robot nhanh hơn khi nó mất cân bằng. Ki phải đủ lớn sao cho góc nghiêng không tăng. Robot sẽ quay trở lại vị trí 0 nếu nó nghiêng.
3. Tăng Kd để giảm dao động. Các overshoots cũng nên được giảm.
4. Lặp lại các bước trên bằng cách tinh chỉnh từng thông số để đạt được kết quả tốt nhất.

Bước 8: Thêm cảm biến khoảng cách

Cảm biến khoảng cách siêu âm sử dụng trong dự án này là US-020. Nó có bốn chân là Vcc, Trig, Echo và Gnd. Nó được cấp nguồn 5V. Các trigger và các chân echo tương ứng với các chân số 9 và 8 của Arduino. Chúng ta sẽ sử dụng thư viện NewPing để lấy giá trị khoảng cách từ cảm biến. Chúng ta sẽ đọc khoảng cách sau mỗi 100 mili giây và nếu giá trị nằm trong khoảng từ 0 đến 20cm, chúng ta sẽ điều khiển rô bốt để thực hiện xoay vòng. Điều này là để điều khiển robot tránh xa chướng ngại vật.

Bước 9: Toàn bộ code

#include “Wire.h”

#include “I2Cdev.h”

#include “MPU6050.h”

#include “math.h”

#include <NewPing.h>

#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;

volatile float accAngle, gyroAngle, currentAngle, prevAngle=0, error, prevError=0, errorSum=0;

volatile byte count=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() {

// initialize Timer1

cli();          // disable global interrupts

TCCR1A = 0;     // set entire TCCR1A register to 0

TCCR1B = 0;     // same for TCCR1B

// set compare match register to set sample time 5ms

OCR1A = 9999;

// turn on CTC mode

TCCR1B |= (1 << WGM12);

// Set CS11 bit for prescaling by 8

TCCR1B |= (1 << CS11);

// enable timer compare interrupt

TIMSK1 |= (1 << OCIE1A);

sei();          // enable global interrupts

}

void setup() {

// set the motor control and PWM pins to output mode

pinMode(leftMotorPWMPin, OUTPUT);

pinMode(leftMotorDirPin, OUTPUT);

pinMode(rightMotorPWMPin, OUTPUT);

pinMode(rightMotorDirPin, OUTPUT);

// set the status LED to output mode

pinMode(13, OUTPUT);

// initialize the MPU6050 and set offset values

mpu.initialize();

mpu.setYAccelOffset(1593);

mpu.setZAccelOffset(963);

mpu.setXGyroOffset(40);

// initialize PID sampling loop

init_PID();

}

void loop() {

// read acceleration and gyroscope values

accY = mpu.getAccelerationY();

accZ = mpu.getAccelerationZ();

gyroX = mpu.getRotationX();

// set motor power after constraining it

motorPower = constrain(motorPower, -255, 255);

setMotors(motorPower, motorPower);

// measure distance every 100 milliseconds

if((count%20) == 0){

distanceCm = sonar.ping_cm();

}

if((distanceCm < 20) && (distanceCm != 0)) {

setMotors(-motorPower, motorPower);

}

}

// The ISR will be called every 5 milliseconds

ISR(TIMER1_COMPA_vect)

{

// calculate the angle of inclination

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

 

error = currentAngle – targetAngle;

errorSum = errorSum + error;

errorSum = constrain(errorSum, -300, 300);

//calculate output from P, I and D values

motorPower = Kp*(error) + Ki*(errorSum)*sampleTime – Kd*(currentAngle-prevAngle)/sampleTime;

prevAngle = currentAngle;

// toggle the led on pin13 every second

count++;

if(count == 200)  {

count = 0;

digitalWrite(13, !digitalRead(13));

}

}

Bước 10: Kết quả