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

Claim 5:
6. The micro-robot control method based on double closed loop three-dimensional path tracking according to claim 4, wherein the state space equations are:, 
   
    {
    
     
      
       
        
         s
         .
        
        =
        
         
          v
          ⁢
          cos
          ⁢
          
           θ
           
            d
            ⁢
            e
           
          
          ⁢
          cos
          ⁢
          
           θ
           ie
          
         
         
          1
          -
          
           c
           ⁢
           
            d
            y
           
          
         
        
       
      
     
     
      
       
        
         
          d
          .
         
         y
        
        =
        
         
          v
          ⁢
          sin
          ⁢
          
           θ
           
            d
            ⁢
            e
           
          
          ⁢
          cos
          ⁢
          
           θ
           ie
          
         
         +
         
          τ
          ⁢
          
           d
           z
          
          ⁢
          
           s
           .
          
         
        
       
      
     
     
      
       
        
         d
         z
        
        =
        
         
          
           -
           v
          
          ⁢
          sin
          ⁢
          
           θ
           ie
          
         
         -
         
          τ
          ⁢
          
           d
           y
          
          ⁢
          
           s
           .
          
         
        
       
      
     
     
      
       
        
         
          θ
          .
         
         ie
        
        =
        
         
          
           Ω
           y
          
          ⁢
          cos
          ⁢
          
           δ
           
            θ
            ⁢
            d
           
          
         
         -
         
          
           Ω
           z
          
          ⁢
          sin
          ⁢
          
           δ
           
            θ
            ⁢
            d
           
          
          ⁢
          sin
          ⁢
          
           δ
           
            θ
            ⁢
            i
           
          
         
         -
         
          
           
            δ
            .
           
           
            θ
            ⁢
            i
           
          
          ⁢
          cos
          ⁢
          
           δ
           
            θ
            ⁢
            d
           
          
         
         +
         
          τ
          ⁢
          
           s
           .
          
          ⁢
          sin
          ⁢
          
           θ
           
            d
            ⁢
            e
           
          
         
        
       
      
     
     
      
       
        
         
          θ
          .
         
         
          d
          ⁢
          e
         
        
        =
        
         
          
           Ω
           z
          
          ⁢
          
           
            cos
            ⁢
            
             δ
             
              θ
              ⁢
              i
             
            
           
           
            cos
            ⁢
            
             θ
             ie
            
           
          
         
         +
         
          
           
            δ
            .
           
           
            θ
            ⁢
            d
           
          
          
           cos
           ⁢
           
            θ
            ie
           
          
         
         -
         
          τ
          ⁢
          
           
             
            s
           
           .
          
          ⁢
          tan
          ⁢
          
           θ
           ie
          
          ⁢
          cos
          ⁢
          
           θ
           
            d
            ⁢
            e
           
          
         
         -
         
          c
          ⁢
          
           s
           .
          
         
        
       
      
     
    
   
  
  ,
 

wherein s represents the three-dimensional coordinates of the desired position point, v represents an actual movement speed, θde represents the direction angle error, θie represents the pitch angle error, c represents the curvature, τ represents the torsion, dy represents the vertical distance, dz represents the horizontal distance, Ωy represents an angular velocity component of the magnetic micro-robot on a Y axis, Ωz represents an angular velocity component of the magnetic micro-robot on a Z axis, δθi represents a compensation pitch angle, and δθd represents a compensation direction angle.