Enhanced real-time pose estimation for closed-loop robotic manipulation of magnetically actuated cap

本文来自Leeds大学的用于胶囊内窥镜同时驱动与定位的经典之作。大体是用拖拉的方式，用外部永磁大磁体（static magnetic field from permanent magnet + time-varying magnetic field from electromagnetic coil）拖动肠道内小磁铁（permanent magnet + sensor array + inertial unit）。在之前的方案中，leeds大学组利用内置IMU计算胶囊的3D orientation信息，利用外部驱动磁铁在胶囊内sensor上产生的磁场反解出胶囊的3D position信息。但是这样的硬件配置本身存在singularity的问题，当胶囊轴线方向与外部驱动磁铁的轴线方向平行且胶囊处于胶囊中心平面上时，胶囊所在位置感受到的磁场大小和方向在胶囊中心平面上的一个圆上都不变，也就是说，如果在这样给定的磁场读数下，胶囊有无数个pose解，这就导致singularity存在了。解决这个singularity的方法是，在永磁铁上再增加一个电磁线圈，电磁线圈能产生时变磁场，时变磁场和磁场方向和永磁铁的磁场方向是垂直的，也就可以破坏原来的singularity，增多了磁场等式，方便求出非singular解。文章后半部分的一些实验设计思路和方法都值得借鉴。

用于磁驱胶囊内窥镜的闭环机器操作的增强型实时位姿估计
Enhanced real-time pose estimation for closed-loop robotic manipulation of magnetically actuated capsule endoscopes [1]
Paper Link
Authors: Addisu Z. Taddese, etc.
2018，International journal of robotics research (IJRR)

0. 摘要 Abstract

用于胶囊内窥镜的机器引导磁驱动的位姿估计最近已经能够轨迹跟随并使重复性内镜演习自动化。但是，这些理论在它们前往临床采用的路上面临重大挑战包括磁场奇异点区域的现象，在这种情况下系统精度下降，还有对胶囊位姿精确初始化的需求。特别的，对于任何使用一个单个磁场源的位姿估计来说奇异点问题都存在如果这理论不依赖于磁铁的运动来获取从不同优势点的多个测量值。我们分析了使用点极子磁场模型的的位姿估计理论的工作区域，并展示了奇异区域存在于在磁驱动过程中胶囊被名义上定位的区域。因为极子模型能大约拟合大部分磁场源，在这里被讨论的问题属于一个更宽泛的位姿估计技术集合。我们然后提出一个新的雇佣静态和时变磁场源的混合方法并且展示这个系统没有奇异点区域。所提出的系统被实验验证了精度、工作区域尺寸、更新率和在磁奇异点区域内的表现。系统表现相等或优于先前的位姿估计理论且不需要精确的初始化，并且对磁奇异点鲁棒。一个使用先进的位姿估计技术的带线磁设备的闭环控制的实验性演示被探明它用于机器引导胶囊内窥镜的合适性。因此，在闭环控制和磁驱胶囊内窥镜的智能自动化的进展能通过雇佣这个位姿估计系统被进一步追求直到临床实现。
Pose estimation methods for robotically guided magnetic actuation of capsule endoscopes have recently enabled trajectory following and automation of repetitive endoscopic maneuvers. However, these methods face significant challenges in their path to clinical adoption including the presence of regions of magnetic field singularity, where the accuracy of the system degrades and the need for accurate initialization of the capsule’s pose. In particular, the singularity problem exists for any pose estimation method that utilizes a single source of magnetic field if the method does not rely on the motion of the magnet to obtain multiple measurements from different vantage points. We analyse the workspace of such pose estimation methods with the use of the point-dipole magnetic field model and show that singular regions exist in the areas where the capsule is nominally located during magnetic actuation. Since the dipole model can approximate most magnetic field sources, the problem discussed herein pertains to a wider set of pose estimation techniques. We then propose a novel hybrid approach employing static and time-varying magnetic field sources and show that this system has no regions of singularity. The proposed system was experimentally validated for accuracy, workspace size, update rate and performance in regions of magnetic singularity. The system performed as well or better than prior pose estimation methods without requiring accurate initialization, and was robust to magnetic singularity. Experimental demonstration of closed-loop control of a tethered magnetic device utilizing the developed pose estimation technique is provided to ascertain its suitability for robotically guided capsule endoscopy. Hence, advances in closed-loop control and intelligent automation of magnetically actuated capsule endoscopes can be further pursued toward clinical realization by employing this pose estimation system.

3. 背景 Background

3.2 现存位姿估计理论的限制 limitations of existing pose estimation methods

理论分析，不失一般性，我们将假设这外部永磁铁是一个轴向充磁圆柱形磁铁，但是这儿描述的奇异点的规则应用于所有能够被拟合成一个极子模型的磁铁。外部永磁体的磁场然后被给定为：
For theoretical analysis, without any loss of generality, we will assume the EPM is an axially magnetized cylindrical magnet, but the principles of singularity described herein apply to all magnets that can sufficiently be approximated by a dipole model. The magnetic field of the EPM is then given by:
$$\text{b}(\text{r},\text{m})=\frac{{\mu }_0||\text{m}||}{4\pi ||\text{r}|{|}^3}\left(3(\hat{\text{m}}\cdot \hat{\text{r}})\hat{\text{r}}-\hat{\text{m}}\right)$$

3.2.1 磁场奇异点的区域 the region of magnetic field singularity

假设胶囊的朝向被精确地确定，位置估计通过非线性逆问题${\text{b}}^{-1}$