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}}$$

胶囊上磁力、磁力矩变化与驱动位移、驱动磁铁方向、胶囊磁铁方向变化的关系：
$$\left[\begin{array}{c}\dot{\mathbf{f}}\\ \dot{\tau }\end{array}\right]={\mathbf{J}}_F(\mathbf{p},\widehat{{\mathbf{m}}_a},\widehat{{\mathbf{m}}_c})\left[\begin{array}{c}\dot{\mathbf{p}}\\ \dot{\widehat{{\mathbf{m}}_a}}\\ \dot{\widehat{{\mathbf{m}}_c}}\end{array}\right]$$

联立以上两式，得到，胶囊上磁力、磁力矩期望变化和胶囊位姿变化与机械臂关节角度变化之间的关系：
$$\left[\begin{array}{c}\dot{\mathbf{f}}\\ \dot{\tau }\end{array}\right]-{\mathbf{J}}_F\left[\begin{array}{c}\dot{{\mathbf{p}}_c}\\ 0\\ \dot{\widehat{{\mathbf{m}}_c}}\end{array}\right]={\mathbf{J}}_F\left[\begin{array}{ccc}-\mathbf{I}& 0& 0\\ 0& \mathbf{I}& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\mathbf{J}}_A\\ 0\end{array}\right]={\mathbf{J}}_{FA}\dot{\mathbf{q}}$$

3.B 轨迹生成 Trajectory Generation

我们现在关注于控制，设置的一条指定并自动跟踪的固定轨迹。控制算法的输入是粗糙描述轨迹的预设轨迹点的集合。一个样条然后被生成一旦控制算法开始，这样条在路径点之间拟合一条曲线，曲线平滑到二次导数。在弯曲上的连续性允许平滑的非完整运动。三次样条插值的使用允许我们创造一条穿过任意数量的期望点的平滑曲线，允许我们来用许多路点来定义轨迹。这个曲线是节点（路点）的三次多项式缝合，不像贝兹曲线，多项式函数的阶数不会随着路点数量的增加而增加，因而不会产生计算过载。
we now focus our attention on control and thus set a fixed trajectory that is commanded and followed autonomously. The input to the controller algorithm is a set of pre-defined trajectory points that roughly describe the trajectory. A spline is then generated once the control algorithm begins that fits a curve that is smooth up to second degree between the path points. A continuum in curvature allows for smooth nonholonomic motion. The use of cubic spline interpolation allows us to create a smooth curve that passes through any number of desired points, allowing us to specify trajectories by a variable number of waypoints. This curve is a stitching of cubic polynomials that join at knots (the waypoints) and, unlike Bezier curves, the order of polynomial functions does not increase with an increased number of waypoints and thus does not become computationally overbearing.

3.C 速度控制 Velocity Control

我们预估作用在胶囊的净力，并且计算期望的力和力矩，这力和力矩将驱使胶囊去最小化来自指令速度输入的误差。我们使用极子极子模型来预估作用到胶囊上的磁力和磁力矩，并且使用一个前馈阻力项来估计作用在胶囊上的从尾绳上传来的力。这些阻力被假设为总是作用在最近的样条点的反向切线方向。
we estimate the net force applied on the capsule and calculate desired forces and torques that will drive the capsule to minimize error from the commanded velocity input. We use the dipoledipole model to estimate the magnetic forces and torques applied to the capsule and use a feed-forward resistance term to estimate the forces applied to the capsule from the tether. These resistance forces are assumed to always act in the negative tangent direction with respect to the nearest spline point.

定义速度误差：$${\mathbf{e}}_v=\dot{{\mathbf{p}}_{cdes}}-\dot{{\mathbf{p}}_c}$$
定义位置误差：$${\mathbf{e}}_p={\mathbf{p}}_{cdes}-{\mathbf{p}}_c$$
定义朝向误差：$${\mathbf{e}}_o=\widehat{{\mathbf{h}}_c}\times \widehat{{\mathbf{h}}_{des}}$$
定义绳子的前馈项：$${\mathbf{f}}_{feed-forward}=-K_i\int {\mathbf{e}}_v$$

期望的位置不是期望的下一个位置，就像位置控制策略力使用的那样，而是在样条上最近的点。恒定大小的一个期望速度被指定在$t$的方向上，速度误差被投影到这个方向。一个比例位置控制在投影到$n$上的误差上执行，并执行非完整运动。$t$方向的速度误差，$n$方向的位置误差，$o$角度误差，生成控制力和力矩。
The desired position is not the desired next position as would be used in a position control scheme, rather the nearest point on the spline. A desired velocity of constant magnitude is commanded in the direction of $t$, onto which the velocity error is projected. A proportional position control acts on the error projected onto $n$ and enforces nonholonomic motion.

对期望磁力和磁力矩的控制器（位置速度环）：

$$\left[\begin{array}{c}{\mathbf{u}}_f\\ {\mathbf{u}}_{\tau }\end{array}\right]=\left[\begin{array}{c}(K_{P_{vel}}{\mathbf{e}}_v+K_{D_{vel}}\dot{{\mathbf{e}}_v})\hat{\mathbf{t}}+K_{P_{pos}}{\mathbf{e}}_p\hat{\mathbf{n}}\\ K_{P{O}_{vel}}{\mathbf{e}}_o\end{array}\right]$$

每次作用到胶囊的磁力和磁力矩增量：

$$\left[\begin{array}{c}\dot{\mathbf{f}}\\ \dot{\tau }\end{array}\right]=\left[\begin{array}{c}{\mathbf{u}}_f-({\mathbf{f}}_m+{\mathbf{f}}_{feed-forward})\\ {\mathbf{u}}_{\tau }\end{array}\right]$$

[1]: Taddese, Addisu Z., et al. “Nonholonomic closed-loop velocity control of a soft-tethered magnetic capsule endoscope.” 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2016.