Bài đăng kỹ thuật
Triển khai và phân tích hệ thống
Mô hình robot delta 3 cánh cho dây chuyền đóng gói: tính động học nghịch, thiết kế quỹ đạo gắp – đặt dạng chữ U với biên dạng vận tốc mượt, theo dõi băng tải bằng encoder để gắp vật đang chuyển động, phối hợp camera xác định vị trí vật.
Robot delta (robot song song) là loại robot nhanh nhất trong công nghiệp cho tác vụ gắp – đặt vật nhẹ: bánh kẹo, dược phẩm, linh kiện. Ba cánh tay nhẹ nối từ đế cố định xuống một bàn kẹp chung, động cơ đặt hết ở đế nên khối lượng chuyển động rất nhỏ. Dự án xây dựng mô hình thu nhỏ để hiểu đầy đủ toán học và kỹ thuật điều khiển.
1. Bài toán thực tế
Sản phẩm chạy liên tục trên băng tải ở vị trí ngẫu nhiên, cần được gắp và xếp vào khay. Robot phải biết vị trí từng vật (camera), dự đoán vật sẽ ở đâu khi tay tới (bám băng tải), và di chuyển cực nhanh mà không rung.
2. Mục tiêu dự án
- Thiết kế cơ khí và tính động học nghịch: tọa độ (x, y, z) → 3 góc động cơ.
- Quỹ đạo chữ U (nâng – di chuyển – hạ) với biên dạng vận tốc dạng S để giảm rung.
- Bám băng tải: cập nhật điểm gắp theo quãng đường băng tải đã đi (encoder).
- Đánh giá năng suất (số lần gắp/phút) và độ chính xác.
3. Phần cứng sử dụng
| Thiết bị | Thông số chính | Vai trò |
|---|---|---|
| 3 servo/động cơ bước có hộp số | Mô-men đủ cho cánh dài 150–200 mm | Dẫn động 3 cánh trên |
| Cánh trên nhôm, cánh dưới hình bình hành (thanh carbon + khớp cầu) | — | Cơ cấu song song |
| Giác hút + bơm chân không mini + van | — | Gắp vật |
| Băng tải + encoder tăng dần | — | Cấp vật, đo quãng đường |
| Camera USB + máy tính (OpenCV) | — | Xác định tọa độ vật |
| STM32F4 / Teensy hoặc driver servo có nội suy | — | Tính IK và phát lệnh động cơ |
4. Sơ đồ khối và đấu nối

Máy tính xử lý ảnh, gửi tọa độ vật (theo hệ băng tải) kèm số đếm encoder tại thời điểm chụp. Bộ điều khiển robot cộng thêm quãng đường băng tải đã chạy kể từ lúc chụp để có vị trí hiện tại của vật, rồi lập quỹ đạo gắp sao cho gặp vật đúng chỗ.
Bảng tham chiếu
| Ký hiệu | Ý nghĩa | Ví dụ |
|---|---|---|
| f | Cạnh tam giác đều của đế cố định | 200 mm |
| e | Cạnh tam giác đều của bàn kẹp | 60 mm |
| rf | Chiều dài cánh trên | 150 mm |
| re | Chiều dài cánh dưới (hình bình hành) | 300 mm |
5. Nguyên lý và thuật toán
Động học nghịch. Xét riêng từng cánh trong mặt phẳng YZ của nó (hai cánh còn lại có được bằng cách xoay tọa độ ±120° quanh trục Z). Khớp vai nằm trên đường tròn tâm (0, −f/(2√3)) bán kính rf; điểm nối của cánh dưới với bàn kẹp chiếu lên mặt phẳng đó cách khớp vai một khoảng re' = √(re² − x²). Giao của hai đường tròn cho vị trí khuỷu, từ đó tính góc θ. Bài toán có nghiệm khi đường tròn giao nhau – đây cũng là điều kiện điểm nằm trong không gian làm việc.
Quỹ đạo chữ U. Gồm đoạn nâng, đoạn ngang và đoạn hạ, các góc được bo tròn để tay không phải dừng hẳn ở hai góc chữ U. Dùng biên dạng vận tốc S (giới hạn jerk) thay cho hình thang làm giảm rung đáng kể ở tốc độ cao.
Bám băng tải. Vị trí vật tại thời điểm t: x(t) = xchụp + (enc(t) − encchụp) × mm/xung. Ở đoạn hạ xuống, mục tiêu x được cập nhật liên tục theo encoder để giác hút "đi cùng" vật trong lúc hút.
6. Chương trình mẫu
Động học nghịch robot delta (C, đơn vị mm và độ):
#include <math.h>
const float f = 200, e = 60, rf = 150, re = 300;
const float sqrt3 = 1.7320508f, tan30 = 1 / 1.7320508f;
const float sin120 = 0.8660254f, cos120 = -0.5f;
// tính góc của một cánh trong mặt phẳng YZ, trả về 0 nếu có nghiệm
static int angleYZ(float x0, float y0, float z0, float *theta)
{
float y1 = -0.5f * tan30 * f; // khớp vai
y0 -= 0.5f * tan30 * e; // dời tâm bàn kẹp ra mép
float a = (x0*x0 + y0*y0 + z0*z0 + rf*rf - re*re - y1*y1) / (2*z0);
float b = (y1 - y0) / z0;
float d = -(a + b*y1)*(a + b*y1) + rf*(b*b*rf + rf); // biệt thức
if (d < 0) return -1; // ngoài không gian làm việc
float yj = (y1 - a*b - sqrtf(d)) / (b*b + 1); // chọn khuỷu ngoài
float zj = a + b*yj;
*theta = atanf(-zj / (y1 - yj)) * 180.0f / (float)M_PI + ((yj > y1) ? 180.0f : 0.0f);
return 0;
}
int delta_ik(float x, float y, float z, float t[3]) // z âm (phía dưới đế)
{
if (angleYZ(x, y, z, &t[0])) return -1;
if (angleYZ(x*cos120 + y*sin120, y*cos120 - x*sin120, z, &t[1])) return -1;
if (angleYZ(x*cos120 - y*sin120, y*cos120 + x*sin120, z, &t[2])) return -1;
return 0;
}Công thức theo bài viết kinh điển về động học robot delta của Trossen Robotics (tham khảo bên dưới). Luôn kiểm tra giá trị trả về: điểm ngoài vùng làm việc phải bị từ chối trước khi gửi lệnh động cơ.
7. Các bước triển khai và kiểm thử
- Đo chính xác f, e, rf, re trên mô hình thật; đặt góc 0° khi cánh trên nằm ngang.
- Kiểm tra IK bằng cách ra lệnh các điểm trên lưới 5 × 5 ở một độ cao, đo vị trí thật bằng thước/giấy kẻ ô.
- Thử quỹ đạo chữ U ở tốc độ tăng dần, quan sát rung ở cuối hành trình.
- Hiệu chuẩn camera – robot: đặt vật ở 4 vị trí đã biết để tìm phép biến đổi pixel → mm.
- Chạy băng tải với vật đặt ngẫu nhiên, đo tỉ lệ gắp thành công và số lần gắp/phút.
8. Lỗi thường gặp và cách xử lý
- Tay giật khi qua một số vùng: điểm gần biên không gian làm việc, góc thay đổi đột ngột – giới hạn vùng làm việc an toàn.
- Gắp hụt vật đang chạy: trễ giữa lúc chụp ảnh và số encoder ghi lại; chụp ảnh bằng trigger phần cứng đồng bộ với encoder.
- Rung mạnh khi dừng: biên dạng hình thang gia tốc đột ngột; khớp cầu có độ rơ.
9. Hướng mở rộng
- Thêm trục xoay thứ 4 ở bàn kẹp để định hướng sản phẩm.
- Hai robot trên cùng băng tải chia việc (load balancing).
- Phân loại sản phẩm lỗi bằng camera trước khi gắp.