機械手系統(tǒng)及其控制方法
【專利摘要】在插入部復雜彎曲的情況下也能夠高精度地對前端的機械手進行控制。提供一種機械手系統(tǒng)，其具有：軟性的插入部(10)；機械手(6)，其在插入部(10)的前端具有被驅(qū)動的關(guān)節(jié)部；驅(qū)動部(7)，其在插入部(10)的基端側(cè)對機械手(6)的關(guān)節(jié)部進行驅(qū)動；形狀估計部，其估計插入部(10)的形狀；以及控制部，其根據(jù)由形狀估計部估計出的插入部(10)的形狀對驅(qū)動部(7)進行控制，形狀估計部具有在插入部(10)中沿著其長度方向隔開間隔配置的多個被檢測體(14)、檢測被檢測體(14)的三維位置的位置檢測部(15)、根據(jù)由位置檢測部(15)檢測到的被檢測體(14)的三維位置將插入部(10)的形狀劃分為沿著其長度方向排列的多個片段(SS)來進行計算的形狀計算部。
【專利說明】
機械手系統(tǒng)及其控制方法
技術(shù)領(lǐng)域
[0001 ]本發(fā)明涉及機械手系統(tǒng)及其控制方法。
【背景技術(shù)】
[0002] 以往，公知有使用UPD裝置進行內(nèi)窺鏡的彎曲部的形狀檢測并利用檢測到的形狀 對彎曲部進行控制的內(nèi)窺鏡系統(tǒng)(例如參照專利文獻1。）。
[0003] 現(xiàn)有技術(shù)文獻
[0004] 專利文獻
[0005] 專利文獻1:日本特開2009-131406號公報

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

[0006] 發(fā)明要解決的課題
[0007] 但是，在軟性內(nèi)窺鏡中，不僅是彎曲部，與其基端側(cè)連接的插入部也根據(jù)插入路徑 而使形狀復雜地變化，所以，在檢測彎曲部的形狀并加以利用的專利文獻1的內(nèi)窺鏡系統(tǒng) 中，存在很難高精度地對彎曲部進行控制的不良情況。
[0008] 本發(fā)明是鑒于上述情況而完成的，提供在插入部復雜彎曲的情況下也能夠高精度 地對前端的機械手進行控制的機械手系統(tǒng)及其控制方法。
[0009] 用于解決課題的手段
[0010] 為了實現(xiàn)上述目的，本發(fā)明提供以下手段。
[0011] 本發(fā)明的一個方式是一種機械手系統(tǒng)，其具有:軟性的插入部;機械手，其在該插 入部的前端具有被驅(qū)動的關(guān)節(jié)部;驅(qū)動部，其在所述插入部的基端側(cè)對所述機械手的所述 關(guān)節(jié)部進行驅(qū)動;形狀估計部，其估計所述插入部的形狀；以及控制部，其根據(jù)由該形狀估 計部估計出的所述插入部的形狀對所述驅(qū)動部進行控制，所述形狀估計部具有在所述插入 部上沿著其長度方向隔開間隔配置的多個被檢測體、檢測各該被檢測體的三維位置的位置 檢測部、根據(jù)由該位置檢測部檢測到的各所述被檢測體的三維位置將所述插入部的形狀劃 分為沿著其長度方向排列的多個片段來進行計算的形狀計算部。
[0012] 根據(jù)本方式，在將插入部從其前端側(cè)插入到體腔內(nèi)時，插入部模仿體腔的形狀而 使其形狀變化。此時，配置在插入部上的被檢測體被配置成在插入部的長度方向上隔開間 隔分散存在，所以，通過位置檢測部檢測各被檢測體的三維位置，由此，能夠通過形狀估計 部估計沿著插入部的長度方向的形狀。
[0013] 該情況下，形狀計算部將插入部的形狀劃分為在長度方向上排列的多個片段來進 行計算，所以，即使在各片段中通過簡單的形狀進行估計，也能夠高精度地計算整體變形為 復雜形狀的插入部的形狀。其結(jié)果，能夠根據(jù)估計出的插入部的形狀，通過控制部高精度地 對配置在插入部的基端側(cè)的驅(qū)動部針對插入部前端的機械手的驅(qū)動進行控制。
[0014] 在上述方式中，也可以是，所述形狀計算部在相鄰的2個所述被檢測體之間設(shè)定各 所述片段。
[0015] 由此，能夠設(shè)定最大數(shù)量的片段，即使在各片段中利用簡單形狀來估計插入部的 形狀，作為插入部整體，也能夠估計復雜的形狀。
[0016] 在上述方式中，也可以是，所述形狀計算部以包含相鄰的3個所述被檢測體的方式 設(shè)定各所述片段。
[0017] 由此，能夠通過穿過3個被檢測體的曲線，更加高精度地估計插入部的形狀。
[0018] 在上述方式中，也可以是，所述形狀計算部按照每個所述片段，利用單一的圓弧對 相鄰的2個所述被檢測體之間進行近似。
[0019] 由此，能夠按照針對每2個被檢測體設(shè)定的每個片段，通過穿過2個被檢測體的圓 弧簡易且有效地估計插入部的形狀。
[0020] 在上述方式中，也可以是，所述形狀計算部按照每個所述片段，利用單一的圓弧對 相鄰的3個所述被檢測體之間進行近似。
[0021] 由此，能夠按照針對每3個被檢測體設(shè)定的每個片段，通過穿過3個被檢測體的圓 弧簡易且有效地估計插入部的形狀。
[0022] 在上述方式中，也可以是，所述形狀計算部以包含相鄰的3個以上的所述被檢測體 的方式設(shè)定各所述片段，根據(jù)各所述被檢測體的三維位置，通過曲線擬合來求出表示各所 述片段內(nèi)的所述插入部的形狀的曲線的多項式。
[0023] 本發(fā)明的另一個方式是一種機械手的控制方法，其中，劃分為沿著軟性的插入部 的長度方向排列的多個片段來計算該插入部的形狀，根據(jù)計算出的所述插入部的形狀，對 配置在該插入部的基端側(cè)的驅(qū)動部針對配置在所述插入部的前端的機械手的驅(qū)動進行控 制。
[0024] 根據(jù)本方式，即使在各片段中通過簡單的形狀進行估計，也能夠高精度地計算整 體變形為復雜形狀的插入部的形狀。而且，能夠根據(jù)估計出的插入部的形狀，高精度地對配 置在插入部的基端側(cè)的驅(qū)動部針對插入部前端的機械手的驅(qū)動進行控制。
[0025]發(fā)明效果
[0026] 根據(jù)本發(fā)明，發(fā)揮在插入部復雜彎曲的情況下也能夠高精度地對前端的機械手進 行控制的效果。
【附圖說明】
[0027] 圖1是示出本發(fā)明的一個實施方式的機械手系統(tǒng)的整體結(jié)構(gòu)圖。
[0028] 圖2是示出圖1的機械手系統(tǒng)的機械手的立體圖。
[0029]圖3是說明圖2的機械手的控制的框圖。
[0030] 圖4是說明安裝在圖2的機械手的軟性部上的磁線圈的配置的圖。
[0031] 圖5是示出圖4的軟性部彎曲的狀態(tài)的圖。
[0032] 圖6是說明通過單一的圓弧來估計圖5的軟性部的1個片段的形狀的情況的圖。 [0033]圖7是對圖6中估計出形狀的片段進行模式化的圖。
[0034] 圖8是示出用于對圖1的機械手系統(tǒng)中的軟性部的（a)l個片段進行模型化的各值 的圖，（b)是示出軟性部的線模型的圖。
[0035] 圖9是示出圖8的變形例的軟性部的線模型的圖。
[0036] 圖10是圖1的機械手系統(tǒng)的變形例，是說明基于包含3個磁線圈的片段的估計的 圖。
[0037] 圖11是圖1的機械手系統(tǒng)的變形例，是說明基于重復的片段的估計的圖。
[0038] 圖12是說明軟性部的形狀估計的另一個方法的圖。
[0039] 圖13是說明軟性部的形狀估計的又一個方法的圖。
【具體實施方式】
[0040] 下面，參照附圖對本發(fā)明的一個實施方式的機械手系統(tǒng)1及其控制方法進行說明。
[0041] 如圖1所示，本實施方式的機械手系統(tǒng)1具有由醫(yī)師A操作的主裝置2、通過經(jīng)由該 主裝置2的輸入而被驅(qū)動的從裝置3、根據(jù)針對主裝置2的輸入而對從裝置3進行控制的控制 器4、監(jiān)視器5。
[0042] 如圖2所示，從裝置3具有插入到患者P的體腔內(nèi)的本實施方式的機械手6、對該機 械手6進行驅(qū)動的驅(qū)動部7、用于檢測機械手6的形狀的傳感器系統(tǒng)8。
[0043]機械手6是具有能夠彎曲的軟性的細長的插入部9的軟性內(nèi)窺鏡，具有細長的軟性 部10、配置在前端的前端部11、配置在這些前端部11與軟性部10之間的彎曲部12。
[0044] 驅(qū)動部7具有由馬達7a驅(qū)動的直動機構(gòu)13a、以能夠拆裝的方式安裝在該直動機構(gòu) 13a上且在內(nèi)部收容有線的連接部13b、使多個連接部13b集合的中繼部13c，所述線將連接 部13b與前端部11之間連接起來。前端部11例如是具有多個關(guān)節(jié)的臂，通過線的牽引力對各 關(guān)節(jié)進彳丁驅(qū)動。
[0045] 如圖3所示，驅(qū)動部7的馬達7a根據(jù)從控制器4送來的指令信號進行驅(qū)動，并且進行 控制，以使得通過反饋馬達7a所具有的檢測驅(qū)動量的編碼器的輸出來實現(xiàn)指令信號那樣的 驅(qū)動量。
[0046] 傳感器系統(tǒng)8例如具有在軟性部10中沿著長度方向隔開間隔配置的多個磁線圈 (被檢測體）14、以及接收從該磁線圈14產(chǎn)生的磁的天線(形狀檢測部）15。通過天線15接收 從磁線圈14產(chǎn)生的磁，由此，能夠取得各磁線圈14的三維位置信息。由傳感器系統(tǒng)8取得的 各磁線圈14的三維位置信息被送到控制器4。
[0047]控制器4具有根據(jù)所送來的各磁線圈14的三維位置信息計算軟性部10的形狀的形 狀計算部16、以及根據(jù)由該形狀計算部16估計出的軟性部10的形狀對驅(qū)動部7進行控制的 控制信號產(chǎn)生部17。
[0048]通過傳感器系統(tǒng)8和形狀計算部16構(gòu)成形狀估計部。
[0049]控制信號產(chǎn)生部17將沿著線的長度方向的移位量作為指令信號輸出到驅(qū)動部7。 [0050]下面，對本實施方式的機械手系統(tǒng)1的控制器4中設(shè)置的形狀計算部16的動作進行 說明。
[0051]如圖4所示，形狀計算部16預先存儲軟性部10所具有的磁線圈14的沿著軟性部10 的間隔尺寸S。而且，形狀計算部16按照相鄰的2個磁線圈14來設(shè)定片段SS，按照每個片段SS 計算軟性部10的形狀。
[0052]使用由傳感器系統(tǒng)8取得的各磁線圈14的三維位置信息，如圖5和圖6所示，通過下 式計算所有相鄰的2個磁線圈14間的弦的長度L。
[0053] L= V ((x2-xl)2+(y2-yl)2+(z2-zl)2)
[0054] 這里，在圖5和圖6中僅示出前端的2個磁線圈14，（11、71、21)、&2、 72、22)分別是 相鄰的2個磁線圈14的坐標位置。
[0055] 當假設(shè)圖5那樣彎曲的相鄰的2個磁線圈14間的軟性部10如圖6和圖7所示以同樣 的曲率半徑R彎曲時，能夠通過反復計算來計算曲率半徑R，以使得下式(1)成立。
[0056] L-2Rsin(S/2R)=0 (1)
[0057] 由此，將所有相鄰的2個磁線圈14間的軟性部10估計為以曲率半徑R彎曲的圓弧。
[0058] 控制器4對驅(qū)動部7進行控制，所以，根據(jù)圖8所示的線模型，具有如數(shù)學式1所示那 樣記述的運動方程式。關(guān)于線模型，在圖8(b)所示的例子中，假設(shè)利用具有規(guī)定剛性的彈簧 19來連結(jié)各片段SS的具有質(zhì)量m的質(zhì)點18,數(shù)學式1的運動方程式成立。這里，例示了片段SS 的數(shù)量為3個的情況。
[0059] 【數(shù)學式1】
[0061] 其中，
[0062] 1111、1112、1113是各片段33(1~3)中的線的質(zhì)量，
[0063] cl、c2、c3是各片段SS(1~3)中的沿著線的長度方向的粘性摩擦系數(shù)，
[0064] zf 、"'是各片段SS(1~3)中的沿著線的長度方向的移動速度，
[0065] z 1〃、z2〃、z3〃是各片段SS( 1~3)中的沿著線的長度方向的加速度，
[0066] 1'0、1'13233是各片段33(1~3)中的對線施加的張力，
[0067] fl、f2、f3是各片段SS(1~3)中的摩擦力。
[0068] 在該運動方程式中，摩擦力f (各摩擦力€14243)由下式表示。
[0069] 【數(shù)學式2】
[0070] f = SgnCz7 )μ( Δ S/R)T
[0071] 其中，
[0072] sgnU')是表示速度y的符號的函數(shù)，
[0073] μ是動摩擦系數(shù)，
[0074] AS是弧的長度，
[0075] R是曲率半徑，
[0076] T是張力。
[0077] 在數(shù)學式2中，曲率半徑R使用通過式(1)估計出的值。
[0078] 根據(jù)該數(shù)學式2,摩擦力f根據(jù)構(gòu)成各片段SS的軟性部10的彎曲而變動。因此，在構(gòu) 成各片段SS的軟性部10的彎曲發(fā)生變化的情況下，作為該摩擦力f的變化，對控制器4中存 儲的運動方程式進行修正。
[0079] 下面，對這樣構(gòu)成的本實施方式的機械手系統(tǒng)1的作用進行說明。
[0080] 在使用本實施方式的機械手系統(tǒng)1對配置在患者P的體腔內(nèi)的患部進行處置時，將 機械手6從前端部11導入到體腔內(nèi)，使前端部11與患部對置。
[0081] 這樣，構(gòu)成機械手6的插入部的軟性部10模仿體腔的形狀而復雜地彎曲。在該狀態(tài) 下，通過傳感器系統(tǒng)8的工作，通過天線15來檢測裝配在軟性部10上的磁線圈14的三維位 置。
[0082] 由天線15檢測到的各磁線圈14的位置信息被送到控制器4,由此，通過控制器4所 具有的形狀計算部16計算軟性部10的形狀。
[0083] 該情況下，在本實施方式的機械手系統(tǒng)1中，形狀計算部16按照沿著該軟性部10的 長度方向排列的多個片段SS來計算軟性部10。
[0084] 在本實施方式所示的例子中，形狀計算部16對所有相鄰的每2個磁傳感器14設(shè)定 片段SS，通過單一的圓弧來估計各片段SS的軟性部10,所以，能夠通過簡易的估計而高精度 地計算復雜的軟性部10的形狀。然后，根據(jù)按照每個片段SS估計出的軟性部10的曲率半徑 R，通過數(shù)學式2計算摩擦力f，通過按照每個片段SS計算出的摩擦力f!、f2、f3對數(shù)學式1的 運動方程式進行修正。
[0085] 控制信號產(chǎn)生部17通過求解該數(shù)學式1的運動方程式，計算作為指令信號的線的 移位量A，并且，從該移位量A換算為實現(xiàn)該移位量A的馬達的驅(qū)動量后，將其輸出到驅(qū)動部 7。由此，馬達7a以所指示的驅(qū)動量進行驅(qū)動，線以期望的移位量進行移位，構(gòu)成前端部11的 臂的各關(guān)節(jié)被尚精度地驅(qū)動。
[0086] 這樣，本實施方式的機械手系統(tǒng)1及其控制方法具有如下優(yōu)點:在通過配置在軟性 部10的基端側(cè)的馬達7a牽引線來驅(qū)動配置在軟性部10的前端的臂的情況下，在軟性部10彎 曲成復雜形狀時，也能夠高精度地估計其形狀，能夠高精度地計算用于驅(qū)動臂的關(guān)節(jié)的指 令信號。而且，在形狀的估計時，將軟性部10分割為多個片段SS，按照每個片段SS利用單一 的圓弧而簡易地估計形狀，所以，能夠通過簡單的計算來抑制計算所需要的時間，能夠?qū)崟r 地進行控制。
[0087] 另外，在本實施方式中，例示了劃分為3個片段SS的情況，但是，如圖9和數(shù)學式3所 示，片段SS的分割數(shù)可以是任意的。
[0088] 【數(shù)學式3】
[0090] 并且，在本實施方式中，按照每個片段SS假設(shè)單一的圓弧來估計其曲率半徑R，但 是，取而代之，也可以估計彎曲角度Θ。
[0091] 并且，在曲率半徑R或彎曲角度Θ的估計中，運算花費時間，所以，在構(gòu)成各片段SS 的軟性部10為直線的情況下或者是同一圓弧這樣的已知形狀的情況下，也可以省略運算。 [0092]例如，在各片段SS中根據(jù)2個磁線圈14間的直線距離計算出的軟性部10的弦的長 度L與沿著軟性部10的方向的磁線圈14間的距離相等的情況下，可以視為該片段SS是直線， 能夠省略曲率半徑R等的估計用的反復運算。并且，在任意片段SS中估計出曲率半徑R等的 情況下，在配置有在該估計出的圓弧上相鄰的片段SS的磁線圈14的情況下，不用重新估計 曲率半徑R等，使用已經(jīng)估計出的曲率半徑R等即可。
[0093] 并且，磁線圈14的間隔可以是等間隔，也可以不是等間隔。
[0094]并且，在相鄰的2個磁線圈14之間設(shè)置片段SS，將2個磁線圈14間的距離作為弦的 長度L來估計圓弧的曲率半徑R，但是，取而代之，如圖10所示，也可以設(shè)各片段SS包含3個以 上的磁線圈14,通過穿過這些磁線圈14的位置的圓弧來估計曲率半徑R。另外，片段SS可以 如圖10那樣獨立，也可以如圖11那樣重復。
[0095]并且，關(guān)于軟性部10的形狀的估計，可以在從裝置3的工作中始終實時地進行估 計，也可以在用于使從裝置3進行工作的準備完成時、或定期地、或根據(jù)需要手動進行估計。 [0096] 并且，在本實施方式中，如圖12所示，也可以根據(jù)3個以上的磁線圈14的位置信息， 通過曲線擬合來求出曲線C的多項式，求出各磁線圈14的位置處的曲線C的切線，運算相鄰 的切線的傾斜角度α、α+Δα的差分Δα作為彎曲角度。該情況下，當設(shè)磁線圈14間的弧長AS 已知時，可以如下求出曲率半徑R。
[0097] R= Δ S/Δα
[0098] 并且，如圖13所示，也可以利用弧長S來表現(xiàn)按照每個片段SS表示軟性部10的曲線 C，利用弧長ds對其切線矢量t、t+dt的差分dt進行微分，由此，如下式那樣求出曲率κ。
[0099] K= I dt/ds
[0100] 并且，在本實施方式中，作為傳感器系統(tǒng)8,例示了由設(shè)置在軟性部10上的磁線圈 14和天線15構(gòu)成的內(nèi)窺鏡插入形狀觀測裝置（UPD;Endoscope Position Detecting Unit)，但是，取而代之，也可以根據(jù)CT或MRI等的術(shù)前的臟器形狀的信息來預測插入后的軟 性部10的形狀。并且，也可以在軟性部10上安裝變形傳感器或光纖傳感器等檢測彎曲的傳 感器來檢測形狀。
[0101] 并且，作為插入部9的前端的關(guān)節(jié)部，例示了具有臂的前端部11，但是，也可以是未 設(shè)置前端部11的軟性內(nèi)窺鏡。該情況下，彎曲部12構(gòu)成插入部的前端的關(guān)節(jié)部。
[0102] 并且，作為機械手6,例示了軟性內(nèi)窺鏡，但是，也可以是軟性處置器械，還可以是 軟性的外套管。
[0103] 標號說明
[0104] SS:片段;1:機械手系統(tǒng);6:機械手;7:驅(qū)動部；10:軟性部(插入部）；14:磁線圈（被 檢測體）；15:天線(位置檢測部、形狀估計部）；16:形狀計算部(形狀估計部）；17:控制信號 產(chǎn)生部(控制部）。
【主權(quán)項】
1. 一種機械手系統(tǒng)，其具有： 軟性的插入部； 機械手，其在該插入部的前端具有被驅(qū)動的關(guān)節(jié)部； 驅(qū)動部，其對該機械手的所述關(guān)節(jié)部進行驅(qū)動； 形狀估計部，其估計所述插入部的形狀；以及 控制部，其根據(jù)由該形狀估計部估計出的所述插入部的形狀對所述驅(qū)動部進行控制， 所述形狀估計部具有在所述插入部上沿著其長度方向隔開間隔配置的多個被檢測體、 檢測各該被檢測體的三維位置的位置檢測部、根據(jù)由該位置檢測部檢測到的各所述被檢測 體的三維位置將所述插入部的形狀劃分為沿著其長度方向排列的多個片段來進行計算的 形狀計算部。2. 根據(jù)權(quán)利要求1所述的機械手系統(tǒng)，其中， 所述形狀計算部在相鄰的2個所述被檢測體之間設(shè)定各所述片段。3. 根據(jù)權(quán)利要求1所述的機械手系統(tǒng)，其中， 所述形狀計算部以包含相鄰的3個所述被檢測體的方式設(shè)定各所述片段。4. 根據(jù)權(quán)利要求2所述的機械手系統(tǒng)，其中， 所述形狀計算部按照每個所述片段，利用單一的圓弧對相鄰的2個所述被檢測體之間 進行近似。5. 根據(jù)權(quán)利要求3所述的機械手系統(tǒng)，其中， 所述形狀計算部按照每個所述片段，利用單一的圓弧對相鄰的3個所述被檢測體之間 進行近似。6. 根據(jù)權(quán)利要求1所述的機械手系統(tǒng)，其中， 所述形狀計算部以包含相鄰的3個以上的所述被檢測體的方式設(shè)定各所述片段，根據(jù) 各所述被檢測體的三維位置，通過曲線擬合來求出表示各所述片段內(nèi)的所述插入部的形狀 的曲線的多項式。7. -種機械手的控制方法，其中， 劃分為沿著軟性的插入部的長度方向排列的多個片段來計算該插入部的形狀， 根據(jù)計算出的所述插入部的形狀，對配置在該插入部的基端側(cè)的驅(qū)動部針對配置在所 述插入部的前端的機械手的驅(qū)動進行控制。
【文檔編號】A61B1/00GK105979850SQ201580008237
【公開日】2016年9月28日
【申請日】2015年2月18日
【發(fā)明人】畠山直也, 飯?zhí)镅琶? 若井浩志
【申請人】奧林巴斯株式會社