談?wù)痄?，?飛，王志國

(1.江南大學(xué) 輕工過程先進(jìn)控制教育部重點(diǎn)實(shí)驗(yàn)室，江蘇 無錫 214122；2.江南大學(xué) 自動(dòng)化研究所，江蘇 無錫 214122)

0 引言

隨著醫(yī)療水平的提高，在全球新冠肺炎疫情的大環(huán)境下，消化內(nèi)窺鏡的使用率越來越高，隨之帶來是內(nèi)窺鏡清洗消毒質(zhì)量效率問題。目前醫(yī)院多采用人工清洗或單缸清洗機(jī)，2018年有資料列舉十大醫(yī)療技術(shù)危害，內(nèi)鏡清洗消毒不徹底的危害僅次于網(wǎng)絡(luò)安全系[1]，因此對消化內(nèi)鏡的高效徹底清洗提出更高要求。

本文研制的全自動(dòng)內(nèi)窺鏡清洗機(jī)，以流水線的方式通過機(jī)械手抓取裝有內(nèi)窺鏡的負(fù)載架到不同的槽內(nèi)進(jìn)行清洗流程，保證清洗消毒效果，同時(shí)彌補(bǔ)消毒環(huán)節(jié)時(shí)間長的短板，顯著提高效率。清洗機(jī)主動(dòng)控制的是機(jī)械手的水平運(yùn)動(dòng)，但實(shí)際機(jī)械手在運(yùn)動(dòng)過程中，其連接的豎桿存在一定的擺動(dòng)，擺動(dòng)角度是一個(gè)不可直接控制的自由度，由此構(gòu)成欠驅(qū)動(dòng)控制問題。關(guān)于機(jī)械手的研究和應(yīng)用已有眾多成果[2]，以智能機(jī)器人或機(jī)械手從事惡劣環(huán)境下重復(fù)、繁重任務(wù)是大勢所趨。目前機(jī)械手廣泛應(yīng)用于故障巡檢[3-4]，重物運(yùn)輸[5-6]，抓取分揀和裝配等各個(gè)領(lǐng)域[7-9]。欠驅(qū)動(dòng)的機(jī)械手是其中的一個(gè)方面，欠驅(qū)動(dòng)系統(tǒng)的控制問題受到廣泛關(guān)注并成為非線性控制研究方向的熱點(diǎn)之一[10]。

全自動(dòng)流水線式內(nèi)窺鏡清洗機(jī)在產(chǎn)品設(shè)計(jì)過程中，醫(yī)院從易于使用的角度，提出盡量縮小這種大型設(shè)備占地空間和整體體積的需求?？紤]到設(shè)備底部結(jié)構(gòu)高度固定，且搬運(yùn)過程中電梯的限制，這些物理尺寸的制約要求清洗機(jī)呈現(xiàn)緊湊的特殊結(jié)構(gòu)，即機(jī)械手豎直方向采用倒L型的剛性機(jī)械結(jié)構(gòu)。對于解決復(fù)雜的動(dòng)力學(xué)問題，第二類拉格朗日方程通常是較為有效的途徑。例如在解決無初速釋放動(dòng)力學(xué)問題時(shí)，第二類拉格朗日方程提供了簡便的求解途徑[11]；針對空間系繩系統(tǒng)展開過程中末端星的姿態(tài)運(yùn)動(dòng)，采用第二類拉格朗日方程建立系統(tǒng)展開及末端星角運(yùn)動(dòng)的數(shù)學(xué)模型，分析展開階段末端星的姿態(tài)動(dòng)力學(xué)特性[12]。本文結(jié)構(gòu)的欠驅(qū)動(dòng)控制方法目前研究甚少，大多是類似龍門吊車的軟質(zhì)細(xì)繩結(jié)構(gòu)，例如采用自適應(yīng)超螺旋滑模控制算法，有效地削弱控制系統(tǒng)的抖振，并抑制系統(tǒng)的匹配擾動(dòng)[13]；基于無源性結(jié)構(gòu)和耗散不等式，設(shè)計(jì)非線性耦合控制器，可以有效克服繩長帶來的干擾[14-15]等等，軟和硬兩種不同的結(jié)構(gòu)，會(huì)導(dǎo)致數(shù)學(xué)模型和控制器設(shè)計(jì)的差異。

本文通過分析實(shí)際研制的全自動(dòng)內(nèi)鏡清洗機(jī)中機(jī)械手的動(dòng)力學(xué)，考慮豎桿不可忽略的轉(zhuǎn)動(dòng)慣量，建立整體的數(shù)學(xué)模型，再采用滑模理論設(shè)計(jì)誤差信號并給出欠驅(qū)動(dòng)控制方法。其中為了抑制控制器抖振現(xiàn)象將符號函數(shù)用飽和函數(shù)代替。利用LaSalle不變性原理和Lyapunov方法證明系統(tǒng)有限時(shí)間抵達(dá)滑模面并最終漸近穩(wěn)定，同時(shí)給出控制器參數(shù)的整定。通過仿真分析機(jī)械手對象的轉(zhuǎn)運(yùn)速率和豎桿擺動(dòng)角度的綜合效果，實(shí)現(xiàn)了快速定位到指定位置且使豎桿的擺動(dòng)幅度盡可能小的優(yōu)越效果。同時(shí)對比了控制器的輸出與伺服電機(jī)允許的負(fù)載容量的差異，為實(shí)際的應(yīng)用提供了一定的理論參考。

1 結(jié)構(gòu)與模型

1.1 功能結(jié)構(gòu)

全自動(dòng)內(nèi)窺鏡清洗機(jī)的內(nèi)部結(jié)構(gòu)固定軸上裝設(shè)有同步帶，通過控制一端的伺服電機(jī)帶動(dòng)同步帶，從而能夠移動(dòng)固定在同步帶上的機(jī)械臂。工作流程是首先操作人員將裝有內(nèi)窺鏡的負(fù)載架放入酶洗槽，然后自動(dòng)進(jìn)行初洗，初洗結(jié)束則機(jī)械手將負(fù)載架轉(zhuǎn)運(yùn)至消毒槽，因?yàn)橄镜臅r(shí)間大于酶洗時(shí)間，所以設(shè)有多個(gè)消毒槽，通過多槽并洗的方式提高效率，消毒完成后機(jī)械手又將其轉(zhuǎn)運(yùn)至干燥槽進(jìn)行干燥。轉(zhuǎn)運(yùn)內(nèi)窺鏡的機(jī)械手的結(jié)構(gòu)如圖1所示，前端長為l=0.5 m的豎桿末端可以將裝有內(nèi)窺鏡的負(fù)載架吸取并通過后端主臂的帶動(dòng)移動(dòng)到不同槽的位置，由于橫桿較長且吸取的負(fù)載較重，機(jī)械手移動(dòng)的過程中豎桿有一定程度的擺動(dòng)。雖然可以通過降低伺服電機(jī)的轉(zhuǎn)速減緩機(jī)械手的轉(zhuǎn)運(yùn)速率，但是對于全自動(dòng)清洗設(shè)備而言，其目的是能夠提高效率。所以既要能夠盡快將內(nèi)窺鏡轉(zhuǎn)運(yùn)到規(guī)定的槽內(nèi)，又要減少豎桿擺動(dòng)的幅度。

圖1 機(jī)械手結(jié)構(gòu)

1.2 第二類拉格朗日方程

分析力學(xué)和矢量力學(xué)是研究力學(xué)的兩種基本方法。不同于分析力學(xué)，矢量力學(xué)把系統(tǒng)拆分，對其中各部分進(jìn)行研究，利用牛頓定律找到力與物體之間的關(guān)系從而建立運(yùn)動(dòng)方程。但分析力學(xué)是把系統(tǒng)看成一個(gè)整體，利用廣義坐標(biāo)研究宏觀的力學(xué)問題。特別地，拉格朗日方程，也稱第二類拉格朗日方程，是一種求解物理系統(tǒng)運(yùn)動(dòng)方程的常用方法。它是通過最小作用量原理推導(dǎo)出來的，從能量的觀點(diǎn)出發(fā)，將物理系統(tǒng)的能量變化與系統(tǒng)的運(yùn)動(dòng)軌跡聯(lián)系起來，從而得到系統(tǒng)的運(yùn)動(dòng)方程。第二類拉格朗日方程是分析力學(xué)中最重要的動(dòng)力學(xué)方程[16]，它的應(yīng)用非常廣泛，可以描述不同形式的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)問題，例如彈性體的振動(dòng)、剛體的旋轉(zhuǎn)、多體系統(tǒng)的運(yùn)動(dòng)等。此外，該方程還可以推廣到場論中，描述場的演化過程。在實(shí)際應(yīng)用中，由于物理系統(tǒng)的復(fù)雜性和非線性特性，通常需要采用數(shù)值方法求解第二類拉格朗日方程。第二類拉格朗日方程為物理學(xué)、工程學(xué)等領(lǐng)域的研究提供了重要的數(shù)值工具對于深入理解物理學(xué)、工程學(xué)等領(lǐng)域中的問題有著重要的意義。

一般地，若作用于系統(tǒng)上的主動(dòng)力有一部分是有勢力，另一部分是非有勢力的廣義力，可以利用第二類拉格朗日方程的一般形式來描述該系統(tǒng)的動(dòng)力學(xué)運(yùn)動(dòng)問題[17]，如式(1)所示：

(1)

1.3 轉(zhuǎn)運(yùn)機(jī)械手建模

在整個(gè)機(jī)器的運(yùn)行中，可想而知用于轉(zhuǎn)運(yùn)裝有內(nèi)鏡負(fù)載架的機(jī)械手是至關(guān)重要的一個(gè)部件。其整體結(jié)構(gòu)如圖1所示，其中實(shí)際重量M=10 kg的部分是機(jī)械手的主臂和橫桿，其沿著固定軸x的正方向移動(dòng)。機(jī)械手前端的豎桿的質(zhì)量m0=1 kg，其底部是磁鐵，可以吸取裝有內(nèi)窺鏡的負(fù)載架，并通過后端主臂的帶動(dòng)移動(dòng)到不同槽的位置。同時(shí)還可以通過橫桿和豎桿的垂直的移動(dòng)，將負(fù)載架下放至不同的槽內(nèi)進(jìn)行不同的清洗步驟。負(fù)載架和內(nèi)窺鏡的質(zhì)量為m=4 kg。后端的主臂固定在皮帶輪上，通過控制伺服電機(jī)的轉(zhuǎn)動(dòng)從而帶動(dòng)機(jī)械手移動(dòng)。此機(jī)械手系統(tǒng)的特點(diǎn)是前段豎桿的質(zhì)量不能忽略，在動(dòng)力學(xué)分析時(shí)豎桿還會(huì)產(chǎn)生轉(zhuǎn)動(dòng)慣量。

以圖2為例，機(jī)械手系統(tǒng)的起始位置為靜平衡位置，則機(jī)械手系統(tǒng)的動(dòng)能Ek如式(1)所示：

圖2 機(jī)械手的位置軌跡

(2)

由于豎桿在運(yùn)動(dòng)方向有擺動(dòng)，所以重力會(huì)對負(fù)載架及內(nèi)鏡做功，產(chǎn)生重力勢能的變化：

Ep= -mglcosθ

(3)

其中：m表示裝有內(nèi)窺鏡的負(fù)載架的質(zhì)量，g= 9.8 m/s2表示重力加速度，l=0.5 m表示機(jī)械手豎桿的長度，θ表示機(jī)械手轉(zhuǎn)運(yùn)裝有內(nèi)窺鏡的負(fù)載架時(shí)豎桿在運(yùn)動(dòng)方向擺動(dòng)的角度。

進(jìn)一步地，vM、vm、vm0和J如式(4)所示：

(4)

其中：x表示機(jī)械手抓取裝有內(nèi)窺鏡的負(fù)載架移動(dòng)到不同槽的距離。

根據(jù)系統(tǒng)的動(dòng)能和勢能可以得到拉格朗日第二類方程，即動(dòng)能和勢能的差值。結(jié)合式(2)、(3)和(4)得系統(tǒng)的拉格朗日函數(shù)L=Ek-Ep：

(5)

根據(jù)式(1)和(5)，分別對機(jī)械手的位移和豎桿擺動(dòng)的角度求偏導(dǎo)數(shù)可以得到如式(6)和(7)所示的動(dòng)力學(xué)微分方程：

(6)

(7)

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

2 機(jī)械手控制器

2.1 滑?？刂品椒?/h3>

滑?？刂埔卜Q變結(jié)構(gòu)控制，本質(zhì)上是一種特殊的非線性控制，最早可以追溯到20世紀(jì)50年代。變結(jié)構(gòu)控制的概念是由蘇聯(lián)的學(xué)者Utkin等人提出的。該控制方法是以二階線性系統(tǒng)為研究對象。隨后在Utkin提出了滑?？刂频姆椒ê?，越來越多的學(xué)者認(rèn)識(shí)到滑?？刂频膬?yōu)越性和魯棒性，進(jìn)而開展了高維系統(tǒng)等更為深入的研究[18]。

滑?？刂频奶攸c(diǎn)是控制是不連續(xù)的，在動(dòng)態(tài)過程中根據(jù)系統(tǒng)的狀態(tài)、偏差及導(dǎo)數(shù)等設(shè)計(jì)控制作用，使系統(tǒng)沿著規(guī)定的狀態(tài)軌跡作小幅高頻的“滑模運(yùn)動(dòng)”。這種滑動(dòng)模態(tài)可以不依賴對象的固有參數(shù)或是干擾，因此滑?？刂品椒ň哂许憫?yīng)速度快、魯棒性強(qiáng)的優(yōu)越性。

考慮如下一般的非線性系統(tǒng)：

p=f(p，u，t)

(8)

其中：p∈Rn表示系統(tǒng)狀態(tài)，u∈Rm表示控制輸入，t∈R表示時(shí)間。

針對非線性系統(tǒng)(8)，基于系統(tǒng)的狀態(tài)和控制目標(biāo)設(shè)計(jì)滑模函數(shù)s。滑模函數(shù)可以包括系統(tǒng)的狀態(tài)、系統(tǒng)狀態(tài)的導(dǎo)數(shù)、系統(tǒng)狀態(tài)和設(shè)定值的偏差等。通過求解控制輸入u使得系統(tǒng)狀態(tài)從初始狀態(tài)吸引到滑模面s=0上。系統(tǒng)在滑模面上的運(yùn)動(dòng)通常有3種情況，假設(shè)滑模面s=0是一個(gè)超平面，第一種是系統(tǒng)的運(yùn)動(dòng)點(diǎn)在到達(dá)滑模面s=0附近時(shí)穿越而過；第二種是系統(tǒng)的運(yùn)動(dòng)點(diǎn)在到達(dá)滑模面s=0附近時(shí)不穿越而離開；第三種是系統(tǒng)的運(yùn)動(dòng)點(diǎn)在到達(dá)滑模面s=0附近時(shí)始終趨于該滑模面。只有第三種情況才能保證系統(tǒng)控制在期望值。為滿足這一要求，當(dāng)系統(tǒng)的運(yùn)動(dòng)點(diǎn)在到達(dá)滑模面s=0附近時(shí)，必須有：

(9)

2.2 準(zhǔn)滑動(dòng)模態(tài)

滑?？刂评碚撟鳛榻鉀Q非線性系統(tǒng)的良好控制方法，在處理復(fù)雜系統(tǒng)方面廣受關(guān)注，在實(shí)際的工程應(yīng)用中，由于從現(xiàn)場的傳感器測得狀態(tài)量到控制器輸出控制作用這中間存在時(shí)間延遲滯后等原因，使得滑?？刂浦邪殡S著高頻抖動(dòng)的控制量，即抖振現(xiàn)象，這樣的高頻輸入會(huì)影響控制性能，同時(shí)還會(huì)給伺服系統(tǒng)中的電氣元件帶來很大的損耗。因此，抖振現(xiàn)象是滑模控制在實(shí)際系統(tǒng)中的一個(gè)一直存在的難題。為了發(fā)揮滑模變結(jié)構(gòu)控制的魯棒性，需要改進(jìn)傳統(tǒng)的滑?？刂破?，以減小抖振現(xiàn)象并保證滑?？刂频牟蛔冃浴Ｒ虼?，改進(jìn)傳統(tǒng)滑模變結(jié)構(gòu)控制，減弱抖振現(xiàn)象，成為研究的重點(diǎn)[19]。Young 等人從實(shí)際工程出發(fā)，對滑?？刂浦械亩墩瘳F(xiàn)象進(jìn)行了全面的分析和研究，針對連續(xù)系統(tǒng)和離散系統(tǒng)，提出了多種抑制抖振的方法，并且給出了多種情況下滑模控制方法的設(shè)計(jì)，該研究在滑?？刂圃诠こ躺系膽?yīng)用具有重大的指導(dǎo)意義[20]。

準(zhǔn)滑動(dòng)模態(tài)是指系統(tǒng)的運(yùn)動(dòng)軌跡被限制在理想滑動(dòng)模態(tài)的某一很小的鄰域內(nèi)的模態(tài)。在這一鄰域內(nèi)，準(zhǔn)滑動(dòng)模態(tài)不要求滿足滑動(dòng)模態(tài)的存在條件，因此準(zhǔn)滑動(dòng)模態(tài)可以不在滑模面上進(jìn)行控制結(jié)構(gòu)的切換，從根本上削弱了抖振。

通常，準(zhǔn)滑動(dòng)模態(tài)的控制方式是將原本的符號函數(shù)替換成飽和函數(shù)sat(s)：

(10)

飽和函數(shù)中的Δ稱為邊界層，可以通過調(diào)整Δ的大小從而調(diào)整線性區(qū)的大小。其本質(zhì)是在邊界層外采用切換控制，而在邊界層內(nèi)采用線性反饋控制。

2.3 基本結(jié)構(gòu)

由于式(7)中不直接含有控制作用，是一種機(jī)械手位置和豎桿擺動(dòng)角度耦合的形式，因此為了的方便控制器的設(shè)計(jì)，需要將其解耦，得到機(jī)械手位置或豎桿擺動(dòng)角度單獨(dú)的表達(dá)形式。

為方便后續(xù)推導(dǎo)的書寫，先將式(6)和(7)改寫為如下的形式：

(11)

(12)

其中：

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

G(θ)=mglSinθ。

顯然地，M22≠0，所以將式(12)改寫為：

(13)

將上式代入式(11)后可化簡為：

(14)

(15)

將上式代入式(14)中可將其改寫為：

(16)

(17)

選取如式(18)所示的滑模函數(shù)：

(18)

其中：λ1、α和λ2是待定的參數(shù)。

對式(18)求導(dǎo)可得：

(19)

聯(lián)立式(13)并用式(16)代入可得：

(20)

可以做出合理假設(shè)，摩擦力等干擾是有界的，即|δ|≤D。因此可設(shè)計(jì)控制器為：

(21)

定理1：考慮系統(tǒng)(16)，當(dāng)σ為正常數(shù)時(shí)，選用控制作用(21)，則系統(tǒng)可以在有限時(shí)間內(nèi)達(dá)到滑模面，即s=0。

證明：選取如下所示的Lyapunov函數(shù)：

(22)

根據(jù)|δ|≤D，對Lyapunov函數(shù)求導(dǎo)得：

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

(23)

進(jìn)一步地，對上式進(jìn)行分離變量后求積分得：

(24)

證明完畢。

2.4 穩(wěn)定性分析

由式(18)可知當(dāng)s=0后，

(25)

聯(lián)立式(12)、式(21)和s=0可得：

(26)

其中：

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

(26)

所以可將系統(tǒng)寫成：

(27)

(28)

其中：

所以，

(29)

其中：

通過分析得到的線性系統(tǒng)的特征值來判斷平衡點(diǎn)處的穩(wěn)定性關(guān)于系統(tǒng)(28)的特征方程可以通過det(sΙ3-A)求得：

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

(30)

根據(jù)赫爾維茨穩(wěn)定判據(jù)可知，可寫出如下使系統(tǒng)穩(wěn)定的充分必要條件：

(31)

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

(32)

因此等效的系統(tǒng)(28)是漸近穩(wěn)定的，原系統(tǒng)的控制作用如式(21)所示，其中的參數(shù)范圍如式(32)所示。因此當(dāng)待定的參數(shù)λ1、α和λ2滿足(32)的條件下，原系統(tǒng)在(21)的控制作用下能夠在有限時(shí)間達(dá)到滑模面，并且等效的系統(tǒng)(28)是漸近穩(wěn)定的。

進(jìn)一步地，為了減少控制器的抖振，將式(21)中的切換項(xiàng)sgn(s)替換成飽和函數(shù)sat(s)。

3 應(yīng)用

將上述的欠驅(qū)動(dòng)控制方法運(yùn)用于全自動(dòng)流水線式內(nèi)窺鏡清洗機(jī)，利用所建立的機(jī)械手模型，受文獻(xiàn)[22]提出的摩擦模型的啟發(fā)，采用以下干擾形式進(jìn)行數(shù)值仿真：

(33)

其中：fr、ξ和kr均為摩擦力相關(guān)的系數(shù)，可以通過離線多次實(shí)驗(yàn)數(shù)據(jù)采集得到，ρ表示傳感器等其他干擾，在仿真中選用高斯白噪聲作為干擾。機(jī)械手的參數(shù)和重力加速的分別為M=10 kg，m=4 kg，m0=1 kg，l=0.5 m，g=9.8 m/2。

另外本文利用的滑?？刂破髦校?/p>

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

機(jī)械手的位移軌跡如圖2所示，可以從圖中看出，本文設(shè)計(jì)的滑?？刂品椒ㄔ?.4 s左右就達(dá)到0.79 m的位置，即機(jī)械手能夠在較快的時(shí)間里達(dá)到并停在指定槽的上方。

機(jī)械手豎桿擺動(dòng)角度軌跡如圖3所示，可以從圖中看出，本文設(shè)計(jì)的滑?？刂品椒ㄔ谧铋_始的兩次擺動(dòng)中的角度都很小，只有2°左右，并且能夠在較快的時(shí)間里達(dá)到穩(wěn)定停擺的狀態(tài)。因?yàn)榭紤]到略微的擺動(dòng)也會(huì)使控制器的輸出來回跳變，所以能盡快地消除擺動(dòng)是十分有意義的。

圖3 機(jī)械手豎桿擺動(dòng)角度軌跡

如圖4所示，使用符號函數(shù)的滑模控制容易產(chǎn)生很強(qiáng)的抖振，控制輸入信號振幅、頻率都過大，嚴(yán)重影響執(zhí)行器的安全，會(huì)增強(qiáng)其磨損，少使用壽命。而本文將控制作用中的切換函數(shù)用飽和函數(shù)代替后，改變了由于頻繁切換結(jié)構(gòu)的特點(diǎn)，從根本上解削弱了抖振，如圖5所示，控制器的控制輸出為較光滑的曲線，具有明顯的抗抖振能力。

圖4 使用符號函數(shù)的控制輸出

圖5 使用飽和函數(shù)的控制輸出

另外考慮到實(shí)際應(yīng)用中，該三菱伺服電機(jī)的型號是HG-KR73BJ，徑向軸的容許負(fù)載達(dá)到392 N，本文方法的輸出初值在222.6 N，在容許范圍內(nèi)。

4 結(jié)束語

本文研究全自動(dòng)流水線式內(nèi)窺鏡清洗機(jī)中欠驅(qū)動(dòng)機(jī)械手控制系統(tǒng)設(shè)計(jì)，以快速精準(zhǔn)定位的同時(shí)減少豎桿擺動(dòng)為期望目標(biāo)，根據(jù)其較為特殊的物理結(jié)構(gòu)給出了滑模面和控制律的設(shè)計(jì)方法。實(shí)驗(yàn)結(jié)果表明機(jī)械手系統(tǒng)在控制器的作用下，實(shí)現(xiàn)了能夠快速定位到指定位置且使豎桿的擺動(dòng)幅度盡可能小的優(yōu)越效果，適用于大型醫(yī)療設(shè)備中轉(zhuǎn)運(yùn)結(jié)構(gòu)的設(shè)計(jì)，對于醫(yī)療設(shè)備的研制有應(yīng)用價(jià)值。