王憲倫，王天宇，丁文壯

(青島科技大學機電工程學院，青島 266061)

0 引言

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

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

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

1 碰撞檢測及控制點設置

1.1 建立碰撞檢測模型

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

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

圖1 包圍盒包覆效果對比

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

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

圖2 機械臂碰撞檢測模型

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

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

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

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

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

1.2 機械臂受力及控制點設置

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

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

(1)

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

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

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

(2)

式中，J為機械臂雅可比矩陣。

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

T=JT·F

(3)

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

2 傳統(tǒng)人工勢場法缺點與改進算法

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

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

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

(2)當障礙物和目標點距離較近時，斥力過大而引力作用弱化，導致目標不可達。

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

2.2 改進的人工勢場法

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

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

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

(4)

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

(5)

如此可使機械臂與目標點較遠時，受恒力牽引靠近目標點，解決距離過遠導致所受引力過大問題；進入變力場后，隨著靠近目標點，機械臂受引力逐漸減小至0。

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

(6)

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

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

Q*=kQ·Dob

(7)

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

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

(8)

(2)特定距離下引力斥力關系設定

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

首先設置某控制點所受斥力與引力關系：

(9)

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

然后設定控制點和障礙物距離與斥力作用范圍之和比值如下：

(10)

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

(11)

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

(12)

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

kDob

(13)

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

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

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

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

圖4 橢圓模型

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

(14)

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

(15)

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

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

圖5 虛擬目標點示意圖

3 仿真與分析

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

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

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

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

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

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

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

(a) 目標不可達 (b) 目標可達

4 總結

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