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过程：

[ f ˙ h ^ ˙ ] = J F ( p , m a ^ ) [ p ˙ m a ^ ˙ ] = J F ( p , m a ^ ) ( [ p c ˙ 0 ] + [ − I 0 0 I ] [ p a ˙ m a ^