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

Claim 1:
2. The global optimal robot visual positioning method based on point-line feature according to claim 1, wherein in step (4), dp(α) and dl(α) are unified as a translation-independent metric d(α):, d
      ⁡
      (
      α
      )
     
     =
     
     
      
       
        d
        1
       
       ⁢
       sinα
      
      +
      
       
        d
        2
       
       ⁢
       cosα
      
      +
      
       d
       3
      
     
    
   
  
  
   
    
     =
     
     
      
       
        
         
          d
          1
          2
         
         +
         
          d
          2
          2
         
        
       
       ⁢
       
        (
        
         
          
           sinαcos
           ⁢
           a
          
          2
         
         +
         
          
           cosαsin
           ⁢
           a
          
          2
         
        
        )
       
      
      +
      
       d
       3
      
     
    
   
  
  
   
    
     =
     
     
      
       
        a
        1
       
       ⁢
       
        sin
        ⁡
        (
        
         α
         +
         
          a
          2
         
        
        )
       
      
      +
      
       d
       3
      
     
    
   
  
 

 
  
   
    a
    1
   
   =
   
    
     
      d
      1
      2
     
     +
     
      d
      2
      2
     
    
   
  
  ,
  
   
    
     sin
     ⁢
     a
    
    2
   
   =
   
    
     d
     2
    
    
     d
     1
    
   
  
  ,
  
   
    
     cos
     ⁢
     a
    
    2
   
   =
   
    
     d
     1
    
    
     a
     1
    
   
  
 

where d1, d2, d3 are coefficients of d(α); a lower bound d(A) of the translation-independent metric of the yaw angle α is:, d
    _
   
   (
   A
   )
  
  =
  
   min
   ⁢
   
    
     ❘
     "\[LeftBracketingBar]"
    
    
     
      
       a
       1
      
      ⁢
      
       sin
       ⁡
       (
       
        α
        +
        
         a
         2
        
       
       )
      
     
     +
     
      d
      3
     
    
    
     ❘
     "\[RightBracketingBar]"
    
   
  
 

where A is an interval subset including the yaw angle α, α∈A; the maximizing consensus set problem corresponding to d(A) is:, max
   
    
     R
     ⁡
     (
     α
     )
    
    ,
    
     {
     
      z
      *
     
     }
    
    ,
    
     α
     ∈
     A
    
   
  
  
   ∑
   
    z
    *
   
  
 

 
  
   
    
     s
     .
     t
     .
        
     
      z
      ij
     
    
    ⁢
    
     
      d
      
       p
       ,
       ij
      
     
     (
     A
     )
    
   
   ≤
   
    n
    ij
   
  
  ,
  i
  ,
  
   j
   ∈
   𝔅
  
 

 
  
   
    
     z
     k
    
    ⁢
    
     
      d
      
       l
       ,
       k
      
     
     (
     A
     )
    
   
   ≤
   
    n
    k
   
  
  ,
  
   k
   ∈
   𝔏
  
 

where dp,ij(A) is a translation-independent metric derived from point features on A, and dl,k(A) is a translation-independent metric derived from line features on A; a number of inner points in the consensus set corresponding to the yaw angle α is recorded as an energy function E(α), and an upper bound of E(α) is recorded as E(A):, E
     ⁡
     (
     α
     )
    
    ≤
    
     
      E
      _
     
     (
     A
     )
    
   
   =
   
    ∑
    
     
      z
      ^
     
     *
    
   
  
  ,
  
   α
   ∈
   A
  
 

where {circumflex over (z)}* is a number of inner points of the optimal consensus set obtained by solving the maximizing consensus set problem;
a whole global optimal rotation solving comprises the following steps: initializing a range [−π, π] of the yaw angle into a plurality of subsets A, the plurality of subsets A forming a sequence q, and initializing an optimal value of the energy function and an optimal value of the yaw angle; before q is empty, cyclically executing the following operations: taking out a first subset A of q to calculate Ē(A), wherein if Ē(A) is greater than a current optimal value of the energy function, E(αc) is calculated according to a center αc of the subset A, while if E(αc) is also greater than the current optimal value of the energy function, the current optimal value of the energy function is updated as E(αc), and the current optimal value of the yaw angle is αc; if Ē(A) is smaller than the current optimal value of the energy function, continuing to traverse remaining subsets A until q is empty, and returning to a final optimal value of the yaw angle.