Patent ID: 11964401
Assignee: ZHEJIANG UNIVERSITY
Field: Control (Instruments)
Classification: CPC B  G | IPC B  G

Claim 2:
3. The global optimal robot visual positioning method based on point-line feature according to claim 2, wherein in step (4), when the yaw angle is known, the maximizing consensus set problem regarding the translation amount is:, max
   
    t
    ,
    
     {
     
      z
      i
     
     }
    
   
  
  
   ∑
   
    z
    i
   
  
 

 
  
   
    
     s
     .
     t
     .
        
     
      z
      i
     
    
    ⁢
    
     
      ❘
      "\[LeftBracketingBar]"
     
     
      
       
        A
        i
       
       ⁢
       t
      
      +
      
       b
       i
      
     
     
      ❘
      "\[RightBracketingBar]"
     
    
   
   ≤
   
    n
    i
   
  
  ,
  
   i
   ∈
   
    𝔅
    ⋃
    𝔏
   
  
 

where Ai and bi are coefficients of a linear function regarding the translation amount corresponding to the known yaw angle;
according to a set of three-dimensional to two-dimensional matched features with at least one point feature, the maximizing consensus set problem regarding the translation amount is updated as follows:, max
   
    t
    ,
    
     {
     
      z
      ij
     
     }
    
   
  
  
   ∑
   
    z
    ij
   
  
 

 
  
   
    
     s
     .
     t
     .
        
     
      z
      ij
     
    
    ⁢
    
     
      ❘
      "\[LeftBracketingBar]"
     
     
      
       
        t
        ^
       
       ij
      
      -
      t
     
     
      ❘
      "\[RightBracketingBar]"
     
    
   
   ≤
   
    n
    ij
   
  
  ,
  
   i
   ∈
   𝔅
  
  ,
  
   j
   ∈
   
    𝔅
    ⋃
    𝔏
   
  
 

where {circumflex over (t)}ij is an estimation of the translation amount; the translation amount is decoupled into three components in x, y and z directions, and the maximizing consensus set problem regarding the translation amount in x direction is as follows:, max
   
    
     t
     x
    
    ,
    
     {
     
      z
      ij
     
     }
    
   
  
  
   ∑
   
    z
    ij
   
  
 

 
  
   
    
     s
     .
     t
     .
        
     
      z
      ij
     
    
    ⁢
    
     
      ❘
      "\[LeftBracketingBar]"
     
     
      
       
        t
        ^
       
       
        ij
        ,
        x
       
      
      -
      
       t
       x
      
     
     
      ❘
      "\[RightBracketingBar]"
     
    
   
   ≤
   
    n
    
     ij
     ,
     x
    
   
  
  ,
  
   i
   ∈
   𝔅
  
  ,
  
   j
   ∈
   
    𝔅
    ⋃
    𝔏
   
  
 

similar to the maximizing consensus set problem regarding the translation amount in x direction, the maximizing consensus set problem regarding the translation amount in y and z directions can be obtained; according to geometric constraints, all feature combinations are traversed to obtain all possible {circumflex over (t)}ij and corresponding upper bounds tij and lower bounds tij thereto; by voting for all {circumflex over (t)}ij, a feature matching support set of consistent estimation results is obtained, and then a global optimal translation estimation is obtained;
a whole global optimal translation solving comprises the following steps: initializing the optimal value of the energy function as 0 according to all translation estimation results and corresponding upper and lower bounds thereto, wherein an optimal inner point set is empty; voting for a translated x component to obtain all voting results and a corresponding voting feature set Sx, and sorting the set in a descending order according to a size of the feature set; traversing each voting result in Sx and feature set Sx([i]): ending if the size of the feature set Sx([i]) is less than the optimal value of the current energy function, and proceeding to the next step if the size of the feature set Sx([i]) is no less than the optimal value of the current energy function; voting for all elements in the feature set of Sx([i]) in a translation y component to obtain a set Sy; traversing each voting set and feature set Sy([j]) in Sy: ending if the size of the feature set Sy([j]) is less than the optimal value of the current energy function, and proceeding to the next step if the size of the feature set Sy([j]) is no less than the optimal value of the current energy function; voting for all elements in the feature set Sy([j]) in a translation z component to obtain a set Sz; if the size of the voting feature set corresponding to a result of a maximum voting number in Sz is larger than the optimal value of the current energy function, updating a larger value to the optimal value of the current energy function, updating the corresponding feature set to a current optimal inner point set, and continuing to traverse; otherwise, continuing to traverse directly instead of updating.