基于軌跡預(yù)測與改進(jìn)人工勢場法的機(jī)械臂動態(tài)避障規(guī)劃方法①

吳芳 趙云波

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

0 引言

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

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

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

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

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

1 問題描述

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

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

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

圖1 TIAGo 機(jī)器人模型

圖2 二維碼檢測識別

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

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

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

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

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

圖3 目標(biāo)附近存在障礙物時不可達(dá)問題示意圖

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

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

2 改進(jìn)的人工勢場法

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

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

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

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

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

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

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

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

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

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

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

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

圖6 基于平面采樣的改進(jìn)人工勢場法流程圖

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

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

其中,頭部姿態(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)捕捉特征點形狀的變化,使用補(bǔ)丁專家(patch experts)對每個特征點的局部外觀的變化建模。同時,OpenFace 2.0 為了保證檢測的實時性,進(jìn)行了深度模型和稀疏響應(yīng)圖計算的優(yōu)化。

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

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

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

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

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

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

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

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

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

3 實驗及結(jié)果

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

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

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

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

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

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

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

圖14 結(jié)合軌跡預(yù)測的改進(jìn)人工勢場法的采樣機(jī)制分析

4 結(jié)論

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