Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431...
Transcript of Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431...
![Page 1: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/1.jpg)
Remembering Where and When
COMP3431 Robot Software Architectures
![Page 2: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/2.jpg)
This time ...
• We look at the robot equivalent of spatiotemporal memory
![Page 3: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/3.jpg)
Probabilistic Robotics
![Page 4: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/4.jpg)
Localisation with Landmarks
![Page 5: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/5.jpg)
Localisation with Edges
Angle a1
distance d1distance d2
length g
width w
(0,0)
x,y,theta
![Page 6: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/6.jpg)
Errors
• Measurement errors
• sensors are never 100% accurate
• Process errors
• actions never do exactly what they’re supposed to
![Page 7: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/7.jpg)
Example
• Estimating distance to the ball
![Page 8: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/8.jpg)
! !
Measurement ErrorExperiments determine errors in distance estimation
![Page 9: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/9.jpg)
Localisation with Landmarks
(x, y)
Current robot position according
to world model
h
dNew position estimate after taking into
account distance to landmark
![Page 10: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/10.jpg)
Localisation with Landmarks
(x, y)
h
d
Updated position
New position estimate after taking into
account distance to landmark
![Page 11: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/11.jpg)
Conditional Probability
P(E|H) ≡ the probability of observing E if H is true
![Page 12: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/12.jpg)
Bayes’ Rule• Probability I have a cold if you hear me cough
• I.e. if we know the prior probability that I have a cold (without any evidence) and I know that a cold causes a cough, with some probability, then we can calculate the posterior probability
P(cold | cough)∝P(cold)× P(cough | cold)
![Page 13: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/13.jpg)
Probabilistic Inference• Make inferences using probabilities
• Based on Bayes’ rule:
bel(xt )∝bel(xt−1) × prob(observation)
P(H | E)∝P(H )× P(E |H )
or
![Page 14: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/14.jpg)
Updating State Estimate• The Kalman filter is commonly used to update the estimate of the robot’s
state
• Two phases:
1. Prediction (Process Update)
• predicts where the robot will be after performing an action
2. Correction (Observation Update)
• use observations to correct prediction
• What follows is only a sketch of the Kalman filter
• It’s nowhere near the complete algorithm
![Page 15: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/15.jpg)
Simplification
• Only one measurement and action
• When there are more, must account for all interactions
• Math becomes more complex
• Scalar variables are replaced by matrices
![Page 16: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/16.jpg)
Process Update (Simplified)
• is the predicted new state after action, u
• Update variance with process noise, Q
• errors accumulate
x← x + u
varx ← varx +Q
x
![Page 17: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/17.jpg)
Measurement Update• Move position estimate toward measurement
estimate but proportional to error estimates
• z is the measured state
• new state estimate is predication plus difference between prediction and measurement, proportional to confidence in measurement
x← x + K × z − x( )
![Page 18: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/18.jpg)
Measurement Update
• update x by the difference in the measured value, z, and the expected value, x, scaled by how much we trust the observation
• If measurement is certain, new state becomes measured state
• otherwise, make change proportional to difference between measurement and prediction
x← x + K × z − x( )if K = 1
x← x + z − xx← z
![Page 19: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/19.jpg)
Measurement Update (Simplified)
• As measurement error R approaches 0, actual measurement z is trusted more and more, while predicted measurement x̅ is trusted less and less.
• As state error estimate varx approaches 0, actual measurement z is trusted less and less, while predicted measurement x̅ is trusted more and more.
K ← varxvarx + R
x← x + K × z − x( )
![Page 20: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/20.jpg)
Robot estimating state of door
![Page 21: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/21.jpg)
Initial beliefs
bel X0 = open( ) = 0.5bel X0 = closed( ) = 0.5
• Door can be in one of two states, open or closed
• Represented by state variable, X
• Initially, X has equal probability of being open or closed
![Page 22: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/22.jpg)
Measurement Noise• Specify the probability of sensor given correct answer
• Z is the measurement, X is the actual value
• Only 0.2 error probability when door is closed
• but 0.4 error probability when door is open
p Zt = sense_open Xt = is_open( ) = 0.6p Zt = sense_closed Xt = is_open( ) = 0.4p Zt = sense_open Xt = is_closed( ) = 0.2p Zt = sense_closed Xt = is_closed( ) = 0.8
![Page 23: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/23.jpg)
Process Noise• Robot uses its manipulator to push door open
• If already open, door stays open
• If closed, robot has 0.8 chance that door will be open after a push
p Xt = is_openUt = push,Xt−1 = is_open( ) = 1p Xt = is_closedUt = push,Xt−1 = is_open( ) = 0p Xt = is_openUt = push,Xt−1 = is_closed( ) = 0.8p Xt = is_closedUt = push,Xt−1 = is_closed( ) = 0.2
![Page 24: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/24.jpg)
Process Noise• The robot may do nothing
• World does not change
p Xt = is_openUt = do_nothing,Xt−1 = is_open( ) = 1p Xt = is_closedUt = do_nothing,Xt−1 = is_open( ) = 0p Xt = is_openUt = do_nothing,Xt−1 = is_closed( ) = 0p Xt = is_closedUt = do_nothing,Xt−1 = is_closed( ) = 1
![Page 25: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/25.jpg)
Probabilistic Robotics
• Belief in a state variable x at time t is its probability at t given all past measurements and actions:
• Belief after action ut but before observation zt, i.e. after prediction but before correction:
bel xt( ) = p xt z1..t ,u1..t( )
bel xt( ) = p xt z1..t−1,u1..t( )
![Page 26: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/26.jpg)
Bayes’ Rule• Don’t have to use entire history
• Use Bayes’ Rule
• Belief is a probability distribution over state variable
• Update must sum probabilities of outcomes of actions for each possible value
bel(xt )∝ prob(observation)× bel(xt−1)
![Page 27: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/27.jpg)
Example• If door is open and robot pushes,
what is the outcome?
• If door is open and robot does nothing, what is the outcome?
• If door is closed and robot pushes, what is the outcome?
• If door is closed and robot does nothing, what is the outcome?
![Page 28: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/28.jpg)
Bayes FilterFor all state variables
Predict value after the next action
Update the value based on the next measurement
Prediction for xt is the sum of predictions for each value of xt
Update prediction by last observation
η is a normalising factor to keep probabilities in 0 .. 1.
forall xt do
bel xt( ) = p xt ut , xt−1( )∫ bel xt−1( ) dxt−1
bel xt( ) =η p zt xt( ) bel xt( )
![Page 29: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/29.jpg)
Example• At t = 1 the robot takes no action but senses an open door
• u1 = do_nothing
• z1 = sense_open
bel x1( ) = p x1 u1, x0( )∫ bel x0( ) dx0= p x1 u1, x0( ) bel x0( )
x0∑
= p X1 Ut = do_nothing,Xo = is_open( ) bel X0 = is_open( )+ p X1 Ut = do_nothing,Xo = is_closed( ) bel X0 = is_closed( )
Integral becomes a sum because values of x are
discrete
![Page 30: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/30.jpg)
ExampleSubstitute values for X1
bel x1 = is_open( )= p X1 = is_openUt = do_nothing,Xo = is_open( ) bel X0 = is_open( )+ p X1 = is_openUt = do_nothing,Xo = is_closed( ) bel X0 = is_closed( )
= 1× 0.5 + 0 × 0.5= 0.5
bel x1 = is_closed( )= p X1 = is_closedUt = do_nothing,Xo = is_open( ) bel X0 = is_open( )+ p X1 = is_closedUt = do_nothing,Xo = is_closed( ) bel X0 = is_closed( )
= 0 × 0.5 +1× 0.5= 0.5
![Page 31: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/31.jpg)
Measurement Updatebel x1( ) =η p z1 = sense_open x1( ) bel x1( )
bel x1 = is_open( ) =η p z1 = sense_open x1 = is_open( ) bel x1 = is_open( )=η × 0.6 × 0.5=η × 0.3
bel x1 = is_closed( ) =η p z1 = sense_open x1 = is_closed( ) bel x1 = is_closed( )=η × 0.2 × 0.5=η × 0.1
η = 10.3+ 0.1
= 2.5
bel x1 = is_open( ) = 0.75
bel x1 = is_closed( ) = 0.25
Normalise to ensure that probabilities add up to 1
![Page 32: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/32.jpg)
Iterate for more actionsIf the next action is push and the measurement is sense_open:
bel x1 = is_open( ) = 1× 0.75 + 0.8 × 0.25 = 0.95
bel x1 = is_closed( ) = 0 × 0.75 + 0.2 × 0.25 = 0.05
and
bel x1 = is_open( ) =η × 0.6 × 0.95 ≈ 0.983
bel x1 = is_closed( ) =η × 0.2 × 0.05 ≈ 0.017
![Page 33: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/33.jpg)
Position Tracking• Robot moves
• Predict new position based on what motor actions are expected to do
• Measure
• Uses sensors to estimate motion
• Update position estimate (often a Kalman Filter)
![Page 34: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/34.jpg)
RoboCup Localisation• Estimates of robot and ball
positions include variance (or error)
• Robot has errors in
• x
• y
• heading
• Robot variance is shown as an ellipse and sector
![Page 35: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/35.jpg)
Particle Filter
Randomly guess position
Move particles according to motion model, including process noise
Weight particles by probability that measurement corresponds to particle
Drop low weight particles and resample
1.
2.
3.
4.
![Page 36: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/36.jpg)
Particle Filterhttps://github.com/mjl/particle_filter_demo
![Page 37: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/37.jpg)
Particle FilterParticleFilter(χ t−1,ut , zt )χ t = χ t =∅for m = 1 to M :
sample xt[m] ~ p xt |ut ,xt−1
[m]( )wt[m] = p zt | xt
[m]( )χ t = χ t + xt
[m],wt[m]
for m = 1 to M :draw i with probabilityα wt
[i]
add xt[i] to χ t
return χ t
Each particle is a hypothesis of what the state is at time t
Sample from the state transition distribution
Weight is probability of measurement for particle
Resample by weight}
![Page 38: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/38.jpg)
Measurement Errors• Position tracking usually uses wheel encoders to
estimate motion
• Unreliable in rescue robots
• We use lasers and RGB-D cameras
• Estimate motion from difference in successive scans
![Page 39: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/39.jpg)
Simultaneous Localisation and Mapping (SLAM)
• Depth sensors also give distance to objects
• Similar estimation methods can be used to update map
![Page 40: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/40.jpg)
Loop Closure (Full SLAM)
• Position tracking alone will accumulate errors
• If the robot recognise a landmark that it has seen before
• it can correct drift by updating estimate based on measurement of landmark
• Error correction is back-propagated
![Page 41: Remembering Where and When - webcms3.cse.unsw.edu.au€¦ · Remembering Where and When COMP3431 Robot Software Architectures. This time ... • We look at the robot equivalent of](https://reader033.fdocuments.us/reader033/viewer/2022052805/605ab44c6a51f3745c05cf32/html5/thumbnails/41.jpg)
Probabilistic Robotics• Position tracking, mapping, localisation
• How confident are we that a robot arm has gripped an object?
• Is what I’m seeing really a ball or is it a cylinder, end-on?
• Is the ground ahead a flat, traversable surface or is it the surface of a deep lake?
• If I drive into that obstacle, what are the chances that it’s a bush that I can go over or it’s a boulder that I’ll crash into?
• How confident is my autonomous car in detecting pedestrians?