微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置及方法

微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置及方法
【專利摘要】本發(fā)明一種微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置及方法，屬于機器人控制【技術(shù)領(lǐng)域】，該裝置包括主手控制裝置、第一控制器、第一電機、導(dǎo)管夾持裝置、第二控制器、第二電機、第一光電編碼器和第二光電編碼器，所述的第一控制器中包括第一模糊控制器和第一PID控制器，所述的第二控制器中包括第二模糊控制器和第二PID控制器；本發(fā)明提高了系統(tǒng)從手對主手的跟蹤性能，降低系統(tǒng)的超調(diào)量，保證了系統(tǒng)的魯棒性，從而進一步提高了微創(chuàng)介入手術(shù)機器人在介入手術(shù)過程中的安全性；對于降低醫(yī)生在手術(shù)過程中受到的輻射危害，提高介入手術(shù)的精度，保障患者的身心健康，都具有重要的意義。
【專利說明】微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置及方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于機器人控制【技術(shù)領(lǐng)域】，具體涉及一種微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置及方法。
【背景技術(shù)】
[0002]隨著國民經(jīng)濟的飛速發(fā)展和人民生活水平的不斷提高，各種疾病也相伴相生，其中心腦血管疾病已成為威脅人類健康的主要病癥之一。根據(jù)衛(wèi)生部統(tǒng)計數(shù)據(jù)，我國心血管患者超過2億人，每年約有300萬人死于心血管疾病。
[0003]微創(chuàng)血管介入手術(shù)是治療心血管疾病的有效方法。微創(chuàng)血管介入手術(shù)是醫(yī)生操縱導(dǎo)管沿人體血管到達體內(nèi)較遠的病變部位，進而對病變部位進行治療的手術(shù)。與傳統(tǒng)手術(shù)相比，微創(chuàng)血管介入手術(shù)具有出血少、創(chuàng)傷小、并發(fā)癥少、安全可靠和術(shù)后恢復(fù)快等優(yōu)點。然而醫(yī)生手動進行介入手術(shù)也存在明顯的不足:首先它對醫(yī)生的技能和操作經(jīng)驗要求較高，因而培養(yǎng)有經(jīng)驗的醫(yī)生需要較長的時間和精力；其次，在手術(shù)過程中醫(yī)生需要長時間工作在X射線下，對醫(yī)生自身健康危害較大；此外，手術(shù)過程中，醫(yī)生的誤操作很容易造成血管刺穿，危害病人的身心健康。
[0004]機器人技術(shù)與血管介入技術(shù)有機結(jié)合是解決上述問題的重要途徑。機器人進行介入手術(shù)具有以下優(yōu)勢:機器人進行介入手術(shù)具有定位精確，安全性高的特點；通過遙控操作技術(shù)和力反饋技術(shù)的相結(jié)合，醫(yī)生可遠程操作，避免了醫(yī)生遭受X射線輻射；機器人進行介入手術(shù)插管速度相對較快，降低了操作時間，減輕了病人的痛苦；結(jié)合視覺反饋可進一步提高插管精度，提高了手術(shù)的安全性。
[0005]目前應(yīng)用于血管介入手術(shù)中的多為主從式導(dǎo)管機器人系統(tǒng)，其控制方法多采用傳統(tǒng)的PID控制。然而，介入手術(shù)中采用的導(dǎo)管是柔性的，非線性的，導(dǎo)管在血管中運動存在血流的阻力、血管的摩擦力等環(huán)境因素，同時主從手之間存在時延效應(yīng)，故不容易建立精確的導(dǎo)管模型。所以主從式導(dǎo)管機器人系統(tǒng)的從手采用PID控制，往往不能很好的跟蹤主手的期望信號，且動態(tài)快速性和控制精度通常不能兼顧，有時會出現(xiàn)較大的超調(diào)量。因而PID控制應(yīng)用在微創(chuàng)血管介入手術(shù)中可能存在安全隱患，危害病人的身心健康。

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

[0006]針對現(xiàn)有技術(shù)的不足，本發(fā)明提出一種微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置及方法，以達到克服采用PID控制方法導(dǎo)致的跟蹤性能差、超調(diào)大，解決動態(tài)快速性與控制精度不能兼顧問題的目的。
[0007]一種微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置，包括主手控制裝置、第一控制器、第一電機、導(dǎo)管夾持裝置、第二控制器、第二電機、第一光電編碼器和第二光電編碼器，所述的第一控制器中包括第一模糊控制器和第一 PID控制器，所述的第二控制器中包括第二模糊控制器和第二 PID控制器；
[0008]其中，[0009]主手控制裝置:用于設(shè)定導(dǎo)管的期望軸向位移值和導(dǎo)管的期望旋轉(zhuǎn)角度值，并將導(dǎo)管的期望軸向位移值發(fā)送至第一控制器中，將導(dǎo)管的期望旋轉(zhuǎn)角度值發(fā)送至第二控制器中；
[0010]第一控制器:用于將期望位移值與實際導(dǎo)管軸向運動位移值相減，將獲得的位移誤差分別發(fā)送至控制器內(nèi)部的第一模糊控制器和第一 PID控制器內(nèi)，同時將位移誤差的微分值發(fā)送至第一模糊控制器內(nèi)；第一模糊控制器通過模糊推理，得到第一比例系數(shù)、第一積分系數(shù)和第一微分系數(shù)，將上述系數(shù)發(fā)送至第一 PID控制器；第一 PID控制器根據(jù)位移誤差、第一比例系數(shù)、第一積分系數(shù)和第一微分系數(shù)獲得導(dǎo)管軸向運動位移的控制信號，并將控制信號發(fā)送至第一電機進行位移補償；
[0011]第一電機:用于控制導(dǎo)管的軸向運動；
[0012]第二控制器:用于將期望旋轉(zhuǎn)角度值與實際導(dǎo)管旋轉(zhuǎn)角度值相減，將獲得的旋轉(zhuǎn)角度誤差分別發(fā)送至控制器內(nèi)部的第二模糊控制器和第二 PID控制器內(nèi)，同時將旋轉(zhuǎn)角度誤差的微分值發(fā)送至第二模糊控制器內(nèi)；第二模糊控制器通過模糊推理，得到第二比例系數(shù)、第二積分系數(shù)和第二微分系數(shù)，將上述系數(shù)發(fā)送至第二 PID控制器；第二 PID控制器根據(jù)旋轉(zhuǎn)角度誤差、第二比例系數(shù)、第二積分系數(shù)和第二微分系數(shù)獲得導(dǎo)管旋轉(zhuǎn)角度的控制信號，并將控制信號發(fā)送至第二電機進行旋轉(zhuǎn)角度補償；
[0013]第二電機:用于控制導(dǎo)管的旋轉(zhuǎn)角度；
[0014]第一光電編碼器:用于米集第一電機輸出軸的軸向位移值，即導(dǎo)管的軸向位移值，并將采集的數(shù)值返回發(fā)送至第一控制器中；
[0015]第二光電編碼器:用于采集第二電機輸出軸的旋轉(zhuǎn)角度值，即導(dǎo)管的旋轉(zhuǎn)角度值，并將采集的數(shù)值返回發(fā)送至第二控制器中。
[0016]所述的主手控制裝置的兩路輸出端分別連接第一控制器的一路輸入端和第二控制器的一路輸入端，第一控制器的輸出端連接第一電機的輸入端，第一電機輸出軸連接導(dǎo)管夾持裝置一路輸入端；第二控制器的輸出端連接第二電機的輸入端，第二電機的輸出端連接導(dǎo)管夾持裝置的另一路輸入端；第一光電編碼器設(shè)置于第一電機的輸出軸上，其輸出端連接第一控制器的另一路輸入端；第二光電編碼器設(shè)置于第二電機的輸出軸上，其輸出端連接第二控制器的另一路輸入端。
[0017]采用微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置進行模糊自適應(yīng)PID控制的方法，包括以下步驟:
[0018]步驟1、通過主手控制裝置設(shè)定導(dǎo)管的期望位置，位置參數(shù)包括導(dǎo)管軸向位移值和導(dǎo)管旋轉(zhuǎn)角度值；
[0019]步驟2、主手裝置將導(dǎo)管的期望軸向位移值發(fā)送至第一控制器中，將導(dǎo)管的期望旋轉(zhuǎn)角度值發(fā)送至第二控制器中，第一控制器通過控制第一電機帶動導(dǎo)管做軸向運動；第二控制器通過控制第二電機帶動導(dǎo)管做旋轉(zhuǎn)運動；
[0020]步驟3、采用設(shè)置于第一電機輸出軸上的第一光電編碼器實時采集導(dǎo)管的實際軸向運動位移數(shù)值，并將上述位移值發(fā)送至第一控制器中；采用設(shè)置于第二電機輸出軸上的第二光電編碼器實時采集導(dǎo)管的實際旋轉(zhuǎn)運動角度值，并將上述角度值發(fā)送至第二控制器中；
[0021]步驟4、第一控制器將期望位移值與實際導(dǎo)管軸向運動位移值相減，獲得位移誤差；第二控制器將實際導(dǎo)管旋轉(zhuǎn)角度值與期望角度值相減，獲得旋轉(zhuǎn)角度誤差；
[0022]步驟5、將位移誤差分別發(fā)送至控制器內(nèi)部的第一模糊控制器和第一 PID控制器內(nèi)，同時將位移誤差的微分值發(fā)送至第一模糊控制器內(nèi)，將旋轉(zhuǎn)角度誤差分別發(fā)送至控制器內(nèi)部的第二模糊控制器和第二 PID控制器內(nèi)，同時將旋轉(zhuǎn)角度誤差的微分值發(fā)送至第二豐旲糊控制器內(nèi)；
[0023]步驟6、第一模糊控制器根據(jù)隸屬函數(shù)和用戶設(shè)定的論域?qū)ξ灰普`差和位移誤差的微分進行模糊化處理，根據(jù)模糊化后的值查詢模糊控制規(guī)則表進行模糊推理，再采用重心法進行去模糊化，得到第一比例系數(shù)、第一積分系數(shù)和第一微分系數(shù)，將上述系數(shù)發(fā)送至第一 PID控制器；第二模糊控制器根據(jù)隸屬函數(shù)和用戶設(shè)定的論域?qū)嵌日`差和角度誤差的微分進行模糊化處理，根據(jù)模糊化后的值查詢模糊控制規(guī)則表進行模糊推理，再采用重心法進行去模糊化，得到第二比例系數(shù)、第二積分系數(shù)和第二微分系數(shù)，將上述系數(shù)發(fā)送至第二 PID控制器；
[0024]步驟7、第一 PID控制器根據(jù)位移誤差、第一比例系數(shù)、第一積分系數(shù)和第一微分系數(shù)獲得導(dǎo)管軸向運動位移的控制信號，并將控制信號發(fā)送至第一電機實現(xiàn)位移補償；第
二PID控制器根據(jù)旋轉(zhuǎn)角度誤差、第二比例系數(shù)、第二積分系數(shù)和第二微分系數(shù)獲得導(dǎo)管軸向運動位移的控制信號，并將控制信號發(fā)送至第二電機實現(xiàn)旋轉(zhuǎn)角度補償；
[0025]步驟8、判斷導(dǎo)管是否到達期望位置，若到達，則停止并等待下一期望位置信號，否則，返回執(zhí)行步驟4。
[0026]本發(fā)明優(yōu)點:
[0027]本發(fā)明一種微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置及方法，動態(tài)性能好，不僅具有了模糊控制的靈活性和適應(yīng)性強的優(yōu)點，還具有PID控制精度高的優(yōu)勢；本發(fā)明設(shè)計的模糊自適應(yīng)PID控制器，提高了系統(tǒng)從手對主手的跟蹤性能，降低系統(tǒng)的超調(diào)量，保證了系統(tǒng)的魯棒性，從而進一步提高了微創(chuàng)介入手術(shù)機器人在介入手術(shù)過程中的安全性；對于降低醫(yī)生在手術(shù)過程中受到的輻射危害，提高介入手術(shù)的精度，保障患者的身心健康，都具有重要的意義。
【專利附圖】

【附圖說明】
[0028]圖1為本發(fā)明一種實施例的整體裝置結(jié)構(gòu)示意圖；
[0029]圖2為本發(fā)明一種實施例的從手裝置機械結(jié)構(gòu)示意圖；
[0030]圖3為本發(fā)明一種實施例的光電編碼器A路信號電路圖；
[0031]圖4為本發(fā)明一種實施例的步進電機驅(qū)動電路；
[0032]圖5為本發(fā)明一種實施例的第一控制器DSP芯片；
[0033]圖6為本發(fā)明一種實施例的無刷直流電機驅(qū)動電路；
[0034]圖7為本發(fā)明一種實施例的第二控制器DSP芯片；
[0035]圖8為本發(fā)明一種實施例的微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制方法流程圖；
[0036]圖9為本發(fā)明一種實施例的微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)模糊自適應(yīng)PID控制結(jié)構(gòu)；
[0037]圖10為本發(fā)明一種實施例的ei (e2)，elC (e2c)和klp (k2p)的隸屬函數(shù)示意圖；
[0038]圖11為本發(fā)明一種實施例的kn (k2i)的隸屬函數(shù)示意圖；[0039]圖12為本發(fā)明一種實施例的kld (k2d)的隸屬函數(shù)示意圖；
[0040]圖13為本發(fā)明一種實施例的軸向運動的位移跟蹤效果示意圖；
[0041]圖14為本發(fā)明一種實施例的旋轉(zhuǎn)運動角度跟蹤效果示意圖。
【具體實施方式】
[0042]下面結(jié)合附圖對本發(fā)明實施例做進一步的詳細說明。
[0043]如圖1所示，微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置，包括主手控制裝置、第一控制器、第一電機、導(dǎo)管夾持裝置、導(dǎo)管、第二控制器、第二電機、第一光電編碼器和第二光電編碼器，所述的第一控制器中包括第一模糊控制器和第一 PID控制器，所述的第二控制器中包括第二模糊控制器和第二 PID控制器；其中，
[0044]主手控制裝置用于設(shè)定導(dǎo)管的期望軸向位移值和導(dǎo)管的期望旋轉(zhuǎn)角度值，并將導(dǎo)管的期望軸向位移值發(fā)送至第一控制器中，將導(dǎo)管的期望旋轉(zhuǎn)角度值發(fā)送至第二控制器中；第一控制器用于將期望位移值與實際導(dǎo)管軸向運動位移值相減，將獲得的位移誤差分別發(fā)送至控制器內(nèi)部的第一模糊控制器和第一 PID控制器內(nèi)，同時將位移誤差的微分值發(fā)送至第一模糊控制器內(nèi)；第一模糊控制器通過模糊推理，得到第一比例系數(shù)、第一積分系數(shù)和第一微分系數(shù),將上述系數(shù)發(fā)送至第一 PID控制器；第一 PID控制器根據(jù)位移誤差、第一比例系數(shù)、第一積分系數(shù)和第一微分系數(shù)獲得導(dǎo)管軸向運動位移的控制信號，并將控制信號發(fā)送至第一電機進行位移補償；第一電機用于控制導(dǎo)管的軸向運動；第二控制器用于將期望旋轉(zhuǎn)角度值與實際導(dǎo)管旋轉(zhuǎn)角度值相減，將獲得的旋轉(zhuǎn)角度誤差分別發(fā)送至控制器內(nèi)部的第二模糊控制器和第二 PID控制器內(nèi)，同時將旋轉(zhuǎn)角度誤差的微分值發(fā)送至第二模糊控制器內(nèi)；第二模糊控制器通過模糊推理，得到第二比例系數(shù)、第二積分系數(shù)和第二微分系數(shù)，將上述系數(shù)發(fā)送至第二 PID控制器；第二 PID控制器根據(jù)旋轉(zhuǎn)角度誤差、第二比例系數(shù)、第二積分系數(shù)和第二微分系數(shù)獲得導(dǎo)管旋轉(zhuǎn)角度的控制信號，并將控制信號發(fā)送至第二電機進行旋轉(zhuǎn)角度補償；第二電機用于控制導(dǎo)管的旋轉(zhuǎn)角度；第一光電編碼器用于采集第一電機輸出軸的軸向位移值，即導(dǎo)管的軸向位移值，并將采集的數(shù)值返回發(fā)送至第一控制器中；第二光電編碼器用于采集第二電機輸出軸的旋轉(zhuǎn)角度值，即導(dǎo)管的旋轉(zhuǎn)角度值，并將采集的數(shù)值返回發(fā)送至第二控制器中。
[0045]微創(chuàng)介入手術(shù)中，醫(yī)生通過對導(dǎo)管的推拉和旋轉(zhuǎn)，將其經(jīng)血管送到病變位置，故本實施例從手裝置(從手部分包括第一控制器、第一電機、導(dǎo)管夾持裝置、導(dǎo)管、第二控制器、第二電機、第一光電編碼器和第二光電編碼器)包含兩個自由度(軸向運動和旋轉(zhuǎn)運動)來模仿醫(yī)生的兩個動作:推拉和旋轉(zhuǎn)。
[0046]圖1中，主手控制裝置把期望的軸向運動的位移值送給第一控制器，第一控制器控制第一電機驅(qū)動器，驅(qū)動第一電機結(jié)合傳動裝置和導(dǎo)管夾持裝置帶動導(dǎo)管作軸向運動。安裝在第一電機軸上的第一光電編碼器，測量電機旋轉(zhuǎn)的角度，通過機械裝置關(guān)系求取導(dǎo)管實際移動的位移值，并將該值反饋到第一控制器。
[0047]主手控制裝置把期望的旋轉(zhuǎn)運動的角度值送給第二控制器，第二控制器控制第二電機驅(qū)動器，驅(qū)動第二電機結(jié)合傳動裝置和導(dǎo)管夾持裝置帶動導(dǎo)管作旋轉(zhuǎn)運動。安裝在第二電機軸上的第二光電編碼器，測量電機旋轉(zhuǎn)的角度，通過機械裝置關(guān)系求取導(dǎo)管實際旋轉(zhuǎn)的角度值，并將該值反饋到第二控制器。[0048]如圖2所示，其中I是滑輪，2是第二電機，3是導(dǎo)管夾持裝置，4是滑輪，5是導(dǎo)管，6是導(dǎo)管夾持裝置，7是絲杠，8是第一電機，9是可移動平臺。
[0049]圖2中，滑輪I和滑輪4組成滑輪組，兩個滑輪采用皮帶連接。第二電機2帶動滑輪I作旋轉(zhuǎn)運動，通過皮帶連接，進而帶動滑輪4作旋轉(zhuǎn)運動。導(dǎo)管5通過滑輪4，故滑輪4作旋轉(zhuǎn)運動，進而帶動導(dǎo)管5作旋轉(zhuǎn)運動。第一電機8帶動絲杠7作旋轉(zhuǎn)運動，絲杠7旋轉(zhuǎn)進而帶動平臺9作軸向運動，平臺移動進而帶動導(dǎo)管作軸向運動。導(dǎo)管夾持裝置3安裝在可移動的平臺9上，導(dǎo)管夾持裝置6安裝在固定的平臺上(可移動平臺下側(cè));導(dǎo)管夾持裝置3和導(dǎo)管夾持裝置6組成一對互鎖裝置，導(dǎo)管夾持裝置3夾持導(dǎo)管時，導(dǎo)管夾持裝置6松開導(dǎo)管，即不夾持導(dǎo)管；導(dǎo)管夾持裝置6夾持導(dǎo)管時，導(dǎo)管夾持裝置3松開導(dǎo)管，即不夾持導(dǎo)管。若導(dǎo)管作旋轉(zhuǎn)運動和軸向運動時，由導(dǎo)管夾持裝置3夾持導(dǎo)管，導(dǎo)管夾持裝置6不夾持導(dǎo)管。可移動平臺9在作軸向運動時，若移動的距離值超過了可移動平臺的移動范圍，則導(dǎo)管夾持裝置6夾持導(dǎo)管，導(dǎo)管夾持裝置3不夾持導(dǎo)管，第一電機驅(qū)動可移動平臺9后退至初始位置。此過程中導(dǎo)管被導(dǎo)管夾持裝置6夾持，導(dǎo)管不作任何運動?？梢苿悠脚_9退至初始位置后，導(dǎo)管夾持裝置6松開導(dǎo)管，導(dǎo)管夾持裝置3夾持導(dǎo)管，進而可移動平臺可以繼續(xù)作軸向運動。
[0050]主手控制裝置的兩路輸出端分別連接第一控制器的一路輸入端和第二控制器的一路輸入端，第一控制器的輸出端連接第一電機的輸入端，第一電機輸出軸連接導(dǎo)管夾持裝置一路輸入端；第二控制器的輸出端連接第二電機的輸入端，第二電機的輸出端連接導(dǎo)管夾持裝置的另一路輸入端；第一光電編碼器設(shè)置于第一電機的輸出軸上，其輸出端連接第一控制器的另一路輸入端；第二光電編碼器設(shè)置于第二電機的輸出軸上，其輸出端連接第二控制器的另一路輸入端。
[0051]本發(fā)明實施例中，主手控制裝置米用美國Novint公司生產(chǎn)的NovintFalcon新型3D手柄，該手柄結(jié)合上位機可以通過編程實現(xiàn)對從手的控制，上位機與下位機之間可采用CAN總線進行數(shù)據(jù)傳輸。本發(fā)明實施例中，第一控制器和第二控制器均采用TMS320F2812DSP的最小系統(tǒng)和外圍電路；第一光電編碼器和第二光電編碼器的型號均為HXB1-BEG05L400BM。
[0052]光電編碼器(指代第一光電編碼器和第二光電編碼器)的輸出端輸出三路信號，分別為A路、B路和Z路，三路信號分別經(jīng)光耦隔離和信號整形后，輸送到DSP控制器的QEPl引腳、QEPl引腳和QEP3引腳(如圖5所示)，本發(fā)明實施例中，采用6N137芯片進行光耦隔離，通過74HC14芯片進行濾波整形。以光電編碼器A路信號為例，如圖3所示，光電編碼器輸出A路信號連接一個2.2K的電阻后，與6N137芯片的引腳3連接。6N137芯片引腳2接一個12V電源；引腳7、8接5V的電源后，接一個0.1uF的電容，然后接地；引腳5直接接地；Vcc接一個Ik電阻后與引腳6相連。6N137芯片引腳6接一個Ik電阻后與74HC14芯片的引腳11相連，引腳11接一個0.1uF電容后，并聯(lián)一個20k的電阻，最后接地。74HC14的引腳10接DSP芯片的QEPl引腳。B路與Z路信號處理電路與A路信號完全相同。
[0053]本發(fā)明實施例中，第一電機采用了森創(chuàng)公司的42BYG250-BASSML-0151型兩相步進電機。步進電機的驅(qū)動電路如圖4所示(本實施例給出了該步進電機一相的驅(qū)動電路，另一相驅(qū)動電路與圖4相同)，圖4中步進電機的驅(qū)動芯片選用L298集成電路芯片，它是雙H橋式驅(qū)動器。步進電機的控制器采用芯片L297，它可以產(chǎn)生四相驅(qū)動信號。續(xù)流二極管V1、V2、V3和V4選用的型號均為IN4007。L297的引腳20與一個IOK電阻R2相連后接VCC (5V)，引腳2直接接地。VCC (5V)串接R2后，再與C2 (IOOuF)相連后接地。L297的引腳4、6、5和14分別與L298的引腳5、7、6和I直接相連。L298的引腳I串接一個0.5 Ω的電阻后接地，引腳8接電容C4(470uF)后接地。L297引腳12直接連接VCC (5V),引腳16與引腳12之間串接一個22K的電阻R1，引腳16與電容Cl (3.3uF)相連后接地。VCC (5V)電源與電容C3 (IOOuF)相連后接地。L298引腳4直接連接VCC(36V)。續(xù)流二極管V1、V3的負極與VCC(36V)相連接，V1、V3的正極分別與V2、V4的負極相連。L2981引腳I與Vl正極連接后接電動機負極端子，引腳3與V3的正極連接后接電動機正極端子。
[0054]如圖5所示，第一控制器DSP芯片采用TMS320F2812型號，該型號DSP芯片最小系統(tǒng)是常見的，故并未詳細闡述。芯片L297的引腳17、18、19、10、11分別與圖5中DSP引腳1、2、3、4、5連接。TMS320F2812的引腳6、7、8的輸入信號，為第一光電編碼器A路、B路和Z路經(jīng)光耦隔離和整形濾波后的信號。
[0055]本發(fā)明實施例中，第二電機采用了無刷直流電機，采用無刷直流電機的型號為42BLF02-003SV1024，無刷直流電機的驅(qū)動電路如圖6所示。圖6中三相橋式變換電路采用3個IR2110芯片驅(qū)動(圖6中以一個IR2110芯片的詳細連接電路為例，本實施例中采用三個與該電路結(jié)構(gòu)相同的驅(qū)動電路)，DSP輸出端PWM信號與IR2110芯片之間采用光電藕合器件TLP521進行信號轉(zhuǎn)換。圖6中Ql、Q2、Q3、Q4、Q5和Q6為M0SFET，型號為2skl328。自舉電容Cl和C2采用IuF陶瓷電容。自舉二極管選Dbs選用的二極管型號為BYT56D，它的功能是防止Vl導(dǎo)通時，較高的電壓反串入VCC端燒壞芯片。01、02、03、04、05和06選用的二極管型號為FR207。
[0056]如圖6所示，DSP引腳PWMl和PWM2分別接電阻Rl (Ik)和R2(lk)后，再分別與對應(yīng)的TPL521芯片的引腳2連接。兩片TPL521芯片的引腳I接3.3V電源，引腳4接15V電源。15V電源接一個0.1uF的電容后接地。PWMl對應(yīng)的TPL521芯片的引腳2連接一個Ik的電阻后接地，PWM2對應(yīng)的TPL521芯片的引腳2連接一個Ik的電阻后接地。IR2110芯片的引腳9和引腳13連接后接地。IR2110芯片引腳I和引腳2相連后接地，引腳2與一個50 Ω的電阻連接后，與Q6M0SFET的柵極相連，引腳3與15V電源相連。IR2110芯片的引腳3和引腳5串聯(lián)一個自舉二極管Dbs，引腳2和引腳3之間串聯(lián)一個0.1uf的自舉電容，引腳5和引腳6之間串聯(lián)一個0.1uf的自舉電容。IR2110芯片的引腳7接一個50的電阻R3后，再與Ql的柵極相連。MOSFET Ql與二極管Dl并聯(lián)、Q2與二極管D2并聯(lián)、Q3與二極管D3并聯(lián)、Q4與二極管D4并聯(lián)、Q5與二極管D5并聯(lián)、Q6與二極管D6并聯(lián)。Ql、Q3和Q5的漏極與母線相連，Q2、Q4和Q6的源極與地相連。Q1、Q3和Q5的源極與Q2、Q4和Q6的漏極相連。引腳5與Ql的源極相連后與無刷直流電機繞線A端子相連。無刷直流電機采用星形連接方式，無刷直流電機繞線B和繞線C具體電路連接與繞線A電路圖完全相似。
[0057]如圖7所示，第二控制器DSP也采用TMS320F2812型號，該型號DSP芯片最小系統(tǒng)是常見的，故并未詳細闡述。圖7中，TMS320F2812的引腳1、2、3、4、5、6分別與無刷直流電機驅(qū)動電路的PWMl端子、PWM2端子、PWM3端子、PWM4端子、PWM5端子和PWM6端子相連。TMS320F2812的引腳7、8、9的輸入信號，為第二光電編碼器A路、B路和Z路經(jīng)光耦隔離和整形濾波后的信號。
[0058]采用微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置進行模糊自適應(yīng)PID控制的方法，流程圖如圖8所示，
[0059]結(jié)合圖9敘述系統(tǒng)模糊自適應(yīng)PID控制過程；包括以下步驟:
[0060]步驟1、主手給定導(dǎo)管期望的運動位置值，包括期望導(dǎo)管軸向運動的位移值y和期望導(dǎo)管旋轉(zhuǎn)運動的角度值θ ；
[0061]步驟2、主手裝置將上述設(shè)定值發(fā)送至從手裝置內(nèi)部的控制器中，軸向位移值y發(fā)送至第一控制器，旋轉(zhuǎn)角度值θ發(fā)送至第二控制器。第一控制器驅(qū)動從手部分的第一電機結(jié)合機械傳動裝置帶動導(dǎo)管作軸向運動；第二控制器驅(qū)動從手部分的第二電機結(jié)合機械傳動裝置帶動導(dǎo)管作旋轉(zhuǎn)運動；
[0062]步驟3、米用設(shè)置于第一電機輸出軸上的第一光電編碼器結(jié)合機械傳輸裝置實時采集導(dǎo)管的實際軸向運動位移數(shù)值y,，并將上述實際位移值y,發(fā)送至第一控制器中；采用設(shè)置于第二電機輸出軸上的第二光電編碼器結(jié)合機械傳輸裝置實時米集導(dǎo)管的實際旋轉(zhuǎn)運動角度值θ，，并將上述實際旋轉(zhuǎn)角度值θ，發(fā)送至第二控制器中；
[0063]步驟4、第一控制器將期望位移值y與實際導(dǎo)管軸向運動位移值y'相減，獲得位移誤ei，即ei=y-y';第二控制器將期望角度值θ與實際導(dǎo)管旋轉(zhuǎn)角度值相減Θ，，獲得旋轉(zhuǎn)角度誤差e2，即θ2=θ-θ '；
[0064]步驟5、將位移誤差ei分別發(fā)送至第一控制器內(nèi)部的第一模糊控制器和第一 PID控制器內(nèi)，同時將位移誤差的微分值ep發(fā)送至第一模糊控制器內(nèi)。將旋轉(zhuǎn)角度誤差e2分別發(fā)送至第二控制器內(nèi)部的第二模糊控制器和第二 PID控制器內(nèi)，同時將旋轉(zhuǎn)角度誤差的微分值e2c發(fā)送至第二模糊控制器內(nèi)；
[0065]步驟6、第一模糊控制器根據(jù)隸屬函數(shù)和用戶設(shè)定的論域?qū)ξ灰普`差和位移誤差的微分進行模糊化處理，根據(jù)模糊化后的值查詢模糊控制規(guī)則表進行模糊推理，再采用重心法進行去模糊化，得到第一比例系數(shù)klp、第一積分系數(shù)kn和第一微分系數(shù)kld，將上述系數(shù)發(fā)送至第一 PID控制器；第二模糊控制器根據(jù)隸屬函數(shù)和用戶設(shè)定的論域?qū)嵌日`差和角度誤差的微分進行模糊化處理，根據(jù)模糊化后的值查詢模糊控制規(guī)則表進行模糊推理，再采用重心法進行去模糊化，得到第二比例系數(shù)k2p、第二積分系數(shù)k2i和第二微分系數(shù)k2d，將上述系數(shù)發(fā)送至第二 PID控制器。模糊控制具體步驟如下:
[0066]步驟6-1,根據(jù)量化因子,對模糊輸入ei Ce2)和ep (e2c)進行模糊化；
[0067]本發(fā)明實施例中，量化因子均為I。設(shè)置ei (e2)的語言變量為E，誤差變化率ei(e2)的語言變量為EC。如圖10所示,設(shè)置兩者的論域均為(-1，IXe1 Ce2WPe1C (e2c)隸屬度函數(shù)為trimf型。其語言變量的取值為{NB,NM,NS,ZO,PS,PM,PB}，其中，他表示負大、匪表示負中、NS表示負小、ZO表示O、PS表示正小、PM表示正中，PB表示正大。
[0068]步驟6-2，根據(jù)語言變量E和EC的取值查詢模糊控制規(guī)則表，進行模糊推理，獲得控制量；
[0069]根據(jù)不同的誤差值e和誤差變化值ec，控制過程對I^ki和kd自整定要求應(yīng)滿足如下規(guī)律:
[0070]I)當(dāng)誤差e取較大值時，應(yīng)取相對較大的1^值來提高響應(yīng)的快速性；同時為了降低超調(diào)，防止誤差e瞬時過大，Ici和kd應(yīng)取較小的值；
[0071]2)當(dāng)誤差e取較中等值時，為使系統(tǒng)具有較小的超調(diào)，應(yīng)取較小的kp值和適當(dāng)大小的h和kd值；[0072]3)當(dāng)誤差e為較小的值時，應(yīng)取較大的1^和Iii值，同時應(yīng)取適當(dāng)大小的kd值，來防止系統(tǒng)在平衡點附近發(fā)生震蕩。
[0073]根據(jù)以上專家總結(jié)的經(jīng)驗，本發(fā)明實施例中，根據(jù)模糊控制規(guī)則(該規(guī)則為公知常識，此處不做詳細描述)如表1所示。本發(fā)明實施例中共釆用了 49條模糊規(guī)則，模糊規(guī)則的形式如下:
[0074](l)If(e is NB) and(ec is NB) then (kp is PB) (Iii is NB) (kd is PS)
[0075]表示:當(dāng)誤差e處于NB范圍，ec處于NB范圍，那么輸出比例系數(shù)kp選用PB范圍的值，積分系數(shù)&選擇NB范圍的值，微分系數(shù)kd選擇PS范圍的值。語句(2)至語句(49)與其思路相同；
[0076](2) If (e is NB) and(ecis NM) then (k± is PB) (Iiiis NB) (kd is NS)
[0077]......
[0078](49) If (e is PB) and(ec is PB) then (k± is NB) (Iiiis PB) (kd is PB)
[0079]表1
【權(quán)利要求】
1.一種微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置，其特征在于:包括主手控制裝置、第一控制器、第一電機、導(dǎo)管夾持裝置、第二控制器、第二電機、第一光電編碼器和第二光電編碼器，所述的第一控制器中包括第一模糊控制器和第一 PID控制器，所述的第二控制器中包括第二模糊控制器和第二 PID控制器；其中， 主手控制裝置:用于設(shè)定導(dǎo)管的期望軸向位移值和導(dǎo)管的期望旋轉(zhuǎn)角度值，并將導(dǎo)管的期望軸向位移值發(fā)送至第一控制器中，將導(dǎo)管的期望旋轉(zhuǎn)角度值發(fā)送至第二控制器中；第一控制器:用于將期望位移值與實際導(dǎo)管軸向運動位移值相減，將獲得的位移誤差分別發(fā)送至控制器內(nèi)部的第一模糊控制器和第一 PID控制器內(nèi)，同時將位移誤差的微分值發(fā)送至第一模糊控制器內(nèi)；第一模糊控制器通過模糊推理，得到第一比例系數(shù)、第一積分系數(shù)和第一微分系數(shù)，將上述系數(shù)發(fā)送至第一 PID控制器；第一 PID控制器根據(jù)位移誤差、第一比例系數(shù)、第一積分系數(shù)和第一微分系數(shù)獲得導(dǎo)管軸向運動位移的控制信號，并將控制信號發(fā)送至第一電機進行位移補償；第一電機:用于控制導(dǎo)管的軸向運動；第二控制器:用于將期望旋轉(zhuǎn)角度值與實際導(dǎo)管旋轉(zhuǎn)角度值相減，將獲得的旋轉(zhuǎn)角度誤差分別發(fā)送至控制器內(nèi)部的第二模糊控制器和第二 PID控制器內(nèi)，同時將旋轉(zhuǎn)角度誤差的微分值發(fā)送至第二模糊控制器內(nèi)；第二模糊控制器通過模糊推理，得到第二比例系數(shù)、第二積分系數(shù)和第二微分系數(shù)，將上述系數(shù)發(fā)送至第二 PID控制器；第二 PID控制器根據(jù)旋轉(zhuǎn)角度誤差、第二比例系數(shù)、第二積分系數(shù)和第二微分系數(shù)獲得導(dǎo)管旋轉(zhuǎn)角度的控制信號，并將控制信號發(fā)送至第二電機進行旋轉(zhuǎn)角度補償；第二電機:用于控制導(dǎo)管的旋轉(zhuǎn)角度；第一光電編碼器:用于米集第一電機輸出軸的軸向位移值，即導(dǎo)管的軸向位移值，并將采集的數(shù)值返回發(fā)送至第一控制器中；第二光電編碼器:用于采集第二電機輸出軸的旋轉(zhuǎn)角度值，即導(dǎo)管的旋轉(zhuǎn)角度值，并將采集的數(shù)值返回發(fā)送至第二控制器中。
2.根據(jù)權(quán)利要求1所述的微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置，其特征在于:所述的主手控制裝置的兩路輸出端分別連接第一控制器的一路輸入端和第二控制器的一路輸入端，第一控制器的輸出端連接第一電機的輸入端，第一電機輸出軸連接導(dǎo)管夾持裝置一路輸入端；第二控制的輸出端連接第二電機的輸入端，第二電機的輸出端連接導(dǎo)管夾持裝置的另一路輸入端；第一光電編碼器設(shè)置于第一電機的輸出軸上，其輸出端連接第一控制器的另一路輸入端；第二光電編碼器設(shè)置于第二電機的輸出軸上，其輸出端連接第二控制器的另一路輸入端。
3.采用權(quán)利要求1所述的微創(chuàng)血管介入手術(shù)導(dǎo)管機器人系統(tǒng)控制裝置進行模糊自適應(yīng)PID控制的方法，其特征在于:包括以下步驟:步驟1、通過主手裝置設(shè)定導(dǎo)管的期望位置，位置參數(shù)包括導(dǎo)管軸向位移值和導(dǎo)管旋轉(zhuǎn)角度值；步驟2、主手裝置將導(dǎo)管的期望軸向位移值發(fā)送至第一控制器中，將導(dǎo)管的期望旋轉(zhuǎn)角度值發(fā)送至第二控制器中，第一控制器通過控制第一電機帶動導(dǎo)管做軸向運動；第二控制器通過控制第二電機帶動導(dǎo)管做旋轉(zhuǎn)運動；步驟3、采用設(shè)置于第一電機輸出軸上的第一光電編碼器實時采集導(dǎo)管的實際軸向運動位移數(shù)值，并將上述位移值發(fā)送至第一控制器中；采用設(shè)置于第二電機輸出軸上的第二光電編碼器實時采集導(dǎo)管的實際旋轉(zhuǎn)運動角度值，并將上述角度值發(fā)送至第二控制器中；步驟4、第一控制器將期望位移值與實際導(dǎo)管軸向運動位移值相減，獲得位移誤差；第二控制器將實際導(dǎo)管旋轉(zhuǎn)角度值與期望角度值相減，獲得旋轉(zhuǎn)角度誤差；步驟5、將位移誤差分別發(fā)送至控制器內(nèi)部的第一模糊控制器和第一 PID控制器內(nèi)，同時將位移誤差的微分值發(fā)送至第一模糊控制器內(nèi)，將旋轉(zhuǎn)角度誤差分別發(fā)送至控制器內(nèi)部的第二模糊控制器和第二 PID控制器內(nèi)，同時將旋轉(zhuǎn)角度誤差的微分值發(fā)送至第二模糊控制器內(nèi)；步驟6、第一模糊控制器根據(jù)隸屬函數(shù)和用戶設(shè)定的論域?qū)ξ灰普`差和位移誤差的微分進行模糊化處理，根據(jù)模糊化后的值查詢模糊控制規(guī)則表進行模糊推理，再采用重心法進行去模糊化，得到第一比例系數(shù)、第一積分系數(shù)和第一微分系數(shù)，將上述系數(shù)發(fā)送至第一PID控制器；第二模糊控制器根據(jù)隸屬函數(shù)和用戶設(shè)定的論域?qū)嵌日`差和角度誤差的微分進行模糊化處理，根據(jù)模糊化后的值查詢模糊控制規(guī)則表進行模糊推理，再采用重心法進行去模糊化，得到第二比例系數(shù)、第二積分系數(shù)和第二微分系數(shù)，將上述系數(shù)發(fā)送至第二PID控制器； 步驟7、第一 PID控制器根據(jù)位移誤差、第一比例系數(shù)、第一積分系數(shù)和第一微分系數(shù)獲得導(dǎo)管軸向運動位移的控制信號，并將控制信號發(fā)送至第一電機實現(xiàn)位移補償；第二PID控制器根據(jù)旋轉(zhuǎn)角度誤差、第二比例系數(shù)、第二積分系數(shù)和第二微分系數(shù)獲得導(dǎo)管軸向運動位移的控制信號，并將控制信號發(fā)送至第二電機實現(xiàn)旋轉(zhuǎn)角度補償；步驟8、判斷導(dǎo)管是否到達期望位置，若到達，則停止并等待下一期望位置信號，否則，返回執(zhí)行步驟4。
【文檔編號】G05D3/12GK103558759SQ201310509102
【公開日】2014年2月5日 申請日期:2013年10月23日 優(yōu)先權(quán)日:2013年10月23日
【發(fā)明者】趙希梅, 姜明明, 游健康, 任成一, 趙久威 申請人:沈陽工業(yè)大學(xué)