Graph-Slam(一)

GraphSLAM is a unifying algorithm for the offline
SLAM problem.It transforms the SLAM posterior into a graphical network,
representing the log-likelihood of the data. It then reduces this
graph using variable elimination techniques, arriving at a lowerdimensional
problems that is then solved using conventional optimization
techniques. As a result, GraphSLAM can generate maps
with 108 or more features.
1 Mapping SLAM Problems into Graphs
1.1 The Offline SLAM Problem
In SLAM, time is usually discrete, and t labels the time index. The robot pose at time t is denoted xt;
we will use x1:t to denote the set of poses from time 1 all the way to time t. The world itself is denoted m, where m is short for map. The map is assumed to be time-invariant, hence we do not use a time index to denote the map. In this paper, we think of the map of a (large) set of features mj.
To acquire an environment map, the robot is able to sense. The measurement at time t is denoted zt. Usually, the robot can sense multiple features at each point in time; hence each individual measurement beam is denoted zti. Commonly, one assumes that zti is a range measurement. The measurement function h describes how such a measurement comes into being:

zti=h(xt,mj,i)+ϵti

here ϵti is a Gaussian random variable modeling the measurement noise, with zero mean and covariance Qt, and mj is the map feature sensed by the i-th measurement beam at time t. Put differently, we have

p(zti|xt,m)=const.exp−12(zti−h(xt,mj,i))TQt−1(zti−h(xt,mj,i))

Some robotic systems are also are provided with a GPS system. Then the measurement is of the form

zti=h(xt,i)+ϵti

where zti is a noisy estimate of the pose xt, and ϵti is once again a Gaussian noise variable The mathematics for such measurements are analogous to those of nearby features; and GraphSLAM admits for arbitrary measurement functions h.
Finally, the robot changes its pose in SLAM by virtue of issuing control commands.The control asserted between time t−1 and time t is denoted ut. The state transition is governed by the function g:

xt=g(ut,xt−1)+δt

where δt∼N(0,Rt) models the noise in the control command. The function g can be thought of as the kinematic model of the robot. Equation (4) induces the state transition probability:

p(xti|ut,m)=const.exp−12(xt−g(ut,xt−1))TQt−1(xt−g(ut,xt−1))

The offline SLAM posterior is now given by the following posterior probability over the robot path xt:1 and the map m:

p(x1:t,m|z1:t,u1:t)

This is the posterior probability over the entire path x1:t along with the map, instead of just the current pose xt: We note that in many SLAM problems, it suffices to determine the mode of this posterior. The actual posterior is usually too difficult to express for high-dimensional maps m, since it contains dependencies between any pair of features in m.
We note that a key assumption in our problem formulation is the assumption of independent Gaussian noise.
1.2 GraphSLAM: Basic Idea


Figure 1 illustrates the GraphSLAM algorithm. Shown there is the graph that GraphSLAM extracts from four poses labeled x1,...,x4, and two map features m1,m2. Arcs in this graph come in two types: motion arcs and measurement arcs.Motion arcs link any two consecutive robot poses, and measurement arcs link poses to features that were measured there. Each edge in the graph corresponds to a nonlinear constraint. As we shall see later, these constraints represent the negative log likelihood of the measurement and the motion models, hence are best thought of as information constraints.Adding such a constraint to the graph is trivial for GraphSLAM; it involves no significant computation. The sum of all constraints results in a nonlinear least squares problem, as stated in Figure 1.
To compute a map posterior,GraphSLAM linearizes the set of constraints. The result of linearization is a sparse information matrix and an information vector. The sparseness of this matrix enables GraphSLAM to apply the variable elimination algorithm,thereby transforming the graph into a much smaller one only defined over robot poses. The path posterior map is then calculated using standard inference techniques. GraphSLAM also computes a map and certain marginal posteriors over the map; the full map posterior is of course quadratic in the size of the map and hence is usually not recovered.