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

Claim 6:
7. The autonomous mapping method according to claim 6, wherein the step (S4) specifically comprises:
(S401): at time t, constructing the state vector Xc(t)=[xt, yt, θt, vt, ωt]T, wherein xt and yt are the positioning global coordinates of the robot in the world coordinate system, θt is the heading angle of the robot in the world coordinate system, vt is the speed of the robot in the world coordinate system, ωt is the wheel angular velocity of the robot in the world coordinate system, and T is matrix transposition;
(S402) constructing the state model of the Kalman filter according to formulas of, X
      
        c
        ⁡
        
          (
          
            t
            +
            1
          
          )
        
      
    
    =
    
      
        f
        ⁡
        
          (
          
            X
            
              c
              ⁡
              
                (
                
                  t
                  +
                  1
                
                )
              
            
          
          )
        
      
      +
      
        W
        t
      
    
  
  ,
  
    

  
  ⁢
  
    
      X
      
        c
        ⁡
        
          (
          
            t
            +
            1
          
          )
        
      
    
    =
    
      
        [
        
          
            
              
                x
                
                  t
                  +
                  1
                
              
            
          
          
            
              
                y
                
                  t
                  +
                  1
                
              
            
          
          
            
              
                θ
                
                  t
                  +
                  1
                
              
            
          
          
            
              
                v
                
                  t
                  +
                  1
                
              
            
          
          
            
              
                ω
                
                  t
                  +
                  1
                
              
            
          
        
        ]
      
      =
      
        
          [
          
            
              
                
                  
                    x
                    t
                  
                  +
                  
                    
                      v
                      t
                    
                    ⁢
                    cos
                    ⁢
                    
                      
                    
                    ⁢
                    
                      (
                      
                        
                          θ
                          t
                        
                        +
                        
                          
                            ω
                            t
                          
                          ⁢
                          Δ
                          ⁢
                          t
                        
                      
                      )
                    
                    ⁢
                    Δ
                    ⁢
                    t
                  
                
              
            
            
              
                
                  
                    y
                    t
                  
                  +
                  
                    
                      v
                      t
                    
                    ⁢
                    sin
                    ⁢
                    
                      
                    
                    ⁢
                    
                      (
                      
                        
                          θ
                          t
                        
                        +
                        
                          
                            ω
                            t
                          
                          ⁢
                          Δ
                          ⁢
                          t
                        
                      
                      )
                    
                    ⁢
                    Δ
                    ⁢
                    t
                  
                
              
            
            
              
                
                  
                    θ
                    t
                  
                  +
                  
                    
                      ω
                      t
                    
                    ⁢
                    Δ
                    ⁢
                    t
                  
                
              
            
            
              
                
                  v
                  t
                
              
            
            
              
                
                  ω
                  t
                
              
            
          
          ]
        
        +
        
          W
          t
        
      
    
  
  ,, wherein Xc(t+1) is the state vector at time t+1, f(Xc(t+1)) is a nonlinear state transition function of the state vector Xc(t+1) at time t+1, Wt is process noise of the Kalman filter, Δt is time interval between two adjacent moments;
(S403) dividing the Kalman filter into a first sub-filter and a second sub-filter which are independent from each other and in parallel, wherein:
an observation model of the first sub-filter is expressed by Z1(t+1)=h1Xc(t)+W1(t), which is specifically, Z
      
        1
        ⁢
        
          (
          
            t
            +
            1
          
          )
        
      
    
    =
    
      
        [
        
          
            
              
                Z
                
                  L
                  ⁢
                  a
                  ⁢
                  s
                
              
            
          
          
            
              
                Z
                IMU
              
            
          
        
        ]
      
      =
      
        
          
            [
            
              
                
                  1
                
                
                  0
                
                
                  0
                
                
                  0
                
                
                  0
                
              
              
                
                  0
                
                
                  1
                
                
                  0
                
                
                  0
                
                
                  0
                
              
              
                
                  0
                
                
                  0
                
                
                  1
                
                
                  0
                
                
                  0
                
              
              
                
                  0
                
                
                  0
                
                
                  0
                
                
                  0
                
                
                  1
                
              
            
            ]
          
          ⁢
          
            X
            
              c
              ⁡
              
                (
                t
                )
              
            
          
        
        +
        
          W
          
            1
            ⁢
            
              (
              t
              )
            
          
        
      
    
  
  ,, here, ZLas is the observation model of the LiDAR, ZIMU is the observation model of the inertial measurement unit, W1(t) is a sum of noise of the LiDAR and the inertial measurement unit, h1 is an observation matrix of the first sub-filter,
an observation model of the second sub-filter is expressed by Z2(t+1)=h2Xc(t)+W2(t), which is specifically, Z
      
        2
        ⁢
        
          (
          
            t
            +
            1
          
          )
        
      
    
    =
    
      
        [
        
          
            
              
                Z
                odom
              
            
          
          
            
              
                Z
                IMU
              
            
          
        
        ]
      
      =
      
        
          
            [
            
              
                
                  1
                
                
                  0
                
                
                  0
                
                
                  0
                
                
                  0
                
              
              
                
                  0
                
                
                  1
                
                
                  0
                
                
                  0
                
                
                  0
                
              
              
                
                  0
                
                
                  0
                
                
                  0
                
                
                  1
                
                
                  0
                
              
              
                
                  0
                
                
                  0
                
                
                  0
                
                
                  0
                
                
                  1
                
              
              
                
                  0
                
                
                  0
                
                
                  1
                
                
                  0
                
                
                  0
                
              
              
                
                  0
                
                
                  0
                
                
                  0
                
                
                  0
                
                
                  1
                
              
            
            ]
          
          ⁢
          
            X
            
              c
              ⁡
              
                (
                t
                )
              
            
          
        
        +
        
          W
          
            2
            ⁢
            
              (
              t
              )
            
          
        
      
    
  
  ,, here, Zodom is the observation model of the odometer, W2(t) is a sum of noise of the odometer and the inertial measurement unit, h2 is an observation matrix of the second sub-filter; and
(S404) processing covariance Q(t) and estimated error covariance P(t) of the process noise Wt of the Kalman filter through formulas of

Q1(t′)=α1−1Q(t),

P1(t′)=(1−α1−1)P(t),

Q2(t′)=α2−1Q(t),

Q2(t′)=α2−1Q(t),

Xc(t)=Xl(t),, wherein Q1(t′) is covariance of the process noise of every sub-filter at time t, P1(t) is estimated error covariance of every sub-filter at time t, α1 is weight distribution coefficient of the first sub-filter; Q2(t′) is covariance of the process noise of every sub-filter at time t, P2(t′) is estimated error covariance of every sub-filter at time t, α2 is weight distribution coefficient of the second sub-filter; Xc(t) is global optimal solution of state vector at time t, Xl(t) is global optimal solution of state vector of every sub-filter at time t.