ΕΝΟΤΗΤΑ 2: Αυτόνομα Ευφυή Κινούμενα Ρομποτικά Συστήματα

Transcript

1 Ε.Μ.Π., ΣΗΜΜΥ, Ακαδημαϊκό Έτος 200-, 8ο Εξάμηνο Μάθημα: Ρομποτική ΙΙ. Διδάσκων: Κ.Τζαφέστας ΕΝΟΤΗΤΑ 2: Αυτόνομα Ευφυή Κινούμενα Ρομποτικά Συστήματα Σύνθεση αισθητηριακών πληροφοριών (data / sensor fuson) Αυτόνομα Ευφυή Κινούμενα Ρομπότ Περιεχόμενα Ενότητας Μηχανισμοί Κίνησης στο χώρο (locomoton) Αισθητήριες Διατάξεις (sensng) Proprocepton (nternal state ) / Exterocepton etc. Σύνθεση αισθητηριακών πληροφοριών (sensor fuson) Αρχιτεκτονικές ελέγχου (moble robot control archtectures) Αυτοεντοπισμός θέσης / Χαρτογράφηση (self-localsaton / map-buldng) Σχεδιασμός Δρόμου - Πλοήγηση (path plannng / navgaton) percepton acton moton control sensng reasonng 2

2 Σύνθεση αισθητηριακών πληροφοριών (sensor fuson) Εισαγωγή () Βασικό Πρόβλημα: Εκτίμηση ενός φυσικού μεγέθους (π.χ. θέση x του ρομπότ) συνδυάζοντας n διαφορετικές ενδείξεις {z } (=,...,n) (π.χ. από διαφορετικούς αισθητήρες) Έστω ότι όλες οι μετρήσεις z έχουν την ίδια «αξιοπιστία», και έστω ότι κάθε μέτρηση είναι το αποτέλεσμα μιας στοχαστικής (Gaussan) διαδικασίας με μέση τιμή z και διασπορά σ 2 = σταθ. για όλα τα. Τότε έχουμε πολύ απλά για την «συνολική εκτίμηση» της μεταβλητής x: 2 2 (unweghted averagng) ˆ σ = και () x ˆ = z n σ n 3 Σύνθεση αισθητηριακών πληροφοριών (sensor fuson) Εισαγωγή (2) Έστω ότι όλες οι μετρήσεις z δεν έχουν την ίδια «αξιοπιστία», δηλαδή ότι για κάθε μέτρηση z αντιστοιχεί διαφορετική διασπορά (varance) σ 2 Τότε για την «συνολική εκτίμηση» ˆx της μεταβλητής x: πρέπει κάθε μέτρηση να ληφθεί υπ όψη με διαφορετική βαρύτητα, δηλ. να αθροιστεί με κατάλληλο βάρος, για τον υπολογισμό μιας «ανισοβαρούς βέλτιστης μέσης τιμής» (weghted mean σταθμισμένος μέσος). Απλό Παράδειγμα Έστω ότι αποκτούμε σειριακά δύο μετρήσεις, z (τη χρονική στιγμή t ) και z 2 (τη χρονική στιγμή t 2 t ) με τυπική απόκλιση σ z και σ z2 αντίστοιχα. Έχουμε: x ˆ t = E( x z ) = z και ˆ σ t = σ z Με την υπόθεση της Gaussan κατανομής, μπορούμε να υπολογίσουμε μια συνολική βέλτιστη εκτίμηση, με δεδομένη και τη μέτρηση z 2, ως εξής: σ σ x 2 2 ˆ t z2 z 2 = σz+ σ z + z2 σz+ σ z z2 και = + σ σ σ ( t ) ˆ 2 z z2 (2) 4

3 Σύνθεση αισθητηριακών πληροφοριών (sensor fuson) Εισαγωγή (3) Απλό Παράδειγμα (συνέχεια) Η παραπάνω σχέση (2) γράφεται επίσης ως εξής x ˆ t = z : ˆ σ t σ t 2 t K t = t xˆ xˆ z xˆ όπου: Kt σ 2 = σ + 2 z 2 2 z σ z2 (3) = z και: t 2 t K t 2 t = ˆ σ ˆ σ ˆ σ 5 Σύνθεση αισθητηριακών πληροφοριών (sensor fuson) Εισαγωγή (4) Απλό Παράδειγμα (συνέχεια) Γραφική απεικόνιση Συνδυασμός δύο μετρήσεων z και z 2 μ z K z, z 2 2 xˆ = = + ( ) z z όπου: K( z z ) σ z 2, 2 = 2 2 σ z+ σ z2 και: 2 ˆ σ = ˆ σ K( ) ˆ σ 2 2 z z, z2 z 6

4 Σύνθεση αισθητηριακών πληροφοριών (sensor fuson) Εισαγωγή (5) Απλό Παράδειγμα (συνέχεια) Εισάγουμε τώρα μια απλή δυναμική συμπεριφορά (πεδίο του χρόνου) στο σύστημα: dx/dt = u + w όπου u σήμα ελέγχου που αντιστοιχεί στην ονομαστική ταχύτητα του ρομπότ, και w σήμα θορύβου που οφείλεται στην ύπαρξη αβεβαιότητας (διαταραχών) στην πραγματική κίνηση του ρομποτικού συστήματος. Υποθέτουμε συνήθως ότι το w μοντελοποιείται ως «λευκός Γκαουσσιανός θόρυβος»: (0,σ w2 ). xˆ t = xˆ t + u t t t 3 t = 2 + t3 t2 ˆ σ ˆ σ σw 7 Σύνθεση αισθητηριακών πληροφοριών (sensor fuson) Εισαγωγή (6) t=t 3 : xˆ t = xˆ t + u t t t 3 t = 2 + t3 t2 ˆ σ ˆ σ σw Αρχική πρόβλεψη στη χρονική στιγμή t 3 2 ˆ σ, t 3 xˆ t 3 Νέα μέτρηση z 3 t=t 3 : (4) t 3 t3 K t = t 3 xˆ xˆ z xˆ t 3 t3 K t = 3 t3 ˆ σ ˆ σ ˆ σ όπου: 2 ˆ σ t3 3 = 2 2 ˆ σ t 3 + σ z3 Kt Βασική δομή αναδρομικού φίλτρου Kalman στο διακριτό χρόνο, για τον αναδρομικό υπολογισμό σε κάθε χρονική στιγμή της νέας «βέλτιστης» εκτίμησης, και των στοχαστικών ιδιοτήτων αυτής (διασπορά) 8

5 Σύνθεση αισθητηριακών πληροφοριών Φίλτρο Kalman Εισαγωγή () Φίλτρο Kalman: αναδρομικός αλγόριθμος εκτίμησης της κατάστασης ενός συστήματος (recursve state-estmaton), χρησιμοποιώντας γραμμικό δυναμικό μοντέλο της συμπεριφοράς του συστήματος (lnear plant model) καθώς και μοντέλο της λειτουργίας των αισθητήρων (sensor/measurement model). Συμβολισμοί/Ορολογία xˆ ( ) : εκτίμηση κατάστασης στη χρονική στιγμή, χρησιμοποιώντας δεδομένα μετρήσεων μέχρι και τη χρονική στιγμή x() ηή xˆ() = x ˆ( ) : πρόβλεψη κατάστασης τη στιγμή, χρησιμοποιώντας δεδομένα μετρήσεων μέχρι και τη χρονική στιγμή - (predcton) xˆ( ) ηή xˆ( + ) = xˆ( ) (updated estmate) : νέα (ανανεωμένη) εκτίμηση κατάστασης τη στιγμή, χρησιμοποιώντας δεδομένα μετρήσεων μέχρι και τη χρονική στιγμή 9 Σύνθεση αισθητηριακών πληροφοριών Φίλτρο Kalman Εισαγωγή (2) Μοντέλο μετρήσεων (ή μοντέλο αισθητήρων): [ ξ] z( ) = hx( ), + v ( ) (measurement/sensor model) nose functon, wth covarance matrx C v model of the envronment Γραμμικό μοντέλο μετρήσεων: z( ) = H x( ) + v( ) (5) Μοντέλο συστήματος (plant model): [ ] x( + ) = Φ x( ), u( ) + w( ) state transton functon nose functon, covarance matrx C w control nput Γραμμικό μοντέλο συστήματος: x( + ) = A x( ) + B u( ) + w( ) (6) 0

6 Σύνθεση αισθητηριακών πληροφοριών: Determnstc Least-Squares estmaton z = H x+ v (z: διάνυσμα l x, x: διάνυσμα n x ) Εαν l n τότε: ( ) xˆ = H H H z least-squares estmate mni = z H xˆ z H xˆ ( ) ( ) (7) ( HRH) - - xˆ = HR z weghted least-squares estmate mni 2 = z H xˆ R z H xˆ (8) ( ) ( ) Determnstc optmal (n the least-squared sense) solutons: no probablty densty functon assocated wth x and z Σύνθεση αισθητηριακών πληροφοριών: Maxmum-Lelhood Estmaton z = H x+ v p(z x) ακολουθεί την κατανομή πιθανότητας p(v) Maxmum-lelhood estmate ˆx ˆx : μεγιστοποιεί την πιθανότητα p(z x), δεδομένων των στατιστικών ιδιοτήτων του σήματος θορύβου v στις μετρήσεις των αισθητήρων Εαν υποθέσουμε κανονική (Gaussan) κατανομή για το θόρυβο v, με μηδενική μέση τιμή και μήτρα διακύμανσης (covarance) R, τότε: p( z x) = e /2 /2 (2 π ) l R ( z H x ) R ( z H x) 2 [ p z x ] ( z H x) R ( z H x) max ( ) mn Άρα εδώ: maxmum-lelhood estmate weghted least-squares estmate (8) 2

7 Σύνθεση αισθητηριακών πληροφοριών: Mnmum-Varance Bayesan Estmate p( xz ) = p( z x) p( x) p( z) : Bayes theorem (a-posteror condtonal densty functon) ( p(x), p(z) : a-pror probablty densty functons ) Mnmum-varance Bayes estmate ˆx ˆx : ελαχιστοποιεί το συναρτησιακό: mn I=... x xˆ x xˆ p( x z) dxdx2... dxn S ( ) ( ) E {( ) x ˆ S( ˆ) } δηλαδή: I= varance( xz ) = x x x x z (διακύμανση) (S: θετικά ορισμένη μήτρα nxn) απ όπου παίρνουμε: xˆ =... x p( x z) dxdx2 dx n = Ex ( x z) και εάν x και v ακολουθούν κανονική (Gaussan) κατανομή, έχουμε: ( ) (9) xˆ = P 0 + H R H H R z όπου P 0 : a-pror covarance matrx of x = cov(x) 3 Σύνθεση αισθητηριακών πληροφοριών: Αναδρομικά φίλτρα (recursve lnear flters) Απλό Παράδειγμα-2: Εκτίμηση του βαθμωτού μεγέθους x στη βάση μετρήσεων: z =h x+v (=,,) z = xˆ( ) = h νέα μέτρηση z + ˆ( ) h + x z ˆ( ) x h h + = + z z + + = = + + = + xˆ( + ) = xˆ( ) + h z ˆ( ) ˆ( ) h ή x + = x + z h xˆ( ) + + Γενικεύοντας για διανυσματικά μεγέθη z, x: z =H x+v (=,,) και εισάγοντας και δυναμική στο χώρο κατάστασης: x + = Ax + Bu + w xˆ( + ) = xˆ( + ) + K z H xˆ( + ) όπου: xˆ( + ) = A xˆ( ) + B u (0) 4

8 Σύνθεση αισθητηριακών πληροφοριών Φίλτρο Kalman () Φίλτρο Kalman Υποθέσεις (assumptons). Γραμμικό μοντέλο συστήματος: x( + ) = A x( ) + B u( ) + w( ) 2. Γραμμικό μοντέλο μετρήσεων: z( ) = H x( ) + v( ) 3. Θόρυβος συστήματος: Γκαουσιανός, λευκός, με μηδενική μέση τιμή E[w ]=0, και ασυσχέτιστος (uncorrelated) covarance matrx: C ( ) = E[( w )( w ) ] w 4. Αντίστοιχα για το θόρυβο μετρήσεων v(), E[v ]=0, C ( ) = R = E[( v )( v ) ] v xˆ( + ) = A xˆ( ) + B u (lnear plant model) (K-) Έστω: ( ) E P = e( ) e ( ) (state covarance matrx) όπου: e( ) = x( ) xˆ ( ) (K-) Ποιό το: ( ) E P + = e( + ) e ( + ) όπου: e( + ) = x( + ) xˆ ( + ) 5 Σύνθεση αισθητηριακών πληροφοριών Φίλτρο Kalman (2) Είναι: ˆ ˆ P( + ) = E x( + ) x( + ) x( + ) x( + ) όπου: x( + ) = A x( ) + B u + w και: xˆ( + ) = A xˆ( ) + B u Άρα: ˆ ˆ x x w x x w P( + ) = E A ( ) ( ) + A ( ) ( ) + + = + P( ) A P( ) A C ( ) (state covarance matrx) (K-2) : Μήτρα συνδιακύμανσης της πρόβλεψης w υποδηλώνει πώς εξελίσσεται χρονικά η διακύμανση της μεταβλητής κατάστασης του συστήματος, δηλαδή πώς μεταβάλλεται η «αξιοπιστία» της εκτίμησης την οποία έχουμε σε κάθε για την τρέχουσα κατάσταση x του συστήματος, χωρίς νέα δεδομένα ανάδρασης z (correctve feedbac). 6

9 Σύνθεση αισθητηριακών πληροφοριών Φίλτρο Kalman (3) Νέα ανανεωμένη εκτίμηση της κατάστασης (updated estmate) χρησιμοποιώντας δεδομένα ανάδρασης (μετρήσεις) και από τη χρονική στιγμή +: xˆ( + ) = xˆ( + ) + K z H xˆ( + ) r(+) r(+): nnovaton, dfference between actual sensor readng : and predcted sensor data : z + (K-3) H xˆ ( ) + + Εύρεση βέλτιστου κέρδους ανάδρασης του φίλτρου (Kalman gan) K + Υπολογισμός της νέας μήτρας διακύμανσης P(+) για την ανανεωμένη εκτίμηση xˆ ( + ) της κατάστασης του συστήματος 7 Σύνθεση αισθητηριακών πληροφοριών Φίλτρο Kalman (4) Εύρεση βέλτιστου κέρδους ανάδρασης του φίλτρου (Kalman gan) K + [ ] mn I = E e e = trace P Είναι: P( + ) = e( + ) e ( + ) όπου: e( + ) = x( + ) xˆ ( + ) E Δεδομένου ότι από την (Κ3): xˆ( + ) = ( I K H ) xˆ( + ) + K έχουμε: e( + ) = ( I K+ H+ ) e( + ) + K+ ( H+ x( + ) z+ ) Άρα: v P( + ) = I K H P( + ) I K H + K R K z (K-4) Ισχύει όμως: A ABA 2 AB, όταν: B =B (δηλ.: B συμμετρική μήτρα) = 8

10 Σύνθεση αισθητηριακών πληροφοριών Φίλτρο Kalman (5) Εύρεση βέλτιστου κέρδους ανάδρασης του φίλτρου Kalman (συνέχεια) mn I + = E e+ e+ = trace[ P+ ] Άρα πρέπει: { trace [ ( ) ] } + K P + = 0 I+ ( ) ( ) ( ) και από τη σχέση (Κ-4): I K H P + H + 2K R = 0 + = ( + ) + + ( + ) K P H H P H R (K-5) Bέλτιστο κέρδος ανάδρασης (optmal Kalman gan) 9 Σύνθεση αισθητηριακών πληροφοριών Φίλτρο Kalman (6) Υπολογισμός της νέας μήτρας διακύμανσης P(+) για την ανανεωμένη εκτίμηση xˆ ( + ) της κατάστασης του συστήματος Από τη σχέση (Κ-4): P( + ) = I K H P( + ) I K H + K R K Αντικαθιστώντας την (Κ-5): P ( + ) = I K H P ( + ) (K-4)

11 Σύνθεση αισθητηριακών πληροφοριών Φίλτρο Kalman: Σύνοψη Αρχική εκτίμηση x ˆ( = 0) με C w ( ) και C ( ) = R, ( =, ) v xˆ( + ) = A xˆ( ) + B u (lnear plant model) (K-) P( + ) = A P( ) A + C w( ) (state covarance matrx) (K-2) μέ Νέα ανανεωμένη εκτίμηση κατάστασης και μήτρα διακύμανσης xˆ( + ) = xˆ( + ) + K z H x ˆ( + ) (K-3) P ( + ) = I K H P ( + ) (K-4) όπου, βέλτιστο κέρδος ανάδρασης (optmal Kalman gan) + = ( + ) + + ( + ) K P H H P H R (K-5) 2 Σύνθεση αισθητηριακών πληροφοριών Παραδείγματα Παράδειγμα Έστω ολονομικό (omn-drectonal) ρομπότ πάνω στο επίπεδο -xy Το διάνυσμα κατάστασης του ρομπότ είναι: x() = [ x(), y() ] Το διάνυσμα σημάτων ελέγχου είναι: u() = [ Δx(), Δy()] x( ) +Δ x( ) + wx ( ) Η εξίσωση κατάστασης είναι: x( + ) = y ( ) +Δ y ( ) + wy ( ) (δηλαδή, Α=Ι) (Π.-) Έστω ότι το ρομπότ διαθέτει αισθητήρα που παρέχει μια μέτρηση της θέσης [x(),y()] του ρομπότ από ένα σημείο αναφοράς [0,0] x( ) + vx( ) 0 x ( ) vx( ) ( ) = Δηλαδή: z = + y ( ) + vy( ) 0 y ( ) vy( ) (Π.-2) H x( ) v( ) 22

12 Σύνθεση αισθητηρίων πληροφοριών Παραδείγματα (συνέχεια) () Παράδειγμα (συνέχεια) Έστω C w ()= C w =σταθ. και C v ()=R=C v =σταθ. Έχουμε, εφαρμόζοντας το αναδρομικό φίλτρο Kalman: με xˆ( ) + Δx( ) xˆ( + ) = y ˆ( ) +Δy ( ) (Π.-3) P( + ) A P( ) A C ( ) = P( ) C = + w + Βέλτιστο κέρδος ανάδρασης φίλτρου Kalman (H=I): K = P( ) P( ) + R w (Π.-5) (Π.-4) Νέα ανανεωμένη εκτίμηση κατάστασης και νέα μήτρα διακύμανσης = + πρόβλεψη/εκτίμηση κατάστασης (a-pror) αβεβαιότητα πρόβλεψης Kalman (optmal) feedbac gan xˆ( + ) xˆ( + ) K+ z ˆ( ) + x + και = + (Π.-6) P( + ) I K P( + ) 23 Σύνθεση αισθητηρίων πληροφοριών Παραδείγματα (συνέχεια) (2) Παράδειγμα (συνέχεια) 2 2 Έστω, π.χ.: Cw = dag [ σw ( ), σ ( )] x w y Έχουμε, εφαρμόζοντας το αναδρομικό φίλτρο Kalman: P ( + ) = P ( ) + C dag [( 2 ( ) 2 ( )),( 2 ( ) 2 w = σx + σw σ ( ))] x y + σw y Βέλτιστο κέρδος ανάδρασης φίλτρου Kalman (H=I): K ( + ) = P ( + ) [ P ( + ) + R] - = σx + σw 0 σ 0 x x + σw + σ x v x = σy + σw 0 σ y y + σw + σ y vy ( σx + σw ) ( σ ) 0 x x + σw + σ x v x = ( σy + σw ) ( σ ) y y + σw + σ y vy Νέα ανανεωμένη εκτίμηση κατάστασης σ σ σx + σw x xˆ = x + ( z ) x x σx + σw + σ y + wy yˆ = y + ( z ) y y x v σ x y + σw + σ y vy - 24

13 Σύνθεση αισθητηρίων πληροφοριών Παραδείγματα (συνέχεια) (3) Παράδειγμα 2 Επεκταμένο Φίλτρο Kalman (Extended KF) Έστω κινούμενο ρομπότ διαφορικής οδήγησης (dfferental drve) Το διάνυσμα κατάστασης του ρομπότ είναι: x() = [ x(), y(), θ() ] Το διάνυσμα σημάτων ελέγχου είναι: u() = [ D(), Δθ() ] Το μη-γραμμικό μοντέλο του συστήματος στο χώρο κατάστασης: x( + ) = Φ[ x( ), u( ) ] + w( ) με Φ[ x u ] ( θ ) ( θ ) x( ) + D( )cos ( ) ( ), ( ) = y( ) + D( )sn ( ) θ( ) +Δθ( ) C w (): μήτρα διακύμανσης (covarance matrx) της τυχαίας διαταραχής w() 2 σ x Cw( ) = E ( w)( w) = 0 σ y 0 και [ w 2 ] 0 0 σθ E = 0 (Π.2-) (Π.2-2) 25 Σύνθεση αισθητηρίων πληροφοριών Παραδείγματα (συνέχεια) (4) Παράδειγμα 2 (συνέχεια) Έστω τώρα ότι το κινούμενο ρομπότ διαθέτει αισθητήριο σύστημα που δίνει την απόσταση d του ρομπότ από κάποια συγκεκριμένη θέση αναφοράς (ορόσημο, landmar): [x b, y b ] (π.χ. specal sgnal-emttng beacon) Το μη-γραμμικό μοντέλο μετρήσεων για το σύστημα, είναι τώρα: = h[ ξ] + v 2 [ x( ), ] = = ( ( ) b) + ( ( ) b) z( ) x( ), ( ) με ξ 2 2 h d x x y y και γνωστή μήτρα διακύμανσης C v ()= C v για το θόρυβο v() (Π.2-3) 26

14 Σύνθεση αισθητηρίων πληροφοριών Παραδείγματα (συνέχεια) (5) Παράδειγμα 2 (συνέχεια) ( ˆ θ ) ( ˆ θ ) xˆ( ) + D( )cos ( ) x ˆ( + ) = y ˆ( ) + D ( )sn ( ) ˆ( θ ) +Δθ ( ) ( ˆ θ ) ( ˆ θ ) (Π.2-4) 0 D ( )sn ( ) A = Φ = 0 D ( )cos ( ) (Π.2-5) x= xˆ ( ) 0 0 P( + ) = A P( ) A + C w( ) (Π.2-6) πρόβλεψη/εκτίμηση κατάστασης (a-pror) γραμμικοποίηση χώρου κατάστασης αβεβαιότητα πρόβλεψης 27 Σύνθεση αισθητηρίων πληροφοριών Παραδείγματα (συνέχεια) (6) Παράδειγμα 2 (συνέχεια) 2( x ˆ( ) xb ) H + = h x= xˆ ( ) = 2( y ˆ( ) yb) (Π.2-7) 0 Βέλτιστο κέρδος ανάδρασης φίλτρου Kalman: + = ( + ) + + ( + ) v + + K P H H P H C (Π.2-8) γραμμικοποίηση μοντέλου μέτρησης Kalman (optmal) feedbac gan Νέα ανανεωμένη εκτίμηση κατάστασης και νέα μήτρα διακύμανσης xˆ( + ) = xˆ( + ) +K r( + ) + με r( + ) = z h( xˆ ( + ) ) + (Π.2-9) και P ( + ) = I K H P ( + ) + + (Π.2-0) 28

15 Σύνθεση αισθητηρίων πληροφοριών Παραδείγματα (συνέχεια) (7) xˆ( ) xˆ( + ) xˆ( + ) Παράδειγμα 3: p x= p θ r z= b x y (range) (bearng) Αρχική Εκτίμηση Δράση / Κίνηση Μέτρηση D u= δθ x( + ) =Φ ((),()) x u + w() x + Φ( x( ), u( ) ) = y + θ h( x( ), u( ) ) = z( + ) = h((),()) x u + v() ( θ ) ( θ ) + δθ p ( ) D( )cos ( ) p ( ) D( )sn ( ) ( ) ( ) 2 2 ( λx px) + ( λy py) λy p y arc tan θ λx px 29 Σύνθεση αισθητηρίων πληροφοριών Παραδείγματα (συνέχεια) (8) Παράδειγμα 3 (συνέχεια): Πρόβλεψη Μοντέλο κίνησης A ( ˆ θ ) ( ˆ θ ) pˆ x ( ) + D( )cos ( ) xˆ( + ) = pˆ y ( ) + D( )sn ( ) ˆ( θ ) +Δθ ( ) x= xˆ ( ) = Φ = ( ˆ θ ) ( ˆ θ ) 0 D ( )sn ( ) 0 D ( )cos ( ) 0 0 P( + ) = A P( ) A + C ( ) w H Μοντέλο μέτρησης + = h = x= xˆ ( ) ( pˆx λx) ( pˆ y λy) 0 r r = ( y pˆ y) ( x pˆ λ λ x ) 2 2 r r + ( + ( )) xˆ( + ) = xˆ( + ) + K z h xˆ( + ) P ( + ) = I K H P ( + ) ( tan ) = ( + ) d a x x dx + = ( + ) + + ( + ) + + v K P H H P H C 30

16 Σύνθεση αισθητηρίων πληροφοριών Χαρτογράφηση χώρου Τροχιά ρομπότ Sensor map 3 Σύνθεση αισθητηρίων πληροφοριών Αυτοεντοπισμός Θέσης () Open loop no odometry Wth odometry Robot nematc uncertantes Τροχιά ρομπότ ρομπότ Wth odometry Precse robot nematcs (stochastc errors) Εκτίμηση θέσης ρομπότ Εκτίμηση θέσης ρομπότ 32

17 Σύνθεση αισθητηρίων πληροφοριών Αυτοεντοπισμός Θέσης (2) Εκτίμηση θέσης ρομπότ Τρέχουσα θέση ρομπότ οδομετρία βελτιωμένη εκτίμηση θέσης : Πραγματική τροχιά ρομπότ ο : Εκτίμηση θέσης ρομπότ με χρήση οδομετρίας (odometry readngs) + : Βελτιωμένη εκτίμηση θέσης ρομπότ (IR readngs geometrc landmars ) 33 Partcle Flters Sequental Monte Carlo Οι μέθοδοι Monte Carlo περιγράφουν, γενικά, κατανομές πιθανότητας ως σύνολα «σταθμισμένων δειγμάτων» (weghted samples) στο χώρο κατάστασης. Μελετώντας τα στατιστικά χαρακτηριστικά των δειγμάτων, καθώς εξελίσσονται μέσω της δυναμικής του συστήματος και της Bayesan λογικής, μπορεί να εκτιμηθεί, μέσω προσομοίωσης, η κατάσταση ενός στοχαστικού συστήματος. Partcle Flters («φίλτρα σωματίων»): sequental Marov Chan Monte Carlo (MCMC) μέθοδοι, βάσει mportance samplng («δειγματοληψία σημαντικότητας») Πρόβλημα: Η εκτίμηση της «εκ των υστέρων» (posteror) κατανομής πιθανότητας ( (=,,N) P x Z, U, x0 ) (w : βάρη δειγμάτων) η οποία προσεγγίζεται μέσω της κατανομής ενός συνόλου δειγμάτων {x,w } N P( x) wδ ( x x ) (όπου w = ) = 34

18 Partcle Flters - Εισαγωγή Έστω: x : διάνυσμα κατάστασης προς εκτίμηση τη χρονική στιγμή u : διάνυσμα σημάτων ελέγχου, εφαρμοζόμενο στο χρονικό διάστημα [, ], οδηγώντας την κατάσταση από το x - στο x. z : διάνυσμα μετρήσεων (παρατήρησης) που λαμβάνεται τη χρονική στιγμή Το ιστορικό των διανυσμάτων κατάστασης: X = { x } { 0, x,, x = X, x} Το ιστορικό των διανυσμάτων ελέγχου: U = { u } {, u2,, u = U, u} Το ιστορικό των διανυσμάτων παρατήρησης: Z = z, z,, = Z, z { 2 z } { } Μοντέλο Παρατήρησης (μέτρησης) Ανανέωση Εκτίμησης (Bayes rule) P( z x) P( x Z, U, x0 ) P( x Z, U, x0 ) = P z Z, U ( ) Μοντέλο Πρόβλεψης Μετάβασης Κατάστασης (Marov assumpton) P x Z, U, x = P x x, u P x Z, U, x dx ( ) ( ) ( ) Δειγματοληψία Importance Samplng Περιγραφή κατανομής πιθανότητας μέσω σταθμισμένων δειγμάτων: N P( x) wδ ( x x ) (όπου w = ) = Επιλογή βαρών (weghts), ώστε να προκύψει «πιστή» αναπαράσταση της εκτιμούμενης κατανομής πιθανότητας P(x): Importance Densty («πιθανότητα σημαντικότητας») q(x) Δειγματοληψία της q(x), και στάθμιση των δειγμάτων ως: P w q ( x ) ( x ) (α) Εαν q(x) ομοιόμορφη κατανομή πυκνότητας πιθανότητας σε όλο το χώρο κατάστασης, τότε: w P x (regular grd model) ( ) (β) Εαν q(x)=p(x), τότε w (x)=(/n) (partcle dstrbuton, wth equal weghts) 36

19 Δειγματοληψία Κατανομών () Προσέγγιση συναρτήσεων πυκνότητας πιθανότητας μέσω συνόλου δειγμάτων (approxmatng partcle sets) Όσο μεγαλύτερος αριθμός δειγμάτων σε ένα διάστημα, τόσο μεγαλύτερη πιθανότητα Πώς μπορεί να γίνει η εξαγωγή δειγμάτων από μια συνάρτηση πιθανότητας; 37 Δειγματοληψία Κατανομών (2) (α) Δειγματοληψία μέσω «Rejecton Samplng» Έστω ότι f(x)< for all x Sample x from a unform dstrbuton Sample c from [0,] f f(x) > c eep the sample otherwse reject the sample 38

20 Δειγματοληψία Κατανομών (3) (β) Δειγματοληψία μέσω «Importance Samplng» Χρήση διαφορετικής κατανομής q για την εξαγωγή δειγμάτων από την f Εισάγοντας ένα βάρος «σημαντικότητας» μπορούμε να σταθμίσουμε τα δείγματα, ισοσταθμίζοντας τις «διαφορές μεταξύ των q και f» w = f / q Η f είναι η συνάρτηση «στόχος» Η q καλείται συνάρτηση «σημαντικότητας» (mportance or proposal) 39 Partcle Flters Εισαγωγή () Στατιστική μέθοδος που βασίζεται στον κανόνα του Bayes και έχει χρησιμοποιηθεί σε πολλά συστήματα παρακολούθησης (tracng) αντικειμένων. Η βασική ιδέα της μεθόδου είναι η χρήση δειγμάτων (partcles) για την αναπαράσταση του μοντέλου του συστήματος (αντί Gaussan κατανομών ή οποιουδήποτε άλλου μοντέλου) και η διάδοση μόνο των επικρατέστερων δειγμάτων. ( Survval of the fttest ) Βασικά Βήματα Αλγορίθμων που χρησιμοποιούν partcle flterng: Τοποθέτηση Ν δειγμάτων (partcles) στο χώρο κατάστασης του συστήματος Ανάθεση βαρών (w ) στα δείγματα με βάση τις παρατηρήσεις (μετρήσεις) που είναι διαθέσιμες για το σύστημα Επαναδειγματοληψία (resamplng): δίνεται περισσότερη βαρύτητα στα δείγματα που έχουν μεγαλύτερο βάρος και "ακυρώνονται" τα δείγματα με μικρά βάρη Εξέλιξη δειγμάτων (με βάση το μοντέλο μετάβασης κατάστασης του συστήματος) Επανάληψη των παραπάνω από το δεύτερο βήμα. 40

21 Partcle Flters Εισαγωγή (2) 4 Partcle Flters () Βασικές Σχέσεις Αρχική Κατάσταση: η προγενέστερη εκ των υστέρων (posteror) κατανομή {δείγματα, βάρη} N P N { x, w } - = ( x ) = ( x x ) q P P ( x Z ) = w-δ ( x x ) Δειγματοληψία Importance Samplng: Μοντέλο μετάβασης κατάστασης Πρόβλεψη: N = Νέα Δείγματα ( x Z ) = w-δ ( x x) = x x από τα με w = w (mportance samplng based on the transton pror) Ανανέωση Βαρών Μοντέλο Παρατήρησης Βελτιωμένη Εκτίμηση: P N ( x Z ) = wδ ( x x) = (updated posteror) όπου w = ( = ) j P( = ) w P z x x - N j w- z x x j= πιθανοφάνεια μέτρησης 42

22 Partcle Flters (2) - Υλοποίηση Μετάβαση Κατάστασης: N { x, w } - = (,, ) = x f x n Draw N samples from P(n - ) Ανανέωση Παρατήρησης: z = h( x, v, ) ( v : nose sequence wth nown P(v ) ) { x, w } N = ( n : nose sequence wth nown P(n ) ) { x, w } N = με w = w Υπολογισμός «πιθανοφάνειας» (lelhoood) μέτρησης για κάθε δείγμα: Λ x = P z x = x ( ) ( ) { x, w } ( ) P( ) Λ = w w x w z x Επαναδειγματοληψία (resamplng): N = Για την αποφυγή προβλημάτων «εκφυλισμού» των δειγμάτων (δηλαδή καταστάσεων όπου όλα τα βάρη πλην ενός τείνουν στο μηδέν) επιχειρείται «επαναδειγματοληψία» των δειγμάτων σε τακτά διαστήματα ώστε τα δείγματα να παραμένουν σχετικώς ισοσταθμισμένα 43 Partcle Flters (3) - Resamplng Sequental Importance Resamplng (Σειριακή Επαναδειγματοληψία σημαντικότητας) Υπολογισμός κανονικοποιημένων βαρών (normalzed mportance weghts): w P( z x) w = N w P z x = Υπολογισμός ενεργού αριθμού δειγμάτων (effectve number of partcles): N eff = w ( ) 2 ( ) Έλεγχος επαναδειγματοληψίας με βάση τον ενεργό αριθμό δειγμάτων: Εαν N eff <N thr (π.χ. N thr =(N/2)) τότε παίρνουμε νέα δείγματα (=,,N) από το τρέχον σύνολο N {, } x w, με πιθανότητα ανάλογη των βαρών = («εξασθένησης» δειγμάτων) (αποφυγή sample mpovershment) Παίρνουμε τελικά Ν ισοσταθμισμένα δείγματα x με βάρη w = ( / N) =,..., N w 44

23 Εφαρμογή Partcle Flterng (-). Οπτική Παρακολούθηση Χειρονομιών (vsual hand gesture tracng) Πιθανότητα χρώματος ( (, ) (, )) w = λ p x y + λ dff x y w 2 Πιθανότητα κίνησης 45 Εφαρμογή Partcle Flterng (-2). Οπτική Παρακολούθηση Χειρονομιών (συνέχεια) 46

24 Εφαρμογή Partcle Flterng (2-) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (localsaton) Κάθε δείγμα (partcle) αντιστοιχεί σε μια δυνατή κατάσταση (θέση ή διάταξη) του ρομπότ στο χώρο Η εξέλιξη των δειγμάτων γίνεται με βάση την πιθανότητα (σφάλματα) που εισάγει το μοντέλο κίνησης (mportance or proposal dstrbuton predcton step) o μοντέλο παρατήρησης (μετρήσεων) χρησιμοποιείται για τον υπολογισμό νέων βαρών για τη στάθμιση των δειγμάτων και την επαναδειγματοληψία (correcton step) 47 Εφαρμογή Partcle Flterng (2-2) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) Μοντέλο Κίνησης 48

25 Εφαρμογή Partcle Flterng (2-3) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) Μοντέλο Παρατήρησης (actve rangng sensors) 49 Εφαρμογή Partcle Flterng (2-4) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ με sonars 50

26 Εφαρμογή Partcle Flterng (2-5) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ με sonars Αρχική Κατανομή 5 Εφαρμογή Partcle Flterng (2-6) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ με sonars Κατανομή μετά την ενσωμάτωση 0 scans 52

27 Εφαρμογή Partcle Flterng (2-7) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ με sonars Κατανομή μετά την ενσωμάτωση 65 scans 53 Εφαρμογή Partcle Flterng (2-8) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ με sonars Εκτιμούμενη Διαδρομή του ρομπότ 54

28 Εφαρμογή Partcle Flterng (2-9) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 55 Εφαρμογή Partcle Flterng (2-0) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 56

29 Εφαρμογή Partcle Flterng (2-) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 57 Εφαρμογή Partcle Flterng (2-2) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 58

30 Εφαρμογή Partcle Flterng (2-3) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 59 Εφαρμογή Partcle Flterng (2-4) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 60

31 Εφαρμογή Partcle Flterng (2-5) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 6 Εφαρμογή Partcle Flterng (2-6) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 62

32 Εφαρμογή Partcle Flterng (2-7) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 63 Εφαρμογή Partcle Flterng (2-8) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 64

33 Εφαρμογή Partcle Flterng (2-9) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 65 Εφαρμογή Partcle Flterng (2-20) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 66

34 Εφαρμογή Partcle Flterng (2-2) 2. Αυτοεντοπισμός Θέσης Κινούμενου Ρομπότ (συνέχεια) 67 Βιβλιογραφία A. Gelb (edtor), Appled Optmal Estmaton, he MI Press, 986. D. Bertseas, Dynamc Programmng and Control, Athena Scentfc: Mass., USA, 995. F. Lews, Optmal Estmaton, John Wley, 986. B. Anderson, J. Moore, Optmal Flterng, Prentce Hall, 979. S. hrun, W. Burgard and D. Fox, Probablstc Robotcs, MI Press, G. Dude, M. Jenn, Computatonal Prncples of Moble Robotcs, Cambrdge Unversty Press, R. Segwart, I. R. Nourbahsh, Introducton to Autonomous Moble Robots, he MI Press, Cambrdge, MA, 2004 Mchael Montemerlo, Sebastan hrun, FastSLAM A Scalable Method for the Smultaneous Localzaton and Mappng Problem n Robotcs, Sprnger racts n Advanced Robotcs, Sclano, Bruno; Khatb, Oussama (Eds.), Sprnger Handboo of Robotcs, Part C: Sensng and Percepton, Sprnger