Multirobot autonomous landmine detection using distributed ...
Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc...
-
Upload
sharleen-nicholson -
Category
Documents
-
view
230 -
download
0
Transcript of Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS LOCALIZATION AND MAPPING’ By Marc...
Marginal Particle and Multirobot Slam: Marginal Particle and Multirobot Slam: SLAM=‘SIMULTANEOUS SLAM=‘SIMULTANEOUS
LOCALIZATION AND MAPPING’LOCALIZATION AND MAPPING’
By Marc SobelBy Marc Sobel
(Includes references to Brian Clipp(Includes references to Brian Clipp
Comp 790-072 Robotics)Comp 790-072 Robotics)
The SLAM ProblemThe SLAM Problem
Given Given Robot controlsRobot controls Nearby measurementsNearby measurements
EstimateEstimate Robot state (position, orientation)Robot state (position, orientation) Map of world featuresMap of world features
SLAM ApplicationsSLAM Applications
Images – Probabilistic RoboticsImages – Probabilistic Robotics
Indoors
Space
Undersea
Underground
OutlineOutline
SensorsSensors SLAMSLAMFull vs. Online SLAMFull vs. Online SLAMMarginal SlamMarginal SlamMultirobot marginal slamMultirobot marginal slamExample AlgorithmsExample Algorithms
Extended Kalman Filter (EKF) SLAMExtended Kalman Filter (EKF) SLAMFastSLAM (particle filter)FastSLAM (particle filter)
Types of SensorsTypes of Sensors
OdometryOdometry Laser Ranging and Detection (LIDAR)Laser Ranging and Detection (LIDAR) Acoustic (sonar, ultrasonic)Acoustic (sonar, ultrasonic) RadarRadar Vision (monocular, stereo etc.)Vision (monocular, stereo etc.) GPSGPS Gyroscopes, Accelerometers (Inertial Gyroscopes, Accelerometers (Inertial
Navigation)Navigation) Etc.Etc.
Sensor CharacteristicsSensor Characteristics
NoiseNoise Dimensionality of OutputDimensionality of Output
LIDAR- 3D pointLIDAR- 3D point Vision- Bearing only (2D ray in space)Vision- Bearing only (2D ray in space)
RangeRange Frame of ReferenceFrame of Reference
Most in robot frame (Vision, LIDAR, etc.)Most in robot frame (Vision, LIDAR, etc.) GPS earth centered coordinate frameGPS earth centered coordinate frame Accelerometers/Gyros in inertial coordinate frameAccelerometers/Gyros in inertial coordinate frame
A Probabilistic ApproachA Probabilistic Approach
Notation:Notation:
1:t
1:t
1:t
t 1:t
t
(1:N)1:t
p(x ,m | z ,u ) = 'joint probability of pose and map'
x = State of the robot at timet
m = Map of the environment from times 1 to t at landmarks 1, ...,N
z = Sensor inputs from time 1 to t
u = Co
1:t
(1) (N)
ntrol (odometry) imputs from time 1 to t;
θ = rotations from one pose to another;
L = (L ,...,L )
Full vs. Online classical SLAMFull vs. Online classical SLAM
Full SLAM calculates the robot pose over Full SLAM calculates the robot pose over all time up to time t given the signal and all time up to time t given the signal and odometry:odometry:
Online SLAM calculates the robot pose for the Online SLAM calculates the robot pose for the current time tcurrent time t
1:t1:t 1:tp(x ,m | z ,u )
1:tt 1:t 1:t 1:t 1:t 1 tp(x ,m | z ,u ) = ..... p(x ,m | z ,u )dx ....dx
Full vs. Online SLAMFull vs. Online SLAM
),|,p( :1:1:1 ttt uzmx
Full SLAM Online SLAM
121:1:1:1:1:1 ...),|,(),|,p( ttttttt dxdxdxuzmxpuzmx
Classical Fast and EKF SlamClassical Fast and EKF Slam
Robot Environment: Robot Environment: (1) N distances: (1) N distances: mmtt ={d(x ={d(xtt,L,L11)) ,….,d(x,….,d(xtt,L,LNN)) };};
m measures distances from landmarks at m measures distances from landmarks at time t.time t. (2) Robot pose at time t: x(2) Robot pose at time t: x t.t.
(3) (Scan) Measurements at time t: z(3) (Scan) Measurements at time t: z tt
Goal: Determine the poses xGoal: Determine the poses x1:T 1:T
given scans zgiven scans z1:t1:t,odometry u,odometry u1:T1:T, and map, and map
measurements m.measurements m.
EKF SLAM (Extended Kalman EKF SLAM (Extended Kalman Filter)Filter)
As the state vector moves, the robot pose moves As the state vector moves, the robot pose moves according to the motion function, g(uaccording to the motion function, g(utt,,xxtt). This can be ). This can be
linearized into a Kalman Filter via:linearized into a Kalman Filter via:
The Jacobian J depends on translational and rotational The Jacobian J depends on translational and rotational velocity. This allows us to assume that the motion and velocity. This allows us to assume that the motion and hence distances are Gaussian. We can calculate the hence distances are Gaussian. We can calculate the mean mean μμ and covariance matrix and covariance matrix ΣΣ for the particle x for the particle xt t at time at time
t.t.
t t-1 t t-1 t-1 t-1 tg(u ,x ) = g(u ,μ ) + J(x - μ ) + N(0,R )
Outline of EKF SLAMOutline of EKF SLAM
By what preceded we assume that the map By what preceded we assume that the map vectors m (measuring distance from landmarks) vectors m (measuring distance from landmarks) are independent multivariate normal: Hence we are independent multivariate normal: Hence we now have:now have:
(1) (1,k) (1,k) (1)1:t 1:t 1:t 1:t
(M) (M,k) (M,k) (M)1:t 1:t 1:t 1:t
Particle 1 : x ,m μ ,Σ ;
......................
Particle M : x ,m μ ,Σ ;
N
N
Conditional IndependenceConditional Independence
For constructing the weights associated with the For constructing the weights associated with the classical fast slam algorithm, under moderate classical fast slam algorithm, under moderate assumptions, we get:assumptions, we get:
We use the notation,We use the notation, And calculate that:And calculate that:
1:t 1:t 1:t 1:t 1:t 1:t 1:t 1:tP x ,m | z ,u = P(m | z ,u )P(x | z ,u )
' function of g, , ';
' on the EKF expansion'
a
depending
(1,k)1:t
(1)1:t
μ
Σ
M
1:t 1:t j 1:t 1:tj=1
P(m | z ,u ) = P(m | z ,u )
Problems With EKF SLAMProblems With EKF SLAM Uses uni-modal Gaussians to model non-Gaussian Uses uni-modal Gaussians to model non-Gaussian
probability density functionprobability density functionP
roba
bilit
y
Position
Particle Filters (without EKF)Particle Filters (without EKF)
The use of EKF depends on the odometry The use of EKF depends on the odometry (u’s) and motion model (g’s) assumptions (u’s) and motion model (g’s) assumptions in a very nonrobust way and fails to allow in a very nonrobust way and fails to allow for multimodality in the motion model. In for multimodality in the motion model. In place of this, we can use particle filters place of this, we can use particle filters without assuming a motion model by without assuming a motion model by modeling the particles without reference to modeling the particles without reference to the parameters.the parameters.
Particle Filters (an alternative)Particle Filters (an alternative)
Represent probability distribution as a set of Represent probability distribution as a set of discrete particles which occupy the state spacediscrete particles which occupy the state space
Particle FiltersParticle Filters
For constructing the weights associated with the For constructing the weights associated with the classical fast slam algorithm, under moderate classical fast slam algorithm, under moderate assumptions, we get: (for x’s simulated by q)assumptions, we get: (for x’s simulated by q)
1
t
t
W
W
1:t 1:t
t 1:t 1:t-1 1:t-1 1:t
t 1:t 1:t-1
t t-1 tt t t-1
t t-1
P(x | z )
P(x | z ,x )P(x | z )
P(x | z ,x )
P(x | x ,z ) P(z | x ) W
q(x | x )
ResamplingResampling
Assign each particle a weight depending Assign each particle a weight depending on how well its estimate of the state on how well its estimate of the state agrees with the measurementsagrees with the measurements
Randomly draw particles from previous Randomly draw particles from previous distribution based on weights creating a distribution based on weights creating a new distributionnew distribution
Particle Filter Update CycleParticle Filter Update Cycle
Generate new particle distribution Generate new particle distribution For each particleFor each particle
Compare particle’s prediction of Compare particle’s prediction of measurements with actual measurementsmeasurements with actual measurements
Particles whose predictions match the Particles whose predictions match the measurements are given a high weightmeasurements are given a high weight
Resample particles based on weightResample particles based on weight
Particle Filter AdvantagesParticle Filter Advantages
Can represent multi-modal distributionsCan represent multi-modal distributionsP
roba
bilit
y
Position
Problems with Particle FiltersProblems with Particle Filters
Degeneracy:Degeneracy: As time evolves particles As time evolves particles increase in dimensionality. Since there is increase in dimensionality. Since there is error at each time point, this evolution error at each time point, this evolution typically leads to vanishingly small typically leads to vanishingly small interparticle (relative to intraparticle) interparticle (relative to intraparticle) variation.variation.
We frequently require estimates of the We frequently require estimates of the ‘marginal’ rather than ‘conditional’ particle ‘marginal’ rather than ‘conditional’ particle distribution. distribution.
Particle Filters do not provide good methods Particle Filters do not provide good methods for estimating for estimating particle features.particle features.
Marginal versus nonmarginal Marginal versus nonmarginal Particle FiltersParticle Filters
Marginal particle filters attempt to update the X’s Marginal particle filters attempt to update the X’s using their marginal (posterior) distribution rather using their marginal (posterior) distribution rather than their conditional (posterior) distribution. The than their conditional (posterior) distribution. The update weights take the general form,update weights take the general form,
t 1:tt
t 1:t
P x | zW =
q(x | z )
Marginal Particle updateMarginal Particle update
We want to update by using the old weights rather We want to update by using the old weights rather than conditioning on the old particles.than conditioning on the old particles.
t 1:t t t t 1:t-1
t-1 1:t-1t 1:t t t t-1 1:t-1 t t-1 t-1
t-1 1:t-1
(j) (j)t t tt-1 t-1
P(x | y ) P(y | x )P(x | y )
P(x | y )P(x | y ) P(y | x ) q(x | y )P(x | x ) dx
q(x | y )
P(y | x ) w P(x | x )
Marginal Particle FiltersMarginal Particle Filters
We specify the proposal distribution ‘q’ via:We specify the proposal distribution ‘q’ via:
k (j) (j)
t 1:t t tt t-1j=1
q(x | y ) = w q x | y ,x
Marginal Particle AlgorithmMarginal Particle Algorithm
(1)(1)
(2) Calculate the importance weights:(2) Calculate the importance weights:
ˆ (kernel density)k(i) (j) (j)
t 1:t t tt t t-1j=1
x p(x | y ) = w P(x | x ,y )
k(i) (j) (i) (j)t t tt-1 t-1
j=1(i)t k (j) (i) (j)
ttt-1 t-1j=1
P y | x w P x | x
w =w q x | y ,x
Updating Map features in the Updating Map features in the marginal modelmarginal model
Up to now, we haven’t assumed any map features. Up to now, we haven’t assumed any map features. Let Let θθ={={θθtt} denote the e.g., distances of the robot at } denote the e.g., distances of the robot at
time t from the given landmarks. We then write time t from the given landmarks. We then write
for the probability associated with scan Zfor the probability associated with scan Ztt
given the position xgiven the position x1:t1:t. .
We’d like to update We’d like to update θθ. This should be based, not on . This should be based, not on the gradient , but rather on the gradient, the gradient , but rather on the gradient, . .
θ t 1:tP (Z | X )
θ t 1:tP (Z | X )
θ t 1:t-1P (Z | Z )
Taking the ‘right’ derivativeTaking the ‘right’ derivative
The gradient, is highly The gradient, is highly non-robust; we are essentially non-robust; we are essentially taking derivatives of noise.taking derivatives of noise.
By contrast, the gradient, By contrast, the gradient, is robust and represents the is robust and represents the ‘right’ derivative.‘right’ derivative.
θ t 1:tP (Z | X )
θ t 1:t-1P (Z | Z )
Estimating the Gradient of a MapEstimating the Gradient of a Map
We have that, We have that,
1
1 +
θ t 1:t-1 θ t t θ t-1 1:t-1 t t-1 t
θ t 1:t-1 θ t t θ t 1:t-1 t t-1 t
θ t t θ t 1:t-1 t t-1 t
P Z | Z = P (Z | X )P (X | Z )P(X | X )dX
P Z | Z = P (Z | X )P (X | Z )P(X | X )dX
P (Z | X ) P (X | Z )P(X | X )dX
SimplificationSimplification
We can then show for the first term that:We can then show for the first term that:
1
1
1 1
θ t t θ t 1:t-1 t t-1 t
θ t 1:t-1 θ t t t t-1 t
θ t 1:t-1 θ t t
P (Z | X )P (X | Z )P(X | X )dX
P (X | Z ) P (Z | X )P(X | X )dX
P (X | Z ) P (Z | X )
Simplification IISimplification II
For the second term, we convert into a discrete For the second term, we convert into a discrete sum by defining ‘derivative weights’ sum by defining ‘derivative weights’
And combining them with the standard weights.And combining them with the standard weights.
(i) θ t-1 1:t-1t
θ t-1 1:t-1
P (X | Z )β = weights for
P (X | Z )
Estimating the GradientEstimating the Gradient
We can further write that:We can further write that:
1 1
( ) ( )1
11
1
( )1
1
;i itt
jt t
j
w
X
θ t 1:t-1 θ t t
θ t
θ t 1:t-1θ t t θ t 1:t-1 t t-1 t
θ t 1:t-1
(j) (j)θ t-1 t-1
P (X | Z ) P (Z | X )
P (Z | X )
P (X | Z )P (Z | X ) P (X | Z )P(X | X )dX
P (X | Z )
P ( | X )β w
Gradient (continued)Gradient (continued)
We can therefore update the gradient weights We can therefore update the gradient weights via:via:
( ) ( )1i i
ttw
(j) (j) (j)θ t t-1 t-1 t-1
j=1(i)θ tt
θ t 1:t
P (X | X )β w
ρ P (Z | X )q (X | Y )
Parameter UpdatesParameter Updates
We update We update θθ by: by:
(j)t-1
j=1t t-1 t (j)
t-1
ρ
θ = θ + γ (Newton - Raphson)w
NormalizationNormalization
The The ββ’s are normalized differently from the w’s. ’s are normalized differently from the w’s. In effect we put: In effect we put:
And then compute that:And then compute that:
θ t 1:tθ t 1:t
θ t 1:t t
ξ (X ,Y )P (X | Y ) =
ξ (X ,Y )dX
θ t 1:t tθ t 1:t
θ t 1:t θ t 1:t
θ t 1:t t θ t 1:t t
ξ (X ,Y )dXξ (X ,Y )
P (X | Y ) = - p (X | Y )
ξ (X ,Y )dX ξ (X ,Y )dX
Weight UpdatesWeight Updates
(i)(i) t(i) (i) (i)tt t t(i) (i)
t ti=1 i=1
ρρ
w β = - ww w
The Bayesian ViewpointThe Bayesian Viewpoint Retain a posterior sample of Retain a posterior sample of θθ at time t-1. at time t-1. Call this (i=1,…,I) Call this (i=1,…,I)
At time t, update this sample:At time t, update this sample:
(i)θ t 1:t-1t-1θ P (Y | Y )π(θ)
( )( )
1ii
t t
(j)t-1
j=1t (j)
t-1
ρ
θ = θ + γ (Newton - Raphson)w
Multi Robot ModelsMulti Robot Models
Write for the poses Write for the poses and scan statistics for the r robots. and scan statistics for the r robots.
At each time point the needed weights have r At each time point the needed weights have r indices:indices:
We also need to update the derivative weights – We also need to update the derivative weights – the derivative is now a matrix derivative. the derivative is now a matrix derivative.
[1],..., [ ] ; [1],..., [ ]t t t tX X r Z Z Z r t tX����������������������������
1( ,..., )1:( | )ri ittW P tX Y ����������������������������
Multi-Robot SLAMMulti-Robot SLAM
The parameter is now a matrix (with time The parameter is now a matrix (with time being the row values and robot index being the being the row values and robot index being the column. Updates depend on derivatives with column. Updates depend on derivatives with respect to each timepoint and with respect to respect to each timepoint and with respect to each robot. each robot.
Θ