Gimbal chống rung một trục với SimpleFOC và ESP32-S3

Gimbal mua sẵn loại một trục cho camera nhỏ giá không rẻ, mà phần điều khiển thì đóng kín. Tôi build lại từ đầu bằng SimpleFOC để hiểu trọn vẹn vòng điều khiển: từ IMU đọc góc nghiêng đến dòng chạy trong ba pha motor. Bài này ghi lại toàn bộ phần cứng, thông số và code.

Phần cứng

Linh kiện	Mã	Thông số chính	Giá (VNĐ)
Motor gimbal	GM4108H-120T	24N22P, R = 11.1 Ω, 22 g·cm	420 000
Driver	DRV8313	3 pha, 2.5 A đỉnh, 8–60 V	65 000
MCU	ESP32-S3	LX7 2 nhân 240 MHz	95 000
IMU	MPU6050	gyro + accel, I2C 400 kHz	28 000
Cảm biến góc	AS5600	từ trường, 12 bit, I2C	35 000

Motor gimbal khác motor drone ở số vòng dây: điện trở pha 11 Ω thay vì dưới 0.1 Ω, chạy êm ở dòng thấp và giữ vị trí tốt — đúng thứ cần cho chống rung, ngược hẳn với nhu cầu kéo tải của motor bay.

Sơ đồ khối

Hai vòng điều khiển lồng nhau:

1. Vòng ngoài (góc) — đọc góc nghiêng từ MPU6050 qua complementary filter, PID ra tốc độ mục tiêu. Chạy 1 kHz.
2. Vòng trong (FOC) — SimpleFOC lo phần chuyển tốc độ mục tiêu thành điện áp ba pha, dùng AS5600 làm cảm biến vị trí rotor.

Code

Khởi tạo SimpleFOC với DRV8313, giới hạn điện áp để motor 11 Ω không nóng:

#include <SimpleFOC.h>

BLDCMotor motor = BLDCMotor(11);            // 11 cặp cực (24N22P)
BLDCDriver3PWM driver = BLDCDriver3PWM(25, 26, 27, 33);
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);

void setup() {
  sensor.init();
  motor.linkSensor(&sensor);

  driver.voltage_power_supply = 12;
  driver.init();
  motor.linkDriver(&driver);

  // Motor gimbal điện trở cao: giới hạn 6 V là đủ moment, đỡ nóng
  motor.voltage_limit = 6;
  motor.controller = MotionControlType::angle;

  motor.init();
  motor.initFOC();
}

Vòng góc với complementary filter — gyro tin ở chu kỳ ngắn, accel kéo về đúng trọng lực ở chu kỳ dài:

// alpha = tau / (tau + dt), tau = 0.5 s, dt = 1 ms
constexpr float ALPHA = 0.998f;

float goc_nghieng(float dt) {
  float acc = atan2f(ay, az) * RAD_TO_DEG;
  goc = ALPHA * (goc + gx * dt) + (1.0f - ALPHA) * acc;
  return goc;
}

Tinh chỉnh PID

Chỉnh theo trình tự Kp → Kd → Ki, mỗi bước ghi lại hiện tượng:

Bước	Kp	Ki	Kd	Hiện tượng
1	2.0	0	0	dao động chậm quanh mục tiêu
2	4.0	0	0	rung tần số cao, nghe rít
3	3.0	0	0.05	hết rít, còn lệch tĩnh ~1.5°
4	3.0	0.8	0.05	lệch tĩnh về 0, đạt

Kết quả cuối: giữ camera lệch dưới 0.3° khi lắc tay chủ động, dòng tiêu thụ trung bình 180 mA ở 12 V.

Việc còn lại

• Vỏ in 3D hiện cộng hưởng nhẹ quanh 40 Hz — cần thêm gân cứng.
• Thử thay complementary filter bằng Mahony xem lệch tĩnh khi xoay nhanh có cải thiện không.
• Lên trục thứ hai (roll) dùng chung mạch driver.