微創(chuàng)血管介入手術(shù)導(dǎo)管機(jī)器人三維模糊控制裝置及方法
【專利摘要】本發(fā)明一種微創(chuàng)血管介入手術(shù)導(dǎo)管機(jī)器人三維模糊控制裝置及方法，屬于微創(chuàng)血管介入手術(shù)機(jī)器人控制領(lǐng)域，該裝置包括主手裝置、上位機(jī)、從端DSP處理器、第一電機(jī)、第二電機(jī)、第三電機(jī)和姿態(tài)傳感器，本發(fā)明針對PID控制方法和二維模糊控制方法在控制導(dǎo)管操作機(jī)器人時(shí)存在的不足，提出了三維模糊控制方法，提高導(dǎo)管操作機(jī)器人的控制精度，進(jìn)而提高對導(dǎo)管的操作精度，降低患者呼吸心跳等環(huán)境因素對導(dǎo)管運(yùn)動的影響，減少操作者操作導(dǎo)管時(shí)產(chǎn)生的超調(diào)，使手術(shù)更精確、安全。
【專利說明】微創(chuàng)血管介入手術(shù)導(dǎo)管機(jī)器人三維模糊控制裝置及方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于微創(chuàng)血管介入手術(shù)機(jī)器人控制領(lǐng)域，具體涉及一種微創(chuàng)血管介入手術(shù)導(dǎo)管機(jī)器人三維模糊控制裝置及方法。
【背景技術(shù)】
[0002]血管介入手術(shù)具有傷口小、出血少、術(shù)后恢復(fù)快且并發(fā)癥少等優(yōu)點(diǎn)，廣泛應(yīng)用于心腦血管疾病的治療。在傳統(tǒng)的血管介入手術(shù)中，操作者直接操作導(dǎo)管，在醫(yī)學(xué)影像的輔助下，經(jīng)血管把導(dǎo)管送到病灶處，進(jìn)行診斷和治療。但是導(dǎo)管操作需要有經(jīng)驗(yàn)的操作者經(jīng)過反復(fù)嘗試才能獲得良好的效果，這樣操作者和病人長時(shí)間接觸射線，長期工作會對操作者造成一定損傷。隨著醫(yī)療輔助機(jī)器人技術(shù)的發(fā)展，主從導(dǎo)管介入的思想已經(jīng)引入微創(chuàng)血管介入手術(shù)領(lǐng)域。采用這種操作方式，操作者能夠脫離手術(shù)現(xiàn)場避免射線輻射傷害。手術(shù)中，操作者脫離現(xiàn)場，通過控制導(dǎo)管操作機(jī)器人間接操作導(dǎo)管到達(dá)病灶處，實(shí)現(xiàn)對病灶部位的治療?，F(xiàn)有的研究中導(dǎo)管操作機(jī)器人的控制通常采用PID控制方法。然而導(dǎo)管是一個高度柔性的設(shè)備，難以給出精確的運(yùn)動學(xué)模型，且導(dǎo)管工作在復(fù)雜的體內(nèi)環(huán)境中，系統(tǒng)參數(shù)會發(fā)生變化。PID控制器的參數(shù)選定后不能改變，它的適應(yīng)性較差。因此，需要一種更有效的方法來提高導(dǎo)管操作機(jī)器人的控制性能。模糊控制不需要知道被控對象的精確的數(shù)學(xué)模型，對過程參數(shù)變化不敏感，應(yīng)用于非線性系統(tǒng)時(shí)具有很強(qiáng)的魯棒性和良好的控制品質(zhì)。然而，二維模糊控制在消除系統(tǒng)穩(wěn)態(tài)誤差方面的性能較差。

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

[0003]針對現(xiàn)有技術(shù)的不足，本發(fā)明提出一種微創(chuàng)血管介入手術(shù)導(dǎo)管機(jī)器人三維模糊控制裝置及方法，以達(dá)克服傳統(tǒng) PID和二維模糊控制方法的不足，提高導(dǎo)管定位的精確度，提高導(dǎo)管的控制精度和系統(tǒng)的魯棒性，使手術(shù)更精確、安全的目的。
[0004]一種微創(chuàng)血管介入手術(shù)導(dǎo)管機(jī)器人三維模糊控制裝置，包括主手裝置、上位機(jī)、從端DSP處理器、第一電機(jī)、第二電機(jī)、第三電機(jī)和姿態(tài)傳感器，其中，
[0005]主手裝置:用于設(shè)置導(dǎo)管末端的目標(biāo)位置，并將該目標(biāo)位置發(fā)送至上位機(jī)中；
[0006]上位機(jī):
[0007](I)當(dāng)接收到導(dǎo)管末端的初始位置信號時(shí)，用于根據(jù)導(dǎo)管末端的目標(biāo)位置和初始位置，計(jì)算獲得兩者的偏差量；將偏差量與初始姿態(tài)信息發(fā)送至從端DSP處理器中；
[0008](2)當(dāng)接收到實(shí)時(shí)采集的導(dǎo)管末端位置信號時(shí)，用于根據(jù)實(shí)際導(dǎo)管末端位置，計(jì)算獲得與目標(biāo)位置的位置誤差，將該位置誤差和導(dǎo)管的實(shí)時(shí)姿態(tài)信息發(fā)送至從端DSP處理器；
[0009]從端DSP處理器:
[0010](I)當(dāng)接收到導(dǎo)管末端目標(biāo)位置與初始位置的偏差量和初始姿態(tài)信息時(shí)，用于根據(jù)導(dǎo)管遠(yuǎn)端彎曲段的初始姿態(tài)信息獲得雅克比矩陣，再根據(jù)逆雅克比矩陣和導(dǎo)管末端目標(biāo)位置與初始位置的偏差量，將位置偏差量變換為導(dǎo)管的軸向距離偏差值、旋轉(zhuǎn)角度偏差值和彎曲角度偏差值，并將上述三個量依次轉(zhuǎn)換為第一電機(jī)的角位移量、第二電機(jī)的角位移量和第三電機(jī)的角位移量，控制電機(jī)帶動導(dǎo)管運(yùn)動；
[0011](2)當(dāng)接收到導(dǎo)管末端的位置誤差信號和導(dǎo)管的實(shí)時(shí)姿態(tài)信息時(shí)，用于計(jì)算獲得導(dǎo)管末端的位置誤差變化率和位置誤差變化率的微分值，并將位置誤差、位置誤差變化率和位置誤差變化率的微分值作為其內(nèi)部的三維模糊控制器的輸入，三維模糊控制器根據(jù)推理規(guī)則公式進(jìn)行計(jì)算獲得導(dǎo)管末端位置的補(bǔ)償量，并根據(jù)逆雅克比矩陣，將導(dǎo)管末端位置補(bǔ)償量變換成導(dǎo)管軸向位移補(bǔ)償量、導(dǎo)管旋轉(zhuǎn)角度補(bǔ)償量和導(dǎo)管彎曲段彎曲角度補(bǔ)償量，將上述三個量依次發(fā)送至第一電機(jī)、第二電機(jī)和第三電機(jī)；
[0012]第一電機(jī):用于控制導(dǎo)管的軸向位移；
[0013]第二電機(jī):用于控制導(dǎo)管的旋轉(zhuǎn)角度；
[0014]第三電機(jī):用于控制導(dǎo)管的彎曲角度；
[0015]姿態(tài)傳感器:用于采集導(dǎo)管遠(yuǎn)端彎曲段的軸向位移、旋轉(zhuǎn)角度和彎曲角度。
[0016]主手裝置的輸出端連接上位機(jī)的一路輸入端，上位機(jī)的輸出端連接從端DSP處理器的一路輸入端，從端DSP處理器的一路輸出端連接第一電機(jī)的輸入端,從端DSP處理器的另一路輸出端連接第二電機(jī)的輸入端，從端DSP處理器的又一路輸出端連接第三電機(jī)的輸入端，第一電機(jī)的輸出端連接從端DSP處理器的另一路輸入端，第二電機(jī)的輸出端連接從端DSP處理器的又一路輸入端，第三電機(jī)的輸出端連接從端DSP處理器的再一路輸入端，第一電機(jī)的輸出軸、第二電機(jī)的輸出軸和第三電機(jī)的輸出軸均連接導(dǎo)管；姿態(tài)傳感器設(shè)置于導(dǎo)管上，其輸出端連接上位機(jī)的另一路輸入端。
[0017]采用微創(chuàng)血管介入手術(shù)導(dǎo)管機(jī)器人三維模糊控制裝置進(jìn)行控制的方法，包括以下步驟:
[0018]步驟1、通過主手裝置設(shè)置導(dǎo)管末端的目標(biāo)位置，并將該目標(biāo)位置發(fā)送至上位機(jī)中，將該位置變換到導(dǎo)管坐標(biāo)系中確定目標(biāo)位置坐標(biāo)；
[0019]步驟2、采用設(shè)置于導(dǎo)管遠(yuǎn)端彎曲段的位姿傳感器獲得導(dǎo)管遠(yuǎn)端的初始姿態(tài)信息和導(dǎo)管末端的初始位置，并將上述數(shù)據(jù)發(fā)送至上位機(jī)中，所述的姿態(tài)信息包括導(dǎo)管遠(yuǎn)端彎曲角度、旋轉(zhuǎn)角度和軸向位移；
[0020]步驟3、上位機(jī)根據(jù)導(dǎo)管末端的目標(biāo)位置坐標(biāo)和初始位置坐標(biāo)，計(jì)算獲得兩者的偏差量，將偏差量與初始姿態(tài)信息發(fā)送至從端DSP處理器中，從端DSP處理器根據(jù)導(dǎo)管遠(yuǎn)端初始彎曲角度、初始旋轉(zhuǎn)角度和初始軸向位移獲得雅克比矩陣，再根據(jù)逆雅克比矩陣和導(dǎo)管末端目標(biāo)位置坐標(biāo)與初始位置坐標(biāo)的偏差量，將位置偏差量變換為導(dǎo)管的軸向距離偏差值、旋轉(zhuǎn)角度偏差值和彎曲角度偏差值，并根據(jù)軸向距離偏差值計(jì)算第一電機(jī)的角位移量，發(fā)送至第一電機(jī)，根據(jù)旋轉(zhuǎn)角度偏差值計(jì)算第二電機(jī)的角位移量，發(fā)送至第二電機(jī)，根據(jù)彎曲角度偏差計(jì)算第三電機(jī)的角位移量，發(fā)送至第三電機(jī)，帶動導(dǎo)管運(yùn)動；
[0021]步驟4、采用位姿傳感器實(shí)時(shí)采集導(dǎo)管遠(yuǎn)端彎曲段的姿態(tài)信息和導(dǎo)管末端位置，并發(fā)送至上位機(jī)中，上位機(jī)根據(jù)實(shí)際導(dǎo)管末端位置，計(jì)算獲得與目標(biāo)位置的位置誤差，將該位置誤差和姿態(tài)信息發(fā)送至從端DSP處理器；
[0022]步驟5、從端DSP處理器計(jì)算獲得導(dǎo)管末端的位置誤差變化率和位置誤差變化率的微分值；
[0023]步驟6、從端DSP處理器采用三維模糊控制器對導(dǎo)管末端位置誤差、位置誤差變化率和位置誤差變化率的微分值進(jìn)行離散化處理，并結(jié)合設(shè)定的論域范圍對上述三個量進(jìn)行模糊化處理，將模糊化后的變量進(jìn)行模糊推理并采用重心法將推理所得結(jié)果去模糊化，即獲得導(dǎo)管末端位置的補(bǔ)償量；
[0024]步驟7、從端DSP處理器根據(jù)逆雅克比矩陣，將導(dǎo)管末端位置補(bǔ)償量變換成導(dǎo)管的三個自由度的補(bǔ)償量，即導(dǎo)管軸向位移補(bǔ)償量、導(dǎo)管旋轉(zhuǎn)角度補(bǔ)償量、導(dǎo)管彎曲段彎曲角度補(bǔ)償量，將導(dǎo)管軸向位移補(bǔ)償量發(fā)送至第一電機(jī)，將導(dǎo)管旋轉(zhuǎn)角度補(bǔ)償量發(fā)送至第二電機(jī)，將導(dǎo)管彎曲段彎曲角度補(bǔ)償量發(fā)送至第三電機(jī)，帶動導(dǎo)管向目標(biāo)位置移動；
[0025]步驟8、判斷導(dǎo)管末端是否到達(dá)期望位置，若到達(dá)，則停止并等待下一期望位置信號，否則，返回執(zhí)行步驟4。
[0026]步驟6所述的離散化處理，所采用的離散量化等級公式如下:
【權(quán)利要求】
1.一種微創(chuàng)血管介入手術(shù)導(dǎo)管機(jī)器人三維模糊控制裝置，其特征在于:包括主手裝置、上位機(jī)、從端DSP處理器、第一電機(jī)、第二電機(jī)、第三電機(jī)和姿態(tài)傳感器，其中， 主手裝置:用于設(shè)置導(dǎo)管末端的目標(biāo)位置，并將該目標(biāo)位置發(fā)送至上位機(jī)中； 上位機(jī): (1)當(dāng)接收到導(dǎo)管末端的初始位置信號時(shí)，用于根據(jù)導(dǎo)管末端的目標(biāo)位置和初始位置，計(jì)算獲得兩者的偏差量；將偏差量與初始姿態(tài)信息發(fā)送至從端DSP處理器中； (2)當(dāng)接收到實(shí)時(shí)采集的導(dǎo)管末端位置信號時(shí)，用于根據(jù)實(shí)際導(dǎo)管末端位置，計(jì)算獲得與目標(biāo)位置的位置誤差，將該位置誤差和導(dǎo)管的實(shí)時(shí)姿態(tài)信息發(fā)送至從端DSP處理器； 從端DSP處理器: (O當(dāng)接收到導(dǎo)管末端目標(biāo)位置與初始位置的偏差量和初始姿態(tài)信息時(shí)，用于根據(jù)導(dǎo)管遠(yuǎn)端彎曲段的初始姿態(tài)信息獲得雅克比矩陣，再根據(jù)逆雅克比矩陣和導(dǎo)管末端目標(biāo)位置與初始位置的偏差量，將位置偏差量變換為導(dǎo)管的軸向距離偏差值、旋轉(zhuǎn)角度偏差值和彎曲角度偏差值，并將上述三個量依次轉(zhuǎn)換為第一電機(jī)的角位移量、第二電機(jī)的角位移量和第三電機(jī)的角位移量，控制電機(jī)帶動導(dǎo)管運(yùn)動； (2)當(dāng)接收到導(dǎo)管末端的位置誤差信號和導(dǎo)管的實(shí)時(shí)姿態(tài)信息時(shí)，用于計(jì)算獲得導(dǎo)管末端的位置誤差變化率和位置誤差變化率的微分值，并將位置誤差、位置誤差變化率和位置誤差變化率的微分值作為其內(nèi)部的三維模糊控制器的輸入，三維模糊控制器根據(jù)推理規(guī)則公式進(jìn)行計(jì)算獲得導(dǎo)管末端位置的補(bǔ)償量，并根據(jù)逆雅克比矩陣，將導(dǎo)管末端位置補(bǔ)償量變換成導(dǎo)管軸向位移補(bǔ)償量、導(dǎo)管旋轉(zhuǎn)角度補(bǔ)償量和導(dǎo)管彎曲段彎曲角度補(bǔ)償量，將上述三個量依次發(fā)送至第一電機(jī)、第二電機(jī)和第三電機(jī)； 第一電機(jī):用于控制導(dǎo)管的軸向位移； 第二電機(jī):用于控制導(dǎo)管的旋轉(zhuǎn)角度； 第三電機(jī):用于控制導(dǎo)管的彎曲角度； 姿態(tài)傳感器:用于采集導(dǎo)管遠(yuǎn)端彎曲段的軸向位移、旋轉(zhuǎn)角度和彎曲角度。
2.根據(jù)權(quán)利要求1所述的微創(chuàng)血管介入手術(shù)導(dǎo)管機(jī)器人三維模糊控制裝置，其特征在于:主手裝置的輸出端連接上位機(jī)的一路輸入端，上位機(jī)的輸出端連接從端DSP處理器的一路輸入端，從端DSP處理器的一路輸出端連接第一電機(jī)的輸入端,從端DSP處理器的另一路輸出端連接第二電機(jī)的輸入端，從端DSP處理器的又一路輸出端連接第三電機(jī)的輸入端，第一電機(jī)的輸出端連接從端DSP處理器的另一路輸入端，第二電機(jī)的輸出端連接從端DSP處理器的又一路輸入端，第三電機(jī)的輸出端連接從端DSP處理器的再一路輸入端，第一電機(jī)的輸出軸、第二電機(jī)的輸出軸和第三電機(jī)的輸出軸均連接導(dǎo)管；姿態(tài)傳感器設(shè)置于導(dǎo)管上，其輸出端連接上位機(jī)的另一路輸入端。
3.采用權(quán)利要求1所述的微創(chuàng)血管介入手術(shù)導(dǎo)管機(jī)器人三維模糊控制裝置進(jìn)行控制的方法， 其特征在于:包括以下步驟: 步驟1、通過主手裝置設(shè)置導(dǎo)管末端的目標(biāo)位置，并將該目標(biāo)位置發(fā)送至上位機(jī)中，將該位置變換到導(dǎo)管坐標(biāo)系中確定目標(biāo)位置坐標(biāo)； 步驟2、采用設(shè)置于導(dǎo)管遠(yuǎn)端彎曲段的位姿傳感器獲得導(dǎo)管遠(yuǎn)端的初始姿態(tài)信息和導(dǎo)管末端的初始位置，并將上述數(shù)據(jù)發(fā)送至上位機(jī)中，所述的姿態(tài)信息包括導(dǎo)管遠(yuǎn)端彎曲角度、旋轉(zhuǎn)角度和軸向位移； 步驟3、上位機(jī)根據(jù)導(dǎo)管末端的目標(biāo)位置和初始位置，計(jì)算獲得兩者的偏差量，將偏差量與初始姿態(tài)信息發(fā)送至從端DSP處理器中，從端DSP處理器根據(jù)導(dǎo)管遠(yuǎn)端初始彎曲角度、初始旋轉(zhuǎn)角度和初始軸向位移獲得雅克比矩陣，再根據(jù)逆雅克比矩陣和導(dǎo)管末端目標(biāo)位置坐標(biāo)與初始位置坐標(biāo)的偏差量，將位置偏差量變換為導(dǎo)管的軸向距離偏差值、旋轉(zhuǎn)角度偏差值和彎曲角度偏差值，并根據(jù)軸向距離偏差值計(jì)算第一電機(jī)的角位移量，發(fā)送至第一電機(jī)，根據(jù)旋轉(zhuǎn)角度偏差值計(jì)算第二電機(jī)的角位移量，發(fā)送至第二電機(jī)，根據(jù)彎曲角度偏差計(jì)算第三電機(jī)的角位移量，發(fā)送至第三電機(jī)，帶動導(dǎo)管運(yùn)動； 步驟4、采用位姿傳感器實(shí)時(shí)采集導(dǎo)管遠(yuǎn)端彎曲段的姿態(tài)信息和導(dǎo)管末端位置，并發(fā)送至上位機(jī)中，上位機(jī)根據(jù)實(shí)際導(dǎo)管末端位置，計(jì)算獲得與目標(biāo)位置的位置誤差，將該位置誤差和姿態(tài)信息發(fā)送至從端DSP處理器； 步驟5、從端 DSP處理器計(jì)算獲得導(dǎo)管末端的位置誤差變化率和位置誤差變化率的微分值； 步驟6、從端DSP處理器采用三維模糊控制器對導(dǎo)管末端位置誤差、位置誤差變化率和位置誤差變化率的微分值進(jìn)行離散化處理，并結(jié)合設(shè)定的論域范圍對上述三個量進(jìn)行模糊化處理，將模糊化后的變量進(jìn)行模糊推理并采用重心法將推理所得結(jié)果去模糊化，即獲得導(dǎo)管末端位置的補(bǔ)償量； 步驟7、從端DSP處理器根據(jù)逆雅克比矩陣，將導(dǎo)管末端位置補(bǔ)償量變換成導(dǎo)管的三個自由度的補(bǔ)償量，即導(dǎo)管軸向位移補(bǔ)償量、導(dǎo)管旋轉(zhuǎn)角度補(bǔ)償量、導(dǎo)管彎曲段彎曲角度補(bǔ)償量，將導(dǎo)管軸向位移補(bǔ)償量發(fā)送至第一電機(jī)，將導(dǎo)管旋轉(zhuǎn)角度補(bǔ)償量發(fā)送至第二電機(jī)，將導(dǎo)管彎曲段彎曲角度補(bǔ)償量發(fā)送至第三電機(jī)，帶動導(dǎo)管向目標(biāo)位置移動； 步驟8、判斷導(dǎo)管末端是否到達(dá)期望位置，若到達(dá)，則停止并等待下一期望位置信號，否貝U，返回執(zhí)行步驟4。
4.根據(jù)權(quán)利要求3所述的微創(chuàng)血管介入手術(shù)導(dǎo)管機(jī)器人三維模糊控制方法，其特征在于:步驟6所述的離散化處理，所采用的離散量化等級公式如下:
5.根據(jù)權(quán)利要求3所述的微創(chuàng)血管介入手術(shù)導(dǎo)管機(jī)器人三維模糊控制方法，其特征在于:步驟6所述的模糊推理，推理規(guī)則公式如下:
【文檔編號】A61B19/00GK103549994SQ201310512479
【公開日】2014年2月5日 申請日期:2013年10月23日 優(yōu)先權(quán)日:2013年10月23日
【發(fā)明者】趙希梅, 游健康, 任成一, 姜明明, 趙久威 申請人:沈陽工業(yè)大學(xué)