本文来自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)
目录 outline
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:
b ( r , m ) = μ 0 ∣ ∣ m ∣ ∣ 4 π ∣ ∣ r ∣ ∣ 3 ( 3 ( m ^ ⋅ r ^ ) r ^ − m ^ ) \textbf{b}(\textbf{r},\textbf{m})=\frac{\mu_{0}||\textbf{m}||}{4\pi||\textbf{r}||^{3}}\left(3(\widehat{\textbf{m}}\cdot\widehat{\textbf{r}})\widehat{\textbf{r}}-\widehat{\textbf{m}}\right) b(r,m)=4π∣∣r∣∣3μ0∣∣m∣∣(3(m
⋅r
)r
−m
)
3.2.1 磁场奇异点的区域 the region of magnetic field singularity
假设胶囊的朝向被精确地确定，位置估计通过非线性逆问题 b − 1 ( r , m ) \textbf{b}^{-1}(\textbf{r},\textbf{m}) b−1(r,m)被表述。奇异点的一个区域是对这个问题有无穷多解的区域。让 P s \textit{P}_{s} Ps指代与磁极子矩垂直并通过EPM中心的平面， P s = { r s ∈ R 3 ∣ r s T m ^ = 0 } \textit{P}_{s}=\left\{\textbf{r}_{s}\in \mathbb{R}^{3}|\textbf{r}_{s}^{T}\widehat{\textbf{m}}=0\right\} Ps={
rs∈R3∣rsTm
=0}。在这个平面上，我们有 b ( r , m ) = − μ 0 ∣ ∣ m ∣ ∣ 4 π ∣ ∣ r ∣ ∣ 3 m ^ \textbf{b}(\textbf{r},\textbf{m})=-\frac{\mu_{0}||\textbf{m}||}{4\pi||\textbf{r}||^{3}}\widehat{\textbf{m}} b(r,m)=−4π∣∣r∣∣3μ0∣∣m∣∣m
， b ( r , m ) ^ = − m ^ \widehat{\textbf{b}(\textbf{r},\textbf{m})}=-\widehat{\textbf{m}} b(r,m)
=−m
， ∣ ∣ b ( r , m ) ∣ ∣ = μ 0 ∣ ∣ m ∣ ∣ 4 π ∣ ∣ r ∣ ∣ 3 ||\textbf{b}(\textbf{r},\textbf{m})||=\frac{\mu_{0}||\textbf{m}||}{4\pi||\textbf{r}||^{3}} ∣∣b(r,m)∣∣=4π∣∣r∣∣3μ0∣∣m∣∣。因为在EPM系中 b ( r , m ) ^ \widehat{\textbf{b}(\textbf{r},\textbf{m})} b(r,m)
是恒定的，并且只有当 ∣ ∣ r ∣ ∣ ||\textbf{r}|| ∣∣r∣∣改变时， ∣ ∣ b ( r , m ) ∣ ∣ ||\textbf{b}(\textbf{r},\textbf{m})|| ∣∣b(r,m)∣∣才会改变， b − 1 ( r , m ) \textbf{b}^{-1}(\textbf{r},\textbf{m}) b−1(r,m)的解的集合是一个半径为 ∣ ∣ r ∣ ∣ ||\textbf{r}|| ∣∣r∣∣的圆。
Assuming the orientation of the capsule is determined accurately, position estimation can be expressed by the non-linear inverse problem b − 1 ( r , m ) \textbf{b}^{-1}(\textbf{r},\textbf{m}) b−1(r,m). A region of singularity is where infinite solutions exist to this problem. Let P s \textit{P}_{s} Ps designate the plane that is normal to the dipole moment and passes through the center of the EPM, P s = { r s ∈ R 3 ∣ r s T m ^ = 0 } \textit{P}_{s}=\left\{\textbf{r}_{s}\in \mathbb{R}^{3}|\textbf{r}_{s}^{T}\widehat{\textbf{m}}=0\right\} Ps={
rs∈R3∣rsTm
=0}. On this plane, we have b ( r , m ) = − μ 0 ∣ ∣ m ∣ ∣ 4 π ∣ ∣ r ∣ ∣ 3 m ^ \textbf{b}(\textbf{r},\textbf{m})=-\frac{\mu_{0}||\textbf{m}||}{4\pi||\textbf{r}||^{3}}\widehat{\textbf{m}} b(r,m)=−4π∣∣r∣∣3μ0∣∣m∣∣m
, b ( r , m ) ^ = − m ^ \widehat{\textbf{b}(\textbf{r},\textbf{m})}=-\widehat{\textbf{m}} b(r,m)
=−m
, ∣ ∣ b ( r , m ) ∣ ∣ = μ 0 ∣ ∣ m ∣ ∣ 4 π ∣ ∣ r ∣ ∣ 3 ||\textbf{b}(\textbf{r},\textbf{m})||=\frac{\mu_{0}||\textbf{m}||}{4\pi||\textbf{r}||^{3}} ∣∣b(r,m)∣∣=4π∣∣r∣∣3μ0∣∣m∣∣. Since b ( r , m ) ^ \widehat{\textbf{b}(\textbf{r},\textbf{m})} b(r,m)
is constant in the EPM frame and ∣ ∣ b ( r , m ) ∣ ∣ ||\textbf{b}(\textbf{r},\textbf{m})|| ∣∣b(r,m)∣∣ changes only when ∣ ∣ r ∣ ∣ ||\textbf{r}|| ∣∣r∣∣ changes, the set of solutions to b − 1 ( r , m ) \textbf{b}^{-1}(\textbf{r},\textbf{m}) b−1(r,m) is a circle of radius ∣ ∣ r ∣ ∣ ||\textbf{r}|| ∣∣r∣∣.
4. 理论 Methods
如果我们用一个电磁线圈来增加系统，电磁线圈产生一个弱时变磁场，并把它装到EPM上，这样它们的极子矩是正交的，EPM的静态场和线圈的时变场能被同时用来获得一组额外的等式，来允许解出胶囊的位置和yaw角。
If we argument the system with an electromagnet coil that generates a weak time-varying magnetic field and attach it to the EPM such that their dipole moments are orthogonal, the static field of the EPM and the time-varying field of the coil can be used simultaneously to obtain an additional set of equations that allow for solving for the position and yaw angle of the capsule.
一个时变信号被使用为了分别测量EPM和线圈的磁场。与之相反，如果两个静态磁场被使用，不可能制造两个分离的测量因为叠加原理，磁场的向量和被测量。这不是期望的因为它减少了可获得等式的数量。测量值被整合来创造一个向量使我们来对待线圈仿佛它是另一个与EPM同源永磁铁。
A time-varying signal is used in order to measure the magnetic fields of the EPM and the coil separately. In contrast, if two static magnetic fields were used, it would not be possible to make two separate measurements owing to the principle of superposition where the vector sum of the magnetic fields is measured. This is not desired as it reduces the number of available equations. The measured values are assembled to create a vector allows us to treat the coil as if it were another permanent magnet with the same source as the EPM.
5. 系统和软件环境 System and software environment
另外，胶囊包含一个惯性测量单元，被用来计算带有一个未知yaw角补偿的胶囊的朝向。
In additional, the capsule contains an inertial measurement unit that is used to compute the orientation of the capsule with a an unknown yaw offset.
IMU以一个100Hz的频率被采集，同时霍尔传感器以一个18kHz的频率被采集。
The IMU is sampled at a rate of 100Hz, while the Hall sensors are sampled at a rate of 18kHz.
默认情况下，一个霍尔传感器测量在空间中一个点的所有静态和时变信号的叠加。为了分开测量由EPM和线圈产生磁场的强度，信号处理技术被使用。首先，通过使用一个采样时间窗口，那是时变信号周期的一个整数倍数，我们确保信号平均值是0。EPM的测量然后被获得通过简答地平均原始传感器的读数。
By default, a Hall sensor measures the superposition of all static and time-varying signals at a point in space. In order to separately measure the strengths of the magnetic fields generated by the EPM and the coil, the signal processing techniques are used. Firstly, by using a sampling time window that is an integer multiple of the period of the time-varying signal, we ensure that the signal’s mean is zero. The EPM measurement is then obtained by simply averaging the raw sensor readings.
6. 实验验证和结果 Experimental Validation and Results
6.1 静态条件下验证 validation in static condition
胶囊被插入一个3D打印的容器内并固定在第二机械操作臂上，第二机械臂被定位于相对于第一机械臂已知位姿上。在第一组静态实验中，EPM沿着与胶囊保持一个固定距离的一个半球面的表面以螺旋形轨迹被移动。
The capsule was inserted into a 3D printed enclosure and secured to the secondary robot manipulator that was positioned in a known pose relative to the first robot. In the first set of static tests, the EPM was moved in a spatial trajectory along the surface of a hemisphere maintaining a constant distance from the capsule.
6.2 动态条件下验证 validation in dynamic condition
为了在动态条件下验证两种实验被进行。静-动实验包含沿着一个轨迹只移动胶囊而EPM是静止的。在动-动实验中，胶囊和EPM保持一个固定相对速度都沿着一个轨迹移动。
Two types of experiments were conducted for validation under dynamic conditions. The static-dynamic experiment consisted of moving only the capsule along a trajectory while the EPM was static. In the dynamic-dynamic experiment, both the capsule and the EPM moved along a trajectory keeping a constant relative speed.
[1]: Taddese, Addisu Z., et al. “Enhanced real-time pose estimation for closed-loop robotic manipulation of magnetically actuated capsule endoscopes.” The International journal of robotics research 37.8 (2018): 890-911.