Các thiết bị phần cứng được lắp ráp là dạng Module đi cùng với nhau do khoa Điện – Điện tử cung cấp nên nhóm chỉ có nhiệm vụ kết nối chúng lại thành một hệ thống hoàn chỉnh thông qua các giao thức có sẵn dựa trên sơ đồ kết nối phần cứng như trong hình 3.12. Tín hiệu từ Camera sẽ được nhận vào máy tính thông qua cổng kết nối USB. Tương tự, cánh tay Dobot Magician và máy tính cũng sẽ giao tiếp tín hiệu với nhau thông qua cổng USB. Từ các chân kết nối ở phần đế của cánh tay Robot, ta kết nối chúng đến các cơ cấu chấp hành tương ứng dựa trên tài liệu hướng dẫn của nhà sản xuất được mô tả như trong hình 3.13 và hình 3.14
Hình 3. 12 Sơ đồ kết nối phần cứng
Hình 3. 13 Các giao thức kết nối ở chân đế của Dobot Magician
- GP1: điều khiển bơm khí, cảm biến màu sắc, cảm biến hồng ngoại - GP2: tương tự như GP1, các chức năng do người dùng tự định nghĩa - Stepper1: điều khiển động cơ băng tải, động cơ đùn nhựa (chế độ in 3D)
- Stepper2: tương tự như Stepper1, các chức năng do người dùng tự định nghĩa
- SW1: nguồn cấp cho bơm khí, nguồn ra 12V
- SW2: nguồn ra 12V
- USB: giao thức kết nối với máy tính
- Reset: reset lại hệ thống, khi reset đèn báo sẽ chuyển sang màu vàng, sau 5 giây nếu đèn chuyển sang màu xanh lá thì cánh tay đã được reset thành công.
39
Khoa ĐT CLC – ĐHSPKT TP.HCM
- Key: nếu bấm nhanh sẽ chạy chương trình ngoại tuyến, nếu bấm giữ trong 2 giây
Robot sẽ tiến hành lấy vị trí Home
- Communication Interface: giao tiếp UART, kết nối Bluetooth, Wifi
- Power: nguồn cấp đầu vào
Hình 3. 14 Các giao thức kết nối trên cánh tay
- GP3: điều khiển tín hiệu động cơ Servo ở cơ cấu chấp hành cuối
- GP4: điều khiểntín hiệu tự động cân bằng
- GP5: điều khiểntín hiệu đầu laser
- SW3: điều khiển tín hiệu đầu nung nhựa (chế độ in 3D)
- SW4: quạt gió (chế độ in 3D), nguồn cấp đầu laser
40
Khoa ĐT CLC – ĐHSPKT TP.HCM
CHƯƠNG 4. THI CÔNG HỆ THỐNG 4.1 Lắp ráp phần cứng
Hình 4. 1 Lắp đặt Camera Hình 4. 2 Các chân kết nối ở đế Robot
Hình 4. 3 Lắp đặt đầu hút Hình 4. 4 Kết nối dây tín hiệu cho bộ hút
41
Khoa ĐT CLC – ĐHSPKT TP.HCM
4.2 Lập trình điều khiển
4.2.1 Thuật toán xử lý ảnh
Hình 4. 7 Thuật toán xử lý ảnh
Dựa vào thuật toán đã đề ra, ta thiết lập nên hai luồng xử lý cho hệ thống. Luồng 1 xử lý ảnh bao gồm các thuật toán nhận diện biên Canny; lấy đường thẳng HoughLine; nhận diện vật thể hình vuông; nhận diện vị trí & hướng vật thể trên băng chuyền; và phân biệt màu sắc vật thể. Luồng 2 điều khiển cánh tay Robot gắp thả vật thể vào kho chứa.
Ở luồng điều khiển 1, ảnh màu nhận được từ Camera sẽ được truyền vào hàm Detector, hàm này sẽ phát hiện vật thể theo từng khung ảnh, xử lý tín hiệu và trả ra một danh sách các vật thể hình vuông mà nó phát hiện được gọi là List of rects (danh sách các hình vuông) với cấu trúc dạng [rect1,
42
Khoa ĐT CLC – ĐHSPKT TP.HCM
Với từng thành phần con, cấu trúc sẽ có dạng rect = [[x, y], hướng, màu], trong đó: [x, y] (theo pixel) là tọa độ khối vật thể trên ảnh được lưu theo dạng danh sách. Hướng là góc của vật so với trục Ox do ta quy ước, tính theo đơn vị độ, giới hạn trong khoảng (−900, 900). Như trong hình 4.9, ta có thể thấy vật thể hợp với trục Ox góc 0 độ, trong hình 4.10 vật thể hợp với trục Ox góc θ độ.
Hình 4. 9 Vật thể lệch góc 0 độ Hình 4. 10 Vật thể lệch góc θ độ
Màu được xác định theo quy tắc Red = 0, Green = 1, Blue = 2, Yellow = 3 Sau khi có được danh sách các hình vuông, ta biết được hệ tọa độ các vật thể trong ảnh. Từ đây, sẽ trả ngược kết quả về hàm Main để tiếp tục xử lý luồng điều khiển hai. Ở luồng điều khiển thứ hai, ngõ vào là danh sách các hình vuông được lấy từ luồng điều khiển thứ nhất, hàm Main sẽ lấy từng phần tử của danh sách này tức rect1, rect2,… để điều khiển vị trí cánh tay Robot. Tuy nhiên, để làm được điều này ta cần phải quay trở lại hàm Detector, hàm này sẽ phát hiện hình vuông theo từng khung hình. Giả sử trong một giây ta chạy được một khung hình, sau khung hình thứ nhất camera thu được [rect1, rect2], sau khung hình thứ hai camera thu được [rect3, rect4].
Hình 4. 11 Vị trí vật thể trong hai khung hình
Trên thực tế, ta có thể thấy hình vuông 1 và hình vuông 3 cùng là một hình vuông, hình vuông 2 và hình vuông 4 cùng là một hình vuông khi băng chuyền hoạt động. Tuy nhiên với máy móc, chúng lại hiểu hai hình vuông ở hai khung hình là khác nhau, vì vậy ta cần phải gom chúng lại thành một. Dựa vào hình 4.11, có thể thấy X1 < X3,
0 0
43
Khoa ĐT CLC – ĐHSPKT TP.HCM
lý do là vì X1 ở gần gốc tọa độ 0 hơn. Ta thiết lập giá trị Δ = 10 pixel là giới hạn nhỏ nhất giữa hai hình vuông, giá trị Δ này sẽ phụ thuộc vào tốc độ của băng chuyền và tốc độ lấy ảnh của Camera, từ đây ta có được điều kiện ràng buộc là: X3 – X1 ≤ Δ. Nếu điều kiện này thỏa mãn, ta kết luận được hình vuông 3 và hình vuông 4 trong khung hình thứ hai chính là hình vuông 1 và hình vuông 2 trong khung hình thứ nhất, ngược lại nếu điều kiện không thỏa thì chúng không phải cùng là một hình vuông. Sau khi xác định được các hình vuông trong từng khung hình là cùng một hình, ta lưu chúng vào một danh sách các vật thể List of Objects = [object1, object2,…]. Các vật thể (object) con có cấu trúc tương tự như hình vuông (rect) con. Ta có: object = [[x,y], hướng, màu], trong đó:
[x, y] (theo pixel) là tọa độ của vật thể trên ảnh được lưu theo dạng danh sách. Hướng là góc của vật thể so với trục Ox, được xác định theo đơn vị là độ, giới hạn trong khoảng (−900, 900).
Màu sắc xác định theo quy tắc Red = 0, Green = 1, Blue = 2, Yellow = 3. Để Robot xác định được đúng vị trí vật thể trên băng chuyền, ta cần phải chuyển đổi hệ tọa độ của vật trên ảnh sang hệ tọa độ của Robot với tỷ lệ đo được trên thực tế là 25mm tương ứng 30 pixel (25mm là độ dài cạnh vật thể). Sau khi quy đổi, ta có được một danh sách các vật thể đã được xác định [vật thể gần Robot nhất, vật thể gần Robot thứ hai, vật thể gần Robot thứ ba,...].
Ngoài ra, ta cần phải xác định thêm khoảng cách từ tâm Camera đến tâm của Robot (hình chiếu của chúng trên băng chuyền) và vận tốc băng chuyền (cấp xung vào để tìm được tốc độ động cơ). Khi đã có đầy đủ những dữ kiện, ta sẽ tính được chính xác thời gian vật đi từ hình chiếu tâm Camera đến hình chiếu tâm Robot trên băng chuyền, lúc đó băng chuyền sẽ dừng lại và Robot sẽ gắp vật thể xếp vào kho. Gắp đến đâu ta lần lượt xóa các biến vật thể đã xác định đến đó theo thứ tự vật thể gần Robot nhất, vật thể gần Robot thứ hai, vật thể gần Robot thứ ba,… liên tục như vậy cho đến khi kho được chứa đầy.
Cuối cùng, ta thiết lập một cấu trúc lưu vào biến “kích cỡ của kho” là một danh sách gồm ba phần tử size = [size x, size y, size z] tương ứng [1, 3, 4], với bốn màu kích thước sẽ là [4, 3, 4] (bốn hàng tương ứng với bốn màu, ba cột tương ứng mỗi màu sẽ có ba vật thể và mỗi màu trên mỗi cột sẽ xếp lên bốn vật thể).
Giả sử, cánh tay Robot đã xếp một vật thể vào vị trí đầu tiên, làm sao để nó biết được vật thể tiếp theo sẽ được xếp vào vị trí nào? Giải quyết điều này, ta tiếp tục tạo ra một biến để lưu được vị trí trống của từng màu có cấu trúc [x, y, z] với giá trị x chạy từ 0 đến 1; giá trị y chạy từ 0 đến 2; giá trị z chạy từ 0 đến 3. Tương ứng với bốn màu, danh sách chung sẽ là [Red [x, y, z], Green [x, y, z], Blue [x, y, z], Yellow [x, y, z]]. Vị trí [x, y, z] của từng màu sẽ liên tục thay đổi qua vị trí trống tiếp theo nếu vị trí hiện tại đã được lấp đầy, tương tự với ba màu còn lại cho đến khi toàn bộ vị trí của bốn màu được lấp đầy thì kho sẽ không nhận thêm vật thể nữa.
44
Khoa ĐT CLC – ĐHSPKT TP.HCM Hàm Detector
Ảnh được truyền vào là ảnh màu BGR (Blue, Green, Red), trước khi xử lý, ta phải chuyển nó sang dạng ảnh xám Gray. Sau đó, tách biên vật thể dùng giải thuật Canny. Tuy nhiên, mọi thứ sẽ không bao giờ hoàn hảo như ta kì vọng, hàm Canny không mạnh đến mức để có thể tách biên vật thể một cách hoàn toàn chính xác để cho ra các cạnh hình vuông như ta mong muốn. Nguyên nhân do ảnh hưởng bởi ánh sáng môi trường, bởi màu vật thể không đồng đều,… mà Canny chỉ cho ta được các
cạnh được mô tả như hình 4.12 Hình 4. 12 Cạnh xác định bởi Canny
Với trực giác của con người khi nhìn vào hình ta có thể xác định được ngay nó là một hình vuông, nhưng với máy móc chúng không thể làm được điều đó. Khắc phục nhược điểm này, thuật toán Houghline được sinh ra để xác định tập hợp các cạnh có thể là cạnh của một hình vuông, giả sử có 5 cạnh như hình 4.12 [0, 1, 2, 3, 4].
Chạy vòng lặp for cho từng cạnh, giả sử xét cạnh 0, nhờ Houghline trả ra tọa độ điểm đầu và cuối, ta tính được trung bình cạnh và xác định được tâm của cạnh. Ngoài ra, nó cũng giúp ta xác định được hướng vật thể so với trục Ox. Từ tâm cạnh 0, xét một đường tròn bán kính Δ, lấy toàn bộ các cạnh nằm trong phạm vi đường tròn này như hình 4.13, kết quả ta sẽ nhận được một danh
sách các cạnh gồm [1, 2, 3, 4]. Hình 4. 13 Các cạnh cần tìm Trường hợp không chỉ một mà có nhiều vật
thể, đường tròn của vật đang xét sẽ lấy thêm được nhiều cạnh của các vật thể khác trong phạm vi của nó. Giả sử trường hợp như hình 4.14, danh sách cạnh lúc này sẽ là [1, 2, 3, 4, 5, 6, 7]. Lúc này, ta cần phải loại đi cạnh [5, 6, 7] vì chúng không thuộc vật thể đang xét. Hướng giải quyết là lấy cạnh 0 làm gốc, giả sử góc cạnh 0 đang là 90 độ so với trục Ox, để tạo thành hình vuông thì những cạnh cần xác định phải tạo với cạnh 0 góc
0 độ hoặc 90 độ (song song hoặc vuông góc). Hình 4. 14 Cạnh không liên quan Các cạnh [1, 3, 4, 7] ta gom vào danh sách các cạnh vuông góc, cạnh 2 ta gom vào danh sách cạnh song song. Do cạnh 5 và cạnh 6 không song song cũng không vuông góc nên bị loại bỏ. Nhiệm vụ là phải giữ lại cạnh [1, 2, 3, 4] để có thể tạo thành một hình vuông. Loại bỏ cạnh 7 bằng cách xét thêm khoảng cách 𝛿𝑣𝑢ô𝑛𝑔 𝑔ó𝑐, cạnh nào
45
Khoa ĐT CLC – ĐHSPKT TP.HCM
nằm trong giá trị 𝛿𝑣𝑢ô𝑛𝑔 𝑔ó𝑐 ta giữ lại, từ đó sẽ loại được cạnh 7. Để biết chắc chắn các cạnh còn lại có thể tạo thành được một hình vuông, ta thực hiện việc bắt cạnh. Trong danh sách cạnh song song, xét được 1 cặp cạnh là cặp [0, 2], danh sách cạnh vuông góc xét được 3 cặp cạnh là cặp [1, 3], cặp [3, 4] và cặp [1, 4]. Sau đó, xét từng cặp song song với từng cặp vuông góc, [0, 2] - [1, 3], [0, 2] - [3, 4], [0, 2] - [1, 4]. Ở trường hợp đầu tiên cặp [0, 2] - [1, 3], ta lấy cặp cạnh [0, 2] làm gốc, có thể dễ dàng thấy cạnh 1 và cạnh 3 nằm giữa cạnh 0 và cạnh 2. Giả sử xảy ra trường hợp cạnh 3 lệch về bên phải, không còn nằm giữa cạnh 0 và cạnh 2 thì ta buộc phải loại loại cặp cạnh [1, 3]. Xét cặp cạnh tiếp theo [0, 2] - [1, 4], cạnh 1 và cạnh 4 đều thỏa mãn do chúng chắc chắn nằm giữa cạnh 0 và cạnh 2. Xét cặp cạnh cuối [0, 2] - [3, 4], cạnh 3 và cạnh 4 đều nằm giữa cạnh 0 và cạnh 2, tuy nhiên chúng bị loại bỏ do ngay từ bước phân cặp, ta ràng buộc điều kiện sao cho các cặp cạnh cách nhau khoảng 25mm thì mới ghép thành 1 cặp cạnh.
Như vậy, chỉ có cặp cạnh [0, 2] - [1, 4] có thể tạo thành một hình vuông. Sau khi đã xác định được hình vuông, ta tìm tâm hình vuông để xác định màu sắc và định vị trí để cánh tay Robot có thể di chuyển vật thể trên băng chuyền vào kho chứa.
4.2.2 Thuật toán điều khiển Robot
Hình 4. 15 Thuật toán điều khiển Robot
Đầu tiên, trước khi hệ thống đi vào hoạt động, ta cần lấy vị trí Home cho cánh tay Robot, mục đích của việc làm này là để tránh sai số khi mới khởi động và cải thiện
46
Khoa ĐT CLC – ĐHSPKT TP.HCM
độ chính xác cho cánh tay. Tiếp theo, băng chuyền sẽ bắt đầu hoạt động. Dựa vào hệ tọa độ ảnh của thuật toán xử lý ảnh trả về, ta chuyển chúng sang hệ tọa độ Robot với giá trị quy đổi 25mm tương ứng 30 pixel (25mm là độ dài cạnh vật thể). Sau đó, thiết lập nên vị trí gốc của kho chứa, đặt khoảng cách giữa các màu là 40mm, khoảng cách giữa các ô trong một màu là 35mm và khoảng cách từ một ô màu xếp lên là 26mm. Robot sẽ có thể định vị chính xác vật thể trên băng chuyền dựa vào hệ tọa độ đã được chuyển đổi và thời gian vật thể đi từ hình chiếu tâm Camera đến hình chiếu tâm của Robot (vị trí Home của cánh tay Robot) trên băng chuyền phụ thuộc vào tốc độ của băng chuyền. Cánh tay Robot sẽ gắp vật thể trên băng chuyền vào kho chứa theo quy tắc gắp vật thể gần Robot nhất đến vật thể gần Robot thứ hai đến vật thể gần Robot thứ ba,… gắp đến đâu chương trình sẽ lần lượt xóa các vị trí đã được xếp vào kho đến đó, cứ liên tục như vậy cho đến khi kho vật thể chứa đầy các khối màu theo kích thước [4, 3, 4] (bốn màu, mỗi màu xếp ba viên, từ ba viên của mỗi màu sẽ xếp lên bốn viên nữa) thì băng chuyền và cánh tay Robot sẽ dừng lại ở vị trí cuối cùng.
Hàm điều khiển Robot
Được điều khiển thông qua Python API cung cấp bởi nhà sản xuất Dobot. API là viết tắt của Application Programming Interface (giao diện lập trình ứng dụng) là phần trung gian cho phép hai thiết bị giao tiếp với nhau. Ta gọi các hàm API liên quan đến chuyển động (Point to Point, Jog,…) trong thư viện nhằm phục vụ cho yêu cầu của hệ thống. Một số hàm được sử dụng trong đề tài như:
(1) api = load(): Tải các DLL và gán đối tượng lưu trữ (API), DLL là viết tắt của Dynamic Link Library (thư viện liên kết động) chứa những hướng dẫn mà các chương trình có thể gọi đến để làm một tác vụ nào đó, là công cụ được phát triển để sử dụng mã chia sẻ và dữ liệu, cho phép nâng cấp các chức năng mà không cần phải tái liên kết hoặc tái biên dịch các ứng dụng. Khi Python API được gọi thì đối tượng này sẽ được sử dụng.
(2) Hàm kết nối/ngắt kết nối máy tính với Dobot Magician và hiển thị thông tin kết nối. Sau khi kết nối thành công thì các đoạn mã liên quan sẽ được xử lý.
state = ConnectDobot(api, "", 115200)[0] print("Connect status:",CON_STR[state])
if (state == DobotConnect.DobotConnect_NoError): #Dobot interactive codes
DisconnectDobot(api)
(3) Điều khiển dữ liệu hàng chờ. Hàng chờ là một cấu trúc dữ liệu dùng để lưu trữ các đối tượng theo cơ chế FIFO (First In First Out) nghĩa là vào trước ra trước. SetQueuedCmdClear(api)
47
Khoa ĐT CLC – ĐHSPKT TP.HCM
SetQueuedCmdStopExec(api)
(4) Thiết lập các tham số chuyển động cần thiết như vị trí điểm Home, thông số các khớp xoay, các thông số chung,… dựa trên các hàm:
SetPTPCmd(api, ptpMode, x, y, z, rHead, isQueued=1)
SetEndEffectorSuctionCup(api, enableCtrl, on, isQueued=1) SetWAITCmd(api, waitTime, isQueued=1)
SetHOMEParams(api, x, y, z, r, isQueued=1)
SetPTPCoordinateParams(api, xyzVelocity, xyzAcceleration, rVelocity,
rAcceleration, isQueued=1)
SetPTPCommonParams(api, velocityRatio, accelerationRatio, isQueued=1) SetPTPJumpParams(api, jumpHeight, zLimit, isQueued=1)
SetHOMECmd(api, temp, isQueued=1)[0]
(5) Tải các lệnh Point to Point vào hàng chờ và lấy chỉ mục của lệnh cuối cùng.
for i in range(0, 5): if i % 2 == 0: offset = 50 else:
offset = -50
lastIndex=SetPTPCmd(api, dType.PTPMode.PTPMOVLXYZMode, 200 + offset, offset, offset, offset, isQueued = 1)[0]
(6) Đợi lệnh chuyển động cuối cùng được hoàn thành.
while lastIndex > GetQueuedCmdCurrentIndex(api)[0]: dSleep(100)
Hàm điều khiển băng chuyền
Tương tự như hàm điều khiển Robot, hàm điều khiển động cơ băng chuyền cũng được tích hợp vào trong thư viện được cung cấp bởi nhà sản xuất với cấu trúc lệnh: