Sưu tầm hướng dẫn làm robot 2 bánh tự cân bằng với PID – Điện Tử Hello
Mục Lục
Đôi nét về Robot 2 bánh tự cân bằng
Robot 2 bánh tự cân bằng dựa trên mô hình con lắc ngược là một đối tượng phi tuyến với các tham số bất định khó điều khiển với 6 biến trạng thái. Đặc điểm nổi bật của Robot 2 bánh tự cân bằng là cơ chế tự cân bằng, giúp cho xe dù chỉ có một trục chuyển động với hai bánh nhưng luôn ở trạng thái cân bằng. Có rất nhiều công trình nghiên cứu về xe hai bánh tự cân bằng, nghiên cứu điều khiển xe 2 bánh tự cân bằng dùng giải thuật cuốn chiếu (backstepping control), H vô cùng, LQR, phương pháp điều khiển bền vững, cho thấy khả năng thích nghi và hiệu quả của những giải pháp điều khiển.
Linh kiện trong bài viết
1. Arduino Pro Mini
2. Module GY-521 với MPU-6050
3. Module DRV8833
4. Module 5V boost converter ( Nguồn boost lên 5V, 2A )
5. Cảm biến siêu âm US-020 ( Bạn cũng hoàn toàn có thể dùng module HC-04, HC-05 )
6. Pin NCR18650 hoặc những loại pin 18650
7. Động cơ giảm tốc N20, 6V, 200 r
8. Các phụ kiện gá lắp khác
Lắp đặt thiết bị
Nguyên tắc con lắc ngược (inverted pendulum)
Nó giống như việc giữ thăng bằng một cây gậy trên ngón tay. Để giữ thăng bằng, chúng ta phải di chuyển ngón tay của mình theo hướng nghiêng và tốc độ nghiêng của cây gậy. Để điều khiển động cơ bánh xe cho robot tự cân bằng qua mạch cầu L298N, chúng ta cần một số thông tin về trạng thái của robot như: điểm thăng bằng cần cài đặt cho robot, hướng mà robot đang nghiêng, góc nghiêng và tốc độ nghiêng. Tất cả các thông tin này được thu thập từ MPU6050 và đưa vào bộ điều khiển PID để tạo ra một tín hiệu điều khiển động cơ, giữ cho robot ở điểm thăng bằ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.
Đo góc nghiêng bằng Accelerometer của MPU6050
MPU6050 có gia tốc kế 3 trục và con quay hồi chuyển 3 trục. Gia tốc kế đo tần suất dọc theo ba trục và con quay hồi chuyển đo vận tốc góc ba trục. Để đo góc nghiêng của robot, tất cả chúng ta cần những giá trị tần suất dọc theo trục y và z Hàm atan2 ( y, z ) cho biết góc có đơn vị chức năng 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ồ đeo tay ( nửa mặt phẳng phải ), y > 0 ) và dấu âm cho những góc theo chiều kim đồng hồ đeo tay ( nửa mặt phẳng trái, y < 0 ). Chúng ta sử dụng thư viện được viết bởi Jeff Rowberg để đọc tài liệu từ MPU6050 .
Ví dụ chương trình đọc góc nghiêng :
#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);
}Đo góc nghiêng bằng Gyroscope của MPU6050
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.
Ví dụ:
#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 khởi đầu chạy là điểm nghiêng bằng không. Góc nghiêng sẽ được đo so với điểm này. Giữ cho robot không thay đổi ở một góc cố định và thắt chặt và bạn sẽ thấy rằng góc sẽ tăng hoặc giảm dần. Nó sẽ không không thay đổi. Điều này là do sự vận động và di chuyển vốn có của con quay hồi chuyển. Trong đoạn code trên, thời hạn vòng lặp được tính bằng hàm millis ( ) được tích hợp vào IDE Arduino. Trong những bước sau, tất cả chúng ta sẽ sử dụng ngắt bộ hẹn giờ để tạo khoảng chừng thời hạn lấy mẫu đúng chuẩn. Khoảng thời hạn 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 và tinh chỉnh PID .
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ừ tần suất bị ảnh hưởng tác động bởi hoạt động ngang bất ngờ đột ngột và phép đo từ con quay hồi chuyển từ từ sai nhiều so với giá trị thực. Nói cách khác, việc đọc tần suất bị tác động ảnh hưởng bởi tín hiệu thời hạn ngắn và con quay hồi chuyển đọc bằng tín hiệu thời hạn dài. Những số liệu này, theo cách nào đó, bổ trợ cho nhau. Kết hợp cả hai bằng cách sử dụng bộ lọc bổ trợ và tất cả chúng ta nhận được phép đo góc không thay đổi, đúng chuẩn. Bộ lọc bổ trợ về cơ bản là một bộ lọc thông cao tác động ảnh hưởng lên con quay hồi chuyển và bộ lọc thông thấp tác động ảnh hưở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)
Xem thêm: Phụ kiện máy đo huyết áp
0.9934 và 0.0066 là thông số lọc cho hằng số thời hạn lọc là 0,75 giây. Bộ lọc thông thấp được cho phép bất kể tín hiệu nào dài hơn khoảng chừng thời hạn này hoàn toàn có thể đi qua nó và bộ lọc thông cao được cho phép bất kể tín hiệu nào ngắn hơn khoảng chừng thời hạn này đi qua. Phản ứng của bộ lọc hoàn toàn có thể được điều khiển và tinh chỉnh bằng cách chọn hằng số thời hạn đúng mực. Giảm hằng số thời hạn sẽ được cho phép tăng gần với giá trị đúng chuẩn .
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 hàm setup() như hình dưới đây.
mpu.setYAccelOffset(1593);
mpu.setZAccelOffset(963);
mpu.setXGyroOffset(40);
Đ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 ứng một hành vi cho robot tự cân đối .
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 những hằng số tương ứng của chúng ( ví dụ, Kp, Ki và Kd ) và lấy tổng kết quả, tất cả chúng ta tạo ra đầu ra được gửi như lệnh để điều khiển và tinh chỉnh động cơ .
Điều chỉnh hằng số PID
- Đặt Ki và Kd về 0 và tăng dần Kp sao cho robot bắt đầu dao động về vị trí 0.
- 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.
- Tăng Kd để giảm dao động. Các overshoots cũng nên được giảm.
- 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.
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 Bất Động Sả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à những chân echo tương ứng với những 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 chừng từ 0 đến 20 cm, tất cả chúng ta sẽ tinh chỉnh và điều khiển rô bốt để thực thi xoay vòng. Điều này là để tinh chỉnh và điều khiển robot tránh xa chướng ngại vật .
Code chương trình
#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;
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));
}
} 
Các dự án Robot 2 bánh cân bằng khác
Electric DIY Lab
https://github.com/mahowik/BalancingWii
https://electricdiylab.com/diy-self-balancing-robot/
Mô hình sản phẩm của ATD Tech sử dụng PID
http://www.atdtech.com/index.php/vi/cong-nghe/98-robot-2-banh-t-can-b-ng
Kết nối phần cứng
Xem thêm: Phụ kiện máy đo huyết áp
| Board ArduinoMega 2560 | Chức năng | Kết nối | Ghi chú |
| Chân 2 | Input | Encode motor | |
| Chân 3 | Input | Encode motor | |
| Chân 4 | Output | Chân input L298N – IN1 | Output L298N nối động cơ |
| Chân 5 | Output | Chân input L298N – IN2 | |
| Chân 6 | Output | Chân input L298N – IN3 | |
| Chân 7 | Output | Chân input L298N – IN4 | |
| Chân 9 | Output | Chân EA – L298N | |
| Chân 10 | Output | Chân EB – L298N | |
| Chân 20 | Input | Chân SDA cảm biến Gyro | Giao tiếp chuẩn I2C, MPU6050 |
| Chân 21 | Input | Chân SCL cảm biến Gyro |
Code chương trình :
#include
#include
#include
#include
#include
MPU6050 CBgoc;
Kalman kalmanX;
//IMU 6050====================================================
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;
float accXangle;
float gyroXangel;
float kalAngelX;
unsigned long timer;
uint8_t i2cData[14];
float CurrentAngle;
// MOTOR====================================================
int AIN1 = 4;
int AIN2 = 5;
int BIN1 = 6;
int BIN2 = 7;
int CIN1 = 9;
int CIN2 = 10;
int speed;
// PID====================================================
const float Kp = 30;
const float Ki = 1;
const float Kd = 8;
float pTerm, iTerm, dTerm, integrated_error, last_error, error;
const float K = 1.9*1.12;
#define GUARD_GAIN 10.0
#define runEvery(t) for (static typeof(t) _lasttime;(typeof(t))((typeof(t))millis() - _lasttime) > (t);_lasttime += (t))
void setup()
{
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
Serial.begin(9600);
Wire.begin();
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00;
i2cData[3] = 0x00;
while(i2cWrite(0x19,i2cData,4,false));
while(i2cWrite(0x6B,0x01,true));
while(i2cRead(0x75,i2cData,1));
if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
Serial.print(F("Error reading sensor"));
while(1);
}
delay(100);
//Kalman====================================================
while(i2cRead(0x3B,i2cData,6));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
kalmanX.setAngle(accXangle);
gyroXangel = accXangle;
timer = micros();
}
void loop()
{
Serial.println(accX);
// Serial.println(accY);
// Serial.println(accZ);
Serial.println(accXangle);
Serial.println(CurrentAngle);
runEvery(25)
{
dof();
if(CurrentAngle <=179 && CurrentAngle >=178.5)
{
stop();
}
else
{
if(CurrentAngle < 230 && CurrentAngle >130)
{
Pid();
Motors();
}
else
{
stop();
}
}
}
}
void Motors()
{
if(speed > 0)
{
analogWrite(CIN1, speed);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
analogWrite(CIN2, speed);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
}
else
{
speed = map(speed,0,-255,0,255);
analogWrite(CIN1, speed);
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
analogWrite(CIN2, speed);
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
}
}
void stop()
{
speed = map(speed,0,-150,0,150);
analogWrite(CIN1, speed);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
analogWrite(CIN2, speed);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
}
void Pid()
{
error = 180 - CurrentAngle; // 180 = level
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki*constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd*(error - last_error);
last_error = error;
speed = constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}
void dof()
{
while(i2cRead(0x3B,i2cData,14));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
tempRaw = ((i2cData[6] << 8) | i2cData[7]);
gyroX = ((i2cData[8] << 8) | i2cData[9]);
gyroY = ((i2cData[10] << 8) | i2cData[11]);
gyroZ = ((i2cData[12] << 8) | i2cData[13]);
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
double gyroXrate = (double)gyroX/131.0;
CurrentAngle = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000);
timer = micros();
} Mô hình robot 2 bánh tự cân bằng của TDHshop
https://tdhshop.com.vn/xe-robot-2-banh-tu-can-bang-su-dung-board-arduino-mega-2560
Source: https://dvn.com.vn
Category: Phụ Kiện
