Graph-Slam(一)

迷南。 2022-09-21 11:47 212阅读 0赞

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 Ofﬂine 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 ofﬂine 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 sufﬁces to determine the mode of this posterior. The actual posterior is usually too difﬁcult 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  
![这里写图片描述][20160911131508103]  
![这里写图片描述][20160911131703672]  
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 signiﬁcant 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 deﬁned 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.

[20160911131508103]: /images/20220721/59f0e01fcc244a20986c98d1faf09dd9.png
[20160911131703672]: /images/20220721/7ec132921312492da180fba58c39a933.png