The GraphSLAM Algorithm
Download
Report
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