Đồ án tốt nghiệp xây dựng chương trình tính toán động học, động lực học hệ tay máy hở tổng quát

M C L CỤ Ụ
2
DANH M C C C T VI T T TỤ Á Ừ Ế Ắ
DH
Denavit Hartenberg
GUI
Graphical User Interface
GUIDE
Graphical User Interface Development Environment
CS
Coordinate system
CG
Center of gravity
3
DANH M C HÌNH NHỤ Ả
4
M UỞĐẦ
Trong s nghi p công nghi p hóa v hi n i hóa t n c, khoa h cự ệ ệ à ệ đạ đấ ướ ọ
v công ngh óng vai trò h t s c quan tr ng, c bi t l l nh v c C i nà ệ đ ế ứ ọ đặ ệ à ĩ ự ơ đ ệ
t v t ng hóa các h th ng s n xu t. Cùng v i s h i nh p, phát tri nử à ự độ ệ ố ả ấ ớ ự ộ ậ ể
c a khoa h c v công ngh n c nh , vi c ng d ng các lo i robot nh mủ ọ à ệ ướ à ệ ứ ụ ạ ằ
m c ích nâng cao n ng su t, gi i phóng s c lao ng c a con ng i ng yụ đ ă ấ ả ứ độ ủ ườ à
c ng c quan tâm v ng d ng r ng rãi. i ôi v i nó l nhu c u o t oà đượ à ứ ụ ộ Đ đ ớ à ầ đà ạ
ngu n nhân l c ch t l ng cao, ph c v cho công cu c công nghi p hóa vồ ự ấ ượ ụ ụ ộ ệ à
hi n i hóa t n c. Chính vì th , ph n l n các tr ng i h c hi n nayệ đạ đấ ướ ế ầ ớ ườ đạ ọ ệ
ã v ang nghiên c u, m r ng vi c o t o nhân l c ng nh C i n t ,đ à đ ứ ở ộ ệ đà ạ ự à ơ đ ệ ử
có th khai thác, s d ng, v n h nh các h th ng robot v cao h n n ađể ể ử ụ ậ à ệ ố à ơ ữ
ó l nghiên c u thi t k các h th ng robot made in Vi t Nam thay th chođ à ứ ế ế ệ ố ệ ế
các h th ng t ng nh p kh u t ti n. ệ ố ự độ ậ ẩ đắ ề
T th p niên 90 tr l i ây, các nghiên c u ng d ng robot công nghi pừ ậ ở ạ đ ứ ứ ụ ệ
ng y c ng nhi u, trong ó ph i k n robot có c u trúc n i ti p. có thà à ề đ ả ể đế ấ ố ế Để ể

xây d ng b i toán i u khi n robot tr c h t chúng ta c n ph i gi i b i toánự à đ ề ể ướ ế ầ ả ả à
ng h c v ng l c h c c a robot. K t qu c a các b i toán n y l nh ngđộ ọ à độ ự ọ ủ ế ả ủ à à à ữ
tham s u v o quan tr ng cho t t c các nghiên c u ti p theo v robot.ố đầ à ọ ấ ả ứ ế ề
Hi n t i, vi c gi i l p b i toán n y ph n l n c th c hi n th công b ngệ ạ ệ ả ớ à à ầ ớ đượ ự ệ ủ ằ
tay òi h i r t nhi u k n ng tính toán, bên c nh ó hi u qu c a vi c tínhđ ỏ ấ ề ỹ ă ạ đ ệ ả ủ ệ
toán theo ph ng pháp n y th p, kh n ng x y ra sai sót r t l n do kh i l ngươ à ấ ả ă ả ấ ớ ố ượ
tính toán r t l n, các phép toán ma tr n khá ph c t p, th ng ch gi i quy tấ ớ ậ ứ ạ ườ ỉ ả ế
c trong nh ng tr ng h p n l , c th . b môn C h c ng d ng,đượ ữ ườ ợ đơ ẻ ụ ể Ở ộ ơ ọ ứ ụ
Vi n C khí, i h c Bách khoa H N i c ng ã xây d ng ch ng trìnhệ ơ Đạ ọ à ộ ũ đ ự ươ
ROBOTDYN ph c v cho vi c t ng hóa tính toán các b i toán trên. Tuyụ ụ ệ ự độ à
nhiên, ây l s n ph m c thù c a h , v vi c ti p c n khai thác ph nđ à ả ẩ đặ ủ ọ à ệ ế ậ ầ
m m n y l r t khó kh n, n u không nói l không th i v i sinh viên.ề à à ấ ă ế à ể đố ớ
V i m c ích t o ra công c t ng hóa tính toán ng h c, ng l cớ ụ đ ạ ụ ự độ độ ọ độ ự
h c, v mô ph ng ng h c robot thu n ti n cho quá trình h c t p, nghiênọ à ỏ độ ọ ậ ệ ọ ậ
c u c a sinh viên, c ng nh quá trình gi ng d y c a giáo viên, em ch n ứ ủ ũ ư ả ạ ủ ọ đề
5
t i mang tên: “à Xây d ng ch ng trình tính toán ng h c, ng l c h c hự ươ độ ọ độ ự ọ ệ
tay máy h t ng quátở ổ ”.
Do th i gian không cho phép, nên ph m vi c a án l các h tay máyờ ạ ủ đồ à ệ
h d i 5 b c t do ã xác nh tr c c u hình theo ph ng pháp Denavit-ở ướ ậ ự đ đị ướ ấ ươ
Hartenberg, v án s t p trung gi i quy t các n i dung ch y u sau:à đồ ẽ ậ ả ế ộ ủ ế
- Tìm hi u ph ng pháp tính toán ng h c, ng l c h c tay máyể ươ độ ọ độ ự ọ
ph c v cho vi c xây d ng ch ng trình.ụ ụ ệ ự ươ
- Xây d ng ch ng trình tính toán ng h c, ng l c h c, mô ph ngự ươ độ ọ độ ự ọ ỏ
ng h c tay máy.độ ọ
- Tìm hi u ph ng pháp, cách th c mô hình hóa tay máy d a v o góiể ươ ứ ự à
công c SimMechanics c a Matlab v ph n m m h a Solidworks.ụ ủ à ầ ề đồ ọ
Trong quá trình th c hi n án n y, do ki n th c v robot có h n, th iự ệ đồ à ế ứ ề ạ ờ
gian v t i li u nghiên c u còn ít nên án c a em v n còn nhi u thi u sót,à à ệ ứ đồ ủ ẫ ề ế
vì v y r t mong c s góp ý c a th y cô v các b n án c ho nậ ấ đượ ự ủ ầ à ạ để đồ đượ à

thi n h n.ệ ơ
Em xin chân th nh c m n th y Nguy n Ho ng Long, th y Nguy nà ả ơ ầ ễ à ầ ễ
H u Phúc v to n th các th y cô trong b môn Robot c bi t & C i nữ à à ể ầ ộ đặ ệ ơ đ ệ
t ã giúp em ho n th nh án n y.ử đ à à đồ à
H N i, ng y tháng n m… …à ộ à ă
2012
Sinh viên th c hi nự ệ
6
CH NG 1: T NG QUAN V TAY M YƯƠ Ổ Ề Á
1. Tay máy
Tay máy l ph n c s , quy t nh kh n ng l m vi c c a Robot côngà ầ ơ ở ế đị ả ă à ệ ủ
nghi p. ó l c c u c khí g m các khâu, kh p, chúng hình th nh cánh tayệ Đ à ơ ấ ơ ồ ớ à
t o ra các chuy n ng c b n, c tay t o nên s khéo léo, linh ho t vđể ạ ể độ ơ ả ổ ạ ự ạ à
b n tay tr c ti p ho n th nh các thao tác trên i t ng.à để ự ế à à đố ượ
2. K t c u c a tay máy ế ấ ủ
Ý t ng ban u c a vi c thi t k v ch t o tay máy l ph ng theo cácưở đầ ủ ệ ế ế à ế ạ à ỏ
c u t o v ch c n ng c a tay ng i.V sau, ây không còn l i u ki n b tấ ạ à ứ ă ủ ườ ề đ à đ ề ệ ắ
bu c n a. Tay máy hi n nay r t a d ng v nhi u lo i v i dáng v khác r tộ ữ ệ ấ đ ạ à ề ạ ớ ẻ ấ
xa do v i tay ng i. Tuy nhiên trong k thu t robot ng i ta v n dùng cácớ ườ ỹ ậ ườ ẫ
thu t ng quen thu c, nh vai (Shoulder), cánh tay (Arm), c tay (Wrist), b nậ ữ ộ ư ổ à
tay (Hand) v các kh p (Ariculation), ch tay máy v các b ph n c a…à ớ để ỉ à ộ ậ ủ
nó.
Trong thi t k v s d ng tay, máy ng i ta th ng quan tâm n cácế ế à ử ụ ườ ườ đế
thông s có nh h ng l n n kh n ng l m vi c c a chúng, nh :ố ả ưở ớ đế ả ă à ệ ủ ư
- S c nâng, c ng v ng, l c k p c a tay máyứ độ ứ ữ ự ẹ ủ
- T m v i hay vùng l m vi c: kích th c v hình dáng vùng m ph n công tácầ ớ à ệ ướ à à ầ
có th v n t i.ể ươ ớ
- S khéo léo, ngh a l kh n ng nh v v nh h ng ph n công tác trongự ĩ à ả ă đị ị à đị ướ ầ
vùng l m vi c. Thông s n y liên quan n s b c t do c a ph n công tác.à ệ ố à đế ố ậ ự ủ ầ
Các tay máy có c i m chung v k t c u l c c u t o b i m t sđặ đ ể ề ế ấ à đượ ấ ạ ở ộ ố

khâu (Links), c n i v i nhau nh các kh p (Joints), t o th nh chu i ngđượ ố ớ ờ ớ ạ à ỗ độ
h c. M t u c a chu i ng h c c n i v i giá (base), còn u kia n iọ ộ đầ ủ ỗ độ ọ đượ ố ớ đầ ố
v i ph n công tác. M i khâu hình th nh cùng v i kh p phía tr c nó m tớ ầ ỗ à ớ ớ ướ ộ
c p khâu–kh p. Tùy theo k t c u m m i lo i kh p m b o cho khâu n iặ ớ ế ấ à ỗ ạ ớ đả ả ố
sau nó các kh n ng chuy n ng nh t nh.ả ă ể độ ấ đị
M i kh p c c tr ng b i 2 tham s : ỗ ớ đượ đặ ư ở ố
- Các tham s không thay i trong quá trình l m vi c c a tay máyố đổ à ệ ủ
c g i l các tham s .đượ ọ à ố
- Các tham s thay i khi tay máy l m vi c, g i l các bi n kh p.ố đổ à ệ ọ à ế ớ
7
Hai lo i kh p thông d ng trong k thu t tay máy l kh p tr t v kh pạ ớ ụ ỹ ậ à ớ ượ à ớ
quay. Chúng u l kh p có m t b c t do.đề à ớ ộ ậ ự
3. Phân lo i tay máyạ
Tuy theo s l ng v cách b trí các kh p có th t o ra tay máy ki u t aố ượ à ố ớ ể ạ ể ọ
các, t a tr , t a c u, SCARA v ki u tay ng i.độ đề ọ độ ụ ọ độ ầ à ể ườ
3.1. Tay máy ki u t a cácể ọ độ đề :
Tay máy n y còn c g i l ki u ch nh t,à đượ ọ à ể ữ ậ
dùng 3 kh p tr t, cho phép ph n công tácớ ượ ầ th c hi nự ệ
m t cách c l p các chuy n ng th ng,ộ độ ậ ể độ ẳ
song song v i 3 tr c t a . Vùng l m vi cớ ụ ọ độ à ệ c aủ
tay máy có d ng h p ch nh t. Do sạ ộ ữ ậ ự
n gi n v k t c u, tay máy ki u n y cóđơ ả ề ế ấ ể à độ
c ngứ
v ng cao, chính xác c m b o ng u trong to n b vùng l mữ độ đượ đả ả đồ đề à ộ à
vi c, nh ng ít khéo léo.Vì v y, tay máy ki u các c dùng ch y uệ ư ậ ể đề đượ ủ ế
trong v n chuy n v l p ráp.ậ ể à ắ
3.2. Tay máy ki u t a trể ọ độ ụ
Khác v i tay máy ki u các kh p u tiên : Dùng kh p quay thay thớ ể đề ở ớ đầ ớ ế
cho kh p t nh ti n. Vùng l m vi c c a nó cóớ ị ế à ệ ủ
d ng tr r ng. Kh p tr t n m ngang cho phépạ ụ ỗ ớ ượ ằ tay

máy “thò” c v o kho ng r ng n m ngang.đượ à ả ỗ ằ Độ
c ng v ng c h c c a tay máy tr t t, thích h pứ ữ ơ ọ ủ ụ ố ợ v iớ
t i n ng, nh ng chính xác nh v v gócả ặ ư độ đị ị à trong
m t ph ng n m ngang gi m khi t m v iặ ẳ ằ ả ầ ớ
t ng.ă
3.3. Tay máy ki u t a c uể ọ độ ầ
Khác ki u tr do kh p th 2 c thay th b ng kh p quay. N u qu oể ụ ớ ứ đượ ế ằ ớ ế ỹ đạ
chuy n ng c a các ph n công tác c mô t trong t a c u thì m iể độ ủ ầ đượ ả ọ độ ầ ỗ
b c t do t ng ng v i kh n ng chuy n ng v vùng l m vi c c a nó lậ ự ươ ứ ớ ả ă ể độ à à ệ ủ à
kh i c u r ng. c ng v ng c a lo i tay máy n y th p h n 2 lo i tay máyố ầ ỗ Độ ứ ữ ủ ạ à ấ ơ ạ
Hình 1.: Tay máy ki u t a cácể ọ độ đề
Hình 1.:Tay máy ki u t a trể ọ độ ụ
8
trên v chính xác nh v ph thu c v o t m v i. Tuy nhiên, lo i n y cóà độ đị ị ụ ộ à ầ ớ ạ à
th “ nh t ” c c v t d i n n.ể ặ đượ ả ậ ướ ề
3.4. SCARA
ây l ki u tay máy có c u t o cĐ à ể ấ ạ đặ
bi t, g m 2 kh p quay v 1 kh p t nh ti n,ệ ồ ớ à ớ ị ế
nh ng c 3 u có tr c song song v iư ả đề ụ ơ
nhau. K t c u n y l m tay máy c ng v ngế ấ à à ứ ữ
h n theo ph ng ngang. Lo i n y chuyên dùngơ ươ ạ à cho
công ngh l p ráp v i t i tr ng nh , theoệ ắ ớ ả ọ ỏ ph ng th ng ng.ươ ẳ đứ
Vùng l m vi c c a SCARA l m t ph n c aà ệ ủ à ộ ầ ủ hình tr r ng.ụ ỗ
3.5. Tay máy ki u tay ng iể ườ
Có c 3 kh p l kh p quay, trong óả ớ à ớ đ
tr c th nh t vuông góc v i 2 tr c kia. Doụ ứ ấ ớ ụ
s t ng tác v i tay ng i, kh p th 2ự ươ ớ ườ ớ ứ
c g i l kh p vai, kh p th 3 g i lđượ ọ à ớ ớ ứ ọ à
kh p kh y tay. V i k t c u n y, không cóớ ủ ớ ế ấ à
s t ng ng gi a kh n ng chuy nự ươ ứ ữ ả ă ể

ng c a các khâu v s b c t do. Tayđộ ủ à ố ậ ự
máy l m vi c r t khéo léo, nh ng chính xác nh v ph thu c v trí c aà ệ ấ ư độ đị ị ụ ộ ị ủ
ph n công tác trong vùng l m vi c. Vùng l m vi c c a tay máy ki u n y g nầ à ệ à ệ ủ ể à ầ
gi ng m t kh i c u.ố ộ ố ầ
Hình 1.: Tay máy ki u t a c uể ọ độ ầ
Hình 1.: Tay máy ki u SCARAể
Hình 1.: Tay máy ki u tay ng iể ườ
9
CH NG 2: NG H C V NG L C H C TAY M YƯƠ ĐỘ Ọ À ĐỘ Ự Ọ Á
I. NG H C TAY M YĐỘ Ọ Á
ng h c tay máy nghiên c u chuy n ng các khâu c a robot vĐộ ọ ứ ể độ ủ ề
ph ng di n hình h c, không quan tâm t i các l c v moment gây ra chuy nươ ệ ọ ớ ự à ể
ng . ng h c tay máy gi i quy t 2 v n :độ Độ ọ ả ế ấ đề
- B i toán ng h c thu n:à độ ọ ậ C n c v o các bi n kh p xác nh vùng l mă ứ à ế ớ để đị à
vi c c a ph n công tác v mô t chuy n ng c a ph n công tác trong vùngệ ủ ầ à ả ể độ ủ ầ
l m vi c c a nó.à ệ ủ
- B i toán ng h c ng cà độ ọ ượ : Xác nh các bi n kh p m b o chuy nđị ế ớ để đả ả ể
ng cho tr c c a ph n công tác.độ ướ ủ ầ
Hình 2. B i toán ng h c thu n, ng h c ng cà độ ọ ậ độ ọ ượ
1. B i toán ng h c thu nà độ ọ ậ
B i toán ng h c thu n nh m mô t th (v trí v h ng) c a ph nà độ ọ ậ ằ ả ế ị à ướ ủ ầ
công tác d i d ng h m s c a bi n kh p. ướ ạ à ố ủ ế ớ
Gi i quy t b i toán ng h c thu n c a robot th c ch t l chúng ta choả ế à độ ọ ậ ủ ự ấ à
tr c c u hình c a robot v quy lu t chuy n ng c a các khâu, t óướ ấ ủ à ậ ể độ ủ ừ đ
chúng ta xác nh quy lu t chuy n ng c a b n k p ( i m tác ng cu i):đị ậ ể độ ủ à ẹ đ ể độ ố
v trí b n k p, v n t c b n k p, gia t c chuy n ng b n k p, v n t c góc,ị à ẹ ậ ố à ẹ ố ể độ à ẹ ậ ố
gia t c góc v.v…ố
gi i b i toán ng h c thu n ta có th dùng ph ng pháp hình h cĐể ả à độ ọ ậ ể ươ ọ
gi i tích. Ph ng pháp n y th ng ch áp d ng cho các c c u n gi n. ả ươ à ườ ỉ ụ ơ ấ đơ ả Để
gi i các b i toán t ng quát ta c n m t ph ng pháp gi i chung. Có 2 ph ngả à ổ ầ ộ ươ ả ươ

pháp chính c s d ng ph bi n: Ph ng pháp Denavit-Hartenberg vđượ ử ụ ổ ế ươ à
10
ph ng pháp Craig. Trong n i dung án xin trình b y ph ng phápươ ộ đồ à ươ
Denavit-Hartenberg.
1.1. Phép bi n i thu n nh t v ma tr n bi n i thu n nh tế đổ ầ ấ à ậ ế đổ ầ ấ
Cho 2 h quy chi u ệ ế
0 0 0 0
{ }R Ox y z
=
v à
1 1 1 1
{ }R Ox y z=

nh hình v . T a ư ẽ ọ độ
i m P trong h quy chi u Rđ ể ệ ế
0
l à
p
r
r
, trong h quyệ
chi u Rế
1
l à
P
u
r
.
Khi ó ta có:đ

P A P
r r u= +
r r r
(1.1)
Trong h quy chi u Rệ ế
0
bi u th c (2.1) cóể ứ d ng:ạ

(0) (0) (0)
P A P
r r u= +
r r r
(1.2)
G i A l ma tr n cosin ch h ng c a h quy chi u Rọ à ậ ỉ ướ ủ ệ ế
1
i v i h quy chi u đố ớ ệ ế
R
0
.
11 12 13
21 22 23
31 32 33
a a a
a a a
a a a
 
 
=
 
 

 
A
Khi ó ta có h th c:đ ệ ứ

(0) (1)
P P
=u Au
(1.3)
Th (2.3) v o (2.2) ta c:ế à đượ
(0) (0) (1)
P A P
= +
r r Au
(1.4)
(1)
(0) (0)
11 12 13
(0) (0) (1)
21 22 23
(0) (0)
(1)
31 32 33
Px
P A
P A Py
P A
Pz
u
x x
a a a

y y a a a u
a a a
z z
u
 
   
 
 
   
 
= +
 
   
 
 
   
 
 
   
 
(1.5)
N u s d ng khái ni m t a thu n nh t ta có:ế ử ụ ệ ọ độ ầ ấ
(1)
(0)
(0)
11 12 13
(1)
(0)
(0)
21 22 23

(0)
(0)
(1)
31 32 33
0 0 0 1
1
1
Px
P
A
Py
P
A
A
P
Pz
u
x
a a a x
u
y
a a a y
a a a z
z
u
 
 
 
 
 

 
 
 
 
=
 
 
 
 
 
 
 
 
 
 
 
(1.6)
Hay:
Hình 2.
11
(0) (1)
(0)
0 1
1 1
P P
A
T
   
 
=

   
 
 
   
r u
A r
(1.7)
N u ta a v o ma tr n:ế đư à ậ
(0)
11 12 13
(0) (0)
21 22 23
(0)
31 32 33
0 1
0 0 0 1
A
A P
T
A
a a a x
a a a y
a a a z
 
 
 
 
= =
 
 

 
 
 
A r
T
(1.8)
Thì ph ng trình (2.7) có th vi t d i d ng:ươ ể ế ướ ạ
(0) (1)
P P
=r Tu
(1.9)
1.2. Các ma tr n quay c b n thu n nh t v ma tr n tinh ti n thu n nh tậ ơ ả ầ ấ à ậ ế ầ ấ
Các ma tr n quay c b n trong không gian thu n nh t:ậ ơ ả ầ ấ

1 0 0 0
0 os( ) sin( ) 0
( ) ( , )
0 sin( ) os( ) 0
0 0 0 1
Rx
c
Rot x
c
ϕ ϕ
ϕ ϕ
ϕ ϕ
 
 
−
 

= =
 
 
 
T
(1.10)

cos( ) 0 sin( ) 0
0 1 0 0
( ) ( , )
sin( ) 0 os( ) 0
0 0 0 1
Ry
Rot y
c
ψ ψ
ψ ψ
ψ ψ
 
 
 
= =
 
−
 
 
T
(1.11)

cos( ) sin( ) 0 0

sin( ) os( ) 0 0
( ) ( , )
0 0 1 0
0 0 0 1
Rz
c
Rot z
θ θ
θ θ
θ θ
−
 
 
 
= =
 
 
 
T
(1.12)
Các ma tr n t nh ti nậ ị ế
1 0 0
0 1 0 0
( ) ( , )
0 0 1 0
0 0 0 1
Tx
a
a Trans x a
 

 
 
= =
 
 
 
T
(1.13)
1 0 0 0
0 1 0
( ) ( , )
0 0 1 0
0 0 0 1
Ty
b
b Trans y b
 
 
 
= =
 
 
 
T
(1.14)
a)
b)
c)
Hình 2.: a,b,c
a)

b)
12
1 0 0 0
0 1 0 0
( ) ( , )
0 0 1
0 0 0 1
Tz
c Trans x c
c
 
 
 
= =
 
 
 
T
(1.15)
N u t nh ti n ng th i trên các tr c x, y ,z ta có ma tr n thu n nh t:ế ị ế đồ ờ ụ ậ ầ ấ
1 0 0
0 1 0
( , , ) ( , , )
0 0 1
0 0 0 1
T
a
b
a b c Trans a b c
c

 
 
 
= =
 
 
 
T
(1.16)
1.3. Ph ng pháp Denavit-Hartenbergươ
1.3.1. Cách xác nh h tr c t a đị ệ ụ ọ độ
Xét các v t r n n i ti p nhau b ng các kh p quay v kh p t nh ti n. Khiậ ắ ố ế ằ ớ à ớ ị ế
ó quan h v trí gi a các khâu k ti p có th c xác nh b i 2 tham sđ ệ ị ữ ế ế ể đượ đị ở ố
kh pớ
Hình 2.: Hình bi u di n kh p v khâuể ễ ớ à
Trong hình trên, khâu i-1 v khâu i c n i v i nhau b ng kh p i. Tr c à đượ ố ớ ằ ớ ụ z
i-1
c ch n l tr c c a kh p th i. Tham s th nh t đượ ọ à ụ ủ ớ ứ ố ứ ấ
i
θ
, l góc quay tr c à ụ x
i-1
quanh tr c ụ z
i-1

n tr c đế ụ
'
/ /
i i
x x

. Tham s th 2 l dố ứ à
i
, l kho n cách gi a tr cà ả ữ ụ
'
i
x
v tr c à ụ
i
x
. N u kh p i l kh p quay thì ế ớ à ớ
i
θ
l bi n, còn à ế
i
d
l h ng s . N uà ằ ố ế
kh p I l kh p t nh ti n thì kho ng cách ớ à ớ ị ế ả
i
d
l bi n, còn à ế
i
θ
l h ng s .à ằ ố
Cách ch n các h tr c t a có g c t i kh p th i:ọ ệ ụ ọ độ ố ạ ớ ứ
1) Tr c ụ
1i
z
−
c ch n d c theo h ng c a tr c kh p ng th iđượ ọ ọ ướ ủ ụ ớ độ ứ
2) Tr cụ

1i
x
−
c ch n d c theo ng vuông góc chung c a 2 tr c đượ ọ ọ đườ ủ ụ
1i
z
−

và
2i
z
−
, h ng t tr cướ ừ ụ
2i
z
−
n tr cđế ụ
1i
z
−
. N u tr cế ụ
1i
z
−
c t tr cắ ụ
2i
z
−
thì
c)

Hình 2. a,b,c
13
h ng c a tr c ướ ủ ụ
1i
x
−
c ch n tùy ý mi n l vuông góc v i tr cđượ ọ ễ à ớ ụ
1i
z
−
.
Khi ó 2 tr c đ ụ
2i
z
−
v à
1i
z
−
song song v i nhau, gi a 2 tr c n y cóớ ữ ụ à
nhi u ng pháp tuy n chung, ta có th ch n tr cề đườ ế ể ọ ụ
1i
x
−
h ng theoướ
pháp tuy n chung n o c ng cế à ũ đượ
3) G c t a Oố ọ độ
i-1
c ch n t i giao i m c a tr cđượ ọ ạ đ ể ủ ụ
1i

x
−
v tr cà ụ
1i
z
−
4) Tr cụ
1i
y
−
c ch n sao cho h (Oxyz)đượ ọ ệ
i-1
l h quy chi u thu nà ệ ế ậ
5) i v i h t a (Oxyz)Đố ớ ệ ọ độ
0
theo quy c trên ta ch ch n c tr cướ ỉ ọ đượ ụ
z
0
, còn tr c xụ
o
ch a có trong quy c trên. Ta có th ch n tr c xư ướ ể ọ ụ
0
tùy
ý, mi n l xế à
0
v zà
0
vuông góc v i nhau.ớ
6) i v i h tr c (Oxyz)Đố ớ ệ ụ
n

, do không có kh p ớ n+1, nên theo quy cướ
trên ta không th xác nh c tr c ể đị đượ ụ z
n
. Tr c ụ z
n
không c xác nhđượ đị
duy nh t, trong khi tr c ấ ụ x
n
l i không c ch n theo pháp tuy n c aạ đượ ọ ế ủ
tr c ụ z
n-1
. Tr ng h p n y, n u kh p n l kh p quay, ta có th ch nườ ợ à ế ớ à ớ ể ọ
tr c ụ z
n
song song v i tr c ớ ụ z
n-1
. Ngo i ra ta có th ch n tùy ý sao choà ể ọ
h p lý.ợ
1.3.2. Các tham s ng h c Denavit-Hartenberg.ố độ ọ
Hình 2.: S thi t l p h t a các khâuơ đồ ế ậ ệ ọ độ
V trí c a h t a d kh p (Oxyz)ị ủ ệ ọ ộ ớ
i
, i v i h t a kh p (Oxyz)đố ớ ệ ọ độ ớ
i-
1
c xác nh b i b n tham s Denavit-Hartenberg: đượ đị ở ố ố
, , ,
i i i i
d a
θ α

nh sau:ư
-
i
θ
: góc quay quanh tr c ụ
1i
z
−
tr c để ụ
1i
x
−
chuy n n tr c ể đế ụ
'
i
x
(
'
/ /
i i
x x
)
-
i
d
: d ch chuy n t nh ti n d c theo tr c ị ể ị ế ọ ụ
1i
z
−
g c t a Ođể ố ọ độ

i-1
chuy nể
n i m đế đ ể
'
i
O
, giao i m c a tr c đ ể ủ ụ
i
x
v à
1i
z
−
14
-
i
a
: d ch chuy n t nh ti n d c theo tr c ị ể ị ế ọ ụ
i
x
i m để đ ể
'
i
O
chuy n nể đế
i m đ ể
i
O
-
i

α
: góc quay quanh tr c ụ
i
x
sao cho tr c ụ
' '
1 1
( / / )
i i i
z z z
− −
chuy n n tr c ể đế ụ
i
z
Do h tr c t a (Oxyz)ệ ụ ọ độ
i-1
g n li n v o khâu th i-1, còn h t a ắ ề à ứ ệ ọ độ
(Oxyz)
i
g n v o khâu th i, cho nên v trí c a khâu th i i v i khâu th i-1ắ à ứ ị ủ ứ đố ớ ứ
c xác nh b i 4 tham s Denavit-Hartenberg.đượ đị ở ố
1.3.3. Ma tr n Denavit-Hartenberg.ậ
Ta có th chuy n h t a kh p (Oxyz)ể ể ệ ọ độ ớ
i-1
sang h t a kh p (Oxyz)ệ ọ độ ớ
i
b ng b n phép bi n i c b n nh sau:ằ ố ế đổ ơ ả ư
- Quay quanh tr c ụ z
i-1
m t gócộ

i
θ
.
- D ch chuy n t nh ti n d c tr c ị ể ị ế ọ ụ z
i-1

m t o n ộ đ ạ d
i
.
- D ch chuy n t nh ti n d c tr c ị ể ị ế ọ ụ x
i
m t o n ộ đ ạ a
i
.
- Quay quanh tr c ụ x
i
m t góc ộ
i
α
Ma tr n c a phép bi n i, ký hi u Hậ ủ ế đổ ệ
i
l tích c a b n ma tr n bi n ià ủ ố ậ ế đổ
c b n v có d ng nh sau:ơ ả à ạ ư
Quay h t a xung quanh tr cệ ọ độ ụ
1i
z
−
m t góc ộ
1
θ

ta c ma tr n bi n i:đượ ậ ế đổ

cos( ) sin( ) 0 0
sin( ) cos( ) 0 0
( , )
0 0 1 0
0 0 0 1
i i
i i
Rz i
Rot z
θ θ
θ θ
θ
 
 
 
= =
 
 
 
T

T nh ti n theo tr c ị ế ụ
i
z
m t o n ộ đ ạ
i
d
ta c ma tr n bi n i:đượ ậ ế đổ

1 0 0 0
0 1 0 0
ans( , )
0 0 1
0 0 0 1
Tz i
i
Tr z d
d
 
 
 
= =
 
 
 
T
T nh ti n theo tr c ị ế ụ
i
x
m t o n ộ đ ạ
i
a
ta c ma tr n bi n i:đượ ậ ế đổ
1 0 0
0 1 0 0
ans( , )
0 0 1 0
0 0 0 1
i

Tx i
a
Tr x d
 
 
 
= =
 
 
 
T
(1.17)
(1.18)
(1.19)
15
V quay quanh tr c à ụ
i
x
m t góc ộ
i
α
ta c ma tr n bi n iđượ ậ ế đổ
1 0 0 0
0 cos( ) sin( ) 0
( , )
0 sin( ) cos( ) 0
0 0 0 1
i i
Rx i
i i

Rot x
α α
α
α α
 
 
−
 
= =
 
 
 
T
Ma tr n bi n i h t a (Oxyz)ậ ế đổ ệ ọ độ
i-1
th nh h t a (Oxyz)à ệ ọ độ
i
c xácđượ
nh b ng tích c a 4 ma tr n bi n i trên:đị ằ ủ ậ ế đổ
( ) ( ) ( ) ( )
cos sin cos sin sin cos
sin cos cos cos sin sin
0 sin cos
0 0 0 1
i Rz i Tz i Tx i Rx i
i i i i i i
i i i i i i i
i i i
d a
a

a
d
θ α
θ θ α θ α θ
θ θ α θ α θ
θ α
=
−
 
 
−
 
=
 
 
 
H T T T T
1.3.4. Ph ng trình xác nh v trí v h ng c a khâu thao tácươ đị ị à ướ ủ
V i ma tr n trên ta có th xác nh c:ớ ậ ể đị đượ
- V trí c a g c t a c a h t a (Oxyz)ị ủ ố ọ độ ủ ệ ọ độ
i
trong h t a ệ ọ độ
1
(Ox )
i
yz
−

trong
- H ng c a v t r n g n v i h t a ướ ủ ậ ắ ắ ớ ệ ọ độ

(Ox )
i
yz
trong h t a t a ệ ọ ọ độ
1
(Ox )
i
yz
−

trong.
- B ng cách chuy n d n h quy chi u ằ ể ầ ệ ế
( )
n
Oxyz
t ừ
n
O
v ề
1 2
, ,
n n
O O
− −
v cu ià ố
cùng l h quy chi u c nh à ệ ế ố đị
0
(Ox )yz
ta s xác nh c v trí c a g c t aẽ đị đượ ị ủ ố ọ
độ

n
O
v h ng c a khâu th n trong h quy chi u c nhà ướ ủ ứ ệ ế ố đị
Hình 2.: Tay máy n khâu
Áp d ng liên ti p phép bi n i i v i robot n khâu ta cóụ ế ế đổ đố ớ :
(1.20)
(1.21)
16
1 2
(0)

0 1
n n
n E
n
=
 
=
 
 
D H H .H
A r
D
Ma tr n ậ
n
D
cho bi t v trí c a i m nh v v h ng c a khâu thao tác c aế ị ủ đ ể đị ị à ướ ủ ủ
robot i v i h quy chi u c nhđố ớ ệ ế ố đị .
2. B i toán ng h c ng cà độ ọ ượ
B i toán ng h c thu n cho phép xác nh th c a ph n công tác, v cóà độ ọ ậ đị ế ủ ầ à

th c vùng l m vi c c a nó theo quan h v i các thông s ng h c c a cácể ả à ệ ủ ệ ớ ố độ ọ ủ
c p khâu- kh p. B i toán ng h c ng c nh m xác nh b thông s ngặ ớ à độ ọ ượ ằ đị ộ ố độ
h c m b o chuy n ng cho tr c c a ph n công tác. Các b i toánọ để đả ả ể độ ướ ủ ầ à
ng c th ng có các c i m:ượ ườ đặ đ ể
- Các ph ng trình có d ng phi tuy n v siêu vi t, th ng không cho l iươ ạ ế à ệ ườ ờ
gi i úng.ả đ
- Có th có nhi u l i gi i.ể ề ờ ả
- Có th g p nghi m vô nh, vì các liên k t th a (gi ng nh liên k tể ặ ệ đị ế ừ ố ư ế
siêu t nh).ĩ
- Có th có nghi m tìm c b ng toán h c, nh ng l i không ch pể ệ đượ ằ ọ ư ạ ấ
nh n c v m t v t lý, do các r ng bu c v k t c u.ậ đượ ề ặ ậ à ộ ề ế ấ
Tính a nghi m c a b i toán ng h c ng c không ch ph thu c v ođ ệ ủ à độ ọ ượ ỉ ụ ộ à
các bi n kh p (s b c t do) m c v o s l ng các tham s khác khôngế ớ ố ậ ự à ả à ố ượ ố
trong k t c u.ế ấ
Vi c tìm các nghi m phù h p òi h i ng i thi t k m t tr c giác vệ ệ ợ đ ỏ ở ườ ế ế ộ ự ề
m t toán h c v v k t c u d oán nh ng c i m ho c khu v c khặ ọ à ề ế ấ để ự đ ữ đặ đ ể ặ ự ả
d gi m c s nghi m c n l a ch n.ĩ ả đượ ố ệ ầ ự ọ
2.1. Không gian thao tác v không gian c u hình c a Robot.à ấ ủ
Xét mô hình robot nh hình 2.13. V trí khâu thao tác c xác nh b iư ị đượ đị ở
sáu tham s ố
, , , , ,
x y z
p p p
ϕ ψ θ
. Trong óđ
[ , , ]
T
x y z
p p p
=

p
xác nh v trí c a i mđị ị ủ đ ể
thao tác E, còn
[ ]
T
ϕ ψ θ
=
α
xác nh h ng c a khâu thao tác. Trong s sáuđị ướ ủ ố
t a trên, trong nhi u tr ng h p ch quan tâm n m t s t a trong sọ độ ề ườ ợ ỉ đế ộ ố ọ độ ố
ó, ký hi u các t a c n quan tâm l đ ệ ọ độ ầ à
1 2
, , , ( 6)
m
x x x m
≤
:
17
[ ]
1 2
, , ,
T
m
x x x
=
x
Các t a ọ độ
1 2
, , ,
m

x x x
c g i l các t ađượ ọ à ọ
thao tác. T p h p các t a thao tác t ođộ ậ ợ ọ độ ạ
th nh không gian Euclide m chi u v cà ề à đượ
g i l không gian thao tác c a Robot.ọ à ủ
{x| x =f (q),q }
x i i q
= ∈¡ ¡
Ta s d ng các t a suy r ng ử ụ ọ độ ộ
1 2
, , ,
n
q q q
xác nh v trí các khâu c ađể đị ị ủ
robot. Thông th ng chúng l t a các kh p. Nh th ta có:ườ à ọ độ ớ ư ế

[ ]
1 2

T
n
q q q
=
q
Các t a ọ độ
1 2
, , ,
n
q q q
c g i l các t a kh p. T p h p các t a đượ ọ à ọ độ ớ ậ ợ ọ độ

1 2
, , ,
n
q q q
t o th nh không gian Euclide n chi u g i l không gian c u hìnhạ à ề ọ à ấ
c a Robot.ủ
min max
{ | q <q <q , i=1,2, ,n}
q i i i
=
q¡
2.2. B i toán ng h c ng cà độ ọ ượ
Khi gi i b i toán ng h c thu n robot ta xác nh c quan h : ả à độ ọ ậ đị đượ ệ

x = f(q)
(1.22)
T ph ng trình trên ta suy ra m t cách hình th c: ừ ươ ộ ứ
-1
q = f (x)
(1.23)
Trong ó: đ
[ ] [ ]
1 2 1 2
, , , , , , ,
T T
m n
x x x q q q
= =
x q
Khi m = n ta quy c g i l robot c u trúc không d ho c robot chu n.ướ ọ à ấ ư ặ ẩ

N u m < n g i l robot có c u trúc d ho c robot d d n ng. ế ọ à ấ ư ặ ư ẫ độ
2.3. Các ph ng pháp gi i b i toán ng c ươ ả à ượ
Các ph ng pháp gi i b i toán ng h c ng c c phân lo iươ ả à độ ọ ượ đượ ạ
th nh 2 nhóm: à các ph ng pháp gi i tích v các ph ng pháp s .ươ ả à ươ ố
• Ph ng pháp gi i tíchươ ả cho phép tìm ra k t qu ế ả q l bi u th c gi i tích ià ể ứ ả đố
v i x. Các ph ng pháp n y cho k t qu chính xác v nhanh chóng nh ngớ ươ à ế ả à ư
Hình 2.: Tay máy 4 khâu
18
quá trình tính toán, th nh l p ph ng trình gi i tích ph c t p v không cóà ậ ươ ả ứ ạ à
cách gi i t ng quát cho m i robot.ả ổ ọ
• Ph ng pháp sươ ố l ph ng pháp tính g n úng v i sai s cho phép c sà ươ ầ đ ớ ố đượ ử
d ng v i s h tr c a máy tính. Ph ng pháp n y cho ta cách gi i t ng ụ ớ ự ỗ ợ ủ ươ à ả ổ
quát cho m i robot, cho k t qu chính xác c n thi t nh ng a ra k t quọ ế ả ầ ế ư đư ế ả
ch m.ậ
2.3.1. Ph ng pháp gi i tíchươ ả
V i ph ng pháp n y, khi gi i b i toán ng h c thu n b ng ph ng ớ ươ à ả à độ ọ ậ ằ ươ
pháp ma tr n Denavit-Hartenberg ta có ma tr n bi n i xác nh v trí c a ậ ậ ế đổ đị ị ủ
khâu thao tác l :à
(0)
1 2 1
( ) ( )
( ) ( )
0 1
n E
n n n n
q
−
 
= = =
 

 
A r q
T q D q H H H H
(1.24)
T ó xác nh c ma tr n côsin ch h ng c a khâu thao tác v véc từ đ đị đượ ậ ỉ ướ ủ à ơ
nh v i m thao tác E l các h m c a các t a suy r ng. N u s d ngđị ị đ ể à à ủ ọ độ ộ ế ử ụ
ph ng pháp ma tr n Craig, ta có:ươ ậ
(0)
1 2 1
( ) ( )
( ) ( )
1 1
n On
n n n n−
 
= = =
 
 
A q r q
T q C q K K K K
(1.25)
T ó ta xác nh c ma tr n côsin ch h ng c a khâu thao tác. Véc từ đ đị đượ ậ ỉ ướ ủ ơ
xác nh v trí c a i m thao tác E có d ng:đị ị ủ đ ể ạ
(0) (0) (0)
E On n E
= +
r r A u
(1.26)
M t khác t nhi m v công ngh c a robot ta có ma tr n c u hình c a khâuặ ừ ệ ụ ệ ủ ậ ấ ủ
thao tác (ma tr n côsin chi h ng c a khâu thao tác ó v véc t xác nh vậ ướ ủ đ à ơ đị ị

trí c a i m thao tác) d i d ng h m c a các t a thao tác:ủ đ ể ướ ạ à ủ ọ độ
( )
11 12 13 14
21 22 23 24
31 32 33 34
( ) ( ) ( ) ( )
( ) ( ) ( )
( )
( ) ( ) ( ) ( )
0 0 0 1
n
a x a x a x a x
a x a x a x a x
x
a x a x a x a x
 
 
 
=
 
 
 
T
(1.27)
T ó ta có ph ng trình ma tr n:ừ đ ươ ậ
( ) ( )
n n
x
=
T q T

(1.28)
19
T ph ng trình trên, s d ng các ph ng pháp i s v hình h c ta có thừ ươ ử ụ ươ đạ ố à ọ ể
tìm ra các h m xác nh các t a kh p:à đị ọ độ ớ
-1
q = f (x)
(1.29)
2.3.2. Ph ng pháp sươ ố
2.3.2.1. Xác nh véc t v n t c v véc t gia t c suy r ngđị ơ ậ ố à ơ ố ộ
Gi s ta ã xác nh c quan h gi a các t a thao tác v các t aả ử đ đị đượ ệ ữ ọ độ à ọ
kh p: độ ớ
, ,
m m
∈ ∈
x = f(q) x q¡ ¡

(1.20)
Khi m < n robot có s b c t do n, l n h n s t a xác nh v trí c aố ậ ự ớ ơ ố ọ độ đị ị ủ
khâu thao tác. Khi ó robot c g i l robot d d n ng. o h m 2 vđ đượ ọ à ư ẫ độ Đạ à ế
c a bi u th c (2.9) theo th i gian ta c:ủ ể ứ ờ đượ

( )
∂
= =
∂
f
x q J q q
q
& &
&

(1.21)
Trong ó:đ
1 1 1
1 2
1 2

( )

n
m m m
n
m n
f f f
q q q
f f f
q q q
×
∂ ∂ ∂
 
 
∂ ∂ ∂
 
∂
= =
 
∂
 
∂ ∂ ∂
 
 

∂ ∂ ∂
 
f
J q
q
(1.22)
J(q) c mxn c g i l ma tr n Jacobi. Gi s h ng c a ma tr n J(q) l m. ỡ đượ ọ à ậ ả ử ạ ủ ậ à
H ph ng trình (2.10) l h tuy n tính d n s v i ệ ươ à ệ ế ư ẩ ố ớ
q
&
.V i m t giá trớ ộ ị
v n t c c a khâu thao tác v m t c u hình c a tay máy s có vô s nghi mậ ố ủ à ộ ấ ủ ẽ ố ệ
l v n t c c a vector suy r ng. gi i h ph ng trình n y c n s d ng lýà ậ ố ủ ộ Để ả ệ ươ à ầ ử ụ
thuy t ma tr n ế ậ
ngh ch o suy r ngị đả ộ
nh ngh a ma tr n ngh ch o suy r ng:Đị ĩ ậ ị đả ộ
Ma tr n ngh ch o suy r ng c a ma tr n ậ ị đả ộ ủ ậ
m n
A R
×
∈
l ma tr n c n x m kíà ậ ỡ
hi u l ệ à
+
A
v c xác nh theo bi u th c:à đượ đị ể ứ
20

+ T T -1
A = A (A.A )

(1.23)
Ma tr n ngh ch o suy r ng có tính ch t t ng t ma tr n ngh ch oậ ị đả ộ ấ ươ ự ậ ị đả
trong phép nhân ma tr n. Khi m=n ta có ậ A
+
= A
-1
.
Nh v y ma tr n ngh ch o suy r ng l ma tr n ngh ch o t ng quát khiư ậ ậ ị đả ộ à ậ ị đả ổ
ma tr n không ph i l ma tr n vuông. ậ ả à ậ
Áp d ng lý thuy t ma tr n ngh ch o suy r ng cho ph ng trình (1.21) taụ ế ậ ị đả ộ ươ
cđượ :
( )
( ) ( )q t x t
+
=
q
J
& &
(1.24)
Ti p t c o h m 2 v c a bi u th c (2.10) ta c : ế ụ đạ à ế ủ ể ứ đượ

( ) ( ) ( ) ( ) ( )x t q t q t= +J q J q
&
&& && &
(1.25)
T ó suy ra ph ng trình xác nh véc t gia t c suy r ngừ đ ươ đị ơ ố ộ ;
( )
( ) ( )( ( ) ( ) ( )) ( ) ( ) ) ( ) ( )q t x t q t x t x t
+ + +
= − = −J q J q J q J(q J q

& &
&& && & && &
(1.26)
Nh v y ta có công th c xác nh véc t v n t c v gia t c trong h t a ư ậ ứ đị ơ ậ ố à ố ệ ọ độ
suy r ngộ :

( )
( ) . ( )q t x t
+
=
J q
& &
(1.27)

( )
( ) ( ) ( ) ( ) ( ) ( )q t x t x t
+ +
= −
J q J q J q
&
&& && &
(1.28)
Trong tr ng h p robot không d d n ng, trong 2 bi u th c trên ta thayườ ợ ư ẫ độ ể ứ
ma tr n t a ngh ch o ậ ự ị đả
+
(q)
J
b ngằ
-1
(q)

J
.
2.3.2.2.
Xác nh véc t t a suy r ngđị ơ ọ độ ộ
T các công th c (12) v (13) cho phép ta xác nh c véc t suy r ngừ ứ à đị đượ ơ ộ
v à
véc t gia t c suy r ng, n u bi t c q(t) t i th i i m kh o sát v các quyơ ố ộ ế ế đượ ạ ờ đ ể ả à
lu t ậ
21
( ), ( ), ( )x t x t x t
& &&
.
xác nh q(t) ta gi s robot l m vi c trong kho ng th i gian t=0 nĐế đị ả ử à ệ ả ờ đế
t=T. Chia kho ng th i gian ó th nh N kho ng b ng nhauả ờ đ à ả ằ :
T
t
N
∆ =
Nh th ta cóư ế :
1
( 0,1, , 1)
k k
t t t k N
+
= + ∆ = −
Áp d ng khai tri n Taylor h m véc t q(t) lân c n giá tr ụ ể à ơ ở ậ ị
k
t t
=
ta cđượ :

2
1
( ) ( ) ( ) ( )( )
2
k k k k
t t t t t t t
+ ∆ = + ∆ + ∆ +
q q q q
& &&
(1.29)
Th bi u th c (1.27) v o (1.29) v b qua các vô cùng bé b c cao ta cế ể ứ à à ỏ ậ đượ :
1
( ) ( ) ( ( )) ( ) 0,1, ,
k k k k
t t t t t k N
+
+
≈ + ∆ =
q q J q x
&
(1.30)
T các công th c trên ta có s thu t toánừ ứ ơ đồ ậ :
22
Hình 2.: S thu t toán xác nh véc t t a suy r ngơ đồ ậ đị ơ ọ độ ộ
K t qu thu c khá thô, vì v y c n nâng cao chính xác c a q(t) t iế ả đượ ậ ầ độ ủ ạ
th i i m t = tờ đ ể
k+1
. Khi ó ta s d ng thu t toán hi u ch nh gia l ng véc tđ ử ụ ậ ệ ỉ ượ ơ
t a ọ độ
suy r ng .ộ

Ph ng pháp hi u ch nh gia l ng ươ ệ ỉ ượ
Gi s ta ã bi tả ử đ ế
k k
q = q(t )
. Tr c tiên ta xác nh giá tr g n úng ướ đị ị ầ đ
k+1
q
%
1
( )
k k k k
t
+
+
= + ∆
q q J q x
%
&
(1.31)
23
Sau ó ta xác nh véc t chính xác h n c a đ đị ơ ơ ủ
k+1
q
theo công th cứ :
1 1 1k k k
+ + +
= + ∆
q q q
%
(1.32)

Ta c n xác nh ầ đị
1k
+
∆
q
: Ta có
1 1 1 1 1 1 1
( ) ( ) ( ) ( )
k k k k k k k
δ
δ
+ + + + + + +
= = + ∆ = + ∆ +
f
x f q f q q f q q q
q
% % %
(1.33)
Suy ra công th c g n úngứ ầ đ
1 1 1 1
( ) ( )
k k k k
+ + + +
∆ ≈ −
J q q x f q
% %
(1.34)

k+1
J(q )

%
l ma tr n c m x n, à ậ ỡ
k +1
Δq
l véc t có n ph n t . Do ó trong ph ngà ơ ầ ử đ ươ
trình trên, s n l n h n s ph ng trình.ố ẩ ớ ơ ố ươ
1 1 1 1
( )[ - ( )]
k k k k
+
+ + + +
∆ =
q J q x f q
% %
(1.35)
Sau ó ta l yđ ấ :

1 1 1k k k+ + +
= + ∆q q q
% %
(1.36)
N u ế
1k
ε
+
∆ ≥q
thì thay (1.36) v o (1.35) v ti p t c gi i h ph ng trình.à à ế ụ ả ệ ươ
N u ế
1k
ε

+
∆ <
q
thì d ng l i v l yừ ạ à ấ :
1 1k k
+ +
=
q q
%
Bi t c ế đượ q
k+1
ta suy ra c đượ
1 1
,
k k
+ +
q q
& &&
ánh giá sai sĐ ố
T các yêu c u công ngh ta cóừ ầ ệ
( )x x t
=
. M t khác t b i toán ng h cặ ừ à độ ọ
thu n ta cóậ
k k
x = f(q )
. Do ó ánh giá sai s c a ph ng pháp ta a v ođ để đ ố ủ ươ đư à
các công th c xác nh sai s c a dich chuy n, v n t c, gia t cứ đị ố ủ ể ậ ố ố
( ) ( ) ( ) ( )
( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( )
k k k k k
k k k k k k
k k k k k k k k
t t t
t t t
t t t
= − = −
= − = −
= − = − −
e x x x f q
e x x x J q q
e x x x J q q J q q
& &
& & &
&
&& & &&
&& && &&
(1.37)
l n các chu n c a véc t Độ ớ ẩ ủ ơ
, ( ), ( )
k k k k k
t te e e
& &&
cho bi t chính xác c aế độ ủ
ph ng pháp s m chúng ta s d ng.ươ ố à ử ụ
S thu t toán gi i b i toán ng h c ng c theo ph ng pháp sơ đồ ậ ả à độ ọ ượ ươ ố có
hi u ch nh gia l ng véc t t a suy r ng:ệ ỉ ượ ơ ọ độ ộ
24
Hình 2.: S thu t toán gi i b i toán ng h c ng cơ đồ ậ ả à độ ọ ượ

25
2.4. Ma tr n Jacobi gi i tích v ma tr n Jacobi hình h cậ ả à ậ ọ
2.4.1. Ma tr n Jacobi gi i tíchậ ả
Gi s c u hình c a robot c xác nh b i t a kh p ả ử ấ ủ đượ đị ở ọ độ ớ
[ ]
1 2
, , ,
T
n
q q q
=
q
.
Nh ã bi t ph ng trình ng h c c a robotư đ ế ươ độ ọ ủ
có d ngạ :
( )
=
x f q
Trong óđ :
 
=
 
 
p
x
α
,
[ ] [ ]
, , , , ,
T T

E E E
x y z
ψ θ ϕ
= =
pα

Các véc t ơ
,p
α
xác nh v trí v h ng c ađị ị à ướ ủ
khâu thao
tác l các h m ph thu c v o bi n kh p qà à ụ ộ à ế ớ :
1 2
( ), ( )
= =
q f qα f q
(1.38)
o h m theo t các ph ng trình trên ta cĐạ à ươ đượ :
1 1
2 2
( )
( )
T
d
dt
d
dt
α
∂
= = =

∂
∂
= = =
∂
f f
p q J q q
q
f f
α q J q q
q
& & &
& & &
(1.39)
Trong ó đ
T
J
c g i l ma tr n Jacobi t nh ti n, còn ma tr n đượ ọ à ậ ị ế ậ
α
J
c g iđượ ọ
l m tr n Jacobi các góc quayà à ậ :
1 2 1 2
1 2 1 2
1 2
1 2

,

E E E
n n

E E E
T
n n
E E E
n
n
x x x
q q q
q q q
y y y
q q q q q q
z z z
q q q
q q q
α
ψ ψ ψ
θ θ θ
ϕ ϕ ϕ
   
∂ ∂ ∂ ∂ ∂ ∂
   
∂ ∂ ∂
∂ ∂ ∂
 
 
 
 
∂ ∂ ∂ ∂ ∂ ∂
= =
   

∂ ∂ ∂ ∂ ∂ ∂
   
   
∂ ∂ ∂ ∂ ∂ ∂
 
 
∂ ∂ ∂
∂ ∂ ∂
  
J J
(1.40)
2.4.2. Ma tr n Jacobi hình h cậ ọ
Tr ng thái v n t c c a khâu thao tác c xác nh b i v n t c ạ ậ ố ủ đượ đị ở ậ ố
i m nh v v v n t c góc c a khâu thao tácđ ể đị ị à ậ ố ủ :
Hình 2.