đồ án tính toán điều khiển và thiết kế robot kuka kr 30 . mô phỏng easy rob robot kuka . tính toán động học,động lực học robot kuka kr 30

Trường Đại Học Kỹ Thuật Công Nghiệp

KHOA ĐIỆN TỬ

Cộng hòa xã hội chủ nghĩa Việt Nam

Bộ môn

Độc Lập – Tự Do – Hạnh Phúc

cơ điện tử

------------o0o------------

ĐỀ TÀI ĐỒ ÁN MÔN HỌC

Đề tài : Tính toán ,điều khiển và thiết kế robot

KUKA KR30 L15/2

Giáo viên hướng dẫn: PGS TS. Phạm Thành Long
Sinh viên thực hiện : Nguyễn Văn Bản
Lớp: K48.CĐT.01
Mssv : K125520114006
Ngày giao đề:
Ngày hoàn thành:

Trưởng Bộ môn

TS.Phạm Thành Long

Giáo viên hướng dẫn

TS. Phạm Thành Long


Đồ án robot công nghiệp – Trường DH KTCN Thái Nguyên

Lời nói đầu
Đã gần một thế kỷ, kể từ khi xuất hiện trên thế giới mẫu hình robot cho tới nay
đã có hàng vạn robot ra đời và đang làm việc trong nhiều lĩnh vực sản xuất và
đời sống – xã hội ở hầu khắp các quốc gia trên thế giới. ngày nay robot đã trở
thành một tên gọi khá phổ biến trong nhân dân. Robot đã và đang trở thành một
nguồn lực lao động với năng suất và chất lượng cao trong nhiều lĩnh vực như:
công nghiệp, nông nghiệp, y học, an ninh quốc phòng v.v. Trong số đó robot
công nghiệp đóng vai trò rất quan trọng.
Robot nói chung và robot công nghiệp nói riêng là sự tích hợp rất tinh vi giữa
các lĩnh vực cơ học – điện – điện tử và kỹ thuật điều khiển. Có thể nói robot là
một trong số các loại hình sản xuất trí tuệ cao cấp trong thời đại ngày nay, do
con người tạo ra để phục vụ con người.
Ở nước ta, robot công nghiệp xuất hiện sau những năm 1990 trong các lĩnh vực
cơ khí, tự động hóa. Để đáp ứng được nhu cầu xã hội và tiếp cận nền khoa học
kỹ thuật tương lai, thì nền tảng của ngành cơ điện tử mà chúng em đang được
các thầy cô truyền đạt và nghiên cứu là cơ sở ban đầu để chúng em tiếp thu và
trang bị những kiến thức cao hơn nữa
Đồ án mà chúng em được giao là “Tính toán ,điều khiển và thiết kế robot
KUKA KR30 L15/2 ”
Trong quá trình thiết kế đồ án với sự hướng dẫn tận tình của thầy giáo Phạm
Thành Long và và các thầy cô khác trong bộ môn cùng với nỗ lực bản thân,
đến nay nhiệm vụ của em đã hoàn thành, mặc dù rất nhiều cố gắng trong quá
trình tìm hiểu, tính toán và thiết kế nhưng chắc chắc không tránh khỏi được

những sai sót. Chúng em rất mong được sự chỉ dẫn của các thầy cô giáo trong
bộ môn để đồ án được hoàn thiện hơn.
Em xin chân thành cảm ơn!
Thái nguyên ngày 23 tháng 12 năm 2015
Sinh viên thực hiện
Nguyễn Văn Bản

SVTH: Nguyễn Văn Bản

2

GVHD: Phạm Thành Long


Đồ án robot công nghiệp – Trường DH KTCN Thái Nguyên

Mục lục
CHƯƠNG 1 :
TỔNG QUAN VỀ CÁC BÀI TOÁN THIẾT KẾ , SỬ DỤNG ROBOT ............. 5
1.1 Quá trình phát triển của robot công nghiệp .............................................. 5
1.2 Ứng dụng robot công nghiệp trong sản xuất ............................................ 5
1.3 Bậc tự do tối thiểu để hoàn thành nhiệm vụ ............................................. 7
1.4 Vùng làm việc của tay máy. ..................................................................... 8
1.5 Bài toán động học tay máy. ...................................................................... 8
1.5.1

Bài toán động học thuận tay máy .................................................... 8

1.5.2

Bài toán động học ngược .............................................................. 12

1.6 Bài toán động lực học ............................................................................. 13
1.6.1

Mục đích nghiên cứu động lực học tay máy. ............................... 14

1.6.2
Các phương pháp sử dụng để nghiên cứu những đặc trưng động
lực học của tay máy ..................................................................................... 14
1.7 Bài toán điều khiển ................................................................................. 14

2

1.7.1

Khối điều khiển ............................................................................. 14

1.7.2

Khối chấp hành ............................................................................. 14

1.7.3

Khối kiểm tra................................................................................. 15

Chương 2 ..................................................... Error! Bookmark not defined.

TÍNH TOÁN ĐỘNG HỌC ROBOT KUKA KR30 L15/2 ................................. 16
2.1 Tính toán động học thuận robot............................................................. 16

2.1.1

Đặt bài toán ................................................................................... 16

2.1.2

Sơ đồ động học.............................................................................. 16

2.1.3

Xác định vị trí và hướng của bàn kẹp ........................................... 17

2.2 Tính toán động học ngược robot ............................................................ 22

3

2.2.1

Đặt bài toán ................................................................................... 22

2.2.2

Giải bài toán ngược động học. ...................................................... 22

CHƯƠNG 3 : ĐỘNG LỰC HỌC ROBOT ................................................ 24

SVTH: Nguyễn Văn Bản

3

GVHD: Phạm Thành Long


Đồ án robot công nghiệp – Trường DH KTCN Thái Nguyên

3.1 Nội suy quỹ đạo ...................................................................................... 24
3.1.1

Tính toán biến khớp. ..................................................................... 24

3.1.2

Tính toán minh họa với biến khớp q3 ........................................... 36

3.2 Mô phỏng robot KUKA KR30 L15/2 trên phần mềm easy_rob 2.0 ...... 39

4

3.2.1

Xây dựng hệ toạ độ. ................................................................ 39

3.2.2

Thiết kế hình dáng robot ....................................................... 41

3.2.3

Lập trình điều khiển robot ..................................................... 46

CHƯƠNG 3 KẾT LUẬN.......................................................................... 48
4.1 Ưu nhược điểm ....................................................................................... 48
4.2 Hướng phát triển của đề tài: ................................................................... 48

SVTH: Nguyễn Văn Bản

4

GVHD: Phạm Thành Long


Đồ án robot công nghiệp

Chương 1
TỔNG QUAN VỀ CÁC BÀI TOÁN THIẾT KẾ , SỬ DỤNG ROBOT
1.1 Quá trình phát triển của robot công nghiệp
Thuật ngữ Robot lần đầu tiên xuất hiện vào năm 1920 trong một tác phẩm văn
học của nhà văn Tiệp Khắc có tên Karel Capek.
Thuật ngữ Inducstrial Robot (IR)- xuất hiện lần đầu tiên tại Mỹ do công ty
AMF (American Manchine Foundry company) quảng cáo mô tả một thiết bị
mang dáng dấp và có một số chức năng của một cánh tay được điều khiển tự
động để thực hiện sản xuất một số thiết bị gọi là versatran
Qúa trình phát triển của IR có thể được tóm tắt như sau :
- Từ những năm 50 của thế kỷ trước tại Mỹ đã có viện nghiên cứu đầu tiên về
robot;
-Đầu những năm 60 đã xuất hiện sản phẩm đầu tiên mang tên versatran của công
ty AMF;
-Ở anh đã bắt đầu nghiên cứu và chế tạo các IR theo bản quyền của công ty
AMF của Mỹ từ năm 1967;
-Các nước tây âu khác bắt đầu nghiên cứu từ những năm 70;

-Châu á có Nhật nghiên cứu từ năm 1968.
Đến nay có trên 200 công ty sản xuất IR, trong đó có 80 công ty của Nhật,90
công ty của các nước tây âu,30 công ty của Mỹ và một số nước khác.
1.2 Ứng dụng robot công nghiệp trong sản xuất
Từ khi mới ra đời, robot công nghiệp được áp dụng trong nhiều lĩnh vực dưới
góc độ thay thế sức người. Nhờ vậy, các dây chuyền sản xuất được tổ chức lại,
năng suất và hiệu quả sản xuất tăng lên rõ rệt.

5
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp
Robot công nghiệp được ứng dụng rất rộng rãi trong sản xuất, các lĩnh vực chủ
yếu là:
- Công nghệ đúc
- Gia công áp lực
- Các quá trình hàn và nhiệt luyện
- Công nghệ gia công lắp ráp
- Phun sơn, vận chuyển hàng hóa (Robocar)

Ứng dụng robot trong công nghiệp
Các xu thế ứng dụng robot trong tương lai :
-

Robot ngày càng thay thế nhiều lao động
Robot ngày càng trở nên chuyên dụng
Robot ngày càng đảm nhận được nhiều loại công việc lắp ráp

Robot di động ngày càng trở nên phổ biến
Robot ngày càng tinh khôn hơn……….

Ứng dụng trong y tế
6
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp

Ứng dụng trong quân sự

1.3 Bậc tự do tối thiểu để hoàn thành nhiệm vụ
Bậc tự do của cơ cấu là số khả năng chuyển động của tay máy( chuyển động
quay hoặc chuyển động tịnh tiến ) . Tay máy gồm các khâu được lien kết với
nhau bởi các khớp động, mỗi một khâu trong không gian có 6 DOF (degree of
freedom ) gồm 3 chuyển động quay

. và 3 chuyển động tịnh tiến

.
Do hai khâu được nối với nhau bằng một khớp động . Do điều kiện liên kết bắt
buộc giữa các thành phần khớp động nên đã làm hạn chế chuyển động tương đối
giữa chúng. Đối với các khớp quay , khớp trượt là những khớp động hạn chế 5
chuyển động tương đối khi hai khâu được nối động với nhau. Vì vậy khi hai
khâu được nối động với nhau bởi một khớp quay thì số bậc tự do là : (6-5)=1 .
Số bậc tự do của tay máy có thể được tính theo công thức : DF= 6( -1)-5
Trong đó : + DF : số bặc tự do của chuỗi động trong không gian

+

: số khâu có trong cơ cấu.
+

: số khớp động có trong cơ cấu .

Đối với các cơ cấu có các khâu được nối động với nhau bằng khớp quay thì số
bậc tự do của cơ cấu chính bằng số khớp động có trong cơ cấu .
7
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp
Cách xác định bậc tự do tối thiểu : Nếu trong tay máy chỉ chứa những khớp
động với số bậc tự do tối thiểu

= 1 (có nghĩa là gồm những khớp tịnh tiến

hoặc khớp quay) thì số bậc tự do chính bằng số khớp động trong tay máy.
Để định vị và định hướng khâu chấp hành cuối của một cánh tay tùy ý trong
không gian 3 chiều , robot cần có 6 bậc tự do trong đó ba bậc tự do để định vị và
ba bậc tự do để định hướng.
 Ví dụ : Bậc tự do của tay máy bằng 5 . có nghĩa là tay máy co 5 chuyển
động độc lập hay nói cách khác là cần 5 động cơ để dẫn động cho tay máy
làm việc.
1.4 Vùng làm việc của tay máy.
- Vùng làm việc của robot là trường vận động của khâu tác động cuối, nó là

một không gian lien tục có hình dáng và thể tích xác định.
- Phương phát số: gọi là các biến khớp, thì vị trí của khâu chất hành
được biểu diễn dưới dạng vectơ →

- →

→(

hay P=

Trong đó : +
+

[ ]
: toạ độ vị trí và hướng của đối tượng cần thao tác
góc quay của của các góc toạ độ trung gian xunng

quanh các trục x,y,z

Vị trí của khâu chấp hành cuối phụ thuộc vào biểu :P=f(
1.5 Bài toán động học tay máy.
1.5.1 Bài toán động học thuận tay máy
a. Nội dung bài toán.
- Đầu vào : là các thông số liên quan đến khớp. cho cơ cấu và quy luật của
các yếu tố chuyển động thể hiện bằng các tọa độ suy rộng p
- Đầu ra : xác định hướng và vị trí của tay kẹp(dụng cụ phun sơn, mỏ hàn
…….) . Nói cách khác ta phải xác định quy luật của điểm trên khâu tác
8
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp
động cuối nói riêng cũng như một khâu nào đó trên tay máy nói chung
trong hệ tọa độ đề các.
b. Mục đích của bài toán.
Xây dựng được hàm f để xác định hướng và vị trí trong không gian của
tay kẹp.
c. Quy ước đặt các trục của hệ tọa độ khớp.
Trên tay máy đặt (n+1) hệ trục tọa độ, trong đó:
-1 hệ trục tọa độ cố định làm gốc (hệ cơ sở): O0x0y0z0.
- N hệ trục tọa độ tương ứng với n khâu (khớp) động, còn gọi là các hệ tọa độ
địa phương: Oixiyizi (i=1÷n). Trong đó: Onxnynzn là hệ trục tọa độ đặt tại tâm
P của khâu chấp hành.
 Hệ trục tọa độ O0x0y0z0:
O0 thường đặt tại tâm khớp 1.
z0 trùng trục khớp động 1.
x0 : chọn tùy ý, miễn là x0 vuông góc z0.
y0 : được chọn sao cho hệ tạo thành một tam diện thuận.
 Hệ trục tọa độ Oixiyizi:
zi : chọn tương tự như z0.
: là đường vuông góc chung nhỏ nhất nối từ zi-1 đến zi .
Oi : là giao của xi và zi.
yi : xác định theo quy tắc bàn tay phải.
Trường hợp đặc biệt:
- Nếu trục zi-1 cắt zi thì hướng của trục xi được chọn tùy ý miễn là vuông
góc với zi-1.
- Khi hai trục zi-1 và zi song song với nhau, giữa hai đường này có vô số
đường pháp tuyến chung, thường quy ước chọn xi nằm trên khâu nối giữa

hai khớp để tâm Oi trùng tâm khớp i+1.

9
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp
 Hệ trục tọa độ đặt tại khâu tác động cuối Onxnynzn :
On: trùng tâm P.
yn : nằm trong mặt phẳng kẹp, vuông góc phương kẹp, chiều tùy ý.
zn ( hoặc xn ): hướng đến đối tượng.
Trục còn lại xn (hoặc zn ): vuông góc mặt phẳng kẹp, chiều đảm bảo hệ
tạo thành một tam diện thuận.
d. Quy tắc Denavit – Hartenberg (D-H).

Xét tại một khớp thứ i:

10
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp
Sau khi được thiết lập, vị trí của hệ Oixiyizi so với hệ Oi-1xi-1yi-1zi-1
hoàn toàn được xác định nhờ 4 thông số:
- ai : khoảng cách giữa 2 khớp liên tiếp theo phương xi .
- di : khoảng cách giữa 2 khớp liên tiếp theo phương zi-1 .

- i : góc quay quanh trục xi giữa zi-1 và zi .
- i : góc quay quanh trục zi-1 giữa xi-1 và xi
 Nếu khớp i là khớp quay :
i là biến khớp
di , ai , i là tham số do thiết kế tạo ra.
 Nếu khớp i là khớp tịnh tiến :
di là biến khớp
i , ai , i : tham số phụ thuộc kết cấu.
e. Ma trận chuyển đổi giữa hệ trục i và i -1.
Để chuyển từ hệ Oi-1xi-1yi-1zi-1 sang hệ Oixiyizi là thực hiện liên tiếp 4
phép biến đổi toạ độ :
1/ Quay quanh zi-1một góc i : Rz,i
2/ Tịnh tiến dọc zi-1 một đoạn di : Tz, di
3/ Tịnh tiến dọc xi một đoạn ai: Tx, ai
4/ Quay quanh xi một góc i : Rx,i
Pi-1 = Rz,i . Tz,di. Tx, ai. Rx,i . Pi
(





Ma trận biến hình tổng hợp của các phép quay trên
(



:



11
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp

=

[

=[

][











][

][











]

]

- Ma trận DH tổng quát cho tay máy 3 DOF

1
2
3

R(z,θ)
(

T(z,d)

T(x,a)

R(x,α)

(

(

1.5.2 Bài toán động học ngược
a. Nội dung bài toán
- Đầu vào : các thông số của tâm tay kẹp ( hướng và vị trí của tay kẹp tại
một vị trí cụ thể) ma trận tọa độ thực .
- Đầu ra : tìm xem q1 , q2 ,q3,…… có giá trị bao nhiêu để thỏa mãn vị trí
của tâm tay kẹp đã cho trước . nói cách khác là tìm đầu ra là một bộ biến
khớp.
b. Mục đích của bài toán
Xác định giá trị của biến khớp để thỏa mãn vị trí của tay kẹp.
c. Cách giải bài toán
Cho ma trận tọa độ thực T = [

Ta có :

= ( q1 , q2 , q3 ……

]

)

12
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp

=[

=

Trong đó ma trận : [

]

] là ma trận tọa độ lý thuyết và (

,

) là

tọa độ tâm tay kẹp .
Xét phần tử của hai ma trận ta được các biến khớp liên quan ( d,a,α,θ)
- Các biến khớp chỉ hướng :
+

=
=

+

=
- Các biến khớp chỉ vị trí :
=
=
=

-

Có hai phương pháp giải bài toán động học ngược :
1, closed from slution ( lời giải vòng kín )
Phương pháp giải tích
Phương pháp hình học
2, Numerical solution (phương pháp số)
Phương pháp Newton
Phương pháp ma trận Jacobian
Phương pháp RGR

1.6 Bài toán động lực học
Động lực học tay máy nghiên cứu mối quan hệ giữa lực và momen, năng lượng.
……… với các thông số chuyển động của nó.
13
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp
1.6.1 Mục đích nghiên cứu động lực học tay máy.
- Mô phỏng hoạt động của tay máy , để khảo sát , thử nghiệm quá trình làm
việc của nó mà không phải dung tay máy thật .
- Phân tích tính toán kết cấu của tay máy
- Phân tích thiết kế hệ thống điều khiển của tay máy
1.6.2 Các phương pháp sử dụng để nghiên cứu những đặc trưng động lực
học của tay máy
- Phương pháp Lagrange
- Phương Pháp thiết lập quy luật chuyển động của tay máy trên cơ sở

phương trình Newton và phương trình Euler gọi là phương trình NewtonEuler.
- Phương pháp D’Alembert-Euler
1.7 Bài toán điều khiển
Điều khiển robot cũng dựa trên nguyên lý chung của hệ điều khiển tự động ,
nhưng đối với điều khiển của robot là điều khiển quá trình hoạt động của tay
máy ( cơ cấu chấp hành ).
Cấu trúc của hệ điều khiển gồm 3 phần : + khối điều khiển
+ khối chấp hành
+ khối kiểm tra
1.7.1 Khối điều khiển
Khối điều khiển nhận tín hiệu từ bộ phận xử lý và phát tín hiệu điều khiển tới
khối chấp hành.
Nguyên tắc hoạt động của hệ thống điều khiển : tín hiệu điều khiển được
phát ra từ hệ thống điều khiển tác động tớ khâu chấp hành . Khối chấp
hành nhận tín hiệu để điều phối , hiệu chỉnh các hoạt động của cơ cấu
chấp hành . Đối với các hệ điều khiển kín có mạch phản hồi phụ, . Mạch
phản hồi phụ cơ chứ năng phát hiẹn các sai lệch của cơ cấu chấp hành
truyền tín hiệu về trung tâm xử lý
1.7.2 Khối chấp hành
- Khối chấp hành có nhiệm vụ thực hiện theo các lệnh của hệ thống điều
khiển thông qua các tín hiệu điều khiển. trong robot công nghiệp khối
chấp hành chính là tay máy
14
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp

1.7.3 Khối kiểm tra
Khối này gồm có các cảm biến dung để phát hiện và kiểm tra các thong số đầu
ra của cơ cấu chấp hành , sau đó chuyển thành các tún hiệu để đưa vào khối điều
khiển, để từ đó khối điều khiển điều chỉnh thông số đầu vào cho phù hợp.
Động cơ một chiều và xoay chiều được dùng trong hệ thống dẫn động robot
gồm động cơ servo một chiều có chổi than và không có chổi than , động cơ xoay
chiều động cơ bước có vận tốc không thay đổi hoặc thay đổi hoặc các động cơ
vô cấp .
Ngoài ra còn có : + dẫn động bằng động cơ bước
+ dẫn động bằng động cơ tịnh tiến
+ dẫn động bằng thủy khí………

15
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp

2

CHƯƠNG 2

TÍNH TOÁN ĐỘNG HỌC ROBOT KUKA KR30 L15/2

2.1 Tính toán động học thuận robot
2.1.1 Đặt bài toán
Biết giá trị các biến khớp αi(i=1..6), cần phải xác định vị trí và hướng của
bàn kẹp, tức là phải xác định ma trận TE

TE = T

0

6

n x
s
0
1
2
3
4
5
 A1 . A 2 . A 3 . A 4 . A 5 . A 6   x
 ax

0

ny
sy

nz
sz

ay
0

az

0

px 
p y 
pz 

1

2.1.2 Sơ đồ động học

Sơ đồ động học của Robot kuka kr30 l15/2.

16
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp

Bảng D-H của robot KUKA KR30 L15/2
Khớp
1
2
3
4
5
6

R(z,q)

( )
( )
( )
( )
( )
( )

T(z,d)
865
0
0
1670
0
300

R(x,α)
90
0
90
-90
90
0

T(x,a)
410
1000
145
0
0
0

2.1.3 Xác định vị trí và hướng của bàn kẹp
Từ các ma trận trên ta có các ma trận DH đối với khớp quay:

[

]

Ma trận DH chuyển từ hệ toạ độ 0 sang hệ toạ độ 1 :
[

]

Ma trận DH chuyển từ hệ toạ độ 1 sang hệ toạ độ 2 :
[

]

Ma trận DH chuyển từ hệ toạ độ 2 sang hệ toạ độ 3 :
[

]

Ma trận DH chuyển từ hệ toạ độ 3 sang hệ toạ độ 4 :

17
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long

Đồ án robot công nghiệp

[

]

Ma trận DH chuyển từ hệ toạ độ 4 sang hệ toạ độ 5 :

[

]

Ma trận DH chuyển từ hệ toạ độ 5 sang hệ toạ độ 6 :
[

]

Ma trận DH chuyển từ hệ toạ độ 0 sang hệ toạ độ 6 :

T 06 = A 10 .A 12 .A 32 .A 34 .A 54 .A 56 =[

]

=[
(((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*cos(q4)+sin(q1)*sin(q4))*cos(q5)+(cos(q1)*cos(q2)*sin(q3)-cos(q1)*sin(q2)*cos(q3))*sin(q5))*cos(q6)+((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*sin(q4)+sin(q1)*cos(q4))*sin(q6),
-(((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*cos(q4)+sin(q1)*sin(q4))*cos(q5)+(cos(q1)*cos(q2)*sin(q3)-cos(q1)*sin(q2)*cos(q3))*sin(q5))*sin(q6)+((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*sin(q4)+sin(q1)*cos(q4))*cos(q5),
((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*cos(q4)+sin(q1)*sin(q4))*sin(q5)-(cos(q1)*cos(q2)*sin(q3)-cos(q1)*sin(q2)*cos(q3))*cos(q5),
300*((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*cos(q4)+sin(q1)*sin(q4))*sin(q5)-300*(18
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp

cos(q1)*cos(q2)*sin(q3)cos(q1)*sin(q2)*cos(q3))*cos(q5)+1670*cos(q1)*cos(q2)*sin(q3)+1670*cos(q1
)*sin(q2)*cos(q3)+145*cos(q1)*cos(q2)*cos(q3)145*cos(q1)*sin(q2)*sin(q3)+1000*cos(q1)*cos(q2)+410*cos(q1)]
[
(((sin(q1)*cos(q2)*cos(q3)sin(q1)*sin(q2)*sin(q3))*cos(q4)-cos(q1)*sin(q4))*cos(q5)+(sin(q1)*cos(q2)*sin(q3)-sin(q1)*sin(q2)*cos(q3))*sin(q5))*cos(q6)+((sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*sin(q4)cos(q1)*cos(q4))*sin(q6),
(((sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*cos(q4)cos(q1)*sin(q4))*cos(q5)+(-sin(q1)*cos(q2)*sin(q3)sin(q1)*sin(q2)*cos(q3))*sin(q5))*sin(q6)+(-(sin(q1)*cos(q2)*cos(q3)sin(q1)*sin(q2)*sin(q3))*sin(q4)-cos(q1)*cos(q4))*cos(q5),
((sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*cos(q4)cos(q1)*sin(q4))*sin(q5)-(-sin(q1)*cos(q2)*sin(q3)sin(q1)*sin(q2)*cos(q3))*cos(q5), 300*((sin(q1)*cos(q2)*cos(q3)sin(q1)*sin(q2)*sin(q3))*cos(q4)-cos(q1)*sin(q4))*sin(q5)-300*(sin(q1)*cos(q2)*sin(q3)sin(q1)*sin(q2)*cos(q3))*cos(q5)+1670*sin(q1)*cos(q2)*sin(q3)+1670*sin(q1)
*sin(q2)*cos(q3)+145*sin(q1)*cos(q2)*cos(q3)145*sin(q1)*sin(q2)*sin(q3)+1000*sin(q1)*cos(q2)+410*sin(q1)]
[
((sin(q1)*cos(q3)+cos(q2)*sin(q3))*cos(q4)*cos(q5)+(sin(q1)*sin(q3)+cos(q2)*cos(q3))*sin(q5))*cos(q6)(sin(q1)*cos(q3)+cos(q2)*sin(q3))*sin(q4)*sin(q6),
-((sin(q1)*cos(q3)+cos(q2)*sin(q3))*cos(q4)*cos(q5)+(sin(q1)*sin(q3)+cos(q2)*cos(q3))*sin(q5))*sin(q6)(sin(q1)*cos(q3)+cos(q2)*sin(q3))*sin(q4)*cos(q5),
(sin(q1)*cos(q3)+cos(q2)*sin(q3))*cos(q4)*sin(q5)-(sin(q1)*sin(q3)+cos(q2)*cos(q3))*cos(q5),
865+300*(sin(q1)*cos(q3)+cos(q2)*sin(q3))*cos(q4)*sin(q5)-300*(sin(q1)*sin(q3)+cos(q2)*cos(q3))*cos(q5)+1670*sin(q1)*sin(q3)-

19
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp

1670*cos(q2)*cos(q3)+145*sin(q1)*cos(q3)+145*cos(q2)*sin(q3)+1000*sin(q1
)]
[

0,
0,
0,
1]
Cho TE = T 06 ta được phương trình động học thuận robot Kuka kr30 l15/2:

= (((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*cos(q4)+sin(q1)*sin(q4))*cos(q5)+(cos(q1)*cos(q2)*sin(q3)-cos(q1)*sin(q2)*cos(q3))*sin(q5))*cos(q6)+((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*sin(q4)+sin(q1)*cos(q4))*sin(q6)

= -(((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*cos(q4)+sin(q1)*sin(q4))*cos(q5)+(cos(q1)*cos(q2)*sin(q3)-cos(q1)*sin(q2)*cos(q3))*sin(q5))*sin(q6)+((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*sin(q4)+sin(q1)*cos(q4))*cos(q6)
= ((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*cos(q4)+sin(q1)*sin(q4))*sin(q5)-(cos(q1)*cos(q2)*sin(q3)-cos(q1)*sin(q2)*cos(q3))*cos(q5)
= (((sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*cos(q4)cos(q1)*sin(q4))*cos(q5)+(-sin(q1)*cos(q2)*sin(q3)sin(q1)*sin(q2)*cos(q3))*sin(q5))*cos(q6)+(-(sin(q1)*cos(q2)*cos(q3)sin(q1)*sin(q2)*sin(q3))*sin(q4)-cos(q1)*cos(q4))*sin(q6)
= -(((sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*cos(q4)cos(q1)*sin(q4))*cos(q5)+(-sin(q1)*cos(q2)*sin(q3)20
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp

sin(q1)*sin(q2)*cos(q3))*sin(q5))*sin(q6)+(-(sin(q1)*cos(q2)*cos(q3)sin(q1)*sin(q2)*sin(q3))*sin(q4)-cos(q1)*cos(q4))*cos(q6)
= ((sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*cos(q4)cos(q1)*sin(q4))*sin(q5)-(-sin(q1)*cos(q2)*sin(q3)sin(q1)*sin(q2)*cos(q3))*cos(q5)
= ((sin(q2)*cos(q3)+cos(q2)*sin(q3))*cos(q4)*cos(q5)+(sin(q2)*sin(q3)+cos(q2)*cos(q3))*sin(q5))*cos(q6)(sin(q2)*cos(q3)+cos(q2)*sin(q3))*sin(q4)*sin(q6)
= -((sin(q2)*cos(q3)+cos(q2)*sin(q3))*cos(q4)*cos(q5)+(sin(q2)*sin(q3)+cos(q2)*cos(q3))*sin(q5))*sin(q6)(sin(q2)*cos(q3)+cos(q2)*sin(q3))*sin(q4)*cos(q6)
= (sin(q2)*cos(q3)+cos(q2)*sin(q3))*cos(q4)*sin(q5)-(sin(q2)*sin(q3)+cos(q2)*cos(q3))*cos(q5)
= 300*((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*cos(q4)+sin(q1)*sin(q4))*sin(q5)-300*(cos(q1)*cos(q2)*sin(q3)cos(q1)*sin(q2)*cos(q3))*cos(q5)+1670*cos(q1)*cos(q2)*sin(q3)+1670*cos(q1
)*sin(q2)*cos(q3)+145*cos(q1)*cos(q2)*cos(q3)145*cos(q1)*sin(q2)*sin(q3)+1000*cos(q1)*cos(q2)+410*cos(q1)
= 300*((sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*cos(q4)cos(q1)*sin(q4))*sin(q5)-300*(-sin(q1)*cos(q2)*sin(q3)sin(q1)*sin(q2)*cos(q3))*cos(q5)+1670*sin(q1)*cos(q2)*sin(q3)+1670*sin(q1)
*sin(q2)*cos(q3)+145*sin(q1)*cos(q2)*cos(q3)145*sin(q1)*sin(q2)*sin(q3)+1000*sin(q1)*cos(q2)+410*sin(q1)
= 865+300*(sin(q2)*cos(q3)+cos(q2)*sin(q3))*cos(q4)*sin(q5)-300*(sin(q2)*sin(q3)+cos(q2)*cos(q3))*cos(q5)+1670*sin(q2)*sin(q3)1670*cos(q2)*cos(q3)+145*sin(q2)*cos(q3)+145*cos(q2)*sin(q3)+1000*sin(q2
)

Từ hệ phương trình trên xác định vị trí của tay kẹp the biến khớp .
21
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp
2.2 Tính toán động học ngược robot
2.2.1 Đặt bài toán
Cho vị trí và hướng của bàn kẹp tức là biết ma trận T E. Cần phải xác định các
biến khớp qi(i=1..6) theo vị trí và hướng bàn kẹp.
2.2.2 Giải bài toán ngược động học.
Từ ma trận T05 và TE ta được hệ phương trình động học nghịch:
(ny-a12)^2=0
(nz-a13)^2=0
(sz-a23)^2=0

(2.3)

(Px-a14)^2=0
(Py-a24)^2=0
(Pz-a34)^2=0

Với : +

= -(((cos(q1)*cos(q2)*cos(q3)-

cos(q1)*sin(q2)*sin(q3))*cos(q4)+sin(q1)*sin(q4))*cos(q5)+(cos(q1)*cos(q2)*sin(q3)-cos(q1)*sin(q2)*cos(q3))*sin(q5))*sin(q6)+((cos(q1)*cos(q2)*cos(q3)cos(q1)*sin(q2)*sin(q3))*sin(q4)+sin(q1)*cos(q4))*cos(q6)
+

= ((cos(q1)*cos(q2)*cos(q3)-

cos(q1)*sin(q2)*sin(q3))*cos(q4)+sin(q1)*sin(q4))*sin(q5)-(cos(q1)*cos(q2)*sin(q3)-cos(q1)*sin(q2)*cos(q3))*cos(q5)
+ = ((sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*cos(q4)cos(q1)*sin(q4))*sin(q5)-(-sin(q1)*cos(q2)*sin(q3)sin(q1)*sin(q2)*cos(q3))*cos(q5)
+

= 300*((cos(q1)*cos(q2)*cos(q3)-

cos(q1)*sin(q2)*sin(q3))*cos(q4)+sin(q1)*sin(q4))*sin(q5)-300*(cos(q1)*cos(q2)*sin(q3)cos(q1)*sin(q2)*cos(q3))*cos(q5)+1670*cos(q1)*cos(q2)*sin(q3)+1670*cos(q1
22
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp

)*sin(q2)*cos(q3)+145*cos(q1)*cos(q2)*cos(q3)145*cos(q1)*sin(q2)*sin(q3)+1000*cos(q1)*cos(q2)+410*cos(q1)
+
= 300*((sin(q1)*cos(q2)*cos(q3)-sin(q1)*sin(q2)*sin(q3))*cos(q4)cos(q1)*sin(q4))*sin(q5)-300*(-sin(q1)*cos(q2)*sin(q3)sin(q1)*sin(q2)*cos(q3))*cos(q5)+1670*sin(q1)*cos(q2)*sin(q3)+1670*sin(q1)
*sin(q2)*cos(q3)+145*sin(q1)*cos(q2)*cos(q3)145*sin(q1)*sin(q2)*sin(q3)+1000*sin(q1)*cos(q2)+410*sin(q1)
+

= 865+300*(sin(q2)*cos(q3)+cos(q2)*sin(q3))*cos(q4)*sin(q5)-300*(-

sin(q2)*sin(q3)+cos(q2)*cos(q3))*cos(q5)+1670*sin(q2)*sin(q3)1670*cos(q2)*cos(q3)+145*sin(q2)*cos(q3)+145*cos(q2)*sin(q3)+1000*sin(q2
)
Giải phương trình exel ra các góc qi(1…6) tương ứng :
bảng solve bài toán hội tụ của robot Kuka kr30 l15/2

23
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp

3

CHƯƠNG 3 : ĐỘNG LỰC HỌC ROBOT

3.1 Nội suy quỹ đạo
3.1.1 Tính toán biến khớp.
Căn cứ vào kiểu dáng và kích thước vùng làm việc của robot, bố trí vị trí phôi
trong vùng làm việc theo sơ đồ sau:

Hình 3.1 :Sơ đồ chuyển động của robot giữa hai trạm cấp phôi

Nếu lấy điểm gốc

của hệ quy chiếu cơ sở làm chuẩn mô tả, đối tượng mô tả

là điểm P mút dụng cụ. Vì mút dụng cụ trong quá trình làm việc cần
trùng với quỹ đạo gia công trên phôi nên có thể viết được quan hệ này
dưới dạng phương trình vòng véc tơ như sau:
= X.E.R
Theo phép chuyển đổi thuần nhất thế của khâu chấp hành là hàm của các biến
khớp, mô tả bằng ma trận tổng hợp của phép chuyển đổi :

∏

24
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long


Đồ án robot công nghiệp
Trong đó: với i = với i= 1 n, là ma trận chuyển đổi giữa hệ toạ độ thứ i đến hệ
i-1, xác định theo quy tắc Denavit-Hartenberg; n là số biến khớp (bậc tự do) của
robot. Vị trí và hướng của khâu chấp hành được xác định từ quỹ đạo cho trước:

0

[

]
0

Trong đó:

0

(

1 2

.

0

0

1

các biến khớp; n, s, a là các vec tơ chỉ

1

phương; p là véc tơ chỉ vị trí; oxyz là hệ toạ độ gốc.
Ma trận chuyển đổi tổng hợp có dạng:
[

11

12

13

1

21
31

22
32

23
33

2
3

0
Các thành phần

0

0

]

1

với i,j = 1 3là các cosin chỉ phương của n,s,a;

1

2

3

lần lượt là các thành phần chiếu lên hệ oxyz của p. Do tính chất trực giao của
các vec tơ chỉ phương, cho nên chỉ có ba thành phần trong các cosin chỉ phương
độc lập. Vì vậy ta nhận được:
(3.1)
Giải hệ phương trình trên ta được giá trị các biến khớp . Tuy nhiên đây là bước
thường gặp nhiều khó khăn nhất vì không phải với cấu trúc robot nào cũng có
lời giải dưới dạng giải tích. Dưới đây giới thiệu phương pháp số dể thực hiện

việc này . Do đặc điểm tham gia và mức độ tham gia của các động cơ trên cánh
tay vào việc kẹp hoặc nhả phôi ở các tư thế khác nhau là hoàn toàn khác nhau
nên bài toán (3.1) cần giải lặp lại trước khi thao tác bàn tay.Để vẽđược đặc tính
chuyển vị đầy đủ với chi phí tính toán nhỏ có thể nội suy lời giải qua một số
điểm chốt, sử dụng đặc tính chuyển vị bậc ba dạng đa thức với một giải thuật số
có tính tổng quát để giải bài toán động học ngược hoàn toàn thỏa mãn được các
yêu cầu này.
Trên hình 3.1 lần lượt thiết lập các ma trận chuyển trục giữa robot đến hai trạm
A và B như sau:

25
SVTH : Nguyễn Văn Bản

GVHD : Phạm Thành Long