Patent ID: 11971726
Assignee: WUHAN UNIVERSITY OF SCIENCE AND TECHNOLOGY
Field: Control (Instruments)
Classification: CPC G | IPC G

Claim 4:
5. The method according to claim 1, wherein
in 4), the point cloud coordinate transformation is described as follows:
transforming the obtained the point cloud coordinate (X′w,Y′w,Z′w) of the non-wall corner semantics in the camera coordinate system into a robot platform coordinate system (Xrobot,w,Yrobot,w,Zrobot,w) in the following relationship:, [
    
     
      
       
        X
        
         robot
         ,
         w
        
       
      
     
     
      
       
        Y
        
          
         
          robot
           
          ,
          w
         
        
       
      
     
     
      
       
        Z
        
          
         
          robot
          ,
          w
         
        
       
      
     
     
      
       1
      
     
    
    ]
   
   =
   
    
     [
     
      
       
        
         R
         1
        
       
       
        
         T
         1
        
       
      
      
       
        
         0
         T
        
       
       
        1
       
      
     
     ]
    
    [
    
     
      
       
        X
        w
        ′
       
      
     
     
      
       
        Y
        w
        ′
       
      
     
     
      
       
        Z
        w
        ′
       
      
     
     
      
       1
      
     
    
    ]
   
  
  ;
 

and transforming the obtained point cloud coordinate (X′e,Y′e,Z′e) of the wall corner semantics in the camera coordinate system into a robot platform coordinate system (Xrobot,e,Yrobot,e,Zrobot,e) in the following relationship:, [
    
     
      
       
        X
        
          
         
          robot
          ,
          e
         
        
       
      
     
     
      
       
        Y
        
          
         
          robot
          ,
          e
         
        
       
      
     
     
      
       
        Z
        
          
         
          robot
          ,
          e
         
        
       
      
     
     
      
       1
      
     
    
    ]
   
   =
   
    
     [
     
      
       
        
         R
         1
        
       
       
        
         T
         1
        
       
      
      
       
        
         0
         T
        
       
       
        1
       
      
     
     ]
    
    [
    
     
      
       
        X
        e
        ′
       
      
     
     
      
       
        Y
        e
        ′
       
      
     
     
      
       
        Z
        e
        ′
       
      
     
     
      
       1
      
     
    
    ]
   
  
  ;
 

in the above formula, R1 and T1 refer to a 3*3 rotation matrix and a 3*1 translation matrix between a Kinect v2 depth camera coordinate system and a mobile robot platform coordinate system respectively, which are determined based on a mounting position relationship of the depth camera on the robot platform, that is, 0T=(0,0,0);
the non-wall corner semantic point cloud coordinate (Xrobot,w,Yrobot,w,Zrobot,w) and the wall corner semantic point cloud coordinate (Xrobot,e,Yrobot,e,Zrobot,e) in the robot platform coordinate system as well as the wall corner and non-wall corner semantic point cloud coordinate set R={(XR,f,YR,f,ZR,f)} are all obtained through the above transformation, where a subscript f refers to the f-th semantic point in the set; for convenience of expression, the following semantic point cloud coordinates in the robot platform coordinate system are uniformly expressed by (XR,YR,ZR);
then, the coordinate (XWo,YWo,ZWo) in a world coordinate system is obtained by transforming the coordinate (XR,YR,ZR) in the robot platform coordinate system in combination with the real-time pose (xo,k,yo,k,θo,k) of the robot platform obtained based on the Gmapping map construction algorithm as follows:
a transformation relationship between the mobile robot platform coordinate system and the world coordinate system is:, [
    
     
      
       
        X
        
         W
         ⁢
         o
        
       
      
     
     
      
       
        Y
        
         W
         ⁢
         o
        
       
      
     
     
      
       
        Z
        
         W
         ⁢
         o
        
       
      
     
     
      
       1
      
     
    
    ]
   
   =
   
    
     [
     
      
       
        
         R
         2
        
       
       
        
         T
         2
        
       
      
      
       
        
         0
         T
        
       
       
        1
       
      
     
     ]
    
    [
    
     
      
       
        X
        R
       
      
     
     
      
       
        Y
        R
       
      
     
     
      
       
        Z
        R
       
      
     
     
      
       1
      
     
    
    ]
   
  
  ;
 

in the above formulas, R2 and T2 refer to a 3*3 rotation matrix and a 3*1 translation matrix between the mobile robot platform coordinate system and the real world coordinate system respectively, and 0T is (0,0,0);, R
   2
  
  =
  
   [
   
    
     
      
       cos
       ⁢
       
        θ
        
         o
         ,
         k
        
       
      
     
     
      
       
        -
         
        sin
       
       ⁢
       
        θ
        
         o
         ,
         k
        
       
      
     
     
      0
     
    
    
     
      
       sin
       ⁢
       
        θ
        
         o
         ,
         k
        
       
      
     
     
      
       cos
       ⁢
       
        θ
        
         o
         ,
         k
        
       
      
     
     
      0
     
    
    
     
      0
     
     
      0
     
     
      1
     
    
   
   ]
  
 

 
  
   
    T
    2
   
   =
   
    [
    
     
      
       
        x
        
         o
         ,
         k
        
       
      
     
     
      
       
        y
        
         o
         ,
         k
        
       
      
     
     
      
       
        θ
        
         o
         ,
         k
        
       
      
     
    
    ]
   
  
  ;
 

finally, the coordinate (Xg,Yg) in an object grid map coordinate system is obtained by transforming the coordinate (XWo,YWo) in the real world coordinate system:, {
  
   
    
     
      
       
        X
        g
       
       =
       
        ceil
        ⁢
        
         (
         
          
           X
           Wo
          
          r
         
         )
        
       
      
     
    
    
     
      
       
        Y
        g
       
       =
       
        ceil
        ⁡
        (
        
         
          Y
          
           W
           ⁢
           o
          
         
         r
        
        )
       
      
     
    
   
   ;
  
 

in the above formula, (XWo,YWo) refer to a coordinate of a semantic point in the world coordinate system, (Xg,Yg) refer to a coordinate of the point in the object grid map coordinate system, r refers to a unit resolution of the object grid map, and cell refers to a symbol of rounding up;
the coordinate value ((Xg,Yg)) of each semantic point in the object grid map coordinate system is obtained through a series of coordinate transformations described above, and each finally-obtained semantic point is marked on the constructed object grid color map; different object types gs are marked by different colors.