Robust Dead Reckoning System for Mobile Robots Based on Particle Filter and Raw Range Scan

Abstract

Robust dead reckoning is a complicated problem for wheeled mobile robots (WMRs), where the robots are faulty, such as the sticking of sensors or the slippage of wheels, for the discrete fault models and the continuous states have to be estimated simultaneously to reach a reliable fault diagnosis and accurate dead reckoning. Particle filters are one of the most promising approaches to handle hybrid system estimation problems, and they have also been widely used in many WMRs applications, such as pose tracking, SLAM, video tracking, fault identification, etc. In this paper, the readings of a laser range finder, which may be also interfered with by noises, are used to reach accurate dead reckoning. The main contribution is that a systematic method to implement fault diagnosis and dead reckoning in a particle filter framework concurrently is proposed. Firstly, the perception model of a laser range finder is given, where the raw scan may be faulty. Secondly, the kinematics of the normal model and different fault models for WMRs are given. Thirdly, the particle filter for fault diagnosis and dead reckoning is discussed. At last, experiments and analyses are reported to show the accuracy and efficiency of the presented method.

1. Introduction

Robust dead reckoning is a critical and challengeable issue for autonomous mobile robots in the presence of faults, such as the sticking of sensors, the slippage of wheels and the noisy readings of internal and/or external sensors. Sensor faults may change the kinematics and measurement models of wheeled mobile robots (WMRs). For example, the sticking of odometry sensors requires the system to switch the kinematics of WMRs. On the other hand, the faults of external sensors (such as a laser range finder, CCD camera, etc.) require a reliable perception model by fusing multiple external sensors.

When the WMRs are governed by faults, the accuracy of the dead reckoning system may decrease significantly. The difficulties of robust dead reckoning with faulty sensors include: (1) faults have to be identified accurately and quickly, for the kinematics and perception models are determined by the discrete fault models; (2) correct and accurate kinematics and perception models have to be built for different fault models; (3) in some cases, the dead reckoning can hardly be achieved when WMRs are seriously damaged.

Concerning the robust dead reckoning problem in the presence of various kinds of sensor faults, one of the intuitive methods includes a two-step process: firstly, fault models are identified with general fault diagnosis methods; secondly, robust dead reckoning is obtained according to different fault models. However, this methodology is generally time consuming and inaccurate.

In this paper, a particle filter-based method is proposed to diagnose faults and compute dead reckoning simultaneously. Particle filters have been widely used in fault diagnosis, pose tracking and simultaneously localization and mapping (SLAM) applications for WMRs. In this methodology, the discussed questions are modeled as a hybrid system estimation problem, where the discrete states of faults and the continuous states of dead reckoning are estimated simultaneously. An external sensor, namely a laser range finder, is used to correct the errors of internal sensors caused by faults or large noises. To achieve this purpose, a robust and fast raw scan projection method using polar coordinates is employed to evaluate the weights of particles. The main contribution of this paper is that it proposes a general method to handle the fault diagnosis of internal sensors and the accurate dead reckoning in the situation of sensor faults with a particle filter based on raw scan matching.

The remainder of the paper is organized as follows. In Section 2, we briefly reviewed the previous works on the relative topics of the robust dead reckoning methods of mobile robots, including fault diagnosis, robust perception, dead reckoning, etc. In Section 3, the general framework of the particle filter for the hybrid state estimation problem is put forward. In Section 4, the robust perception model for laser range finder is presented. In Section 5, fault diagnosis and robust dead reckoning based on the particle filter are discussed. The experimental results and analyses are given in Section 6.

2. Previous Works

2.1. Fault Diagnosis for Mobile Robots

Fault detection and diagnosis (FDD) is increasingly important for wheeled mobile robots (WMRs), especially those under unknown environments, such as planetary exploration [1]. Multiple model methods are widely used in sensor fault diagnosis for mobile robots [2–4]. In multiple models methods, the dynamics of each fault model are represented with Kalman filters (KFs). As the number of possible fault models increases exponentially over the number of components, the number of KFs increases respectively. Additionally, the precondition of KFs is that the process noise and the measurement noise are a zero mean Gaussian process with known covariance.

To deal with these problems, many researchers employ particle filters (PFs) to study fault diagnosis problems after the pioneering work of de Freitas [5]. However, general particle filters have the following drawbacks: degeneracy in sequential importance sampling (SIS), loss of diversity in sample importance resampling (SIR) [6] and the curse of dimensionality, i.e., the rate of convergence of the approximation error decreases as the state dimension increases [7].

Many improvements in particle filters focus on tackling the problems mentioned above. For example, the Rao–Blackwellised particle filter (RBPF) decreases the sample number by only sampling the discrete states; the trade-off is that it must exploit linear-Gaussian models of each discrete state [5]; and the variable resolution particle filter (VRPF) tracks abstract states that may represent single state or sets of states [8]. Lookahead-RBPF takes account of the current readings of external sensors in the sampling step to improve the efficiency and accuracy [9].

Fox presented a statistical approach to increasing the efficiency of particle filters by adapting the size of sample sets, which bound the approximation error introduced by the sample-based representation of the particle filter, and the approximation error is measured by the Kullback–Leibler divergence (KLD-sampling) [10]. Duan presented an adaptive particle filter for soft fault compensation, which adapts the parameter of the noise variance of the process and the number of samples simultaneously [11].

Hashimoto et al. presented a method to diagnose the faults of internal and external faults of mobile robots. The gains of internal sensors (two encoders and one gyroscope) are estimated via the scan matching method of the laser range finder, which may be also influenced by abrupt faults. The laser range finder is supposed to be faulty if the laser images in several successive scans are not matched [12]. Another work conducted by Hashimoto et al. is a voting-based fault isolation approach, in which the velocity estimates with the four sensors (three LRSs and one dead reckoning) are compared with each other, and the sensor whose velocity estimates do not match the others is decided to be the faulty one [13].

Gage et al. put forward a survey on sensing assessment in unknown environments, especially for mobile robot applications [14]. They showed that only a few studies focused on the detection (and identification) of real exteroceptive sensor faults.

Recently, various kinds of data-driven methods have been employed to handle fault diagnosis for mobile robots when the system model can hardly be obtained. For example, Christensen et al. employed back-propagation neural network to synthesize fault detection components with a fault injection scheme [15]; Raphael et al. proposed an online data-driven fault detection for robot systems, which learns the posterior density based on online data collected with sensors [16]; Lau et al. put forward an adaptive data-driven error detection in swarm robotics with statistical classifiers, namely the receptor density algorithm (RDA), which is based on immune computation [17]; Lin et al. used a support vector machine (SVM) to handle fault diagnosis problem for mobile robots [18]. Most recently, model-based methods and data-driven methods have been integrated to handle fault diagnosis problem for UAVs, in which the model-based methods serves as a residual generator, and the data driven-methods are employed to detect an anomaly [19].

2.2. Robust Dead Reckoning for Mobile Robots

A dead reckoning system computes the relative transform between two consecutive frames of the robot, $A=\left[\begin{array}{cc}H& P\\ 0& 1\end{array}\right]$, where $H=\left[\begin{array}{cc}\mathit{\text{cos}}\mathit{\theta }& -\mathit{\text{sin}}\mathit{\theta }\\ \mathit{\text{sin}}\mathit{\theta }& \mathit{\text{cos}}\mathit{\theta }\end{array}\right]$ is a rotation matrix, and P =(x, y)^T , is a translation vector.

The translation vector and rotation matrix are typically given by the readings of internal sensors, such as the gyroscope and encoders. However, these readings consist of both systematic and non-systematic errors. The former depend on the structure of the sensors and the mobile platform adopted; the latter are due to undesired interactions between the robot and the environment, such as slippage and sticking of wheels caused by uneven ground. Systematic errors, both deterministic and probabilistic, can be predicted. For example, the finite resolution of encoders causes a normally-distributed error [20]. Borenstein et al. presented a method for measuring and reducing systematic odometry errors of differential drive mobile robots, which is caused by uncertain wheelbase and unequal wheel diameters, based on carefully designed error models [20]. Non-systematic errors, which play a significant role in robust dead reckoning system, cannot be predicted. Meng et al. presented a trigonometry-based model to increase the accuracy of odometry-based pose estimation of a mobile robot with two steerable drive wheels. The underlying idea uses the ratio of two drive wheels' incremental displacements to detect non-systematic errors mainly caused by slippage [21]. It is suggested to take advantage of particular external sensors, such as the laser range finder and camera, to build an accurate environment map [20,22].

Ward et al. proposed a dynamic model-based wheel slip detector, which estimates longitudinal wheel slip and detects immobilized conditions of autonomous mobile robots operating on outdoor terrain [23]. Firstly, a tire traction/braking model is exploited to calculate vehicle dynamic forces with an extended Kalman filter framework. Internal sensors and GPS are then fused to estimate external forces and robot velocity. In this work, the GPS is used to estimate the velocity of the robot. However, the GPS is usually useless in indoor environments.

Chung et al. presented a method to improve dead reckoning accuracy with fiber optic gyroscopes (FOGs) in mobile robots [24]. The key idea is to build an accurate model for FOGs to handle the non-linearity of the scale factor and the temperature dependency of the gyroscope and fuse the sensor data from the FOG with the odometry system by an indirect Kalman filter. The accuracy of FOG is usually higher than that of encoders; however, the measurements of FOG itself will drift and cannot recover without external sensors.

Sekimori et al. proposed a dead reckoning method based on increments of the robot movements read directly from the floor using optical mouse sensors, in which the measurement values from multiple optical mouse sensors are compared to each other and only the reliable values are selected; accurate dead reckoning can be realized compared with the conventional method based on increments of wheel rotations [25]. This is a kind of hardware redundancy method, which is usually expensive and cannot be deployed in small robots, due to the limited volumes.

Cobos et al. put forward a method that combines a monocular vision system-based visual odometer and onboard odometer systems to reduce the errors of dead reckoning, which uses optical flow techniques and planar models to obtain qualitative 3D information and robot localization by using time integration series of acquired frames [26]. However, the accuracy of the monocular vision-based method is low, and the computational burden is high.

Up to now, almost all of the research concerning robust dead reckoning has focused on error estimation. Robust dead reckoning under the conditions of internal or external sensors being faulty is still an open problem. The difficulty of robust and accurate dead reckoning includes: (1) the sensor readings are noisy; and (2) an accurate model can hardly be obtained, especially under complicated environments and when components are faulty. External sensor readings can significantly improve the accuracy of dead reckoning; however, two key issues arise, i.e., the perception model of the external sensor must be accurate, and the computational burden must be decreased to reach real-time estimation.

2.3. Raw Scan Matching

As mentioned above, the errors cannot be corrected inside the internal sensors of the dead reckoning system. A typical way to estimate accurate dead reckoning is to employ external sensors, such as a camera or a laser range finder. A laser range finder is more accurate and efficient than a camera, especially in indoor environments. In this section, we briefly review the scan matching methods for a laser range finder found in the current literature.

Scan matching approaches typically include two categories, namely feature-based matching [27,28] and point-to-point matching [29–33].

In feature-based matching, features, such as line segments, corners or range extrema, are extracted from laser scans and then matched. The difficulties for the feature-based method include: (1) features are difficult to extract in some situations, such as outdoor environments; (2) the corresponding feature matching is time consuming.

Typical point-to-point matching methods include iterative closet point (ICP) [32], iterative matching range point (IMRP) [33], iterative dual correspondence (IDC) [33], corresponding vector fitting sample and consensus (CVFSAC) [29], polar scan matching (PSM) [31] and inverse ray tracing (IVT) [34].

The point-to-point matching algorithms apply a so-called projection filter before matching. The computational complexity for the projection filter in the Euclidean coordinate system is typically O(n²), where n denotes the number of points. This is the case for the algorithms of ICP, IMRP, IDC and CVFSAC.

Another drawback of ICP-based methods is that they need an accurate initial guess. Minguez et al. defined a metric distance for ICP in the configuration space of the sensor, which takes into account both the translation and rotation error of the sensor [30]. However, how to choose an the parameter combining translation and orientation difference in the proposed metric is still a difficult issue [31]. Martinez et al. presented an GA-ICP scan matching method, which includes two steps. The genetic algorithm is used to search the optimal match roughly; after that, the ICP algorithm is employed to optimize the best guess of GA [35].

PSM works in the laser scanner's polar coordinate system, which takes advantage of the structure of the laser measurements and eliminates the need for an expensive search for corresponding points in other scan match approaches [31]. Similarly, Duan et al. presented a robust and fast measure for the estimation of assignment of two consecutive frames, which proposed a fast inverse ray tracing method [34]. Both PSM and IVT compute the projection in the polar coordinate system and reduce the computational complexity to O(n).

3. Hybrid System Estimation Based on Particle Filter

A particle filter is a Monte Carlo (i.e., choosing randomly) method to monitor dynamic systems, which non-parametrically approximates a probabilistic distribution using weighted samples (particles). A particle filter gives a computationally feasible method for the state estimation of nonlinear and non-Gaussian hybrid systems. The term ‘hybrid’ denotes that the system states contain discrete and continuous parts. Fault diagnosis for a dynamic system is a typical kind of hybrid system, in which discrete states are fault modes and continuous states are determined according to the system to be estimated.

The main idea for using a particle filter as a hybrid system estimation method is described as follows. Let S represent the finite set of discrete models in the system; s_t represent the discrete model of the system to be estimated at time t and s_t ∈ S, {s_t} represent discrete, first order Markov chain representing the evolution of the state over time; x_t stand for the multivariate continuous state of the system at time t. In Bayesian theory, the problem of hybrid system estimation consists of providing a belief (a distribution over the state set S) at each time step as it evolves according to the following transition model:

$$p(s_t=j|s_{t-1}=i),i,j,\in \mathbf{\text{S}}$$

(1)

Each of the discrete models changes the dynamics of the system. The non-linear conditional state transition models are denoted by p(x_t|x_t−1,s_t). The state of the system is observed through a sequence of measurements, {z_t}, based on the perception model p(z_t|x_t,s_t). The problem of hybrid state estimation consists of two consecutive steps. The first step is estimating the marginal distribution p(s_t|z_1..t) of the posterior distribution p(x_t,s_t|z_1…t). The second step is to estimate the continuous state distribution p(x_t|s_t, z₁…_t) according the discrete models obtained previously.

A recursive estimate of this posterior distribution may be obtained using the Bayes filter:

$$p({\mathbf{\text{x}}}_t,s_t|{\mathbf{\text{z}}}_{1\dots t})={\eta }_{t}p({\mathbf{\text{z}}}_t|{\mathbf{\text{x}}}_t,s_t)\int \sum _{s_{t-1}}p({\mathbf{\text{x}}}_t,s_t|{\mathbf{\text{x}}}_{t-1},s_{t-1})d{\mathbf{\text{x}}}_{t-1}$$

(2)

There is no closed form solution for this recursion. Particle filters appropriate the posterior with a set of N fully instantiated state samples or particles $\{(s_t^{[1]},{\mathbf{\text{x}}}_t^{[1]}),\cdots ,(s_t^{[N]},{\mathbf{\text{x}}}_t^{[N]})\}$ and importance weights $\left\{w_t^{[i]}\right\}$:

$${\widehat{p}}_N({\mathbf{\text{x}}}_t,s_t|{\mathbf{\text{z}}}_{1\cdots t})=\sum _{i=1}^N{w}_t^{[i]}{\delta }_{({\mathbf{\text{x}}}_t^{[i]},s_t^{[i]})}({\mathbf{\text{x}}}_t,s_t)$$

(3)

where δ(.) denotes the Dirac delta function. The appropriation in Equation (3) approaches the true posterior density as N → ∞. Because it is difficult to draw samples from the true posterior, samples are drawn from a more tractable distribution q(.), which is called the proposal (or importance) distribution. The most widely used proposal distribution is the transition distribution (4):

$$q(.)=p({\mathbf{\text{x}}}_t,s_t|{\mathbf{\text{x}}}_{t-1},s_{t-1})$$

(4)

The importance weights are used to account for the discrepancy between the proposal distribution q(.) and the true distribution p(x_t,s_t|z_1…t). When the proposal distribution is given by Equation (4), the importance weight of sample $(s_t^{[i]},{\mathbf{\text{x}}}_t^{[i]})$ is:

$$w_t^{[i]}=p({\mathbf{\text{z}}}_t|{\mathbf{\text{x}}}_t^{[i]},s_t^{[i]})$$

(5)

The general particle filter algorithm for hybrid system estimation is expressed in Algorithm 1.

Algorithm 1: General particle filter for hybrid system estimation.

Initialize: For N particles ${\{{\mathbf{\text{x}}}_t^{[i]},s_t^{[i]}\}}_{i=1}^N$, sample discrete state ${\left\{s_0^{[i]}\right\}}_{i=1}^N$, from the prior p(s0), sample ${\left\{{\mathbf{\text{x}}}_t^{[i]}\right\}}_{i=1}^N$ from $p({\mathbf{\text{x}}}_0|s_0^i)$.;

for each time step t do

for each particle i do

Sample discrete state: $s_t^{[i]}~p(s_t|s_{t-1}^{[i]})$;

Sample continuous state: ${\mathbf{\text{x}}}_t^{[i]}~p({\mathbf{\text{x}}}_t|{\mathbf{\text{x}}}_{t-1}^{[i]},s_t^{[i]})$;

Update $w_t^{[i]}=p(z_t|{\mathbf{\text{x}}}_t^{[i]},s_t^{[i]})$;

end

Discrete state estimation: ŝt = argmaxstp̂N(st|z1..t);

Continuous state estimation: ${\widehat{\mathbf{\text{x}}}}_t={\sum }_{i=1,s_t^{[i]}={\widehat{s}}_t}^N{w}_t^{[i]}{\mathbf{\text{x}}}_t^{[i]}$;

Normalize the weights: $w_t^{[i]}=\frac{w_t^{[i]}}{{\sum }_N^{i=1}{w}_t^{[i]}}$;

Resample: generating ${\left\{{\mathbf{\text{x}}}_t^{[i\ast ]},s_t^{[i\ast ]}\right\}}_{i=1}^N$, such that $p(({\mathbf{\text{x}}}_t^{[i\ast ]},s_t^{[i\ast ]})=({\mathbf{\text{x}}}_t^{[j]},s_t^j))=w_t^{[j]}$, ${\left\{{\mathbf{\text{x}}}_t^{[i]},s_t^{[i]}\right\}}_{i=1}^N={\left\{{\mathbf{\text{x}}}_t^{[i\ast ]},s_t^{[i\ast ]}\right\}}_{i=1}^N,w_t^{[i]}=1/N$;

end

4. Perception Model of Laser Range Finder Based on Scan Projection in Polar Coordinates

In this section, we construct a perception model for a laser range finder based on the scan projection method in polar coordinates. This kind of scan projection was firstly proposed in [31]. We first filter out noisy rays with a kind of segmentation technique. Then, we employ the scan projection method of PSM to quickly compute corresponding rays of two consecutive scans. The distances of corresponding points, after transforming to the same frame, are influenced by two factors: the accuracy of relative transformation and the changes of the environment.

4.1. Segment Analysis and Effective Scan Window

Let R_t denote range data at time t,

$$R_t=\{b_t^j=({\mathit{\rho }}_t^j,{\mathit{\alpha }}_t^j)\},j\in [\mathrm{1..}L]$$

(6)

where $b_t^j$ is the j − th ray of the scan R_t, L is the number of rays of the scan, ${\mathit{\rho }}_t^j$ is the range readings of the j − th ray and ${\mathit{\alpha }}_t^j$ is the orientation of the j − th ray, which is calculated as follows,

$${\mathit{\alpha }}_t^j=\mathit{\varrho }+(j-1)\varsigma$$

(7)

where ϱ is the angular offset of the first ray and ς is the angular resolution.

For example, in typical scan readings of LMS291, L = 361, ϱ =0 and ς =0.5deg. We will show later that the angular structure of scan may play an important role in the proposed perception model.

The process of segment analysis of a raw scan is to divide the rays into several segments; each segment of scan is a set of consecutive rays in the scan, such that the range of consecutive rays that does not jump dramatically. Segment analysis of scan R_t is done by dividing it into segments as follows,

$$R_t=\underset{k=1}{\overset{c_t}{\cup }}{G}_t^k$$

(8)

where $G_t^k=\{b_t^i|i\in [{\mathit{\text{start}}}_t^k\mathrm{..}{\mathit{\text{end}}}_t^k]\}$ denotes the k − th segment, c_t denotes the number of segments of the scan, ${\mathit{\text{start}}}_t^1=1,{\mathit{\text{end}}}_t^{c_t}=L,{\mathit{\text{start}}}_t^{k+1}={\mathit{\text{end}}}_t^k+1,|{\mathit{\rho }}_t^{i+1}-{\mathit{\rho }}_t^i|<\mathit{\text{gap}},i+1,i\in [{\mathit{\text{start}}}_t^k\mathrm{..}{\mathit{\text{end}}}_t^k],\parallel {\mathit{\rho }}_t^{{\mathit{\text{start}}}_t^{k+1}}-{\mathit{\rho }}_t^{{\mathit{\text{end}}}_t^k}|\ge \mathit{\text{gap}},\mathit{\text{gap}}$ denotes a threshold.

Let ${\mathit{\text{num}}}_t^k={\mathit{\text{end}}}_t^k-{\mathit{\text{start}}}_t^k+1$ denote the numbers of rays in segment k, and ${\stackrel{\sim }{\mathit{\rho }}}_t^k=\frac{1}{{\mathit{\text{num}}}_t^k}{\sum }_{i={\mathit{\text{start}}}_t^k}^{{\mathit{\text{end}}}_t^k}{\mathit{\rho }}_t^i$ denotes the average distance of segment k. ${\mathit{\text{num}}}_t^k$ and ${\stackrel{\sim }{\mathit{\rho }}}_t^k$ are important features about segments.

Based on the features of the segments of raw scan, noise readings can be filtered out with the following method to construct the effective scan windows for the readings,

$$W_t=\underset{k=1}{\overset{c_t}{\cup }}{G}_t^k,{\mathit{\text{num}}}_t^k>\mathit{\gamma },{\lambda }_2\ge {\stackrel{\sim }{\rho }}_t^k\ge {\mathit{\lambda }}_1$$

(9)

where W_t denotes the effective scan of R_t, and γ, λ₂, λ₁ denote the threshold, respectively. λ₂ depends on the maximal perception distance of the range finder. λ₁ stands for the minimal valid measurements of environments. This is because the range finder is mounted on the robot, and for a mechanical reason, the minimal distance from the range finder to the environment is larger than zero. For the LMS291, we set γ =5, λ₂ = 81.9 m and λ₁ =0.03 m.

4.2. Scan Projection in Polar Coordinates

Let Γ_t denote the coordinate frame of scanner at time t, and the homogeneous transformation matrix of Γ_t with respect to Γ_t−1 is A_t,

$$A_t=\left[\begin{array}{cc}{H}_t& P_t\\ 0& 1\end{array}\right]$$

(10)

where $H_t\left[\begin{array}{cc}\mathit{\text{cos}}{\mathit{\theta }}_t& -\mathit{\text{sin}}{\mathit{\theta }}_t\\ \mathit{\text{sin}}{\mathit{\theta }}_t& \mathit{\text{cos}}{\mathit{\theta }}_t\end{array}\right]$ is a rotation matrix, and P_t = (x_t, y_t)^T, is a translation vector. Both the rotation matrix and translation vector are determined by the continuous state of the hybrid system, namely x_t,

$${\mathbf{\text{x}}}_t={\left(x_t,y_t,{\mathit{\theta }}_t\right)}^T$$

(11)

The purpose of the scan projection is to find out what the current scan would look like if it were taken from the reference position given the pose estimation x_t. The fast scan projection of PSM contains the following steps.

Step 1: Compute the projected ray of each ray $b_t^i$ of the current scan, and let $d_t^i$ and ${\mathit{\gamma }}_t^i$ denote the length and orientation of the projected ray, respectively.

$$d_t^i=\sqrt{{\left({\mathit{\rho }}_t^i\mathit{\text{cos}}({\mathit{\theta }}_t+{\mathit{\alpha }}_t^i)+x_t\right)}^2+{\left({\mathit{\rho }}_t^i\mathit{\text{sin}}({\mathit{\theta }}_t+{\mathit{\alpha }}_t^i)+y_t\right)}^2}$$

(12)

$${\mathit{\gamma }}_t^i=\mathit{\text{atan}}2({\mathit{\rho }}_t^i\mathit{\text{sin}}({\mathit{\theta }}_t+{\mathit{\alpha }}_t^i)+y_t,{\mathit{\rho }}_t^i\mathit{\text{cos}}({\mathit{\theta }}_t+{\mathit{\alpha }}_t^i)+x_t)$$

(13)

Step 2: Calculate the expectation rays that would have been measured between ${\mathit{\gamma }}_t^{i-1}$ and ${\mathit{\gamma }}_t^i$ with interpolation, where $(d_t^{i-1},{\mathit{\gamma }}_t^{i-1})$ and $(d_t^i,{\mathit{\gamma }}_t^i)$ denote two consecutive projected measurements from the same segments.

Let $j_1=\mathit{\text{floor}}(\frac{{\mathit{\gamma }}_t^i-\mathit{\rho }}{\varsigma }),j_0=\mathit{\text{ceil}}(\frac{{\mathit{\gamma }}_t^{i-1}-\mathit{\rho }}{\varsigma })$. For every j(j > = j₀ and j < = j₁), compute the expected distance of the ray j of scan R_t−1 with interpolation as follows,

$$d(j,R_{t-1})=\frac{d_t^i-d_t^{i-1}}{{\mathit{\gamma }}_t^i-{\mathit{\gamma }}_t^{i-1}}({\mathit{\alpha }}_{t-1}^j-{\mathit{\gamma }}_t^{i-1})+d_t^{i-1}$$

(14)

The discrepancy of the expectation and measurement of the ray $b_{t-1}^j$ is,

$$e_t^j=d(j,R_{t-1})-{\mathit{\rho }}_{t-1}^j$$

(15)

4.3. Robust Perception Model

The robust perception model calculates p(R_t|R_t−1, x_t). Notice that the error $e_t^i$ is influenced by the following aspects (1) the accuracy of continuous state of x_t; (2) dynamic environments, such as occlusion by dynamic objects; (3) the abnormality of the laser range finder, such as separated beams.

A robust perception model is sensitive to the accuracy of x_t and robust to dynamic environments and the abnormality of the readings of sensors.

Let the measurement noise of a laser range finder follow a normal distribution with a variance of σ².

For the laser scanner SICK-LMS291, σ is set as 10 mm. The likelihood $p(b_{t-1}^j|R_{t-1},{\mathbf{\text{x}}}_t)$ is,

$$p(b_{t-1}^j|R_{t-1},{\mathbf{\text{x}}}_t)=\frac{1}{\sqrt{2\mathit{\pi }\mathit{\sigma }}}{e}^{-\frac{e_t^j}{2{\mathit{\sigma }}^2}}$$

(16)

Our perception model computes the likelihood with Equation (17).

$$p(R_t|R_{t-1},{\mathbf{\text{x}}}_t)=\frac{1}{|R_{t-1}|}\sum _{b_{t-1}^j\in R_{t-1}}p(b_{t-1}^j|R_{t-1},{\mathbf{\text{x}}}_t)$$

(17)

5. Fault Diagnosis and Robust Dead Reckoning Based on Particle Filter

In this section, we give the detailed ideas of our work on the problem of fault diagnosis and robust dead reckoning based on a particle filter and raw scan matching. Firstly, the fault models, as well as the correspondent kinematics of different fault models are derived for a wheeled mobile robots equipped with two encoders, one fiber gyroscope and one laser range finder. Secondly, a fast and robust raw scan matching method, which is called ‘inverse ray tracing’, is discussed. In the presented method, the ‘inverse ray tracing’ method served as the perception model, which may be used to weight the particles. At last, the complete algorithm is presented.

5.1. Fault Models and Kinematics

For the problem of fault diagnosis and a dead reckoning system based on particle filters, the fault models and there kinematics are described as follows. Four kinds of discrete models are taken into consideration, as shown in

Table 1. Fault Models for Mobile Robots.

Fault Models	Fault Components	Kinematics
1	(No Faults)	Equation (19)
2	Left Encoder	Equation (21)
3	Right Encoder	Equation (22)
4	Gyroscope	Equation (23)

.

Each discrete fault model determines the continuous state transition. In robust dead reckoning, the continuous state is x_t = (x_t, y_t, θ_t)^T. In the model state, the kinematics model is shown as Equation (18).

$$\{\begin{array}{ll}{\mathit{\theta }}_t\hfill & ={\tau }_t·{\dot{\mathit{\theta }}}_t\hfill \\ x_t\hfill & ={\tau }_t·{\upsilon }_t·\mathit{\text{cos}}({\mathit{\theta }}_t)\hfill \\ y_t\hfill & ={\tau }_t·{\upsilon }_t·\mathit{\text{sin}}({\mathit{\theta }}_t)\hfill \end{array}$$

(18)

where υ_t and θ̇_t denote the linear speed and yaw rate of the robot at time step t, which are recorded with encoders and the gyroscope, respectively. Normally, υ_t and θ̇_t are reckoned based on the readings of the internal sensors, namely, encoders and the gyroscope. τ_t denotes the interval of time step t.

Let $e_t^L,e_t^R$ denote the linear velocity of left wheel and right wheel, which are obtained from the readings of the encoders by multiplying the radius of the wheels; g_t denotes the yaw rate, which is the readings of the gyroscope, respectively. The velocity kinematics model of the normal state (model 1) is as follows,

$$\{\begin{array}{ll}{\upsilon }_t^1\hfill & =(e_t^L+e_t^R)/2\hfill \\ {\dot{\mathit{\theta }}}_t^1\hfill & =g_t\hfill \end{array}$$

(19)

Notice that the yaw rate can be measured with the gyroscope and reckoned with the measurements of encoders as follows,

$${\dot{\mathit{\theta }}}_t=(e_t^R-e_t^L)/D$$

(20)

where D denotes the axis length.

The gyroscope is more accurate than the encoder. In a normal situation, we use the readings of the gyroscope as the estimation of the yaw rate. When the gyroscope is stuck, we use the readings of the encoders to reckon the yaw rate according to Equation (20). Similarly, the readings of one encoder can be computed based on the readings of the other encoder and the gyroscope, according to Equation (20). In this way, we get the velocity kinematics for the fault Models 2, 3 and 4.

The velocity kinematics for fault Mode 2 is as follows,

$$\{\begin{array}{ll}{\upsilon }_t^2\hfill & =e_t^R-D·g_t/2\hfill \\ {\dot{\mathit{\theta }}}_t^2\hfill & =g_t\hfill \end{array}$$

(21)

The velocity kinematics for fault Mode 3 is as follows,

$$\{\begin{array}{ll}{\upsilon }_t^3\hfill & =e_t^L+D·g_t/2\hfill \\ {\dot{\mathit{\theta }}}_t^3\hfill & =g_t\hfill \end{array}$$

(22)

The velocity kinematics for fault Mode 4 is as follows,

$$\{\begin{array}{ll}{\upsilon }_t^4\hfill & =(e_t^L+e_t^R)/2\hfill \\ {\dot{\mathit{\theta }}}_t^4\hfill & =(e_t^R-e_t^L)/D\hfill \end{array}$$

(23)

5.2. Particle Filter for Fault Diagnosis and Dead Reckoning

In this paper, robust dead reckoning is defined as computing the relative transform between two consecutive frames based on the readings of internal sensors and raw range data matching of two consecutive frames. The initial guess is based on the readings of internal sensors and the fault models. The particles are then drawn and evaluated according to the matching accuracy of consecutive raw data of laser range finder.

The particle filter for simultaneous fault diagnosis and robust dead reckoning is shown as Algorithm 2. The importance sampling is shown from Step 3 to Step 13. Firstly, during Step 3–9, the loop guarantees that every fault model is sampled by at least one particle. Noticed that Step 8 computes the weights of the dead reckoning of different models. Step 11 shows that the discrete particles are drawn according to the weights obtained with Step 8.

6. Experimental Results

6.1. The Robot and Experimental Scenario

The experimental data are obtained with the robot, MORCS-1, as shown in Figure 1, which is driven by a person remotely through a narrow door. The speed is about 50 mm/s. The axis length D is about 0.6 m. The MORCS-1 is equip with four encoders, one gyroscope one laser range finder (LMS291) and one CCD camera. LMS291 is a 2D laser range finder produced by SICK. LMS291 has a scanning range up to 81.92 meters from negative 90 degrees to 90 degrees at every 0.5 degree. The range resolution of LMS291 is 10 mm [36]. For convenience, the sampling time of the odometry and laser scanner are both set as 0.25 s. In our experimental scenarios, we only use the data of two encoders, the gyroscope and the laser range finder. The velocity obtained from the internal sensors is shown in Figure 2. Figure 2a,b shows the mean speed during the time interval τ_t of the left wheel and the right wheel, respectively. Figure 2c shows the mean angular speed of the robot. Figure 2d shows the value of each time step.

Algorithm 2: Particle filter for simultaneous fault diagnosis and robust dead reckoning.

Initialize: Set particle number N, S = {1, 2, 3, 4}.;

for each time step t do

Determine the time interval τt, such that, the |Wt| > reliabeScanTh, the linear and angular distance of the robot is less than distotalth and angtotalth, respectively.;

for i=1 to 4 do

$s_t^{[i]}=i$;

${\mathit{\theta }}_t^{[i]}={\tau }_t·{\dot{\mathit{\theta }}}_t^i$;

$x_t^{[i]}={\tau }_t·{\upsilon }_t^i·\mathit{\text{cos}}({\mathit{\theta }}_t^{[i]})$;

$y_t^{[i]}={\tau }_t·{\upsilon }_t^i·\mathit{\text{sin}}({\mathit{\theta }}_t^{[i]})$;

compute weight, $w_t^{[i]}=p(R_t|R_{t-1},{\mathbf{\text{x}}}_t^{[i]})$, according to Equation (17);

end

for i=5 to N do

Draw $s_t^{[i]}$, st , $s_t^{[i]}=j\propto w_t^{[j]}$, j ∈ [1..4];

Draw continuous state: ${\mathbf{\text{x}}}_t^{[i]}~p({\mathbf{\text{x}}}_t|{\mathbf{\text{x}}}_{t-1}^i,s_t^{[i]})$ according to kinematic model of different fault modes, which is given by Equations (19), (21), (22) and (23), and the variance of state noise is governed by $w_t^{[s_t^{[i]}]}$.;

Compute weights: $w_t^{[i]}=p(R_t|R_{t-1},{\mathbf{\text{x}}}_t^{[i]})$, according to Equation (17);

end

Calculate marginal probability distribution ${\widehat{p}}_N(s_t|{\mathbf{\text{z}}}_{\mathrm{1..}t})={\sum }_{i=1}^N{w}_t{\delta }_{s_t^{[i]}}(s_t)$;

Fault diagnosis: state estimation ${\widehat{s}}_t^{\mathit{\text{MAP}}}={\mathit{\text{argamax}}}_{s_t}{\widehat{p}}_N(s_t|{\mathbf{\text{z}}}_{\mathrm{1..}t})$;

Dead reckoning estimation: ${\widehat{\mathbf{\text{x}}}}_t={\mathit{\text{argamax}}}_{{\mathbf{\text{x}}}_t^{[i]}}{w}_t^{[i]}$;

end

The movement of the robot at each time step is shown in Figure 3. The faults are simulated by setting the value of movement of some time step to zero. Specifically, during time Steps 5 to 9, the movement of the left wheel is set as zero (set the fault model as 2). During time Steps 10 to 14, the movement of the right wheel is set as zero (set the fault model as 3). During time Steps 15 to 19, the angular movement is set as zero (set the fault model as 4).

The results are obtained off line with MATLAB. The particle number N is set as 200. The distance threshold between two time step, distotalth, is set as 300 mm, and the angle threshold between two time step, angtotalth, is set as 15 degrees. The threshold reliabeScanT h, which controls the reliability of raw scan readings, is set as 240.

6.2. The Result of Fault Diagnosis

The result of fault diagnosis is shown in Figure 4 and

Table 2. Likelihood of dead reckoning for four models.

t	Model 1	Model 2	Model 3	Model 4
1	0.3103	0.2485	0.4062	0.1247
2	0.1538	0.2021	0.1218	0.1598
3	0.5538	0.6027	0.4523	0.2907
4	0.1708	0.2006	0.1200	0.1690
5	0.0626	0.2583	0.0370	0.1165
6	0.0053	0.6390	0.0216	0.0301
7	0.0025	0.5691	0.0043	0.0346
8	0.0215	0.3831	0.0088	0.0378
9	0.0145	0.2756	0.0040	0.0081
10	0.0051	0.0000	0.3957	0.0093
11	0.0159	0.0009	0.5132	0.0105
12	0.1272	0.1056	0.1986	0.0151
13	0.0955	0.0460	0.5226	0.0148
14	0.0327	0.0040	0.2864	0.0103
15	0.0923	0.0852	0.0555	0.2845
16	0.0610	0.0265	0.0285	0.0940
17	0.0394	0.0452	0.0450	0.1238
18	0.0648	0.0207	0.0726	0.2077
19	0.0355	0.0158	0.0774	0.0949
20	0.1774	0.1013	0.1383	0.0353

.

Figure 4 shows the results of fault diagnosis, as well as the true states. It shows that the algorithm works well during the periods of time Steps 5 to 19, when one internal sensor is stuck at zero. There are 10-times the misdiagnoses during the whole testing data of 30 time steps. Most misdiagnoses occur in the situation of ‘no faults’. This is mainly because of the redundancy of the system, i.e., four kinds of kinematics are all correct when the system is normal, so the likelihoods of all models are nearly the same. This kind of diagnosability will be discussed in the next subsection.

Table 2 shows the likelihood of four kind of dead reckoning estimation of different fault models of the first 20 time steps. It shows that during the period of time Steps 5 to 9, the score of Model 2 is significantly larger than the scores of other models, this is consistent with fault Model 2 injected at this period. Similarly, during time Steps 10 to 14, the score of Model 3 is dramatically larger than the score of others; during time Steps 15 to 19, the score of Model 4 is larger the score of others, obviously.

6.3. Accuracy of the Perception Model

Figures 5, 6, 7 and 8 illustrate the accuracy of the perception model. Figure 5 illustrates the raw scan match of the initial guess of four fault models at time Step 5. The score of fault Model 2 is the largest, and the matching result is also the best among the four subfigures. Accordingly, Figure 6 shows the likelihood of rays of the initial guess of four fault models (time Step 5). It shows that there are more high likelihood rays in the second model (subfigure (b)) than others. Figures 7 and 8 illustrate the match result of four method at time Step 5. It show that the match results and the likelihoods are consistent.

6.4. Accuracy of Robust Dead Reckoning

The accuracy of the proposed robust dead reckoning method is discussed in this section. Four kinds of method are implemented. The first one is the robust dead reckoning method (RDR for short) proposed in this paper. The second is the conventional dead reckoning method, which uses the readings for internal sensors directly (DR for short). The third is the PSM method. The fourth is the GA-ICP method (ICP for short). The GA is implemented with the sampling step of our method, but we only use the particles of normal models. ICP is then performed based on the best particle of normal models.

Figures 9, 10, 11 and 12 show the raw ray matching results of four kinds of dead reckoning (based on different fault models) and robust dead reckoning, in the situation of fault Model 1, i.e., the case of ‘no fault’. It is shown that the four kinds of dead reckoning are almost the same.

Similarly, Figures 13, 14, 15 and 16 show the matching results of the case of fault Model 2, i.e., ‘left encoder error’. Figures 17, 18, 19 and 20 show the results of the case of fault Model 3. Figures 21, 22, 23 and 24 show the results of the case of fault Model 4.

These figures show that in the case of ‘no faults’, the ICP method is superior to others. The proposed RDR method is superior to others in most cases when one of the internal sensors is stuck at zero. This is mainly because the ICP and PSM method do not take into consideration the faults. They suppose that the robot is in the normal state and then employ a bad initial guess. In fact, one can improve the accuracy of dead reckoning by taking the results of RDR as an initial guess of the PSM or ICP methods.

6.5. Diagnosability

There are three typical cases that are hard to diagnose. The first is that the kinematics of different models has a similar performance. The second is the that real state has not been modeled in the system. The third is that the readings of the external sensors are seriously damaged.

Figure 25 shows the case of ‘no faults’; in this case, each model is correct, because of redundancy between the gyroscope and encoders. Therefore, every model has similar weights. Figure 26 shows the case of ‘two faults’, that is to say, two sensors are faulty simultaneously. Since none of the models given by the system can govern the situation, the weights of all four models are insignificant.

The presented robust dead reckoning system uses the raw scan of the laser range finder to improve the accuracy of dead reckoning. If the internal and external sensors both have faults in a short time interval (and there are no other sensors for help), the problem can be modeled as a kidnap problem, and some kind of global positioning method is recommended to handle this kind of situation.

7. Conclusion

In this paper, a robust accurate dead reckoning method based on particle filters is put forward by integrating the fault diagnosis of internal sensors and the readings of a laser range finder. In this framework, fault diagnosis and dead reckoning is computed simultaneously. The discrete fault models govern the continuous state transition of the dead reckoning system. The errors are corrected with the reading of external sensor, i.e., laser range finder. The perception model of external sensors is fast and accurate by exploring the inverse ray tracing method. Experimental results and analysis are given. The presented method can serve as the first step for many other applications, such as SLAM, robust navigation, exploration, object tracking, and so on.