Abstract:
A method for real-time object locating and mapping includes: 1) performing real-time locating for a moving object in the range of movement; 2) updating a geomagnetic field map in the range of movement of the object in real time according to the locating of the moving object; 3) locating the next position of the moving object according to the updated geomagnetic field map; and repeating steps 2) and 3) until the object stops moving. A device for real-time object locating and mapping is also disclosed. The present application can solve the problem of real-time object locating and accurate geomagnetic field mapping without an indoor priori magnetic field map.

Description:
CROSS REFERENCE TO RELATED APPLICATIONS 
       [0001]    This application is a continuation of International Application No. PCT/CN2015/081429, filed on Jun. 16, 2015, which is based upon and claims priority to Chinese Patent Application No. 201410277471.7, filed on Jun. 19, 2014, the entire contents of which are incorporated herein by reference. 
     
    
     TECHNICAL FIELD 
       [0002]    The present invention relates to the field of indoor locating and mapping, and more particularly, to a method and device for combining indoor object locating with mapping. 
       BACKGROUND 
       [0003]    The problem to be solved by simultaneous locating and mapping is that, an object needs to determine its own position while using sensor data containing noise to construct the distribution of the items in the environment. Moreover, solution to the problem of locating and mapping can enable the object to achieve true autonomous navigation. In the past ten years, the problem of simultaneous locating and mapping has become a hotspot in the field of mobile robots. Conventionally, EKF (Extended Kalman Filter) is used to simultaneously estimate the positions of a robot and landmarks. However, when the positions of the landmark increase and the movement range of the robot becomes larger, the calculation amount of this method becomes larger and the timeliness gets worse. In addition, this method requires a prior arrangement of the environment, for example, waypoints needs to be arranged, thus consuming human and material resources. 
       SUMMARY 
       [0004]    In view of the above defects of the prior art, the present invention provides a method for object locating and mapping. The method uses a motion model through measuring the current geomagnetic field to correct the current position, updates the data of a local map by using a spatial interpolation algorithm, and merges same into a global map, which can achieve the synchronization of object locating and mapping, while greatly reduces the calculation amount and eliminates the need to deploy the environment in advance, thereby saving resources. 
         [0005]    According to one aspect of the present invention, a method for object locating and mapping is provided. The method may include: at a device, 1) performing real-time locating for a moving object in the range of movement; 2) updating a geomagnetic field map in the range of movement of the object in real time according to the locating of the moving object; 3) locating the next position of the moving object according to the updated geomagnetic field map; and repeating steps 2) and 3) until the object stops moving. 
         [0006]    In the above methods, step 1) may include: initializing a position of the object in the range of movement and a global geomagnetic map in the range of movement, the object being provided with a sensor configured to measure a traveling distance, a rotation angle value and a magnetic field value of the object; adding a geomagnetic field value at an initial position of the object to the initialized global geomagnetic grid map by means of spatial interpolation; measuring traveling control variables of the object and a geomagnetic field value at the current position of the object; and estimating a position of the randomly moving object by using a particle filter algorithm. 
         [0007]    The step of initializing a position of the object in the range of movement and a global geomagnetic map in the range of movement may include: abstracting an indoor movement area of the object to grid coordinates, the size of the grid coordinates being set in proportion to the actual size of an activity room. The initial position of the object in the activity room is determined by an internal sensor of the object. 
         [0008]    The measured geomagnetic field value is added to the initialized map by means of spatial interpolation such as Kriging interpolation. Measured values around the object are weighted to get a prediction of a position which is not measured by the formula 
         [0000]    
       
         
           
             
               
                 
                   Z 
                   ^ 
                 
                  
                 
                   ( 
                   
                     S 
                     0 
                   
                   ) 
                 
               
               = 
               
                 
                   ∑ 
                   
                     i 
                     = 
                     1 
                   
                   N 
                 
                  
                 
                   
                     λ 
                     i 
                   
                    
                   
                     Z 
                      
                     
                       ( 
                       
                         S 
                         i 
                       
                       ) 
                     
                   
                 
               
             
             , 
           
         
       
     
         [0000]    where λ i  is an unknown weight of a measured value at the ith position, S 0  is a predicated position, N is the number of the measured values, where the weight λ i  depends on a fitting model of the spatial relationship among a measurement point, a distance of the predicated position and the measured values around the predicated position when the spatial interpolation is Kriging interpolation. 
         [0009]    The step of adding the geomagnetic field value to a map using an interpolation algorithm may include: extracting grid points data in a first predetermined range around the position of the object in the geomagnetic grid map after the position of the object is estimated, then merging the grid points obtained by means of interpolation in the first predetermined range into a grid map in a second predetermined range. 
         [0010]    According to another aspect of the present invention, a device for object locating and map updating is provided. The device having at least one processor, and a memory in electronic communication with the processor and having instructions stored therein may include: a locating module and a map updating module. The locating module is implemented by the at least one processor and configured to perform real-time locating for a moving object in a range of the object&#39;s movement. The map updating module is implemented by the at least one processor and configured to update a geomagnetic field map in the range of movement of the object in real time according to the locating of the moving object. The locating module is implemented by the at least one processor and further configured to locate the next position of the moving object according to the updated geomagnetic field map until the object stops moving. 
         [0011]    The locating module may include: an initialization module configured to initialize an initial position of the object and a geomagnetic grid map; and a position estimation module configured to estimate the initial position of the object and positions during the movement of the object by using a particle filter algorithm and a kinematic model. The map updating module may include: an interpolation module configured to perform interpolation for the geomagnetic field value in the range of movement of the object by means of spatial interpolation and merge a local map into a global map. 
         [0012]    The position estimation module can be implemented on the basis of a particle filter algorithm which may include: a particle filter algorithm module configured to correct the estimated position of the object; and a motion model module configured to analyze the moving in the range of movement of the object by means of a predefined mathematical model. 
         [0013]    During the determination of the weight, grid maps which correspond to the direction and intensity of the magnetic field represented by vectors          H x ,H y ,H z            can be used to compose a database, in which each vector denotes a distribution of corresponding vector. Thus, characteristics of the map may be increased, which is useful for locating. 
         [0014]    The mapping module may include: an interpolation module configured to extract grid points data in a predetermined range around the position of the object in the geomagnetic grid map after the position of the object is estimated, and interpolate the same with the measured geomagnetic field value; and a merging module configured to merge the grid points obtained by means of interpolation in the predetermined range into a grid map in the predetermined range. 
         [0015]    According to yet another aspect of the present invention, there is further provided a non-transitory computer-readable storage medium storing executable instructions used to execute any one of methods of the present invention as described above. 
         [0016]    Compared with the prior art, the above-mentioned solution uses real-time object locating to update a magnetic field map based on the result of real-time locating and further to locate an object by means of the updated magnetic field map, which takes a combination of locating and map updating to locate the object accurately and rapidly and to build a consistent map. 
         [0017]    Further, after the inventors of the present invention have made intensive and extensive researches, they choose to use the particle filter algorithm with errors produced by correction to update indoor geomagnetic field maps in the face of numerous algorithms, and use the Kriging interpolation algorithm that is the best interpolation method of spatial covariance. So the result has high reliability. The combination of these two methods can locate a robot more accurately and rapidly and build a consistent map. 
         [0018]    Now, in most buildings, there are always indoor magnetic field fluctuations because of the existence of objects such as reinforced concrete structures and furniture. But the present invention just uses indoor magnetic field fluctuations to realize the localization and further to realize real-time locating and mapping. Therefore, the implementation of the present invention does not require the additional arrangement of reference points which are required for locating, thus the cost can be reduced. 
     
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         [0019]      FIG. 1  is a flowchart showing the steps of a method for object locating according to an embodiment of the present invention; 
           [0020]      FIG. 2  is a schematic diagram of initializing an initial position of an object in the range of movement according to an embodiment of the present invention; 
           [0021]      FIG. 3  is a schematic diagram of an initial geomagnetic field grid map in the range of movement according to an embodiment of the present invention; 
           [0022]      FIG. 4  is a schematic diagram of adding a geomagnetic field value at an initial position of an object to an initial map by using a Kriging interpolation algorithm according to an embodiment of the present invention; 
           [0023]      FIG. 5  is a schematic diagram of correcting the position of a moving object according to an embodiment of the present invention; 
           [0024]      FIG. 6  is a schematic diagram of measuring a geomagnetic field value at the current position of an object according to an embodiment of the present invention; 
           [0025]      FIG. 7  is a schematic diagram of adding a geomagnetic field value to a local map by using an interpolation algorithm according to an embodiment of the present invention; 
           [0026]      FIG. 8  is a schematic diagram of merging a local map into a global map; and 
           [0027]      FIG. 9  is structural schematic diagram of a device for real-time object locating and mapping in a geomagnetic field according to an embodiment of the present invention. 
       
    
    
     DETAILED DESCRIPTION 
       [0028]    The present invention will be described in further details in conjunction with the drawings. 
         [0029]      FIG. 1  is a flowchart of the steps of a method for real-time object locating and mapping in a geomagnetic field according to an embodiment of the present invention. In an embodiment, the method may include the following steps, as shown in  FIG. 1 . 
         [0030]    Step S 101 : An activity area (in this embodiment, taking an activity room as an example) of an object to be located is conducted a field measurement to obtain an indoor plan. Then the plan is simulated as an initialization grid map. Combined with the data (l i , θ i ) from an internal sensor of the object, where l i  is a distance of the ith movement, θ i  is a deflection angle of the ith movement, the position of the object to be located in the simulated initialization grid map can be determined.  FIG. 2  is a schematic diagram of initializing an initial position of an object in the range of movement according to an embodiment of the present invention. As shown in  FIG. 2 , “A” refers to the initial position of the object, the black spots next to it are sampling points for position workspace in an Importance Sampling Algorithm of a particle filtering algorithm. A mean value of these sampling points denotes a position estimate of the object. 
         [0031]    Step S 102 : If the object locates at the initial position, the particles are evenly distributed around A. The position of A is measured by the object&#39;s internal sensor. With the movement of the object, based on a motion model, i.e., x k =f(x k&#39;11 , v k−1 , w k ), x j  can be calculated by means of x k−1  and a control input variable v k−1 , where x k  is the state at time k, v k−1  is a control input variable at time k−1 (i.e., the traveling distance of the object and a rotation angle), w k  is a correlated error from time k−1 to time k. Since both the sensor itself and the surrounding environment will cause certain errors in the measurement, it is difficult for a robot to realize accurate locating based on such inaccurate control information. The errors in the present invention are a cumulative sum of random noise and white Gaussian noise. The motion model used in the present invention is: 
         [0000]    
       
         
           
               
             
               { 
               
                 
                   
                     
                       
                         
                           
                             
                               &lt; 
                               
                                 x 
                                 t 
                               
                             
                             , 
                             
                               
                                 y 
                                 t 
                               
                               &gt;= 
                               
                                 MOVE 
                                 + 
                               
                               &lt; 
                               
                                 x 
                                 
                                   t 
                                   - 
                                   1 
                                 
                               
                             
                             , 
                             
                               
                                 y 
                                 
                                   t 
                                   - 
                                   1 
                                 
                               
                               &gt; 
                               + 
                             
                           
                         
                       
                       
                         
                           
                             
                               
                                 0.5 
                               
                               * 
                               
                                 randn 
                                  
                                 
                                   ( 
                                   
                                     1 
                                     , 
                                     2 
                                   
                                   ) 
                                 
                               
                             
                             + 
                             
                               0.01 
                               * 
                               
                                 wgn 
                                  
                                 
                                   ( 
                                   
                                     1 
                                     , 
                                     2 
                                     , 
                                     0.05 
                                   
                                   ) 
                                 
                               
                             
                           
                         
                       
                     
                   
                 
                 
                   
                     
                       
                         MOVE 
                         = 
                         
                           [ 
                           
                             
                               
                                 - 
                                 
                                   l 
                                   t 
                                 
                               
                               * 
                               
                                 sin 
                                  
                                 
                                   ( 
                                   
                                     θ 
                                     t 
                                   
                                   ) 
                                 
                               
                             
                             , 
                             
                               
                                 l 
                                 t 
                               
                               * 
                               
                                 cos 
                                  
                                 
                                   ( 
                                   
                                     θ 
                                     t 
                                   
                                   ) 
                                 
                               
                             
                           
                           ] 
                         
                       
                       , 
                       
                         0 
                         ≤ 
                         
                           θ 
                           t 
                         
                         &lt; 
                         90 
                       
                     
                   
                 
                 
                   
                     
                       
                         MOVE 
                         = 
                         
                           [ 
                           
                             
                               
                                 - 
                                 
                                   l 
                                   t 
                                 
                               
                               * 
                               
                                 cos 
                                  
                                 
                                   ( 
                                   
                                     
                                       θ 
                                       t 
                                     
                                     - 
                                     90 
                                   
                                   ) 
                                 
                               
                             
                             , 
                             
                               
                                 - 
                                 
                                   l 
                                   t 
                                 
                               
                               * 
                               
                                 sin 
                                  
                                 
                                   ( 
                                   
                                     
                                       θ 
                                       t 
                                     
                                     - 
                                     90 
                                   
                                   ) 
                                 
                               
                             
                           
                           ] 
                         
                       
                       , 
                       
                         90 
                         ≤ 
                         
                           θ 
                           t 
                         
                         &lt; 
                         180 
                       
                     
                   
                 
                 
                   
                     
                       
                         MOVE 
                         = 
                         
                           [ 
                           
                             
                               l 
                               * 
                               
                                 cos 
                                  
                                 
                                   ( 
                                   
                                     270 
                                     - 
                                     
                                       θ 
                                       t 
                                     
                                   
                                   ) 
                                 
                               
                             
                             , 
                             
                               
                                 - 
                                 
                                   l 
                                   t 
                                 
                               
                               * 
                               
                                 sin 
                                  
                                 
                                   ( 
                                   
                                     270 
                                     - 
                                     
                                       θ 
                                       t 
                                     
                                   
                                   ) 
                                 
                               
                             
                           
                           ] 
                         
                       
                       , 
                       
                         180 
                         ≤ 
                         
                           θ 
                           t 
                         
                         &lt; 
                         270 
                       
                     
                   
                 
                 
                   
                     
                       
                         MOVE 
                         = 
                         
                           [ 
                           
                             
                               
                                 l 
                                 t 
                               
                               * 
                               
                                 cos 
                                  
                                 
                                   ( 
                                   
                                     360 
                                     - 
                                     
                                       θ 
                                       t 
                                     
                                   
                                   ) 
                                 
                               
                             
                             , 
                             
                               
                                 l 
                                 t 
                               
                               * 
                               
                                 sin 
                                  
                                 
                                   ( 
                                   
                                     360 
                                     - 
                                     
                                       θ 
                                       t 
                                     
                                   
                                   ) 
                                 
                               
                             
                           
                           ] 
                         
                       
                       , 
                       
                         270 
                         ≤ 
                         
                           θ 
                           t 
                         
                         &lt; 
                         360 
                       
                     
                   
                 
               
             
           
         
       
     
         [0032]    where l t  is a movement distance of the object from time t−1 to time t; θ t  is a rotation angle of the object from time t−1 to time t, which is based on due north direction and rotates anti-clockwise; randn(1,2) generates random numbers having 1×2 vectors, and wgn(1, 2, 0.05) generates white Gaussian noise with 0.05 dBw having 1×2 vectors. All the particles that are set will move with the motion model (as shown in  FIG. 5 ) to obtain the sampling values at the particles&#39; new position. The movement of the sampling points in the indoor plan is shown in  FIG. 5 . Then the current position of the object is estimated based on the magnetic field values detected by an external sensor carried by the object. Although the position of the object can be roughly estimated by controlling the input parameters, there are deviations which will accumulate. Therefore, in an embodiment, the position of the object is further corrected such that the measured position is consistent with the actual position, and the accumulation of the deviations is eliminated to the great extent. 
         [0033]    The specific implementation of the particle filter algorithm is: 
         [0034]    Setting an initial particle distribution: an initial distribution of particles is a uniform distribution around the starting position of an object (robot), which produces Num=100 particles X 0   (i) , the weight of the particles is at ω 0   (i) . Based on such an assumption, it is sufficient to have only 100 particles. Since a major drawback in the particle filter algorithm is that the calculation amount is large and the timeliness is poor, the quantity of particles is a key factor to determine the calculation amount. In this embodiment, the calculation amount can be decreased and the implementation can be ensured through reducing the quantity of particles while the precision is guaranteed. So the convergence of particle filtering will be very fast to achieve real-time operation; 
         [0035]    Importance resampling: the state of each particle is updated from X t−1   (i)  to X t   (i) , then new locations of the 100 particles can be obtained based on conditional probability distribution P(X t   (i) |X t&#39;11   (i) ,U t ), where X t−1   (i) , is the state of the ith particle at time t−1, U t  is a control input variable at time t. Particles are obtained by means of sampling in a position space where the object is located. The distribution of these particles is a representation of the probability distribution of the spatial position of the object. 
         [0036]    Weights calculation: the theoretical formula for calculating the weight is 
         [0000]    
       
         
           
             
               
                 ω 
                 t 
                 
                   ( 
                   i 
                   ) 
                 
               
               = 
               
                 
                   p 
                    
                   
                     ( 
                     
                       
                         
                           X 
                           t 
                           
                             ( 
                             i 
                             ) 
                           
                         
                          
                         
                           z 
                           
                             1 
                              
                             
                               : 
                             
                              
                             
                                 
                             
                              
                             t 
                           
                         
                       
                       , 
                       
                         u 
                         
                           0 
                            
                           
                             : 
                           
                            
                           
                               
                           
                            
                           t 
                         
                       
                     
                     ) 
                   
                 
                 
                   q 
                    
                   
                     ( 
                     
                       
                         
                           X 
                           t 
                           
                             ( 
                             i 
                             ) 
                           
                         
                          
                         
                           z 
                           
                             1 
                              
                             
                               : 
                             
                              
                             
                                 
                             
                              
                             t 
                           
                         
                       
                       , 
                       
                         u 
                         
                           0 
                            
                           
                             : 
                           
                            
                           
                               
                           
                            
                           t 
                         
                       
                     
                     ) 
                   
                 
               
             
             , 
           
         
       
     
         [0000]    where p(X t   (i) |z 1:t , u 0:t ) is a true distribution, while q(X t   (i) |z 1:t , u 0:t ) is a suggestive distribution. Since the direction and intensity of the magnetic field are represented by vectors          H x ,H y ,H z           , the database used in this embodiment is composed of three grid maps of the same size, each map denotes a distribution of H x ,H y , H z , respectively. In this way, the number of characteristics of the map will increase, which is conducive to locating. The calculation of the weights is based on an exponential distribution. The computational complexity of exponential distribution (usually expressed in terms of time complexity) is O(N), while the computational complexity of Gaussian distribution is O(N 2 ), where N denotes the quantity of particles. Thus it can be seen that the method of calculating weights based on exponential distribution in this embodiment can reduce the amount of computations, speed up the convergence speed and improve the locating accuracy. The exponential distribution function is: 
         [0000]    
       
         
           
               
             
               { 
               
                 
                   
                     
                       Dis 
                       = 
                       
                         
                           
                              
                             
                               
                                 R 
                                 . 
                                 magx 
                               
                               - 
                               mapx 
                             
                              
                           
                           + 
                           
                              
                             
                               
                                 R 
                                 . 
                                 magy 
                               
                               - 
                               mapy 
                             
                              
                           
                           + 
                           
                              
                             
                               
                                 R 
                                 . 
                                 magz 
                               
                               - 
                               mapz 
                             
                              
                           
                         
                       
                     
                   
                 
                 
                   
                     
                       
                         R 
                         . 
                         w 
                       
                       = 
                       
                         
                           R 
                           . 
                           w 
                         
                         * 
                         λ 
                         * 
                         
                           exp 
                            
                           
                             ( 
                             
                               
                                 ( 
                                 
                                   - 
                                   Dis 
                                 
                                 ) 
                               
                               / 
                               λ 
                             
                             ) 
                           
                         
                       
                     
                   
                 
               
             
           
         
       
     
         [0037]    Where R,magx is the value of the magnetic field measured in H x  direction at the current position point of the object, mapx is the value of the current position on the grid map, and R.w is the weight of a particle. The magnetic field value on the grid map may be derived from the interpolation algorithm, or it may be directly added when the object passed by before. So are R.magy, mapy, R.magz. mapz . The value of λ can be reasonably adjusted according to the data obtained by experiment. The value of λ is 2 in this embodiment. 
         [0038]    Weight normalization: 
         [0000]    
       
         
           
             
               
                 ω 
                 k 
                 
                   ( 
                   i 
                   ) 
                 
               
               = 
               
                 
                   ω 
                   k 
                   i 
                 
                  
                 
                   / 
                 
                  
                 
                   
                     ∑ 
                     
                       i 
                       = 
                       1 
                     
                     
                       N 
                       um 
                     
                   
                    
                   
                     ω 
                     k 
                     
                       ( 
                       i 
                       ) 
                     
                   
                 
               
             
             ; 
           
         
       
     
         [0039]    The number of effective particles (e.g. the number of effective particles are determined by weight) 
         [0000]    
       
         
           
             
               N 
               eff 
             
             = 
             
               1 
                
               
                 / 
               
                
               
                 
                   ∑ 
                   
                     i 
                     - 
                     1 
                   
                   N 
                 
                  
                 
                   
                     ( 
                     
                       ω 
                       k 
                       i 
                     
                     ) 
                   
                   2 
                 
               
             
           
         
       
     
         [0000]    is calculated. A threshold value δ is set based on an empirical value, such as according to the proportion of the number of particles. For example, the value of δ can be set 75% of the total number of particles. If the number of effective particles is smaller than this threshold, the particles X k  are resampled based on the value of particle weight ω k   (i) . The particles with small weights are removed, and the particles with large weights are copied by the number of removed particles with small weights so as to keep the total number of the particles unchanged. 
         [0040]    Object position estimation: based on the particles sampled in the last step, the mean value of the spatial distribution of the particles is determined. Specifically, the weight of the particle is multiplied by the position coordinates, and then the product is superimposed to estimate the current position of the object. 
         [0041]    Step S 103 : On the simulated indoor grid map (as shown in  FIG. 3 ) obtained in step S 101 , the values measured by the external sensor of the object are interpolated to the simulated grid map using the Kriging interpolation algorithm, the result of interpolation is shown in  FIG. 4 .  FIG. 4  is a schematic diagram of adding a geomagnetic field value at an initial position of an object to an initialization map by using the Kriging interpolation algorithm according to an embodiment of the present invention. Here, the right-side part of the figure is a partial enlargement of the left-side interpolation part. 
         [0042]    In a simulation experiment, the geomagnetic field database of the present invention is obtained by measuring a group of true magnetic field values in a range of activity room with 10 m×10 m at intervals of 0.5 m, altogether 21×21 groups are measured. Then a spatial interpolation operation is performed on these 441 groups of data to get 81×81 groups of data. Owing to the stability of the indoor magnetic field fluctuations, these 81×81 groups of data are used as the geomagnetic field database in this embodiment to measure actual values in simulation. 
         [0043]    Step S 104 : The geomagnetic field values obtained in step S 103  are firstly inserted into a local geomagnetic field. The local geomagnetic field is extracted from a global geomagnetic field. The extraction rule is as follows: 10×10 grid data centered on an estimate point are extracted from a grid map at the estimated position point to implement an interpolation operation; if a 10×10 local grid cannot be obtained when the estimated position point is regarded as the center, then the extraction is performed from a 10×10 grid corners map including the estimated position point. For example, if the coordinates (x, y) of the estimated position point satisfy a condition (abs (y−1)&lt;5 &amp;&amp; abs (x−1)&lt;5), then an extraction is carried out on an area with abscissa values from 1 to 10, ordinate values from 1 to 10 in the global map. The process of merging the interpolated local map (as shown in  FIG. 7 ) into the global map is an inverse of extraction (as shown in  FIG. 8 ). In this way, the correlation between the magnetic field data and the magnetic field values around the estimated position point can be taken into account, and the computation of interpolation can be reduced. 
         [0044]    Step S 105 : The object is moved randomly in the activity range. The relevant data obtained by an internal sensor of the object is collected so as to provide a basis for updating the particles in the next step. 
         [0045]    Step S 106 : Whether the movement of the object is stopped or not is detected. If not, the operation returns to step S 102 , and the operation (iteration) is repeated until the movement of the object is stopped. 
         [0046]    A device for real-time object locating and mapping is provided according to an embodiment of the present invention. As shown in  FIG. 9 , the device may include: 
         [0047]    an initialization module  201  configured to initialize an initial position of an object and a geomagnetic field grid map; 
         [0048]    a position estimation module  202  configured to estimate the initial position and the position during the movement of the object using a particle filter algorithm and a kinematic model; and 
         [0049]    a map updating module  203  configured to interpolate the geomagnetic field values measured by a sensor using a spatial interpolation algorithm and merge a local map into a global map. 
         [0050]    The position estimation module  202  may include: 
         [0051]    a particle filter algorithm module  2021 , including: 
         [0052]    an initialization particles distribution module  20211  configured to provide an original reference for the movement of subsequent particle sets; 
         [0053]    an importance resampling module  20212  configured to generate a set of particle estimates based on an object movement process; 
         [0054]    a weight calculation module  20213  configured to assign a related weight to the particle, the closer to the characteristic of the object, the larger the weight, and vice versa; 
         [0055]    a weight normalization module  20214  configured to normalize the weights of the particles; 
         [0056]    an effective particle number determination module  20215  configured to find the number of particles that are close to the characteristic of the object; and 
         [0057]    a position estimation correction module  20216  configured to correct the movement position estimation of the object. 
         [0058]    Through the operation of the above modules, the initial particle distribution, the importance re-sampling, the weight calculation, the weight normalization and the calculation of the effective particle number are carried out by using the particle filtering method as described above to estimate the current position of the object (robot). 
         [0059]    The position estimation module  202  may further include a motion model unit  2022  configured to analyze the movement of the object in the range of movement through a predetermined mathematical model. A nonlinear dynamic model i.e., 
         [0000]    
       
         
           
             { 
             
               
                 
                   
                     
                       
                         x 
                         t 
                       
                       = 
                       
                         f 
                          
                         
                           ( 
                           
                             
                               x 
                               
                                 t 
                                 - 
                                 1 
                               
                             
                             , 
                             
                               v 
                               
                                 t 
                                 - 
                                 1 
                               
                             
                           
                           ) 
                         
                       
                     
                   
                 
                 
                   
                     
                       
                         y 
                         t 
                       
                       = 
                       
                         h 
                          
                         
                           ( 
                           
                             
                               x 
                               t 
                             
                             , 
                             
                               n 
                               t 
                             
                           
                           ) 
                         
                       
                     
                   
                 
               
               , 
             
           
         
       
     
         [0000]    is used in this embodiment. 
         [0060]    This model can make a prediction for the observed values using prior information. Posterior information can be obtained using this formula after the observed values are predicted so as to correct the prior information. The corrected information is again used as prior information for prediction. So the cycle continues, the observed values are continuously obtained to correct the prior information (previous position) of the object&#39; position. In the present invention, formula x t =f(x  t−1 , v t−1 ) is the same as the motion model in step  102 . However, formula y t =h(x t ,n t ) is used to correct position information in a map of the particles based on interpolation. 
         [0061]    The map updating module  203  may include: 
         [0062]    an interpolation module  2031  configured to derive a prediction of an unmeasured position, such as by weighting the surrounding measured values with a Kriging method. The prediction may be derived from a weighted sum of data using the following formula: 
         [0000]    
       
         
           
             
               
                 
                   Z 
                   ^ 
                 
                  
                 
                   ( 
                   
                     S 
                     0 
                   
                   ) 
                 
               
               = 
               
                 
                   ∑ 
                   
                     i 
                     = 
                     1 
                   
                   N 
                 
                  
                 
                   
                     λ 
                     i 
                   
                    
                   
                     Z 
                      
                     
                       ( 
                       
                         S 
                         i 
                       
                       ) 
                     
                   
                 
               
             
             , 
             
               
                 
                   ∑ 
                   
                     i 
                     = 
                     1 
                   
                   N 
                 
                  
                 
                   λ 
                   i 
                 
               
               = 
               1. 
             
           
         
       
     
         [0000]    where λ i  is a position weight of a measured value at the ith position, S 0  is a predicted position, S t  is a random point around the predicted position, Z(S i ) is a magnetic field value at the position S i , N is the quantity of the measured values which is 100 in this embodiment. 
         [0063]    In this embodiment, the data point is represented with coordinates point S t (a i ,b i ) i=1,2, . . . , n, the coordinates of the point to be interpolated are represented with S 0 (a 0 ,b 0 ) , then a distance h ij =√{square root over ((a i −a j ) 2 +(b i −b j ) 2 )} between scattered points is calculated, where i=1,2, . . . , n; and j=1,2, . . . , n. The distance values are divided into several groups after sorting the distance by the values in an ascending order. Each group includes a certain number of distance values. These distance values can be combined into a distance group (which is represented by {h′ m }): 
         [0000]    
       
         
           
             
               
                 { 
                 
                   h 
                   m 
                   ′ 
                 
                 } 
               
               = 
               
                 m 
                 × 
                 
                   
                     ( 
                     
                       
                         max 
                          
                         
                             
                         
                          
                         
                           h 
                           ij 
                         
                       
                       - 
                       
                         min 
                          
                         
                             
                         
                          
                         
                           h 
                           ij 
                         
                       
                     
                     ) 
                   
                   
                     N 
                     H 
                   
                 
               
             
             , 
             
               m 
               = 
               1 
             
             , 
             2 
             , 
             … 
              
             
                 
             
             , 
             
               N 
               H 
             
             , 
           
         
       
     
         [0000]    where N H  is the number of distance groups, a distribution shape 
         [0000]    
       
         
           
             
               
                 γ 
                 * 
               
                
               
                 ( 
                 
                   h 
                   m 
                   ′ 
                 
                 ) 
               
             
             = 
             
               
                 1 
                 
                   2 
                    
                   
                     N 
                      
                     
                       ( 
                       
                         h 
                         m 
                         ′ 
                       
                       ) 
                     
                   
                 
               
                
               
                 
                   ∑ 
                   
                     i 
                     = 
                     1 
                   
                   
                     N 
                      
                     
                       ( 
                       
                         h 
                         m 
                         ′ 
                       
                       ) 
                     
                   
                 
                  
                 
                   
                     
                       [ 
                       
                         
                           Z 
                            
                           
                             ( 
                             
                               S 
                               i 
                             
                             ) 
                           
                         
                         - 
                         
                           Z 
                            
                           
                             ( 
                             
                               
                                 S 
                                 i 
                               
                               - 
                               
                                 h 
                                 m 
                                 ′ 
                               
                             
                             ) 
                           
                         
                       
                       ] 
                     
                     2 
                   
                    
                   
                       
                   
                    
                   
                     ( 
                     
                       
                         i 
                         = 
                         1 
                       
                       , 
                       … 
                        
                       
                           
                       
                       , 
                       
                         N 
                          
                         
                           ( 
                           h 
                           ) 
                         
                       
                     
                     ) 
                   
                 
               
             
           
         
       
     
         [0000]    can be calculated by {h′ m }. h′ m  is abscissa, and γ*(h′ m ) is ordinate. It is needed to choose a suitable model to fit so as to produce a continuous curve that represents the distribution of γ*(h′ m ). The semevariation fitting model is circular in this embodiment: 
         [0000]    
       
         
           
             
               γ 
                
               
                 ( 
                 h 
                 ) 
               
             
             = 
             
               { 
               
                 
                   
                     
                       
                         c 
                         0 
                       
                       + 
                       
                         c 
                         ( 
                         
                           1 
                           - 
                           
                             
                               2 
                               π 
                             
                              
                             
                               
                                 cos 
                                 
                                   - 
                                   1 
                                 
                               
                                
                               
                                 ( 
                                 
                                   h 
                                   α 
                                 
                                 ) 
                               
                             
                           
                           + 
                           
                             
                               ( 
                               
                                 1 
                                 - 
                                 
                                   
                                     h 
                                     2 
                                   
                                   
                                     α 
                                     2 
                                   
                                 
                               
                               ) 
                             
                           
                         
                       
                     
                   
                   
                     
                       0 
                       &lt; 
                       h 
                       &lt; 
                       α 
                     
                   
                 
                 
                   
                     
                       
                         c 
                         0 
                       
                       + 
                       c 
                     
                   
                   
                     
                       h 
                       &gt; 
                       α 
                     
                   
                 
                 
                   
                     0 
                   
                   
                     others 
                   
                 
               
             
           
         
       
     
         [0064]    Where c 0  is nugget value, c is sill value and a is range. The model parameters (such as c 0 , c, α, etc.) can be calculated through fitting the distribution of function γ*(h m ) (it is a discrete distribution) to get a distribution (i.e., variation function γ*(h)) that is fit for the samples in this experiment. Then the following equations are used, that is, 
         [0000]    
       
         
           
             { 
             
               
                 
                   
                     
                       
                         
                           
                             ∑ 
                             
                               i 
                               = 
                               1 
                             
                             N 
                           
                            
                           
                             
                               
                                 γ 
                                 * 
                               
                                
                               
                                 ( 
                                 
                                   
                                     S 
                                     i 
                                   
                                   - 
                                   
                                     S 
                                     j 
                                   
                                 
                                 ) 
                               
                             
                              
                             
                               λ 
                               i 
                             
                           
                         
                         - 
                         μ 
                       
                       = 
                       
                         
                           
                             γ 
                             * 
                           
                            
                           
                             ( 
                             
                               
                                 S 
                                 0 
                               
                               - 
                               
                                 S 
                                 j 
                               
                             
                             ) 
                           
                         
                          
                         
                             
                         
                          
                         
                           ( 
                           
                             
                               j 
                               = 
                               1 
                             
                             , 
                             … 
                              
                             
                                 
                             
                             , 
                             N 
                           
                           ) 
                         
                       
                     
                   
                 
                 
                   
                     
                       
                         
                           ∑ 
                           
                             i 
                             = 
                             1 
                           
                           N 
                         
                          
                         
                           λ 
                           i 
                         
                       
                       = 
                       1 
                     
                   
                 
               
               , 
             
           
         
       
     
         [0000]    where μ is Lagrange multiplier. Weight λ i  and Lagrange multiplier μ are determined by solving the above equations. Finally, a predicted value {circumflex over (Z)}(S 0 ) at position S 0 (a 0 ,b 0 ) is determined by means of formula 
         [0000]    
       
         
           
             
               
                 Z 
                 ^ 
               
                
               
                 ( 
                 
                   S 
                   0 
                 
                 ) 
               
             
             = 
             
               
                 ∑ 
                 
                   i 
                   = 
                   1 
                 
                 N 
               
                
               
                 
                   λ 
                   i 
                 
                  
                 
                   
                     Z 
                      
                     
                       ( 
                       
                         S 
                         i 
                       
                       ) 
                     
                   
                   . 
                 
               
             
           
         
       
     
         [0065]    The map updating module  203  may further include a map merging module  2032  which may be configured to extract a local geomagnetic field from a global geomagnetic field. The extraction rule is, for example, 10×10 local grid data centered on an estimated position point are extracted from a global grid map (81×81) at the estimated position point to implement interpolation; if the estimated position point is in the peripheral position of the grid map, then the extraction is performed from a 10×10 grid corners map including the estimated position point. Since the local map (as shown in  FIG. 7 ) after interpolation and the extracted map before interpolation are of the same size, the process of merging into the global map (as shown in  FIG. 8 ) is as follows: the local map extracted from the global map covers (or is inserted into) the location of extraction. It is essentially to create a small map with the same size as the local map (that is, the size of the grid is the same as the data on grid distribution) through the above-mentioned method. The size of the small map after interpolation remains unchanged, whilst the data on the grid distribution has changed. The small map after interpolation gradually covers the global map. Thus, a large amount of computation caused by global map interpolation can be avoided. 
         [0066]    An embodiment of the present invention also provides a non-transitory computer-readable storage medium storing executable instructions used to execute any one of methods of the present invention as described above.