面向冗余機(jī)械臂避障運(yùn)動(dòng)規(guī)劃的觸控交互技術(shù)

張寶琛, 惠建江, 張 琦, 劉正雄,*, 黃攀峰

(1. 西北工業(yè)大學(xué)航天學(xué)院, 陜西 西安 710072; 2. 北京跟蹤與通信技術(shù)研究所, 北京 100094;3. 中國(guó)航天空氣動(dòng)力技術(shù)研究院, 北京 100074)

0 引 言

冗余機(jī)械臂是一種運(yùn)動(dòng)機(jī)動(dòng)度大于末端位姿運(yùn)動(dòng)自由度的機(jī)械臂[1],相較于常見(jiàn)的非冗余機(jī)械臂,理論上存在無(wú)數(shù)組關(guān)節(jié)空間構(gòu)型與末端位姿相對(duì)應(yīng)。冗余機(jī)械臂具有工作空間自由靈活、避障能力強(qiáng)等特點(diǎn)。當(dāng)周?chē)嬖谡系K時(shí),可進(jìn)行相應(yīng)的構(gòu)型調(diào)整,有良好的應(yīng)用前景[2-3]。然而,冗余機(jī)械臂構(gòu)型的靈活性也帶來(lái)了如何在避障過(guò)程中調(diào)整和確定機(jī)械臂構(gòu)型的問(wèn)題。

針對(duì)上述問(wèn)題,有研究者提出通過(guò)啟發(fā)式算法進(jìn)行機(jī)械臂構(gòu)型的解算,例如:概率路標(biāo)法(probabilistic road map, PRM)[4-5]、快速擴(kuò)展隨機(jī)樹(shù)法(rapidly exploring random trees,RRT)[6-8]。這些算法在不引入視覺(jué)[9-10]、觸覺(jué)[11-14]等其他傳感器的情況下,只能進(jìn)行結(jié)構(gòu)化工作環(huán)境下的避障。而引入視覺(jué)、觸覺(jué)等傳感器,雖可對(duì)非結(jié)構(gòu)化環(huán)境進(jìn)行避障,但增加了機(jī)械臂的硬件成本[15]。同時(shí),上述算法一方面解算效率不高,另一方面解算結(jié)果不穩(wěn)定,使得冗余機(jī)械臂難以進(jìn)行重復(fù)性的實(shí)施操作。

隨著人機(jī)交互技術(shù)的發(fā)展,也有學(xué)者將人機(jī)交互技術(shù)用于解決機(jī)械臂構(gòu)型選擇、避障規(guī)劃的問(wèn)題。人機(jī)交互作為一種實(shí)現(xiàn)用戶(hù)與計(jì)算機(jī)之間信息交換的方式[16],通過(guò)交互設(shè)備將人引入操作回路,充分發(fā)揮人的決策能力和操作能力,來(lái)解決冗余機(jī)械臂構(gòu)型選擇問(wèn)題、降低硬件成本,典型的交互設(shè)備包括:手控器[17]、虛擬現(xiàn)實(shí)[18-20]、外骨骼[21]等。但是,上述交互方式不論是對(duì)操作人員的編程能力,還是對(duì)設(shè)備的使用熟練程度,都有較高的要求[22]。此外,冗余機(jī)械臂構(gòu)型選擇、避關(guān)節(jié)限位等基于冗余特性的交互功能并不能通過(guò)上述方式體現(xiàn)。

近些年,觸控交互方式逐漸受到人們的關(guān)注[23]。觸控交互方式操作簡(jiǎn)單、學(xué)習(xí)成本低,符合人的自然操作習(xí)慣,可視化程度高,給人們帶來(lái)了更為直觀的視覺(jué)體驗(yàn)[24]。但是,在通過(guò)觸控方式操作機(jī)械臂時(shí),存在二維屏幕坐標(biāo)到三維操作空間坐標(biāo)的映射過(guò)程。升維時(shí)的維度缺失問(wèn)題,使得映射到三維操作空間的坐標(biāo)均在同一平面上,這就決定了如果通過(guò)觸控交互方式操作機(jī)械臂,至少需結(jié)合兩個(gè)以上的相機(jī)視角,才能完成機(jī)械臂末端在三維空間中的移動(dòng),以達(dá)到預(yù)期位置以及完成避障,這大大降低了操作效率。

為充分發(fā)揮人在回路的作用,提高人機(jī)交互的效率,降低冗余機(jī)械臂的運(yùn)動(dòng)操控難度,本文研究了一種面向冗余機(jī)械臂避障規(guī)劃的觸控交互技術(shù),該技術(shù)在保證操作交互友好性的同時(shí),可進(jìn)行結(jié)構(gòu)化、非結(jié)構(gòu)化障礙的躲避。

本文以庫(kù)卡輕型機(jī)械臂KUKA iiwa為例,進(jìn)行避障運(yùn)動(dòng)規(guī)劃的觸控交互研究。

1 觸控交互實(shí)現(xiàn)原理

1.1 坐標(biāo)系定義

世界坐標(biāo)系:世界坐標(biāo)系OwXwYwZw是用來(lái)提供其他坐標(biāo)系基準(zhǔn)的坐標(biāo)系。在世界坐標(biāo)系下,對(duì)象有相同的參考標(biāo)準(zhǔn),擁有自己的絕對(duì)坐標(biāo)。

屏幕坐標(biāo)系:屏幕坐標(biāo)系OsXsYs是為了表征觸控位置坐標(biāo)而建立的二維坐標(biāo)系。屏幕坐標(biāo)系以仿真環(huán)境視口中心為坐標(biāo)原點(diǎn),以仿真視口寬度方向?yàn)閄軸方向,以高度方向?yàn)閅軸方向。

基坐標(biāo)系:基坐標(biāo)系O0X0Y0Z0是用來(lái)描述機(jī)械臂模型的坐標(biāo)系。在基坐標(biāo)系下,機(jī)械臂各關(guān)節(jié)的位置和姿態(tài)均可以表示,建立D-H坐標(biāo)系進(jìn)行正逆運(yùn)動(dòng)學(xué)的解算。

裁剪坐標(biāo)系:裁剪坐標(biāo)系OtXtYtZt是針對(duì)仿真環(huán)境中模型相對(duì)于相機(jī)的投影來(lái)建立的,投影方式包括正交投影和透視投影[25]。裁剪坐標(biāo)系是絕對(duì)坐標(biāo)轉(zhuǎn)化為相機(jī)坐標(biāo)系坐標(biāo)的中介。

相機(jī)坐標(biāo)系:操作人員通過(guò)虛擬相機(jī)來(lái)觀察仿真環(huán)境中的場(chǎng)景。當(dāng)用仿真環(huán)境視口觀察對(duì)象時(shí),對(duì)象以相機(jī)坐標(biāo)系OcXcYcZc作為參考標(biāo)準(zhǔn)。

上述坐標(biāo)系均為右手坐標(biāo)系,如圖1所示。

圖1 坐標(biāo)系示意圖Fig.1 Schematic diagram of coordinates

1.2 坐標(biāo)系轉(zhuǎn)換關(guān)系

對(duì)于屏幕坐標(biāo)系下的任一點(diǎn)Qs(xs,ys),其在裁剪坐標(biāo)系下的坐標(biāo)為

(1)

式中:nx和ny為相機(jī)近裁剪平面寬度與高度; width和height分別為屏幕寬度與高度。

在仿真物理引擎中,相機(jī)采用透視投影的方式。因此, 對(duì)于裁剪坐標(biāo)系下的任一點(diǎn)Qt(xt,yt,zt),其在相機(jī)坐標(biāo)系下的坐標(biāo)為

(2)

式中:a為相機(jī)鏡頭寬高比;FOV為視場(chǎng)角;|n|、|f|分別為相機(jī)近裁剪平面深度與遠(yuǎn)裁剪平面深度,f

對(duì)于相機(jī)坐標(biāo)系下的任一點(diǎn)Qc(xc,yc,zc),其在基坐標(biāo)系下的坐標(biāo)Q0(x0,y0,z0)為

(3)

式中:Rc和Pc分別為相機(jī)坐標(biāo)系相對(duì)基坐標(biāo)系的旋轉(zhuǎn)矩陣與平移矩陣。

1.3 KUKA iiwa機(jī)械臂逆運(yùn)動(dòng)學(xué)

冗余機(jī)械臂的逆運(yùn)動(dòng)學(xué)解算方法主要可分為兩類(lèi):數(shù)值法和解析法。常用的數(shù)值法如引言中提到的PRM、RRT等,具有通用性好、但時(shí)間開(kāi)銷(xiāo)較大等特點(diǎn)。而解析法主要有鎖定關(guān)節(jié)角[26]、臂型角[27-28]等方法,具有實(shí)時(shí)性強(qiáng)但通用性較差的特點(diǎn)。本文將實(shí)現(xiàn)實(shí)時(shí)的觸控交互、改變機(jī)械臂構(gòu)型,故采用臂型角方法來(lái)進(jìn)行逆運(yùn)動(dòng)學(xué)的解算。

作為冗余機(jī)械臂,KUKA iiwa機(jī)械臂的7個(gè)連桿關(guān)節(jié)兩兩垂直。連桿1～連桿3可視為一個(gè)3自由度的球關(guān)節(jié),連桿4可視為一個(gè)單獨(dú)的旋轉(zhuǎn)關(guān)節(jié)[29],連桿5～連桿7也可視為一個(gè)3自由度的球關(guān)節(jié),其簡(jiǎn)化結(jié)構(gòu)如圖2所示。

圖2 KUKA iiwa簡(jiǎn)化圖Fig.2 Simplification of KUKA iiwa

圖3 臂型角示意圖Fig.3 Schematic diagram of arm-angle

設(shè)定一個(gè)臂型角,按文獻(xiàn)[28]中方法進(jìn)行逆解,即可得到符合要求的逆解,取與上一時(shí)刻逆解向量差值2范數(shù)最小的一組逆解,作為逆解結(jié)果。

(4)

式中:θ∈R7×1為逆運(yùn)動(dòng)學(xué)結(jié)果;0T7∈R4×4表示機(jī)械臂末端相對(duì)于基座的位姿矩陣。

1.4 觸控交互實(shí)現(xiàn)原理

1.4.1 觸控移動(dòng)機(jī)械臂末端實(shí)現(xiàn)原理

觸控移動(dòng)機(jī)械臂末端的實(shí)現(xiàn)原理是,尋找觸控屏幕空間與機(jī)械臂運(yùn)動(dòng)空間的坐標(biāo)映射關(guān)系,實(shí)時(shí)計(jì)算指尖觸控屏幕對(duì)應(yīng)的機(jī)械臂末端坐標(biāo),使得機(jī)械臂末端跟隨觸控屏幕的指尖進(jìn)行運(yùn)動(dòng),如圖4所示。

圖4 觸控移動(dòng)機(jī)械臂末端實(shí)現(xiàn)原理Fig.4 Implementation principle of manipulator motion bytouch interaction

1.4.2 改變機(jī)械臂構(gòu)型實(shí)現(xiàn)原理

基于臂型角的逆運(yùn)動(dòng)學(xué),以臂型角和末端位姿矩陣作為輸入條件。利用這一特性,通過(guò)拖動(dòng)所設(shè)計(jì)軟件的進(jìn)度條,設(shè)置期望的臂型角,角度范圍為0～2π rad。將進(jìn)度條設(shè)置的臂型角和當(dāng)前機(jī)械臂末端位姿作為逆解算法輸入,進(jìn)行實(shí)時(shí)解算,由此改變機(jī)械臂構(gòu)型,如圖5所示。

圖5 改變機(jī)械臂構(gòu)型實(shí)現(xiàn)原理Fig.5 Implementation principle of manipulator configurationchanging by touch interaction

2 避障運(yùn)動(dòng)規(guī)劃

對(duì)比式(1)和式(2),由式中Qs(xs,ys)求得Qc(xc,yc,zc)的過(guò)程中,Qt(xt,yt,zt)的zt坐標(biāo)是未知定值,因此將二維屏幕坐標(biāo)映射到三維操作空間時(shí),得到的基坐標(biāo)系下坐標(biāo)Q0(x0,y0,z0)均在同一平面γ上。|zt|即平面γ與裁剪坐標(biāo)系原點(diǎn)的距離。針對(duì)坐標(biāo)升維后均在同一平面、操作效率不高的問(wèn)題,本文對(duì)機(jī)械臂末端的平面路徑進(jìn)行柱面運(yùn)動(dòng)規(guī)劃。

下面闡述柱面運(yùn)動(dòng)規(guī)劃的具體過(guò)程。

步驟 1建立規(guī)劃坐標(biāo)系。以結(jié)構(gòu)化障礙的結(jié)構(gòu)中心O作為規(guī)劃坐標(biāo)系原點(diǎn),y軸垂直于屏幕空間坐標(biāo)映射到操作空間的平面,且指向該平面方向,z軸平行于基坐標(biāo)系O0Z0方向,x軸方向由右手定則確定,如圖6所示。

圖6 避障運(yùn)動(dòng)規(guī)劃步驟1Fig.6 Step 1 of obstacle avoidance motion planning

步驟 2建立圓柱。以規(guī)劃坐標(biāo)系的Oz軸為圓柱的旋轉(zhuǎn)軸,以障礙中心O與避障實(shí)驗(yàn)開(kāi)始時(shí)機(jī)械臂末端位置Qlast的歐氏距離作為圓柱半徑(見(jiàn)圖7),其表達(dá)式為

圖7 避障運(yùn)動(dòng)規(guī)劃步驟2Fig.7 Step 2 of obstacle avoidance motion planning

r=|OQlast|

(5)

步驟 3以障礙中心O為端點(diǎn),向映射到平面γ的機(jī)械臂末端坐標(biāo)Q0(x0,y0,z0)方向作射線,射線與圓柱面相交于Q1。為使規(guī)劃前后的z軸坐標(biāo)一致,將Q1坐標(biāo)變換為Q2(見(jiàn)圖8),計(jì)算過(guò)程如式(6)～式(9)所示。

圖8 避障運(yùn)動(dòng)規(guī)劃步驟3Fig.8 Step 3 of obstacle avoidance motion planning

(6)

(7)

(8)

(9)

步驟 4最后,將Q2坐標(biāo)變換到基坐標(biāo)系下:

(10)

進(jìn)行避障運(yùn)動(dòng)規(guī)劃后,操作人員只需在單視角下進(jìn)行操作,即可實(shí)現(xiàn)機(jī)械臂末端繞結(jié)構(gòu)化障礙進(jìn)行圓柱面自動(dòng)避障。

3 仿真驗(yàn)證

3.1 仿真環(huán)境

本文基于虛幻4(Unreal Engine 4)游戲開(kāi)發(fā)引擎,開(kāi)發(fā)觸控交互軟件。操作人員可通過(guò)觸控的方式進(jìn)行UI交互,可實(shí)現(xiàn)移動(dòng)機(jī)械臂末端、改變臂型角、改變視角等功能。

3.2 躲避結(jié)構(gòu)化障礙實(shí)驗(yàn)

本文以KUKA iiwa R800機(jī)械臂作為對(duì)象,使用開(kāi)發(fā)的觸控交互軟件進(jìn)行躲避結(jié)構(gòu)化障礙仿真實(shí)驗(yàn),以下簡(jiǎn)稱(chēng)實(shí)驗(yàn)1。

3.2.1 實(shí)驗(yàn)1設(shè)計(jì)

實(shí)驗(yàn)內(nèi)容為操作人員將移動(dòng)機(jī)械臂末端移動(dòng)到目標(biāo)區(qū)域,同時(shí)躲避結(jié)構(gòu)化障礙,如圖9所示。

圖9 實(shí)驗(yàn)1示意圖Fig.9 Schematic diagram of Experiment 1

操作人員將采用本文規(guī)劃的柱面避障運(yùn)動(dòng)規(guī)劃方法(a組實(shí)驗(yàn))與平面結(jié)合多個(gè)視角的方法(b組實(shí)驗(yàn))進(jìn)行對(duì)比實(shí)驗(yàn),測(cè)試兩種方法的操作效率及與障礙的碰撞頻率。兩組實(shí)驗(yàn)的實(shí)驗(yàn)流程如圖10和圖11所示。

圖10 實(shí)驗(yàn)1 a組實(shí)驗(yàn)流程Fig.10 Group a procedure of Experiment 1

圖11 實(shí)驗(yàn)1 b組實(shí)驗(yàn)流程Fig.11 Group b procedure of Experiment 1

3.2.2 實(shí)驗(yàn)1結(jié)果及分析

實(shí)驗(yàn)1中的兩組實(shí)驗(yàn)分別進(jìn)行了10次,實(shí)驗(yàn)結(jié)果如表1所示。

表1 實(shí)驗(yàn)1數(shù)據(jù)Table 1 Data of Experiment 1

兩組實(shí)驗(yàn)均采用虛幻引擎里的定時(shí)器計(jì)時(shí)。a組實(shí)驗(yàn)平均時(shí)長(zhǎng)為1.88 s,機(jī)械臂均未與障礙α碰撞。b組實(shí)驗(yàn)平均時(shí)長(zhǎng)為11.39 s,實(shí)驗(yàn)機(jī)械臂與障礙α碰撞2次。

通過(guò)分析對(duì)比實(shí)驗(yàn)結(jié)果可以得出如下結(jié)論:

(1) 與進(jìn)行避障運(yùn)動(dòng)規(guī)劃前的平面躲避結(jié)構(gòu)化障礙相比,柱面避障只需對(duì)單個(gè)相機(jī)視角進(jìn)行操作,平均縮短操作時(shí)間83%,顯著提高了操作效率。

(2) 平面躲避結(jié)構(gòu)化障礙在實(shí)驗(yàn)中出現(xiàn)了2次機(jī)械臂與障礙碰撞的情況,而柱面規(guī)劃之后沒(méi)有出現(xiàn)碰撞,降低了操作員的出錯(cuò)頻率。

(3) 觸控交互相較于PRM、RRT等算法避障,有效地降低了算法實(shí)現(xiàn)的時(shí)間開(kāi)銷(xiāo),具有更高效的人機(jī)交互效能。

3.3 躲避非結(jié)構(gòu)化障礙實(shí)驗(yàn)

實(shí)驗(yàn)內(nèi)容仍然為操作機(jī)械臂使末端移動(dòng)到目標(biāo)區(qū)域,同時(shí)躲避非結(jié)構(gòu)化障礙,以下簡(jiǎn)稱(chēng)實(shí)驗(yàn)2。

3.3.1 實(shí)驗(yàn)2設(shè)計(jì)

圖12 實(shí)驗(yàn)2示意Fig.12 Schematic diagram of Experiment 2

3.3.2 實(shí)驗(yàn)2結(jié)果及分析

實(shí)驗(yàn)2共進(jìn)行了5次,機(jī)械臂末端均到達(dá)目標(biāo)區(qū)域,且過(guò)程中未與非結(jié)構(gòu)化障礙發(fā)生碰撞,實(shí)驗(yàn)過(guò)程如圖13所示。

圖13 實(shí)驗(yàn)2過(guò)程Fig.13 Process of Experiment 2

通過(guò)分析以上結(jié)果,驗(yàn)證了通過(guò)觸控交互方式改變?nèi)哂鄼C(jī)械臂構(gòu)型,從而躲避非結(jié)構(gòu)化障礙的方法切實(shí)可行,具有可操作性。

4 結(jié) 論

本文提出了一種面向冗余機(jī)械臂避障運(yùn)動(dòng)規(guī)劃的觸控交互方法。實(shí)驗(yàn)證明,此種方法可快速地躲避結(jié)構(gòu)化障礙,有效地躲避非結(jié)構(gòu)化障礙,在發(fā)揮人的決策能力和操作能力的同時(shí),提高了觸控交互的操作效率,降低了操作人員的出錯(cuò)率,具有較高效的人機(jī)交互性。