TRƯỜNG ĐẠI HỌC GIAO THÔNG VẬN TẢI
***KHOA CƠ KHÍ***
BỘ MÔN KỸ THUẬT MÁY

BÁO CÁO TỔNG KẾT
ĐỀ TÀI NGHIÊN CỨU KHOA HỌC CỦA SINH VIÊN
NĂM 2018

NGHIÊN CỨU GIẢI BÀI TOÁN ĐỘNG HỌC
VÀ THIẾT KẾ QUỸ ĐẠO ROBOT PUMA

SVTH: Phạm Quang Huy
Lớp: Cơ điện tử K56
Khoa: Cơ khí
GVHD: TS. Đinh Thị Thanh Huyền

Hà Nội, tháng 1, năm 2018


TRƯỜNG ĐẠI HỌC GIAO THÔNG VẬN TẢI
***KHOA CƠ KHÍ***
BỘ MÔN KỸ THUẬT MÁY

BÁO CÁO TỔNG KẾT
ĐỀ TÀI NGHIÊN CỨU KHOA HỌC CỦA SINH VIÊN
NĂM 2018
NGHIÊN CỨU GIẢI BÀI TOÁN ĐỘNG HỌC
VÀ THIẾT KẾ QUỸ ĐẠO ROBOT PUMA

SVTH: Phạm Quang Huy

Giới tính: nam

Dân tộc: Kinh

Lớp: Cơ điện tử K56

Khoa: Cơ khí

Năm thứ: 3 / 4

Nghành học: Cơ điện tử
GVHD: TS. Đinh Thị Thanh Huyền


LỜI NÓI ĐẦU
Trong sự nghiệp công nghiệp hóa, hiện đại hóa đất nước vấn đề tự động hóa sản
xuất có vai trò đặc biệt quan trọng. Nhằm nâng cao năng suất dây chuyền công nghệ,
nâng cao chất lượng và khả năng cạnh tranh của sản phẩm, cải thiện điều kiện lao động,
nâng cao năng suất lao động…đặt ra là hệ thống sản xuất phải có tính linh hoạt cao.
Robot công nghiệp là một bộ phận quan trọng để tạo ra những hệ thống tự động đó.
Robot công nghiệp đã có mặt trong sản xuất từ nhiều năm trước, ngày nay robot công
nghiệp được dùng ở nhiều lĩnh vực sản xuất. Robot có những tính năng mà con người
không thể có được, khả năng làm việc ổn định, làm việc trong môi trường độc hại… Do
đó việc đầu tư nghiên cứu, chế tạo ra những loại robot phục vụ cho công cuộc tự động
hóa sản xuất là rất cần thiết cho hiện tại và trong tương lai.
Đề tài nghiên cứu khoa học năm nay mà em được giao là: “Nghiên cứu giải bài toán
động học và thiết kế quỹ đạo robot Puma”.
Nội dung đề tài nghiên cuus gồm 3 phần:
➢ Chương 1: Tổng quan về robot công nghiệp, giới hiệu robot Puma và ứng dụng
của nó.

➢ Chương 2: Thành lập bài toán động học robot Puma
➢ Chương 3: Thiết kế quỹ đạo cho robot Puma
Trong thời gian 4 tháng nghiên cứu, với những kiến thức đã học được từ các thầy
cô truyền đạt và vốn hiểu biết của bản thân em đã hiểu được cấu tạo và cách thức hoạt
động của robot cụ thể là Robot Puma. Có thể báo cáo đề tài còn thiếu sót trong quá trình
nghiên cứu và trình bày, rất mong được sư đóng góp ý kiến của các thầy, cô giáo để đề
tài nghiên cứu được hoàn chỉnh hơn.
Thông qua đây em cũng gửi lời cảm ơn đến cô Đinh Thị Thanh Huyền đã hướng dẫn
giúp đỡ em hoàn thành báo cáo nghiên cứu này.
Xin chân trọng cảm ơn!
Hà nội, tháng 1 năm 2018
Sinh viên thực hiện
Huy
Phạm Quang Huy


MỤC LỤC
Chương 1 ................................................................................................................................... 1
TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP ........................................................................ 1
1.1. QUÁ TRÌNH PHÁT TRIỂN CỦA ROBOT CÔNG NGHIỆP ................................. 1
1.1.1. Khái niệm ................................................................................................................ 1
1.1.2. Quá trình phát triển ............................................................................................... 1
1.2. CẤU TRÚC CƠ BẢN CỦA ROBOT CÔNG NGHIỆP ............................................. 2
1.2.1. Các bộ phận cấu thành robot công nghiệp ........................................................... 2
1.2.2. Các đặc trưng cấu trúc của robbot công nghiệp.................................................. 3
1.2.3. Phân loại robot công nghiệp .................................................................................. 5
1.2.4. Ứng dụng robot công nghiệp ................................................................................. 8
1.3. ROBOT PUMA .............................................................................................................. 9
1.3.1. Phạm vi và đối tượng nghiên cứu.......................................................................... 9
1.3.2. Ứng dụng của cánh tay robot Puma ................................................................... 10

Chương 2 ................................................................................................................................. 11
ĐỘNG HỌC ROBOT PUMA ................................................................................................ 11
2.1. CÁC THAM SỐ KHÂU CỦA ROBOT CÔNG NGHIỆP VÀ MA TRẬN BIẾN
ĐỔI THUẦN NHẤT........................................................................................................... 11
2.1.1. Các tham số khâu của robot công nghiệp .......................................................... 11
2.1.2. Ma trận biến đổi thuần nhất................................................................................ 12
2.1.3. Ma trận quan hệ ................................................................................................... 13
2.2. ĐỘNG HỌC ROBOT.................................................................................................. 14
2.2.1.Mục đích, nội dung và phương pháp nghiên cứu động học............................... 14
2.2.2. Các bước giải bài toán động học ......................................................................... 15
2.3. ĐỘNG HỌC THUẬN ROBOT PUMA ..................................................................... 16
2.3.1. Các bước thiết lập phương trình động học robot .............................................. 16
2.3.2. Giải bài toán động học thuận robot Puma ......................................................... 17
2.4. BÀI TOÁN ĐỘNG HỌC NGƯỢC CÁNH TAY ROBOT PUMA.......................... 26
2.4.1.Các phương pháp giải bài toán động học ngược ................................................ 26
2.4.2 Giải bài toán động học ngược robot Puma.......................................................... 28
Chương 3 ................................................................................................................................. 34
THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO ROBOT PUMA ..................................... 34
3.1. GIỚI THIỆU VÀ CƠ SỞ THIẾT KẾ QUỸ ĐẠO.................................................... 34
3.2 TÍNH TOÁN THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG .......................................... 34
3.2.1. Thiết kế quỹ đạo chuyển dộng trong không gian khớp .................................... 34
KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN CỦA ĐỀ TÀI .................................................... 36
TÀI LIỆU THAM KHẢO...................................................................................................... 37


Robot Puma

Cơ điện tử K56_Utc

1


Chương 1
TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP
1.1. QUÁ TRÌNH PHÁT TRIỂN CỦA ROBOT CÔNG NGHIỆP
1.1.1. Khái niệm
Robot công nghiệp có thể được hiểu là những thiết bị tự động linh hoạt, thực hiện
các chức năng lao động công nghiệp của con người dưới một hệ thống điều khiển theo
những chương trình đã được lập trình sẵn.
1.1.2. Quá trình phát triển
Trên thế giới
•

Vào năm 1921 Thuật ngữ “Robot” xuất phát từ tiếng Séc (Czech) “Robota” có
nghĩa là công việc tạp dịch trong một vở kịch.

•

Năm 1950 ở Mỹ thành lập viện nghiên cứu đầu tiên.

•

Đầu năm 1960 công ty AMF cho ra đời sản phẩm đầu tiên có tên gọi là Versatran.

•

Từ năm 1967, ở Anh, người ta đã bắt đầu nghiên cứu và chế tạo IR.

•

Từ năm 1968, ở Châu Á, Nhật bắt đầu nghiên cứu những ứng dụng của IR, năm

1970, Robot đã được chú ý nhiều hơn và bắt đầu xuất hiện ở các nước Đức, Ý,
Pháp...

•

Nhất là vào những năm 1990 số lượng Robot công nghiệp đã gia tăng với nhiều
tính năng vượt bậc.

•

Đến nay, trên thế giới có khoảng trên 200 công ty sản xuất IR. Trong đó Mỹ và
Nhật chiếm đa số.

Tại Việt Nam
•

Nghiên cứu phát triển Robot công nghiệp đã có những bước tiến đáng kể trong
25 năm vừa qua các nghiên động lực học Robot được các khoa cơ khí, chế tạo

SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

2


máy ở các trường đại học và các viện nghiên cứu quan tâm với nhiều sản phẩm
ấn tượng trên trường quốc tế.
1.2. CẤU TRÚC CƠ BẢN CỦA ROBOT CÔNG NGHIỆP
1.2.1. Các bộ phận cấu thành robot công nghiệp
Trên hình 1.1 dưới đây giới thiệu các bộ phận chủ yếu của robot công nghiệp
thông thường.
Cánh tay robot bao gồm đế máy 1 đặt cố định hoặc gắn đế di động 2, thân 3,
cánh tay trên 4, cánh tay dưới 5, bàn tay nắm bắt 6. Ngoài ra còn 1 số bộ phận khác nữa
để điều khiển hoạt động của cánh tay robot như: hệ thống dẫn động, hệ thống điều khiển
và hệ thống cảm biến tín hiệu.
-

Hệ thống truyền dẫn động : Có thể là cơ khí, thủy khí hoặc bằng điện… đây là
bộ phận chủ yếu tạo nên sự chuyển dịch ở các khớp động của robot.

-

Hệ thống điều khiển : là hệ thống đảm bảo hoạt động của robot theo thông tin đã
lập trình sẵn hoặc nhận biết được trong quá trình điều khiển.

-

Hệ thống cảm biến tín hiệu : Nhận biết và biến đổi thông tin hoạt động của robot
và của môi trường, đối tượng mà robot phục vụ.
Các thông tin lập trình sẵn hoặc cảm biến sẽ được đưa vào hệ thống điều khiển sau khi

được xử lí qua máy tính, rồi tác động tới hệ thống truyền dẫn động.

Hình 1.1: Các bộ phận cấu thành robot công nghiệp
SVTH: Phạm Quang Huy


GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

3

1.2.2. Các đặc trưng cấu trúc của robbot công nghiệp
a) Bậc tự do
Bậc tự do là chỉ tiêu kỹ thuật quan trọng của robot công nghiệp, nó được quyết định
bởi cấu trúc của robot công nghiệp và ảnh hưởng trực tiếp đến tính cơ động của nó. Số
bậc tự do của tay máy được định nghĩa là số khả năng chuyển động động lập của các
khâu của cơ cấu đối với hệ quy chiếu gắn với thân của tay máy; nó đồng thời là tham số
độc lập xác định vị trí của tay máy với 1 hệ quy chiếu gắn với thân robot.
Bậc tự do của tay máy được xác định bằng công thức :
W = 6n -∑5𝑖=1 𝑖. 𝑝𝑖
Trong đó : W là số bậc tự do của tay máy
n là số khâu động
𝑝𝑖 là số khớp động loại i
b) Tọa độ suy rộng
Là hệ tọa độ gắn lên các khâu của robot, các tọa độ này độc lập với nhau và ứng với
mỗi tọa độ là một bậc tự do của cơ cấu. Ta dùng hệ tọa độ suy rộng để xác định cấu hình
của cơ cấu tay máy tại các thời điểm khác nhau. Tọa độ suy rộng thường biểu thị các
chuyển vị dài hoặc chuyển vị góc của các khớp trượt hoặc khớp quay tạo nên cơ cấu tay
máy.

Hình 1.2: Các tọa độ suy rộng của robot

SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

4

c) Khâu và khớp động
Khâu là do một hay một số chi tiết máy nối cứng với nhau tạo thành. Khâu là đơn vị
chuyển động còn chi tiết máy là đơn vị chế tạo.
Khớp động là một liên kết động của hai khâu. Thông thường các khâu của tay máy
được nối với nhau bởi các khớp quay và khớp tịnh tiến. Trong đó các khớp quay và các
khớp tịnh tiến đều thuộc khớp động học loại 5.

Hình 1.3: Khớp trượt

Hình 1.4: Khớp quay

d) Bàn tay nắm bắt
Bàn tay nắm bắt là bộ phận làm việc trực tiếp của tay máy dùng để nắm bắt đối
tượng hoặc thiết bị thao tác trong quá trình robot làm việc. Đối tượng cần nắm bắt của
tay máy là những chi tiết rất đa dạng nên bàn tay nắm bắt của tay máy cũng rất khác
nhau về cấu trúc cũng như về nguyên lý làm việc.
Người ta thường dùng các bàn tay nắm bắt cơ khí, ngoài ra còn có thể thực hiện việc
nắm bắt bằng khí nén, thủy lực hoặc điện tử…
e) Độ cơ động

Độ cơ động là số bậc tự do của tay máy khi cố định bàn tay nắm bắt và được xác định
bằng công thức :
m=W–6
Trong đó :

m là độ cơ động
W là số bậc tự do của tay máy

SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

5

Khi W < 6 thì không phải lúc nào bàn tay nắm bắt cũng đạt tới vị trí và định hướng
mong muốn.
Khi W > 6 thì sẽ có nhiều giải pháp đưa bàn tay nắm bắt tới vị trí và định hướng theo
yêu cầu.
Như vậy độ cơ động của robot càng lớn thì càng có nhiều phương án di chuyển bàn tay
nắm bắt tới mục tiêu mong muốn làm cho khả năng tránh các chướng ngại vật trên đường
dịch chuyển tốt hơn. Ngược lại khi độ cơ động của robot nhỏ thì bàn tay nắm bắt sẽ bị
hạn chế trong quá trình di chuyển.
f) Miền làm việc và các kiểu hệ tọa độ của robot công nghiệp
Miền làm việc của robot là khoảng không gian hoạt động của bàn tay nắm bắt mà
trong miền làm việc này bàn tay nắm bắt có thể hoạt động dễ dàng.

Một số không gian làm việc của robot :

Hình 1.5: Không gian hình hộp

Hình 1.6: Không gian hình vành khăn

1.2.3. Phân loại robot công nghiệp
Tùy vào mục đích sử dụng mà robot công nghiệp được phân ra làm các tiêu chuẩn
khác nhau.
+ Theo tải trọng: robot mini (<1kg), robot loại nhỏ (1÷10 kg), robot loại vừa
(>10÷200kg), robot loại lớn (>200 ÷ 1000 kg) và robot loại siêu lớn (>1000kg).
+ Theo số bậc tự do: robot có 1 bậc tự do, robot có 2 bậc tự do và robot có n bậc tự do.
+ Theo khả năng di chuyển: robot di động và robot có vị trí cố định.
SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

6

+ Theo hệ tọa độ được dùng khi thực hiện các chuyển động: robot hoạt động trong hệ
tọa độ vuông góc (đề các), trong hệ tọa độ trụ, trong hệ tọa độ cầu, trong hệ tọa độ góc
hoặc SCARA.
Robot kiểu tọa độ Đề các: là tay máy có 3 chuyển động cơ bản tịnh tiến theo phương
của các trục hệ tọa độ gốc (cấu hình TTT). Trường công tác có dạng khối hộp chữ nhật,
do đó kết cấu đơn giản, độ cứng vững cao, độ chính xác cơ khí dễ đảm bảo vì vậy thường

được dùng để vẫn chuyển phôi liệu, lắp ráp, hàn trong mặt phẳng….

Hình 1.7: Robot kiểu tọa độ đề các
Robot kiểu tọa độ trụ: vùng làm việc của robot có hình trụ rỗng, thường khớp thứ nhất
chuyển động quay.

Hình 1.8: Robot kiểu tọa độ trụ
Robot kiểu tọa độ cầu: vùng làm việc của robot có dạng hình cầu, thường độ cứng vững
của loại robot này thấp hơn so với 2 loại trên. Ví dụ robot 3 bậc tự do cấu hình R.R.R
hoặc R.R.T làm việc theo kiểu tọa độ cầu.
SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

7

Hình 1.9: Robot kiểu tọa độ cầu
Robot kiểu tọa độ góc: Là loại robot được sử dụng nhiều hơn cả, 3 chuyển động đầu tiên
là các chuyển động quay, trục quay thứ nhất vuông góc với hai trục quay kia. Các chuyển
động định hướng khác cũng là các chuyển động quay. Vùng làm việc của tay máy gần
giống một khối cầu. Tất cả các khâu đều nằm trong mặt phẳng thẳng đứng nên các tính
toán cơ bản là bài toán phẳng. Ưu điểm nổi bật của các loại robot hoạt động theo hệ tọa
độ góc là gọn nhẹ, tức là có vùng làm việc tương đối lớn so với kích thước của bản thân
robot, độ linh hoạt cao. Các robot hoạt động theo hệ tọa độ góc như: Robot PUMA của
hãng Unimation –Nokia (Hoa kì – Phần Lan ), IRb-60 (Thụy Điển), Toshiba, Mitsubishi,

Mazak (NhậtBản ). vv…..
Ví dụ một loại robot hoạt động theo kiểu hệ tọa độ góc có cấu hình RRR.RRR.

Hình 1.10: Robot kiểu tọa độ góc

SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

8

Robot kiểu SCARA: là một kiểu robot mới ra đời nhằm đáp ứng sự đa dạng của quá
trình sản xuất. Tên gọi SCARA là viết tắt của “Selectic Compliant Articulated Robot
Arm” - tay máy mềm dẻo tùy ý. Loại robot này thường được dùng trong công việc lắp
giáp. Ba khớp đầu tiên của robot này có cấu hình R.R.T, các trục khớp đều theo phương
thẳng đứng.

Hình 1.11: Robot kiểu SCARA
+ Theo mục đích sử dụng: robot vận chuyển, robot cấp phôi, robot sơn, robot hàn, robot
lắp ráp, robot vạn năng…
+ Theo kiểu truyền động: truyền động thủy lực, truyền động khí nén, truyền động điện…
+ Theo phương thức điều khiển: điều khiển bằng tay và điều khiển tự động.
+ Theo kiểu điều khiển: điều khiển hở và điều khiển kín.
1.2.4. Ứng dụng robot công nghiệp
a) Mục tiêu ứng dụng robot công nghiệp

Mục tiêu ứng dụng robot công nghiệp nhằm góp phần năng cao năng suất dây
chuyền công nghiệp, giảm giá thành, năng cao chất lượng và khả năng cạnh tranh của
sản phẩm, đồng thời cải thiện điều kiện lao động. Điều đó xuất phát từ những ưu điểm
cơ bản của robot, đã đúc kết lại qua bao nhiêu năm được ứng dụng ở nhiều nước. Đó là:

SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

-

Cơ điện tử K56_Utc

9

Robot có thể thực hiện được một quy trình thao tác hợp lý bằng hoặc hơn một
người thợ lành nghề một cách ổn định trong suốt thời gian làm việc.

-

Khả năng giảm giá thành sản phẩm do ứng dụng robot là vì giảm được đáng kể chi
phí cho người lao động, nhất là ở các nước có mức cao về tiền lương lao động,
cộng các khoản phụ cấp và bảo hiểm xã hội.

-

Việc áp dụng robot có thể làm tăng năng suất dây chuyền công nghệ vì nếu tăng

nhịp độ khẩn trương của dây chuyền sản xuất, nếu không thay thế bằng robot thì
người thợ không thể theo kịp hoặc rất chóng mệt mỏi.

-

Ứng dụng robot có thể cải thiện điều kiện lao động chính là ưu điểm nổi bật nhất.

b) Một số ứng dụng điển hình của robot công nghiệp
Robot được ứng dụng rộng rãi trong nhiều ngành công nghiệp. Những ứng dụng
ban đầu bao gồm gắp đặt vật liệu, hàn điểm và phun sơn. Một trong những công việc
kém năng suất nhất của con người là rèn kim loại ở nhiệt độ cao. Các công việc này đòi
hỏi công nhân di chuyển phôi có khối lượng lớn với nhiệt độ cao khắp nơi trong xưởng.
Việc tuyển dụng công nhân làm việc trong môi trường nhiệt độ cao như vậy là một vấn
đề khó khăn đối với ngành công nghiệp này, và robot ban đầu đã được sử dụng để thay
thế công nhân làm việc trong điều kiện môi trường khắc nghiệt như trong lò đúc, xưởng
rèn và xưởng hàn. Đối với robot thì nhiệt độ cao lại không đáng sợ. Những ứng dụng
điển hình có thể kể ra của robot công nghiệp là:
-

Ứng dụng robot trong công nghệ hàn đường (hàn theo vết hoặc đường dẫn liên
tục).

-

Ứng dụng robot trong lắp ráp.

-

Ứng dụng robot trong nhà máy sản xuất.


1.3. ROBOT PUMA
1.3.1. Phạm vi và đối tượng nghiên cứu
Ra đời cách đây nửa thế kỷ, robot công nghiệp đã có những phát triển vượt bậc.
Nhiều nước trên thế giới sớm áp dụng mạnh mẽ kỹ thuật robot vào sản xuất và nó đã
đem lại những hiệu quả to lớn về kinh tế và kỹ thuật, nâng cao năng suất lao động, tăng
SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

10

Cơ điện tử K56_Utc

chất lượng và cạnh tranh sản phẩm, cải thiện điều kiện và giải phóng sức lao động cho
con người. Vì vậy đề tài nghiên cứu này nhằm tìm hiểu về robot Puma 6 bậc tự do sử
dụng phần mềm Matlab để lập trình và giải toán động học thuận và động học ngược cho
robot Puma. Đồng thời thiết kế mô phỏng không gian làm việc của robot Puma trên nền
tảng phần mềm Matlab.
1.3.2. Ứng dụng của cánh tay robot Puma
Với tốc độ lặp lại cao và tính linh hoạt thì robot PUMA phù hợp với các bộ phận xử
lý nhỏ và nó có thể dễ dàng thiết kế các chương trình ứng dụng để thực hiện nhiệm vụ
khó khăn nhất. Robot PUMA được ứng dụng thực tế vào việc lắp các bảng mạch ô tô,
động cơ điện, bo mạch in, radio, tivi và đồ gia dụng…Ngoài ra còn có chức năng đóng
gói trong công nghiệp thực phẩm, dược phẩm …, nó còn được ứng dụng trong các hệ
thống sản xuất linh hoạt, dùng để tháo lắp thiết bị, gá đặt phôi…

SVTH: Phạm Quang Huy


GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

11

Chương 2
ĐỘNG HỌC ROBOT PUMA
2.1. CÁC THAM SỐ KHÂU CỦA ROBOT CÔNG NGHIỆP VÀ MA TRẬN BIẾN
ĐỔI THUẦN NHẤT
2.1.1. Các tham số khâu của robot công nghiệp
Xét khớp i và khớp i+1 ở 2 đầu của khâu i (hình 2.1). Để mô tả khâu, người ta
dùng 2 đại lượng là độ dài đoạn thẳng vuông góc chung 𝑎𝑖 của 2 đường trục khớp (gọi
là chiều dài khâu) và góc 𝛼𝑖 là góc giữa 2 trục khớp (gọi là góc vặn khâu). Hai tham số
này là các tham số kích thước của khâu.
Hai khâu liên tiếp được liên kết với nhau thông qua khớp động như trên hình 2.2.
Vị trí tương đối của chúng được xác định nhờ hai tham số 𝑑𝑖 và 𝜃𝑖 . Trong đó 𝑑𝑖 gọi là
khoảng cách khâu, được xác định bằng cách đo dọc theo đường trục khớp i, nó đồng
thời là độ dài đoạn vuông góc chung của đường thẳng 𝑎𝑖−1 và 𝑎𝑖 . Tham số 𝜃𝑖 là góc
khớp, nó được xác định là góc giữa đường thẳng 𝑎𝑖−1 và 𝑎𝑖 đo trong mặt phẳng vuông
góc với trục khớp i. Đây là hai tham số biểu thị mối quan hệ giữa các khâu cạnh nhau.
Như vậy, mỗi khâu có thể mô tả nhờ 4 tham số, trong đó 2 tham số mô tả kích thước của
khâu và hai tham số còn lại mô tả mối quan hệ giữa 2 khâu nối với nhau qua khớp động.
Đối với khớp quay thì θi là biến khớp, 3 tham số còn lại luôn không đổi. Đối với khớp
trượt thì di là biến khớp, 3 tham số còn lại luôn không đổi.


Hình 2.1: Chiều dài và góc xoắn của 1 khâu
SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

12

Cơ điện tử K56_Utc

Để mô tả mối liên hệ giữa các khâu ta gắn lên mỗi khâu một hệ quy chiếu với
nguyên tắc như sau: Điểm gốc tọa độ của hệ tọa độ i (gọi tắt là hệ i) đặt tại giao điểm
của đường trục khớp i+1 với đường vuông góc chung của đường trục khớp i và đường
trục khớp i+1 (hình 2.2). Nếu 2 đường trục của 2 khớp i và i+1 giao nhau thì lấy gốc tọa
độ là giao điểm của 2 đường trục đó. Nếu 2 đường trục của 2 khớp đó song song thì gốc
tọa độ có thể đặt tại điểm bất kỳ trên đường trục khớp động i+1. Trục Z của hệ i trùng
với đường trục của khớp i+1, trục X trùng với đường vuông góc chung ai, có chiều từ
khớp động i tới khớp động i+1. Trục Y vuông góc với trục X và trục Z xác định theo
quy tắc bàn tay phải.

Hình 2.2: Đặt hệ quy chiếu lên các khâu
2.1.2. Ma trận biến đổi thuần nhất
Sau khi thiết lập hệ tọa độ của các khâu, mối quan hệ chuyển đổi giữa hệ i-1 và
hệ i có thể thực hiện nhờ phép tịnh tiến và quay hệ tọa độ theo trình tự sau đây:
Để biến đổi từ hệ i-1 tới hệ i, thoạt đầu quay hệ i-1 quanh trục Zi-1 một góc θi,
tiếp đó tịnh tiến dọc theo trục Zi-1 một đoạn di, rồi tịnh tiến dọc theo trục Xi (khi này
đã trùng với trục Xi-1) một đoạn ai cuối cùng quay đi góc αi quanh trục Xi khiến cho hệ
SVTH: Phạm Quang Huy


GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

13

i-1 trùng với hệ i. Khi dùng một ma trận biến đổi Ai để biểu thị tổng hợp 4 lần biến đổi
trên đây cần chú ý tới hệ tọa độ sau mỗi lần quay hoặc tịnh tiến đều thay đổi. Mỗi lần
biến đổi sau đều thực hiện đối với hệ tọa độ động, do đó khi tính toán toán tử biến đổi
cần nhân phải. Như vậy, ma trận biến đổi thuần nhất của khâu thứ i sẽ là:
𝑇𝑖 = 𝑇𝑟𝑎𝑛(0,0, 𝑑𝑖 ). 𝑅𝑜𝑡(𝑧, 𝜃𝑖 ). 𝑇𝑟𝑎𝑛𝑠(𝑎𝑖 , 0,0). 𝑅𝑜𝑡(𝑥, 𝛼𝑖 )
1
𝑇𝑖 = [0
0
0
cθi
sθ
𝑇𝑖 = [ i
0
0

0
1
0
0


cθi
0 0
0 0 ] . [sθ1
1 𝑑𝑖
0
0 1
0
−sθi cαi
cθi cαi
sαi
0

−sθi
cθi
0
0

sθi sαi
−cθi sαi
cαi
0

0
0
1
0

0 1
0] . [ 0
0 0

1 0

0
1
0
0

0 𝑎𝑖 1
0 0 ] [0
1 0 0
0 1 0

0
cθi
sθ1
0

0
−sθi
cθ1
0

0
0
]
0
1

ai cθi
ai sθi

]
di
1

Trong đó các hàm lượng giác được viết gọn như sau:
cosθi = cθi ; sinθi = sθi ;
cosαi = cαi ; sinαi = sαi ;
Trên thực tế, khi thiết kế rất nhiều robot công nghiệp một số tham số khâu thường lấy
các giá trị đặc biệt, ví như lấy 𝛼𝑖 = 0 hoặc 90, cũng có thể lấy 𝑑𝑖 = 0 hoặc 𝑎𝑖 = 0, nhờ
đó việc tính toán ma trận biến đổi Ai trở nên đơn giản hơn, đồng thời việc điều khiển
cũng dễ dàng hơn.
2.1.3. Ma trận quan hệ
Đối với mỗi khâu của robot ta sẽ thiết lập một hệ tọa độ và dùng phép biến đổi
thuần nhất để mô tả mối quan hệ tương đối giữa các hệ tọa độ đó, còn gọi là vị thế tương
đối. Thông thường ta gọi ma trận biến đổi thuần nhất mô tả mối quan hệ tương đối giữa
hệ tọa độ của một khâu với hệ tọa độ của khâu tiếp theo là ma trận biến đổi T hoặc ma
trận T. Nếu 01T là ma trận biểu thị vị thế của hệ tọa độ khâu thứ nhất đối với hệ tọa độ
cố định, 12T là ma trận biểu thị vị thế của hệ tọa độ khâu thứ 2 đối với hệ tọa độ khâu thứ
SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

14

nhất thì vị thế của hệ tọa độ khâu thứ 2 trong hệ tọa độ cố định có thể biểu thị bằng tích

của 01T và 12T:
0
2T

= 01T. 12T

Tương tự có ma trận 23T biểu thị vị thế của hệ tọa độ khâu thứ 3 đối với hệ tọa độ cố
định:
0
3T

= 01T. 12T. 23T

Cứ thế ta suy ra ma trận 𝑛0𝑇 bằng cách nhân các ma trận
0
𝑛𝑇

= 01T. 12T. 23T … 𝑖−1𝑖T;

i−1
𝑖 T.

Khi đó ta có:

i = 1; 2; 3…n.

(2.2)

Phương trình (2.2) được gọi là phương trình động học của robot, kết quả cho ta ma trận
có dạng:

𝑛𝑥
𝑛𝑦
0
𝑛T = [
𝑛𝑧
0

𝑜𝑥
𝑜𝑦
𝑜𝑧
0

𝑎𝑥
𝑎𝑦
𝑎𝑧
0

𝑝𝑥
𝑝𝑦
]
𝑝𝑧
1

Trong đó 3 cột đầu biểu thị tư thế của bàn tay nắm bắt, còn cột 4 biểu thị vị trí của bàn
tay.
2.2. ĐỘNG HỌC ROBOT
2.2.1.Mục đích, nội dung và phương pháp nghiên cứu động học
Nội dung chủ yếu của nghiên cứu động học robot công nghiệp là xác định vị trí,
vận tốc và gia tốc của các điểm đặc trưng trên các khâu của cơ cấu robot. Động học
robot công ngiệp nghiên cứu cách giải của hai lớp bài toán:

+Bài toán thuận hay còn gọi là bài toán phân tích động học
Đây là bài toán cho trước chương trình chuyển động dưới dạng quan hệ hàm qi(t) của
các biến khớp, ta cần phải xác định quy luật biến đổi của các tham số động học đặc trưng
cho chuyển động của các khâu. Nói cách khác, bản chất của việc giải bài toán thuận của
động học robot chủ yếu nhằm thiết lập phương trình động học robot và xác định vị thế
của bàn tay nắm bắt.
+Bài toán ngược hay còn gọi là bài toán tổng hợp chuyển động
SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

15

Đây là bài toán ta cần xác định biến khớp qi(t) đảm bảo cho bộ phận công tác của robot
thực hiện các chuyển động cho trước. Bài toán tổng hợp chuyển động robot được quan
tâm đặc biệt vì lời giải của nó là cơ sở để xây dựng chương trình điều khiển chuyển động
của robot công nghiệp. Để giải các bài toán động học có thể dùng các phương pháp khác
nhau. Tuy nhiên trong đề tài này ta sẽ dùng phương pháp phân ly biến số để giải.

2.2.2. Các bước giải bài toán động học
a) Bài toán động học thuận
Bước 1: Gắn hệ tọa độ (Oxyz)i với i=0,1,2…n lên các khâu của tay máy.
Việc gắn tọa độ lên các khâu đóng vai trò rất quan trọng khi xác lập hệ phương trình
động học của robot.
Bước 2: Lập bảng các tham số Denavit-Hartenberg.

Bước 3: Tính được các ma trận chuyển

i−1
𝑖T

Bước 4: Nhân liên tiếp các ma trận chuyển
𝑛𝑥
𝑛𝑦
0
𝑛T = [
𝑛𝑧
0

(i=1,2,…n) của 2 khâu liên tiếp nhau.

i−1
𝑖T

𝑜𝑥
𝑜𝑦
𝑜𝑧
0

(i=1,2,…n), ta được ma trận 𝑛0T:

𝑎𝑥
𝑎𝑦
𝑎𝑧
0


𝑝𝑥
𝑝𝑦
]
𝑝𝑧
1

Vị trí bàn tay kẹp được xác định bằng vecto 𝑝⃗ = (𝑝𝑥 , 𝑝𝑦 , 𝑝𝑧 )
Bước 5: Xác định phương hướng của bàn tay kẹp
b) Bài toán động học ngược
Bước 1: Từ ma trận 𝑛0T tìm được ở bài toán động học thuận :
𝑛𝑥 𝑜𝑥 𝑎𝑥 𝑝𝑥
𝑛𝑦 𝑜𝑦 𝑎𝑦 𝑝𝑦
0
[
]
T
=
𝑛
𝑛𝑧 𝑜𝑧 𝑎𝑧 𝑝𝑧
0 0
0 1
0
𝑛T=

𝑇𝑖 . 𝑇𝑛𝑖

(1)

(2)


Bước 2: Nhân 2 vế của phương trình (2) với (𝑇𝑖 )−1
SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

16

(𝑇𝑖 )−1 . 𝑛0𝑇 = 𝑇𝑛𝑖

(3)

Với (𝑇𝑖 )−1 = 𝑇𝑖−1 … . 𝑇2−1 . 𝑇1−1
Từ (1) và (3) ta có:
𝑛𝑥
𝑛
𝑇𝑛𝑖 = 𝑇𝑖−1 … . 𝑇2−1 . 𝑇1−1 . [ 𝑦
𝑛𝑧
0

𝑜𝑥
𝑜𝑦
𝑜𝑧
0

𝑎𝑥

𝑎𝑦
𝑎𝑧
0

𝑝𝑥
𝑝𝑦
]
𝑝𝑧
1

Từ đó ta giải bài toán động học ngược bằng phương pháp đại số
2.3. ĐỘNG HỌC THUẬN ROBOT PUMA
2.3.1. Các bước thiết lập phương trình động học robot
Để thiết lập phương trình động học của robot ta có thể tiến hành theo các bước sau:
a) Xác định các hệ tọa độ
Việc gắn tọa độ lên các khâu có vai trò rất quan trọng khi thiết lập hệ phương
trình động học của robot. Nguyên tắc chung đã trình bày một cách tổng quát trong phần
trên. Trong thực tế các trục nối khớp động của robot thường song song hoặc vuông góc
với nhau, tức là rơi vào những trường hợp đặc biệt, nên có thể gây nhầm lẫn. Hơn nữa
việc xác định các hệ tọa độ cần phải phù hợp với các phép biến đổi của ma trận

𝑖−1
𝑖T

để

có thể sử dụng được bộ thông số DH. Vì thế, trình tự xác định các hệ tọa độ cần được
lưu ý các điểm sau:
- Trục Zi phải chọn cùng phương với trục khớp động i+1.
- Các hệ tọa độ Oxyz tuân theo quy tắc bàn tay phải.

- Khi gắn hệ tọa độ lên các khâu, phải tuân theo phép biến đổi của ma trận

𝑖−1
𝑖 T.

Đó là 4 phép biến đổi : 𝑇𝑟𝑎𝑛(0,0, 𝑑𝑖 ), 𝑅𝑜𝑡(𝑧, 𝜃𝑖 ), 𝑇𝑟𝑎𝑛𝑠(𝑎𝑖 , 0,0), 𝑅𝑜𝑡(𝑥, 𝛼𝑖 ). Như vậy
có thể xem hệ tọa độ thứ i+1 là do phép biến đổi từ hệ tọa độ thứ i. Các phép quay Rot
và tịnh tiến Trans trong các phép biến đổi này phải có mặt trong các phép biến đổi của
ma trận 𝑖−1𝑖T. Các thông số DH cũng được xác định dựa vào các phép biến đổi này và
việc gắn các hệ tọa độ lên các khâu ở vị trí, khi mà biến khớp có giá trị ban đầu, thường
bằng 0.
b. Lập bảng thông số DH.
c. Xác định các ma trận Ai theo các thông số DH.
SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

17

d. Tính các ma trận

i−1
iT.

e. Lập phương trình động học cơ bản.

2.3.2. Giải bài toán động học thuận robot Puma
Robot PUMA là robot có 6 bậc tự do, cấu hình RRRRRR. Để giải bài toán động
học thuận của robot Puma 6 bậc tự do ta cũng thực hiện lần lượt các bước giải bài toán
động học của robot bất kì.
-

Bước 1: Gắn hệ trục tọa độ lên các khâu của cánh tay robot Puma.

Với bộ 4 thông số αi , ai , di , θi xác định ở mục trên được gọi là bộ thông số DH. Bộ
thông số này được mô tả chi tiết theo mối liên hệ với các trục tọa độ và được xác định
như sau :
• 𝛼𝑖 là góc giữa trục 𝑍⃗𝑖 và trục 𝑍⃗𝑖−1 đo theo trục 𝑋⃗𝑖 .
• 𝑎𝑖 là khoảng cách giữa trục 𝑍⃗𝑖−1 và trục 𝑍⃗𝑖 đo dọc theo trục 𝑋⃗𝑖−1 .
• 𝑑𝑖 là khoảng cách giữa trục 𝑋⃗𝑖−1 và trục 𝑋⃗𝑖 đo dọc theo trục 𝑍⃗𝑖−1 .
• 𝜃𝑖 là khoảng cách giữa trục 𝑋⃗𝑖−1 và trục 𝑋⃗i đo dọc theo trục 𝑍⃗𝑖−1 .
Với cách xác định như trên ta gắn các hệ trục tọa độ lên các khâu của cánh tay robot như
hình 2.3 dưới đây:

Hình 2.3: Hệ tọa độ gắn trên các khâu của robot Puma.
SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

18

Cơ điện tử K56_Utc


Kích thước các khâu của cánh tay robot, cụ thể là robot Puma 762 được trình bày
dưới hình sau:

Hình 2.4: Kích thước các khâu của robot Puma 762
Trong đó a2 = 650 mm ; d2 = 190 mm ; d4 = 600 mm ; 𝑑6 = 125 mm.
-

Bước 2: Thành lập bảng thông số DH

Từ hệ tọa độ gắn trên các khâu của cánh tay robot Puma như trên ta xây dựng được
bảng thông số động học như sau:

SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

19

BẢNG THÔNG SỐ DENAVIT- HARTENBERA ( DH )
Khâu i

𝛼𝑖

𝑎𝑖


1

-90

0

2

0

3

90

0

4

-90

0

5

90

0

6


0

0

𝑑𝑖

𝜃1 + 90

0

𝑎2

𝜃𝑖

𝑑2

𝜃2
𝜃3 + 90

0

𝑑4

𝜃4
𝜃5

0

𝑑6


𝜃6

Giới hạn chuyển động của các khâu :
-Khâu1:−1600 ≤ 𝜃1 ≤ 1600
-Khâu 2: −1100 ≤ 𝜃2 ≤ 1100
-Khâu 3: −1350 ≤ 𝜃3 ≤ 1350
-Khâu 4: −2660 ≤ 𝜃4 ≤ 2660
-Khâu 5: −1000 ≤ 𝜃5 ≤ 1000
-Khâu 6: −3000 ≤ 𝜃6 ≤ 3000
Bước 3: Dựa vào bảng thông số DH ở trên, ta xác định ma trận biến đổi thuần nhất 𝑖−1𝑖T
theo công thức :
cosθi
sinθi
i−1
𝑖T = [
0
0

−sinθi cosαi
cosθi cosαi
sinαi
0

SVTH: Phạm Quang Huy

sinθi sinαi
−cosθi sinαi
cosαi
0


ai cosθi
ai sinθi
]
di
1
GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

Cơ điện tử K56_Utc

20

Từ đó ta có:
cθ1
sθ1
0
1T = [
0
0

−sθ1 𝑐(−90)
cθ1 c(−90)
s(−90)
0

cθ2
sθ2
1

2T = [
0
0

−sθ2 𝑐0
cθ2 c0
s0
0

cθ3
sθ3
2
3T = [
0
0

−sθ3 𝑐90
cθ3 c90
s90
0

cθ4
sθ4
3
4T = [
0
0

−sθ4 𝑐(−90)
cθ4 c(−90)

s(−90)
0

cθ5
sθ5
4
5T = [
0
0

−sθ5 𝑐90
cθ5 s90
s90
0

cθ6
sθ6
5
6T = [
0
0

−sθ6 𝑐0
cθ6 c0
s0
0

0
6T


sθ1 𝑠(−90)
−cθ1 s(−90)
𝑐(−90)
0

sθ2 𝑠0
−cθ2 s0
𝑐0
0

a2 cθ2
cθ2
a2 sθ2
sθ
]=[ 2
d2
0
0
1

sθ3 𝑠90
−cθ3 s90
c90
0

−sθ2
cθ2
0
0


0. cθ3
cθ3
0. sθ3
sθ
]=[ 3
0
0
1
0

sθ4 𝑠(−90)
−cθ4 s(−90)
c(−90)
0

sθ5 𝑠90
−cθ5 s90
c90
0
sθ6 𝑠0
cθ6 s0
c0
0

0. cθ1
cθ1 0
0. sθ1
sθ 0
]=[ 1
0 −1

0
0
0
1

0
0
1
0

0. cθ6
cθ6
0. sθ6
sθ
]=[ 6
d6
0
0
1

−sθ6
cθ6
0
0

0
0
]
0
1


0 a2 cθ2
0 a2 sθ2
]
1
d2
0
1
sθ3
−cθ3
0
0

0. cθ4
cθ4 0
sθ 0
0. sθ4
]=[ 4
0 −1
d4
0
0
1

0. cθ5
cθ5 0
0. sθ5
sθ 0
]=[ 5
0

0
1
1
0
0

−sθ1
cθ1
0
0

sθ5
−cθ5
0
0

0
0
]
0
1
−sθ4
cθ4
0
0

0
0
]
d4

1

0
0
]
0
1

0 0
0 0]
1 d6
0 1

= 01T 12T 23T 34T 45T 65T

*Đến đây ta dùng phần mềm Matlab nhân các ma trận và được kết quả như sau :
0
6T

= 01T 12T 23T 34T 45T 65T =

SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền


Robot Puma

[


Cơ điện tử K56_Utc

21

-sθ6 (cθ4 sθ1 -sθ4 (cθ1 sθ2 sθ3 - cθ1 cθ2 cθ3 )) - cθ6 (cθ5 (sθ1 sθ4 + cθ4 (cθ1 sθ2 sθ3 -

cθ1 cθ2 cθ3 )) + sθ5 (cθ1 cθ2 sθ3 + cθ1 cθ3 sθ2 ))

, sθ6 (cθ5 (sθ1 sθ4 + cθ4 (cθ1 sθ2 sθ3 -

cθ1 cθ2 cθ3 )) + sθ5 (cθ1 cθ2 sθ3 + cθ1 cθ3 sθ2 )) - cθ6 (cθ4 sθ1 - sθ4 (cθ1 sθ2 sθ3 cθ1 cθ2 cθ3 )) ,

cθ5 (cθ1 cθ2 sθ3 + cθ1 cθ3 sθ2 ) - sθ5 (sθ1 sθ4 + cθ4 (cθ1 sθ2 sθ3 -

cθ1 cθ2 cθ3 ))

,

d4(cθ1 cθ2 sθ3

+

cθ1 cθ3 sθ2 )

-

d6(sθ5 (sθ1 sθ4

+


cθ4 (cθ1 sθ2 sθ3 - cθ1 cθ2 cθ3 )) - cθ5 (cθ1 cθ2 sθ3 + cθ1 cθ3 sθ2 )) - d2sθ1 + a2cθ1 cθ2
]

[

sθ6 (cθ1 cθ4 + sθ4 (sθ1 sθ2 sθ3 - cθ2 cθ3 sθ1 )) + cθ6 (cθ5 (cθ1 sθ4 - cθ4 (sθ1 sθ2 sθ3 -

cθ2 cθ3 sθ1 )) - sθ5 (cθ2 sθ1 sθ3 + cθ3 sθ1 sθ2 ))

, cθ6 (cθ1 cθ4

+ sθ4 (sθ1 sθ2 sθ3

-

cθ2 cθ3 sθ1 )) - sθ6 (cθ5 (cθ1 sθ4 - cθ4 (sθ1 sθ2 sθ3 - cθ2 cθ3 sθ1 )) - sθ5 (cθ2 sθ1 sθ3 +
cθ3 sθ1 sθ2 )) ,
cθ3 sθ1 sθ2 )

sθ5 (cθ1 sθ4 - cθ4 (sθ1 sθ2 sθ3 - cθ2 cθ3 sθ1 )) + cθ5 (cθ2 sθ1 sθ3 +

,

d4(cθ2 sθ1 sθ3 + cθ3 sθ1 sθ2 ) + d6(sθ5 (cθ1 sθ4 - cθ4 (sθ1 sθ2 sθ3 -

cθ2 cθ3 sθ1 )) + cθ5 (cθ2 sθ1 sθ3 + cθ3 sθ1 sθ2 )) + d2cθ1 + a2cθ2 sθ1
[

]


sθ4 sθ6 (cθ2 sθ3 + cθ3 sθ2 ) - cθ6 (sθ5 (cθ2 cθ3 - sθ2 sθ3 ) + cθ4 cθ5 (cθ2 sθ3 + cθ3 sθ2 ))

, sθ6 (sθ5 (cθ2 cθ3 - sθ2 sθ3 ) + cθ4 cθ5 (cθ2 sθ3 + cθ3 sθ2 )) + cθ6 sθ4 (cθ2 sθ3 + cθ3 sθ2 ) ,
cθ5 (cθ2 cθ3

-

sθ2 sθ3 )

-

cθ4 sθ5 (cθ2 sθ3

+

cθ3 sθ2 )

,

d4(cθ2 cθ3 - sθ2 sθ3 ) - a2sθ2 + d6(cθ5 (cθ2 cθ3 - sθ2 sθ3 ) - cθ4 sθ5 (cθ2 sθ3 + cθ3 sθ2 ))
]

[

0,

0,

0,


1

]

*** Code Matlab :
syms theta1 theta2 theta3 theta4 theta5 theta6 a2 d2 d4 d6
T01=[cos(theta1) 0 -sin(theta1) 0; sin(theta1) 0 cos(theta1) 0; 0 -1 0 0; 0 0 0 1];
T12=[cos(theta2) -sin(theta2) 0 a2*cos(theta2); sin(theta2) cos(theta2) 0 a2*sin(theta2);
0 0 1 d2; 0 0 0 1];
T23=[cos(theta3) 0 sin(theta3) 0; sin(theta3) 0 -cos(theta3) 0; 0 1 0 0; 0 0 0 1];
T34=[cos(theta4) 0 -sin(theta4) 0; sin(theta4) 0 cos(theta4) 0; 0 -1 0 d4; 0 0 0 1];
SVTH: Phạm Quang Huy

GVHD: TS. Đinh Thị Thanh Huyền