基于人工勢場引力因子的機(jī)械臂避障規(guī)劃算法

苗田恬，方厚招，安 康，孫延博，方祖華，管西強(qiáng)+

(1.上海師范大學(xué) 信息與機(jī)電工程學(xué)院，上海 201418；2.中國航天科技集團(tuán)有限公司 上海航天技術(shù)研究院 上海衛(wèi)星工程研究所，上海 201108)

0 引 言

機(jī)器人技術(shù)的發(fā)展已經(jīng)成為國家科學(xué)技術(shù)發(fā)展水平和工業(yè)自動化程度[1]的重要衡量指標(biāo)之一。其中，像人一樣的機(jī)械臂能夠高效協(xié)助人們完成加工、搬運(yùn)、整理等繁瑣的工作任務(wù)，減少、簡化冗雜的體力勞動[2]。比如KUKA公司的iiwa[3]、Kinova公司的Jaco[4]、FANUC公司的M-430iA[5]、國內(nèi)JAKA公司的Ai3[6]等機(jī)械臂的宜人化、靈巧化，讓生活、工作、娛樂變得越來越智能。因此，提高機(jī)械臂的安全性、可靠性，合理的路徑規(guī)劃與實(shí)時(shí)避障必不可少。機(jī)械臂避障路徑規(guī)劃的典型方法有遺傳算法[7]、模糊邏輯控制算法、柵格法、人工勢場法[8，9]、RRT[10]、PRM等。

針對人工勢場法存在的目標(biāo)不可達(dá)和局部極小值[11]問題，國內(nèi)外眾多學(xué)者對人工勢場法進(jìn)行了不同程度的研究和優(yōu)化。張大志等[12]提出一種基于水流場改進(jìn)的人工勢場方法，解決了傳統(tǒng)人工勢場法中的局部極小值、路徑振蕩以及目標(biāo)不可達(dá)等問題，但是在進(jìn)行動態(tài)避障時(shí)，移動機(jī)器人需要從動態(tài)障礙物前方繞過，會造成增加避障路程和避障失效概率的問題。J. Lee等[13]對附加的防碰撞條件進(jìn)行了改進(jìn)，并引入改進(jìn)算法，克服對稱對齊的用戶-障礙-目標(biāo)問題。M.H. Mabrouk等[14]用一階微分方程表示移動機(jī)器人的內(nèi)部狀態(tài)，并添加函數(shù)使之克服局部極小值，向目標(biāo)位置移動。但兩者均未考慮位置定位精度問題，即使通過人工勢場法或其變型產(chǎn)生一個(gè)完美的目標(biāo)路徑，當(dāng)路徑上的位置精度不足時(shí)，移動機(jī)器人可能會經(jīng)歷不必要的振蕩運(yùn)動或意外碰撞。S. D. Das等[15]研究了人工勢場在機(jī)器人三維運(yùn)動規(guī)劃中的應(yīng)用，然而，它的應(yīng)用僅限于僅包含點(diǎn)障礙物，以及忽略末端效應(yīng)器方向的場景。

1 人工勢場模型

1.1 人工勢場算法

人工勢場算法示意如圖1所示，Xo表示障礙物的中心位置，圓形陰影區(qū)域代表障礙物范圍，星號中心位置Xg表示目標(biāo)點(diǎn)位置，圓形Xt(x，y) 代表機(jī)械臂末端位置。機(jī)械臂末端在運(yùn)動的過程中由于同時(shí)受到障礙物斥力Fatt和目標(biāo)點(diǎn)引力Frep的作用，最終在其合力的作用下成功避開障礙物并停止在目標(biāo)點(diǎn)位置。

圖1 人工勢場算法

其中目標(biāo)點(diǎn)對機(jī)械臂產(chǎn)生的引力勢場表示為

(1)

其中，ka為引力勢場系數(shù)；l(Xt，Xg) 表示一個(gè)矢量，其大小為目標(biāo)點(diǎn)中心位置Xg(x，y) 到機(jī)械臂末端位置Xt(x，y) 的距離 |Xt(x，y)-Xg(x，y)|， 方向由機(jī)械臂末端指向目標(biāo)，Xt(x，y) 越大，對應(yīng)的引力勢場Uatt越大。

機(jī)械臂受到的障礙物斥力勢場表示為

(2)

其中，kb為斥力勢場系數(shù)；ρ0為常數(shù)，表示障礙物產(chǎn)生的斥力勢場的最大范圍；l(Xt，Xo) 表示一個(gè)矢量，為障礙物中心位置Xo(x，y) 到機(jī)械臂末端位置Xt(x，y) 的距離 |Xo(x，y)-Xg(x，y)|， 方向由障礙物指向機(jī)械臂末端，距離矢量l(Xt，Xo) 越大，對應(yīng)的斥力勢場Urep越小。

目標(biāo)點(diǎn)中心位置Xg(x，y) 和障礙物中心位置Xo(x，y) 固定，在式(1)中引力勢場表達(dá)式的引力勢場系數(shù)ka為給定的值，其值的合適取值則會影響到引力的大小，直接影響到機(jī)械臂末端的運(yùn)動路徑。同理，在式(2)斥力勢場表達(dá)式中，斥力勢場系數(shù)kb和斥力勢場作用范圍常數(shù)ρ0的確定影響到斥力的大小。研究模型參數(shù)如何設(shè)定能夠使得機(jī)械臂末端獲取最短避障路徑是本文的重點(diǎn)。因此，本文將充分分析引力勢場系數(shù)ka、 斥力勢場系數(shù)kb以及斥力勢場作用范圍常數(shù)ρ0這3個(gè)參數(shù)對機(jī)械臂末端運(yùn)動路徑的影響。

1.2 斥力勢場作用范圍常數(shù)ρ0分析

研究斥力勢場作用范圍常數(shù)ρ0變化對機(jī)械臂末端路徑的影響，定義機(jī)械臂起點(diǎn)與目標(biāo)點(diǎn)的連線為L， 障礙物中心位置到該連線的垂直距離為Lf。 結(jié)合斥力勢場公式(式(2))，可做以下分析(如圖2所示，假設(shè)障礙物與目標(biāo)點(diǎn)距離為0.5，Lf=0.4)：

圖2 以ρ0為半徑的斥力范圍作用區(qū)域

(1)當(dāng)調(diào)節(jié)斥力勢場作用范圍常數(shù)ρ0值小于障礙物中心位置到連線L的垂直距離Lf， 即ρ0處于區(qū)域A(ρ0<0.4) 時(shí)，滿足l(Xt，Xo)≥ρ0條件，障礙物對機(jī)械臂末端斥力為零，機(jī)械臂僅受到引力的影響，機(jī)械臂末端路徑為直線。

(2)當(dāng)調(diào)節(jié)斥力勢場作用范圍常數(shù)ρ0值大于障礙物中心位置到連線L的垂直距離Lf， 小于障礙物與目標(biāo)點(diǎn)距離，即ρ0處于區(qū)域B(0.4<ρ0<0.5) 時(shí)，機(jī)械臂末端受到障礙物的斥力，運(yùn)動過程中將呈現(xiàn)出遠(yuǎn)離障礙物的現(xiàn)象。

(3)當(dāng)調(diào)節(jié)斥力勢場作用范圍常數(shù)ρ0值等于障礙物中心位置與目標(biāo)點(diǎn)距離(即ρ0=0.5)， 機(jī)械臂末端在運(yùn)動的過程中不僅遠(yuǎn)離障礙物，且在達(dá)到目標(biāo)點(diǎn)時(shí)所受斥力為零，與情況(2)相比，該情況的軌跡更加平滑，機(jī)械臂末端最終以平滑路徑軌跡停止在目標(biāo)點(diǎn)位置。

(4)當(dāng)調(diào)節(jié)斥力勢場作用范圍常數(shù)ρ0值大于障礙物中心位置與目標(biāo)點(diǎn)距離，即ρ0處于區(qū)域C(ρ0>0.5) 時(shí)，當(dāng)機(jī)械臂末端到達(dá)目標(biāo)點(diǎn)附近時(shí)仍受到斥力作用，使得機(jī)械臂末端最終無法到達(dá)目標(biāo)點(diǎn)位置。

以ρ0為半徑的斥力范圍作用區(qū)域如圖2所示。

綜上分析，斥力勢場作用范圍常數(shù)ρ0等于障礙物位置與目標(biāo)點(diǎn)位置距離時(shí)，機(jī)械臂末端在運(yùn)動過程中擁有較為平滑的軌跡并成功到達(dá)目標(biāo)點(diǎn)。因此，本文選取ρ0等于障礙物位置與目標(biāo)點(diǎn)位置距離對機(jī)械臂末端路徑進(jìn)行后續(xù)分析。

1.3 引力勢場系數(shù)與斥力勢場系數(shù)比值 (ka/kb) 分析

由式(1)和式(2)得到引力勢場和斥力勢場的比值為

(3)

2 基于引力因子的最短路徑規(guī)劃模型

經(jīng)過對人工勢場法相關(guān)參數(shù)的分析，確定了斥力勢場作用范圍常數(shù)ρ0為障礙物與目標(biāo)點(diǎn)距離，引力勢場系數(shù)ka、 斥力勢場系數(shù)kb的比值 (ka/kb) 變化影響著機(jī)械臂末端的運(yùn)動路徑。然而，對于在機(jī)械臂實(shí)際的工作環(huán)境中，對于不同位置和大小的障礙物，模型參數(shù)如何設(shè)定能夠獲取最短避障路徑是本文關(guān)注的重點(diǎn)問題。因此，在本節(jié)中將構(gòu)建基于引力因子的最短避障路徑規(guī)劃模型，求解機(jī)械臂末端最短路徑及參數(shù)，使機(jī)械臂末端能夠以最短路徑避開障礙物并達(dá)到目標(biāo)點(diǎn)。

2.1 機(jī)械臂避障模型

工業(yè)SCARA的平面兩自由度Matlab機(jī)械臂避障模型如圖3所示，圖中采用平面兩自由度機(jī)器人，該機(jī)構(gòu)常用于平面多關(guān)節(jié)工業(yè)機(jī)器人(SCARA工業(yè)機(jī)器人)中。定義如圖3所示的以機(jī)器人肩關(guān)節(jié)中心的機(jī)器人笛卡爾坐標(biāo)系，機(jī)器人第一關(guān)節(jié)角度為q1，第二關(guān)節(jié)角度為q2。本模型對障礙物進(jìn)行簡化，定義障礙物區(qū)域外接圓做簡化描述障礙物。其中，Xo(x，y) 為圓心坐標(biāo)，r為圓半徑。該方法盡管增加了障礙物區(qū)域，但極大地簡化了對障礙物的描述，方便模型的建立，能夠提高避障路徑規(guī)劃的效率。

圖3 工業(yè)SCARA平面兩自由度機(jī)械臂避障模型

對于任意位置和大小的障礙物，確定斥力勢場作用范圍常數(shù)ρ0等于障礙物位置與目標(biāo)點(diǎn)位置距離后，機(jī)械臂末端運(yùn)動路徑的變化由引力勢場系數(shù)ka、 斥力勢場系數(shù)kb的比值 (ka/kb) 決定。本文將建立機(jī)械臂最短避障路徑規(guī)劃模型，通過對非線性目標(biāo)函數(shù)求最優(yōu)解，得到機(jī)械臂的最優(yōu)路徑。

2.2 機(jī)械臂最短路徑規(guī)劃算法

由1.3節(jié)分析可得 (ka/kb) 保持不變時(shí)，機(jī)械臂末端的路徑不變。ka/kb增大時(shí)機(jī)械臂末端避障路徑弧度會變小，反之，機(jī)械臂末端避障路徑弧度則會變大。當(dāng)ka遠(yuǎn)遠(yuǎn)大于kb或ka遠(yuǎn)遠(yuǎn)小于kb時(shí)，ka/kb處于無窮大和無窮小的狀態(tài)，這對于我們分析參數(shù)對避障路徑的關(guān)系增加了難度，為了使現(xiàn)象和結(jié)果更加直觀，定義引力因子κf

κf=ka/(ka+kb)

(4)

其中，當(dāng)引力勢場系數(shù)ka所占比例越大，κf則越大，機(jī)械臂末端避障路徑弧度會變小，ka/kb無窮大時(shí)κf極值為1；反之，機(jī)械臂末端避障路徑弧度會變大，ka/kb無窮小時(shí)κf極值為0。通過定義該引力因子，使我們能夠更加直觀有效地研究機(jī)器人的最短避障路徑。

機(jī)械臂末端從起始位置出發(fā)到達(dá)目標(biāo)點(diǎn)位置，在運(yùn)動過程中，研究機(jī)械臂的避障問題，時(shí)刻需要關(guān)注機(jī)械臂末端位置與障礙物中心位置之間的距離，防止機(jī)械臂與障礙物的碰撞。令機(jī)械臂末端運(yùn)動路徑點(diǎn)的集合為Ptraj

Ptraj=Γ(t，κf，λ)

(5)

其中，Ptraj為機(jī)械臂末端運(yùn)動路徑函數(shù)，能夠表示出機(jī)械臂末端任意時(shí)刻所在的位置；Ptraj與t，κf，λ有關(guān)，其關(guān)系函數(shù)定義為Γ(t，κf，λ)， 表明了機(jī)械臂末端運(yùn)動路徑與t，κf，λ之間的關(guān)系。其中t為機(jī)械臂末端運(yùn)動的時(shí)間；κf為引力因子；對于指定的障礙物和目標(biāo)點(diǎn)信息，λ為一組常數(shù)如式(6)所示

λ={ρ0，Xo(x，y)，Xg(x，y)，Xt(x，y)}

(6)

其中，ρ0為斥力勢場作用范圍常數(shù)，Xo(x，y) 為障礙物的位置，Xg(x，y) 為目標(biāo)點(diǎn)的位置，Xt(x，y) 為機(jī)械臂末端初始位置。

在機(jī)械臂末端運(yùn)動路徑點(diǎn)的集合Ptraj中，存在與障礙物中心Xo(x，y) 距離最短的點(diǎn)xmin(x，y)， 定義該最短距離為dmin(圖4)

圖4 機(jī)械臂末端的最短避障路徑規(guī)劃

dmin=Φmin(κf，λ)

(7)

其中，κf為引力因子；λ如式(6)所示為一組常數(shù)；dmin與κf、λ有關(guān)，Φ表示了最短距離dmin與κf與λ之間的函數(shù)關(guān)系。

機(jī)械臂末端的最短避障路徑規(guī)劃如圖4所示，機(jī)械臂末端的最短避障路徑，在路徑平面內(nèi)，圓形表示障礙物區(qū)域，實(shí)心大圓代表以Xo(x，y) 為圓心r為半徑的障礙物，五角星則代表目標(biāo)點(diǎn)的中心點(diǎn)位置，實(shí)心圓點(diǎn)狀路徑為機(jī)械臂末端從起點(diǎn)到目標(biāo)點(diǎn)的運(yùn)動路徑，其集合為Ptraj。

機(jī)械臂末端從起始位置出發(fā)到達(dá)目標(biāo)點(diǎn)位置，在運(yùn)動過程中，機(jī)械臂的碰撞檢測問題可以被描述為路徑末端Ptraj到障礙物中心點(diǎn)Xo(x，y) 最小距離dmin與障礙物的半徑r之間的關(guān)系問題：

(1)dmin

(2)dmin=r，則表示機(jī)械臂末端與障礙物外接圓處于相切的狀態(tài)，存在唯一解，此臨界狀態(tài)為機(jī)械臂避障的最短路徑。

(3)dmin>r，則表示機(jī)械臂末端處于障礙物范圍之外，存在多個(gè)解，機(jī)械臂末端未與障礙物發(fā)生碰撞現(xiàn)象。在多個(gè)解中，存在可以近似認(rèn)為機(jī)械臂末端軌跡與半徑r的圓相切，此時(shí)狀態(tài)下，機(jī)械臂末端的路徑長度大于(2)中的情況。因此，當(dāng)dmin=r時(shí)，機(jī)械臂末端從起始點(diǎn)到達(dá)目標(biāo)點(diǎn)的路徑最短。

當(dāng)機(jī)械臂末端路徑經(jīng)過最短路徑到達(dá)目標(biāo)點(diǎn)時(shí)，則該運(yùn)動狀態(tài)模型可以被描述為

(8)

(9)

(10)

圖5 基于人工勢場引力因子的機(jī)械臂最短避障路徑規(guī)劃控制流程

則構(gòu)建避障路徑的目標(biāo)函數(shù)

L(κf)=f2(κf)+g2(κf)

(11)

(1)給定初值κf0∈R0， 允許誤差ε>0， 令k=0。

(4)選擇步長因子μk>0， 以保證L(κfk+μkhk)

(5)計(jì)算κfk+1=κfk+μkhk。

(7)置k=k+1，轉(zhuǎn)第(1)步。

3 實(shí)驗(yàn)與討論

本節(jié)設(shè)計(jì)改變障礙物半徑和障礙物縱坐標(biāo)位置的適應(yīng)性實(shí)驗(yàn)，研究不同障礙物大小和位置的最短路徑的特征以及機(jī)械臂的關(guān)節(jié)角度路徑特征，本文在Matlab中搭建了SCARA機(jī)器人實(shí)驗(yàn)平臺，并進(jìn)行了相關(guān)算法的驗(yàn)證。與傳統(tǒng)人工勢場法相比，本文提出的算法在求解避障最短路徑的同時(shí)，實(shí)現(xiàn)了運(yùn)動軌跡與障礙物半徑的相切控制，并取得了較好的控制效果。

3.1 障礙物大小變化情況的最短路徑特征分析

表1 不同障礙物半徑的路徑參數(shù)

圖6 障礙物半徑變化的機(jī)械臂末端路徑

圖7 障礙物半徑r與引力因子的關(guān)系

3.2 障礙物圓心位置變化情況的最短路徑特征分析

表2 不同障礙物圓心位置的路徑參數(shù)

圖8 障礙物位置變化的機(jī)械臂末端路徑

圖9 障礙物縱坐標(biāo)y與引力因子的關(guān)系

3.3 機(jī)械臂關(guān)節(jié)路徑分析

上述討論針對不同圓心位置和半徑的障礙物，機(jī)械臂末端軌跡與障礙物外切圓相切且又能收斂到末端的最短避障路徑。對于不同的末端路徑，本節(jié)基于工業(yè)SCARA機(jī)器人的平面模型對機(jī)械臂的關(guān)節(jié)路徑進(jìn)行求解分析。關(guān)節(jié)路徑通過機(jī)器人逆運(yùn)動學(xué)計(jì)算得到。

本節(jié)選擇圖6(a)和圖6(c)的機(jī)械臂末端最短避障路徑為例，其中障礙物半徑r分別為0.4和0.44，中心位置為(0.6，0.6)，目標(biāo)點(diǎn)為(0.9，0.2)，機(jī)械臂末端起始位置(0.2，0.2)。圖3所示平面兩自由度機(jī)械臂模型大臂和小臂長度分別為0.4 m和0.35 m。設(shè)置避障模型坐標(biāo)系原點(diǎn)在機(jī)械臂坐標(biāo)系的(-0.5，0.3)，從而使機(jī)械臂能夠在運(yùn)動空間范圍內(nèi)完成避障運(yùn)動任務(wù)。通過控制機(jī)械臂的兩個(gè)關(guān)節(jié)電機(jī)，使得機(jī)械臂末端精確跟蹤最短避障路徑。

通過機(jī)器人逆運(yùn)動學(xué)計(jì)算得到兩種情況的機(jī)械臂關(guān)節(jié)角度變化情況如圖10所示，橫坐標(biāo)為無量綱的時(shí)間坐標(biāo)，縱坐標(biāo)表示關(guān)節(jié)角度，其中五角星表示大臂關(guān)節(jié)角度q1，實(shí)心大圓表示小臂關(guān)節(jié)角度q2，可以看到虛線為r=0.4情況，機(jī)械臂小臂關(guān)節(jié)先增大后減小，大臂關(guān)節(jié)先減小再增大，從而實(shí)現(xiàn)機(jī)械臂末端的直線運(yùn)動。在機(jī)械臂開始運(yùn)動時(shí)，圖中兩種情況的關(guān)節(jié)運(yùn)動基本相同。當(dāng)r=0.44情況下機(jī)械臂末端在t∈[0.2，1] 時(shí)間段內(nèi)，相比r=0.4情況機(jī)械臂小臂關(guān)節(jié)增大更多的角度，而同時(shí)大臂減小更多的角度，這主要是因?yàn)楫?dāng)r=0.44情況的機(jī)械臂末端出現(xiàn)弧線路徑，使得機(jī)械臂需要擺動更多的角度實(shí)現(xiàn)避障運(yùn)動。

圖10 最短避障路徑的機(jī)械臂關(guān)節(jié)角度變化情況(r為0.4和0.44)

4 結(jié)束語

本文提出一種基于人工勢場的機(jī)械臂最短避障路徑規(guī)劃模型，以引力因子κf為核心，求解機(jī)械臂二維平面內(nèi)的最短避障路徑。結(jié)果表明：當(dāng)ρ0等于障礙物與目標(biāo)點(diǎn)距離時(shí)，機(jī)械臂末端在運(yùn)動過程中的軌跡較為平滑并最終能夠到達(dá)目標(biāo)點(diǎn)；比值ka/kb與避障路徑弧度的變化成反比關(guān)系，當(dāng)該比值不變時(shí)，機(jī)械臂末端的運(yùn)動路徑不變。本文提出的算法能夠準(zhǔn)確適應(yīng)障礙物位置和大小的變化，滿足機(jī)械臂末端在最短空間狀態(tài)下避障的要求。與傳統(tǒng)人工勢場法相比，本文提出的算法在求解避障最短路徑的同時(shí)，實(shí)現(xiàn)了運(yùn)動軌跡與障礙物半徑的相切控制，并取得了較好的控制效果。進(jìn)一步為機(jī)器人路徑規(guī)劃研究提供理論意義，同時(shí)也為改進(jìn)人工勢場法提供分析思路。