Trajectory Planning and Control for Robotic Magnetic Manipulation

TL;DR

This work tackles trajectory planning and control for magnetic capsule endoscopy by coupling an external permanent magnet (EPM) on a robotic arm with an internal permanent magnet (IPM) inside the GI tract. It introduces a constrained iterative linear quadratic regulator (iLQR) solved with an augmented Lagrangian to enforce joint, obstacle, and magnetic constraints, yielding optimal state/input trajectories and time-varying gains for real-time closed-loop control. The method demonstrates robustness to disturbances and precise IPM tracking in simulations and real-world experiments, with an emphasis on safety and manipulability of the EPM. The results indicate a unified framework that directly accounts for IPM dynamics, enabling safer, more efficient navigation of a magnetic capsule and offering a path toward higher autonomy in capsule endoscopy.

Abstract

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.

Figures (6)

• Figure 1: Conceptual design of the external permanent magnet-based robotic capsule endoscopy system. Constrained trajectory optimization accounts for capsule dynamics, enabling precise control and obstacle avoidance.
• Figure 2: Block diagram of the planning and control phases for robotic magnetic manipulation. The planning phase (left) utilizes iLQR to generate optimal state trajectories $\mathbf{x}^*$, input trajectories $\mathbf{u}^*$, and time-varying optimal controller gains $\mathbf{K}(k)$, based on user-defined constraints and initial/goal positions. In the control phase (right), joint angles $\mathbf{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 $\hat{\mathbf{p}}_I$ and velocity $\hat{\mathbf{v}}_I$, which are used to update the control input $\mathbf{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.
• Figure 3: (a) Simulation case 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 ($\kappa$) analysis of the robotic arm’s manipulability.
• Figure 4: Comparison of open-loop and closed-loop IPM position tracking under Gaussian noise. In the open-loop (left), significant deviations from the desired trajectories (solid colored lines) are observed, with gray lines indicating variability across trials. In the closed-loop (right), the controller reduces deviations, closely following the desired trajectories. Dashed lines represent goal positions, solid colored lines are optimal/desired trajectories, and gray lines show variations across multiple trials.
• Figure 5: 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 (a) 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 (b) displays IPM velocity, with mean velocity and standard deviation, and includes velocity constraints (dashed red line) in the Y-axis plot. The right column (c) 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.