Robot Do Đường Va Tranh Vật Cản Sử Dụng Thiết Bị Kinect

I HC QUC GIA THNH PH H CH MINH
TRNG I HC BCH KHOA

KHOA IN IN T
B MN VIN THNG
LUN VN TT NGHIP
ROBOT T HNH TRNH VT CN S DNG

THIT B KINECT
GVHD: PGS. TS. Hong nh Chin

SVTH : Nguyn Hng c
40700566
Nguyn Vn c
40700577
- Tp. H Ch Minh, Thng 1-2012 -
B GIO DC V O TO
TRNG H BCH KHOA
Thnh ph H Ch Minh
CNG HA X HI CH NGHA VIT NAM

c Lp T Do Hnh Phc
S:______/BKT
Khoa: in in t
B Mn: Vin Thng
NHIM V LUN VN TT NGHIP

H v tn:
H v tn:
Ngnh:
NGUYN HNG C
NGUYN VN C
VIN THNG
MSSV: 40700566
MSSV: 40700577
LP: DD07DV4
1. u lun vn: Robot t hnh trnh vt cn s dng thit b Kinect

2. Nhim v ( Yu cu v ni dung v s liu ban u):
......................................................................................................................................
......................................................................................................................................
......................................................................................................................................
......................................................................................................................................
3. Ngy giao nhim v lun vn:
4. Ngy hon thnh nhim v:
5. H v tn ngi hng dn:
Phn hng dn
................................................
Ni dung v yu cu LVTN c thng qua B Mn.
Ngy . ........ thng ........... nm 2012
CH NHIM B MN
NGI HNG DN CHNH
(K v ghi r h tn)
(K v ghi r h tn)
PHN DNH CHO KHOA, B MN:

Ngi duyt (chm s b):
n v:
Ngy bo v:
im tng kt:
Ni lu tr lun vn:
TRNG H BCH KHOA

KHOA IN IN T

c Lp T Do Hnh Phc
--o0o-Ngy
thng
nm 2012
PHIU CHM BO V LVTN

(Dnh cho ngi hng dn)
H v tn:
NGUYN HNG C
MSSV: 40700566
H v tn:
NGUYN VN C
MSSV: 40700577
Ngnh:
VIN THNG
LP: DD07DV4
1. ti: Robot t hnh trnh vt cn s dng thit b Kinect
2. H tn ngi hng dn: PGS. TS. HONG NH CHIN
3. Tng qut v bn thuyt minh:
S trang
........
S chng
........
S bng s liu
........
S hnh v
........
S ti liu tham kho
........
Phn mm tnh ton
........
4. Tng qut v cc bn v:
- S bn v:
bn A1
bn A2
kh khc
- S bn v tay
s bn v trn my tnh
5. Nhng u im chnh ca LVTN:
6. Nhng thiu st chnh ca LVTN:
7. ngh: c bo v ,
B sung thm bo v ,
Khng c bo v .
8. 3 cu hi sinh vin tr li trc Hi ng:
a)
b)
c)
9. nh gi chung (bng ch: gii, kh, TB): im .
K tn (ghi r h tn)
TRNG H BCH KHOA

KHOA IN IN T

c Lp T Do Hnh Phc
--o0o-Ngy
thng
nm 2012
PHIU CHM BO V LVTN

(Dnh cho ngi phn bin)
H v tn:
NGUYN HNG C
MSSV: 40700566
H v tn:
NGUYN VN C
MSSV: 40700577
Ngnh:
VIN THNG
LP: DD07DV4
10. ti: Robot t hnh trnh vt cn s dng thit b Kinect
11. H tn ngi phn bin:
12. Tng qut v bn thuyt minh:
S trang
........
S chng
........
S bng s liu
........
S hnh v
........
S ti liu tham kho
........
Phn mm tnh ton
........
13. Tng qut v cc bn v:
- S bn v:
bn A1
bn A2
kh khc
- S bn v tay
s bn v trn my tnh
14. Nhng u im chnh ca LVTN:
15. Nhng thiu st chnh ca LVTN:
16. ngh: c bo v ,
B sung thm bo v ,
Khng c bo v .
17. 3 cu hi sinh vin tr li trc Hi ng:
a)
b)
c)
18. nh gi chung (bng ch: gii, kh, TB): im .
K tn (ghi r h tn)
LI CM N
Li u tin, chng em xin gi n Thy, PGS. TS. Hong nh Chin li cm

n chn thnh v su sc nht. Nh c s hng dn v gip tn tnh ca Thy
trong sut thi gian qua, chng em c th thc hin v hon thnh n Mn
Hc 2, Thc Tp Tt Nghip v Lun Vn Tt Nghip. Nhng li nhn xt, gp
v hng dn tn tnh ca Thy gip chng em c mt nh hng ng n
trong sut qu trnh thc hin ti, gip chng em nhn ra c nhng u khuyt
im ca ti v tng bc hon thin hn.
ng thi, chng em xin trn trng cm n cc Thy C ca Trng i Hc
Bch Khoa ni chung v ca khoa in - in T ni ring dy d chng em
sut qung thi gian ngi trn gh ging ng i hc. Nhng li ging ca Thy
C trn bc ging trang b cho chng em nhng kin thc v gip chng em tch
ly thm nhng kinh nghim.
Chng em cng xin gi li cm n ti hai cu sinh vin Bch Khoa: anh
Nguyn Quc Thnh v anh Nguyn Thanh Tm tn tnh hng dn chng em
nh hng ng trong nhng ngy bt u nhn ti lun vn.
Bn cnh , chng ti xin cm n s h tr v gip ca bn b trong thi
gian hc tp ti Trng i Hc Bch Khoa v trong qu trnh hon trnh hon
thnh Lun Vn Tt Nghip ny.
Cui cng, chng con cng chn thnh cm n s ng vin v s h tr ca
gia nh v cha m trong sut thi gian hc tp. c bit, chng con xin gi li cm
n trn trng nht n cha m, ngi sinh ra v nui dng chng con nn
ngi. S quan tm, lo lng v hy sinh ln lao ca cha m lun l ng lc cho
chng con c gng phn u trn con ng hc tp ca mnh. Mt ln na, chng
con xin gi n cha m s bit n su sc nht.
H Ch Minh, ngy 8 thng 1 nm 2012

NGUYN HNG C
NGUYN VN C
TM TT LUN VN
Theo d on trong tng lai, robot s l tm im ca mt cuc cch mng
ln sau Internet. Con ngi s c nhu cu s hu mt robot c nhn nh nhu cu
mt my tnh PC by gi. Vi xu hng ny, cng cc ng dng truyn thng khc
ca robot trong cng nghip, y t, gio dc o to, gii tr v c bit l trong an
ninh quc phng th th trng robot s v cng to ln. ti lun vn hng ti
vic ng dng cng ngh x l nh mi cho robot t hnh, to tin cho vic xy
dng mt robot dch v hon chnh, c kh nng phc v cho i sng con ngi.
Trong khun kh ca lun vn, nhm s tp trung xy dng mt m hnh
mobile robot hon chnh c kh nng tm ng n ch v trnh chng ngi vt
trn qung ng di chuyn. Mt im mi c nhn mnh l khi th gic my
tnh cho mobile robot,vi s h tr ca thit b chi game Kinect c kh nng khi
phc mi trng pha trc robot di dng 3D, p ng c s chnh xc cn
thit khi phi hp vi cc gii thut iu khin truyn thng cho robot.
Nhm sinh vin thc hin
NGUYN HNG C
NGUYN VN C
ii
mc
Trang
Li cm n........................................................................................................................ i
Tm tt lun vn .............................................................................................................. ii
Mc lc ........................................................................................................................... iii
Danh mc t vit tt ........................................................................................................ v
Danh mc hnh ............................................................................................................. viii
Danh mc bng ............................................................................................................... xi
Mc lc
Chng 1: Gii thiu .................................................................................................... 1
1.1 Xu hng pht trin ca robot hin i ................................................................. 2
1.2 Nhng vn ca robot di ng ............................................................................ 2
1.3 Mc tiu lun vn v phng php thc hin ........................................................ 3
1.4 S lc v ni dung lun vn ................................................................................ 4
Chng 2: Tm hiu v Kinect ..................................................................................... 5
2.1 Gii thiu chung ..................................................................................................... 6
2.2 Nhng thnh phn chnh ca Kinect ...................................................................... 7
2.3 Tnh ton su ..................................................................................................... 8
2.4 Mt s c tnh khc ............................................................................................ 12
Chng 3: Th vin x l nh ................................................................................... 15
3.1 Th vin h tr Kinect ......................................................................................... 16
3.2 So snh Kinect SDK beta v OpenNI .................................................................. 17
3.3 Point Cloud Library ............................................................................................. 20
Chng 4: Pht hin vt cn ...................................................................................... 22
4.1 Cc phng php pht hin vt cn khng s dng camera ................................ 23
4.1.1 Dng cng tc hnh trnh............................................................................... 23
4.1.2 Dng cm bin siu m [13] .......................................................................... 23
4.2 Cc phng php pht hin vt cn s dng camera ........................................... 26
iii
4.2.1
X l nh vi mt camera (Monocular vision)......................................... 26
4.2.2
X l nh vi hai camera (Stereo vision) ................................................. 29
4.3 Pht hin vt cn s dng Kinect ......................................................................... 31

Chng 5: Module iu khin ng c ..................................................................... 39
5.1 PIC 18F4550 ........................................................................................................ 40
5.1.1 Gii thiu chung ............................................................................................ 40
5.1.2 Nhng module chnh s dng trong lun vn ............................................... 45
5.2 Mch cng sut (mch cu H) ............................................................................. 55
Chng 6: ng c v gii thut PID v tr .............................................................. 56
6.1 ng c Servo DC .............................................................................................. 57
6.1.1 ng c DC ................................................................................................... 57
6.1.2 Encoder .......................................................................................................... 59
6.2 Gii thut PID v tr [19] ...................................................................................... 63
Chng 7: Tnh ton ta Robot v Kinect .......................................................... 68
7.1 Cc php chuyn i h trc ta c bn ......................................................... 69
7.2 Tnh ton ta robot .......................................................................................... 70
7.3 Tnh ton ta Kinect........................................................................................ 73
Chng 8: Chng trnh iu khin ......................................................................... 76
8.1 Ni dung chng trnh iu khin ....................................................................... 77
8.2 Gii thut chng trnh do my tnh x l........................................................... 77
8.3 Gii thut chng trnh do vi iu khin x l .................................................... 88
KT LUN V HNG PHT TRIN ................................................................. 90
TI LIU THAM KHO .......................................................................................... 92
PH LC 1: Kt hp th vin OpenNI v Code Laboratories Kinect (CL) s
dng chc nng iu khin ng c Kinect. ............................................................. 93
PH LC 2: Cch u dy dng pin 12V thay adapter v to gn ln robot
cho Kinect ..................................................................................................................... 96
PH LC 3: Kch thc robot .................................................................................. 99
iv
Danh mc t vit tt
A
ADC
AGV
API
AUV
AUX
Analog Digital Converter

Autonomous Guided Vehicles
Application Programming Interface
Autonomous Underwater Vehicles
AUXiliary
C
CCP
CL
CMOS
CNC
CPU
Capture/Compare/PWM
Code Laboratories
Complementary Metal Oxide Semiconductor
Computerized Numerical Control
Central Processing Unit
E
Enhanced Capture/Compare/PWM
ECCP
EEPROM Electrically Erasable Programmable Read Only Memory
EUSART Enhanced Universal Synchronous Asynchronous Receiver Transmitter
F
FLANN
Fast Library for Approximate Nearest Neighbors
G
GPIO
General Purpose Input Ouput
H
HDD
Hard Disk Drive

v
I
ICD
ICSP
IR
In Circuit Debugger
In Circuit Serial Programming
Infrared
J
JNA
JNI
Java Native Access

Java Native Interface
M
MFC
MSSP
MUX
Microsoft Foundation Class Library

Master Synchronous Serial Port
Multiplexer
N
NI
NUI
Natural Interaction
Natural User Interface
Q
QVGA
Quarter Video Graphics Array
R
RAM
RANSAC
RGB
Random Access Memory

RANdom SAmple Consensus
Red, Green, Blue
S
SDK
Software Development Kit

vi
SSP
SXGA
Synchronous Serial Port

Super eXtended Graphics Array
T
TOF
TTL
Time Of Flight
Transistor Transistor Logic
U
UAV
USART
USB
Unmanned Arial Vehicles

Universal Synchronous Asynchronous Receiver Transmitter
Universal Serial Bus
V
VGA
VTK
Video Graphics Array

Visualization Toolkit
vii
Danh mc hnh
Hnh 2.1: Thit b Kinect ............................................................................................... 6
Hnh 2.2: Nhng thnh phn chnh ca Kinect ............................................................. 7
Hnh 2.3: ng c iu khin gc ngng Kinect .......................................................... 8
Hnh 2.4: Bn trong Kinect: RGB, IR camera v IR projector ..................................... 8
Hnh 2.5: Qu trnh thu v bn su nh ............................................................... 9
Hnh 2.6: Mu hnh c chiu bi projector v chp li bng IR camera ................ 10
Hnh 2.7: Tnh ton khong cch ti mt im chiu t Projector ............................. 11
Hnh 2.8: Kinect adapter ............................................................................................. 14
Hnh 3.1: Th vin OpenNI phi hp gia phn cng v ng dng u cui............ 19
Hnh 3.2: Point cloud library logo ............................................................................... 20
Hnh 4.1: M hnh robot dng cng tc hnh trnh ..................................................... 23
Hnh 4.2: Cm bin siu m ........................................................................................ 24
Hnh 4.3: Hin tng Forecasting ............................................................................... 25
Hnh 4.4: Hin tng Crosstalk................................................................................... 25
Hnh 4.5: Th trng ca robot vi optical flow ......................................................... 26
Hnh 4.6: nh gc v nh sau khi tch bin................................................................ 27
Hnh 4.7: Hn ch ca phng php d bin .............................................................. 28
Hnh 4.8: Phng php d nn .................................................................................... 28
Hnh 4.9: Phng php Stereo Vision ......................................................................... 29
Hnh 4.10: S x l: pht hin v tch vt cn ...................................................... 31
Hnh 4.11: nh RGB v bn su ....................................................................... 32
Hnh 4.12: RGB point cloud........................................................................................ 33
Hnh 4.13: Voxel grid .................................................................................................. 34
Hnh 4.14: Pass through khng c v c voxel grid.................................................... 35
Hnh 4.15: Tm m hnh ng thng bng thut ton RANSAC .............................. 36
Hnh 4.16: Point cloud sau khi thc hin xong bc lc v phn on ...................... 37
viii
Hnh 4.17: Object cluster............................................................................................. 38

Hnh 5.1: PICLAB-V2 v board mch cu H ............................................................. 40
Hnh 5.2: PIC 18F4550 ............................................................................................... 40
Hnh 5.3: S chn PIC18F4550 ............................................................................. 41
Hnh 5.4: S khi ca PIC 18F4550 ...................................................................... 44
Hnh 5.5: S khi b dao ng ca PIC 18F4550 ................................................. 45
Hnh 5.6: Cu to ca chn GPIO ............................................................................... 48
Hnh 5.7: S khi logic ca h thng ngt trong PIC18F4550 .............................. 49
Hnh 5.8: S khi ca b Timer 1 .......................................................................... 50
Hnh 5.9: S khi ca b Timer 2 .......................................................................... 51
Hnh 5.10: S khi b PWM .................................................................................. 52
Hnh 5.11: Gin thi gian ca sng iu xung ti chn CCPx............................... 52
Hnh 5.12: S khi b truyn ca module EUSART ............................................. 54
Hnh 5.13: S khi b nhn ca module EUSART ............................................... 55
Hnh 6.1: Cp ng c Servo DC ................................................................................ 57
Hnh 6.2: iu chnh rng xung PWM .................................................................. 58
Hnh 6.3: Dng sng p v dng trn ng c ............................................................ 59
Hnh 6.4: Optical Encoder ........................................................................................... 60
Hnh 6.5: Hai knh A v B lch pha trong encoder .................................................... 61
Hnh 6.6: Encoder i km ng c Servo DC ............................................................. 62
Hnh 6.7: PID vng kn ............................................................................................... 63
Hnh 6.8: p ng ca cc h thng iu khin ......................................................... 65
Hnh 6.9: Qu trnh tnh ton PID ............................................................................... 66
Hnh 7.1: Php tnh tin ............................................................................................... 69
Hnh 7.2: Php quay .................................................................................................... 70
Hnh 7.3: M hnh robot .............................................................................................. 71
Hnh 7.4: Ta robot (quan st t trn xung) ......................................................... 72
Hnh 7.5: H trc ta Kinect .................................................................................. 73
Hnh 7.6: ng nht h trc ta Kinect v Robot .................................................. 74
ix
Hnh 7.7: Ta Kinect trc (tri) v sau (phi) khi chuyn trc ............................ 75
Hnh 8.1: Ni dung chng trnh iu khin .............................................................. 77
Hnh 8.2: X l a tin trnh ....................................................................................... 78
Hnh 8.3: Giao din chng trnh iu khin .............................................................. 78
Hnh 8.4: S gii thut iu khin robot do my tnh x l ................................... 80
Hnh 8.5: Tnh gc quay v ch ................................................................................. 81
Hnh 8.6: Vt cn bn tri robot .................................................................................. 83
Hnh 8.7: Vt cn bn phi robot ................................................................................ 84
Hnh 8.8: Vt cn nm gia ng di chuyn ca robot ......................................... 85
Hnh 8.9: i mt on an ton v pha phi vt cn ................................................... 86
Hnh 8.10: C bo c vt cn v ng trng ............................................................ 87
Hnh 8.11: Khng gian cho robot lch qua ................................................................. 88
Hnh 8.12: S gii thut trn vi iu khin ............................................................ 89
Danh mc bng
Bng 2.1: Gc m v tiu c RGB v IR camera ....................................................... 13
Bng 2.2: Cng sut tiu th trn Kinect .................................................................... 14
Bng 5.1: Bng m t cc chc nng tng chn ca PIC18F4550 ............................. 42
Bng 6.1: nh hng ca cc thnh phn Kp, Ki, Kd i vi h kn ......................... 64
xi
Chng 1: Gii thiu
Ni dung chnh
1.1 Xu hng pht trin ca robot hin i
1.2 Nhng vn ca robot di ng
1.3 Mc tiu lun vn v phng php thc hin
1.4 S lc v ni dung lun vn
Trang 1
Chng 1: Gii thiu

1.1 Xu hng pht trin ca robot hin i
Theo d on trong vng 20 nm na mi ngi s c nhu cu s dng mt robot
c nhn nh nhu cu mt my tnh PC hin nay v robot s l tm im ca mt cuc
cch mng ln sau Internet. Vi xu hng ny, cng cc ng dng truyn thng khc
ca robot trong cng nghip, y t, gio dc o to, gii tr v c bit l trong an ninh
quc phng th th trng robot s v cng to ln.
Robot c nhng bc tin ng k trong hn na th k qua. Robot u tin
c ng dng trong cng nghip vo nhng nm 60 thay th con ngi lm nhng
cng vic nng nhc, nguy him trong mi trng c hi. Do nhu cu s dng ngy
cng nhiu trong qu trnh sn xut phc tp nn robot cng nghip cn c nhng kh
nng thch ng linh hot v thng minh hn. Ngy nay, ngoi ng dng s khai ban
u ca robot trong ch to my th cc ng dng khc nh trong y t, chm sc sc
khe, nng nghip, ng tu, xy dng, an ninh quc phng ang l ng lc cho s
pht trin ca ngnh cng nghip robot.
C th k n nhng loi robot c quan tm nhiu trong thi gian qua l: tay
my robot (Robot Manipulators), robot di ng (Mobile Robots), robot phng sinh hc
(Bio Inspired Robots) v robot c nhn (Personal Robots). Robot di ng c nghin
cu nhiu nh xe t hnh trn mt t AGV (Autonomous Guided Vehicles), robot t
hnh di nc AUV (Autonomous Underwater Vehicles), robot t hnh trn khng
UAV (Unmanned Arial Vehicles) v robot v tr (Space robots). Vi robot phng sinh
hc, cc nghin cu trong thi gian qua tp trung vo hai loi chnh l robot i b
(Walking robot) v robot dng ngi (Humanoid robot). Bn cnh cc loi robot
phng sinh hc nh c di nc, cc cu trc chuyn ng phng theo sinh vt bin
cng c nhiu nhm nghin cu, pht trin.
1.2 Nhng vn ca robot di ng
Robot di ng c nh ngha l mt loi xe robot c kh nng t di chuyn, t
vn ng (c th lp trnh li c) di s iu khin t ng thc hin tt nhng
cng vic c giao. Mi trng hot ng ca robot c th l t, nc, khng kh,
Trang 2
Chng 1: Gii thiu

khng gian v tr hay l s t hp ca cc mi trng trn. B mt a hnh robot di
chuyn c th l bng phng hoc thay i li lm.
Nhng ng dng thc t i hi nhng robot di ng c tnh t ng cao v
nhng k thut hin i, bao gm s a dng ca nhng cm bin r m ng tin cy
v tnh ton in t cng sut lm tng tnh t ng ha ca robot di ng. Tnh t
ng ha c ngha l robot phi da vo chnh kh nng ca n xut ra nhng d
liu vn hnh c ch t b phn cm bin v t n a ra quyt nh thch hp.
Mt trong cc yu cu c bn ca robot t ng thc th l kh nng nh hng
tt trong phm vi mi trng cha xc nh v hnh dung ra mt bn nh hng.
Bng cch s dng nhng quan st thch hp t mi trng, kt hp vi bn cng
lc nh hng cho robot ang l mt yu cu cn nghin cu cho robot di ng.
Vic ng thi nh v v v bn cng lc l mt phng php chung c lin quan
n vic trin khai mt h thng di ng trong mi trng cha xc nh. i vi mt
robot di ng t ng, nh hng l mt cng vic di chuyn mt cch an ton t
ni ny n ni khc.
Vic nh hng gp nhiu kh khn do nhiu vn kh phc tp. Vn gy
tr ngi chnh l nhng hn ch ca vic c tnh nng lng, nhng kh khn trong
vic pht hin v nhn bit i tng, nhng kh khn trong vic trnh xung t vi
cc i tng khc nhau, v nhng kh khn lin quan ti vic s dng thng tin cung
cp t mi trng.
1.3 Mc tiu lun vn v phng php thc hin
Mc tiu lun vn:
Xy dng m hnh mobile robot c kh nng t nh hng v ch vi ta
ch cho trc lc bt u khi ng. Ngoi kh nng nh hng, robot phi trnh
c cc vt cn c trn qung ng di chuyn. Mc ch su xa ca ti l pht
trin mt robot t ng thng minh, c th c s dng trong lnh vc phc v con
ngi, gip vic nh robot hng dn du khch hoc lm mt s cng vic n gin
trong vn phng hoc ti gia nh mang caf, ht bi, lau nh,
Trang 3
Chng 1: Gii thiu

Phng php thc hin:
Thit k m hnh mt robot di ng t ng, trong bao gm phn gia cng c
kh, mch cng sut, mch vi iu khin, ng thi kt hp k thut x l nh thng
qua my tnh (hay th gic my tnh). Khi th gic c chn l thit b chi game
Kinect, thng qua x l t my tnh n s kt hp vi mch vi iu khin gip robot c
kh nng nh hng v ch v trnh vt cn hon ton t ng.
1.4 S lc v ni dung lun vn
Ni dung lun vn bao gm 8 chng v 3 ph lc:
Chng 1: Gii thiu. S lc v ni dung ti.
Chng 2: Tm hiu v Kinect. Ni dung chnh cp ti phn cng Kinect,
cc sensor tch hp (RGB camera, depth sensor, microphone) v cch thc hot
ng ca Kinect.
Chng 3: Th vin x l nh. Gii thiu s lc cc th vin hin hnh cho
Kinect, so snh tng quan cc th vin v i su vo th vin s dng trong
lun vn.
Chng 4: Pht hin vt cn. Gii thiu cc phng php trnh vt cn ph
bin, tp trung vo phng php x l nh trong khng gian 3D nhm a ra
cc thng s chnh xc nht v vt cn cho k hoch di chuyn ca robot.
Chng 5: Module iu khin ng c. Bao gm: mch vi iu khin s dng
PIC 18F4550 v mch cng sut.
Chng 6: ng c v gii thut PID v tr. Gii thiu s lc ng c Servo
DC v chi tit gii thut PID v tr.
Chng 7: Tnh ton ta Robot v Kinect. S lc v cc php chuyn i
h trc c bn, di h trc ta Kinect v h trc ta robot ng nht
cho qu trnh x l.
Chng 8: Chng trnh iu khin. Tp trung vo gii thut iu khin cho
my tnh v module iu khin ng c.
Trang 4
Chng 2: Tm hiu v Kinect
Ni dung chnh
2.1 Gii thiu chung
2.2 Nhng thnh phn chnh ca Kinect
2.3 Tnh ton su
2.4 Mt s c tnh khc
Trang 5

2.1 Gii thiu chung
Hnh 2.1: Thit b Kinect

Kinect l sn phm ca Microsoft da trn cng ngh camera c pht trin bi
PrimeSense, nhng sn phm u tin c bn ti Bc M vo ngy 4 thng 11 nm
2010 [1]. Kinect c coi nh l mt thit b ngoi vi cho Xbox 360, cho php giao
tip vi con ngi thng qua cc c ch, em li nhng cm gic th v cho ngi chi
game trn Xbox. Kh nng hiu c c ch con ngi ca Kinect da trn hai c
tnh chnh sau: thng tin v su nh (depth map), kh nng pht hin v bm theo
c tnh c th ngi (body skeleton tracking).
Kinect ang gi k lc Guiness th gii v Thit b in t c tiu th nhanh
nht vi 8 triu sn phm trong 60 ngy. Mi triu sn phm Kinect c phn
phi trn th gii vo ngy 9 thng 3 nm 2011. Bn cnh phc v cho mc ch chi
game, sn phm Kinect cn c dng vo mc ch nghin cu x l nh 3D, pht
hin c ch (gesture recognition), bm theo ngi (body tracking) v nhiu mc ch
khc. L do chnh cho s thnh cng ca sn phm Kinect l gi c kh r (khong
140$ trn 1 sn phm) cho thit b c kh nng cung cp cc thng tin 3D vi cht
lng chp nhn c.
Trang 6
2.2 Nhng thnh phn chnh ca Kinect
Hnh 2.2: Nhng thnh phn chnh ca Kinect

Kinect gm c: RGB camera, cm bin su (3D Depth Sensors), dy
microphone (Multi-array Mic) v ng c iu khin gc ngng (Motorized Tilt).
RGB Camera: nh mt camera thng thng, c phn gii 640480 vi
tc 30 fps.
Cm bin su: su c thu v nh s kt hp ca hai cm bin: n
chiu hng ngoi (IR Projector) v camera hng ngoi (IR camera).
Dy a microphone: gm bn microphone c b tr dc Kinect nh trn
hnh 2.2, c dng vo cc ng dng iu khin bng ging ni.
ng c iu khin gc ngng: l loi ng c DC kh nh, cho php ta iu
chnh camera ln xung bo m camera c c gc nhn tt nht.
Trang 7
Hnh 2.3: ng c iu khin gc ngng Kinect

Mt trong nhng c tnh quan trng nht ca Kinect l thu v gi tr su
hay gi tr khong cch ti vt th trong th gii thc. Phn tip theo s ni v nguyn
l hot ng ca Kinect trong vic tnh ton gi tr ny.
2.3 Tnh ton su
Hnh 2.4: Bn trong Kinect: RGB, IR camera v IR projector

Cp cm bin IR camera v IR projector s phi hp vi nhau cho ra gi tr
su nh bng cng ngh Light Coding ca PrimeSense [2].
Trang 8
Hnh 2.5: Qu trnh thu v bn su nh

Khc vi k thut Stereo Camera vi vic dng cp camera ging nhau xy
dng nn bn su, hay k thut Time-Of-Flight (TOF) nh ngha khong cch
bng c lng thi gian di chuyn ca tia sng i v v trong khng gian; k thut
Light Coding dng mt ngun sng hng ngoi chiu lin tc kt hp vi mt camera
hng ngoi tnh ton khong cch [3]. Cng vic tnh ton ny c thc hin bn
trong Kinect bng chip PS1080 SoC ca PrimeSense. Cng ngh mi ny c cho l
p ng chnh xc hn, gi c r hn cho vic s dng mi trng trong nh.
Projector s chiu mt chm sng hng ngoi, to nn nhng m sng khng
gian pha trc Kinect, tp hp m sng c pht ra ny l c nh. Nhng m
sng ny c to ra nh mt ngun sng truyn qua li nhiu x (diffraction
Trang 9

gratings). Tp hp cc m sng ny c IR camera chp li, thng qua gii thut
c bit c tch hp trong PS1080 SoC [4] cho ra bn su. Bn cht ca gii
thut ny l cc php ton hnh hc da trn quan h gia hai cm bin IR camera v
Projector m ta s cp sau. Hnh 2.6 cho ta thy r mu hnh tp hp cc m sng
t Projector v c chp li bi IR camera.
Hnh 2.6: Mu hnh c chiu bi projector v chp li bng IR camera

hiu cch thc Kinect c lng khong cch ti vt th trong mi trng
nh th no, ta quan st hnh 2.7 trong trng hp phn tch vi mt im n gin.
Trang 10
Hnh 2.7: Tnh ton khong cch ti mt im chiu t Projector [5]

Ta gi s Projector pht i mt tia sng dc ng mu xanh l, n s c chp
li di dng mt m sng bi IR camera khi chm vo b mt vt th trong khng
gian. Ta xt ba mt phng ba khong cch khc nhau: mt phng gn Kinect (close
plane), mt phng xa Kinect (distant plane) v mt phng tham chiu (reference
plane) gia hai mt phng trn. Trong , mt phng tham chiu ngm c bit
trc bn trong Kinect vi y thng tin v khong cch. Ngoi ra, ta cng cp
thm mt phng nh (image plane) ca IR camera, l mt phng hnh chiu ca cc
im trong khng gian thu v bi IR camera. Ta xt trong ba trng hp khi tia sng
mu xanh l chm vo ba im trn ba mt phng ln lt l A, B, C; ba im ny
c chiu ln mt phng nh tng ng l A, B, C. Quan st v tr A, B v C, ta
c nhn xt: im A cng gn Kinect (hay close plane cng gn Kinect) th A cng xa
B v pha bn phi; ngc li, im C cng xa Kinect (hay distant plane cng xa
Kinect) th C cng xa B v pha bn tri. T : khi ta bit trc hng, im xut
Trang 11

pht ca tia sng t Projector v v tr B l hnh chiu ca im B trn mt phng
tham chiu ln mt phng nh, ta hon ton c th tnh ton c su nh hay
khong cch ti vt th.
Kinect lm iu tng t vi tp hp cc m sng cn li pht i t projector,
vi mt phng tham chiu bit trc. N tm im l tm ca m sng m IR camera
chp li c v im tng ng ca m sng trn mt phng tham chiu (v d:
hnh 2.7 ta c A v B, C v B l cc cp im tng ng), tm khong chnh lch
gia hai im ny theo chiu ngang khi chiu v trn mt phng nh; v lu l gi tr
chnh lch ny c tnh bng n v pixel. Tp hp ca tt c cc gi tr chnh lch
t tp hp m sng, s to nn bn chnh lch (disparity map), gi tr ny cng
ln th khong cch hay gi tr su nh (depth) cng ln, t m ta xy dng c
bn su (depth map) vi gi tr tnh bng mt thc s. Tuy nhin, do tp hp s
lng m sng pht i t projector nh hn so vi tng s pixel trn mt phng nh
ca IR camera nn mt phn gi tr su nh cn li s c ni suy.
Theo tnh ton ca Nicolas Burrus [6], mt trong nhng ngi m ng cho
vic tm hiu v Kinect qua cc th nghim ca ng. ng cng thc ha c quan
h gia gi tr khong cch tht z tnh bng mt v gi tr chnh lch d:
z
1
-0.0030711016 d + 3.3309495161
Trong d l con s nguyn biu din di dng 11 bit, tc khong thay i t

02047. Vi kt qu o c thc nghim trn th vin OpenNI, gi tr z bin thin
trong khong 0.56.0 mt v bn su n nh trong khong 0.55.0 mt. Do ,
gi tr d thc s bin thin trong khong t 4341030. Nh vy, trong khng gian t
00.5 mt pha trc Kinect, Kinect khng th a v bn su, y l mt
nhc im s c khc phc v cp n trong chng 7.
2.4 Mt s c tnh khc
Mt s c tnh khc ca Kinect ng quan tm: tiu c v gc m camera (field
of view), ngun cung cp v cng sut tiu th, mi trng hot ng. Kinect l sn
Trang 12

phm thng mi ca Microsoft nn cc thng s k thut chi tit khng c cng
b. Cc thng s c trnh by di y l kt qu o c thc nghim:
Tiu c, gc m IR camera v RGB camera:
Hai camera RGB v IR c t cch nhau 2.5 cm nn c cht khc nhau
khung hnh thu v t hai camera. m bo khung hnh RGB c th cha c
khung hnh IR, ngi ta thit k gc m ca RGB camera ln hn. iu ny cng dn
n tiu c ca RGB camera nh hn. Cc thng s trong bng 2.1 c o c bng
thc nghim:
Feature
RGB
camera
~62o
~48o
~72o
IR
camera
~58o
~44o
~69o
Horizontal
Field of View
Vertical
(degrees)
Diagonal
525
580
Focal length (pixels)
Bng 2.1: Gc m v tiu c ca RGB v IR camera [3]
Ngun cung cp v cng sut tiu th:
V Kinect cn nhiu in nng hot ng nn cng USB ca Xbox-360 khng
th p ng m phi qua mt cng chia chia thnh 2 kt ni ring l USB v kt ni
ngun, gip cho thit b kt ni vi Xbox-360 bng cng USB trong khi ngun in
cn cho Kinect l 12VDC c ly t adapter. Phin bn Xbox-360 mi s khng cn
adapter v n c cc AUX port c bit cung cp cho cng kt ni. Vi kt ni USB
ta hon ton c th cho Kinect giao tip vi my tnh. Cch thay adapter bng ngun
pin 12V dng trn mobile robot c ni thm phn ph lc 2.
Trang 13
Hnh 2.8: Kinect adapter

Cng sut tiu th o bng thc nghim:
Power consumption (idle)
~3.3W
Power consumption (active)
~4.7W
Bng 2.2: Cng sut tiu th trn Kinect [3]
Mi trng hot ng:
Kinect l thit b c thit k cho vic s dng mi trng trong nh (indoor).
mi trng ngoi tri, kt qu th nghim cho bn su khng chnh xc vo
thi im nh sng mnh, nhng cho kt qu chp nhn c khi nh sng yu (vo
thi im bui chiu ti).
Trang 14
Chng 3: Th vin x l nh
Ni dung chnh
3.1 Th vin h tr Kinect
3.2 So snh Kinect SDK beta v OpenNI
3.3 Point Cloud Library
Trang 15
3.1 Th vin h tr Kinect
Ngay khi mi ra i, Kinect c quan tm bi rt nhiu nh pht trin phn
mm, khng ch trn mng pht trin game cho Xbox m cn trn mng x l nh ng
dng trong y hc, robot, mapping Do m nhiu th vin c vit cho Kinect ra
i. Cho n thi im hin ti, cc th vin ng ch l Libfreenect, Code
Laboratories Kinect, OpenNI v Kinect SDK beta.
Libfreenect:
Libfreenect [7] l th vin c pht trin bi OpenKinect, do mt cng ng
nhng ngi quan tm n phn cng Kinect vit ra v chia s. Cng ng
OpenKinect lm vic hon ton t nguyn v khng v mc ch li nhun, h pht
trin Libfreenect thnh mt m ngun m cho cc h iu hnh khc nhau Windows,
Linux v OS X. Hin ti, Libfreenect c ng gi cho vic s dng trn Python, C,
C++, C#, Java JNI, Java JNA, Javascript.
Code Laboratories Kinect:
Code Laboratories (CL) [8] l mt cng ty v phn mm chuyn h tr cc nh
pht trin, lp trnh vin khai thc cc tnh nng ca cc thit b x l nh. Trong s
Kinect khng phi l ngoi l, CL cung cp cho ngi s dng nhng tnh nng c bn
nht ca Kinect v camera, audio v motor.
OpenNI:
Th vin OpenNI [9] c xem l th vin mnh nht trc s c mt ca Kinect
SDK beta, th vin ny h tr a ngn ng trn nhiu platform khc nhau, gip cho
cc lp trnh vin c th vit cc ng dng trn Kinect rt d dng vi tng tc t
nhin Natural Interaction (NI). Mc ch chnh ca OpenNI l xy dng cc hm API
chun, cho php th vin c kh nng kt hp vi cc middleware nhm lm tng sc
mnh cho Kinect.
Kinect SDK beta:
Kinect SDK [10] beta c Microsoft a ra vo ngy 16 thng 6 nm 2011, l
mt cng c lp trnh mnh cho cc nh pht trin. N cho php lp trnh vin truy
xut ton b tnh nng ca thit b Kinect. Mt iu bt tin l th vin ny ch h tr
Trang 16
trn cng c lp trnh ca Microsoft l Visual Studio 2010 vi cc ngn ng l C++,
C# v Visual Basic. Cc tnh nng ni bt nh: thu nh t cc sensor, skeleton tracking
v iu khin bng ging ni thng qua cng c nhn bit ging ni, Windows Speech
Recognition API. Phin bn beta hin ti ch cho php s dng vo nhng mc ch
phi li nhun, phin bn thng mi ha hn s sm ra mt trong nm 2012. Phin bn
beta mi nht c cp nht vo ngy 1 thng 11 nm 2011 vi nhiu li c sa v
chy tt trn Windows 8 Developer Preview.
Ti thi im hin ti, hai th vin OpenNI v Kinect SDK beta l la chn sng
sut cho vic lp trnh trn Kinect bi tnh nng h tr mnh m ca hai th vin ny.
Mc 3.2 s phn tch v so snh hai th vin ny v chn la th vin ph hp cho
ti lun vn.
3.2 So snh Kinect SDK beta v OpenNI
Sau y l nhn nh v u, khuyt im ca hai th vin trn [11] (lu l
Kinect SDK ang trong giai on beta nn s c vi im thay i ti bn chnh thc
cui cng)
Kinect SDK beta:
u im:
+ H tr x l m thanh.
+ H tr ng c iu khin gc ngng.
+ Skeleton tracking (bm c tnh c th ngi): khng cn hiu chnh trc
khi bm, vn bm tt trong trng hp c th ngi quay theo nhiu
hng.
+ H tr truy xut cc sensor ca Kinect ng thi.
+ Vic ci t n gin.
Khuyt im:
+ Ch dng cho mc ch phi thng mi.
+ Ch h tr bm ton th c tnh c th ngi (khng c ch hand
tracking hay bm tng phn c th nh trn OpenNI).
Trang 17
+ Khng h tr nhn bit c ch.
+ Ch h tr trn Win7 (x86 v x64) vi i hi kh cao:
My tnh dual-core, tc 2.66 GHz hoc nhanh hn.
Windows 7 vi h tr ca DirectX 9.0 tr ln.
Ram ti thiu 2 GB.
Lp trnh trn Visual Studio 2010.
+ Khng h tr vic thu nh trc tip t IR camera.

+ Vng Kinect khng nhn thy trong khong 00.8 mt trc Kinect.
OpenNI:
u im:
+ Cho php xy dng cc ng dng thng mi ha.
+ H tr pht hin c ch.
+ Skeleton tracking: h tr bm tng phn c th ngi v hand tracking
thng qua lin kt vi cc middleware. Hn na, tiu th cng sut ca
CPU t hn so vi khi s dng Kinect SDK.
+ H tr truy xut cc sensor ca Kinect ng thi.
+ H tr cho Windows, Linux v Mac OSX.
+ Cho php truy xut hnh nh thu v t IR camera.
+ Vng Kinect khng nhn thy trong khong chp nhn c l 0.5 mt
trc Kinect.
Khuyt im:
+ Khng h tr phn x l m thanh cho dy microphone.
+ Skeleton tracking: bm c tnh c th cn nhiu li v cha c n nh
nh trn Kinect SDK.
+ Khng h tr ng c iu khin gc ngng (x l vn ny bng cht
th thut kt hp vi Code Laboratories Kinect, xem thm phn ph lc
1).
+ Vic ci t c phn ri rm hn Kinect SDK.
Trang 18
Kt lun: Sinh vin cng c thi gian lm vic trn c hai th vin, nn c
nhng kt lun sau:
Kinect SDK beta mnh hn OpenNI c tnh bm c th ngi n nh hn, do
s p ng chnh xc vi cc c ch c th ngi; tuy nhin pht hin c ch ta
phi t vit gii thut trong khi trn OpenNI h tr sn. Ngoi ra, Kinect SDK hn
OpenNI phn h tr cho x l m thanh, cho php xy dng cc ng dng iu
khin bng ging ni d dng hn.
Hnh 3.1: Th vin OpenNI phi hp gia phn cng v ng dng u cui
OpenNI cho php thu bn su trong gii hn t 0.5 mt tr v pha trc
Kinect trong khi Kinect SDK th gii hn ny l 0.8 mt v pha trc, cht lng bn
su thu v trn hai th vin l nh nhau. Kinect SDK l th vin mi ra i ang
trong giai on beta v cng cha c s h tr ca cc th vin x l nh khc nh
Point Cloud. Trong khi OpenNI ra i trc v gn nh c tch hp vi th vin
x l nh Point Cloud. Do , vi ng dng Kinect trong ti robot t hnh trnh vt
Trang 19
cn th OpenNI l la chn ti u hn. cng chnh l l do sinh vin chn th vin
OpenNI kt hp vi Point Cloud cho ti lun vn ny.
Ni n tm nhn xa hn th ta thy: v tnh ph bin, OpenNI c th pht trin
trn cc nn tng h iu hnh khc nhau v nhn c s ng gp ca cng ng
xy dng m ngun m rng ln; v chuyn kinh doanh, ta c th pht trin thnh cc
sn phm thng mi ha trn th trng trong rt nhiu lnh vc nh robot, y t, gio
dc, gii tr
Th vin h tr Kinect gip ta ly v chiu su nh v mt s c tnh c bit
khc (nh skeleton tracking, hand tracking, gesture recognition trn OpenNI); tn
dng ti u sc mnh ca n ta kt hp cng th vin x l nh trong khng gian 3D
l Point Cloud m s c cp trong mc 3.3.
3.3 Point Cloud Library
Hnh 3.2: Point cloud library logo

PCL [12] l th vin h tr cho n-D Point Cloud v cho vic x l nh trong
khng gian 3D. Th vin c xy dng vi nhiu gii thut nh lc (filtering), khi
phc b mt (surface reconstruction), phn vng (segmentation), c lng c tnh
vt (feature estimation), ... PCL c th dng trn nhiu platform nh Linux, MacOS,
Windows v Android. n gin cho vic pht trin, PCL c chia ra thnh nhiu
th vin nh v c th bin dch mt cch ring l. Phin bn mi nht l PCL 1.3 a
ra vo ngy 31 thng 10 nm 2011. PCL hon ton min ph cho vic nghin cu hay
pht trin cc sn phm thng mi ha.
Trang 20
C th ni PCL l s kt hp ca nhiu module nh. Nhng module ny thc
cht cng l cc th vin thc hin cc chc nng ring l trc khi c PCL ng
gi. Cc th vin c bn ny l:
Eigen: mt th vin m h tr cho cc php ton tuyn tnh, c dng trong hu
ht cc tnh ton ton hc ca PCL.
FLANN: (Fast Library for Approximate Nearest Neighbors) h tr cho vic tm
kim nhanh cc im ln cn trong khng gian 3D.
Boost: gip cho vic chia s con tr trn tt c cc module v thut ton trong
PCL trnh vic sao chp trng lp d liu c ly v trong h thng.
VTK: (Visualization Toolkit) h tr cho nhiu platform trong vic thu v d liu
3D, h tr vic hin th, c lng th tch vt th.
CMinPack: mt th vin m gip cho vic gii quyt cc php ton tuyn tnh v
khng tuyn tnh.
Trang 21
Chng 4: Pht hin vt cn
Ni dung chnh
4.1 Cc phng php pht hin vt cn khng s dng camera
4.1.1 Dng cng tc hnh trnh
4.1.2 Dng cm bin siu m
4.2 Cc phng php pht hin vt cn s dng camera
4.2.1 X l nh vi mt camera (Monocular vision)
4.2.2 X l nh vi hai camera (Stereo vision)
4.3 Pht hin vt cn s dng Kinect
Trang 22

Vn pht hin v trnh vt cn l iu lun c cp ti cho robot t hnh.
Cng vi s pht trin ca cng ngh, ngy cng c nhiu cm bin tch hp gip
robot lm c iu ny n gin v chnh xc hn. Trc y, cc loi cm bin siu
m c dng rng ri; gn y, ngi ta quan tm n p dng cng ngh x l nh
nhiu hn. Sau y, ta im qua mt s phng php pht hin v trnh vt cn; nhn
nh u, khuyt im v la chn phng php ti u nht.
4.1 Cc phng php pht hin vt cn khng s dng camera
4.1.1 Dng cng tc hnh trnh
y l phng php n gin nht v t tn km.
Hot ng: cng tc hnh trnh c gn cho robot nh hnh 4.1, khi robot di
chuyn, nu chm chng ngi vt, tc ng cng tc, robot s nhn ra v r
hng khc.
Hnh 4.1: M hnh robot dng cng tc hnh trnh

u im: n gin, d thc hin, chi ph thp.
Khuyt im: robot phi va chm mi pht hin c chng ngi vt. Va chm
nhiu ln s d gy h hng v nh hng n nhng g robot ang ti.
4.1.2 Dng cm bin siu m [13]
y l mt phng php kh hiu qu v thng dng.
Hot ng: Cm bin s pht ra sng siu m vi gc m nht nh. Khi , nu
trong tm qut ca n pht hin chng ngi vt th sng siu m s phn hi li.
Ta c th o khong cch bng cch tnh thi gian t lc sng siu m pht ra n
Trang 23

lc thu sng v, sau kt hp vi vn tc sng siu m (khong 343 m/s) bit
c qung ng m sng i.
Hnh 4.2: Cm bin siu m

Cc cm bin c t lch mt gc , khong cch ln nht (tnh t D) m cc
cm bin c th nhn din c l dmax; dmax v phi m bo sao cho cm
bin c vng kim tra rng khi tin thng robot c th nhn din c vt
cn.
u im: x l nhanh, kt qu tng i chnh xc.
Khuyt im:
+ Cm bin siu m ch nhn bit c vt cn khi mt phng qut ca cm bin
ct ngang vt cn, do n s khng pht hin ra nhng vt cn nh, thp v
nm st mt t.
+ Do s dng sng siu m v s phn x ca n tnh khong cch v pht
hin vt cn nn gii thut iu khin kh phc tp v pht sinh mt s trng
hp sai s kh khc phc: sai s lp, hin tng Forecasting, hin tng c
cho (Crosstalk).
Sai s lp: sai s lp l sai s lun xy ra vi tt c cc thit b o lng,

trong c c cm bin siu m.
Hin tng forecasting: hin tng Forecasting l hin tng phn x gc

sai lch ca cm bin. Theo nguyn l TOF, c khong cch ng, cm
bin siu m phi hng vung gc vi b mt chng ngi vt cn o.
Tuy nhin, cc chng ngi vt khng bao gi l phng, mn nn tia phn
Trang 24

x c th khng tng ng vi gc ti. Cc chm tia phn x ny c nng
lng phn x thp hn. Tuy vy, mt khong cch no , cm bin
siu m vn c th ghi nhn c nhng tn hiu phn x ny. Kt qu l
thng s c v t cm bin siu m b lch do gc m ca cm bin siu
m ln.
Hnh 4.3: Hin tng Forecasting

Ngoi ra, v gc m rng nn khng ch sai v nhn dng v tr chng
ngi vt m khong cch ghi nhn cng b sai lch. Tuy nhin sai s ny
khng ng k nh sai s do hin tng c cho gy ra.
Hin tng crosstalk: hin tng c cho (Crosstalk) l hin tng m

cm bin siu m ny ghi nhn tn hiu phn x hoc trc tip t cm bin
siu m khc; hoc sau qu trnh sng siu m truyn i v phn x qua
cc b mt n quay li cm bin theo mt cch khng mong mun.
Hnh 4.4: Hin tng Crosstalk
Trang 25

4.2 Cc phng php pht hin vt cn s dng camera
4.2.1 X l nh vi mt camera (Monocular vision)
Phng php ny ch yu da trn cc phn tch mu sc hay s thay i do
chuyn ng ca cc khung hnh lin tip nhau; k thut x l nh n gin nhng ch
hiu qu trong mt s mi trng nht nh. Sau y l mt s phng php thng
c s dng: optical flow, edge detection, floor finder technique.
Optical Flow:
+ Phng php: l phng php trnh chng ngi vt nh vo quan st s di
chuyn ca phn t nh. Gii thut s tm nhng phn t nh c bit trong nh
ti mt frame no v quan st dch chuyn ca n frame tip theo. Vt
cng gn, dch chuyn cng ln.
Phng php ny chia th trng ca robot ra lm 2 phn tri, phi. Da vo gi
tr optical flow tnh c t 2 vng, ta s bit c chng ngi vt ang pha
no ca robot v ra lnh iu khin robot r trnh v hng lm gim gi tr
optical flow.
Hnh 4.5: Th trng ca robot vi optical flow

+ u im: Pht hin chng ngi vt khng ph thuc nhiu vo hnh dng vt.
+ Khuyt im:
Trang 26
Ch ti u i vi nhng vt c gc cnh, c nhiu im c bit. Gi s

nu gp mt bc tng trng, robot khng th phn bit c phn t no
l phn t tng ng khi xt t frame ny sang frame khc.
D b nhiu: nu nn c hoa vn hay ng vin s gy nhiu do camera

s bm theo cc phn t c bit trn nn. Ngoi ra, nhng vt trong th
trng ca robot phi ng yn, nu c vt chuyn ng xa nhng vn
trong th trng ca robot s lm tng optical flow khin robot nhm ln
ang c vt cn trc mt.
Tc x l chm do yu cu tnh ton nng.
Edge Detection [14]:

+ Phng php: phng php ny dng nhng k thut tch bin (nh Canny)
cho ta nh ch hin th ng bin ca vt th nh hnh 4.6. T gip ta phn
bit c nn v cc vt cn. Vt cn s l nhng vt c vin bao quanh, cn
nn l vng khng gian cn li.
Hnh 4.6: nh gc v nh sau khi tch bin

+ u im: x l nhanh, d tm ra vt cn tt, chnh xc.
+ Khuyt im: ch hot ng tt trong iu kin nn n sc v khng c hoa vn
hay ha tit.
Trang 27
Hnh 4.7: Hn ch ca phng php d bin

Floor Finder Technique [14]:
+ Phng php: phng php ny da trn mu sc ca cc im nh, nhng
im nh khng trng mu vi mu nn th c xem l vt cn. Ta gi nh
vng khng gian nh trc mt robot l khng c vt cn. Bng cch ly gi tr
mu sc cc im nh trong vng ny, ta c mt tp mu mu sc ca nn.
So snh gi tr mu ca tng im nh cn li trong hnh vi tp mu ny ta s
xc nh c im nh no thuc v nn, im nh no thuc v vt cn.
Hnh 4.8: Phng php d nn

+ u im: n gin, hiu qu, pht hin vt cn chnh xc, khng ph thuc vo
hnh dng v kch thc ca vt cn. ng thi, tc x l nhanh do yu cu
tnh ton khng nhiu.
Trang 28

+ Khuyt im: nh cc phng php dng x l nh khc, do s dng mu sc
nhn bit nn d nhm ln gia bng trn sn v vt cn do bng c mu
sc khc vi nn. Bn cnh , nu vt c cng mu vi nn th phng php
ny khng t hiu qu cao.
4.2.2 X l nh vi hai camera (Stereo vision)
Hn ch ca phng php s dng mt camera n t s nh hng ca mu sc
hay ha tit mi trng phc tp. cng l hn ch chung ca x l nh trong khng
gian 2D cho robot t hnh trnh vt cn. Bng vic s dng t hai camera tr ln ta thu
c y hn thng tin t mi trng, nh vy m nhng hn ch trn c khc
phc. Thng tin thm y l gi tr khong cch ti vt, lc ny cng vic x l nh
phc tp hn do phi lm vic trong khng gian 3D.
Hnh 4.9: Phng php Stereo Vision

+ Phng php: nh t th gii thc c thu v thng qua hai camera, sau
qua khu hiu chnh, khc phc mo trc khi cho ra bn su. Cng vic
ny c thc hin bng cc gii thut c bit vi s h tr ca th vin
OpenCV. Ta thu c y thng tin ca vt cn trong mi trng nh chiu
cao, b rng hay khong cch t camera ti vt sau khu khi phc 3D (3D
reconstruction).
+ u im : pht hin vt cn chnh xc, hiu qu, khng ph thuc hnh dng,
kch thc hay mu sc vt, c th pht hin cc vt th trong khng trung.
+ Khuyt im: x l phc tp, i hi s chnh xc cao trong khu hiu chnh.
Tnh ton kh nng nn yu cu b vi x l cao.
Trang 29
Nhn xt:
Vi s pht trin ca cng ngh hin nay th tc x l khng cn l vn kh
khn na; vi mc ch trnh vt cn mt cch tt nht th phng php cui cng,
Stereo Vision ni tri hn c.
Sinh vin c thi gian thc hin v hon thnh vic pht hin vt cn bng
phng php stereo vision trn hai camera. Kt qu ph thuc khu hiu chnh ban
u rt nhiu. V th, sinh vin tm n thit b c kh nng h tr vic ly bn
su trc tip, m khng bn tm nhiu n vic hiu chnh cho camera, l thit b
chi game Kinect.
Kinect cng mc ch vi phng php stereo vision l thu v bn su
nhng cch thc hin li c cht khc bit, d s dng gii thut tnh ton tng t
nhau. iu ny c trnh by mc 2.3. Cng ngh m Kinect s dng c xem
nh s giao thoa gia stereo vision v range finder (phng php o c khong cch
bng sng nh laser, hng ngoi hay sng siu m). Kt qu thu v trn Kinect chnh
xc hn, n nh hn, tiu tn ti nguyn my tnh t hn nhiu so vi vic s dng hai
camera trong phng php stereo vision. Do y l mt cng ngh mi nn sinh vin
gi l phng php x l nh trn Kinect v c trnh by chi tit trong mc 4.3.
Trang 30

4.3 Pht hin vt cn s dng Kinect
Hnh 4.10: S x l: pht hin v tch vt cn

Hnh 4.10 lit k tt c cc khi chnh trong s pht hin v tch vt cn s
dng Kinect. Khi s 1 lm nhim v thu v bn su, cng vic ny cn n th
vin OpenNI. Cc khi cn li t 27 lm nhim v x l bn su v cho ta y
thng tin v vt cn, cng vic ny cn n th vin Point Cloud (PCL). OpenNI v
PCL c cng ng m ngun m xy dng mi lin h kh mt thit vi nhau, nn
hn ch c cc xung t c th xy ra nh lm vic PCL cng vi cc th vin khc.
By gi ta tm hiu cch thc hot ng tng khi trong s trn.
Trang 31

Depth map:
Hnh 4.11: nh RGB v bn su

Depth map hay bn su cha thng tin v tr vt trong khng gian pha
trc Kinect. Bn su c OpenNI h tr cc phn gii v tc thu nh
sau:
SXGA_15Hz: phn gii 12801024, tc 15 fps.
VGA_30Hz: phn gii 640x480, tc 30 fps.
QVGA_30Hz: phn gii 320x240, tc 30 fps.
Vic la chn phn gii cng nh th tc x l khi qua th vin Point Cloud
cng c tng ln.
Trang 32
Point cloud:
Hnh 4.12: RGB point cloud

Point cloud P l mt tp hp cc phn t pi, mi phn t ny s cha tp cc gi
tr biu din khng gian nD (thng th n = 3) [15]:
p1 , p2 ,..., pi ,..., pn , pi xi , yi , zi
Bn cnh thng tin d liu l XYZ, mi im pi cn c th cha thm cc thng

tin khc nh: RGB colors, intensity values,
Mi gi tr xi, yi, zi c lu tr bng kiu float32. Nu ta chn phn gii
VGA th ta c: n = 640x480 = 307200 phn t p, y l mt con s kh ln. V vy:
X l chm (d liu ln dn n vic tnh ton s lu hn).
D liu kiu float 32 bit nn cn khng gian lu tr ln (cn RAM v HDD

ln).
My tnh sinh vin s dng c cu hnh nh sau:

Trang 33
CPU: Core i5-2430, 2.4 GHz
RAM: 4 GB
VGA: GeForce GT540 1GB
Tc thu v point cloud o c bc ny vo khong 1315 fps.

Chnh v hn ch ny m ta cn phi thc hin cc bc lc tin x l p ng
c yu cu x l thi gian thc. Cng vic ny c PCL nh ngha l
downsampling v removing points. Hai bc downsampling v removing points
c s dng l Voxel Grid v Pass Through.
Voxel grid:
Hnh 4.13: Voxel grid

Voxel grid lm gim mt s im xung; tp hp cc im qu gn nhau s
ch cn mt im i din. Ta chn gi tr mt ph hp m vn m bo quan st r
hnh dng vt th. Mt ta chn y l 3 centimet theo ba chiu X, Y v Z.
Hm h tr: setLeafSize(0.03f, 0.03f, 0.03f);
Trang 34
Pass through:
Hnh 4.14: Pass through khng c (tri) v c voxel grid (phi)

Pass through s gii hn khng gian ca point cloud theo cc chiu X, Y v Z.
y ta ch gii hn theo chiu Z. Nh phn tch trong mc 2.3, gi tr khong cch
theo chiu Z m Kinect c th nhn thy khong 0.55.0 mt, y l tm nhn khng
cn thit cho mobile robot, vic gii hn li s gip cho PCL x l nhanh hn. V th,
ta ch cn cho Kinect nhn trong khong 0.51.4 mt, l tm quan st cho ng dng
robot trnh vt cn. Tc thu hnh sau khi kt hp pass through v voxel grid (Hnh
4.14 (phi)) t gi tr 30 fps, tc t tc ti a.
Hm h tr:
setFilterFieldName ("z");
setFilterLimits(0.5, 1.4);
Plannar segmentation:
Plannar segmentation s tch cc point cloud c cu trc phng, sau tch ra
point cloud c tng s im ln nht, bng gii thut RANSAC (RANdom SAmple
Consensus). y l gii thut c lng mt m hnh ton hc t mt tp hp cc
im c cha nhiu (outliers). Gii thut ny ln u c cng b vo nm 1981 bi
Fischler v Bolles [16].
Hnh 4.15 cho ta thy ng dng thut ton RANSAC tm m hnh ng thng
(c dng ax + by + c = 0) trong mt tp hp cc im c cha nhiu.
Trang 35
Hnh 4.15: Tm m hnh ng thng bng thut ton RANSAC

Gii thut tm ng thng bng thut ton RANSAC c m t nh sau:
u vo:
data
- tp hp cc im
k
- s ln lp
t
- ngng (threshold) sai s xc nh im no c khp m hnh khng
u ra:
best_model
- m hnh tt nht
best_consensus_set
- tp hp cc im khp vi best_model
best_model
= null
best_consensus_set
= null
best_num_points
= 0
Lp k ln:
consensus_set
= tp hp 2 im ngu nhin thuc data
model
= m hnh ng thng suy ra t 2 im trn
Vi mi im thuc data nhng khng thuc consensus_set, ta xt:
distance = khong cch t im n ng thng;
if distance < t (im thuc m hnh nu sai s nh hn mc ngng t trc)
thm im vo consensus_set
num_points = s lng phn t trong consensus_set

if num_points > best_num_points
best_model
= model
best_consensus_set = consensus_set
best_num_points
= num_points
Tr v gi tr: best_model v best_consensus_set
Trang 36

Vi m hnh mt phng c dng ax + by + cz + d =0, thut ton RANSAC
cng lm c iu tng t nh i vi m hnh ng thng, thay v chn hai im
bt k tm m hnh th mt phng cn ba im. Quan st hnh 4.16, point cloud c
mu xanh dng l mt phng nn nh, c tm ra nh thut ton RANSAC.
Hm h tr:
setModelType(pcl::SACMODEL_PLANE);
setMethodType(pcl::SAC_RANSAC);
setDistanceThreshold(0.02);//threshold = 2cm
Euclidean cluster extraction:
Hnh 4.16: Point cloud sau khi thc hin xong bc

lc (filtering) v phn on (segmentation)
Euclidean cluster extraction lm cng vic tch cc point cloud c mt trn nn

nh, tp hp cc im gn nhau s c nhm li thnh mt point cloud hay cluster,
mi cluster i din mt vt th. Hnh 4.16 cho ta hai cluster: cluster mu (cluster
gn Kinect) v cluster mu xanh l (cluster xa Kinect) trn mt phng nn nh (point
cloud mu xanh dng).
Trang 37

Hm h tr:
setInputCloud(cloud_filtered);
setIndices(inliers);
filter(*cloud_cluster);
Object clusters:
Hnh 4.17: Object cluster

Object clusters l cc vt cn m ta c c sau bc Euclidean cluster
extraction. Bc ny lm nhim v phn tch c tnh vt cn v kch thc cng nh
v tr vt trong khng gian pha trc Kinect. y l nhng thng tin cn thit cho k
hoch trnh vt cn ca mobile robot. Cng vic ny c thc hin trn tng cluster,
v c h tr hm cn thit bi PCL.
Hm h tr:
getMinMax3D(&cloud_cluster, min_point, max_point);
Trang 38
Chng 5: Module iu khin ng c
Ni dung chnh
5.1 PIC 18F4550
5.1.1 Gii thiu chung
5.1.2 Nhng module chnh s dng trong lun vn
5.2 Mch cng sut (mch cu H)
Trang 39

Module iu khin ng c bao gm: board mch pht trin PIC microcontroller
PICLAB-V2 [17] ca Thin Minh v board mch cu H ko ng thi hai ng c ca
HLAB. Vi iu khin chnh c chn l PIC18F4550.
Hnh 5.1: PICLAB-V2 (tri) v board mch cu H (phi)

5.1 PIC 18F4550
5.1.1 Gii thiu chung
PIC18F4550 l mt vi x l c bn a chc nng v r. N l sn phm ca h vi
x l PIC thng dng ca cng ty Microchip ca M c tr s t ti Chandler,
Arizona (M).
Hnh 5.2: PIC 18F4550

Trang 40

PIC 18F4550 dng b nh Flash, c th ghi nhiu ln, dung lng ln p ng
c hu ht cc ng dng trong thc t:
in p hot ng rng t 2V n 5.5V.
B nh chng trnh Flash 32K nh cho php ghi 100,000 ln. B nh
d liu RAM c 2048 Bytes gm cc thanh ghi chc nng c bit v cc
thanh ghi a mc ch. Ngoi ra, PIC18F4550 cn c tch hp 256
Bytes EEPROM cho php ghi n 1,000,000 ln.
C 5 Port I/O vi 34 chn (Port A, Port B, Port C, Port D, Port E).
C 13 knh c ADC 10 bit.
C 1 knh CCP (Capture/Compare/PWM) v 1 knh ECCP (Enhanced
Capture/Compare/PWM).
Giao tip SSP (Synchronous Serial Port) v MSSP (Master Synchronous
Serial Port).
Module Enhanced USART h tr RS-485, RS-232.
C 1 timer 8 bit, 3 timer 16 bit.
C 3 ngt ngoi.
Sau y l s chn ca PIC18F4550 trong hp DIP-40:
Hnh 5.3: S chn PIC18F4550

Sau y l bng h thng chc nng cc chn v s khi ca PIC18F4550:
Trang 41
Chn
AN0-AN12
Hng
I
M t chc nng v cc c tnh

13 knh Input c tc dng l cng bin i ADC
Avdd
Ngun dng cho module ADC
Avss
GND cho module ADC
CLKI
CLKO
CN0 - CN7
CN17 - CN18
Li vo ca xung Clock ngoi, lun kt hp vi chn OSC1

Li ra ca b dao ng tinh th, ni vi tinh th hoc b cng
hng trong ch dao ng thch anh. Lun kt hp vi
chn chc nng OSC2
Khai bo thay i li vo
COFS
I/O
Cng giao tip chuyn i d liu ng b khung
CSCK
I/O
Cng giao tip chuyn i d liu Clock vo ra ni tip
CSDI
Li vo d liu ni tip
CSDO
Li ra d liu ni tip
C1RX
Cng nhn bus CAN1
C1TX
Cng pht bus CAN1
EMUD
I/O
Cng vo ra d liu knh truyn thng s cp ca ICD
EMUC
I/O
Vo ra xung nhp knh s cp
EMUD1
I/O
Vo ra d liu knh th cp
EMUC1
I/O
EMUD2
EMUC2
EMUD3
EMUC3
IC1 - IC8
I/O
I/O
I/O
I/O
I
Cc cng vo ca module Capture
INT0 - INT2
Cc ngt ngoi
LVDIN
Cng vo pht hin st th
/MCLR
Chn Reset, mc tch cc thp

Trang 42
OSC1
Li vo b giao ng tinh th. B m Trigger Schmitt c

s dng khi cu hnh trong ch RC
OSC2
Li ra b dao ng tinh th
PGD
I/O
Vo ra d liu ca ICSP
PGC
Li vo Clock ca ICSP
RA0 - RA6
I/O
Port A
RB0 - RB7
RC0 - RC7
RD0 - RD7
I/O
I/O
I/O
Port B
Port C
Port D
RE0 - RE3
I/O
Port E
SCK1
I/O
Vo ra Clock ng b ca khi SPI1
SDI1
Li vo d liu ca khi SPI1
SDO1
Li ra d liu ca SPI1
SS1
Slaver ng b
SCL
I/O
Vo ra Clock ni tip ca I2C
SDA
I/O
Vo ra Data ni tip ng b ca I2C
SOSCO
Li ra b dao ng tinh th cng sut thp 32Khz
SOSCI
Li vo b dao ng 32Khz
T1CK
Li vo xung Clock ngoi ca Timer1
T2CK
Li vo xung Clock ngoi ca Timer2
U1RX
Cng nhn khi UART1
U1TX
Cng pht khi UART1
U1ARX
Cng nhn m rng khi UART1
U1ATX
Cng pht m rng khi UART1
VDD
Chn ngun dng ca PIC
VSS
Chn GND
Vref+
Li vo Vref+ (cao) chun ca ADC
Vref-
Li vo Vref- (thp) chun ca ADC
Bng 5.1: Bng m t cc chc nng tng chn ca PIC18F4550

Trang 43
Hnh 5.4: S khi ca PIC 18F4550
Trang 44

5.1.2 Nhng module chnh s dng trong lun vn
Vi yu cu iu khin ng c c giao tip vi my tnh, nhng module chnh
ca PIC18F4550 c s dng l: b dao ng, cc port I/O, ngt ngoi, cc b
Timer, khi iu xung PWM v khi giao tip EUSART.
B dao ng ca PIC 18F4550:
Hnh 5.5: S khi b dao ng ca PIC 18F4550

Trang 45

Hot ng ca b dao ng trong PIC 18F4550 c iu khin thng qua hai
thanh ghi Configuration v hai thanh ghi control. Cc thanh ghi CONFIG1L v
CONFIG1H la chn ch dao ng v cc options cho prescaler/postcaler ca
USB. V l cc bits Configuration nn chng c set khi thit b c lp trnh v gi
nguyn cu hnh ny cho n khi thit b c lp trnh mi li. Thanh ghi OSCCON
la chn ch Active Clock, n c dng ch yu trong vic iu khin clock
switching trong ch qun l nng lng. Thanh ghi OSCTUNE dng cho vic loi
b ngun tn s INTRC, cng nh la chn ngun clock tn s thp.
PIC 18F4550 c th thc hin 12 ch dao ng khc nhau. Ngi lp trnh c
th iu chnh cc bit Configuration FOSC3:FOSC0 la chn ch dao ng thch
hp:
XT: Crystal/Resonator
XTPLL: Crystal/Resonator with PLL enabled
HS: High-Speed Crystal/Resonator
HSPLL: High-Speed Crystal/Resonator with PLL enabled
EC: External Clock with FOSC/4 output
ECIO: External Clock with I/O on RA6
ECPLL: External Clock with PLL enabled and FOSC/4 output on RA6
ECPIO: External Clock with PLL enabled, I/O on RA6
INTHS: Internal Oscillator used as microcontroller clock source, HS

Oscillator used as USB clock source
INTXT: Internal Oscillator used as microcontroller clock source, XT

Oscillator used as USB clock source
INTIO: Internal Oscillator used as microcontroller clock source, EC

Oscillator used as USB clock source, digital I/O on RA6
INTCKO: Internal Oscillator used as microcontroller clock source, EC

Oscillator used as USB clock source, FOSC/4 output on RA6
Clock h thng ca PIC18F4550 c th c chn t hai ngun dao ng ni

(Internal Oscillator) hoc dao ng ngoi (Primary Oscillator v Secondary Oscillator)
Trang 46

nh b chn knh MUX. B MUX c iu khin bi cc bit FOSC<3:0> (bit 3, bit
2, bit 1, bit 0 ca thanh ghi CONFIG1 16-bit nh v ti a ch 2007H v 2008H trong
b nh chng trnh) v cc bit SCS<1:0> (bit 1, bit 0 ca thanh ghi OSCCON).Cc
bits SCS dng la chn ch Primary Clock, T1OSC hay Internal Oscillator. Cc
bit FOSC<3:0> c s dng cu hnh b dao ng Primary Clock.
B dao ng ni gm 2 b dao ng HFINTOSC 8MHz v LFINTOSC 31kHz.
Clock 8MHz ca b HFINTOSC c chia thnh cc tn s 8MHz, 4MHz, 2MHz,
1MHz, 500kHz, 250kHz, 125kHz nh b chia tn s postscaler. Tn s ngun clock
(INTOSC direct, INTRC direct hoc INTOSC postscaler) c la chn bng vic
iu chnh cc bits IRCF trong thanh ghi OSCCON.
B dao ng ngoi (c tch hp bn trong PIC) cn c kt ni vi cc b lc
ti cc chn OSC1, OSC2. Trong ti ny, ta s dng thch anh 12MHz to thnh
xung clock 12MHz cung cp cho clock h thng.
Cc port I/O:
PIC18F4550 tt c 34 chn I/O mc ch thng thng (GPIO: General Purpose
Input Ouput) c th c s dng. Ty theo nhng thit b ngoi vi c chn m mt
vi chn c th khng c s dng chc nng GPIO. Thng thng, khi mt thit
b ngoi vi c chn, nhng chn lin quan ca thit b ngoi vi c th khng c s
dng chc nng GPIO. 34 chn GPIO c chia cho 5 Port: Port A gm 7 chn, Port
B gm 8 chn, Port C gm 8 chn, Port D gm 8 chn v Port E gm 3 chn.
Mi port c iu khin bi 2 thanh ghi 8-bit, thanh ghi Port v thanh ghi Tris.
Thanh ghi Tris c s dng iu khin port l nhp hay xut. Mi bit ca Tris s
iu khin mi chn ca port , nu gi tr ca bit l 1 th chn lin quan l nhp,
ngc li nu gi tr ca bit l 0 th chn lin quan l xut. Thanh ghi Port c s
dng cha gi tr ca port lin quan. Mi bit ca thanh ghi Port s cha gi tr ca
chn lin quan.
Trang 47
Hnh 5.6: Cu to ca chn GPIO

Ngt ngoi trn cc chn RB:
Trang 48
Hnh 5.7: S khi logic ca h thng ngt trong PIC18F4550

Cc ngt ngoi INT0 ng vi chn RB0, INT1 ng vi chn RB1, INT2 ng vi
chn RB2.
Cc ngt ngoi trn cc chn RB c kch khi theo sn. Sn ln nu nh bit
INTEDG = 1 (bit 6 ca thanh ghi OPTION_REG), sn xung nu INTEDG = 0. Khi
mt sn thch hp xut hin trn chn RB, c INTF c bt ln 1. Ngt ny c th
c cho php nu bit INTE=1, khng cho php nu INTE=0. C INTF cn c xa
bng phn mm trong trnh phc v ngt trc khi cho php ngt tr li.
Trong ti ny, ta dng ngt ngoi INT1 (trn chn RB1) v INT2 (trn chn
RB2) c encoder v phc v cho vic tnh ton PID.
Trang 49

Cu to v hot ng ca b Timer 1 v Timer 2:
B Timer 1 l b nh thi 16-bit c cu to nh hnh sau:
Hnh 5.8: S khi ca b Timer 1

B Timer 1 l b m ln 16-bit c truy xut gin tip thng qua cp thanh ghi
TMR1H, TMR1L. Khi c c hoc ghi cc thanh ghi ny s cp nht trc tip gi
tr cho b Timer. Khi c s dng vi ngun clock ni, b Timer 1 s c vai tr l b
nh thi. Khi c s dng vi ngun clock ngoi, n s c vai tr l nh thi hoc
b m. S dng bit TMR1CS chn ngun clock.
Cc bit T1CKPS<1:0> nh gi tr cho b chia tn s Prescaler. Khi b TMR1
trn (t FFFFh n 0000h) c ngt TMR1IF s c thit lp ln 1. Nu lc ny c
TMR1IE =1, c PEIE=1 v GIE=1 th ngt Timer1 s xy ra. C TMR1IF cn c
xa trong trnh phc v ngt Timer 1. Trong ti ny, ta dng Timer 1 cho vic ly
mu ca b PID s.
Sau y l s khi b Timer 2:
Trang 50
Hnh 5.9: S khi ca b Timer 2

TMR2 tng t 00H vi mi clock (FOSC/4), hai bit counter/prescaler trn ng
vo clock cho ng vo trc tip vi la chn divide-by-4 hoc divide-by-16 prescale.
Chng c la chn bng bit prescaler control, T2CKPS1:T2CKPS0 (T2CON<1:0>).
Gi tr ca TMR2 c so snh vi gi tr ca thanh ghi chu k, PR2, trong mi chu k
clock. Khi hai gi tr ny tng ng nhau, b so snh to ra mt tn hiu iu khin
ng ra ca b Timer, tn hiu ny cng reset li gi tr ca TMR2 v 00H chu k k
tip v li ng ra b counter/postscaler. Cc thanh ghi TMR2 v PR2 u c th c v
ghi. Thanh ghi TMR2 b xa khi c mt thit b no reset, trong khi thanh ghi
PR2 khi to FFH. Ng ra khng chia t l ca TMR2 ch yu c dng cho
module CCP, c s thi gian cho cc php ton trong ch PWM.
Trong ti ny, ta dng Timer 2 cho vic iu xung PWM.
Cu to v hot ng ca khi iu xung PWM:
PIC18F4550 c hai b iu xung, hai b ny s to ra cc tn hiu iu xung trn
cc chn CCP1 v CCP2. rng, chu k v phn gii ca hai b iu xung c
xc nh bi cc thanh ghi PR2, T2CON, CCPR1L, CCPR2L, CCP1CON, CCP2CON.
ch iu xung (PWM), chn CCPx c th to ra u ra PWM vi phn gii ln
n 10 bit.
V chn CCPx c nhp chung vi ng d liu port B hoc port C nn
cc chn CCPx (CCP1 v CCP2) hot ng ch PWM, cn xa bit TRIS tng
ng ca cc chn . S khi ca cc b iu xung c m t trong hnh di y:
Trang 51
Hnh 5.10: S khi b PWM

Ch thch (Note 1) trong hnh trn biu th rng thanh ghi 8-bit TMR2 c kt
hp vi 2-bit prescaler ca b dao ng ni to ra b nh thi 10-bit. Cc thanh ghi
CCPRxH l cc thanh ghi ch c, kt hp vi 2 bit 5 v 4 ca cc thanh ghi
CCPxCON c vai tr nh rng ca xung; cc thanh ghi ny c ghi gin tip
thng qua cc thanh ghi CCPRxL. Thanh ghi 8-bit PR2 nh chu k cho xung ra.
Sng iu xung ti cc chn CCPx c gin thi gian nh sau:
Hnh 5.11: Gin thi gian ca sng iu xung ti chn CCPx

Trang 52

Thanh ghi TMR2 kt hp vi 2 bit prescaler s m ln nh xung clock ca h
thng. Khi gi tr ca TMR2 nh hn gi tr ca CCPRxL:CCPxCON<5:4>, chn
CCPx mc cao. Khi gi tr ca TMR2 bng vi gi tr ny, b so snh s o chn
CCPx xung mc 0. Khi gi tr ca TMR2 bng vi PR2, TMR2 s c xa v 0
ng thi kt thc chu k xung, chn CCPx li c thit lp mc cao.
Chu k ca xung c tnh theo cng thc sau:
Chu k PWM = [(PR2)+1] 4 TOSC (gi tr Pre-scale ca TMR2)
Trong :
TOSC l chu k ca clock h thng. TOSC = 1/FOSC.
Trong ti, ta ci t: setup_timer_2(T2_DIV_BY_4, 255, 1), do
TPWM = (PR2+1) 4 TOSC Pre-scale = 256 4 (12 106)-1 4.
fPWM = 1/ TPWM 2,93 (kHz).
Hot ng ca khi giao tip EUSART:
Khi giao tip ni tip EUSART (Enhanced Universal Synchronous
Asynchronous Receiver Transmitter) cho php cu hnh hot ng ch giao tip
ni tip ng b v khng ng b. Trong ti ny, ch giao tip khng ng b
c s dng. Do y ta s tp trung m t hot ng ca module EUSART ch
khng ng b.
Hot ng truyn:
S khi b truyn c th hin trong hnh di:
Trang 53
Hnh 5.12: S khi b truyn ca module EUSART

B phn chnh ca khi truyn l thanh ghi truyn TSR. Thanh ghi ny khng th
truy cp bng phn mm m phi truy cp gin tip qua thanh ghi m truyn TXREG.
Qu trnh truyn c khi to bng cch ghi d liu truyn vo thanh ghi TXREG.
Nu y l d liu truyn ln u tin hoc d liu truyn trc truyn hon tt th
d liu trong TXREG ngay lp tc s c truyn vo thanh ghi TSR. Nu thanh ghi
TSR vn cn cha d liu ca k t truyn trc th d liu mi trong TXREG s
c gi cho n khi bit Stop ca k t ang truyn hon tt. Sau , d liu ch trong
TXREG s c truyn vo TSR.
Hot ng nhn:
S khi b nhn c th hin trong hnh di:
Trang 54
Hnh 5.13: S khi b nhn ca module EUSART

D liu c nhn trn chn RX/DT v y vo khi Data Recovery. Khi tt c 8
hoc 9 bit ca k t nhn c dch vo, chng s ngay lp tc c chuyn vo b
m 2 k t FIFO. B m FIFO v thanh ghi RSR khng th truy cp trc tip bng
phn mm m phi truy cp gin tip thng qua thanh ghi RCREG. D liu trong b
m nhn c c bng cch c thanh ghi RCREG.
5.2 Mch cng sut (mch cu H)
Dng Module Dual H-Bridge ca HLAB (xem thm hnh 5.1), c mt s c
im sau:
Module iu khin hai ng c.

in p 12V 24V, dng ti ti a 5A.
in p iu khin 5V TTL.
H tr chn PWM tch cc mc cao hoc thp bng jumper.
Diode bo v cu H.
Led ch th chiu quay ca ng c.
Trang 55
Chng 6: ng c v gii thut PID v tr
Ni dung chnh
6.1 ng c Servo DC
6.1.1 ng c DC
6.1.2 Encoder
6.2 Gii thut PID v tr
Trang 56

6.1 ng c Servo DC
ng c Servo DC [18] l ng c khng ng b. ng c Servo DC bao gm
hai thnh phn chnh: ng c DC v encoder. Ngoi ra, ng c c th c gn thm
hp s (Gear box) c tc dng tng momen quay v gim tc ng c. ng c
Servo DC c ng dng cho vic iu khin chnh xc: gc quay, tc , momen.
Trong ti, cp ng c c chn l ng c Servo DC hiu DS48DE25 ca Nht
vi cng sut 60W, s dng ngun DC 24V, encoder 60 xung v c trang b hp s
1:5.
Hnh 6.1: Cp ng c Servo DC

6.1.1 ng c DC
ng c DC l ng c in hot ng vi dng in mt chiu. ng c in
mt chiu ng dng rng ri trong cc ng dng dn dng cng nh cng nghip. Cu
to ca ng c gm c hai phn: stato ng yn v roto quay so vi stato. Phn cm
Trang 57

(phn kch t thng t trn stato) to ra t trng i trong mch t, xuyn qua cc
vng dy qun ca phn ng (thng t trn roto). Khi c dng in chy trong mch
phn ng, cc thanh dn phn ng s chu tc ng bi cc lc in t theo phng
tip tuyn vi mt tr roto, lm cho roto quay.
Ty theo cch mc cun dy roto v stato m ngi ta c cc loi ng c sau:
ng c kch t c lp: Cun dy kch t (cun dy stato) v cun dy phn
ng (roto) mc ring r nhau, c th cp ngun ring bit.
ng c kch t ni tip: Cun dy kch t mc ni tip vi cun dy phn ng.
i vi loi ng c kch t c lp, ngi ta c th thay th cun dy kch t
bi nam chm vnh cu, khi ta c loi ng c in mt chiu dng nam chm vnh
cu.
i vi loi ng c kch t c lp dng nam chm vnh cu, thay i tc ,
ta thay i in p cung cp cho roto. Vic cp p mt chiu thay i thng kh
khn, do vy ngi ta dng phng php iu xung (PWM):
Hnh 6.2: iu chnh rng xung PWM

Mch iu khin ng c bng phng php PWM hot ng da theo nguyn
tc cp ngun cho ng c bng chui xung ng m vi tc nhanh. Ngun DC
c chuyn i thnh tn hiu xung vung (ch gm hai mc 0 volt v xp x in p
hot ng). Tn hiu xung vung ny c cp cho ng c. Nu tn s chuyn mch
ln ng c s chy vi mt tc u n ph thuc vo momen ca trc quay.
Trang 58

Vi phng php PWM ny, ta iu chnh tc ca ng c thng qua vic
iu ch rng ca xung, tc l thi gian mc cao ca chui xung vung cp cho
ng c. Vic iu chnh ny s tc ng n cng sut trung bnh cp cho ng c v
do s lm thay i tc ca ng c cn iu khin. Nh trn hnh 6.2, vi dy
xung iu khin trn cng, xung c rng nh nn ng c chy chm. Nu rng
xung cng ln (nh dy xung th 2 v th 3) ng c DC chy cng nhanh.
Do c tnh cm khng ca ng c, dng qua ng c l dng lin tc, gn sng
nh sau:
Hnh 6.3: Dng sng p v dng trn ng c
6.1.2 Encoder
Encoder l dng c dng xc nh v tr gc ca a quay hoc xc nh qung
ng di chuyn cho bnh xe, trc ng c hay bt k thit b no c c cu quay v
cn xc nh dch chuyn.
C hai loi encoder l encoder tuyt i (absolute encoder) v encoder tng i
(incremental encoder). Encoder tuyt i cung cp cho ta chnh xc v tr encoder m
khng cn qua x l, trong khi encoder tng i th i hi thm khu x l t tn
hiu encoder trc khi xc nh c v tr quay ca ng c. Vi gi thnh r hn v
vic x l cng khng qu phc tp nn encoder tng i c chn trong ti ny.
y, ta tp trung ni v incremental optical encoder l loi encoder tng i
s dng cm bin quang. H thng optical encoder bao gm mt ngun pht quang
(thng l hng ngoi infrared), mt cm bin quang v mt a c chia rnh.
Trang 59
Hnh 6.4: Optical Encoder

Encoder thng c ba knh (ba ng ra) bao gm knh A, knh B v knh Z
(Index). Trn hnh 6.4, ta thy mt l nh trn a quay v mt cp pht thu dnh
ring cho l nh ny. l knh Z ca encoder. C mi ln ng c quay c mt
vng, hng ngoi t ngun pht s xuyn qua l nh n cm bin quang v cm bin
li thu c mt tn hiu. Nh th c mi vng quay ca ng c, li xut hin mt
xung knh Z. Bn ngoi a quay c chia thnh cc rnh nh v c mt cp thu
pht khc dnh cho cc rnh ny. y chnh l knh A ca encoder, hot ng ca
knh A cng tng t knh Z, im khc nhau l trong mt vng quay ca ng c, s
c N xung xut hin trn knh A. N l s rnh trn a v cn c gi l phn gii
(resolution) ca encoder. Mi loi encoder c mt phn gii khc nhau, c khi trn
mi a ch c vi rnh nhng cng c trng hp c n hng nghn rnh c chia.
iu khin ng c, ta phi bit c phn gii ca encoder ang dng. phn
gii nh hng n chnh xc iu khin v c phng php iu khin. Ngoi ra,
trn encoder cn c mt cp thu pht khc c t trn cng ng trn vi knh A
nhng lch mt cht (lch M+0.5 rnh), y l knh B ca encoder. Tn hiu xung t
knh B c cng tn s vi knh A nhng lch pha 90o. Bng cch phi hp knh A v
knh B ta c th bit chiu quay ca ng c.
Trang 60
Hnh 6.5: Hai knh A v B lch pha trong encoder

Khi cm bin A bt u b che th cm bin B vn nhn c hng ngoi xuyn
qua v ngc li. Hnh trn l dng xung ng ra trn 2 knh. Xt trng hp ng c
quay cng chiu kim ng h, tn hiu di chuyn t tri sang phi. Lc tn hiu knh A
chuyn t mc cao xung thp (cnh xung) th knh B ang mc thp. Ngc li,
nu ng c quay ngc chiu kim ng h, tn hiu di chuyn t phi qua tri. Lc
ny, ti cnh xung ca knh A th knh B ang mc cao. Nh vy, bng cch phi
hp hai knh A v B, ta khng nhng xc nh c gc quay (thng qua s xung) m
cn bit c chiu quay ca ng c (thng qua mc ca knh B cnh xung ca
knh A).
Encoder c s dng trong ti l loi 60 xung trn mt vng quay vi ba
knh A, B v Z, tuy nhin ch cn s dng 2 knh A, B l cho ng dng iu khin
v tr.
Trang 61
Hnh 6.6: Encoder i km ng c Servo DC

Cch c encoder bng PIC:
Ta s dng ngt ngoi ca PIC 18F4550 c encoder v, y l phng php
n gin nhng chnh xc. tng ca phng php ny rt n gin, ta ni knh A
ca encoder ca ng c bn phi vi ngt ngoi (chn RB1) v knh B vi mt chn
no bt k ( y ta dng chn RD1). C mi ln ngt ngoi xy ra, tc c 1 xung
xut hin knh A th trnh phc v ngt ngoi t ng c gi. Trong trnh phc v
ngt ny ta kim tra mc ca knh B, ty theo mc ca knh B ta s tng bin m
xung ln 1 hoc gim i 1. Lm tng t vi encoder ca ng c bn tri, ni knh A
ca encoder vi ngt ngoi (chn RB2) v knh B vi mt chn no bt k (chn
RD2).
Trang 62

6.2 Gii thut PID v tr [19]
Trong cc lnh vc iu khin m ta cn iu khin u ra theo mong mun
hoc cn s n nh, chng ta cn mt thut ton iu khin v d nh iu khin ng
c, nhit , p sut, tc hay cc bin khc trong cc ng dng khc nhau. Thut
ton PID l mt trong nhng thut ton kinh in c gii thiu gii quyt cc bi
ton v iu khin. B iu khin PID c th s dng iu khin bt k bin no
min sao gi tr ca chng b tc ng thng qua thut ton PID.
Thut ton PID c th m t nh hnh 6.7. B iu khin PID s gim st u ra
ca h thng, sau so snh vi gi tr mong mun. Nu h thng cha t c gi
tr mong mun, ngha l sai s e khc khng, b PID s tnh ton ra gi tr tc ng u
mi (tc ng vo h thng) hiu chnh h thng. Qu trnh ny s c thc hin
lin tc cho n khi no e = 0, lc h thng s t trng thi n nh. H thng nh
th cn c gi l h thng iu khin vng kn (Closed Loop Control) v n c hi
tip trng thi ca h thng v b iu khin.
Hnh 6.7: PID vng kn

PID l ch vit tt ca t l - tch phn - vi phn. Hm rng b iu khin s
tnh gi tr t l, tnh tch phn v vi phn ca sai s hiu chnh h thng v trng
thi cn bng ng.
c tnh b iu khin P,I,D:
Thnh phn t l (KP) c tc dng lm tng tc p ng ca h v lm gim
(ch khng trit tiu) sai s xc lp ca h (steady state error).
Thnh phn tch phn (KI) c tc dng trit tiu sai s xc lp nhng c th lm
gim tc p ng ca h.
Trang 63

Thnh phn vi phn (KD) lm tng n nh ca h thng, gim vt l v
ci thin tc p ng ca h.
nh hng ca cc thnh phn KP, KI, KD i vi h kn c tm tt trong bng
sau:
Bng 6.1: nh hng ca cc thnh phn Kp, Ki, Kd i vi h kn

H PID c th s dng ch P, PI, PD ty vo c tnh ca h thng.
Hiu chnh t l P: KP cng ln th sai s xc lp cng nh, vt l cao, tuy
nhin ta khng th tng KP n v cng khi exl khng bng khng, KP Kgh.
Hiu chnh t l PD: Hiu chnh PD lm nhanh p ng ca h thng, gim
thi gian qu nhng li nhy vi nhiu tn s cao.
Hiu chnh t l PI: Hiu chnh t l PI lm chm p ng qu , tng
vt l, gim sai s xc lp.
Hiu chnh t l PID: Ci thin p ng qu (gim vt l, gim thi gian
qu ), gim sai s xc lp.
Kt qu ca b iu khin thng c nh gi thng qua vic o c p ng
ca b iu khin trong mt h thng nht nh nh hnh sau:
Trang 64
Hnh 6.8 p ng ca cc h thng iu khin

Hnh 6.8 m t p ng ca h thng iu khin theo s thay i v p ng ca
ba phng php iu khin P, PI, PID. Trong p ng ca b PID l tt nht vi
vt l t v thi gian n nh nhanh.
B PID c th lm vic trn d liu lin tc hoc dng ri rc. Hin nay, PID ri
rc c ng dng rng ri trong cc h iu khin do hu ht cc h thng hin nay
u l h thng s. Cng thc b PID c th c m t nh sau:
n
u n K p e n Ki e k K d ( y n y n 1)
k 0
Trang 65

Qu trnh tnh ton ca b PID:
Hnh 6.9: Qu trnh tnh ton PID

B iu khin PID c hm truyn dng lin tc nh sau:
G( s) K p
KI
KD s
s
Bin i Z ca n nh sau:
G( z ) K P
KI T z 1 KD z 1
(
)
(
)
2
z 1
T
z
Vit li G(z) nh sau:

G( z )
(KP KI
K
K
T KD
T
) ( K P K I 2 D ) z 1 D z 2
2 T
2
T
T
1
1 z
t:
T KD
2 T
K
T
a1 K P K I 2 D
2
T
K
a2 D
T
a0 K P K I
Suy ra:
Trang 66

a0 a1 z 1 a2 z 2
G( z )
1 z 1
T , ta tnh c tn hiu iu khin u(k) khi tn hiu vo e(k) nh sau:

u (k ) G( z ) e(k )
a0 a1 z 1 a2 z 2
e(k )
1 z 1
Suy ra:
u(k ) u(k 1) a0 e(k ) a1 e(k 1) a2 e(k 2)
Trong : T l thi gian ly mu

e(k) l sai s hin ti: e(k) = gi tr t gi ng ra
e(k-1) v e(k-2) l cc sai s tch ly
u(k) l ng ra b iu khin
KP, KI, KD l cc thng s P, I, D ca b iu khin
Cch iu chnh cc h s KP, KI, KD:
u tin, ta chnh cho KP = 1 v KD, KI = 0 ngha l ch iu khin P. Sau ,
tng KP ln dn dn v quan st ng c. Khi thy ng c dao ng (ngha l n lc
nhanh lc chm), ta ly KP ti nhn vi 0.6 tnh ton: KP_t = 0.6 KP_dao_ng. Sau
, ta tng KD ln dn dn (gi nguyn KP), nu KD qu ln s lm cho ng c b dao
ng mnh. Gim KD li cho n khi ng c ht dao ng. iu chnh KI l kh nht,
bt u t gi tr rt nh (v d ly KI = 1/KD chng hn). H s KI khng cn ln v
ng c t n cha thnh phn KI, th hin moment qun tn hay sc ca ng
c. Do , thng vi i tng iu khin l nhit hay ng c (cc i tng c
qun tnh) th ch cn b iu khin PD l .
Trang 67
Chng 7: Tnh ton ta Robot v Kinect
Ni dung chnh
7.1 Cc php chuyn i h trc ta c bn
7.2 Tnh ton ta robot
7.3 Tnh ton ta Kinect
Trang 68

7.1 Cc php chuyn i h trc ta c bn
Trc khi i vo cc php bin i, ta lm quen vi khi nim h ta thun
nht. Trong h ta thun nht, mi im (x, y, z) trong h ta Descartes c
biu din bi mt b bn gi tr trong khng gian bn chiu thu gn (hx, hy, hz, h).
tin li cho tnh ton, ngi ta thng chn h = 1. Nh vy, mt im (x, y, z) trong h
ta Descartes s bin thnh im (x, y, z, 1) trong h ta thun nht; cn im
(x, y, z, w) trong h ta thun nht (vi w 0) s tng ng vi im (x/w, y/w,
z/w) trong h ta Descartes [20].
Php bin i ba chiu bin im P thnh im Q c dng: Q = P.M. Trong :
Q = (x, y, z, 1), P = (x, y, z, 1), d = (dx, dy, dz) l vector tnh tin v M l ma trn
bin i 4x4 trong h ta thun nht:
a b c
d e
f
M
g h i
dx dy dz
0
0
0
Php tnh tin:
Hnh 7.1: Php tnh tin
Trang 69

Vector tnh tin trong php bin i ba chiu c mt tc ng rt trc quan: mi
im c dch i mt khong l dx, dy, dz theo ba trc. Ma trn M cho php tnh tin
c dng nh sau:
1 0 0
0 1 0
M
0 0 1
dx dy dz
0
0
, Q = [x y z 1] = P.M = [x+dx y+dy z+dz 1].
0
Php quay:
Ta kho st php quay quanh mt trc ta . Khc vi php quay trong hai chiu
quanh mt im bt k, trong ba chiu ta c php quay quanh mt trc ta . Ta c
cc ma trn biu din cc php quay quanh trc x, y, z ngc chiu kim ng h vi
gc tng ng , , ln lt l Rx(), Ry(), Rz():
Hnh 7.2: Php quay

0
1
Rx ( ) 0 cos
0 sin
cos
Rz ( ) sin
0
0
sin
cos
sin
cos
0
cos 0 sin
R y ( ) 0
1
0
sin 0 cos
0
0
1
7.2 Tnh ton ta robot

d hnh dung, ta quan st m hnh robot c v trn Autodesk Inventor 2012.
Vi thit k vi bn bnh gip robot gi thng bng tt, hai bnh sau lm nhim v
Trang 70

truyn ng, ta t gc ta robot ti v tr chnh gia hai tm cc bnh ny (gi l
tm robot).
Hnh 7.3: M hnh robot

Quan st hnh chiu bng ca m hnh robot hnh 7.4, ta gc ca robot
c chn l tm O(0, 0). V tr tng ng ca tm hai bnh xe tri v phi l
R(20.5, 0), L(-20.5, 0) (n v l centimet). Trong qu trnh di chuyn, ta ca
robot s c cp nht so vi v tr lc khi ng khi robot n mt v tr mi. Cc gi
tr cp nht bao gm: ta x hin ti (x_cur), ta y hin ti (y_cur) v gc hin
ti (trc OY so vi trc OX, angle_cur ). Nh vy, ti thi im bt u khi ng:
x_cur = 0, y_cur = 0 v angle_cur = 90o.
Trang 71
Hnh 7.4: Ta robot (quan st t trn xung)

Khi di chuyn robot s c hai ng tc chnh: i thng (tin hoc li) mt on L
v xoay mt gc anpha quanh tm robot. Bi ton ch n thun p dng cc php
tnh tin v xoay c bn trong khng gian 2D. Phng trnh tnh ton ta mi ca
robot cho tng ng tc:
i thng mt on L:
x _new = x_cur L.cos(angle_cur)
y _new = y_cur L.sin(angle_cur)
angle_new = angle_cur
Du + cho trng hp tin v - cho trng hp li.
Xoay mt gc anpha:
x_new = x_cur
y_new = y_cur
angle_new = angle_cur + anpha
Trang 72

anpha m khi robot xoay sang phi (thun chiu kim ng h) v dng khi
xoay sang tri (ngc chiu kim ng h). Gi tr angle_cur nm trong khong
02*PI nn lu khi gc ny nm ngoi khong ny th:
angle_cur < 0 : angle_cur = angle_cur + 2*PI
angle_cur > 2*PI : angle_cur = angle_cur - 2*PI
7.3 Tnh ton ta Kinect
Hnh 7.5: H trc ta Kinect

Thng tin v mi trng trong qu trnh di chuyn ca robot c Kinect a
v x l dng 3D nhm a ra nhng lnh truyn cho robot mt cch chnh xc nht.
Vi s h tr mnh m ca th vin Point Cloud, Kinect cho ta nhng thng tin chnh
xc v ta cc im hay vt th trong mi trng thc t so vi ta gc ti
Kinect. Mt nhc im ln ca Kinect l vng m (Kinect khng th nhn thy)
nm trong khong 00.50 mt, l mt khong cch ln i vi chc nng trnh vt
Trang 73

cn ca robot. Chnh v l do m ta b tr Kinect c t trn cao v hng xung
vi mt gc nghing nht nh, ng thi c t li pha sau robot nhn c mt
cch tng quan nht mi trng pha trc robot.
Chnh iu ny t ra bi ton chuyn di h trc ta ca Kinect v h trc
ta ca robot ng b ha. C cht khc bit gia hai h trc: i vi h trc ta
Kinect, phng Z i din cho khong cch ti vt th, phng Y i din cho
chiu cao vt th ln lt tng ng vi phng Y v Z ca h trc ta robot.
Phng X cn li khng c g thay i.
Mc ch ca ta l a mt phng OXZ ca Kinect v trng vi mt phng OXY
ca robot. Quan st hnh 7.6, ta thy ngoi vic nghing mt gc 30o, Kinect cn lch
theo chiu Z mt on 9 cm v chiu Y mt on 60 cm.
Hnh 7.6: ng nht h trc ta Kinect v robot

Ta ln lt thc hin cc cng vic sau i vi h trc Kinect:
Xoay mt phng OYZ quanh trc X ngc chiu kim ng h mt gc 30o.
X1
Y X
1
Z1
0
1
Z 0 cos
0 sin
0
X
sin Y cos Z sin

cos Y sin Z cos
Trang 74

Tnh tin trc Y xung mt on 60 cm v tnh tin trc Z v sau mt on 9
cm.
X
X 2 X1 0
Y Y 60 Y cos Z sin 60
2 1

Z 2 Z1 9 Y sin Z cos 9
o chiu Y.
X
X3 X2
Y Y Y cos Z sin 60
3 2
Z3 Z 2 Y sin Z cos 9
Hnh 7.7 cho ta thy h trc Kinect sau khi thc hin cc php chuyn i. H
trc c cc chiu X, Y, Z tng ng vi cc mu , xanh l, xanh dng.
Hnh 7.7: Ta Kinect trc (tri) v sau (phi) khi chuyn trc
Trang 75
Chng 8: Chng trnh iu khin
Ni dung chnh
8.1 Ni dung chng trnh iu khin
8.2 Gii thut chng trnh do my tnh x l
8.3 Gii thut chng trnh do vi iu khin x l
Trang 76

8.1 Ni dung chng trnh iu khin
Hnh 8.1: Ni dung chng trnh iu khin

Hnh 8.1 khi qut cho ta cc khi chnh v nhim v ca cc khi. My tnh kt
hp Kinect cng cc cng c lp trnh to nn khi s 1, mch vi iu khin v mch
cng sut to nn khi s 2, khi s 3 l m hnh c kh ca mobile robot.
Khi 1 ng vai tr nh cp mt v b no ca robot, gip robot phn tch khng
gian pha trc v truyn lnh thch hp xung khi s 2. Khi 2 s thc hin iu
khin robot (khi 3) theo ng yu cu ca khi 1, cc ng tc nh quay tri, phi
hay chy tin, li vi mt gi tr xc nh. Sau khi tnh ton ta t tn hiu phn hi
encoder trn robot, gi tr v tr hin ti s c khi 2 truyn li cho khi 1 cp nht
v hin th.
8.2 Gii thut chng trnh do my tnh x l
My tnh thc hin cng vic iu khin bng ba lung x l song song: x l
Kinect, x l k hoch di chuyn ca robot v x l giao tip vi vi iu khin.
Trang 77
Hnh 8.2: X l a tin trnh

Lung Kinect (s 1) lm nhim v phn tch mi trng, a ra cc thng tin v
kch thc, v tr vt cn nu xut hin trc robot, c trnh by mc 4.3.
Lung giao tip vi vi iu khin (s 3) theo chun RS232 lm nhim v truyn
lnh xung v nhn thng tin v tr ln t vi iu khin.
Lung x l k hoch di chuyn ca robot s dng thng tin t lung s 1 v
lung s 3, iu khin robot di chuyn hp l v ch.
Hnh 8.3: Giao din chng trnh iu khin

Hnh 8.3 l giao din iu khin vit bng MFC trong b Visual Studio 2010 ca
Microsoft. V tr ch cn di chuyn n s c nhp vo khi Target Coordinate;
ta hin ti ca robot s t ng hin th trong khi Current Coordinate; ty chn
Trang 78

cc thng s cho giao tip RS232, trng thi cng giao tip c t trong khi Serial
Control; trong khi Robot Status hin th cc thng tin v c pht hin vt cn, c
pht hin ng trng, s lng vt cn v trng thi robot v tr ch hay cha.
Tilt Control iu khin gc ngng Kinect.
nh dng iu khin qua giao tip RS232
Lnh iu khin truyn xung qua giao tip ni tip c nh dng theo mu
sau: *[k t iu khin][gi tr iu khin]#, trong :
k t iu khin: R(quay phi), L(quay tri), F(i thng), T(i thng v

ch).
gi tr iu khin: l gi tr gc quay hay qung ng di chuyn, nh

dng kiu d liu double, ly ba ch s thp phn, n v l centimet.
*, #: l cc k t c bit cho bit k t bt u v kt thc chui.
Lnh nhn ln t vi iu khin c dng: X[gi tr 1]Y[gi tr 2]A[gi tr 3]N,

trong :
X, Y, A: ln lt l k t bt u cho cc gi tr i din ta X, Y, gc
hin ti ca robot. N l k t kt thc chui.
gi tr 13: tng ng l gi tr ta , gc hin ti ca robot.
Gii thut iu khin robot ng x vi vt cn trn ng i n ch:
Trang 79

Start
Yes
Khi to cc gi tr
u
Tnh gc quay
v ch
Gi lnh quay
Tnh gc quay
ti u trnh vt
Gi lnh quay
Robot thc
hin xong?
No
i mt on an ton
Yes
No ng trng?
No
Robot thc
hin xong?
No
Robot thc
hin xong?
Yes
Yes
Yes
No
Gi lnh di chuyn
v ch
ng trng? Yes
Robot thc
hin xong?
No
i mt on an ton
Tnh khong cch

ti ch D
khng
gian cho robot
lch qua?
Yes
Yes
No
Yes
D < 1.3 mt &

ng trng?
No
No
Tnh gc quay
ngc li trnh vt
Gi lnh quay
C vt cn? Yes
No
No
No
End
Tnh gc quay song song

vi hng ban u
Gi lnh quay
Robot thc
hin xong?
Yes
No
Yes
Robot thc
hin xong?
ng trng? Yes
Robot thc
hin xong?
Yes
Robot thc
hin xong?
i mt on an ton
No
Tnh khong cch di chuyn
sao cho cch vt 55 cm
Gi lnh di chuyn
No
Robot thc
hin xong?
Yes
Yes
No
Robot thc
hin xong?
Tnh gc quay trnh vt

theo hng c
Gi lnh quay
Hnh 8.4: S gii thut iu khin robot do my tnh x l
Trang 80

Mt s hm ch :
Tnh gc quay v ch:
Hnh 8.5: Tnh gc quay v ch

Hnh 8.5 m t mt trng hp ch nm bn phi robot, lc ny robot s quay
mt gc anpha v bn phi hng v ch. Ta c cch tnh cho cc trng hp nh
sau (ta gi s y_cur lun dng, robot lun di chuyn v pha trc):
gamma = arctan(
y_target - y_cur
)
x_target - x_cur
gamma < 0 (ch nm bn tri robot):
0 angle_cur < PI + gamma:

+ anpha = PI angle_cur + gamma
+ Quay v bn TRI robot
PI + gamma angle_cur < 2*PI + gamma:

+ anpha = angle_cur (PI + gamma)
Trang 81

+ Quay v bn PHI robot
2*PI + gamma angle_cur 2*PI:

+ anpha = 3*PI - angle_cur + gamma
gamma 0 (ch nm bn phi robot):
0 angle_cur < gamma:

+ anpha = gamma - angle_cur
gamma angle_cur < PI + gamma:

+ anpha = angle_cur - gamma
+ Quay v bn PHI robot
PI + gamma angle_cur 2* PI:

+ anpha = 2*PI - angle_cur + gamma
Tnh gc quay ti u trnh vt:

Mc ch l iu khin robot theo hng c gc quay nh hn khi trnh vt cn.
Cc trng hp c th xy ra l:
Trang 82
Vt cn nm bn tri robot
Hnh 8.6: Vt cn bn tri robot

Quan st hnh 8.6, gc quay cn thit thot khi vt cn l anpha( )
khi robot v tr cch vt cn mt on. Gc anpha hon ton tnh c
khi ta bit c ta max_point, gc v ln OL.
cos1(
x _ max
) , hng quay v pha bn phi robot.
O'L
Trang 83
Vt cn nm bn phi robot
Hnh 8.7: Vt cn bn phi robot

Tng t trng hp vt cn nm bn tri, gc anpha c tnh nh sau:
cos1( x _ min/ O ' L) , hng quay v pha bn tri robot.
Trang 84
Vt cn nm gia ng robot
Hnh 8.8: Vt cn nm gia ng di chuyn ca robot

Trong trng hp ny, robot s tm gc quay n vt sao cho anpha nh
nht; lc , nu ln vt cn nhiu hn v pha bn tri th robot quay
v bn phi (nh trn hnh 8.8) v ngc li. Cng thc tnh trong tng
trng hp: (trong gc v OL = OL bit trc)
Quay tri:
sin 1 (
x _ min
)
O'L
Quay phi:
sin 1 (
x _ max
)
O'L
Trang 85

Tnh gc quay ngc li trnh vt:
Nh cc trng hp quay mt gc xc nh n vt cn, nhng thay v quay theo
hng c gc quay nh hn th robot s quay theo hng ngc li. Cng thc tnh
hon ton tng t.
i mt on an ton:
Sau khi quay mt gc trnh vt, robot s di chuyn tip mt on thot hon
ton khi vt.
Hnh 8.9: i mt on an ton v pha phi vt cn

Qung ng di chuyn an ton thot khi vt cn bng on OH cng thm
mt gi tr c nh va deltaMove( y chn deltaMove = 15 cm da trn thc
nghim ).
Trang 86

i bn phi vt cn:
OH ' OM cos( )
1
( x _ max x _ cur)2 ( y _ max y _ cur)2 cos( tan 1(
x _ max
))
y _ max
i bn tri vt cn:
OH ' OM cos( )
1
x _ min
( x _ min x _ cur)2 ( y _ max y _ cur)2 cos( tan 1(
))
y _ max
C bo c vt cn (Obstacle_flag) v ng trng (freePath_flag):
Hnh 8.10: C bo c vt cn v ng trng

Hnh 8.10 biu din khng gian xut hin vt cn pha trc robot, lc cc c
tng ng s bt ln trong cc vng xc nh. Tm nhn xa ca robot gii hn
khong cch 140 cm.
Trang 87

C bo khng gian cho robot lch qua:
Hnh 8.11: Khng gian cho robot lch qua

C bo khng gian cho robot lch qua s bt ln 1 khi khong cch H1H2 (hnh
8.11) ln cho robot lt qua (khng gian cho robot lt qua khi H1H2>50 cm).
Trong OH1 c tnh nh trong hm i mt on an ton, OH2 tng t, ta c:
O ' H 2 O ' M 4 cos( )

( x4 x _ cur) 2 ( y4 y _ cur) 2 cos(tan 1 (
x4
y4
))
H H O'H O'H
1 2
2
1
8.3 Gii thut chng trnh do vi iu khin x l
Cng vic iu khin ng c v tnh ton ta , thc hin giao tip vi my
tnh s c PIC 18F4550 x l. thc hin cc cng vic ny, PIC18F4550 kt hp
thng tin lnh t my tnh v tn hiu hi tip v t encoder thng qua cc ngt ngoi.
Sau y l s khi m t hot ng ca chng trnh chnh x l bi PIC18F4550:
Trang 88

Start
Khi to cc module
Khi to cc module:
Khi to cc module s dng trn PIC: cc
port I/O, timer, ngt ngoi, PWM v giao
tip RS232.
No
Timer1_flag = 1
Timer1_flag:
C bo bng 1 khi timer1 trn sau 5 ms,
Yes
No
InitPID_flag = 1
Yes
Khi to cc gi tr u
cho PID
InitPID_flag = 0
donePID_flag = 0
No
phc v cho vic ly mu, tnh ton PID.

InitPID_flag:
C bo khi to PID, bng 1 khi nhn
c lnh t my tnh.
Khi to cc gi tr u cho PID:
Khi to cc gi tr ban u cho tnh ton
PID: cc thng s kp, ki, kd; gi tr t.
Target_flag = 1
Yes
Tnh ton ta robot
Gi thng tin ln my tnh
Target_flag = 0
Target_flag:
C bo ln 1 khi robot ang di chuyn v
ch v gp vt cn.
Tnh ton ta robot:
Tnh ton ta robot, c trnh by
No
donePID_flag = 1
Yes
Tnh ton ta robot

Gi thng tin ln my tnh
donePID_flag = 0
ti mc 7.2.
Gi thng tin ln my tnh:
Thng tin v ta v gc hin ti ca
robot: (x_cur, y_cur, angle_cur).
Thc hin PID trn hai bnh xe:
Thc hin PID cho 2 bnh

xe (khi PID thc hin xong,
donePID_flag = 1)
Timer1_flag = 0
Da trn gii thut PID v tr c trnh

by mc 6.2. PID c thc hin xong
khi sai s nm trong khong cho php (xp
Hnh 8.12: S gii thut trn

vi iu khin
x bng khng), lc donePID_flag bng

1.
Trang 89
Kt lun v hng pht trin
KT LUN V HNG PHT TRIN

Vi mc ch xy dng m hnh robot c kh nng di chuyn n im ch xc
nh trc, trnh chng ngi vt v n ch an ton. Nhm p dng cng ngh x
l nh 3D cho phn th gic ca robot vi thit b chi game Kinect, p ng c
tin cy cao hn so vi cc cm bin truyn thng. Qu trnh thc hin hon thnh
nhng nhim v sau:
-
Thit k v thi cng m hnh mobile robot hon chnh.
Chng trnh x l nh khi phc c khng gian pha trc robot di dng
3D, cung cp y thng tin v mi trng cho robot.
Tc x l nh trn my tnh khong 13-15 fps, p ng thi gian thc cho

robot.
Hon thnh k hoch di chuyn n ch c vt cn vi sai s chp nhn c

mi trng trong nh.
Mt s im cn khc phc:
Phn c kh ca mobile robot khng c hon ho nn nh hng n sai s

khi di chuyn. im ny, mt con robot omi s p ng y cho ng dng
robot di chuyn trong nh vi s linh hot, kt cu gn nh v s chnh xc cao.
Trang 90
Kt lun v hng pht trin

Mt con robot rt ng quan tm l iRobot vi cc la chn khc nhau, gi dao
ng t 130$ 300$, chi tit xem ti: http://store.irobot.com
-
Phn th gic robot ch quan st c khng gian pha trc robot nn robot c
th s ng vt cn bn hng hay pha sau khi di chuyn li. Vn ny c th
c gii quyt bng bic trang b thm cc cm bin pht hin vt cn xung
quanh robot; hoc thit k thm h thng xoay cho phn th gic, th gic s xoay
mt gc xc nh trc khi quyt nh di chuyn.
Tch hp thm cc cm bin nh v cn thit tng s chnh xc.
Chng trnh c vit trn mi trng Windows vi s h tr t th vin Point

Cloud cn nhiu hn ch, nht l tc x l. S ti u nht nu vit trn mi
trng Linux v nu pht trin thnh sn phm thng mi s h c gi thnh
phn mm xung, tng tnh cnh tranh.
Hng pht trin:

-
Mc ch ca ti to ra nn tng cho vic xy dng mt m hnh robot dch

v hon chnh: mt robot c kh nng lm cc cng vic thay cho con ngi
nh: bng b n, lau nh, hng dn khch hng,
Bn cnh , vi sc mnh ca thit b Kinect c th gip ta xy dng c bn

(mapping) dng 3D. V vic tch hp ln robot s gip ta xy dng c bn
nhng khu vc m con ngi khng th vo c.
Trang 91
Ti liu tham kho
TI LIU THAM KHO

[1] http://en.wikipedia.org/wiki/Kinect
[2] http://www.ros.org/wiki/kinect_calibration/technical
[3] Mikkel Viager, Analysis of Kinect for Mobile Robots, Technical University of
Denmark, p. 11
[4] http://www.primesense.com/en/technology
[5] Jacob Kjr. A Qualitative Analysis of Two Automated Registration Algorithms In
a Real World Scenario Using Point Clouds from the Kinect. June 27, 2011.
[6] http://nicolas.burrus.name/index.php/Research/KinectCalibration
[7] http://openkinect.org/wiki/Main_Page
[8] http://codelaboratories.com/nui
[9] http://openni.org/Documentation
[10] http://kinectforwindows.org
[11] http://www.brekel.com/?page_id=671
[12] http://pointclouds.org/documentation
[13] http://www.dientuvietnam.net/forums. Hin tng crosstalk.
[14] http://www.roborealm.com/tutorial/Obstacle_Avoidance
[15] Radu Bogdan RUSU. PointCloud(2) processing in ROS. May 2, 2010
[16] http://en.wikipedia.org/wiki/RANSAC
[17] http://tme.com.vn/Product.aspx?id=826&CateId=114#page=pro_info
[18] http://www.hocavr.com/index.php/app/dcservo
[19] John A. Shaw, The PID Control Algorithm , 2003
[20] http://www.uit.edu.vn/data/gtrinh/TH109/Htm/Chuong6.htm
[21] Erick Ball, Greg Taschuk. Reverse Engineering the Kinect Stereo Algorithm
[22] Michael YingYang, Wolfgang Forstner. Plane Detection in Point Cloud Data.
January 25, 2010.
Trang 92
Ph lc 1
PH LC 1: Kt hp th vin OpenNI v Code Laboratories Kinect (CL)

s dng chc nng iu khin ng c Kinect.
Nh trnh by trong mc 3.2, th vin OpenNI khng h tr cho vic iu
khin ng c ca Kinect. Sau y l th thut bng vic ci t thm CL. C OpenNI
v CL u ng vai tr nh driver truy xut phn cng Kinect nn s xung t nu ci
t ng thi hai th vin ny.
Bc 1:
Ci t CL bnh thng v m bo cha ci OpenNI v cc chng trnh i km
trc (nu c th phi g b).
CL c th ti v ti: http://codelaboratories.com/nui
Bc 2:
M ca s Computer Management, v chn Uninstall cc mc di CL devices:
NUI audio, NUI camera, NUI Motor.
Trang 93
Ph lc 1
Bc 3:
Ci t OpenNI v cc chng trnh km theo.
Bc 4:
Vo li ca s nh bc 1, lc ny Kinect c nhn bi PrimeSense. Ta nhp
phi chut trn Kinect Motor v chn Update Driver Software
V duyt ti th mc driver ca CL:
Trang 94
Ph lc 1
Lc ny ta c th s dng chc nng iu khin ng c Kinect trn th vin
OpenNI kt hp vi CL.
Trang 95
Ph lc 2
PH LC 2: Cch u dy dng pin 12V thay adapter v to gn ln

robot cho Kinect
Cch u dy dng pin 12V thay adapter cho Kinect
to s linh ng cho robot, ta khng dng ngun adapter m thay bng ngun
pin. u tin ct dy adapter ra:
Sau dng u cm DC cho pin v on dy trn:
Trang 96
Ph lc 2
To cho Kinect gn ln robot

y l bc quan trng nhm gi s an ton cho Kinect v trnh b rung khi di
chuyn (s nh hng ti kt qu x l nh trn my tnh).
u tin tho phn gn trn Kinect:
Trang 97
Ph lc 2
Sau khi tho:
Khoan bn l v gn bn c t trong ra:
V kt qu cui cng:
Trang 98
Ph lc 3
PH LC 3: Kch thc robot

Gm cc kch thc c bn cc chiu trn cc hnh chiu ng, bng v cnh. n v
l milimet.
Trang 99

Robot Do Đường Va Tranh Vật Cản Sử Dụng Thiết Bị Kinect

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Robot Do Đường Va Tranh Vật Cản Sử Dụng Thiết Bị Kinect

Uploaded by

Copyright:

Available Formats

I HC QUC GIA THNH PH H CH MINH

TRNG I HC BCH KHOA

ROBOT T HNH TRNH VT CN S DNG

GVHD: PGS. TS. Hong nh Chin

- Tp. H Ch Minh, Thng 1-2012 -

CNG HA X HI CH NGHA VIT NAM

NHIM V LUN VN TT NGHIP

1. u lun vn: Robot t hnh trnh vt cn s dng thit b Kinect

PHN DNH CHO KHOA, B MN:

TRNG H BCH KHOA

CNG HA X HI CH NGHA VIT NAM

PHIU CHM BO V LVTN

6. Nhng thiu st chnh ca LVTN:

TRNG H BCH KHOA

CNG HA X HI CH NGHA VIT NAM

PHIU CHM BO V LVTN

15. Nhng thiu st chnh ca LVTN:

Li u tin, chng em xin gi n Thy, PGS. TS. Hong nh Chin li cm

H Ch Minh, ngy 8 thng 1 nm 2012

X l nh vi mt camera (Monocular vision)......................................... 26

X l nh vi hai camera (Stereo vision) ................................................. 29

4.3 Pht hin vt cn s dng Kinect ......................................................................... 31

Analog Digital Converter

Fast Library for Approximate Nearest Neighbors

General Purpose Input Ouput

Hard Disk Drive

Java Native Access

Microsoft Foundation Class Library

Quarter Video Graphics Array

Random Access Memory

Software Development Kit

Synchronous Serial Port

Unmanned Arial Vehicles

Video Graphics Array

Hnh 4.17: Object cluster............................................................................................. 38

Chng 1: Gii thiu

Chng 1: Gii thiu

Chng 1: Gii thiu

Chng 1: Gii thiu

Chng 2: Tm hiu v Kinect

Chng 2: Tm hiu v Kinect

Hnh 2.1: Thit b Kinect

Chng 2: Tm hiu v Kinect

2.2 Nhng thnh phn chnh ca Kinect

Hnh 2.2: Nhng thnh phn chnh ca Kinect

Chng 2: Tm hiu v Kinect

Hnh 2.3: ng c iu khin gc ngng Kinect

Hnh 2.4: Bn trong Kinect: RGB, IR camera v IR projector

Chng 2: Tm hiu v Kinect

Hnh 2.5: Qu trnh thu v bn su nh

Chng 2: Tm hiu v Kinect

Hnh 2.6: Mu hnh c chiu bi projector v chp li bng IR camera

Chng 2: Tm hiu v Kinect

Hnh 2.7: Tnh ton khong cch ti mt im chiu t Projector [5]

Chng 2: Tm hiu v Kinect

Trong d l con s nguyn biu din di dng 11 bit, tc khong thay i t

Chng 2: Tm hiu v Kinect

Chng 2: Tm hiu v Kinect

Hnh 2.8: Kinect adapter

My tnh dual-core, tc 2.66 GHz hoc nhanh hn.

Windows 7 vi h tr ca DirectX 9.0 tr ln.

Ram ti thiu 2 GB.

Lp trnh trn Visual Studio 2010.

+ Khng h tr vic thu nh trc tip t IR camera.