Explicit Model Predictive Control of a Magnetic Flexible Endoscope

对胶囊的动力学进行建模，能更好的对胶囊进行控制，在已知胶囊预定义轨迹的情况下，对胶囊进行预测控制和定位。

一个磁灵活内窥镜的显式模型预测控制
Explicit Model Predictive Control of a Magnetic Flexible Endoscope [1]
Paper Link
Authors: Scaglioni, Bruno, et al.
2019, IEEE robotics and automation letters (RA-L)

0. 摘要 Abstract

显式模型预测控制与非线性优化结合被首次应用在磁驱柔性内窥镜。这个方法目标是在给定期望力和力矩下，计算外部永磁体的运动。这里描述的策略利用磁驱动的非线性本质的优势，并且显式地考虑工作空间的边界，和驱动限制。一开始，一个带尾绳的胶囊的简化的动态模型，基于欧拉-拉格朗日等式被开发出来。接着，显式模型预测控制被描述，并且一个基于单步，非线性优化路线的用于外部磁铁定位的新方法被提出。
explicit model predictive control is applied in conjunction with nonlinear optimization to a magnetically actuated flexible endoscope for the first time. The approach is aimed at computing the motion of the external permanent magnet, given the desired forces and torques. The strategy described here takes advantage of the nonlinear nature of the magnetic actuation and explicitly considers the workspace boundaries, as well as the actuation constraints. Initially, a simplified dynamic model of the tethered capsule, based on the Euler-Lagrange equations is developed. Subsequently, the explicit model predictive control is described and a novel approach for the external magnet positioning, based on a single step, nonlinear optimisation routine, is proposed.

1. 介绍 Introduction

在台式平台以及猪活体标本上的广泛测试表明，影响胶囊可操作性的主要限制因素是结肠特征性的恶劣环境。这里，腹部褶皱和绷紧的弯曲阻塞胶囊，因而这些阻塞在控制动作的计算中最小化胶囊动力学的相关性。解剖障碍是最重要的的干扰源且不能被忽略，因为这可能导致对胶囊施加不期望的和次佳的作用力矩和力。这对PI控制器尤其如此，因为没有考虑工作空间限制和驱动的非线性本质。
Extensive tests on bench-top platforms, as well as porcine live specimen, have demonstrated that the main limiting factor affecting the manoeuvrability of the capsule is the hostile environment that characterises the colon. Here, haustral folds and tight bends obstruct the capsule, thus minimising the relevance of the capsule’s dynamics in the computation of the control action. The anatomical obstructions are significant sources of disturbances and cannot be neglected as this can lead to unexpected and suboptimal application of torques and forces to the capsule. This is particularly true with PI controllers, as the workspace limits and nonlinear nature of the actuation are not taken into account.

为了解决这个问题，一个基于显式模型预测控制(eMPC)控制方法被讨论在这篇信中。常规的MPC是一种著名的且广泛使用的技术，原本为化学过程开发的。MPC有特别的特性，显式考虑驱动和状态的限制来解决一个优化控制问题，因此对于上面讨论的问题来说非常有利。在另一方面，它要求优化问题的在线解决，这会极大地增加计算负担。由于这个原因，我们选择eMPC方法，这种方法转移大量的计算负担到线下，作为回报，限制定义了有限的灵活度。
In order to overcome this issue, a control approach based on explicit Model Predictive Control (eMPC) is discussed in this letter. Conventional MPC is a well known and widely adopted technique, originally developed for chemical processes. MPC has the unique characteristic of solving an optimal control problem explicitly considering constraints on actuation and state, thus being particularly beneficial with respect to the issues discussed above. On the other hand, it requires the on-line solution of an optimization problem, which can significantly increase the computational burden. For this reason, we use the eMPC approach, which shifts most of the computational burden off-line, in return for a limited flexibility in the definition of constraints.

这释放了的计算能力被用来提出一个新的方法来解决磁问题。比如，EPM相对于胶囊的位姿。所提出的技术是基于一个单步优化过程的解，目的是控制EPM的绝对位置，而不是增量位移。
This freed computational power is then used to propose a new approach to the solution of the magnetic problem, the pose of the EPM with respect to the capsule, required to obtain a desired force/torque. The proposed technique is based on the solution of a single-step optimization procedure, with the aim of controlling the absolute position of the EPM rather than the incremental displacement.

2. 理论 Methods

用于概念验证，我们专注于一个预定义的轨迹在一个临床相关性结肠仿体中。采用不正确的轨迹和强迫胶囊沿着一个受约束方向的力的用户输入将导致一个无效的控制动作。这问题不能被解决在运动控制层面，而是需要一个额外的有环境知识的智能层面。
For proof-of-concept, we focus on following a pre-defined trajectory in a clinically relevant colon phantom. The adoption of ill-planned trajectories as well as user inputs that force the capsule in a constrained direction would result in an ineffective control action. This issue cannot be tackled at a motion control level, but rather requires an additional layer of intelligence with knowledge of the environment. This interesting topic will be discussed in future works.

2.A 胶囊模型 Capsule Model

动态模型，用于控制设计目的的，在这里被首次描述。
the dynamic model, used for control design purposes, is described here for the first time.

$$\frac{d}{dt}\frac{\partial L}{\partial \dot{\mathbf{q}}}-\frac{\partial L}{\partial \mathbf{q}}=\xi$$

$L=T-U$, $T$和$U$分别是是kinetic and potential energy terms. $\xi$是胶囊上施加的磁力和滑动阻力的合力。$\mathbf{q}(t)=[x(t),y(t),\varphi (t),\theta (t),\phi (t)]$

$\mathbf{u}=[f_x,f_y,{\tau }_{\varphi },{\tau }_{\theta },{\tau }_{\phi }]$是磁耦合施加的磁力。$\mu$是阻力系数。${\mathbf{f}}_h=\mu \frac{d\mathbf{q}}{dt}$是阻力。$\xi =\mathbf{u}-{\mathbf{f}}_h$

许多关于下消化道的阻力的工作可以在文献中获得，但是参数的识别需要专门的实验工作。在这篇文章中，有意义的阻力系数已经被采用，并且在实验工作中进一步微调。
Much work on friction in lower GI tract is available in literature, but the identification of the parameters requires dedicated experimental work. In this letter, reasonable friction coefficients have been adopted and subsequently tuned during the experimental work.

胶囊被建模为刚体，实际上是铰接在与组织接触的点上。 选择这种方法是导航策略的结果。 为了促进越过障碍物，胶囊相对于接触表面一直朝相反的方向前进。
the capsule is modelled as a rigid body, virtually hinged in the point of contact with the tissue. This approach has been chosen as a consequence of the navigation strategy. In order to promote the traversing of obstacles, the capsule is constantly heading in the opposite direction, with respect to the contact surface.

考虑到胶囊与结肠的上壁接触，z（t）上的动力学被忽略了。 这种方法避免了仅由重力构成的势能（U = 0）的计算。 值得一提的是，这种方法相对于结肠壁z（t）的变化是稳健的，因为与EPM的磁耦合不断将胶囊吸引到顶部表面.
Considering the capsule in contact with the upper wall of the colon, the dynamic on z(t) is neglected. This approach avoids the computation of the potential energy (U = 0), constituted only by gravity. It is worth mentioning that this approach is robust with respect to variations in z(t) of the colon’s wall, as the magnetic coupling with the EPM is constantly attracting the capsule toward the top surface.

已经根据局部坐标计算了动能，以简化惯性矩阵（I）的计算，该矩阵在此方法中是恒定的。 而且，转速的计算更容易。
The kinetic energy has been computed in terms of local coordinates to simplify the computation of the inertia matrix (I¯) which is constant in this approach. Moreover, the computation of the rotational velocity is easier.

胶囊的运动等式能被写为以下形式：

$$\ddot{\mathbf{q}}=\mathbf{M}(\mathbf{q}{)}^{-1}(-\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}+\xi )$$

最后，它已在平衡点\bar{q} = 0周围线性化，即在胶囊水平和静态的条件下，从而获得状态空间形式的线性系统。由于胶囊的缓慢运动和力的变化，已经选择了在这种情况下线性化系统的选择。上面描述的数学步骤已经使用MATLAB符号操作工具箱执行，因此获得了适用于MPC实现的LTI系统，如下一节所述。
Finally, it has been linearised around the equilibrium point \bar{q} = 0, i.e., in the condition of capsule horizontal and static, thus obtaining a linear system in state space form. The choice of linearising the system in this condition has been adopted as a consequence of the slow movements and changes in force experienced by the capsule. The mathematical steps described above have been carried out using the MATLAB symbolic manipulation toolbox thus obtaining an LTI system suitable for the implementation of MPC, as described in the next section.

显式MPC部分就是如下2.B，通过离线计算，得到一个分段函数，已知r和q的差值，就可以给出期望的力,矩f,tau，作为输入给2.C。
2.C是一个优化问题，将作用力和力矩尽量接近给定期望值，优化机械臂运动。

2.B 显式MPC Explicit MPC

这个工作主要是预定义轨迹的追踪。
this work focuses on the tracking of a predefined trajectory.

这个问题已经通过显式MPC（eMPC）技术解决了。 最初将控制方案描述为线性，离散时间系统上的经典MPC，其中通过最小化成本函数来计算控制动作（即施加到胶囊的力和扭矩，此处用u表示）:
This problem has been tackled by means of the explicit MPC (eMPC) technique. The control scheme has been initially described as a classical MPC over a linear, discrete time system, where the control action (i.e., the forces and torques applied to the capsule, here described with u) are computed by minimising the cost function:

$$\begin{array}{rl}\arg \underset{\det \mathbf{u}}{\min }& \mathbf{J}(\det \mathbf{u},\mathbf{q}(t))=\sum _{k=0}^{N-1}({\mathbf{q}}_k-\mathbf{r}(t){)}^T{W}_q(({\mathbf{q}}_k-\mathbf{r}(t))+\det {\mathbf{u}}_k^T{W}_u\det {\mathbf{u}}_k+{\mathbf{q}}_N^T{W}_N{\mathbf{q}}_N\\ subjectto& {\mathbf{q}}_{k+1}=\mathbf{A}{\mathbf{q}}_k+\mathbf{B}{\mathbf{u}}_k,\\ & {\mathbf{q}}_0=\mathbf{q}(t),\\ & {\mathbf{u}}_{k+1}={\mathbf{u}}_k+\det {\mathbf{u}}_k,\\ & \det {\mathbf{u}}_{min}\le \det {\mathbf{u}}_k\le \det {\mathbf{u}}_{max},\\ & k=i\cdots N-1,\\ & {\mathbf{u}}_{min}\le {\mathbf{u}}_k\le {\mathbf{u}}_{max},\\ & {\mathbf{q}}_{min}\le {\mathbf{q}}_k\le {\mathbf{q}}_{max}.\end{array}$$

其中N是预测水平，r（t）是状态在水平线上的设定点，$\det u_k$是控制动作在每一步的增量，$W_q$，$W_N$，$W_u$分别是状态，终端状态和输入的权重矩阵。
第一第二行的constraints表示胶囊在预测范围内的线性化动力学。第三行constraint是旨在计算控制动作值的积分，采用$\det u_k$作为最小变量（即，控制算法为速度形式），从而通过避免静态误差来跟踪参考信号r（t）。
第四到第六行描述了执行器极限和状态空间的允许部分。
以上式子将u（t）隐式定义为q（t）的函数，因此，通常需要实时求解二次程序（QP），就像通常在隐式MPC技术中所做的那样。相反，通过离线求解q（t）的所有可行值来解决优化问题。

2.C 磁驱动Magnetic Actuation

之前的力和力矩控制方式极大简化了关节值的计算但是忽略了操作器的限制。更进一步的，在操作器运动的计算中，期望的力和力矩的可行性没有显式地被考虑。给定期望的力和力矩，EPM在每个时间点的位置和朝向通过解一个优化问题被计算：
This approach greatly simplifies the computation of the joint values but neglects the manipulator’s limits in terms of absolute position of the EPM and joint velocities. Moreover, the feasibility of the desired ξ˙ is not explicitly considered during the computation of the manipulator’s motion. Given the desired forces and torques computed as described in the previous section, the position and orientation of the EPM at each time step is computed by solving an optimisation problem as follows:

$$\arg \underset{{\mathbf{p}}_a,\widehat{{\mathbf{m}}_a}}{\min }{\mathbf{J}}_{\xi }({\mathbf{p}}_a,\widehat{{\mathbf{m}}_a},{\mathbf{m}}_c)={\mathbf{W}}_f||{\mathbf{f}}_{des}-{\mathbf{f}}_m({\mathbf{p}}_a,{\mathbf{m}}_a,{\mathbf{m}}_c)|{|}^2+{\mathbf{W}}_{\tau }||{\tau }_{des}-{\tau }_m({\mathbf{p}}_a,{\mathbf{m}}_a,{\mathbf{m}}_c)|{|}^2$$

[1]: Scaglioni, Bruno, et al. “Explicit model predictive control of a magnetic flexible endoscope.” IEEE robotics and automation letters 4.2 (2019): 716-723.