Simultaneous localization and odometry self calibration for mobile robot

Abstract

This paper presents both the theory and the experimental results of a method allowing simultaneous robot localization and odometry error estimation (both systematic and non-systematic) during the navigation. The estimation of the systematic components is carried out through an augmented Kalman filter, which estimates a state containing the robot configuration and the parameters characterizing the systematic component of the odometry error. It uses encoder readings as inputs and the readings from a laser range finder as observations. In this first filter, the non-systematic error is defined as constant and it is overestimated. Then, the estimation of the real non-systematic component is carried out through another Kalman filter, where the observations are obtained by two subsequent robot configurations provided by the previous augmented Kalman filter. There, the systematic parameters in the model are regularly updated with the values estimated by the first filter. The approach is theoretically developed for both the synchronous and the differential drive. A first validation is performed through very accurate simulations where both the drive systems are considered. Then, a series of experiments are carried out in an indoor environment by using a mobile platform with a differential drive.

1 Introduction

Mobile robot navigation highly relies on odometry. The encoder data are extensively used in the localization process by fusing them with data coming from another (or several) sensor. At least one of the sensors used for data fusion has to be exteroceptive in order to avoid the error growing with the distance traveled by the robot. Clearly, any fusion architecture needs to know the accuracy of the estimation of each sensor in order to weigh all the data in a proper manner. In particular, when the fusion regards the encoder data the accuracy is completely described by the odometry error covariance matrix. Therefore, determining the odometry errors of a mobile robot is very important both in order to reduce them, and to know the accuracy of the state configuration estimated by using encoder data.

Odometry errors can be separated in systematic and non-systematic. In a series of papers Borenstein and collaborators (Borenstein, 1994, 1995, 1998; Borenstein and Feng, 1994, 1995, 1996) investigated on possible sources of both kind of errors. A review of all the types of these sources is given in Borenstein (1998). In the work by Borenstein and Feng (1994), a calibration technique called UMBmark test has been developed to calibrate for systematic errors of a mobile robot with a differential drive.

Goel et al. (1999) used another calibration procedure to compensate systematic errors. They referred to the differential drive mobile robot Pioneer AT. They measured the actual velocities of the wheels and the velocity measurements from the encoders, when the robot was sitting on a box and the wheels rotated freely in the air. In this way they found a relationship between the velocity returned by encoders and the actual velocity measured by using a precise tachometer. Furthermore, they measured the effective axle length due to skid steering which differs from that given by the specifications for the robot.

Larsen (1998), Larsen et al. (1998) Martinelli and Siegwart (2003), Martinelliet al. (2003) and Caltabiano et al. (2004) suggested an algorithm that uses the robot's sensors to automatically calibrate the robot as it operates. In particular, they introduced an augmented Kalman filter (AKF) which simultaneously estimates the robot configuration and the parameters characterizing the systematic odometry error. This filter uses encoder readings as inputs and the measurements coming from an exteroceptive sensor as observations. In Larsen (1998) and Larsen et al. (1998) the vision sensor is adopted and in Martinelli and Siegwart (2003) and Martinelli et al. (2003) a laser range-finder sensor. In these cases the method was experimentally validated in an indoor environment. In Caltabiano et al. (2004) an outdoor environment was considered.

Finally, Doh et al. (2003) suggested a procedure to calibrate an odometry system called the PC-method. The basic idea characterizing this method consists in moving the robot forth and back along the same trajectory. For this purpose, the Generalized Voronoi Graph was adopted.

Many investigations have been carried out on the odometry error from a theoretical point of view. Wang (1988) and Chong and Kleeman (1997) analyzed the non-systematic errors and computed the odometry covariance matrix Q for special kind of the robot trajectory. Kelly (2001) presented the general solution for linearized systematic error propagation for any trajectory and any error model. Martinelli (2002) derived general formulas for the covariance matrix and suggested a strategy to estimate the model parameters for a mobile robot with a synchronous drive system. This strategy is based on the evaluation of the mean values of some quantities (called observables) which depend on the model parameters and on the chosen robot motion.

Martinelli and Siegwart(2003) and Martinelliet al. (2003) suggested a method to estimate both systematic and non-systematic odometry error of a mobile robot, during navigation. Concerning the systematic component, they adopted the AKF as in Caltabiano et al. (2004), Larsen (1998), Larsen et al. (1998), Martinelli and Siegwart (2003) and Martinelli et al. (2003) by considering also the case of a synchronous drive. Concerning the non-systematic parameters, they introduced a new filter (the Observable Filter, OF) where the state to be estimated contains the parameters characterizing the non-systematic error and the observations are provided by the observables as defined in Martinelli (2002) and which can be evaluated by knowing two subsequent robot configurations.

The main contribution of this paper is twofold. On one side the previous AKF and the OF are theoretically discussed for mobile robot with synchronous and differential drive. On the other side, here the two filters are integrated in a coherent framework allowing to auto-calibrate the odometry system while localizing during the robot movement. Therefore, the strategy presented here differs dramatically from the one suggested in Martinelli (2002) not only from a computational point of view but also for a very important practical reason. While with the method in Martinelli (2002) it was possible to calibrate the odometry only before using a system (off-line), the strategy suggested here embeds this capability in a complete framework, able to auto-calibrate while localizing during the movement.

In Section 2 we introduce the model adopted here to characterize the odometry error for a mobile robot with the synchronous and the differential drive. In Section 3 we summarize the AKF and we extend the same filter to the case of a synchronous drive. The OF is presented in Section 4 and deeply discussed for the specific case implemented in the experiments. In particular, the influence on the accuracy on the non-systematic parameter estimations due to the error on the systematic error evaluation is investigated. In Section 5, we show and discuss both the results obtained through simulations and the experimental results obtained with the fully autonomous robot Donald Duck in an indoor environment. Finally, conclusions are provided in Section 6.

2 The odometry error model

We consider two different drive systems: synchronous and differential. Concerning the former we adopt the same model introduced in Martinelli (2002) whereas for the latter we adopt a model very similar to the one introduced by Chong and Kleeman (1997). In the next sections we briefly summarize these odometry error models.

2.1 Synchronous drive

In the synchronous drive system each wheel is capable of being driven and steered. Let us denote with $\delta\rho_i$ and $\delta\theta_i$ respectively the robot translation and rotation in the ith time step with respect to a global world coordinate frame. Because of the odometry errors these values differ from the encoder readings. The model here adopted assumes that $\delta\rho_i$ and $\delta\theta_i$ are random variables, uncorrelated, with gaussian distribution. In particular their mean values are given by the encoder readings corrected for the systematic error. It is assumed that the systematic errors (both in translation and rotation) increase linearly with the distance traveled by the robot. Therefore,

$$ \overline{\delta\rho}_i=\delta_{\rho}\delta\rho^e_i\qquad\overline{\delta\theta}_i=\delta\theta^e_i+E_{\theta}\delta\rho^e_i $$

(1)

where $\delta\rho^e_i$ and $\delta\theta^e_i$ are the encoder readings respectively for the robot translation and rotation, and $\delta_{\rho}$ and $E_{\theta}$ characterize the systematic errors. Finally, it is also assumed that the variances increase linearly with the distance traveled by the robot. We therefore can write:

$$ {delta_rho_i}\delta\rho_i= \overline{\delta\rho}_i +\nu^{\rho}_i \qquad \delta\theta_i=\overline{\delta\theta}_i+\nu^{\theta}_i $$

(2)

where

$$ \nu^{\rho}_i\sim N\big(0,K_{\rho}\big|\delta\rho^e_i\big|\big)\qquad \nu^{\theta}_i\sim N\big(0,K_{\theta} \big|\delta\rho^e_i\big|\big) $$

(3)

The odometry error model presented here is based on 4 parameters. Two of them ($\delta_{\rho}, E_{\theta}$) characterize the systematic components whereas the other two ($K_{\rho}$, $K_{\theta}$) characterize the non-systematic components. Clearly, these parameters depend on the environment where the robot moves.

2.2 Differential drive

A simple way to characterize the odometry error for a mobile robot with a differential drive system is obtained by modeling separately the error in the translation of each wheel Chong and Kleeman (1997). The actual translation of the right/left wheel related to the ith time step is assumed to be a gaussian random variable satisfying the following relation:

$$ \displaylines{ \delta\rho^{R/L}_i = \overline{\delta\rho}_i^{R/L}+\nu^{R/L}_i\cr \overline{\delta\rho}_i^{R/L} = \delta\rho_i^{eR/L}\delta_{R/L}\cr \nu^{R/L}_i \sim N\big(0,K_{w}\big|\delta\rho_i^{eR/L}\big|\big)} $$

(4)

In other words, both $\delta\rho_i^{R}$ and $\delta\rho_i^{L}$ are assumed gaussian random variables, whose mean values are given by the encoder readings (respectively $\delta\rho_i^{eR}$ and $\delta\rho_i^{eL}$) corrected for the systematic errors (which are assumed to increase linearly with the distance traveled by each wheel), and whose variances also increase linearly with the traveled distance. Furthermore, it is assumed that $\delta\rho_i^{R}$ and $\delta\rho_i^{L}$ are uncorrelated. With respect to the Chong–Kleeman model, here only one parameter ($K_w$) is adopted to characterize both the variances for the right and left wheel. The robot translation and rotation are given by the following relations:

$$ \delta\rho_i=\frac{\delta\rho_i^R+\delta\rho_i^L}{2}\qquad\delta\theta_i=\frac{\delta\rho_i^R-\delta\rho_i^L}{d\delta_d} $$

(7)

where d is the estimated distance between the wheels and $\delta_d$ characterizes the uncertainty on this estimation. Clearly, the robot translation and rotation are correlated accordingly to the Eqs. (4)–(7).

The proposed odometry error model is based on 4 parameters. Three ($\delta_{R}, \delta_L$ and $\delta_d$) characterize the systematic components whereas the last one ($K_w$) characterizes the non-systematic components.

In Sections 3 and 4 we introduce the strategy to simultaneously estimate all these parameters via the localization during the standard robot navigation.

3 Systematic parameters estimation during navigation

In order to estimate the parameters characterizing the systematic error (both for synchronous and differential drive) we adopt an extended Kalman filter which estimates a state containing the robot configuration and the systematic parameters (augmented state). This method was already adopted for a differential drive system (Caltabiano et al., 2004; Larsen, 1998; Larsen et al., 1998; Martinelli and Siegwart, 2003; Martinelli et al., 2003).

Let us denote with X the robot configuration ($X=[x, y, \theta]^T$), with $X_a$ the augmented state and with $P_a$ its covariance matrix. We have, respectively for the synchronous and differential drive $X_a=[x, y, \theta, \delta_{\rho}, E_{\theta}]^T \qquad X_a=[x, y, \theta, \delta_{R}, \delta_{L}, \delta_{d}]^T$

The state X evolves accordingly to the dynamical equation $X_{i+1}=f(X_i, U_i)$ where $U_i=[\delta\rho_i, \delta\theta_i]^T$ for the synchronous drive and $U_i=[\delta\rho^R_i, \delta\rho^L_i]^T$ for the differential drive. The observation at the ith time step depends on the current robot configuration and it is assumed to be affected by an error $w_i$ with a gaussian distribution, zero-mean and covariance matrix $R_i=\langle w_iw^T_i\rangle$

$$ z_{i}=h(X_{i}, w_{i}) $$

(8)

A simple example for this function is obtained by defining z as the vector containing all the distances in several directions of observation from the robot configuration towards the landmarks (e.g. straight lines stored in a map a priori known). In this case the function h characterizes the measurement prediction of a laser range finder. In the experiments carried out in our lab and discussed in Section 5, this function was different since, instead of using the raw data, we extracted the straight lines from the data (see also Arras et al. (2001)).

The dynamical equation for the augmented state $X_a$ is given by the equation:

$$ X_{ai+1}=f_a(X_{ai}, U_i) $$

(9)

The function $f_a$, restricted to the first three components, is obtained directly from the function f including the dependence on the systematic parameters in the input $U_i$; regarding the last components (two for the synchronous drive and three for the differential drive) $f_a$ is the identity function since there is no evolution in time for the error parameters.

In order to obtain the EKF equations for the augmented state (i.e. the equations of the AKF), it is necessary to compute the Jacobian $F_{a}$ of the function $f_a$ with respect to $X_{a}$ and the Jacobian $G_{a}$ of the function $f_a$ with respect to the vector ν, which is $[\nu^{\rho}, \nu^{\theta}]^T$ in the synchronous drive (Eq. (2)) and $[\nu^{R}, \nu^{L}]^T$ in the differential drive (Eq. (6)): $F_{a}=\nabla_{X_a}f_a|_{X_{a}(i|i),\overline{U}_i}\qquad G_{a}=\nabla_{\nu}f_a|_{X_{a}(i|i), \overline{U}_i}$where $X_{a}(i|i)$ is the state estimated at the previous time step and $\overline{U}_i$ is the mean value of the vector $U_i$ previously defined. The computation of these matrices can be found in Larsen (1998) and Larsen et al. (1998) for the differential drive and can be easily carried out for the synchronous drive.

Finally, in order to implement this filter, we must know the covariance of the vector $w$ in (8) and the covariance of ν in (4) and (2). The former regards the exteroceptive sensor, the latter regards the odometry and it is known once the non-systematic parameters are known (see Eqs. (6) for the differential drive and the Eq. (3) for the synchronous drive). Since at the beginning these parameters are not known, to maintain consistent the estimation process of the AKF, we adopt constant values which overestimate the true values of the non-systematic parameters. This results in a consistent estimation. The price to pay is a slower convergence since in practice the AKF strongly relies on the exteroceptive sensor and it partially exploits the information coming from the odometry. The use of constant values for the non-systematic parameters makes the AKF independent of the non-systematic parameters estimation which will be carried out by a separate filter discussed in the next section.

Once the Jacobians $F_a$ and $G_a$ are computed and the covariance matrices of $w$ and ν are also determined, it is possible to implement the AKF by applying the standard EKF equations (Bar-Shalom and Fortmann, 1988; Leonard and Durrant-Whyte, 1992).

4 Non-systematic parameters estimation during navigation via localization

The non-systematic parameters cannot be evaluated following the previous method. Indeed, by including them in the augmented state, the Kalman gain related to these parameters is null.

In the following, we define the encoder trajectory between two time steps $t_A$ and $t_B$, as the set of the encoder displacements occurred during the interval $t_B-t_A$. Obviously, the encoder trajectory is provided by the encoder sensors and therefore it is known.

The method we suggest to estimate the non-systematic parameters exploits the observables defined in Martinelli (2002). The observables are random variables related to a given robot motion whose statistical properties (e.g. mean value and variance) depend on the parameters characterizing the odometry error and on the encoder trajectory. Therefore, for a given encoder trajectory, the statistics of the observables only depends on the non-systematic and systematic parameters. In particular, we have for the mean value:

$$ \langle {\it Obs} \rangle=m^{\it Obs}(K, K_s) $$

(10)

where Obs is the considered observable, K is the vector containing the non-systematic parameters ($K=[K_{\rho}, K_{\theta}]^T$ for the synchronous drive and $K=K_{w}$ for the differential drive), $K_s$ is the vector containing the systematic parameters and the function $m^{\it Obs}$ is completely determined for the observable Obs for a given encoder trajectory. In Martinelli (2002a, 2002b) we derived analytical formulas to compute the function $m^{\it Obs}$ for several encoder trajectories.

On the other hand, in Martinelli (2002a) it was shown that it is possible to estimate the mean value of an observable in another way which does not require the knowledge of the odometry parameters. We remark that this second method does not provide the entire statistics of the chosen observable but only its mean value. This second method only requires to know the actual initial and final robot configurations of the considered robot motion, i.e. the configurations at $t_A$ and $t_B$ (respectively $C_A$ and $C_B$). Indeed, when the robot moves from $C_A$ to $C_B$, all the observables are built as functions of $C_A$ and $C_B$. For instance, an observable is the distance between $C_A$ and $C_B$, another one is the difference in orientation between $C_A$ and $C_B$ (see also Martinelli (2002a)). In general we have

$$ {\it Obs}\equiv \mu(C_A, C_B) $$

(11)

where μ is the function defining the considered observable.

Now, if we perfectly know $C_A$ and $C_B$ we are able to compute the value of the observable. This value corresponds to a special realization of the observable occurred during the considered motion. Therefore, it can be used to estimate the mean value of the observable. The error of this estimation will be characterized by a covariance matrix which is the covariance of the observable itself. In other words we have:

$$ \langle {\it Obs}\rangle = \mu(C_A, C_B) + w^{\it Obs} $$

(12)

where

$$ w^{\it Obs}=N(0, {\it Cov}^{\it Obs}) $$

(13)

Our goal is to introduce a filter, the Observable Filter (OF), where the measurement is

$$ z^{\it Obs}\equiv\mu(C_A, C_B) $$

(14)

Therefore, from (12) we have

$$ z^{\it Obs}= \langle {\it Obs}\rangle-w^{\it Obs} $$

(15)

Unfortunately, $C_A$ and $C_B$ are not perfectly known in a real world scenario. However, the AKF provides at each time step an estimation of the robot configuration. Therefore, the exteroceptive sensor adopted by the OF will be the AKF. Obviously, we must take into account the accuracy on the configuration estimated by the AKF. Hence, instead of (15) we have

$$ z^{\it Obs}= \langle {\it Obs}\rangle -w^{\it Obs}+w^X $$

(16)

where $w^{X}$ is a zero-mean random variable (independent from $w^{\it Obs}$) whose covariance matrix depends on the accuracy of the AKF in estimating the robot pose at $t_A$ and $t_B$, i.e.

$$ \displaylines{ {\it Cov}_{w^{X}}= [\nabla_{C_A}\mu ] [P(t_A \,|\, t_A)] [\nabla_{C_A}\mu]^T\cr +\, [\nabla_{C_B}\mu] [P(t_B\,|\,t_B)] [\nabla_{C_B} \mu]^T} $$

(17)

where P is the upper left block $3\times 3$ of the matrix $P_a$ defined in Section 3 .

If we combine Eqs. (10) and (16) we obtain an observation on the state $[K,K_s]$:

$$z^{\it Obs}=m^{\it Obs}(K, K_s) - w^{\it Obs} + w^{X}$$

(18)

On the other hand, the vector $K_s$ is estimated through the AKF. Let us indicate the estimated value of the systematic parameters with $K_{s0}$. We have:

$$ z^{\it Obs}=m^{\it Obs}(K, K_{s0}) - w^{\it Obs} + w^{X}+w^{\it syst} $$

(19)

where $w^{\it syst}\equiv [\nabla_{K_s} m^{\it Obs}]\delta K_s$ and $\delta K_s$ is the error on the systematic parameters whose statistics is provided by the AKF.

The Eq. (19) can be interpreted as an observation on the state K. Since the environment is assumed to be homogeneous and stationary the dynamical equation for the state K is the identity:

$$ K_{t_B}=f_K(K_{t_A})=K_{t_A} $$

(20)

The measurements $z^{\it Obs}$ are provided at the frequency of the AKF. Therefore, the OF can run with a frequency which is not larger than the one of the AKF. We will indicate with i and $i+j$ (where j is an integer $\geq 1$) two subsequent time steps in the OF.In practice, we run the OF once every j cycles of the AKF.

The knowledge of the function $m^{\it Obs}$ allows us to make a prediction for the measurement $z^{\it Obs}$. Therefore, the innovation is obtained from the difference between this prediction and the value obtained through (14) where the robot configuration at $t_A=i$ and $t_B=i+j$ are estimated through the AKF.

In order to introduce the adopted observable we define the following quantities. Let us indicate with $\Delta X_o, \Delta Y_o$ and $\Delta \theta_o$ the displacements respectively in the x-axis, y-axis and orientation between the $(i+j){\rm th}$ and $i{\rm th}$ time step as evaluated by the odometry corrected for the systematic errors by using the systematic parameters estimated by the AKF at the $(i+j){\rm th}$ time step. Furthermore, we denote with $\Delta X_a, \Delta Y_a$ and $\Delta \theta_a$ the actual displacements. The observable we adopt is:

$$ {\it Obs}=\left[\begin{array}{c}(\Delta X_a-\Delta X_o)^2 + (\Delta Y_a-\Delta Y_o)^2\\ (\Delta \theta_a-\Delta \theta_o)^2 \\ \end{array}\right] $$

(21)

As explained before (see Eqs. (14) and (16)), the correspondent measurement $z^{\it Obs}$ is obtained by estimating the actual robot displacements through the AKF.

The mean value of the second component of the observable defined in (21) can be computed without approximation for any trajectory followed by the robot between the $i$th and $(i+j)$th time step (Martinelli, 2002a). Concerning the first component the same property holds only for the synchronous drive. However, even in this case we show the result obtained by approximating the trajectory by an arc of circumference for the sake of simplicity (Martinelli, 2002b). In the next section we compute the mean value of this observable for the synchronous drive. Concerning the differential drive, we adopt a simpler observable consisting only of the second component of the previous observable, ${\it Obs}=(\Delta \theta_a-\Delta \theta_o)^2$.

In the Sections 4.1 and 4.2 we provide the analytical expressions for the statistics of the chosen observable, respectively for the synchronous and the differential drive systems. In the Section 4.3 we provide the procedure to compute the matrices characterizing the OF. In the Section 4.4 we discuss the optimal frequency of the OF and we show that it depends on the error on the estimated augmented state.

4.1 Synchronous drive

It is possible to define the robot trajectory by giving the orientation as a function of the curve length. In the synchronous drive, both the orientation and the curve length are directly estimated by the odometry. We obtain for the increments in the orientation and translation between the $i$th and $(i+j)$th time step respectively $\Delta\theta^e=\sum_{k=i}^{i+j}\delta\theta^e_k$ and $\Delta\rho^e=\sum_{k=i}^{i+j}\delta\rho^e_k$. Moreover, we obtain for the mean value of the observable in (21), (see Martinelli (2002b))

$$ \langle {\it Obs}\rangle = m^{\it Obs}(K, K_{s0}) =\left[\begin{array} {c}K_{\rho}\Delta\rho^e+ 2(\delta_{\rho}\Delta\rho^e)^2\left(\Re \left\{F(z)\right\}-\frac{1-{\rm cos}(\Delta\theta^e)}{(\Delta\theta^e)^2}\right)\\ K_{\theta}\Delta\rho^e\\\end{array}\right] $$

(22)

where ${\it F}(z)=\frac{z-1+e^{-z}}{z^2}$ and $z=\frac{K_{\theta}\Delta\rho^e}{2} +i(E_{\theta}+\frac{\Delta\theta^e}{\Delta\rho^e})\Delta\rho^e$. We do not report here the computation of the covariance matrix. It can be carried out following similar computation as described in Martinelli (2002a).

4.2 Differential drive

From the Eqs. (4–7) it is easy to obtain the mean value and the variance of the observable ${\it Obs}_{}=(\Delta \theta_{a}-\Delta\theta^e_{})^2$:

$$ \displaylines{ \langle {\it Obs}\rangle = m^{\it Obs}(K, K_{s0})=\frac{K_{w_{}}(\Delta\rho^{eR}_{}+\Delta\rho^{eL}_{})} {\left(d\delta_{d_{}} \right)^2}\cr {\it Cov}_{\it Obs}=\frac{2K_{w_{}}^2}{d^4 \delta_{d_{}}^4}(\Delta\rho^{eR}_{}+\Delta\rho^{eL}_{})^2} $$

(23)

where $\Delta\rho^{eR}=\sum_{k=i}^{i+j} | \delta\rho^{eR}_k |$ and $\Delta\rho^{eL}=\sum_{k=i}^{i+j} |\delta\rho^{eL}_k |$.

4.3 OF implementation

The equations of the filter are the equations of the EKF. Clearly, the matrix $F=\nabla_K f_K$ is the identity and the matrix G is the zero-matrix since the dynamical Eq. (20) is not affected by any error. Furthermore, we need to compute the matrix H, i.e. the Jacobian of the observational equation with respect to the state estimated by the filter itself. We get this matrix, respectively for the synchronous and differential drive, by computing the Jacobian of the function in Eq. (22) and in Eq. (23) with respect to the state K. Finally, the matrix R (i.e. the covariance matrix of $w^{\it Obs} + w^{X} + w^{\it syst}$ in (19)) is the sum of the covariance matrix of the observable (${\it Cov}_{\it Obs}$) plus the covariance matrix of $w^{X}$ in (17) plus the covariance matrix of $w^{syst}$ (i.e. $[\nabla_{K_s} m^{\it Obs}] P_s(i+j|i+j) [\nabla_{K_s} m^{\it Obs}]^T$), where $P_s$ is the block in $P_a$ regarding the systematic parameters.

Note that in most cases the function $m^{\it Obs}$ is linear in K (second component in the synchronous drive and in the differential drive).

4.4 The optimal frequency for the OF

In this section we consider only the case of the differential drive since it is the drive system of the robot used in the experiments.

The goal is to choose the frequency of the OF (i.e. the value of j) in order to minimize the effect of the three noise terms ($w^{\it Obs}, w^{X}$ and $w^{syst}$) in the observation. In other words, we want to minimize the ratio $\frac{w^{\it Obs} + w^{X} + w^{\it syst}}{m^{\it Obs}}$.

Fig. 1

The three terms $m^{\it Obs}, w^{X}$ and $w^{syst}$ vs the total distance traveled by the two wheels between two subsequent OF updates. In the case shown in (b) the noise terms are larger than the term containing the non-systematic parameter for any frequency

From (23) we can see that $m^{\it Obs}$ increases linearly with the traveled distance. On the other hand, from (24) we see that also the standard deviation of $w^{\it Obs}$ increases linearly with the traveled distance. Therefore, the ratio $\frac{w^{\it Obs}}{m^{\it Obs}}$ is independent of the distance traveled between two observations, i.e. it is independent of the frequency of the OF. Regarding the other two components in the error ($w^{X}$ and $w^{syst}$) we note that the standard deviation of the former is independent of the traveled distance, while the latter increases squarely. Let us consider the three terms $m^{\it Obs}, w^{X}$ and $w^{syst}$. The best frequency for the OF is fixed by requiring that the first one is the largest. Clearly, as shown in Fig. 1(b) this requirement could not be satisfied (e.g. when the value of $K_w$ is very small). Let us indicate with S the total distance traveled by the two wheels ($S=\Delta\rho^{eR}+\Delta\rho^{eL}$). The value of $S_0$ shown in the figure corresponds to the S where the standard deviation of $w^{X}$ is equal to the standard deviation of $w^{syst}$.

In the next section we show the experimental results obtained by choosing the value of $S_0$ in order to fix the frequency of the OF.

5 Results

In this section we present the results obtained through simulations (Section 5.1) and real experiments (Section 5.2).

5.1 Simulations

We simulate a mobile robot moving in an environment consisting of a square with side measure $10\,{\rm m}$. Therefore, the map consists of four straight lines and it is a priori known. The exteroceptive sensor is simulated through a function which provides the distance of the map lines from the actual robot configuration. In particular, at each time step, 36 distances are provided yielding a 10 °angular resolution. An error source is introduced by adding at each distance a gaussian random variable, zero-mean, and whose variance is equal to (3 cm²). The random variables corresponding to different distances are independent. The errors in the encoder readings are obtained by introducing gaussian random variables accordingly to the models described in the Sections 3and 2.2. The AKF introduced in Section 2.2 is used to estimate the robot configuration $(x, y, \theta)$ and the systematic parameters ($\delta_{\rho}$ and $E_{\theta}$ for the synchronous drive and $\delta_R, \delta_L$ and $\delta_d$ for the differential drive). The systematic parameters are initialized in order to have a null systematic error ($\delta_{\rho}=1, E_{\theta}=0$ and $\delta_R=\delta_L=\delta_d=1$). The non-systematic parameters are initialized in the OF at a value which differs from the one defined for the truth by a factor 100 (we both considered the cases of smaller and larger initial value obtaining similar results). As explained in Section 3 , the AKF adopts fixed values for the non-systematic parameters in order to compute the error covariance matrices. These values are set 1000 times larger than the truth. Table 1 shows the values of the adopted actual parameters.

Table 1 The systematic and non-systematic model parameters for the synchronous and differential drive as defined for the simulation

Fig. 2

Simulation results for the synchronous drive. The units adopted to represent the model parameters are rad for angle and m for length

We simulated the same robot motion (a circumference with radius equal to 5 m) 100 times. The length of each robot motion is about 30 m. The error on the estimated robot configuration at each time step is about $1\,{\rm cm}$ for the position and $1\,{\rm deg}$ for the orientation (and this is consistent with the experimental results obtained in our laboratory Arras et al. (2001)).

Figures 2 and 3 show the results related to the synchronous drive. Figure 2(a)–(d) display the estimated parameter mean values at each time step i (e.g. $\delta_{\rho_i}$ in Fig. 2(a)). These mean values are plotted vs the traveled distance (in m). The values are obtained from the 100 simulated robot motion (for instance, concerning the former, $\delta_{\rho_i}=\frac{1}{100}\sum_{sim=1}^{100}\delta_{\rho_{i, sim}}$). Figure 3(a)–(d) display the accuracy on the previous parameter estimations (in %) (for instance $\frac{\Delta \delta_{\rho_i}}{\delta_{\rho}}\times 100\%$, where $\Delta \delta_{\rho_i}=\sqrt{\frac{1}{100}\sum_{sim=1}^{100}(\delta_{\rho_{i, sim}}-\delta_{\rho})^2}$). Concerning the non-systematic parameters the frequency of the OF is the same as for the AKF (i.e. $j=1$).

Figures 4 and 5 show the results related to the differential drive. We plot the same quantities as in the previous case.

Fig. 3

Simulation results for the synchronous drive. The accuracy in % is plotted vs the traveled distance. The units adopted to represent the model parameters are rad for angle and m for length

We can conclude that it is possible to reach good accuracy on the parameter estimation by moving the mobile robot along quite short distances.

5.2 Real experiments

For the experiments, a fully autonomous mobile vehicle has been used. Donald Duck (Fig. 6) is a mobile robot with a differential drive. It is equipped with wheel encoders, a 360${^{\circ}}$ laser range finder and a grey-level CCD camera (not used here). It is connected via radio ethernet only for data visualization via web and data logging for statistical purposes.

We performed two set of experiments. In each experiment the robot moved along a distance of about 300 m in our laboratory. The two set of experiments differed because in one case we added on both the robot wheels a small piece of tape in order to increase slightly the wheel diameters and to test the accuracy of the implemented AKF. In all the cases the OF started to work only when the systematic parameter errors, as estimated by the covariance matrix of the AKF ($P_a$), were smaller than $5\times10^{-4}$. This accuracy was always achieved after about 100 m (see Figs. 7–9).

Fig. 4

Simulation results for the differential drive. The units adopted to represent the model parameters are rad for angle and m for length

Fig. 5

Simulation results for the differential drive. The accuracy in % is plotted vs the traveled distance. The units adopted to represent the model parameters are rad for angle and m for length

Fig. 6

The autonomous robot Donald Duck. Its controller consists of a VME standard backplane with a Motorola PowerPC 604 microprocessor clocked at 300 Mhz. Among its peripheral devices, the most important are the wheel encoders, a 360${^{\circ}}$ laser range finder and a grey-level CCD camera (not used here)

Concerning the AKF, we set the initial covariance matrix $P_a$ as diagonal. Moreover, the variances corresponding to the systematic parameters were set equal to $(0.05)^2$ for all the three parameters. Finally, the initial values were fixed equal to 1.0 for all of them.

Fig. 7

The $\delta_R$ parameter estimated by the AKF vs the distance traveled by the robot (unity m). The dotted line refers to the case with the tape on the wheels

Fig. 8

The $\delta_L$ parameter estimated by the AKF vs the distance traveled by the robot (unity m). The dotted line refers to the case with the tape on the wheels

Fig. 9

The $\delta_d$ parameter estimated by the AKF vs the distance traveled by the robot (unity m). The dotted line refers to the case with the tape on the wheels

Fig. 10

The non-systematic parameter $K_w$ as estimated by the OF vs the distance traveled by the robot (unity m in both axis). The dotted line refers to the case with the tape on the wheels

Regarding the non-systematic parameter $K_w$, we set in the most of the experiments, the initial value equal to 0.01 m. This value is very large. Indeed, it corresponds to have a non-systematic error whose standard deviation after 100 m of navigation, is equal to 1 m for each wheel. Therefore, the AKF used nearly only the laser range finder to localize the robot.

Figures 7–9 show the systematic parameter results. Dotted line is adopted for the case with the tape on the wheels. As expected, the values of $\delta_R$ and $\delta_L$ increase with respect to the case without tape. The variation is equal to about 0.01 corresponding to a diameter change of 0.4 mm, since the wheel diameter is equal to 3.8 cm. Figure 9 shows also a change in the wheels base distance due to the tape. This change shows that the point where the wheel touches the terrain seems to be pushed out by the tape.

Figure 10 concerns the non-systematic parameter results. Again, dotted line is adopted for the case with the tape on the wheels. In this case the tape does not produce variation. The frequency of the OF was chosen accordingly to the considerations given in Section 4.4. We obtain the value $S_0\simeq 15$ m. Observe that the frequency of the AKF is much higher (${\simeq}$10 cm). Similar results for the estimated $K_w$ were obtained by changing the value of $S_0$. In particular, we performed many experiments in the range $10\, {\rm m} \leq S_0\leq 30 \,{\rm m}$ obtaining a variation in $K_w$ within the 20%. We also performed other trials by changing the initial value of $K_w$ (always in the range $0.0001\, {\rm m} \leq K_w \leq 0.1 \,{\rm m}$) obtaining again a slight variation in the results (within the 20%). The final estimated $K_w$ shown in the Fig. 10 are $K_w=(4.7\pm1.6)10^{-5}$ m and $K_w=(5.4\pm1.8)10^{-5}$ m respectively with and without tape. This value of $K_w$ corresponds to have a non-systematic error whose standard deviation after 100 m of navigation, is ${\simeq}$5 cm for each wheel.

By comparing the results obtained through the experiments and the ones obtained in the simulations we can find a good consistency, confirming that both the adopted model to characterize the odometry error of a differential drive and the approach introduced here are right. In particular, concerning the systematic parameters, we observe from Figs. 7–9 a rapid variation of the estimated value during the first 30 m. After this distance, the overall change never exceeds the 2%. This is perfectly in agreement with the results obtained with our simulation (see Fig. 5). Concerning the non-systematic parameter, the experiment shows that it is required to move the robot along a distance of about 100 m. After this distance, the overall change never exceeds the 10%.

6 Conclusions

Two filters — the Augmented Kalman Filter and the Observable Filter — are introduced to estimate the systematic and the non-systematic odometry errors.

The main contributions are the theoretical discussion of these filters both for synchronous and differential drive, and the integration of them in a coherent framework allowing to auto-calibrate the odometry system while localizing during the robot navigation.

Simulations and real experiments show that the approach performs well both with respect to the precision and the convergence. In particular, simulations show that it is possible to estimate the systematic error with low relative error (1% by moving the robot for 30 m) and the non-systematic error with a relative error of 90%. Observe that our experiments were carried out in an indoor environment with a very smooth floor and therefore the non-systematic component is very low and very difficult to be evaluated.

We are performing some experiments showing the usefulness of an odometry auto calibration in the framework of the SLAM problem. We want to remark that in the localization problem with a precise map a priori known, and when a precise exteroceptive sensor is available, the localization error is very small compared to the odometry error (calibrated or not), since the localization task is nearly completely relied to the external sensor. In the SLAM problem, however, the odometry could play a very important role especially in solving the data association problem.