Ổn định tĩnh và trọng tâm robot

Ổn định tĩnh và trọng tâm (CoG) robot

Điều kiện ổn định tĩnh

Robot ổn định tĩnh khi hình chiếu trọng tâm (CoG) nằm trong polygon hỗ trợ — đa giác tạo bởi các điểm chạm đất.

Hexapod chân 1,3,5 chạm đất → tam giác ABC
CoG phải nằm bên trong tam giác ABC

Tính CoG trong code

struct Vec2 { float x, y; };

// Giả sử body robot là hình chữ nhật 20×12 cm, trọng tâm ở giữa
const Vec2 BODY_CENTER = {10.0f, 6.0f};

// Vị trí chân khi chạm đất (tọa độ x,y tính từ center body)
Vec2 footPositions[6] = {
  {-8, -6},  // chân 0: trái trước
  { 0, -8},  // chân 1: trái giữa
  { 8, -6},  // chân 2: trái sau
  { 8,  6},  // chân 3: phải sau
  { 0,  8},  // chân 4: phải giữa
  {-8,  6},  // chân 5: phải trước
};

bool isPointInTriangle(Vec2 p, Vec2 a, Vec2 b, Vec2 c) {
  // Dùng cross product để kiểm tra
  auto cross = [](Vec2 v1, Vec2 v2) {
    return v1.x * v2.y - v1.y * v2.x;
  };
  float d1 = cross({p.x-a.x, p.y-a.y}, {b.x-a.x, b.y-a.y});
  float d2 = cross({p.x-b.x, p.y-b.y}, {c.x-b.x, c.y-b.y});
  float d3 = cross({p.x-c.x, p.y-c.y}, {a.x-c.x, a.y-c.y});
  bool hasNeg = (d1<0)||(d2<0)||(d3<0);
  bool hasPos = (d1>0)||(d2>0)||(d3>0);
  return !(hasNeg && hasPos);
}

// Kiểm tra tripod gait (chân 0,2,4 hoặc 1,3,5 chạm đất)
bool isStableGroup(int legA, int legB, int legC) {
  Vec2 CoG = {0, 0}; // CoG tại origin
  return isPointInTriangle(CoG,
    footPositions[legA],
    footPositions[legB],
    footPositions[legC]);
}

void setup() {
  Serial.begin(9600);
  Serial.println("Tripod A (0,2,4) stable:", isStableGroup(0,2,4) ? "YES" : "NO");
  Serial.println("Tripod B (1,3,5) stable:", isStableGroup(1,3,5) ? "YES" : "NO");
}

Safety margin

Để robot không ngã khi gặp perturbation nhỏ, CoG nên cách mép tam giác ít nhất 2–3 cm.

Ảnh hưởng tải trọng

Khi mang vật nặng, CoG dịch chuyển. Cần:

1. Di chuyển body về phía đối diện để bù
2. Giảm tốc độ dáng đi
3. Tránh dùng tripod gait khi tải nặng — dùng wave gait (4 chân chạm đất)

Giáo án giảng dạy

Mục tiêu bài học

• Học viên giải thích được vai trò của bài Ổn định tĩnh và trọng tâm robot trong mạch robot có chân và lập trình dáng đi.
• Học viên đọc được sơ đồ chân, nhận biết nguồn, GND và đường tín hiệu trước khi cấp điện.
• Học viên chạy được ví dụ mẫu, quan sát hiện tượng, rồi sửa một tham số để hiểu tác động kỹ thuật.

Kiến thức nền

Bài này đặt trong khóa Robot Có Chân Cơ Bản. Phần cứng trung tâm là Arduino/ESP32 với nhiều chân PWM hoặc servo driver PCA9685, cơ cấu chấp hành gồm servo SG90/MG90S/MG996R tùy kích thước chân, cảm biến hoặc tín hiệu liên quan gồm tùy chọn IMU, cảm biến khoảng cách hoặc điều khiển Bluetooth. Khi dạy, giáo viên nên nhấn mạnh quan hệ giữa 3 lớp: phần cứng nhận tín hiệu, chương trình xử lý logic, và cơ cấu tạo chuyển động thật.

Linh kiện và dụng cụ

Sơ đồ nối dây khuyến nghị

Quy trình thực hành trên lớp (35-50 phút)

1. Giáo viên giới thiệu mục tiêu và chỉ ra linh kiện thật trên bàn học.
2. Học viên nối dây khi chưa cấp nguồn tải, sau đó kiểm tra chéo theo cặp.
3. Upload code mẫu, mở Serial Monitor nếu bài có dữ liệu cần quan sát.
4. Chạy thử ở tốc độ/tham số thấp để giảm rủi ro va chạm hoặc quá dòng.
5. Học viên thay đổi một tham số: tốc độ PWM, ngưỡng cảm biến, góc servo hoặc thời gian giữ.
6. Cả lớp ghi lại hiện tượng: robot nhanh/chậm hơn, lệch hướng, rung, mất line hoặc phản ứng trễ.
7. Giáo viên tổng kết bằng sơ đồ "input → xử lý → output" của bài học.

Code mẫu nền tảng

#include <Servo.h>

Servo hipFL, kneeFL;

void pose(int hip, int knee, int holdMs=180) {
  hipFL.write(hip);
  kneeFL.write(knee);
  delay(holdMs);
}

void setup() {
  hipFL.attach(5);
  kneeFL.attach(6);
  pose(90, 90, 500);
}

void loop() {
  pose(70, 110);
  pose(95, 85);
  pose(115, 100);
}

Giải thích kỹ thuật

• Nguồn quyết định độ ổn định: motor và servo kéo dòng cao nên cần nguồn riêng, tụ lọc và dây đủ chắc.
• Tín hiệu điều khiển chỉ mang lệnh, không mang công suất. Board điều khiển gửi HIGH/LOW/PWM, driver mới cấp dòng cho tải.
• Calibration giúp robot hoạt động lặp lại được: cảm biến cần ngưỡng, servo cần góc home, motor cần bù sai lệch hai bên.
• Tách hàm làm code dễ dạy: một hàm cho đọc input, một hàm xử lý quyết định, một hàm điều khiển output.

Checklist an toàn phần cứng

• tháo chân khỏi mặt đất khi test góc đầu tiên; kiểm tra servo nóng sau mỗi 3-5 phút.
• Kiểm tra cực dương/âm của pin bằng mắt và bằng đồng hồ đo nếu có.
• Không để dây nguồn trần chạm nhau hoặc chạm khung kim loại.
• Khi motor/servo nóng bất thường, dừng nguồn tải trước rồi mới sửa code hoặc sửa dây.
• Với pin Li-ion/LiPo, không sạc khi không giám sát và không dùng pin phồng.

Lỗi thường gặp và cách debug

Bài tập trên lớp

• Hoàn thành mạch và chạy code mẫu ổn định ít nhất 3 lần liên tiếp.
• Giải thích bằng lời vai trò của từng dây: VCC, GND, signal/PWM và nguồn tải.
• Thay đổi một tham số trong code và ghi lại hiện tượng trước/sau.

Bài tập về nhà

• Viết lại kiến thức chính bằng sơ đồ khối và liên hệ với một lỗi phần cứng có thể xảy ra.
• Vẽ lại sơ đồ nối dây của bài vào vở hoặc slide nhóm.
• Chuẩn bị một câu hỏi debug cho buổi sau: "Nếu robot không chạy, em sẽ kiểm tra gì đầu tiên?"

Tiêu chí đánh giá

Gợi ý cho giáo viên

Với học viên mới, đừng bắt đầu bằng code dài. Hãy cho học viên chạm linh kiện, đoán chức năng, rồi mới nối dây. Khi có lỗi, yêu cầu các nhóm đọc lỗi theo thứ tự: nguồn → GND → chân tín hiệu → code → cơ khí. Cách này hình thành tư duy hardware engineering thay vì chỉ sửa mò.