改進(jìn)人工勢場的手術(shù)機(jī)器人位姿規(guī)劃

郝林佳,葉 燦,都書鮮,王 宇,武 博,張 楠

(首都醫(yī)科大學(xué)生物醫(yī)學(xué)工程學(xué)院,北京 100069;首都醫(yī)科大學(xué)臨床生物力學(xué)應(yīng)用基礎(chǔ)研究北京市重點(diǎn)實(shí)驗(yàn)室,北京 100069)

1 引言

近年來,外科手術(shù)向著微創(chuàng)化的方向發(fā)展,特別是手術(shù)機(jī)器人在臨床中的應(yīng)用,不僅節(jié)省了手術(shù)時間,減輕了醫(yī)生工作量,同時也提高了手術(shù)精度,并將對患者的損傷減少到最小,給醫(yī)生和患者均帶來了較大益處.另一方面,由于機(jī)器人執(zhí)行手術(shù),它處理對象的特性與工業(yè)應(yīng)用的情況有較大差異,因此要求手術(shù)機(jī)器人具有十分細(xì)致、柔順的操作性能以及安全的應(yīng)急系統(tǒng).對于術(shù)中執(zhí)行手術(shù)的機(jī)械臂而言,需要避免機(jī)械臂末端執(zhí)行器以及各連桿與器械等運(yùn)動對象發(fā)生碰撞乃至接觸,以免干預(yù)手術(shù)的進(jìn)行,影響手術(shù)精度.

目前為止,國內(nèi)外對手術(shù)機(jī)器人的研究基本都是主從式控制,避免了外界物體對機(jī)器人路徑的干擾問題,如國外的達(dá)芬奇系統(tǒng)[1]、Magellan手術(shù)系統(tǒng)[2]以及國內(nèi)的妙手機(jī)器人[3]、神經(jīng)外科機(jī)器人Remebot[4]等.面向未來非主從式醫(yī)療機(jī)器人研發(fā),避障變得尤為重要.本研究以工業(yè)機(jī)器人為起點(diǎn)提出一種位姿規(guī)劃算法,解決了手術(shù)機(jī)器人的安全避障問題.

常用的機(jī)械臂避障算法主要包括改進(jìn)逆運(yùn)動學(xué)求解法[5–6]、自由空間法[7]、遺傳算法[8]、A*算法[9]等,其中:逆運(yùn)動學(xué)求解法無法有效解決奇異點(diǎn)問題;自由空間法無法適用于空間冗余機(jī)械臂;A*算法和遺傳算法均為搜索算法,耗時長.另外也有學(xué)者提出基于自適應(yīng)權(quán)重的優(yōu)化方法[10]以及基于預(yù)測窗的避障避碰算法[11],兼顧了機(jī)器人的避障和運(yùn)動的平穩(wěn)性,但前者是在關(guān)節(jié)空間內(nèi)進(jìn)行規(guī)劃,而后者針對的是移動機(jī)器人,因此在本文的場景中并不適用.

人工勢場法[12]是一種高效的局部路徑規(guī)劃算法,其基本思想是在機(jī)器人工作空間或執(zhí)行空間構(gòu)建虛擬勢場,在目標(biāo)點(diǎn)設(shè)置作用于全局的引力場,在障礙物設(shè)置作用于局部范圍的斥力場,機(jī)器人在兩場的共同作用下到達(dá)目標(biāo)并避開沿途障礙.它結(jié)構(gòu)簡單,運(yùn)用方便,可重復(fù)性強(qiáng),在動態(tài)避障以及目標(biāo)追蹤方面應(yīng)用較為廣泛.由于是局部路徑規(guī)劃算法,因此也容易出現(xiàn)局部極小值問題.

為了解決局部極小值問題,不少學(xué)者引入虛擬目標(biāo)點(diǎn)[13–16],在極小值附近添加虛擬障礙物[17]或者在和速度的法方向添加加速度[18],其中文獻(xiàn)[13–16]所提算法對于障礙物最近距離的計算不準(zhǔn)確,同時需要進(jìn)行搜索和遍歷,耗時長;文獻(xiàn)[17–18]中的算法雖然可以進(jìn)行動態(tài)避障和跟蹤,但是需要計算偽逆矩陣,不能很好地解決奇異值問題.并且除文獻(xiàn)[13,16]外,上述算法針對的是對末端執(zhí)行器位置的規(guī)劃,并沒有考慮姿態(tài).而文獻(xiàn)[13,16]提出的算法雖然可以規(guī)劃姿態(tài),但其需要事先計算機(jī)械臂目標(biāo)位置的各關(guān)節(jié)角,而冗余機(jī)械臂的運(yùn)動學(xué)逆解又會涉及最優(yōu)解、奇異值等問題,對于動態(tài)障礙物而言,事先規(guī)劃的關(guān)節(jié)位置可能并不安全.

為此本文提出改進(jìn)人工勢場的手術(shù)機(jī)器人位姿規(guī)劃算法,首先將末端執(zhí)行器兩端位置信息作用于引力函數(shù)中,解決了傳統(tǒng)人工勢場算法不能約束姿態(tài)的問題;接著在避障方面,相對于其它方法,通過GJK算法計算凸體障礙物與機(jī)械臂之間的實(shí)時距離[19]會更加準(zhǔn)確,提高了避障精度;然后,將末端執(zhí)行器目標(biāo)位姿與當(dāng)前位姿之間的差距作為自適應(yīng)步長中的變量,讓機(jī)械臂運(yùn)動更加平緩高效;最后,在機(jī)械臂陷入局部極小值時,引入動態(tài)引力常數(shù),使機(jī)械臂逃離局部最優(yōu),以應(yīng)對手術(shù)場景的多種情況.

2 環(huán)境建模

2.1 運(yùn)動學(xué)模型

本文采用德國庫卡公司的LBR iiwa 7七自由度冗余機(jī)械臂作為研究對象,加入末端執(zhí)行器后如圖1所示,采用標(biāo)準(zhǔn)D–H法建立機(jī)械臂運(yùn)動學(xué)模型,可得該機(jī)械臂的標(biāo)準(zhǔn)D–H模型參數(shù),如表1所示,機(jī)械臂正運(yùn)動學(xué)模型可用式(1)表示為

表1 機(jī)械臂D–H模型參數(shù)Table 1 The D–H parameter of the robot

圖1 庫卡LBR iiwa 7七自由度冗余機(jī)械臂Fig.1 KUKA LBR iiwa 7 7-DOF redundant robot

式中n表示機(jī)械臂關(guān)節(jié)數(shù),表示相鄰連桿坐標(biāo)系之間的變換矩陣,帶入標(biāo)準(zhǔn)D–H模型參數(shù)便可得機(jī)械臂末端在工作空間中相對世界坐標(biāo)系的位姿.

機(jī)器人學(xué)中,需要研究機(jī)器人末端執(zhí)行器速度和關(guān)節(jié)速度之間的映射關(guān)系,而反映兩者之間的關(guān)系的變換矩陣稱為雅可比矩陣,用式(2)表示為

式中:=[vxvyvzωxωyωz]T,[vxvyvz]為機(jī)械臂末端笛卡爾線速度,[ωxωyωz]為機(jī)械臂末端笛卡爾角速度;J(θ)∈R6×n為雅克比矩陣,代表機(jī)械臂各個關(guān)節(jié)速度.雅可比矩陣不僅揭示了速度之間的關(guān)系,還表示了力的傳遞關(guān)系,同時為關(guān)節(jié)力矩的確定以及不同坐標(biāo)系之間的速度,加速度的變換提供了計算上的方便.

2.2 障礙物模型

由于機(jī)械臂的工作環(huán)境不同,有的機(jī)械臂面對的是提前規(guī)劃好的規(guī)則障礙物,例如工業(yè)機(jī)械臂,然而更多的時候,機(jī)械臂運(yùn)行環(huán)境中的障礙物都是不規(guī)則形狀,想要精確建模需要花費(fèi)大量時間,例如球體,AABB,OBB,k-Dops[20]等包圍盒.但上述的建模的方法會在很大程度上增大障礙物的體積,從而導(dǎo)致機(jī)械臂的工作空間減小,因此本文采用快速凸包算法[21]盡可能少的增大障礙物的體積,保證工作空間的同時不會過多影響機(jī)械臂的實(shí)時控制,如圖2所示.

圖2 障礙物凸體化過程Fig.2 The process of bulging obstacles

算法核心包括兩方面:一方面是初始點(diǎn)的選取;另一方面是凸點(diǎn)的計算.

選取的初始點(diǎn)有3個,首先根據(jù)障礙物的頂點(diǎn)數(shù)據(jù),選取z坐標(biāo)最大的頂點(diǎn)pa(xa,ya,za),計算pa與剩下所有頂點(diǎn)在水平面的夾角θx,θy,使θx,θy最小的兩個點(diǎn)pb(xb,yb,zb),pc(xc,yc,zc)與pa共同組成平面f1.θx,θy用式(3)表示為

atan 2(x,y)定義如式(4)所示:

接著遍歷剩余頂點(diǎn),與初始頂點(diǎn)中的任意兩點(diǎn)組成新的平面f2,計算平面f1與平面f2的夾角,將使θf最大的面中的3個頂點(diǎn)重排序后作為新的初始點(diǎn),以此類推,直到找出所有凸點(diǎn).兩平面的夾角可用式(5)表示:

2.3 碰撞檢測模型

機(jī)械臂與障礙物間的碰撞檢測采用GJK算法,根據(jù)圖1建立的機(jī)械臂D–H模型,將連桿等效為以5條線段為軸線的等效圓柱面,圖3(a)為連桿等效圓柱面示意圖,pi(xi,yi,zi)為第i個關(guān)節(jié)的坐標(biāo),可由正運(yùn)動學(xué)計算求出,ri為等效半徑,則等效圓柱面可用方程(6)表示為

圖3 機(jī)械臂各連桿等效圓柱面Fig.3 The equivalent cylinders of links of the robot

式中:nx=xi+1?xi,ny=yi+1?yi,nz=zi+1?zi,(x,y,z)為機(jī)械臂第i個連桿等效圓柱面上點(diǎn)的坐標(biāo),(pix,piy,piz)為線段pipi+1中的任意一點(diǎn),滿足:pix=xi+tnx,piy=yi+tny,piz=zi+tnz,0 ≤t≤1.

由于圓柱本身就是凸體,圓柱上下底面圓周上的點(diǎn)就可以代表整個圓柱在空間中的位置,以此減少圓柱面點(diǎn)的個數(shù),如圖3(b)所示.

另一方面,障礙物的位置信息由深度相機(jī)給出,經(jīng)過快速凸包算法處理后可以將其當(dāng)做凸體處理.因此,可以獲取任意時刻空間中兩個凸體頂點(diǎn)信息.GJK算法的核心思想是將兩個凸體的頂點(diǎn)依次相減得到閔科夫斯基差,如果兩個物體相交,則閔科夫斯基差將包含原點(diǎn),否則計算閔科夫斯基差形成的多面體與原點(diǎn)的最近距離以及最近距離點(diǎn),此距離即為兩個凸體之間的最近距離.

需要注意的是,在每次迭代中,方向向量要朝著原點(diǎn)方向迭代,保證單純形逐漸向原點(diǎn)靠近.以二維凸體為例,用式(7)表示為

式中:A,B為單純形中的兩點(diǎn),O為坐標(biāo)原點(diǎn).

當(dāng)單純形迭代到距離原點(diǎn)最近時,需要根據(jù)單純形中距離原點(diǎn)最近的點(diǎn)E計算出原凸體中對應(yīng)的最近距離點(diǎn)F,G,它們之間的關(guān)系可用方程(8)表示為

式中:C,D分別為單純形中距離原點(diǎn)最近的邊上的兩點(diǎn),C由凸體1中的點(diǎn)C1減凸體2中的點(diǎn)C2所得,D由凸體1中的點(diǎn)D1減凸體2中的點(diǎn)D2所得,λ1≥0,λ2≥0.

3 改進(jìn)人工勢場算法

3.1 勢場函數(shù)的建立

由于將障礙物從笛卡爾空間映射到機(jī)械臂關(guān)節(jié)空間計算量大且較難描述,為保證機(jī)械臂能在線實(shí)時規(guī)劃,并避開動/靜態(tài)障礙物,本文直接在笛卡爾空間構(gòu)造勢場模型.考慮到機(jī)械臂任務(wù)只由其末端執(zhí)行器執(zhí)行,并不關(guān)心執(zhí)行任務(wù)時其連桿所在位置,而任何情況下都不允許機(jī)械臂與障礙物碰撞.因此,吸引力Fatt通過目標(biāo)在機(jī)械臂末端產(chǎn)生,排斥力Frej通過障礙物在機(jī)械臂與障礙物的最近距離點(diǎn)處產(chǎn)生,機(jī)械臂在兩種虛擬力的共同作用下運(yùn)動,到達(dá)目標(biāo)并避開障礙.需要注意的是,末端執(zhí)行器是按照規(guī)劃姿態(tài)到達(dá)目標(biāo)位置,傳統(tǒng)的人工勢場算法只能對末端執(zhí)行器的位置進(jìn)行約束,不能兼顧姿態(tài),本文通過對執(zhí)行器兩端的位置分別進(jìn)行限制,解決了這一問題.

在笛卡爾空間中建立引力勢能函數(shù)為

式中:Ka為引力常數(shù),pgoal1,pgoal2為末端執(zhí)行器兩端的目標(biāo)位置,θ為機(jī)械臂運(yùn)動過程中各關(guān)節(jié)的角度,p1(θ),p2(θ)為機(jī)械臂運(yùn)動過程中末端執(zhí)行器兩端的實(shí)時位置,如圖1所示.則對應(yīng)的引力函數(shù)為

在笛卡爾空間中,障礙物凸包與機(jī)械臂連桿等效圓柱面間的斥力勢能函數(shù)為

式中:Kr為斥力常數(shù),i(i=1,2,···n)為機(jī)械臂連桿序號,k(k=1,2,···m)為障礙物的序號,m為空間中障礙物的總個數(shù),表示連桿i與障礙物k凸包表面的最近距離,d0為斥力勢場作用范圍,為障礙物k作用于連桿i的斥力勢能,它隨著的減小而增大,當(dāng)趨近于d0時,斥力勢能迅速增大并趨于無窮,引導(dǎo)機(jī)械臂向遠(yuǎn)離障礙物方向運(yùn)動.對應(yīng)的斥力函數(shù)為

式中:R3為單位向量,方向?yàn)檫B桿i等效圓柱面的最近點(diǎn)指向第k個凸包表面最近點(diǎn).

3.2 合扭矩的計算

不少學(xué)者采用遍歷的方式尋找使得總勢能最小的關(guān)節(jié)組合,此方法計算速度很慢,不能表現(xiàn)出良好的實(shí)時性,本文則利用勢能的負(fù)梯度作為力的大小和方向,將人工勢場產(chǎn)生的虛擬力作用于機(jī)械臂,以此驅(qū)動機(jī)械臂運(yùn)動,使得機(jī)械臂每一步的運(yùn)動都是局部最優(yōu).接觸力F與關(guān)節(jié)扭矩τ的關(guān)系可以表示為

式中:J∈R3×n為接觸力作用點(diǎn)處的雅克比矩陣,F∈R3為接觸力,此處為人工勢場產(chǎn)生的虛擬力,τ∈Rn為接觸力作用點(diǎn)產(chǎn)生的各關(guān)節(jié)扭矩,吸引力作用于機(jī)械臂末端執(zhí)行器兩端,用式(16)表示為

式中:J1(θ)∈R3×n,J2(θ)∈R3×n為機(jī)械臂末端執(zhí)行器兩端的雅克比矩陣.排斥力作用于連桿上的最近距離點(diǎn),用式(17)表示為

結(jié)合引力扭矩以及斥力扭矩,合扭矩τtotal可用式(18)表示為

3.3 自適應(yīng)步長的設(shè)計

根據(jù)第3.2節(jié)合扭矩的計算結(jié)果,可以得出機(jī)械臂各關(guān)節(jié)的運(yùn)動情況,設(shè)置每一步的運(yùn)動步長約束,記為θstep,則下一步各關(guān)節(jié)的角度可以用式(19)表示為

式中:θcurrent∈Rn為機(jī)械臂各關(guān)節(jié)當(dāng)前角度,θstep∈R+為設(shè)置的約束步長,∥∥為二范數(shù)計算符,θnext為機(jī)械臂運(yùn)動到下一步時各關(guān)節(jié)角度.

從式(19)可用看出,機(jī)械臂運(yùn)動幅度不會因?yàn)橐蛘叱饬^大而過大,而是與設(shè)置的運(yùn)動步長相關(guān).為了讓機(jī)械臂平穩(wěn)高效地運(yùn)動,本研究對運(yùn)動步長做了如下改進(jìn),使其隨著末端執(zhí)行器與目標(biāo)的距離自適應(yīng)的調(diào)整,用式(20)表示為

式中:a1,a2,θmax均為正常數(shù),min為取兩者之間的較小者操作符,δ(θ)代表機(jī)械臂各關(guān)節(jié)角為θ時,末端執(zhí)行器兩端與目標(biāo)之間的距離,

3.4 局部極小值的逃離

當(dāng)引力勢函數(shù)與多個斥力勢函數(shù)共同作用時,總勢能容易陷入局部極小而非全局最小值.如果不改變勢場分布將難以引導(dǎo)機(jī)械臂繼續(xù)運(yùn)動,導(dǎo)致避障任務(wù)失敗.不少研究通過添加虛擬目標(biāo)或者虛擬障礙物以逃離局部極小值,這兩個方法均不適用于本研究在操作空間的位姿規(guī)劃.針對目標(biāo)關(guān)節(jié)角不可知的情況,本研究對引力常數(shù)Ka進(jìn)行改進(jìn),使Ka的值隨著局部極小值出現(xiàn)的次數(shù)r以及預(yù)檢測碰撞的次數(shù)c動態(tài)變化,如式(21)所示,從而破壞勢場平衡,使機(jī)械臂逃離局部極小值.

局部極小值的判定是計算一段時間內(nèi)末端執(zhí)行器兩端與目標(biāo)之間誤差平均改變量,如果它小于閾值δthreshold則判定為局部極小值.定義如下:

式中:N為機(jī)械臂運(yùn)動的當(dāng)前時刻,M為檢測局部極小值的時間步長,δj代表j時刻末端執(zhí)行器兩端與目標(biāo)位置的距離,定義如下:

p1(j),p2(j)代表時刻末端執(zhí)行器兩端的位置.

預(yù)檢測碰撞是指對局部極小值進(jìn)行處理后,會很大程度上改變勢場的分布,此時極有可能導(dǎo)致機(jī)械臂與障礙物發(fā)生碰撞,因此算法會計算機(jī)械臂下一步的運(yùn)動是否發(fā)生碰撞,如果發(fā)生碰撞,則根據(jù)發(fā)生碰撞的次數(shù)減小引力常數(shù).

引力常數(shù)改變后,勢場有很大波動,機(jī)械臂末端執(zhí)行器的狀態(tài)也會發(fā)生一定的改變,因此局部極小值檢測會在一段時間內(nèi)不再進(jìn)行,待勢場穩(wěn)定后重新檢測.

基于改進(jìn)人工勢場的手術(shù)機(jī)器人位姿規(guī)劃算法主流程如下:

步驟1獲取機(jī)械臂當(dāng)前位姿下的關(guān)節(jié)角,根據(jù)正運(yùn)動學(xué)計算出各關(guān)節(jié)點(diǎn)在基坐標(biāo)系下的坐標(biāo);

步驟2根據(jù)各關(guān)節(jié)計算機(jī)械臂各連桿等效線段以及柱面方程式,同時計算末端執(zhí)行器兩端與目標(biāo)的距離,確定自適應(yīng)步長;

步驟3將深度相機(jī)捕獲的障礙物點(diǎn)云利用快速凸包算法進(jìn)行凸體化,利用GJK算法計算障礙物與各連桿以及末端執(zhí)行器之間的最近距離以及最近距離點(diǎn),由障礙物上最近距離點(diǎn)指向機(jī)械臂連桿(末端執(zhí)行器)上的最近距離點(diǎn)的方向即為斥力勢能最快的下降方向;

步驟4計算末端執(zhí)行器兩端的引力以及機(jī)械臂連桿(末端執(zhí)行器)上最近距離點(diǎn)的斥力,獲取對應(yīng)位置的雅克比矩陣,用于合扭矩的計算;

步驟5根據(jù)自適應(yīng)步長以及合扭矩求出機(jī)械臂下一步各關(guān)節(jié)角;

步驟6利用GJK算法檢測機(jī)械臂在關(guān)節(jié)角下是否與障礙物發(fā)生碰撞,如果碰撞則減小引力常數(shù),返回步驟4,否則進(jìn)行步驟7;

步驟7檢查計數(shù)器是否在工作狀態(tài)(初始化計數(shù)器為非工作狀態(tài)):

如果是非工作狀態(tài),判定機(jī)械臂是否陷入局部極小:若陷入局部極小,則逐漸增大引力常數(shù)以破壞勢場平衡,同時啟動計數(shù)器,在計數(shù)期間內(nèi)停止對局部極小值的檢測,進(jìn)行步驟6,否則進(jìn)行步驟8;如果是工作狀態(tài),則進(jìn)行步驟8;

步驟8判定末端執(zhí)行器是否穩(wěn)定于目標(biāo)位姿附近,滿足條件則停止運(yùn)動,否則返回步驟1.

4 仿真與測試

為驗(yàn)證本文所設(shè)計算法的可行性以及有效性,本節(jié)利用MATLAB 2020b模擬實(shí)際機(jī)器人系統(tǒng),進(jìn)行虛擬實(shí)物測試,驗(yàn)證機(jī)械臂能否避開動態(tài)障礙物達(dá)到指定位姿.設(shè)計的仿真實(shí)驗(yàn)包括:不同目標(biāo)位姿下可達(dá)性實(shí)驗(yàn),靜態(tài)以及動態(tài)避障實(shí)驗(yàn),局部極小值實(shí)驗(yàn).

4.1 不同目標(biāo)位姿下可達(dá)性實(shí)驗(yàn)

為了驗(yàn)證兩點(diǎn)約束法的有效性,本節(jié)設(shè)計不同位姿下機(jī)械臂的可達(dá)性實(shí)驗(yàn),δ(θ)<10?4m代表末端執(zhí)行器到達(dá)目標(biāo)位姿,初始化參數(shù)如下:

機(jī)械臂初始位姿為θcurrent=[0 0 0 0 0 0 0]T,Ka=106,a1=0.2,a2=0.2,θmax=0.01 rad,十組不同目標(biāo)位姿如圖4所示,目標(biāo)點(diǎn)為pgoal1,方向?yàn)榧^所示由pgoal2指向pgoal1.

圖4 十組目標(biāo)位姿Fig.4 Ten target poses

表2顯示了在不同的目標(biāo)位姿下,末端執(zhí)行器都能準(zhǔn)確快速的到達(dá).圖5為第1組目標(biāo)位姿下機(jī)械臂各關(guān)節(jié)的運(yùn)動軌跡,可以看出,機(jī)械臂各關(guān)節(jié)角度變化很小,運(yùn)動較為平緩.圖6為機(jī)械臂末端執(zhí)行器兩端與目標(biāo)之間的位置誤差與時間的關(guān)系,可以看出,末端執(zhí)行器在離目標(biāo)較遠(yuǎn)時,機(jī)械臂運(yùn)動的幅度較大,隨著與目標(biāo)逐漸靠近,運(yùn)動逐漸變慢,從而避免了因運(yùn)動步長過大錯過了目標(biāo)位置或者在目標(biāo)附近震蕩.

表2 機(jī)械臂在不同目標(biāo)位姿下的運(yùn)動情況Table 2 The motion of the robot in different target poses

圖5 機(jī)械臂各關(guān)節(jié)的運(yùn)動軌跡Fig.5 The motion trajectory of each joint of the robot

圖6 末端執(zhí)行器兩端與目標(biāo)之間的位置誤差Fig.6 Position error between the end effector and the target

4.2 靜態(tài)及動態(tài)避障實(shí)驗(yàn)

本節(jié)在第4.1節(jié)的基礎(chǔ)上添加動靜態(tài)障礙物來驗(yàn)證機(jī)械臂的避障性能,障礙物由一系列點(diǎn)組成,經(jīng)過快速凸包算法處理轉(zhuǎn)化為凸體.

參數(shù)初始化:Kr=10,d0=0.05 m,目標(biāo)位姿對應(yīng)的pgoal1,pgoal2如表2中的第1組所示,障礙物的頂點(diǎn)坐標(biāo)與表3障礙物1一致.其它參數(shù)與第4.1節(jié)一致.初始化實(shí)驗(yàn)配置如圖7所示.動態(tài)避障時,障礙物速度vobs=[vx,vy,vz],其 中vx=0.0005 m/步,vy=0.0005 m/步,vz=0.0005 m/步.

表3 各個障礙物頂點(diǎn)的位置Table 3 The position of the vertex of each obstacle

圖7 避障實(shí)驗(yàn)初始化環(huán)境配置Fig.7 Initialization environment configuration of obstacle avoidance experiment

圖8為文獻(xiàn)[13–16]所提方法(分別為多個球包圍盒、橢球包圍盒以及球包圍盒)與GJK算法對于障礙物的處理以及最近距離的計算,其中p1為機(jī)械臂連桿等效面上的一點(diǎn),p2為機(jī)械臂與障礙物之間的最近距離點(diǎn).從圖中可以看出文獻(xiàn)[13–16]所提方法會在較大程度上增大障礙物的體積,計算出的最近點(diǎn)在包圍盒上,最近距離比實(shí)際的要小,而GJK算法計算出的最近點(diǎn)在障礙物凸點(diǎn)上或者在凸點(diǎn)連線上,計算的距離更準(zhǔn)確.

圖8 機(jī)械臂各連桿等效圓柱面Fig.8 The equivalent cylinders of links of the robot

圖9與圖10展示了機(jī)械臂在到達(dá)目標(biāo)位姿的過程中,各連桿與靜動態(tài)障礙物之間沒有發(fā)生接觸,說明避障算法是有效的.從圖10(a)可以看出各連桿與障礙物之間距離在500步后呈現(xiàn)線性增長,這是因?yàn)?00步之后,末端執(zhí)行器比較接近目標(biāo),此時機(jī)械臂整體的運(yùn)動就非常慢,而障礙物是勻速運(yùn)動的,逐漸遠(yuǎn)離機(jī)械臂,因此與機(jī)械臂各連桿之間的距離會呈現(xiàn)線性增長;從圖10(b)可以看出末端執(zhí)行器與目標(biāo)之間的位置誤差并不總是逐漸減小的,當(dāng)動態(tài)障礙物距離機(jī)械臂很近時,斥力遠(yuǎn)大于引力,此時機(jī)械臂會以避障任務(wù)為主,在不與障礙物碰撞的情況下,逐漸接近目標(biāo).

圖9 靜態(tài)避障下機(jī)械臂的運(yùn)動情況Fig.9 The motion of the manipulator in static obstacle avoidance

圖10 動態(tài)避障下機(jī)械臂的運(yùn)動情況Fig.10 The motion of the manipulator in dynamic obstacle avoidance

4.3 局部極小值實(shí)驗(yàn)

本節(jié)在手術(shù)場景中添加多個障礙物,使末端執(zhí)行器在進(jìn)行位姿對準(zhǔn)的過程中陷入局部極小值,觀察機(jī)械臂后續(xù)的運(yùn)動狀況,進(jìn)而驗(yàn)證動態(tài)引力常數(shù)具有逃離局部極小值的能力.

實(shí)驗(yàn)場景的初始化配置如圖11所示,包括LBR iiwa 7七自由度冗余機(jī)械臂(與末端執(zhí)行器結(jié)合)、障礙物、脊柱模型以及帶有光學(xué)標(biāo)記物的穿刺針模型,模型大小與實(shí)物一致,通過點(diǎn)面信息展示在機(jī)械臂坐標(biāo)系下.

圖11 椎弓根螺釘位姿對準(zhǔn)實(shí)驗(yàn)場景配置Fig.11 Experimental scene configuration of pedicle screw pose alignment

參數(shù)初始化:不改變引力常數(shù)時,Ka=106:改變引力常數(shù)時,Ka=106+r?c.Kr=1,d0=0.05 m,b=6,M=50,δthreshold=10?6m,tcount=400,目 標(biāo)位姿對應(yīng)的pgoal1,pgoal2如表2中的第1組所示,障礙物的頂點(diǎn)坐標(biāo)如表3所示,其它參數(shù)與第4.1節(jié)一致.

實(shí)驗(yàn)中,如果末端執(zhí)行器在3500步以內(nèi)未達(dá)到目標(biāo)位姿,則認(rèn)為機(jī)器人到達(dá)目標(biāo)位姿失敗.從圖12(a)中可以看出,末端執(zhí)行器與目標(biāo)位姿之間的距離在302步陷入局部最小值,并在0.06 m附近振蕩,無法達(dá)到目標(biāo)位姿.圖12(b)為采用動態(tài)引力常數(shù)后末端執(zhí)行器與目標(biāo)之間的距離,其中紅點(diǎn)為檢測到的局部極小值點(diǎn),每次出現(xiàn)局部極小值或發(fā)生預(yù)碰撞后,都會改變引力常數(shù),可以看出,在第4次改變引力常數(shù)后,末端執(zhí)行器達(dá)到目標(biāo)位姿.圖13為改進(jìn)引力常數(shù)后,機(jī)械臂各連桿與各個障礙物之間的距離,可以看出改變引力常數(shù)并沒有導(dǎo)致機(jī)械臂與障礙物發(fā)生碰撞.由此,證明了動態(tài)引力常數(shù)使機(jī)械臂具有逃離局部極小值的能力.

圖12 末端執(zhí)行器兩端與目標(biāo)之間的位置誤差Fig.12 Position error between the end effector and the target

圖13 機(jī)械臂連桿與各障礙物之間的最近距離Fig.13 The shortest distance between links of the robot arm and the obstacles

5 結(jié)論

本研究針對手術(shù)機(jī)器人自動化手術(shù)路徑執(zhí)行中的安全性及準(zhǔn)確性問題,提出一種改進(jìn)人工勢場的手術(shù)機(jī)器人位姿規(guī)劃算法.在位姿可達(dá)方面,對引力函數(shù)進(jìn)行改進(jìn),利用兩點(diǎn)法約束末端執(zhí)行器姿態(tài),在無需求取運(yùn)動學(xué)逆解的情況下,準(zhǔn)確驅(qū)動機(jī)械臂到達(dá)指定位姿;在避障方面,利用快速凸包算法將障礙物凸體化,通過GJK算法計算障礙物與機(jī)械臂連桿等效圓柱面之間的最近距離,使避障距離更加準(zhǔn)確;在機(jī)械臂運(yùn)動過程中,通過自適應(yīng)步長,使其運(yùn)動更加平穩(wěn)快速;在陷入局部極小值時,通過動態(tài)引力常數(shù),機(jī)械臂能順利逃離.

本研究給定了實(shí)驗(yàn)中障礙物點(diǎn)云位置信息,后續(xù)會配合視覺系統(tǒng),捕獲真實(shí)場景下障礙物的點(diǎn)云,使研究更加符合臨床應(yīng)用.