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

Claim 6:
7. The micro-robot control method based on double closed loop three-dimensional path tracking according to claim 6, wherein the designing a position closed loop controller through the state space equations to obtain a required rotating magnetic field comprises:
linearizing the state space equations according to a chain rule and a path tracking task to obtain inputs of the position closed loop controller:, {
   
    
     
      
       
        u
        1
       
       =
       
        
         v
         ⁢
         cos
         ⁢
         
          θ
          
           d
           ⁢
           e
          
         
         ⁢
         cos
         ⁢
         
          θ
          ie
         
        
        
         1
         -
         
          c
          ⁢
          
           d
           y
          
         
        
       
      
     
    
    
     
      
       
        u
        2
       
       =
       
        
         
          -
          
           k
           
            d
            ⁢
            1
           
          
         
         ⁢
         
          u
          1
         
         ⁢
         
          d
          y
         
        
        -
        
         
          k
          
           t
           ⁢
           1
          
         
         ⁢
         
          
           ❘
           "\[LeftBracketingBar]"
          
          
           u
           1
          
          
           ❘
           "\[RightBracketingBar]"
          
         
         ⁢
         
          (
          
           
            τ
            ⁢
            
             d
             z
            
           
           +
           
            
             (
             
              1
              -
              
               c
               ⁢
               
                d
                y
               
              
             
             )
            
            ⁢
            tan
            ⁢
            
             θ
             
              d
              ⁢
              e
             
            
           
          
          )
         
        
       
      
     
    
    
     
      
       
        u
        3
       
       =
       
        
         
          -
          
           k
           
            d
            ⁢
            2
           
          
         
         ⁢
         
          u
          1
         
         ⁢
         
          d
          z
         
        
        -
        
         
          k
          
           t
           ⁢
           2
          
         
         ⁢
         
          
           ❘
           "\[LeftBracketingBar]"
          
          
           u
           1
          
          
           ❘
           "\[RightBracketingBar]"
          
         
         ⁢
         
          (
          
           
            
             -
             τ
            
            ⁢
            
             d
             y
            
           
           +
           
            
             (
             
              
               c
               ⁢
               
                d
                y
               
              
              -
              1
             
             )
            
            ⁢
            cos
            ⁢
            
             1
             
              θ
              
               d
               ⁢
               e
              
             
            
            ⁢
            tan
            ⁢
            
             θ
             ie
            
           
          
          )
         
        
       
      
     
    
   
  
  ,
 

wherein kd1, kt1, kd2, and kt2 are control gains and are positive;
the path tracking task is configured to enable the direction angle error, the pitch angle error, the vertical distance, and the horizontal distance to converge to zero; and
an output of the position closed loop controller is:, {
  
    
      
        
          
            
              Ω
              z
            
            =
            
              
                (
                
                  
                    u
                    2
                  
                  -
                  
                    γ
                    22
                  
                
                )
              
              ⁢
              
                γ
                21
                
                  -
                  1
                
              
            
          
        
      
      
        
          
            
              Ω
              y
            
            =
            
              (
              
                
                  u
                  3
                
                -
                
                  γ
                  33
                
                -
                
                  
                    γ
                    32
                  
                  ⁢
                  
                    
                      γ
                      21
                      
                        -
                        1
                      
                    
                    ⁡
                    
                      (
                      
                        
                          u
                          2
                        
                        -
                        
                          γ
                          22
                        
                      
                      )
                    
                  
                  ⁢
                  
                    γ
                    31
                    
                      -
                      1
                    
                  
                
              
              )
            
          
        
      
    
    ,
  

wherein specific expressions of γ21, γ22, γ31, γ32, and γ33 are:, 
  
   {
   
    
     
      
       
        
         γ
         
          2
          ⁢
          1
         
        
        =
        
         v
         ⁢
         
          
           s
           .
          
          
           -
           1
          
         
         ⁢
         
          cos
          
           -
           1
          
         
         ⁢
         
          θ
          
           d
           ⁢
           e
          
         
         ⁢
         cos
         ⁢
         
          δ
          
           θ
           ⁢
           i
          
         
        
       
      
     
     
      
       
        
         γ
         
          2
          ⁢
          2
         
        
        =
        
         
          v
          ⁢
          
           
            s
            .
           
           
            -
            1
           
          
          ⁢
          
           cos
           
            -
            1
           
          
          ⁢
          
           θ
           
            d
            ⁢
            e
           
          
          ⁢
          cos
          ⁢
          
           δ
           
            θ
            ⁢
            i
           
          
         
         -
         
          
           s
           .
          
          (
          
           
            2
            ⁢
            v
            ⁢
            
             
              s
              .
             
             
              -
              1
             
            
            ⁢
            τ
            ⁢
            sin
            ⁢
            
             θ
             ie
            
           
           +
           
            
             τ
             2
            
            ⁢
            
             d
             y
            
           
           -
           
            
             d
             z
            
            ⁢
            
             
              ∂
              τ
             
             
              ∂
              s
             
            
           
           +
           
            
             c
             ⁡
             (
             
              
               c
               ⁢
               
                d
                y
               
              
              -
              1
             
             )
            
            ⁢
            
             (
             
              1
              -
              
               2
               ⁢
               
                cos
                
                 -
                 2
                
               
               ⁢
               
                θ
                
                 d
                 ⁢
                 e
                
               
              
             
             )
            
           
           +
           
            
             (
             
              
               c
               ⁢
               τ
               ⁢
               
                d
                z
               
              
              +
              
               
                d
                y
               
               ⁢
               
                
                 ∂
                 c
                
                
                 ∂
                 s
                
               
              
             
             )
            
            ⁢
            tan
            ⁢
            
             θ
             
              d
              ⁢
              e
             
            
           
          
          )
         
        
       
      
     
     
      
       
        
         γ
         
          3
          ⁢
          1
         
        
        =
        
         v
         ⁢
         
          
           s
           .
          
          
           -
           1
          
         
         ⁢
         cos
         ⁢
         
          δ
          
           θ
           ⁢
           i
          
         
         ⁢
         
          cos
          
           -
           1
          
         
         ⁢
         
          θ
          ie
         
        
       
      
     
     
      
       
        
         γ
         
          3
          ⁢
          2
         
        
        =
        
         v
         ⁢
         
          
           s
           .
          
          
           -
           1
          
         
         ⁢
         
          cos
          
           -
           1
          
         
         ⁢
         
          
           θ
           ie
          
          (
          
           
            sin
            ⁢
            
             δ
             
              θ
              ⁢
              i
             
            
            ⁢
            sin
            ⁢
            
             δ
             
              θ
              ⁢
              d
             
            
           
           -
           
            cos
            ⁢
            
             δ
             
              θ
              ⁢
              i
             
            
            ⁢
            sin
            ⁢
            
             θ
             ie
            
            ⁢
            tan
            ⁢
            
             θ
             
              d
              ⁢
              e
             
            
           
          
          )
         
        
       
      
     
     
      
       
        
         γ
         
          3
          ⁢
          3
         
        
        =
        
         
          
           (
           
            
             
              -
              
               d
               y
              
             
             ⁢
             
              
               ∂
               τ
              
              
               ∂
               s
              
             
            
            -
            
             
              (
              
               
                
                 d
                 z
                
                ⁢
                τ
               
               +
               
                2
                ⁢
                
                 (
                 
                  1
                  -
                  
                   c
                   ⁢
                   
                    d
                    y
                   
                  
                 
                 )
                
                ⁢
                tan
                ⁢
                
                 θ
                 
                  d
                  ⁢
                  e
                 
                
               
              
              )
             
             ⁢
             
              (
              
               τ
               +
               
                c
                ⁢
                
                 
                  tan
                  ⁢
                  
                   θ
                   ie
                  
                 
                 
                  cos
                  ⁢
                  
                   θ
                   
                    d
                    ⁢
                    e
                   
                  
                 
                
               
              
              )
             
            
            +
            
             d
             ⁢
             y
             ⁢
             
              
               ∂
               c
              
              
               ∂
               s
              
             
             ⁢
             
              
               tan
               ⁢
               
                θ
                ie
               
              
              
               cos
               ⁢
               
                θ
                
                 d
                 ⁢
                 e
                
               
              
             
            
           
           )
          
          ⁢
          
           s
           .
          
         
         +
         
          
           (
           
            1
            -
            
             c
             ⁢
             
              d
              y
             
            
           
           )
          
          ⁢
           
          
           (
           
            
             
              
               δ
               .
              
              
               θ
               ⁢
               i
              
             
             ⁢
             cos
             ⁢
             
              δ
              
               θ
               ⁢
               d
              
             
            
            -
            
             
              
               δ
               .
              
              
               θ
               ⁢
               d
              
             
             ⁢
             sin
             ⁢
             
              θ
              ie
             
             ⁢
             tan
             ⁢
             
              θ
              
               d
               ⁢
               e
              
             
            
           
           )
          
         
        
       
      
     
    
    ,
   
  
 

the required rotating magnetic field is calculated by the following formulas:, B
      ⊥
    
    =
    
      
        
          B
          0
        
        ⁢
        
          cos
          ⁡
          
            (
            
              2
              ⁢
              
                π
                ⁢
                ft
              
            
            )
          
        
        ⁢
        
          y
          b
        
      
      +
      
        
          B
          0
        
        ⁢
        
          sin
          ⁡
          
            (
            
              2
              ⁢
              
                π
                ⁢
                ft
              
            
            )
          
        
        ⁢
        
          z
          b
        
      
    
  
  ,
  
    

  
  ⁢
  
    
      B
      
        
        
      
    
    =
    
      
        -
        
          sgn
          ⁡
          
            (
            
              
                (
                
                  
                    B
                    ⊥
                  
                  ×
                  Ω
                
                )
              
              ⁢
              
                X
                b
              
            
            )
          
        
      
      ⁢
      λ
      ⁢
      
        
        
          
            (
            
              
                I
                1
              
              -
              
                
                  X
                  B
                
                ⁢
                
                  X
                  B
                  T
                
              
            
            )
          
          ⁢
          Ω
        
        
      
      ⁢
      
        X
        B
      
    
  
  ,
  
    

  
  ⁢
  
    B
    =
    
      
        B
        
          
          
        
      
      +
      
        B
        ⊥
      
    
  
  ,

wherein B⊥ is a magnetic field perpendicular to an axial direction and is configured to provide rotation for the magnetic micro-robot, B∥ is a magnetic field parallel to the axial direction and is configured to provide steering for the magnetic micro-robot, B0 is a magnetic flux density of a center of a working space, f is a rotation frequency, t is a rotation time, yb and zb are basis vectors of a XB plane perpendicular to the actual axial direction of the magnetic micro-robot, λ is a control gain, Ω is the output of the position closed loop controller, Ω=[Ωx Ωy Ωz], and I1 represents a third order identity matrix.