柔性內(nèi)鏡介入機(jī)器人模糊PID控制研究

, , , ,

(中車青島四方機(jī)車車輛股有限公司 高速列車國家工程研究中心，山東 青島 216000)

0 引言

據(jù)世界衛(wèi)生組織統(tǒng)計，目前全世界約有70%的人患有不同程度的消化道疾病，隨著微創(chuàng)手術(shù)的普及，柔性消化內(nèi)鏡以其損傷小、痛苦少、并發(fā)癥少、恢復(fù)快、檢查直觀等優(yōu)點在消化道疾病的診斷與治療中得到廣泛的應(yīng)用。典型內(nèi)鏡下手術(shù)有治療消化道早期癌癥的內(nèi)鏡下粘膜切除術(shù)(EMR)、內(nèi)鏡粘膜下剝離術(shù)(ESD)、治療賁門失弛緩癥的經(jīng)口內(nèi)鏡下賁門括約肌切斷術(shù)(POEM)、治療胰膽管疾病的經(jīng)內(nèi)鏡逆行胰膽管造影術(shù)(ERCP)以及經(jīng)自然腔道微創(chuàng)介入手術(shù)(natural orifice transluminal endoscopic surgery，NOTES)。

柔性消化內(nèi)鏡需要通過人體自然腔道進(jìn)行介入操作，相對于其他手術(shù)環(huán)境更為復(fù)雜，稍有不慎可能損傷組織甚至穿孔,如圖1所示；鏡體操作部結(jié)構(gòu)的設(shè)計一定程度上增加了操作難度，醫(yī)生需要單手控制大小撥輪以及按鈕等實現(xiàn)對內(nèi)鏡的操作，長時間重復(fù)高強(qiáng)度的操作引起醫(yī)生的不適[1]；

機(jī)器人輔助介入為解決上述問題提供了一條途徑。中國科學(xué)院沈陽自動化研究所李言民等人基于傳統(tǒng)胃鏡開發(fā)了主從式胃鏡輔助介入機(jī)器人，用于鏡體的主從介入，開展了動物實驗研究[2]；法國斯特拉斯堡大學(xué)Allemann等人設(shè)計了內(nèi)鏡操作部操作器，并通過手柄控制鏡體進(jìn)行介入操作[3]；日本產(chǎn)業(yè)醫(yī)科大學(xué)的Kume等人研制了一款用于輸送腸鏡的機(jī)器人EOR(endoscopic operation robot)[4]，通過兩手柄進(jìn)行主從操作，實現(xiàn)末端彎曲、鏡體旋轉(zhuǎn)與進(jìn)退自由度；荷蘭屯特大學(xué)研制的柔性內(nèi)鏡機(jī)器人[1,5]，分為鏡體輸送單元與鏡體操作部單元，通過自制模型評估機(jī)器人的性能。新加坡南洋理工大學(xué)開發(fā)的MASTER機(jī)器人[6]用于EMR與NOTES等手術(shù)的主從操作。目前在消化內(nèi)鏡輔助介入操作機(jī)器人方面，大部分機(jī)構(gòu)關(guān)注于機(jī)器人本體功能的研發(fā)，關(guān)注于手術(shù)操作過程中生理活動補(bǔ)充的研究較少。受人體生理活動的影響，病灶靶點的活動對柔性內(nèi)鏡的操作產(chǎn)生重要的影響，而在內(nèi)鏡介入手術(shù)中，需要時刻保持病灶在視野中央，受生理活動的影響，靶點時有可能運(yùn)動出操作視野外，而頻繁操作胃鏡跟蹤病灶又會分散醫(yī)生工作注意力，使其無法專注于手術(shù)，難以實現(xiàn)安全手術(shù)介入操作。美國約翰霍普金斯大學(xué)研發(fā)的“STAR”(Smart Tissue Autonomous Robot)手術(shù)機(jī)器人[7]，在醫(yī)生的監(jiān)督與輔助操作下，實現(xiàn)腸道的自動縫合，術(shù)中醫(yī)生只負(fù)責(zé)手術(shù)規(guī)劃與在線調(diào)整，重復(fù)性的縫合操作由機(jī)器人自主完成，將醫(yī)生從繁重的勞動中解放出來，并提高了手術(shù)效率。

圖1 常用消化內(nèi)鏡介入檢查術(shù)

法國斯特拉斯堡大學(xué)的Ott等人嘗試采用預(yù)測控制進(jìn)行補(bǔ)償，采用經(jīng)典的重復(fù)與預(yù)測控制，未考慮系統(tǒng)模型的不穩(wěn)定性與變化[8]。內(nèi)鏡鏡體高度柔性，輔助機(jī)器人的運(yùn)動量不一定全部傳遞到末端，手術(shù)過程中，需要實時調(diào)整控制器參數(shù)，以確保操作的快速、準(zhǔn)確以及安全。本文針對內(nèi)鏡機(jī)器人操作存在的問題，分析內(nèi)鏡結(jié)構(gòu)，建立內(nèi)鏡運(yùn)動學(xué)模型，提出了基于模糊PID控制器的算法適應(yīng)不確定的環(huán)境，以改善系統(tǒng)的跟蹤性能。

1 內(nèi)鏡介入操作機(jī)器人系統(tǒng)以及運(yùn)動學(xué)模型

1.1 內(nèi)鏡介入操作機(jī)器人系統(tǒng)的組成

一般內(nèi)鏡介入機(jī)器人系統(tǒng)采用主從控制。主端由主手和控制臺構(gòu)成，具備四個自由度，分別與鏡體的進(jìn)退、旋轉(zhuǎn)、上下彎曲、左右彎曲這4種運(yùn)動相對應(yīng)。從端由機(jī)器人與機(jī)器人控制柜構(gòu)成，響應(yīng)人手操作完成對鏡體的進(jìn)退、旋轉(zhuǎn)以及彎曲操作。

為實現(xiàn)內(nèi)鏡對人體生理活動的自動補(bǔ)償，提取上下彎曲與左右彎曲兩個自由度，由機(jī)器人根據(jù)目標(biāo)靶點在視野的位置，自動調(diào)節(jié)兩彎曲自由度。鏡體進(jìn)退與旋轉(zhuǎn)自由度由人根據(jù)手術(shù)進(jìn)行手動調(diào)節(jié)。

1.2 內(nèi)鏡彎曲段運(yùn)動模型

消化內(nèi)鏡彎曲段的彎曲是通過旋轉(zhuǎn)鏡體操作部撥輪實現(xiàn)的，其基本結(jié)構(gòu)如圖2所示。撥輪旋轉(zhuǎn)帶動末端固定在撥輪上的鋼絲動作，鋼絲貫穿整條內(nèi)鏡鏡體。彎曲段內(nèi)部是由一段段類似蛇骨的環(huán)形零件鉚接而成，相鄰蛇骨零件之間可上下或左右方向活動[9]。當(dāng)旋轉(zhuǎn)撥輪拉動鋼絲時，彎曲段內(nèi)一側(cè)鋼絲長度變短，另一側(cè)鋼絲長度邊長，產(chǎn)生彎曲，在蛇骨內(nèi)側(cè)設(shè)有導(dǎo)引環(huán)用作鋼絲活動通道。其中鋼絲1，3隨大撥輪旋轉(zhuǎn)動作，控制上下彎曲；鋼絲2，4隨小撥輪旋轉(zhuǎn)動作，控制左右彎曲。在蛇骨外側(cè)，附有金屬編制網(wǎng)對蛇骨起到支撐作用；最外側(cè)布有橡皮管，在起到密封作用的同時也可提供支撐。

圖2 消化內(nèi)鏡末端彎曲機(jī)理

根據(jù)檢查對象的區(qū)別，消化內(nèi)鏡主要分為腸鏡與胃鏡，兩者結(jié)構(gòu)以及組成一致，僅在鏡體長度、鏡身直徑以及彎曲角度范圍等具體參數(shù)上略有差別。本文以臨床胃鏡為例進(jìn)行建模分析，胃鏡彎曲段的彎曲角度范圍為左右方向-100～100°，上下方向-90～210°，由于兩方向彎曲角度范圍差距較大，蛇骨分布并不是均勻的；兩上下彎曲關(guān)節(jié)與一左右彎曲關(guān)接交替排布。

表1 D-H參數(shù)列表

對內(nèi)鏡彎曲自由度而言，其輸入為大小撥輪旋轉(zhuǎn)兩自由度，而輸出是幾十個自由度，計算很復(fù)雜。為此，考慮到彎曲過程中蛇骨受外側(cè)金屬編網(wǎng)與橡皮管的作用，彎曲段可假設(shè)為圓弧進(jìn)行處理[8]，并認(rèn)為彎曲部與鋼絲在其軸向方向是不可伸展的。內(nèi)鏡末端模型如圖2所示，以彎曲部起始點所在平面建立坐標(biāo)系{O0}，以內(nèi)鏡中心點作為原點，對驅(qū)動鋼絲按逆時針進(jìn)行編號，以中心點到1號鋼絲連線為x0軸，中心點到2號鋼絲連線為y0軸，垂直平面的方向為z0軸；以彎曲部結(jié)束段所在平面建立坐標(biāo)系{O1}，以內(nèi)鏡末端所在平面建立坐標(biāo)系{O2}；找出坐標(biāo)系{O2}相對于坐標(biāo)系{O0}的旋轉(zhuǎn)變換與平移變化關(guān)系，Π為內(nèi)鏡中心軸線所在平面，α為x1軸與平面Π的夾角，β為平面Π內(nèi)彎曲部彎曲的角度。其中彎曲段的長度為Lb，末端直線段長度為Le。

在初始狀態(tài)下，彎曲部是直的，坐標(biāo)系{O0}與{O2}所在平面是平行的，坐標(biāo)系{O0}繞平面Π的法向量n旋轉(zhuǎn)角度β，并平移獲得{O2}，其齊次變換矩陣為:

(1)

式中，sx和cx分別表示sin(x)和cos(x)。

設(shè)撥輪的直徑分別為d1與d2，撥輪旋轉(zhuǎn)的角度q=[q1q2]，初始條件下q0=[q10q20]，彎曲段是直的；旋轉(zhuǎn)撥輪pj一定角度Δqj=qj-qj0(j=1,2)，對鋼絲施加力，改變鋼絲在內(nèi)鏡內(nèi)的長度分布；設(shè)彎曲部中心軸的長度為L，Li為第i(i=1,2,3,4)根鋼絲在彎曲部內(nèi)的長度，設(shè)R為內(nèi)鏡中心軸線對應(yīng)的彎曲曲率半徑，Ri為第i根鋼絲在彎曲角度為β時的彎曲曲率半徑；內(nèi)鏡的牽引鋼絲在其四周均勻布置，因此第3根鋼絲和第4根鋼絲的長度變化量分別與第1根鋼絲和第2根鋼絲的長度變化量大小相等，方向相反。坐標(biāo)系{O0}的x0o0y0所在平面截面如圖3所示，D為內(nèi)鏡的直徑，驅(qū)動線長度的變化量Δli(i=1,2,3,4)與變量α、β及內(nèi)鏡操作部撥輪角度變化的關(guān)系如下：

(2)

由式(2)可求得:

(3)

(4)

圖3 彎曲段近端{(lán)0}坐標(biāo)系截面圖

2 內(nèi)鏡介入機(jī)器人模糊PID控制

2.1 內(nèi)鏡機(jī)器人執(zhí)行器

內(nèi)鏡機(jī)器人采用電機(jī)作為執(zhí)行器，選用電機(jī)的參數(shù)與性能影響著機(jī)器人動態(tài)與靜態(tài)精度?？紤]到直流無刷電機(jī)具有動態(tài)響應(yīng)好、效率高、壽命長、易于維護(hù)等優(yōu)點，采用直流無刷電機(jī)作為機(jī)器人執(zhí)行器，其簡化模型如圖4所示。

圖4 直流電機(jī)簡化模型

其中，Ua為電機(jī)電樞電壓，Ia為電樞電流，Ra與La分別為電樞電路的電阻與電感，Ea為電機(jī)繞組反電動勢，ω為電機(jī)轉(zhuǎn)速，其動態(tài)響應(yīng)方程由可由式(5)描述：

(5)

式中，Ke與Km分別為電機(jī)的反電勢系數(shù)與轉(zhuǎn)矩系數(shù)，T為電機(jī)驅(qū)動力矩，TL為負(fù)載力矩，Ja為電機(jī)和負(fù)載折合到電機(jī)軸上的轉(zhuǎn)動慣量，kf為電機(jī)和負(fù)載折合到電機(jī)軸上的粘性摩擦系數(shù)。根據(jù)該方程可得電機(jī)的傳遞函數(shù)如圖5所示。

圖5 直流電機(jī)傳遞函數(shù)

2.2 模糊PID控制器設(shè)計

由于消化內(nèi)鏡本身的柔性特點，內(nèi)鏡近端撥輪的操作與末端響應(yīng)并不是嚴(yán)格一一對應(yīng)的關(guān)系，且受人體解剖結(jié)構(gòu)的影響，內(nèi)鏡在人體內(nèi)的形態(tài)并不是固定的，同時受人體生理活動的影響，并考慮到運(yùn)動學(xué)模型的近似性，在內(nèi)鏡介入過程中，系統(tǒng)的參數(shù)會發(fā)生變化，從而降低模型的精確性，導(dǎo)致內(nèi)鏡末端的動作存在明顯的不確定性。因此在內(nèi)鏡介入操作過程中需實時調(diào)整控制參數(shù)，以期適應(yīng)內(nèi)鏡鏡體和環(huán)境的不確定性與參數(shù)的變化。如果驅(qū)動器采用傳統(tǒng)的PID控制、前饋補(bǔ)償PID控制等方法，在不確定環(huán)境以及參數(shù)因素的影響下，系統(tǒng)的控制精度會降低。而模糊控制具備較強(qiáng)的魯棒性，其不依賴于被控對象的精確模型，可削弱外界干擾以及模型參數(shù)變化對控制效果的影響，適應(yīng)于內(nèi)鏡介入機(jī)器人的操作。模糊PID控制根據(jù)偏差與偏差的變化率實時調(diào)整PID控制器參數(shù)，適應(yīng)不同外界環(huán)境，可提高控制系統(tǒng)的性能。

自適應(yīng)模糊PID控制器以傳統(tǒng)PID控制器為基礎(chǔ)，采用模糊推理的思想，以關(guān)節(jié)變量的偏差e和偏差變化率ec作為二維模糊控制器的輸入變量，以PID控制器中Kp,Ki,Kd的變化值作為輸出，利用模糊控制律在線整定PID各個參數(shù)。通過整定參數(shù)后的PID控制輸出控制驅(qū)動器，實現(xiàn)對位置的實時跟蹤。

2.2.1 確定輸入輸出變量

以偏差e與偏差變化率ec作為模糊控制器輸入，PID控制器的Kp,Ki,Kd的變化量作為輸出。Kp,Ki,Kd的調(diào)整公式為:

(6)

式中，Kp0,Ki0,Kd0為PID初始參數(shù)，輸入變量E、EC與輸出變量ΔKp,ΔKi,ΔKd的模糊子集均取為{NBNMNSZOPSPMPB},子集中各元素分別代表負(fù)大、負(fù)中、負(fù)小、零、正小、正中、正大，變量論域統(tǒng)一設(shè)定為{-3, -2 , -1, 0, 1, 2, 3}，三角形函數(shù)在論域范圍內(nèi)均勻分布，其靈敏度較高，以其作為系統(tǒng)的隸屬度函數(shù)。

2.2.2 確立模糊控制規(guī)則

模糊控制器設(shè)計的關(guān)鍵在于模糊規(guī)則的確定，參照工程方面的經(jīng)驗，以下三項作為參數(shù)Kp,Ki,Kd在不同e與ec下的自調(diào)整原則[10]:

1)當(dāng)誤差|e|較大時，為保證系統(tǒng)足夠快的跟蹤響應(yīng)性能，取較大的Kp;同時如積分作用明顯可能出現(xiàn)過大超調(diào)，因此選用較小Ki值進(jìn)行限制。

2)當(dāng)誤差|e|為中等大小時，需考慮減小系統(tǒng)超調(diào)，并考慮系統(tǒng)的快速響應(yīng)速度，需選取大小適中的Kp、Ki和Kd。

3)當(dāng)誤差|e|比較小時，為減小系統(tǒng)的穩(wěn)態(tài)誤差，取較大為保證系統(tǒng)具有較好的穩(wěn)態(tài)性能，應(yīng)取較大的Kp與Ki；同時需考慮系統(tǒng)的魯棒性，并避免振蕩，Kd的選值大小與|ec|的值呈反向關(guān)系。

綜上，建立ΔKp,ΔKi,ΔKd的模糊控制規(guī)則。

2.3 基于模糊PID控制的內(nèi)鏡介入機(jī)器人系統(tǒng)

內(nèi)鏡介入操作手術(shù)中，醫(yī)生通過圖像標(biāo)記病灶的位置，機(jī)器人識別病灶特征后，以病灶處于視野中心為控制目標(biāo)，計算內(nèi)鏡機(jī)器人目標(biāo)位置，根據(jù)目標(biāo)位置與當(dāng)前位置的偏差，使用逆運(yùn)動學(xué)與逆雅可比矩陣，將位置偏差換算為關(guān)節(jié)量偏差，通過模擬PID控制器在線調(diào)整PID控制器的Kp,Ki,Kd參數(shù)，計算出各關(guān)節(jié)變量的控制指令，命令機(jī)器人操作內(nèi)鏡到達(dá)目標(biāo)位置，圖6為系統(tǒng)的控制原理框圖。

圖6 機(jī)器人系統(tǒng)模糊PID控制原理框圖

3 系統(tǒng)仿真實驗結(jié)果

通過仿真實驗來驗證所提出模糊PID控制器的有效性。在機(jī)器人控制內(nèi)鏡彎曲段跟蹤鏡下病灶靶點時，大小撥輪的旋轉(zhuǎn)操作是獨(dú)立進(jìn)行的，兩自由度相對獨(dú)立。因此，以一個關(guān)節(jié)量作為對象，對其動作進(jìn)行仿真驗證。以大撥輪旋轉(zhuǎn)鏡體實現(xiàn)上下彎曲自由度的仿真中，初始PID控制參數(shù)為Kp0=60,Ki0=10,Kd0=1，內(nèi)鏡彎曲段的位置跟蹤曲線如圖7與圖8所示。圖7中給出了階躍響應(yīng)特性。圖8中，給定指令為階梯信號。

通過圖7與圖8的仿真結(jié)果可以看出，當(dāng)輸入指令為階躍信號時，模糊PID控制算法在快速性與穩(wěn)態(tài)誤差方面均優(yōu)于傳統(tǒng)的PID算法。只有超調(diào)略大于傳統(tǒng)PID算法。還需要對算法進(jìn)一步改進(jìn)優(yōu)化，以改善其性能。所提出的模糊PID控制算法相對于傳統(tǒng)PID算法更具優(yōu)勢，加快系統(tǒng)的響應(yīng)時間，并可減小穩(wěn)態(tài)誤差。該算法的性能還有待通過實際的內(nèi)鏡機(jī)器人操作實驗來評估驗證。

圖7 系統(tǒng)階躍響應(yīng)

圖8 階梯輸入信號系統(tǒng)響應(yīng)

4 結(jié)束語

針對消化內(nèi)鏡機(jī)器人介入操作過程中克服生理活動對干擾鏡下手術(shù)的問題，提出了模糊PID控制算法。首先對機(jī)器人系統(tǒng)的運(yùn)動學(xué)模型進(jìn)行分析；其次面向機(jī)器人控制提出了模糊PID控制算法，并通過仿真進(jìn)行驗證分析。仿真結(jié)果標(biāo)明，模糊PID控制算法提升了系統(tǒng)的響應(yīng)速度，改善了系統(tǒng)的穩(wěn)態(tài)誤差。因此，所提出的模糊PID控制算法在控制消化內(nèi)鏡機(jī)器人介入操作過程中能改善系統(tǒng)的性能。