基于改進(jìn)人工勢場法的機(jī)械臂路徑規(guī)劃*

王憲倫，王天宇，丁文壯

(青島科技大學(xué)機(jī)電工程學(xué)院，青島 266061)

0 引言

制造業(yè)智能化快速發(fā)展的背景下，以機(jī)器人為代表的智能設(shè)備在產(chǎn)品生產(chǎn)過程中自動(dòng)化應(yīng)用愈加廣泛[1]。實(shí)現(xiàn)機(jī)器人自主作業(yè)，需要各類傳感器的加持，更需要機(jī)器人能夠進(jìn)行快速可靠的路徑規(guī)劃。

目前，常用的路徑規(guī)劃方法主要有以下幾種：A*算法[2]、快速擴(kuò)展隨機(jī)樹(RRT)[3]、蟻群算法[4]及人工勢場法[5]等。其中，人工勢場法憑借其簡單直接的數(shù)學(xué)描述的優(yōu)點(diǎn)，在機(jī)器人路徑規(guī)劃領(lǐng)域應(yīng)用廣泛。該算法結(jié)構(gòu)簡單，實(shí)時(shí)性好，生成路徑較為平滑，但是也存在一些問題，如易陷入局部最小值陷阱和目標(biāo)不可達(dá)等[6]。陳勁峰等[7]提出一種逃離或穿越的策略以解決局部最小值問題。XI等[8]將人工勢場法與RRT算法結(jié)合，利用偏目標(biāo)的RRT方式引導(dǎo)跳出局部最小值，并提高規(guī)劃效率。SANG等[9]將A*算法與人工勢場法結(jié)合，將路徑分為多目標(biāo)點(diǎn)并實(shí)現(xiàn)切換，極大降低陷入最小值的可能。以上學(xué)者等針對人工勢場法的缺點(diǎn)進(jìn)行了改進(jìn)，但是目前包括其在內(nèi)的多數(shù)學(xué)者仍主要以移動(dòng)機(jī)器人為研究對象，而算法隨機(jī)器人維度的增加計(jì)算更為復(fù)雜，因此針對多自由度的三維機(jī)械臂領(lǐng)域進(jìn)行研究仍是一重點(diǎn)方向。

本文以三維機(jī)械臂為研究對象，針對原算法不足之處進(jìn)行改進(jìn)，包括對不同距離下的引力斥力函數(shù)重新定義，給出一種跳出局部最小值陷阱的解決方案，并對求解路徑進(jìn)一步優(yōu)化，最后在仿真平臺驗(yàn)證了改進(jìn)算法應(yīng)用于機(jī)械臂的有效性。

1 碰撞檢測及控制點(diǎn)設(shè)置

1.1 建立碰撞檢測模型

現(xiàn)實(shí)場景中各類物體外形復(fù)雜，機(jī)械臂與障礙物進(jìn)行有效的碰撞檢測是路徑規(guī)劃的前提。目前機(jī)械臂的碰撞檢測主要有傳感式和建模式，其中建模式不需外部傳感設(shè)備，在碰撞發(fā)生前即可預(yù)警，因此在實(shí)際應(yīng)用中該方法更為常用[9]。

包圍盒算法是建模式碰撞檢測中的常用方法[10]，算法基本思想是用體積稍大且特性簡單的幾何體來近似地代替復(fù)雜的幾何對象。常見的包圍盒算法有包圍球、AABB包圍盒、OBB包圍盒等，各包覆效果如圖1所示。

圖1 包圍盒包覆效果對比

其中，OBB包圍盒[11]可以根據(jù)被包圍對象的形狀特點(diǎn)盡可能地包覆對象，同時(shí)可以涵蓋被包圍物體的姿態(tài)信息，與另外兩種類型包圍盒相比雖然計(jì)算復(fù)雜度有所增加，但本文研究所用機(jī)械臂和障礙物外形規(guī)整，權(quán)衡之下該法更優(yōu)。

故本文采用DH參數(shù)法對機(jī)械臂運(yùn)動(dòng)學(xué)建模后，基于OBB包圍盒算法以圓柱體包覆機(jī)械臂建立碰撞檢測模型。具體建模規(guī)則為：以機(jī)械臂坐標(biāo)系原點(diǎn)間距作為母線長度，以坐標(biāo)原點(diǎn)所在平面作為圓柱體上下圓面，以機(jī)械臂各連桿半徑作為圓柱體半徑，搭建模型如圖2所示。

圖2 機(jī)械臂碰撞檢測模型

機(jī)械臂與障礙物進(jìn)行距離檢測時(shí)，為簡化模型，視機(jī)械臂連桿為線段，擴(kuò)大障礙物體積補(bǔ)償簡化的包覆圓柱體直徑，碰撞與否根據(jù)障礙物中心點(diǎn)與連桿線段關(guān)系的兩種情況判斷。

第一種情況：點(diǎn)在線段上。該情況發(fā)生時(shí)，機(jī)械臂已與障礙物碰撞。

第二種情況：點(diǎn)在線段外。在此用三點(diǎn)法進(jìn)行判斷。計(jì)算連桿線段的兩端點(diǎn)P1、P2和中點(diǎn)Pm與障礙物中心點(diǎn)O距離，設(shè)點(diǎn)P1、P2和Pm與點(diǎn)O距離分別為d1、d2和dm。

若d1>dm>d2或d2>dm>d1，點(diǎn)O與線段垂線投影在線段延長線上，此時(shí)碰撞檢測最小距離為d2或d1，如圖3上面兩幅圖所示；若d1>dmdm

圖3 碰撞檢測最小距離確定方法

1.2 機(jī)械臂受力及控制點(diǎn)設(shè)置

運(yùn)用人工勢場法進(jìn)行路徑規(guī)劃需要將工作空間中的力映射到機(jī)械臂關(guān)節(jié)空間。根據(jù)虛功原理可得到機(jī)械臂工作空間與構(gòu)型空間的力和力矩轉(zhuǎn)換關(guān)系，如式(1)所示。

F·(δx,δy,δz)T=T·δq

(1)

式中，F(xiàn)為工作空間廣義力；(δx,δy,δz)為工作空間中虛位移；δq為構(gòu)造空間中虛位移。

根據(jù)機(jī)械臂雅可比矩陣定義有：

(δx,δy,δz)T=J·δq

(2)

式中，J為機(jī)械臂雅可比矩陣。

式(1)和式(2)聯(lián)立可得：

T=JT·F

(3)

通過式(3)可將工作空間中力轉(zhuǎn)換為構(gòu)型空間中的力和力矩。在此將機(jī)械臂上評估所受工作空間中引力與斥力合力的點(diǎn)稱為機(jī)械臂控制點(diǎn)。顯然，單一控制點(diǎn)無法滿足機(jī)械臂各連桿的避障要求，故以DH法建系坐標(biāo)原點(diǎn)及在其間插值設(shè)置多個(gè)控制點(diǎn)，控制點(diǎn)個(gè)數(shù)根據(jù)連桿長度而定，對各點(diǎn)進(jìn)行引力斥力的合力計(jì)算后平移至對應(yīng)關(guān)節(jié)末端，并將各連桿合力根據(jù)控制點(diǎn)個(gè)數(shù)求取平均值作為各連桿的最終受力，以應(yīng)用于人工勢場法進(jìn)行路徑規(guī)劃。

2 傳統(tǒng)人工勢場法缺點(diǎn)與改進(jìn)算法

2.1 傳統(tǒng)人工勢場法缺點(diǎn)

人工勢場法是一種虛擬力場法。其基本原理是將機(jī)器人在環(huán)境中的運(yùn)動(dòng)視為一種機(jī)器人在虛擬的人工勢力場的運(yùn)動(dòng)。障礙物對機(jī)器人產(chǎn)生斥力，目標(biāo)點(diǎn)對機(jī)器人產(chǎn)生引力，引力和斥力的合力作為機(jī)器人的驅(qū)動(dòng)力，以控制機(jī)器人的運(yùn)動(dòng)方向和位置。但是該算法也存在一些問題：

(1)機(jī)械臂控制點(diǎn)和目標(biāo)點(diǎn)距離較遠(yuǎn)時(shí)，引力過大而斥力作用弱化，可能與障礙物發(fā)生碰撞。

(2)當(dāng)障礙物和目標(biāo)點(diǎn)距離較近時(shí)，斥力過大而引力作用弱化，導(dǎo)致目標(biāo)不可達(dá)。

(3)在局部區(qū)域內(nèi)某點(diǎn)引力和斥力大小相等方向相反時(shí)，算法易陷入局部最小值和局部震蕩。

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

(1)重新定義引力斥力函數(shù)

本文以引力場為全局力場，斥力場為局部力場，建立新的引力斥力場函數(shù)，機(jī)械臂控制點(diǎn)所受引力與斥力分別為引力場與斥力場對距離的導(dǎo)數(shù)。

首先將引力場劃界分為恒力場與變力場，建立引力場函數(shù)如下所示：

(4)

式中，dL表示恒力場和變力場的邊界距離；d(q,qgoal)表示控制點(diǎn)到目標(biāo)點(diǎn)的距離；ξ表示引力系數(shù)，對應(yīng)的引力函數(shù)為：

(5)

如此可使機(jī)械臂與目標(biāo)點(diǎn)較遠(yuǎn)時(shí)，受恒力牽引靠近目標(biāo)點(diǎn)，解決距離過遠(yuǎn)導(dǎo)致所受引力過大問題；進(jìn)入變力場后，隨著靠近目標(biāo)點(diǎn)，機(jī)械臂受引力逐漸減小至0。

設(shè)定斥力場函數(shù)時(shí)，在斥力勢場函數(shù)中引入安全距離，以保證機(jī)械臂和障礙物之間的最小距離不會發(fā)生碰撞。建立的斥力場函數(shù)如下所示：

(6)

式中，η表示斥力系數(shù)；D(q)表示當(dāng)前控制點(diǎn)到障礙物點(diǎn)的距離；Q*表示斥力場的作用范圍；k表示安全距離系數(shù)，安全系數(shù)的選取應(yīng)大于等于0；Dob表示障礙物在某一個(gè)方向上的長度(球體為其半徑長度；長方體為其各邊長度一半)。

斥力場作用范圍確定如式(7)所示：

Q*=kQ·Dob

(7)

式中，kQ為長度系數(shù)，其值應(yīng)大于1。

對應(yīng)的斥力函數(shù)為：

(8)

(2)特定距離下引力斥力關(guān)系設(shè)定

前文通過建立新的引力斥力函數(shù)，解決了機(jī)械臂距離目標(biāo)點(diǎn)較遠(yuǎn)時(shí)引力過大而斥力作用弱化可能出現(xiàn)的碰撞問題。針對障礙物距離目標(biāo)點(diǎn)過近導(dǎo)致的目標(biāo)不可達(dá)問題，本小節(jié)在建立的引力斥力函數(shù)基礎(chǔ)上，提出一種特定距離下的引力與斥力關(guān)系的設(shè)定方式。

首先設(shè)置某控制點(diǎn)所受斥力與引力關(guān)系：

(9)

式中，ρ為該控制點(diǎn)處斥力相對引力的倍數(shù)。

然后設(shè)定控制點(diǎn)和障礙物距離與斥力作用范圍之和比值如下：

(10)

將Q*+kDob以x表示，式(9)可以表示為：

(11)

將式(11)進(jìn)行化簡得到：

(12)

根據(jù)式(12)，即可求得特定距離下控制點(diǎn)所受斥力的大小，配合新建立的引力斥力函數(shù)，解決原算法目標(biāo)不可達(dá)的問題。但需要注意，上式適用條件為：

kDob

(13)

(3)局部最小值和局部震蕩處理

人工勢場法根據(jù)機(jī)械臂所受合力實(shí)現(xiàn)對其控制，當(dāng)機(jī)械臂控制點(diǎn)在某一位置處所受到的引力和斥力之和為0時(shí)，即出現(xiàn)局部最小值問題；如果合力不為0而控制點(diǎn)在連續(xù)的兩個(gè)位置所受合力方向相反時(shí)，則會出現(xiàn)局部震蕩，以上兩種情況都會導(dǎo)致機(jī)械臂難以到達(dá)目標(biāo)位置。

人工勢場法因其局部極小值問題導(dǎo)致目標(biāo)不可達(dá)現(xiàn)象而限制了其應(yīng)用。只有當(dāng)虛擬目標(biāo)點(diǎn)的引力場起到主導(dǎo)作用時(shí)才能有效脫離局部極小點(diǎn)位置。在此本文基于虛擬目標(biāo)引力場，提出一種橢球模型策略來解決上述問題。

建立橢球模型首先建立橢圓模型：以障礙物中心為原點(diǎn)O；橢圓所在的平面由障礙物中心點(diǎn)、控制點(diǎn)及目標(biāo)點(diǎn)確定。橢圓的長軸AB為控制點(diǎn)與障礙物中心點(diǎn)所在直線，橢圓的焦點(diǎn)F1F2為產(chǎn)生局部極小值或者局部震蕩的控制點(diǎn)所在位置，建立橢圓模型如圖4所示。

圖4 橢圓模型

設(shè)橢圓的短半軸長度b=(1+k)Dob，橢圓長半軸根據(jù)橢圓的短半軸長度b和產(chǎn)生局部最小值或者局部震蕩的控制點(diǎn)與障礙物中心點(diǎn)的距離c確定：若b

(14)

若b≥c，則對a賦值為a=b，建立橢圓方程，此時(shí)橢圓成為圓形。將橢圓模型應(yīng)用于三維空間時(shí)，需將其拓展為橢球模型，在此令橢球Z軸方向半軸長度值為b，建立橢球方程如下：

(15)

建立方程后，分別計(jì)算控制點(diǎn)與障礙物中心點(diǎn)矢量e1，障礙物中心點(diǎn)與目標(biāo)點(diǎn)的矢量e2以及矢量叉乘后的法向量n，通過e1、n即可建立橢球與機(jī)械臂基座之間的坐標(biāo)變換關(guān)系。

局部最小值問題通過在橢球XOY面設(shè)立動(dòng)態(tài)虛擬目標(biāo)點(diǎn)處理。當(dāng)機(jī)械臂陷入局部最小值陷阱后，在控制點(diǎn)所在橢圓焦點(diǎn)上方圓弧依次設(shè)定臨時(shí)目標(biāo)點(diǎn)并建立引力勢場，當(dāng)機(jī)械臂到達(dá)橢圓的近心點(diǎn)時(shí)視為已脫離局部最小值陷阱。當(dāng)一個(gè)方向路徑規(guī)劃出現(xiàn)問題時(shí)，令機(jī)械臂原路返回至原焦點(diǎn)位置，反方向設(shè)置虛擬目標(biāo)點(diǎn)，如圖5所示。

圖5 虛擬目標(biāo)點(diǎn)示意圖

3 仿真與分析

機(jī)械臂初始關(guān)節(jié)角為[1.65 0.09 -1.76 -1.09 -1.68 1.31]，目標(biāo)物位姿[-0.35 -0.3 0.005 2.2 -0.64 0.34]，在初始點(diǎn)和目標(biāo)點(diǎn)之間隨機(jī)位置設(shè)置幾組障礙物，以驗(yàn)證改進(jìn)算法下機(jī)械臂避障性能。設(shè)定實(shí)驗(yàn)參數(shù)ξ=500，η=10，KQ=0.9，k=1，ρ=5，限制各關(guān)節(jié)角步長π/90，實(shí)驗(yàn)仿真結(jié)果如圖6～圖9所示。

(a) 無障礙物路徑規(guī)劃 (b) 單障礙物路徑規(guī)劃

圖6中機(jī)械臂分別在有無障礙物情況下由初始點(diǎn)至目標(biāo)物進(jìn)行路徑規(guī)劃。圖6a中，機(jī)械臂在無障礙物情況下順利到達(dá)目標(biāo)位置。圖6b中，雖設(shè)置障礙物但機(jī)械臂行進(jìn)及避障搜索中未遇到產(chǎn)生局部最小值情況，因此兩種情況下機(jī)械臂均以較平滑路徑到達(dá)目標(biāo)物位置。

在圖7a中，設(shè)置多個(gè)障礙物，機(jī)械臂經(jīng)過障礙物1進(jìn)行避障時(shí)，由于合力作用，產(chǎn)生局部震蕩情況，機(jī)械臂末端路徑反復(fù)，難以跳出繼續(xù)行進(jìn)?；诟倪M(jìn)算法下局部震蕩解決策略，在圖7b中，障礙物位置不變，機(jī)械臂在相同位置遇到局部震蕩完成判定以后，迅速跳出局部震蕩情況并在后續(xù)搜索中成功避開障礙物2，到達(dá)目標(biāo)位置。本次規(guī)劃中機(jī)械臂共設(shè)置13組控制點(diǎn)，運(yùn)行共200步，對步數(shù)間隔提取，部分控制點(diǎn)與障礙物1、2的實(shí)時(shí)最小距離如圖8所示。

(a) 發(fā)生局部震蕩 (b) 跳出局部震蕩

(a) 與障礙物1實(shí)時(shí)距離 (b) 與障礙物2實(shí)時(shí)距離

在圖9a中，在目標(biāo)物附近設(shè)置障礙物，機(jī)械臂靠近時(shí)，由于障礙物較大的斥力作用，機(jī)械臂反復(fù)震蕩停滯不前，末端產(chǎn)生反復(fù)離散的路徑點(diǎn)。在圖9b中，障礙物位置不變，機(jī)械臂行進(jìn)至目標(biāo)物附近時(shí)，判斷障礙物與目標(biāo)物位置，根據(jù)特定距離下的斥力設(shè)定方式，使斥力相對減小，機(jī)械臂得以連續(xù)平滑的到達(dá)目標(biāo)位置。以上圖6、圖7、圖9所列幾種情況的對比實(shí)驗(yàn)表明，改進(jìn)人工勢場法較好解決了傳統(tǒng)算法缺陷，機(jī)械臂能有效完成三維空間的路徑規(guī)劃，滿足規(guī)劃過程中的避障要求。

(a) 目標(biāo)不可達(dá) (b) 目標(biāo)可達(dá)

4 總結(jié)

針對三維空間機(jī)械臂的路徑規(guī)劃，本文設(shè)計(jì)了一種改進(jìn)的人工勢場算法。將引力場作為全局引力場，斥力場作為局部力場，對引力斥力勢場函數(shù)再定義；接著給出一種特定距離下引力斥力的設(shè)定方法作為輔助以解決障礙物離目標(biāo)物過近時(shí)導(dǎo)致的目標(biāo)不可達(dá)問題；然后設(shè)計(jì)一種基于橢圓模型的局部震蕩跳出策略，以提高機(jī)械臂的路徑規(guī)劃能力；最后仿真結(jié)果表明，本文建立的碰撞檢測模型能夠?qū)φ系K物進(jìn)行實(shí)時(shí)有效的碰撞檢測。改進(jìn)后的算法相比傳統(tǒng)算法，對引力斥力的數(shù)值及作用范圍進(jìn)行不同情形下的多元設(shè)定更加合理，機(jī)械臂可根據(jù)不同問題快速判斷并反應(yīng)，通過設(shè)置動(dòng)態(tài)虛擬目標(biāo)點(diǎn)或更改障礙物斥力大小的方式，解決了局部震蕩和目標(biāo)不可達(dá)問題，滿足三維空間中機(jī)械臂路徑規(guī)劃及避障要求。