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.


Introduction
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 GLO-NASS 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.

Kalman filtering algorithm
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.

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 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.

Kalman filtering indoor location algorithm 2.2.1 System schematic diagram
System schematic diagram is shown in Fig. 3.

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: ¼ a x1 a y1 a z1 a x2 a y2 a z2 : : : : : : : : : a xn a yn a zn In the formular i ¼  . . (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.

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Þ The forecast process of Kalman filtering equation: The correction process of the Kalman filtering equation: The simulation results in two Dimensional path, namely the two-dimensional position information According to the system node location, obtain the distance information According to system node and distance information, location information is solved by locating model

Random sequence noise
Obtain position information with noise The location information is processed through the Kalman filter The initial value selection is X(0) = 0, P 0j0 ð Þ ¼ 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.

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.

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. 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. 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.

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. 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.
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 3 ), and the space complexity is O(n).

Results and discussion
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.