全自動內窺鏡清洗機的機械手抗擺控制

談震錁，劉 飛，王志國

(1.江南大學 輕工過程先進控制教育部重點實驗室，江蘇 無錫 214122；2.江南大學 自動化研究所，江蘇 無錫 214122)

0 引言

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

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

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

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

1 結構與模型

1.1 功能結構

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

圖1 機械手結構

1.2 第二類拉格朗日方程

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

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

(1)

1.3 轉運機械手建模

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

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

圖2 機械手的位置軌跡

(2)

由于豎桿在運動方向有擺動，所以重力會對負載架及內鏡做功，產生重力勢能的變化：

Ep= -mglcosθ

(3)

其中：m表示裝有內窺鏡的負載架的質量，g= 9.8 m/s2表示重力加速度，l=0.5 m表示機械手豎桿的長度，θ表示機械手轉運裝有內窺鏡的負載架時豎桿在運動方向擺動的角度。

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

(4)

其中：x表示機械手抓取裝有內窺鏡的負載架移動到不同槽的距離。

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

(5)

根據式(1)和(5)，分別對機械手的位移和豎桿擺動的角度求偏導數可以得到如式(6)和(7)所示的動力學微分方程：

(6)

(7)

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

2 機械手控制器

2.1 滑模控制方法

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

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

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

p=f(p，u，t)

(8)

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

針對非線性系統(tǒng)(8)，基于系統(tǒng)的狀態(tài)和控制目標設計滑模函數s?；：瘮悼梢园ㄏ到y(tǒng)的狀態(tài)、系統(tǒng)狀態(tài)的導數、系統(tǒng)狀態(tài)和設定值的偏差等。通過求解控制輸入u使得系統(tǒng)狀態(tài)從初始狀態(tài)吸引到滑模面s=0上。系統(tǒng)在滑模面上的運動通常有3種情況，假設滑模面s=0是一個超平面，第一種是系統(tǒng)的運動點在到達滑模面s=0附近時穿越而過；第二種是系統(tǒng)的運動點在到達滑模面s=0附近時不穿越而離開；第三種是系統(tǒng)的運動點在到達滑模面s=0附近時始終趨于該滑模面。只有第三種情況才能保證系統(tǒng)控制在期望值。為滿足這一要求，當系統(tǒng)的運動點在到達滑模面s=0附近時，必須有：

(9)

2.2 準滑動模態(tài)

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

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

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

(10)

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

2.3 基本結構

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

為方便后續(xù)推導的書寫，先將式(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：考慮系統(tǒng)(16)，當σ為正常數時，選用控制作用(21)，則系統(tǒng)可以在有限時間內達到滑模面，即s=0。

證明：選取如下所示的Lyapunov函數：

(22)

根據|δ|≤D，對Lyapunov函數求導得：

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

(23)

進一步地，對上式進行分離變量后求積分得：

(24)

證明完畢。

2.4 穩(wěn)定性分析

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

(25)

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

(26)

其中：

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

(26)

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

(27)

(28)

其中：

所以，

(29)

其中：

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

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

(30)

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

(31)

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

(32)

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

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

3 應用

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

(33)

其中：fr、ξ和kr均為摩擦力相關的系數，可以通過離線多次實驗數據采集得到，ρ表示傳感器等其他干擾，在仿真中選用高斯白噪聲作為干擾。機械手的參數和重力加速的分別為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。

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

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

圖3 機械手豎桿擺動角度軌跡

如圖4所示，使用符號函數的滑?？刂迫菀桩a生很強的抖振，控制輸入信號振幅、頻率都過大，嚴重影響執(zhí)行器的安全，會增強其磨損，少使用壽命。而本文將控制作用中的切換函數用飽和函數代替后，改變了由于頻繁切換結構的特點，從根本上解削弱了抖振，如圖5所示，控制器的控制輸出為較光滑的曲線，具有明顯的抗抖振能力。

圖4 使用符號函數的控制輸出

圖5 使用飽和函數的控制輸出

另外考慮到實際應用中，該三菱伺服電機的型號是HG-KR73BJ，徑向軸的容許負載達到392 N，本文方法的輸出初值在222.6 N，在容許范圍內。

4 結束語

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