Học

Một robot tự cân bằng trên một Arduino

Nâng cao

Hoặc xem mọi bài học.

  • 526linh kiện trên bàn làm mạch
  • 46bo mạch đang chạy
  • 1.00xthời gian thực, trên mọi bo mạch
Arduino Uno: robot tự cân bằngtrực tiếp0.000 s 0.00x
Nhấp để mở trong trình soạn thảo
Ví dụ Robot cân bằng: MPU6050 qua bộ lọc bổ trợ và bộ PID trên ba núm xoay. Đẩy nó bằng các nút mũi tên.

Robot tự cân bằng đứng trên hai bánh xe, giống một con lắc ngược, và đứng vững được chỉ vì nó liên tục đưa các bánh xe về lại ngay dưới thân mình. Đây là một dự án Arduino kinh điển vì nó gộp hai ý tưởng lớn trong một bản dựng: biến một cảm biến nhiều nhiễu thành một góc chính xác, và một bộ điều khiển PID tác động lên góc đó hàng trăm lần mỗi giây.

Mạch ở trên là một Uno chạy một robot hai bánh với MPU6050 và trình điều khiển động cơ TB6612 của riêng nó. Ba núm xoay đặt các hệ số PID trong lúc nó chạy, và một màn hình OLED hiển thị góc và các hệ số. Nhấn Chạy: robot được giữ thẳng trong chốc lát rồi thả ra, và nó tự giữ thăng bằng. Sau đó đẩy nó bằng các nút mũi tên trên robot, và xoay các núm để xem từng hệ số có tác dụng gì.

Những gì bạn cần

  • Một Arduino Uno
  • Khung xe robot hai bánh với động cơ DC có hộp số
  • Cảm biến gia tốc và con quay hồi chuyển MPU6050
  • Driver động cơ TB6612FNG
  • Ba biến trở 10k
  • Màn hình OLED SSD1306, 128x64, I2C (tùy chọn)
  • Hộp pin cho các động cơ

Đi dây

Linh kiện và chânNối tớiGhi chú
SCL và SDA của robotChân 12 và 2MPU6050 của robot ở địa chỉ 0x68, dùng đường I2C bằng phần mềm riêng
AIN1, AIN2, PWMAChân 10, 9 và 6Động cơ trái
BIN1, BIN2, PWMBChân 5, 4 và 3Động cơ phải
STBY5 VDriver chạy bất cứ khi nào được cấp nguồn
Núm chỉnh Kp, Ki và KdA0, A1 và A2Hai đầu vào 5 V và GND
SCL và SDA của OLEDA5 và A4Góc và các hệ số

Bước 1: tính độ nghiêng

MPU6050 có hai cảm biến, và không cảm biến nào là đủ tốt khi đứng một mình. Cảm biến gia tốc biết hướng nào là hướng xuống, nên atan2(-ax, az) cho ra độ nghiêng so với trọng lực. Nhưng nó cũng cảm nhận từng cú hích của bánh xe truyền vào thân xe, nên khi robot di chuyển nó bị nhiễu và cho kết quả sai. Con quay hồi chuyển đo tốc độ quay của thân xe một cách mượt mà, nhưng bạn phải cộng dồn tốc độ đó để có góc, và các sai số nhỏ cũng cộng dồn theo, khiến góc bị trôi.

Bộ lọc bù (complementary filter) lấy điểm mạnh của từng cảm biến: angle = 0.98 * (angle + rate * dt) + 0.02 * accelAngle. Từng khoảnh khắc một, con quay làm góc thay đổi, còn cảm biến gia tốc kéo nó nhẹ nhàng về giá trị đúng, 2% mỗi bước. Nó chỉ là một dòng code, và đây chính là thứ mà hầu hết robot tự cân bằng dùng.

Bước 2: bộ PID trên góc nghiêng

Cứ mỗi 10 ms, sketch tính xem thân xe nghiêng bao xa so với thẳng đứng và điều khiển bánh xe để sửa lại. Đầu ra là ba số hạng cộng lại với nhau. P (tỉ lệ) là độ nghiêng nhân Kp: nghiêng càng nhiều, chạy càng mạnh. D (đạo hàm) là tốc độ con quay nhân Kd: nó chống lại việc thân xe vung quá nhanh, giúp robot không vượt quá đích. I (tích phân) cộng dồn độ nghiêng theo thời gian nhân Ki: nó cung cấp lực đẩy ổn định mà động cơ cần chỉ để giữ một tốc độ.

Tổng bị giới hạn trong khoảng -255 đến 255, dấu của nó chọn hướng quay (AIN1 và AIN2) còn độ lớn thì đưa vào analogWrite trên PWMA và PWMB. Khi nghiêng về phía trước, bánh xe chạy về phía trước để trở lại ngay dưới trọng tâm thân xe.

Ba núm chỉnh đặt Kp từ 0 đến 80, Ki từ 0 đến 600 và Kd từ 0 đến 2. Khi mỗi núm ở giữa (40, 300 và 1), robot tự cân bằng: buông tay ở giây thứ nhất, nó đứng vững, và khoảng bốn giây sau khi nhấn Chạy, serial monitor báo "Balanced for 3 s". Đẩy nó bằng mũi tên và một lúc sau nó báo "Caught it: upright again".

Phần lõi của vòng lặp robot tự cân bằng, đã lược bớt
const float ALPHA = 0.98;
const float FALLEN = 45.0;
const float I_LIMIT = 150.0;

void loop() {
  unsigned long now = micros();
  if (now - lastUs < PERIOD_US) return;   // every 10 ms
  float dt = (now - lastUs) / 1000000.0;
  lastUs = now;

  Mpu6050Reading r;
  if (!imu.read(r)) return;
  float accelAngle = atan2(-(float)r.ax, (float)r.az) * 57.29578;
  float rate = r.gy / 131.0;
  angle = ALPHA * (angle + rate * dt) + (1 - ALPHA) * accelAngle;

  if (fabs(angle) > FALLEN) {
    fallen = true;
    integral = 0;
    brake();
    Serial.println("FALLEN");
  } else {
    integral += angle * dt;
    if (ki > 0.01) {
      float limit = I_LIMIT / ki;
      if (integral > limit) integral = limit;
      if (integral < -limit) integral = -limit;
    }
    float pid = kp * angle + ki * integral + kd * rate;
    output = (int)constrain(pid, -255, 255);
    drive(output);
  }
  readKnob();
}

Từng hệ số có tác dụng gì: thử ngay

Vặn Ki về thấp nhất. Không có số hạng tích phân, robot không giữ nổi lực đẩy mà động cơ cần ở tốc độ cao, nên nó từ từ trôi đi, nghiêng ngày càng nhiều để theo kịp, rồi ngã: serial monitor báo "FALLEN". Đó là công lao của số hạng I.

Vặn Kd về thấp nhất, rồi nhấn nút dựng đứng trên robot. Không có lực cản, mỗi lần vung đều vượt xa lần trước: nó đu đưa ngày càng mạnh cho tới khi ngã. Vặn Kd trở lại, cùng một cú đẩy đó tắt đi chỉ sau một hai lần đu.

Kp quyết định mức nó chống lại độ nghiêng. Quá thấp thì nó lười biếng và lắc lư; quá cao thì nó rung lên. Người ta chỉnh robot thật theo thứ tự đó: Kp cho đến khi nó gần đứng vững và bắt đầu rung, Kd cho đến khi hết rung, rồi Ki cho đến khi hết trôi.

Được giữ, buông tay, ngã

A real robot is held upright while it powers up. If the PID ran during that time, the integral would fill up with a lean the wheels cannot fix (your hand is holding it), and the stored push would throw it over the moment you let go. So the sketch keeps the motors braked while the gyro says the body is still, and starts balancing when the rate goes over 1 degree a second: the hand has let go.

Quá 45 độ là coi như ngã. Các động cơ phanh, tích phân được đưa về không, và trên robot nó nằm trên sàn ở khoảng 80 độ. Dựng nó đứng lên bằng nút nhấn và nó lại được giữ, sẵn sàng cho lần thử tiếp theo.

Mô hình là một con lắc thật trên bánh xe, được tính toán mỗi mili giây, với các động cơ đẩy thân xe trở lại khi chúng kéo bánh xe về phía trước. Những gì nó bỏ qua là các chỗ gồ ghề của sàn nhà, độ trễ trong hộp số giá rẻ và pin bị tụt áp, đây chính là những thứ khiến robot thật khó chỉnh hơn.

Những lỗi thường gặp

Bánh xe quay sai chiều. Nếu robot chạy ra xa thay vì chống lại độ nghiêng, nó ngã ngay lập tức. Đổi chỗ dây của một động cơ, hoặc đổi dấu của đầu ra.

Góc bị sai dấu, vì MPU6050 được gắn theo hướng khác. Nghiêng robot về phía trước bằng tay và kiểm tra góc tăng dương trước khi chạy động cơ.

Chỉ tin vào cảm biến gia tốc. Nó đọc các cú hích của bánh xe như là độ nghiêng. Hãy trộn nó với con quay hồi chuyển.

Chân STBY trên TB6612 bị bỏ trống không nối. Nó có điện trở kéo xuống, nên driver luôn tắt và động cơ không bao giờ quay. Kéo chân STBY vào mạch ở trên và robot sẽ ngã ngay khi được buông ra.

Vòng lặp quá chậm. In ra serial monitor mỗi vòng lặp, hoặc vẽ nguyên màn OLED một lúc, sẽ lấy mất thời gian mà PID cần. Sketch chỉ in bốn lần mỗi giây và vẽ một dòng OLED mỗi lần gọi.

Tích phân tràn. Nếu không giới hạn, số hạng I cứ lớn dần khi robot bị giữ hoặc bị kẹt. Sketch giới hạn nó và đưa về không khi ngã.

Mở mạch này trong trình soạn thảo. Arduino Uno: robot tự cân bằng, được nối dây đúng như đang chạy ở đầu trang này. Đổi một linh kiện hoặc một giá trị rồi chạy lại.

Mở mạch

Câu hỏi

Robot tự cân bằng cần cảm biến nào?

Cần cả cảm biến gia tốc và con quay hồi chuyển, chẳng hạn MPU6050. Cảm biến gia tốc cho góc nghiêng từ trọng lực, con quay cho tốc độ quay, và bộ lọc bù hoặc Kalman trộn hai nguồn này lại.

Làm thế nào để chỉnh PID cho robot tự cân bằng?

Bắt đầu với Ki và Kd bằng không. Tăng Kp cho đến khi robot gần đứng vững và bắt đầu rung, tăng Kd cho đến khi hết rung, rồi thêm một chút Ki để nó hết trôi đi.

Driver động cơ nào tốt cho robot tự cân bằng?

TB6612FNG là lựa chọn tốt cho động cơ có hộp số nhỏ: nó hiệu quả và chuyển mạch nhanh. L298N cũng dùng được nhưng tiêu tốn nhiều năng lượng hơn thành nhiệt.

Vì sao robot tự cân bằng của tôi trôi đi rồi ngã?

Thường là do tích phân quá ít, nên nó không giữ nổi lực đẩy cần thiết ở tốc độ cao, hoặc điểm cân bằng chưa hoàn toàn thẳng đứng. Thêm chút Ki, hoặc chỉnh lại góc zero.

Lắp mạch này ngoài đời

Mở trình soạn thảo, đổi một giá trị và xem con số thay đổi theo. Không cần cài gì, không cần tài khoản.