一種主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)及控制方法
【專利摘要】一種主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)及控制方法，本發(fā)明涉及機(jī)器人控制領(lǐng)域。過(guò)程為：用戶在主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)的主手端設(shè)置導(dǎo)管移動(dòng)位置期望值；位姿傳感器測(cè)得導(dǎo)管實(shí)際的運(yùn)動(dòng)關(guān)節(jié)量，利用變論域模糊PID控制方法對(duì)導(dǎo)管位置進(jìn)行調(diào)整；成像裝置接收檢測(cè)到的圖像信息并成像；通過(guò)成像裝置呈現(xiàn)的圖像判斷是否到達(dá)期望位置。本發(fā)明在常規(guī)PID控制的基礎(chǔ)上增強(qiáng)了位置跟蹤性能及魯棒性，基于誤差分級(jí)選擇伸縮因子，解決了模糊PID控制精度與控制規(guī)則數(shù)目之間的矛盾，避免了每個(gè)采樣周期計(jì)算一次伸縮因子，節(jié)省了時(shí)間，對(duì)系統(tǒng)硬件要求降低，在確保微創(chuàng)手術(shù)安全性的同時(shí)，增強(qiáng)了主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)在微創(chuàng)手術(shù)中的有效性和實(shí)用性。
【專利說(shuō)明】一種主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)及控制方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及機(jī)器人控制領(lǐng)域，特別涉及一種主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)及控制方法。
【背景技術(shù)】
[0002]隨著科學(xué)技術(shù)的發(fā)展，微創(chuàng)手術(shù)在心腦血管疾病內(nèi)的應(yīng)用越來(lái)越廣泛。傳統(tǒng)血管介入微創(chuàng)手術(shù)中，導(dǎo)管被插入到股動(dòng)脈中，在血管中運(yùn)動(dòng)直到目標(biāo)點(diǎn)，通過(guò)膠囊或支架，實(shí)現(xiàn)心腦血管中的血栓溶解，血管擴(kuò)充以及堵塞畸形血管等功能。由于介入創(chuàng)口微乎其微，損傷程度比傳統(tǒng)手術(shù)遠(yuǎn)遠(yuǎn)要輕，減輕病人痛苦，術(shù)后恢復(fù)更快。但是也存在著許多缺點(diǎn):導(dǎo)管介入完全依賴于醫(yī)生的專業(yè)性和熟練度；導(dǎo)管末端與血管壁碰撞的摩擦力醫(yī)生不能準(zhǔn)確估計(jì)，而過(guò)大的力導(dǎo)致血管穿孔，大量出血；手術(shù)時(shí)間長(zhǎng)，而且醫(yī)生要暴露在射線下忍受儀器的強(qiáng)烈輻射，對(duì)醫(yī)生身體帶來(lái)傷害。
[0003]針對(duì)上述問(wèn)題，國(guó)內(nèi)外進(jìn)行了大量研究，并提出主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)應(yīng)用于微創(chuàng)手術(shù)中。但多數(shù)主從系統(tǒng)中，對(duì)驅(qū)動(dòng)器的控制方式較為單一，大多采用常規(guī)PID控制，而實(shí)際導(dǎo)管驅(qū)動(dòng)系統(tǒng)中存在許多非線性因素，如系統(tǒng)的摩擦力、血流的阻力、死區(qū)、時(shí)滯效應(yīng)、導(dǎo)管自身參數(shù)及其運(yùn)動(dòng)環(huán)境條件不斷變化等，致使系統(tǒng)的動(dòng)態(tài)特性不夠理想，一定程度上制約了主從控制的操作性能。模糊PID控制，能夠?qū)ID參數(shù)進(jìn)行在線調(diào)整，提高PID的跟蹤性能，增強(qiáng)介入手術(shù)的安全性。然而保證控制系統(tǒng)的精度往往是通過(guò)增加語(yǔ)言變量和控制規(guī)則的數(shù)目，制定足夠多的控制規(guī)則既不現(xiàn)實(shí)又不便于在微創(chuàng)手術(shù)中的應(yīng)用；隨著控制過(guò)程的進(jìn)行，誤差 逐漸變小，向隸屬度函數(shù)的零位移動(dòng)，此時(shí)使用原有的誤差隸屬度函數(shù)致使精度不足。

【發(fā)明內(nèi)容】

[0004]針對(duì)現(xiàn)有技術(shù)存在的不足，本發(fā)明的目的是提供一種用于主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)裝置的控制方法，以達(dá)到提高主從微創(chuàng)手術(shù)安全性的目的。
[0005]本發(fā)明的技術(shù)方案是這樣實(shí)現(xiàn)的:一種主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)，包括:
[0006]主手:用于實(shí)現(xiàn)醫(yī)生對(duì)導(dǎo)管位置的控制。主手接收醫(yī)生給出的導(dǎo)管移動(dòng)位置的期望值，并且通過(guò)映射將主手手柄移動(dòng)的位置信息轉(zhuǎn)換為導(dǎo)管運(yùn)動(dòng)關(guān)節(jié)量；
[0007]導(dǎo)管:用于主從微創(chuàng)手術(shù)中向病灶處輸送膠囊或支架；
[0008]檢測(cè)裝置:用于檢測(cè)導(dǎo)管末端和目標(biāo)點(diǎn)的位姿；
[0009]上位機(jī):用于接收主手和檢測(cè)裝置傳送的位置信號(hào)，將信號(hào)進(jìn)行處理，并將結(jié)果傳送給伺服驅(qū)動(dòng)裝置；
[0010]伺服驅(qū)動(dòng)裝置:用于接收上位機(jī)傳送的位置信號(hào)，驅(qū)使導(dǎo)管對(duì)醫(yī)生給出的導(dǎo)管移動(dòng)位置的期望值進(jìn)行跟蹤；
[0011]成像裝置:用于圖像處理和人機(jī)交互，將導(dǎo)管和血管姿態(tài)呈現(xiàn)，使醫(yī)生更加直觀地了解手術(shù)進(jìn)行情況；[0012]在所述上位機(jī)中還進(jìn)一步包括:
[0013]位置運(yùn)動(dòng)轉(zhuǎn)換器:用于將上位機(jī)接收到的來(lái)自主手和檢測(cè)裝置的位置信息轉(zhuǎn)換為導(dǎo)管期望關(guān)節(jié)量和導(dǎo)管實(shí)際關(guān)節(jié)量；
[0014]變論域模糊控制器:用于將導(dǎo)管期望關(guān)節(jié)量和導(dǎo)管實(shí)際關(guān)節(jié)量之間的誤差及誤差變化率進(jìn)行模糊處理，經(jīng)模糊化，帶有變論域過(guò)程的模糊推理，去模糊化過(guò)程，將對(duì)導(dǎo)管關(guān)節(jié)量誤差和誤差變化率的控制作用轉(zhuǎn)換為PID控制的比例控制、積分控制、微分控制作用，并實(shí)現(xiàn)PID控制的比例參數(shù)、積分參數(shù)、微分參數(shù)在線調(diào)整；
[0015]PID控制器:用于根據(jù)接收到的變論域模糊控制器的輸出信號(hào)完成導(dǎo)管關(guān)節(jié)量的跟蹤。
[0016]一種對(duì)主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)的導(dǎo)管進(jìn)行控制的方法，包括以下步驟:
[0017]步驟1:首先，用戶在主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)的主手端設(shè)置導(dǎo)管移動(dòng)位置的期望位置；其次用戶操作主手手柄控制導(dǎo)管向期望位置移動(dòng)，在主手手柄的初始位置建立坐標(biāo)系，把主手手柄最終移動(dòng)到的位置信息映射到導(dǎo)管所處任務(wù)空間，通過(guò)映射得到的坐標(biāo)，利用雅克比逆矩陣最終確定導(dǎo)管運(yùn)動(dòng)關(guān)節(jié)量，確定導(dǎo)管運(yùn)動(dòng)關(guān)節(jié)量，包括導(dǎo)管的進(jìn)退量、旋轉(zhuǎn)量和彎曲量；
[0018]步驟2:位姿傳感器測(cè)得導(dǎo)管實(shí)際對(duì)應(yīng)的運(yùn)動(dòng)關(guān)節(jié)量，對(duì)導(dǎo)管位置進(jìn)行調(diào)整；
[0019]步驟2-1:期望關(guān)節(jié)量與實(shí)際關(guān)節(jié)量比較，計(jì)算二者的誤差e，將誤差e和誤差變化率ec作為輸入送入變論域模糊控制器中，確定變論域模糊控制器輸入和輸出的模糊子集，具體為:
[0020]在變論域模糊控制器內(nèi)，首先定義各個(gè)關(guān)節(jié)量誤差的語(yǔ)言變量E和誤差變化率的語(yǔ)言變量EC ;然后，用戶自定義誤差的論域e和誤差變化率ec的論域，并在誤差的論域范圍內(nèi)將誤差的語(yǔ)言變量離散化，在誤差變化率的論域范圍內(nèi)將誤差變化率的語(yǔ)言變量離散化，確定誤差對(duì)應(yīng)的模糊子集和誤差變化率對(duì)應(yīng)的模糊子集，表達(dá)式均為:
[0021 ] {NB, NM, NS, O, PS, PM, PB}
[0022]式中，NB代表負(fù)大，匪代表負(fù)中，NS代表負(fù)小，O代表零，PS代表正小，PM代表正中，PB代表正大；
[0023]設(shè)定變論域模糊控制器輸出的PID參數(shù)的三個(gè)變化量Λ kp, Δ ki； Δ kd的模糊子集表達(dá)式也均為:
[0024]{NB, NM, NS, O, PS, PM, PB}
[0025]步驟2-2:確定PID參數(shù)的三個(gè)變化量Λ kp, Λ L Λ kd各自的隸屬度函數(shù)、誤差的隸屬度和誤差變化率的隸屬度函數(shù)；
[0026]步驟2-3:選取伸縮因子，分別為誤差對(duì)應(yīng)的伸縮因子、誤差變化率對(duì)應(yīng)的伸縮因子、比例因數(shù)變化量對(duì)應(yīng)的伸縮因子、積分因數(shù)變化量對(duì)應(yīng)的伸縮因子、微分因數(shù)變化量對(duì)應(yīng)的伸縮因子；選取伸縮因子的過(guò)程為:
[0027]首先將誤差e劃分為四個(gè)區(qū)域，即誤差區(qū)域I~4，其中:
[0028]誤差區(qū)域I定義為:誤差隸屬度函數(shù)中NB和PB最高的部分對(duì)應(yīng)的橫坐標(biāo)區(qū)間，該區(qū)域表示誤差較大；
[0029]誤差區(qū)域2定義為:誤差隸屬度函數(shù)中匪和PM最高的部分對(duì)應(yīng)的橫坐標(biāo)區(qū)間，該區(qū)域表示誤差適中；[0030]誤差區(qū)域3定義為:誤差隸屬度函數(shù)中NS和PS最高的部分對(duì)應(yīng)的橫坐標(biāo)區(qū)間，該區(qū)域表示誤差較??；
[0031]誤差區(qū)域4定義為:誤差隸屬度函數(shù)中O最高的部分對(duì)應(yīng)的橫坐標(biāo)區(qū)間，該區(qū)域表不誤差很?。?br> [0032]設(shè)置誤差區(qū)域I~4的伸縮因子的取值范圍:
[0033]誤差區(qū)域I內(nèi)的伸縮因子設(shè)置如下:誤差對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.7，I];誤差變化率對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.7，I];比例因數(shù)變化量對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.7，I];積分因數(shù)變化量對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(O, 0.2];微分因數(shù)變化量對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0，0.2]；
[0034]誤差區(qū)域2內(nèi)的伸縮因子設(shè)置如下:誤差對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.4,0.7];誤差變化率對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.4,0.7];比例因數(shù)變化量對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.4,0.7];積分因數(shù)變化量對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.2,0.4];微分因數(shù)變化量對(duì) 應(yīng)的伸縮因子取值范圍為區(qū)間(0.2,0.4]；
[0035]誤差區(qū)域3內(nèi)的伸縮因子設(shè)置如下:誤差對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.2,0.4];誤差變化率對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.2，0.4];比例因數(shù)變化量對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.2,0.4];積分因數(shù)變化量對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.4,0.7];微分因數(shù)變化量對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.4,0.7]；
[0036]誤差區(qū)域4內(nèi)的伸縮因子設(shè)置如下:誤差對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(O, 0.2];誤差變化率對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0，0.2];比例因數(shù)變化量對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0，0.2];積分因數(shù)變化量對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.7，I];微分因數(shù)變化量對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.4,0.7]；
[0037]伸縮因子的確定過(guò)程如下:
[0038]根據(jù)首個(gè)采樣時(shí)刻的誤差值，判斷該誤差值屬于哪個(gè)誤差區(qū)域，伸縮因子取值即為該誤差區(qū)域?qū)?yīng)的伸縮因子；再接收下一個(gè)米樣時(shí)刻的誤差值，若下一個(gè)米樣時(shí)刻的誤差值與上一個(gè)采樣時(shí)刻的誤差值位于同一個(gè)誤差區(qū)域內(nèi)，則伸縮因子不變，否則，修改令伸縮因子為此時(shí)誤差所在的誤差區(qū)域所對(duì)應(yīng)的伸縮因子，重復(fù)該過(guò)程，直至采樣過(guò)程結(jié)束；
[0039]步驟2-4:確定變論域模糊控制器各輸入輸出參數(shù)的模糊規(guī)則表；
[0040]所述的變論域模糊控制器內(nèi)的模糊控制規(guī)則如下:
[0041 ]當(dāng)e的模糊子集為NB，ec的模糊子集為NB時(shí)，kp的模糊子集為PB，Iii的模糊子集為NB，kd的模糊子集為PS ；
[0042]當(dāng)e的模糊子集為NB，ec的模糊子集為匪時(shí)，kp的模糊子集為PB，Iii的模糊子集為NB，kd的模糊子集為NS ；
[0043]當(dāng)e的模糊子集為NB，ec的模糊子集為NS時(shí)，kp的模糊子集為PM，Iii的模糊子集為匪，kd的模糊子集為NB ；
[0044]當(dāng)e的模糊子集為NB，ec的模糊子集為O時(shí)，kp的模糊子集為PM，Iii的模糊子集為匪，kd的模糊子集為NB ；
[0045]當(dāng)e的模糊子集為NB，ec的模糊子集為PS時(shí)，kp的模糊子集為PS，Iii的模糊子集為NS，kd的模糊子集為NM ;
[0046]當(dāng)e的模糊子集為NB，ec的模糊子集為PM時(shí)，kp的模糊子集為0，Iii的模糊子集為O，kd的模糊子集為匪；
[0047]當(dāng)e的模糊子集為NB，ec的模糊子集為PB時(shí)，kp的模糊子集為0，k,的模糊子集為0，kd的模糊子集為PS ；
[0048]當(dāng)e的模糊子集為NM，ec的模糊子集為NB時(shí)，kp的模糊子集為PB，Iii的模糊子集為NB，kd的模糊子集為PS ；
[0049]當(dāng)e的模糊子集為NM，ec的模糊子集為匪時(shí)，kp的模糊子集為PB，Iii的模糊子集為NB，kd的模糊子集為NS ；
[0050]當(dāng)e的模糊子集為NM，ec的模糊子集為NS時(shí)，kp的模糊子集為PM，Iii的模糊子集為匪，kd的模糊子集為NB ；
[0051]當(dāng)e的模糊子集為NM，ec的模糊子集為O時(shí)，kp的模糊子集為PS，Iii的模糊子集為NS，kd的模糊子集為NM ;
[0052]當(dāng)e的模糊子集為NM，ec的模糊子集為PS時(shí)，kp的模糊子集為PS，Iii的模糊子集為NS，kd的模糊子集為NS ；
[0053]當(dāng)e的模糊子集為NM，ec的模糊子集為PM時(shí)，kp的模糊子集為0，Iii的模糊子集為0，kd的模糊子集為NS ；
[0054]當(dāng)e的模糊子集為NM，ec的模糊子集為PB時(shí)，kp的模糊子集為NS，Iii的模糊子集為0，kd的模糊子集為O ;
[0055]當(dāng)e的模糊子集為N S，ec的模糊子集為NB時(shí)，kp的模糊子集為PM，Iii的模糊子集為NB，kd的模糊子集為O ;
[0056]當(dāng)e的模糊子集為NS，ec的模糊子集為匪時(shí)，kp的模糊子集為PM，Iii的模糊子集為匪，kd的模糊子集為NS ；
[0057]當(dāng)e的模糊子集為NS，ec的模糊子集為NS時(shí)，kp的模糊子集為PM，Iii的模糊子集為NS，kd的模糊子集為NM ;
[0058]當(dāng)e的模糊子集為NS，ec的模糊子集為O時(shí)，kp的模糊子集為PS，Iii的模糊子集為NS，kd的模糊子集為NM ;
[0059]當(dāng)e的模糊子集為NS，ec的模糊子集為PS時(shí)，kp的模糊子集為0，Iii的模糊子集為0，kd的模糊子集為NS ；
[0060]當(dāng)e的模糊子集為NS，ec的模糊子集為PM時(shí)，kp的模糊子集為NS，Iii的模糊子集為PS，kd的模糊子集為NS ；
[0061 ]當(dāng)e的模糊子集為NS，ec的模糊子集為PB時(shí)，kp的模糊子集為NS，Iii的模糊子集為PS，kd的模糊子集為O ;
[0062]當(dāng)e的模糊子集為0，ec的模糊子集為NB時(shí)，kp的模糊子集為PM，Iii的模糊子集為匪，kd的模糊子集為O ;
[0063]當(dāng)e的模糊子集為0，ec的模糊子集為匪時(shí)，kp的模糊子集為PM，Iii的模糊子集為匪，kd的模糊子集為NS ；
[0064]當(dāng)e的模糊子集為0，ec的模糊子集為NS時(shí)，kp的模糊子集為PS，Iii的模糊子集為NS，kd的模糊子集為NS ；
[0065]當(dāng)e的模糊子集為O,ec的模糊子集為O時(shí)，kp的模糊子集為O, Iii的模糊子集為0，kd的模糊子集為NS ；[0066]當(dāng)e的模糊子集為0，ec的模糊子集為PS時(shí)，kp的模糊子集為NS，k,的模糊子集為PS，kd的模糊子集為NS ；
[0067]當(dāng)e的模糊子集為0，ec的模糊子集為PM時(shí)，kp的模糊子集為NM，Iii的模糊子集為PM，kd的模糊子集為NS ；
[0068]當(dāng)e的模糊子集為0，ec的模糊子集為PB時(shí)，kp的模糊子集為NM，Iii的模糊子集為PM，kd的模糊子集為O ;
[0069]當(dāng)e的模糊子集為PS，ec的模糊子集為NB時(shí)，kp的模糊子集為PS，Iii的模糊子集為匪，kd的模糊子集為O ;
[0070]當(dāng)e的模糊子集為PS，ec的模糊子集為匪時(shí)，kp的模糊子集為PS，Iii的模糊子集為NS，kd的模糊子集為O ;
[0071]當(dāng)e的模糊 子集為PS，ec的模糊子集為NS時(shí)，kp的模糊子集為0，Iii的模糊子集為0，kd的模糊子集為O ;
[0072]當(dāng)e的模糊子集為PS，ec的模糊子集為O時(shí)，kp的模糊子集為NS，Iii的模糊子集為PS，kd的模糊子集為O ;
[0073]當(dāng)e的模糊子集為PS，ec的模糊子集為PS時(shí)，kp的模糊子集為NS，Iii的模糊子集為PS，kd的模糊子集為O ;
[0074]當(dāng)e的模糊子集為PS，ec的模糊子集為PM時(shí)，kp的模糊子集為匪，Iii的模糊子集為PM，kd的模糊子集為O ;
[0075]當(dāng)e的模糊子集為PS，ec的模糊子集為PB時(shí)，kp的模糊子集為NM，Iii的模糊子集為PB，kd的模糊子集為O ;
[0076]當(dāng)e的模糊子集為PM，ec的模糊子集為NB時(shí)，kp的模糊子集為PS，Iii的模糊子集為0，kd的模糊子集為PB ；
[0077]當(dāng)e的模糊子集為PM，ec的模糊子集為匪時(shí)，kp的模糊子集為0，Iii的模糊子集為0，kd的模糊子集為PS ；
[0078]當(dāng)e的模糊子集為PM，ec的模糊子集為NS時(shí)，kp的模糊子集為NS，Iii的模糊子集為PS，kd的模糊子集為PM ；
[0079]當(dāng)e的模糊子集為PM，ec的模糊子集為O時(shí)，kp的模糊子集為NM，Iii的模糊子集為PS，kd的模糊子集為PS ；
[0080]當(dāng)e的模糊子集為PM，ec的模糊子集為PS時(shí)，kp的模糊子集為NM，Iii的模糊子集為PM，kd的模糊子集為PS ；
[0081 ]當(dāng)e的模糊子集為PM，ec的模糊子集為PM時(shí)，kp的模糊子集為匪，Iii的模糊子集為PB，kd的模糊子集為PS ；
[0082]當(dāng)e的模糊子集為PM，ec的模糊子集為PB時(shí)，kp的模糊子集為NB，Iii的模糊子集為PB，kd的模糊子集為PB ；
[0083]當(dāng)e的模糊子集為PB，ec的模糊子集為NB時(shí)，kp的模糊子集為0，Iii的模糊子集為0，kd的模糊子集為PB ；
[0084]當(dāng)e的模糊子集為PB，ec的模糊子集為NM時(shí)，kp的模糊子集為0，Iii的模糊子集為0，kd的模糊子集為PM ；
[0085]當(dāng)e的模糊子集為PB，ec的模糊子集為NS時(shí)，kp的模糊子集為NM，Iii的模糊子集為PS，kd的模糊子集為PM ；
[0086]當(dāng)e的模糊子集為PB，ec的模糊子集為O時(shí)，kp的模糊子集為NM，Iii的模糊子集為PM，kd的模糊子集為PM ；
[0087]當(dāng)e的模糊子集為PB，ec的模糊子集為PS時(shí)，kp的模糊子集為NM，Iii的模糊子集為PM，kd的模糊子集為PS ；
[0088]當(dāng)e的模糊子集為PB，ec的模糊子集為PM時(shí)，kp的模糊子集為NB，Iii的模糊子集為PB，kd的模糊子集為PS ；
[0089]當(dāng)e的模糊子集為PB，ec的模糊子集為PB時(shí)，kp的模糊子集為NB，Iii的模糊子集為PB，kd的模糊子集為PB ；
[0090]步驟2-5:步驟2-1產(chǎn)生的輸入變量的模糊量根據(jù)步驟2-4產(chǎn)生的模糊規(guī)則、步驟2-3產(chǎn)生的伸縮因子，產(chǎn)生輸出變量的模糊量，經(jīng)去模糊化，得到變論域模糊控制器的輸出Λ kp, Δ ki； Δ kd ；
[0091]步驟2-6:將Akp, Aki, Akd值送到PID控制器中，對(duì)PID的三個(gè)控制參數(shù)進(jìn)行調(diào)節(jié)，完成三個(gè)參數(shù)的在線整定，公式如下:
【權(quán)利要求】
1.一種主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)，包括: 主手:用于實(shí)現(xiàn)醫(yī)生對(duì)導(dǎo)管位置的控制，主手接收醫(yī)生給出的導(dǎo)管移動(dòng)位置的期望值，并且通過(guò)映射將主手手柄移動(dòng)的位置信息轉(zhuǎn)換為導(dǎo)管運(yùn)動(dòng)關(guān)節(jié)量； 導(dǎo)管:用于主從微創(chuàng)手術(shù)中向病灶處輸送膠囊或支架； 檢測(cè)裝置:用于檢測(cè)導(dǎo)管末端和目標(biāo)點(diǎn)的位姿； 上位機(jī):用于接收主手和檢測(cè)裝置傳送的位置信號(hào)，將信號(hào)進(jìn)行處理，并將結(jié)果傳送給伺服驅(qū)動(dòng)裝置； 伺服驅(qū)動(dòng)裝置:用于接收上位機(jī)傳送的位置信號(hào)，驅(qū)使導(dǎo)管對(duì)醫(yī)生給出的導(dǎo)管移動(dòng)位置的期望值進(jìn)行跟蹤； 成像裝置:用于圖像處理和人機(jī)交互，將導(dǎo)管和血管姿態(tài)呈現(xiàn)，使醫(yī)生更加直觀地了解手術(shù)進(jìn)行情況； 其特征在于:所述的上位機(jī)中還進(jìn)一步包括: 位置運(yùn)動(dòng)轉(zhuǎn)換器:用于將上位機(jī)接收到的來(lái)自主手和檢測(cè)裝置的位置信息轉(zhuǎn)換為導(dǎo)管期望關(guān)節(jié)量和導(dǎo)管實(shí)際關(guān)節(jié)量； 變論域模糊控制器:用于將導(dǎo)管期望關(guān)節(jié)量和導(dǎo)管實(shí)際關(guān)節(jié)量之間的誤差及誤差變化率進(jìn)行模糊處理，經(jīng)模糊化，帶有變論域過(guò)程的模糊推理，去模糊化過(guò)程，將對(duì)導(dǎo)管關(guān)節(jié)量誤差和誤差變化率的控制作用轉(zhuǎn)換為PID控制的比例控制、積分控制、微分控制作用，并實(shí)現(xiàn)PID控制的比例參數(shù)、積分參數(shù)、微分參數(shù)在線調(diào)整； PID控制器:用于根據(jù)接收到的變論域模糊控制器的輸出信號(hào)完成導(dǎo)管關(guān)節(jié)量的跟足示O
2.一種主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)的控制的方法，其特征在于:包括以下步驟: 步驟1:首先，用戶在主從微創(chuàng)手術(shù)機(jī)器人系統(tǒng)的主手端設(shè)置導(dǎo)管移動(dòng)位置的期望位置；其次用戶操作主手手柄控制導(dǎo)管向期望位置移動(dòng)，在主手手柄的初始位置建立坐標(biāo)系，把主手手柄最終移動(dòng)到的位置信息映射到導(dǎo)管所處任務(wù)空間，通過(guò)映射得到的坐標(biāo)，利用雅克比逆矩陣最終確定導(dǎo)管運(yùn)動(dòng)關(guān)節(jié)量，確定導(dǎo)管運(yùn)動(dòng)關(guān)節(jié)量，包括導(dǎo)管的進(jìn)退量、旋轉(zhuǎn)量和彎曲量； 步驟2:位姿傳感器測(cè)得導(dǎo)管實(shí)際對(duì)應(yīng)的運(yùn)動(dòng)關(guān)節(jié)量，對(duì)導(dǎo)管位置進(jìn)行調(diào)整； 步驟2-1:期望關(guān)節(jié)量與實(shí)際關(guān)節(jié)量比較，計(jì)算二者的誤差e，將誤差e和誤差變化率ec作為輸入送入變論域模糊控制器中，確定變論域模糊控制器輸入和輸出的模糊子集，具體為: 在變論域模糊控制器內(nèi)，首先定義各個(gè)關(guān)節(jié)量誤差的語(yǔ)言變量E和誤差變化率的語(yǔ)言變量EC ;然后，用戶自定義誤差的論域e和誤差變化率ec的論域，并在誤差的論域范圍內(nèi)將誤差的語(yǔ)言變量離散化，在誤差變化率的論域范圍內(nèi)將誤差變化率的語(yǔ)言變量離散化，確定誤差對(duì)應(yīng)的模糊子集和誤差變化率對(duì)應(yīng)的模糊子集，表達(dá)式均為:
{NB, NM, NS, O, PS, PM, PB} 式中，NB代表負(fù)大，匪代表負(fù)中，NS代表負(fù)小，O代表零，PS代表正小，PM代表正中，PB代表正大； 設(shè)定變論域模糊控制器輸出的PID參數(shù)的三個(gè)變化量Akp，Aki, Akd的模糊子集表達(dá)式也均為:{NB, NM, NS, O, PS, PM, PB} 步驟2-2:確定PID參數(shù)的三個(gè)變化量Akp，Aki, Akd各自的隸屬度函數(shù)、誤差的隸屬度函數(shù)和誤差變化率的隸屬度函數(shù)； 步驟2-3:選取伸縮因子，分別為誤差對(duì)應(yīng)的伸縮因子、誤差變化率對(duì)應(yīng)的伸縮因子、比例因數(shù)變化量對(duì)應(yīng)的伸縮因子、積分因數(shù)變化量對(duì)應(yīng)的伸縮因子、微分因數(shù)變化量對(duì)應(yīng)的伸縮因子；選取伸縮因子的過(guò)程為: 首先將誤差e劃分為四個(gè)區(qū)域，即誤差區(qū)域I~4，其中: 誤差區(qū)域I定義為:誤差隸屬度函數(shù)中NB和PB最高的部分對(duì)應(yīng)的橫坐標(biāo)區(qū)間，該區(qū)域表示誤差較大； 誤差區(qū)域2定義為:誤差隸屬度函數(shù)中匪和PM最高的部分對(duì)應(yīng)的橫坐標(biāo)區(qū)間，該區(qū)域表不誤差適中； 誤差區(qū)域3定義為:誤差隸屬度函數(shù)中NS和PS最高的部分對(duì)應(yīng)的橫坐標(biāo)區(qū)間，該區(qū)域表不誤差較?。? 誤差區(qū)域4定義為:誤差隸屬度函數(shù)中O最高的部分對(duì)應(yīng)的橫坐標(biāo)區(qū)間，該區(qū)域表示誤差很??； 設(shè)置誤差區(qū)域I~4的伸縮因子的取值范圍: 誤差區(qū)域I內(nèi)的伸縮因子設(shè)置如下:誤差對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.7，I];誤差變化率對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.7，I];比例因數(shù)變化量對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.7，I];積分因數(shù)變化量對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(O，0.2];微分因數(shù)變化量對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(O，0.2]； 誤差區(qū)域2內(nèi)的伸縮因子設(shè)置如下:誤差對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.4,0.7]；誤差變化率對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.4,0.7];比例因數(shù)變化量對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.4,0.7];積分因數(shù)變化量對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.2，0.4]；微分因數(shù)變化量對(duì)應(yīng)的伸縮因子取值范圍為區(qū)間(0.2,0.4]； 誤差區(qū)域3內(nèi)的伸縮因子設(shè)置如下:誤差對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.2,0.4];誤差變化率對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.2,0.4];比例因數(shù)變化量對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.2,0.4];積分因數(shù)變化量對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.4,0.7];微分因數(shù)變化量對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.4,0.7]； 誤差區(qū)域4內(nèi)的伸縮因子設(shè)置如下:誤差對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0，0.2]；誤差變化率對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0，0.2];比例因數(shù)變化量對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0，0.2];積分因數(shù)變化量對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.7，I]；微分因數(shù)變化量對(duì)應(yīng)的伸縮因子的取值范圍為區(qū)間(0.4,0.7]； 伸縮因子的確定過(guò)程如下: 根據(jù)首個(gè)采樣時(shí)刻的誤差值，判斷該誤差值屬于哪個(gè)誤差區(qū)域，伸縮因子取值即為該誤差區(qū)域?qū)?yīng)的伸縮因子；再接收下一個(gè)采樣時(shí)刻的誤差值，若下一個(gè)采樣時(shí)刻的誤差值與上一個(gè)采樣時(shí)刻的誤差值位于同一個(gè)誤差區(qū)域內(nèi)，則伸縮因子不變，否則，修改令伸縮因子為此時(shí)誤差所在的誤差區(qū)域所對(duì)應(yīng)的伸縮因子，重復(fù)該過(guò)程，直至采樣過(guò)程結(jié)束； 步驟2-4:確定變論域模糊控制器各輸入輸出參數(shù)的模糊規(guī)則表； 所述的變論域模糊控制器內(nèi)的模糊控制規(guī)則如下:當(dāng)e的模糊子集為NB，ec的模糊子集為NB時(shí)，kp的模糊子集為PB，Ici的模糊子集為NB，kd的模糊子集為PS ； 當(dāng)e的模糊子集為NB，ec的模糊子集為NM時(shí)，kp的模糊子集為PB，Ici的模糊子集為NB，kd的模糊子集為NS ； 當(dāng)e的模糊子集為NB，ec的模糊子集為NS時(shí)，kp的模糊子集為PM，Ici的模糊子集為匪，kd的模糊子集為NB ； 當(dāng)e的模糊子集為NB，ec的模糊子集為O時(shí)，kp的模糊子集為PM，Ici的模糊子集為匪，kd的模糊子集為NB ； 當(dāng)e的模糊子集為NB，ec的模糊子集為PS時(shí)，kp的模糊子集為PS，Ici的模糊子集為NS，kd的模糊子集為NM ; 當(dāng)e的模糊子集為NB，ec的模糊子集為PM時(shí)，kp的模糊子集為O，Ici的模糊子集為O，kd的模糊子集為NM ; 當(dāng)e的模糊子集為NB，ec的模糊子集為PB時(shí)，kp的模糊子集為O，Ici的模糊子集為O，kd的模糊子集為PS ； 當(dāng)e的模糊子集為匪，ec的模糊子集為NB時(shí)，kp的模糊子集為PB，Ici的模糊子集為NB，kd的模糊子集為PS ； 當(dāng)e的模糊子集為匪，ec的模糊子集為NM時(shí)，kp的模糊子集為PB，Ici的模糊子集為NB，kd的模糊子集為NS ； 當(dāng)e的模糊子集為匪，ec的模糊子集為NS時(shí)，kp的模糊子集為PM，Ici的模糊子集為匪，kd的模糊子集為NB ； 當(dāng)e的模糊子集為NM，ec的模糊子集為O時(shí)，kp的模糊子集為PS，Ici的模糊子集為NS，kd的模糊子集為NM ; 當(dāng)e的模糊子集為匪，ec的模糊子集為PS時(shí)，kp的模糊子集為PS，Ici的模糊子集為NS，kd的模糊子集為NS ； 當(dāng)e的模糊子集為NM，ec的模糊子集為PM時(shí)，kp的模糊子集為O，Ici的模糊子集為O，kd的模糊子集為NS ； 當(dāng)e的模糊子集為匪，ec的模糊子集為PB時(shí)，kp的模糊子集為NS，Ici的模糊子集為O,kd的模糊子集為O ; 當(dāng)e的模糊子集為NS，ec的模糊子集為NB時(shí)，kp的模糊子集為PM，Ici的模糊子集為NB，kd的模糊子集為O ; 當(dāng)e的模糊子集為NS，ec的模糊子集為NM時(shí)，kp的模糊子集為PM，Ici的模糊子集為匪，kd的模糊子集為NS ； 當(dāng)e的模糊子集為NS，ec的模糊子集為NS時(shí)，kp的模糊子集為PM，Ici的模糊子集為NS，kd的模糊子集為NM ; 當(dāng)e的模糊子集為NS，ec的模糊子集為O時(shí)，kp的模糊子集為PS，Ici的模糊子集為NS，kd的模糊子集為NM ; 當(dāng)e的模糊子集為NS，ec的模糊子集為PS時(shí)，kp的模糊子集為O，Ici的模糊子集為O，kd的模糊子集為NS ； 當(dāng)e的模糊子集為NS，ec的模糊子集為PM時(shí)，kp的模糊子集為NS，Ici的模糊子集為PS, kd的模糊子集為NS ； 當(dāng)e的模糊子集為NS，ec的模糊子集為PB時(shí)，kp的模糊子集為NS，Ici的模糊子集為PS, kd的模糊子集為O ; 當(dāng)e的模糊子集為O，ec的模糊子集為NB時(shí)，kp的模糊子集為PM，Ici的模糊子集為匪，kd的模糊子集為O ; 當(dāng)e的模糊子集為O，ec的模糊子集為匪時(shí)，kp的模糊子集為PM，Ici的模糊子集為匪，kd的模糊子集為NS ； 當(dāng)e的模糊子集為O，ec的模糊子集為NS時(shí)，kp的模糊子集為PS，Ici的模糊子集為NS，kd的模糊子集為NS ； 當(dāng)e的模糊子集為O，ec的模糊子集為O時(shí)，kp的模糊子集為O，k,的模糊子集為O，kd的模糊子集為NS ； 當(dāng)e的模糊子集為O，ec的模糊子集為PS時(shí)，kp的模糊子集為NS，Ici的模糊子集為PS，kd的模糊子集為NS ； 當(dāng)e的模糊子集為O，ec的模糊子集為PM時(shí)，kp的模糊子集為NM，Iii的模糊子集為PM，kd的模糊子集為NS ； 當(dāng)e的模糊子集為O，ec的模糊子集為PB時(shí)，kp的模糊子集為NM，Iii的模糊子集為PM，kd的模糊子集為O ; 當(dāng)e的模糊子集為PS，ec的模糊子集為NB時(shí)，kp的模糊子集為PS，Ici的模糊子集為匪，kd的模糊子集為O ; 當(dāng)e的模糊子集為PS，ec的模糊子集為NM時(shí)，kp的模糊子集為PS，Ici的模糊子集為NS，kd的模糊子集為O ; 當(dāng)e的模糊子集為PS，ec的模糊子集為NS時(shí)，kp的模糊子集為O，Ici的模糊子集為O，kd的模糊子集為O ; 當(dāng)e的模糊子集為PS，ec的模糊子集為O時(shí)，kp的模糊子集為NS，Ici的模糊子集為PS，kd的模糊子集為O ; 當(dāng)e的模糊子集為PS，ec的模糊子集為PS時(shí)，kp的模糊子集為NS，Ici的模糊子集為PS, kd的模糊子集為O ; 當(dāng)e的模糊子集為PS，ec的模糊子集為PM時(shí)，kp的模糊子集為匪，Iii的模糊子集為PM，kd的模糊子集為O ; 當(dāng)e的模糊子集為PS，ec的模糊子集為PB時(shí)，kp的模糊子集為匪，Iii的模糊子集為PB，kd的模糊子集為O ; 當(dāng)e的模糊子集為PM，ec的模糊子集為NB時(shí)，kp的模糊子集為PS，Ici的模糊子集為O，kd的模糊子集為PB ； 當(dāng)e的模糊子集為PM，ec的模糊子集為匪時(shí)，kp的模糊子集為O，Ici的模糊子集為O，kd的模糊子集為PS ； 當(dāng)e的模糊子集為PM，ec的模糊子集為NS時(shí)，kp的模糊子集為NS，Ici的模糊子集為PS, kd的模糊子集為PM ； 當(dāng)e的模糊子集為PM，ec的模糊子集為O時(shí)，kp的模糊子集為NM，Iii的模糊子集為PS，kd的模糊子集為PS ；當(dāng)e的模糊子集為PM，ec的模糊子集為PS時(shí)，kp的模糊子集為匪，Iii的模糊子集為PM，kd的模糊子集為PS ； 當(dāng)e的模糊子集為PM，ec的模糊子集為PM時(shí)，kp的模糊子集為匪，Iii的模糊子集為PB，kd的模糊子集為PS ； 當(dāng)e的模糊子集為PM，ec的模糊子集為PB時(shí)，kp的模糊子集為NB，Ici的模糊子集為PB，kd的模糊子集為PB ； 當(dāng)e的模糊子集為PB，ec的模糊子集為NB時(shí)，kp的模糊子集為O，Ici的模糊子集為O，kd的模糊子集為PB ； 當(dāng)e的模糊子集為PB，ec的模糊子集為NM時(shí)，kp的模糊子集為O，Ici的模糊子集為O，kd的模糊子集為PM ； 當(dāng)e的模糊子集為PB，ec的模糊子集為NS時(shí)，kp的模糊子集為匪，Iii的模糊子集為PS, kd的模糊子集為PM ； 當(dāng)e的模糊子集為PB，ec的模糊子集為O時(shí)，kp的模糊子集為NM，Iii的模糊子集為PM，kd的模糊子集為PM ； 當(dāng)e的模糊子集為PB，ec的模糊子集為PS時(shí)，kp的模糊子集為匪，Iii的模糊子集為PM，kd的模糊子集為PS ； 當(dāng)e的模糊子集為PB，ec 的模糊子集為PM時(shí)，kp的模糊子集為NB，Ici的模糊子集為PB，kd的模糊子集為PS ； 當(dāng)e的模糊子集為PB，ec的模糊子集為PB時(shí)，kp的模糊子集為NB，Ici的模糊子集為PB，kd的模糊子集為PB ； 步驟2-5:步驟2-1產(chǎn)生的輸入變量的模糊量根據(jù)步驟2-4產(chǎn)生的模糊規(guī)則、步驟2-3產(chǎn)生的伸縮因子，產(chǎn)生輸出變量的模糊量，經(jīng)去模糊化，得到變論域模糊控制器的輸出Λ kp, Δ ki； Δ kd ； 步驟2-6:將Akp, Aki, Akd值送到PID控制器中，對(duì)PID的三個(gè)控制參數(shù)進(jìn)行調(diào)節(jié)，完成三個(gè)參數(shù)的在線整定，公式如下:
Kp =Kp0+Akp
s K1 — K1Q + Δ 灸i
[d = [dO + AZid 式中，Kptl為比例因數(shù)初值、Kitl為積分因數(shù)初值、Kdtl為微分因數(shù)初值，Kp、K1、Kd為PID控制器的控制參數(shù)，分別為比例因數(shù)、積分因數(shù)、微分因數(shù)； 步驟2-7:將步驟2-6得到的位置控制信號(hào)傳輸給伺服驅(qū)動(dòng)裝置，伺服驅(qū)動(dòng)裝置輸送導(dǎo)管運(yùn)動(dòng)到指定位置，實(shí)現(xiàn)導(dǎo)管的位置跟蹤； 步驟3:成像裝置接收檢測(cè)裝置中的圖像信息并成像； 步驟4:用戶通過(guò)成像裝置呈現(xiàn)的圖像判斷是否到達(dá)期望位置，若未到達(dá)，則執(zhí)行步驟2;若已到達(dá)，則等待用戶的下一步控制指令。
【文檔編號(hào)】A61B19/00GK103536367SQ201310508004
【公開(kāi)日】2014年1月29日 申請(qǐng)日期:2013年10月23日 優(yōu)先權(quán)日:2013年10月23日
【發(fā)明者】趙希梅, 任成一, 游健康, 姜明明, 朱江生 申請(qǐng)人:沈陽(yáng)工業(yè)大學(xué)