Transcript The GraphSLAM Algorithm

The GraphSLAM
Algorithm
Daniel Holman
CS 5391: AI Robotics
March 12, 2014
Introduction
 The SLAM Problem
 Given a robot’s control signals and observations of
nearby features, calculate a map of the features and
robot path
 Full SLAM Problem
 Calculate the entire path of the robot, or all poses from
time 1 to t rather than simply the pose at time t
p(x1:t , m | z1:t , u1:t )
 GraphSLAM provides a solution to the offline full SLAM
problem
GraphSLAM
 GraphSLAM - Describes the SLAM problem as a
sparse graph
 Each node in the graph represents a robot pose or a
feature of the map
 Each edge in the graph corresponds to a nonlinear
constraint of either the motion or measurement models
EKFSlam vs. GraphSLAM
 EKFSlam
 Online, Proactive
 As a filter, only maintain




posterior pose at t
No time dependence
for memory allocation
Represents information
though mean vector
and covariance matrix
Best estimate of robot
pose and map
Updating covariance is
computationally
expensive, particularly
for large maps
 GraphSLAM
 Offline, Batch, “lazy”
 Estimates posterior for all




poses 1:t
Graph increases linearly over
time
Represents information as
soft constraints, less
expensive to compute
Adds an additional inference
phase in which information is
transformed to estimate state
Can revise past data
association, multiple
linearization: produce more
accurate maps
Sum of all constraints is nonlinear least squares problem
JGraphSLAM = xoT W0 x0 + å[xt - g(ut , xt-1 )]T R-1[xt - g(ut , xt-1 )]+ å[zt - h(mct , xt )]T Q-1[zt - h(mct , xt )]
t
Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic
robotics. MIT press, 2005.
t
Inference
 Map and path are obtained from linearized information
matrix W and information vector 
-1
 S=W
-1
 m =W x
 If each feature is only seen locally once, the graph
represented by constraints is linear and thus W can
be reordered as a band-ordered diagonal matrix
 Realistically, features are seen at multiple time steps,
computing full inverse of
W can be expensive
Inference
 Factorization Trick – variable elimination algorithm
 Want to remove 𝑚𝑗 from Ω and 𝜉
 Let 𝜏 𝑗 be the set of all poses at which 𝑚𝑗 was observed
 Set links between 𝜏 𝑗 and 𝑚𝑗 to zero and introduce new link
between poses 𝑥𝑡 , 𝑥𝑡′ ∈ 𝜏 𝑗
 Update 𝜉 for all poses
 Can now safely remove 𝑚𝑗 from Ω and 𝜉
 Results in smaller Ω and 𝜉, reducing inference problem into a
smaller one
 Robot path can be recovered
 Σ = Ω−1
 𝜇 = Σ𝜉
Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic
robotics. MIT press, 2005.
Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic
robotics. MIT press, 2005.
Feature Recovery
 Build a new information matrix and information vector
for each 𝑚𝑗
 Both defined over 𝑚𝑗 and 𝜏 𝑗
 Contains original links between 𝑚𝑗 and 𝜏 𝑗 , but poses
are set to values in 𝜇, without uncertainty
 From this information form, can calculate location of 𝑚𝑗
using common matrix inversion trick
Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic
robotics. MIT press, 2005.
Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic
robotics. MIT press, 2005.
Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic
robotics. MIT press, 2005.
Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic
robotics. MIT press, 2005.
Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic
robotics. MIT press, 2005.
Thrun, Sebastian, Wolfram Burgard, and Dieter Fox. Probabilistic
robotics. MIT press, 2005.
Conclusion
 Addresses full SLAM problem: calculates posteriors for full
robot path along with map
 Constructs a sparse graph of nonlinear constraints between
poses and sensed features, and motion commands into soft
constraints between consecutive poses
 Performs inference by mapping the graph into an isomorphic
information matrix and vector, defined over all pose
variables and the entire map
 Construct linear information form, reduction of the form to
remove the map, and solves resulting optimization problem
over robot poses