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
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 4Hì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.
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.
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ớ.
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.
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.
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à
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:
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
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
(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
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
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
θ
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
−
=
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
−
− −
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
θ θ
θ θ
= +
= +
&
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
= = = =
& &
&
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 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ø
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
& &
&
( )
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 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
θ θ θ θ θ
− − + × −
& & & & &
⇒ + + = + + + + +
& & & & &
& & &
GVHD: TS. Voõ Thu Haø
SVTH: Löu Tuaán
Khanh
Copyright: Tài liệu đại học ©