Robot tự cân bằng hai bánh với Arduino và bộ điều khiển PID
Robot tự cân bằng (self-balancing robot) là bài toán “con lắc ngược” kinh điển trong điều khiển học, nhưng bạn hoàn toàn có thể tự làm bằng Arduino với vài chục nghìn phép tính mỗi giây thay vì phương trình vi phân phức tạp. Đây cũng là dự án tốt nhất để học PID — bộ điều khiển xuất hiện trong gần như mọi hệ thống tự động, từ máy in 3D đến máy bay không người lái.
Nguyên lý hoạt động
Robot có hai bánh, trọng tâm nằm cao hơn trục bánh — về bản chất nó luôn muốn ngã. Nhiệm vụ của vi điều khiển là liên tục đọc góc nghiêng và điều chỉnh tốc độ động cơ để “đuổi theo” điểm cân bằng, giống hệt cách bạn giữ một cây gậy đứng thẳng trên lòng bàn tay.
- Cảm biến MPU6050 (gia tốc kế + con quay hồi chuyển) đo góc nghiêng thực tế của thân robot.
- Bộ điều khiển PID so sánh góc đo được với góc mục tiêu (thường là 0°, tức thẳng đứng) để tính ra sai số, rồi tính ra một giá trị điều khiển:
- P (Proportional) — phản ứng tỉ lệ thuận với sai số hiện tại: nghiêng càng nhiều, động cơ đẩy càng mạnh.
- I (Integral) — cộng dồn sai số theo thời gian để triệt tiêu độ lệch ổn định (steady-state error) — ví dụ khi trọng tâm robot không hoàn toàn cân, chỉ riêng P sẽ không bao giờ đưa robot về đúng 0°.
- D (Derivative) — phản ứng theo tốc độ thay đổi của sai số, đóng vai trò “phanh” để chặn dao động qua lại (overshoot) trước khi nó xảy ra.
- Giá trị PID tính ra được chuyển thành tốc độ và chiều quay của hai động cơ, đẩy robot về phía đang ngã để giữ thăng bằng — lặp lại toàn bộ quy trình hàng trăm lần mỗi giây.
Linh kiện
- Arduino Uno hoặc Nano (đủ nhanh cho vòng lặp PID ở tần số vài trăm Hz)
- Cảm biến MPU6050 (gia tốc kế + con quay 6 trục), giao tiếp I2C
- Driver động cơ 2 kênh (TB6612FNG hoặc L298N)
- 2 động cơ DC giảm tốc kèm bánh xe
- Khung robot hai bánh (chassis) tự cắt hoặc in 3D, đặt trọng tâm cao vừa phải
- Pin 7.4V-11V (2S hoặc 3S Li-ion/LiPo) cấp nguồn cho driver và động cơ
Code mẫu
#include <Wire.h>
#include <MPU6050_light.h>
MPU6050 mpu(Wire);
// Chan dieu khien driver dong co (vi du TB6612FNG)
const int AIN1 = 7, AIN2 = 8, PWMA = 5; // Dong co trai
const int BIN1 = 9, BIN2 = 10, PWMB = 6; // Dong co phai
const int STBY = 4;
// Hang so PID - can hieu chinh thuc te cho tung robot, day chi la diem xuat phat
double Kp = 25.0;
double Ki = 140.0;
double Kd = 0.8;
double setpoint = 0.0; // goc can bang ly tuong, tinh chinh sau khi lap rap xong
double integral = 0.0;
double lastError = 0.0;
unsigned long lastTime = 0;
void setup() {
Serial.begin(115200);
Wire.begin();
byte status = mpu.begin();
while (status != 0) { } // dung lai neu MPU6050 loi ket noi
Serial.println("Dang can chinh MPU6050, giu robot dung yen...");
delay(1000);
mpu.calcOffsets(); // tu dong bu offset gyro/accel
Serial.println("Xong!");
pinMode(AIN1, OUTPUT); pinMode(AIN2, OUTPUT); pinMode(PWMA, OUTPUT);
pinMode(BIN1, OUTPUT); pinMode(BIN2, OUTPUT); pinMode(PWMB, OUTPUT);
pinMode(STBY, OUTPUT);
digitalWrite(STBY, HIGH);
lastTime = millis();
}
void loop() {
mpu.update();
double angle = mpu.getAngleX(); // doi truc X/Y/Z tuy huong lap MPU6050 thuc te
unsigned long now = millis();
double dt = (now - lastTime) / 1000.0;
if (dt <= 0) dt = 0.001;
double error = setpoint - angle;
integral += error * dt;
integral = constrain(integral, -255, 255); // chong tich luy qua da (anti-windup)
double derivative = (error - lastError) / dt;
double output = Kp * error + Ki * integral + Kd * derivative;
output = constrain(output, -255, 255);
driveMotors(output);
lastError = error;
lastTime = now;
// Nga qua 45 do coi nhu that bai, dung dong co tranh chay lung tung vo ich
if (abs(angle) > 45) {
driveMotors(0);
integral = 0;
}
}
void driveMotors(double speed) {
bool forward = speed >= 0;
int pwm = constrain(abs((int)speed), 0, 255);
digitalWrite(AIN1, forward ? HIGH : LOW);
digitalWrite(AIN2, forward ? LOW : HIGH);
analogWrite(PWMA, pwm);
digitalWrite(BIN1, forward ? HIGH : LOW);
digitalWrite(BIN2, forward ? LOW : HIGH);
analogWrite(PWMB, pwm);
}Thư viện MPU6050_light tự tính một góc nghiêng ổn định bằng bộ lọc bù (complementary filter) kết hợp dữ liệu gia tốc kế và con quay hồi chuyển — đủ tốt cho bài toán này mà không cần tự viết bộ lọc Kalman phức tạp. Nếu muốn độ chính xác cao hơn, MPU6050 còn có DMP (Digital Motion Processor) tích hợp sẵn có thể tính quaternion ngay trên chip.
Những lỗi thường gặp
- Hướng lắp MPU6050 quyết định trục nào là “góc nghiêng” — nếu robot ngã theo hướng ngược với dấu của
angle, hãy đảo dấu output hoặc đổi trục đọc (getAngleX/getAngleY) cho khớp thực tế. - Vùng chết của động cơ (motor deadband): PWM quá nhỏ (dưới ~30-40) không đủ để thắng ma sát tĩnh, động cơ đứng yên dù có tín hiệu — có thể cần cộng thêm một mức PWM tối thiểu khi output khác 0.
- Tích lũy I không giới hạn (integral windup) khiến robot lắc mạnh sau khi bị giữ nghiêng lâu rồi thả ra — luôn giới hạn (
constrain) biếnintegral. - In ra Serial mỗi vòng lặp làm chậm tần số vòng lặp PID đáng kể — chỉ nên in để debug, và tắt hẳn khi đã hiệu chỉnh xong.
Mẹo hiệu chỉnh PID theo kinh nghiệm thực tế: bắt đầu với Ki = Kd = 0, tăng dần Kp cho đến khi robot bắt đầu dao động rung nhẹ quanh vị trí thẳng đứng, rồi giảm Kp xuống khoảng 70-80% giá trị đó. Tiếp theo thêm Kd để dập dao động cho mượt hơn. Cuối cùng thêm một chút Ki nhỏ để triệt tiêu độ lệch còn sót lại — nếu Ki quá lớn, robot sẽ lắc qua lại chậm và rộng.
Mở rộng
Thêm module Bluetooth hoặc ESP32 để điều khiển từ xa (đổi setpoint một chút để robot tiến/lùi theo lệnh), thử auto-tuning PID bằng thuật toán Ziegler-Nichols, hoặc nâng cấp lên ESP32 để gửi log góc nghiêng theo thời gian thực qua WiFi cho dễ debug.
Bình luận