吳芳 趙云波

(浙江工業(yè)大學信息工程學院 杭州 310023)

0 引言

近些年,機器人產業(yè)發(fā)展迅速,工業(yè)機器人廣泛應用于多個產業(yè)。隨著機器人技術和人工智能的蓬勃發(fā)展,人機交互和協(xié)作中的用戶安全是機器人研究的重點問題[1-2],即機器人需要具備動態(tài)避障的功能。經典的路徑規(guī)劃算法大多是在靜態(tài)場景下進行路徑規(guī)劃得到最優(yōu)或漸進最優(yōu)的路徑,如A*算法[3]、快速擴展隨機樹(rapidly-exploring random trees,RRT)算法[4]以及蟻群算法[5]等;而對于動態(tài)場景,大多使用局部路徑規(guī)劃算法,如動態(tài)窗口法(dynamic window approach,DWA)算法[6]以及人工勢場法[7]。

人工勢場法是由Khatib[7]提出的一種虛擬力法,目標點對機器人會產生一個引力場,障礙物對機器人會產生一個斥力場,最終引力和斥力的合力方向就是機器人的運動方向。人工勢場法具有實現(xiàn)簡單、計算實時性高等優(yōu)點,因此常被用于機器人的路徑規(guī)劃。對于人工勢場法存在的局部極小值以及障礙附近不可達問題,有較多文章針對不同需求對傳統(tǒng)的人工勢場法進行了改進。文獻[8]建立了新的勢場函數(shù),通過對機器人勢場區(qū)域的限制和實時調整,可以消除局部極小值問題,但是限制勢場區(qū)域的方案不夠智能化。文獻[9]考慮的是無人機系統(tǒng),根據(jù)周圍障礙物的相對運動狀態(tài)和無人機自身性能自適應調整安全距離,同時還對斥力和引力函數(shù)進行了修改,以此調整無人機的飛行方向,讓無人機更快地躲避障礙物。

機械臂的運動空間中障礙物較少,而且主要考慮的障礙物是和機器人交互的用戶,因此人工勢場法在該場景存在的主要問題是局部極小值問題。針對局部極小值問題,本文提出了一種改進的人工勢場法,每次在計算得到機器人的運動方向后,以該方向為中心,在垂直于地面、包含該方向向量的平面上以特定的角度間隔分別逆時針和順時針方向采樣90 °范圍內的運動方向,然后選取引力值和斥力值的加權和最小為最佳運動方向。

其次,由于動態(tài)場景中障礙物(本文考慮用戶手臂作為動態(tài)障礙物)的運動可能會導致規(guī)劃的路徑發(fā)生突變,路徑存在較多的尖峰點。為了處理該情況,本文在路徑規(guī)劃中考慮了用戶手臂運動的軌跡預測,以此改善規(guī)劃路徑尖峰點問題。常見的軌跡預測是使用深度學習[10-12],事先使用大量的動作數(shù)據(jù)集訓練網絡,然后應用于離線的視頻或者真實場景中,完成動作識別從而實現(xiàn)手臂接下來的運動軌跡預測。但是在實際場景中,用戶和機器人的交互動作可能是隨意的,并不是訓練集中特定的動作,因此使用深度學習進行手臂的軌跡預測還需進一步的研究。

然而,對于人類來說,手臂運動是依賴于感官的[13],比如想要拿取物品或者放置物品,那么頭部需要先轉向物品所在位置,也就是視角朝向物品所在位置,然后手臂才會去拿取或者放置物品。因此本文提出了基于頭部姿態(tài)估計的手臂運動軌跡預測,最終再結合改進的人工勢場法,從而實現(xiàn)機械臂的動態(tài)避障規(guī)劃。目前,頭部姿態(tài)的檢測主要通過深度學習實現(xiàn)[14-16],即神經網絡經過數(shù)據(jù)集的訓練可以得到網絡模型。其中文獻[16]提出的Open-Face 2.0 工具是一個較為完整、并且方便使用的集臉部輪廓檢測、臉部動作單元識別、頭部姿態(tài)以及視角朝向檢測為一體的開源工具。因此,本文選擇OpenFace 2.0 來完成頭部姿態(tài)的檢測。最終實驗結果表明,相比于傳統(tǒng)的人工勢場法,改進的算法可以有效地在機械臂應用場景中進行動態(tài)避障,在動態(tài)環(huán)境下規(guī)劃的路徑具有更少的尖峰點、路徑更加平滑,并且規(guī)劃的路徑更短。

1 問題描述

1.1 人機交互的動態(tài)場景

隨著機器人技術和產業(yè)的快速發(fā)展,在不遠的將來,機器人可能會在人們生活中隨處可見,包括未來能夠為人們提供各種家庭服務的機器人。對于家庭服務機器人來說,所處環(huán)境存在的主要動態(tài)障礙物是人類用戶。

本文的實驗場景是機器人和用戶分別位于桌子的兩側,二者需要合作完成桌面物品的分類,其中仿真環(huán)境使用的是Gazebo 仿真器,機器人是TIAGo 模型,如圖1 所示。由于機器人抓取物品技術的欠缺,展示通過物品粘貼二維碼實現(xiàn)物品的檢測識別和區(qū)分,如圖2(a)所示,該二維碼識別結果為26;而圖2(b)的二維碼識別結果為582。因此粘有不同二維碼的物品可以被區(qū)分,TIAGo 機器人只處理粘有識別結果為26 的二維碼的物品,然后粘有識別結果為582 的二維碼的物品則由用戶處理(對TIAGo機器人來說用戶手臂是障礙物),當用戶拿取該物品時,TIAGo 需要檢測用戶手臂的運動狀態(tài),并且實現(xiàn)動態(tài)避障。

圖1 TIAGo 機器人模型

圖2 二維碼檢測識別

1.2 傳統(tǒng)的人工勢場法

傳統(tǒng)的人工勢場法核心思想是構建虛擬勢場,目標點對運動物體產生引力場,引力場函數(shù)定義如式(1);障礙物對運動物體產生斥力場,斥力場函數(shù)定義如式(2);最終引力和斥力的合力方向就是物體的運動方向。

其中,ka是引力增益;X表示運動物體當前位置,Xg表示目標點的位置,X和Xg都是三維向量;kr是斥力增益;px表示運動物體和障礙物之間的距離;p0表示障礙物的影響范圍,即當物體處于障礙物的影響范圍外,所受斥力為0。

在計算引力和斥力后,2 個力的合力方向就是機器人的運動方向。但是,人工勢場法也在某些情況下存在不足。

(1) 當障礙物處于目標附近,機器人運動至目標附近時,機器人可能處于障礙物的影響范圍內。此時,目標對機器人產生的引力可能不足以“抵抗”障礙物對機器人產生的斥力,那么機器人永遠也到達不了目標,也就是目標附近存在障礙物時不可達問題,如圖3 所示。

圖3 目標附近存在障礙物時不可達問題示意圖

(2) 當機器人、障礙物以及目標點3 者處于同一直線。若障礙物對機器人產生的斥力和目標點對機器人產生的引力等大反向時,最終的合力為0,那么機器人就無法運動;若引力稍大于斥力,那么機器人可以往前運動,但往前運動后,斥力可能稍大于引力,那么機器人就往后運動,因此會陷入局部震蕩,也就是局部極小值問題,如圖4 所示。

圖4 合力為0 的局部極小值問題示意圖

2 改進的人工勢場法

2.1 基于平面采樣的局部極小值問題解決方案

本節(jié)針對人工勢場法存在的局部極小值問題,提出了基于平面采樣的解決方案。對于本文中桌面物品分類的實驗場景,再結合機械臂的運動主要是左右范圍(在前后方向上的運動范圍比較有限),因此機械臂的運動方向采樣不是整個三維(3D)空間,而是在垂直于地面(XY平面)、包含合力向量的平面上進行采樣。為了便于展示,通過YZ二維平面的位置采樣進行采樣機制的展示說明,如圖5 所示。

圖5 YZ 二維平面的位置采樣示意圖

首先,根據(jù)目標點對機械臂的引力和障礙物對機械臂的斥力(弧線內側是斥力范圍)得到合力,然后以該合力方向為中心,在YZ平面上分別以15°為間隔,得到逆時針和順時針90 °范圍內其他的位置采樣點。對于距離障礙物較近的采樣點來說,它們受到的斥力較大;而對于處于障礙物斥力范圍邊緣的采樣點來說,它們受到的斥力較小。一般情況下,希望機器人遠離障礙物,又盡快抵達目標點。從斥力、引力層面看,就是在選擇斥力較小的采樣方向的同時,又要讓引力更小,其實就是障礙物斥力范圍邊緣的那些采樣方向。通過代價函數(shù)表示的話,則是

其中,x表示機器人的位置;Fat(x)、Fre(x) 分別代表引力函數(shù)、斥力函數(shù);α和β是權重,分別代表對引力和斥力的懲罰力度,在本文中以執(zhí)行任務過程中避免對用戶的碰撞為目標,因此對斥力的懲罰力度較大,選擇α=0.3,β=0.7。根據(jù)該代價函數(shù)選擇使得代價最低的采樣點。

其次,在通過圖像展示了平面采樣機制后,通過具體的式(4)來表示采樣點以及最佳采樣點的選擇。

為了更加清晰地說明不同方向采樣的單位向量和合力向量F(fx,fy,fz) 之間的運算關系,接下來以為例。

式中,θxy表示合力方向在XY平面的夾角,θyz表示合力方向在垂直于XY平面、且包含合力向量的平面的夾角,θ5表示合力向量逆時針旋轉的第一個采樣點在采樣平面的夾角,(fx,fy,fz) 分別表示合力向量在X、Y和Z軸上的各個分量,而分別表示合力向量逆時針旋轉15 °的采樣點在X、Y和Z軸上的各個分量。transCos函數(shù)用于余弦角度值的轉換,轉換規(guī)則為

而transTan函數(shù)用于正切角度值的轉換,轉換規(guī)則為

而trans函數(shù)用于合力向量逆時針和順時針旋轉的采樣點角度轉換,轉換規(guī)則為

在計算得到各個采樣點之后,根據(jù)式(9)和(10)可以選出最佳采樣點αt+1,從而解決人工勢場法存在的局部極小值問題。

圖6 是基于平面采樣解決人工勢場法局部極小值問題的流程圖。

圖6 基于平面采樣的改進人工勢場法流程圖

2.2 基于頭部姿態(tài)估計的軌跡預測方法

本文所考慮的人機交互場景中,機器人主要面對的動態(tài)障礙物是用戶,而對于本文的實驗場景來說,機器人運動過程中主要考慮的是用戶手臂的運動。在桌面物品分類這樣一個人機協(xié)作場景中,用戶的頭部姿態(tài)很大程度上預示了手臂接下來的運動方向以及目標位置。因此,本文提出了人機交互場景下基于用戶頭部姿態(tài)進行用戶手臂運動的軌跡預測方法。

其中,頭部姿態(tài)的檢測通過一個比較完善、方便使用并且開源的臉部行為分析工具——OpenFace2.0來實現(xiàn)。若要實現(xiàn)頭部姿態(tài)估計,首先需要實現(xiàn)臉部特征點檢測和跟蹤。為了實現(xiàn)臉部特征點檢測和跟蹤,OpenFace 2.0 使用了卷積專家約束局部模型(convolutional experts constrained local model,CECLM[17]),該模型使用點分布模型(point distribution model,PDM)捕捉特征點形狀的變化,使用補丁專家(patch experts)對每個特征點的局部外觀的變化建模。同時,OpenFace 2.0 為了保證檢測的實時性,進行了深度模型和稀疏響應圖計算的優(yōu)化。

由于CE-CLM 內部使用了臉部特征點的3D 表示,并且根據(jù)正交投影將臉部特征點的3D 表示投影到圖像上,因此當臉部特征點檢測后,OpenFace 2.0 就可以實現(xiàn)頭部姿態(tài)估計。圖7 和圖8 分別是頭部朝向左邊和右邊的頭部姿態(tài)估計展示圖。頭部姿態(tài)估計結果不僅僅在圖像上有所展示,而且會得到一個六維的頭部姿態(tài)向量P(Tx,Ty,Tz,Rx,Ry,Rz)。其中Tx、Ty、Tz分別表示頭部在X、Y、Z3 個軸上的偏移量,而Rx、Ry、Rz分別表示頭部在X、Y、Z3 個軸上旋轉的旋轉量。

圖7 頭部朝向左邊的頭部姿態(tài)估計展示圖

圖8 頭部朝向右邊的頭部姿態(tài)估計展示圖

本文的實驗場景主要涉及物品在左、右方向上的拿取和擺放,而且機械臂的運動在前后范圍比較有限,因此只需要考慮頭部姿態(tài)在Z軸上的轉動,即頭部的左右轉動,以此來預測用戶手臂在左右方向的軌跡。圖9 是基于頭部姿態(tài)估計的軌跡預測流程圖。

圖9 基于頭部姿態(tài)估計的軌跡預測流程圖

圖10 所示為基于平面位置采樣的改進人工勢場法采樣示意圖。實線圓圈表示真實障礙物的實際斥力范圍,虛線圓圈表示預測的障礙物斥力范圍。黑實線箭頭表示初始合力方向,黑虛線箭頭表示以初始合力方向為中心,左右各90 °范圍、以15 °為間隔的采樣方向,淺色虛線箭頭表示最佳采樣方向的候選采樣方向(淺色虛線箭頭只是為了展示采樣機制而已,實際上不存在候選采樣方向,最佳采樣方向直接根據(jù)式(3)計算得到)。圖10(a)是不進行軌跡預測的采樣示意圖。從圖10(a)可以看出,若不進行軌跡預測,機械臂和障礙物距離較近時才會觸發(fā)采樣機制,而且此時障礙物的斥力范圍(虛線空心圓圈)比較大,所以機械臂計算得到的最佳采樣點不夠平滑。圖10(b)是進行軌跡預測但不縮放預測的障礙物斥力范圍的采樣示意圖,圖10(c)是進行軌跡預測且縮小預測的障礙物斥力范圍的采樣示意圖。從圖10(b)和(c)可以看出,此時即使機械臂和障礙物真實距離比較遠,但是機械臂和預測的障礙物位置比較近,也會觸發(fā)采樣機制。

圖10 基于平面位置采樣的改進人工勢場法采樣示意圖

圖10(b)和(c)的區(qū)別在于是否對預測的障礙物斥力范圍進行縮放。如圖10(b)所示,若對預測的斥力范圍不進行縮放,即使對障礙物位置進行了預測,僅僅讓機械臂提早偏離障礙物,機械臂仍會以較大的轉折幅度進行運動,導致最終的路徑出現(xiàn)拐點。如圖10(c)所示,縮小預測的障礙物斥力范圍則能夠讓機器人提早以較為平緩的幅度偏離避開障礙物。經過調試,采用3/4 作為對預測的障礙物斥力范圍進行縮放較為合適,即若當前時刻是t時刻,設定預測的t+1 時刻障礙物的斥力范圍是初始斥力范圍的3/4,而預測的t+2 時刻障礙物的斥力范圍是初始斥力范圍的(3/4)2。最終,基于軌跡預測和改進人工勢場法的機械臂動態(tài)避障方案的總流程如圖11 所示。

圖11 基于軌跡預測和改進人工勢場法的機械臂動態(tài)避障方案的總流程圖

3 實驗及結果

為了驗證本文提出算法的可行性和有效性,本節(jié)對傳統(tǒng)人工勢場法、改進人工勢場法和結合軌跡預測的改進人工勢場法進行了對比實驗,并對3 種算法規(guī)劃得到的路徑長度、路徑點數(shù)量、規(guī)劃時間和尖峰點數(shù)量進行了定量分析。實驗所使用的計算機是 搭載Intel i7-9750H CPU、Ubuntu16.04 操作系統(tǒng)的惠普筆記本電腦,仿真環(huán)境使用的是Gazebo 仿真器。

圖12 所示為Gazebo 仿真器中的實驗場景,TIAGo 一號代表機器人,TIAGo 二號代表與機器人交互的用戶。因為仿真環(huán)境下人的運動不方便模擬,因此通過TIAGo 機器人模擬人類用戶。貼有二維碼的物品1 表示物品分類任務中機器人需要處理的物品,而貼有二維碼的物品2 表示物品分類任務中用戶需要處理的物品。

圖12 人機交互動態(tài)場景的Gazebo 環(huán)境示意圖

傳統(tǒng)人工勢場法、改進人工勢場法以及結合軌跡預測的改進人工勢場法在三維空間下的路徑如圖13所示?？梢悦黠@看出,傳統(tǒng)人工勢場法由于沒有采樣機制和軌跡預測,機器人的運動路徑存在一定的振蕩,規(guī)劃得到的路徑存在較多的尖峰點。單純改進人工勢場法雖然增加了采樣機制,可以避免發(fā)生振蕩,但是路徑還是存在著突變的尖峰點。而結合軌跡預測的改進人工勢場法規(guī)劃的路徑不僅不存在振蕩,而且更加平滑。三種算法規(guī)劃的路徑長度、路徑點數(shù)量、尖峰點數(shù)量以及規(guī)劃時間如表1 所示,相比傳統(tǒng)人工勢場法,結合軌跡預測的改進人工勢場法所規(guī)劃的路徑長度縮短約18%,路徑點數(shù)量減少約23%,尖峰點數(shù)量減少100%,規(guī)劃時間縮短約27%。

表1 實驗數(shù)據(jù)對比

圖13 三維空間運動路徑曲線圖

為了更加詳細地說明本算法的設計方案,對實驗結果進行了2.1 節(jié)描述的采樣機制分析。圖14所示為結合軌跡預測的改進人工勢場法的采樣機制分析。五角星表示目標點,實線圓圈表示當前障礙物的斥力范圍;小實心圓表示障礙物位置以及預測的障礙物位置,小實心圓越來越小的方向是障礙物的運動方向;虛線圓圈表示預測的障礙物的斥力范圍;大實心圓表示機器人位置,大實心圓周圍的黑色箭頭指向初始合力方向;虛線箭頭指向可選采樣方向,灰色實線箭頭指向根據(jù)采樣機制得到的最佳采樣方向,即機器人接下來會沿著該采樣方向運動一個步長。實驗數(shù)據(jù)如圖14 所示,子圖(a)和(c)分別是路徑點數(shù)量為4 和7 的采樣機制示意圖,而子圖(b)和(d)分別是路徑點數(shù)量為5 和8 的機器人和障礙物軌跡圖,即子圖(b)和(d)分別對子圖(a)和(c)采樣機制的結果進行了驗證。根據(jù)采樣機制需要遠離障礙物、又盡快抵達目標點的工作原理,對于遠離障礙物的條件,采樣機制選擇那些處于障礙物斥力范圍外的采樣方向,即斥力最小的采樣方向;對于盡快抵達目標點的條件,采樣機制需要選擇那些朝向目標點的采樣方向,而機器人沿著朝向目標點的采樣方向運動一個步長所處位置的引力小于沿著其他采樣方向運動一個步長所處位置的引力,即引力最小的采樣方向。因此,機器人沿著最佳采樣方向運動一個步長所處位置的斥力和引力兩者的加權和是最小的,即最小化式(3)所表述的代價函數(shù)值,即min(αFat(x)+βFre(x)),其中,x表示機器人的位置,Fat(x) 是引力函數(shù),Fre(x) 是斥力函數(shù)。為了對采樣機制進行分析,分別選取了機器人路徑前4 個路徑點和前7 個路徑點所形成的軌跡,然后根據(jù)min(αFat(x)+βFre(x)) 計算得到最佳采樣方向如圖14(a)和(c)中的灰色實線箭頭所示,并且圖14(b)和(d)分別驗證了機器人實際上是沿著該最佳采樣方向運動的。

圖14 結合軌跡預測的改進人工勢場法的采樣機制分析

4 結論

本文針對傳統(tǒng)人工勢場法存在的局部極小值問題,采用了基于平面位置采樣的解決方案,避免機器人的運動發(fā)生振蕩。同時,根據(jù)頭部姿態(tài)和手臂運動的關聯(lián)關系,通過OpenFace 2.0 進行頭部姿態(tài)估計,進而對用戶手臂運動進行軌跡預測,可以使得規(guī)劃的路徑更加平滑。通過實驗數(shù)據(jù)發(fā)現(xiàn),本文提出的基于軌跡預測和改進人工勢場法的機械臂動態(tài)避障規(guī)劃方法不僅可以解決傳統(tǒng)人工勢場法的局部極小值問題,還可以應用于動態(tài)環(huán)境下的機械臂動態(tài)避障場景。