董帥帥, 辛紹杰
(上海電機(jī)學(xué)院 電氣學(xué)院, 上海 201306)
微創(chuàng)手術(shù)器械穿過(guò)在體表切開(kāi)的小切口探入體內(nèi)[1],在狹小的空間里完成多種動(dòng)作,微創(chuàng)手術(shù)具有手術(shù)質(zhì)量高、術(shù)后并發(fā)癥少等優(yōu)點(diǎn)[2]。本文在一種手術(shù)機(jī)器人結(jié)構(gòu)的基礎(chǔ)上采用DH建模方法完成對(duì)手術(shù)機(jī)器人的建模,本文研究的手術(shù)器械共有7個(gè)自由度,其中,3個(gè)自由度由醫(yī)生的手腕部分提供,剩下4個(gè)自由度由手術(shù)器械的驅(qū)動(dòng)電動(dòng)機(jī)提供,驅(qū)動(dòng)電動(dòng)機(jī)選用步進(jìn)電動(dòng)機(jī),步進(jìn)電動(dòng)機(jī)具有誤差不累加的優(yōu)點(diǎn)[3-4]。對(duì)于給定型號(hào)的步進(jìn)電動(dòng)機(jī),其運(yùn)轉(zhuǎn)性能主要受驅(qū)動(dòng)電路控制策略影響[5-7]。在建模的基礎(chǔ)上完成手術(shù)器械的動(dòng)作仿真、姿態(tài)穩(wěn)定、力反饋仿真和精度分析。
手術(shù)場(chǎng)景中醫(yī)生的手部抖動(dòng)影響手術(shù)質(zhì)量[8],本文使用加速度傳感器進(jìn)行機(jī)器人姿態(tài)保持的研究,探索如何在設(shè)計(jì)上能夠緩解手抖造成的誤差。將抖動(dòng)當(dāng)作一種不規(guī)律的外部擾動(dòng),通過(guò)傳感器采樣數(shù)據(jù)比對(duì)計(jì)算補(bǔ)償誤差,研究采樣對(duì)精度的影響。
Okamura等[9]提出的經(jīng)驗(yàn)穿刺力模型,將穿刺過(guò)程的受力分為3個(gè)不同階段,實(shí)驗(yàn)表明,單位長(zhǎng)度所受摩擦力隨進(jìn)針?biāo)俣仍黾佣黾?,而針尖處的峰值幾乎不隨進(jìn)針?biāo)俣榷淖?。常用的組織建模方法有質(zhì)點(diǎn)彈簧模型和有限元模型[10],兩種模型根據(jù)適用的情形有各自的特點(diǎn)[11]。本文研究的是穿刺過(guò)程中力的變化以及手術(shù)器械前端的運(yùn)動(dòng)控制,為了得到這樣的數(shù)據(jù),根據(jù)人體組織的特性[12],將組織看成一群細(xì)胞質(zhì)點(diǎn)的集群,密度不一的質(zhì)點(diǎn)集群對(duì)外表現(xiàn)出不一樣的力學(xué)特性,用生成隨機(jī)點(diǎn)的方法模擬同一組織內(nèi)部小范圍的稀疏變化。本文將穿刺前端抽象成半徑0.5 cm的球體,球體在1 cm3空間內(nèi)沿著某條軌跡運(yùn)動(dòng),以過(guò)程中框住的點(diǎn)數(shù)來(lái)模擬受力F的大小,根據(jù)恒功率條件下負(fù)載特性[13]關(guān)系通過(guò)一定的函數(shù)映射來(lái)模擬轉(zhuǎn)速。最后由神經(jīng)網(wǎng)絡(luò)對(duì)穿刺過(guò)程的特征點(diǎn)進(jìn)行分類(lèi),根據(jù)分類(lèi)結(jié)果進(jìn)行運(yùn)動(dòng)控制,驗(yàn)證控制的精度。
本文設(shè)計(jì)的手術(shù)器械如圖1(a)所示,該器械可以實(shí)現(xiàn)4個(gè)自由度的運(yùn)動(dòng),前端的旋轉(zhuǎn)和搖擺以及執(zhí)行器械的旋轉(zhuǎn)和夾取。仿真中,手術(shù)器械還需添加手腕部分的3個(gè)自由度,一共是7個(gè)自由度。模型的尺寸做了簡(jiǎn)化處理,并不與實(shí)際尺寸相對(duì)應(yīng)。使用Matlab機(jī)器人工具箱建立的手術(shù)器械模型如圖1(b)所示。
圖1 手術(shù)器械模型及建模圖
DH建模方法是常用的機(jī)器人建模方法,將兩個(gè)機(jī)械臂抽象成兩個(gè)連桿,兩個(gè)連桿之間通過(guò)齊次變換矩陣相乘獲得坐標(biāo)系之間的相對(duì)位置和姿態(tài)。如果機(jī)械臂有多組,可以使用齊次變換矩陣連續(xù)相乘的方法,得到末端相對(duì)于基坐標(biāo)系的姿態(tài)和位置。
根據(jù)DH建模方法,建模原則:① 將兩個(gè)關(guān)節(jié)軸方向都定為第i-1個(gè)坐標(biāo)系和第i個(gè)坐標(biāo)系的z軸;② 將兩個(gè)關(guān)節(jié)軸之間公垂線(xiàn)方向定為第i個(gè)坐標(biāo)系的x軸,而在公垂線(xiàn)第i個(gè)關(guān)節(jié)軸上的交點(diǎn)作為原點(diǎn)。同理,第i-1個(gè)坐標(biāo)系的x軸和原點(diǎn)由之前兩個(gè)坐標(biāo)系公垂線(xiàn)共同確定;③ 在已經(jīng)確定坐標(biāo)系z(mì)軸和x軸的情況下,坐標(biāo)系的y軸通過(guò)右手定則來(lái)確定。
根據(jù)標(biāo)準(zhǔn)DH參數(shù)描述法,第i-1個(gè)連桿坐標(biāo)系相比于第i個(gè)坐標(biāo)系的4階變換矩陣如下:
(1)
式中:c為cos,s為sin;ai-1為連桿長(zhǎng)度,即關(guān)節(jié)軸i-1和關(guān)節(jié)i之間公垂線(xiàn)的長(zhǎng)度;αi-1為連桿轉(zhuǎn)角,即第i-1個(gè)關(guān)節(jié)和第i個(gè)關(guān)節(jié)軸之間的夾角;di為連桿偏距,即沿兩個(gè)相鄰連桿公共軸線(xiàn)方向的距離;θi為關(guān)節(jié)角,即兩個(gè)相鄰連桿繞公共軸線(xiàn)旋轉(zhuǎn)的夾角。
(2)
由式(2)可計(jì)算任意時(shí)刻機(jī)器人頂端相對(duì)于基坐標(biāo)系的空間位置。模型建立之后,a、α是定值不會(huì)改變,θi的改變決定機(jī)器人關(guān)節(jié)姿態(tài)的變化,di的變化決定滑動(dòng)的距離。
為了體現(xiàn)機(jī)器人在實(shí)際使用時(shí)的情況,建模的時(shí)候共用到了7個(gè)連桿模擬手持時(shí)的7個(gè)自由度的運(yùn)動(dòng),在Matlab中建立的機(jī)器人模型主要參數(shù)如表1所示。其中,L1~L7為連桿編號(hào),模型的L1為滑動(dòng)關(guān)節(jié),L2到L7為旋轉(zhuǎn)關(guān)節(jié);P、R為連桿類(lèi)型,P為滑動(dòng)關(guān)節(jié),R為旋轉(zhuǎn)關(guān)節(jié);offset為初始偏移角度;q1~q7為Matlab機(jī)器人模型參數(shù)。
表1 手術(shù)器械建模參數(shù)表
手術(shù)器械模型在進(jìn)行直線(xiàn)、曲線(xiàn)、圓形軌跡運(yùn)動(dòng)時(shí),模型前端的主要參數(shù)變化不同,如圖2~4所示。
圖2 直線(xiàn)軌跡手術(shù)器械運(yùn)動(dòng)參數(shù)變化
圖3 曲線(xiàn)軌跡手術(shù)器械運(yùn)動(dòng)參數(shù)變化
圖4 圓形軌跡手術(shù)器械運(yùn)動(dòng)參數(shù)變化
由圖2~4可知,雖然手術(shù)器械模型前端的運(yùn)動(dòng)軌跡以及參與運(yùn)動(dòng)的主要連桿L關(guān)節(jié)的角度變化,但運(yùn)動(dòng)軌跡數(shù)據(jù)變化平滑,不存在突變,表明該模型建立恰當(dāng)。
為了方便問(wèn)題研究,本文先對(duì)研究的內(nèi)容做簡(jiǎn)單定義,使研究具有前提條件。
(1) 穩(wěn)定的含義是手術(shù)器械的某個(gè)參考點(diǎn)相對(duì)于組織是位置不變的或者速度是恒定的。
(2) 醫(yī)生進(jìn)行手術(shù)時(shí)輕微的手抖是非自愿行為,手抖主要集中在手腕部分,醫(yī)生可以較好地控制上臂,或者采取外加輔助設(shè)備的方式幫助醫(yī)生獲得手臂手肘關(guān)節(jié)的相對(duì)穩(wěn)定性。
(3) 不開(kāi)啟姿態(tài)穩(wěn)定功能時(shí),默認(rèn)醫(yī)生可以在自主意志下完成手術(shù)動(dòng)作,醫(yī)生可以很好地把握手術(shù)節(jié)奏控制誤差等情況。
(4) 開(kāi)啟姿態(tài)穩(wěn)定功能,默認(rèn)醫(yī)生具有自主意志可以完成幅度較大的動(dòng)作,而對(duì)于精細(xì)動(dòng)作,醫(yī)生不可以憑借自主意志完成動(dòng)作,會(huì)出現(xiàn)不可預(yù)測(cè)的手抖造成誤差,此時(shí)精細(xì)動(dòng)作的完成交給手術(shù)器械,在這里從邏輯上界定了誤差與自主意志動(dòng)作的識(shí)別,由醫(yī)生選擇是否開(kāi)啟姿態(tài)穩(wěn)定功能。
以上述說(shuō)明為基礎(chǔ),定義3個(gè)坐標(biāo)系:① 手術(shù)部位坐標(biāo)系O。固定坐標(biāo)系不可動(dòng),對(duì)應(yīng)到實(shí)際情況是進(jìn)行手術(shù)的部位。② 手肘坐標(biāo)系O1。醫(yī)生手持機(jī)器人的上臂關(guān)節(jié),將陀螺儀以恰當(dāng)?shù)姆绞桨惭b在手肘部位。③ 手術(shù)器械坐標(biāo)系O2。將陀螺儀以恰當(dāng)?shù)姆绞桨惭b在手術(shù)器械上。
手術(shù)部位坐標(biāo)系虛擬空間的組成如圖5所示,O、O1、O2為坐標(biāo)軸方向一致的3個(gè)坐標(biāo)系,分別代表以標(biāo)準(zhǔn)姿態(tài)(遵循右手準(zhǔn)則建立的空間坐標(biāo)系且z軸垂直于地平面)放置于手術(shù)部位、手術(shù)器械前端以及醫(yī)生手肘部分的陀螺儀建立的虛擬坐標(biāo)系。在靜止條件下,陀螺儀和加速度計(jì)數(shù)據(jù)不會(huì)發(fā)生變化;在手術(shù)場(chǎng)景下,坐標(biāo)系O(手術(shù)部位)是穩(wěn)定不動(dòng)的,手肘坐標(biāo)系O1與手腕坐標(biāo)系O2是醫(yī)生進(jìn)行手術(shù)動(dòng)作時(shí)的運(yùn)動(dòng)坐標(biāo)系。根據(jù)6軸陀螺儀可以提供3個(gè)軸向上的加速度以及3個(gè)軸向上的陀螺儀數(shù)據(jù)可以計(jì)算出坐標(biāo)系O1與坐標(biāo)系O2相對(duì)于坐標(biāo)系O的運(yùn)動(dòng)方向以及距離。根據(jù)前提
圖5 手術(shù)部位坐標(biāo)系虛擬空間圖
假設(shè),在開(kāi)啟姿態(tài)穩(wěn)定功能之后,坐標(biāo)系O1相對(duì)于坐標(biāo)系O2的運(yùn)動(dòng)就是手術(shù)前端的誤差。
手術(shù)器械在空間中的運(yùn)動(dòng)可以分解為3個(gè)軸向上的運(yùn)動(dòng),同樣,誤差動(dòng)作在空間中也可以分為3個(gè)軸向上的運(yùn)動(dòng)。設(shè)陀螺儀和加速度計(jì)提供的6個(gè)參數(shù),ax、ay、az表示由加速度計(jì)得到的坐標(biāo)系在x、y、z3個(gè)方向上的加速度,θx、θy、θz表示由陀螺儀得到的機(jī)器人姿態(tài)相對(duì)于初始坐標(biāo)系x、y、z軸逆時(shí)針?lè)较蛏掀频慕嵌取?/p>
根據(jù)以上條件,在開(kāi)啟姿態(tài)穩(wěn)定功能后,由手抖造成的手術(shù)器械誤差實(shí)際上是坐標(biāo)系O1相對(duì)于坐標(biāo)系O2的位移。設(shè)根據(jù)陀螺儀和加速度計(jì)得到坐標(biāo)系O1的運(yùn)動(dòng)參數(shù)為Ax、Ay、Az,坐標(biāo)系O2的運(yùn)動(dòng)參數(shù)為Bx、By、Bz,加速度計(jì)從0時(shí)刻開(kāi)始采樣,得到間隔為t的t0,t1,…,tn采樣序列,獲得O1坐標(biāo)系x坐標(biāo)軸上的加速度序列Ax0,Ax1,…,Axn,獲得O1坐標(biāo)系y坐標(biāo)軸上的加速度序列Ay0,Ay1,…,Ayn,獲得的O1坐標(biāo)系z(mì)坐標(biāo)軸上的加速度序列Az0,Az1,…,Azn。
計(jì)算可得O1坐標(biāo)系在0~tn時(shí)間內(nèi)x軸的位移為
(3)
同理可得,在0~tn時(shí)間內(nèi)y軸的位移為
(4)
在0~tn時(shí)間內(nèi)z軸的位移為
(5)
同理可得,O2坐標(biāo)系在0~tn時(shí)間內(nèi)x軸的位移為
(6)
在0~tn時(shí)間內(nèi)y軸的位移為
(7)
在0~tn時(shí)間內(nèi)z軸的位移為
(8)
由式(3)~式(8)可計(jì)算O1坐標(biāo)系相對(duì)于O2坐標(biāo)系的位移為
Δxa=Δx1-Δx2
Δya=Δy1-Δy2
Δza=Δz1-Δz2
這3個(gè)量即機(jī)器人前端相對(duì)于醫(yī)生手肘部的抖動(dòng)誤差,以上不計(jì)入O1坐標(biāo)系相對(duì)于O2坐標(biāo)系發(fā)生角度偏移的情況,如果加入O1坐標(biāo)系相對(duì)于O2坐標(biāo)系偏移角度,即
Δθx=θx1-θx2
Δθy=θy1-θy2
Δθz=θz1-θz2
造成的x軸上總誤差為
[(1-cos Δθz)+sinθy]
(9)
造成的y軸上總誤差為
[(1-cos Δθx)+sinθz]
(10)
造成的z軸上總誤差為
[(1-cos Δθy)+sinθx]
(11)
式中:θx1、θx2、θy1、θy2、θz1、θz2均為O1、O2坐標(biāo)系相對(duì)于標(biāo)準(zhǔn)姿態(tài)的逆時(shí)針旋轉(zhuǎn)角度。
由于手術(shù)動(dòng)作具有明顯的非線(xiàn)性加減速過(guò)程,反映到加速度的數(shù)據(jù)上就是在0附近波動(dòng)變化,故在3個(gè)軸向上選取sinX函數(shù)作為加速度描述曲線(xiàn),X可以隨意選擇取值范圍,每個(gè)軸向上的X可以取值不同,但是要保證3個(gè)軸向上X的寬度一致,因?yàn)樵谙嗤蓸宇l率下X寬度不一致會(huì)導(dǎo)致采樣點(diǎn)個(gè)數(shù)不一致。sinX函數(shù)穿越0值的次數(shù)表示手術(shù)動(dòng)作的次數(shù)。
圖6給出了采樣頻率為80、90和100 Hz計(jì)算出來(lái)的抖動(dòng)誤差,可以看出采樣頻率不同,抖動(dòng)誤差結(jié)果也不同,隨著采樣頻率的升高,抖動(dòng)誤差越來(lái)越小。由于抖動(dòng)誤差是隨機(jī)生成的,故每次的抖動(dòng)誤差曲線(xiàn)形狀有所不同,但是都處在μm量級(jí)。
圖6 采樣頻率提高對(duì)抖動(dòng)誤差的影響
圖7給出了固定采樣頻率100 Hz條件下,O1相對(duì)于O2坐標(biāo)系加速度誤差范圍從10%增加到20%的過(guò)程中抖動(dòng)誤差的變化??梢钥闯觯S著加速度誤差范圍的增加,抖動(dòng)誤差也會(huì)波動(dòng)上升,但此時(shí)相應(yīng)提高采樣頻率可以有效降低誤差波動(dòng)范圍增大帶來(lái)的影響。圖8給出了極端抖動(dòng)情形下采樣頻率對(duì)抖動(dòng)誤差影響,加速度誤差范圍增加到50%,采樣頻率從100 Hz增加到300 Hz后,仍能維持ε在μm量級(jí)。
圖7 加速度誤差范圍增加對(duì)抖動(dòng)誤差的影響
圖8 極端抖動(dòng)情形下采樣頻率增加對(duì)ε的影響
對(duì)比圖7和圖8兩種情況可以得出提高采樣頻率能很好降低抖動(dòng)誤差的結(jié)論,同時(shí),在誤差極端增大情況下提高采樣頻率仍然可以維持整體的誤差范圍,對(duì)應(yīng)于手術(shù)器械的補(bǔ)償機(jī)構(gòu)在x、y、z軸向上輸出-Δx、-Δy、-Δz的位移即可完成對(duì)手術(shù)器械動(dòng)作誤差的補(bǔ)償。
現(xiàn)有常用加速度陀螺儀傳感器件,例如MPU6050默認(rèn)采樣頻率可以達(dá)到100 Hz,通過(guò)對(duì)傳感器的設(shè)置還可以獲得更高的采樣頻率,與本文的仿真參數(shù)一致,可知該方法具有可行性。
壓力值增加的程度和電動(dòng)機(jī)值減小的程度這兩者之間變化的劇烈程度構(gòu)成力反饋的標(biāo)準(zhǔn),這就產(chǎn)生了對(duì)兩個(gè)參數(shù)進(jìn)行數(shù)據(jù)歸類(lèi)劃分的需求,需要進(jìn)行分類(lèi)的情形,如表2所示。
表2 力反饋參數(shù)界定邏輯表
表2對(duì)應(yīng)到實(shí)際物理情況,情形1與情形4是不存在的,因?yàn)椴环想妱?dòng)機(jī)的負(fù)載特性,故只需要研究情形2與情形3。
使用Matlab在1 cm3的空間中隨機(jī)生成200 000組(x,y,z)坐標(biāo),其中100 000個(gè)點(diǎn)的坐標(biāo)在0≤x≤1,0≤y≤1,0≤z≤1隨機(jī)分布,100 000個(gè)點(diǎn)的坐標(biāo)在0≤x≤0.5,0≤y≤1,0≤z≤1隨機(jī)分布,如圖9所示,這樣在1 cm3中會(huì)形成兩個(gè)點(diǎn)密度不同的區(qū)域代表兩種組織。
圖9 組織仿真
計(jì)算可得點(diǎn)密度較高的區(qū)域?yàn)? 570.8 mm-3,取名組織A,神經(jīng)網(wǎng)絡(luò)對(duì)應(yīng)的輸出為(0,1);點(diǎn)密度較低的區(qū)域?yàn)?23.6 mm-3,取名組織B,神經(jīng)網(wǎng)絡(luò)對(duì)應(yīng)的輸出為(1,0)。球體內(nèi)部點(diǎn)的個(gè)數(shù)為電動(dòng)機(jī)的負(fù)載受到的阻力,電動(dòng)機(jī)轉(zhuǎn)速是指機(jī)器人末端在進(jìn)行動(dòng)作時(shí)對(duì)應(yīng)的電動(dòng)機(jī)轉(zhuǎn)速。根據(jù)恒功率負(fù)載下電動(dòng)機(jī)負(fù)載與轉(zhuǎn)速的關(guān)系P=F×v,取P=7 854 MW。圖10(a)所示為器械末端針尖在組織A中沿著(0.25,0.1,0.5)到(0.25,0.9,0.5)直線(xiàn)軌跡上80個(gè)采樣點(diǎn)的轉(zhuǎn)速與采集到的點(diǎn)數(shù)據(jù)以及映射為壓力傳感器探測(cè)值電動(dòng)機(jī)的轉(zhuǎn)速值。圖10(b)所示為器械末端針尖在組織B中沿著(0.75,0.1,0.5)到(0.75,0.9,0.5)直線(xiàn)軌跡上80個(gè)采樣點(diǎn)采集到的點(diǎn)數(shù)據(jù)以及映射為壓力傳感器探測(cè)值電動(dòng)機(jī)的轉(zhuǎn)速值。
圖10 組織A、B特征點(diǎn)數(shù)據(jù)
將圖10得到的兩組數(shù)據(jù)送入兩輸入兩輸出具有3層隱含網(wǎng)絡(luò)的BP神經(jīng)網(wǎng)絡(luò)進(jìn)行訓(xùn)練,訓(xùn)練的結(jié)果作為組織標(biāo)準(zhǔn)模型,將(0.1,0.5,0.5)~(0.9,0.5,0.5)軌跡上的特征點(diǎn)數(shù)據(jù)送入標(biāo)準(zhǔn)模型進(jìn)行仿真,仿真結(jié)果如圖11所示。圖11(a)為特征點(diǎn)的神經(jīng)網(wǎng)絡(luò)分類(lèi)結(jié)果,圖11(b)為分類(lèi)前對(duì)特征點(diǎn)的歸一化處理結(jié)果。
圖11 特征點(diǎn)分類(lèi)結(jié)果
運(yùn)動(dòng)控制器的位移隨著神經(jīng)網(wǎng)絡(luò)的輸出進(jìn)行變化的過(guò)程,在t=10 s時(shí),神經(jīng)網(wǎng)絡(luò)控制器判斷穿刺到不同的組織,控制器采用PID控制使手術(shù)器械末端回歸穿刺初始點(diǎn),末端位移變化如圖12所示,可知控制器可以根據(jù)神經(jīng)網(wǎng)絡(luò)的反饋進(jìn)行力反饋動(dòng)作。
圖12 手術(shù)器械末端的位移
采用步進(jìn)電動(dòng)機(jī)細(xì)分技術(shù)[14-15],對(duì)于二相混合式步進(jìn)電動(dòng)機(jī)整步轉(zhuǎn)動(dòng)時(shí),其機(jī)械結(jié)構(gòu)決定的固有步距角為
(12)
式中:z為轉(zhuǎn)子齒數(shù);c為相數(shù),決定于通電方式。
對(duì)步距角為1.8°的步進(jìn)電動(dòng)機(jī)進(jìn)行64細(xì)分[16],最小步進(jìn)角度可以達(dá)到0.028 125°,手術(shù)器械的前端長(zhǎng)度取30 mm,末端執(zhí)行器由步進(jìn)電動(dòng)機(jī)直接驅(qū)動(dòng)的情形下,步進(jìn)電動(dòng)機(jī)接收一個(gè)脈沖末端執(zhí)行器動(dòng)作的弧長(zhǎng)為14.7 μm,可以實(shí)現(xiàn)比較精確的位移控制。仿真出現(xiàn)的結(jié)果有3個(gè)相對(duì)孤立的特征點(diǎn),對(duì)這些特征點(diǎn)的劃分不同,控制器最大會(huì)有3步的誤差,此時(shí)手術(shù)器械末端最大的弧度誤差為44.1 μm。
本文以Matlab機(jī)器人工具箱建立了機(jī)器人模型,驗(yàn)證了機(jī)器人執(zhí)行較復(fù)雜的動(dòng)作時(shí)參考點(diǎn)的軌跡特性,提出了一種基于加速度采樣的抖動(dòng)補(bǔ)償方法,驗(yàn)證該方法對(duì)誤差的補(bǔ)償作用。使用隨機(jī)生成的數(shù)據(jù)模擬組織,并在組織中按照某種軌跡生成特征數(shù)據(jù),這些數(shù)據(jù)作為神經(jīng)網(wǎng)絡(luò)的學(xué)習(xí)基礎(chǔ),通過(guò)對(duì)神經(jīng)網(wǎng)絡(luò)控制器的仿真驗(yàn)證了力反饋的正確性與可行性。