Robotic magnetic manipulation offers a minimally invasive approach to gastrointestinal examinations through capsule endoscopy. However, controlling such systems using external permanent magnets (EPM) is challenging due to nonlinear magnetic interactions, especially when there are complex navigation requirements such as avoidance of sensitive tissues.
In this work, we present a novel trajectory planning and control method incorporating dynamics and navigation requirements, using a single EPM fixed to a robotic arm to manipulate an internal permanent magnet (IPM). Our approach employs a constrained iterative linear quadratic regulator that considers the dynamics of the IPM to generate optimal trajectories for both the EPM and IPM.
Extensive simulations and real-world experiments, motivated by capsule endoscopy operations, demonstrate the robustness of the method, showcasing resilience to external disturbances and precise control under varying conditions. The experimental results show that the IPM reaches the goal position with a maximum mean error of 0.18 cm and a standard deviation of 0.21 cm. This work introduces a unified framework for constrained trajectory optimization in magnetic manipulation, directly incorporating both the IPM's dynamics and the EPM's manipulability.
Block diagram of the control and planning phases for robotic magnetic capsule endoscopy. The planning phase (left) utilizes iLQR to generate optimal state trajectories x*, input trajectories u*, and time-varying optimal controller gains K(k), based on user-defined constraints and initial/goal positions. In the control phase (right), joint angles q are measured from the robotic arm encoders, and the 3D position of the IPM is obtained through object detection and triangulation using two orthogonal cameras. The Extended Kalman Filter then estimates the IPM's position p_I and velocity v_I, which are used to update the control input u and follow the optimal trajectories. The system includes the EPM for external magnetic manipulation and the IPM within the targeted environment, with the IPM dimensions shown in the figure and given in millimeters.
This video shows the data collection methods for the system modelling and parameter estimation.
To establish the simulation environment, we first collect experimental data by hovering the IPM in the tank using a predefined oscillatory motion of the EPM. During this process, we record the EPM positions as well as the IPM positions and velocities. A video of the data collection process is available on the website. These measurements serve as the basis for parameter identification.
Using a gradient descent method, we determine the system parameters by minimizing the metric of equation error typically used in system identification. These identified parameters are used in both the experimental and simulation studies.
To evaluate further the accuracy of the simulation, we input the experimental data into the simulation and compare the simulated IPM positions with the experimental measurements. Eventhough we optimize our simulation model using the equation error metric, we evaluated the prediction performance of the model for 2 seconds and compared against real world data .
(a) Simulation Case 1 visualization.
(b) Time series data of the optimal IPM trajectory. (c) IPM velocities along the trajectory. (d) Time series data of the optimal EPM trajectory. (e) Joint angles of the robotic arm. (f) Time series data of the IPM orientation. (g) Condition number (Κ) analysis of the robotic arm’s manipulability.
(a) Simulation Case 2 visualization.
In Case 2, the initial configuration of the robot is chosen to achieve the same IPM position and orientation but with a poorer initial condition number (5, compared to 2.7 in Case 1). The optimal trajectory is designed to improve the condition number over time while adhering to the imposed constraints on position, velocity, joint limits, and orientation.
The objective steps (change in the objective value) are plotted for the simulation case 1 (the most complex scenario including all position, velocity, obstacle and manipulability constraints). The objective steps eventually reach very small changes, indicating convergence to a stable solution and demonstrating the effectiveness of the proposed approach for offline trajectory planning.
The algorithm without constraints typically converges after approximately 10 iterations. Adding positional constraints on IPM increases the required number of iterations due to Lagrangian formulation and generally achieves convergence with less than 60 iterations. In general, in most complex scenarios with all constraints included, our method required less than 65 iterations
Real-world experimental results showing repetitive IPM trajectories in 3D space under the given constraints, including obstacle avoidance (red sphere). Each colored line represents an individual trial of the IPM navigating around the obstacle, demonstrating the system's ability to consistently follow the desired path while respecting imposed constraints.
Experimental results showing the time evolution of IPM position, IPM velocity, and EPM position along the X, Y, and Z axes under closed-loop control. The left column shows the IPM position, with mean position (solid line) and standard deviation (shaded area) around the mean, alongside goal positions (red crosses). The middle column displays IPM velocity, with mean velocity and standard deviation, and includes velocity constraints (dashed red line) in the Y-axis plot. The right column represents EPM position with mean position (solid line) and standard deviation (shaded area) around the mean, as well as position constraints in the Z-axis plot (dashed red line). This figure demonstrates the tracking accuracy and stability of both IPM and EPM under the control framework.
Isitman, Ogulcan, Gokhan Alcan, and Ville Kyrki. "Trajectory Planning and Control for Robotic Manipulation of Magnetic Capsules." IEEE Robotics and Automation Letters (2025).