Vision dẫn hướng robot gắp vật: tọa độ, góc kẹp, bám băng tải

Robot công nghiệp lặp lại vị trí tới vài phần trăm milimét, nhưng chỉ khi chi tiết luôn nằm đúng chỗ. Đồ gá cơ khí để giữ mọi chi tiết đúng chỗ thì đắt, cứng nhắc, và mỗi lần đổi mã hàng lại phải làm lại. Thêm một camera, robot có thể gắp chi tiết nằm ngẫu nhiên trên băng tải hay khay: đó là vision guided robotics (VGR), một trong những ứng dụng mang lại giá trị rõ nhất của Machine Vision. Bài này ghép những mảnh bạn đã có: phép biến đổi pixel → robot của bài "Hand-eye calibration camera – robot", và kênh gửi tin có số thứ tự, ACK của bài "TCP socket: gửi kết quả và tọa độ cho robot". Ta sẽ đi qua kiến trúc hệ thống, cách tìm tâm và góc chi tiết, cách đổi cả góc sang hệ robot (chỗ rất nhiều người làm sai), bù chuyển động của băng tải bằng encoder, rồi chạy một mô phỏng hoàn chỉnh: vision tìm chi tiết, đổi tọa độ, gửi cho robot giả qua TCP. Bài sau, "MES và truy xuất nguồn gốc", đưa dữ liệu từ trạm lên tầng quản lý sản xuất.

1. Kiến trúc hệ thống pick and place

Trình tự một chu kỳ (camera cố định, chi tiết đứng yên trên khay):

1. Robot ra khỏi vùng nhìn của camera, báo "sẵn sàng" (qua socket hoặc PLC).
2. Vision chụp, tìm tất cả chi tiết, đổi tọa độ, kiểm tra vùng với tới, sắp thứ tự gắp.
3. Vision gửi từng tin seq,OK,X,Y,góc; robot ACK, di chuyển, gắp, đặt.
4. Khi hết chi tiết hợp lệ, vision gửi tin "hết" hoặc robot yêu cầu chụp lại.

Ví dụ thời gian. Robot SCARA gắp – đặt mất khoảng 0,6 s mỗi chi tiết. Nếu mỗi lần chụp phải chờ robot rời vùng nhìn (0,3 s) rồi xử lý ảnh (0,15 s), mỗi chi tiết tốn 1,05 s. Chụp một lần cho cả khay 6 chi tiết thì thời gian chụp – xử lý chia đều còn 0,075 s mỗi chi tiết: 0,675 s mỗi chi tiết, nhanh hơn khoảng 35%. Điều kiện: robot không làm xê dịch các chi tiết còn lại khi gắp.

2. Hiệu chuẩn: ma trận pixel → robot

Bài "Hand-eye calibration camera – robot" đã dạy kỹ quy trình 9 điểm và cách chọn mô hình. Ở đây ta dựng lại nhanh một trạm mô phỏng: camera cố định nhìn xuống băng tải, ảnh 800 × 500 px, khoảng 0,25 mm/px. Phép biến đổi "thật" (mà ngoài đời ta không biết) có trục ảnh xoay khoảng 91° so với trục robot và một phép lật, giống mọi camera nhìn xuống. Ta lấy 9 cặp điểm có nhiễu rồi ước lượng ma trận affine 2 × 3 bằng cv2.estimateAffine2D, lưu ra file như ở trạm thật.

import json
import cv2
import numpy as np

# ---- "Sự thật" của mô phỏng (ngoài đời ta không biết): pixel → robot là affine có lật trục
s, th = 0.25, np.radians(91.0)                           # 0,25 mm/px, trục ảnh xoay ~91° so với robot
L_that = s * np.array([[np.cos(th), -np.sin(th)], [np.sin(th), np.cos(th)]]) @ np.diag([1.0, -1.0])
t_that = np.array([350.0, -40.0])                         # tọa độ robot (mm) của pixel (0, 0)


def px_sang_robot_that(P):
    return np.asarray(P, float) @ L_that.T + t_that


# ---- Hiệu chuẩn 9 điểm: lưới 3 × 3 phủ ảnh, robot chạm tâm từng dấu (sai số chạm ~0,05 mm)
rng = np.random.default_rng(3)
px9 = np.array([(u, v) for v in (60, 250, 440) for u in (80, 400, 720)], float) + rng.normal(0, 0.15, (9, 2))
rb9 = px_sang_robot_that(px9) + rng.normal(0, 0.05, (9, 2))
M, _ = cv2.estimateAffine2D(px9, rb9, method=cv2.LMEDS)
du = np.linalg.norm(px9 @ M[:, :2].T + M[:, 2] - rb9, axis=1)
print("M =\n", np.round(M, 5))
print(f"Phần dư 9 điểm: trung bình {du.mean():.3f} mm, lớn nhất {du.max():.3f} mm")
print(f"Tỉ lệ ≈ {np.sqrt(abs(np.linalg.det(M[:, :2]))):.4f} mm/px, định thức {np.linalg.det(M[:, :2]):+.4f}")
json.dump({"M": M.tolist(), "ngay": "2026-10-10", "do_cao_mm": 0}, open("hieu_chuan_2d.json", "w"))

M =
 [[-4.3500000e-03  2.4990000e-01  3.5002783e+02]
 [ 2.4999000e-01  4.3700000e-03 -3.9995860e+01]]
Phần dư 9 điểm: trung bình 0.045 mm, lớn nhất 0.085 mm
Tỉ lệ ≈ 0.2500 mm/px, định thức -0.0625

Định thức âm cho biết có phép lật: đi theo chiều tăng của u trên ảnh tương ứng chiều tăng của Y robot, nhưng chiều tăng của v lại tương ứng chiều tăng của X, nên một góc quay ngược chiều kim đồng hồ trên ảnh thành góc quay cùng chiều kim đồng hồ trong hệ robot. Hãy nhớ chi tiết này, nó quyết định cách đổi góc ở mục 4. File JSON lưu kèm ngày và độ cao hiệu chuẩn: khi có chuyện, đó là thứ đầu tiên cần xem.

3. Định vị chi tiết: tâm và góc trong ảnh

Chi tiết là những tấm chữ nhật 30 × 12 mm nằm ngẫu nhiên trên băng tải. Với chi tiết có bóng rõ trên nền tương phản, chuỗi xử lý quen thuộc từ chặng xử lý ảnh là đủ: lọc nhiễu, ngưỡng Otsu, contour ngoài, lọc theo diện tích, rồi cv2.minAreaRect cho hình chữ nhật xoay bao nhỏ nhất. Hai điểm cần cẩn thận:

• Đừng dựa vào góc trả về của minAreaRect. Quy ước của nó (góc nằm trong khoảng nào, đo theo cạnh nào, width hay height là cạnh dài) đã thay đổi giữa các phiên bản OpenCV (thay đổi lớn từ bản 4.5.1). Cách bền vững: lấy 4 đỉnh bằng cv2.boxPoints, chọn cạnh dài, tự tính góc bằng atan2. Kết quả không phụ thuộc phiên bản.
• Bỏ chi tiết chạm mép ảnh. Một chi tiết bị cắt mất một phần cho tâm và góc sai mà vẫn trông "hợp lý". Nó sẽ được gắp ở lần chụp sau (băng tải chạy) hoặc báo cho người vận hành.

Lưu ý quy ước góc trong ảnh: trục v hướng xuống, nên atan2(dv, du) dương nghĩa là quay theo chiều kim đồng hồ khi nhìn ảnh trên màn hình.

import cv2
import numpy as np

L_inv = np.linalg.inv(L_that)


def robot_sang_px_that(P):
    return (np.asarray(P, float) - t_that) @ L_inv.T


# Chi tiết thật trên băng tải, trong hệ robot: (X mm, Y mm, góc độ). Chi tiết D nằm lấn ra mép ảnh.
VAT_THAT = {"A": (380.0, 20.0, 30.0), "B": (440.0, 100.0, -60.0), "C": (410.0, 135.0, 85.0), "D": (395.0, 158.0, 0.0)}
DAI, RONG = 30.0, 12.0                                    # kích thước chi tiết, mm

anh = np.full((500, 800), 40, np.uint8)
for X, Y, goc in VAT_THAT.values():
    g = np.radians(goc)
    truc_dai, truc_ngan = np.array([np.cos(g), np.sin(g)]), np.array([-np.sin(g), np.cos(g)])
    dinh = [np.array([X, Y]) + a * DAI / 2 * truc_dai + b * RONG / 2 * truc_ngan
            for a, b in [(1, 1), (-1, 1), (-1, -1), (1, -1)]]
    poly = np.round(robot_sang_px_that(dinh) * 16).astype(np.int32)          # vẽ subpixel: shift = 4
    cv2.fillPoly(anh, [poly], 210, lineType=cv2.LINE_AA, shift=4)
anh = cv2.add(anh, np.random.default_rng(0).normal(0, 4, anh.shape).clip(0, 255).astype(np.uint8))
cv2.imwrite("bang_tai.png", anh)


def dinh_vi(gray, dien_tich_min=2000):
    """Tìm chi tiết: tâm (px), góc cạnh dài (độ, trong [-90, 90)), kích thước; bỏ vật chạm mép ảnh."""
    _, nhi_phan = cv2.threshold(cv2.GaussianBlur(gray, (5, 5), 0), 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
    cnts, _ = cv2.findContours(nhi_phan, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
    h, w = gray.shape
    ket_qua, bo_qua = [], []
    for c in cnts:
        if cv2.contourArea(c) < dien_tich_min:
            continue
        x, y, bw, bh = cv2.boundingRect(c)
        rect = cv2.minAreaRect(c)
        if x <= 1 or y <= 1 or x + bw >= w - 1 or y + bh >= h - 1:
            bo_qua.append(rect[0])
            continue
        p = cv2.boxPoints(rect)                                    # 4 đỉnh, theo thứ tự quanh hình
        canh1, canh2 = p[1] - p[0], p[2] - p[1]
        dai = canh1 if np.linalg.norm(canh1) >= np.linalg.norm(canh2) else canh2
        goc = np.degrees(np.arctan2(dai[1], dai[0]))
        goc = (goc + 90) % 180 - 90                                # cạnh dài không có "đầu", đưa về [-90, 90)
        ket_qua.append((rect[0][0], rect[0][1], goc, max(rect[1]), min(rect[1])))
    return sorted(ket_qua), bo_qua


vat, bo_qua = dinh_vi(anh)
print(f"Tìm thấy {len(vat)} chi tiết, bỏ qua {len(bo_qua)} vật chạm mép ảnh")
for u, v, goc, d, r in vat:
    print(f"  tâm ({u:6.1f}, {v:6.1f}) px   góc ảnh {goc:6.1f}°   kích thước {d:.1f} × {r:.1f} px")

Tìm thấy 3 chi tiết, bỏ qua 1 vật chạm mép ảnh
  tâm ( 237.9,  124.0) px   góc ảnh   61.0°   kích thước 121.5 × 49.3 px
  tâm ( 553.6,  370.0) px   góc ảnh  -28.9°   kích thước 121.3 × 49.4 px
  tâm ( 695.7,  252.3) px   góc ảnh    6.0°   kích thước 121.4 × 49.6 px

Chi tiết D bị loại vì chạm mép. Kích thước đo được khoảng 121 × 49 px so với 120 × 48 px lý thuyết: chênh khoảng 1 px do biên làm mềm và ngưỡng Otsu, không ảnh hưởng tới tâm và góc (chênh đều hai phía). Trong trạm thật, kích thước là bộ lọc tốt: hai chi tiết dính nhau hay một chi tiết dựng đứng sẽ cho kích thước khác hẳn, và phải bị loại thay vì gắp.

4. Đổi tâm và góc sang hệ robot

Tâm: áp ma trận affine, [X, Y] = M[:, :2] · [u, v] + M[:, 2]. Góc: nhiều người làm góc_robot = góc_ảnh + offset, với offset là góc xoay giữa hai hệ. Cách này chỉ đúng khi phép biến đổi là xoay thuần túy. Với phép lật (định thức âm, như mọi camera nhìn xuống) hoặc tỉ lệ hai trục khác nhau, nó sai. Cách đúng và tổng quát: biến đổi vectơ hướng của cạnh dài bằng phần 2 × 2 của M (vectơ không chịu tịnh tiến), rồi đo lại góc của vectơ đó trong hệ robot.

Quy ước góc của tay kẹp. Robot cần một góc xoay quanh trục Z của dụng cụ (thường gọi Rz, hay góc cổ tay). Ba điều phải thống nhất với lập trình viên robot: (1) góc 0 ứng với hướng nào của ngón kẹp so với trục X của user frame; (2) chiều dương (thường ngược chiều kim đồng hồ nhìn từ trên xuống khi trục Z hướng lên, nhưng phải kiểm tra); (3) khoảng cho phép của khớp cổ tay. Tay kẹp hai ngón song song đối xứng 180°: gắp ở góc 100° hay −80° là như nhau, nên đưa góc về [−90°, 90°) để cổ tay xoay ít nhất và không đụng giới hạn khớp. Giác hút tròn thì không cần góc khi gắp, nhưng có thể cần góc khi đặt. Chi tiết không đối xứng (có lỗ lệch, chữ in) cần đủ 360°: phải dùng thêm một đặc trưng (vị trí lỗ, khối lượng lệch tâm) để biết "đầu" nào là đầu.

import json
import numpy as np

M = np.array(json.load(open("hieu_chuan_2d.json"))["M"])


def chuan_hoa_goc_kep(goc, chu_ky=180.0):
    """Tay kẹp đối xứng chu_ky độ: đưa góc về [-chu_ky/2, chu_ky/2) để cổ tay xoay ít nhất."""
    return (goc + chu_ky / 2) % chu_ky - chu_ky / 2


def px_sang_robot(u, v, goc_anh, M):
    """Tâm: áp affine. Góc: biến đổi VECTƠ hướng cạnh dài bằng phần 2 × 2 của M rồi đo lại góc."""
    X, Y = M[:, :2] @ [u, v] + M[:, 2]
    g = np.radians(goc_anh)
    dx, dy = M[:, :2] @ [np.cos(g), np.sin(g)]
    return X, Y, chuan_hoa_goc_kep(np.degrees(np.arctan2(dy, dx)))


print(f"{'vật':<4}{'X':>8}{'Y':>8}{'góc':>7}   {'sai XY (mm)':>11}{'sai góc':>8}   cách 'cộng offset'")
for u, v, goc_anh, _, _ in vat:
    X, Y, goc = px_sang_robot(u, v, goc_anh, M)
    ten, (Xt, Yt, gt) = min(VAT_THAT.items(), key=lambda kv: np.hypot(kv[1][0] - X, kv[1][1] - Y))
    sai_goc = chuan_hoa_goc_kep(goc - gt)
    sai_ngay_tho = chuan_hoa_goc_kep((goc_anh + 91.0) - gt)          # coi như chỉ xoay 91°, quên phép lật
    print(f"{ten:<4}{X:8.2f}{Y:8.2f}{goc:7.1f}   {np.hypot(X - Xt, Y - Yt):11.3f}{sai_goc:8.2f}   sai {sai_ngay_tho:6.1f}°")

vật        X       Y    góc   sai XY (mm) sai góc   cách 'cộng offset'
A     379.98   20.01   30.0         0.023   -0.00   sai  -58.0°
B     440.08  100.03  -60.1         0.080   -0.08   sai  -57.9°
C     410.04  135.02   85.0         0.045   -0.01   sai   12.0°

Đổi đúng cách, sai số vị trí dưới 0,1 mm và sai số góc dưới 0,1°: cỡ phần dư hiệu chuẩn. Cột cuối là cách "cộng offset 91°" (ngay cả khi đã biết chính xác góc xoay giữa hai hệ): sai 12° đến 58° tùy chi tiết, vì phép lật làm góc ảnh tăng thì góc robot giảm. Lỗi này đặc biệt nguy hiểm vì nó có thể trông đúng với chi tiết đặt gần một góc nào đó lúc chạy thử, rồi sai khi chi tiết nằm xiên khác đi.

5. Băng tải đang chạy: bù chuyển động

Khi băng tải không dừng, chi tiết tiếp tục đi trong khoảng thời gian từ lúc chụp đến lúc robot tới nơi, thường 0,5–2 s, tức hàng trăm mm. Gọi hướng chạy của băng tải là vectơ đơn vị d trong hệ robot (lấy được khi hiệu chuẩn: đặt dấu, chụp, cho băng tải chạy, chụp lại). Vị trí chi tiết lúc gắp:

P_gắp = P_ảnh + d · s

  s = v · Δt                                   (không có encoder: tốc độ × thời gian)
  s = (E_gắp − E_chụp) · k                     (có encoder: k = mm băng tải mỗi xung)

  P_ảnh   : vị trí tìm được trong ảnh, đổi sang hệ robot
  Δt      : từ lúc PHƠI SÁNG đến lúc gắp (không phải từ lúc PC nhận ảnh)
  E_chụp  : giá trị encoder chốt đúng lúc trigger/phơi sáng

Cách tính bằng thời gian nghe đơn giản nhưng có hai nguồn sai: tốc độ thật lệch khỏi tốc độ danh định (tải, trượt, biến tần), và nhãn thời gian lệch so với lúc phơi sáng (ảnh truyền về PC mất vài chục ms, không cố định). Mô phỏng 1000 chi tiết dưới đây so sánh bốn cách:

import numpy as np

rng = np.random.default_rng(7)
N = 1000
V_DANH_DINH = 200.0                         # mm/s, tốc độ ghi trên bản vẽ
MM_MOI_XUNG = 0.02                          # độ phân giải encoder quy ra mm băng tải

v = rng.uniform(190, 210, N)                # tốc độ thật dao động ±5 % theo tải
tre = rng.uniform(0.030, 0.060, N)          # trễ từ lúc phơi sáng đến lúc PC gán nhãn thời gian
dt = rng.uniform(1.0, 1.5, N)               # từ lúc phơi sáng đến lúc robot gắp

x0 = rng.uniform(0, 1000, N)                # vị trí băng tải (mm) lúc phơi sáng
quang_duong_that = v * dt                   # chi tiết đã đi được bao xa khi robot gắp

cach = {
    "không bù":                           np.zeros(N),
    "v·Δt, nhãn thời gian của PC":        V_DANH_DINH * (dt - tre),
    "v·Δt, nhãn thời gian lúc phơi sáng": V_DANH_DINH * dt,
    "encoder chốt lúc trigger":           (np.floor((x0 + quang_duong_that) / MM_MOI_XUNG)
                                           - np.floor(x0 / MM_MOI_XUNG)) * MM_MOI_XUNG,
}
for ten, bu in cach.items():
    sai = np.abs(bu - quang_duong_that)
    print(f"{ten:<36}: sai trung bình {sai.mean():8.3f} mm, lớn nhất {sai.max():8.3f} mm")

không bù                            : sai trung bình  250.736 mm, lớn nhất  312.922 mm
v·Δt, nhãn thời gian của PC         : sai trung bình    9.542 mm, lớn nhất   25.038 mm
v·Δt, nhãn thời gian lúc phơi sáng  : sai trung bình    6.283 mm, lớn nhất   14.540 mm
encoder chốt lúc trigger            : sai trung bình    0.007 mm, lớn nhất    0.020 mm

Bù bằng thời gian, kể cả với nhãn thời gian đúng lúc phơi sáng, vẫn sai tới khoảng 15 mm vì tốc độ dao động ±5%. Encoder đo trực tiếp quãng đường nên sai số chỉ còn cỡ độ phân giải. Vì vậy hệ bám băng tải thực tế đều dùng encoder:

• Chốt encoder đúng lúc chụp. Tín hiệu trigger (hoặc ngõ strobe/exposure-active của camera) được đưa vào ngõ chốt (latch) của bộ điều khiển robot hoặc bộ đếm tốc độ cao của PLC. Đọc encoder bằng phần mềm "ngay sau khi chụp" sẽ mang theo sai số trễ như dòng thứ hai.
• Gửi E_chụp cùng tọa độ. Robot tự tính quãng đường đã chạy bằng encoder của nó; vision không cần biết khi nào robot gắp.
• Cửa sổ bám (tracking window). Robot chỉ gắp khi chi tiết nằm trong vùng từ "bắt đầu với tới" đến "sắp ra khỏi tầm"; chi tiết đi quá giới hạn trên thì bỏ qua (để trạm sau hoặc thùng hồi lưu xử lý). Thứ tự gắp: chi tiết đi trước (xa nhất theo chiều chạy) gắp trước.
• Chụp trùng. Nếu camera chụp liên tục theo quãng đường (ví dụ mỗi 100 mm băng tải với vùng nhìn 125 mm), một chi tiết xuất hiện trong hai ảnh liên tiếp. Phải loại trùng: quy chi tiết của hai ảnh về cùng một mốc encoder rồi bỏ những chi tiết cách nhau dưới vài mm.

Khi không có encoder (băng tải nhỏ, tốc độ rất ổn định), công thức P_ảnh + d · v · Δt vẫn dùng được nếu Δt tính từ lúc phơi sáng (dùng nhãn thời gian của camera hoặc thời điểm trigger) và tốc độ được đo lại định kỳ. Ở 200 mm/s, mỗi 1 ms sai lệch thời gian là 0,2 mm sai lệch vị trí.

6. Mô phỏng hoàn chỉnh: vision gửi tọa độ cho robot qua TCP

Bây giờ ghép tất cả (bài thực hành của bài học): lấy các chi tiết đã định vị ở mục 3, đổi sang hệ robot bằng ma trận từ hiệu chuẩn, kiểm tra vùng với tới, sắp thứ tự (chi tiết đi trước gắp trước), gửi cho một robot giả qua TCP trên 127.0.0.1. Định dạng tin mở rộng từ bài TCP socket bằng một trường encoder: seq,OK|NG,X,Y,góc,E_chụp\r\n. Robot giả bám băng tải: nó tự đọc "encoder" của nó lúc gắp và cộng quãng đường đã chạy, rồi so với vị trí thật của chi tiết để in độ lệch. Code dùng lại vat, VAT_THAT, M và px_sang_robot ở các mục trên; BoDemDong và tao_tin là bản rút gọn của bài TCP socket.

import socket
import threading
import numpy as np

V_BANG_TAI = 200.0               # mm/s, băng tải chạy theo +X của user frame
MM_MOI_XUNG = 0.02
ENC_LUC_CHUP = 150000            # giá trị encoder được chốt khi trigger chụp
VUNG_Y = (-30.0, 130.0)          # giới hạn với tới của robot theo Y (mm)


class BoDemDong:
    """Gom dòng byte thành từng tin kết thúc bằng CR LF (rút gọn từ bài TCP socket)."""

    def __init__(self):
        self.buf = bytearray()

    def nap(self, du_lieu):
        self.buf += du_lieu
        tin = []
        while (i := self.buf.find(b"\r\n")) >= 0:
            tin.append(bytes(self.buf[:i]).decode("ascii"))
            del self.buf[:i + 2]
        return tin


def tao_tin(seq, ok, x, y, goc, enc):
    return f"{seq:04d},{'OK' if ok else 'NG'},{x:.2f},{y:.2f},{goc:.2f},{enc}\r\n".encode("ascii")


class RobotGia:
    """Robot giả làm TCP server, bám băng tải: X_gắp = X_tin + (E_gắp − E_chụp) · mm/xung."""

    def __init__(self, port, vat_that):
        self.srv = socket.create_server(("127.0.0.1", port))
        self.srv.settimeout(3.0)
        self.vat_that, self.nhat_ky = vat_that, []
        self.t = threading.Thread(target=self._chay, daemon=True)
        self.t.start()

    def _chay(self):
        conn, _ = self.srv.accept()
        conn.settimeout(3.0)
        bd, k = BoDemDong(), 0
        with conn:
            while du_lieu := conn.recv(1024):
                for dong in bd.nap(du_lieu):
                    seq, kq, x, y, goc, enc = dong.split(",")
                    if kq == "OK":
                        k += 1
                        t_gap = 1.0 + 0.4 * k                  # robot tới chỗ chi tiết sau t_gap giây (giờ ảo)
                        enc_gap = ENC_LUC_CHUP + round(V_BANG_TAI * t_gap / MM_MOI_XUNG)
                        X = float(x) + (enc_gap - int(enc)) * MM_MOI_XUNG
                        Xt, Yt, _ = min(self.vat_that, key=lambda p: abs(p[1] - float(y)))
                        lech = np.hypot(X - (Xt + V_BANG_TAI * t_gap), float(y) - Yt)
                        self.nhat_ky.append(f"robot #{seq}: gắp tại X={X:.2f} Y={y} góc={goc}, "
                                            f"lệch so với chi tiết thật {lech:.3f} mm")
                    else:
                        self.nhat_ky.append(f"robot #{seq}: NG, không gắp")
                    conn.sendall(f"ACK,{seq}\r\n".encode("ascii"))
        self.srv.close()


# Đổi sang hệ robot, sắp xếp: X lớn (đi trước theo chiều chạy) gắp trước
muc_tieu = sorted((px_sang_robot(u, v_px, g, M) for u, v_px, g, _, _ in vat), key=lambda p: -p[0])

robot = RobotGia(6030, list(VAT_THAT.values()))
with socket.create_connection(("127.0.0.1", 6030), timeout=1.0) as s:
    bd = BoDemDong()
    for seq, (X, Y, goc) in enumerate(muc_tieu, start=1):
        ok = VUNG_Y[0] <= Y <= VUNG_Y[1]                   # ngoài tầm với: gửi NG, không để robot tự xoay xở
        s.sendall(tao_tin(seq, ok, X, Y, goc, ENC_LUC_CHUP))
        tin = []
        while not tin:                                     # chờ ACK, có timeout 1 s của socket
            du_lieu = s.recv(64)
            if not du_lieu:
                raise ConnectionError("robot đóng kết nối")
            tin = bd.nap(du_lieu)
        print(f"vision #{seq}: {'OK' if ok else 'NG (ngoài tầm với)'} X={X:.2f} Y={Y:.2f} góc={goc:.2f} → {tin[0]}")
robot.t.join(timeout=3)
print("\n".join(robot.nhat_ky))

vision #1: OK X=440.08 Y=100.03 góc=-60.08 → ACK,0001
vision #2: NG (ngoài tầm với) X=410.04 Y=135.02 góc=84.99 → ACK,0002
vision #3: OK X=379.98 Y=20.01 góc=30.00 → ACK,0003
robot #0001: gắp tại X=720.08 Y=100.03 góc=-60.08, lệch so với chi tiết thật 0.085 mm
robot #0002: NG, không gắp
robot #0003: gắp tại X=739.98 Y=20.01 góc=30.00, lệch so với chi tiết thật 0.022 mm

Chi tiết B (X lớn nhất) được gửi trước. Chi tiết C có Y = 135 mm, ngoài tầm với 130 mm, nên vision gửi NG thay vì để robot cố với tới (và báo lỗi vùng làm việc, dừng chương trình). Robot gắp ở X ≈ 720–740 mm, cách vị trí trong ảnh 280–360 mm (băng tải đã chạy 1,4–1,8 s), nhưng độ lệch so với chi tiết thật vẫn chỉ cỡ 0,02–0,09 mm: bù bằng encoder không thêm sai số đáng kể, phần còn lại là sai số hiệu chuẩn và định vị. Bản dùng thật cần thêm những gì bài TCP socket đã dạy: kiểm tra định dạng tin phía robot, timeout chờ ACK rồi kết nối lại và gửi lại, robot nhớ seq để không gắp hai lần.

7. Phân tích sai số: chỗ nào làm robot gắp trượt?

Khi khách hàng hỏi "hệ thống gắp chính xác bao nhiêu?", câu trả lời có căn cứ là một ngân sách sai số: liệt kê từng nguồn, ước lượng độ lớn, cộng lại. Với các nguồn độc lập, cộng theo căn tổng bình phương (RSS); cộng thẳng cho cận bi quan. Riêng sai số do độ cao thường lớn hơn tất cả các nguồn khác cộng lại: hiệu chuẩn 2D chỉ đúng ở mặt phẳng hiệu chuẩn; một điểm cao hơn mặt đó h mm, cách trục quang r mm, nhìn từ camera ở độ cao H sẽ bị "đẩy" ra xa trục một đoạn khoảng h · r / (H − h).

import numpy as np

# Ngân sách sai số vị trí gắp (mm, ước lượng 1 độ lệch chuẩn) cho trạm ở trên
nguon = {
    "hiệu chuẩn 9 điểm (sai số kiểm chứng)":  0.08,
    "tìm tâm chi tiết (~0,3 px × 0,25 mm/px)": 0.075,
    "encoder: chốt lệch ~1 ms ở 200 mm/s":     0.20,
    "độ lặp lại của robot":                    0.03,
    "chi tiết trượt trên băng tải":            0.20,
}
for ten, sigma in nguon.items():
    print(f"  {ten:<42} {sigma:5.3f}")
rss = np.sqrt(sum(s ** 2 for s in nguon.values()))
print(f"Tổng hợp RSS (độc lập): {rss:.3f} mm  → ±3σ ≈ ±{3 * rss:.2f} mm")
print(f"Cộng thẳng các σ (bi quan): {sum(nguon.values()):.3f} mm")

# Sai số thị sai khi mặt trên chi tiết cao hơn mặt phẳng hiệu chuẩn: Δ ≈ h · r / (H − h)
H = 800.0                                   # khoảng cách camera → mặt phẳng hiệu chuẩn (mm)
for h in (5.0, 25.0):
    for r in (50.0, 150.0):
        print(f"chi tiết cao h = {h:4.0f} mm, cách trục quang r = {r:5.0f} mm → lệch {h * r / (H - h):.2f} mm")

  hiệu chuẩn 9 điểm (sai số kiểm chứng)      0.080
  tìm tâm chi tiết (~0,3 px × 0,25 mm/px)    0.075
  encoder: chốt lệch ~1 ms ở 200 mm/s        0.200
  độ lặp lại của robot                       0.030
  chi tiết trượt trên băng tải               0.200
Tổng hợp RSS (độc lập): 0.305 mm  → ±3σ ≈ ±0.91 mm
Cộng thẳng các σ (bi quan): 0.585 mm
chi tiết cao h =    5 mm, cách trục quang r =    50 mm → lệch 0.31 mm
chi tiết cao h =    5 mm, cách trục quang r =   150 mm → lệch 0.94 mm
chi tiết cao h =   25 mm, cách trục quang r =    50 mm → lệch 1.61 mm
chi tiết cao h =   25 mm, cách trục quang r =   150 mm → lệch 4.84 mm

Với những con số trên, trạm đạt khoảng ±0,9 mm (3σ) nếu mặt chi tiết nằm đúng mặt phẳng hiệu chuẩn. Một chi tiết cao 25 mm ở rìa vùng nhìn lệch gần 5 mm, gấp năm lần mọi thứ khác. Cách xử lý, theo thứ tự ưu tiên: hiệu chuẩn ở đúng độ cao mặt gắp (đặt tấm hiệu chuẩn lên đế cao bằng chi tiết); nếu có nhiều loại chi tiết cao khác nhau, lưu một ma trận cho mỗi độ cao hoặc hiệu chỉnh theo tâm quang học; dùng ống kính telecentric khi vùng nhìn nhỏ; hoặc chuyển sang camera 3D khi độ cao thay đổi liên tục. So kết quả với dung sai gắp: ngón kẹp mở rộng hơn chi tiết 4 mm thì cho phép lệch khoảng ±2 mm; giác hút Ø10 mm trên mặt phẳng 20 mm thì rộng rãi hơn nhiều.

8. Lỗi thường gặp

9. Bài tập

for goc in (-170.0, 100.0, 46.0, 135.0):
    print(f"{goc:7.1f}° → kẹp 2 ngón (180°): {chuan_hoa_goc_kep(goc, 180):6.1f}°   "
          f"kẹp 4 ngón (90°): {chuan_hoa_goc_kep(goc, 90):6.1f}°")

 -170.0° → kẹp 2 ngón (180°):   10.0°   kẹp 4 ngón (90°):   10.0°
  100.0° → kẹp 2 ngón (180°):  -80.0°   kẹp 4 ngón (90°):   10.0°
   46.0° → kẹp 2 ngón (180°):   46.0°   kẹp 4 ngón (90°):  -44.0°
  135.0° → kẹp 2 ngón (180°):  -45.0°   kẹp 4 ngón (90°):  -45.0°

Tóm tắt

• Hệ pick and place: camera + đèn, vision tìm (u, v, góc), hiệu chuẩn pixel → robot, kênh truyền có ACK, robot (bám băng tải bằng encoder). Vision lọc chi tiết không gắp được và báo NG thay vì gửi tọa độ "gần đúng".
• Góc trong ảnh: tự tính từ boxPoints theo cạnh dài, không dựa vào quy ước góc của minAreaRect.
• Đổi sang robot: tâm áp affine; góc biến đổi vectơ hướng bằng phần 2 × 2 của M (đúng cả khi có phép lật). Chuẩn hóa góc theo đối xứng tay kẹp (180° với kẹp hai ngón).
• Băng tải chạy: P_gắp = P_ảnh + d · (E_gắp − E_chụp) · k; chốt encoder bằng phần cứng lúc trigger; bù theo thời gian chỉ dùng khi tốc độ ổn định và nhãn thời gian đúng lúc phơi sáng.
• Ngân sách sai số: cộng RSS các nguồn; sai số độ cao h · r / (H − h) thường lớn nhất, nên hiệu chuẩn ở đúng độ cao gắp và kiểm chứng định kỳ.

Bài tiếp theo: MES và truy xuất nguồn gốc, nơi kết quả của trạm được gửi lên hệ thống điều hành sản xuất mà không mất bản ghi nào khi mạng gián đoạn.