Nonholonomic Closed-loop Velocity Control of a Soft-tethered Magnetic Capsule Endoscope

此文非常好！此文章是一篇标准的工程实现，从机器人joint space推导到磁力和磁力矩的关系，然后可以通过雅可比反解出joint增量来控制机械臂完成轨迹规划。在速度控制方面，利用胶囊的位置误差和速度误差，以及角度误差，可以估计出需要的磁力和磁力矩，还考虑累计尾绳的阻力，在驱动的同时，还对胶囊和机器臂的位姿进行定位，根据新的位姿和位姿导数，重新进行新一轮计算。从而完成闭环控制。

一个软尾绳磁胶囊内窥镜的非完整的闭环速度控制
Nonholonomic Closed-loop Velocity Control of a Soft-tethered Magnetic Capsule Endoscope [1]
Paper Link
Authors: Taddese, Addisu Z., et al.
2016, IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)

0. Abstract 摘要

在这片文章中，我们展示了一个拖线磁胶囊内窥镜的速度级闭环控制我们的控制，内窥镜通过在末端执行器装有一个永磁铁的串型操作器驱动。闭环控制（在位置上2自由度，且在朝向上2自由度）是可能的，使用了一个实时磁定位算法，这个算法使用驱动磁场，且不需要额外的硬件。速度控制被实现来创建光滑运动，这是结肠癌检查中临床必须的。我们的控制算法生成一条样条，该样条通过一组输入点，这些输入点大致定义了所需轨迹的形状。速度控制器沿与路径的切线方向起作用，而辅助位置控制器对胶囊运动施加非完整约束。内腔自然会施加一个柔软的非完整限制，同时我们执行一个严格的限制用于尾绳干扰更精确的估计和医师远程操作的假设直观性。积分干扰力估计控制项被引入来预测尾绳的干扰。这篇文章展示理论公式和我们理论的实验验证。结果表明，该系统具有在低稳态误差的情况下实现可重复的速度阶跃响应的能力，以及系留胶囊在弯道附近机动的能力。
In this paper, we demonstrate velocity-level closedloop control of a tethered magnetic capsule endoscope that is actuated via serial manipulator with a permanent magnet at its end-effector. Closed-loop control (2 degrees-of-freedom in position, and 2 in orientation) is made possible with the use of a real-time magnetic localization algorithm that utilizes the actuating magnetic field and thus does not require additional hardware. Velocity control is implemented to create smooth motion that is clinically necessary for colorectal cancer diagnostics. Our control algorithm generates a spline that passes through a set of input points that roughly defines the shape of the desired trajectory. The velocity controller acts in the tangential direction to the path, while a secondary position controller enforces a nonholonomic constraint on capsule motion. A soft nonholonomic constraint is naturally imposed by the lumen while we enforce a strict constraint for both more accurate estimation of tether disturbance and hypothesized intuitiveness for a clinician’s teleoperation. An integrating disturbance force estimation control term is introduced to predict the disturbance of the tether. This paper presents the theoretical formulations and experimental validation of our methodology. Results show the system’s ability to achieve a repeatable velocity step response with low steady-state error as well as ability of the tethered capsule to maneuver around a bend.

1. 介绍 Introduction

在位置控制方案中，根据一个固定达到路点来计算误差，这导致误差减少，同时，直到下一个路点被设置前速度减少。在速度控制中，更平滑运动是可能的因为这样的减速不会发生。我们期望最小化生涩的运动的影响。
In a position control scheme, the error is computed according to a fixed approaching waypoint, that results in a decrease in error and, in turn, a decrease in velocity until the next waypoint is set. In velocity control, on the other hand, smoother motion is possible because this deceleration does not occur. We wish to minimize effects of such jerky motion.

3. 控制 Control

3.A 理论模型 Theoretical Modeling

驱动磁铁位置、磁矩方向变化与机械臂关节角度变化的关系：
$$\left[\begin{array}{c}\dot{{\mathbf{p}}_a}\\ \dot{\widehat{{\mathbf{m}}_a}}\end{array}\right]=\left[\begin{array}{cc}\mathbf{I}& 0\\ 0& \mathbf{S}(\widehat{{\mathbf{m}}_a}{)}^T\end{array}\right]{\mathbf{J}}_R\dot{\mathbf{q}}={\mathbf{J}}_A\dot{\mathbf{q}}$$

胶囊上磁力、磁力矩变化与驱动位移、驱动磁铁方向、胶囊磁铁方向变化的关系：
$$[\begin{array}{c}\\ \dot{\mathbf{f}}\end{array}$$