Điều khiển phân tán chuyển động robot công nghiệp shephent theo quỹ đạo định trước

Trong điều kiện Việt Nam hiện nay, khi xây dựng các hệ thống điều khiển chuyển động phân tán, nền tảng truyền thông trong hệ thống đều dựa vào các thiết bị đắt tiền của nước ngoài.. Với

TRƯỜNG ĐẠI HỌC CÔNG NGHỆ

MỤC LỤC

Mục lục 1

Danh mục các hình vẽ, bảng 4

Mở đầu 6

CHƯƠNG 1: Tổng quan 7

1.1 Đặt vấn đề 7

1.2 Một số cấu trúc hệ thống điều khiển 8

1.2.1 Hệ thống điều khiển theo cấu trúc tập trung 8

1.2.2 Cấu trúc điều khiển phân tán 9

1.3 Nội dung của luận văn 12

CHƯƠNG 2: Đối tượng ứng dụng - ROBOT SHEPHENT 14

2.1 Giới thiệu robot shephent 14

2.2 Động học vị trí của robot SHEPHENT 15

2.2.1 Cấu hình Robot Scara – SHEPHENT: 15

2.2.2 Động học thuận Robot SHEPHENT 15

2.2.3 Động học ngược Robot SHEPHENT 19

2.3 Xây dựng hệ điều khiển phân tán cho robot shephent 22

2.3.1 Cấu trúc hệ thống điều khiển phân tán 22

2.3.2 Chiến lược điều khiển phân tán 23

2.3.3 Phần mềm điều khiển chạy trên máy tính 24

2.3.4 Thuật toán điều khiển tại các bộ điều khiển từng khớp 24

2.4 Các vấn đề đặt ra với hệ thống điều khiển phân tán robot shephent 26

2.4.1 Tính quỹ đạo rời rạc theo thời gian định trước 26

2.4.2 Truyền dữ liệu, giám sát bộ điều khiển thực hiện quỹ đạo và đồng bộ chuyển động của các khớp 26

2.4.3 Xây dựng các bộ điều khiển động cơ và các bộ thu thập số liệu, điều khiển các thiết bị chấp hành khác 27

CHƯƠNG 3: Module tính quỹ đạo 28

3.1 Một số phương pháp tạo quỹ đạo 28

3.1.1 Tạo quỹ đạo cho các chuyển động PTP 29

3.1.2 Tạo quỹ đạo cho các chuyển động CP 35

3.1.3 Một thuật toán nội suy quỹ đạo với nhiều điểm tựa 43

3.2 Xây dựng module nội suy quỹ đạo, vận tốc cho robot shephent 45

3.2.1 Triển khai các module nội suy quỹ đạo trên C++ 45

3.2.2 Module nội suy bậc 3 – Cnoisuyx3 45

3.2.3 Module nội suy quỹ đạo n điểm - CnoisuyQDx3 47

3.2.4 Một số ví dụ ứng dụng thuật giải 47

3.3 Yêu cầu cho hệ thống truyền dữ liệu quỹ đạo đến các bộ điều khiển phân tán 49

CHƯƠNG 4: Xây dựng cơ sở truyền thông trong hệ điều khiển phân tán 50

4.1 Mạng truyền thông công nghiệp CAN 51

4.1.1 Kiến trúc giao thức 51

4.1.2 Các cấu trúc mạng và kỹ thuật truyền dẫn 52

4.1.3 Cơ chế giao tiếp 53

4.2 Vi xử lý Atmel AT89c52cc02 56

4.2.1 Các tính năng chính 56

4.2.2 Bộ điều khiển truyền thông CAN (CAN Controller) của AT89C51CC02 58

4.3 Vi điều khiển tích hợp bộ xử lý tín hiệu số Microchip dsPic30F4011 60

4.3.1 Các tính năng chính của dsPIP30F401: 60

4.3.2 Các module chuyên dụng cho điều khiển động cơ của vi điều khiển dsPIP30F4011 62

4.3.3 Bộ điều khiển truyền thông CAN (CAN Controller) của dsPic30F4011 64

4.4 Xây dựng nền tảng truyền thông tin cho hệ thống 66

4.4.1 Triển khai lớp vật lý 66

4.4.2 Triển khai lớp ứng dụng 66

4.4.3 Cơ chế giao tiếp 67

4.4.4 Phân loại thiết bị và dữ liệu trong VCCAN 67

4.4.5 Mô hình đối tượng 68

4.4.6 Mô hình địa chỉ 69

4.4.7 Cấu trúc bức điện 69

CHƯƠNG 5: Triển khai phần cứng và các kết quả đã đạt được 70

5.1 Bộ chuyển đổi USB-CAN 70

5.1.1 Sơ đồ nguyên lý 70

5.1.2 Phương thức hoạt động 71

5.2 Bộ điều khiển động cơ servo một chiều 71

5.2.1 Sơ đồ nguyên lý 71

5.2.2 Phương thức hoạt động 73

5.2.3 Thuật toán điều khiển động cơ 74

5.2.4 Thủ tục homing 76

5.3 Thiết kế, chế tạo robot SHEPHENT, chạy thử nghiệm 77

5.3.1 Thiết kế, chế tạo phần cơ khí robot SHEPHENT 77

5.3.2 Kết quả thiết kế, chế tạo mạch điện tử 78

5.3.3 Kết quả chạy thử nghiệm 79

5.3.4 Các việc đang được tiến hành để hoàn thiện hệ thống 79

Kết luận 81

Tài liệu tham khảo 82

DANH MỤC CÁC HÌNH VẼ, BẢNG

Hình 1.1: Cấu trúc một hệ điều khiển truyền động truyền thống 8

Hình 1.2: Một hệ điều khiển truyền động truyền thống 9

Hình 1.3: Cấu trúc một hệ điều khiển truyền động phân tán 10

Hình 1.4: Một hệ điều khiển truyền động phân tán 11

Hình 2.1: Robot SHEPHENT 14

Hình 2.2: Cấu hình robot SHEPHENT 15

Hình 2.3: Giải bài toán động học ngược robot Shephent bằng hình học 19

Hình 2.4: Cấu trúc hệ thống điều khiển phân tán robot Shephent 22

Hình 2.5: Cấu trúc phần mềm điều khiển 24

Hình 2.6: Thuật toán điều khiển 25

Hình 3.1: Chuyển động PTP giữa hai khớp nối 30

Hình 3.2: Đường đi với dạng tốc độ hình thang 31

Hình 3.3: Dạng đường hình sin 33

Hình 3.4: Nội suy theo đường thẳng để xác định vị trí của TCP 36

Hình 3.5: Thuật toán nội suy cho các chuyển động theo đường thẳng 38

Hình 3.6: Nội suy theo đường tròn để xác định vị trí của TCP 40

Hình 3.7: Mềm hoá các đoạn gấp khúc 40

Hình 3.8: Các đa thức Bernstein với n=2 (bậc 2) và n=3 (bậc 3) 41

Hình 3.9: Ví dụ về các đường cong Bezier 42

Hình 3.10: Chia liên tục đa giác 42

Hình 3.11: Ví dụ ứng dụng thuật toán nội suy 47

Hình 3.12: Ví dụ ứng dụng thuật toán nội suy qua 3 điểm 48

Hình 3.13: Ví dụ ứng dụng thuật toán nội suy qua 4 điểm 48

Hình 3.14: Ví dụ ứng dụng thuật toán nội suy qua 6 điểm 48

Hình 4.1: Phạm vi định nghĩa của CAN trong mô hình OSi 52

Hình 4.2: Sơ đồ khối của vi xử lý AT89C51CC02 57

Hình 4.3: Sơ đồ khối bộ điều khiển truyền thông CAN của vi xử lý

AT89C51CC02 58

Hình 4.4: Sơ đồ các ngắt của bộ điều khiển CAN trong vi xử lý AT89C51CC02 60

Hình 5.1: Mạch nguyên lý bộ chuyển đổi USB-CAN 70

Hình 5.2: Mạch nguyên lý phần vi điều khiển bộ điều động cơ một chiều 71

Hình 5.3: Mạch nguyên lý phần công suất bộ điều khiển động cơ một chiều 72

Hình 5.4: Mạch nguyên lý phần vi điều khiển bộ điều khiển động cơ một chiều không chổi quét 72

Hình 5.5: Mạch nguyên lý phần công suất bộ điều khiển động cơ một chiều không chổi quét 73

Hình 5.6: Thuật toán điều khiển vị trí động cơ 75

Hình 5.7: Thuật toán homing 76

Hình 5.8: Robot SHEPHENT 77

Hình 5.9: Đi dây trong thân robot SHEPHENT 77

Hình 5.10: Các cơ cấu truyền động trong robot SHEPHENT 78

Hình 5.11: Bộ điều khiển động cơ một chiều 78

Hình 5.12: Các bộ điều khiển động cơ đƣợc kết nối qua mạng VCCAN 79

Bảng 2-1: Tham số của Robot SHEPHENT 16

MỞ ĐẦU

Theo định hướng phát triển Tự động hoá tại Việt Nam, việc phát triển các robot công nghiệp đang được nhiều đơn vị chú trọng Những robot này làm việc thay con người trong rất nhiều lĩnh vực như: đóng gói sản phẩm, hàn, phun sơn, lắp ráp Chúng được ứng dụng rộng rãi trong công nghiệp để thay thế lao động của con người, đặc biệt là làm thay con người những công việc nặng nhọc, độc hại, nguy hiểm Các robot được điều khiển bằng máy tính hoặc các bộ vi xử lí, chúng

có thể dễ dàng lập trình để thực hiện các tác vụ khác nhau Bên cạnh đó các bộ phận được chế tạo dưới dạng môdun hoá làm cho việc thay đổi, mở rộng tính năng được dễ dàng và linh hoạt Việc xây dựng các hệ thống điều khiển phân tán cho robot công nghiệp nói riêng và các hệ truyền động nói chung đang là một hướng đi khả thi và hứa hẹn mang lại hiệu quả kinh tế cao Các hệ thống này bên cạnh giá thành rẻ còn có khả năng mở rộng và tính linh hoạt rất cao, đặc biệt là cho việc nâng cấp các máy công cụ thành các máy CNC và các hệ thống truyền động không yêu cầu độ chính xác quá cao Trong điều kiện Việt Nam hiện nay, khi xây dựng các hệ thống điều khiển chuyển động phân tán, nền tảng truyền thông trong hệ thống đều dựa vào các thiết bị đắt tiền của nước ngoài

Luận văn hướng tới việc xây dựng một hệ thống điều khiển phân tán cho các hệ truyền động với một chuẩn truyền thông sử dụng các linh kiện rẻ tiền, dựa trên các chuẩn mở Với phạm vi hẹp của các hệ điều khiển chuyển động, ngoài tính

mở và khả thi, chuẩn truyền thông này còn yêu cầu một sự kết hợp hợp lý giữa các phần cơ khí thể hiện bằng các biến trong các phương trình cơ, các thuật toán điều khiển và khả năng truyền thông bởi các thiết bị sử dụng trong hệ thống

Tác giả xin chân thành cảm ơn TS Nguyễn Hồng Quang, GS TSKH Đỗ Sanh, cùng toàn thể các thầy cô giáo thuộc trung tâm hợp tác đào tạo Viện Cơ

học đã hướng dẫn, giúp đỡ tác giả hoàn thành đồ án này

Hà Nội, 8/2005

Đặng Anh Việt

CH¦¥NG 1: TỔNG QUAN

1.1 ĐẶT VẤN ĐỀ

Hiện nay, ở Việt Nam hầu hết chúng ta phải nhập các dây chuyền máy móc từ nước ngoài trong đó không thể thiếu phần truyền động Các hệ thống truyền động này thường sử dụng các bộ điều khiển truyền động và phần công suất có khả năng kết nối truyền thông mạng với nhau nhưng chúng có giá rất cao Hệ thống máy móc này thường được các chuyên gia nước ngoài sau khi lắp đặt và chạy thử tại Việt Nam thì sẽ tiến hành đào tạo và chuyển giao công nghệ cho phía Việt Nam Mặc dù vậy khi hệ thống gặp sự cố thì do nhiều nguyên nhân mà chúng ta cũng không thể khắc phục được khi đó lại phải cần đến các chuyên gia nước ngoài Việc đó có ảnh hưởng nhiều mặt, trong đó ảnh hưởng đến nền kinh tế vẫn

là lớn nhất do chúng ta phải nhập dây chuyền máy móc từ nước ngoài cộng với chi phí chuyên gia cực cao Bên cạnh đó có một bài toán thực tế là nhu cầu về các máy CNC hoặc tay máy công nghiệp tại các phân xưởng sản xuất ngày càng cao Tuy nhiên vốn đầu tư cho các thiết bị mới rất lớn cũng như các hệ thống cơ khí chính xác Việt Nam chưa sản xuất được nên một giải pháp hợp lý là nâng cấp các hệ thống máy gia công có sẵn thành máy CNC hoặc mua các hệ thống CNC

cũ, hỏng của nước ngoài về và sửa chữa, nâng cấp Do đó, việc tự nghiên cứu sản xuất toàn bộ hệ thống truyền động, hay ít nhất là tự chế tạo được các bộ điều khiển các hệ thống máy có sẵn hứa hẹn mang lại hiệu quả kinh tế cao, có thể tiết kiệm cho ngân sách Nhà nước một khoản tài chính rất lớn

Xuất phát từ mong muốn xây một hệ thống điều khiển cho các hệ truyền động, cụ thể là cho các máy CNC hoặc các tay máy công nghiệp, tác giả hướng tới việc nghiên cứu một hệ thống điều khiển phân tán và giải quyết một số bài toán đặt ra cho hệ thống điều khiển phân tán, kết quả được ứng dụng trên một đối tượng cụ thể là robot SHEPHENT Hệ thống điều khiển cần xây dựng có thể không yêu cầu độ chính xác cao hoặc thời gian đáp ứng nhanh, nhưng phải rẻ tiền, linh hoạt

và quan trọng nhất là có thể được thiết kế, chế tạo tại Việt Nam Ngoài ra, hệ thống này cần được xây dựng dựa theo các chuẩn quốc tế, tiến tới hợp chuẩn để

có thể kết nối với các thiết bị cao cấp hơn của nước ngoài

1.2 MỘT SỐ CẤU TRÚC HỆ THỐNG ĐIỀU KHIỂN

1.2.1 Hệ thống điều khiển theo cấu trúc tập trung

Các hệ thống truyền động truyền thống dựa trên cấu trúc tập trung Trong cấu trúc này, một bộ điều khiển trung tâm sẽ làm toàn bộ nhiệm vụ thu thập dữ liệu phản hồi trong quá trình hoạt động, trên cơ sở đó đưa ra tín hiệu điều khiển cho các bộ điều khiển động cơ Các bộ điều khiển động cơ này chủ yếu là các bộ điều khiển tốc độ hoặc thuần tuý là mạch khuếch đại công suất Các tín hiệu điều khiển là các tín hiệu tương tự (analog) Cấu trúc hệ điều khiển tập trung thể hiện trong hình vẽ 1.1

Hình 1.1: Cấu trúc một hệ điều khiển truyền động truyền thống

Cấu trúc tập trung này yêu cầu một bộ vi xử lý trung tâm cực mạnh nên giá thành cao Bên cạnh đó, khối lượng dây dẫn tín hiệu và chống nhiễu cho các dây tín hiệu cũng dẫn tới nhiều khó khăn khi thiết kế và thi công hệ thống Một hệ thống điều khiển truyền động điển hình, đang được sử dụng nhiều tại Việt Nam của hãng TrioMotion thể hiện trong hình 1.2

Bộ điều khiển trung tâm

Bộ điều khiển động cơ 3 (khớp 3)

Các đầu vào ra số/tương

Hình 1.2: Một hệ điều khiển truyền động truyền thống

Mặc dù không thể phủ nhận tốc độ đáp ứng và độ chính xác cao của các hệ thống này, nhƣng giá thành và kết nối là những nhƣợc điểm lớn, không mang lại hiệu quả cao trong điều kiện Việt Nam

1.2.2 Cấu trúc điều khiển phân tán

Cùng với sự phát triển của lý thuyết điều khiển phân tán và sự phát triển nhanh chóng của các mạng truyền thông công nghiệp, các hãng sản xuất thiết bị công nghiệp cũng đã phát triển các hệ truyền động theo cấu trúc phân tán Trong cấu trúc phân tán, các bộ điều khiển động cơ đƣợc tích hợp cả chức năng điều khiển

vị trí và đƣợc nối mạng với nhau thông qua một mạng truyền thông Hệ thống có thể có hoặc không cần một bộ điều khiển trung tâm Tuy nhiên, bộ điều khiển trung tâm này (nếu có) thì chỉ giữ chức năng chính là đồng bộ các chuyển động

và truyền dữ liệu về quỹ đạo yêu cầu cho các bộ điều khiển Cấu trúc điều khiển phân tán hệ truyền động đƣợc thể hiện trong hình 1.3

Hình 1.3: Cấu trúc một hệ điều khiển truyền động phân tán

Thị phần về thiết bị truyền động, đặc biệt là các hệ truyền động phân tán tại Việt nam đang thuộc về Siemens Tuy nhiên, hệ thống truyền động mà Siemens cung cấp sử dụng là mạng Profibus DP Công nghệ truyền thông này có những nhược điểm nhất định khi ứng dụng tại Việt Nam Profibus DP mặc dù là một giao thức truyền thông mở đã được thiết kế và phát triển cho những ứng dụng trong công nghiệp, nhưng các thiết bị này khá đắt tiền và các linh kiện không được cung cấp rộng rãi Không chỉ khó khăn về mặt kỹ thuật, việc phát triển các hệ thống trong nước dựa trên Profibus DP còn yêu cầu đăng ký với tổ chức Profibus

Xét về tính năng kỹ thuật và đặc biệt là hiệu quả kinh tế thì các hệ thống truyền động của các hãng khác, đặc biệt là các hãng của Mỹ, với mạng truyền thông CAN, CANOpen, DeviceNet rất thích hợp với điều kiện Việt Nam Một hệ thống tiêu biểu thể hiện trong hình 1.4

Bộ điều khiển trung tâm

Bộ điều khiển động cơ 3 (khớp 3)

Các đầu vào ra số/tương

Hình 1.4: Một hệ điều khiển truyền động phân tán

Những hệ thống này có rất nhiều ưu điểm:

 Hệ thống thường theo các chuẩn mở nên việc sử dụng rất linh hoạt

 Các chức năng điều khiển có thể thay đổi mà không cần đi lại dây

 Dễ dàng mở rộng nâng cấp hoặc tích hợp vào các hệ thống khác

 Các tham số có thể được cài đặt và thay đổi thông qua giao diện

 Phần mềm hỗ trợ phát triển luôn sẵn sàng

 Hệ thống có thể được điều khiển và giám sát một cách liên tục

 Khoảng cách truyền xa, khả năng chống nhiễu tốt

 Chi phí lắp đặt thấp vì thời gian ngắn, số lượng dây cáp và chi phí đi dây không nhiều

 Một mạng có thể kết nối nhiều loại thiết bị như PLC, màn hình vận hành, biến tần, thiết bị đo, thiết bị chấp hành,…

 Chuẩn đoán hệ thống từ xa

 Đã được chứng minh qua các hệ thống công nghiệp khác nhau

Một số các điểm hạn chế

 Tốc độ đáp ứng của hệ thống bị giới hạn bởi mạng truyền thông

 Đội ngũ kỹ thuật vận hành, bảo trì bảo dưỡng phải được đào tạo tốt

1.3 NỘI DUNG CỦA LUẬN VĂN

Một hệ thống tryền động mà Việt Nam hoàn toàn làm chủ cần dựa trên các nền tảng được thế giới công nhận, mặt khác phải tìm cách ứng dụng các chuẩn đó cho phù hợp với các điều kiện hiện có Các hệ thống ngày càng thông minh và chính xác Để đạt được độ chính xác cao thì hệ thống điều khiển vừa phải sử dụng các thuật toán cho độ chính xác cho phép, vừa phải đảm bảo tính năng thời gian Trong điều kiện Việt Nam, với thế mạnh của các hệ phân tán là linh hoạt, giá thành rẻ và tốc độ ngày càng được cải thiện, việc ứng dụng các hệ điều khiển phân tán vào điều khiển robot công nghiệp là một hướng đi phù hợp Tuy nhiên, với cấu trúc phân tán, robot cần một thuật toán tính toán quỹ đạo, vận tốc có thể được chia nhỏ cho từng bộ vi xử lý (xử lý song song) Một cấu trúc kết hợp là

sử dụng máy tính để tính toán quỹ đạo, vận tốc và truyền các kết quả này xưống các module điều khiển phân tán thông qua mạng Thuật toán tính toán quỹ đạo, vận tốc không chỉ đảm bảo điều khiển robot di chuyển theo một quỹ đạo liên tục

mà còn phải thỏa mãn các giới hạn của các bộ vi xử lý và khả năng truyền thông trên mạng

Nhằm hướng tới một hệ thống điều khiển chuyển động phân tán, không chỉ phát huy ưu điểm của các hệ thống phân tán như giá thành rẻ và khả năng mở rộng cao, hệ thống vẫn có khả năng đáp ứng độ chính xác và tính năng thời gian của các hệ truyền động Luận văn tập trung vào việc xây dựng thuật toán nội suy quỹ đạo, vận tốc và việc thu thập, xử lý, phân chia các kết quả tính toán trong hệ thống điều khiển robot phân tán Trên cơ sở đó, luận văn đưa ra một cơ sở có thể

sử dụng được cho phép kết nối máy tính và các thiết bị chấp hành, và hướng phát triển để hệ thống có thể kết nối và làm việc được với các thiết bị theo các chuẩn quốc tế Kết qủa được ứng dụng trên robot cụ thể SHEPHENT Luận văn được trình bày thành 5 chương:

 Chương 1 - Tổng quan: Phân tích các hệ thống điều khiển robot và hướng

nghiên cứu cho đề tài

 Chương 2 - Đối tượng ứng dụng - Robot SHEPHENT: Nghiên cứu một

đối tượng cụ thể là robot SHEPHENT để phục vụ việc nghiên cứu và kiểm chứng hệ thống điều khiển phân tán cần xây dựng

 Mô hình hoá robot SHEPHENT

 Cấu trúc điều khiển phân tán cho robot SHEPHENT

 Thuật toán điều khiển và mô phỏng cho hệ điều khiển robot SHEPHENT

 Chương 3 - Xây dựng module nội suy quỹ đạo, vận tốc: Tìm hiểu các cơ

sở lý thuyết cho việc xây dựng thuật toán tính toán quỹ đạo và module tính toán quỹ đạo dựa trên thuật toán nội suy bậc 3

 Chương 4 - Xây dựng cơ sở truyền thông tin trong hệ thống phân tán:

nghiên cứu một số chuẩn truyền thông công nghiệp, khả năng triển khai trên phần cứng và đưa ra cơ sở truyền thông VCCAN

 Tìm hiểu chuẩn truyền thông công nghiệp CAN và các ưu điểm nổi trội của chuẩn truyền thông này trong lĩnh vực điều khiển chuyển động

 Tìm hiểu phần cứng sẵn có: vi xử lý Atmel AT89C51CC02

 Dựa trên cơ sở đó để xây dựng chuẩn truyền thông VCCAN, có khả năng chuyển tải các kết quả tính toán, lệnh điều khiển, đồng bộ các chuyển động và các cảnh báo lỗi

 Chương 5 - Một số kết quả triển khai phần cứng: Trình bày các kết quả

triển khai phần cứng và phần mềm

CH¦¥NG 2: ĐỐI TƢỢNG ỨNG DỤNG - ROBOT SHEPHENT

2.1 GIỚI THIỆU ROBOT SHEPHENT

Robot SCARA là một trong những Robot phổ biến nhất trong công nghiệp trong

đó Robot SHEPHENT là tiêu biểu thuộc họ này với 3 khớp quay dùng động cơ

DC Servo và 1 khớp tịnh tiến dùng xilanh khí nén Chuyển động Robot này rất đơn giản nhƣng lại phù hợp với các dây chuyền và ứng dụng hữu hiệu trong nhiệm vụ nhặt và đặt sản phẩm

cơ điện một chiều có phản hồi vị trí

Đối với Robot SHEPHENT, động cơ điều khiển cổ tay đƣợc đặt phía sau cánh tay, truyền động với cổ tay bằng dây đai Điều này hạn chế phạm vi quay của cổ tay khi cánh tay dịch chuyển Chuyển động thẳng đứng của Robot đƣợc cung cấp bởi một ống trụ khí nén, ống này chỉ hoạt động khi cánh tay máy dịch chuyển đến các điểm cuối của hành trình Robot SHEPHENT có thể đƣợc lập trình từ máy tính bằng việc gắn số liệu cho từng trục hoặc sử dụng thiết bị điều khiển cầm tay(Teach Pendant)

2.2 ĐỘNG HỌC VỊ TRÍ CỦA ROBOT SHEPHENT

2.2.1 Cấu hình Robot Scara – SHEPHENT:

Robot SHEPHENT gồm 3 khớp quay và một khớp chuyển động tịnh tiến

Hình 2.2: Cấu hình robot SHEPHENT

Gán cho mỗi khớp quay một hệ trục toạ độ ( hình 2.2 ) , ta có:

2.2.2 Động học thuận Robot SHEPHENT

Động học thuận cho biết vị trí và hướng (giá trị) của các khớp Robot, ta xác định được vị trí và hướng của tay Robot Sử dụng phương pháp ma trận DH với các kết quả trong tài liệu [4]

Ta có bảng các tham số DH của Robot SHEPHENT:

Thanh nối Biến  a D

Bảng 2-1: Tham số của Robot SHEPHENT

Ma trận 0 T4 là ma trận biểu diễn tay máy Robot trong hệ trục toạ độ gốc:

A

Ta có:

4 4

4 4

4 4 3

Ma trận 0T4 biểu diễn cánh tay máy Robot trong hệ toạ độ gốc

Mặt khác theo kí hiệu tổng quát:

2.2.3 Động học ngược Robot SHEPHENT

Động học ngược: xác định các biến khớp khi biết vị trí tay

Với robot SHEPHENT, các cơ cấu tương đối đơn giản, ta có thể sử dụng phương pháp hình học để giải bài toán động học ngược Robot cho Shephent

Từ hệ phương trình động học thuận ta có ngay

Hình 2.3: Giải bài toán động học ngược robot Shephent bằng hình học

Ta thấy ngay có thể có 2 nghiệm , 1 2thoả mãn hệ phương trình động học thuận

Xét OA1A2 ta có

Chọn dấu cho nghiệm  1 , 2tìm được:

Như đã trình bày ở trên, ta có thể tìm được 2 bộ nghiệm  1 , 2đều thoả mãn phương trình động học thuận Điều đó có nghĩa là có 2 bộ giá trị biến khớp đều

có khả năng đưa robot đến vị trí Px, Py yêu cầu

Việc chọn một bộ giá trị để làm đầu vào cho bộ điều khiển được dựa trên khả năng đáp ứng của hệ thống cơ khí Bên cạnh đó, ta có thể chọn  1 , 2theo yêu cầu các giá trị này là giá trị gần nhất với các giá trị thực trên robot tại thời điểm yêu cầu điều khiển Trong thực tế, ta chọn  1 , 2với các giá trị 2luôn dương Như vậy ta có hệ phương trình động học ngược của robot Shephent:

2.3 XÂY DỰNG HỆ ĐIỀU KHIỂN PHÂN TÁN CHO ROBOT SHEPHENT

2.3.1 Cấu trúc hệ thống điều khiển phân tán

Hình 2.4: Cấu trúc hệ thống điều khiển phân tán robot Shephent

Hệ thống điều khiển phân tán robot Shephent được xây dựng từ những bộ điều khiển riêng cho từng động cơ Các bộ điều khiển động cơ này, cùng với các bộ

đo lường và điều khiển khác (các đầu vào ra số/tương tự cho các cảm biến và cơ cấu chấp hành khác động cơ) được kết nối với nhau thông qua mạng truyền thông VCCAN (được trình bày cụ thể trong phần 4.2) Hệ thống sử dụng một máy tính

PC thông dụng làm bộ điều khiển trung tâm Để một máy tính PC thông dụng có thể kết nối được với mạng truyền thông VCCAN hay CAN cần có bộ chuyển đổi USB-CAN converter (tác giả tự phát triển) hoặc một card mở rộng CAN chuyên dụng cùng với phần mềm tương thích

Chương trình điều khiển trên PC sẽ hỗ trợ người sử dụng tạo quỹ đạo cho robot

Hỗ trợ cơ bản là tạo các quỹ đạo từ các điểm tựa, cho vị trí, hướng, vận tốc của

USB-CAN converter

Bộ điều khiển động cơ 3 (khớp 3)

Bộ điều khiển động cơ 4 (khớp 4)

Mạng truyền thông VCCAN

Kết nối USB

điểm đầu tay máy Sau khi nội suy lại quỹ đạo, chương trình sẽ tính toán động học ngược của robot và gửi dữ liệu (giá trị các biến khớp tại các điểm nút thời gian) xuống các bộ điều khiển động cơ của từng khớp Lệnh chạy/dừng cũng được gửi tới các bộ điều khiển từ máy tính chủ hoặc từ Teach pendant Chương trình sẽ liên tục cập nhật vị trí các khớp, tính toán động học thuận để đưa ra màn hình một robot mô phỏng đang chạy theo các giá trị của robot thực Các lệnh đồng bộ chuyển động cũng như việc theo dõi cả hệ thống về các sự cố, robot chạy quá các giới hạn hay từ các cảm biến, các bộ kết nối với các hệ thống điều khiển khác được tuỳ chọn khi khai báo cấu hình hệ thống

Ngoài ra, chương trình điều khiển sẽ được thiết kế hướng đối tượng, sử dụng các công nghệ lập trình tiên tiến để có thể cho phép mở rộng hệ thống hoặc thay đổi chức năng các phần có sẵn một cách rất linh hoạt

2.3.2 Chiến lược điều khiển phân tán

Hệ thống điều khiển phân tán nói riêng và hệ thống điều khiển số nói chung đều dựa trên các tính toán rời rạc Một cách cụ thể là các tín hiệu điều khiển là kết quả tính toán từ các giá trị đầu vào tại các điểm thời gian rời rạc (các điểm lấy mẫu) Hệ điều khiển phân tán điều khiển robot theo một quỹ đạo cho trước dựa trên các điểm tựa Các điểm tựa trên quỹ đạo là các điểm mà quỹ đạo bắt buộc phải đi qua, có thể cho trước thời gian và vận tốc tại các điểm tựa Hệ thống phải tính toán để đưa ra được các điểm tựa rời rạc theo thời gian Chính các điểm tựa rời rạc theo thời gian này sẽ là các đầu vào cho từng bộ điều khiển các trục Như vậy, ta có các bước từ khi người sử dụng đặt quỹ đạo đến điều khiển hệ thống thực theo quỹ đạo đó:

 Người sử dụng cho quỹ đạo dưới dạng một số các điểm tựa và vận tốc tại các điểm tựa trong không gian làm việc của robot

 Nội suy quỹ đạo để ra được các điểm tựa theo thời gian rời rạc

 Tính toán động học ngược để ra được toạ độ khớp tại các điểm tựa rời rạc này

 Truyền kết quả tính toán đến từng bộ điều khiển cho các khớp trong hệ thống điều khiển phân tán

 Sau khi có lệnh chạy, cập nhật tọa độ các khớp theo thời gian thực, tính toán động học thuận để tìm lại quỹ đạo chạy thực của robot

2.3.3 Phần mềm điều khiển chạy trên máy tính

Dựa trên chiến lược điều khiển đã đặt ra, phần mềm điều khiển chạy trên máy tính sẽ không chỉ làm nhiệm vụ điều khiển mà còn phải là một ứng dụng hoàn chỉnh bao gồm nhiều module thực hiện các chức năng riêng biệt Một trong những nhiệm vụ quan trọng của phần mềm này là phải cung cấp cho người sử dụng một giao diện hỗ trợ việc tạo quỹ đạo, cài đặt các tham số cho hệ thống và giao diện cho phép người sử dụng giám sát hoạt động của hệ thống chấp hành từ

xa Các khối chức năng và cấu trúc phần mềm điều khiển được thể hiện trong hình 2.3

Hình 2.5: Cấu trúc phần mềm điều khiển

2.3.4 Thuật toán điều khiển tại các bộ điều khiển từng khớp

Với các hệ thống thông thường, không yêu cầu độ chính xác rất cao, thuật toán PID điển hình là đủ để đáp ứng các yêu cầu của bài toán điều khiển quỹ đạo Với từng bộ điều khiển động cơ trong hệ thống, thuật toán PID được áp dụng cho vòng lặp điều khiển vị trí Ngoài ra, các bộ điều khiển này còn có thể có các vòng lặp PID cho dòng điện, vận tốc Ví dụ cụ thể là vị trí thực của các khớp được

Giao diện

hỗ trợ tạo quỹ đạo

Modul nội suy quỹ đạo

Modul tính toán động học thuận/ngược

Modul điều khiển, đồng

Giao diện cài đặt

phản hồi về qua các bộ giải mã (rotary encoder), sai số giữa vị trí thực và vị trí yêu cầu của khớp là đầu vào của bộ điều khiển PID

Việc lựa chọn các tham số của bộ điều khiển PID dựa trên tiêu chuẩn ổn định Lyapunov Tác giả đã sử dụng kết quả tính toán trong tài liệu [6] cho phần mô phỏng hệ thống Trên hệ thống thực, việc mô hình hoá phần mạch điện tử công suất điều khiển động cơ và bản thân động cơ gặp nhiều khó khăn do tài liệu kỹ thuật không đầy đủ hoặc hệ thống quá phức tạp Trong trường hợp đó, một giải pháp hiệu quả là coi toàn bộ phần mạch điện tử công suất, động cơ cùng với tải là một hộp đen Bộ điều khiển được đặt một xung đơn vị với một bộ tham số PID đầu tiên theo kinh nghiệm của người kỹ sư Đầu ra được lưu lại và vẽ đồ thị theo thời gian Các tham số PID được chỉnh dần, cũng theo kinh nghiệm của người kỹ

sư, cho đến khi bộ điều khiển đạt được đáp ứng mong muốn

Máy tính với chương trình điều khiển sẽ làm nhiệm vụ giám sát và đồng bộ các chuyển động Thuật toán điều khiển nằm tại các bộ điều khiển động cơ cho từng khớp:

Hình 2.6: Thuật toán điều khiển

Mạch điện tử công suất

-

cơ

Rotary Encoder

2.4 CÁC VẤN ĐỀ ĐẶT RA VỚI HỆ THỐNG ĐIỀU KHIỂN PHÂN TÁN ROBOT SHEPHENT

Với chiến lược điều khiển phân tán đã đặt ra, có 4 vấn đề lớn cần phải giải quyết:

 Nội suy quỹ đạo dựa trên 2 điểm đầu cuối với quỹ đạo điểm-điểm hoặc dựa trên các điểm tựa với quỹ đạo đường liên tục để tính các điểm quỹ đạo rời rạc theo thời gian định trước

 Truyền dữ liệu tính toán được này đến từng bộ điều khiển, giám sát việc bộ điều khiển thực hiện quỹ đạo, đồng bộ chuyển động của các khớp

 Xây dựng bộ điều khiển động cơ cho từng khớp

 Xây dựng bộ điều khiển các đầu vào/ra để thu thập dữ liệu của các cảm biến cũng như điều khiển các cơ cấu chấp hành khác như xi lanh khí nén

2.4.1 Tính quỹ đạo rời rạc theo thời gian định trước

Các bộ điều khiển cho các khớp sẽ hoạt động theo nguyên lý điều khiển động cơ đến các vị trí sau các khoảng thời gian nhất định ví dụ 1ms Với khoảng thời gian rời rạc này, vận tốc của động cơ có thể coi như không thay đổi Sai số tại điểm cuối sẽ được đưa qua thuật toán PID để tính toán tín hiệu điều khiển cho bộ điều khiển điều khiển động cơ đến vị trí tiếp theo, cũng sau khoảng thời gian rời rạc

đó

Do đó, đầu vào của các bộ điều khiển là một bảng các giá trị cho khớp (vị trí động cơ) theo một bước chia thời gian xác định Để xây dựng được bảng các giá trị này thì chúng ta phải xây dựng một module nội suy quỹ đạo, trong đó việc nội suy quỹ đạo các điểm thời gian rời rạc được thực hiện cùng với việc tính đến vận tốc và gia tốc của hệ thống cơ tại các điểm đầu tiên và cuối cùng của quỹ đạo Module này được trình bày cụ thể trong chương 3

2.4.2 Truyền dữ liệu, giám sát bộ điều khiển thực hiện quỹ đạo và đồng bộ chuyển động của các khớp

Việc truyền các dữ liệu tính toán được sau module nội suy quỹ đạo đến từng bộ điều khiển, giám sát việc bộ điều khiển thực hiện quỹ đạo và đồng bộ chuyển động của các khớp trong hệ thống phân tán được thực hiện thông qua một mạng truyền thông Mạng truyền thông này không chỉ yêu cầu độ chính xác và bảo toàn dữ liệu cao, có khả năng chống nhiễu trong điều kiện công nghiệp mà còn

phải bảo đảm tốc độ truyền dữ liệu đủ cao để có thể giám sát chuyển động theo thời gian thực

Do đó, không chỉ xây dựng mới hay áp dụng một chuẩn vật lý truyền thông có tốc độ và khả năng chống nhiễu cao mà chúng ta còn phải xây dựng một bộ giao thức truyền thông hiệu quả nhằm đảm bảo tính năng thời gian thực của hệ thống Bên cạnh đó, mạng truyền thông này phải được hỗ trợ một cách rộng rãi, các thành phần để xây dựng mạng dễ kiếm Chương 4 sẽ trình bày cụ thể việc xây dựng một cơ sở truyền thông trong hệ thống điều khiển phân tán và một số linh kiện cụ thể sẽ được sử dụng để thiết kế, chế tạo các bộ điều khiển

2.4.3 Xây dựng các bộ điều khiển động cơ và các bộ thu thập số liệu, điều khiển các thiết bị chấp hành khác

Sau khi đã có dữ liệu quỹ đạo cũng như một nền tảng truyền thông, một bước quan trọng trong triển khai hệ thống là phải xây dựng được các bộ điều khiển thực, phát huy được thế mạnh của các thuật toán điều khiển Các thiết bị sẽ được tích hợp từ các linh kiện sẵn có trên thị trường Đặc biệt, với các thiết bị được xây dựng dựa trên các bộ vi điều khiển thì việc lựa chọn vi điều khiển bảo đảm tốc độ xử lý cho thuật toán và phù hợp với điều kiện phát triển tại Việt Nam trở nên rất quan trọng Hơn nữa, các thiết bị không những phải được thiết kế để chống được nhiễu, khả năng bảo vệ khi có sự cố, bảo đảm tin cậy trong môi trường công nghiệp, mà còn phải được thiết kế về cơ khí chắc chắn, dễ dàng bảo trì, nâng cấp

Chương 5 sẽ trình bày các kết quả cụ thể trong việc phát triển phần cứng của các

bộ điều khiển

CH¦¥NG 3: MODULE TÍNH QUỸ ĐẠO

3.1 MỘT SỐ PHƯƠNG PHÁP TẠO QUỸ ĐẠO

Phần này tập trung vào một số phương pháp tính toán quỹ đạo trong không gian

đa chiều Quỹ đạo là một chuỗi theo thời gian các vị trí, tốc độ và gia tốc cho mỗi mức độ chuyển động

Vấn đề cơ bản trong điều khiển quỹ đạo robot là di chuyển robot từ vị trí đầu, xác định bởi tool frame Tinitial đến vị trí cuối xác định bởi tool frame Tfinal Với một tay máy robot thì vị trí của điểm đầu tay máy và hướng tác động là các điểm cần xác định (Tool Center Point - TCP)

Những điều kiện của chuyển động trong không gian:

 Việc xác định một chuyển động có thể thông qua các điểm trung gian, là các điểm nằm giữa điểm đầu và điểm cuối hoặc giữa các điểm tựa Các điểm trung gian này thỏa mãn những điều kiện của chuyển động về thời gian

 Việc xác định một chuyển động có thể bao gồm thời gian tiêu tốn giữa các điểm trung gian

Yêu cầu đối với việc nội suy quỹ đạo:

 Thực hiện các chuyển động uyển chuyển: hàm và đạo hàm bậc 1 của chuyển động liên tục Trong một số trường hợp cần thiết, đạo hàm bậc 2 của chuyển động cũng cần liên tục Các chuyển động đột ngột làm tăng độ mài mòn lên máy móc (các bánh răng) và gây chấn động do kích thích các cộng hưởng của robot

Có hai dạng chuyển động:

 Điểm đầu tay máy (TCP) của Robot chỉ cần đạt được vị trí và hướng để có thể thực hiện các thao tác cầm nắm đối tượng hoặc buông thả đối tượng Đây

là trường hợp các Robot thực hiện các công việc vận chuyển và trao đổi phôi liệu trong một hệ thống tự động linh hoạt Robot hoá (đúc, gia công áp lực, gia công cắt gọt ) Bàn tay máy không trực tiếp tham gia vào các chuyên môn công nghệ như dập tạo hình hoặc cắt gọt kim loại Các điểm nút (điểm đầu và điểm cuối) là mục tiêu quan trọng nhất, còn dạng đường đi tới các điểm là vấn đề thứ yếu Robot sẽ hiểu được nhiệm vụ của nó khi được “dạy học” bởi cụm bàn phím điều khiển (Teach Pendant), ví dụ đơn giản nhất là các Robot nhấc và đặt vật thể (Tick and Place) Thông thường hình dạng đường trong không gian và thời gian được mô tả nhờ các hàm của toạ độ khớp Dạng chuyển động này được gọi là chuyển động điểm - điểm (Point to Point - PTP)

 Robot phải xác định dạng đường đi qua các điểm nút theo thời gian thực Đó

là trường hợp các tay máy trực tiếp thực hiện các nguyên công công nghệ như hàn, sơn, cắt (Laze hay hồ quang áp lực .) Vấn đề thiết kế quỹ đạo cho Robot ở trường hợp ứng dụng này đóng vai trò quan trọng, quyết định trực tiếp tới chất lượng thực hiện các nhiệm vụ công nghệ mà nó được giao phó Trong chương này, chúng ta đề cập đến bài toán thiết kế quỹ đạo cho một số quỹ đạo điển hình Dễ nhận thấy các quỹ đạo này không chỉ có ý nghĩa với trường hợp ứng dụng thứ hai mà nó bao hàm một ý nghĩa chung cho mọi Robot vì rằng ngay cả trưòng hợp đơn giản như Robot “nhấc - đặt” đối tượng cũng là những quỹ đạo cơ bản Thông thường hình dạng đường trong không gian và thời gian được mô tả nhờ các hàm toạ độ Đề các, và có thể có yêu cầu đồng bộ giữa các chuyển động của các khớp Dạng chuyển động này gọi là chuyển động đường liên tục (Continuos Path - CP)

3.1.1 Tạo quỹ đạo cho các chuyển động PTP

Với các chuyển động PTP thông thường của robot Shephent, hình dạng quỹ đạo nói chung không quan trọng ta chỉ cần quan tâm đến vị trí điểm đầu và điểm cuối, vận tốc tại 2 điểm này bằng không Ví dụ cụ thể cho các chuyển động này

là thao tác nhấc - đặt hoặc di chuyển từ vị trí hiện tại đến vị trí bắt đầu thao tác

Hình 3.1: Chuyển động PTP giữa hai khớp nối

Từ 2 vị trí đầu và cuối, tính động học ngược để có tọa độ khớp tại 2 vị trí này và tính ra được khoảng dịch chuyển của mỗi khớp (góc quay) Với tốc độ cực đại của mỗi khớp, ta tính được khoảng dịch chuyển nhỏ nhất đối với mỗi khớp Khớp

có khoảng thời gian dịch chuyển lớn nhất sẽ quyết định thời gian chuyển động Chuyển động của các khớp nối có thể tính toán độc lập với nhau Có rất nhiều cách để tạo ra các chuyển động PTP

3.1.1.1 Hàm tuyến tính (dạng tốc độ hình thang)

Có thể coi đây là trường hợp chuyển động đơn giản nhất với dạng tốc độ tại mỗi khớp là một đường hình thang Tốc độ tại mỗi khớp tăng dần với gia tốc không đổi ai, khi đạt tới tốc độ vi thì gia tốc bằng 0, giữ nguyên vận tốc đến thời điểm hãm, gia tốc bằng –ai cho đến khi vận tốc bằng 0 thì cũng đạt tới vị trí cần đến Thuật toán trong trường hợp này tuy đơn giản và chính xác nhưng có một nhược điểm rất lớn là gia tốc không liên tục, dễ gây ra chấn động hoặc các bộ điều khiển động cơ không đáp ứng kịp

Hình 3.2: Đường đi với dạng tốc độ hình thang

Với dạng tốc độ hình thang, ta có gia tốc bằng a từ t0 đến tv Lúc này khớp đạt tới tốc độ ổn định, đến thời điểm tF - tb , gia tốc là –a, và khớp dừng lại (vận tốc bằng 0) tại tF

Thuật toán nội suy PTP

 Với gia tốc, vận tốc giới hạn cho mỗi khớp và khoảng dịch chuyển, tính ra thời gian chuyển động của mỗi khớp

 Lấy thời gian chuyển động dài nhất trong các khớp để tính lại gia tốc cho các khớp còn lại

a Tính khoảng thời gian dịch chuyển của từng khớp

Từ hình vẽ ta có thể tính ngay khoảng dịch chuyển: (theo t)

0 0

2 max max

max f 2 max 2 f max

q.t4

2 b 2 b

2 f b f 2 f max f

v

b max

v

b

b 2

2 b 2 max b

tt.t

2cos14

tt2

ttt2

ttt2

qtq

1t.qtq

2cos8

tt4

1qt

mà không cần quan tâm đến việc đồng bộ các khớp hay thời gian chuyển động

Để đơn giản và hiệu quả, chúng ta có thể sử dụng kết quả trong trường hợp PTP đồng bộ Giải độc lập các góc khớp nối theo từng đường riêng biệt với các giá trị

3.1.2 Tạo quỹ đạo cho các chuyển động CP

Robot cần được điều khiển chuyển động giữa điểm đầu và điểm cuối theo một quỹ đạo định trước Quỹ đạo này được cho bởi các hàm của các toạ độ của TCP hoặc được cho bởi các điểm tựa Các điểm tựa là các điểm mà quỹ đạo của robot bắt buộc phải qua điểm đó tại thời điểm xác định hoặc vận tốc xác định Quỹ đạo của robot sẽ được nội suy từ các điểm tựa này

Một số khả năng xảy ra:

 Chuyển động theo đường thẳng (TCP theo một đường thẳng)

 Chuyển động tròn (TCP theo một cung tròn)

 Chuyển động quanh trục

 Chuyển động học ngược cần được tính toán lúc hoạt động

3.1.2.1 Nội suy theo đường thẳng

Cho điểm đầu và điểm cuối như sau:

A A A

A , z

A , y

A , x

A

PP

PP

B B B

B , z

B , y

B , x

B

PP

PP

x

Hình 3.4: Nội suy theo đường thẳng để xác định vị trí của TCP

 TCP dịch chuyển giữa các điểm A và B theo một đường thẳng

 Người sử dụng robot đặt tốc độ và gia tốc chuyển động

B

f

A B E A

s

s

t s

Người sử dụng robot đặt tốc độ và gia tốc cho vị trí và hướng

Thời gian chuyển động (trong trường hợp dạng sin) là:

b

v

b

f f

.4

.2

t v t v

t s t s

t t t t

m m

V b V b

Thuật toán cho các chuyển động theo đường thẳng

Hình 3.5: Thuật toán nội suy cho các chuyển động theo đường thẳng

3.1.2.2 Nội suy tròn

Chuyển động của TCP theo một cung tròn từ điểm P1 qua điểm P2 tới điểm P3

P2 đôi khi còn gọi là điểm phụ trợ

Các điểm P1, P2, P3 và tốc độ vC và gia tốc bC được người sử dụng robot xácđịnh

 Từ các điểm P1, P2, P3 tính toán điểm trung tâm M và bán kính r như sau:

E

P P P

P

n

i i i

|3

2 3 1

1 2

1

2

1:,:

2

1:,:

n P P d

P P

n

n P P d

P P

 Các bước tương tự như nội suy theo đường thẳng

Ví dụ cho các điểm P1, P2, P3 xác định một cung tròn, với

P1 = [0 1 1]T, P2 = [1 1 2]T, P3 = [2 1 1]T, Tính điểm trung tâm M và bán kính r