Robot indoor location modeling and simulation based on Kalman filtering

Wireless signal fingerprint positioning technology has been widely used in indoor positioning. In view of the influence of a large number of interference noise in indoor, the error of receive signal strength indicator is large, the more complex and chaotic indoor environment, the location accuracy deviation of the system will be very large; an algorithm based on Kalman filter is proposed to filter the velocity and direction of motion of indoor robots. The position coordinates of the robot are estimated by RSSI-based positioning method, and the indoor robot positioning model and Kalman filter model are established. Kalman filter autoregressive algorithm is used to optimize the estimated position coordinates of the robot. Mathematical reasoning and simulation results show that the probability of positioning error is 80% when Kalman filter is not used, and the location error is controlled within 1.2 m after Kalman filter, which effectively improves the location accuracy of indoor robots.

China’s demographic dividend is fading, the trend of machine replacement is accelerating, and mobile robots promote the advent of the era of intellectualization [1]. With the decline of the birth rate and the increase of the proportion of the elderly population in China, the demographic dividend has gradually disappeared, “labor shortage” is frequent, and the cost of labor has risen. On the other hand, with the improvement of living standards, workers have higher requirements for the quality of work, and their willingness to engage in indoor boring, harsh environment and dangerous work is reduced. Robots can work continuously for a long time, with higher efficiency and accuracy than human beings, and the pace of “machine replacement” is accelerated. Obviously, the robot market will flourish day by day, but the difficulty of locating mobile robots in an indoor environment is a problem. Satellite positioning can be used and the following are mainly commercial and some of the most successful: American Global Positioning System [2, 3], Russia’s GLONASS satellite positioning system, Europe’s Galileo satellite positioning system, and Chinese BeiDou Navigation Satellite System [4, 5] in our country. GPS is used to locate the personnel, but the indoor GPS signal cannot be used properly due to the occlusion of the building, the interference of the reinforced concrete to the signal, and the low positioning accuracy. Many scientific research institutions and scholars at home and abroad have been making unremitting efforts in this regard.

The application of radio frequency identification [6, 7] technology to intelligent robots is also developing such as robots for monitoring kindergarten children, vacuum cleaner robots, and shopping guide robots. In the process of motion, the robot has the problem of self-localization. Airborne sensors are mostly used for localization. RFID [8] technology is a technology which uses an electromagnetic induction principle to generate information reading by wireless excitation. Radio frequency identification ranges from a few centimeters to a dozen meters. When RFID [9] is used for robot localization, mainly for personnel whether exists in the identification of a region, it cannot achieve real-time tracking, and the standard network system of location application is not yet mature. Ultra-wideband [10] is a non-carrier communication technology; by laying a certain number of UWB positioning base stations indoors, the robot carries positioning labels and finally realizes the precise positioning and navigation of the robot. Base stations are usually set up as square according to a need, and one is laid every 50–200 m. The principle is to ensure that the pulses emitted by the target to be located can be received by any three base stations at the same time, so as to determine the location of the location tag. On the other hand, its technology could realize cm-level positioning level, but positioning the cost is very high, suitable for locating the area which is small, has high value-added industries and the field of plant at big millions of investment, and to the person’s cm-level positioning, it is a little bit “big small”.

Kalman filter is widely used in mobile robot localization [11, 12]. In view of the unstable transmission and poor positioning accuracy of indoor wireless sensor network communication, an autonomous dynamic positioning system for mobile robots is proposed, by real-time selection of adjacent beacon nodes, determination of boundaries, and drawing of local grid space; the dynamic positioning of the robot is realized [13, 14]. Ranging is achieved by using received signal strength index. Then, the improved approximate triangle interior point test (APIT) algorithm based on ranging is used to complete the positioning, and the Kalman algorithm is used to correct the positioning error. This method is suitable for the actual situation of unstable transmission in an indoor network [15, 16]. Kalman filter is used to obtain the optimal data, so that the indoor robot positioning has better accuracy and adaptability [17, 18]. In order to solve the problem that the positioning accuracy of mobile robots decreases sharply due to the sensor measurement error and the pose error caused by the robot model in the positioning process, a filtering algorithm is proposed [19, 20]. Based on the standard Kalman filter, when the sensor measurement error exists, the positioning accuracy is improved by adjusting the size of the state covariance matrix to resist the filtering divergence caused by the pose error.

In this text, the Kalman filtering algorithm is put forward to select the appropriate state error matrix and observation error array and optimize the positioning accuracy, so as to improve the location performance of the robot room. Mathematical reasoning and simulation results show that the probability of positioning error is 80% when Kalman filter is not used, and the positioning error is controlled within 1.2 m after Kalman filter, which effectively improves the positioning effect of indoor robots.

Suppose the equation of the stochastic linear discrete system (without considering the control action first) is

In the formula, X(k) is the n-dimensional state vector of the system, and W(k) is the process noise vector of the p-dimensional system. Z(k) is the m-dimensional observation vector of the system, and V(k) is the m-dimensional observation noise vector. Φ is the n × n-dimensional transfer matrix of the system, Γ is n × p-dimensional noise input matrix, and H is the m × n-dimensional observation matrix.

The statistic characteristics of system observation noise and process noise are assumed as below:

Q(k) is the p by p dimension symmetric non-negative definite variance matrix of system process noise W(k), and R(k) is the m by m dimension symmetric positive definite variance matrix of system observation noise V(k), and δ_kj is the function Kronecker-δ.

The calculation process of Kalman filtering of the above discrete system is as follows:

The calculation state is further predicted:

State estimation:

Filter gain matrix:

One-step prediction error variance matrix:

Estimation error variance matrix:

The above five equations are the fundamental equations of Kalman filtering for stochastic linear discrete systems [9]. In a filtering cycle, from the Kalman filtering in the use of information and observation of order, the Kalman filtering has two obvious information update processes: time update observation and process update process. In this calculation, only information related to the dynamic characteristics of the system is used, such as state-step transfer matrix, noise input array, and process noise variance matrix. As shown in Fig. 1, the Kalman filtering algorithm has two computing loops: gain calculation loop and filter calculation loop. The filter calculation loop relies on the gain calculation loop, and the gain calculation loop is calculated independently.

The chart of Kalman filtering algorithm. Given the initial value, the state estimation at k time can be obtained by recursive calculation according to the observed value Z (k) at K time

Full size image

2.1 Kalman filtering

Kalman filtering is a digital signal filter based on Kalman filtering. The observed value of the system state is the input of the filter, and the estimated value of the system state is the output. The flow diagram of Kalman filtering is shown in Fig. 2.

Structure diagram of Kalman filtering of random linear discrete system

Full size image

Mobile robot localization is to ensure the mobile robot working in a two-dimensional working condition relative to the global coordinates and its attitude, is the use of a priori map information environment, the robot position of the current estimated and observed value of the sensor input information, through certain processing and transformation, to produce more accurate estimate of the current robot position. For most mobile robot applications, the system is nonlinear. So we need to apply the Kalman filtering after linearization.

2.2 Kalman filtering indoor location algorithm

2.2.1 System schematic diagram

System schematic diagram is shown in Fig. 3.

System schematic diagram

Full size image

2.2.2 Building an indoor positioning model

In this paper, RSSI [10] values of locating nodes are collected by various reference nodes, and the mathematical model of RSSI value and distance is built on the basis of the particular relation between RSSI measured value and distance value:

In Eq. (9), n is the signal propagation constant, d is the distance from the transmitter, and A is the RSSI measured value when the distance is 1 m.

Assuming the space position of the reference node is (x_i, y_i, z_i), the space position of the locating node is (x_u, y_u, z_u), the distance between the locating node and the reference node is d_i, and the equation is obtained the matrix form is as follows:

In the formula \( {\hat{r}}_i=\sqrt{{\left({x}_i-{\hat{x}}_u\right)}^2+{\left({y}_i-{\hat{y}}_u\right)}^2+{\left({z}_i-{\hat{z}}_u\right)}^2} \), \( {a}_{xi}=\left({x}_i-{\hat{x}}_u\right)/{\hat{r}}_i \), \( {a}_{yi}=\left({y}_i-{\hat{y}}_u\right)/{\hat{r}}_i \), and \( {a}_{zi}=\left({z}_i-{\hat{z}}_u\right)/{\hat{r}}_i \), make Δd = [Δd₁ Δd₂ .  .  .  Δd_n]^T and \( \Delta x={\left[\Delta {x}_u\kern0.5em \Delta {y}_u\kern0.5em \Delta {z}_u\right]}^T \).

Formula (10) can be written as follows:

When the reference node is more than 4, Eq. (4) is the contradictory equation. Another calculation method is

The above equation to solve the least squares method can be used for iteration calculation, namely from the first position the nodes and transmission distance error values probably start, and gradually to the calculation results that meet the requirements of measurement and the value as a result of positioning. The advantage of this method is that when the computer is solved, it can utilize all kinds of valuable information as much as possible to reduce the error imported in the solution.

2.2.3 Establish the model of Kalman filter

RSSI-based localization focuses on distance measurement, while RSSI ranging is easily affected by interference noise, and the above method cannot meet the requirements of the robot for positioning accuracy. Kalman filtering is a valid algorithm for the best filtering of Gaussian process. When the objective model is sufficiently accurate and the system state and parameters are not mutated, the performance is better. Therefore, the Kalman filtering is combined with the above method to reduce the influence of noise on the system by using the filter function of Kalman filtering, so as to improve the positioning accuracy.

Firstly, the state equation of displacement and velocity of the system is established according to the above positioning information and discretization. The state equation of the positioning system is:

In the formula, the state vector X(k) is the positioning information of the robot to be optimized.

\( X(k)={\left[{x}_k\kern0.5em {y}_k\kern0.5em {z}_k\kern0.5em {V_k}^x\kern0.5em {V_k}^y\kern0.5em {V_k}^z\right]}^T \), X_k, Y_k, Z_k, and V_k^x, V_k^y, V_k^z are the displacement and velocity estimates of the three directions of the robot at the time of k. A is the system matrix; the positioning information of the robot is the observation vector S(k). \( S(k)={\left[{s_k}^x\kern0.5em {s_k}^y\kern0.5em {s_k}^z\right]}^T \) and s_k^x, s_k^y, s_k^z are the observed values of the displacement of the robot in three directions in the coordinate system; C is the output matrix; V(k) and W(k) are observation noise and state noise and meet E[W(k)] = E[V(k)] = 0, E[W(k)W(k)^T] = Q, and E[V(k)V(k)^T] = R, that is, W(k) and V(k) are separate white noise sequences with zero mean values.

The initial value of the state vector X(0)’ statistical features are defined as E[X(0)] = μ₀, Var[X(0)] = E[[X(0) − μ₀][X(0) − μ₀]^T] = P₀. The calculation of Kalman filtering is divided into the following equations:

The forecast process of Kalman filtering equation:

The correction process of the Kalman filtering equation:

The initial value selection is X(0) = 0,

The advantage of the Kalman filtering algorithm is that it is only related to the value of the previous moment, and the value of the latter time is estimated by the previous moment, and the variance is considered to be optimized in the feedback system. The computer memory is not piled with redundant data, and the computational efficiency is improved.

3.1 The simulation environment

MATLAB platform is used in the simulation experiment, and the emulation test is set in a square plane district of 20 m × 20 m. The robot’s angle is θ, the X-axis is going to be 0°, and it is going counterclockwise. The working process of the positioning system is first through locating node gathers the signal intensity of 8 beacon nodes, according to the radio signal strength and the related theory for positioning the distance between the nodes and 8 beacon nodes. Then, the location model is established by using the location of distance information and beacon nodes to determine the location of the locating node. Furthermore, the Kalman filtering model is established to filter the location message of the locating node. See Table 1 for reference node addresses and coordinates.

3.2 The emulation results

3.2.1 Emulation results of uniform motion of the robot

Robot positioning includes robot position and direction of movement. Figure 4 shows that the uniform motion of the robot is true. Kalman filtering is close to the real orbit, and it can reduce noise interference. In Fig. 4, the acceleration value processed by the Kalman filter algorithm is obviously different from the acceleration acquisition value, and the data obtained is more stable than the measured data. Because the attitude of moving objects is different, the acceleration is affected by gravity, so the measured value is a gravity acceleration g.

The uniform motion of the robot is true. The robot moves from the point (0, 3) to the point (14, 3) along the line, and the movement azimuth θ = 0°. Without Kalman filter, the probability of positioning error is 80% within 2 m, while after Kalman filter, the positioning error is controlled within 1 m

Full size image

Figure 5 is the observation position information of X and Y direction generated by simulation. The two-dimensional indoor road map of the robot shown in Fig. 5 shows the real position and observed the position of the robot in uniform motion. Taking the cement ground covered with pebbles as the experimental platform, the global coordinates are set and the starting heading angle of the robot is recorded as 0. Then, the straight and turning walking of the robot is realized by the encoder. In the process of robot motion, due to the influence of noise and odometer error, the actual trajectory of the robot deviates from the set trajectory.

Observation position information. It can be clearly observed that the initial state is not good for the actual target state, and the error is large. However, as the number of sampling increases, the error decreases

Full size image

Figures 6 and 7 show that the tracking effect is very good in the effective detection range. The estimated motion curve is accurate for the actual position, which can reflect the actual state of the robot at the moment. In addition, it can be seen that because of the velocity component in the Y direction; therefore, the estimation error in the Y direction is larger than that in the X direction. Later, near the real motion trajectory error levels near constant; thus, it can be found that the Kalman filtering estimates of robot target motion trajectory prediction can achieve a better effect. Specific functions of the Kalman filter are as follows: The velocity and starting point coordinates of the robot have a great error noise, and the label measurement value also has a great error noise. Kalman filter can get the optimal coordinates, i.e., the optimal state, by fusing the above two noisy measurements together. This is a noise reduction process. The filtered coordinates are noisy, but very small. When the optimal coordinates and noise arrive at the next time t + 1, the label observations and noise of t + 1 are deduced again as the input of filtering combined with the label observations of t + 1. The optimal trajectory of the robot is deduced by the continuous autoregressive filtering of the algorithm. The smaller the interval Δt, the higher the accuracy of filtering and the more accurate the trajectory is estimated.

Uniform motion X direction effect

Full size image

Uniform motion Y direction effect

Full size image

3.2.2 Simulation results of robot uniform acceleration motion

The 2D road map of the robot produced by simulation is shown in Fig. 8. The position of the corresponding X direction and Y direction is shown in Fig. 9. From Figs. 8 and 9, it can be clearly seen that the mean error of the Y direction is larger than that of the X direction because of the velocity component in the Y direction. Kalman filter is close to the real trajectory. After reaching the real trajectory, the error level remains near zero, so it can be seen that the Kalman filter can really achieve good results in estimating and predicting the trajectory of maneuvering target.

The real and observation position information

Full size image

Movement X and Y observation

Full size image

It can be found from Figs. 10 and 11 the orientation effect of X direction and Y direction. In Figs. 10 and 11, multi-sensor information fusion technology is adopted. The main advantage is that it can provide better performance in the repeatability and complementarity of indoor robot motion observation information. The distance of obstacles around the robot is measured by uniformly spaced sonar, and multi-sensor information fusion is carried out. Based on the data of multi-sensor fusion, the navigation map of the robot can be established to achieve accurate positioning of indoor robot motion.

X orientation effect. It can be found from this figure the orientation effect of X direction. Estimated curve for actual location estimation is more accurate and basically could reflect the actual status of robot at the moment, using Kalman filtering algorithm, and is used for the observation location information filtering

Full size image

Y orientation effect. It can be found from this figure the orientation effect of Y direction. Estimated curve for actual location estimation is more accurate and basically could reflect the actual status of robot at the moment, using Kalman filtering algorithm, and is used for the observation location information filtering

Full size image

The partial correctness of the algorithm is proved by Floyd invariant assertion method. The termination of the algorithm is proved by a well-ordered set method. The algorithm is completely correct, which can be illustrated by the conclusion which is that the algorithm is partially correct and can be terminated. The time complexity of the algorithm is O(n³), and the space complexity is O(n).

Through simulation, it is shown that without using Kalman filtering to make system optimization, the effects of indoor environment have a lot of interference noise, the error of RSSI values will be very big, there is more complex indoor environment confusion, and deviation system positioning precision is very large. Given an indoor environment, the proper selection of state error matrix and observation error array, an indoor robot positioning model and Kalman filter model are established, and Kalman filter autoregressive algorithm is used to optimize the estimated position coordinates of the robot. The simulation results show that when the Kalman filter is not used, the probability of robot positioning error is less than 1.8 m, and after the Kalman filter, the positioning error is less than 1.2 m. The positioning effect of the indoor robot is improved obviously.

In conclusion, the Kalman filtering algorithm can improve the positioning accuracy of the robot indoor. But there are other shortcomings that need to be further improved, such as that this article’s main research is limited in the two-dimensional space, cannot carry on the omnidirectional location measurement of carrier, and a further work that needs to be done is to study the robot positioning in this regard and the multidimensional space of carrier in order to realize the all-round position measurement. The main work in the future includes the integration and conversion of various indoor positioning modes, the perfect realization of the docking and conversion of indoor and outdoor mobile object positioning, the realization of a wide range of multi-choice positioning system, and the further improvement of positioning speed and accuracy, as well as the application in mobile terminals.

APIT:

Approximation point-in-triangulation test

GLONASS:

Global Navigation Satellite System

GPS:

Global Positioning System

RFID:

Radio frequency identification

RSSI:

Received signal strength indication

UWB:

Ultra-wideband

The research presented in this paper was supported by Chao Hu University, Hefei, Anhui, China.

Funding

The research was funded within the project No. 2018zygc029 entitled “Network engineering excellence engineer education and training program,” supported by funds for a key project from Quality Engineering Project in Colleges and Universities in Anhui Province, No. ch18xnfz03 entitled “Network engineering virtual simulation experiment teaching center,” supported by funds for a project from Chao Hu University.

Availability of data and materials

Not applicable.

Authors and Affiliations

1. College of Information Engineering, Chao Hu University, Hefei, Anhui, China

   Jian Yin Lu & Xinjie Li

Contributions

JYL is the main writer of this paper. She proposed the main idea, proposed and deduced the idea, completed the simulation, and analyzed the result. XJL mainly translated the Chinese article into English. All authors read and approved the final manuscript.

Corresponding author

Correspondence to Jian Yin Lu.

Competing interests

The authors declare that they have no competing interests.

Publisher’s Note

Springer Nature remains neutral with regard to jurisdictional claims in published maps and institutional affiliations.

Open Access This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http://creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.

Reprints and permissions