For improving the localization accuracy, a multi-interval extended finite impulse response (EFIR)-based Rauch-Tung-Striebel (R-T-S) smoother is proposed for the range-only ultra wide band (UWB) simultaneous localization and mapping (SLAM) for robot localization. In this mode, the EFIR R-T-S (ERTS) smoother employs EFIR filter as the forward filter and the R-T-S smoothing method to smooth the EFIR filter's output. When the east or the north position is considered as stance, the ERTS is used to smooth the position directly. Moreover, the estimation of the UWB Reference Nodes’ (RNs’) position is smoothed by the R-T-S smooth method in parallel. The test illustrates that the proposed multi-interval ERTS smoothing for range-only UWB SLAM is able to provide accurate estimation. Compared with the EFIR filter, the proposed method improves the localization accuracy by about 25.35% and 40.66% in east and north directions, respectively.
Robot localizationultra wide bandRauch-Tung-Striebel smootherextended FIR filterIntroduction
In recent years, the mobile robots have been widely used in the medical field [1,2]. Especially affected by the COVID-19 more and more jobs in hospitals require mobile robots to replace doctors, which provides the demand for indoor localization of mobile robots in indoor environment. However, there are challenges in the research of mobile robot localization for indoor environment [3,4].
To the mobile robot localization, the global navigation satellite system (GNSS) has been proposed and widely used [5,6]. For instance, in [7], the global positioning system (GPS)-based approach has been widely used in the small mobile robot. the GPS has been used to assist the simultaneous localization and mapping (SLAM) for mobile robot [8]. In [9], the GPS/BeiDou integrated navigation satellite system (BDS) has been proposed. It should be emphasized that although localization method based on the GNSS can provide the position information, this method still have shortcomings:
The localization accuracy is meter-lever, which is not suitable for indoor localization.
In the indoor environment, the GNSS signal may be outage.
In order to overcome these shortcomings, there are many attempts. For instance, some existing research proposes to use radio frequency identification based scheme (RFID) which has been investigated for indoor robot localization [10,11]. Moreover, the RFID is also been used in moving object localization [12]. A new method of RFID system has been developed for localizing mobile robot employing configures reader (antenna) [13]. In [14], the WiFi has been used in mobile robot positioning. Noted that both the RFID and the WiFi are able to provide the position information in indoor environment, the localization accuracy of the RFID and the WiFi is not suitable for the high precision indoor mobile robot localization [15]. In order to provide more accurate localization information, the localization scheme employing ultra wide band (UWB) has been proposed [16]. For example, in [17] and [16], the UWB is used to measure the mobile robot's position. It should be emphasized that the localization scheme using UWB improves the positioning accuracy.
Based on the localization method, the data fusion method is investigated. For example, the Kalman filter (KF) is used to fuse the data of the laser range finder (LRF) sensor for the mobile robot [18,19]. In order to face the nonlinear system, the extended KF (EKF) filter is proposed for the indoor localization of a mini unmanned air vehicle (UAV) based on low-cost inertial measurement unit (IMU)/UWB/Vision [20]. The improved cubature Kalman filter for GNSS/INS using transformation of posterior sigma-points error is investigated [21]. In [22], the vehicular INS/GPS-integration system based on the unscented KF UKF is derived. The cubature KF is designed for GNSS/INS under GNSS-challenged environment in [23]. One can infer that the Kalman filter is sensitive to the noise initial value and noise statistics, which is hard to obtain in real-time localization [18]. In order to improve the robustness of the localization, the unbiased finite impulse response (UFIR) filter is proposed [24]. For example, in [25], an adaptive-horizon iterative UFIR filter is derived. the FIR filter is employed for the RFID localization [26]. Moreover, in order to improve the localization accuracy, the Rauch-Tung-Striebel (R-T-S) smoother is proposed. for example, in [27], the ensemble Kalman smoother is designed for indoor mobile robot.
For improving the localization accuracy, this work proposes a multi-interval extended finite impulse response (EFIR) R-T-S smoother for the indoor robot's range-only ultra wide band (UWB) simultaneous localization and mapping (SLAM), which employs EFIR filter as the forward filter and the R-T-S smoothing method to smooth the EFIR filter's estimation of the east position, north position, and the UWB Reference Node (RN)'s position, respectively.
The contribution of this work resides in the following:
A range-only UWB SLAM scheme is designed for indoor robot.
A new ERTS smoothing algorithm employs the EFIR filter as the forward filtering and the R-T-S smoothing as the backward smoothing method is proposed.
A new smoothing scheme is proposed for the estimation of the east position, north position, and the UWB RN's position, respectively.
The Data Fusion Model for Range-Only UWB SLAM for Robot Localization
In this section, the robot localization scheme for fusing the range-only UWB measurements will be designed firstly. Then, the data fusion model based on the range-only UWB SLAM scheme will be investigated.
The Scheme for Fusing the Range-Only UWB Measurements
Fig. 1 displays the structure of the scheme for fusing the range-only UWB measurements. In this work, the UWB localization system includes 4 reference nodes (RNs) and one blind node (BN). The RNs are pre-positioned on static coordinates, and the BN is fixed on the robot. When the UWB localization system works, BN is used to collect the ranges diU,i∈[1,4] from i^{th} RN to BN. Then, the ranges diU,i∈[1,4] are input to the multi-interval EFIR Rauch-Tung-Striebel (ERTS) smoother, which used as the data fusion filter in this work. Finally, the multi-interval ERTSS provides the mobile robot's position P^o.
Range-only UWB measurements fusion structureThe Localization Model for the Data Fusion Filter
In this subsection, the localization model for the data fusion filter for the model mentioned in Section 2.1 will be investigated. The state equation can be written as Eq. (1).
[X^l|l−1RPo^l|l−1RN]⏟X^l|l−1=[A4×4R04×808×4I8×8]⏟AX^l−1+wl−1,
where the time index is expressed in l, wl∼N(0,Q) means the noise in system, the vector of the UWB RNs’ position is shown as the follows:
PolRN=[x1,y1,x2,y2,…,x4,y4]T,
where (xi,yi),i∈[1,4] denotes the i^{th} UWB RN's position, the vector of the mobile robot is shown as the follows:
X^lR=[xlylVxlVyl],
where the items x_{l}, y_{l},Vx_{l},Vy_{l} is the robot's position in east directions, the robot's position in north, the robot's velocity in east and the robot's velocity in north directions at the index l for time, the A4×4R in Eq. (1) is listed in Eq. (4).
A4×4R=[10Δl0010Δl00100001],
where the Δl is the sampling interval.
The measurement equation for the data fusion filter is listed in Eq. (5).
[d1,lUd2,lUd3,lUd4,lU]⏟Yl=[(xl−x1)2+(yl−y1)2(xl−x2)2+(yl−y2)2(xl−x3)2+(yl−y3)2(xl−x4)2+(yl−y4)2]⏟h(X^l|l−1)+vl,
where the di,lU,i∈[1,4] is range from the ith UWB RN to the mobile robot at the time index l, vl∼N(0,R) is the measurement noise.
The Multi-Interval EFIR Rauch-Tung-Striebel Smoother
In this section, the multi-interval ERTS smoother will be designed based on the models (1) and (5). The multi-interval ERTS smoother consists of forward filtering and backward smoothing, consequently, we will design the UFIR filter and the R-T-S smoother based on the models (1) and (5), secondly, the online multi-interval R-T-S smoothing scheme will be investigated.
EFIR Filter Algorithm
In the multi-interval ERTS smoother, the EFIR filter is employed as the forward filtering algorithm of the EFIR R-T-S smoother. The EFIR filter algorithm for the models (1) and (5) is sketched in Algorithm 1. We can infer that the performance of the EFIR filter depends on the size M^{E} of the X^l and the filtering window size N^{E}. From the models (1) and (5), we can get that the M^{E }=^{ }4 + 8 = 12, and thus, the performance of the EFIR filter for the models (1) and (5) depends on the N^{E}, which should be set optimally. Moreover, we employ the extend Kalman filter (EKF) (lines 4–8) as the data fusion filter when the EFIR filter does not work in dead zone.
R-T-S Smoothing Algorithm
When the forward EFIR filter works, the X^l and P^l,l∈[1,L] should be stored. Here, the L means the last time index of the forward filtering. The R-T-S smoothing algorithm can smooth the outputs of the EFIR filter from the time index L to the time index L_{b} in reverse order, here, the ‘b’ means the last time index for the smoothing algorithm. Employing the X^l and P^l,l∈[Lb,L], the pseudo code of the R-T-S smoothing algorithm based on the models (1) and (5) can be listed in Algorithm 2.
In the R-T-S smoothing algorithm, we set the X^lS=X^l,l∈[Lb,L] and P^lS=P^l,l∈[Lb,L], the Pl−1|lS can be computed as the Eq. (6).
Pl−1|lS=APlSAT+Q,
where superscript ‘S’ means the smoother. The smoothing gain K_{l} can be computed as the Eq. (7).
Kl−1S=Pl−1SAT(Pl−1|lS)−1,
Then, Eqs. (8) and (9) calculate X^lS (smoothed state estimation) and P^lS (smoothed error cov.).
X^l−1S=X^l−1S+Kl−1S(X^lS−AX^l−1S),P^l−1S=Pl−1S+Kl−1S(P^lS−Pl−1|lS)(Kl−1S)T,
Multi-Interval Smoothing Scheme
From the Algorithm 2, it can be seen easily that the R-T-S smoothing using the X^l and P^l,l∈[1,L] is an off-line mode. In this work, to improve accuracy of filtering and the real-time performance, a multi-interval smoothing scheme is proposed for the east position, north position, and the RN's position respectively. The structure of the multi-interval R-T-S smoothing algorithm is shown in Fig. 2. Based on the models (1) and (5), the east position x_{l}, north position y_{l}, and the RN's position (x_{i}, y_{i}), the smoothing schemes are listed in the follows:
• To the east and the north position (x_{l}, y_{l}), we firstly set one threshold door for the position change rate in the east direction (Δ x_{l} = x_{l} − x_{l − 1}) and north (Δ y_{l} = y_{l} − y_{l − 1}) direction.
• When the Δ x_{l} ≤ door, the east position of the robot is considered as the stance, and we set L_{x} = l. If it is the first time that ΔxlΔxl−1≥door change to Δx_{l} ≤ door, we set L_{bx} = l. The R-T-S smoothing algorithm will be work for smoothing the EFIR filter outputs X^l and P^l from the time index L_{x} to the time index L_{bx}. Since the robot's east position is stance, we can compute the x_{l} using the Eq. (10). When the Δx > door, the EFIR filter's output x_{l} is selected as the output without smoothing.
When the Δ y_{l} ≤ door, similar to the previous step, the robot's north position is considered as the stance, we set L_{y} = l. If it is the first time that Δy_{ l − 1} > door change to Δ y_{l} ≤ door, we set L_{by} = l. The R-T-S smoothing algorithm will be work for smoothing the EFIR filter outputs X^l and P^l from the time index L_{y} to the time index L_{by}. Finally, the y_{l} can be computed by the Eq. (11). When the Δy > door, the y_{l} is output without smoothing.
yl=mean(X^lyS(2,1),…,X^lS(2,1)),
To the UWB RN's position (xi,yi),i∈[1,4], since the UWB RNs are always stance, thus, we can smooth the EFIR's outputs using the R-T-S smoothing method from the time index l to time index 1 directly, Then, the (xi,yi),i∈[1,4] can be computed by the Eqs. (12) and (13).
xi=mean(X^1S(j,1),…,X^lS(j,1))i=1,2,3,4,j=5,7,9,11,yi=mean(X^1S(j,1),…,X^lS(j,1))i=1,2,3,4,j=6,8,10,12,
The multi-interval EFIR R-T-S smoothing for the Eqs. (1) and (5) can be listed in Algorithm 3.
Experimental Verification
The following part describes experiments for performance verification of the method using real-world data. First, the experiment configurations are introduced. Second, the results are investigated and performance is analyzed.
The Setting of the Test
In this subsection, the experiment configurations are introduced. The real test is done inside the 7th building of University of Jinan, which can be seen in Fig. 3. This work employs the UWB localization system, mobile robot, and the computer to test. The UWB localization system employs the DW−1000 based localization system, which includes one UWB BN (fixed on the mobile robot) and four UWB RNs (fixed on the selected positions so that the positions of RNs are stance), The BN can measure the ranges diU,i∈[1,4] from the UWB RNs to the UWB BN fixed on the mobile robot. Then, the diU,i∈[1,4] can be sent to the computer via RS232. The mobile robot with the UWB BN is shown in Fig. 4. We set Δl =0.5 s in this work. It should be emphasized that the positioning accuracy of the DW−1000 based localization system used in this work under weak occlusion is about 0.20 m, thus, we set the door =0.20/5 = 0.04 in Algorithm 3, which means that when the estimated displacement of the mobile robot's position in a period is less than 0.04 m, we consider that the robot is stance in this direction. The X_{0}, P_{0}, Q, and R can be set as the follows:
P0=I12×12,Q=I12×12,R=I4×4,X0=[0,0,0,0,−0.85,7.21+0.15,4.8+0.1,7.21,−0.85+0.15,−2.04−0.05,4.8−0.15,−2.04]T,
where ‘+’ and ‘-’ mean the errors of the UWB RN's position. For example, ‘7.21 + 0.15’ means the the errors of the UWB RN's position is 0.15 m.
The test environment in the 7th buildingThe mobile robot with the UWB BN
From the models (1) and (5) (through EFIR filter), we can get that the M^{E }=^{ }12. And the optimal N^{E} can be found by the follows:
NoptE=argminNE{tr(Pl(NE))},
where
Pl=E{(Xl−X^l)(Xl−X^l)T},
The effect of N^{E}N on the localization error using a set of independent data with 200 sampling points is sketched in Fig. 5. From the figure, one can see that the NoptE=13, thus we set N^{E }=^{ }13 in this work.
The Performance of the Multi-Interval ERTS Smoother for the Trajectory Estimation
In this section, the performances of the UWB, EFIR, and the multi-interval ERTS smoother will be investigated. Fig. 6 shows the trajectories measured by the UWB, EFIR, and the multi-interval ERTS smoother. Fig. 7 shows errors in the east and north position and as we can see, both the EFIR filter and the multi-interval ERTS smoother can reduce the localization errors when being compared to the UWB. Also, the proposed multi-interval ERTS smoother's path is much closer to the ground truth path. Table 1 shows the root mean square errors (RMSEs) given by UWB, EFIR, and multi-interval ERTS smoother. We can see that the proposed multi-interval ERTS smoother has the lowest position error. East- and north-direction positioning errors of the multi-interval ERTS smoother are 0.1175 and 0.0932 m, which improved by 25.36% and 36.92% respectively compared with the EFIR filter. It should be pointed out that the EFIR filter is unsuitable for the estimation of the north position due to the uncertain beacons. Fig. 8 shows the displacements in directions east (Δ x_{l}) and north (Δ y_{l}). Fig. 8 also shows the east- and north-directioned judgment for stance.
The effect of <italic>N<sup>E</sup></italic>N on the localization errorThe trajectories measured by the UWB, EFIR, multi-interval ERTS smoother, and the planned pathThe localization error. Directions: (a) East (b) NorthRMSEs comparison of different methods
Algorithm
RMSE (m)
E
N
UWB
0.2168
0.1318
EFIR
0.1633
0.1509
Multi-interval ERTS smoother
0.1219
0.0900
The localization error. Directions: (a) East (b) NorthThe Performance of the Multi-Interval ERTS Smoother for the Estimation of the UWB RNs’ Position
In this section, the performances of the EFIR and the multi-interval ERTS smoother for the estimation of the UWB RNs’ position will be investigated. Fig. 9 shows the estimation of the UWB RNs’ position using the EFIR and the multi-interval ERTS smoother. The RMSEs produced by the EFIR and the multi-interval ERTS smoother for UWB RN #1, #2, #3, and #4 are sketched in the Tables 2–5. From the figure and the tables, we can see obviously that the multi-interval ERTS smoother is effectively to estimate the UWB RN's position. Compared with the EFIR filter, the proposed multi-interval ERTS smoother improved by the 83.51%, 81.61%, 57.44%, 37.73%, 23.92%, 75.36%, 53.75%, and 62.37%.
The estimation of the UWB RN’s position using the FIR and multi-interval ERTS smootherEFIR and the multi-interval ERTS smoother RMSEs of the 1st UWB RN
Algorithm
RMSE (m)
E
Improvement
N
Improvement
EFIR
0.1285
–
0.2819
–
Multi-interval
ERTS smoother
0.0212
83.51%
0.0518
81.61%
EFIR and the multi-interval ERTS smoother RMSEs of the 2nd UWB RN
Algorithm
RMSE (m)
E
Improvement
N
Improvement
EFIR
0.1351
–
0.1213
–
Multi-interval
ERTS smoother
0.0574
57.44%
0.0755
37.73%
EFIR and the multi-interval ERTS smoother RMSEs of the 3rd UWB RN
Algorithm
RMSE (m)
E
Improvement
N
Improvement
EFIR
0.2104
–
0.1738
–
Multi-interval
ERTS smoother
0.1601
23.92%
0.0428
75.36%
RMSEs produced by the EFIR and the multi-interval ERTS smoother for UWB RN #4
Method
RMSE (m)
E
Improvement
N
Improvement
EFIR
0.2058
–
0.1149
–
Multi-interval
ERTS smoother
0.0952
53.75%
0.0433
62.37%
Conclusion
Range-only UWB SLAM for robot localization employing multi-interval EFIR R-T-S smoother has been proposed in this work. The contribution of this work resides in the following:
An indoor robot's localization scheme is designed for fusing the range-only UWB measurement.
A new EFIR R-T-S smoothing algorithm using the EFIR filter and the R-T-S smoothing for forward filtering and the backward smoothing method is designed.
A new smoothing scheme is proposed for the estimation of the east position, north position, and the UWB RN's position, respectively.
An experiment using real-world data is conducted and verifies that the proposed multi-interval EFIR R-T-S smoothing algorithm improves the performance.
Funding Statement: The authors received no specific funding for this study.
Conflicts of Interest: The authors declare that they have no conflicts of interest to report regarding the present study.
ReferencesXu, Y., Shmaliy, Y. S., Ma, W., Jiang, X., Guo, H. (2021). Improving tightly LiDAR/Compass/Encoder-integrated mobile robot localization with uncertain sampling period utilizing EFIR filter. Rafflin, C., Fournier, A. (1996). Learning with a friendly interactive robot for service tasks in hospital environments. Zhuang, Y., Hua, L., Wang, Q., Cao, Y., Thompson, J. S. (2019). Visible light positioning and navigation using noise measurement and mitigation. Xu, Y., Shmaliy, Y. S., Ahn, C. K., Shen, T., Zhuang, Y. (2021). Tightly-coupled integration of INS and UWB using fixed-lag extended UFIR smoothing for quadrotor localization. El-Sheimy, N., Li, Y. (2021). Indoor navigation: State of the art and future trends. Shi, T., Zhuang, X., Xie, L. (2021). Performance evaluation of multi-GNSSs navigation in super synchronous transfer orbit and geostationary earth orbit. Rodina, J. (2013). Localization of small mobile robot by low-cost GPS receiver. Liu, R., Zhang, J., Chen, S., Yang, T., Arth, C. (2021). Accurate real-time visual SLAM combining building models and GPS for mobile robot. Liu, P., Qin, H., Cong, L. (2019). The unified form of code biases and positioning performance analysis in global positioning system (GPS)/BeiDou navigation satellite system (BDS) precise point positioning using real triple-frequency data. Choi, B. S., Lee, J. W., Lee, J. J., Park, K. T. (2011). A hierarchical algorithm for indoor mobile robot localization using RFID sensor fusion. Al-Jarrah, M. A., Al-Dweik, A., Alsusa, E., Damiani, E. (2019). RFID reader localization using hard decisions with error concealment. He, J., Liu, R., Xiao, Y., Liang, G., Fu, Y. (2018). Moving object localization by fusing RFID phase difference and laser scanning. Luo, R. C., Chuang, C. T., Huang, S. S. (2007). RFID-based indoor antenna localization system using passive tag and variable RF-attenuation. Canedo-Rodriguez, A., Rodriguez, J. M., Alvarez-Santos, V., Iglesias, R., Regueiro, C. V. (2015). Mobile robot positioning with 433-MHz wireless motes with varying transmission powers and a particle filter. Decarli, N., Guidi, F., Dardari, D. (2016). Passive UWB RFID for tag localization: Architectures and design. Xu, Y., Shmaliy, Y. S., Li, Y., Chen, X. (2017). UWB-Based indoor human localization with time-delayed data using EFIR filtering. Xu, Y., Shmaliy, Y. S., Ki, A. C., Tian, G., Chen, X. (2018). Robust and accurate UWB-based indoor robot localisation using integrated EKF/EFIR filtering. Zhao, S., Huang, B. (2020). Trial-and-error or avoiding a guess? Initialization of the Kalman filter. Tesli, L., Krjanc, I., Klan Ar, G. (2010). Using a LRF sensor in the kalman-filtering-based localization of a mobile robot. Benini, A., Mancini, A., Longhi, S. (2013). An IMU/UWB/Vision-based extended kalman filter for mini-UAV localization in indoor environment using 802.15.4a wireless sensor network. Cui, B., Chen, X., Xinhua, T. (2017). Improved cubature Kalman filter for GNSS/INS based on transformation of posterior sigma-points error. Hu, G., Gao, B., Zhong, Y., Gu, C. (2020). Unscented Kalman filter with process noise covariance estimation for vehicular INS/GPS integration system. Cui, B., Wei, X., Chen, X., Li, J., Li, L. (2019). On sigma-point update of cubature Kalman filter for GNSS/INS under GNSS-challenged environment. Zhao, S., Shmaliy, Y. S., Liu, F. (2016). Fast Kalman-like optimal unbiased FIR filtering with applications. Zhao, S., Shmaliy, Y. S., Ahn, C. K., Liu, F. (2018). Adaptive-horizon iterative UFIR filtering algorithm with applications. Pomarico-Franquiz, J. J., Shmaliy, Y. S. (2014). Accurate self-localization in RFID tag information grids using FIR filtering. Zhuang, Y., Wang, Q., Shi, M., Cao, P., Qi, L.et al. (2019). Low-power centimeter-level localization for indoor mobile robots based on ensemble Kalman smoother using received signal strength.