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

Đôi nét về Robot 2 bánh tự cân bằng

Robot 2 bánh tự cân đối dựa trên quy mô con lắc ngược là một đối tượng người dùng phi tuyến với các tham số bất định khó điều khiển và tinh chỉnh với 6 biến trạng thái. Đặc điểm điển hình nổi bật của Robot 2 bánh tự cân đối là chính sách tự cân đối, giúp cho xe dù chỉ có một trục hoạt động với hai bánh nhưng luôn ở trạng thái cân đối. Có rất nhiều khu công trình điều tra và nghiên cứu về xe hai bánh tự cân đối, điều tra và nghiên cứu tinh chỉnh và điều khiển xe 2 bánh tự cân đối dùng giải thuật cuốn chiếu ( backstepping control ), H vô cùng, LQR, giải pháp điều khiển và tinh chỉnh vững chắc, cho thấy năng lực thích nghi và hiệu suất cao của những giải pháp điều khiển và tinh chỉnh.

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 các 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ữ cân đối, tất cả chúng ta phải chuyển dời ngón tay của mình theo hướng nghiêng và vận tốc nghiêng của cây gậy. Để điều khiển và tinh chỉnh động cơ bánh xe cho robot tự cân đối qua mạch cầu L298N, tất cả chúng ta cần 1 số ít thông tin về trạng thái của robot như : điểm cân đối cần thiết lập cho robot, hướng mà robot đang nghiêng, góc nghiêng và vận tốc nghiêng. Tất cả các thông tin này được tích lũy từ MPU6050 và đưa vào bộ tinh chỉnh và điều khiển PID để tạo ra một tín hiệu điều khiển và tinh chỉnh động cơ, giữ cho robot ở điểm cân đối. Những gì tất cả chúng ta đang nỗ lực 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 các giá trị tần suất dọc theo trục y và zHàmatan2 ( 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 các góc theo chiều kim đồng hồ đeo tay ( nửa mặt phẳng trái, y

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 vận tốc góc ( tốc độ quay ) dọc theo ba trục. Đối với robot tự cân đối, tốc độ góc dọc theo trục x là đủ để đo vận tốc ngã của robot. Trong code được đưa ra dưới đây, tất cả chúng ta đọc giá trị con quay hồi chuyển về trục x, quy đổi nó thành độ / giây và sau đó nhân nó với thời hạn vòng lặp để có được sự đổi khác 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 các 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 mực. 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ộ tinh chỉnh và điều khiển 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ị tác động ảnh hưởng bởi hoạt động ngang bất thần 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ụngbộ lọc bổ sungvà tất cả chúng ta nhận được phép đo góc không thay đổi, đúng mực. Bộ lọc bổ trợ về cơ bản là một bộ lọc thông cao ảnh hưởng tác độ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 )

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 tinh chỉnh và điều khiển bằng cách chọn hằng số thời hạn đúng chuẩn. Giảm hằng số thời hạn sẽ được cho phép tăng gần với giá trị đúng mực.

Loại bỏ lỗi bù vận 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 hoàn toàn có thể được vô hiệu bằng cách xác lập 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 phân phối một hành vi cho robot tự cân đối.

Proportional tạo ra một phản ứng là tỷ suất thuận với lỗi. Đối với mạng lưới hệ thống của tất cả 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 góp. Về cơ bản, đây là tổng của toàn bộ các lỗi nhân với khoảng chừng thời hạn lấy mẫu. Đây là một phản ứng dựa trên hành vi của mạng lưới hệ thống trong quá khứ. Derivative tỷ suất với đạo hàm của lỗi. Đây là sự độc lạ giữa lỗi hiện tại và lỗi trước đó chia cho khoảng chừng thời hạn lấy mẫu. Điều này đóng vai trò như một thuật ngữ Dự kiến giúp cách robot hoàn toàn có thể hoạt động giải trí 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ả, tất cả chúng ta tạo ra đầu ra được gửi như lệnh để tinh chỉnh và điều khiển động cơ.

Điều chỉnh hằng số PID

1. Đặt Ki và Kd về 0 và tăng dần Kp sao cho robot khởi đầu xê dịch về vị trí 0.
2. Tăng Ki để phản xạ của robot nhanh hơn khi nó mất cân đối. 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 giao độ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 và điều khiển từng thông số kỹ thuật để đạt được tác dụng 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à 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ụngthư viện NewPing để lấy giá trị khoảng cách từ cảm ứng. 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

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

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] =178.5)
{
stop();
}
else
{
if(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]

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