Patent ID: 11892855
Assignee: ZHEJIANG UNIVERSITY
Field: Measurement (Instruments)
Classification: CPC G  H | IPC G  H

Claim 7:
8. The autonomous mapping method according to claim 6, wherein the step (S6) specifically comprises:
(S601) converting a motion observation model to a likelihood function;
(S602) evenly dividing the area to be constructed on the map into multiple grids, scanning the area to be constructed on the map with the LiDAR, setting a grid with obstacles to 1, setting a grid without obstacles to 0, and obtaining a local grid map which is regarded as an initial global map;
(S603) creating particles based on Monte Carlo algorithm, taking positions of the particles as possible locations of the robot, performing weight fusion on the second real-time global coordinates obtained by the odometer and the first real-time global coordinates obtained by the inertial measurement unit in the world coordinate system, and obtaining new locations of the robot, which is expressed by formulas of, P
      i
    
    =
    
      
        
          P
          
            o
            ⁢
            d
            ⁢
            a
            ⁢
            m
          
        
        ·
        
          θ
          
            Δ
            ⁢
            t
          
        
      
      +
      
        
          P
          IMU
        
        ·
        
          (
          
            1
            -
            
              θ
              
                Δ
                ⁢
                
                  t
                  ′
                
              
            
          
          )
        
      
    
  
  ,
  
    

  
  ⁢
  
    
      θ
      
        (
        
          Δ
          ⁢
          
            t
            ′
          
        
        )
      
    
    =
    
      1
      
        1
        +
        
          
            (
            
              
                Δ
                ⁢
                
                  t
                  ′
                
              
              γ
            
            )
          
          
            2
            ⁢
            n
          
        
      
    
  
  ,, here, P is a position after weight and fusion, Podam is the second real-time global coordinates obtained by the odometer in the world coordinate system, PIMU is the first real-time global coordinates obtained by the inertial measurement unit in the world coordinate system, θΔt′ is weight of the odometer, Δt′ is location duration time, γ is stable location duration time that the second real-time global coordinates obtained by the odometer in the world coordinate system reaches, n is time index parameter which depends on actual situations and is generally 3;
(S604) describing a particle weight update method by Gaussian distribution with a mean value of 0 and a variance of σ2, and updating a particle weight of the Monte Carlo algorithm, wherein the updated particle weight is expressed by a formula of, w
      k
      i
    
    =
    
      
        1
        
          
            
              2
              ⁢
              π
            
          
          ⁢
          σ
        
      
      ⁢
      
        e
        
          
            
              -
              
                
                  (
                  
                    
                      x
                      k
                      i
                    
                    -
                    
                      x
                      0
                      i
                    
                  
                  )
                
                2
              
            
            -
            
              
                (
                
                  
                    y
                    k
                    j
                  
                  -
                  
                    y
                    0
                    j
                  
                
                )
              
              2
            
          
          
            2
            ⁢
            
              σ
              2
            
          
        
      
    
  
  ,, here, (xki,yki) is position of the ith particle at time k, e is natural constant, x0i and y0i are initial positions of the ith particle, ωki is weight of the ith particle at time k, k is time, and then normalizing the updated particle weight;
(S605) calculating a current location of the robot according to the normalized updated particle weight through a formula of

Xk=Σi=1n(ωki·Pi),, here, n is a total number of the particles, Pi is a position of the ith particle after weight and fusion; and
(S606) according to the normalized updated particle weight, abandoning particles with less weight ωki and remaining particles with larger weight, which comprises:
(S6061) performing polynomial resample on the updated weights of all particles, and constructing a discrete cumulative distribution function by a formula of

F(i)=Σm=1iωki,, here, F(i) is a cumulative weight of the ith particle;
(S6062) generating a uniformly distributed random number set {uj} on [0,1], wherein uj is a set of random numbers generated above, j is the jth random number randomly generated in the set above, and judging the weight ωki, wherein the weight ωki is smaller if F(i)≤uj; the weight ωki is larger if F(i)>uj, copying a current particle as a new particle, and setting a weight of the new particle to be 1/N; and
(S6063) repeating polynomial resample in the step (S6062) for N times, obtaining N new particles, completing particle update, and using positions of the particles finally updated as locations of the robot in the world coordinate system.