基于虛擬力引導(dǎo)的人機(jī)協(xié)同目標(biāo)抓取方法

姜通維，姜勇

（1.沈陽(yáng)建筑大學(xué) 信息與控制工程學(xué)院，遼寧 沈陽(yáng) 110168;2.中國(guó)科學(xué)院 網(wǎng)絡(luò)化控制系統(tǒng)重點(diǎn)實(shí)驗(yàn)室，遼寧沈陽(yáng) 110016;3.中國(guó)科學(xué)院 沈陽(yáng)自動(dòng)化研究所，遼寧 沈陽(yáng) 110016;4.中國(guó)科學(xué)院 機(jī)器人與智能制造創(chuàng)新研究院，遼寧 沈陽(yáng) 110169）

一些特殊的工作場(chǎng)景如帶電作業(yè)，由于其危險(xiǎn)的工作環(huán)境使得工人們?cè)诠ぷ鲿r(shí)往往處于危險(xiǎn)環(huán)境中如高壓和高空[1]。力反饋遙操作機(jī)器人能夠讓使用者在安全的距離外通過(guò)主端設(shè)備控制從端設(shè)備進(jìn)行作業(yè)，因而被廣泛應(yīng)用于帶電作業(yè)以及其他危險(xiǎn)環(huán)境的工作。人機(jī)協(xié)同技術(shù)能夠?qū)⑷说臎Q策能力實(shí)時(shí)地融入到機(jī)器人的自主控制中，提高機(jī)器人在復(fù)雜環(huán)境下作業(yè)的能力和可靠度，從而提高其對(duì)復(fù)雜環(huán)境下作業(yè)的適應(yīng)能力[2-3]。

自Rosenberg 首次提出虛擬夾具技術(shù)[4]以來(lái)，國(guó)內(nèi)外學(xué)者對(duì)力反饋裝置與人機(jī)協(xié)同系統(tǒng)進(jìn)行了一系列的研究。主要集中于操作者的控制輔助以及與從端環(huán)境的交互和觸覺(jué)信息的感知方面[5-8]。Lee 等[6]提出了一種機(jī)器人控制方法，操作者通過(guò)在實(shí)時(shí)視頻圖像上繪制所需的參考路徑，據(jù)此生成機(jī)器人移動(dòng)的實(shí)際路徑并產(chǎn)生虛擬引導(dǎo)力協(xié)助操作者操作移動(dòng)機(jī)器人完成相關(guān)動(dòng)作。Nikolaidis等[9]提出了人機(jī)相互適應(yīng)的人機(jī)協(xié)同方法，通過(guò)MOMDP 計(jì)算用戶的目標(biāo)分布概率，在保持人的信任的同時(shí)引導(dǎo)操作者向最優(yōu)目標(biāo)前進(jìn)。Wang等[10]提出了一種共享控制遙操作方法，通過(guò)利用多自由度機(jī)械臂的冗余自由度，在不影響機(jī)械臂末端位置的情況下實(shí)現(xiàn)機(jī)械臂本體對(duì)動(dòng)態(tài)障礙物進(jìn)行避障。

由于主端和從端設(shè)備的結(jié)構(gòu)不同，操作者只能對(duì)從端設(shè)備的一部分進(jìn)行控制。所以目前這個(gè)方向的研究大多將從端視為一個(gè)等效質(zhì)點(diǎn)，注重于從端的位置(如移動(dòng)機(jī)器人和機(jī)械臂末端)[11-13]。傳統(tǒng)基于人工勢(shì)場(chǎng)法的虛擬力引導(dǎo)方法會(huì)將機(jī)械臂末端受到的斥力和引力疊加后通過(guò)力反饋設(shè)備反饋給操作者[11-14]。這種方法由于目標(biāo)吸引力的存在導(dǎo)致操作者感受到的力并不適合于引導(dǎo)操作者進(jìn)行機(jī)械臂本體避障任務(wù)。此外，對(duì)于6 自由度機(jī)械臂的目標(biāo)抓取操作而言，往往需要操作者操作機(jī)器人末端以需要的姿態(tài)到達(dá)目標(biāo)位置[15]。但在直接遙操作中，操作者往往難以將機(jī)械臂移動(dòng)到某一精確的位置和姿態(tài)，并且由于人的手臂和手腕構(gòu)成了一個(gè)耦合系統(tǒng)，二者的運(yùn)動(dòng)通常會(huì)相互影響，導(dǎo)致在操作過(guò)程中經(jīng)常不可避免地會(huì)出現(xiàn)操作偏差。

因此，本文研究了一種基于虛擬力引導(dǎo)的遙操作方法?；趶亩藱C(jī)械臂末端、障礙物和目標(biāo)點(diǎn)的相對(duì)位置，通過(guò)在預(yù)定義路徑周圍構(gòu)建管道形虛擬力場(chǎng)，結(jié)合障礙物的斥力與虛擬力場(chǎng)的約束力，生成趨近目標(biāo)的同時(shí)進(jìn)行避障操作的力引導(dǎo)。通過(guò)構(gòu)建錐形虛擬力場(chǎng)并對(duì)操作者的輸出進(jìn)行限制，生成協(xié)助抓取任務(wù)的力引導(dǎo)，同時(shí)減少操作者的誤操作。

1 基于虛擬力引導(dǎo)目標(biāo)抓取方法

目標(biāo)抓取任務(wù)可以分為趨近目標(biāo)點(diǎn)和到達(dá)目標(biāo)點(diǎn)兩個(gè)階段。在趨近目標(biāo)階段，通過(guò)構(gòu)建管道形虛擬力場(chǎng)(以下簡(jiǎn)稱管形力場(chǎng))，利用管形力場(chǎng)內(nèi)部的受約束空間協(xié)助操作者避開(kāi)障礙物后盡可能再回到預(yù)定義的路徑上。當(dāng)機(jī)械臂末端趨近目標(biāo)點(diǎn)到達(dá)一定的距離后，通過(guò)構(gòu)建錐形虛擬力場(chǎng)(以下簡(jiǎn)稱錐形力場(chǎng))協(xié)助操作者到達(dá)目標(biāo)點(diǎn)實(shí)施抓取任務(wù)。

1.1 管道形虛擬力場(chǎng)的構(gòu)建

在本系統(tǒng)中，我們使用forbidden region virtual fixture (FRVF)[8]來(lái)限制機(jī)械臂的末端不要過(guò)多地偏離理想的路徑。

假設(shè)P是一條預(yù)定義的路徑(可由任意軌跡規(guī)劃算法生成)。以P為軸線，rd為半徑可以構(gòu)建出一個(gè)管形力場(chǎng)(如圖1 所示)。再假設(shè)xc是機(jī)械臂末端在笛卡爾空間中的坐標(biāo)，路徑P上的點(diǎn)s(xc)是該路徑上距離xc最近的點(diǎn)，其定義為

圖1 管形虛擬力場(chǎng)示意Fig.1 Virtual force field of pipe

力場(chǎng)中的約束力可以定義為

式中：δ(xc)是約束力的參考方向，由xc和s(xc)的相對(duì)位置得出：

1.2 避障任務(wù)力引導(dǎo)的構(gòu)建

針對(duì)傳統(tǒng)人工勢(shì)場(chǎng)法不適用于引導(dǎo)操作者進(jìn)行機(jī)械臂本體避障的問(wèn)題，提出的改進(jìn)避障引導(dǎo)策略如圖2 所示，由目標(biāo)點(diǎn)對(duì)機(jī)械臂產(chǎn)生的引力被管形力場(chǎng)所產(chǎn)生的約束力代替，以防止產(chǎn)生錯(cuò)誤的力引導(dǎo)信息。值得注意的是，我們所提的方法基于一個(gè)前提，即由于遙操作是一個(gè)human-inthe-loop 系統(tǒng)，操作者根據(jù)人的決策能力結(jié)合系統(tǒng)給與的力覺(jué)引導(dǎo)做出合理的操作，所以系統(tǒng)反饋給操作者的引導(dǎo)信息并不需要是完美的[2-3]。

圖2 避障任務(wù)的虛擬力引導(dǎo)Fig.2 Virtual force guidance for obstacle avoidance

對(duì)于斥力場(chǎng)的構(gòu)建，其通常的表達(dá)式如式(4)所示。當(dāng)機(jī)械臂本體或者末端進(jìn)入障礙物的作用范圍時(shí)，作用在機(jī)械臂本體的斥力被映射到機(jī)械臂末端，經(jīng)主端的力反饋設(shè)備反饋給操作者。距離障礙物越近，機(jī)械臂所受到的斥力越大。

式中：ηrep是一個(gè)斥力的增益因子；ρ是機(jī)械臂本體上距離障礙物最近的點(diǎn)；ρo是障礙物的作用范圍。

障礙物所產(chǎn)生的斥力大小為斥力場(chǎng)對(duì)距離的導(dǎo)數(shù)，其表達(dá)式為

則操作者受到的引導(dǎo)力為機(jī)械臂在工作空間中受到的引力與斥力的合，其表達(dá)式為

若存在多個(gè)障礙物，則式(6)可變?yōu)?/p>

式中：Fc為管形力場(chǎng)產(chǎn)生的約束力。當(dāng)機(jī)械臂本體距離障礙物較近時(shí)，其系統(tǒng)產(chǎn)生的斥力會(huì)大于管形力場(chǎng)的約束力，以確保避障任務(wù)的優(yōu)先級(jí)高于趨近目標(biāo)的任務(wù)。

約束力一直指向管形力場(chǎng)的中軸線(預(yù)定義路徑)，則約束力的方向與斥力的方向的夾角會(huì)接近一條直線，所以約束力的加入不會(huì)對(duì)避障任務(wù)造成不利影響。當(dāng)避障任務(wù)完成之后，操作者感受到的力引導(dǎo)只由虛擬力場(chǎng)產(chǎn)生，用于協(xié)助操作者跟隨預(yù)定義的路徑移動(dòng)。

1.3 錐形虛擬力場(chǎng)的構(gòu)建

當(dāng)機(jī)械臂末端足夠接近目標(biāo)點(diǎn)后，操作者需要調(diào)整手臂和手腕來(lái)操作機(jī)械臂到達(dá)理想的位置和姿態(tài)。與管形力場(chǎng)的構(gòu)建方法類似，當(dāng)機(jī)械臂末端與目標(biāo)點(diǎn)之間的距離小于一個(gè)特定的距離(即dc)時(shí)，可以建立一個(gè)基于機(jī)械臂末端位置的錐形力場(chǎng)(如圖3 所示)來(lái)協(xié)助操作者進(jìn)行精細(xì)的操作。對(duì)于一個(gè)目標(biāo)點(diǎn)xt，一個(gè)初始點(diǎn)xs和一個(gè)基本圓錐的開(kāi)口角度 α，則其對(duì)應(yīng)的圓錐的中軸線可以定義為

圖3 錐形虛擬力場(chǎng)示意Fig.3 Conical virtual force field

給定一個(gè)機(jī)械臂末端的笛卡爾空間位置xa，將它與目標(biāo)點(diǎn)連線分解為與圓錐軸線相關(guān)的正交分量：

定義dcone為錐形力場(chǎng)邊界到圓錐中軸線的距離，則如果機(jī)械臂末端在圓錐范圍內(nèi)就必須滿足：

若xc在當(dāng)前構(gòu)建的錐形力場(chǎng)范圍之外，即時(shí)可以根據(jù)當(dāng)前機(jī)械臂末端的位置重新構(gòu)建錐形力場(chǎng)協(xié)助操作者完成操作任務(wù)。

根據(jù)所建立的錐形力場(chǎng)，可以計(jì)算出虛擬引導(dǎo)的力，其表達(dá)式為其中k1和k2是兩個(gè)增益參數(shù)，用于調(diào)節(jié)引導(dǎo)力的大小。從式(14) 可以看出，對(duì)于等量的位置偏差，隨著機(jī)械臂末端位置與目標(biāo)點(diǎn)的距離的減小，而操作者感受到的引導(dǎo)強(qiáng)度會(huì)增加。

對(duì)于機(jī)械臂末端姿態(tài)調(diào)整的虛擬力引導(dǎo)，其計(jì)算方式與對(duì)末端位置的引導(dǎo)一樣。假設(shè) θc是機(jī)械臂末端當(dāng)前姿態(tài)的旋轉(zhuǎn)角度，θt是目標(biāo)姿態(tài)所對(duì)應(yīng)的機(jī)械臂的旋轉(zhuǎn)角度。則對(duì)于一個(gè)當(dāng)前機(jī)械臂的狀態(tài)X[xc,θc]，系統(tǒng)對(duì)操作者反饋的總引導(dǎo)力信息定義為

1.4 運(yùn)動(dòng)約束

如圖4 所示，為了實(shí)現(xiàn)對(duì)操作者輸出的限制，以下信息將會(huì)被用到：采樣時(shí)間 ?T、錐形力場(chǎng)的中軸線(式(8))a；機(jī)械臂末端所經(jīng)過(guò)的離散路徑P={x1,x2,···,xn}∈R3；在k時(shí)刻，機(jī)械臂末端位置其中x(k)表示末端位置，θ(k)表示當(dāng)前姿態(tài)。在k時(shí)刻，機(jī)械臂末端速度其中ω(k)為后3 個(gè)關(guān)節(jié)的角速度。

圖4 速度矢量分解Fig.4 Examples of decomposed velocity vectors

對(duì)一個(gè)機(jī)械臂末端速度v(k)，可以將其分解為

式中：vO(k)和vR(k)分別是v(k)垂直于圓錐中軸線和平行于中軸線的分量。

此時(shí)，加入一個(gè)調(diào)整參數(shù)μ(k)，則式(16)變?yōu)?/p>

通過(guò)調(diào)節(jié)參數(shù)μ(k)，可以實(shí)現(xiàn)對(duì)操作者輸出的限制。當(dāng)μ(k)=1時(shí)，機(jī)械臂末端將完全按照操作者發(fā)送的指令移動(dòng)；當(dāng)μ(k)=0時(shí)，則從端機(jī)械臂將完全忽略垂直于中心軸方向的分量。

在人機(jī)協(xié)作領(lǐng)域，存在一個(gè)如何分配機(jī)器人自主性能與人的操作權(quán)重的問(wèn)題。許多學(xué)者對(duì)該問(wèn)題進(jìn)行了研究和探討[2-3,16-18]，但當(dāng)前還沒(méi)有一個(gè)公認(rèn)的權(quán)重分配方案。結(jié)合實(shí)際操作需求，本文認(rèn)為人在大多數(shù)時(shí)間可以順從機(jī)器人的自主決策和引導(dǎo)，但一旦人想要自己進(jìn)行操作就應(yīng)該擁有足夠的控制權(quán)限。故將參數(shù)μ(k)定義為

式中：dcone(k)和d(k)分別由式(9)和式(12)計(jì)算得出。圖5 描述了參數(shù)μ(k)與d(k)之間的關(guān)系。機(jī)械臂末端越接近目標(biāo)位置，操作者的輸出在垂直圓錐軸線的分量的控制權(quán)重越低，以降低誤操作的影響。但系統(tǒng)不會(huì)忽視操作者的輸出，操作者依然對(duì)從端有足夠的控制權(quán)限。對(duì)于姿態(tài)誤操作的限制與末端位置的限制相同，在此不再贅述。

圖5 參數(shù) μ(k)與 d(k)之間的關(guān)系Fig.5 Relationship between μ (k) andd(k)

2 實(shí)驗(yàn)驗(yàn)證

本文所提基于虛擬力引導(dǎo)的遙操作系統(tǒng)由一個(gè)主從控制子系統(tǒng)和深度相機(jī)組成，如圖6 所示。

圖6 虛擬引導(dǎo)力遙操作系統(tǒng)結(jié)構(gòu)Fig.6 Virtual force guidance teleoperation system

相機(jī)用于獲取目標(biāo)點(diǎn)位置、障礙物位置和從端機(jī)械臂與障礙物的相對(duì)距離。從端設(shè)備為一個(gè)UR5 協(xié)作機(jī)器人，主端為一個(gè)Virtuose 6D 力反饋儀，能夠與操作者實(shí)現(xiàn)力覺(jué)交互。在實(shí)驗(yàn)中，從端機(jī)械臂向主端發(fā)送位置和速度信息，而主端力反饋設(shè)備結(jié)合從端發(fā)送的信息和相機(jī)發(fā)送的距離信息計(jì)算出引導(dǎo)力并反饋給操作者；同時(shí)向從端發(fā)送速度指令用以控制從端設(shè)備的運(yùn)動(dòng)。

2.1 機(jī)械臂避障實(shí)驗(yàn)

在機(jī)械臂本體避障實(shí)驗(yàn)中，我們僅考慮障礙物和目標(biāo)物都為靜態(tài)且唯一的情況。根據(jù)機(jī)械臂末端、障礙物和目標(biāo)物的位置建立起斥力場(chǎng)和管形力場(chǎng)，并計(jì)算出虛擬力引導(dǎo)(由式(6)得出)，通過(guò)主端反饋給操作者。操作者根據(jù)這些力反饋信息對(duì)從端機(jī)械臂進(jìn)行操作，最終避過(guò)障礙物并接近目標(biāo)點(diǎn)。當(dāng)機(jī)械臂避過(guò)障礙物后，機(jī)械臂末端距離目標(biāo)點(diǎn)10 cm 內(nèi)即可判定為完成實(shí)驗(yàn)。當(dāng)機(jī)械臂本體碰到障礙物則實(shí)驗(yàn)失敗。

實(shí)驗(yàn)在兩種場(chǎng)景中進(jìn)行(如圖7 所示)，兩個(gè)場(chǎng)景的目標(biāo)點(diǎn)位置和機(jī)械臂初始位置相同，但障礙物的位置不同，兩場(chǎng)景中障礙物分別作用于機(jī)械臂的末端和連桿。我們分別在兩個(gè)場(chǎng)景中使用本文所提虛擬力引導(dǎo)方法、文獻(xiàn)[7]所提力感知方法和無(wú)力引導(dǎo)操作方法分別進(jìn)行多次實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果如表1 和表2 所示。

圖7 機(jī)械臂避障實(shí)驗(yàn)的實(shí)驗(yàn)環(huán)境Fig.7 Experimental environment for obstacle avoidance

表1 障礙物作用于機(jī)械臂末端避障實(shí)驗(yàn)結(jié)果Table 1 Experimental results of the obstacle approaching the end

表2 障礙物作用于機(jī)械臂連桿避障實(shí)驗(yàn)結(jié)果Table 2 Experimental results of the obstacle approaching the links

圖8(a) 和圖8(b)～(d) 分別是一次避障實(shí)驗(yàn)中，在3 種方法協(xié)助下的機(jī)械臂末端的軌跡和其末端與預(yù)定義路徑的偏差?？梢?jiàn)，在虛擬力引導(dǎo)的情況下，當(dāng)機(jī)械臂在障礙物的作用范圍內(nèi)時(shí)，障礙物對(duì)機(jī)械臂的斥力對(duì)操作者的引導(dǎo)占主要作用從而避免碰撞發(fā)生。操作者可以在虛擬力的引導(dǎo)下完成避障任務(wù)。同時(shí)，相較于文獻(xiàn)[7]所提力感知引導(dǎo)方法，由于管形力場(chǎng)的作用，機(jī)械臂末端軌跡更接近障礙物表面，使得末端與預(yù)定義路徑的最大偏差大幅減小。并且在完成了避障任務(wù)后，末端與預(yù)定義路徑的偏差能快速縮小。此外，即使沒(méi)有受到指向目標(biāo)點(diǎn)的力引導(dǎo)，操作者依然能夠通過(guò)自身的決策能力結(jié)合管形力場(chǎng)提供的約束力快速完成實(shí)驗(yàn)任務(wù)，縮短耗時(shí)，且機(jī)械臂末端的與預(yù)定于路徑的偏差在較小的范圍之內(nèi)。圖8(e) 是一次當(dāng)障礙物作用于機(jī)械臂連桿時(shí)3 種方法的機(jī)械臂末端軌跡對(duì)比，其中紅色路徑為機(jī)械臂與障礙物發(fā)生碰撞的點(diǎn)。可見(jiàn)基于人工勢(shì)場(chǎng)的力感知方法沒(méi)有考慮到機(jī)械臂連桿的作用，導(dǎo)致會(huì)產(chǎn)生錯(cuò)誤的引導(dǎo)信息，使得與障礙物發(fā)生碰撞的概率增加。而虛擬力能夠正確地引導(dǎo)操作者完成操作任務(wù)。

圖8 避障實(shí)驗(yàn)中機(jī)械臂末端的軌跡及偏差Fig.8 Trajectory and deviation of the end of the manipulator in obstacle avoidance experiment

2.2 目標(biāo)抓取驗(yàn)證

與避障實(shí)驗(yàn)相似，在精細(xì)操作的實(shí)驗(yàn)中，目標(biāo)點(diǎn)的位置和目標(biāo)姿態(tài)不變，而機(jī)械臂末端的初始位置和姿態(tài)不同。實(shí)驗(yàn)在有引導(dǎo)力、無(wú)引導(dǎo)力和僅有運(yùn)動(dòng)約束3 種條件下進(jìn)行。操作者操作機(jī)械臂末端從初始點(diǎn)移動(dòng)到目標(biāo)點(diǎn)，并完成姿態(tài)的調(diào)整。當(dāng)機(jī)械臂末端與目標(biāo)點(diǎn)的距離小于1 cm 且后3 個(gè)關(guān)節(jié)旋轉(zhuǎn)角度與目標(biāo)角度相差小于 5?則判定實(shí)驗(yàn)成功，當(dāng)操作時(shí)間超過(guò)70 s 則判定操作失敗。實(shí)驗(yàn)結(jié)果如表3 所示。

表3 目標(biāo)抓取實(shí)驗(yàn)結(jié)果Table 3 Results of the object grasping experiment

圖9(a)和圖9(b)～(d)分別是有力引導(dǎo)、無(wú)力引導(dǎo)以及僅運(yùn)動(dòng)約束作用下的一次目標(biāo)抓取操作實(shí)驗(yàn)中機(jī)械臂末端的軌跡對(duì)比和與圓錐軸線的偏差對(duì)比?？梢钥闯觯噍^于在僅有運(yùn)動(dòng)約束的協(xié)助方法，在錐形力場(chǎng)的引導(dǎo)下操作者能夠在實(shí)驗(yàn)開(kāi)始階段便快速接近圓錐軸線并向目標(biāo)點(diǎn)接近。表明虛擬引導(dǎo)力的加入能夠提高操作者對(duì)從端設(shè)備的操作效率。同時(shí)大幅減輕操作者的操作負(fù)擔(dān)。而相較于無(wú)力引導(dǎo)的直接遙操作方法，運(yùn)動(dòng)約束的加入能夠減小在機(jī)械臂末端接近軸線后路徑偏差的波動(dòng)，有效低降低在精細(xì)操作時(shí)由于人手的生理構(gòu)造造成的操作偏差。

圖9 不同協(xié)助下的機(jī)械臂末端軌跡Fig.9 Trajectory and deviation of the end of the manipulator with different assistance

3 結(jié)束語(yǔ)

本文，主要關(guān)注于力反饋遙操作系統(tǒng)中的兩個(gè)問(wèn)題：1)在進(jìn)行避障任務(wù)時(shí)從端的機(jī)械臂應(yīng)被視為一個(gè)整體而非一個(gè)等效質(zhì)點(diǎn)來(lái)控制；2)由于人手構(gòu)造的問(wèn)題，在進(jìn)行目標(biāo)抓取任務(wù)時(shí)操作者往往難以將機(jī)械臂移動(dòng)到較為精確的位置和姿態(tài)。

針對(duì)第一個(gè)問(wèn)題我們提出了一個(gè)改進(jìn)方案，基于機(jī)械臂和障礙物的位置信息，結(jié)合人工勢(shì)場(chǎng)法和虛擬夾具構(gòu)建對(duì)障礙物的斥力場(chǎng)和管形力場(chǎng)，能夠?yàn)椴僮髡咛峁┫鄳?yīng)的導(dǎo)向力，協(xié)助操作者完成機(jī)械臂的整體避障任務(wù)，接近目標(biāo)點(diǎn)。對(duì)于第二個(gè)問(wèn)題，當(dāng)機(jī)械臂末端需要到達(dá)目標(biāo)點(diǎn)時(shí)，構(gòu)建出基于位置和旋轉(zhuǎn)角的錐形力場(chǎng)來(lái)輔助操作者到達(dá)目標(biāo)點(diǎn)并調(diào)整姿態(tài)。此外，使用了一種輸出限制方法以減少操作者在精細(xì)操作時(shí)的操作失誤。最后通過(guò)相關(guān)的實(shí)驗(yàn)驗(yàn)證了該方法能有效提高遠(yuǎn)程操作的效率，降低操作失誤的發(fā)生頻率。