CSE-571 Robotics Extremely noisy (nonlinear) motion of observer - PowerPoint PPT Presentation

Ball Tracking in RoboCup CSE-571 Robotics § Extremely noisy (nonlinear) motion of observer Rao-Blackwelized Particle § Inaccurate sensing, limited processing Filters for State Estimation power § Interactions between target and environment Goal: Unified framework for modeling the ball § Interactions between robot(s) and target and its interactions. Dieter Fox CSE-571: Probabilistic Robotics 2 Tracking Techniques Dynamic Bayes Network for Ball Tracking Robot localization § Kalman Filter l l l z k-2 z k-1 z k Landmark detection § Highly efficient, robust (even for nonlinear) r k-2 r k-1 r k Map and robot location § Uni-modal, limited handling of nonlinearities u k-2 u k-1 Robot control § Particle Filter m m m Ball motion mode k-2 k-1 k § Less efficient, highly robust Ball tracking § Multi-modal, nonlinear, non-Gaussian b b b Ball location and velocity k-2 k-1 k § Rao-Blackwellised Particle Filter, MHT § Combines PF with KF b b b z k-2 z k-1 z k Ball observation § Multi-modal, highly efficient Dieter Fox CSE-571: Probabilistic Robotics 3 Dieter Fox CSE-571: Probabilistic Robotics 4 1

Robot and Ball Location (and Robot Location velocity) Robot localization Robot localization l l l l l l z k-2 z k-1 z k Landmark detection z k-2 z k-1 z k Landmark detection r k-2 r k-1 r k r k-2 r k-1 r k Map and robot location Map and robot location u k-2 u k-1 u k-2 u k-1 Robot control Robot control m m m m m m Ball motion mode Ball motion mode k-2 k-1 k k-2 k-1 k Ball tracking Ball tracking b b b b b b Ball location and velocity Ball location and velocity k-2 k-1 k k-2 k-1 k b b b b b b z k-2 z k-1 z k Ball observation z k-2 z k-1 z k Ball observation Dieter Fox CSE-571: Probabilistic Robotics 5 Dieter Fox CSE-571: Probabilistic Robotics 6 Ball-Environment Interactions Ball-Environment Interactions (0.8) Robot loses grab (residual prob .) (0.2) None Grabbed None Grabbed Within grab range and robot grabs (prob. from model ) Robot kicks ball (0.9) Kick fails (0.1) Bounced Deflected Bounced Deflected Kicked Kicked Dieter Fox CSE-571: Probabilistic Robotics 7 Dieter Fox CSE-571: Probabilistic Robotics 8 2

Integrating Discrete Ball Motion Mode Grab Example (1) Robot localization Robot localization l l l l l l z k-2 z k-1 z k Landmark detection z k-2 z k-1 z k Landmark detection r k-2 r k-1 r k r k-2 r k-1 r k Map and robot location Map and robot location u k-2 u k-1 u k-2 u k-1 Robot control Robot control m m m m m m Ball motion mode Ball motion mode k-2 k-1 k k-2 k-1 k Ball tracking Ball tracking b b b b b b Ball location and velocity Ball location and velocity k-2 k-1 k k-2 k-1 k b b b b b b z k-2 z k-1 z k Ball observation z k-2 z k-1 z k Ball observation Dieter Fox CSE-571: Probabilistic Robotics 9 Dieter Fox CSE-571: Probabilistic Robotics 10 Inference: Posterior Grab Example (2) Estimation Robot localization Robot localization l l l l l l z k-2 z k-1 z k Landmark detection z k-2 z k-1 z k Landmark detection r k-2 r k-1 r k r k-2 r k-1 r k Map and robot location Map and robot location u k-2 u k-1 u k-2 u k-1 Robot control Robot control m m m m m m Ball motion mode Ball motion mode k-2 k-1 k k-2 k-1 k Ball tracking Ball tracking b b b b b b Ball location and velocity Ball location and velocity k-2 k-1 k k-2 k-1 k b b b b b b z k-2 z k-1 z k Ball observation z k-2 z k-1 z k Ball observation b l p ( b , m , r | z , z , u ) - k k k 1 : k 1 : k 1 : k 1 Dieter Fox CSE-571: Probabilistic Robotics 11 Dieter Fox 12 3

Rao-Blackwellised Particle Filter for Rao-Blackwellised PF for Inference Inference § Represent posterior by random samples Robot localization § Each sample = = q µ S s r , m , b x , y , , m , , r k-1 r k Map and robot location i i i i i i i contains robot location, ball mode, ball Kalman m m Ball motion mode k-1 k filter Ball tracking § Generate individual components of a particle b b Ball location and velocity k-1 k stepwise using the factorization = p ( b , m , r | z , u ) k 1 : k 1 : k 1 : k 1 : k - 1 § Draw a sample from the previous sample set: × p ( b | m , r , z , u ) p ( m | r , z , u ) p ( r | z , u ) - - - k 1 : k 1 : k 1 : k 1 : k 1 1 : k 1 : k 1 : k 1 : k 1 1 : k 1 : k 1 : k 1 ( i ) ( i ) ( i ) r , m , b - - - k 1 k 1 k 1 Dieter Fox CSE-571: Probabilistic Robotics 13 Dieter Fox CSE-571: Probabilistic Robotics 14 Generate Robot Location Generate Ball Motion Model Robot localization Robot localization l l z k Landmark detection z k Landmark detection r k-1 r k r k-1 r k Map and robot location Map and robot location u k-1 u k-1 Robot control Robot control m m m m Ball motion mode Ball motion mode k-1 k k-1 k Ball tracking Ball tracking b b b b Ball location and velocity Ball location and velocity k-1 k k-1 k ( i ) ( i ) ( i ) ( i ) Þ ( i ) ( i ) ( i ) ( i ) ( i ) Þ ( i ) ( i ) r ~ p ( r | r , m , b , z , u ) r , _, _ m ~ p ( m | r , m , b , z , u ) r , m , _ - - - - - - - k k k 1 k 1 k 1 k k 1 k k k k k 1 k 1 k k 1 k k Dieter Fox CSE-571: Probabilistic Robotics 15 Dieter Fox CSE-571: Probabilistic Robotics 16 4

Update Ball Location and Importance Resampling Velocity § Weight sample by Robot localization l z k Landmark detection ( i ) µ l ( i ) w p ( z | r ) k k k if observation is landmark detection and by r k-1 r k Map and robot location u k-1 Robot control ( i ) µ b ( i ) ( i ) ( i ) w p ( z | m , r , b ) k k k k k - 1 m m Ball motion mode k-1 k ò = b ( i ) ( i ) ( i ) ( i ) ( i ) ( i ) ( i ) p ( z | m , r , b ) p ( b | m , r , b ) d b - k k k k k k k k 1 k Ball tracking if observation is ball detection. b b Ball location and velocity k-1 k b z k § Resample Þ ( i ) ( i ) ( i ) ( i ) ( i ) ( i ) ( i ) b ~ p ( b | r , m , b , z ) r , m , b - k k k k k 1 k k k k Dieter Fox CSE-571: Probabilistic Robotics 17 Dieter Fox CSE-571: Probabilistic Robotics 18 Ball-Environment Interaction Ball-Environment Interaction Dieter Fox CSE473: Introduction to AI 19 Dieter Fox CSE473: Introduction to AI 20 5

Tracking and Finding the Ball Experiment: Real Robot § Robot kicks ball 100 times, tries to find it § Cluster ball samples by discretizing afterwards pan / tilt angles § Finds ball in 1.5 seconds on average § Uses negative information 0.5 With Map Without Map 0.45 Percentage of ball lost 0.4 0.35 0.3 0.25 0.2 0.15 0.1 0.05 0 0 5 10 15 20 25 30 35 40 45 50 Number of ball samples Dieter Fox CSE-571: Probabilistic Robotics 21 Dieter Fox 22 Comparison to KF* (optimized for straight motion) Simulation Runs Reference RBPF * Observations KF* Reference * Observations Dieter Fox CSE-571: Probabilistic Robotics 23 Dieter Fox CSE-571: Probabilistic Robotics 24 6

Comparison to KF ’ (inflated prediction noise) Error vs. Prediction Time RBPF KF’ Reference 80 * Observations RBPF KF' 70 KF* 60 RMS Error [cm] 50 40 30 20 10 0 0 0.5 1 1.5 2 Prediction time [sec] Dieter Fox CSE-571: Probabilistic Robotics 25 Dieter Fox CSE-571: Probabilistic Robotics 26 Orientation Errors Goalie 180 RBPF KF* 160 KF' Orientation Error [degrees] 140 120 100 80 60 40 20 0 2 3 4 5 6 7 8 9 10 11 Time [sec] Dieter Fox CSE-571: Probabilistic Robotics 27 Dieter Fox CSE-571: Probabilistic Robotics 28 7

Discussion § Particle filters are intuitive and simple § Support point-wise thinking (reduced uncertainty) § Good for test implementation if system behavior is not well known § Inefficient compared to Kalman filter § Rao-Blackwellization § Only sample discrete / highly non-linear parts of state space § Solve remaining part analytically (KF,discrete) Dieter Fox CSE-571: Probabilistic Robotics 29 8