First demonstration of simultaneous localization and propulsion for a magnetic capsule in a lumen

本文是Utah大学最新的闭环同步驱动与定位技术的整套系统成果。使用EKF框架融合驱动和定位两个step，在驱动step中，利用T-RO中提出的算法，根据前一时刻的定位pose，解出此刻为了保持胶囊在旋转磁场下驱动，驱动器磁铁的位姿。然后计算在这样的驱动器和胶囊的pose下，胶囊上所受到的magnetic force 和magnetic torque，利用之前发表的旋转前进模型，预估胶囊在此磁力和磁力矩情况下的位姿变化。在定位step中，采集此刻瞬间的磁场数值，利用上一步驱动中预估的pose，根据此时的磁场采集数值，更新胶囊pose。驱动定位step交替进行，外部看来，在驱动的过程中完成了定位，或者说，在定位时，也能驱动控制胶囊，成为simultaneous actuation and localization，同样，依据定位pose，更新驱动器pose的方法，是系统close-loop的保证。所以这个是利用旋转驱动方式中，胶囊的闭环同时驱动与定位系统的第一篇文章，有重要意义。这篇文章在机器人国际顶会ICRA 2017拿到了best paper in medical robotics奖项。

使用一个单一旋转磁铁在腔道中对一个磁胶囊同时定位与驱动的第一个演示
First demonstration of simultaneous localization and propulsion for a magnetic capsule in a lumen using a single rotating magnet [1]
Paper Link
Authors: Katie M. Popek, etc.
2017，IEEE International Conference on Robotics and Automation (ICRA)

摘要

这篇文章展示了用于一个带有嵌入式霍尔效应传感器的螺旋型磁胶囊且使用一个单独旋转驱动器磁铁的闭环推进的理论。此理论估计胶囊的6自由度位姿当它正随着施加场同步旋转时。它面向肠道主动胶囊内窥镜方面的应用。一个扩展卡尔曼滤波，它用一个限制胶囊前进或后退运动以及绕它的中心轴旋转的简化的2自由度过程模型，被使用来提供一个胶囊位姿的完整6自由度估计当胶囊在一个腔体内移动时。胶囊在施加场中的移动被恒定地监视来决定胶囊是否跟随施加场同步旋转。基于这个信息，外部源的旋转速度被调节来防止在期望磁耦合上的丢失。我们实验性的演示，第一次，使用一个单个旋转磁体通过腔体的一个胶囊的同时定位和闭环推进。先前的工作假设胶囊在定位过程中没有运动，要求解耦的定位和推进。这个闭环展示达到了一个三倍增速在完成时间方面，与以前解耦方法对比。
The paper presents a method for closed-loop propulsion of a screw-type magnetic capsule with embedded Hall-effect sensors using a single rotating actuator magnet. The method estimates the 6 DoF pose of the capsule while it is synchronously rotating with the applied field. It is intended application in active capsule endoscopy of the intestines. An extended Kalman filter, which uses a simplified 2 DoF process model restricting the capsule to forward or backward movement and rotation about its principle axis, is used to provide a full 6 DoF estimate of the capsule’s pose as the capsule travels through a lumen. The capsule’s movement in the applied field is constantly monitored to determine if the capsule is synchronously rotating with the applied field. Based on this information, the rotating speed of the external source is adjusted to prevent a loss in the desired magnetic coupling. We experimental demonstrate, for the first time, simultaneous localization and closed-loop propulsion of a capsule through a lumen using a single rotating magnet. Prior work assumed the capsule had no net motion during the localization phase, requiring decoupled localization and propulsion. This closed-loop performance results in a three times speed up in completion time, compared to the previous decoupled approach.

I. 介绍 （节选）

之前，我们描述了一个定位理论来估计在没有运动假设下的一个磁胶囊的完整6自由度位姿，并且我们应用了这个位姿估计为了在验证概念的推进系统中位置和头部朝向反馈。这个推进和定位是解耦的，这样推进被开环地执行，而且我们胶囊的运动为了定位而周期性暂停。
recently, we described a localization method to estimate the full 6 DoF pose of a magnetic capsule under the assumption of no net motion, and we applied this pose estimate for position and heading feedback in a proof-of-concept propulsion system. This propulsion and localization were decoupled, such that propulsion was executed open-loop, and our capsule’s movement was periodically paused for localization.
在这篇文章中我们展示一个扩展卡尔曼滤波器来提供胶囊的6自由度位姿的一个连续估计当它跟随一个施加的磁极子场同步旋转。这EKF使用一个简化的2自由度过程模型，模型假设胶囊的运动被限制为沿着它的轴线的移动和旋转。我们限制剩余的四个自由度并让这腔体指挥在胶囊头部方向的改变。
In this paper, we presents an extended Kalman filter to provide a continuous estimate of the capsule’s 6 DoF pose as it rotates synchronously with an applied magnetic dipole field. The EKF uses a simplified 2 DoF process model that assumes the capsule movement is restricted to translation along and rotation about its principle axis. We restrict the remaining four DoF and let the lumen dicate the changes in the capsule’s heading.

VI. 扩展卡尔曼滤波 （节选）

A. 处理模型实现

这胶囊有一根用于推进的螺旋线，将磁力和力矩转换为向前和角速度：
The capsule has a helical thread for propulsion, which translates magnetic force and torque into forward and angular velocity:
$$\left(\begin{array}{c}\mathbf{\omega }\\ \mathbf{v}\end{array}\right)=\left(\begin{array}{cc}\mathbf{A}& \mathbf{E}\\ {\mathbf{E}}^T& \mathbf{L}\end{array}\right)\left(\begin{array}{c}\mathbf{\tau }\\ \mathbf{f}\end{array}\right)$$
来自施加磁场的作用在胶囊上磁力和力矩：
The magnetic force and torque on the capsule from the applied magnetic field:
$$\mathbf{f}=\frac{3{\mu }_0||{\mathbf{m}}_a||\cdot ||{\mathbf{m}}_c||}{4\pi }\cdot \frac{\hat{\mathbf{r}}}{||\mathbf{r}|{|}^4}\cdot (\widehat{{\mathbf{m}}_c}{\widehat{{\mathbf{m}}_a}}^T+\widehat{{\mathbf{m}}_a}{\widehat{{\mathbf{m}}_c}}^T+{\widehat{{\mathbf{m}}_c}}^T(\mathbf{I}-5\hat{\mathbf{r}}{\hat{\mathbf{r}}}^T)\widehat{{\mathbf{m}}_a}\cdot \mathbf{I})$$
$$\mathbf{\tau }=\frac{{\mu }_0||{\mathbf{m}}_a||\cdot ||{\mathbf{m}}_c||}{4\pi ||\mathbf{r}|{|}^3}\widehat{{\mathbf{m}}_c}\times (3\hat{\mathbf{r}}{\hat{\mathbf{r}}}^T-\mathbf{I})\widehat{{\mathbf{m}}_a}$$

B. 测量模型实现

在每个传感器上读取磁场数据，然后和前一步中的预测pose的理论磁场作差，用这个差值根据卡尔曼滤波的update步骤，更新capsule的pose。

VII. 检测胶囊的运行状况

我们只需要分别胶囊是否跟随外部场同步旋转因为以前的理论能够被使用来估计静止或失步状态下的胶囊的位姿。
We only need to distinguish whether or not the capsule is synchronously rotating with the external field because the previous method can be used to estimate the pose of a capsule that is either stationary or in step-out.
给定不论来自于初始化或EKF的胶囊估计姿态，并且不论胶囊是否随施加场转动，驱动器极子的位姿被更新。
Given the capsule’s estimated pose from either the initialization or the EKF, and whether the capsule is rotating with the applied field, the actuator dipole’s pose is updated.

VIII. 闭环驱动的演示（节选）

直线轨迹的5自由度误差被找到通过比较胶囊的EKF估计状态和立体视觉系统给出的状态。这平均位置误差是8.5毫米和7.1度。
The 5 DoF error for the straight trajectory was found by comparing the capsule’s estimated state from the EKF with that given by a stereo vision system. The average position error was $8.5mm$ and $7.1^{\circ }$.

[1]: Popek, Katie M., Tucker Hermans, and Jake J. Abbott. “First demonstration of simultaneous localization and propulsion of a magnetic capsule in a lumen using a single rotating magnet.” 2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017.