Đồ án ứng dụng PLC S7-1200 điều khiển robot Delta phân loại sản phẩm theo màu - ĐH SPKT TPHCM

Đồ án ứng dụng PLC điều khiển robot delta phân loại sản phẩm theo màu sắc. Bản vẽ thiết kế, lưu đồ thuật toán và code điều khiển chi tiết.

• Tác giả: Phùng Huỳnh Minh Tuấn
• Người hướng dẫn: TS. Trần Vi Đô
• Trường học: Trường Đại học Sư phạm Kỹ thuật TP. Hồ Chí Minh
• Chuyên ngành: Điện công nghiệp
• Đề tài: Ứng dụng PLC điều khiển robot Delta phân loại sản phẩm theo màu
• Loại tài liệu: Đồ án tốt nghiệp
• Năm xuất bản: 2021
• Địa điểm: TP. Hồ Chí Minh

Tóm tắt

I. Tổng quan đề tài PLC điều khiển robot delta phân loại theo màu

Đề tài ứng dụng PLC điều khiển robot delta phân loại sản phẩm theo màu là một nghiên cứu thực tiễn trong ngành tự động hóa. Đồ án kết hợp nhiều công nghệ then chốt bao gồm PLC S7-1200, robot delta 3 bậc tự do và camera công nghiệp. Mục tiêu chính là thay thế quy trình phân loại thủ công kém hiệu quả bằng hệ thống robot tốc độ cao. Dây chuyền hoạt động liên tục dựa trên tín hiệu thị giác máy tính và giải thuật điều khiển chuyển động. Sản phẩm di chuyển trên băng tải được camera ghi lại hình ảnh sắc nét. Phần mềm xử lý ảnh phân tích không gian màu để trích xuất tọa độ phôi. PLC tiếp nhận dữ liệu vị trí và phát xung điều khiển các động cơ servo hoặc bước. Tay gắp robot delta di chuyển chính xác đến tọa độ mục tiêu để gắp vật thể. Cơ cấu hút chân không thực hiện thao tác nhặt và thả vào ô chứa tương ứng. Mô hình này giúp nâng cao năng suất đóng gói và giảm thiểu tỷ lệ lỗi sản phẩm trong nhà máy sản xuất hiện đại. Toàn bộ chu trình vận hành diễn ra hoàn toàn tự động, khép kín và đạt độ ổn định cao.

1.1. Cấu trúc phần cứng của hệ thống robot delta và PLC S7 1200

Hệ thống phần cứng bao gồm bộ điều khiển trung tâm PLC Siemens S7-1200 CPU 1212C DC/DC/DC cùng module mở rộng SM 1222. Robot delta có cấu trúc cơ khí song song ba bậc tự do đối xứng không gian 120 độ. Ba động cơ chấp hành kết nối với các cánh tay chủ động và bị động để dẫn động đế chấp hành cuối. Bộ giác hút chân không gắn tại đầu công tác làm nhiệm vụ gắp nhả phôi liệu. Băng tải cấp phôi vận hành bằng động cơ giảm tốc DC được tích hợp cảm biến phát hiện quang điện. Tủ điện trung tâm chứa nguồn 24VDC, driver điều khiển động cơ và rơ le trung gian bảo đảm an toàn cho toàn bộ thiết bị.

1.2. Vai trò của công nghệ xử lý ảnh trong phân loại sản phẩm

Thị giác máy tính đóng vai trò là mắt thần của dây chuyền phân loại tự động. Camera độ phân giải 5MP lắp đặt cố định phía trên băng tải để ghi hình ảnh liên tục. Khung hình thu nhận được chuyển đổi sang không gian màu HSV nhằm tách biệt ngưỡng màu sắc chuẩn xác. Thuật toán phát hiện biên và tìm tâm hình học xác định vị trí thực tế của từng sản phẩm. Dữ liệu tọa độ tâm vật thể được tính toán và truyền thông trực tiếp sang PLC qua giao thức công nghiệp Ethernet TCP/IP. Quá trình xử lý ảnh giúp loại bỏ sai số nhận dạng do điều kiện ánh sáng môi trường thay đổi.

II. Phân tích thách thức khi ứng dụng PLC điều khiển robot delta

Quá trình thiết kế mô hình robot delta phân loại sản phẩm đặt ra nhiều thách thức kỹ thuật phức tạp. Robot delta có cấu trúc động học phi tuyến tính cao đòi hỏi thuật toán điều khiển chính xác. Việc tính toán động học nghịch đảo phải xử lý các ma trận góc xoay 120 độ giữa ba trục khớp. Thời gian đáp ứng của hệ thống bị giới hạn bởi tốc độ băng tải và độ trễ xử lý ảnh. Nếu camera truyền tọa độ chậm, tay máy sẽ gắp trượt vị trí vật thể đang di chuyển. Sự thay đổi ánh sáng ngoại cảnh dễ làm sai lệch kết quả phân đoạn màu trong thuật toán thị giác. Độ rung lắc cơ khí khi robot di chuyển với gia tốc lớn ảnh hưởng tiêu cực đến độ chính xác điểm đặt. Ngoài ra, việc đồng bộ hóa xung điều khiển giữa PLC và các driver động cơ đòi hỏi cấu hình công nghệ chuyển động tối ưu. Hiện tượng trôi bước hoặc giật cục làm giảm tuổi thọ của khớp cầu cơ cấu tay máy. Cân bằng giữa tốc độ phân loại và độ chính xác gắp nhặt là bài toán quan trọng cần giải quyết triệt để.

2.1. Khó khăn trong giải thuật động học nghịch robot delta

Động học nghịch đảo là cơ sở để chuyển đổi tọa độ Descartes không gian thành góc quay ba trục động cơ. Khác với robot chuỗi hở, robot delta sở hữu mạch động học song song khép kín. Mỗi cánh tay chịu ràng buộc hình học lẫn nhau trong hệ quy chiếu gốc tọa độ cố định. Phương trình hình học giao điểm giữa mặt cầu và đường tròn tạo ra các nghiệm kép phức tạp. Thuật toán đòi hỏi phép biến đổi ma trận xoay góc liên tục để xác định chính xác vị trí đế di động. Sai số cơ khí khớp cầu và độ rơ trục vít làm gia tăng độ lệch vị trí trong thực tế.

2.2. Vấn đề trễ truyền thông và nhận diện màu sắc môi trường

Hệ thống thị giác máy tính đối mặt với hiện tượng nhiễu quang học khi cường độ sáng thay đổi bất thường. Không gian màu RGB truyền thống rất nhạy cảm với ánh sáng chói, gây sai sót khi nhận diện màu đỏ, xanh lá hay xanh lam. Bên cạnh đó, độ trễ truyền dữ liệu qua giao tiếp mạng giữa máy tính xử lý ảnh và PLC S7-1200 làm sai lệch thời điểm kích hoạt gắp. Khi phôi trôi nhanh trên băng tải, sai lệch vài mili-giây có thể dẫn đến gắp hỏng. Vấn đề này đòi hỏi giải pháp khử nhiễu ảnh và tối ưu hóa thời gian quét chu kỳ PLC.

III. Giải pháp PLC điều khiển robot delta phân loại màu hiệu quả

Giải pháp hoàn thiện hệ thống phân loại tập trung vào việc tối ưu thuật toán điều khiển và tích hợp module phần mềm. PLC Siemens S7-1200 sử dụng công nghệ Technology Object Motion Control với các hàm chuyên dụng MC_Power, MC_MoveAbsolute và MC_Home. Giao tiếp Ethernet truyền thông điệp dữ liệu nhịp nhàng giữa máy tính xử lý ảnh và khối bộ nhớ DB của PLC. Khối lệnh điều khiển chuyển động hỗ trợ cấu hình đường cong chữ S, giới hạn độ giật Jerk Limit để tay máy di chuyển êm ái. Chương trình xử lý ảnh trên máy tính áp dụng kỹ thuật lọc trung vị và chuyển đổi sang không gian màu HSV. Ngưỡng màu của ba loại phôi đỏ, lục, lam được hiệu chuẩn cố định chống nhiễu quang học. Tọa độ tâm vật thể được tính toán bằng phương pháp tìm moment hình học và bù trừ vận tốc trôi của băng tải. Khi phôi đi qua vùng quét, PLC tự động lập lịch quỹ đạo chuyển động tối ưu cho robot delta. Cơ cấu hút chân không điện từ kích hoạt đúng thời điểm vật thể chạm vị trí gắp chỉ định.

3.1. Cấu hình Technology Object và khối hàm Motion Control

Phần mềm TIA Portal hỗ trợ tạo các đối tượng công nghệ Technology Object dạng trục phát xung PTO cho PLC S7-1200. Kỹ sư thiết lập các thông số động học như vận tốc tối đa, gia tốc tăng giảm và thời gian hãm. Khối hàm MC_Power cấp nguồn kích hoạt các trục động cơ sẵn sàng hoạt động. Khối lệnh MC_Home định vị điểm gốc tọa độ ban đầu tuyệt đối hoặc tương đối một cách chuẩn xác. Khi nhận tọa độ từ camera, hàm MC_MoveAbsolute điều khiển vị trí đích đến của từng khớp quay. Chức năng làm mịn Jerk Limit triệt tiêu xung lực giật, bảo vệ cơ cấu chấp hành song song.

3.2. Thuật toán xử lý ảnh HSV và truyền thông liên kết PLC

Chương trình thị giác thu hình ảnh từ webcam 5MP và thực hiện tiền xử lý làm mờ Gaussian. Ảnh được chuyển từ không gian RGB sang HSV để phân tách độc lập kênh sắc độ và độ bão hòa. Thuật toán tìm đường viền bao ngoài và tính toán trọng tâm đối tượng theo hệ trục pixel. Hệ số tỉ lệ thực tế được dùng để chuyển đổi pixel sang milimet trong không gian làm việc của robot. Dữ liệu tọa độ và mã màu được đóng gói thành chuỗi byte gửi qua socket TCP/IP đến thanh ghi dữ liệu PLC S7-1200 tức thời.

IV. Đánh giá kết quả và tiềm năng ứng dụng robot delta phân loại

Đồ án ứng dụng PLC điều khiển robot delta phân loại sản phẩm theo màu đã chứng minh tính khả thi và hiệu quả vượt trội. Mô hình thực nghiệm vận hành ổn định với độ chính xác phân loại đạt trên 95% trong điều kiện tiêu chuẩn. Tay máy delta phản hồi linh hoạt, bám bắt vật thể di chuyển mượt mà trên băng chuyền. Hệ thống loại bỏ hoàn toàn sự can thiệp thủ công trong khâu phân loại màu sắc cơ bản. Sự kết hợp giữa bộ điều khiển lập trình PLC S7-1200 và thị giác máy tính mở ra giải pháp tự động hóa tin cậy. Kết quả đồ án cung cấp tài liệu nghiên cứu chuyên sâu về động học song song và kỹ thuật điều khiển phát xung vị trí. Mô hình dễ dàng nâng cấp mở rộng với nhiều màu sắc và kích thước vật thể khác nhau. Đây là nền tảng vững chắc cho sinh viên ngành Điện - Điện tử tiếp cận công nghệ tự động hóa hiện đại. Thiết kế đáp ứng tốt nhu cầu chuyển đổi số và nâng cao năng suất cho các doanh nghiệp sản xuất.

4.1. Khả năng ứng dụng thực tiễn trong các ngành công nghiệp

Mô hình robot delta phân loại tốc độ cao có phạm vi ứng dụng rộng rãi trong nhiều dây chuyền sản xuất công nghiệp. Ngành chế biến thực phẩm và bánh kẹo ứng dụng tay máy delta để chọn lọc và đóng gói sản phẩm theo màu sắc quy chuẩn. Ngành dược phẩm sử dụng hệ thống để phân loại vỉ thuốc hoặc bao bì y tế đạt chuẩn chất lượng. Trong sản xuất linh kiện điện tử, robot hỗ trợ gắp đặt các chi tiết vi mạch nhỏ với độ chính xác cao. Việc thay thế nhân công tại các khâu lặp đi lặp lại giúp doanh nghiệp tối ưu chi phí vận hành và tăng năng suất.

4.2. Hướng phát triển và nâng cấp hệ thống trong tương lai

Hệ thống có thể phát triển theo hướng tích hợp thêm các thuật toán học máy Deep Learning như YOLO để nhận diện đa dạng hình dạng và khuyết tật. Nâng cấp camera công nghiệp tốc độ khung hình cao giúp cải thiện đáng kể năng suất phân loại trên băng chuyền siêu tốc. Việc chuyển đổi từ động cơ bước sang động cơ AC Servo kèm encoder phản hồi tuyệt đối nâng cao độ chính xác gắp nhả. Bên cạnh đó, bổ sung giao diện giám sát SCADA hoặc màn hình HMI giúp người vận hành theo dõi trạng thái lỗi và số lượng sản phẩm theo thời gian thực thuận tiện.

Nội dung chính

Bối cảnh và vấn đề nghiên cứu

Trong xu thế phát triển của tự động hóa sản xuất hiện đại, đặc biệt sau bối cảnh đại dịch COVID-19, nhu cầu ứng dụng robot công nghiệp nhằm thay thế lao động thủ công, bảo vệ an toàn cho công nhân và nâng cao năng suất, độ chính xác trên các dây chuyền đóng gói ngày càng gia tăng. Bên cạnh các dòng cánh tay robot nối tiếp truyền thống như SCARA hay robot khớp bản lề nhiều bậc tự do, robot song song Delta (Delta Robot) sở hữu những ưu thế vượt trội về tốc độ vận hành, gia tốc lớn và độ chính xác lặp lại cao nhờ cấu trúc động học vòng kín đặc thù. Nhằm giải quyết bài toán phân loại sản phẩm tự động trong công nghiệp với chi phí tối ưu, đồ án tập trung nghiên cứu giải pháp tích hợp giữa bộ điều khiển lập trình logic (PLC) và hệ thống thị giác máy tính.

Mục tiêu và nhiệm vụ nghiên cứu của đề tài bao gồm:

1. Tìm hiểu cấu tạo, cấu trúc cơ bản của Robot Delta, xây dựng mô hình toán học và giải bài toán động học thuận - nghịch ứng dụng vào phân loại sản phẩm.
2. Xây dựng phần mềm điều khiển Robot Delta trên máy tính với 2 chế độ vận hành độc lập: Tự động (Auto) và Bằng tay (Manual).
3. Thiết lập hệ thống nhận dạng đối tượng bằng Web Camera 5MP, trích xuất dữ liệu tọa độ, màu sắc và kích thước của vật mẫu gửi về máy tính xử lý.
4. Thiết kế giải thuật tính toán động học trên máy tính để truyền tham số vị trí xuống PLC điều khiển 3 động cơ bước chấp hành điều hướng cánh tay robot gắp và thả vật vào đúng vị trí quy định.
5. Thi công mô hình cơ khí bằng công nghệ in 3D, lắp ráp tủ điện điều khiển và hoàn thiện hệ thống phân loại hoàn chỉnh gồm robot, băng tải và cơ cấu gắp hút chân không.

Quy trình triển khai đồ án gồm 7 bước:

• Bước 1: Xây dựng chương trình tính toán động lực học thuận và nghịch của Robot Delta.
• Bước 2: Cấu hình PLC S7-1200 để điều khiển đồng thời nhiều động cơ bước thông qua phần mềm TIA Portal V15.
• Bước 3: Xây dựng giao diện điều khiển WinForms trên C# gồm các khối: Kết nối PC - PLC, chế độ Manual và chế độ Auto.
• Bước 4: Thiết kế 3D bằng Autodesk Fusion 360 và gia công chi tiết cơ khí bằng công nghệ in 3D.
• Bước 5: Tích hợp phương trình động học vào phần mềm, thiết lập truyền thông giữa PC và PLC thông qua OPC KepServer, thử nghiệm chế độ Manual.
• Bước 6: Nhúng thư viện xử lý ảnh EmguCV, kết nối Web Camera, lập trình giải thuật nhận diện màu sắc, tọa độ vật mẫu và vận hành chế độ Auto.
• Bước 7: Vận hành, hiệu chỉnh đồng bộ phần cứng và phần mềm.

Đối tượng nghiên cứu là mô hình Robot Delta 3 bậc tự do kết hợp hệ thống thị giác máy tính và bộ điều khiển PLC Siemens S7-1200. Phạm vi nghiên cứu thực nghiệm được thực hiện trên mô hình sản phẩm mẫu dạng khối hình học với 3 nhóm màu cơ bản: Đỏ (Red), Xanh lục (Green) và Xanh lam (Blue) di chuyển trên băng tải.

Cơ sở lý thuyết và phương pháp

Cơ sở lý thuyết động học Robot Delta

Cấu tạo hình học của Robot Delta trong đề tài gồm một bàn máy cố định $F$, một bàn máy động $E$ và ba nhánh chuyển động độc lập. Mỗi nhánh gồm khâu chủ động $F_iJ_i$ liên kết khớp quay với bàn máy cố định (được dẫn động bởi một động cơ bước) và khâu bị động $J_iE_i$ có cấu trúc hình bình hành liên kết khớp cầu. Nhờ tính chất hình học này, bàn máy động luôn duy trì hướng song song với bàn máy cố định và chỉ chuyển động tịnh tiến 3 bậc tự do ($X, Y, Z$) trong không gian công tác.

• Động học thuận: Xác định tọa độ vị trí của bàn máy động $E_0(x_0, y_0, z_0)$ từ 3 góc quay của khớp chủ động $(\theta_1, \theta_2, \theta_3)$. Bài toán được giải bằng cách xác định giao điểm của 3 mặt cầu có bán kính $r_e$ (chiều dài khâu bị động), với tâm đặt tại các điểm $J'_1, J'_2, J'_3$ (vị trí khớp sau phép tịnh tiến tương ứng): $$(x - x_j)^2 + (y - y_j)^2 + (z - z_j)^2 = r_e^2 \quad (j = 1, 2, 3)$$ Bằng phương pháp đại số, hệ 3 phương trình bậc hai được chuyển đổi về dạng tuyến tính theo $z$: $x = a_1z + b_1$ và $y = a_2z + b_2$, từ đó giải phương trình bậc hai tìm nghiệm $z_0$ (chọn nghiệm âm nhỏ nhất) và suy ra $x_0, y_0$.

• Động học nghịch: Xác định các góc quay khớp $(\theta_1, \theta_2, \theta_3)$ khi biết trước tọa độ đích $E_0(x_0, y_0, z_0)$. Do khâu $F_1J_1$ chỉ chuyển động trong mặt phẳng $YZ$, tọa độ điểm $J_1$ được xác định qua giao điểm của hai đường tròn tâm $F_1$ bán kính $r_f$ và tâm $E'1$ bán kính $\sqrt{r_e^2 - x_0^2}$. Góc quay khớp thứ nhất được tính bằng: $$\theta_1 = \arctan\left(\frac{z{J1}}{y_{F1} - y_{J1}}\right)$$ Đối với các góc $\theta_2$ và $\theta_3$, hệ thống áp dụng phép quay hệ trục tọa độ trong mặt phẳng $XY$ quanh trục $Z$ các góc lần lượt $+120^\circ$ và $-120^\circ$ để tái sử dụng cùng thuật toán giải tích của $\theta_1$.

Cơ sở lý thuyết xử lý ảnh số

Hệ thống thị giác sử dụng camera thu nhận khung hình 8-bit (256 mức xám trên mỗi kênh) và chuyển đổi từ không gian màu RGB sang không gian màu HSV ($H \in [0, 255]$, $S \in [0, 255]$, $V \in [0, 255]$) nhằm hạn chế ảnh hưởng của điều kiện chiếu sáng biến thiên. Quy trình xử lý ảnh gồm:

1. Phân ngưỡng nhị phân: Sử dụng hàm InRange dựa trên các dải giá trị $H, S, V$ định trước của 3 màu đỏ, xanh lục, xanh lam để tách đối tượng khỏi nền (background).
2. Hình thái học (Morphology): Áp dụng 2 lần phép toán co ảnh (Erosion) với phần tử cấu trúc kích thước $8 \times 8$ nhằm triệt tiêu các điểm nhiễu hạt và tách rời các biên dính, sau đó áp dụng 2 lần phép toán giãn ảnh (Dilation) để phục hồi kích thước nguyên bản của vật thể.
3. Trích xuất đường bao (Contours): Sử dụng hàm findContours với chế độ phân cấp RETR_CCOMP và phương pháp nén điểm chainApproxSimple.
4. Thuật toán xấp xỉ Approximation Contour: Sử dụng thuật toán đệ quy Ramer-Douglas-Peucker với ngưỡng sai số $\varepsilon > 0$ để làm mịn đường biên và phân loại dạng hình học cơ bản của vật thể.
5. Xác định tâm và diện tích bằng Image Moments: Tính toán các giá trị moment không gian $M_{00}$ (diện tích), $M_{10}$ và $M_{01}$ để trích xuất tọa độ tâm đối tượng trên ảnh: $$C_x = \frac{M_{10}}{M_{00}}, \quad C_y = \frac{M_{01}}{M_{00}}$$

Phương pháp nghiên cứu và nguồn dữ liệu

Đề tài sử dụng kết hợp phương pháp nghiên cứu lý thuyết động học giải tích, phương pháp mô hình hóa kỹ thuật (sử dụng phần mềm CAD 3D Fusion 360 để thiết kế khung vỏ và khớp cầu), phương pháp lập trình điều khiển chuyển động công nghiệp (Motion Control PLC) và phương pháp thực nghiệm kiểm chứng. Dữ liệu thực nghiệm được thu thập trực tiếp từ Web Camera 5MP, cảm biến vị trí Encoder gắn trên trục băng tải và các tín hiệu phát xung tốc độ cao PTO từ PLC S7-1200.

Thiết kế và triển khai

Hệ thống được thiết kế theo cấu trúc điều khiển phân tán gồm tầng giám sát/xử lý ảnh cấp cao (PC) và tầng điều khiển thời gian thực cấp chấp hành (PLC).

[Web Camera 5MP] ----> [PC / C# WinForms (EmguCV)]
                               |
                               | (OPC KepServer / Ethernet)
                               v
                       [PLC S7-1200 CPU 1212C]
                               |
       +-----------------------+-----------------------+
       | (Xung PTO)            | (Relay 24V)           | (HSC)
       v                       v                       ^
[3x Driver TB6600]     [Module SM 1222]          [Encoder Băng tải]
       |                       |
       v                       +--> [Bơm hút chân không]
[3x Động cơ bước Robot]        +--> [Động cơ băng tải]

Danh mục thiết bị phần cứng

Thiết bị / Linh kiện	Thông số kỹ thuật chính	Vai trò trong hệ thống
PLC S7-1200 CPU 1212C DC/DC/DC	Nguồn 24VDC, tích hợp I/O tốc độ cao, hỗ trợ phát xung PTO lên đến 100 kHz, cổng PROFINET	Bộ điều khiển trung tâm, phát xung điều khiển vị trí/vận tốc 3 trục robot
Module mở rộng SM 1222	8 ngõ ra Relay (DQ 8xRelay/2A, 6ES7222-1HF32-0XB0), tải định mức 24VDC/2A	Đóng ngắt van đảo chiều, bơm hút chân không và động cơ kéo băng tải
Driver TB6600	Điện áp hoạt động 9V - 42VDC, dòng tải cực đại 4A, ngõ vào cách ly quang, hỗ trợ vi bước	Khuếch đại công suất và điều khiển bước cho 3 động cơ cánh tay
Động cơ bước (3 bộ)	Động cơ bước 2 pha, điều khiển vị trí góc quay thông qua chuỗi xung rời rạc	Dẫn động trực tiếp 3 khớp quay chủ động $F_1, F_2, F_3$ của Robot Delta
Web Camera	Độ phân giải 5 Megapixel, giao tiếp USB	Thu nhận hình ảnh bề mặt băng tải để trích xuất màu sắc và vị trí
Encoder	Cảm biến đo dịch chuyển dạng quang học, tạo xung vuông	Đo tốc độ và quãng đường di chuyển của băng tải, phản hồi về bộ đếm HSC của PLC
Công tắc hành trình KW11	Công tắc cơ khí tiếp điểm đóng/cắt	Xác định vị trí giới hạn hành trình và điểm gốc (Home) của 3 cánh tay
Cơ cấu gắp	Bơm hút chân không 12V/24V	Tạo áp suất âm để hút giữ và nhả sản phẩm mẫu tại khâu chấp hành

Thiết lập Motion Control trên TIA Portal V15

Hệ thống điều khiển phát xung cho 3 trục động cơ bước được cấu hình thông qua đối tượng công nghệ (Technology Object - TO) của Siemens:

• Ngõ ra xung PTO (Pulse Train Output): Cấu hình tần số phát xung lên đến 100 kHz trên các chân ngõ ra số nhanh (ví dụ: Q0.0 phát xung xung PUL, Q0.1 xuất tín hiệu đảo chiều DIR).
• Cấu hình động lực học (Dynamics): Thiết lập giới hạn vận tốc tối đa, gia tốc tăng tốc (Ramp-up time), gia tốc giảm tốc (Ramp-down time) và hàm làm mịn chuyển động Jerk limit để triệt tiêu hiện tượng rung lắc cơ khí khi đổi hướng đột ngột.
• Khối hàm điều khiển chuẩn PLCopen:
  • MC_Power: Kích hoạt/ngắt nguồn điều khiển trục.
  • MC_Reset: Xóa trạng thái lỗi phát sinh trên Technology Object.
  • MC_Home: Thực hiện quy trình tìm điểm gốc tọa độ cho 3 cánh tay robot qua công tắc hành trình KW11 (chế độ Active/Passive Homing).
  • MC_Halt: Dừng chuyển động trục có kiểm soát gia tốc giảm.
  • MC_MoveAbsolute / MC_MoveRelative: Phát xung điều khiển robot dịch chuyển đến vị trí tọa độ tuyệt đối hoặc tương đối.
  • MC_MoveJog: Điều khiển chuyển động nhích từng bước phục vụ việc căn chỉnh chế độ Manual.

Giao thức truyền thông và phần mềm máy tính

Ứng dụng trên máy tính được xây dựng bằng ngôn ngữ C# (.NET WinForms) tích hợp thư viện EmguCV. Dữ liệu trao đổi giữa WinForms, hệ quản trị cơ sở dữ liệu SQL Server và PLC S7-1200 được thực hiện qua máy chủ trung gian OPC KepServer theo chuẩn giao thức Ethernet/PROFINET. Phần mềm WinForms tính toán ma trận chuyển đổi tọa độ từ hệ quy chiếu camera (pixel) sang hệ quy chiếu làm việc thực của robot (mm), giải phương trình động học nghịch và ghi dữ liệu góc quay xuống các biến nhớ tương ứng trên PLC.

Nội dung chính theo từng chương

Chương 1: Tổng quan về đề tài

Chương 1 giới thiệu tổng quan lịch sử hình thành cấu trúc robot song song từ năm 1962 bởi Gough và Whitehall, phân tích các ứng dụng thực tế của Robot Delta trong công nghiệp (đóng gói sữa Vinamilk), y tế (hỗ trợ phẫu thuật) và nông nghiệp tự động (diệt cỏ, bắt sâu). Tác giả xác định rõ mục tiêu, nhiệm vụ kỹ thuật của đồ án, đồng thời vạch ra lịch trình 7 bước tiến hành từ khâu tính toán lý thuyết, thiết kế 3D, lập trình PLC đến hiệu chỉnh hoàn thiện mô hình.

Chương 2: Cơ sở lý thuyết

Chương 2 thiết lập toàn bộ nền tảng toán học và phần cứng của đồ án:

• Trình bày chi tiết hệ phương trình vi phân hình học mô tả bài toán động học thuận (giao điểm 3 mặt cầu) và động học nghịch (chiếu phẳng và xoay ma trận $120^\circ$) của Robot Delta 3 bậc tự do.
• Tổng hợp lý thuyết xử lý ảnh số: Không gian màu HSV, phương pháp tách biên Sobel/Laplacian, các toán tử hình thái học (Erosion/Dilation với ma trận $8 \times 8$), thuật toán trích xuất đường bao Contours (RETR_CCOMP) và thuật toán xấp xỉ đa giác Ramer-Douglas-Peucker.
• Trình bày cấu tạo, thông số kỹ thuật và sơ đồ chân của phần cứng: PLC S7-1200 (CPU 1212C DC/DC/DC), module mở rộng SM 1222 (DQ 8xRelay/2A), Driver động cơ bước TB6600, công tắc hành trình KW11, cảm biến đo tốc độ Encoder.
• Phân tích cấu trúc thư viện EmguCV trên C#, cơ chế phát xung PTO trong TIA Portal V15, cấu hình Technology Object, các khối hàm Motion Control và giao thức kết nối OPC KepServer.

Chương 3: Thuật toán điều khiển

Chương 3 trình bày cấu trúc giải thuật điều khiển toàn hệ thống:

• Lưu đồ thuật toán chính của PLC xử lý các tín hiệu START, STOP, RESET từ giao diện WinForms, chương trình con đọc tín hiệu bộ đếm tốc độ cao (HSC) từ Encoder để tính vận tốc băng tải.
• Cấu trúc giao diện WinForms gồm 4 phân hệ màn hình: Màn hình kết nối truyền thông PC-PLC-SQL, Màn hình điều khiển Manual, Màn hình tự động Auto và Màn hình cài đặt Setting.
• Giải thuật xử lý ảnh: Quy trình lọc ảnh HSV, nhị phân hóa bằng hàm InRange, khử nhiễu 2 lần co - 2 lần giãn, trích xuất biên findContours, tính tâm bằng Image Moments ($C_x, C_y$) và thuật toán sắp xếp thứ tự các vật thể liên tục xuất hiện trên băng tải.
• Thuật toán chuyển đổi tọa độ giữa mặt phẳng ảnh của Camera và không gian làm việc của Robot Delta.

Chương 4: Thi công mô hình

Chương 4 mô tả chi tiết quá trình chế tạo và tích hợp phần cứng - phần mềm:

• Quá trình thiết kế mô hình 3D trên phần mềm Autodesk Fusion 360 và xuất file in 3D các chi tiết giá đỡ, cánh tay đòn, đế gá động cơ và bàn máy động.
• Thi công lắp ráp tủ điện điều khiển sử dụng hai đường nguồn độc lập 24VDC và 12VDC, sơ đồ đấu dây chi tiết giữa PLC S7-1200, module SM 1222, 3 bộ Driver TB6600, 3 động cơ bước, công tắc hành trình và Encoder.
• Lắp ráp cơ cấu chấp hành băng tải và đầu hút chân không.
• Xây dựng hoàn chỉnh giao diện điều khiển WinForms và nạp chương trình điều khiển xuống CPU 1212C qua TIA Portal V15.

Chương 5: Đánh giá kết quả và nhận xét

Chương 5 công bố các kết quả thực nghiệm thu được từ mô hình thực tế:

• Đánh giá khả năng xử lý ảnh: Hệ thống nhận diện chính xác vùng biên, xác định tâm đối tượng và phân loại đúng 3 mã màu mẫu (Đỏ, Xanh lục, Xanh lam).
• Đánh giá khả năng gắp/thả sản phẩm của Robot Delta ở cả hai chế độ Manual và Auto.
• Tổng kết các mục tiêu kỹ thuật đã làm chủ, chỉ ra các mặt hạn chế còn tồn tại về độ trễ và độ cứng vững cơ khí, từ đó đề xuất các hướng phát triển tiếp theo của đề tài.

Kết quả và đóng góp

Kết quả thực nghiệm

Mô hình thực tế đã vận hành đồng bộ giữa hệ thống thị giác máy tính và cơ cấu cơ điện tử. Robot Delta thực hiện đúng chu trình: Camera nhận diện màu sắc và tính tọa độ $\rightarrow$ gửi dữ liệu về PC $\rightarrow$ PC tính động học nghịch $\rightarrow$ chuyển giá trị góc xuống PLC qua OPC KepServer $\rightarrow$ PLC phát xung điều khiển 3 động cơ bước đưa đầu hút xuống gắp vật trên băng tải và phân loại vào các ô chứa tương ứng.

Tiêu chí thực nghiệm	Kết quả ghi nhận trong văn bản
Nhận diện màu sắc	Phân loại chính xác 3 màu mẫu: Đỏ, Xanh dương (Xanh lam) và Xanh lá (Xanh lục) thông qua dải màu HSV
Xử lý hình thái học	Khử nhiễu nền thành công sau 2 lần co (Erosion) và 2 lần giãn (Dilation) với kernel $8 \times 8$
Xác định tọa độ vật	Trích xuất chính xác tâm vật $C_x, C_y$ qua hàm Moments và quy đổi thành công sang tọa độ không gian Robot
Điều khiển vị trí Robot	Điều khiển chính xác 3 cánh tay đòn bằng xung PTO thông qua cấu hình Technology Object trên TIA Portal V15
Chế độ vận hành	Vận hành ổn định ở cả 2 chế độ: Điều khiển bằng tay (Manual) và Điều khiển tự động phân loại (Auto)

Đóng góp kỹ thuật của đề tài

1. Ứng dụng thành công cấu trúc điều khiển chuyển động bằng đối tượng công nghệ (Technology Object) và hàm chuẩn PLCopen trên dòng PLC cỡ nhỏ S7-1200 để điều khiển cơ cấu robot song song 3 bậc tự do phức tạp.
2. Xây dựng giải pháp tích hợp thị giác máy tính giá thành thấp sử dụng Web Camera phổ thông và thư viện EmguCV trên C# thay thế cho các hệ thống Vision chuyên dụng đắt tiền trong công nghiệp.
3. Thiết lập giải pháp truyền thông liên tầng khép kín giữa phần mềm thị giác trên PC, máy chủ OPC KepServer và PLC công nghiệp, đảm bảo tính mở và khả năng giám sát dữ liệu sản xuất trực tiếp qua phần mềm máy tính.

Hạn chế và hướng nghiên cứu tiếp

Hạn chế

• Hệ thống cơ khí sử dụng vật liệu in 3D nên độ cứng vững chưa cao, dễ phát sinh sai số cơ khí khi robot vận hành ở dải tốc độ lớn.
• Sử dụng Web Camera thông thường nên quá trình nhận diện màu sắc còn phụ thuộc vào sự ổn định của nguồn sáng môi trường xung quanh.
• Khả năng đồng bộ vị trí giữa robot và băng tải khi băng tải chạy ở tốc độ cao liên tục còn có độ trễ nhất định.

Hướng phát triển

• Gia công lại hệ thống cơ khí bằng các vật liệu kim loại chịu lực (nhôm định hình, thép gia công CNC) để nâng cao độ cứng vững và giảm thiểu rung động khi hoạt động ở tốc độ cao.
• Nâng cấp hệ thống chiếu sáng chuẩn công nghiệp (vòng đèn LED chuyên dụng) và sử dụng camera công nghiệp có tốc độ khung hình cao để ổn định chất lượng ảnh.
• Tối ưu hóa thuật toán bám chuyển động (Tracking) liên tục của băng tải thông qua thuật toán ngắt xung tốc độ cao nhằm tăng năng suất phân loại trên dây chuyền thực tế.

Giá trị tham khảo

Đồ án là tài liệu tham khảo chuyên môn cho sinh viên, giảng viên và kỹ sư thuộc các chuyên ngành Kỹ thuật Điện - Điện tử, Tự động hóa, Cơ điện tử và Kỹ thuật Robot:

• Phần tính toán động học: Cung cấp phương pháp chi tiết, tường minh về giải bài toán động học thuận và nghịch của Robot Delta bằng phương pháp hình học giải tích và ma trận xoay.
• Phần cấu hình Motion Control PLC: Cung cấp tài liệu hướng dẫn thực tế về việc sử dụng Technology Object, cấu hình phát xung PTO, thiết lập Jerk limit và lập trình các khối hàm chuẩn PLCopen (MC_Power, MC_Home, MC_MoveAbsolute...) trên TIA Portal V15.
• Phần xử lý ảnh EmguCV: Là tài liệu tham khảo cho việc lập trình thị giác máy tính trên nền tảng C# WinForms, từ khâu lọc màu HSV, phép toán hình thái học đến trích xuất tọa độ vật thể.

Câu hỏi thường gặp

1. Robot Delta trong đề tài có bao nhiêu bậc tự do và chuyển động của bàn máy động diễn ra như thế nào?

Robot Delta trong đồ án có cơ cấu động học song song 3 bậc tự do với cấu trúc vòng kín. Nhờ đặc tính của ba khâu bị động dạng hình bình hành, bàn máy động luôn được giữ song song với bàn máy cố định và chỉ có thể chuyển động tịnh tiến dọc theo 3 trục không gian $X, Y, Z$.

2. Tại sao đề tài sử dụng không gian màu HSV thay vì RGB để phân loại sản phẩm?

Đề tài sử dụng không gian màu HSV vì mô hình này tách biệt rõ ràng giữa thông tin màu sắc (Hue - góc màu), độ bão hòa màu (Saturation) và cường độ ánh sáng (Value). Điều này giúp thuật toán nhận diện màu sắc ít bị ảnh hưởng bởi sự thay đổi của độ sáng môi trường so với việc sử dụng trực tiếp các kênh màu RGB.

3. Phương pháp quy đổi động học nghịch cho 2 góc $\theta_2$ và $\theta_3$ được thực hiện như thế nào?

Do cấu trúc đối xứng hình học của Robot Delta, sau khi tìm được công thức giải tích cho góc $\theta_1$ trên mặt phẳng $YZ$, tác giả áp dụng phép xoay hệ tọa độ trong mặt phẳng $XY$ quanh trục $Z$ một góc $+120^\circ$ (ngược chiều kim đồng hồ) để tính $\theta_2$, và xoay $-120^\circ$ (theo chiều kim đồng hồ) để tính $\theta_3$ bằng cùng một thuật toán.

4. PLC S7-1200 giao tiếp với phần mềm WinForms trên máy tính bằng phương thức nào?

PLC S7-1200 giao tiếp với ứng dụng C# WinForms thông qua máy chủ phần mềm trung gian OPC KepServer sử dụng kết nối Ethernet/PROFINET. Dữ liệu tính toán tọa độ từ thị giác máy tính được truyền qua OPC xuống PLC, đồng thời các trạng thái nút bấm (START, STOP, RESET) và dữ liệu quản lý cũng được ghi nhận xuống cơ sở dữ liệu SQL Server.

5. Khối hàm nào trong TIA Portal được sử dụng để đưa cánh tay robot về vị trí gốc ban đầu?

Tác giả sử dụng khối lệnh điều khiển chuyển động MC_Home tích hợp trong tập lệnh Motion Control của S7-1200, kết hợp với các tiếp điểm từ công tắc hành trình KW11 để thực hiện quy trình tìm và thiết lập điểm tham chiếu gốc tọa độ (Home) cho 3 trục động cơ bước.

Kết luận

Đồ án tốt nghiệp đã hoàn thành việc thiết kế, chế tạo và thử nghiệm thành công mô hình Robot Delta 3 bậc tự do phân loại sản phẩm theo màu sắc dưới sự điều khiển của PLC S7-1200 và hệ thống thị giác máy tính EmguCV. Nhóm tác giả đã làm chủ quy trình tính toán động học robot song song, kỹ thuật xử lý ảnh nhận dạng màu sắc và phương pháp điều khiển phát xung chuyển động công nghiệp thông qua cấu hình Technology Object trên phần mềm TIA Portal V15. Kết quả nghiên cứu chứng minh tính khả thi của giải pháp tích hợp phần cứng - phần mềm chi phí thấp phục vụ cho các ứng dụng tự động hóa phân loại sản phẩm trong công nghiệp.

Trích đoạn nội dung tài liệu

CHƯƠNG 1: TỔNG QUAN VỀ ĐỀ TÀI 1. Giới thiệu Robot Delta Khái niệm robot có cấu trúc song song được Gough và Whitehall đưa ra vào năm 1962. Ngày nay, Delta robot (robot song song là một loại robot được sử dụng nhiều trong công nghiệp, đặc biệt là trong khâu phân loại và đóng gói sản phẩm nhờ vào đặc tính có tốc độ và độ chính xác cao. Robot Delta có cơ cấu động học song song ba bậc tự do, cấu trúc vòng kín.

Trong đó có ba nhánh, với mỗi nhánh, một đầu được nối vào giá di động thông qua cơ cấu hình bình hành. Với cấu trúc hình học như vậy nên giá di động luôn định hướng và chỉ có thể chuyển động tịnh tiến theo 3 trục XYZ. Với nhiều đặc tính nổi trội nên Robot Delta được ứng dụng rộng rãi ở nhiều lĩnh vực khác nhau trong cuộc sống. Dưới đây là 1 số ví dụ cụ thể: Ứng Dụng Trong Công Nghiệp: Hình 1.1 Robot Delta công đoạn đóng gói sản phẩm nhà máy Vinamilk 1 Hình 1.2 Robot Delta trong công nghiệp Ứng dụng trong Y học: Hình 1.3 Robot Delta sử dụng để nâng kính giúp bác sĩ phẫu thuật 2 Ứng dụng trong học tập, nguyên cứu khoa học: Hình 1.4 Sáng kiến SV ứng dụng robot Delta diệt cỏ loại sâu 1.

Mục tiêu, nhiệm vụ đề tài Tìm hiểu cấu tạo thành phần, cấu trúc cơ bản của Robot Delta, tính toán động lực học và áp dụng thực tế về phân loại sản phẩm. Xây dựng phần mềm điều khiển Robot Delta với cả 2 chế độ Auto và Manual, mô phỏng quá trình phân loại sản phẩm trong công nghiệp. Phương án Nhận dạng đối tượng bằng Camera sau đó gửi về PC các giá trị tọa độ, màu sắc, kích thước của đối tượng. PC nhận được dữ liệu sẽ tính toán qua phương trình động lực học để gửi thông tin cho PLC để điều khiển Robot tới gắp và thả vào ô phân loại.

Sử dụng động cơ bước cho 3 cánh tay của Robot để robot vận hành một cách linh hoạt. Sử dụng Web Camera để để nhận dạng vì Web Camera có giá thành rẻ, dễ dàng kết nối với PC nhưng vẫn đáp ứng đủ các tiêu chí theo yêu cầu của đề tài. Các bước tiến hành B1: Xây dựng chương trình tính toán động lực học thuận/nghịch của Robot Delta. B2: Cấu hình PLC S7-1200 đề điều khiển nhiều động cơ bước cùng lúc, sử dụng phần mềm TIA Portal V15.

3 B3: Xây dựng giao diện chương trình điều khiển Robot Delta với ba phần chính: Kết nối PC với PLC, Chế độ Manual, Chế độ Auto. Sử dụng nền tảng Winform của C#. B4: Thiết kế và thi công mô hình in 3D của Robot Delta. B5: Đưa phương trình động lực vào trong chương trình điều khiển truyền thông giữa chương trình điều khiển với PLC bằng OPC Kepserver, tiến hành vận hành Manual.

B6: Nhúng thư viện EmguCV vào phần mềm, lắp thêm Web Camera, điều khiển, xây dựng chương trình xác định tọa độ và màu sắc của vật mẫu để tiến hành phân loại, tiến hành vận hành Auto. B7: Vận hành hiểu chỉnh phần cứng và phần mềm. 4 CHƯƠNG 2: CƠ SỞ LÝ THUYẾT 2. Phương trình động lực học Robot Delta 2.

Cấu tạo robot song song Delta Hình 2.1 Cấu tạo cơ bản của Robot Delta Cấu tạo của robot bao gồm các phần như sau: - Bàn máy cố định F, bàn máy động E. - Ba khâu chủ động F1J1, F2J2, F3J3 liên kết với bàn máy cố định bằng các khớp quay và được dẫn động bởi 3 động cơ, các động cơ này gắn chặn với bàn máy cố định F. - Ba khâu bị động J1E1, J2E2, J3E3, mỗi khâu là một cấu trúc hình bình hành. Nhờ tính chất của các khâu hình bình hành nên bàn máy động là một vật rắn chuyển động tịnh tiến trong không gian.

Như vậy robot có 3 bậc tự do xác định bởi 3 toạ độ θ1, θ2, θ3 trong không gian khớp. Động học thuận Robot Từ các góc θ1, θ2, θ3→ Tọa độ ( , Nếu ta đã biết góc θx, chúng ta có thể dễ dàng tìm thấy tọa độ của các điểm J1, J2 và J3. Các khớp nối J1E1, J2E2 và , ) của điểm E0. J3E3 có thể tự do xoay quanh các điểm J1, J2 và J3 tương ứng, tạo thành ba hình cầu có bán kính re.2 Mô hình động học robot delta Ta di chuyển các tâm của các hình cầu từ các điểm J1, J2 và J3 đến các điểm J’1, J2 và J’3 bằng cách sử dụng các vector chuyển tiếp ′ E1E0, E2E0 và E3E0 tương ứng.

Sau quá trình chuyển đổi này, tất cả ba hình cầu sẽ giao nhau tại một điểm: E0, như được thể hiện trong hình phía dưới: Hình 2.3 Mô hình động học robot delta Vì vậy, để tìm tọa độ (x0, y0, z0) của điểm E0, chúng ta cần giải quyết tập hợp ba phương trình như (x − xj) + (y − yj) + (z − zj) = re , nơi tọa độ các 2 2 2 2 tâm hình cầu (xj, yj, zj) và bán kính re đã biết.4 Tọa độ các điểm J ,J ,J trên mặt phẳng Oxy 1 2 3 OF = OF = OF = f tan(300) = f 1 2 3 2 2√3 J J′ =J J′ =J J′ = e tan(300) = e 1 1 2 2 3 3 2 2√3 F1J1 = rf cos(θ1) , F2J2 = rf cos(θ2) , F3J3 = rf cos(θ3) J′ (0; − f− e −r cos θ ; −r sin θ ) 1 f 1 f 1 2√3 f− e f− e J′ (( +r cos θ ) cos 300 ;( +r cos θ ) sin 300 ; −r sin θ ) f f f 2 2 2 2 2√3 2√3 J′ (( f− e +r cos θ ) cos 30 0 ; (f − e +r cos(θ )) sin 30 ; −r 0 sin θ ) 3 f 3 f 3 f 3 2√3 2√3 Trong các phương trình sau đây ta sẽ chỉ định tọa độ của các điểm J1, J2, J3 là (x1, y1, z1), (x2, y2, z2) và (x3, y3, z3). Dưới đây là phương trình của ba hình cầu: x2 + (y − y )2 + (z − z )2 = r2 1 1 e {(x − x )2 + (y − y )2 + (z − z )2 = r2 2 2 2 e (x − x )2 + (y − y )2 + (z − z )2 = r2 3 3 3 e 2 2 2 x + y + z − 2y y − 2z z = r 2 −y 2 2 −z (1) 1 1 e 1 1 ⇒ {x2 + y2 + z2 − 2x x − 2y y − 2z z = r2 − x2 − y2 − z2 (2) 2 2 2 e 2 2 2 x2 + y2 + z2 − 2x x − 2y y − 2z z = r2 − x2 − y2 − z2 (3) 3 3 3 e 3 3 3 Đặt: w =x 2 +y 2 2 + z , ta được: i i i i 7 (w1 − w2) x2x + (y1 − y2)y + (z1 − z2)z = (4) = (1) − (2) 2 x3x + (y1 − y3)y + (z1 − z3)z = (w1 − w3) (5) = (1) − (3) 2 (w2 − w3) { (x2 − x3)x + (y2 − y3)y + (z2 − z3)z = 2 (6) = (2) − (3) Từ (4) - (5): x = a1z + b1 (7) y = a2z + b2 (8) a =1 [(z − z )(y −y ) − (z − z )(y − y )] 2 1 3 1 3 1 2 1 d 1 1 b1 = − 2d [(w2 − w1)(y3 − y1) − (w3 − w1)(y2 − y1)] 1 a2 = − d [(z2 − z1)x3 − (z3 − z1)x2] 1 b2 = 2d [(w2 − w1)x3 − (w3 − w1)x2] d = (y2 − y1)x3 − (y3 − y1)x2 Thế (7), (8) vào (1) ta được: (a21 + a22 + 1)z2 + 2(a1 + a2(b2 − y1) − z1)z + (b12 + (b2 − y1)2 + z12 − re2) = 0 Cuối cùng, ta cần giải phương trình này và tìm z0 (chúng ta nên chọn gốc phương trình âm nhỏ nhất), và sau đó tính x0 và y0 từ (7) và (8). Động học nghịch Robot Động học nghịch: Hình 2.5 Mô hình động học robot delta với các thông số cho trước 8 Các thông số: f, rf, re, e cho trước. Do khớp nối thiết kế của robot F 1J1 (xem hình bên dưới) chỉ có thể xoay trong mặt phẳng YZ, tạo thành hình tròn với tâm tại điểm F1 và bán kính rf.

J1 và E1 được gọi là khớp nối phổ quát, có nghĩa là E1J1 có thể xoay tự do tương đối với E1, tạo thành hình cầu với tâm tại điểm E1 và bán kính re.6 Minh họa động học của cánh tay đòn Giao điểm của hình cầu này và mặt phẳng YZ là một đường tròn có tâm tại điểm E1 và bán kính E1 J1 (trong đó E1 là phép chiếu của điểm ′ ′ ′ E1 trên mặt phẳng YZ). Điểm J1 có thể được tìm thấy bây giờ như là giao điểm của các đường tròn bán kính đã biết với tâm là E1 và F1 (chúng ta ′ nhỏ hơn). Và nếu ta biết J1, ta có thể tính được góc θ1. Xét mặt phẳng YZ: chỉ nên chọn một điểm giao nhau với tọa độ Y Hình 2.7 Mặt phẳng YZ 9 Ta có: E(x0, y0, z0) EE = e tan30o = e 1 2 2√3 E (x ,y − e , z ) ⇒ E′ = (0, y − e ,z) 1 0 0 2√3 0 1 0 2√3 0 EE ′ =x 0 ⇒E′J =√E J2 −E E ′2 2 = √r −x 2 1 11 1 1 1 1 e 0 f F1 (0, − , 0) (yJ1 − yF1 )2 + (zJ1 − zF1 )2 = rf2 2√3 { (y − y ′ )2 + (z −z ′ ) 2 = r2 − x2 J J 1 E1 1 E1 e 0 2 f (y + )+z 2 =r 2 J 2√3 J1 f 1 ⇒ ⇒ J1(0, yJ1, zJ1) e 2 2 (y −y+ ) + (z −z ) = r2 − x2 J1 { 2√3 J1 0 0 e 0 ) z θ1 = arctan ( J1 yF − yJ Do khớp F1J1 chỉ di chuyển trong mặt phẳng YZ, vì vậy chúng ta hoàn toàn bỏ qua tọa độ X.

Để tận dụng tính chất 1 1 này cho các góc còn lại θ2 và θ3, chúng ta nên sử dụng tính đối xứng của delta robot. Đầu tiên, chúng ta hãy xoay hệ tọa độ trong mặt phẳng XY xung quanh trục Z một góc 1200 ngược chiều kim đồng hồ, như được hiển thị bên dưới.8 Quy đổi tọa độ 10 Ta đã có một khung tham chiếu mới X'Y'Z', và với khung này ta có thể tìm thấy góc bằng cách sử dụng cùng một thuật toán mà ta sử dụng để tìm .