Xây dựng mô hình thực nghiệm ứng dụng bộ vi điều khiển dsPic30f4013 cho hệ chuyển động robot công nghiệp 3 bậc tự do

Thiết kế chế tạo vaø điều khiển tay maùy Trang 1
BỘ CÔNG THƯƠNG
TRƯỜNG ĐẠI HỌC KINH TÊ - KỸ THUẬT CÔNG NGHIỆP
LUẬN VĂN TỐT NGHIỆP
TÊN ĐỀ TÀI LUẬN VĂN:
Xây dựng mô hình thực nghiệm ứng dụng bộ vi điều khiển dsPic30f4013 cho
hệ chuyển động robot công nghiệp 3 bậc tự do
Ngành đào tạo: KỸ THUẬT ĐIỆN
Mã số ngành: 103
Họ và tên sinh viên: Lưu Tuấn Khanh
Người hướng dẫn luận văn tốt nghiệp
1. ThS. Võ Thu Hà

GVHD: TS. Voõ Thu Haø

SVTH: Löu Tuaán
Khanh
Thit k ch to vaứ iu khin tay maựy Trang 2
H ni - Nm 2012
LI NểI U
Vo u th k trc, ý tng u tiờn v robot ó xut hin, th hin c
m, khao khỏt ca con ngi l sỏng to ra nhng ngi mỏy thay th mỡnh
trong cỏc cụng vic nng nhc, nhm chỏn, nguy him. Vo khong nm 1940, mu
robot u tiờn ra i to phũng thớ nghim quc gia OAK Ridge v Argonnecuar
M, vn chuyn cỏc cht phúng x. n thp k 80 trờn th gii ó cú 40 nghỡn
robot thuc 500 kiu, do 200 hóng tham gia sn xut. Ngy nay robot c s dng
rng rói trong sn xut, nghiờn cu khoa hc v i sng. ú l thit b khụng th
thiu c trờn cỏc h thng sn xut, c bit l cỏc h thng sn xut linh hot.
Robot ngy cng khộo lộo v thụng minh, c coi l sn phm in hỡnh ca mt
ngnh k thut mi: Ngnh c-in t (Mechatronics).
Cho n nay, hu ht cỏc trng i hc k thut Vit Nam ó a mụn

robot cụng nghip vo chng trỡnh chớnh khúa. Mụn hc ny c ging dy ch
yu cho cỏc ngnh c khớ, song cỏc khớa cnh riờng ca k thut robot, nh k thut
iu khin, lp trỡnh, mụ phng cng c cỏc ngnh khỏc rt quan tõm.
gúp phn vo vic tỡm hiu ng dng Robot cụng nghip vo sn xut.
Trong khuõn kh ca ỏn tt nghip vi ti em c giao l: Xõy dng mụ
hỡnh thc nghim ng dng b vi iu khin dsPic30f4013 cho h chuyn ng
robot cụng nghip 3 bc t do. õy l ti mi, cng vi s thu nhn kin thc
hn ch, nờn trong quỏ trỡnh thc hin ti v hon thnh ỏn, em cng khụng
trỏnh khi nhng thiu sút.
Vy em kớnh mong nhn c nhng ý kin phờ bỡnh v s hng dn ca
cỏc thy cụ. em hon thnh c ỏn ny l nh s hng dn tn tỡnh ca cụ
Vừ Thu H cựng vi s giỳp ca cỏc thy cụ trong khoa in. Em xin chõn
thnh cm n cụ Vừ Thu H v cỏc thy cụ trong khoa v kớnh chỳc cỏc thy cụ sc
khe di do v cụng tỏc tt.

H Ni, ngy 10 thỏng 06 nm 2012.

GVHD: TS. Voừ Thu Haứ

SVTH: Lửu Tuaỏn
Khanh
Thiết kế chế tạo vaø điều khiển tay maùy Trang 3
MỤC LỤC
1.2 GIỚI THIỆU VỀ ROBOT CÔNG NGHIỆP 4
1.4.1 Đặt vấn đề 11
CHƯƠNG 1
TỔNG QUAN VỀ ROBOT CÔNG NGHIỆP
1.1 ĐẶT VẤN ĐỀ.
Thuật ngữ ROBOT đã được nhắc đến lần đầu tiên trong một tác phẩm của
nhà văn Karel Capek, Tiệp Khắc (cũ).

Thuật ngữ Inducstrial Robot (IR), xuất hiện đầu tiên ở Mỹ vào đầu những
năm 1960 do công ty AMF( American Machine and Foundry company) quảng cáo mô
tả một thiết bị mang dáng dấp và có một chức năng như tay người được điều khiển tự
động để thực hiện một số thao tác sản xuất thiết bị có tên gọi là “Versatran”.
Châu Á đi đầu là Nhật Bản cũng bắt đầu nghiên cứu Robot công nghiệp từ
những năm 1968. Ở các nước châu Âu như: Đức, Ý, Anh, Thụy Điển….bắt đầu nghiên
cứu và sản xuất Robot từ những năm 1970.
Sự phát triển không ngừng của công nghệ bán dẫn, công nghệ thông tin, trí
tuệ nhân tạo và cơ khí chính xác đã đưa Robot lên một tầm cao mới, từ đó robot không
còn là những cỗ máy cồng kềnh vô tri vô giác chỉ biết lặp đi lặp lại một hành vi nhất
định nữa mà nó đã bắt đầu có cảm giác, suy nghĩ, phân tích. Từ đó, rất nhiều thế hệ
robot đã ra đời với nhiều chủng loại và nhiệm vụ khác nhau như: Robot công nghiệp,
robot giúp việc trong gia đình, robot thăm dò nghiên cứu, robot y học trong đó có
những loại robot sản xuất ra để tăng năng suất lao động như robot công nghiệp, đó
cũng là loại robot chúng ta sẽ nghiên cứu cụ thể trong đề tài này.

GVHD: TS. Voõ Thu Haø

SVTH: Löu Tuaán
Khanh
Thiết kế chế tạo vaø điều khiển tay maùy Trang 4


Hình 1.1 Robot công công nghiệp.
1.2 GIỚI THIỆU VỀ ROBOT CÔNG NGHIỆP.
1.2.1 Các thành phần chính của robot công nghiệp.
Hình 1.2 Hình dạng điển hình và các bộ phận của robot công nghiệp.
Một robot công nghiệp thường có các thành phần chính sau:
- Tay máy.
- Bộ phận dẫn động.

- Bộ phận điều khiển.
- Cảm biến.

GVHD: TS. Voõ Thu Haø

SVTH: Löu Tuaán
Khanh
Thiết kế chế tạo vaø điều khiển tay maùy Trang 5
1.2.1.1 Tay máy.
Tay máy công nghiệp là một bộ phận cơ khí quan trọng đóng vai trò là bộ
phận chấp hành của robot. Tay máy được mô phỏng giống bàn tay của con người
nên nó có các bộ phận chính sau:
- Cánh tay robot.
- Khớp robot.
- Bàn tay robot.
- Khuỷa tay và cẳng tay.
- Bậc tự do của robot.
- Bàn tay robot.
- Cơ cấu kẹp.
Cánh tay được nối với chân đế bằng khớp vai, còn bàn tay nối với cánh tay
bằng khớp cổ tay. Như vậy, giữa bàn tay và chân đế có nhiều khâu và khớp trun
gian, số lượng và khớp để tạo nên một cánh tay máy tùy thuộc vào chức năng,
nhiệm vụ của robot mà cấu tạo của tay máy cũng rất khác nhau.
Vì thế, tay máy có thể là một cơ cấu phẳng hay không gian. Tay máy có
nhiều bậc tự do có khả năng thực hiện các thao tắc trong mặt phẳng hoặc không
gian với một phan vi nhất định. Không gian hoạt động của tay máy còn được gọi là
trường hoạt động của tay máy.

GVHD: TS. Voõ Thu Haø

SVTH: Löu Tuaán
Khanh
Giao tiếp ngõ vào:
Con người
Bộ điều khiển
Bộ phận dẫn động
Tay máy
Cảm biến
Giao tiếp ngõ ra: Quá
trình tự động được
thực hiện
Hình 1.3 Các thành phần chính của hệ thống robot.
Thiết kế chế tạo và điều khiển tay máy Trang 6
Tùy thuộc vào u cầu từng loại robot mà người ta thường đưa ra các thơng
số kĩ thuật để đánh giá khả năng hoạt động của robot. Một số thơng cơ bản thường
gặp của robot:
- Tải trọng: khẳ năng mang tải của robot.
- Trường hoạt động của robot: là phạm vi khơng gian mà khả năng
robot có thể với tới.
- Độ phân giải khơng gian: là khả năng dịch chuyển bé nhất của các
khớp bao gồm các chuyển dịch về góc và chuyển dịch tịnh tiến.
- Độ chính xác: là khả năng robot có thể đạt được.
1.2.1.2 Bộ phận dẫn động.
Bộ phận dẫn động của robot thơng thường là các động cơ:
- Động cơ điện 1 chiều.
- Động cơ điện xoay chiều.
- Động cơ bước.
- Động cơ khí nén và thủy lực.
Động cơ khí nén và thủy lực sử dụng trong các trường hợp u cầu về cơng
suất lớn, tốc độ chậm mà các loại động cơ điện khơng đáp ứng được.

Tuy nhiên, khi sử dụng hệ thống dẫn động robot bằng thủy lực thì kết cấu
robot khá cồng kềnh và phức tạp hơn so với dung động cơ điện.
1.2.1.3 Bộ phận điều khiển.
Để điều khiển các hoạt động của robot,, bộ điều khiển robot giữ vai trò quan
trọng như bộ não của con người. Bộ phận điều khiển thường được thực thiện thơng
qua một hệ thống các chương trình điều khiển – mỗi chường trình đẳm nhiệm một
nhiệm vụ cụ thể. Tùy thuộc vào đặ điểm làm việc của từng robot mà các chương
trình điều khiển nói trên được xây dựng khác nhau cho phù hợp.
Bộ phận điều khiển có nhiệm vụ cung cấp một chuỗi logic cho chương trình
vận hành tay máy. Ngồi ra bộ điều khiển còn có nhiệm vụ giải quyết các bài tốn
lý thuyết cần cho mỗi bước của chương trình và liên tục đo đạc, xử lý sai số giữa vị
trí thực của robot và các tọa độ tính tốn trong q trình chyển động dưới dạng:
điểm- điểm, hoặc theo một quỹ đạo liên tục.

GVHD: TS. Võ Thu Hà

SVTH: Lưu Tuấn
Khanh
Thiết kế chế tạo và điều khiển tay máy Trang 7
- Động học thuận: có chức năng thiết lập và giải các bài tốn động học,
trên cơ sở các tham số đầu vào bao gồm góc quay và độ dịch chuyển
tịnh tiến của các biến khớp.
- Lưu trữ và chuyển giao: có chức năng tiếp nhận và chuyển giao các
kết quả vừa giải ra động học thuận robot. Cụm lập trình có chức năng
lập trình để sao cho quỹ đạo của robot đi qua các điểm đã định trước
hay thực hiện trọn vẹn quỹ đạo do u cầu đặt ra.
1.2.1.4 Cảm biến.
Cảm biến trong robot có thể chia làm hai loại: cảm biến trạng thái bên trong
và cảm biến trạng thái bên ngồi. Cảm biến trạng thái bên trong có chức năng đo
lường các biến, tham số như: vị trí hay tốc độ khớp được sử dụng cho các mạch

trong hệ thống điều khiển robot. Cam biến trạng thái bên ngồi sẽ đo lường các biến
khoảng cách, mức, tiếp xúc.
Sử dụng cảm biến bên ngồi cho phép robot tiếp xúc với mơi trường một
cách mềm dẻo và thơng minh hơn. Với các cảm biến bên ngồi, các robot có thể
nhìn và cảm giác để thực hiện các nhiệm vụ phức tạp trong khi cần ít các cơ cấu
điều khiển hơn các robot được lập trình sẵn. Đồng thời robot cũng có khả năng
thích nghi với các cơng việc khác nhau, đạt được độ van năng cao hơn.
1.2.2 Đặc tính của robot cơng nghiệp.
1.2.2.1 Tải trọng.
Tải trọng là trọng lượng robot có thể mang và giữ trong khi vẫn đảm bảo một
số đặc tính nào đó. Tải trọng lớn nhất lớn hơn tải trọng định mức rất nhiều, nhưng
robot khơng thể mang tải trọng lớn hơn định mức. Tải trọng của robot thơng thường
rất nhỏ so với trọng lượng của robot.
Ví dụ: robot LR Mate của hang Fanuc có trọng lượng 40 Kg chỉ mang được
tải trọng 3Kg, robot M-16i có trọng lượng 269 Kg mang được tải trọng 18.5 Kg.
1.2.2.2 Độ phân giải khơng gian.
Độ phân giải khơng gian là gia tang nhỏ nhất robot có thể thực hiện khi di
chuyển trong khơng gian. Độ phân giải phụ thuộc vào độ phân giải điều khiển và độ
chính xác cơ khí. Độ phân giải điều khiển được xác định bởi độ phân giải hệ thống

GVHD: TS. Võ Thu Hà

SVTH: Lưu Tuấn
Khanh
Thiết kế chế tạo vaø điều khiển tay maùy Trang 8
điều khiển vị trí và hệ thống phản hồi: là tỷ số của phạm vi di chuyển và số bước di
chuyển của khớp được địa chỉ hóa trong bộ điều khiển robot:
Số bước di chuyển = 2
n
Trong đó: n – số bít nhớ.

Độ di chuyển của robot là tống các dịch chuyển thành phần. Do đó độ phân
giải của cả robot là tổng các độ phân giải của từng khớp robot.
1.2.2.3 Tầm với.
Tầm với là khoảng cách lớn nhất mà robot có thể vươn tới trong phạm vi làm
việc. tầm với là một hàm phụ thuộc vào cấu trúc bobot.
1.2.2.4 Độ chính xác.
Độ chính xác đánh giá độ chính xác vị trí tay robot có thể đạt được. Độ chính
xác được địn nghĩa theo độ phân giải của cơ cấu chấp hành. Độ chính xác di
chuyển đến vị trí mong muốn sẽ phụ thuộc vào độ dịch chuyển nhỏ nhất của khớp.
Khi coi cơ cấu cơ khí có độ chính xác rất cao.
Độ chính xác sẽ thay đổi tùy thuộc vào phạm vi di chuyển của tay robot:
Phạm vi di chuyển càng xa bệ robot, độ chính xác càng giảm do độ mất chính xác
cơ khí càng lớn. Độ chính xác của robot sẽ được cải thiện nếu di chuyển của robot
được giới hạn trong phạm vi cho phép. Tải trọng cũng ảnh hưởng hưởn đến độ
chính xác, tải trọng lớn sẽ gây ra độ chính xác cơ khí thấp và làm giảm độ chính xác
di chuyển. Thông thường độ chính xác di chuyển của robot công nghiệp đạt
0,025mm.

GVHD: TS. Voõ Thu Haø

SVTH: Löu Tuaán
Khanh
Điểm được
địa chỉ hóa
Đích
Độ chính xác
Điểm được
địa chỉ hóa
Độ phân giải đều
Thit k ch to vaứ iu khin tay maựy Trang 9

1.2.2.5 lp li.
lp li ỏnh giỏ chớnh xỏc khi robot di chuyn vi ti mt im
trong nhiu ln hot ng (Vớ d: 100 ln). Do mt s yu t m robot khụng th
vi ti cựng mt im trong nhiu ln hot ng, m cỏc im vi ca robot nm
trong mt vũng trũn vi tõm l im ớch mong mun. Bỏn kớnh ca ng trũn ú
l lp li.
lp li l i lng cú ý ngha quan trng hn chớnh xỏc. chớnh xỏc
ỏnh giỏ bng sai s c nh, sai s c nh cú th phỏn oỏn c v cú th hiu
chnh bng chng trỡnh. Nhng sai s ngu nhiờn khú cú th kh c. lp li
cn phi c xỏc nh bng thc nghim vi ti trng v cỏc hng di chuyn
khỏc nhau (phng thng ng v phng nm ngang). lp li ca cỏc robot
cụng nghip thng 0,025mm.
1.2.2.6 nhỳn.
nhỳn biu th s dch chuyn ca im cui c tay robot ỏp ng li lc
hoc moment tỏc dng. nhỳn ln cú ý ngha l tay robot dch chuyn nhiu khi
lc tỏc ng nh v ngc li. nhỳn cú ý ngha quan trng vỡ nú lm gim
chớnh xỏc dch chuyn khi robot mang ti trng. Nu robot mang ti trng nng,
trng lng ti s lm cho cỏnh tay robot b dch chuyn
Nu robot c lp trỡnh trong iu kin khụng ti ca c cu tay, chớnh
xỏc s gim trong iu kin lm vic cú ti.
1.3 NG DNG CA ROBOT CễNG NGHIP.
1.3.1 Mc tiờu ng dng Robot cụng nghip.
Mc tiờu ng dng Robot cụng nghip nhm nõng cao nng sut dõy truyn
cụng ngh, gim giỏ thnh, nõng cao cht lng v kh nng cnh tranh ca sn
phm, ng thi ci thin iu kin lao ng. iu ú xut phỏt t nhng u im
c bn ca Robot ú l :
- Robot cú th thc hin mt quy trỡnh thao tỏc hp lý bng hoc hn ngi
th lnh ngh mt cỏch n nh trong sut thi gian di lm vic. Do ú Robot giỳp
nõng cao cht lng v kh nng cnh tranh ca sn phm.

GVHD: TS. Voừ Thu Haứ

SVTH: Lửu Tuaỏn
Khanh
Hỡnh 1.4 Minh ha chớnh xỏc v phõn gii iu khin.
Thiết kế chế tạo và điều khiển tay máy Trang 10
- 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.
- Robot giúp tăng năng suất dây chuyền cơng nghệ.
- Robot giúp cải thiện điều kiện lao động. Đó là ưu điểm nổi bật nhất mà
chúng ta cần quan tâm. Trong thực tế sản xuất có rất nhiều nơi người lao động phải
làm việc trong mơi trường ơ nhiễm, ẩm ướt, nóng nực. Thậm chí rất độc hại đến sức
khoẻ và tính mạng như mơi trường hố chất, điện từ, phóng xạ …
1.3.2 Ứng dụng của robot cơng nghiệp.
Robot được ứng dụng rộng rãi trong nhiều lĩnh vực sản suất và đời sống của
con người, trong đó robot cơng nghiệp đóng vai trò rất quan trọng và được ứng
dụng trong nhiều lĩnh vực khác như:
- Phụ vụ máy NC và các hệ thống tự động linh hoạt.
- Đúc.
- Lắp ráp, đóng gói.
- Phun, phủ.
- Tự động hàn.
- Đảm nhận thực hiện cấp phơi phục vụ các ngun cơng trong dây chuyền sản
xuất tự động.
- Chế tạo máy.
- Kiểm tra sản phẩm, hàng hóa.
- Sơn.
- Bảo vệ.
1.3.3 Tình hình ứng dụng của robot cơng nghiệp trên thế giới.
Nhìn chung việc sử dụng robot ở các nước cơng nghiệp phát triển như: Mỹ,

Nhật, Đức… rất phổ biến với tốc độ phát triển hàng năm là rất nhanh. Theo thống
kê chưa đầy đủ, tỷ lệ phân bố phạm vi ứng dụng robot cơng nghiệp trong các lĩnh
vực như sau: Ơtơ 31%, thiết bị điện 16%, dầu khí và thực phẩm 16%, hóa thực
phẩm 12%, thiết bị viễn thơng 8%, máy cơng cụ 6%, kim khí 4% và các ngành khác
chiếm 20%.
Sự phát triển nhanh chóng của khoa học cơng nghệ thuộc các ngành đào tạo ra
bước nhảy vọt về loại hình robot trên thế giới. Hiện nay, có rất nhiều loại robot
thơng minh, loại robot này có các giác quan nhận thức như con người. Ngồi ra,

GVHD: TS. Võ Thu Hà

SVTH: Lưu Tuấn
Khanh
Thiết kế chế tạo và điều khiển tay máy Trang 11
ngày càng nhiều robot siêu nhỏ dùng trong Y học để quan sát chụp các bộ phận bên
trong cơ thể con người. Trong tương lai khơng xa sẽ xuất hiện nhiều loại robot có
khả năng phục vụ gia đình.
1.3.4 Tình hình tiếp cận và ứng dụng Robot cơng nghiệp ở Việt Nam.
Trong giai đoạn trước năm 1990, hầu như trong nước hồn tồn chưa du
nhập về kỹ thuật Robot, thậm chí chưa nhận được nhiều thơng tin kỹ thuật về lĩnh
vực này. Tuy vậy, với mục tiêu chủ yếu là tiếp cận lĩnh vực mới mẻ này trong nước
đã có triển khai các đề tài nghiên cứu khoa học cấp nhà nước: Đề tài 58.01.03 và
52B.03.01.
Giai đoạn tiếp theo từ năm 1990 các ngành cơng nghiệp trong nước bắt đầu
đổi mới. Nhiều cơ sở đã nhập ngoại nhiều loại Robot cơng nghiệp phục vụ các cơng
việc như: tháo lắp dụng cụ, lắp ráp linh kiện điện tử, hàn vỏ Ơtơ xe máy, phun phủ
các bề mặt
Một sự kiện đáng chú ý là tháng 4 năm 1998, nhà máy Rorze/Robotech đã
bước vào hoạt động ở khu cơng nghiệp Nomura Hải Phòng. Đây là nhà máy đầu
tiên ở Việt Nam chế tạo và lắp ráp Robot.

Những năm gần đây, Trung tâm nghiên cứu kỹ thuật Tự động hóa, Trường
đại học Bách Khoa Hà Nội, đã nghiên cứu thiết kế một kiểu Robot mới là Robot
RP. Robot RP thuộc loại Robot phỏng sinh (bắt chước cơ cấu tay người). Hiện nay
đã chế tạo 2 mẫu: Robot RPS-406 dùng để phun men và Robot RPS-4102 dùng
trong cơng nghệ bề mặt.
Ngồi ra Trung tâm còn chế tạo các loại Robot khác như: Robot SCA mini
dùng để dạy học, Robocar cơng nghiệp phục vụ phân xưởng, Robocar chữ thập đỏ
cho người tàn tật … Bên cạnh đó còn xây dựng các thuật tốn mới để điều khiển
Robot, xây dựng “thư viện” các mơ hình của Robot trên máy tính …
1.4 CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN ROBOT CƠNG NGHIỆP.
1.4.1 Đặt vấn đề.
Như ta đã biết khi Robot chuyển động trong khơng gian quỹ đạo thì sẽ chịu
ảnh hưởng của các yếu tố bên ngồi như: nhiễu tác động, ma sát chuyển động, ảnh
hưởng của gia tốc trọng trường. Những yếu tố này làm cho việc chuyển động và
điều khiển Robot hết sức là khó khăn. Cũng vì vậy mà ta ta phải tìm hiểu, nghiên

GVHD: TS. Võ Thu Hà

SVTH: Lưu Tuấn
Khanh
Thit k ch to vaứ iu khin tay maựy Trang 12
cu cỏc phng phỏp iu khin cú th khc phc c nhng nguyờn nhõn nh
hng trờn.
1.4.2 H thng iu khin Robot cụng nghip.
Liờn quan ti cỏc c im ca robot cú th chia bi toỏn iu khin robot
thnh 2 loi: iu khin thụ v iu khin tinh. bi toỏn iu khin thụ s xỏc
nh lut iu khin thớch hp tc v trớ do ú chuyn ng ca cỏc khp bỏm
sỏt qu o thit k trong thi gian quỏ nh nht. Bi toỏn th 2 liờn quan n
quỏ trỡnh robot di chuyn tip xỳc vi mụi trng lm vic nh trng hp robot
lp giỏp mt chi tit vo mt thit b mỏy. Nh vy, quỏ trỡnh lm vic ny s yờu

cu iu khin c lc v v trớ.
T yờu cu thc t ca tng robot ta phõn loi phng phỏp iu khin robot
theo mc ớch c th ca tng bi toỏn t ra. Mi phng phỏp iu khin u cú
mụ hỡnh khỏc nhau, nhng mt thnh phn khụng th thiu l b iu khin ca
robot. B iu khin quyt nh ng dng ca robot. Thy c tm quan trng ca
b iu khin, cỏc nh khoa hc khụng ngng nghiờn cu, tỡm hiu nhm a ra b
iu khin ti u nht.

GVHD: TS. Voừ Thu Haứ

SVTH: Lửu Tuaỏn
Khanh
Thiết kế chế tạo và điều khiển tay máy Trang 13
Hình 1.5 Các bài tốn điều khiển robot.
Điều khiển chuyển động thơ hay điều khiển quỹ đạo có thể thực hiện ở hệ tọa
độ khớp hay tọa độ Decac. Điều khiển chuyển động tinh là điều khiển lực, thực chất
là kết hợp điều khiển lực và quỹ đạo. Điều khiển lực gồm điều khiển trở kháng và
điều khiển hỗn hợp.
Ngồi các phương pháp truyền thống ra, thì còn một số phương pháp điều
khiển thơng minh, có khả năng thích nghi và chống nhiễu cao, đó là điều khiển mờ,
điều khiển mạng nơ ron, điều khiển thích nghi, điều khiển bền vững…
1.4.2.1 Bài tốn điều khiển quỹ đạo.
Chức năng của hệ thống điều khiển chuyển động là đảm bảo tay máy robot chuyển
động bám theo quỹ đạo đặt trước trong mơi trường làm việc. Chuyển động của tay
robot được thực hiện nhờ các hệ thống truyền động khớp robot. Trên cơ sở đó, có
hai dạng hệ thống điều khiển chuyển động: hệ thống điều khiển khơng gian khớp và
hệ thống điều khiển tọa độ Đề - các.

GVHD: TS. Võ Thu Hà

SVTH: Lưu Tuấn
Khanh
Thit k ch to vaứ iu khin tay maựy Trang 14
1.4.2.2 iu khin ta khp.
H thng iu khin ta khp cú i lng iu khin l v trớ ca khp
robot: gúc quay i vi khp quay, dch chuyn ca khp tnh tin. B iu
khin c thit k m bo v trớ khp luụn bỏm theo v trớ t, tc l sai lch v trớ
khp hi t v khụng vi thi gian nh nht. V trớ t ca khp c tớnh toỏn t
lng t v trớ ca tay robot trong khụng gian lm vic thụng qua khõu tớnh toỏn
ng hc ngc.
u im ca phng phỏp iu khin khụng gian khp l b iu khin tỏc
ng trc tip n h truyn ng ca khp. Tuy nhiờn, h thng iu khin ny
khú m bo chớnh xỏc v trớ ca tay khi tn ti cỏc sai lch trong c cu c khớ
hoc thiu thụng tin v quan h v trớ gia tay robot v i tng.
Hỡnh 1.6 S khi th thng iu khin khụng gian khp.
1.4.2.3 iu khin ta cỏc.
H thng iu khin ta cỏc cũn c gi l h thng iu khin khụng
gian lm vic cú chc nng duy trỡ trc tip sai lch v trớ ca tay robot trong khụng
gian lm vic bng khụng.
Lng t ca h thng iu khin l v trớ tay mỏy trong khụng gian lm
vic v lng phn hi l v trớ thc ca tay. Khõu tớnh toỏn ng hc ngc s
thuc mch vũng iu khin phn hi.
u im ca phng phỏp iu khin ta cỏc l tỏc ng trc tip cỏc
bin khụng gian lm vic. Nhng nhc im ca phng phỏp l khi lng tớnh
toỏn ln do tn ti khõu tớnh toỏn ng hc ngc trong vũng iu khin.

GVHD: TS. Voừ Thu Haứ

SVTH: Lửu Tuaỏn
Khanh

ng hc
ngc
B iu
khin
B bin i ng
c
C cu
Robot
Cm bin
d
q
q
d
S
S
Thit k ch to vaứ iu khin tay maựy Trang 15
Hỡnh 1.7 S h thng iu khin ta -Cỏc.
1.4.2.4 Bi toỏn iu khin lc.
iu khin lc l trng hp tng quỏt ca bi toỏn iu khin qu o, hu
ht robot trong quỏ trỡnh lm vic s tip xỳc vi mụi trng khi di chuyn theo mt
qu o cho trc. Khi ú cn iu khin c v trớ v lc ca Robot sinh ra. Hin
nay, cú hai phng phỏp iu khin dựng iu khin lc: phng phỏp iu
khin tr khỏng v phng phỏp iu khin h lai.
1.4.2.5 iu khin tr khỏng.
Nguyờn lý c bn ca iu khin tr khỏng l iu khin v trớ v lc bng
iu chnh tr khỏng ca c cu c khớ tay robot phự hp vi ngoi lc tỏc dng lờn
tay robot khi tip xỳc vi mụi trng.
Tr khỏng c khớ l mt khỏi nim bo th cng ca c cu chng li lc
tỏc dng lờn nú.
iu khin tr khỏng cú hai phng phỏp:

- iu khin tr khỏng th ng.
- iu khin tr khỏng tớch cc.
1.4.2.6 iu khin h lai.
iu khin h lai l phng phỏp iu khin c lp v trớ v lc. Cỏc tớn
hiu u ra ca b iu khin s iu khin c cu chp hnh khp bỏm theo qu
o v trớ v sinh ra lc t trc. Khi robot lm vic tip xỳc vi mụi trng, lc s
c iu chnh theo mt hng, cũn iu khin v trớ s theo mt hng khỏc.
Do c im lm vic ca tay robot khi tip xỳc vi mụi trng m chia lm
hai loi rng buc: rng buc t nhiờn v rng buc nhõn to:
B iu khin h lai c chia lm hai phn:

GVHD: TS. Voừ Thu Haứ

SVTH: Lửu Tuaỏn
Khanh
B iu
khin
B bin i ng
c
C cu
Robot
Cm bin
q
d
S
S
Thiết kế chế tạo và điều khiển tay máy Trang 16
- Bộ điều khiển tuyến tính hóa phản hồi nhằm khử các thành phần phi tuyến và rằng
buộc của hệ thống để nhận được hệ thống độc lập tuyến tính.
- Bộ điều khiển thứ hai được thiết kế để cơ cấu tay robot bám theo quỹ đạo mong

muốn và sinh lực cần thiết.
1.4.3 Một số phương pháp điều khiển robot.
Hệ động lực học của robot thường là các hệ phi tuyến cao, có hàm lượng
giác và nhiều yếu tố bất định nên phương pháp điều khiển robot là các phương pháp
điều khiển phi tuyến. Hiện nay, có nhiều phương pháp điều khiển robot đã được
nghiên cứu và ngày càng phát triển như: điều khiển bền vững, điều khiển thích nghi,
điều khiển trượt và một số phương pháp điều khiển thơng minh như điều khiển mờ
hay điều khiển nơron.
1.4.3.1 Phương pháp điều khiển mơ men tính tốn.
Phương pháp mơ men tính tốn rất phổ biến trong kỹ thuật điều khiển robot
hiện đại. Mục đích của phương pháp là lựa chọn bộ điều khiển có khả năng loại trừ
hồn tồn các tác động phi tuyến của robot đưa hệ thống thành hệ tuyến tính dạng
tích phân kép. Do vậy, các thành phần tạo nên tính tuyến tính của hệ thống như:
trọng trường, ma sát, mơ men…. hồn tồn được loại bỏ. Sau đó, một bộ điều khiển
phản hồi thơng thường dạng PD hay PID được áp dụng để đưa robot đến vị trí mong
muốn. Xuất phát từ phương trình động lực học robot dạng:
( ) ( , ) ( )D q q C q q q g q r
+ + =
&& &&
(1.1)
Từ luật điều khiển mơ men tính tốn như sau:
( ) ( , ) ( )D q u C q q q g q
τ
= + +
&&
(1.2)
Phương trình đặc trưng:
2
Is 0
D P

K s K
+ + =
(1.3)
Trong đó:
s: là tốn tử Laplace
I: ma trận đơn vị
K
D
, K
P
: là các ma trận đường chéo.

GVHD: TS. Võ Thu Hà

SVTH: Lưu Tuấn
Khanh
Thiết kế chế tạo và điều khiển tay máy Trang 17
Khi đó phương trình (2-3) thành phương trình độc lập cho các khớp.
Nhược điểm của phương pháp điều khiển mơ men qn tính là phải biết đầy
đủ và chính xác hệ phương trình động lực của robot. Trong thực tế, đáng tiếc các
thơng số của robot có nhiều bất định và có thể thay đổi theo thời gian. Do đó, để
khử hồn tồn các thành phần phi tuyến cần phải ước lượng chính xác các thơng số
của robot hoặc áp dụng phương pháp tự thích nghi, bù các yếu tố bất định trong q
trình điều khiển.
1.4.3.2 Phương pháp điều khiển trượt.
Phương pháp điều khiển trượt là một dạng của hệ điều khiển có cấu trúc thay
đổi VSC (variable structure control) thuộc lớp các hệ thống điều khiển có phản hồi
khơng liên tục. Điều khiển trượt được ứng dụng cho các hệ thống liên tục và rời rạc
nhiều đầu vào nhiều đầu ra.
Đặc điểm nổi bật nhất của điều khiển trượt là tính bền vững hay nói cách

khác là hệ thống ít nhạy cảm với sự thay đổi của tham số. Điều khiển trượt ổn định
cho lớp đối tượng có tính phi tuyến cao và các tham số khơng được biết đến chính
xác, nên phương pháp này dùng phổ biến cho các bài tốn điều khiển robot.
Các hệ điều khiển trượt có tính ổn định và hội tụ tồn cục. Tuy nhiên, khi mà
giải tần là hữu hạn, ảnh hưởng của độ trễ và qn tính của đối tượng dẫn đến hiện
tượng dao động xung quanh mặt trượt gây nên sai số điều khiển. Để khắc phục hiện
tượng này người ta có thể sử dụng thêm phương pháp điều khiển nâng cao chất
lượng điều khiển.
Thiết kế bộ điều khiển trượt chia làm hai bước :
- Đầu tiên là lựa chọn mặt trượt sao cho sai số điều khiển trượt về khơng.
- Bước tiếp theo là tìm tín hiệu điều khiển để duy trì hệ điều khiển trượt theo
mặt trượt về gốc tọa độ.
Ngun lý điều khiển bằng phương pháp trượt :

GVHD: TS. Võ Thu Hà

SVTH: Lưu Tuấn
Khanh
Thit k ch to vaứ iu khin tay maựy Trang 18
Hỡnh1.8 ng trt trờn mt phng
e e

&
.
1.4.3.3 Phng phỏp iu khin m.
Tp m F xỏc nh trờn tp kinh in M l mt tp m mi phn t ca nú l
mt cp cỏc giỏ tr (x,
à
F
(x)) trong ú x

M v
à
F
l ỏnh x.
à
F
: M

[0, 1]
nh x
à
F
c gi l hm liờn thuc (hoc hm ph thuc) ca tp m F.
Tp kinh in M c gi l c s ca tp m F.
S dng cỏc hm liờn thuc tớnh ph thuc ca mt phn t x no ú cú
hai cỏch: tớnh trc tip (nu
à
F
(x) dng cụng thc tng minh) hoc tra bng (nu
à
F
(x) dng bng).
Cỏc hm liờn thuc
à
F
(x) cú dng trn c gi l hm liờn thuc kiu S.
i vi hm liờn thuc kiu S, do cỏc cụng thc biu din
à
F

(x) cú phc tp ln
nờn thi gian tớnh ph thuc cho mt phn t lõu. Trong k thut iu khin m
thụng thng, cỏc hm liờn thuc kiu S thng c thay gn ỳng bng mt hm
tuyn tớnh tng on.
Mt hm liờn thuc cú dng tuyn tớnh tng on c gi l hm liờn thuc
cú mc chuyn i tuyn tớnh.

GVHD: TS. Voừ Thu Haứ

SVTH: Lửu Tuaỏn
Khanh

&
s>0
0

s=0
s<0
s(t=
0)
Hỡnh 1.9 Hm liờn thuc à
F
(x) cú mc
chuyn i tuyn tớnh.
m
1
à
F
(x)
m

2
m
3
m
4
x
1
0
Thit k ch to vaứ iu khin tay maựy Trang 19
Hm liờn thuc
à
F
(x) nh trờn vi m
1
= m
2
v m
3
= m
4
chớnh l hm ph thuc
ca mt tp kinh in.
1.4.3.5 Phng phỏp iu khin bn vng.
1.4.3.5.1 Ni dung phng phỏp.
B iu khin bn vng cú cu trỳc thay i c thit k cho nhng h thng
robot cng khụng cn bit chớnh xỏc cỏc tham s ca mụ hỡnh robot. Nhim v ca
b iu khin bn vng l xỏc nh c vựng bao m trong vựng bao thỡ h thng
s n nh m khụng chu tỏc ng bi bt c nhiu no, c th nhim v ca b
iu khin VCS l xỏc nh c mt mt trt v lỏi h thng v b mt trt ú
thỡ sau ú h thng s trt trờn b mt ú v s luụn n nh ngay c di tỏc

ng nhiu. Nh vy b iu khin ny loi b tớnh nh hng phi tuyn ca cỏc
khp bng cỏch t h thng ti mt trt. Lut iu khin ny cng c xõy dng
da trờn tiờu chun n nh Luyapunov.
Hỡnh 1.10 Mụ hỡnh lut iu khin bn vng VSS.
1.4.3.5.2 u nhc im.
- u im: B iu khin ny cng ging vi b iu khin thớch nghi Li
Stotile l khụng cn bit chớnh xỏc cỏc tham s ng lc hc ca robot m vn n
nh c h thng, m bo sai lch gia v trớ t v giỏ tr thc gim v 0 nhanh
chúng, n nh h thng ngay c khi cú nhiu tỏc ng.
- Nhc im: Vic xỏc nh vựng bao, vựng gii m trong ú h thng n
nh l rt khú khn khụng phi lỳc no cng xỏc nh c, thm chớ l khụng xỏc
nh c. hn ch c nhc im ny thỡ cỏc tt nht l kt hp vi b iu

GVHD: TS. Voừ Thu Haứ

SVTH: Lửu Tuaỏn
Khanh
Thiết kế chế tạo vaø điều khiển tay maùy Trang 20
khiển thích nghi vì bộ điều khiển thích nghi có khả năng xác định được sự thay đổi
của vùng bao do đó mà hệ thống có thể ổn định được.
1.4.3.4 Phương pháp điều khiển thích nghi.
1.4.3.4.1 Nội dung phương pháp.
Khi mô hình robot là không biết rõ hoặc không xác định được chính xác các
tham số mô hình thì luật điều khiển thích nghi theo Li-Slotine đề xuất bằng việc ước
lượng các thông số động lực học không biết chính xác như ma trận quán tính M,
vectơ tương hỗ và ly tâm H, vectơ gia tốc trọng trường G.
Trong luật điều khiển thích nghi theo Li-Slotine luôn có hệ thống nhận dạng
tham số động của hệ thống, luôn cập nhật các tham số đó so sánh giá trị ước lượng
được với giá trị thực rồi đưa vào bộ điều khiển để hiệu chỉnh đảm bảo hệ thống ổn
định và sai lệch các khớp quay sẽ hội tụ về không, luật điều khiển cũng được xây

dựng dựa trên tiêu chuẩn ổn định Lyapunov.
Hình 1.11 Mô hình luật điều khiển Li-Slotine.
- Ưu điểm : Ưu điểm lớn nhất của phương pháp này là không cần biết chính
xác các tham số của mô hình thì bộ điều khiển vẫn đảm bảo hệ thống ổn định vì bộ
điều khiển luôn luôn cập nhật sự thay đổi tham số của mô hình với việc chọn các
tham số của bộ điều khiển tối ưu nhất thì hệ thống sẽ vừa bám chính xác quỹ đạo
đặt tốt, vừa tác động nhanh.
- Nhược điểm : Nhược điểm lớn nhất của phương pháp điều khiển này là
không bền với nhiễu. Bộ điều khiển thích nghi Li-Slotine không có khả năng loại
được nhiễu cho dù là nhiễu nhỏ. Mà trong quá trình robot hoạt động luôn có nhiễu
tác động chính vì thế mà để có bộ điều khiển thích nghi thôi thì rất khó áp dụng vào
thực tế.

GVHD: TS. Voõ Thu Haø

SVTH: Löu Tuaán
Khanh
D
K r
Robot
ˆ
T
p y r= −Γ
&
dk
τ
Thiết kế chế tạo vaø điều khiển tay maùy Trang 21
CHƯƠNG 2
THIẾT KẾ ROBOT
2.1 ĐẶT VẤN ĐỀ.

Xây dựng mô hình robot 5 khớp quay để gắp và nhả vật như sau:
Hình 2.1 Mô hình robot công nghiệp 5 khớp.
2.1.1 Giới thiệu.
Mô hình robot công nghiệp 5 bậc tự do sử dụng 5 động cơ Radio controlled
servo viết tắt là RC servo. Cánh tay robot được điều khiển bằng bộ điều khiển
dsPic30f4013 sử dụng vi điều khiển dsPic30f4013. Sử dụng trình biên dịch Mplab x
ide và ngôn ngữ lập trình C30 để viết chương trình hoạt động cho Robot. Sử dụng

GVHD: TS. Voõ Thu Haø

SVTH: Löu Tuaán
Khanh
Thiết kế chế tạo vaø điều khiển tay maùy Trang 22
mạch nạp Pickit v2.61 để nạp chương trình cho chip. Sử dụng cảm biến quang
OMRON D3F-E30C4 để phát hiện vật.
2.2 PHƯƠNG TRÌNH ĐỘNG HỌC THUẬN.
2.2.1 Lập bảng Denavit-Hartenberg (D-H).

Hình 2.2 Gắn hệ
tọa độ vào robot.
Bảng
Denavit-
Hartenberg
(DH).
Khâu
i
θ
1
1
θ

2
2
θ
3
3
θ
2.2.2 Tìm các ma trận A
i
.

Ma trận A
i

GVHD: TS. Voõ Thu Haø

SVTH: Löu Tuaán
Khanh
Thiết kế chế tạo vaø điều khiển tay maùy Trang 23

1 1
1 1
0
1
1
0 0
0 0
0 1 0
0 0 0 1
c s
s c

A
d
 
 
−
 
=
 
 
 

2 2 2 2
2 2 2 2
1
2
0
0
0 0 1 0
0 0 0 1
c s c l
s c s l
A
−
 
 
 
=
 
 
 

3 3 3 3
3 3 3 3
2
3
0
0
0 0 1 0
0 0 0 1
c s c l
s c s l
A
−
 
 
 
=
 
 
 
2.2.3 Xác định phương trình động lực học thuận.

1 1 2 2 2 2
1 1 2 2 2 2
0 0 1
2 1 2
1
0 0 0
0 0 0
0 1 0 0 0 1 0

0 0 0 1 0 0 0 1
c s c s c l
s c s c s l
A A A
d
−
   
   
−
   
= × = ×
   
   
   
1 2 1 2 1 1 2 2
1 2 1 2 2 1 2 2
0 0 1
2 1 2
2 2 1 2 2
0
0 0 0 1
c c c s s c c l
s s s s c s c l
A A A
s c d l s
−
 
 
− −
 

= × =
 
+
 
 
1 2 1 2 1 1 2 2 3 3 3 3
1 2 1 2 1 1 2 2 3 3 3 3
0 0 1 2 0 2
2 1 2 3 2 3
2 2 1 2 2
0
0
0 0 0 1 0
0 0 0 1 0 0 0 1
c c c s s c c l c s c l
s c s s c s c l s c s l
A A A A A A
s c d l s
− −
   
   
− −
   
= × × = × = ×
   
+
   
   
1 23 1 23 1 1 23 3 1 2 2
1 23 1 23 1 1 2 2 1 23 3

0 0 1 2 0 2
2 1 2 3 2 3
23 23 1 2 2 23 3
0
0 0 0 1
c c c s s c c l c c l
s c s s c s c l s c l
A A A A A A
s c d l s s l
− +
 
 
− − +
 
= × × = × =
 
+ +
 
 
Với
23 2 3
23 2 3
cos( )
sin( )
c
s
θ θ
θ θ
= +
= +

[ ]
1 23 3 1 2 2 1 23 3 1 2 2 23 3 2 2 1
, , ( ),( ),( )
T
x y z
P P P P c c l c c l s c l s c l s l s l d
 
= = + + + +
 
2.3.PHƯƠNG TRÌNH ĐỘNG LỰC HỌC.

GVHD: TS. Voõ Thu Haø

SVTH: Löu Tuaán
Khanh
Thiết kế chế tạo vaø điều khiển tay maùy Trang 24
2.3.1.Động năng, thế năng.
1 1
0,0,w
θ
 
=
 
&

2 2
0,0,w
θ
 
=

 
&
2.3.1.1Khớp 1:

0
1 1
A A=
Vị trí các trục x,y,z:
1 1 1 1
0, 0,x y z d= = =
- Động năng:
2 0 0
1 1
1 1
. w . . w
2 2
T
i ci i i
K v m I= +
Thay số vào ta có:
.
2 2 2
1 2 2
0, 0, 0
1
x y z
d
= = = =
& &
&

. . .
2
2 2 2
0
1 1 1
ci
v
x y z
= + + =
→

2 0 0
1 1 1 1 1
1 1
. w . . w
2 2
T
c i
K v m I= +
2
1 1 1
1
2
K I
θ
=
&
- Thế năng.
0 0
. . .

T
i i ci
P m G g P= −
,
[ ]
0
0 0 1 1
T
G = −
→

[ ]
1 1 1
. 0 0 1 1 . .P m g d= − −
1 1 1
. .P m g d=
2.3.1.2.Khớp 2:
0 0 1
2 1 2
.A A A=
2 1 2 2
cos .cosX l
θ θ
=

2 1 2 1 2 1 2 2
sin cos . cos sin .
c c
l l
θ θ θ θ θ θ

= − −
& &
2 1 2 2
sin cosY l
θ θ
=


2 1 2 2 2 1 2 2
.sin .cos . .cos .sin .
c c
Y l l
θ θ θ θ θ θ
⇒ = −
& &
&
2 1 2 2
sinZ d l
θ
= +

2 2 2 2
. os .
c
Z l c
θ θ
⇒ =
&
&
2 2 2 2

2 2 2 2
2 2
2 1 2 1 1 2 2
2
1 2 1 1 2 2 2 2
2 2 2 2 2
2 2 2 1 2
( ) .[ (sin cos . + cos sin . ) +
+(cos cos . +sin sin . ) + (cos . ) ]
( ) . cos .
c
c
c c
v X Y Z
l
v l
θ θ θ θ θ θ
θ θ θ θ θ θ θ θ
θ θ θ
= + +
= −
 
= +
 
& & &
& &
& & &
& &

GVHD: TS. Voõ Thu Haø

SVTH: Löu Tuaán
Khanh
Thiết kế chế tạo vaø điều khiển tay maùy Trang 25
 Động năng tâm khối khớp 2 là:
2 0
2 2 2 2. 2
1 1
.
2 2
T
c
K v m I
ω ω
= +
2 2 2 2 2
2 2 2 2 1 2 2 2
1 1
( ) (cos . )
2 2
c
K m l I
θ θ θ θ
= + +
& & &
 Thế năng của thanh nối 2 là:
0 0
2 2 2
. . .
T

c
P m G g P= −
với
[ ]
0
0 0 1 1
T
G = −
•
[ ]
0
2 2 2
2 2 1 2 2 2
. 0 0 1 1 . .
. . . . .sin
c
c
P m g P
P m g d m g l
θ
= − −
= +
2.3.1.3.Khớp 3:

0 0 0 2
3 1 2 3
. .A A A A=
.
3 1 23 3 1 2 2
3 1 23 1 3 1 23 2 3 1 2 2 1 1 2 2 2

. .
. ( )
c c
X c c l c c l
l s c l c s s c l c s l
θ θ θ θ θ
= +
= − − + − −
& & & &
3 1 23 3 1 2 2
Y s c l s c l= +
3 3 1 23 1 3 1 23 2 3 1 2 3 1 1 2 2 2
( )
c c
Y l c c l s c c c l s s l
θ θ θ θ θ
⇒ = − + + −
& & & & &
&
3 23 3 2 2 1
dZ s l s l
= + +

( )
3 23 2 3 3 2 2
os
c
Z c l c l
θ θ
= + +

& &
&

( )
3 23 2 3 3 2 2 2c
Z c l c l
θ θ θ
⇒ = + +
& & &
&
.
( ) ( )
2 2
2 2
3 3 1 23 1 23 1 2 3
( )
c
X l s c s c
θ θ θ
 
⇒ = − − +
 
 
& & &
&
( ) ( )
2 2
2 2
2 1 2 1 1 2 2
l s c c s

θ θ
 
+ − +
 
 
& &

( ) ( ) ( )
( )
2 3 1 23 1 1 23 2 3 1 1 1 1 2 2
2
c
l l s c c s s c c s
θ θ θ θ θ
   
+ + +
   
& & & & &
.

( ) ( )
( )
2
2
2 2 2 2
3 3 1 23 1 1 23 2 3c
Y l c c s s
θ θ θ
 
⇒ = + − +

 
 
& & &
&
( ) ( )
2 2
2
2 1 2 1 1 2 2
l c c s s
θ θ
 
+ + − −
 
 
& &

( )
( )
2 2
2 3 1 23 1 1 23 2 3 1 2 1 1 2 2
2
c
l l c c s s c c s s
θ θ θ θ θ
 
 
− − + × −
 
 
& & & & &

( )
( )
( )
2
2
2 2 2
3 3 23 2 3 2 2 2c
Z l c l c
θ θ θ
⇒ = + +
& & &
&
( ) ( )
( )
2 3 23 2 3 2 2
2
c
l l c c
θ θ θ
+ +
& & &
.

( )
2
2 2 2 2 2 2 2 2 2 2
3 3 3 3 23 1 2 3 2 2 1 2c
X Y Z l c l c
θ θ θ θ θ

 
 
⇒ + + = + + + + +
 
 
 
& & & & &
& & &

GVHD: TS. Voõ Thu Haø

SVTH: Löu Tuaán
Khanh