1. sample b( ) 2. return 12 1 1 d( , ) 6 i ran b b 3.2.3 Mô hình c m biến c a robot
Ph ngăphápăMCLăsửăd ngăm tă môăhìnhă c măbi năđ ợcăg iălƠămô hình trường tương thích. Th tăra,ăđơyălƠăm tăthu tătoánăb căphát,ăkhôngăđ ợcăt oăraătừăsựătínhă toánăxácăsuấtădựaătrênăỦănghĩaăv tălỦăc aăcácăc măbi n.ăMôăhìnhăkhôngăcóăđ ợcăsựă gi iăthíchăchặtăchẽăv ămặtăv tălỦ.ăTuyănhiên,ăph ngăphápănƠyăl iălƠmăvi căt tătrongă thựcăt .
ụăt ngăchínhălƠătr c h t ph i chuy năđổiăđi m cu i c a c m bi n zt vào không gian h t aăđ toàn c c c a b năđ .ăĐ chuy n từng giá tr c m bi n riêng bi t zk
t
vào h t aăđ toàn c c c a b năđ m thì t aăđ c a robot trong b năđ và v trí c a tia c m bi n zktrên robot ph iăđ ợc bi t rõ. N u g i , , T
t
x x y là v trí c a robot t i th iăđi m t và xét trong h t aăđ tham chi u 2 chi u c c b c a robot thì m i c m bi n có m t t aăđ c đnh là xk sens, ,yk sens, và l ch so v iăh ng di chuy n c a robot m t góc là k s. ens.ăĐơyăđ ợc xem là những giá tr đặcătr ngăc a c m bi n. Khiăđó,ăđi m cu i c a tia c m bi năđoăkho ng cách zk
t đ ợc bi năđổi sang h t aăđ toàn c c thông qua phép bi năđổiăl ợng giác:
, ens , ens , ens , ens os os sin sin os sin t k t k k s k s z k t k s k s z x x c x c z y c y y (3.20)
49
Hình 3. 6. L y mẫu từ mô hình vận động.
Những h t aăđ này ch cóăỦănghĩaăkhiăc m bi n phát hi năđ ợc m t v t c n nào đó.ăN u giá tr kho ng cách c m bi năđ căđ ợc là t iăđaă(zk ax
t zm ) thì các h t aăđ nƠyăkhôngăcóăỦănghĩaă(khôngăcóăthôngătinăgìăv v t c nătrongămôiătr ng).ăCǜngă gi ngănh ătrongămôăhìnhăv năđ ng, mô hình c m bi năcǜngăb nhăh ng b i nhi u d năđ n sự không chính xác.
Th nhất là nhi u lên giá tr đo.ăNhi u b t ngu n trongăquáătrìnhăđoăđ ợc mô hình hóa bằng m t phân b Gaus.ăQuáătrìnhăđoăthực hi n vi c tìm ra v t c n g n nhất trong h t aăđ 2 chi u xy. G i dist là kho ngăcácă clităgiữa h t aăđ c a c m bi n k, k
t t
T z z
x y và v t c n g n nhất trong b năđ m.ăKhiăđóăxácăsuất c a giá tr đoă đ ợc cho b i m t phân ph i chu năhóaăcóăđ l ch chu n là hit:
2 2 ( | , ) ( is ) hit k hit t t p z x m d t (3.21)
Ti p theo là sai s giá tr đo.ăSaiăs giá tr đoăđ ợc bi u di n b i m t phân b đi m
ax
m p .
Cu i cùng là phân b đ ng d ng prand đ mô hình hóa các nhi u phát sinh ng u nhiênătrongăquáătrìnhăđo.
Xác suất mong mu n c aămôăhìnhăđ ợc tổng hợp từ ba phân b đƣănêuătrên: k| , . d. d ax. ax
t t hit hit ran ran m m
50
B ng 3.4 trình bày thu t toán cho mô hình c m bi nă dùngă ph ngă phápă tr ng t ngăthích.
Vòng lặpăphíaăngoƠiăđ ợcădùngăđ nhân các giá tr xác suất riêng k| ,
t t
p z x m v i các giá tr nhi uăđ c l p nhau. Dòng 4 ki m tra xem giá tr c m bi năđ căđ ợc có thỏaăđi u ki n nhỏh năkho ng cách t iăđaăc a c m bi năkhông.ăDòngăηăđ n 8 thực hi n công vi c chính: tính kho ngăcáchăđ n v t c n g n nhất trong không gian xy
(dòng 7) và k t qu c a sự phù hợp (dòng 8).
B ng 3. 4 Thu t toán cho mô hình c m biến.
1. measureme _ mod ( , , )nt el z x mt t
2. q = 1 3. for all k do
4. if zk ax
t zm
5. k , ens os , enssin os( , ens)
t
k
k s k s t k s
z
x x x c y z c
6. k , ens os , enssin sin( , ens)
t k k s k s t k s z y y y c x z 7. 2 2 2 , is min k k t t z z x y d t x x y y ( x y , là v t c n trong ) 8. 2 2 d ax . . r ( is , ran om hit m z q q z p ob d t z 9. return
3.2.4 ng d ngăph ngăphápăMCLătrongăđi u khi n robot tự hành
Trong ph n này, m t thu tătoánăđ yăđ cho toàn b quáătrìnhăđi u khi nărobotăđiă đ n m cătiêuăchoătr c sẽđ ợc xây dựng. Dựa trên những nghiên c u v các vấnăđ đi u khi n robot tự hƠnhă đƣă trìnhă bƠyă trongă ch ngă 2ă vƠă thu t toán MCL đ u
51
ch ngănƠyăcóăth thấyăquáătrìnhăđi u khi n robot bao g măcácăb c:ăđ nh v , l p k ho chăđ ngăđiăvƠătránhăv t c n.ăTheoăđó,ăl uăđ thu tătoánăđi u khi nărobotăđiăđ n đúngă m că tiêuă trongă tr ng hợp robot không bi tă tr c v tríă bană đ uă đ ợc xây dựngănh ătrong hình 3.3.
Trongătr ng hợp này, robot có th đ ợcăđặtăvƠoă môiătr ng t i m t v trí ng u nhiên bất kỳ (robot không bi tătr c v tríăđặtăbanăđ u).ăTr c tiên, robot di chuy n m t cách bấtăđ nhătrongămôiătr ngăđ thu th p thông tin v các v t c n xung quanh thông qua h th ng c m bi năđ ợc trang b . Trong quá trình robot di chuy n ng u nhiên thì thu tătoánăMCLăcǜngăđ ợc thực thi nhằm tìm ra v tríăđúngăc a robot từ t p m uăđƣăt oăraăbanăđ u. M t m uăđ ợc xem là v tríăđúngăc a robot khi tr ng s c a nó có giá tr l n nhất và ph iăđ tăđ n m t giá tr ng ỡngăchoătr c. Những tr ng s nƠyăđ ợc thu t toán MCL tính toán dựaătrênăcácăthôngătinăthuăđ ợc từ h th ng c m bi n và b năđ đƣăcungăcấp cho robot. Vi c di chuy năvƠătínhătoánănƠyăđ ợc l p l iăchoăđ n khi robotăxácăđ nhăđ ợc v tríăđúngăc aănóătrongămôiătr ng. Sau khi v tríăđúngăc aărobotăđƣăđ ợcăxácăđnh thì m tăđ ngăđiăt iă uătừ v tríănƠyăđ n m c tiênă đ ợc tính toán nh thu t toán D*. Cu iă cùng,ă ph ngă phápă tr ng th nĕngă đ ợc sử d ngă đ giúpă robotă điă đ n m c tiêu theo k t qu đ ngă điă tìmă đ ợc và tránh va ch m v i các v t c nătrênăđ ng. Trong quá trình robot di chuy n bằng tr ng th nĕngăthìăthu t toán MCL v n liên t căđ ợc g iăđ đ m b o v trí hi n t i c aărobotăluônăđúng.
52
Hình 3. 7. ầưu đồ thuật toán điều khiển robot với vịtrí ban đ u không biết trước.
Đúng Đúng Sai Sai B tăđ u G i thu t toán MCL G i thu tătoánătìmăđ ng D* Tr ng s m u l n h năng ỡng?
G i thu t toán Trường thếnăng
Di chuy n ng uănhiênătrongămôiătr ng
K t thúc Đƣăđ n m c tiêu?
53
Ch ngă4
Xây dựng mô hình và phân tích mô
ph ng thiết kế
4.1 Phần m m mô ph ng robot Player/Stage
4.1.1. Gi i thi u
Player/StageălƠăph năm mămƣăngu năm ăcungăcấpăcácăti năíchăđ ămôăphỏngărobotă vƠăđặcăbi tălƠăh ănhi uărobot.ăDựăánăPlayer/Stageăđ ợcăb tăđ uăt iătr ngăđ iăh că NamăCarliforniaătừănĕmă1999ănh ălƠăm tăph năm măgiaoăti păvƠămôăphỏngădƠnhă choănghiênăc uărobotătựăhƠnh.ăBanăđ u,ăPlayer/Stageăch ălƠăm tăph năm mănghiênă c uătrongăph măviăn iăb ăc aănhƠătr ngănh ngăsauăđóănóăđƣăđ ợcăchấpănh n,ăhi uă ch nhăvƠăm ăr ngăb iăcácănhƠănghiênăc uătrênăkh păth ăgi i.ăTrongăcácă ngăd ngăv ă robot,ăđặcăbi tălƠăcácănghiênăc uăv ărobotătựăhƠnhăthìăPlayer/Stageăcóăth ăđ ợcăxem lƠăph năm mănghiênăc uăt tănhấtănh ăvƠoăsựăho tăđ ngăthôngăsu t,ătínhălinhăho tăvƠă t căđ ăc aănó [14].
Player/Stageă baoă g mă m tă ch ngă trìnhă Playeră vƠă m tă giaoă di nă môă phỏngă Stage.ă Playerăcungăcấpăcácăti năíchăph năc ngăvƠăđi uăkhi năc năthi tăchoăcácă ngd ngărobot.ă Stageăchoăphépămôăphỏngăho tăđ ngăc aărobotăcùngăv iănhi uălo iăc măbi nătrongăm tă môiătr ngăhaiăchi u.ăPlayerăvƠăStageăho tăđ ngătrênăn năt ngăUNIXăvƠăđ ợcăcungăcấpă nh ălƠăm tăph năm mămi năphíăd iăd ngăGNUă(GeneralăPublicăLicense).
Banăđ u,ăPlayerăđ ợcăphátătri năb iăBrianăGerkeyăvƠăKasperăStøy,ăb ăgiaoădi nămôă phỏngăStageăđ ợcăvi tăb iăRichardăT.ăVaughanăvƠăAndrewăHoward.ăGazeboăvƠăgiaoă di năPlayerăc aănóăđ ợcăvi tăb iăNateăKoeingăvƠăAndrewăHoward.ăHi nănay,ăm tă ph năl năc aădựăánăPlayer/Stage ậ b ăph năphátătri năPlayeră- đ ợcăđi uăhƠnhăb iă Brian Gerkey, Andrew Howard và Richard T. Vaughan.
54
4.1.2. Đặcăđi m c a Player
PlayerălƠăm tăm ngăd chăv ăđi uăkhi năRobot,ăch yăđ ợcătrênăRobotăthực.ăPlayeră cungăcấpăm tăgiaoădi năđ năgi năvƠărõărƠngăv ăcácăc măbi năvƠăcácăc ăcấuăchấpă hƠnhă c aă robotă dựaă trênă giaoă di nă m ng.ă Ch ngă trìnhă đi uă khi nă giaoă ti pă v iă Playerăquaăgiaoăth căTCP,ăthựcăhi năđ cădữăli uătừăc măbi n;ăghiăl nhăđ năc ăcấuă chấpăhƠnhăvƠăcấuăhìnhăcácăthi tăb ăc aărobot.ă
Playeră h ă trợă rất đaă d ngă ph nă c ngă Robot.ă N nă t ngă c aă Playeră lƠă m tă h ă ActivMediaăPioneeră2ănh ngăcònăh ătrợăthêmănhi uălo iăRobotăvƠănhi uălo iăc mă bi n.ăKh iăki nătrúcăc aăPlayerăchoăphépăd ădƠngăc pănh tăthêmăcácăph năc ngăm i.ă Playerăđ ợcăthi tăk ăđ ătr ăthƠnhăm tăngônăngữăvƠăn năt ngăđ căl p.ăCh ngătrìnhă đi uăkhi năcóăth ăch yătrênăbấtăkỳămáyănƠoăcóăk tăn iăm ngăđ năRobotăvƠăcóăth ă vi tăđ ợcătrênăbấtăkỳăngônăngữănƠoăcóăh ătrợăđi uăkhi năsocketăTCP/IP.ăNhữngăti nă íchăchoăkháchăhƠngăhi năt iăđƣăcóăsẳnătrongăC,ăC++,ăTcl, LISP, Java, và Python. NgoƠiăra,ănhữngăthôngătinăliênăquanăđ năPlayerănh ăcáchăcƠiăđặt,ăvi tăch ngătrìnhă ngăd ng,ăcáchăsửăd ng,...ăđ ợcătrìnhăbƠyărấtărõărƠngătrongărấtănhi uătƠiăli uăđ ợcă cungăcấpămi năphíăquaăInternet [1 15 16].ăHìnhă4.1ăbi uădi nămô hình Client/server c aăPlayer;ăsựăliênăh ăgiữaăch ngătrìnhăng iădùng,ăPlayerăserverăvƠăthi tăb hình 4.1.
Hình 4. 1. Mô hình Client/Sever của Player.
Player Server
Device driver Interface
Player Client Library C/C++ C# Java Tcl Python Ruby Lisp Octave User Program SICK LMS 200 Pioneer SICK PLS Segway Khepera sicklms200 sickpls p2os segwayrmp khepera laser laser position position position Data Configuration Command TCP/IP IPC Ch ngă trình ng i dùng SICK PLS Các c m bi n, c ă cấu chấp hành, Laser, ầ
55
4.1.3. Đặcăđi m c a Stage
Stage là s n ph m c a dự án Player/Stage, là m t t p hợp những công c ph n m m đ h trợ nghiên c uăRobotădiăđ ng và những h th ng c m bi n thông minh. Stage mô phỏng tổng th Robotădiăđ ng, những c m bi n và nhữngăđ iăt ợng trong môi tr ng không gian 2 chi u.ăStageăđ ợc thi t k v i những h th ng nhi uăđ i di n vì v y nó cung cấp nhữngămôăhìnhăkháăđ năgi n, ít tính toán v i nhi u thi t b h nălƠă thử nghi m trên thi t b v iăđ trung thựcăcao.ăĐi u này rất hữu ích khi ti p c n b ph n m m này [1 14].
Stageăth ngăđ ợc ki m soát thông qua Player, m t d ch v robotăđ ợc n i m ng. Player cung cấp m t giao di n ti n lợiăđ thi t l p thi t b đi u khi n cho Robot thực và các c m bi n.ăNg i dùng vi tăch ngătrìnhăđi u khi n Robot và những thu t toán c m bi nănh ălƠăkháchăhƠngăc a Player server. C th , những khách hàng không th thấy sự khác nhau giữa những thi t b Robot thực và sự mô phỏngăt ngă đ ngăc a Stage. NhữngăkháchăhƠngăđ ợc phát tri n bằng cách sử d ng Stage sẽ làm vi c v i sự thayăđổi rấtăítăhayăkhôngăthayăđổi v i những Robot thựcăvƠăng ợc l i.ăNh ăv y Stage cho phép t o nhanh những nguyên m u cho vi căđi u khi n v i những Robot thực v i rất nhi u c m bi n và những b d nă đ ngă đ ợc cung cấp nh :ăc m bi n siêu âm, b đnh kho ngăcáchălaser,ămáyăđoăhƠnhătrình,ăb kẹp, b gi măxócăvƠăcácăRobotădiăđ ng.
Stageăđ ợc thi t k đ h trợ cho vi c nghiên c u những h th ng tự tr , vì v y Stage cung cấp nhữngămôăhìnhăđ năgi n,ăđaăd ng v i nhi u thi t b .ăStageăth ng sử d ng m t b Playerăpluginăđ cung cấp thi t b cho Player. Nhữngăng i dùng vi t những thu t toán c m bi năvƠăđi u khi năRobotănh ălƠăkháchăhƠngăc a Player server.
V i công c Player/Stage cùng v i những gi i thu tă đƣă lựa ch n cho phép mô phỏngăvƠăđi u ch nh m t cách hoàn thi n ho tăđ ng c aăRobot,ălƠmăc ăs cho vi c thực thi trên Robot thực.ăCh ngătrìnhăđi u khi n Robot có th đ ợcăđi u ch nh m t
56
cách d dàng. Stage cung cấp m t giao di n rõ ràng và rất thực v môiătr ng ho t đ ng c a Robot. Từ giao di nănƠyăng i dùng có th quanăsátăđ ợc k t qu ho t đ ng c a robot và hi u ch nhă ch ngă trìnhă đi u khi n theo ý mu n. Ngoài ra, Player/Stage còn có th giao ti p v i các Robot chu năđƣăđ ợc ch t o sẵn trong các phòng thí nghi m l n trên th gi i. Vấnăđ k t n iăđ ợc thực hi n d dƠngăvƠăng i dùng ch cònăquanătơmăđ n vấnăđ vi tăch ngătrìnhăđi u khi n hình 4.2.
Hình 4. 2. Kết nối giữa Player/Stage và Robot.
4.2 Kết qu mô ph ng trên phần m m Player/Stage
Khi sử d ng Player/Stage c n ph i có ba ki u t p tin: .world, .cfg và .inc. Ki u t p tin .world khai báo cho Player/Stage bi t nhữngăgìăđ ợc sử d ngătrongămôiătr ng mô hình, nó mô t cấuăhìnhăbanăđ u c aărobot,ăkíchăth c c aăs ăđ cǜngănh ăbất kỳ thành ph n nào mu n hi n di nătrongăs ăđ . T p tin ki uă.incădùngăđ thi t l p cấu hình c aăcácăđ iăt ợngănh ărobot,ăsonar,ălaser,ăand cameraầvƠănóăcóăth đ ợc chèn vào t p tin .world. T p tin ki u .cfg cung cấp cho Player những thông tin v robotăđangăsử d ng. T p tin này khai báo cho Player bi t những driver c n thi tăđ t ngătácăv iărobot.ăTr ng hợp sử d ng robot thực thì nhữngădriverăđóăđ ợc lấy ngayăPlayer.ăNh ngăkhiămôăphỏng thì Stage sẽ cung cấp các driver này.
Player Server User Program Robot Stage Gazebo Player Server Player Server Th ă vi n ph c v Player C/C++ C# Java Tcl Python Ruby Lisp Octave Ch ngă trình ng i dùng
57
Trongăđ tài này tất c các thí nghi măđ uăđ ợc ti n hành mô phỏng trên ph n m m Player/Stage. Mô hình c aămobileărobotădùngăđ mô phỏng các gi i thu t trong lu n vĕnănƠyădựa trên m t robot có th t tên là Pioneer. Robot mô hình có d ng hình vuông,ăkíchăth c (50 50) cm, xung quanh robot có g n c m bi n laser, cóăđ phân gi i 0.50. Ph m vi l n nhất mà c m bi n có th nh n bi tăđ ợc v t c n là 5m. Robot mô hình sử d ngăph ngăth căláiăviăsaiăđ thayăđổiăh ng di chuy n. Tr ng l ợng c a robot 15kg. Kh o sát m tăs ăđ cóăkíchăth c (1616) m, các v t c n đ ợc b trí bất kì trên b năđ .
Hình 4. 3. Mô hình robot và vật cản trên ph n mềm Player/Stage
4.2.1.ăMôăph ngăđ ngăđiăc aărobotădùngăph ngăphápăPF (Potential field)
Robot sẽ di chuy n trong nhữngămôiătr ng trong nhà khác nhau, khi ấy sẽ có m t s môiătr ng rôbot sẽ đ năđ ợc m cătiêuăđ raăvƠăcǜngăcóămôiătr ng robot sẽ khôngătìmăđ ợcăđ năđích.ăPh ngăphápăPF là khi gặp v t c n thì robot sẽ b m t lực đ yărobotăraăvƠăđíchăsẽ t o lựcăhútăđ n robot, Hình 4.4 & hình 4.5 cho k t qu khá t t và robot ti p c n m c tiêu đ ng th iătránhăđ ợc các v t c nătrênăđ ngăđi. Đ tài sử d ngă ph ngă phápă tr ng th nĕng đ mô phỏng v i thông s ng ỡng nh h ng c a c m bi n đ ợc ch n d0= 0.3m. Doăđó, robot luôn giữ kho ng cách t i thi u v i v t c n là 0.3m và v i t căđ di chuy n v = 0.4m/s.
C m bi n laser 2D V t c n Đích Đu ng di chuy n robot T aăđ robotă(x,y, )
58
Hình 4.4 v iămôiătr ng ít v t c n ít, nó mô t đ ngăđiăc a robot khi di chuy n từ v trí (-2, -3)ăđ n v trí (7, 7), v i t i v trí góc 00. Rôbotăđƣădiăchuy năđ n m c tiêu đƣăđặtăraăbanăđ u sử d ng thu t toán di chuy n và tránh v t c nădùngăph ngăphápă PF.
Hình 4.5 v iămôiătr ng ph c t păh năvƠărobotăsẽ di chuy n từ v trí (-1, -7)ăđ n v trí (7,7) v i v trí góc là 900. Rôbotăđƣădiăchuy năđ n m cătiêuăđƣăđặtăraăbanăđ u sử d ng thu t toán di chuy n và tránh v t c nădùngăph ngăphápăPF.
Hình 4. 4 Đường đi của robot sử dụng phương pháp PF đến được mục tiêu
59
Hình 4.6 v iămôiătr ng ph c t păh năvƠăcóăb y c c b , khi đó robot di chuy n vào b y cực ti u c c b thì nó di chuy n trong vòng l p khép kín và không th ti p c n đ ợc m c tiêu. Hình 4.6 mô t đ ngăđiăc a robot không ti p c năđ ợc m c tiêu v i v tríăbanăđ u là (-7, -7) và v trí m c tiêu là (7, 7). Rôbot không di chuy năđ n m c tiêuă đƣă đặtă raă bană đ u khi sử d ng thu t toán di chuy n và tránh v t c n dùng ph ngăphápăPF.
Hình 4. 6 Robot không thểđến đích vì vật cản phức tạp (bẫy cục bộ)
4.2.2.ăMôăph ngăđ ngăđiăc aărobotădùngăph ngăphápăPFăkếtăh păv iăD*
Gi i thu t k t hợpăph ngăphápădiăchuy n, tránh v t c n PF và thu t toán tìm ki m D*đƣăkh c ph căđ ợc vi c m c b y cực ti u c c b , sử d ng thu t toán D* thì robot sẽ tự v ch ra k ho chăđ ngăđiăvƠăk t hợp v i thu t toán tru ng th nĕngăđ tránh v t c n thì robot có th đ năđíchăv i môi tru ng ph c t p. V iăphu ngăphápănƠyăthìă robot c n ph i khai báo v tríăbanăđ u.
Hình 4.7 robotădiăchuy nătừăv ătríă(-7, -7)ăđ năv ătríă(7, 7) sửăd ngăgi iăthu tăk tăhợpă thu tătoánătìmăki măD*ăv iăph ngăphápăPF.ăRobotădiăchuy năvƠăti păc năđ ợcăm că tiêuăm tăcáchăanătoƠn,ărobotăkhôngăr iăvƠoăb yăcựcăti uăc căb đ ngăth iătránhăđ ợcă
60 cácăv tăc nătrongăsu tăquƣngăđ ngăđi.ă
Hình 4. 7 Robot đã đến đích với vật cản phức tạp
4.2.3.ăMôăph ngăđ ngăđiăc aărobotădùngăph ngăphápăMCLăkếtăh păPFă&ă
D*
Robot sử d ngăđ mô phỏngăđ ợc trang b 1c m bi n laserăcóăđ phân gi i là 0.50 , doăđóăsẽ t o ra 361 v trí đ c m nh nămôiătr ng xung quanh (zt) và m t b c m bi năđoăhƠnhătrìnhăđ lấyăthôngătinăđi u khi n (ut). B năđ cóăkíchăth c 16x16m,