全自动内窥镜清洗机的机械手抗摆控制

谈震锞，刘 飞，王志国

(1.江南大学 轻工过程先进控制教育部重点实验室，江苏 无锡 214122；2.江南大学 自动化研究所，江苏 无锡 214122)

0 引言

随着医疗水平的提高，在全球新冠肺炎疫情的大环境下，消化内窥镜的使用率越来越高，随之带来是内窥镜清洗消毒质量效率问题。目前医院多采用人工清洗或单缸清洗机，2018年有资料列举十大医疗技术危害，内镜清洗消毒不彻底的危害仅次于网络安全系[1]，因此对消化内镜的高效彻底清洗提出更高要求。

本文研制的全自动内窥镜清洗机，以流水线的方式通过机械手抓取装有内窥镜的负载架到不同的槽内进行清洗流程，保证清洗消毒效果，同时弥补消毒环节时间长的短板，显著提高效率。清洗机主动控制的是机械手的水平运动，但实际机械手在运动过程中，其连接的竖杆存在一定的摆动，摆动角度是一个不可直接控制的自由度，由此构成欠驱动控制问题。关于机械手的研究和应用已有众多成果[2]，以智能机器人或机械手从事恶劣环境下重复、繁重任务是大势所趋。目前机械手广泛应用于故障巡检[3-4]，重物运输[5-6]，抓取分拣和装配等各个领域[7-9]。欠驱动的机械手是其中的一个方面，欠驱动系统的控制问题受到广泛关注并成为非线性控制研究方向的热点之一[10]。

全自动流水线式内窥镜清洗机在产品设计过程中，医院从易于使用的角度，提出尽量缩小这种大型设备占地空间和整体体积的需求。考虑到设备底部结构高度固定，且搬运过程中电梯的限制，这些物理尺寸的制约要求清洗机呈现紧凑的特殊结构，即机械手竖直方向采用倒L型的刚性机械结构。对于解决复杂的动力学问题，第二类拉格朗日方程通常是较为有效的途径。例如在解决无初速释放动力学问题时，第二类拉格朗日方程提供了简便的求解途径[11]；针对空间系绳系统展开过程中末端星的姿态运动，采用第二类拉格朗日方程建立系统展开及末端星角运动的数学模型，分析展开阶段末端星的姿态动力学特性[12]。本文结构的欠驱动控制方法目前研究甚少，大多是类似龙门吊车的软质细绳结构，例如采用自适应超螺旋滑模控制算法，有效地削弱控制系统的抖振，并抑制系统的匹配扰动[13]；基于无源性结构和耗散不等式，设计非线性耦合控制器，可以有效克服绳长带来的干扰[14-15]等等，软和硬两种不同的结构，会导致数学模型和控制器设计的差异。

本文通过分析实际研制的全自动内镜清洗机中机械手的动力学，考虑竖杆不可忽略的转动惯量，建立整体的数学模型，再采用滑模理论设计误差信号并给出欠驱动控制方法。其中为了抑制控制器抖振现象将符号函数用饱和函数代替。利用LaSalle不变性原理和Lyapunov方法证明系统有限时间抵达滑模面并最终渐近稳定，同时给出控制器参数的整定。通过仿真分析机械手对象的转运速率和竖杆摆动角度的综合效果，实现了快速定位到指定位置且使竖杆的摆动幅度尽可能小的优越效果。同时对比了控制器的输出与伺服电机允许的负载容量的差异，为实际的应用提供了一定的理论参考。

1 结构与模型

1.1 功能结构

全自动内窥镜清洗机的内部结构固定轴上装设有同步带，通过控制一端的伺服电机带动同步带，从而能够移动固定在同步带上的机械臂。工作流程是首先操作人员将装有内窥镜的负载架放入酶洗槽，然后自动进行初洗，初洗结束则机械手将负载架转运至消毒槽，因为消毒的时间大于酶洗时间，所以设有多个消毒槽，通过多槽并洗的方式提高效率，消毒完成后机械手又将其转运至干燥槽进行干燥。转运内窥镜的机械手的结构如图1所示，前端长为l=0.5 m的竖杆末端可以将装有内窥镜的负载架吸取并通过后端主臂的带动移动到不同槽的位置，由于横杆较长且吸取的负载较重，机械手移动的过程中竖杆有一定程度的摆动。虽然可以通过降低伺服电机的转速减缓机械手的转运速率，但是对于全自动清洗设备而言，其目的是能够提高效率。所以既要能够尽快将内窥镜转运到规定的槽内，又要减少竖杆摆动的幅度。

图1 机械手结构

1.2 第二类拉格朗日方程

分析力学和矢量力学是研究力学的两种基本方法。不同于分析力学，矢量力学把系统拆分，对其中各部分进行研究，利用牛顿定律找到力与物体之间的关系从而建立运动方程。但分析力学是把系统看成一个整体，利用广义坐标研究宏观的力学问题。特别地，拉格朗日方程，也称第二类拉格朗日方程，是一种求解物理系统运动方程的常用方法。它是通过最小作用量原理推导出来的，从能量的观点出发，将物理系统的能量变化与系统的运动轨迹联系起来，从而得到系统的运动方程。第二类拉格朗日方程是分析力学中最重要的动力学方程[16]，它的应用非常广泛，可以描述不同形式的运动学和动力学问题，例如弹性体的振动、刚体的旋转、多体系统的运动等。此外，该方程还可以推广到场论中，描述场的演化过程。在实际应用中，由于物理系统的复杂性和非线性特性，通常需要采用数值方法求解第二类拉格朗日方程。第二类拉格朗日方程为物理学、工程学等领域的研究提供了重要的数值工具对于深入理解物理学、工程学等领域中的问题有着重要的意义。

一般地，若作用于系统上的主动力有一部分是有势力，另一部分是非有势力的广义力，可以利用第二类拉格朗日方程的一般形式来描述该系统的动力学运动问题[17]，如式(1)所示：

(1)

1.3 转运机械手建模

在整个机器的运行中，可想而知用于转运装有内镜负载架的机械手是至关重要的一个部件。其整体结构如图1所示，其中实际重量M=10 kg的部分是机械手的主臂和横杆，其沿着固定轴x的正方向移动。机械手前端的竖杆的质量m0=1 kg，其底部是磁铁，可以吸取装有内窥镜的负载架，并通过后端主臂的带动移动到不同槽的位置。同时还可以通过横杆和竖杆的垂直的移动，将负载架下放至不同的槽内进行不同的清洗步骤。负载架和内窥镜的质量为m=4 kg。后端的主臂固定在皮带轮上，通过控制伺服电机的转动从而带动机械手移动。此机械手系统的特点是前段竖杆的质量不能忽略，在动力学分析时竖杆还会产生转动惯量。

以图2为例，机械手系统的起始位置为静平衡位置，则机械手系统的动能Ek如式(1)所示：

图2 机械手的位置轨迹

(2)

由于竖杆在运动方向有摆动，所以重力会对负载架及内镜做功，产生重力势能的变化：

Ep= -mglcosθ

(3)

其中：m表示装有内窥镜的负载架的质量，g= 9.8 m/s2表示重力加速度，l=0.5 m表示机械手竖杆的长度，θ表示机械手转运装有内窥镜的负载架时竖杆在运动方向摆动的角度。

进一步地，vM、vm、vm0和J如式(4)所示：

(4)

其中：x表示机械手抓取装有内窥镜的负载架移动到不同槽的距离。

根据系统的动能和势能可以得到拉格朗日第二类方程，即动能和势能的差值。结合式(2)、(3)和(4)得系统的拉格朗日函数L=Ek-Ep：

(5)

根据式(1)和(5)，分别对机械手的位移和竖杆摆动的角度求偏导数可以得到如式(6)和(7)所示的动力学微分方程：

(6)

(7)

其中：F是非有势力，即控制作用力，δ表示摩擦力等不可测的干扰。

2 机械手控制器

2.1 滑模控制方法

滑模控制也称变结构控制，本质上是一种特殊的非线性控制，最早可以追溯到20世纪50年代。变结构控制的概念是由苏联的学者Utkin等人提出的。该控制方法是以二阶线性系统为研究对象。随后在Utkin提出了滑模控制的方法后，越来越多的学者认识到滑模控制的优越性和鲁棒性，进而开展了高维系统等更为深入的研究[18]。

滑模控制的特点是控制是不连续的，在动态过程中根据系统的状态、偏差及导数等设计控制作用，使系统沿着规定的状态轨迹作小幅高频的“滑模运动”。这种滑动模态可以不依赖对象的固有参数或是干扰，因此滑模控制方法具有响应速度快、鲁棒性强的优越性。

考虑如下一般的非线性系统：

p=f(p，u，t)

(8)

其中：p∈Rn表示系统状态，u∈Rm表示控制输入，t∈R表示时间。

针对非线性系统(8)，基于系统的状态和控制目标设计滑模函数s。滑模函数可以包括系统的状态、系统状态的导数、系统状态和设定值的偏差等。通过求解控制输入u使得系统状态从初始状态吸引到滑模面s=0上。系统在滑模面上的运动通常有3种情况，假设滑模面s=0是一个超平面，第一种是系统的运动点在到达滑模面s=0附近时穿越而过；第二种是系统的运动点在到达滑模面s=0附近时不穿越而离开；第三种是系统的运动点在到达滑模面s=0附近时始终趋于该滑模面。只有第三种情况才能保证系统控制在期望值。为满足这一要求，当系统的运动点在到达滑模面s=0附近时，必须有：

(9)

2.2 准滑动模态

滑模控制理论作为解决非线性系统的良好控制方法，在处理复杂系统方面广受关注，在实际的工程应用中，由于从现场的传感器测得状态量到控制器输出控制作用这中间存在时间延迟滞后等原因，使得滑模控制中伴随着高频抖动的控制量，即抖振现象，这样的高频输入会影响控制性能，同时还会给伺服系统中的电气元件带来很大的损耗。因此，抖振现象是滑模控制在实际系统中的一个一直存在的难题。为了发挥滑模变结构控制的鲁棒性，需要改进传统的滑模控制器，以减小抖振现象并保证滑模控制的不变性。因此，改进传统滑模变结构控制，减弱抖振现象，成为研究的重点[19]。Young 等人从实际工程出发，对滑模控制中的抖振现象进行了全面的分析和研究，针对连续系统和离散系统，提出了多种抑制抖振的方法，并且给出了多种情况下滑模控制方法的设计，该研究在滑模控制在工程上的应用具有重大的指导意义[20]。

准滑动模态是指系统的运动轨迹被限制在理想滑动模态的某一很小的邻域内的模态。在这一邻域内，准滑动模态不要求满足滑动模态的存在条件，因此准滑动模态可以不在滑模面上进行控制结构的切换，从根本上削弱了抖振。

通常，准滑动模态的控制方式是将原本的符号函数替换成饱和函数sat(s)：

(10)

饱和函数中的Δ称为边界层，可以通过调整Δ的大小从而调整线性区的大小。其本质是在边界层外采用切换控制，而在边界层内采用线性反馈控制。

2.3 基本结构

由于式(7)中不直接含有控制作用，是一种机械手位置和竖杆摆动角度耦合的形式，因此为了的方便控制器的设计，需要将其解耦，得到机械手位置或竖杆摆动角度单独的表达形式。

为方便后续推导的书写，先将式(6)和(7)改写为如下的形式：

(11)

(12)

其中：

M11=M+m+m0，M12=M21=mlCosθ，

G(θ)=mglSinθ。

显然地，M22≠0，所以将式(12)改写为：

(13)

将上式代入式(11)后可化简为：

(14)

(15)

将上式代入式(14)中可将其改写为：

(16)

(17)

选取如式(18)所示的滑模函数：

(18)

其中：λ1、α和λ2是待定的参数。

对式(18)求导可得：

(19)

联立式(13)并用式(16)代入可得：

(20)

可以做出合理假设，摩擦力等干扰是有界的，即|δ|≤D。因此可设计控制器为：

(21)

定理1：考虑系统(16)，当σ为正常数时，选用控制作用(21)，则系统可以在有限时间内达到滑模面，即s=0。

证明：选取如下所示的Lyapunov函数：

(22)

根据|δ|≤D，对Lyapunov函数求导得：

-Dm|s|+δs=-D|s|+δs-σ|s|≤-σ|s|=

(23)

进一步地，对上式进行分离变量后求积分得：

(24)

证明完毕。

2.4 稳定性分析

由式(18)可知当s=0后，

(25)

联立式(12)、式(21)和s=0可得：

(26)

其中：

-λ1ω3-αω2-λ2ω1

(26)

所以可将系统写成：

(27)

(28)

其中：

所以，

(29)

其中：

通过分析得到的线性系统的特征值来判断平衡点处的稳定性关于系统(28)的特征方程可以通过det(sΙ3-A)求得：

s3+(λ1-t2)s2+(t3α-λ1t2-t1)s+t3λ2-t1λ1=0

(30)

根据赫尔维茨稳定判据可知，可写出如下使系统稳定的充分必要条件：

(31)

将t1、t2和t3代入可求得：

(32)

因此等效的系统(28)是渐近稳定的，原系统的控制作用如式(21)所示，其中的参数范围如式(32)所示。因此当待定的参数λ1、α和λ2满足(32)的条件下，原系统在(21)的控制作用下能够在有限时间达到滑模面，并且等效的系统(28)是渐近稳定的。

进一步地，为了减少控制器的抖振，将式(21)中的切换项sgn(s)替换成饱和函数sat(s)。

3 应用

将上述的欠驱动控制方法运用于全自动流水线式内窥镜清洗机，利用所建立的机械手模型，受文献[22]提出的摩擦模型的启发，采用以下干扰形式进行数值仿真：

(33)

其中：fr、ξ和kr均为摩擦力相关的系数，可以通过离线多次实验数据采集得到，ρ表示传感器等其他干扰，在仿真中选用高斯白噪声作为干扰。机械手的参数和重力加速的分别为M=10 kg，m=4 kg，m0=1 kg，l=0.5 m，g=9.8 m/2。

另外本文利用的滑模控制器中：

λ1=0.49，λ2=-8.7，α=0.005。

机械手的位移轨迹如图2所示，可以从图中看出，本文设计的滑模控制方法在5.4 s左右就达到0.79 m的位置，即机械手能够在较快的时间里达到并停在指定槽的上方。

机械手竖杆摆动角度轨迹如图3所示，可以从图中看出，本文设计的滑模控制方法在最开始的两次摆动中的角度都很小，只有2°左右，并且能够在较快的时间里达到稳定停摆的状态。因为考虑到略微的摆动也会使控制器的输出来回跳变，所以能尽快地消除摆动是十分有意义的。

图3 机械手竖杆摆动角度轨迹

如图4所示，使用符号函数的滑模控制容易产生很强的抖振，控制输入信号振幅、频率都过大，严重影响执行器的安全，会增强其磨损，少使用寿命。而本文将控制作用中的切换函数用饱和函数代替后，改变了由于频繁切换结构的特点，从根本上解削弱了抖振，如图5所示，控制器的控制输出为较光滑的曲线，具有明显的抗抖振能力。

图4 使用符号函数的控制输出

图5 使用饱和函数的控制输出

另外考虑到实际应用中，该三菱伺服电机的型号是HG-KR73BJ，径向轴的容许负载达到392 N，本文方法的输出初值在222.6 N，在容许范围内。

4 结束语

本文研究全自动流水线式内窥镜清洗机中欠驱动机械手控制系统设计，以快速精准定位的同时减少竖杆摆动为期望目标，根据其较为特殊的物理结构给出了滑模面和控制律的设计方法。实验结果表明机械手系统在控制器的作用下，实现了能够快速定位到指定位置且使竖杆的摆动幅度尽可能小的优越效果，适用于大型医疗设备中转运结构的设计，对于医疗设备的研制有应用价值。