1. Cooperative Localization (CL)

2. Simultaneous Localization and Mapping (SLAM)

3. Cooperative SLAM (C-SLAM)

Contribution:

• the accuracy of the robots' sensors

• the number of robots and features

• the structure of Relative Position Measurement Graph (RPMG) that describes the measurement topology between the robots and/or landmarks

• the statistical properties of the robots' trajectories

• the spatial distribution of features, in SLAM and C-SLAM

Approach:

Therefore, in order to analytically study the properties of CL for robots moving in 2D, we resort to the derivation of upper bounds for the covariance of the position estimates. In particular, in our work we derive upper bounds on

• the worst-case uncertainty of the position estimates, and

• the expected uncertainty of the position estimates

In order to compute the upper bounds on the worst-case uncertainty, our approach is based on deriving a "bounding" LTI system, whose covariance at every time step is provably an upper bound of the covariance in the original, nonlinear system. This is achieved by determining upper bounds on the system noise covariance matrix and the relative position measurement covariance matrix.  By obtaining the steady-state solution of the Riccati corresponding to this LTI system, we are able to predict the guaranteed asymptotic accuracy of localization.

A similar approach, based on defining a different "bounding" LTI system, whose covariance is provably an upper bound to the expected uncertainty of the original nonlinear system, is followed for characterizing the expected performance of the system.

Results:

We have carried out extensive simulation tests and real-world experiments to validate the theoretical analysis, and to demonstrate its usefulness in predicting the performance of localization systems. In what follows, some representative results are described:

A. Cooperative Localization

We here present simulation results showing the effects of RPMG reconfigurations. The behavior of the covariance that is observed in the plots is predicted by, and corroborates, the theoretical analysis. The following figure shows the four different RPMG topologies that are used for this simulation test:

Figure 1

For this simulation, a homogeneous robot team comprising 9 robots is considered. The time evolution of the covariance is shown in Fig. 2:

Figure 2

In this plot, the covariance along the x axis for all robots is plotted. During the first 200sec of their mission, the robots localize independently (Dead Reckoning, DR). At t = 200sec, the robots start recording relative position measurements, which are described by a complete RPMG topology (cf. Fig. 1-I). The sharp improvement in localization accuracy, as well as the decrease in the rate of covariance increase, becomes evident. At t = 400sec, the RPMG topology changes to a sparser, ring graph (cf. Fig. 1-II). Clearly, after an initial transient response, the rate at which the covariance increases with the new RPMG topology remains unchanged, and a small constant penalty in performance is incurred. This result, which has very important practical implications, is predicted by our theoretical analysis.

At t = 600sec, a simulated communication failure occurs, and only two of the robots are able to record and communicate relative position measurements (cf. Fig. 1-III). Careful examination of the plot reveals that the rate of uncertainty increase for these two robots is precisely half that of the rest of the robots, which localize independently. This result verifies an important prediction of our analysis, which stipulates that the rate of uncertainty increase, for homogeneous robot teams, is inversely proportional to the number of robots.

At t = 800sec, the initial complete graph topology is restored (cf. Fig. 1-I). We observe that, as expected by the theoretical analysis, the covariance for all robots is identical to the one that would arise if no RPMG reconfigurations had occurred. Finally, at t = 1000sec, the RPMG assumes a random (i.e., non-canonical) topology (cf. Fig. 1-IV). Clearly, for this topology the asymptotic rate of uncertainty increase for all robots is identical, even though the constant term of the covariance varies among robots. 

B. Cooperative Simultaneous Localization and Mapping

We here present simulation results that demonstrate the application of the derived upper bound, and show the effects of RPMG reconfigurations in C-SLAM. The RPMG used in this simulation is shown in Fig. 3:

Figure 3

In particular, a team comprised of 4 robots, performing C-SLAM with 3 features, is considered.  For the first 1000sec, the RPMG has the structure shown in Fig. 3, while for the remaining 1000sec, the RPMG is a denser one, where each robot records measurements to all other robots and landmarks. The time evolution of the covariance of the position estimates for this case is shown in Fig. 4, along with the theoretically computed bounds:

Figure 4

From this plot we observe that the covariance of the landmarks' position estimates does not change after the RPMG changes. This is an important result, which is predicted by the closed-form expressions for the covariance. On the other hand, the robots' position estimates become more accurate, as a result of the increased positioning information that is available to each robot, in the new dense RPMG. Note also that, since in the new RPMG all robots perform the same number of measurements, the covariance bound is identical for all of them, in contrast to the situation occurring with the initial RPMG topology.

C. Simultaneous Localization and Mapping

For the case of SLAM, we present experimental results that demonstrate the usefulness of the analytical covariance bounds for predicting the accuracy of the robot's and landmarks' localization. In this experiment, a Pioneer 3 robot equipped with two opposite-facing SICK LMS200 laser scanners, which provide a 360^o field of view, was employed. The robot is shown in Fig. 5:

Figure 5

During the experiment, the robot moves randomly while performing SLAM in an area of approximate dimensions 10m×4m. The laser scans are processed for detecting four prominent corners in the area, which are used as landmarks. For detecting each corner, line-fitting is employed to compute the equations of adjacent wall lines, and the intersection of these lines is determined. The robot also receives translational and rotational velocity measurements from its wheel encoders. The estimated robot trajectory, as well as the landmark positions, are shown in Fig. 6. In the same figure, a sample laser scan is superimposed (after being transformed to the global frame), to illustrate the geometry of the area where the robot operates.

Figure 6

In Fig. 7, the standard deviation of the estimation errors (solid lines), as this is computed by the filter, is compared to the standard deviation computed with the theoretically derived bounds (dashed lines).

(a)

  (b)                                                                     (c)

From the above plots we conclude that the analytical bounds that we have derived can be employed in order to predict the localization accuracy of SLAM, without having to resort to extensive simulations or experimentation. We should point out that in this particular case, where the robot moves randomly in space, the actual standard deviations are approximately 2-3 times smaller than the corresponding upper bounds. If the robot’s trajectory was such that the robot-to-landmark distances were always close to their maximum allowable values (which are used in the bound computation), the bounds would have been significantly tighter. This fact has been verified in numerous simulation studies of “adverse” SLAM setups. Finally, it is worth mentioning that due to occlusions and data association failures, the landmarks were not detected in every laser scan. On the average, the landmarks were successfully detected 94% of the time. Despite these fluctuations in the number of observed landmarks, the theoretical bounds still provide a quite accurate characterization of the uncertainty in SLAM.

Related Publications: