A novel STSOSLAM algorithm based on strong tracking second order central difference Kalman filter

Robotics and Autonomous Systems 116 (2019) 114–125

Contents lists available at ScienceDirect

Robotics and Autonomous Systems journal homepage: www.elsevier.com/locate/robot

A novel STSOSLAM algorithm based on strong tracking second order central difference Kalman filter ∗

Dai Jiahui a,b , , Li Xiaobo b , Wang Kequan b , Liang Yunpei a a b

State Key Laboratory of Coal Mine Disaster Dynamics and Control, Chongqing University, Chongqing 400044, China China Coal Technology Engineering Group Chongqing Research Institute, Chongqing 400037, China

highlights • • • • •

A high accuracy and robustness SLAM algorithm is proposed. It combines 3 filters: STF, SOCDF, PF. STF is introduced into the algorithm to suppress the influence of uncertain noise. It can be used in coal mines and similar terrain. The experimental results demonstrate a good performance.

article

info

Article history: Received 14 June 2018 Received in revised form 26 December 2018 Accepted 18 March 2019 Available online 29 March 2019 Keywords: FastSLAM CDFastSLAM STSOSLAM Particle filter Second order central difference particle filter Strong tracking filter

a b s t r a c t Simultaneous Localization and Mapping (SLAM) is an effective technique in the field of robot location and navigation. However, when the existing SLAM algorithm is applied in harsh terrain, such as the terrain found in coal mines, accuracy suffers, and on-line adaptive adjustment capability is poor. Furthermore, the system suffers from low robustness and is susceptible to random noise. In order to solve these problems, we propose an innovative Strong Tracking Second Order Central Difference SLAM (STSOSLAM) algorithm that combines a Strong Tracking Filter (STF), a Second-Order Central Differential Filter (SOCDF), and a Particle Filter (PF). The new algorithm utilizes the second order sterling interpolation formula to deal with the nonlinear system problem using the Cholesky decomposition technique, which propagates directly by using the covariance square root factor in the SLAM probabilistic estimation. This technique not only guarantees the positive definite property of the covariance matrix, but also reduces the truncation error of local linearization. In addition, STF is introduced into the algorithm. It updates every sigma point using an adaptive algorithm and obtains optimized filter gain through the STF online adjustment factor and suppresses uncertain noise and the influence of initial value selection. Through simulation and experiments, STSOSLAM algorithm shows much better performance in terms of estimation accuracy, robustness and reliability than FastSLAM2.0 and Central Difference FastSLAM (CDFastSLAM) algorithms, establishing the foundation of applying the STSOSLAM algorithm in the harsh terrain of coal mines. © 2019 Elsevier B.V. All rights reserved.

1. Introduction

incrementally and then locates itself therein [2].

SLAM proposed by Smith in the mid-1980s [1], is one of the key technologies for autonomous mobile robot localization and navigation. SLAM is a process by which a mobile robot obtains environmental feature information via its own sensors in an unknown environment and builds an environment map

divided into laser SLAM and visual SLAM(VSLAM). Research on

According to the different types of sensors, SLAM is mainly laser SLAM began earlier than VSLAM. It is more mature in theory, technology and products. Laser SLAM is currently the most stable, most accurate, and most popular positioning and navigation method. With the rapid development of computer vision, VSLAM (vision-based positioning and construction) has ∗

Corresponding author at: State Key Laboratory of Coal Mine Disaster Dynamics and Control, Chongqing University, Chongqing 400044, China. E-mail address: [email protected] (J. Dai). https://doi.org/10.1016/j.robot.2019.03.006 0921-8890/© 2019 Elsevier B.V. All rights reserved.

attracted wide attention due to its large amount of information and wide application scope. There are three mainstream VSLAM algorithms: ORB-SLAM [3], LSD-SLAM [4], SVO-SLAM [5].

J. Dai, X. Li, K. Wang et al. / Robotics and Autonomous Systems 116 (2019) 114–125

VSLAM can work in indoor and outdoor environments, but it is highly dependent on light and cannot work in dark places or in some non-textured areas. In the underground environment of a coal mine, especially in coal mines after a disaster, there is no visible light source and VSLAM cannot be used. Therefore, only laser SLAM can be used to carry out environment mapping work in underground coal mines and similar terrain. Among laser SLAM algorithms, FastSLAM [6] is most commonly used. FastSLAM was proposed by Montermerlo in 2003. There are two versions of FastSLAM: FastSLAM1.0 and FastSLAM2.0 [7]. However, there are some shortcomings of FastSLAM. Firstly, in the new pose sampling process, FastSLAM 2.0 uses first order linearization. If the equation is highly nonlinear, the results suffer from inaccuracies. Secondly, in the environmental features updating process, it uses the standard EKF algorithm, which has low accuracy and high computational cost. To overcome these problems, Xin Yuan presented the AEKF-SLAM algorithm [8] based on an Augmented Extended Kalman Filter (AEKF). In addition, Deusch H proposed the LMB-SLAM [9], which does not require the approximations used in Probability Hypothesis Density SLAM (PHD-SLAM). At the same time, other researchers began to solve the problem of low accuracy in the SLAM algorithm by focusing on various aspects. Lv T designed FC&ASD-PSO-SLAM [10] to make Qt (Control noise) and Rt (Observation noise) match with their real value, improving the accuracy of the algorithm but increasing the amount of calculation. Li Q L introduced a neural network into FastSLAM [11] to compensate for odometry error and Jun Cao proposed a SLAM algorithm which is based on deep learning [12]. However, these algorithms require long training time. Choi J introduced a data association method into SLAM [13] to handle the false and spurious association problems effectively. Seung-Hwan L presented a Relational FastSLAM which improved the Rao-Blackwellized particle filtering framework [14]. Havangi R proposed an Intelligent FastSLAM [15], in which an evolutionary filter (EF) and an adaptive fuzzy unscented Kalman filter (AFUKF) are introduced. Muhammet Balcılar proposed the R-SLAM to overcome systematic and non-systematic odometry errors [16]. However, these algorithms can only reach the accuracy of the first-order Taylor series. In 1998, Nørgaard [17] and Ito [18] expounded and proofed the central difference filter theory and created the central difference filter(CDKF), which is based on the Stirling interpolation formula [19]. Hence, Schei [20] suggested the use of the central difference method to improve the EKF. But the literature [21,22] has proved that no matter the degree of system nonlinearity, EKF and CDKF can only achieve the first order Taylor accuracy. It is found that, among the existing SLAM algorithms, some have high accuracy but with more complicated calculation, and that some have simple and rapid calculation but with lower accuracy. For these problems, this article introduces a high-precision second order central difference filter (SOCDF) [23]. It uses the second order truncated Stirling interpolation to estimate posterior covariance, so that the approximation accuracy of the posterior mean and auto-covariance can reach the first two items of UT transformation. The approximation accuracy of cross covariance can reach the item of Px GTf in UT transformation [24]. In addition, the second order central difference filter can achieve the second order Taylor accuracy. For SOCDF, the accuracy is not only higher than EKF, but also can be applied to strongly nonlinear Gaussian systems. The second order central difference filter does not need to calculate the Jacobian matrix and is more easily realized than the EKF. It does not require that the nonlinear function be continuously differentiable, and overcomes the limitation of EKF theory effectively. In addition, due to the poor robustness of FastSLAM and CDFastSLAM algorithms, they are easily influenced by the noise

115

caused by harsh terrain when working in coal mines. This article introduces a strong tracking factor and it updates every sigma point by using an adaptive algorithm. It obtains optimized filter gain through the STF online adjustment factor [25], and suppresses the influence of uncertain noise and initial value selection on state estimation. This paper proposes the Strong Tracking Second Order Central Difference SLAM algorithm (STSOSLAM) based on strong tracking factor and second-order central difference Kalman filter. The new algorithm combines the adaptivity of STF and high nonlinear approximation of Second-Order CDKF. Furthermore, it adds a particle filter which is not affected by non-Gaussian and nonlinear models. Therefore, our new algorithm has strong adaptive ability, accuracy and robustness [26]. The remainder of this paper is organized as follows. The required background for second order central difference filters and STF are reviewed in Section 2. In Section 3, the STSOSLAM framework and algorithm details are presented. The simulation results and analysis based on the SLAM simulation tool are presented in Section 4. In Section 5, the validity of proposed algorithm is verified by a SLAM experiment in a real environment. The conclusions are given in Section 6. 2. Background Before introducing the new algorithm, we first must present some background knowledge on second-order Stirling polynomial interpolation expansion and strong tracking filtering. These two concepts form the basis of our algorithm. 2.1. Second-order Stirling’s polynomial interpolation expansion Second-order Stirling’s polynomial interpolation expansion is the theoretical basis of second order central difference filtering. Second-order Central Difference Filtering [27] uses second-order Stirling’s polynomial interpolation formulae to expand non-linear models. It does not need to calculate the Jacobian matrix, and even can estimate the state when the non-linear function is discontinuous and has a singular point. Setting x ∈ Rn as an n dimensional vector; in x = x¯ , expanding the nonlinear function y = f (x) by Stirling’s polynomial interpolation formula: y ≈ f (x¯ ) + Dx f (x¯ ) +

1 2!

D2x f (x¯ )

(1)

where Dx and D2x are the first difference and second difference operator respectively. In this paper, we use the Second-order Central Difference Filter which is more accurate. Dx f (x¯ ) = D2x f (x¯ ) =

1 [∑

λ

] ∆xp µp δp f (x¯ )

n 1 ∑

λ

[ 2

p=1

∆x2p δ 2p +

n n ∑ ∑

(2)

∆xp ∆xq

p=1 q=1 q̸ =p

× (µp δp )(µp δp )]f (x¯ )

(3)

where δp is the partial differential operator, µp is the mean operator, λ is variable step size and its optimal choice is λ2 = e3∗(nx +ny ) (nx , ny are the dimension of X ). 2.2. Strong Tracking Filter (STF) Strong Tracking Filter (STF) introduces fading factor λk+1 into the state prediction covariance, adjusting gain matrix online realtime, forcing output residual sequence and keeping orthogonality. In that way, STF can keep the tracking ability for the system state when the system model is uncertain.

116

J. Dai, X. Li, K. Wang et al. / Robotics and Autonomous Systems 116 (2019) 114–125

Given the nonlinear model: xk = fk−1 (xk−1 , uk−1 , wk−1 )

(4)

zk = hk (xk , vk )

(5)

Here xk is the state parameter of robot, zk is the observation of environmental feature; f is the nonlinear kinematic model and h is the environmental observation model; uk−1 is the control action for the robot in the discrete time [k, k − 1]; vk is control noise and its covariance matrix is Q; wk−1 is observation noise of the sensor and its covariance matrix is R. wk−1 and vk are unrelated white Gaussian noise. According to the literature [28], when the theoretical model matches the actual system exactly, the output residual sequence of the KF is non-autocorrelation or a mutually orthogonal Gaussian white noise sequence. In a physical sense, it means that all useful information in the residual sequence has been extracted by the filter. Therefore, the irrelevance of the residual sequence can be used as a measure of the quality of filter performance. However, EKF is a sub-optimal filter. It is impossible that the residual sequence is completely uncorrelated. Therefore, as long as the EKF residual sequences are weakly auto-correlated, it can be considered that EKF has good filtering effect. In regards to the STF online real-time adjustment of variable gain matrix Kk , the theoretical basis (Orthogonality principle) is as follows: [( )( )T ] Condition 1: E xk − xˆ k xk − xˆ k = min

)

Condition 1 is the performance index of EKF. Condition 2 requires that the output residual sequences in different times remain orthogonal. The fading factor λk+1 is calculated as follows [29]:

λk+1 =

{ λ0 1

λ0 ≥ 1 , λ0 < 1

λ0 =

tr [Nk+1 ] tr [Mk+1 ]

(6)

where tr(·) is the operator of trace of the matrix. Nk+1 = Vk+1 −

Hk+1 Qk HkT+1

− Rk+1

Mk+1 = Hk+1 Φk+1,k Pk ΦkT+1,k HkT+1

(7) (8)

⏐ ∂ fk (xk ) ⏐⏐ ∂ xk ⏐ xk= xˆ k ⏐ ∂ hk+1 (xk+1 ) ⏐⏐ = ∂ xk+1 ⏐ xk+1= xˆ k+1|k

Hk+1

(9)

Vk+1 is the covariance matrix of the actual output residual sequence, which is estimated by the following formula:

Vk+1

ε1 ε1T = ρ Vk + εk+1 εkT+1 ⎩ 1+ρ

⎧ ⎨

k=0 k≥1

Fig. 1 shows the flow of robot state estimation in STSOSLAM. According to Fig. 1, the process is as follows [30]: First, we estimate robot state and control input as a whole [31]. The state matrix after augmentation is: a[k]

xt −1 = [x[tk−] 1

[ a[k]

Pt −1 =

0]

T

(11)

]

Pt −1

[k]

0

0

Qt

[k]

a[k]

where xt −1 is the previous mean of the robot state, xt −1 is the augmented matrix with robot pose and control input, and its dimensionality is (n + r) × 1. Here n is the robot dimension, [k]

r is the dimension of system noise. Here n = 3, r = 2. Pt −1 a[k] is the previous covariance, Pt −1 is the augmented covariance matrix which contains robot covariance and the control noise. Its dimensionality is 5 × 5. Qt is the control noise covariance. [k]

We decompose Pt −1 and Qt by Cholesky decomposition, then put them into the matrix: a[k] Pt −1

[¯x =

St −1

0

0

Sv

] (12)

Step 1: we extract 2N+1 sigma points around the mean point deterministically, where N is the dimensionality of the mean vector [32]. Here, N = 5. Thus, the set of extracted sigma points are:

χta−[01][k] = xat −[k1]

( √ ) χta−[i1][k] = xat −[k1] + h Pta−[k1] ( √ )i χta−[i1][k] = xat −[k1] − h Pta−[k1]

(i = 1, . . . , N)

(13)

(i = 1, . . . , 2N)

i−N

where, Hk+1 is m × n dimensional measurement matrix, Qk is the covariance matrix of control noise, Rk+1 is the covariance matrix of observation noise, Φk+1,k is n × n dimensional state-transition matrix. Pk is the state prediction covariance. Here, Φk+1,k and Hk+1 are as follows:

Φk+1,k =

(1) Robot state estimation

√

Condition 2: E εk+j εkT = 0, k = 1, 2, . . . , j = 1, 2, . . .

(

the first-order linearization and extended Kalman filter. Furthermore, we introduce a Strong Tracking Filter (STF) to overcome the problem of the poor robustness of CDKF when the model is uncertain. The new algorithm is divided into 2 parts: robot state estimation and environmental features estimation.

(10)

where h is the step size of central interpolation. Although in the same dimension and the number of sigma point between UFastSLAM [33] and our algorithm is the same, the principle of production sigma points is different, the numerical value and weight are different, and the estimated variance and covariance are different. For the Gaussian random √ variable of arbitrary dimensions, the optimal value of h is 3. Now every sigma point contains robot state and control input information. Therefore, we can calculate the robot state information by sigma points which are transformed by the nonlinear kinematic model (formula 4). We can obtain the robot pose information as follows.

( ) χ¯ t[i][k] = f ut , χt[−i][1k]

(14)

The robot state mean and the prediction of the covariance are calculated as follows:

where 0 < ρ ≤ 1 is the forgetting factor, the value is set to 0.95 usually.

xkt|t −1 =

3. Description of STSOSLAM

Pt |t −1 =

2N ∑

ωim χ¯ t[i][k]

(15)

i=0 k(l)

N ( )2 ∑ [ωic1 χ¯ t[i][k] − χ¯ t[i+N ][k] i=1

STSOSLAM is a kind of process which takes FastSLAM2.0 as a frame and uses a Second-order Central Difference filter to replace

)2 ( + ωic2 χ¯ t[i][k] + χ¯ t[i+N ][k] − 2χ¯ t[0][k] ]

(16)

J. Dai, X. Li, K. Wang et al. / Robotics and Autonomous Systems 116 (2019) 114–125

117

Fig. 1. Flow chart of robot state estimation.

where the weight of sigma points can be calculated as followed: h2 − n − r

ω0m =

h2 1

ωim = ωic1 =

(17)

2h2 1 4h2 h2 − 1

ωic2 =

, i = 1, 2, . . ., 2n 4h4 In order to obtain a more exact robot state, the observation information must be considered. We update the prediction in the above formulas by using environmental observation model (formula 5). The formulas are as follows. [i][k](l)

N¯ t

( ) = h χ¯ t[i][k](l) , µkkˆ ,t −1

(18)

2N

[k](l)

nˆ t

=

∑

ωim N¯ t[i][k](l)

(19)

i=0 N

[k](l)

St

=

) ( [i][k](l) ∑ [i+N ][k](l) 2 [ωic1 N t − Nt i=1

( )2 [i+N ][k](l) + ωic2 N¯ t[i][k](l) + N t − 2N¯ t[0][k](l) ] [i][k](l)

(20)

where N¯ t is the sigma point which is calculated via nonlinear [k](l) [k](l) observation equation, nˆ t is the prediction of observation, St is the updating vector (auto-covariance). So that we can get the ∑x,n[k](l) : cross-covariance matrix t x,n[k](l)

∑ t

N √ ( )( ) ∑ [i+N ][k](l) T = ωic1 χ¯ t[i][k](l) − xkt|t −1 N¯ t[i][k](l) − N t i=1

Fig. 2. Flow chart of environmental features position estimation.

(21) The introduction of fading factor λk+1 : According to the literature [34], we get the fading factor λk+1 . In order to make the algorithm have better robustness when the

118

J. Dai, X. Li, K. Wang et al. / Robotics and Autonomous Systems 116 (2019) 114–125

system model is uncertain, we introduce the fading factors λk+1 into the state prediction covariance matrix.

[ ] √ χta[i][k] = xat [k] , xat [k] ± h Pta[k]

N )2 ( ∑ = λk+1 [ωic1 χ¯ t[i][k] − χ¯ t[i+N ][k]

Ptk|t −1

We update the sigma points set in the formula 13 basing on the formulas 28 and 29. The new sigma points set is as follows: (30)

i=1

+ω

c2 i

We got the

(2) Environmental features estimation

)2 ( χ¯ t[i][k] + χ¯ t[i+N ][k] − 2χ¯ t[0][k] ] Ptk|t −1

(22)

after introducing λk+1 . So we use [i][k]

Ptk|t −1 to calculate the sigma point χ¯ t

xkt|t −1

and

in accordance with the [i][k]

sample strategy in step1. The sigma point χ¯ t

transforms into

[i][k]

Nt through the nonlinear measurement function (formula 5). [k] We can calculate the output prediction nˆ t , the auto-covariance [k]

and the cross-covariance

St

[i][k]

N¯ t

[k]

nˆ t

∑x,n[k] t

[i][k]

from N t

.

) ( = h χ¯ t[i][k] , µkkˆ ,t −1

=

2N ∑

(23)

m [i][k] i Nt

ω ¯

(24)

Fig. 2 shows the flow of environmental features position estimation in STSOSLAM. The details of the algorithm are presented below: The observed features need to be estimated and updated. Similarly with the robot state estimation, we structure the sigma points set first while we estimate the position of the environmental features. As the SLAM operates in a two-dimensional scene, the dimensionality of the environment feature is 2. The sigma points are as follows:

χ a[0][k] = µ[nkt],t −1

⎛   [m] ⎞ ∑ ⎠ (i = 1, . . . , n) = µ[nkt],t −1 + ⎝h√

χ a[i][k]

i=0

[k]

St

=

N ∑

c1 i

[ω

(

[i][k]

Nt

) [i+N ][k] 2

− Nt

χ a[i][k] = µ[nkt],t −1

i=1

(

[i+N ][k] + ωic2 N¯ t[i][k] + N t − 2N¯ t[0][k] x,n[k]

∑

=

t

N √ ∑

)2

]

) ( )( [i+N ][k] T ωic1 χ¯ t[i][k] − xkt|t −1 N¯ t[i][k] − N t

(25) where (26)

[i][k]

[k]

=

[k]

St

[k]

)−1

(27)

zˆt

=

[k]

xt

(

= xkt|t −1 + Kt[k] zt − nˆ [tk]

)

(28)

is the mean of the nth feature at time t − 1,

2N ∑

ωim Z¯t[i][k]

[k]

S¯t

=

( )T = Ptk|t −1 − Kt[k] St[k] Kt[k]

(29)

nt ,t −1

(32)

(33)

N ( )2 ∑ [ωic1 Z¯t[i][k] − Z¯t[i+N ][k] i=1

( )2 + ωic2 Z¯t[i][k] + Z¯t[i+N ][k] − 2Z¯t[0][k] ] [i][k]

[k]

Pt

∑[m]

i=0

t

At time t, the robot state mean and the covariance estimation can be computed as follows:

i−n

( ) = h χ [i][k] , xkt (i = 0, . . . , 2n)

Then get the Kalman gain Kt .

∑(

µ[nkt],t −1

(i = 1, . . . , 2n)

is its covariance. The other parameters can be calculated like previous section. Here N = 4. N¯ t

[k]

x,n[k]

i ⎛   [m] ⎞ ∑ ⎠ − ⎝h√ nt ,t −1

i=1

Kt

(31)

nt ,t −1

(34)

Here N¯ t are the sigma points which calculated by nonlinear [k] [k] observation formulas. zˆt and S¯t are mean and covariance of

Fig. 3. Simulation environment based on a Matlab SLAM simulator.

J. Dai, X. Li, K. Wang et al. / Robotics and Autonomous Systems 116 (2019) 114–125

119

Fig. 4. The comparison between estimated path and real path of the robot.

Fig. 5. FastSLAM2.0 simulation graph.

observation prediction respectively. The next step is to calculate [k] [k] the Kalman gain K¯ t and update the mean µnt ,t and covariance ∑[k] nt ,t of environmental features. [k]

Σt

In the last step, we need to do the judgment resample for the particles. The resample method is the same as in FastSLAM2.0.

nt ,t −1

[k]

K¯ t

(

¯ t[k] S¯t[k] =Σ

Here:

)−1

(35)

x,n[k]

[k]

(

µ[nkt],t = µ[nkt],t −1 + K¯ t[k] zt − zˆt[k] [k] ∑ nt ,t

=

[k] ∑

Target Distribution

wt[k] =

Proposal Distribution { } 1 −1 = |2π L[tk] | 2 exp − (zt − z t )Lt[k−1] (zt − z t ) 2

  [m] ( )T ∑  [i:N ][k] Z¯t − Z¯t[N +1:2N ][k] = √ωic1

( )T −K¯ t[k] S¯t[k] K¯ t[k]

nt ,t −1

(3) Resampling strategy

Lt

)

=(

∑

x,n[k]

[k]

)T (Pt )−1

t

∑

[k]

+S t

t

4. Simulation result (36) In order to verify the effectiveness and accuracy of STSOSLAM, we design a simulation test and compare it with FastSLAM2.0 and CDFastSLAM [35].

120

J. Dai, X. Li, K. Wang et al. / Robotics and Autonomous Systems 116 (2019) 114–125

Fig. 6. CDFastSLAM simulation graph.

Fig. 7. STSOSLAM simulation graph.

By using MATLAB2015, we use the SLAM algorithm simulation tool published by the Australian Centre for Field Robotics to compare the three algorithms. We use the Car-like motion model, and use the Bearing-Rang Lidar Scanning Model as the observation model. We set the wheel base to 4 m, velocity to 3 m/s, the sensor effective distance to 40 m, and the front viewing angle to 180◦ . The noise is normally distributed (0, 1) white Gaussian noise. The process noise is set to (σV = 0.3 m/s, σG = 3◦ )and the observation noise to (σr = 0.4 m, σθ = 5◦ ). Figs. 3 and 4 show the simulation environment. Its length and width are 200 m. The green ‘‘※’’ stands for landmarks and ‘‘o’’ stands for the path points along which the robot moves. The lines between those points are the robot path. The robot moves from the original point and walks along the curve for two laps. We set 35 landmarks and 17 path points in this experiment. The robot starts from the (0,0) point and moves counterclockwise along the

path.(Fig. 3). All tests are repeated 30 times, and we utilize the average value as the statistical result. In Fig. 4, the black curve shows the estimated path trajectory of the robot, while the red points show its estimation of the landmarks’ location. There are some deviations between the estimation track and the real track and estimation landmarks and real landmarks due to odometer and observation error. This simulation experiment evaluates the quality of these algorithms by comparing location errors, landmark errors and consistency. Lower location error demonstrates higher algorithm accuracy. Figs. 5–7 show comparison of the simulated paths among FastSLAM2.0, CDFastSLAM and STSOSLAM. In the beginning, we can see that the estimation accuracies of these three algorithms are very high, and their estimated paths and real paths are almost identical. Later, with the increase of robot mileage, the error between estimation path and real path

J. Dai, X. Li, K. Wang et al. / Robotics and Autonomous Systems 116 (2019) 114–125

121

Fig. 8. Estimated position error in different algorithms.

Fig. 10. RMSE of STSOSLAM by different number of particles.

Fig. 9. RMSE of different algorithms in different level of measurement noise.

Fig. 11. Estimated position error of STSOSLAM in different number of particles.

become more and more pronounced. In later periods, the error of FastSLAM2.0 reaches the maximum level, and the CDFastSLAM is comparatively less. As STSOSLAM has better numerical characteristics and introduced the strong adaptive factor λk+1 , it obtains the highest accuracy as shown by the high level of congruency between its estimated path and the actual path. Fig. 8 compares robot location estimation error among the standard FastSLAM2.0, CDFastSLAM and STSOSLAM under the same conditions, where the system noise and observation noise are white Gaussian noise. The X axis represents the time step size, and every experiment takes 17383 steps. Fig. 8 demonstrates several findings. First, in general, there is an obvious increase of the estimation accuracy of these three algorithms after the first lap. The reason is that when the robot runs the 2nd loop, the algorithm will match with the route of the first loop and correct its error. Second, the location error of FastSLAM2.0 is much larger than the other two algorithms. The location error of CDFastSLAM and STSOSLAM is similar in the beginning, but as the robot mileage increases, the error of CDFastSLAM becomes larger than that of STSOSLAM. As STSOSLAM has higher filter accuracy and introduces a strong adaptive factor, the error is perfectly controlled, especially in the later period, where the error is pronouncedly less than the other two algorithms’.

Fig. 12. Average runtime of different algorithms with the same accuracy.

122

J. Dai, X. Li, K. Wang et al. / Robotics and Autonomous Systems 116 (2019) 114–125

Fig. 15. Office room environment CAD reduced graph.

Fig. 13. P3DX Robot.

In order to verify the accuracy and stability of these three algorithms, we set four different groups of environmental noise:(σr = 0.1 m, σb = 3◦ ), (σr = 0.3 m, σb = 3◦ ), (σr = 0.4 m, σb = 5◦ ), (σr = 0.4 m, σb = 8◦ ). Meanwhile, we set 20 particles and mile meter noise as (σv = 0.3 m/s, σG = 3◦ ). In each algorithm, the evaluation criteria of the deviation between the observed values and the true ones is the root-mean-square error (RMSE): RMSE =

( ∑Ns ⏐ ⏐ )1 ⏐xk − xˆ k ⏐2 2 k=0

Ns − 1

(37)

where, xk represents the real state of the robot and xˆ k represents the estimation by using SLAM algorithm at time k. From Fig. 9, we observe that the RMSE value of these three algorithms increases as the noise increases. In all conditions, the RMSE of STSOSLAM is the least and it is clearly superior to the other two algorithms. When the measurement noise is σr = 0.1, σb = 3, the RMSE of CDFastSLAM is close to FastSLAM’s, but the RMSE of STSOSLAM is much lower than the others. As the noise increases, the advantage of STSOSLAM becomes more and more obvious. When the noise value is σr = 0.4, σb = 8, the gap between the RMSE of CDFastSLAM and STSOSLAM decreases, but the RMSE of these two are much lower than FastSLAM2.0’s.

As the noise increases, the standard deviations of these three algorithms increases as well. But at all noise levels, the standard deviation of STSOSLAM is lower than the others. This illustrates the superior consistency and stability of the new algorithm. Fig. 10 shows the RMSE value for different numbers of particles when STSOSLAM is in the same noise level. When the number of particles is less than or equal to 40, the RMSE decreases linearly. When the number of particles is greater than 40, the decreasing trend of RMSE becomes gentler. As the number of particles increases, its standard deviation decreases as well. This shows that more particles can bring the algorithm higher stability and consistency. Therefore, as the number of particles increase, the degree of accuracy will improve. When the number of particles is more than 40, the algorithm will spend more time on calculation, but the improvement of accuracy is less pronounced. Therefore, STSOSLAM reaches optimal performance at 40 particles. Fig. 11 shows the estimated position error of STSOSLAM in different numbers of particles. Fig. 11 corresponds with Fig. 10, and shows when the number of particles ranges between 10 and 40 the accuracy increases markedly as the number of particles increases. However, when the number of particles reaches 80, the accuracy is only slightly higher than accuracy at 40 particles. In order to compare the complexity, we compared the runtime of these three algorithms in the simulator. Due to the higher accuracy of STSOSLAM algorithm, it only needs fewer particles to achieve the same accuracy compared with the other two algorithms. Thus, at the same level of accuracy, STSOSLAM uses less runtime than other algorithms (as shown in Fig. 12).

Fig. 14. Experiment environment built in the office.

J. Dai, X. Li, K. Wang et al. / Robotics and Autonomous Systems 116 (2019) 114–125

123

Fig. 16. Robot route map in the experiment environment.

5. Experimental test on the P3DX robot In order to verify the performance of the new algorithm, robot SLAM experiments were carried out in our office. The test platform uses the Pioneer-3DX Autonomous Mobile Robot (Fig. 13) which is produced by Active Media company in the USA. It is a two-wheeled differential drive robot. The Pioneer 3-DX is equipped with a 500-line encoder motor, a 19-cm tire, an aluminum housing, eight front anti-collision sonars and eight rear anti-collision sonars. In addition, it is equipped with a SICK LMS radar, whose working distance is 0.5–20 m, viewing angle is 270 degrees, and scanning frequency is 25 Hz/50 Hz. It has the advantages of good flexibility, high stability, strong generality and easy operation. It is one of the most mature robots for robot research experiments in the world. Fig. 14 is a room where the robot will perform the SLAM test. The left picture is an empty room. We used railroad tracks, sand and wood to set up an obstacle on the ground to simulate rough road conditions found in coal mines. When the robot passes the obstacle, the tires will slip. This can test the robustness of the algorithm. There is a desk in the middle of the right picture. Because of the installation height of the laser sensor, the chair may not be detected by the laser sensor. In order to let the reader understand the layout of the room better, we used SolidWorks software to draw the scene image of the office. As shown in Fig. 15: Fig. 16 is a road map that the robot follows in the office. The robot starts from point A, passes point B, passes through the corridor (point C), and enters another room. After passing through the previously set obstacle at point D, the robot will enter the corridor again and finally return to point A. Fig. 17 shows the maps built by STSOSLAM, CDFastSLAM and FastSLAM2.0, respectively. Fig. 17a is a map created by the STSOSLAM algorithm, Fig. 17b is built by CDFastSLAM and Fig. 17c is built by the FastSLAM2.0 algorithm. From Fig. 17, it can be seen that Fig. 17a is the clearest and that all the details are nearly perfectly represented. This is due to the high accuracy of the second-order central difference filter and the high robustness of the STF. When the tire slightly slips, it will not affect the map drawing. Fig. 17b has been somewhat obscured and ghosting has occurred in some places. The map deviation of Fig. 17c is the highest because the odometer error causes the map created by the trip and the map created by the return trip to completely fail to match. In addition, Fig. 17a shows the best closed-loop performance. But in Fig. 17c, FastSLAM2.0 failed to close the loop. To quantitatively test the performance of different algorithms, we selected 8 measurement distances from A to H in the office, measured and recorded the length of these 8 distances (Fig. 18). We evaluated the performance of the algorithm by comparing the actual distance with the distance obtained by the SLAM algorithm. For each measurement distance, we performed 10 tests to get the average value as the statistical result. Table 1 lists the

Fig. 17. Comparison of maps built by the three algorithms.

Table 1 Comparison of actual dimension and measurement dimension. Measurement number

Actual dimension (mm)

A B C D E F G H

5 970 6 200 13 050 13 360 4 770 5 710 12 800 2 260

Measurement dimension (mm) STSOSLAM

CDFastSLAM FastSLAM2.0

6 008 6 241 13 192 13 511 4 797 5 747 12 955 2 271

6 327 6 230 13 181 13 340 4 981 5 275 13 025 2 410

6 346 6 276 12 659 13 574 5 155 6 005 12 531 2 411

actual dimension of the building and the average measurement dimension of the 3 algorithms. Table 1 shows that the dimension measured by STOSLAM is closer to the actual value, which means that the accuracy of STOSLAM is higher than that of the other two algorithms in practical applications. In order to make the reader more clearly understand the actual performance of these three algorithms, we drew Fig. 19 based on the measurement data. From Fig. 19, we observe that the RMSE and average error of the STSOSLAM algorithm are the lowest. The RMSE and error of CDFastSLAM are second, and FastSLAM2.0 has the maximum

124

J. Dai, X. Li, K. Wang et al. / Robotics and Autonomous Systems 116 (2019) 114–125

Fig. 18. Office environment plan.

Acknowledgments This work is financially supported by the State Key Research Development Program of China (Grant No. 2016YFC0801404), the National Science and Technology Major Project of China (Grant No. 2016ZX05043005), the National Natural Science Foundation of China (51674050), and the National Key R&D Program of China (Grant No. 2018YFC0808003), which are gratefully acknowledged. References

Fig. 19. Comparison of RMSE and average error for the three algorithms.

RMSE and average error. The STSOSLAM obviously has the highest measurement consistency and mapping accuracy. This also corroborates the results from the previous virtual simulation. 6. Conclusions In order to improve the accuracy and robustness of the SLAM algorithm, we propose a STSOSLAM algorithm. The new algorithm is based on the frame of FastSLAM 2.0 and innovatively adopts a second order central difference filter. Firstly, without calculating the Jacobian matrix, the second order central difference filter was employed to realize the algorithm. It does not require that the nonlinear function be continuously differentiable, and overcomes the limitation of EKF theory effectively. In addition, in order to keep the stability and robustness of the algorithm, we introduce the strong tracking factor, which updates every sigma point by using an adaptive algorithm. It gets optimized filter gain through the STF online adjustment factors, and suppresses the uncertain noise and the influence of initial value selection for the state estimation for the purpose of increasing the system state estimation accuracy. The new algorithm increases the robustness and is better suited to complex road environments, especially when the wheels slide due to uneven ground. The simulation and experiment results show that compared to FastSLAM2.0 and CDFastSLAM, the proposed algorithm has higher estimation precision and lower RMSE.

[1] R. Smith, M. Self, P. Cheeseman, Estimating uncertain spatialrelationships in robotics, in: Autonomous Robot Vehicles, NewYork, NY, USA, Springer-Verlag, 1990, pp. 167–193. [2] S. Thrun, Y. Liu, D. Koller, et al., Simultaneous localization and mapping with sparse extended information filters, Int. J. Robot. Res. 23 (7–8) (2010) 693–716. [3] R. Mur-Artal, J.M.M. Montiel, J.D. Tardós, ORB-SLAM: A versatile and accurate monocular SLAM system, IEEE Trans. Robot. 31 (5) (2015) 1147–1163. [4] J. Engel, T. Schöps, D. Cremers, LSD-SLAM: Large-Scale Direct Monocular SLAM. 8690 (2014) 834-849. [5] C. Forster, M. Pizzoli, D. Scaramuzza, SVO: Fast semi-direct monocular visual odometry, in: IEEE International Conference on Robotics and Automation, IEEE, 2014, pp. 15–22. [6] M. Montemerlo, S. Thrun, D. Koller, et al., Fastslam: a factored solution to the simultaneous localization and mapping problem, in: Eighteenth national conference on Artificial intelligence. Vol. 2002, American Association for Artificial Intelligence, 2004. [7] M. Montemerlo, S. Thrun, D. Roller, et al., Fastslam 2.0: an improved particle filtering algorithm for simultaneous localization and mapping that provably converges, in: International Joint Conference on Artificial Intelligence, Morgan Kaufmann Publishers Inc, 2003, pp. 1151–1156. [8] X. Yuan, J.F. Martínezortega, F. Jas, et al., AEKF-SLAM: A New algorithm for robotic underwater navigation:, Sensors 17 (5) (2017). [9] H. Deusch, S. Reuter, K. Dietmayer, The labeled multi-bernoulli SLAM filter, IEEE Signal Process. Lett. 22 (10) (2015) 1561–1565. [10] T. Lv, M. Feng, A FastSLAM 2.0 algorithm based on FC & ASD-PSO, Robotica 35 (9) (2016) 1–21. [11] Q.L. Li, Y. Song, Z.G. Hou, Neural network based FastSLAM for autonomous robots in unknown environments, Neurocomputing 165 (C) (2015) 99–110. [12] J. Cao, B. Zeng, J. Liu, et al., A novel relocation method for simultaneous localization and mapping based on deep learning algorithm, Comput. Electr. Eng. (2017). [13] J. Choi, M. Choi, K.C. Wan, et al., Data association using relative compatibility of multiple observations for EKF-SLAM, Intell. Serv. Robot. 9 (3) (2016) 177–185. [14] L. Seung-Hwan, E. Gyuho, B.H. Lee, Relational FastSLAM: an improved Rao-Blackwellized particle filtering framework using particle swarm characteristics, Robotica 34 (6) (2016) 1282–1296. [15] R. Havangi, Intelligent FastSLAM: An intelligent factorized solution to simultaneous localization and mapping, Int. J. Humanoid Robot. (2017) 1650026. [16] M. Balcılar, S. Yavuz, M.F. Amasyalı, et al., R-SLAM: Resilient localization and mapping in challenging environments, Robot. Auton. Syst. 87 (2017) 66–80. [17] M. Nørgaard, N.K. Poulsen, O. Ravn, Advances in Derivative-Free State Estimation for Nonlinear Systems. 1998.

J. Dai, X. Li, K. Wang et al. / Robotics and Autonomous Systems 116 (2019) 114–125

125

Jiahui Dai received his B.S. degree from Chengdu university of Technology, Chengdu, China in 2007 and his M.S. degree in Royal Melbourne Institute of Technology University, Melbourne, Australia in 2010. Now, he is studying at Chongqing University in his Ph.D. degree. His research interests include SLAM, mobile robot and rescue robot.

[18] K. Ito, Gaussian filter for nonlinear filtering problems, in: Decision and Control, 2000. Proceedings of the, IEEE Conference on. vol.2, IEEE, 2000, pp. 1218–1223. [19] C.E. Forberg, Introduction to Numerical Analysis, second ed., AddisonWesley, Reading, 1972. [20] T.S. Schei, A Finite-Difference Method for Linearization in Nonlinear Estimation Algorithms, Pergamon Press, Inc, 1997. [21] Magnus Nørgaard, Niels K. Poulsen, Ravn, ole new developments in state estimation for nonlinear systems, Autom. J. IFAC 36 (11) (2000) 1627–1638. [22] S. Julier, J.K. Uhlmann, A General Method for Approximating Nonlinear Transformations of Probability Distributions. 1996. [23] T.S. Schei, A finite-difference method for linearization in nonlinear estimation algorithms, Automatic 33 (11) (1997) 2053–2058. [24] S. Julier, J. Uhlmann, H.F. Durrantwhyte, A new method for nonlinear transformation of means and covariances in filters and estimates, IEEE Trans.on Automat. contr 45 (3) (2000) 477–482. [25] D.H. Zhou, Q.L. Wang, Strong tracking filtering of nonlinear systems with colored noise, J. Beijing Inst. Technol. 17 (3) (1997) 321–326. [26] Zhou Hongren, Jin Zhongliang, Wang Peide, Tracking of Maneuvering Targets, Beijing: National Defense Industry Press, 1991. [27] Shi Yong, Han Chongzhao, Shi yong han chongzhao particle filter using second-order central difference, J. Xi’An Jiaotong Univ. 42 (4) (2008) 409–413. [28] P.S. MayBeck, Stochastic Models, Estimation and Control, New York: Academic Press, 1979. [29] Zhao Lin, Nonlinear System Filtering Theory, Beijing: National Defense Industry Press, 2012, pp. 124–127. [30] Zhao Xiaobing, Implementation of Unscented FastSLAM algorithm for mobile robot based on vision, Harbin Institute of Technology, 2010. [31] YAN De-li, SONG Yong-duan, SONG Yu, KANG Yi-fei, The application of square-root cubature kalman filter and probability hypothesis density in simultaneous localization and mapping for mobile robots, Control Theory Appl. 31 (08) (2014) 1008–1016. [32] Merwe R. van der, Sigma-Point Kalman Filters for Probabilistic Inference in Dynamic State-Space Models, (ph. D.dissertation), Oregon Health and Science University, USA, 2004. [33] C. Kim, R. Sakthivel, W.K. Chung, Unscented FastSLAM: a robust and efficient solution to the SLAM problem, IEEE Trans. Robot. 24 (4) (2008) 808–820. [34] X.M. Dai, L. Lang, M.Y. Chen, Strong tracking square-root cubature kalman filter based on SLAM algorithm, J. Electron. Meas. Instrum. 29 (10) (2015) 1493–1499. [35] TIAN Xiang, ZHANG Liang, CHEN Yao-wu, Fast SLAM algorithm based on central difference Kalman filter, J. Harbin Inst. Technol. 42 (09) (2010) 1454–1461.

Xiaobo Li received his B.S. degree from Xi’an Jiao Tong University, Xi an, China in 1999 , his M.S. degree in Zhejiang University, Hangzhou, China in 2002 and his the Ph.D. degree in Florida State University, Tallahassee, USA in 2009. 2010, he was postdoctoral researcher in Florida Agricultural & Mechanical University, Tallahassee. Now he heads the Robotics Institute in China Coal Technology Engineering Group Chongqing Research Institute. His research interests include SLAM, robot control and rescue robot.

Kequan Wang received his B.S. degree from Chongqing university, Chongqing, China in 1983 and his M.S. degree in China Coal Science Research Institute, Beijing, in 1986. He is currently the general manager of China Coal Technology Engineering Group Chongqing Research Institute. His research interests include coal mine safety and rescue robot.

Yunpei Liang received his B.S. degree from Chongqing university, Chongqing, China in 1993, his M.S. degree in China Coal Science Research Institute, Beijing in 1999 and his the Ph.D. degree in Shandong University of Science and Technology in 2007. 2008, he was postdoctoral researcher in Commonwealth Scientific and Industrial Research Organization, Canberra. He is currently a professor and doctoral supervisor in College of Resources and Environmental Sciences, Chongqing University. His current research interests include rescue robot, common extraction technology of coal and gas and coal mine safety.