Πξλρηεςμείξ Κοήηηπ Τμήμα ΗΜΜΥ Αρηόμξμξι Ποάκηξοεπ ΠΛΗ513- Intelligence Lab Σιδωοάκηπ Νικόλαξπ ΑΜ: 2008030041 Αμαθξοά Εογαζίαπ Εναμήμξρ 2012-2013 «Πλξήγηζη ζε Λαβύοιμθξ-Mapping, Localization, Path Planning, Navigation» θμπόξ ηεξ ενγαζίαξ μμο είκαη ε πιμήγεζε ζε έκακ ήδε γκωζηό πάνηε ζημ νμμπόη- e puck από μηα ανπηθή ζέζε ζε μηα ηοπαία θάζε θμνά ζέζε goal.o controller ημο e puck γνάθηεθε ζε Java. Γηα ημκ ιαβύνηκζμ πήνα έκα ζηηγμηόηοπμ από ημ γκωζηό δηαγωκηζμό ημο Ratslife.Σμ πνόβιεμα ηεξ πιμήγεζεξ γηα κα ημ ακηημεηωπίζω, έπνεπε κα ημ ζπάζω ζηα ελήξ οπμπνμβιήμαηα: -Χανημγνάθεζε -Γκημπηζμόξ-ογπνμκηζμόξ ζέζεξ/πνμζακαημιηζμμύ -πεδηαζμό βέιηηζηεξ δηαδνμμήξ Καη -Κίκεζε θαηά μήθμξ ηεξ βέιηηζηεξ δηαδνμμήξ Ανπηθά, αξ γκωνίζμομε ημ νμμπόη μαξ. Γίκαη ημ γκωζηό e puck. Απμηειείηαη από 8 αηζζεηήνεξ απόζηαζεξ, θάμενα( δεκ πνεζημμπμηείηαη ζηε ζογθεθνημέκε ενγαζία) μμηέν γηα έιεγπμ θίκεζε ηωκ ηνμπώκ(2 ηνμπμί) θαη εκζωμαηωμέκμ GPS(γηα ημ GPS έπνεπε κα επελενγαζηώ ημ.proto ανπείμ ημο e puck, κα πνμζζέζω «θιάζε» γηα κα εκενγμπμηεζεί). Ξεθίκεζα με ηεκ Χανημγνάθεζε, ή αιιηώξ Mapping.ημκ ζηαηηθό θόζμμ, έπνεπε κα απμζεθεύζω ώζηε κα γκωνίδεη ημ e puck ηα εμπόδηαημίπμξ θαζώξ θαη ηα ειεύζενα πενάζμαηα όπμο μπμνεί κα θηκεζεί. Μεηέθναζα ημκ θόζμμ ζε έκα grid-πίκαθα 10x10.ημ ζπήμα 1 θαίκεηαη ε ακηηζημηπία ημο πάνηε ζε πίκαθα-θειηά.
9 8 7 6 5 4 3 2 1 0 x/y 0 1 2 3 4 5 6 7 8 9 Σχήμα 1. Αναπαράςταςη του κόςμου ςε πίνακα. Με τισ bold γραμμζσ δείχνω τουσ τοίχουσ. Το x δείχνει τισ γραμμζσ ενώ το y τισ ςτήλεσ. Οι ζντονεσ μαφρεσ γραμμζσ είναι τα εμπόδιατοίχοσ ενώ οι υπόλοιπεσ τα όρια κάθε κελιοφ. ε θάζε θειί ημο πίκαθα απμζεθεύεηαη γηα ημ πμύ οπάνπεη ημίπμξ ή όπη, 1 θαη 0 ακηίζημηπα. Σμίπμ μπμνώ κα έπω ζημ βμννά, κόημ, ακαημιή δύζε. Έηζη γηα πανάδεηγμα ζημ θειί Cell[4][7] έπμομε ηηξ ηημέξ 1,0,1,0 γηα θάζε ημίπμ ακηίζημηπα. Γπίζεξ, γηα θάζε θειί απμζεθεύμκηαη μη ΠΡΟΒΑΙΜΟΙ εγγύηενμη γείημκεξ ημο, δειαδή ΟΛΑ ηα θειία με ηα μπμία ζοκμνεύεη. Όηακ ιέμε πνμζβαζημμξ εκκμμύμε εθείκμοξ ημο γείημκεξ μπμύ δεκ πωνίδμκηαη από ημίπμ
θαη ζοκεπώξ μπμνεί ημ e puck κα μεηαβεί απ ημκ έκακ ζημκ άιιμ. Γηα πανάδεηγμα, ημ θειί Cell[4][7] έπεη γείημκεξ ηα θειηά Cell[3][7] θαη Cell[4][8].Ta θειία Cell[4][6] θαη Cell[5][7] πανόιμ πμο ζοκμνεύμοκ με ημ Cell[4][7] είκαη γείημκεξ αιιά όπη πνμζβάζημμη, άνα απμννίπημκηαη. Άιιε απαναίηεηε πιενμθμνία γηα ημ θάζε θειί είκαη ηα θόζηε ημοξ, δειαδή ημ πόζμ θμζηίδεη γηα κα ημ δηαβμύμε ώζηε κα θηάζμομε ζημκ ζηόπμ. Ανπηθά ηα θόζηε είκαη όια μεδεκηθά, αθμύ δεκ έπεη ηεζεί μ ζηόπμξ θαη ζοκεπώξ δεκ έπεη κόεμα κα έπμοκ θάπμηα ηημή. ημ θμμμάηη ημο Path Planning δίκμκηαη μη ηημέξ απ ηα θόζηε βάζε ημο ζηόπμο. Χνεηαδόμαζηε 3 μεηαβιεηέξ γηα ηα θόζηε PathCost, LocalCost, ParentCost, όπμο PathCost = LocalCost + ParentCost (ζα γίκεη ακάιοζε παναθάηω). ηεκ ζοκέπεηα, αζπμιήζεθα με ημκ εκημπηζμό ηεξ ζέζεξ ημο e puck ζημ πώνμ, ε αιιηώξ Localization.Πνμθεημέκμο κα αθμιμοζήζεη μπμηαδήπμηε δηαδνμμή, πνέπεη κα γκωνίδεη ημ πμύ βνίζθεηαη θάζε ζηηγμή ημ e puck.γηα κα επηηεοπζεί αοηό, πνεζημμπμίεζα ημ εκζωμαηωμέκμ GPS ημο e puck.με ημ πμο εκενγμπμηεζεί, επηζηνέθμκηαη ακά time step ζοκηεηαγμέκεξ x,y,z.σμ x αθμνά ημκ μνηδόκηημ άλμκα, ημ y ημκ θαηαθόνοθμ θαη ημ z ημκ θάζεημ. Άνα ζηεκ ενγαζία μμο εθμεηαιιεύηεθα ηηξ ηημέξ μόκμ x θαη z. Οη ζοκηεηαγμέκεξ x,z παίνκμοκ ηημέξ ζημ δηάζηεμα [0.0 1.0].Σμ Gps μεηνά ζακ ανπή από πάκω δεληά ημο θόζμμο μαξ. Άνα πνέπεη κα ακηηζημηπίζω ηηξ ηημέξ-δεογάνηα ζηα ζωζηά θειηά (πήμα 2). Κώδηθαξ πμο ημ θάκεη αοηό (έπεηηα από δμθημέξ): coordgps = Gps.getValues(); if ((int) Math.round(coordGPS[0]*9) ==9 ) axis_x = 0; else if ((int) Math.round(coordGPS[0]*9) ==8 ) axis_x = 1; else if ((int) Math.round(coordGPS[0]*9) ==7 ) axis_x = 2; else if ((int) Math.round(coordGPS[0]*9) ==6 ) axis_x = 3; else if ((int) Math.round(coordGPS[0]*9) ==5 ) axis_x = 4; else if ((int) Math.round(coordGPS[0]*9) ==4 ) axis_x = 5; else if ((int) Math.round(coordGPS[0]*9) ==3 ) axis_x = 6; else if ((int) Math.round(coordGPS[0]*9) ==2 ) axis_x = 7; else if ((int) Math.round(coordGPS[0]*9) ==1 ) axis_x = 8; else if ((int) Math.round(coordGPS[0]*9) ==0 )
axis_x = 9; if ((int) Math.round(coordGPS[2]*9) ==9 ) axis_y = 0; else if ((int) Math.round(coordGPS[2]*9) ==8 ) axis_y = 1; else if ((int) Math.round(coordGPS[2]*9) ==7 ) axis_y = 2; else if ((int) Math.round(coordGPS[2]*9) ==6 ) axis_y = 3; else if ((int) Math.round(coordGPS[2]*9) ==5 ) axis_y = 4; else if ((int) Math.round(coordGPS[2]*9) ==4 ) axis_y = 5; else if ((int) Math.round(coordGPS[2]*9) ==3 ) axis_y = 6; else if ((int) Math.round(coordGPS[2]*9) ==2 ) axis_y = 7; else if ((int) Math.round(coordGPS[2]*9) ==1 ) axis_y = 8; else if ((int) Math.round(coordGPS[2]*9) ==0 ) axis_y = 0; coordgps[0] = axis_x; coordgps[2] = axis_y; 1.0 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0.0 Σχήμα 2. Αναπαράςταςη των ςυντεταγμζνων GPS ςτα κελία του χάρτη 0.0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1.0 Γηα πανάδεηγμα, όηακ μη ηημέξ ημο x ακήθμοκ ζημ [0.0 0,099] θαη μη ηημέξ ημο z ακήθμοκ ζημ[ 0.0 0,099] είμαζηε ημ θειί Cell[9][9](ή αιιηώξ πάκω δεληά ζημ πάνηε). Ακ μη ηημέξ ημο x ακήθμοκ ζημ [0.99 1.00] θαη ημο z ακήθμοκ ζημ [0.99 1.00] βνηζθόμαζηε ζημ θειί Cell[0][0].(ή αιιηώξ θάηω ανηζηενά ζημ πάνηε)
Ακ μη ηημέξ ημο x ακήθμοκ ζημ [0.40 0.49] θαη ημο z ακήθμοκ ζημ [0.40 0.49] βνηζθόμαζηε ζημ θειί Cell[4][4].(ή αιιηώξ ζημ θέκηνμ ημο πάνηε) θ.μ.θ Όζμ αθμνά ημ Path Planning πνεζημμπμίεζα ημκ Α* Algorithm. Ο A- Star αιγόνηζμμξ είκαη μηα ηεπκηθή γηα ηεκ εύνεζε ηεξ θαιύηενεξ δηαδνμμήξ μέζα ζε έκα ιαβύνηκζμ έπμκηαξ δεδμμέκε μηα αθεηενία θαη έκα πνμμνηζμό. Καζμνίδεηαη ε ανπηθή ζέζε, έζηω Α, ημο μμκμπαηημύ (ζηεκ πενίπηωζή μαξ είκαη ημ θειί Cell[0][0]) θαη επηιέγεηαη ηοπαία έκαξ ζηόπμξ-goal,έζηω Β(Cell[a][b]).ύμθωκα με ημκ A* Αιγόνηζμμ, επηιέγεηαη ε δηαδνμμή εθείκε ε μπμία ειαπηζημπμηεί ημ θόζημξ μεηάβαζεξ από ηεκ Α ζηεκ ζέζε Β. Οη ηνείξ ηημέξ ημο θόζημοξ γηα θάζε θειί είκαη LocalCost:Σμ θόζημξ ημο κα θηάζεη από ημ ηνέπμκ θειί ζημ ζηόπμ (πνεζημμπμηώ ημκ ηύπμ ημο Manhattan distance) ζημ ηνέπμκ θειί ParentCost: Σμ θόζημξ πμο ζέιεη κα θηάζεη από ημ παηένα θειί PathCost: Σμ θόζημξ πμο ζέιεη γηα κα δηαζπίζεη ηε δηαδνμμή απ ημ θειί Α ζημ ηνέπμκ θειί. PathCost = LocalCost+ParentCost. Αοζαίνεηα, ζέηω ημ θόζημξ από θειί ζε θειί ίζμ με 1 θαη Κώδηθαξ γηα ημκ οπμιμγηζμό ημο θόζημοξ δίκεηαη παναθάηω: public double getlocalcost(cell goal){ if (Start){ return 0.0; else{ LocalCost = 1.0*(Math.abs(Row - goal.row)+math.abs(column - goal.column)); return LocalCost; public double getparentcost(){ if (Start){ return 0.0; if (ParentCost == 0.0 ){ ParentCost = 1.0 + 0.5 * (Parent.getParentCost() - 1.0); return ParentCost; public double getpassthrough(cell goal) {
if (Start) { return 0.0; PassThroughCost = getlocalcost(goal) + getparentcost(); return PassThroughCost; Παναθάηω δείπκω έκα πανάδεηγμα (πήμα 3) εύνεζεξ βέιηηζηεξ δηαδνμμή από έκα ανπηθό θειί Α ζημ ζηόπμ Β. Οη μαύνεξ γναμμέξ δειώκμοκ ημίπμξ, μη δηαθεθμμμέκεξ ημκ γείημκα ημο θειημύ εκώ μη πνάζηκεξ ηα όνηα θάζε θειημύ. ε θάζε θειί πμο μειεηάηαη θάηω ανηζηενά θαίκεηαη ημ ParentCost εκώ θάηω δεληά ημ LocalCost. 0.5 7 A A 0.5 5 B B πήμα 3 Ανπηθό μμκμπάηη ζημ ζπήμα πάκω ανηζηενά, εκδηάμεζμ μμκμπάηη ζημ ζπήμα πάκω δελία θαη ηειηθό ζημ θεκηνηθό ζπήμα. Με θίηνηκεξ θμοθίδεξ είκαη ημ ηειηθό μμκμπάηη.
Αθμύ γίκμοκ ηα παναπάκω, ημ e puck πνέπεη κα ανπίζεη κα πλξηγείηαι ζηξμ λαβύοιμθξ.ξεθηκά με έκακ ζογπνμκηζμό, ώζηε κα πνμζακαημιηζηεί πνμξ ηεκ ζωζηή θαηεύζοκζε. ηεκ ζοκέπεηα, αθμιμοζείηαη ημ μμκμπάηη πμο οπμιμγίζηεθε από ημ A* Algorithm.Με ηεκ βμήζεηα ηωκ αηζζεηήνωκ απόζηαζεξ, ζηνίβεη ανηζηενά θαη δεληά πνμξ ηα ζωζηά θειία, θναηώκηαξ απόζηαζε αζθαιείαξ από ημοξ ημίπμοξ. Ποξβλήμαηα: **Γίπα ακαθένεη ζηεκ πανμοζίαζε πωξ γηα θάπμημ άγκωζημ ιόγμ ημ ζύζηεμα μμο θναζάνεη.. εκώ δεκ οπάνπεη ιόγμξ πεηάεη exit 0 θαη ηενμαηίδεηαη ζηεκ μέζε ε εθηέιεζε ημο πνμγνάμμαημξ.. Αθηένωζα άπεηνμ πνόκμ όμωξ δεκ θαηάθενα κα θαηαιάβω ημ γηαηί. Δμθίμαζα μήπωξ έθηαηγε ε έθδμζε ημο Webots αιιά όπη. Μάιιμκ ηνώγεηαη ανθεηή μκήμε από ημ πνόγναμμα με απμηέιεζμα κα δηαθόπηεηαη.***