Five-degree-of-freedom manipulation of an untethered magnetic device in fluid using a single permane

一篇IJRR，非常经典的控制驱动下的定位过程，非常详细，系统算法描述完成后，对系统中存在的奇异点进行了分析，还有其他的性能的分析。是一篇详细系统阐述和系统性能的解释。

使用单个永磁体用于液体中的无尾绳磁设备的五自由度操纵并应用于胃部胶囊内窥镜检查
Five-degree-of-freedom manipulation of an untethered magnetic device in fluid using a single permanent magnet with application in stomach capsule endoscopy [1]
Paper Link
Authors: Mahoney, Arthur W., and Jake J. Abbott.
2016, The International Journal of Robotics Research (IJRR)

0. 摘要 Abstract

这篇文章演示在液体中的一个样品磁胶囊内窥镜的磁三自由度闭环位置和二自由度闭环朝向控制，使用一个被一个商业6D机械操作器指位的单独永磁铁，使用被一个定位系统测量的仅仅3D胶囊位置反馈，应用在一个液体膨胀胃部的胶囊内窥镜检查。我们分析磁操纵的运动学，磁操纵使用一个单个永磁铁作为串型机器人操作器的末端执行器，同时我们列出一个控制理论，使能胶囊的位置和方向被控制当机械操作器不在一个运动学奇点的时候，同时那个理论牺牲了胶囊方向上的控制来保持机器人位置上的控制当操作器进入一个奇点时。我们演示理论在控制率降低到25Hz,降低定位率到30Hz的鲁棒性，展示所施加磁场和期望的偏差，和操作器的奇点。一个无绳磁设备的5自由度操纵之前已经仅仅被使用电磁系统所展示。
This paper demonstrates magnetic 3DoF closed-loop position and 2DoF open-loop orientation control of a mockup magnetic capsule endoscope in fluid with a single permanent magnet positioned by a commercial 6D robotic manipulator, using feedback of only the 3DoF capsule position measured by a localization system, with application in capsule endoscopy of a fluid-distended stomach. We analyze the kinematics of magnetic manipulation using a single permanent as the end-effector of a serial-link robotic manipulator, and we formulate a control method the enables the position and orientation of the capsule to be controlled when the robotic manipulator is not in a kinematic singularity, and that sacrifices the control over the direction of the capsule to maintain control over robot’s position when the manipulator enters a singularity. We demonstate the method’s robustness to a reduced control rate of 25Hz, reduced localization rates down to 30Hz, deviation in the applied magentic field from that expected, and the presence of manipulator singularities. 5DoF manipulation of a untethered magnetic device has been previously demonstrated by electromagnetic systems only.

2. 用一个单个永磁体控制 Control using a single permanent magnet

主动磁场使用dipole model建模：$\mathbf{h}(\mathbf{p},\widehat{{\mathbf{m}}_a})=\frac{||{\mathbf{m}}_a||}{4\pi ||\mathbf{p}|{|}^3}D(\hat{\mathbf{p}})\widehat{{\mathbf{m}}_a}$, $D(\hat{\mathbf{p}})=3\hat{\mathbf{p}}{\hat{\mathbf{p}}}^T-\mathbf{I}$, $\mathbf{p}={\mathbf{p}}_c-{\mathbf{p}}_a$，而且使用了球形磁铁，最适合dipole model建模的三维体。

机器人joint速度与终端的pose速度之间的关系：

$$\left[\begin{array}{c}\dot{{\mathbf{p}}_a}\\ {\omega }_{\mathbf{a}}\end{array}\right]=J_R(\mathbf{q})\dot{\mathbf{q}}$$

主动磁铁位置，磁矩方向变换与机器人joint速度之间的关系：

$$\left[\begin{array}{c}\dot{{\mathbf{p}}_a}\\ \dot{\widehat{{\mathbf{m}}_a}}\end{array}\right]=\left[\begin{array}{cc}\mathbf{I}& 0\\ 0& S(\widehat{{\mathbf{m}}_a}{)}^T\end{array}\right]J_R(\mathbf{q})\dot{\mathbf{q}}=J_A(\mathbf{q})\dot{\mathbf{q}}$$

当磁胶囊在液体中被低速小加速度驱动，并与其他物体没有接触，对于胶囊朝向改变有微小阻力，这能使磁力矩快速将胶囊的偶极子矩与外加磁场对齐。
When the capsule is actuated in fluid at low speeds, small accelerations, and without contact with other objects, there is little resistance to change in the capsule’s heading, which enables the magnetic torque to quickly align the capsule’s dipole moment with the applied magnetic field.

我们能假设，胶囊的偶极磁矩每时每刻大约与施加场对齐：$\widehat{{\mathbf{m}}_c}=\hat{\mathbf{h}}$

请注意，5D磁控制能被施行而不需要低速，小加速度，非接触的假设，如果胶囊的5D位姿可测量的话。在这种情况下，施加力和施加磁力矩的方向能被独立控制（磁力矩大小不能独立于磁力大小之外控制）。正如我们将演示的，5D控制能只通过3D位置测量获得，胶囊能自由对齐施加场。
Note that 5DoF magnetic control can be performed without making the low-speed, small-acceleration, and non-contact assumptions, if a 5DoF measurement of the capsule’s pose is available. In this case, the applied force and the direction of the applied magnetic torque can be independently controlled (the torque magnitude can not be controlled independently of the force magnitude). As we will demonstrate, 5DoF control can still be achieved using only 3DoF measurement of the capsule’s position, provided the capsule is free to align itself with the applied field.

则：$$\left[\begin{array}{c}\mathbf{f}\\ \hat{\mathbf{h}}\end{array}\right]=\mathbf{F}(\mathbf{p},\widehat{{\mathbf{m}}_a})$$

开始求inverse过程：

$$\left[\begin{array}{c}\dot{\mathbf{f}}\\ \dot{\hat{\mathbf{h}}}\end{array}\right]={\mathbf{J}}_F(\mathbf{p},\widehat{{\mathbf{m}}_a})\left[\begin{array}{c}\dot{\mathbf{p}}\\ \dot{\widehat{{\mathbf{m}}_a}}\end{array}\right]={\mathbf{J}}_F(\mathbf{p},\widehat{{\mathbf{m}}_a})(\left[\begin{array}{c}\dot{{\mathbf{p}}_c}\\ 0\end{array}\right]+\left[\begin{array}{cc}-\mathbf{I}& 0\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}\dot{{\mathbf{p}}_a}\\ \dot{\widehat{{\mathbf{m}}_a}}\end{array}\right])$$

所以：

$$\left[\begin{array}{c}\dot{\mathbf{f}}\\ \dot{\hat{\mathbf{h}}}\end{array}\right]-{\mathbf{J}}_F(\mathbf{p},\widehat{{\mathbf{m}}_a})\left[\begin{array}{c}\dot{{\mathbf{p}}_c}\\ 0\end{array}\right]={\mathbf{J}}_F(\mathbf{p},\widehat{{\mathbf{m}}_a})\left[\begin{array}{cc}-\mathbf{I}& 0\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}\dot{{\mathbf{p}}_a}\\ \dot{\widehat{{\mathbf{m}}_a}}\end{array}\right]$$

$$\to \delta \mathbf{d}=\left[\begin{array}{c}\dot{\mathbf{f}}\\ \dot{\hat{\mathbf{h}}}\end{array}\right]-{\mathbf{J}}_F(\mathbf{p},\widehat{{\mathbf{m}}_a})\left[\begin{array}{c}\dot{{\mathbf{p}}_c}\\ 0\end{array}\right]={\mathbf{J}}_F(\mathbf{p},\widehat{{\mathbf{m}}_a})\left[\begin{array}{cc}-\mathbf{I}& 0\\ 0& \mathbf{I}\end{array}\right]J_A(\mathbf{q})\dot{\mathbf{q}}={\mathbf{J}}_{FA}(\mathbf{q})\dot{\mathbf{q}}$$

左侧是期望变化，右边是机械臂应该做出的改变。

则pseudoinverse为：

$$\delta \mathbf{q}=J_{FA}(\mathbf{q}{)}^{†}\delta \mathbf{d}$$

2.1 分析雅可比${\mathbf{J}}_{FA}$ analyzing the Jacobian ${\mathbf{J}}_{FA}$

对于5D完整控制，$J_{FA}$一定是rank5。因为$J_{FA}$是$J_F$和$J_A$的乘积，我们将分别分析雅可比$J_F$和雅可比$J_A$的rank。
For 5D holonomic control, $J_{FA}$ must be rank 5. Since $J_{FA}$ is the product of $J_F$ and $J_A$, we will analyze the rank of $J_F$ and $J_A$ separately.

我们首先放缩$J_F$的列和行来制造一个无量纲的雅可比${\tilde{J}}_F$。
We first scale the rows and columns of $J_F$ to produce a non-dimentional ${\tilde{J}}_F$.

$$\widetilde{{\mathbf{J}}_F}(\mathbf{p},\widehat{{\mathbf{m}}_a})=\left[\begin{array}{cc}\frac{1}{||\mathbf{f}||}\mathbf{I}& 0\\ 0& \mathbf{I}\end{array}\right]{\mathbf{J}}_F\left[\begin{array}{cc}||\mathbf{p}||\mathbf{I}& 0\\ 0& \mathbf{I}\end{array}\right]$$

无量纲的Jacobian $\widetilde{{\mathbf{J}}_F}$是通过将JF与一系列基本矩阵进行预乘和乘积运算得到的，这保证了$\widetilde{{\mathbf{J}}_F}$的秩=${\mathbf{J}}_F$的秩，并使得能够使用单位一致奇异值的$\widetilde{{\mathbf{J}}_F}$的奇异值分解来找到${\mathbf{J}}_F$的秩，单位一致奇异值揭示${\mathbf{J}}_F$的秩。因为施加场方向在平行于自己的方向不能改变，所以最小奇异值${\sigma }_6$一定是零。
The nondimensional Jacobian Jacobian$\widetilde{{\mathbf{J}}_F}$ is produced by post- and pre-multiplying ${\mathbf{J}}_F$ with a series of elementary matrices, which guarantees that rank $\widetilde{{\mathbf{J}}_F}$ = ${\mathbf{J}}_F$ and enables the rank of ${\mathbf{J}}_F$ to be found using the singular value decomposition of $\widetilde{{\mathbf{J}}_F}$ with unit-consistent singular values, which reveal the rank of ${\mathbf{J}}_F$. Since the applied field direction cannot change in a direction parallel to itself, the smallest singular value ${\sigma }_6$ must be zero.

$J_F$rank5的事实推断出在空间中一个单个永磁体，不管机器人操作器如何操作它，只能展示对一个无绳磁设备的5D控制。完整的机器人系统（包括磁体和操纵器）具有5自由度磁控制的能力仅受机器人操纵器具有三个自由度的驱动器磁体和两个自由度的驱动器磁体的偶极矩定位的能力所阻碍。如果$J_{Ａ}$的rank是5，那么机器人系统具有对无绳胶囊的5D控制。如果被要求获得一个期望合力和磁场朝向的驱动器磁铁位姿使得操作器进入一个动力学奇异点，那么5D磁操作就丢失了。
The fact that $J_F$ is always rank five implies that a single permanent magnet in space, irrespective of the robot manipulator that maneuvers it, can exhibit 5-DOF control over an untethered magnetic device. The ability of a complete robotic system, including magnet and manipulator, to exhibit 5-DOF magnetic control is precluded only by the ability of the robot manipulator to position the actuator magnet with three DOFs and the actuator magnet’s dipole moment with two DOFs. If the rank of the Jacobian JA is five, then the robotic system possesses 5-DOF control over the untethered capsule. If the actuator magnet pose required to achieve a desired applied total force and magnetic field heading places the manipulator into a kinematic singularity, then 5-DOF magnetic control is lost.

$J_A$的无量纲的雅可比${\tilde{J}}_A$:

$$\widetilde{{\mathbf{J}}_A}(\mathbf{q})=\left[\begin{array}{cc}\frac{1}{||\mathbf{p}||}\mathbf{I}& 0\\ 0& \mathbf{I}\end{array}\right]{\mathbf{J}}_A(\mathbf{q})$$

归一化雅可比：
the normalized Jacobian:

$$\widetilde{{\mathbf{J}}_{FA}}(\mathbf{q},\mathbf{p})=\stackrel{~}{{\mathbf{J}}_F(\mathbf{p},\widehat{{\mathbf{m}}_a})}\left[\begin{array}{cc}-\mathbf{I}& 0\\ 0& \mathbf{I}\end{array}\right]\widetilde{J_A(\mathbf{q})}$$

伪逆$\widetilde{{\mathbf{J}}_{FA}^{†}}$是最小化$||\delta \mathbf{q}||$逆映射如果机器人操纵器是过驱动的。$\widetilde{{\mathbf{J}}_{FA}^{†}}$最大的奇异值（ie,$\widetilde{{\mathbf{J}}_{FA}}$的最小非零奇异值的倒数）能被用来描述在施加力和场朝向的无量纲变化的一个单位大小向量如何近似被映射到在操作器关节的一个大小改变的最差情况。如果最大奇异值接近无穷，则机器人操纵器接近一个运动学奇异点。
The pseudoinverse$\widetilde{{\mathbf{J}}_{FA}^{†}}$ is the inverse mapping that minimizes $||\delta \mathbf{q}||$ if the robot manipulator is over-actuated. The largest singular value of $\widetilde{{\mathbf{J}}_{FA}^{†}}$ (i.e. the reciprocal of the smallest non-zero singular value of $\widetilde{{\mathbf{J}}_{FA}}$) can be used to describe the worst case of how a unitmagnitude vector of nondimensional change in applied force and field heading are approximately mapped to a magnitude change in manipulator joints. If the largest singular value approaches infinity, then the robot manipulator is near a kinematic singularity.

第一个例子是，将胶囊竖直放置，被动磁矩朝下，为了产生和胶囊磁矩一样的磁场方向，作者选取了周围很多个主动磁铁位姿，都要确保在胶囊上产生向下的磁场，但不一样的磁力。然后计算在这些位置，伪逆的奇异值的最大值（也就是$\widetilde{{\mathbf{J}}_{FA}}$最小非零值的导数），plot到图上，可以观察到奇异值偏大的区域。第二个例子是，将胶囊水平放置，也为了产生和胶囊磁矩一样的磁场方向，选取周围很多主动磁铁驱动位姿，产生相同的磁场方向但不一样的磁力。计算在这些位置的伪逆的最大值，plot到图中方便观察。

2.2 处理操纵器奇异点 Managing manipulator singularities

在磁操纵内容中，在驱动器磁铁配置的追踪误差将导致在施加到胶囊的期望磁力和场方向的误差，潜在导致失去控制。而不是可能牺牲整个胶囊控制当操作器在一个奇异点附近的时候，我们选择实现一个策略，这个策略牺牲胶囊朝向控制同时保持施加在胶囊上磁力的控制。
In the context of magnetic manipulation, tracking error in the actuator magnet’s configuration will cause error in the desired magnetic force and field heading applied to the capsule, potentially resulting in loss of control. Rather than potentially sacrificing total capsule control when the manipulator nears a singularity, we have chosen to implement a strategy that sacrifices control over the capsule’s heading while maintaining control over the magnetic force applied to the capsule.

给在施加方向上一些小的期望的变化$\delta \widehat{{\mathbf{h}}_d}$和在施加磁力上一些小的期望的变化$\delta {\mathbf{f}}_d$，牺牲朝向控制同时保持施加磁力控制的问题被表示为一个有限制的，二次的最小二乘问题：
Given some small desired change in applied field heading $\delta \widehat{{\mathbf{h}}_d}$ and some small desired change in applied magnetic force $\delta {\mathbf{f}}_d$, the problem of sacrificing heading control while maintaining control over the applied magnetic force is posed as a constrained, quadratic least-squares problem:

$$minimize||\frac{\partial \hat{\mathbf{h}}}{\partial \mathbf{q}}\delta \mathbf{q}-\delta \widehat{{\mathbf{h}}_d}|{|}^2$$

$$subjectto\frac{\partial \mathbf{f}}{\partial \mathbf{q}}\delta \mathbf{q}=\delta {\mathbf{f}}_d$$

$$||W\delta \mathbf{q}||\le r$$

这里矩阵$\frac{\partial \mathbf{f}}{\partial \mathbf{q}}$和$\frac{\partial \hat{\mathbf{h}}}{\partial \mathbf{q}}$分别是雅可比${\mathbf{J}}_{FA}$的顶部和底部三行。限制条件保证了在施加力的期望改变$\delta {\mathbf{f}}_d$会达到(由于$\frac{\partial \mathbf{f}}{\partial \mathbf{q}}$是行满秩)，并且在关节运动的大小上强置一个最大值边界通过可逆矩阵$W$。这个代价函数尝试减少在施加场朝向上期望和实际改变的差值。权重矩阵$W$能被用来增加选择关节运动的代价或者均一化$\delta \mathbf{q}$不同单位。
where the matrices $\frac{\partial \mathbf{f}}{\partial \mathbf{q}}$ and $\frac{\partial \hat{\mathbf{h}}}{\partial \mathbf{q}}$ are the top and bottom three rows of the Jacobian ${\mathbf{J}}_{FA}$, respectively. The constraint guarantees the desired change in applied force $\delta {\mathbf{f}}_d$ is met (provided $\frac{\partial \mathbf{f}}{\partial \mathbf{q}}$ has full row rank), and the constraint enforces a maximum bound on the magnitude of joint motion weighted by the invertible matrix $W$. The cost function attempts to reduce the error between the desired and actual change in applied field heading. The weight matrix $W$ can be used to increase the ‘cost’ of select joint motions or to homogenize disparate units of $\delta \mathbf{q}$.

2.3 放缩驱动磁铁 Scaling the actuator magnet

如果胶囊重力有变化，通过改变驱动器磁矩大小，可以保持在一定的分离距离平衡。

[1]: Mahoney, Arthur W., and Jake J. Abbott. “Five-degree-of-freedom manipulation of an untethered magnetic device in fluid using a single permanent magnet with application in stomach capsule endoscopy.” The International Journal of Robotics Research 35.1-3 (2016): 129-147.