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

Claim 0:
1. A global optimal robot visual positioning method based on point-line features, applied in a visual positioning system, for improving positioning accuracy of the robot's self-positioning, comprising the following steps:
(1) acquiring a priori three-dimensional map of a current scene, the priori three-dimensional map including measurement data of gravity acceleration and three-dimensional point-line features;
(2) acquiring a current camera image of the robot and a measured gravity acceleration;
(3) calculating a pitch angle and a roll angle of a current pose of the robot according to the matched two-dimensional to a measured gravity acceleration of the robot and the measurement data of gravity acceleration in the priori three-dimensional map; detecting two-dimensional point-line features of the current camera image of the robot, and matching the detected two-dimensional point-line features with the three-dimensional point-line features in the priori three-dimensional map;
(4) constructing translation-independent metrics according to matched two-dimensional to three-dimensional point-line features, and decoupling a robot pose solving problem into two sub-problems of rotation and translation, the two sub-problems comprising solving a rotation unknown and then solving a translation unknown, wherein, the rotation unknown is a yaw angle, the rotation unknown being solved by one-dimensional search enumeration completed by a branch-and-bound algorithm, and the translation unknown is solved by one-dimensional search voting on components in three directions of the translation unknown, respectively; obtaining an optimal solution of global pose estimation;
(5) utilizing the optimal solution of global pose estimation for self-positioning of the robot,
wherein in step (3), the gravity acceleration [xc yc zc]T measured in a coordinate system of the robot and the gravity acceleration [xw tw zw]T measured in a coordinate system of the priori three-dimensional map are aligned to obtain the pitch angle β and the roll angle γ of the current coordinate system of the robot relative to the coordinate system of the priori three-dimensional map, the pitch angle β and the roll angle γ are specifically as follows:, [
      
       
        
         1
        
        
         0
        
        
         0
        
       
       
        
         0
        
        
         
          cos
          ⁢
          
           (
           γ
           )
          
         
        
        
         
          
           -
           
            s
            ⁢
            in
           
          
          ⁢
          
           (
           γ
           )
          
         
        
       
       
        
         0
        
        
         
          sin
          ⁢
          
           (
           γ
           )
          
         
        
        
         
          cos
          ⁢
          
           (
           γ
           )
          
         
        
       
      
      ]
     
     [
     
      
       
        
         cos
         ⁢
         
          (
          β
          )
         
        
       
       
        0
       
       
        
         sin
         ⁢
         
          (
          β
          )
         
        
       
      
      
       
        0
       
       
        1
       
       
        0
       
      
      
       
        
         
          -
          
           s
           ⁢
           in
          
         
         ⁢
         
          (
          β
          )
         
        
       
       
        0
       
       
        
         cos
         ⁢
         
          (
          β
          )
         
        
       
      
     
     ]
    
    [
    
     
      
       
        x
        c
       
      
     
     
      
       
        y
        c
       
      
     
     
      
       
        z
        c
       
      
     
    
    ]
   
   =
   
    [
    
     
      
       
        x
        w
       
      
     
     
      
       
        y
        w
       
      
     
     
      
       
        z
        w
       
      
     
    
    ]
   
  
  ;
 

in step (3), a distance between corresponding descriptors of the two-dimensional point-line features of the current image of the robot and the three-dimensional point-line features of the priori three-dimensional map in a metric space is calculated, and the features with the distance which is less than a preset threshold value are matched to obtain a plurality of pairs of matched features;
in step (4), a point pi in the three-dimensional map and a corresponding matched point ui in a two-dimensional image satisfy the following relationship:, u
   i
  
  =
  
   
    π
    ⁡
    (
    
     
      
       Rp
       i
      
      +
      t
     
     ,
     K
    
    )
   
   +
   
    o
    i
   
   +
   
    e
    i
   
  
 

where R is a rotation matrix, π is a camera projection equation of a camera internal reference K, ei is observation noise; a wrong match is an outer point, a correct match is an inner point, oi is zero for the inner point and is arbitrary for the outer point, and a problem of robot pose estimation is transformed into a problem of a maximizing consensus set:, max
   
    R
    ,
    t
    ,
    
     {
     
      z
      i
     
     }
    
   
  
  
   ∑
   
    z
    i
   
  
 

 
  
   
    
     s
     .
     t
     .
        
     
      z
      i
     
    
    ⁢
    
     
      ❘
      "\[LeftBracketingBar]"
     
     
      
       u
       i
      
      -
      
       π
       ⁡
       (
       
        
         
          Rp
          i
         
         +
         t
        
        ,
        K
       
       )
      
     
     
      ❘
      "\[RightBracketingBar]"
     
    
   
   ≤
   
    n
    i
   
  
  ,
  
   i
   ∈
     
 

where t is a translation vector; zi=0 means oi is 0, zi=1 means oi is not 0; ni>|ei| is a bound of the observation noise;  is a set of three-dimensional to two-dimensional matched point features;
in step (4), a non-normalized vector ũi is obtained from a center of the camera according to the point ui in the two-dimensional image:, u
    ~
   
   i
  
  
   =
   Δ
  
  
   
    (
    
     
      
       
        
         u
         ~
        
        
         i
         ,
         x
        
       
      
     
     
      
       
        
         u
         ~
        
        
         i
         ,
         y
        
       
      
     
     
      
       1
      
     
    
    )
   
   =
   
    
     K
     
      -
      1
     
    
    (
    
     
      
       
        u
        i
       
      
     
     
      
       1
      
     
    
    )
   
  
 

the point pi in the three-dimensional map corresponding to the point ui in the two-dimensional image is transformed into a coordinate system of the camera:, R
      1
     
     ⁢
     
      p
      i
     
    
    +
    
     t
     x
    
   
   
    
     u
     ~
    
    
     i
     ,
     x
    
   
  
  =
  
   
    
     
      
       R
       2
      
      ⁢
      
       p
       i
      
     
     +
     
      t
      y
     
    
    
     
      u
      ~
     
     
      i
      ,
      y
     
    
   
   =
   
    
     
      R
      3
     
     ⁢
     
      p
      i
     
    
    +
    
     t
     z
    
   
  
 

Where R(R1T, R2T, R3T)T, t(tx, ty, tz)T; two constraints are obtained from the above formula, and two constraints are further obtained similarly from another group of three-dimensional to two-dimensional matched features, and a translation amount t is eliminated by combining the above four constraints to obtain a translation-independent metric dp(α) derived from feature matching points:, d
    p
   
   (
   α
   )
  
  =
  
   
    
     d
     
      p
      ,
      1
     
    
    ⁢
    sin
    ⁢
    α
   
   +
   
    
     d
     
      p
      ,
      2
     
    
    ⁢
    cos
    ⁢
    α
   
   +
   
    d
    
     p
     ,
     3
    
   
  
 

where dp,1, dp,2, dp,3 are coefficients of dp(α); α is the yaw angle; the robot pose estimation problem is transformed into the following maximizing consensus set problem:, max
   
    
     R
     ⁡
     (
     α
     )
    
    ,
    
     {
     
      z
      ij
     
     }
    
   
  
  
   ∑
   
    z
    ij
   
  
 

 
  
   
    
     s
     .
     t
     .
        
     
      z
      ij
     
    
    ⁢
    
     
      ❘
      "\[LeftBracketingBar]"
     
     
      
       d
       
        p
        ,
        ij
       
      
      (
      α
      )
     
     
      ❘
      "\[RightBracketingBar]"
     
    
   
   ≤
   
    n
    ij
   
  
  ,
  i
  ,
  
   j
   ∈
   𝔅
  
 

where nij=min(ni, nj); zij=1 means that feature matching points of an ith group and a jth group are all inner points, otherwise, zij=0;
in step (4), two ends ũk1 and ũk2 of a two-dimensional line segment and a point pk on a corresponding three-dimensional line segment satisfy the following relationship:, (
     
      
       
        u
        ~
       
       
        k
        ⁢
        1
       
      
      ×
      
       
        u
        ~
       
       
        k
        ⁢
        2
       
      
     
     )
    
    T
   
   ⁢
   
    (
    
     
      Rp
      k
     
     +
     t
    
    )
   
  
  =
  0
 

a constraint is obtained from the above formula, another point on the three-dimensional line segment is arbitrarily taken to obtain another constraint, and the translation amount t is eliminated by combining the above two constraints to obtain a translation-independent metric dl(α) derived from line feature matching:, d
    l
   
   (
   α
   )
  
  =
  
   
    
     d
     
      l
      ,
      1
     
    
    ⁢
    sinα
   
   +
   
    
     d
     
      l
      ,
      2
     
    
    ⁢
    cosα
   
   +
   
    d
    
     l
     ,
     3
    
   
  
 

where dl,1, dl,2, dl,3 are coefficients of dl(α); the robot pose estimation problem is transformed into the following maximizing consensus set problem:, max
   
    
     R
     ⁡
     (
     α
     )
    
    ,
    
     {
     
      z
      *
     
     }
    
   
  
  
   ∑
   
    z
    *
   
  
 

 
  
   
    
     s
     .
     t
     .
        
     
      z
      ij
     
    
    ⁢
    
     
      ❘
      "\[LeftBracketingBar]"
     
     
      
       d
       
        p
        ,
        ij
       
      
      (
      α
      )
     
     
      ❘
      "\[RightBracketingBar]"
     
    
   
   ≤
   
    n
    ij
   
  
  ,
  i
  ,
  
   j
   ∈
   𝔅
  
 

 
  
   
    
     z
     k
    
    ⁢
    
     
      ❘
      "\[LeftBracketingBar]"
     
     
      
       d
       
        l
        ,
        k
       
      
      (
      α
      )
     
     
      ❘
      "\[RightBracketingBar]"
     
    
   
   ≤
   
    n
    k
   
  
  ,
  
   k
   ∈
   𝔏
  
 

where z* is used to refer to zij and zk; wherein zk=1 indicates that the feature matching corresponding to a kth line feature is an inner point, and zij=1 indicates that the feature matchings corresponding to ith and jth point features are inner points;  is a set of line features, and nk is a bound of the observation noise corresponding to the kth line feature.