雙工業機器人同步作業的無碰撞路徑規劃方法

張立香，孟欣佳，王曉敏，丁志杰

（河北工程大學 機械與裝備工程學院，邯鄲 056038）

0 引言

工業機器人是現代智能制造系統的重要組成部分，是衡量一個國家科技創新實力和高端制造業水平的重要標志[1,2]。工業領域機器人化的加速發展，使得機器人系統涵蓋的作業任務愈加復雜和繁重，單機器人獨立作業已難以滿足協同裝配[3]、復雜焊接[4]、大尺寸重物搬運[5]等復雜工況的需求。多機器人系統[6]通過機器人間的有機協作完成各種復雜任務，從而滿足不同作業需要，成為了解決復雜作業問題的有效途徑。多機器人的協同作業給機器人技術的發展帶來了新的挑戰，而路徑規劃作為機器人發展的核心技術之一，也成為多機器人領域研究的熱點[7,8]。

張瑞星等[9]對雙機器人協同焊接的路徑規劃進行研究，結合蟻群算法和粒子群算法，提出一種基于蟻群粒子群的融合算法。李洋等[10]對快速隨機擴展樹（Rapidly exploring-random tree，RRT）方法進行改進，提出了隨機樹被動生長方法，實現了雙機器人在各自構型空間中的協同路徑規劃。徐金雄等[11]提出一種雙蟻群路徑規劃方法，利用不同種群蟻群之間信息素的負反饋作用，降低兩個機器人間的沖突，進而找到最優移動路徑。Das等[12]提出改進的粒子群算法來計算多機器人在已知環境中的最優無碰撞路徑。Nazarahari等[13]提出一種改進的遺傳算法，首先采用人工勢場法找到初始的可行路徑，然后利用改進的遺傳算法改進初始方案，并加入碰撞移除操作，從而保證多機器人獲得最優的無碰撞路徑。彭鵬等[14]對6自由度雙機械臂的工作空間進行分析，分別利用改進的人工勢場法（Artificial potential field method，APF）和RRT算法對主、從機械臂的路徑進行規劃，有效解決了從機械臂路徑規劃時陷入局部極小值的問題。

目前，針對多工業機器人的路徑規劃主要集中在多個機器人完成不同任務時，各機器人的最優路徑規劃。但在實際工程中，存在多個機器人協同完成同一任務的情況，如，多個機器人需要共同合作裝配或搬運同一個零件，即多個機器人處于同步作業，此時機器人的執行末端在行進過程中始終保持固定距離。本文針對多個機器人同步作業時的避障路徑規劃問題，在距離約束下，將粒子群優化（Particle Swarm Optimization，PSO）算法與隨機抽樣技術相結合，提出一種用于雙工業機器人同步作業時的無碰撞路徑規劃方法。

1 雙機器人無碰撞路徑規劃建模

如圖1所示，機器人R1與機器人R2同步作業時，R1的執行末端從起點Q1沿路徑L1運行到目標點G1，而R2的執行末端從起點Q2沿路徑L2運行到目標點G2。在此過程中，R1與R2的執行末端之間的距離始終保持為兩機器人抓取點之間的距離d不變，且在行進過程中路徑L1、L2和作業物體W均不能與障礙物發生碰撞。因此，雙工業機器人的避障路徑規劃實際上是在有約束的情況下，進行雙路徑優化求解問題。

圖1 雙機器人同步作業示意圖

由此，建立雙工業機器人同步作業的無碰撞路徑規劃模型為：

式(1)中，f為雙路徑總距離；f1和f2分別為R1和R2行走的路徑L1和L2的距離；i為機器人走過的路徑點數；表示求模數；d為兩條路徑之間的固定距離；W為作業物體所在區域集合；Ψ為障礙物所在區域集合；?表示不屬于。

2 基于PSO算法和距離約束的雙路徑避障規劃方法

2.1 雙路徑避障規劃方法原理分析

針對式(1)所示的優化問題，實際上是一個無碰撞的雙路徑優化問題，可將其拆解成兩個嵌套的優化模型，具體見式(2)和式(3)。

式(2)所示的優化問題實際上就是單機器人的避障路徑規劃問題，現有的RRT算法、人工勢場法、PSO算法等方法均可對其進行求解。其中，PSO算法因其結構簡單，易于實現，在機器人的路徑規劃中得到了廣泛應用[15,16]。利用PSO算法進行路徑求解時，每個粒子都視為一條路徑，通過不斷迭代求解最終獲得距離最短的粒子，即最優路徑。

在對式(3)所示的路徑優化問題求解時，需要在距離約束下，求解式(3)所示的路徑規劃問題時，其原理如圖2所示。

圖2 距離約束下的路徑求解原理圖

如圖2所示，L1(i)為路徑L1的第i個路徑點，與之對應的路徑L2的第i個路徑點為L2(i)。為保證路徑點L1(i)與路徑點L1(i)之間的距離為d，點L2(i)只能在以為L1(i)圓心，以d為半徑的球面上。因此，求點L2(i)時，可以點L1(i)為原點，構建球坐標系，球坐標系的參數為：ρ=d，θ∈[0,π]，φ∈[0,2π]，d為已知。參數θ和φ可利用抽樣原理在其取值范圍內進行隨機抽樣，獲得球坐標θ和φ的不同組合，利用坐標變換獲得直角坐標系下點L2(i)的不同取值。再依據最優原則和避障約束，取距離L2的上一個點L2(i-1)最近的點為L1(i)，且要保證L2(i)到L1(i)的連線不能經過障礙物所在區域。

通過上述分析，本文以PSO算法求解路徑L1的基本流程為主體，在每一代求解的最優粒子下，基于距離約束下的路徑求解原理，嵌套進行路徑L2的求解，通過循環求解，最終獲得L1和L2的最優路徑，以此構建一種基于PSO算法和距離約束的雙路徑避障規劃方法。

2.2 雙路徑避障規劃方法流程

基于PSO算法和距離約束的雙路徑避障規劃方法是在PSO優化求解的流程上嵌入距離約束下的路徑規劃，其流程如圖3所示。

圖3 基于PSO算法和距離約束的雙路徑避障規劃方法流程

基于PSO算法和距離約束的雙路徑避障規劃方法的具體步驟如下：

Step1：PSO算法初始化

PSO算法初始化主要包括以下幾個方面：

1）分別確立兩個機器人的起點Q1、Q2和對應的目標點G1、G2。

2）設初始迭代次數為k1=0，最大迭代次數為N。

3）設置粒子群總數為M，每個粒子的位置控制點的總個數為n。

4）設慣性權重為ω，粒子學習因子為c1、c2。

5）確定速度界限[Vmin,Vmax]和位置界限[Xmin,Xmax]，初始速度為V0。

6）粒子群初始化，在起點Q1和對應的目標點G1之間隨機生成n個隨機數，以此獲得第i個粒子的初始位置點

Step2：B樣條插值

對當前迭代中，每個粒子進行B樣條插值獲得M條光滑路徑。每個粒子的B樣條曲線方程為：

Step3：碰撞檢測

依據障礙物的外形特點，建立碰撞模型。判斷是否碰撞的準則為：

式(6)中，Pi為生成路徑離散的第i個路徑點。當生成的路徑點Pi位于障礙物所在區域時，發生碰撞，此路徑不可行；當生成的路徑點Pi不位于障礙物所在區域時，不發生碰撞，路徑可行。

Step4：適應度計算

適應度函數是評價路徑質量的重要指標。通過適應度函數求解各粒子的路徑長度，以便尋找當代粒子群中的最優粒子。將Step3獲得的可行路徑進行點與點之間的路徑求解，再整體求和獲得整個粒子的路徑長度，具體為：

式(7)中，f1,j為第j個粒子的路徑長度，下角標1表示路徑L1；m為路徑離散點的個數；Pi為第i個路徑點；P0為起點；Lm+1為目標點。

Step5：最優粒子獲取

對第k1次迭代的可行路徑的適應度值進行對比，適應度值最小的即為路徑L1在第k1次迭代的最優粒子記為L1,k1，其適應度值為f1,k1。將適應度值傳遞到Step7，并將L1,k1的路徑點傳遞到Step6，進行路徑2的求解。

Step6：距離約束下的路徑求解

在第一條路徑L1,k1已知大前提下，求解與其對應的第二條路徑L2,k1。

Step6-1：初始化設置

設路徑L1,k1的路徑點總個數為N1，初始迭代次數為k2=1，路徑L2,k1的初始距離f2,k1=0。

Step6-2：坐標系建立

提取路徑L1,k1的第k2個路徑點P1,k2，以該點為原點建立局部坐標系x'y'z'，坐標軸的方向與全局坐標系xyz相同，如圖4所示。

圖4 路徑點局部坐標系

全局坐標系xyz與局部坐標系x'y'z'的坐標變換公式為：

式(8)中，x、y、z為直角坐標系下的全局坐標，x'y'z'為直角坐標系下的局部坐標，(x1,k2,y1,k2,z1,k2)為點P1,k2的全局直角坐標值。

以參數d、θ、φ，以x'、y'、z'為坐標軸建立球坐標系，與點P1,k2相對應的點P2,k2在x'y'z'坐標系的直角坐標為：

Step6-3：隨機抽樣

對球坐標系的參數θ和φ進行隨機抽樣，抽樣范圍為：θ∈[0,π]，φ∈[0,π]，抽樣個數為N2。為避免取樣點集中的問題，在此采用拉丁超立方(LHS)抽樣法。依據式(8)和式(9)，將抽樣獲得球坐標轉換到全局直角坐標，從而獲得N2組樣本。

Step6-4：碰撞檢測

依據Step3對Step6-3獲得的所有抽樣點進行碰撞檢測，同時還需要對點P2,k2和P2,k2的連線進行碰撞檢測。以此將會產生碰撞的點去除，最終獲得可行的抽樣點。

Step6-5：距離求解

依據點到點的距離公式，求解所有可行的抽樣點到上一個點P2,k2-1的距離，以及到目標點的距離。選取值最小的抽樣點作為點P2,k2，得

Step6-5：收斂判斷

距離約束下路徑求解的收斂準則，路徑L2,k1的所有路徑點都求解完，即：

當滿足收斂準則時，Step 6求解結束，由此獲得第k1次的路徑L2,k1和路徑長度f2,k1，并將f2,k1值傳遞到Step7，進行整體收斂判斷；否則，令k2=k2+1，繼續進行下一個路徑點的求解。

Step7：收斂判斷

將Step5和Step6獲得f1,k1和f2,k1進行求和，先計算整體路徑長度fk1=f1,k1+f2,k1，再做收斂判斷，其收斂準則為：

式(12)中，fk1和fk1-1分別為前后兩次迭代求解整體路徑值；Nε為滿足收斂精度的次數；[Nε]為滿足收斂精度的允許最小次數，以保證求解的穩定性。

當滿足收斂準則時，給出粒子群求解的最優路徑；當不滿則收斂準則時，令k1=k1+1，轉到Step8繼續求解。

Step8：粒子速度更新

每次迭代完成后，需要更新粒子速度，以便獲得新的粒子群。粒子速度更新公式為：

Step9：粒子群更新

利用Step8獲得粒子速度更新粒子位置，即獲得新的粒子群。粒子位置更新公式為：

當獲得新的粒子群后，轉到Step2，繼續進行迭代求解，直至滿足Step7的收斂準則。

3 仿真驗證

3.1 仿真設計

本文是在障礙物形狀，機器人的起點和目標點、抓取物體形狀均已知的情況下，對所提方法進行的驗證。作業物體形狀為圓柱體，簡化成一條線段。機器人抓取位置為物體兩端，簡化為點，兩機器人抓取點之間的距離d=5。機器人R1與R2的起點和目標點在全局坐標系下的坐標分別為Q1(1,1,1)、G1(100,100,100)和Q2(4,5,1)、G2(103,104,100)。從起點到目標點設置4個橢球形狀的障礙物，其參數如表1所示。

表1 障礙物參數

在采用本方法進行路徑規劃時，Step3和Step6-4的碰撞檢測包括路徑點碰撞檢測和物體碰撞檢測。鑒于障礙物的形狀，路徑點碰撞檢測是判斷路徑點是否在橢球面內，具體為將路徑點坐標代入橢球公式，橢球公式為：

式(17)中，(x,y,z)為路徑點的全局直角坐標，(xc,yc,zc)為橢球中心坐標，(xr,yr,zr)為橢球半軸。當QT≤1時，將與障礙物發生碰撞；否則不發生碰撞。

作業物體碰撞檢測方法，先求解橢球中心Ac(xc,yc,zc)到兩路徑點連線的垂直距離l和垂足坐標A0(x0,y0,z0)，為：

通過判斷垂足所在位置，及橢球中心到垂足的距離來判斷作業物體是否與障礙物碰撞。當垂足A0位于兩路徑點P1,k2和P2,k2之間時，將垂足坐標代入式(17)，判斷原則與路徑點相同；當垂足A0位于兩路徑點P1,k2和P2,k2之外時，只要兩路徑點P1,k2和P2,k2不碰撞，整體作業物體即不與障礙物發生碰撞。

3.2 仿真分析

本方法涉及到PSO算法和隨機抽樣，在距離約束下求解路徑L2時，還需要將路徑L1離散成多個路徑點。PSO算法的種群大小、隨機抽樣的個數以及路徑點的個數對求解結果有較大的影響，采用MATLAB在上述不同參數下進行仿真，對三種系統參數的靈敏度進行分析。

表2為隨機抽樣個數和路徑點相同時，在不同種群大小下的仿真結果。由表2可知，種群大小為90時，三次仿真求解的最優值差距最小，求解結果相對穩定，且路徑長度的平均值最小。

表2 不同種群大小的仿真結果

表3為種群大小和路徑點相同時，在不同抽樣個數下的仿真結果，表中差值表示與前一組優化解的差，“－”表示減少。由表3可知，隨著抽樣個數的增加，最優值越來越小，求解時間越來越長。抽樣個數每增加100，仿真獲得的最優值與前一組的差值逐漸減小。此外，抽樣個數為400時，迭代次數最少，仿真時間與抽樣個數為300時，差距不多。

表3 不同抽樣個數的仿真結果

表4為種群大小和抽樣個數相同時，在不同路徑點下的仿真結果。由表4可知，路徑點越多，最優值越大。這是由于路徑的長度是由點到點的直線距離疊加而成的，路徑離散的點越少，越接近直線，求解的長度就越小，路徑也就越失真。此外，由表4還可看出，路徑點個數為50時，迭代次數最大為53，但其所用的仿真時間仍然是最少的。這表明仿真時間受路徑點個數的影響較大，路徑點越多，仿真時間越長。綜合考慮路徑的保真性和求解時間，可選取路徑點個數為100。

表4 不同路徑點個數的仿真結果

通過上述分析可選取種群大小90，抽樣個數400以及路徑點個數100，對其進行仿真，其迭代歷程如圖5所示。

圖5 迭代歷程

由圖5可知，在8次迭代之后，最優值趨于穩定，并于29次迭代時滿足20次達到收斂精度的要求，迭代終止，獲得總路徑的最優值為377.9529，路徑L1的最優值為174.8287，路徑L2的最優值為203.1242。獲得的雙路徑如圖6所示。

圖6 路徑圖

圖6中，紅色線為路徑L1，藍色線條為路徑L2。結合圖6中，方向1、方向2和方向3三個方向的視圖，可以看出求解獲得的雙路徑均未與四個障礙物發生碰撞。

4 結語

本文針對雙工業機器人同步作業時，機器人執行末端需保持固定距離的無碰撞路徑規劃問題，提出了一種基于PSO算法和距離約束的雙路徑避障規劃方法。本文的主要貢獻如下：

1）依據雙工業機器人同步作業問題，建立了雙機器人無碰撞路徑規劃模型。

2）將雙機器人無碰撞路徑規劃模型分解為兩個嵌套的優化模型。外部模型為單機器人的無碰撞路徑規劃，內部模型為已知路徑下的距離約束路徑規劃。

3）對分解后的優化模型進行分析，給出了所提方法的原理與求解流程。

4）基于MATLAB仿真對所提方法進行驗證，并對種群大小、路徑點個數以及抽樣數量三個主要系統參數進行靈敏度分析。仿真結果表明，該方法可有效實現雙機器人的無碰撞路徑規劃。

本方法是對雙工業機器人同步作業問題的初探，僅考慮了雙機器人執行末端的路徑規劃，后續仍需要在距離約束下兩條路徑的平滑過渡以及同步作業時雙機器人的運動規劃兩個方面做進一步研究。