Simultaneous Localization and Map Building with Modified System State

Abstract

The full covariance solution to simultaneous localization and map building based on extended Kalman filter requires update time quadratic in the number of landmarks in the map. In order to improve the computational efficiency, this paper reorganizes system state vector and system models. The state of mobile robot is redefined and represented indirectly. The higher dimensional system models and covariance matrix can be represented with two lower dimensional submatrices. An optimization solution is proposed based on this property. The computational requirement and memory requirement are decreased by half. The covariance matrix is fully updated without any approximation during estimation. The optimization solution is consistent and convergent theoretically and realistically. The experiment also compares the performance of optimization solution with the full covariance solution. All these techniques have been implemented on mobile robot ATRVII equipped with 2D laser rangefinder SICK.

Keywords

Mobile Robot SLAM Kalman Filter

1. Introduction

The simultaneous localization and map building (SLAM) or concurrent mapping and localization (CML) is the problem of robotic mapping. Mobile robot starts from an unknown location in an unknown environment and then incrementally builds a map of this environment while simultaneously using this map to compute robot location. A number of approaches (Durrant-whyte, H. & Bailey, T., 2006) have been proposed to address the SLAM problem. One solution is filter estimation approach based on Extended Kalman Filter (EKF), proposed by Smith, Self and Cheesman (Smith, R.; Self, M. & Cheeseman, P., 1990; Smith, R. & Cheeseman, P., 1986), who developed an EKF approach for building a stochastic map of spatial relationships. The Kalman Filter is optimal in the sense that it yields a least square estimation. The difficult problem is maintaining the covariance matrix which is quadratic in size with respect to the number of features. Compressed extended Kalman filter (CEKF) (Guivant, J. & Nebot, E., 2003), Extended Information filter (EIF) (Thrun, S.; Liu, Y.; Koller, D.; Ghahramani, Z. & Durrant-Whyte, H., 2004) and Square Root Unscented Kalman filter (SRUKF) (Holmes, S.; Klein G. & Murray, D., 2008) are three modified version of EKF. Another solution is Bayesian estimate approach that does not assume Gaussian probability distributions. Particle filter (Montemerlo, M.; Thrun, S.; Koller, D.; Wegbreit, B., 2002, Kwok, N. & Rad, A., 2006), especial Rao-Blackwellized particle filters (Blanco, J.; Fernández-Madrigal, J. & Gonzalez, J., 2008, Grisetti, G.; Tipaldi, G.; Stachniss, C.; Burgard, W. & Nardi, D., 2007, Li, M.; Hong B. & Luo, R., 2006) have been introduced to solve the simultaneous localization and map building problem. Expectation maximization (EM) (Thrun, S.; Fox, D. & Burgard, W., 1998) describes the map building problem as a constrained maximum-likelihood (ML) probabilistic estimation problem with E-step and M-step.

In addition, other solutions include RatSLAM (Milford, M., Schulz, R., Prasser, D., Wyeth, G. and Wiles, J., 2007) and fuzzy logic (FL) (Begum, M.; Mann, K. & Gosine, R., 2008). RatSLAM is a SLAM solution based a hippocampal model. The spatial representation is an experience map as the robot learns the environment. FL solution solves SLAM problem by integrating fuzzy logic and genetic algorithm. A GA model is used to search for the most probable map.

The full covariance SLAM solution based on EKF requires that the system state vector consists of the states of robot and the states of all landmarks in the map. Robot location and map estimation are highly correlated and cannot be obtained independently (Castellanos, J.; Tardos, J. & Schmidt, G., 1997). A full covariance SLAM solution is defined in (Dissanayake, G.; Newman, P.; Clark, S. Durrant-Whyte H. & Csorba, M., 2001). The full covariance matrix needs to be maintained and updated following each observation. Due to the high computational complexity of full SLAM solution, many methods for reducing the computational requirements have been proposed.

The first work is concentrated on fewer landmarks. The computational complexity is decreased by limiting the number of landmarks (Dissanayake, G.; Durrant-Whyte, H. & Bailey, T., 2000). Durrant-whyte (Durrant-Whyte, H.; Dissanayake, G. & Gibbens, P., 2000) presented a method for choosing the best landmark in the environment while maintaining the SLAM algorithm as convergent and consistent as possible. Dissanayake (Dissanayake, G.; Williams, S.; Durrant-Whyte, H. & Bailey, T., 2002) presented a method for removing a large percentage of the landmarks from the map without making the map statistically inconsistent.

The second work is concentrated on applying approximation technique for the state covariance matrix updating. These approximation SLAM solutions (Guivant, J. & Nebot, E., 2001) assume that the correlations between landmarks could be minimized or eliminated. The covariance matrix is updated based on different simplification. Covariance Intersect (CI) (Julier, S. & Uhlmann, J., 2007) only considers the diagonal elements of covariance matrix.

The third work is concentrated on alternative map representations. One is relative map (Wang, Z.; Huang, S. & Dissanayake, G., 2005, Deans, M. & Hebert, M., 2000). Relative map features are invariant to robot location. It avoids the computational scaling problem occurring in absolute map estimation. Another is submaps (Leonard, J. & Feder, H., 2001, Williams, S.; Dissanayake, G. & Durrant-Whyte, H., 2002). A large map is divided into a series of smaller submaps in order to reduce the computational complexity of covariance matrix update.

In this paper, by reorganizing the state vector of mobile robot and by selecting properly the observation vector, the system models are linear. System models and system covariance matrix can be represented with two special matrices respectively. An optimization solution is proposed based on this property. The computational requirement and memory requirement are decreased by half. The covariance matrix is fully updated without any approximation, hence the proposed solution is consistent and convergent theoretically and realistically.

An outline of this paper is as follows. Section 2 introduces the full covariance SLAM solution based on EKF. In Section 3 we describe the modified system state vector and system models. In Section 4 two special matrices are defined. In Section 5 an optimization SLAM solution is represented based on modified system state and system models. Experimental results are presented in Section 6. We conclude in Section 7.

2. Full SLAM solution

In the estimation theoretic formulation of the SLAM problem, Kalman filter is used to provide estimates of robot location and all landmarks location. The location prediction of robot is according to the motion model. The observation prediction is according to the observation model. The locations of robot and all landmarks in the map are updated according to updated rules. The location of new landmark is augmented according to the augmentation model. The Kalman filter recursively estimate the states of robot and all landmarks. The covariance matrix is updated recursively.

2.1. State Space and System Models

The augmented state vector X(k) consists of the state of the robot together with the state of landmarks.

X (k) = [\begin{matrix} X_{v} (k) \\ X_{1} (k) \\ ⋮ \\ X_{n} (k) \end{matrix}] \begin{array}{l} X_{v} (k) = [\begin{matrix} x_{v} (k) \\ y_{v} (k) \\ θ_{v} (k) \end{matrix}] \\ X_{i} (k) = [\begin{matrix} x_{i} (k) \\ y_{i} (k) \end{matrix}] \end{array}

(1)

And the system covariance matrix is:

P (k) = [\begin{matrix} p_{v v} (k) & P_{v 1} (k) & \dots & P_{v n} (k) \\ P_{v 1} (k)^{T} & P_{11} (k) & \dots & P_{1 n} (k) \\ ⋮ & ⋮ & ⋱ & ⋮ \\ P_{v n} (k)^{T} & P_{1 n} (k)^{T} & ⋮ & P_{n n} (k) \end{matrix}]

(2)

The robot motion model, landmark process model, observation model and augmentation model are:

\begin{aligned} X_{v} (k) = f (X_{v} (k - 1)) + ω (k) \\ X_{i} (k) = X_{i} (k - 1) i = 1, \dots, N \\ Z (k) = h (X (k)) + v (k) \\ X_{N + 1} (k) = g (X_{v} (k)) + v (k) \end{aligned}

(3)

Where ω(k)∼N(0, Q) is the uncertainty on robot slipping and v(k)∼N(0, R) is the uncertainty on sensor measurement. Z(k) is the observation vector at step k.

SLAM algorithm process localization and mapping problem in three steps: prediction, update and augmentation. At each step, the system state vector and covariance matrix are updated.

2.2. Prediction

According to above system motion model, the state of robot and landmarks are predicted. The state vector and covariance matrix are predicted as:

\begin{aligned} X_{v}^{-} (k) = f (X_{v}^{+ +} (k - 1)) \\ X_{i}^{-} (k) = X_{i}^{+ +} (k - 1) i = 1, \dots, N \\ P_{k}^{-} = \\ [\begin{matrix} Δ f * P_{v v}^{+ +} (k - 1) * Δ f^{T} + Q Δ f * P_{v m}^{+ +} (k - 1) \\ (Δ f * P_{v m}^{+ +} (k - 1))^{T} P_{n v m}^{+ +} (k - 1) \end{matrix}] \end{aligned}

(4)

Where X⁺⁺, P⁺⁺ denotes the posterior estimation after augmentation and X⁻, P⁻ denotes the prior estimation with respect to each observation.

2.3. Update

With data association, the difference between real observation and observation prediction of a landmark is called innovation. Innovation and innovation covariance is computed as follows:

\begin{aligned} v_{i} = Z_{i} - h (X (k)) \\ S_{i} (k) = \nabla h * P^{-} (k) * Δ h^{T} + R \end{aligned}

(5)

The system state vector and covariance matrix are updated according to following update rules:

\begin{aligned} W_{i} = P^{-} (k) * \nabla h^{T} * S_{i} {(k)}^{- 1} \\ X^{+} (k) = X^{-} (k) + W_{i} * v_{i} (k) \\ P^{+} (k) = P^{-} (k) - W_{i} * S_{i} (k) * W_{i}^{T} \end{aligned}

(6)

Where X⁺ denotes the posterior estimation before augmentation and X⁻ denotes the prior estimation.

The update of the state covariance matrix is of paramount importance to the SLAM problem. It is the key to the consistence and convergence of SLAM filter algorithm. Although it requires considerable computational cost, it is necessary to completely maintain and update the covariance matrix recursively.

2.4. Augmentation

When a new landmark is observed and validated, the state of new landmark is incorporated into the system state vector according to augmentation model. The system state vector and covariance matrix are augmented as:

\begin{aligned} X^{+ +} (k) = [\begin{matrix} X^{+} (k) \\ g (X_{v}^{+} (k)) \end{matrix}] \\ P^{+ +} (k) = [\begin{matrix} P_{v v}^{+} (k) & P_{v m}^{+} (k) & (\nabla_{g} * P_{v v}^{+} (k))^{T} \\ P_{v m}^{+} (k)^{T} & P_{m m}^{+} (k) & (\nabla_{g} * P_{v m}^{+} (k))^{T} \\ \nabla_{g} * P_{v v}^{+} (k) & \nabla_{g} * P_{v m}^{+} (k) & \nabla_{g} * P_{v v}^{+} (k) * \nabla_{g}^{T} + R \end{matrix}] \end{aligned}

(7)

From step k-1 to step k, system state mean, including the location of robot and locations of all landmarks, and their covariance are estimated recursively.

3. Modified state space and system model

Mobile robot starts from an unknown location and moves through an environment containing a population of features or landmarks. Robot is equipped with an odometer and a laser rangefinder. The data from the odometer is used to predict the position of robot. Laser rangefinder scans environment information and extracts natural landmark to update the location of robot and all landmarks. We assume that the mobile robot moves on a ground plane. There is a global coordinates system X_WO_WY_W. Another is robot's local coordinates system that is represented with X_LO_LY_L. It is shown in Fig.1.

Fig. 1.

State space

3.1. Modified State Space

In classical SLAM solution, the state of robot includes the position and heading of robot. The augmented state vector consists of the state (xv, yv, θv) of robot and the state (xi, yi) of each landmark. In this paper, we do not represent directly the position and heading of robot. The state of robot is represented indirectly with (xv, yv, xs, ys). (xv, yv) is the position of robot and (xs, ys) is the position of laser rangefinder. The state of landmark is represented with (xi, yi). Additionally, all x-coordinates are grouped into X_X and all y-coordinates are grouped into X_Y. Augmented state vector X(k) consists of X_X(k) and X_Y(k). It is defined as:

\begin{aligned} X (k) = [\begin{matrix} X_{X} (k) \\ X_{Y} (k) \end{matrix}] \\ X_{X} (k) = [\begin{matrix} x_{v} (k) \\ x_{s} (k) \\ x_{1} (k) \\ ⋮ \\ x_{N} (k) \end{matrix}] X_{Y} (k) = [\begin{matrix} y_{v} (k) \\ y_{s} (k) \\ y_{1} (k) \\ ⋮ \\ y_{N} (k) \end{matrix}] \\ X_{V} (k) = [\begin{matrix} x_{v} (k) \\ y_{v} (k) \end{matrix}] X_{S} (k) = [\begin{matrix} x_{s} (k) \\ y_{s} (k) \end{matrix}] \\ X_{i} (k) = [\begin{matrix} x_{i} (k) \\ y_{i} (k) \end{matrix}] \end{aligned}

(8)

Where X_V, X_S and Xi (i=1,2…N) are the position estimation of robot, laser rangefinder and landmark respectively. The covariance matrix is defined as:

P (k) = [\begin{matrix} P_{x} (k) & P_{x y} (k) \\ P_{x y} (k)^{T} & P_{Y} (k) \end{matrix}]

(9)

Based on the estimation X_V and X_S, the heading of robot is computed as:

\begin{aligned} θ_{V} = arctan (x_{V} - x_{S}, y_{S} - y_{V}) \\ P_{θ_{v}} [\frac{y_{S} - y_{V}}{L L^{2}} - \frac{y_{S} - y_{V}}{L L^{2}} 0 \dots 0 \frac{x_{V} - x_{S}}{L L^{2}} 0 \dots 0] * \\ P * {[\frac{y_{S} - y_{V}}{L L^{2}} \frac{y_{S} - y_{V}}{L L^{2}} 0 \dots 0 \frac{x_{V} - x_{S}}{L L^{2}} \frac{x_{V} - x_{S}}{L L^{2}} 0 \dots 0]}^{T} \end{aligned}

(10)

Where LL is the distance from Laser rangefinder to the center of robot as shown in Fig.1.

3.2. Motion Model

It is assumed that the robot moves along a circular arc at each step. It is shown in Fig.2. The state transition of robot is based on odometer information. Date from odometer is U_k = (D_k, γ_k) at step k. D_k is the distance traveled along the arc and γ_k is the change of heading direction. R_k = D_k /γ_k is the radius of arc. It is assumed that all considering landmark are static. The system state transition is:

\begin{aligned} X (k) = f (X (k - 1), U_{k}) + ω (k) \\ = [\begin{matrix} 1 - \frac{D_{k} \sin γ_{k}}{L L γ_{k}} & \frac{D_{k} \sin γ_{k}}{L L γ_{k}} & \dots & \frac{D_{k} (\cos γ_{k} - 1)}{L L γ_{k}} & \frac{D_{k} (\cos γ_{k} - 1)}{L L γ_{k}} & \dots \\ 1 - \frac{D_{k} \sin γ_{k}}{L L γ_{k}} - \cos γ_{k} & \frac{D_{k} \sin γ_{k}}{L L γ_{k}} + \cos γ_{k} & \dots & - (\frac{D_{k} (\cos γ_{k} - 1)}{L L γ_{k}} - \sin γ_{k}) & \frac{D_{k} (\cos γ_{k} - 1)}{L L γ_{k}} - \sin γ_{k} & \dots \\ ⋮ & ⋮ & I & ⋮ & ⋮ & ⋮ \\ \frac{D_{k} (\cos γ_{k} - 1)}{L L γ_{k}} & - \frac{D_{k} (\cos γ_{k} - 1)}{L L γ_{k}} & \dots & 1 - \frac{D_{k} \sin γ_{k}}{L L γ_{k}} & \frac{D_{k} \sin γ_{k}}{L L γ_{k}} & \dots \\ \frac{D_{k} (\cos γ_{k} - 1)}{L L γ_{k}} - \sin γ_{k} & - (\frac{D_{k} (\cos γ_{k} - 1)}{L L γ_{k}} - \sin γ_{k}) & \dots & 1 - \frac{D_{k} \sin γ_{k}}{L L γ_{k}} - \cos γ_{k} & \frac{D_{k} \sin γ_{k}}{L L γ_{k}} + \cos γ_{k} & \dots \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ & I \end{matrix}] \\ l * X (k - 1) + ω (k) \\ = F_{k} * X (k - 1) + ω (k) \end{aligned}

(11)

Fig. 2.

Motion model

Where ω(k) is the uncertainty on odometer modeling and slipping. It is represented with a white noise vector with zero mean and covariance Q. Matrix F_k only depends on the input vector and is uncorrelated with current system state estimation. So, system motion model is linear.

3.3. Observation Model

Mobile robot is equipped with a laser rangefinder. Laser rangefinder scans environmental information, extracts environmental landmark feature and compute the local coordinates (x_L, y_L) of landmark. It is then translated into global coordinates (x_G, y_G) based on current system state estimation. With data association technique, this observed landmark is associated with ith landmark in map. We use the difference between (x_G, y_G) and its corresponding landmark (xi, yi) as observation value Z. In fact, Z is a zero vector. The observation model is:

\begin{aligned} Z (k) = h (X (k)) + v (k) \\ = [\begin{matrix} 1 - \frac{y_{L}}{L L} & \frac{y_{L}}{L L} & \dots & - 1 & \dots & - \frac{x_{L}}{L L} & \frac{x_{L}}{L L} & \dots & 0 & \dots \\ \frac{x_{L}}{L L} & - \frac{x_{L}}{L L} & \dots & 0 & \dots & 1 - \frac{y_{L}}{L L} & \frac{y_{L}}{L L} & \dots & - 1 & \dots \end{matrix}] * X (k) + v (k) \\ = H_{k} * X (k) + v (k) \end{aligned}

(12)

Where v(k) is the uncertainty on sensor measurement and can be represented with a two-dimensional white noise vector with zero mean and covariance R. Matrix Hk only depends on the observed coordinates of landmark and is uncorrelated with current system state prediction. So, observation model is linear.

3.4. Augmentation Model

After a new landmark is observed and valided, its global coordinate is augmented into the system state vector. Based on its local coordinates (x_L, y_L) and current system state estimation, its global coordinates (x_G, y_G) is computed as:

\begin{aligned} X (G) = & g (X (k), (x, y)_{L}) + v (k) \\ = & [\begin{matrix} 1 - \frac{y_{L}}{L L} & \frac{y_{L}}{L L} & 0 \dots 0 & - \frac{x_{L}}{L L} & \frac{x_{L}}{L L} & 0 \dots 0 \\ \frac{x_{L}}{L L} & \frac{x_{L}}{L L} & 0 \dots 0 & 1 - \frac{y_{L}}{L L} & \frac{y_{L}}{L L} & 0 \dots 0 \end{matrix}] * X (k) + v (k) \\ = & G_{k} * X (k) + v (k) \end{aligned}

(13)

Where v(k) is a noise vector as defined in observation model. Matrix G_k is uncorrelated with current system state estimation. So, augmentation model is linear.

4. Matrix definition

In this paper, two special matrices are used. They are defined as follows:

Definition 1

block matrix can be represented as:

A = [\begin{matrix} A_{1} & A_{2} \\ - A_{2} & A_{1} \end{matrix}]

(14)

Blocks on diagonal are two same submatrix and blocks under diagonal are two opposite submatrix.

Definition 2

if matrix fulfils definition 1. Block A1 is a symmetrical square submatrix and A2 is an anti-symmetrical square submatrix.

Let Ω is the matrix set that fulfils definition 1. Let Θ is the matrix set that fulfils definition 2. They have following properties:

Theorem 1

if matrix A and B can be represented as definition 1, then AB can be represented as definition 1. That is:

If A \in Ω and B \in Ω, then A * B \in Ω .

Theorem 2

if matrix A can be represented as definition 1, then its transpose matrix can be represented as definition 1. That is:

If A \in Ω, then A^{T} \in Ω .

Theorem 3

if matrix A can be represented as definition 2, then its inverse can be represented as definition 2. That is:

If A \in Θ, then A^{- 1} \in Θ .

Theorem 4

if matrix A can be represented as definition 1 and matrix B can be represented as definition 2, then ABA^T can be represented as definition 2. That is:

If A \in Ω and B \in Θ, then A * B * A^{T} \in Θ .

Theorem 5

if matrix A and B can be represented as definition 1, then A±B can be represented as definition 1. That is:

If A \in Ω and B \in Ω, then A \pm B \in Ω .

Theorem 6

if matrix A and B can be represented as definition 2, then A±B can be represented as definition 2. That is:

If A \in Θ and B \in Θ, then A \pm B \in Θ .

5. Optimization SLAM solution

According above matrix definition, the modified system motion model Fk, the modified system observation model Hk and the modified system augmentation model G_k can be represented as definition 1:

\begin{aligned} F_{k} = [\begin{matrix} F_{1} & F_{2} \\ - F_{2} & F_{1} \end{matrix}] \in Ω \\ H_{k} = [\begin{matrix} H_{1} & H_{2} \\ - H_{2} & H_{1} \end{matrix}] \in Ω \\ G_{k} = [\begin{matrix} G_{1} & G_{2} \\ - G_{2} & G_{1} \end{matrix}] \in Ω \end{aligned}

(15)

Based on the properties of two special matrices, if the initial covariance matrix can be represented as definition 2. That is:

P (0) \in Θ

(16)

the covariance matrix can still be represented as definition 2 after prediction, update and augmentation.

P (k) = [\begin{matrix} P_{1} & P_{2} \\ - P_{2} & P_{1} \end{matrix}] \in Θ

(17)

The higher dimensional system models and covariance matrix can be represented with lower dimensional submatrix. With this property, an optimization SLAM solution is proposed. The computational requirement and memory requirement can be decreased at least by half.

5.1. Prediction

With above system motion model, the states of robot and landmarks are predicted. The state vector and covariance matrix are predicted as:

\begin{aligned} X^{-} (k) = F_{k} * X^{+ +} (k - 1) \\ P^{-} (k) = F_{k} P^{+ +} (k - 1) F_{k}^{T} + Q \end{aligned}

(18)

Where X⁺⁺, P⁺⁺ denotes the posterior estimation after augmentation and X⁻, P⁻ denotes the prior estimation with respect to each observation.

According to theorem 1, 2, 4 and 6, if the posterior covariance estimation after augmentation P⁺⁺ (k − 1) ∈ Θ, then the prior covariance estimation P⁻ (k) ∈ Θ.

5.2. Update

With data association, the difference between real observation and observation prediction is called innovation. In this paper, the real observation vector Z is a zero vector. Obserbation prediction is computed based on system observation model. Innovation, innovation covariance and kalman Gain are computed as follows:

\begin{aligned} v_{i} (k) = 0 - Z_{i} (k) = - H_{k} * X^{-} (k) \\ S_{i} (k) = H_{k} * P^{-} (k) * H_{k}^{T} + R \\ W_{i} = P^{-} (k) * H_{k}^{T} * S_{i} (k)^{- 1} \end{aligned}

(19)

According to theorem 1, 2, 3, 4 and 6, if the prior covariance estimation P⁻ (k) ∈ Θ, then innovation covariance S_i ∈ Θ and Kalman gain W_i ∈ Ω.

The system state vector and covariance matrix are updated according to following update rules:

\begin{aligned} X^{+} (k) = X^{-} (k) + W_{i} * v_{i} (k) \\ P^{+} (k) = P^{-} (k) - W_{i} * S_{i} (k) * W_{i}^{T} \end{aligned}

(20)

According to theorem 1, 2, 4 and 6, if prior covariance estimation P⁻ (k) ∈ Θ, then posterior covariance estimation P⁺ (k) ∈ Θ.

5.3. Augmentation

When a new landmark is observed and validated, the global coordinates of new landmark is incorporated into the system vector according to augmentation model. Covariance matrix before augmentation is denoted as P⁺(k) and covariance matrix after augmentation is denoted as P⁺⁺(k).

Firstly, there are two matrices: M = G_kP⁺(k) and N = G_kP⁺ (k) G^T_k. According to theorem 1, 2, 3, 4 and 6, if P⁺ (k) ∈ Θ, then M ∈ Ω and N ∈ Θ. Matrix M and matrix N can be represented as:

\begin{aligned} P^{+} (k) = [\begin{matrix} P_{1} & P_{2} \\ - P_{2} & P_{1} \end{matrix}] \\ M = G_{k} P^{+} (k) \\ = [\begin{matrix} G_{1} P_{1} - G_{2} P_{2} & G_{1} P_{2} + G_{2} P_{1} \\ - G_{1} P_{2} - G_{2} P_{1} & G_{1} P_{1} - G_{2} P_{2} \end{matrix}] = [\begin{matrix} M_{1} & M_{2} \\ - M_{2} & M_{1} \end{matrix}] \\ N = G_{k} P^{+} (k) G_{k}^{T} = [\begin{matrix} N_{1} & N_{2} \\ - N_{2} & N_{1} \end{matrix}] = [\begin{matrix} n_{1} & n_{2} \\ - n_{2} & n_{1} \end{matrix}] \end{aligned}

(21)

Because matrix N is a 2×2 symmetrical matrix and N ∈ Θ, n2=0.

The system state vector and covariance matrix are augmented as:

\begin{aligned} X^{+ +} (k) = [\begin{matrix} X_{X}^{+} (k) \\ G_{1} X_{X}^{+} (k) + G_{2} X_{Y}^{+} (k) \\ X_{Y}^{+} (k) \\ - G_{2} X_{X}^{+} (k) + G_{1} X_{Y}^{+} (k) \end{matrix}] \\ P^{+ +} (k) = [\begin{matrix} P_{1} & M_{1}^{T} & P_{2} & - M_{2}^{T} \\ M_{1} & n_{1} & M_{2} & 0 \\ - P_{2} & M_{2}^{T} & P_{1} & M_{1}^{T} \\ - M_{2} & 0 & M_{1} & n_{1} \end{matrix}] \end{aligned}

(22)

Let ${P_{x}}^{'} = [\begin{matrix} P_{1} & M_{1}^{T} \\ M_{1} & n_{1} \end{matrix}]$ and ${P_{y}}^{'} = [\begin{matrix} P_{2} & - M_{2}^{T} \\ M_{2} & 0 \end{matrix}]$ . The former is a symmetrical square matrix and the latter is an anti-symmetrical square matrix. Posterior covariance estimation after augmentation P⁺⁺(k) can be represented as definition 2.

P^{+ +} (k) = [\begin{matrix} P_{x}^{'} & P_{y}^{'} \\ - P_{y}^{'} & P_{x}^{'} \end{matrix}] \in Θ

(23)

From step k-1 to step k, system state mean and covariance are estimated recursively. After prediction, update and augmentation, covariance matrix can still be represented as definition 2. The system models and covariance matrix are represented with two lower dimensional submatrix, therefore the computational requirement and memory requirement can be decreased. The covariance matrix is fully updated without any approximation at each step, so the algorighm is consistent and convergent theoretically.

6. Experimental results

In this section, we demonstrate the experimental results of proposed simultaneous localization and map building solution with mobile robot ATRVII. Robot moves around our lab, extracts natural landmarks and build incrementally environment map.

Corner points extracted from data of rangfinder are used as the environmental landmark features. There are 36 corner points. At each step, the traveling distance of robot ranges form 100 (unit: mm, the same below) to 3000 and observation distance ranges form 500 to 4000. In covariance matrix Q of dynamic noise, the items corresponding to x_V, x_S, y_V, y_S are set as: σ_XV=30, σ_XS=30, σ_YV=30, σ_YS=30. The other items are all zero. In covariance matrix R of measurement noise, the items corresponding to xi, yi are set as: σxi=20, σyi=20.

Fig. 3 shows the estimated position errors of robot with σ uncertainty. Fig. 4 shows σ bounds of robot position uncertainty. It is compared with full SLAM solution. Experimental results show that this SLAM solution is consistent and convergent realistically.

Fig. 3.

Error and σ bounds of robot position estimation

Fig. 4.

σ bounds of robot position estimation (solid line for optimization solution and dashed line for full solution)

7. Conclusions

Kalman filter is an efficient sulution to simultaneous localization and map building. A number of sub-optimal solutions to SLAM reduce computational complexity with approximation techniques. However, there is not theoretical foundation for consistence and convergence of algorithm. Computational efficiency increases at the cost of decreasing consistence and convergence.

In this paper, by representing indirectly the state of robot and by selecting properly observation vector, the motion model, observation model and augmentation model are represented as a kind of special matrix. The covariance matrix is represented as another kind of special matrix. The higher dimensional system models and covariance matrix are represented with two lower dimensional submatrices. The computational requirement and memory requirement are decreased at least by half. Since the covariance matrix is fully updated without any approximation during estimation, the proposed solution is consistent and convergent theoretically and realistically. It fulfils the 3C (Consistency, Convergence, Computational efficiency) requirement of SLAM algorithm. Because the system models are all linear, the result of optimization solution is more conservative comparing with the full covariance SLAM solution.

References

Begum

Mann

& Gosine

(2008). Integrated fuzzy logic and genetic algorithmic approach for simultaneous localization and mapping of mobile robots, Applied Soft Computing, Vol.8, No.1, pp. 150–165, ISSN: 1568–4946.

Blanco

Fernández-Madrigal

& Gonzalez

(2008). A Novel Measure of Uncertainty for Mobile Robot SLAM with Rao-Blackwellized Particle Filters, International Journal of Robotics Research, Vol.27, No.1, pp. 73–89, ISSN: 1741–3176.

Castellanos

Tardos

& Schmidt

(1997). Building a Global Map of the Environment of a Mobile Robot: The Importance of Correlations, Proceedings of IEEE International Conference on Robotics and Automation, pp. 1053–1059, ISBN: 0-7803-3612-7, Albuquerque, New Mexico, USA, Apr 1997. IEEE.

Deans

& Hebert

(2000). Invariant filtering for simultaneous localization and mapping, Proceedings of the 2000 IEEE International Conference on Robotics and Automation, pp. 1042–1047, ISBN 0-7803-5886-4, San Francisco, CA, USA, April 2000, IEEE.

Dissanayake

Durrant-Whyte

& Bailey

(2000). A Computationally Efficient Solution to the Simultaneous Localisation and Map Building (SLAM) Problem, Proceedings of IEEE International Conference on Robotics and Automation, pp. 1009–1014, ISBN: 0-7803-5886-4, San Francisco, CA, USA, April 2000. IEEE.

Dissanayake

Newman

Clark

Durrant-Whyte

& Csorba

(2001). A solution to the simultaneous localization and map building (SLAM) problem, IEEE Trans. on Robotics and Automation, Vol. 17, No. 3, pp. 229–241, ISSN: 1042–296X.

Dissanayake

Williams

Durrant-Whyte

& Bailey

(2002). Map Management for Efficient Simultaneous Localization and Mapping (SLAM), Autonomous Robots, Vol.12, No.3, pp. 267–286, ISSN: 1573–7527.

Durrant-Whyte

Dissanayake

& Gibbens

(2000). Toward deployment of large–scale simultaneous localisation and map building (SLAM) systems, In: Robotics Research - The Ninth International Symposium, Hollerbach

J. M.

and Koditschek

D. E.

, pp. 161–168, Springer Verlag, ISBN: 1852332921, London, 2000.

Durrant-whyte

& Bailey

(2006). Simultaneous Localization and Mapping (SLAM): Part I The Essential Algorithms, Robotics and Automation Magazine, Vol.13, pp.99–110, ISSN: 1070–9932.

10.

Grisetti

Tipaldi

Stachniss

Burgard

& Nardi

(2007). Fast and accurate SLAM with Rao-Blackwellized particle filters, Robotics and Autonomous Systems, Vol.55, No.1, pp. 30–38, ISSN: 0921–8890.

11.

Guivant

& Nebot

(2001). Optimization of the simultaneous localization and map-building algorithm for real-time implementation, IEEE Transactions on Robotics and Automation, Vol. 17, No, 3, pp. 242–257, ISSN: 1042–296X.

12.

Guivant

& Nebot

(2003). Solving computational and memory requirements of feature based simultaneous localization and map building algorithms, IEEE Trans. on Robotics and Automation, Vol.19, No.4, pp. 749–755, ISSN: 1042–296X.

13.

Holmes

Klein

& Murray

(2008). A Square Root Unscented Kalman Filter for visual monoSLAM, Proceedings of IEEE International Conference on Robotics and Automation, pp. 3710–3716, ISBN: 978-1-4244-1646-2, Pasadena, California, May 2008. IEEE.

14.

Julier

& Uhlmann

(2007). Using covariance intersection for SLAM, Robotics and Autonomous Systems, Vol.55, No.1, pp.3–20, ISSN: 0921–8890.

15.

Kwok

& Rad

(2006). A Modified Particle Filter for Simultaneous Localization and Mapping, Journal of Intelligent and Robotic Systems, Vol.46, No. 4, pp. 365–382, ISSN: 0921–0296.

16.

Leonard

& Feder

(2001). Decoupled Stochastic Mapping, IEEE Journal of Oceanic Engineering, Vol. 26, No. 4, pp. 561–571, ISSN: 0364–9059.

17.

Hong

& Luo

(2006). Novel Mobile Robot Simultaneous Localization and Mapping Using Rao-Blackwellised Particle Filter, International Journal of Advanced Robotic Systems, Vol. 3, No. 3, pp. 231–238, ISSN: 1729–8814.

18.

Milford

Schulz

Prasser

Wyeth

and Wiles

(2007). Learning spatial concepts from RatSLAM representations, Robotics And Autonomous Systems, Vol. 55, No. 5, PP. 403–410, ISSN:0921–8890.

19.

Montemerlo

Thrun

Koller

Wegbreit

(2002). FastSLAM: A factored solution to the simultaneous localization and mapping problem. Proceedings of the AAAI National Conference on Artificial Intelligence, pp. 593–598, ISBN: 0-262-51129-0, Edmonton, Canada, August 2002. AAAI Press.

20.

Smith

& Cheeseman

(1986). On the Representation of Spatial Uncertainty, The International Journal of Robotics Research, Vol. 5, No. 4, pp. 56–68, ISSN: 1741–3176.

21.

Smith

Self

& Cheeseman

(1990). Estimating uncertain spatial relationships in robotics, In: Autonomous robot vehicles, Cox

I.J.

and Wilfong

G. T.

, ed., pp. 167–193, Springer Verlag, New York, Inc., ISBN:0-387-97240-4, New York, NY.

22.

Thrun

Fox

& Burgard

(1998). A probabilistic approach to concurrent mapping and localization for mobile robots, Machine Learning, Vol. 31, pp. 29–53, ISSN: 1573–0565.

23.

Thrun

Liu

Koller

Ghahramani

& Durrant-Whyte

(2004). Simultaneous Localization and Mapping with Sparse Extended Information Filters, The International Journal of Robotics Research, Vol. 23, No. 7–8, pp.693–716, ISSN: 1741–3176.

24.

Wang

Huang

& Dissanayake

(2005). Decoupling localization and mapping in SLAM using compact relative maps, Proceedings of International Conference on Intelligent Robotics and Systems, pp. 1041–1046, ISBN: 0-7803-8912-3, Aug 2005, Edmonton, Canada. IEEE.

25.

Williams

Dissanayake

& Durrant-Whyte

(2002). An Efficient Approach to the Simultaneous Localization and Mapping Problem, Proceedings of IEEE International Conference on Robotics and Automation, pp. 406–411, ISBN: 0-7803-7272-7, Washington, DC, May 2002. IEEE.