Nghiên cứu và ứng dụng một số thuật toán tránh vật cản cho tay máy công nghiệp

TR NG Đ I H


C BÔCH KHOA H NàI

LUN VN TH C S
Nghin c"u v% "ng d'ng m)t
s, thu-t to/n tr/nh v-t c1n
cho tay m/y c4ng nghi5p
NGUY8N Đ9NH HUYNH

Ng%nh K: thu-t Cơ đi5n t=

Gi1ng vi n hư?ng d@n:

TS. Đ Đ ng Khoa
Ch) k+ c,a GVHD

NhBm chuy n m4n:

Cơ h"c $ng d%ng

Khoa:

Cơ đi'n t(


H N I, 12/2022


H TI LUN VN

Nghin c"u v% "ng d'ng m)t s, thu-t to/n tr/nh v-t c1n cho
tay m/y c4ng nghi5p

Gi/o vi0n hư2ng d3n
K+ v4 ghi r6 h" t0n

Đ Đ ng Khoa


L I CIM ƠN
Qua th8i gian h"c t9p v4 nghi0n c$u t;i Trư8ng Cơ kh<, Đ;i h"c B/ch khoa
H4 N?i, đ@n nay tAi đB ho4n th4nh lu9n v n th;c sD c,a mFnh.
V2i lGng kơn chIn th4nh t2i: TS. Đ Đ ng Khoa, ngư8i thPy đB t9n tFnh hư2ng d3n v4
truyQn cho tAi nh)ng kinh nghi'm qu+ b/u trong nghi0n c$u khoa h"c, tư vSn
đTnh hư2ng tAi ti@p c9n v2i lDnh vUc chuy0n mAn n4y.
T/c giK xin g(i l8i cKm ơn t2i Vi'n Nghi0n c$u Cơ kh< – B? CAng Thương đB
t;o m"i điQu ki'n thu9n lMi trong suWt qu/ trFnh l4m lu9n v n. T/c giK xin g(i l8i
cKm ơn NhXm chuy0n mAn Cơ h"c $ng d%ng – Khoa Cơ đi'n t( đB h trM v4 t;o
m"i điQu ki'n thu9n lMi cho t/c giK trong suWt th8i gian h"c t9p v4 nghi0n c$u.

CuWi cYng, t/c giK xin chIn th4nh cKm ơn đ@n gia đFnh đB luAn s/t c/nh,
đ?ng vi0n v4 ,ng h? t/c giK trong suWt qu/ trFnh l4m lu9n v n c,a mFnh.
NguyLn ĐMnh Huynh


MNC LNC
DANH M[C C\C K] HI_U Va CHb VIcT TdT......................................................................1
DANH M[C HfNH gNH...................................................................................................................3
DANH M[C BgNG..............................................................................................................................6

CHƯƠNG 1. TkNG QUAN............................................................................................................7
L+ do ch"n đQ t4i......................................................................................................................7
B4i to/n tr/nh va ch;m cho tay m/y dư d3n đ?ng.........................................................8
Tnng quan vQ phương ph/p x/c đTnh va ch;m v2i tay m/y.................................11
Tnng quan vQ phương ph/p thi@t k@ quo đ;o tr/nh v9t cKn............................17
Nghi0n c$u $ng d%ng b9c tU do dư trong vi'c tr/nh v9t cKn c,a tay m/y dư
d3n đ?ng.................................................................................................................................................24
Hư2ng nghi0n c$u c,a lu9n v n.........................................................................................31
K@t lu9n chương 1................................................................................................................35
CHƯƠNG 2. X\C ĐrNH VA CHsM CtA TAY M\Y TRONG QU\ TRfNH
HOsT ĐxNG........................................................................................................................................36
Gi2i thi'u chung........................................................................................................................36
MA hFnh hXa v9t thy...........................................................................................................36
T
tTThu9t to/n x/c đTnh va ch;m c,a tay m/y......................................................................54
MA phNng x/c đTnh va ch;m c,a tay m/y....................................................................56
K@t lu9n chương 2................................................................................................................64
CHƯƠNG 3. THIcT Kc QU{ ĐsO TR\NH VA CHsM CHO TAY M\Y .. 66
Gi2i thi'u chung........................................................................................................................66


Thi@t k@ quo đ;o off-line.................................................................................................66
Thi@t k@ quo đ;o on-line..................................................................................................72
K@t lu9n chương 3................................................................................................................76
CHƯƠNG 4. M~ PH NG THIcT Kc QU{ ĐsO TR\NH V T CgN CtA
TAY M\Y..............................................................................................................................................77
Gi2i thi'u chung........................................................................................................................77

GiKi ph/p đQ xuSt..................................................................................................................77
N?i dung mA phNng..............................................................................................................79
K@t quK mA phNng.............................................................................................................81
K@t lu9n chương 4................................................................................................................97
KcT LU N Va KIcN NGHr...........................................................................................................98
1. K@t lu9n...........................................................................................................................................98
2. Ki@n nghT nh)ng nghi0n c$u ti@p theo..........................................................................99
TaI LI_U THAM KHgO..............................................................................................................100
PH[ L[C................................................................................................................................................106


DANH MNC CÔC KP HIQU V CHR VIST TTT
TT

KU
hi5u

M4 t1

1

SW t"a đ? suy r?ng c,a khAng gian thao t/c

2

SW b9c tU do c,a tay m/y

3

q

Vecto t"a đ? kh2p

4

q̇

Vecto v9n tWc kh2p

5

q̈

Vecto gia tWc kh2p

6

J

Ma tr9n Jacobi

7

x

Vecto t"a đ? điym cuWi

8

ẋ

Vecto v9n tWc điym cuWi

9

J#

Ma tr9n Jacobi nghTch đKo suy r?ng

10

J+

11

∗

J
N

12
13
14

Ma tr9n Jacobi nghTch đKo theo phương ph/p DLS
Ma tr9n bY c,a ma tr9n Jacobi
Vecto v9n tWc kh2p tYy +

q̇

H' sW DLS

15
16

Ma tr9n Jacobi tUa nghTch đKo

SW đo khK n ng thao t/c c,a tay m/y
e

17

ẋ

18

J0

0

Vecto sai sW vT tr< điym cuWi
Vecto v9n tWc tr/nh va ch;m
Ma tr9n Jacobi t;i điym đưa v9n tWc tr/nh va ch;m

19

SW lPn l p tWi đa v2i thu9t to/n RRT

20

SW lPn l p c,a thu9t to/n r t g"n

21

KhoKng t ng tWi đa c,a thu9t to/n RRT

22

H' sW chuyyn đni ưu ti0n

23
24
25

d0

K

Vecto t điym tr0n v9t cKn gPn tay m/y nhSt đ@n tay
m/y
KhoKng c/ch t2i h;n cho ph p gi)a v9t cKn v4 tay m/y
H' sW trong phương trFnh đ?ng h"c sai sW

1


TT

ChV viWt tXt

M4 t1

1

DLS

Phương ph/p bFnh phương tWi thiyu (Damped Least
Squares)

2

GJK

Thu9t to/n Gilbert – Johnson – Keerthi

3

RRT

Thu9t to/n cIy ng3u nhi0n (Rapidly Exploring
Random Tree)

4

Bi-RRT

Thu9t to/n cIy ng3u nhi0n hai hư2ng (Bidirectional Rapidly Exploring Random Tree)

5

APF

Phương ph/p trư8ng th@ nhIn t;o (Artificial
Potential Fields)

6

CLIK

Phương ph/p điQu khiyn phKn h i vGng k
2


DANH MNC H9NH INH
HFnh 1.1. C/c cSu hFnh tay m/y cho cYng m?t vT tr< l4m vi'c ............................ 10
HFnh 1.2. C/c n?i dung nghi0n c$u b4i to/n tr/nh va ch;m cho tay m/y .......... 11
HFnh 1.3. H' thWng cKm bi@n x c gi/c tr0n tay m/y YuMi [2] ........................... 12
HFnh 1.4. Điym t2i h;n tr0n tay m/y .................................................................. 13
HFnh 1.5. Bao ph, hFnh h"c tay m/y (hFnh tr%) v4 v9t cKn (hFnh cPu) [12] ........ 15
HFnh 1.6. Bao ph, c/c khIu v2i h?p bao đTnh hư2ng [10] ................................. 15
HFnh 1.7. Bao ph, v9t cKn v4 tay m/y v2i nhiQu khWi cPu [9] ............................ 16
HFnh 1.8. PhIn lo;i c/c phương ph/p thi@t k@ quo đ;o hFnh h"c [29] ................ 19
HFnh 1.9. Thi@t k@ quo đ;o dUa tr0n trư8ng th@ nhIn t;o [7] ............................. 21
HFnh 1.10. V< d% vQ “b3y cUc tiyu c,a trư8ng th@ nhIn t;o [35] ...................... 22
HFnh 1.11. So s/nh đư8ng Spline tU nhi0n v4 đư8ng b9c ba đơn đi'u [40] ....... 24
HFnh 1.12. Sơ đ giKi b4i to/n đ?ng h"c ngưMc m$c v9n tWc v2i CLIK ............ 30
HFnh 1.13. Tay m/y KUKA iiwa 14 (đơn vT: mm) ............................................. 33
HFnh 1.14. H' tr%c t"a đ? tay m/y theo nguy0n tJc DH ..................................... 34
HFnh 2.1. C/c thu?c t

HFnh 2.2. C/c thu?c tHFnh 2.3. KhoKng c/ch gi)a hFnh tr% v4 hFnh cPu khi
2

1

39

∉

1

40

2

∈

42

......................

HFnh 2.4. KhoKng c/ch gi)a hFnh tr% v4 hFnh cPu khi
2

∈

......................

HFnh 2.5. KhoKng c/ch gi)a hai hFnh tr% khi

1

∈

1

v4

HFnh 2.6. KhoKng c/ch gi)a hai hFnh tr% khi

1

∉

1

ho c

HFnh 2.7. KhoKng c/ch gi)a hai hFnh tr% khi

1

∉

1

v4

1

∥

2

2

...................

...................

HFnh 2.8. KhoKng c/ch gi)a hai hFnh tr% khi

2

2
2

∉

∉

2

............... 43
44

......................................... 45

HFnh 2.9. H' tr%c t"a đ? tr0n c/c hFnh tr% ........................................................... 46

3


HFnh 2.10. T9p l i (tr/i) v4 t9p khAng l i (phKi)................................................................48
HFnh 2.11. Bao l i c,a t9p h)u h;n điym trong m t ph ng.................................................48
HFnh 2.12. C/c d;ng đơn hFnh cơ bKn....................................................................................49
HFnh 2.13. Minh h"a x/c đTnh điym tUa...............................................................................49
HFnh 2.14. Minh h"a x/c đTnh hi'u Minkowski tr0n m t ph ng...................................50
HFnh 2.15. X/c đTnh t"a đ? điym tUa......................................................................................52
HFnh 2.16. Thu9t to/n x/c đTnh va ch;m gi)a hai khIu....................................................55
HFnh 2.17. Thu9t to/n x/c đTnh va ch;m khIu tay m/y v4 v9t cKn............................56
HFnh 2.18. MA hFnh hXa c/c khIu c,a tay m/y...................................................................57
HFnh 2.19. Quy lu9t c/c gXc kh2p............................................................................................59
HFnh 2.20. MAi trư8ng l4m vi'c cX v9t cKn đưMc cho trư2c.....................................59
HFnh 2.21. KhoKng c/ch nhN nhSt gi)a khIu 4 v4 khIu 7.............................................60
HFnh 2.22. Đ? l'ch khoKng c/ch

47

gi)a hai phương ph/p............................................60

HFnh 2.23. KhoKng c/ch gi)a khIu 4 v4 c/c khIu 0, 1, 2, 6, 7......................................61
HFnh 2.24. KhoKng c/ch nhN nhSt gi)a khIu 4 v4 hFnh tr%.......................................63
HFnh 2.25. KhoKng c/ch nhN nhSt gi)a khIu 4 v4 hFnh cPu.......................................63
HFnh 2.26. H4m tr;ng th/i va ch;m gi)a tay m/y v4 mAi trư8ng.................................63
HFnh 2.27. KhoKng c/ch nhN nhSt gi)a tay m/y v4 v9t cKn........................................64
HFnh 3.1. MA tK ho;t đ?ng h4m EXTEND..........................................................................67
HFnh 3.2. Minh h"a đư8ng dTch chuyyn cX n t dư...........................................................69

HFnh 3.3. Sơ đ thu9t to/n r t g"n đư8ng dTch chuyyn....................................................70
HFnh 4.1. GiKi ph/p đQ xuSt.......................................................................................................78
HFnh 4.2. MAi trư8ng l4m vi'c cX v9t cKn..........................................................................79
HFnh 4.3. K@t quK thu9t to/n Bi-RRT..................................................................................81

4


HFnh 4.4. K@t quK r t g"n đư8ng dTch chuyyn................................................................82
HFnh 4.5. Quo đ;o v4 v9n tWc theo tr%c..............................................................................83
HFnh 4.6. Quo đ;o v4 v9n tWc theo tr%c..............................................................................84
HFnh 4.7. Quo đ;o v4 v9n tWc theo tr%c..............................................................................84
HFnh 4.8. Quo đ;o trong khAng gian.......................................................................................85
HFnh 4.9. HFnh chi@u quo đ;o tr0n m t ph ng...................................................................86
HFnh 4.10. HFnh chi@u quo đ;o tr0n m t ph ng.................................................................86
HFnh 4.11. HFnh chi@u quo đ;o tr0n m t ph ng.................................................................87
HFnh 4.12. Sai l'ch hai quo đ;o v2i đư8ng dTch chuyyn................................................87
HFnh 4.13. Quo đ;o c/c gXc kh2p (TH1)...............................................................................88
HFnh 4.15. KhoKng c/ch nhN nhSt t tay m/y đ@n c/c v9t cKn (TH1)...................89
HFnh 4.16. Quo đ;o điym cuWi trong trư8ng hMp cX th0m v9t cKn (TH2.1)....90
HFnh 4.17. Sai l'ch quo đ;o điym cuWi so v2i đư8ng PCHIP (TH2.1)....................90
HFnh 4.19. Sai l'ch gXc kh2p so v2i khi chưa cX th0m v9t cKn (TH2.1)..............91
HFnh 4.18. KhoKng c/ch nhN nhSt t tay m/y đ@n c/c v9t cKn (TH2.1)...............92
HFnh 4.19. Quo đ;o điym cuWi khi cX th0m v9t cKn chuyyn đ?ng.........................93
HFnh 4.20. Sai l'ch quo đ;o so v2i khi chưa cX th0m v9t cKn (TH2.2)...................93
HFnh 4.21. KhoKng c/ch nhN nhSt t tay m/y đ@n c/c v9t cKn (TH2.2)...............94
HFnh 4.22. H' sW chuyyn đni ưu ti0n......................................................................................94
HFnh 4.23. Sai l'ch gXc kh2p so v2i khi chưa cX th0m v9t cKn (TH2.2)..............95
HFnh 4.24. CSu hFnh tay m/y trong qu/ trFnh tr/nh v9t cKn........................................96

5


DANH MNC BING
BKng 1.1. C/c ko thu9t thi@t k@ quo đ;o đ?ng h"c qua t9p điym [36]...................23
BKng 1.2. BKng thAng sW DH c,a tay m/y KUKA iiwa 14........................................34
BKng 2.1. T"a đ? tIm, chiQu d4i đư8ng sinh c/c khWi tr%..........................................57
BKng 2.2. KhoKng c/ch nhN nhSt gi)a c/c khIu.................................................................62
BKng 4.1. C/c tham sW mA phNng..........................................................................................80
BKng 4.2. Th8i gian thUc hi'n thu9t to/n Bi-RRT khi mAi trư8ng thay đni..........82

6


CHƯƠNG 1. TZNG QUAN
LU do ch\n đ] t%i
Trong nhiQu th9p k qua, robot cAng nghi'p đB cX nh)ng bư2c ph/t triyn
v4 ti@n hXa m;nh m , c/c hư2ng nghi0n c$u robot chuyyn t robot cAng nghi'p
sang ph/t triyn c/c robot dTch v% v4 đưa robot hGa nh9p v2i nhu cPu xB h?i.
Tuy nhi0n, v2i nh)ng robot như robot h trM, robot ph%c h i ch$c n ng trong y
t@,… đGi hNi m$c đ? tinh vi cao hơn do cX sU tham gia c,a con ngư8i v4o
khAng gian l4m vi'c c,a ch ng. MAi trư8ng l4 m?t trong nh)ng y@u tW b0n
ngo4i v4 cX Knh hư ng qua l;i t2i sU ho;t đ?ng c,a robot. SU Knh hư ng qua l;i
n4y chvi'c c,a robot m4 sU tương t/c n4y l4 r4ng bu?c vQ vT tr<, v9n tWc, gia tWc,
r4ng bu?c vQ lUc, r4ng bu?c vQ khoKng c/ch,… Robot tương t/c v2i con ngư8i
ho c v2i c/c t/c nhIn trong mAi trư8ng l4m vi'c phKi đKm bKo an to4n, sU an
to4n n4y đưMc x( l+ theo hai c/ch [1]: th$ nhSt b ng c/ch thi@t k@ robot v2i cSu
tr c v4 v9t li'u phY hMp v4 th$ hai thAng qua vi'c t(cKm bi@n, camera…) cYng c/c thu9t to/n chuyyn đ?ng thAng minh.

Robot cAng nghi'p l4 nh)ng robot đưMc ch@ t;o vQ tWc đ? v4 đ? chnhi0n lUc tương t/c c,a ch ng khi va phKi ngư8i s gIy ra nhiQu thương thư ng đ@n đ? bQn khi va ch;m v2i c/c t/c nhIn mAi trư8ng xung quanh. M c dY cX m?t
sW s(a đni thi@t k@ cX thy giKi quy@t lo ng;i vQ an to4n trong qu/ trFnh robot l4m
vi'c trong mAi trư8ng cAng nghi'p, vi'c bao g m c/c t/c nhIn mAi trư8ng thay đni li0n t
%c đGi hNi vQ nhu cPu vQ c/c chương trFnh điQu khiyn đy thVF m%c đm/y cAng nghi'p tr/nh v9t cKn đB đưMc nghi0n c$u.

T;i Vi't Nam, vi'c nghi0n c$u, $ng d%ng, cKi ti@n c/c tay m/y robot phY hMp
v2i phương th$c sKn xuSt đB cX nhưng vi'c c,a tay m/y đy t;o ra m?t quo đ;o l4m vi'c khAng cX va ch;m trong khAng gian
l4m vi'c. XuSt ph/t t thUc t@ đX, t/c giK ch"n đQ t4i “Nghi0n c$u v4 $ng d%ng m?
t sW thu9t to/n tr/nh v9t cKn cho tay m/y cAng nghi'p nh m nghi0n c$u

7


thu9t to/n tr/nh v9t cKn trong khAng gian l4m vi'c, t đX đưa ra đưMc quo đ;o
l4m vi'c đKm bKo đ? tin c9y, đ? chB%i to/n tr/nh va ch^m cho tay m/y dư d@n đ)ng
Tng quan v tay m


y
Tay m/y l4 m?t lo;i robot thư8ng đưMc s( d%ng mAi trư8ng cAng nghi'p,
trong đX bao g m c/c khIu l4 c/c v9t rJn nWi v2i nhau b i c/c kh2p t;o th4nh m?t
chu i đ?ng h"c m .
T"a đ? suy r?ng c,a tay m/y l4 t9p hMp c/c tham sW x/c đTnh vT tr< c,a tSt
cK c/c khIu thu?c tay m/y. Vectơ t"a đ? suy r?ng hay vectơ mA tK cSu hFnh c,a

tay m/y thư8ng cho b i
(1.1)

⊺

=[ 1, 2, … , ]

Trong đX l4 sW t"a đ? suy r?ng c,a tay m/y.
KhIu cuWi cYng c,a tay m/y thư8ng l4 b4n k p ho c l4 khIu gJn liQn v2i d
%ng c% l4m vi'c. Điym n t c,a khIu n4y đưMc g"i l4 điym t/c đ?ng cuWi, tương
t/c trUc ti@p v2i đWi tưMng ngo4i mAi trư8ng. VT tr< v4 hư2ng c,a điym t/c đ?
ng cuWi gJn liQn v2i b4i to/n đ?ng h"c tay m/y, vectơ ch$a thAng tin vT tr< v4
hư2ng c,a điym t/c đ?ng cuWi đưMc k< hi'u b i
(1.2)

⊺

=[ 1, 2, … , ]
Trong đX

l4 sW t"a đ? x/c đTnh vT tr< điym t/c đ?ng cuWi.

Tay m/y dư d3n đ?ng
Tay m/y dư d3n đ?ng l4 tay m/y cX sW t"a đ? suy r?ng l2n hơn sW t"a đ?
x/c đTnh vT tr< c,a điym t/c đ?ng cuWi trong trư8ng hMp khAng xKy ra vSn đQ
k dT đ?ng h"c, hay v4

( ) = , l4 ma tr9n Jacobi. ĐWi v2i tay m/y dư d3n

đ?ng ph ng, b4n k p chuyyn đ?ng trong m t ph ng cX ba b9c tU do n0n

> 3 v4
( ) = 3. ĐWi v2i tay m/y dư d3n đ?ng khAng gian, b4n k p chuyyn
đ?ng trong khAng gian cX s/u b9c tU do n0n > 6 v4

( ) = 6. N@u ta bN qua

hư2ng c,a b4n k p m4 ch quan tIm đ@n vT tr< c,a nX, l c n4y ta ch cPn điQu

8


ki'n > 2 v2i tay m/y dư d3n đ?ng ph ng, > 3 v2i tay m/y dư d3n đ?ng khAng gian
v4 điQu ki'n khAng xKy ra k dT đ?ng h"c.
Tay m/y dư d3n đ?ng t n t;i nhiQu b? nghi'm c,a b4i to/n đ?ng h"c ngưMc
khi@n tay m/y n4y cX nhiQu ưu điym hơn so v2i c/c tay m/y chu n như cho ph p
tWi ưu quo đ;o chuyyn đ?ng, tr/nh đưMc v9t cKn, tr/nh đưMc c/c điym k dT,
tr/nh c/c gi2i h;n kh2p, tr/nh sU cW k t kh2p [1].
KhAng gian kh2p v4 khAng gian thao t/c
Tư th@ x/c đTnh c,a tay m/y đưMc g"i l4 m?t cSu hFnh v4 cung cSp thAng sW
đPy đ, vQ vT tr< c,a m"i điym tr0n tay m/y. T9p hMp tSt cK c/c cSu hFnh đưMc g"i
l4 khAng gian cSu hFnh hay khAng gian kh2p (C-space). C/c bi@n kh2p thư8ng
đưMc s( d%ng đy mA tK tay m/y trong khAng gian kh2p. Khi x( l+ vi'c tr/nh va
ch;m, khAng gian kh2p thư8ng đưMc chia th4nh hai phPn. KhAng gian kh2p đưMc
quan tIm l4 , l4 t9p hMp tSt cK c/c cSu hFnh m4 tay m/y khAng xKy ra bSt k va
ch;m n4o v4 , l4 t9p hMp tSt cK c/c cSu hFnh m4 tay m/y cX xKy ra va ch;m.

KhAng gian thao t/c (khAng gian l4m vi'c) c,a tay m/y l4 t9p hMp tSt cK c/c
điym m4 điym t/c đ?ng cuWi c,a tay m/y cX thy ch;m đ@n khi ho;t đ?ng. KhAng
gian thao t/c s bT h;n ch@ b i gi2i h;n kh2p v4 hFnh d;ng c,a c/c khIu.
C

c ni dung nghi§n cu gii quyt bi to


n tr