A simultaneous calibration method for magnetic robot localization and actuation systems

这篇文章发表在T-RO上，非常有价值。本文提出了一种新的驱动方式和详细的设计介绍，对磁机器人定位驱动系统的大量参数的同时标定，借鉴了SLAM中的BA方法。

一种用于磁机器人定位和驱动系统的同时标定理论
A simultaneous calibration method for magnetic robot localization and actuation systems [1]
Paper Link
Authors: Donghoon Son, etc.
2019, IEEE Transactions on Robotics

摘要 Abstract

这篇文章提出一个用于磁驱动的机器人的同时标定磁定位和驱动系统的理论。在这个理论中，未标定的磁定位和驱动系统以最小的人力介入被同时标定，它能自我标定，灵活重配置，以及系统参数的长期矫正。这个理论应用一个光束调节框架，使用一个用于传感器的二次测量模型和用于驱动器的磁极子模型。所提出的理论已经被证实在与有限元仿真和用于磁驱动器和传感器的现存标定理论的比较中。在实验中，在标定后用于传感器的协参数的行列式为99.84%而用于驱动系统的为99.45%，与分离的标定磁驱动器和传感器阵列的最新的标定理论不相上下。这个理论有潜力来提高磁机器人定位和驱动系统的重配置能力和长时间精度，比如磁驱动的胶囊内窥镜。
This paper proposes a method of simultaneously calibrating magnetic localization and actuation systems for magnetically actuated robots. In this method, uncalculated magnetic localization and actuation systems are calculated simultaneously with minimal human intervention, which enables self-calibration, flexible reconfiguration, and long-term correctness of the system parameters. This method employs a bundle adjustment framework using a quadratic measurement for sensors and the magnetic dipole model for actuators. The proposed method has been verified in comparision with finite element simulations and existing calibration methods for magnetic actuators and sensor arrays. In the experiments, the determinant of coefficient was 99.84% for the sensor system and 99.45% for the actuation system, comparable with individual state-of-art calibration methods of calibrating magnetic actuators and sensor arrays. This method has potential to improve the reconfigurability and long-term accuracy of the magnetic robot localization and actuation systems, such as magnetically actuated capsule endoscopes.

2. 同时标定理论 simultaneous calibration method

在同时标定理论中，未标定的传感器和电磁铁工作作为标定源。即使系统的参数是不准确的在标定前，所有用于传感器和驱动系统的参数被同时标定通过标定模型的性质。假设当前的传感器被预先标定在标定前，同时磁传感器的位置是已知的。
In the simultaneous calibration method, the uncalibrated sensors and electromagnets work as calibration sources. Although the parameters of the system are inaccurate before the calibration, all the parameters for the sensor and actuation systems are calibrated simultaneously by the property of the calibration model. It is assumed that the current sensors are pre-calibrated before the calibration and the positions of the magnetic sensors are known.

理论包含三步。首先，电磁铁产生许多随机磁场。对于每次尝试，磁场被测量通过磁传感器，同时电流被测量通过一个电流传感器在每一个线圈上。第二，一个已知的样本磁铁的磁场被测量通过传感器阵列。样本磁铁被放置在工作区域内以一个任意的位置和朝向。第三，所有测量数据被喂入标定模型中，一个数值求解器找解。
The method is composed of three steps. First, the electromagnets generate a number of random magnetic fields. For each trial, the magnetic field is measured by magnetic sensors while the electric currents are measured by a current sensor at each coil. Second, the magnetic field of a known sample magnet is measured by the sensor array. The sample magnet is placed inside the working space with an arbitrary position and orientation. Third, all the measurement data are fed into the calibration model, and a numerical solver finds the solution.

2.B. 框架：像BA一样标定 Framework: calibration as BA

在我们的理论中，电流和磁场的测量能被同等地对待为BA方法中图像的特征点。位置，朝向，和驱动器和传感器的增益类似于路标和相机的参数。这些参数被特定条件下的磁场极子模型所限制。
In our method, the measurements of the electric currents and the magnetic field can be treated equivalently as the feature points of the images in BA. The positions, orientations, and gains of the actuators and sensors are similar to the parameters of the landmarks and camera(s). These parameters are constrained by the magnetic field dipole model under specific conditions.

2.C. 参数化 parametrization

2.C.(1). 传感器参数 sensor parameters

在这篇文章中，我们用一个3D二次模型。这个模型不仅仅能够精确地抓取传感器的非线性，而且也是计算上实用的。包含传感器朝向的3D二次模型涉及27个参数对于每个传感器。我们假设传感器的位置是已知的。实验装置有64个传感器，因此1782个参数对于传感器们被表示。((9+6*3)*64=1728)
In this paper, we use a 3D quadratic model. This model is not only able to capture the nonlinearity of the sensors precisely, but is also computationally practical. The 3D quadratic model with sensor orientations involves 27 parameters for each sensor. We assume that the positions of the sensors are known. The experimental setup has 64 sensors, thus 1728 parameters for the sensors are represented.

2.C.(2). 驱动器参数 actuator paremeters

一个磁驱动器有一个位置，一个朝向，和一个磁矩。一个典型的电磁铁有一个线圈和一个核，并且它们的轴是一起对齐的。在这样的一个情境下，电磁铁的朝向和磁矩的大小能被组合进入一个3D向量(朝向和大小可以用一个3D向量表示)。一个带有多于两个核的系统有一个互相感应，这可能会改变磁性中心的位置和磁矩的朝向与一个单独电磁铁相比的话。实验装置有9个驱动器，因此54个参数对于这些驱动器被表示。(9*6=54, 3D [position] + 3D [orientation + magnidute])
A magnetic actuator has a position, an orientation, and a magnetic moment. A typical electromagnet has a coil and a core, and their axes are aligned together. In such a case, the orientation and the magnitude of the magnetic moment of the electromagnet can be combined into a 3D vector. A system with more than two cores has a mutual induction, which might change the magnetic center’s position and the magnetic moment’s orientation compared to a single electromagnet. The experimental setup has 9 actuators, thus 54 parameters for these actuators are represented.

2.D. 标定模型 calibration model

标定模型是基于一个传感器测量模型和一个磁驱动模型。来自驱动器的磁场被磁传感器测量，并且数值被表述以任意单位。另外，电流被测量通过被连接到驱动器的电流传感器。我们假设电流传感器是唯一可信赖的带有0平均值的高斯噪声污染的传感器。
The calibration model is based on a sensor measurement model and a magnetic actuation model. The magnetic fields from the actuators are measured by the magnetic sensors, and the values are expressed in arbitrary units. Additionally, the electric currents are measured by the current sensor attached to the actuators. We assume that the current sensors are the only reliable sensors with zero-mean Gaussian noise corruption.

2.D.(1). 传感器测量模型 sensor measurement model

我们假设传感器是非线性的，它们的轴是弯曲的和旋转的，并且增益是不同于厂家规格的。使用一个二次模型，在第i个传感器上的恢复的B-场在第k次测量是：
We assume that the sensor is non-linear, of which the axes are skewed and rotated, and the gains are different from the factory specifications. Using a quadratic model, the recovered B-field on the i-th sensor at the k-th measurement is:

$${\mathbf{b}}_{ik}({\mathbf{p}}_{s,i},{\mathbf{v}}_{ik})=G_i{\mathbf{v}}_{ik}+\left(\begin{array}{c}{\mathbf{v}}_{ik}^T{H}_{x,i}{\mathbf{v}}_{ik}\\ {\mathbf{v}}_{ik}^T{H}_{y,i}{\mathbf{v}}_{ik}\\ {\mathbf{v}}_{ik}^T{H}_{z,i}{\mathbf{v}}_{ik}\end{array}\right)$$

这儿的${\mathbf{p}}_{s,i}$是用于 $G_i$, $H_{x,i}$, $H_{y,i}$ 和 $H_{z,i}$ 的参数的一个排列。在这个表达式中，$G_i$ 是一个不带有额外限制的线性映射，表示它代表尺度，旋转，偏移的影响。$H_{x,i}$, $H_{y,i}$, $H_{z,i}$ 是对称的3×3矩阵，它们包含了旋转和二次项。（${\mathbf{p}}_{s,i}$包含6*3+9个参数）

因为每个传感器在传感器阵列中有多个测量，这是方便的来表达恢复B-场数值从整个测量中以一个堆叠矩阵形式：

$${\mathbb{V}}_s({\mathbf{p}}_s,\mathbf{v})=\left(\begin{array}{ccc}{\mathbf{b}}_{11}({\mathbf{p}}_{s,1},{\mathbf{v}}_{11})& \cdots & {\mathbf{b}}_{1L}({\mathbf{p}}_{s,1},{\mathbf{v}}_{1L})\\ ⋮& \ddots & ⋮\\ {\mathbf{b}}_{N1}({\mathbf{p}}_{s,N},{\mathbf{v}}_{N1})& \cdots & {\mathbf{b}}_{NL}({\mathbf{p}}_{s,N},{\mathbf{v}}_{NL})\end{array}\right)$$

2.D.(2). B-场驱动模型 B-field actuation model

在当前的问题中，作者选择一个单个极子模型用于每一个单位电流贡献。在第i个传感器来自第j个驱动器的极子磁场在第k次测量被表述为：
In the current problem, the authors chose a single dipole for each unit-current contribution. The dipole magnetic field the i-th sensor from the j-th actuator at the k-th measurement is expressed as:

$${\mathbf{b}}_{i,j,k}={\mathbf{B}}_{i,j}{\mathbf{m}}_j{I}_{jk}$$

$${\mathbf{B}}_{i,j}=\frac{{\mu }_0}{4\pi ||{\mathbf{r}}_{ij}|{|}^3}\left(3\widehat{{\mathbf{r}}_{ij}}{\widehat{{\mathbf{r}}_{ij}}}^T-{\mathbf{I}}^3\right),{\mathbf{r}}_{ij}={\mathbf{p}}_i-{\mathbf{p}}_j$$

在来自多个电流输入的多个测量点的磁场能被表达为一个矩阵等式：

$${\mathbf{b}}_k=\mathbb{B}\mathbb{M}{\mathbf{i}}_k$$

$$\mathbb{B}=\left(\begin{array}{ccc}{\mathbf{B}}_{11}& \cdots & {\mathbf{B}}_{1M}\\ ⋮& \ddots & ⋮\\ {\mathbf{B}}_{N1}& \cdots & {\mathbf{B}}_{NM}\end{array}\right),\mathbb{M}=\left(\begin{array}{ccc}{\mathbf{m}}_1& \cdots & 0\\ ⋮& \ddots & ⋮\\ 0& \cdots & {\mathbf{m}}_M\end{array}\right)$$

这是方便的来表述B场值进入一个堆叠矩阵形式：

$${\mathbb{V}}_a({\mathbf{p}}_a,\mathbf{i})=\left(\begin{array}{ccc}{\mathbf{b}}_1& \cdots & {\mathbf{b}}_L\end{array}\right)=\mathbb{B}\mathbb{M}\left(\begin{array}{ccc}{\mathbf{i}}_1& \cdots & {\mathbf{i}}_L\end{array}\right)=\mathbb{B}\mathbb{M}\mathbb{J}$$

这儿的${\mathbf{p}}_a$是一个包含所有驱动参数的向量。（${\mathbf{p}}_a$包含6*9个参数）

2.E. 一个代价函数的定制 the formulation of a cost function

代价函数被定制通过最小化由传感器模型和驱动器模型给定的B场数值之间的误差。问题能以一个最小二乘方式被呈现：
The cost function is formulated by minimizing the errors between the B-field values given by the sensor model and the actuator model. The problem can be stated in a least square manner:

$$min||{\mathbb{V}}_s({\mathbf{p}}_s,\mathbf{v})-{\mathbb{V}}_a({\mathbf{p}}_a,\mathbf{i})|{|}_F^2$$

[1]: Son, Donghoon, Xiaoguang Dong, and Metin Sitti. “A Simultaneous Calibration Method for Magnetic Robot Localization and Actuation Systems.” IEEE Transactions on Robotics 35.2 (2018): 343-352.