A 5-D Localization Method for a Magnetically Manipulated Untethered Robot Using a 2-D Array of Hall

本文提出了一种新的驱动定位方式。利用电磁线圈产生的磁场来驱动胶囊行进，同时利用传感器采集到的总磁场，去除其中的驱动磁场，剩余的就应该是胶囊磁场，但是这样有问题，因为驱动磁场是由电磁线圈产生的，把电磁线圈产生的磁场也模拟为一个磁极子磁场，这样很难估算这个磁极子的位置参数，估计的肯定不准，那就有误差。作者发现，对磁极子表达式求导两次，可以让胶囊磁极子磁场的二次导数值远远大于误差磁极子磁场的二次导数值，所以就把原来的的磁极子模型磁场的目标函数（包含无法估算的误差项）替换为磁极子模型磁场二次导数值的目标函数（没有误差项）。这样优化之后，就可以解算出胶囊的5D pose了。

使用一个2D霍尔效益传感器阵列的用于一个磁操作无线机器人的一个5D定位理论
A 5-D Localization Method for a Magnetically Manipulated Untethered Robot Using a 2-D Array of Hall-Effect Sensors [1]
Paper Link
Authors: Son Donghoon, etc.
2015, IEEE/ASME Transactions on Mechatronics

我们介绍两步为了更精确地定位一个磁机器人。首先，这极子模型的电磁铁磁场从测量数据中被减去为了确定机器人的磁场。第二步，被减后的磁场被二次求导在阵列的特定方向，以便在定位过程中的电磁铁磁场的影响被最小化。关于机器人位置和朝向的五个变量通过最小化在优化理论中测量磁场和模型磁场之间的误差被确定。在$200Hz$磁传感器的可行范围内（$5cm$）结果位置误差是$2.1\pm 0.8mm$，角度误差是$6.7\pm 4.3^{\circ }$。
We introduce two steps for localizing a magnetic robot more accurately. Firstly, the dipole-modeled magnetic field of the electromagnet is subtracted from the measured data in order to determine the robot’s magnetic field. Secondly, the subtracted magnetic field is twice differentiated in the particular direction of the array, so that the effect of the electromagnet field in the localization process is minimized. Five variables regarding to the position and orientation of the robot are determined by minimizing the error between the measured magnetic field and the modeled magnetic field in an optimization method. The resulting position error is $2.1\pm 0.8mm$ and angular error is $6.7\pm 4.3^{\circ }$ within the applicable range ($5cm$) of the magnetic sensors at $200Hz$.

磁场传感器经受来自胶囊和外部磁铁的磁场。那些磁场被表达为：${\text{B}}_s={\text{B}}_c+{\text{B}}_e$。来建模一个磁场的通用方法是来使用磁极子等式：$\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)$。${\text{B}}_e$可以被表达为：${\text{B}}_e={\text{B}}_{dpl}({\text{r}}_e,{\text{m}}_e)$。然而，在实际中，我们不能测量精确的${\text{r}}_e$和${\text{m}}_e$，${\text{B}}_s={\text{B}}_c+{\text{B}}_e$能被重写为${\text{B}}_c={\text{B}}_s-[{\text{B}}_{dpl}({\text{r}}_e,{\text{m}}_e)+{\text{B}}_{err}]$，${\text{B}}_{err}={\text{B}}_s-{\text{B}}_c-{\text{B}}_{dpl}({\text{r}}_e,{\text{m}}_e)$。
The magnetic sensors experience the magnetic fields both from the capsule and the external magnet. Those magnetic fields are expressed as: ${\text{B}}_s={\text{B}}_c+{\text{B}}_e$。The general way to model a magnetic field is to use magnetic dipole equation: $\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)$。${\text{B}}_e$ can be expressed as: ${\text{B}}_e={\text{B}}_{dpl}({\text{r}}_e,{\text{m}}_e)$。However, in actuality, we can not measure the exact ${\text{r}}_e$ and ${\text{m}}_e$，${\text{B}}_s={\text{B}}_c+{\text{B}}_e$ can be rewritten as :${\text{B}}_c={\text{B}}_s-[{\text{B}}_{dpl}({\text{r}}_e,{\text{m}}_e)+{\text{B}}_{err}]$，${\text{B}}_{err}={\text{B}}_s-{\text{B}}_c-{\text{B}}_{dpl}({\text{r}}_e,{\text{m}}_e)$。

我们定义一个新的参数信号质量比（SQR）在磁场，为：
We define a new parameter, signal quality ratio (SQR) in B-field, as:
$$SQ{R}_B=\frac{||{\text{B}}_c||}{||{\text{B}}_{err}||}\alpha {\left(\frac{||{\text{r}}_e||}{||{\text{r}}_c||}\right)}^3$$
SQR在磁场的二阶导数被表示为：
SQR in the second-order differentiated B-field is expressed as:
$$SQ{R}_L=\frac{||{\partial }^2{\text{B}}_c/\partial ||{\text{r}}_c|{|}^2||}{||{\partial }^2{\text{B}}_{err}/\partial ||{\text{r}}_e|{|}^2||}\alpha {\left(\frac{||{\text{r}}_e||}{||{\text{r}}_c||}\right)}^5$$

代价函数被定义为：
The cost function is defined as:
$$cost=\sum _i\sum _j(K^{i,j}-K_{e,dpl}^{i,j}({\text{r}}_e,{\text{m}}_e)-K_{e,dpl}^{i,j}({\text{r}}_c,{\text{m}}_c){)}^2$$

[1]: Son, Donghoon, Sehyuk Yim, and Metin Sitti. “A 5-D localization method for a magnetically manipulated untethered robot using a 2-D array of Hall-effect sensors.” IEEE/ASME Transactions on Mechatronics 21.2 (2015): 708-716.