Graph-slam(二)

Dear 丶 2022-09-25 13:25 135阅读 0赞

1.3 Building Up the Graph  
Suppose we are given a set of measurements z1:t with associated correspondence variables c1:t, and a set of controls u1:t. GraphSLAM turns this data into a graph. The nodes of this graph are the robot poses x1:t and the features in the map m =mj. Each edge in the graph corresponds to an event: a motion event generates an edge between two robot poses, and a measurement event creates a link between a pose and a feature in the map. Edges represent soft constraints between poses and features in GraphSLAM.  
For a linear system, these constraints are equivalent to entries in an information matrix and an information vector of a large system of equations. As usual, we will denote the information matrix by Ω and the information vector by ϵ. As we shall see below, each measurement and each control leads to a local update of Ω and ϵ, which corresponds to a local addition of an edge to the graph in GraphSLAM. In fact, the rule for incorporating a control or a measurement into Ω and ϵ is a local addition, paying tribute to the important fact that information is an additive quantity.  
Figure 2 illustrates the process of constructing the graph along with the corresponding information matrix. First consider a measurement zit. This measurement provides information between the location of the feature j=cit and the robot pose xt at time t. In GraphSLAM, this information is mapped into a constraint between xt and mj.We can think of this edge as a (possibly degenerate) “spring” in a spring-mass model. As we shall see below, the constraint is of the type:

(zti−h(xt,mj,i))TQt−1(zti−h(xt,mj,i))

Here h is the measurement function, and  Qt is the covariance of the measurement noise. Figure 2(a) shows the addition of such a link into the graph maintained by GraphSLAM. Note that the constraint may be degenerate, that is, it may not constrain all dimensions of the robot pose  xt . This will be of no concern for the material yet to come.  
In information form, the constraint is incorporated into  Ω and  ϵ by adding values between the rows and columns connecting  xt−1 and  xt . The magnitude of these values corresponds to the stiffness of the constraint, as governed by the uncertainty covariance  Qt of the motion model. This is illustrated in Figure 2(b), which shows the link between two robot poses along with the corresponding element in the information matrix.  
Now consider robot motion. The control  ut provides information about the relative value of the robot pose at time t −1 and the pose at time t. Again, this information induces a constraint in the graph, which will be of the form:

(xt−g(ut,xt−1))TRt−1(xt−g(ut,xt−1))

Here g is the kinematic motion model of the robot, and  Rt is the covariance of the motion noise. Figure 2(b) illustrates the addition of such a link in the graph. It also shows the addition of a new element in the information matrix, between the pose  xt and the measurement  zti . This update is again additive. As before, the magnitude of these values reﬂects the residual uncertainty  Rt due to the measurement noise; the less noisy the sensor, the larger the value added to  Ω and  ϵ . After incorporating all measurements  z1:t and controls  u1:t , we obtain a sparse graph of soft constraints. The number of constraints in the graph is linear in the time elapsed, hence the graph is sparse. The sum of all constraints in the graph will be of the form:

JGraphSLAM=x0TΩ0x0\+∑t(xt−g(ut,xt−1))TRt−1(xt−g(ut,xt−1))\+∑t∑i(zti−h(yt,cti,i))TQt−1(zti−h(yt,cti,i))

It is a function deﬁned over pose variables  x1:t and all feature locations in the map m. Notice that this expression also features an anchoring constraint of the form  x0TΩ0x0 . This constraint anchors the absolute coordinates of the map by initializing the very ﬁrst pose of the robot as  (0,0,0)T .  
In the associated information matrix , the off-diagonal elements are all zero with two exceptions: between any two consecutive poses  xt−1 and  xt will be a non-zero value that represents the information link introduced by the control  ut . Also non-zero will be any element between a map feature  mj and a pose  xt , if  mj was observed when the robot was at  xt .All elements between pairs of different features remain zero.This reﬂects the fact that we never receive information pertaining to their relative location—all we receive in SLAM are measurements that constrain the location of a feature relative to a robot pose. Thus, the information matrix is equally sparse; all but a linear number of its elements are zero.