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

苗田恬，方厚招，安 康，孫延博，方祖華，管西強+

(1.上海師范大學 信息與機電工程學院，上海 201418；2.中國航天科技集團有限公司 上海航天技術研究院 上海衛星工程研究所，上海 201108)

0 引 言

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

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

1 人工勢場模型

1.1 人工勢場算法

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

圖1 人工勢場算法

其中目標點對機械臂產生的引力勢場表示為

(1)

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

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

(2)

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

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

1.2 斥力勢場作用范圍常數ρ0分析

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

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

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

(2)當調節斥力勢場作用范圍常數ρ0值大于障礙物中心位置到連線L的垂直距離Lf， 小于障礙物與目標點距離，即ρ0處于區域B(0.4<ρ0<0.5) 時，機械臂末端受到障礙物的斥力，運動過程中將呈現出遠離障礙物的現象。

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

(4)當調節斥力勢場作用范圍常數ρ0值大于障礙物中心位置與目標點距離，即ρ0處于區域C(ρ0>0.5) 時，當機械臂末端到達目標點附近時仍受到斥力作用，使得機械臂末端最終無法到達目標點位置。

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

綜上分析，斥力勢場作用范圍常數ρ0等于障礙物位置與目標點位置距離時，機械臂末端在運動過程中擁有較為平滑的軌跡并成功到達目標點。因此，本文選取ρ0等于障礙物位置與目標點位置距離對機械臂末端路徑進行后續分析。

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

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

(3)

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

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

2.1 機械臂避障模型

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

圖3 工業SCARA平面兩自由度機械臂避障模型

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

2.2 機械臂最短路徑規劃算法

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

κf=ka/(ka+kb)

(4)

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

機械臂末端從起始位置出發到達目標點位置，在運動過程中，研究機械臂的避障問題，時刻需要關注機械臂末端位置與障礙物中心位置之間的距離，防止機械臂與障礙物的碰撞。令機械臂末端運動路徑點的集合為Ptraj

Ptraj=Γ(t，κf，λ)

(5)

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

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

(6)

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

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

圖4 機械臂末端的最短避障路徑規劃

dmin=Φmin(κf，λ)

(7)

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

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

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

(1)dmin

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

(3)dmin>r，則表示機械臂末端處于障礙物范圍之外，存在多個解，機械臂末端未與障礙物發生碰撞現象。在多個解中，存在可以近似認為機械臂末端軌跡與半徑r的圓相切，此時狀態下，機械臂末端的路徑長度大于(2)中的情況。因此，當dmin=r時，機械臂末端從起始點到達目標點的路徑最短。

當機械臂末端路徑經過最短路徑到達目標點時，則該運動狀態模型可以被描述為

(8)

(9)

(10)

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

則構建避障路徑的目標函數

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

(11)

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

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

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

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

3 實驗與討論

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

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

表1 不同障礙物半徑的路徑參數

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

圖7 障礙物半徑r與引力因子的關系

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

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

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

圖9 障礙物縱坐標y與引力因子的關系

3.3 機械臂關節路徑分析

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

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

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

圖10 最短避障路徑的機械臂關節角度變化情況(r為0.4和0.44)

4 結束語

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