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), giải thuật điều khiển trượt (sliding mode control), LQR, phương pháp điều khiển thông minh fuzzy, noron [8] 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.
Để điều khiển robot 2 bánh tự cân bằng theo các bước sau:
1/ Mô hình hóa robot 2 bánh
2/ Tìm phương trình mô tả trạng thái robot
3/ Mô phỏng matlab theo luật điều khiển muốn điều khiển - tìm Udk
K_u = 1.7946 -0.0030 1.8764 0.2079
4/ Thiết kế phần cứng
Board điều khiển trung tâm: Arduino Mega2560
Board điều khiển motor: L298N
Cảm biến góc nghiêng
Khung robot
Kết nối phần cứng
Board Arduino
Mega 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 motor
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
Chân 21 Input Chân SCL cảm biến Gyro
5/ Viết code arduino theo luật điều khiển
B1. Code điều khiển xe 2 bánh tự cân bằng dùng phương pháp PID
#include <Kalman.h>
#include<Servo.h>
#include<Wire.h>
#include<I2Cdev.h>
#include<MPU6050.h>
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();
}
Nguồn sưu tầm
Điện Tử eChipKool.Com - Chia sẻ kiến thức - Kết nối đam mê điên tử
