Bài tập lớn Robot Công nghiệp KuKa Robot (6R)

Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 1

LỜI NÓI ĐẦU

Ngày nay robot đã được áp dụng rộng rãi trong hầu hết các lĩnh vực như sản
xuất, công nghiệp, công nghệ cao như vũ trụ, hàng không, giao thông vận tải…
Các nước phát triển đã xuất hiện xu hướng tạo ra những dây chuyền và thiết bị
tự động có tính linh hoạt cao và xu hướng này đã đạt được khá nhiều kết quả
khả quan. Chính vì thế việc nghiên, ứng dụng và phát triển các hệ thống sả
n
xuất tự động linh hoạt ngày càng trở nên cấp thiết, nhất là các loại robot. Robot
công nghiệp có cấu trúc động học nối tiếp tuy đã được nghiên cứu và phát triển
mạnh song những tính toán và ứng dụng của nó luôn luôn là một lĩnh vực rất
phong phú, đa dạng và đây cũng là loại robot thông dụng nhất hiện nay. Trong
đó có KUKA KR - một loại tay máy 6 bậc tự do hoạt động rất linh hoạt, được
ứng dụng nhi
ều lĩnh vực sản xuất và trong công nghiệp. Trong đồ án này sẽ
nghiên cứu loại robot này về các phần động học, động lực học, miền làm việc
cũng như phần mô phỏng lập trình sử dụng các phần mềm hỗ trợ.


Hà Nội, 5/2011

Vũ Ngọc Trọng
Cơ điện tử 2 – K52

Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 2

CHƯƠNG 1
TÌM HIỂU TỔNG QUAN VỀ ROBOT CHUỖI HỞ
1.1. Sự ra đời của robot
Thuật ngữ “robot” lần đầu tiên xuất hiện năm 1922 trong tác phẩm
“Rossum’s Universal Robot” của Karel Capek. Theo tiếng Séc thì robot là
người làm tạp dịch. Trong tác phẩm này, nhân vật Rossum và con trai của ông
đã tạo ra những chiếc máy gần giống như con người để hầu hạ con người.
Hơn 20 năm sau, ngay sau chiến tranh thế giới lần thứ 2, ở Hoa Kì đã xuất
hiện những tay máy chép hình điều khiển từ xa trong các phòng thí nghiệ
m về
vật liệu phóng xạ.
Chiếc robot công nghiệp được đưa vào ứng dụng đầu tiên, năm 1961 ở một
nhà máy ô tô của General Motors tại Trenton, New Jersey Hoa Kì.
Năm 1967 Nhật Bản mới nhập chiếc robot công nghiệp đầu tiên từ công ty
AMF của Hoa Kì (American Machine and Foundry Company). Đến năm 1990
đã có hơn 40 công ty Nhật Bản, trong đó có những công ty khổng lồ như
Hitachi và Mitsubishi đã đưa ra thị trường nhiều loại robot nổi tiếng. Cho đến
nay, công nghệ
chế tạo robot đã phát triển mạnh và được ứng dụng nhiều trong
đời sống và sản xuất.
1.2. Phân loại robot
1.2.1. Phân loại theo số bậc tự do (BTD) trong trường công tác
Robot có 2 loại chuyển động cơ bản làm chuẩn :

- Chuyển động thẳng theo các trục X, Y, Z trong không gian Đề-các với các
khớp lăng trụ hay khớp trượt, kí hiệu là P hoặc T.
- Chuyển động quay quanh các trục X, Y, Z kí hiệu là R.
Tùy số BTD và và cách tổ hợp T-R, robot sẽ hoạ
t động trong trường công tác
với các hình khối khác nhau. Ví dụ: Robot có trường làm việc là hình trụ như
RRT, RTT…
1.2.2. Phân loại theo cấu trúc động học
Một phương pháp phân loại Robot chính là phân loại theo câu trúc hình học
của chúng.
Một robot được gọi là robot chuỗi hở (robot tuần tự - serial robot) nếu cấu
trục động học của chúng có dạng một chuỗi động hở; gọi là robot song song
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 3

(parallel robot) nếu cấu trúc động học của chúng có dạng một chuỗi đóng; và
gọi là robot hỗn hợp nếu nó bao gồm cả 2 loại này.
1) Robot chuỗi hở (robot nối tiếp – Serial Manipulators)
Đây là dạng phổ biến nhất của robot công nghiệp. Thường có dạng cấu trúc
giống cánh tay người, là một chuỗi các khâu rắn liên kết với nhau bằng các
khớp (joints) có dạng như vai, khuỷu tay và cổ tay. Robot này có ưu điểm chính
là vùng làm việc (workspace) r
ộng và linh động. Dưới đây ta đưa ra các bộ
phận chính của một robot công nghiệp:
- Cấu trúc cơ khí gồm các khâu
(links) và khớp (joints),
- Cơ cấu chấp hành (actuators) tác
động tại các khớp làm tay máy
chuyển động,
- Cảm biến (sensors) dùng để đo

lường các trạng thái của tay máy
(cảm biến trong) và trạng thái môi
trường (cảm biến ngoài) nếu cần,
- Một hệ thống điề
u khiển (có cả
máy tính) để điều khiển và giám
sát chuyển động của tay máy.
Nhược điểm:
- Đặc điểm cố hữu là độ cứng vững thấp để tạo một cấu trúc động học.
- Sai số tích lũy được khuếch đại từ khâu trước sang khâu sau.
- Do phải mang và chuyển động cùng các bộ dẫn động (thường là các động cơ)
có khối lượng lớn nên tố
c độ nên tốc độ làm việc không cao.
- Khả năng mang tải không cao.
Robot loại này yêu cầu phải có ít nhất 6 BTD trong không gian (ứng với 6
BTD của đối tượng làm việc) để có thể tiếp cận được đối tượng ở vị trí bất kì
với khả năng định hướng trong miền làm việc của nó. Chính vì vậy chủ yếu là
robot có 6 khớp, như các loại robot Stanford (RRTRRR), KUKA (6R), Elbow
(6R), Puma (6R) …với ứng dụng chính trong công nghiệp hiện nay là lắp ráp,
vận chuyển phôi và sản phẩm trong quá trình sản xuất.


Hình 1.1. IRB660-palletiser của hãng
ABB Robotics.

Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 4

2) Robot song song (Parallel Manipulator)
Loại robot song song điển hình thường có bàn chân di chuyển được nối với

giá cố định bằng một số nhánh hay chân. Do vậy, các khâu chỉ chịu lực kéo
hoặc lực nén, không phải chịu uốn. Thường số chân chính bằng số BTD, các
chân được điều khiển bằng một nguồn phát động đặt trên giá cố định hoặc ngay
trên thân của chân.
Ưu điểm:
- Khả năng chịu tải l
ớn vì tải trọng được
chia cho các chân,
- Tốc độ cao hơn và chính xác hơn, cấu
trúc gọn nhẹ hơn,
- Do tốc độ cao và chính xác cao nên có
thể hoạt động như một máy phay, ngày
nay đã được áp dụng vào việc phay các
bề mặt gia công cần độ chính xác cao,
năng suất.
- Nhiều ứng dụng: Mô phỏng tạo song,
ghế lái máy bay (Steward, 1965), bàn khung lắp ráp (Reinholtz và Gokhale,
1987), bàn điều khiển dao, gá của máy phay (Arai, 1991), các thiết bị định điể
m
(Gosselin và Hamel, 1994), các máy chuyển động có chân bước (Waldron,
1984).
Tuy nhiên nó cũng có một số nhược điểm vì gặp phải một số trở ngại như
không gian làm việc nhỏ, các chân có thể va chạm lẫn nhau cũng như việc thiết
kế rất khó khăn.
3) Robot di chuyển (Mobile Robot)
Đơn giản như loại ôtô, xe đạp với các bánh lái có 2 BTD. Hiện nay dùng
trong chuyên chở vật liệu trong nhà may rộng, bệnh viện, văn phòng …
4) Robot giống người (Humanoid Robot)
Nói mộ
t cách đơn giản, robot này có dạng hình dáng con người, 2 chân, 2

tay, đầu, và dung các cơ cấu đó với chuyển động như các bộ phận của con
người.
Hình 1.2. ABB’s IRB340 palletising robot
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 5

Các kiểu phân loại khác như phân loại theo Hệ thống truyền động, theo
Phương pháp điều khiển, theo Đặc điểm hoạt động… tùy các tiêu chí đặt ra mà
robot được phân loại nhằm nêu bật tiêu chí đó.
1.3. Tay máy (Manipulator)
Thuật ngữ “tay máy” và robot trong quan niệm của nhiều nhà chuyên môn
trong lĩnh vực này không có sự khác biệt. Để thuận tiện trong trình bày, ở đây
ta hiểu:
Tay máy là một dạng robot có cấu tạo mô phỏng theo những đặc đi
ểm cấu
tạo cơ bản của cánh tay người. Cũng có thể hiểu tay máy là tập hợp các bộ phận
và cơ cấu cơ khí được thiết kế để hình thành các khối có chuyển động tương đối
nhau, được gọi là các khâu động. Trong đó phần liên kết giữa các khâu động
được gọi là các khớp động hay còn gọi là các phần tử thực sự thực hiện các
chuyển động để vậ
n hành tay máy như động cơ điện, xylanh dầu ép, xylanh khí
nén ,… Phần quan trọng khác trên các tay máy là bộ phận hay khâu tác động
cuối để thao tác trên đối tượng làm việc – thường là các tay gắp hoặc các đầu
công cụ chuyên dùng.
Tay máy hay có thể gọi là cánh tay cơ khí của robot công nghiệp thông
thường là một chuỗi động hở được tạo thành từ nhiều khâu được kết nối với
nhau nhờ các khớp động. Khâu cuối của tay máy thường có dạng một tay g
ắp
hoặc được gắn dụng cụ công tác. Mỗi khâu động trên tay máy có nguồn dẫn
động riêng, năng lượng và chuyển động truyền đến cho chung được điều khiển

trên cơ sở tín hiệu nhận được từ bộ phận phản hồi là các cảm biến nhằm thông
báo trạng thái hoạt động của các khâu chấp hành, trong đó vấn đề được đặc biệt
quan tâm là vị trí vận tốc dị
ch chuyển của khâu cuối – khâu thể hiện kết quả
tổng hợp các chuyển đông của các khâu thành phần.
1.4. Một số robot chuỗi hở thường gặp
1.4.1. Tay máy tọa độ vuông góc (Cartesian robot/ Gantry robot)
Robot hoạt động trong hệ tọa độ này được minh họa như hình vẽ gồm ba
chuyển động định vị X,Y,Z theo các trục tọa độ vuông góc:
Loại robot này thích hợp với công việc gắp vật thể và đặt chúng vào vị trí
khác. Các ứng dụng có thể là đóng gói, lắp ráp máy móc, cầm nắm các máy
công cụ hoặc hàn theo hình cung. Đối với loại robot này, cánh tay được tạo
thành từ 3 khớp trượt mà trục của chúng trùng với hệ trục tọa độ Đê-các.
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 6

Ưu điểm:
- Không gian làm việc lớn, có thể
dài đến 20m,
- Đối với loại gắn trên trần sẽ dành
được diện tích sàn lớn cho các
công việc khác,
- Hệ thống điều khiển đơn giản.
Hạn chế: Việc thêm vào các loại cần
trục hay các loại thiết bị vận chuyển vật
liệu khác trong không gian làm việc
của robot không được thích hợp
lắm.Vi
ệc duy trì vị trí của các cơ cấu dẫn động và các thiết bị dẫn động điện đối
với loại trên đều gặp nhiều trở ngại.

1.4.2. Tay máy tọa độ trụ (Cylindrical robot)
Trong ba chuyển động chính , robot được trang bị hai chuyển động tịnh tiến
và một chuyển động quay.
Sử dụng trong công nghệ lắp ráp,
cầm nắm các máy công cụ, hàn
điểm. Loại robot này, trục có dạng
hệ tọa độ trụ.
Ưu điểm :
- Có khả năng chuyển động ngang
và sâu vào trong các máy sản xuất,
- Cấu trúc theo chiều dọc của máy
để lại nhiều khoảng trống cho sàn,
- Kết cấu vững chắc, có khả năng
mang tải lớn,
- Khả năng lặp lại tốt.
Nhược điểm duy nhất là giới hạn tiến về phía trái và phía phải do kết c
ấu cơ
khí và giới hạn các kích cỡ của cơ cấu tác động theo chiều ngang.



Hình 1.3. Tay máy tọa độ vuông góc
(Cartesian Robot)

Hình 1.4. Tay máy tọa độ trụ (Cylinderical
Robot)

Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 7

1.4.3. Tay máy tọa độ cầu (Spherical/Polar robot)
Robot loại này được bố trí có ít nhất hai
chuyển động quay trong ba chuyển động
định vị. Dạng robot này là dạng sử dụng
điều khiển servo sớm nhất.
Được sử dụng để cầm nắm các dụng cụ,
hàn điểm, đánh bóng, hàn hơi hoặc hàn
theo cung. Trục của robot tạo thành hệ tọa
độ cầu.
1.4.4. Tay máy toàn khớp bản lề
Loại cấ
u hình dễ thực hiện nhất được
ứng dụng cho robot là dạng khớp nối bản
lề và kế đó là dạng ba trục thẳng, gọi tắt là dạng SCARA (Selective Compliance
Assembly Robot Arm). Dạng này và dạng tọa độ trụ là phổ cập nhất trong ứng
dụng công nghiệp bởi vì chúng cho phép các nhà sản xuất robot sử dụng một
cách trực tiếp và dễ dàng các cơ cấu tác động quay như các động cơ đi
ện, động
cơ dầu ép, khí nén.
Được sử dụng để gắp nhả vật, đóng gói, lắp ráp hay cầm nắm máy công cụ.
Robot có 2 khớp xoay song song với nhau cho phép thao tác trên mặt phẳng.
Ưu điểm :
- Mặc dù chiếm diện tích làm
việc ít song tầm vươn khá lớn,
tỉ lệ kích thước - tầm vươn
được đánh giá cao.
- Về mặt hình học, cấu hình dạng
khớp nố
i bản lề với ba trục
quay bố trí theo phương thẳng

đứng là dạng đơn giản và có
hiệu quả nhất trong trường hợp yêu cầu gắp đặt và đặt chi tiết theo
phương thẳng. Trong trường hợp này bài toán tọa độ hoặc quỹ đạo
chuyển động đối với robot chỉ cần giải quyết ở hai phương x và y còn lại
bằng cách phối hợp ba chuyển động quay quanh ba trục song song vớ
i
trục z.

Hình 1.5. Tay máy tọa độ cầu (Polar
Robot)
Hình 1.6. Robot SCARA
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 8

1.4.5. Về KUKA Robotics



Hình 1.7. Robot KUKA KR 6/2 và 15/2 của KUKA Robotics
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 9

KUKA được thành lập vào năm 1898 tại
Augsburg, Đức, tên là Keller und Knappich
Augsburg . Có công ty mẹ là KUKA Roboter
GmbH, đặt tại Augsburg - Đức, KUKA
Robotics là một trong những nhà sản xuất hàng
đầu thế giới về robot công nghiệp với khối
lượng hàng năm đạt gần 10.000 con. Dải robot 5
và 6 trục có tải trọng từ 3kg đến 570 kg và tầm

với từ 635mm đến 3700mm, đều được điều
khiển từ một nền t
ảng bộ điều khiển dựa trên
PC; riêng KUKA KR 1000 – robot khỏe nhất
thế giới của KUKA, là loại robot có 6 trục được
làm bằng titan. Khả năng nâng của nó lên đến
1000 kg và tầm với là 3200 mm. Và nó được thiết kế nhắm tới các ứng dụng
đặc biệt nặng. Do vậy, nó rất lý tưởng cho các ngành công nghiệp như sản xuất
vật liệu xây dựng, ôtô và giặt là.
Robot của KUKA được sử dụng rộng rãi trong nhiề
u ngành công nghiệp,
gồm sản xuất thiết bị gia dụng, sản xuất ô tô, không gian, sản xuất hàng tiêu
dùng, chế biến thực phẩm, dược, y, đúc khuôn, nhựa, và trong nhiều ứng dụng
phức hợp khác như vận chuyển vật, lắp ráp, đóng gói, xếp-dỡ pallet, hàn, uốn,
đánh bóng bề mặt. Thông tin chi tiết về tập đoàn có tại trang web
www.kukarobotics.com.


Hình 1.8. KUKA KR 1000 – robot
khỏe nhất thế giới của KUKA

Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 10

Vài hình ảnh khác về robot của hãng KUKA :


Hình 1.9. Zepnick Solutions đã chọn
KUKA Robotics cung cấp Robot bằng
thép không gỉ KUKA KR 15 SL cho hệ

thống xử lý và đóng gói thực phẩm
mới của công ty.

















Hình 1.10. Một khoang làm việc của robot
hãng KUKA đang thao tác với các thùng đồ
uống.
Hình 1.11. Một khoang làm
việc của robot đang bỏ bánh
mì nhặt từ băng chuyền vào
hộp.


Hình 1.12. Hệ thống robot tự động
phục vụ cho các quầy bar của hãng

Motoman đang dùng một trong 2 bộ
kẹp để lấy cốc.

Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 11

1.5. Ứng dụng của robot
Ứng dụng chủ yếu của robot công nghiệp là hàn và lắp ráp. Gần 25% robot
công nghiệp là robot hàn. Các robot lắp ráp chiếm 33% dân số robot trên thế
giới, có mặt nhiều nhất trong các nhà máy sản xuất xe hơi và đồ điện tử.
.

Hình 1.14. Các cánh tay robot hàn.





Hình 1.13. Robot hàn Motoman EA1400
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 12

CHƯƠNG 2
TÍNH TOÁN ĐỘNG HỌC VÀ ĐỘNG LỰC HỌC ROBOT
KUKA 6 BẬC TỰ DO

2.1. Tính toán động học robot KUKA 6 bậc tự do
Đây là bước cơ sở cho việc
thiết kế sơ bộ robot, từ đó có
thể giải bài toán điều khiển

robot theo các quỹ đạo. Từ đây
ta mới có đủ các thông số để
điều khiển robot theo một quỹ
đạo cho trước hoặc với lực cho
trước ta thu được một quỹ đạo
chuyển động nhất
định.
Dưới đây là phần tính toán
động học cho Robot 6 bậc tự do
mà cụ thể ứng dụng giải bài
toán động học cho Robot
KUKA KR6/2.


Thực chất của việc giải bài toán động học là đi giải 2 bài toán động học sau:
+ Bài toán động học thuận: Biết các góc quay của các khớp, tìm vị trí và hướng
của khâu chấp hành cuối cùng.
+ Bài toán động học ngược: Cho biết vị trí của khâu chấp hành cuối, tìm các
góc quay của các khớp tương ứng, tức là tìm các thông số điều khiển cho từng
khâu để đảm bảo vị trí của khâu chấ
p hành.

Do tính chất của công việc của từng robot mà quỹ đạo hoạt động của nó tạo
ra một số bài toán về điều khiển như sau:
- Nếu không quan tâm đến quỹ đạo chuyển động mà chỉ quan tâm đến việc
trong khoảng thời gian cho trước, robot đi tới đúng vị trí điểm làm việc.
Do vậy mà bài toán trở nên đơn giản hơn. Nó chỉ cần thỏa mãn các đi
ều
Hình 2.1. Các khớp của Robot KUKA
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)

Vũ Ngọc Trọng – CDT2_K52 13

kiện trong phần thiết kế quỹ đạo là được; tức là vận tốc điểm đầu và
điểm cuối bằng 0.
- Do điều kiện làm việc mà khi di chuyển từ điểm đầu đến điểm cuối,
chúng bắt buộc phải đi qua một số điểm trung gian nhất định. Dạng này
chúng ta có thể dung phép nội suy để tạo ra một quỹ đạ
o trơn đi qua các
điểm đó. Điều kiện liên tục về quỹ đạo và vận tốc được thảo mãn.
- Khâu chấp hành phải đi theo một quỹ đạo cho trước và có thể biết trước
luật chuyển động dọc theo quỹ đạo nào đó. Đây là bài toán điều khiển
robot đi theo một quỹ đạo với vận tốc biết trước.
Ở đ
ây chúng ta trình bày phần tính toán cho trường hợp này: Điều khiển
robot đi theo một quỹ đạo cho trước.
2.1.1. Phương trình động học thuận

Trục Giới hạn chuyển động Tốc độ (
0
/sec)
1


185
0
152
2 +115
0
tới -55
0

152
3 +70
0
tới -210
0
152
4


350
0

250
5


130
0

357
6


350
0

660

Bảng 2.1. Giới hạn chuyển động của các khớp của robot KUKA
1) Bộ thông số DH

Ta xây dựng mối quan hệ động học thông qua bộ thông số DH :
Theo Denavit & Hartenberg, hai ông đã đề xuất dùng ma trận thuần nhất 4x4
để mô tả quan hệ giữa 2 khâu liên tiếp trong cơ cấu không gian.
Trước hết, xác định bộ thông số cơ bản giữa 2 trục quay của 2 khớp động i+1
và i:
- a
i
là độ dài đường vuông góc chung giữa 2 trục khớp động i+1 và i,
- 
i
là góc chéo giữa 2 trục khớp động i+1 và i,
- d
i
là khoảng cách đo dọc trục khớp động i kể từ đường vuông góc chung
giữa trục khớp động i+1 và trục khớp động i tới đường vuông góc chung
giữa trục khớp động i và trục khớp động i-1.
- θ
i
là góc giữa 2 đường vuông góc nói trên.
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 14

Biến khớp :
- Nếu khớp động i là khớp quay thì θ
i
là biến khớp
- Nếu khớp động i là tịnh tiến thì d
i
là biến khớp
Để kí hiệu biến khớp, ta dùng thêm dấu * và trong trường hợp khớp tịnh tiến

thì a
i
được xem là bằng 0.
Hình 2.2 biểu diễn các khớp
quay đặt trên robot KUKA KR 6/2.
Trong đó có thể gọi các khớp
A1, A2, A3 lần lượt là khớp hông
(waist), khớp bả vai (shoulder),
khớp khuỷu tay (elbow); tập hợp cả
3 khớp này gọi là cánh tay (arm),
các khớp A1, A2, A3 này quyết
định vị trí của khâu chấp hành cuối
(Điểm P trên hình 2.3 dưới đây).
Các khớp A4, A5, A6 còn lại
được gọi chung là khớp cổ tay
(wrist), chúng quyết
định hướng
của khâu chấp hành cuối và có tâm
quay tại điểm P.
2) Thiết lập các hệ tọa độ
Khi gắn các hệ tọa độ với các khâu, có một số lưu ý sau:
Gốc của hệ tọa độ gắn liền với khâu thứ i (gọi là hệ tọa độ thứ i) đặt tại giao
điểm giữa 2 đường vuông góc chung (a
i
) và trục khớp động i+1. Trường hợp 2
trục giao nhau thì gốc hệ tọa độ lấy trùng với giao điểm đó. Nếu 2 trục song
song với nhau thì chọn gốc hệ tọa độ là điểm bất kì trên trục khớp động i+1.
Trục z
i
của hệ tọa độ thứ i nằm dọc theo trục khớp động i+1.

Trục x
i
của hệ tọa độ thứ i nằm dọc theo đường vuông góc chung và hướng
từ khớp động i đến khớp động i+1. Trường hợp 2 trục giao nhau thì hướng của
trục x
i
trùng với hướng của vectơ tích z
i
 z
i-1
, tức là vuông góc với mặt phẳng
chứa z
i
, z
i-1
và phương của vectơ tích có hướng.

Hình 2.2. Các khớp của Robot KUKA KR 6/2.
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 15

Hình vẽ sau đây thể hiện các hệ tọa độ đặt trên robot KUKA KR 6/2 và các
thông số động học theo bảng DH:


Hình 2.3. Sơ đồ bố trí các hệ tọa độ và các thông số động học cho Robot KUKA.

Sau khi gắn các hệ trục tọa độ trên các khâu và khớp của robot theo nguyên
tắc đã trình bày ở trên, ta có các thông số cho bảng DH như sau:

Khâu θ
i
d
i
a
i


i

Khớp
1 θ
1
*
0 a
1
= 300 90
0
R
2 θ
2
*
0 a
2
= 650 0
0
R
3 θ
3
*

0 a
3
= 155 90
0
R
4 θ
4
*
d
4
= 600 0 -90
0
R
5 θ
5
*
0 0 90
0
R
6 θ
6
*
d
6
= 125 0 0
0
R

Bảng 2.2. Bảng thông số DH.
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)

Vũ Ngọc Trọng – CDT2_K52 16

3) Mô hình biến đổi
Trên cơ sở đã xây dựng các hệ tọa độ với 2 khâu động liên tiếp như trên đã
trình bày, ta có thể thiết lập mối quan hệ giữa 2 hệ tọa độ liên tiếp theo 4 bước
sau đây:
1 - Quay quanh trục z
i-1
một góc θ
i
2 - Tịnh tiến dọc trục z
i-1
một đoạn d
i
3 - Tịnh tiến dọc trục x
i-1
(đã trùng với x
i
) một đoạn a
i
4 - Quay quanh trục x
i
một góc α
i
.
Bốn bước này được biểu diễn bằng tích các ma trận thuần nhất sau:
A
i
= R(z,θ
i

).T
p
(0,0,d
i
).T
p
(a
i
,0,0).R(x,α
i
) (2.1)
Sau khi thực hiện các phép nhân các ma trận đơn giản (quay, tịnh tiến), ta có:

iiiiiii
iii iiii
i
iii
CSCSSaC
SCC CSaS
A
0S C d
00 0 1
 
  











(2.2)
Thay các giá trị trong bảng DH vào công thức A
i
ta tính được các ma trận A
i

như sau:
1111
1111
0
1
C0S aC
S0 CaS
A
01 0 0
00 0 1









22 22

22 22
1
2
CS0aC
SC0aS
A
0010
0001















3333
3333
2
3
C0S aC
S0 CaS
A

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

44
44
3
4
i4
C0S0
S0C0
A
01Cd
00 0 1


















55
55
4
5
C0S 0
S0C0
A
01 0 0
00 0 1









66
66
5
6

6
CS00
SC00
A
001d
0001


















Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 17

4) Phương trình động học
Ma trận T
i

là tích các ma trận A
i
và là ma trận mô tả vị trí và hướng của hệ
tọa độ gắn liền với khâu thứ i, so với hệ tọa độ cố định: T
i
= A
1
A
2
…A
i
, i = 1 n
Trong trường hợp i = n, với n là số hiệu chỉ hệ tọa độ gắn liền với điểm tác
động cuối (End-Effector) thì ta có:
T
n
= A
1
A
2
…A
n
cũng chính là ma trận trạng thái cuối T
E
: T
E
= T
n

hay:

xx x x
yy y y
n
zz z z
nsap
nsap
T
nsap
0001








Ở đây ta gọi tọa độ điểm cuối là D(d
x
, d
y
, d
z
) còn điểm P quyết định vị trí của
D.
Vì vậy vị trí và hướng của khâu chấp hành cuối được cho bởi ma trận :

xx x x
yy y y
0012345

6123456
zz z z
nsad
nsad
A A.A.A.A.A.A
nsad
0001













(2.3)
- Nhân 3 ma trận đầu với nhau ta được:
0012
3123
AA.A.A






123 1 123 1 1 22 323
123 1 123 1 1 22 323
23 23 2 2 3 23
CC SCSCaaCaC
SC C SS S a a C aC
S0C aSaS
000 1










(2.4)
- Nhân 3 ma trận còn lại:
3345
6456
AA.A.A


4 5 6 46 4 56 4 6 45 6 45
4 5 6 46 4 56 4 6 45 645
56 56 5 4 65
CCC SS CCS SC CS dCS
SCC CS SCS CC SS dSS
SC SS C d dC

0001
 









(2.5)

Ta có phương trình cân bằng sau:

003
636
AA.A (2.6)
Trong các phương trình trên: C
i


cosθ
i
S
i


sinθ
i

C
ij


cos(θ
i
+ θ
j
) S
ij


sin(θ
i
+ θ
j
)
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 18

Thay các phương trình (2.3), (2.4), (2.5) vào PT (2.6) và cân bằng các phần tử
ta có hệ 12 phương trình:
n
x
= C
1
[C
23
(C
4

C
5
C
6
– S
4
S
6
) – S
23
S
5
C
6
] + S
1
(S
4
C
5
C
6
+ C
4
S
6
)
n
y
= S

1
[C
23
(C
4
C
5
C
6
– S
4
S
6
) – S
23
S
5
C
6
] – C
1
(S
4
C
5
C
6
+ C
4
S

6
)

n
z
= S
23
(C
4
C
5
C
6
– S
4
S
6
) + C
23
S
5
C
6
s
x
= C
1
[-C
23
(C

4
C
5
C
6
+ S
4
S
6
) + S
23
S
5
C
6
] + S
1
(-S
4
C
5
S
6
+ C
4
C
6
)

s

y
= S
1
[-C
23
(C
4
C
5
C
6
+ S
4
S
6
) + S
23
S
5
C
6
] – C
1
(-S
4
C
5
S
6
+ C

4
C
6
)

s
z
= - S
23
(C
4
C
5
C
6
+ S
4
S
6
) – C
23
S
5
S
6

a
x
= C
1

(C
23
C
4
S
5
+ S
23
C
5
) + S
1
S
4
S
5

a
y
= S
1
(C
23
C
4
S
5
+ S
23
C

5
) – C
1
S
4
S
5

a
z
= S
23
C
4
S
5
+ C
23
C
5

d
x
= C
1
[a
1
+ a
2
C

2
+ a
3
C
23
+ d
4
S
23
+ d
6
(C
23
C
4
S
5
+ S
23
C
5
)] + d
6
S
1
S
4
S
5
d

y
= S
1
[a
1
+ a
2
C
2
+ a
3
C
23
+ d
4
S
23
+ d
6
(C
23
C
4
S
5
+ S
23
C
5
)] – d

6
S
1
S
4
S
5
d
z
= a
2
S
2
+ a
3
S
23
+ d
4
C
23
+ d
6
(S
23
C
4
S
5
– C

23
C
5
).
Như vậy khi biết được các giá trị của θ
i
ta hoàn toàn có thể xác định được
hướng và vị trí của khâu chấp hành cuối bằng cách thay các giá trị này vào 12
phương trình trên.
Bài toàn động học thuận có thể dùng MATLAB để lập một hàm hoặc chương
trình chung để giải ra 12 nghiệm cả dưới dạng chữ hoặc tính toán với số, được
trình bày ở phần phụ lục.
2.1.2. Phương trình động học ngược
Theo phương pháp chung khi giải bài toán động học ngược, với n = 6, khó
khăn là ở chỗ làm sao xác định được phương trình nào trong các hệ phương
trình để tính ra được các góc θ
i
ở dạng công thức với các kí hiệu tổng quát.
Có thể nói chưa có một thuật toán chung nào mà nhờ đó có thể tìm ra được
tập nghiệm của bài toán động học ngược cho mọi robot. Phương pháp giới thiệu
sau đây có thể xác định được θ
i
dễ dàng và trực giác hơn: Phương pháp “các
nhóm 3”:
Như đã biết, vị trí của một điểm hoàn toàn được xác định trong hệ tọa độ
không gian 3 chiều, nên có thể phân robot ra những nhóm 3, nói chung được
viết thành n = 3+3+3+… Tuy nhiên, gốc tọa độ của những nhóm 3 kể từ nhóm
thứ 2 trở đều là đối tượng cần được xác định. Còn đối với những robot dùng
trong công nghiệp thường có không quá 6 BTD nên việc phân nhóm 3+3 lại
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)

Vũ Ngọc Trọng – CDT2_K52 19

phù hợp với các thao tác trong thực tế. Thao tác đầu tiên là robot đưa bàn kẹp
đến gần vị trí làm việc. Có thể gọi tên là thao tác “thô”. Sau đó bàn kẹp được xê
dịch nhỏ và xoay hướng sao cho phù hợp với công việc, gọi là thao tác “tinh”.
Vậy các nhóm 3+3 tương ứng với các thao tác “thô + tinh”.
Việc giải bài toán động học ngược trở nên đơn giản hơn nhưng phải lưu ý
đến các vấn đề sau:
-
Đối với nhóm 3 thực hiện thao tác “thô” thì thường chỉ quan tâm đến vị
trí, mà không quan tâm đến định hướng nên các thành phần của các vectơ
n, s, a đều bằng 0.
-
Đối với các nhóm 3 thực hiện thao tác “tinh” thì ngược lại, cho các thành
phần của vectơ p bằng 0.
Nhược điểm của phương pháp này, như đã nói ở trên, gốc tọa độ của các
nhóm 3, kể từ nhóm thứ 2 trở đi đều là những vị trí cần xác định hoặc cho
trước. Thông thường người ta cho trước chúng như những điểm tựa công nghệ.
Mặc dù có thể dùng hệ 12 PT trong phần tính toán động h
ọc thuận trên để
giải bài toán động học ngược nhưng do chúng đều là các phương trình phi
tuyến nên rất khó giải. Tuy vậy, ở đây KUKA là loại robot có đặc điểm 6 khớp
đều là khớp quay, trong đó tâm trục quay của 3 khớp cuối giao nhau tại 1 điểm,
thông thường với cấu trúc này thì mỗi điểm trong không gian robot sẽ có 8 khả
năng để đạt được đến điểm này, hay nói cách khác, với mỗi
điểm xác định thì
bài toán động học ngược sẽ có 8 nghiệm.
Vì vậy, để giải bài toán động học ngược ta chia làm 2 bước, bước 1 dựa vào
phương pháp hình học giải tích để tìm các góc liên quan đến vị trí của khâu
chấp hành cuối (θ

1
, θ
2
, θ
3
) – “Phương trình động học ngược – Vị trí” sau đó
bước 2 có nhiệm vụ tính nốt các góc còn lại từ các góc đã tìm được –
“Phương
trình động học ngược – Hướng”
.
Từ các thành phần của phương trình (2.6) và 12 phương trình thành phần ta
có thể biết được vị trí của điểm tâm quay cổ tay (điểm P trên hình 1.15):

xx6x
yy6y
zz6z
p
dda
p
dda
p
dda

















(2.7)


Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 20

1) Phương trình động học ngược - Vị trí (Position)
Trên hình vẽ 2.4 là hình chiếu bằng của cánh tay robot trong 2 trường hợp
của góc θ
1
.

Hình 2.4. Hình chiếu bằng của cánh tay
- Góc θ
1
được tính từ P
y
và P
x
:
θ
1

= atan2(P
y
, P
x
); (2.8)
Theo hình vẽ, θ
1
có 2 nghiệm là θ
1
và (θ
1
+ 180
0
), cũng đều thỏa mãn nghiệm
của phương trình (2.8). Trường hợp P
x
= P
y
= 0, phương trình (2.8) sẽ có vô số
nghiệm, do vậy hàm atan2 sẽ chọn nghiệm của θ
1
= 0.
- Góc θ
3
được tính dựa vào hình 2.5:

Hình 2.5. Hình chiếu vuông góc với mặt phẳng cánh tay
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 21

Sử dụng định lí hàm số cos, ta có:
22 222 22
234 234 3
q + r = a a d 2a a d.cos(180 )  
(2.9)

2
33343
atan2( 1 D ,D ) atan2(d ,a );    
(2.10)
với
22 2 2 2
234
3
22
23 4
qraad
D
2a a d



; (2.11)
- Góc θ
2
tính dựa vào góc ψ và γ như trên hình vẽ 2.5.
Ta có:
22222 22
342 2
a d = a q + r 2a q + r .cos 

(2.12)

2
22
atan2( 1 D ,D );  
(2.13)
với
22222
342
2
22
2
adaqr
D
2a q r



; (2.14)
Từ đó:
θ
2
= γ  ψ = atan2(r,q) –
2
22
atan2( 1 D ,D );
(2.15)
Như vậy, các góc (θ
1
, θ

2
, θ
3
) đã tính được và tổ hợp của chúng sẽ cho ra 8
nghiệm của bộ (θ
1
, θ
2
, θ
3
).

2) Phương trình động học ngược - Hướng (Orientation)
Hướng của khâu chấp hành cuối (gắn dụng cụ) được giải sau khi đã biết
được các góc vị trí (θ
1
, θ
2
, θ
3
).
Trong công thức (2.6), nếu chỉ xét đến thành phần định hướng (ma trận R
3x3
),
ta có:

003
636
RR.R

30T0
636
R(R).R (2.16)

0
R
6
đã được giải từ phần động học thuận, còn
0
R
3
đã có khi giải ra các góc
(θ
1
,θ
2
,θ
3
) theo PT động học ngược – vị trí ở trên.
Còn
3
R
6
bao gồm các thành phần ma trận quay của (θ
4
,θ
5
,θ
6
) theo (2.5) và là

các giá trị cần được xác định:

456 46 456 46 45
3
6 456 46 456 46 45
56 56 5
CCC SS CCS SC CS
RSCCCSSCSCCSS
SC SS C









(2.17)
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 22

Giả sử các thành phần của tích của 2 ma trận vế phải (2.16) sau khi tính được
là ma trận [b
ij
]
3x3
:
(
0

R
3
)
T
.
0
R
6
=
1 23 1 23 23 x x x
11 yyy
1 23 1 23 23 z z z
CC SC S n s a
SC0.nsa
CS SS C n s a

















=
11 12 13
21 22 23
31 32 33
b
bb
b
bb
b
bb










(2.18)
Từ đó, sau khi nhân được vế phải theo (2.18) ra các giá trị b
ij
rồi đồng nhất
với các phần tử của (2.17) ta có được các nghiệm (θ
4
,θ
5
,θ

6
):
Có 2 bộ nghiệm của (θ
4
, θ
5
, θ
6
):
θ
4
= atan2(b
23
, b
13
); θ
4
= atan2(b
23
, b
13
) + 180
0
;
θ
5
= atan2


2

33 33
1b,b
; θ
5
= atan2


2
33 33
1b,b
;
θ
6
= atan2(b
32
, -b
31
); θ
6
= atan2(b
32
, -b
31
) + 180
0
;

Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 23

CHƯƠNG 3
CHƯƠNG TRÌNH MÔ PHỎNG KẾT QUẢ TÍNH TOÁN

3.1. Mô phỏng sử dụng thư viện OpenGL với MFC
– Công cụ để mô phỏng :
+ Ngôn ngữ lập trình C++ : Xây dựng giao diện bằng MFC.
+ Tìm hiểu lập trình đồ họa OpenGL để mô phỏng qua các khối đồ họa.
– Phần mềm sử dụng:
+ Visual Studio 2008
+ SolidWorks 2010
+ Matlab R2010b với Robotics Toolbox for Matlab (release 8)
Copyright Peter Corke 1992-2008

3.1.1. Giới thiệu về OpenGL
OpenGL (Open Graphics Library) là một thư viện đồ họa tốc độ cao và độc
lập với hệ thống giao diện các hệ điều hành. Tiền thân của OpenGL là IRIS GL
do hãng Silicon Graphic Library Inc phát triển cho các WorkStation đồ họa tốc
độ cao từ năm 1982.Sau đó từ năm 1992 thì OpenGL đã trở thành một chuẩn
công nghiệp và đắc tính kỹ thuật của OpenGL do ủy ban kỹ thuật ARB
(
Architectural Review Board ) phê chuẩn.
Hình 3.1. Khối lập phương rỗng và đặc vẽ bằng OpenGL.

OpenGL là một tiêu chuẩn kỹ thuật đồ họa có mục đích định ra một giao diện
lập trình ứng dụng (tiếng Anh: API) đồ họa 3 chiều. OpenGL cũng có thể được
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 24

dùng trong các ứng dụng đồ họa 2 chiều. Giao diện lập trình này chứa khoảng
250 hàm để vẽ các cảnh phức tạp từ những hàm đơn giản. Nó được dùng rộng

rãi trong các trò chơi điện tử. Ngoài ra nó còn dùng trong các ứng dụng CAD,
thực tế ảo, mô phỏng khoa học, mô phỏng thông tin, phát triển trò chơi.
OpenGL còn có một đối thủ cạnh tranh là DirectX của Microsoft.
3.1.2. Giới thiệu ngôn ngữ lập trình Visual C++
Visual C++ là một trong các ngôn ngữ lập trình thuộc dòng Visual Studio
của hãng Microsoft, phát triển dựa trên nền tảng là ngôn ngữ lập trình hướng
đối tượng C++. Visual C++ ra đời khoảng những năm 1990 và càng ngày càng
được sử dụng rộng rãi nhất là các ứng dụng trong kỹ thuật như là lập trình
nhúng, lập trình điều khiển và lập trình mô phỏng. Việc tạo ra các giao diện
phức tạp và trình bày đẹp đối với Visual C++ khá là đơn giản, và đây chính là
thế mạnh c
ủa Visual C++ trong việc trợ giúp đắc lực cho người lập trình khi
xây dựng những dự án lớn hoặc trong kỹ thuật lập trình hệ thống.

Hình 3.2. Giao diện VC++ và tạo Project MFC.
Bài tập lớn Robot Công nghiệp KuKa Robot (6R)
Vũ Ngọc Trọng – CDT2_K52 25

MFC là lớp nền tảng của Microsoft (Microsoft Foundation Classes), là một
công cụ mạnh trong lập trình ứng dụng cho hầu hết các lĩnh vực vì nó có giao
diện đáp ứng mọi nhu cầu của người sử dụng trong việc thiết kế các giao diện
của chương trình mô phỏng.
3.1.3. Giới thiệu phần mềm đồ họa SolidWorks
Phần mềm SolidWorks là một phần mềm hỗ trợ thiết kế cơ khí (CAD) cho
phép ta vẽ và thể hiện các đối tượng trong không gian 3 chiều (3D) một cách
trực quan với giao diện thân thiện dễ sử dụng cũng như với hệ thống Help rất
chi tiết và dễ hiểu ; nó còn cho phép lưu và mở file dưới nhiều dạng khác nhau
giúp liên kết đến nhiều phần mềm khác nhau. Ở đây ta lựa chọn định d
ạng file
STL(Standard Template Library) để chuẩn bị cho việc mô phỏng.

Hình 3.3. Tạo file STL từ Solidwork (2010)