Robot dò đường và tránh 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 CNG HA X HI CH NGHA VIT NAM
Thnh ph H Ch Minh 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: NGUYN HNG C MSSV: 40700566
H v tn: NGUYN VN C MSSV: 40700577
Ngnh: VIN THNG 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:

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)

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)

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)

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)

i

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
ii

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
iii

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
iv

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

v

Danh mc t vit tt
A

ADC Analog Digital Converter
AGV Autonomous Guided Vehicles
API Application Programming Interface
AUV Autonomous Underwater Vehicles
AUX AUXiliary

C

CCP Capture/Compare/PWM
CL Code Laboratories
CMOS Complementary Metal Oxide Semiconductor
CNC Computerized Numerical Control
CPU Central Processing Unit

E

ECCP Enhanced Capture/Compare/PWM
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
vi

I

ICD In Circuit Debugger
ICSP In Circuit Serial Programming
IR Infrared

J

JNA Java Native Access
JNI Java Native Interface

M

MFC Microsoft Foundation Class Library
MSSP Master Synchronous Serial Port
MUX Multiplexer

N

NI Natural Interaction
NUI Natural User Interface

Q

QVGA Quarter Video Graphics Array

R

RAM Random Access Memory
RANSAC RANdom SAmple Consensus
RGB Red, Green, Blue

S

SDK Software Development Kit
vii

SSP Synchronous Serial Port
SXGA Super eXtended Graphics Array

T

TOF Time Of Flight
TTL Transistor Transistor Logic

U

UAV Unmanned Arial Vehicles
USART Universal Synchronous Asynchronous Receiver Transmitter
USB Universal Serial Bus

V

VGA
VTK
Video Graphics Array
Visualization Toolkit

viii

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
ix

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
x

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

xi

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

Trang 1

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
Chng 1: Gii thiu

Trang 2

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,
Chng 1: Gii thiu

Trang 3

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,

Chng 1: Gii thiu

Trang 4

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 5

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 6

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 7

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 8

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 9

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 10

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 11

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 12

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:
1
-0.0030711016 d + 3.3309495161
z =

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 13

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
IR
camera
Field of View
(degrees)
Horizontal ~62
o
~58
o
Vertical ~48
o
~44
o
Diagonal ~72
o
~69
o
Focal length (pixels) 525 580
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 14

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 15

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 16

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 17

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 18

+ 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 19

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 20

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 21

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 22

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 23

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 24

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 25

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 26

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 27

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 28

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 29

+ 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 30

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 31

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 32

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 33

Point cloud:

Hnh 4.12: RGB point cloud
Point cloud P l mt tp hp cc phn t p
i
, mi phn t ny s cha tp cc gi
tr biu din khng gian nD (thng th n = 3) [15]:

Bn cnh thng tin d liu l XYZ, mi im p
i
cn c th cha thm cc thng
tin khc nh: RGB colors, intensity values,
Mi gi tr x
i
, y
i
, z
i
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:
{ } { }
1 2
, ,..., ,..., , , ,
i n i i i i
p p p p p x y z P = =

Trang 34

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 35

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 36

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 37

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 38

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 39

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 40

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 41

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 42

Chn Hng

M t chc nng v cc c tnh

AN0-AN12 I 13 knh Input c tc dng l cng bin i ADC
Avdd

Ngun dng cho module ADC
Avss

GND cho module ADC
CLKI I Li vo ca xung Clock ngoi, lun kt hp vi chn OSC1
CLKO O
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
CN0 - CN7
I Khai bo thay i li vo
CN17 - CN18
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 I Li vo d liu ni tip
CSDO O Li ra d liu ni tip
C1RX I Cng nhn bus CAN1
C1TX O 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 Vo ra d liu knh th cp
IC1 - IC8 I Cc cng vo ca module Capture
INT0 - INT2 I Cc ngt ngoi
LVDIN I Cng vo pht hin st th
/MCLR I Chn Reset, mc tch cc thp

Trang 43

OSC1 I
Li vo b giao ng tinh th. B m Trigger Schmitt c
s dng khi cu hnh trong ch RC
OSC2 O Li ra b dao ng tinh th
PGD I/O Vo ra d liu ca ICSP
PGC I Li vo Clock ca ICSP
RA0 - RA6 I/O Port A
RB0 - RB7 I/O Port B
RC0 - RC7 I/O Port C
RD0 - RD7 I/O Port D
RE0 - RE3 I/O Port E
SCK1 I/O Vo ra Clock ng b ca khi SPI1
SDI1 I Li vo d liu ca khi SPI1
SDO1 O Li ra d liu ca SPI1
SS1 I 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 O Li ra b dao ng tinh th cng sut thp 32Khz
SOSCI I Li vo b dao ng 32Khz
T1CK I Li vo xung Clock ngoi ca Timer1
T2CK I Li vo xung Clock ngoi ca Timer2
U1RX I Cng nhn khi UART1
U1TX O Cng pht khi UART1
U1ARX I Cng nhn m rng khi UART1
U1ATX O Cng pht m rng khi UART1
VDD

Chn ngun dng ca PIC
VSS

Chn GND
Vref+ I Li vo Vref+ (cao) chun ca ADC
Vref- I Li vo Vref- (thp) chun ca ADC
Bng 5.1: Bng m t cc chc nng tng chn ca PIC18F4550

Trang 44

Hnh 5.4: S khi ca PIC 18F4550


Trang 45

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 46

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 47

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 48

Hnh 5.6: Cu to ca chn GPIO
Ngt ngoi trn cc chn RB:

Trang 49

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 50

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 51

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 52

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 53

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:

Trong :
T
OSC
l chu k ca clock h thng. T
OSC
= 1/F
OSC.

Trong ti, ta ci t: setup_timer_2(T2_DIV_BY_4, 255, 1), do
T
PWM
= (PR2+1) 4 T
OSC
Pre-scale = 256 4 (12 10
6
)
-1
4.
f
PWM
= 1/ T
PWM
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:
Chu k PWM = [(PR2)+1] 4 T
OSC
(gi tr Pre-scale ca TMR2)


Trang 54

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 55

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 56

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 57

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 58

(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 59

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 60

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 90
o
. Bng cch phi hp knh A v
knh B ta c th bit chiu quay ca ng c.

Trang 61

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 62

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 63

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 (K
P
) 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 (K
I
) c tc dng trit tiu sai s xc lp nhng c th lm
gim tc p ng ca h.

Trang 64

Thnh phn vi phn (K
D
) lm tng n nh ca h thng, gim vt l v
ci thin tc p ng ca h.
nh hng ca cc thnh phn K
P
, K
I
, K
D
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: K
P
cng ln th sai s xc lp cng nh, vt l cao, tuy
nhin ta khng th tng K
P
n v cng khi e
xl
khng bng khng, K
P
K
gh
.
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 65

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:
( ) ( ) ( ) ( ) ( )
0
( 1 )
n
p i d
k
u n K e n K e k K y n y n
=
= + +


Trang 66

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:
( )
I
p D
K
G s K K s
s
= + +
Bin i Z ca n nh sau:
1 1
( ) ( ) ( )
2 1
I D
P
K T K z z
G z K
z T z
+
= + +

Vit li G(z) nh sau:
1 2
1
( ) ( 2 )
2 2
( )
1
D D D
P I P I
K K K T T
K K K K z z
T T T
G z
z

+ + + + +
=

t:
0
1
2
2
2
2
D
P I
D
P I
D
K T
a K K
T
K T
a K K
T
K
a
T
= + +
= +
=

Suy ra:

Trang 67

1 2
0 1 2
1
( )
1
a a z a z
G z
z

+ +
=

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

+ +
= =

Suy ra:
0 1 2
( ) ( 1) ( ) ( 1) ( 2) u k u k a e k a e k a e k = + + +
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
K
P
, K
I
, K
D
l cc thng s P, I, D ca b iu khin
Cch iu chnh cc h s K
P
, K
I
, K
D
:
u tin, ta chnh cho K
P
= 1 v K
D
, K
I
= 0 ngha l ch iu khin P. Sau ,
tng K
P
ln dn dn v quan st ng c. Khi thy ng c dao ng (ngha l n lc
nhanh lc chm), ta ly K
P
ti nhn vi 0.6 tnh ton: K
P_t
= 0.6 K
P_dao_ng
. Sau
, ta tng K
D
ln dn dn (gi nguyn K
P
), nu K
D
qu ln s lm cho ng c b dao
ng mnh. Gim K
D
li cho n khi ng c ht dao ng. iu chnh K
I
l kh nht,
bt u t gi tr rt nh (v d ly K
I
= 1/K
D
chng hn). H s K
I
khng cn ln v
ng c t n cha thnh phn K
I
, 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 68

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 69

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 (h
x
, h
y
, h
z
, 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:
0
0
0
1
a b c
d e f
M
g h i
dx dy dz
(
(
(
=
(
(

Php tnh tin:

Hnh 7.1: Php tnh tin

Trang 70

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
0 1 0 0
0 0 1 0
1
M
dx dy dz
(
(
(
=
(
(

, Q = [x y z 1] = P.M = [x+dx y+dy z+dz 1].

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 R
x
(), R
y
(), R
z
():

Hnh 7.2: Php quay
1 0 0
( ) 0 cos sin
0 sin cos
x
R | | |
| |
(
(
=
(
(

(
(
(
cos 0 sin
0 1 0
sin 0 cos
) (
y
R
(
(
(
=
1 0 0
0 cos sin
0 sin cos
) ( u u
u u
u
z
R
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 71

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 = 90
o
.

Trang 72

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:

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

x _new = x_cur L.cos(angle_cur)
y _new = y_cur L.sin(angle_cur)
angle_new = angle_cur


Trang 73

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:

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
angle_cur < 0 : angle_cur = angle_cur + 2*PI
angle_cur > 2*PI : angle_cur = angle_cur - 2*PI


Trang 74

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 30
o
, 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 30
o
.
| |
1
1
1
1 0 0
0 cos sin cos sin
0 sin cos sin cos
X X
Y X Y Z Y Z
Z Y Z
| | | |
| | | |
( ( (
( ( (
= =
( ( (
( ( ( +


Trang 75

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

o chiu Y.
3 2
3 2
3 2
cos sin 60
sin cos 9
X X X
Y Y Y Z
Z Z Y Z
| |
| |
( ( (
( ( (
= = + +
( ( (
( ( ( + +

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 76

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 77

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 78

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 79

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 80

Start
Khi to cc gi tr
u
- Tnh gc quay
v ch
- Gi lnh quay
Robot thc
hin xong?
Gi lnh di chuyn
v ch
D < 1.3 mt &
ng trng?
C vt cn?
Robot thc
hin xong?
End
No
No
Yes
Tnh khong cch
ti ch D
No
Yes
No
- Tnh gc quay
ti u trnh vt
- Gi lnh quay
Robot thc
hin xong?
Yes
No
ng trng?
i mt on an ton
Robot thc
hin xong?
No Yes
Yes
Yes
khng
gian cho robot
lch qua?
Yes
- Tnh gc quay
ngc li trnh vt
- Gi lnh quay
Robot thc
hin xong? No
ng trng?
Yes
No
Yes
- Tnh khong cch di chuyn
sao cho cch vt 55 cm
- Gi lnh di chuyn
No
Robot thc
hin xong? No
No
- Tnh gc quay trnh vt
theo hng c
- Gi lnh quay
Yes
Robot thc
hin xong?
No
Yes
i mt on an ton
Robot thc
hin xong?
No
Yes
- Tnh gc quay song song
vi hng ban u
- Gi lnh quay
Robot thc
hin xong?
No
Yes
ng trng?
Yes
No
i mt on an ton
Robot thc
hin xong?
No
Yes
Yes

Hnh 8.4: S gii thut iu khin robot do my tnh x l


Trang 81

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):
y_target - y_cur
gamma = arctan( )
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 82

+ 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 83

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(o )
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.
_ ax
1
os ( )
'
x m
c
O L
o | | |
' = = , hng quay v pha bn phi robot.


Trang 84

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:
1
os ( _min/ ' ) c x O L o | | |
' = = , hng quay v pha bn tri robot.


Trang 85

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:
1
_ min
sin ( )
'
x
O L
o | | |
' = + = +
Quay phi:
1
_ ax
sin ( )
'
x m
O L
o | | |
' = + = +


Trang 86

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 87

i bn phi vt cn:
' cos( )
1
_ ax
2 2 1
( _ ax _ ur) ( _ ax _ ur) os( tan ( ))
_ ax
OH OM
x m
x m x c y m y c c
y m
o u
o
=
= +

i bn tri vt cn:
' cos( )
1
_ in
2 2 1
( _ in _ ur) ( _ ax _ ur) os( tan ( ))
_ ax
OH OM
x m
x m x c y m y c c
y m
o u
o
=
= +

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 88

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 H
1
H
2
(hnh
8.11) ln cho robot lt qua (khng gian cho robot lt qua khi H
1
H
2
>50 cm).
Trong OH
1
c tnh nh trong hm i mt on an ton, OH
2
tng t, ta c:
2 4
4 2 2 1
4 4
4
' ' cos( )
( _ ur) ( _ ur) cos(tan ( ))
O H O M
x
x x c y y c
y
_
=
= +

' '
1 2 2 1
H H O H O H =
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 89

Start
Khi to cc module
Timer1_flag = 1
InitPID_flag = 1
- Khi to cc gi tr u
cho PID
- InitPID_flag = 0
- donePID_flag = 0
Target_flag = 1
- Tnh ton ta robot
- Gi thng tin ln my tnh
- Target_flag = 0
donePID_flag = 1
- Tnh ton ta robot
- Gi thng tin ln my tnh
- donePID_flag = 0
- Thc hin PID cho 2 bnh
xe (khi PID thc hin xong,
donePID_flag = 1)
- Timer1_flag = 0
Yes
No
Yes
No
No
Yes
Yes
No

Hnh 8.12: S gii thut trn
vi iu khin
Khi to cc module:
Khi to cc module s dng trn PIC: cc
port I/O, timer, ngt ngoi, PWM v giao
tip RS232.
Timer1_flag:
C bo bng 1 khi timer1 trn sau 5 ms,
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 k
p
, k
i
, k
d
; gi tr t.
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
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:
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
x bng khng), lc donePID_flag bng
1.
Kt lun v hng pht trin

Trang 90

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.
Kt lun v hng pht trin

Trang 91

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.

Ti liu tham kho

Trang 92

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.
Ph lc 1

Trang 93

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.
Ph lc 1

Trang 94

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:

Ph lc 1

Trang 95

Lc ny ta c th s dng chc nng iu khin ng c Kinect trn th vin
OpenNI kt hp vi CL.

Ph lc 2

Trang 96

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:

Ph lc 2

Trang 97

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:

Ph lc 2

Trang 98

Sau khi tho:

Khoan bn l v gn bn c t trong ra:

V kt qu cui cng:
Ph lc 3

Trang 99

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.

Robot dò đường và tránh 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 dò đường và tránh 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

' = = , hng quay v pha bn phi robot.

' = = , hng quay v pha bn tri robot.

You might also like