Adaptive Dynamic Control for Magnetically Actuated Medical Robots

一种动态控制方法，用于胶囊重力抵消，使胶囊悬浮起来。还可以自动估计动力学方程的参数。

用于磁驱动医疗机器人的自适应动态控制
Adaptive Dynamic Control for Magnetically Actuated Medical Robots [1]
Paper Link
Authors: Barducci, Lavinia, et al.
2019, IEEE robotics and automation letters (RA-L)

0. 摘要 Abstract

我们讨论一个新型动态控制方案，用于磁驱动机器人，通过提出一个自适应控制技术，针对参数不确定性和未知边界干扰。前者通常产生于片面知晓机器人动力学参数，后者是与非结构化环境的不可预测交互的结果。为了展示所提出方案的应用，我们考虑控制被单个外部永磁体驱动的磁性灵巧内窥镜，这包含一个带有软尾绳的内永磁体。我们提供实验分析来展示MFE的磁悬浮可能性-用这个平台的最困难任务之一-在片面知晓IPM的动力学方程和不知晓尾绳的行为的情况下。在亚克力管道中的实验展示了，与非磁悬浮相比，减少了32%的接触，并且任务完成速度相对于原先提出的磁悬浮技术快了1.75倍。
we discuss a novel dynamic control approach for magnetically actuated robots, by proposing an adaptive control technique, robust toward parametric uncertainties and unknown bounded disturbances. The former generally arise due to partial knowledge of the robots’ dynamic parameters, such as inertial factors, the latter are the outcome of unpredictable interaction with unstructured environments. In order to show the application of the proposed approach, we consider controlling the magnetic flexible endoscope (MFE), which is composed of a soft-tethered internal permanent magnet (IPM), actuated with a single external permanent magnet. We provide with experimental analysis to show the possibility of levitating the MFE—one of the most difficult tasks with this platform—in case of partial knowledge of the IPM’s dynamics and no knowledge of the tether’s behavior. Experiments in an acrylic tube show a reduction of contact of the 32% compared to non-levitating techniques and 1.75 times faster task completion with respect to previously proposed levitating techniques.

1. 介绍 Introduction

因为这些机器人的控制输入是力和力矩，这特别有效去考虑一个准静态控制或一个动态控制策略。后者有考虑机器人的整体物理性质，并保证更快更精准的控制。
Since the control inputs for these robots are forces and torques, it is particularly effecacious to consider a quasi-static, or a dynamic control approach. The latter has the advantage of considering the overall physical properties of the robots and permits faster and more accurate control.

我们考虑部分知晓IPM的质量和尺寸，不知道尾绳的信息。
Moreover,we consider partial knowledge of themass and dimensions of the IPM and no information about the tether.

在这里，我们展示成功的磁悬浮，这帮助克服以前提出控制技术的主要问题：在IPM和EPM之间连续吸引。成功的悬浮能鼓励避障和更柔性的导航。它也能导致施加到环境的压力的一个衰减，这将较少病人的不舒适和不良事件的危险。
Herein, we show successful magnetic levitation which helps overcoming the major issue of previously proposed control techniques: continuous attraction between the IPM and EPM. Successful levitation can encourage obstacle avoidance and a smoother navigation. It can also result in a reduction of pressure applied to the environment which will reduce discomfort for the patient and risk of adverse events.

2. 控制概况 Control Overview

实际上，我们需要保证在IPM上的力在平衡状态下抵消重力，平衡状态是高度不稳定的。一个动态控制方案考虑作用在系统上的所有力。特别地，在磁铁之间的耦合以相互作用力的名义表达；悬浮是这些力和重力的平衡的结果。
In fact, we need to guarantee that the force on the IPM counteracts gravity in an equilibrium state that is highly unstable. A dynamic control approach takes into account all forces that act on the system. In particular, the coupling between magnets is directly expressed in terms of interaction (generalized) forces; levitation is the outcome of the equilibrium of these forces with gravity

之前技术的主要缺点依赖于两个主要假设：期望的轨迹被认为是一个时间的分段函数，IPM的尾绳交互被认为可忽略。前者限制IPM运动的速度，同时后者不保证在通用场景下收敛。在这篇文章中，我们目的是弱化这两个假设，通过使用一个自适应动态控制和通过证明所提出方法的最终统一的有界稳定性。
The main drawback of previous technique lies in two main assumptions: the desired trajectory was considered as a piece-wise constant function of the time and IPM-tether interactions were assumed negligible. The former restricts velocity of the IPM movements, while the latter does not guarantee convergence in a general scenario. In this letter, we aim to weaken both assumptions by employing an adaptive dynamic control and by proving ultimately uniform bounded stability of the proposed approach.

与之前提出的解法相比，我们展示一个新方式，它考虑IPM的动力学并处理可能的参数不确定性。这所提出的技术是一种动态控制，考虑不确定性，比如IPM的质量，尺寸，和尾绳的动力学。特别的，IPM的质量被尾绳剧烈地影响。在事实上，在悬浮中，尾绳的一个一致部分被抬起。所提出的控制策略自动修改它的参数使它们适应实际的系统动力学。因此，一个进一步的控制环被插入动态控制中来获得估计动态参数的收敛。
Compared to the previously proposed solution, we present a novel approach which takes into consideration the dynamics of IPM and deals with possible parametric uncertainties. The proposed technique is an adaptive control, which considers uncertainties such as the mass, dimensions of the IPM and the dynamics of the tether. In particular, the IPM mass is strongly affected by the tether. In fact, during levitation, a consistent section of the tether is lifted. The proposed control strategy autonomously modifies its parameters in order to adapt them to the actual system dynamics. Therefore, a further control loop has been inserted in the dynamic control in order to achieve convergence of the estimated dynamic parameters.

3. 动态控制 Dynamic Control

这控制有三个关键部件：位姿控制，参数估计和力控制。位姿控制，被认为是外环，目的是调节IPM到一个期望位姿。参数估计拓展控制器性质，因为这允许假设被弱化通过知晓系统动力学。力控制，也被指代为内环，目的是收敛真实力到期望的力。这种反推方法的稳定性保证了整体收敛性。
the control has three key components: pose control, parameters estimation and force control. Pose control, considered as the external loop, aims to steer the IPM to the desired pose. The parameter estimation improves the controller properties, since this allows the assumptions to be weakened by the knowledge of the system dynamics. Force control, referred to also as “internal loop”, aims to converge the actual force to the desired one. The stability of this backstepping approach guarantees the overall convergence.

考虑IPM的动力学：$B(x)\ddot{x}+C(x,\dot{x})\dot{x}+G(x)={\tau }_m(x,q)$

我们的目标是找到$q$使$x$接近一个期望值$x_d$。

这用两步获得：首先期望转矩(\tau_{d})的值被找到通过$x\to x_d$，考虑未知参数的动力学，然后我们定义$\dot{q}$用于${\tau }_m\to {\tau }_d$，根据力和力矩的动力学:

$$\dot{{\tau }_m}=\frac{\partial \tau (x,q)}{\partial x}\dot{x}+\frac{\partial \tau (x,q)}{\partial q}\dot{q}={\mathbf{J}}_x\dot{x}+{\mathbf{J}}_q\dot{q}$$

变量$\dot{q}$能被集成去控制机器人通过它的DK。我们控制系统的新颖性，是我们对${\tau }_m$应用一个闭环控制。我们介绍了一个进一步的控制环，在这个环中我们保证了动态系统的位置参数的收敛性。

这个系统考虑了尾绳能够如何影响IPM的动力学。它在这儿被考虑为对IPM动力学的一个未建模的干扰，为了突出所提出系统的鲁棒性。然而，我们展示所提出系统的稳定性，还是在无尾绳的情况下。我们不考虑已知尾绳性质的情况是因为即使在尾绳能被预测的情况下，与环境的交互会扰乱它们。因此，我们考虑单个IPM的动态控制的最一般的情况。
The proposed approach takes into consideration how the tether can affect the dynamics of the IPM. It is herein considered an unmodelled disturbance on the IPM dynamics, in order to underline the robustness of the proposed approach. However, we show the stability of the proposed technique also in absence of the tether, as in the case of untethered capsules.We do not consider the case of known tether properties since, even in the case tether dynamics can be predicted, interaction with the environment would confound them. Therefore, we consider the most general case of dynamic control of a single IPM.

为了考虑可能的参数化不确定性，嵌入参数向量$\pi \in {\mathbb{R}}^p$，我们重写动力学：
In order to consider possible parametric uncertainties, embedded in the parameters vector $\pi \in {\mathbb{R}}^p$, we rewrite the dynamics as

$$B(x)\ddot{x}+C(x,\dot{x})\dot{x}+G(x)=Y(x,\dot{x},\ddot{x})\pi$$

这儿的$Y(x,\dot{x},\ddot{x})\in {\mathbb{R}}^{n\times p}$是动态回归器。

$\pi$的更新律允许未知参数收敛到它们的真值，确保整个系统的鲁棒渐进稳定性。我们为了控制的系统的整体动力学表示为：
The update law of $\pi$ allows the unknown parameters to converge to their real values, guaranteeing the robust asymptotic stability of the overall system.

$$Y(x,\dot{x},\ddot{x})\pi =\tau$$

$$\dot{\tau }={\mathbf{J}}_x\dot{x}+{\mathbf{J}}_q\dot{q}+\dot{v}$$

这里$v$对尾绳与环境的交互建模（比如：拖动，弹性行为，阻力和肠道运动），并且$\pi$嵌入在IPM的不确定参数中。
$v$ models the tether interaction with the environment (such as: drag, elastic behaviour, friction and colon motions) and $\pi$ embeds the uncertain parameters of the IPM, such as the mass, the length and the diameter.

3.A 位姿控制 Pose Control

作为第一步，我们定义一个位姿控制器，试图调整IPM到一个期望的轨迹$x_d$。我们为了找到一组期望的力和力矩，被指代为${\tau }_d$，这调整IPM到期望位姿。我们考虑系统动力学的部分知识，使用动态后退控制。

控制规则被直接决定通过一个标准的李雅普诺夫方式，定义

$${\tau }_d=\hat{B}(x)\ddot{x_r}+\hat{C}(x,\dot{x})\dot{x_r}+\hat{G}(x)-K_{d}s-\tilde{x}=Y(x,\dot{x},\dot{x_r},\ddot{x_r})\hat{\pi }-K_{d}s-\tilde{x}$$

这里$\hat{B}$,$\hat{C}$和$\hat{G}$是预估动力学矩阵，它们的参数都被嵌入$\hat{\pi }$。IPM的位置误差被定义为$\tilde{x}=x_d-x$同时$s=\dot{\tilde{x}}+\Lambda \tilde{x}=\dot{x}-(\dot{x_d}-\Lambda \tilde{x})=\dot{x}-\dot{x_r}$，$\Lambda$是对称的，正定矩阵，$\dot{x_r}$被指代为参考速度，是IPM被控制的速度。

3.B 参数估计 Parameters Estimation

这个内环估计IPM动力学的未知参数，比如IPM的质量和尺寸，这允许我们使我们的控制器适应系统的动力学。
This internal loop estimates the unknown parameters of the IPM dynamics, such as the mass and the dimensions of the IPM; this allows us to adapt our controller to the real dynamics of the system.

控制律从李雅普诺夫理论被导出，定义$\dot{\tilde{\pi }}=\dot{\pi }-\dot{\hat{\pi }}=-\dot{\hat{\pi }}=u_{\pi }$，控制律表示为

$$u_{pi}=R^{-1}{Y}^T(x,\dot{x},{\dot{x}}_r,{\ddot{x}}_r)s$$

这里$R$是一个正定增益。

3.C 力控制 Force Control

作为第三步，我们设计一个控制器，它确保磁力（${\tau }_m$）收敛到期望力（${\tau }_d$）。

磁力和力矩从$x$和$q$被计算通过使用定位输出和偶极子模型。

用于$\dot{q}={\mathbf{J}}_q^{†}(\dot{{\tau }_d}+K\tilde{\tau }-{\mathbf{J}}_x\dot{x})$的选择当$K$为正定增益时产生$\dot{\tilde{\tau }}=\dot{{\tau }_d}-\dot{{\tau }_m}=-K\tilde{\tau }$。

[1]: Barducci, Lavinia, et al. “Adaptive Dynamic Control for Magnetically Actuated Medical Robots.” IEEE robotics and automation letters 4.4 (2019): 3633-3640.