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

Claim 3:
4. A micro-robot control method based on double closed loop three-dimensional path tracking, applied to the magnetic drive device according to claim 1, the control method comprising:
inputting a desired tracking path into the host computer to obtain a desired direction of movement, wherein the cameras obtain current pose information of the magnetic micro-robot, and feed 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;
finding a desired position point with the shortest distance from the center of mass on the desired tracking path, and establishing a curve coordinate system of the desired tracking path to obtain three-dimensional coordinates of the desired position point and a curvature and a torsion of a curve of the desired tracking path;
processing the actual axial direction and the desired direction of movement to obtain a direction error and the three-dimensional coordinates of the center of mass and the three-dimensional coordinates of the desired position point to obtain a position error, wherein the direction error comprises a pitch angle error and a direction angle error, and the position error comprises a horizontal distance and a vertical distance between the center of mass and the desired position point;
modeling three-dimensional kinematics of the desired tracking path according to the curvature, the torsion, the direction error, and the position error to obtain state space equations;
designing a position closed loop controller through the state space equations to obtain a required rotating magnetic field, and inputting the rotating magnetic field into an established current closed loop magnetic field controller to output a desired magnetic field, to implement a double closed loop three-dimensional movement control of the magnetic micro-robot; and
repeating the step of obtaining 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 until the entire desired tracking path is tracked.