同时驱动与定位理论，使用的方法非常经典。作者提出了一个基于雅可比迭代的定位方式，就是普通的robotics课程中学习的建模计算过程，但是输入为capsule position，输出为磁场向量，这是磁前向过程，有了磁前向过程，就可以导出雅可比矩阵，从而得到反向的磁逆向过程，从而对胶囊相对于驱动磁铁的相对位姿的定位。
作者将3维位置的定位简化为2维。胶囊的姿态变化是通过IMU直接读出来的，作者主要要处理的是位置变化/定位。
在磁前向过程中，需要用到胶囊位置-磁场向量的关系，这个往往使用dipole model这个简化模型，但是作者在这篇文章中，使用了较为高级的其他模型（利用FEA软件进行建模计算，保存数据），得到一部分模型数据以后，建立一个磁场特性矩阵和两个插值向量函数进行拟合，得到拟合的磁场特性矩阵后，对模态基向量函数输入不同的位置，就可以对磁场进行插值计算，从而得到磁场。
用于机器人胶囊内窥镜磁定位的基于雅可比的迭代方法
Jacobian-Based Iterative Method for Magnetic Localization in Robotic Capsule Endoscopy [1]
Paper Link
Authors: Pietro Valdastri, etc.
2016, T-RO IEEE Transactions on Robotics
目录 outline
1. 摘要 Abstract
这个研究的目的是验证用于磁控内窥镜胶囊的实时定位的基于雅可比的迭代理论。所提出的方法是对磁场问题应用有限元解法，和最小平方插值来获得磁场的闭形式快速估计。通过定义一个用于磁场相对于胶囊位姿改变的雅可比的闭形式表达，我们能够获得一个迭代定位，当与之前工作相比有更快的计算时间，且没有遭受从偶极假设来的不准确源。这个新算法可以和一个绝对定位技术结合起来使用，一个绝对定位技术能提供初始解并运行在一个低更新速率。
The purpose of this study is to validate a jacobian based iterative method for real-time localization of magnetically controlled endoscopic capsules. The proposed approach applies finite element solutions to the magnetic field problem, and least-square interpolations to obtain closed-form and fast estimates of the magnetic field. By defining a closed-form expression for the Jacobian of the magnetic field relative to the changes in the capsule pose, we are able to obtain an iterative localization at a faster computational time when compared with prior work, without suffering from the inaccuracies stemming from dipole assumptions. This new algorithm can be used in conjunction with an absolute localization technology that provides initialization values at a slower refresh rate.
整体刷新速率，包括传感器数据采集和无线传输是7ms，从而确保用于磁操作的闭环控制策略工作在超过100Hz。
The overall refresh rate, including sensor data acquisition and wireless communication is 7ms, thus ensuring closed-loop control strategies for magnetic manipulation running faster than 100Hz.
2. 理论 Methods
在这篇文章中，我们假设位姿追踪的更行速率是足够快的以至于在接连的位姿测量之间只有WCE的小运动可能发生。我们也假设胶囊的朝向是已知，通过在IMU数据上运行的算法。
In this paper, we assume that the refresh rate for pose tracking is fast enough that only small movements of the WCE may occur between subsequent pose measurements. We also assume that the orientation of the capsule is known through the algorithm running on IMU data.
一个MDR的显性公式通过磁场模型的有限元积分获得，然而一个数值估计通过一个标准有限元理论软件包被提供。
An explicit formulation of the MDR can be obtained by finite element integration of magnetic field models, while a numerical estimation can be provided by a standard finite element method software package.
所提出的磁定位算法利用磁场和惯性测量的传感器融合。磁场插值（磁场标定）被离线获得，这导致获得特性矩阵。一旦插值被获得，在线算法将磁场，惯性测量，和外部永磁铁朝向作为输入，返回胶囊位姿。
The proposed method exploits sensor fusion of magnetic field and inertial measurements. The magnetic field interpolation (magnetic field calibration) is achieved offline, which leads to obtaining the characteristic matrices. Once the interpolation is obtained, the online algorithm takes as inputs the magnetic field, the inertial measurements, and the external permanent magnet orientation, returning capsule pose.
定位能利用对称的优点来减少计算负担。特别地，3D位置追踪问题能被减少到2维。
The localization can take advantage of the symmetry to reduce the computational burden. In particular, the 3D position tracking problem can be reduced to 2 dimentions.
B r ( r , z ) B_{r}(r,z) Br(r,z)和 B z ( r , z ) B_{z}(r,z) Bz(r,z)是两个常数值，用来表示磁场向量的径向和轴向分量，它们是相对于EPM中心的径向和轴向空间坐标的函数。
B r ( r , z ) B_{r}(r,z) Br(r,z) and B z ( r , z ) B_{z}(r,z) Bz(r,z) are two scalar values representing the radial and axial component of the magnetic field vector, which are functions of radial and axial spatial coordinates with respect to the center of the EPM.
数值解能通过要么应用电流密度磁场模型要么充电密度磁场模型获得。
The numerical solutions can be obtained by either applying the current density magnetic model or the charge density magnetic model.
然后，磁场数据被投射到两个数据矩阵， Φ r ∈ R m × p \Phi_{r}\in\mathbb{R}^{m \times p} Φr∈Rm×p和 Φ z ∈ R m × p \Phi_{z}\in\mathbb{R}^{m \times p} Φz∈Rm×p。这些矩阵表示在任何给定位置 p c \textbf{p}_{c} pc的 m × p m\times p m×p磁场数值解，m是沿 r ^ \hat{r} r^方向采的磁场测量的数量，p是沿 z ^ \hat{z} z^采的磁场测量的数量。
Then, the magnetic field values can be casted in two data matrices, Φ r ∈ R m × p \Phi_{r}\in\mathbb{R}^{m \times p} Φr∈Rm×p and Φ z ∈ R m × p \Phi_{z}\in\mathbb{R}^{m \times p} Φz∈Rm×p. These matrices represent the m × p m\times p m×p magnetic field numerical solutions for any given position p c \textbf{p}_{c} pc, where m is the number of magnetic field measurements taken along the r ^ \hat{r} r^ direction, p is the number of magnetic field measurements taken along z ^ \hat{z} z^.
Φ r i j \Phi_{rij} Φrij和 Φ z i j \Phi_{zij} Φzij是在 ( i , j ) (i,j) (i,j)位置的磁场数值
Φ r i j \Phi_{rij} Φrij and Φ z i j \Phi_{zij} Φzij are the magnetic field values at position ( i , j ) (i,j) (i,j).
我们描述如何推导协参数 A r A_{r} Ar和 A z A_{z} Az的特性矩阵。
We describe how to derive the charateristic matrices of coefficients A r A_{r} Ar and A z A_{z} Az.
Ω \Omega Ω和 Γ \Gamma Γ是模态基矩阵，并且包含用于 Ω \Omega Ω的n个正交基和用于 Γ \Gamma Γ的q个正交基的集合。最终，m和p分别是区域 r ∈ [ 0 , L ] r\in[0,L] r∈[0,L]和 z ∈ [ 0 , L ] z\in[0,L] z∈[0,L]内估计值的数量。
Ω \Omega Ω and Γ \Gamma Γ are the modal basis matrices, and constitute the collection of n orthogonal basis for Ω \Omega Ω and q orthogonal basis for Γ \Gamma Γ. Finally, m and p are the number of values estimated in the domain r ∈ [ 0 , L ] r\in[0,L] r∈[0,L] and z ∈ [ 0 , L ] z\in[0,L] z∈[0,L], respectively.
A r A_{r} Ar和 A z A_{z} Az的解能通过应用Kronecker乘法理论被获得。
The solutions for A r A_{r} Ar and A z A_{z} Az can be obtained by applying Kronecker Product theory.
3. 实验 Experiments
COMSOLMultiphysics被使用来创建 15 × 15 15\times15 15×15的矩阵 Φ r \Phi_{r} Φr和 18 × 18 18\times18 18×18的矩阵 Φ z \Phi_{z} Φz。这两个矩阵被插值，使用两个模态基函数向量 ω \omega ω和 γ \gamma γ。
COMSOLMultiphysics is used to create the 15 × 15 15\times15 15×15 matrix Φ r \Phi_{r} Φr and the 18 × 18 18\times18 18×18 matrix Φ z \Phi_{z} Φz. These two matrices are interpolated using two vectors of modular basis functions ω \omega ω and γ \gamma γ.
[1]: Di Natali, Christian , et al. “Jacobian-Based Iterative Method for Magnetic Localization in Robotic Capsule Endoscopy.” IEEE Transactions on Robotics (2016):1-12.