ĐẠI HỌC QUỐC GIA HÀ NỘI
TRƯỜNG ĐẠI HỌC CÔNG NGHỆ
o0o




Đặng Anh Việt






ĐIỀU KHIỂN PHÂN TÁN ROBOT CÔNG NGHIỆP
SEPHENT THEO QUỸ ĐẠO ĐỊNH TRƯỚC








LUẬN VĂN THẠC SĨ

Hà Nội - 2006





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


2
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


3
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




4
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


5
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



6
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


7
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.


8
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ơ 1
(khớp 1)
Bộ điều
khiển
động cơ 2
(khớp 2)
Bộ điều
khiển
động cơ 3
(khớp 3)
Các đầu
vào ra
số/tƣơng
tự
động cơ 1
Phản hồi
vị trí
động cơ 3
Phản hồi
vị trí
động cơ 2

Phản hồi
vị trí


9

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


10

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ơ 1
(khớp 1)
Bộ điều
khiển
động cơ 2
(khớp 2)
Bộ điều
khiển
động cơ 3
(khớp 3)
Các đầu
vào ra
số/tƣơng
tự
động cơ 1
Phản hồi
vị trí
động cơ 3

Phản hồi
vị trí
động cơ 2
Phản hồi
vị trí
Mạng truyền thông


11

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



12
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.


13
 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.


14
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.

Hình 2.1: Robot SHEPHENT
Robot SCARA ( Selectively Compliant Articulated Robot Arm ) có nghĩa là tay
máy lắp ráp chọn lọc. Cấu trúc động học loại tay máy này thuộc hệ phỏng sinh,
có các trục quay, các khớp đều là thẳng đứng. Nó có cấu tạo hai khớp ở cánh tay,
một khớp ở cổ tay và một khớp tịnh tiến, các khớp quay hoạt động nhờ các động
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).


15
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ó:
 Khớp 1 quay quanh trục z
0
, góc 
1

.
 Khớp 2 quay quanh trục z
1
, góc 
2
.
 Khớp 3 chuyển động tịnh tiến theo trục z
2
đoạn d
3
.
 Khớp 4 quay quanh trục z
3
, góc 
4
.
Tuỳ vào vị trí của cánh tay mà các thanh nối cũng nhƣ các khớp sẽ có vị trí khác
nhau trong không gian và ngƣợc lại.
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:


16
Thanh nối
Biến

a

D
1

1

0
a
1
0
2

2

- 180
0

a
2
0
3
0
0
0
d
3
4

4

0

0
0
Bảng 2-1: Tham số của Robot SHEPHENT
Ma trận
0
T
4
là ma trận biểu diễn tay máy Robot trong hệ trục toạ độ gốc:

0
4 1 2 3 4
1 1 1 1
1 1 1 1
1
2 2 2 2
2 2 2 2
2
3
3
44
44
4
. . .
0.
0.
0 0 1 0
0 0 0 1
0.
0.
0 0 1 0

0 0 0 1
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
00
0




























T A A A A
Cos Sin a Cos
Sin Cos a Sin
A
Cos Sin a Cos
Sin Cos a Sin
A
A
d
Cos Sin
Sin Cos a
A
  
  
  
  


0 0 1 0
0 0 0 1








Kí hiệu:

1 1 2 2 3 3
1 1 2 2 3 3
  
  
S Sin S Sin S Sin
C Cos C Cos C Cos
  
  

Ta có:


17

3
44
44
44
3
44
23
4 3 4
4 4 4 4
4 4 4 4
2
4
3 3 3

12
4 2 4
2 2 2 2
2 2 2 2
1
4
1.
00
00
0 0 1 0
0 0 0 1
2. .
1 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0
0 0 1 0 0 1 0 0 1
0 0 0 1 0 0 0 1 0 0 0 1
3. .
0.
0.
0
TA
CS
SC
TA
T A T
C S C S
S C S C
T
d d d
T A T

C S a C
S C a S
T











     
     
     

     
     
     



44
44
3
2 4 2 4 2 4 2 4 2 2
2 4 2 4 2 4 2 4 2 2
1

4
3
0 21
4 1 4
1 1 1 1 2 4 2 4
1 1 1 1
0
4
00
00
0 1 0 0 0 1
0 0 0 1 0 0 0 1
. . 0 .
0.
0 0 1
0 0 0 1
4. .
0.
0.
0 0 1 0
0 0 0 1
CS
SC
d
C C S S C S S C a C
S C C S S S C C a S
T
d
T A T
C S a C C C S S

S C a S
T

   
   
   
   

   
   
  


  















2 4 2 4 2 2

2 4 2 4 2 4 2 4 2 2
3
1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 2 1 2 2 1 2 1 1
1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 1 2 4 2 4 2 1
0.
0.
0 1 1
0 0 1
( ) ( ) ( ) ( ) 0
( ) ( ) ( ) ( ) 0
C S S C a C
S C C S S S C C a S
d
C C C S S S S C C S C C C S S S S C C S a C C a S S a C
S C S S C C S S C C S C S S C C S S C C a S



  



  



        
      
2 2 1 2 1 1
3

0 0 1
0 0 0 1
C a C S a S
d









Ma trận
0
T
4
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:


18

0
4
0 0 0 1
X X X X
Y Y Y Y
Z Z Z Z
n o a p

n o a p
T
n o a p








Từ ma trận trên ta có hệ phƣơng trình động học thuận tay máy Robot:

1 2 4 2 4 1 2 4 2 4
1 2 4 2 4 1 2 4 2 4
1 2 4 2 4 1 2 4 2 4
1 2 4 2 4 1 2 4 2 4
1 1 2
( ) ( ) (2 1)
( ) ( ) (2 2)
0 (2 3)
( ) ( ) (2 4)
( ) ( ) (2 5)
0 (2 6)
0 (2 7)
0 (2 8)
1 (2 9)

X
Y

Z
X
Y
Z
X
Y
Z
X
n C C C S S S S C C S
n S C C S S C S C C S
n
o C C S S C S S S C C
o S C S S C C S S C C
o
a
a
a
p a C a
    
    

     
     



  

12
1 1 2 12

3
(2 10)
. . (2 11)
(2 12)
Y
Z
C
p a S a S
pd

  
  




19
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

3
4 1 2
-
-
z
z
dp

   



Ta cần xác định
12
,


Chiếu robot Shephent lên mặt phẳng Oxy ta có
y
P
y
O x
A
2
A
1
A
1
'
P
x
a
1
a
2
θ
2
θ

3
θ
1
'θ
1
θ
2
'

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
12
,

thoả mãn hệ phƣơng trình động học
thuận
Xét OA
1
A
2
ta có


20
 
 
2 2 2
2 1 2 1 2 2
2 2 2 2
1 2 2 1 2

2 2 2 2
12
22
12
2 os
2 os
cos (2-13)
2
xy
xy
OA a a a a c
a a c a a p p
a a p p
a
aa



  
    

  

  




Ta có


13
=arctan (2-14)
x
y
p
p



Trong OA
1
A
2
 
2 2 2 2 2 2
2 1 1 3
2 2 2 2
21
3
22
1
2 2 2 2
21
3
22
1
2 os
arccos
2
arccos

2
x y x y
xy
xy
xy
xy
a a p p a p p c
a a p p
a p p
a a p p
a p p



    

  





  








Thay
33
, '

vào hệ phƣơng trình động học thuận ứng với mỗi giá trị
2


tìm
đƣợc ở (2-13) ta xác định đƣợc
1

ứng với mỗi giá trị
2



Chọn dấu cho nghiệm
12
,

tìm đƣợc:
Nhƣ đã trình bày ở trên, ta có thể tìm đƣợc 2 bộ nghiệm
12
,

đề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í P
x

, P
y
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
12
,

theo 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
12
,

với các giá trị
2


luôn dƣơng.
Nhƣ vậy ta có hệ phƣơng trình động học ngƣợc của robot Shephent:


21
 
13
2 2 2 2
12
22
12
3

4 1 2
=arctan
cos
2
-
-
x
y
xy
z
z
p
p
a a p p
a
aa
dp


   






  














(2-15)
Việc sử dụng các phƣơng pháp số để giải hệ phƣơng trình động học ngƣợc nói
chung có thể áp dụng trong hầu hết các trƣờng hợp. Giải hệ phƣơng trình động
học ngƣợc của robot ta tính đƣợc giá trị các khớp quay và khớp tịnh tiến.


22
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
PC
Bộ điều
khiển
động cơ 1
(khớp 1)
Bộ điều
khiển
động cơ 2
(khớp 2)
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
Các đầu vào
ra số/tƣơng tự
Các đầu vào

ra số/tƣơng tự
Teach
pendant


23
đ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


24
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
bộ
Modul đóng gói dữ liệu, điều khiển truyền thông
Giao diện
mô phỏng,
giám sát
Giao diện
điều khiển
Giao diện
cài đặt