Patent ID: 11938629
Assignee: JIANGNAN UNIVERSITY
Field: Medical technology (Instruments)
Classification: CPC A  B  H | IPC B

Claim 4:
5. The micro-robot control method based on double closed loop three-dimensional path tracking according to claim 4, wherein the obtaining, by the cameras, current pose information of the magnetic micro-robot, and feeding back the current pose information to the host computer to obtain three-dimensional coordinates of a center of mass and an actual axial direction of the magnetic micro-robot comprises:
detecting the current pose information of the magnetic micro-robot by using a side camera and a top camera, fixing a coordinate system of the top camera as a world coordinate system, and calculating an actual axial direction of the magnetic micro-robot in the world coordinate system by the following formula: XB=ctht×cths=ctht×(ctRcscshs),
wherein ctht represents a normal vector of a plane of the top camera, cshs represents a normal vector of a plane of the side camera, cths represents a transformation from the normal vector of the plane of the side camera to the plane of the top camera, ctRcs represents a rotation matrix from a coordinate system of the side camera to the coordinate system of the top camera, and XB represents the actual axial direction of the magnetic micro-robot in the world coordinate system; and
calculating three-dimensional coordinates of the center of mass of the magnetic micro-robot in the world coordinate system by the following formula:, G
   =
   
    
     
      
        
       ct
      
      
       R
       cs
      
     
     ⁢
     
      
        
       cs
      
      G
     
    
    +
    
     
       
      ct
     
     
      t
      
       c
       ⁢
       s
      
     
    
   
  
  ,
 

wherein csG represents coordinates of the center of mass in the plane of the side camera, cttcs represents a translation matrix from the coordinate system of the side camera to the coordinate system of the top camera, and G represents the three-dimensional coordinates of the center of mass in the world coordinate system.