雙工業(yè)機(jī)器人同步作業(yè)的無碰撞路徑規(guī)劃方法

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

（河北工程大學(xué) 機(jī)械與裝備工程學(xué)院，邯鄲 056038）

0 引言

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

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

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

1 雙機(jī)器人無碰撞路徑規(guī)劃建模

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

圖1 雙機(jī)器人同步作業(yè)示意圖

由此，建立雙工業(yè)機(jī)器人同步作業(yè)的無碰撞路徑規(guī)劃模型為：

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

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

2.1 雙路徑避障規(guī)劃方法原理分析

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

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

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

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

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

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

2.2 雙路徑避障規(guī)劃方法流程

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

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

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

Step1：PSO算法初始化

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

1）分別確立兩個機(jī)器人的起點Q1、Q2和對應(yīng)的目標(biāo)點G1、G2。

2）設(shè)初始迭代次數(shù)為k1=0，最大迭代次數(shù)為N。

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

4）設(shè)慣性權(quán)重為ω，粒子學(xué)習(xí)因子為c1、c2。

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

6）粒子群初始化，在起點Q1和對應(yīng)的目標(biāo)點G1之間隨機(jī)生成n個隨機(jī)數(shù)，以此獲得第i個粒子的初始位置點

Step2：B樣條插值

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

Step3：碰撞檢測

依據(jù)障礙物的外形特點，建立碰撞模型。判斷是否碰撞的準(zhǔn)則為：

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

Step4：適應(yīng)度計算

適應(yīng)度函數(shù)是評價路徑質(zhì)量的重要指標(biāo)。通過適應(yīng)度函數(shù)求解各粒子的路徑長度，以便尋找當(dāng)代粒子群中的最優(yōu)粒子。將Step3獲得的可行路徑進(jìn)行點與點之間的路徑求解，再整體求和獲得整個粒子的路徑長度，具體為：

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

Step5：最優(yōu)粒子獲取

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

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

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

Step6-1：初始化設(shè)置

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

Step6-2：坐標(biāo)系建立

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

圖4 路徑點局部坐標(biāo)系

全局坐標(biāo)系xyz與局部坐標(biāo)系x'y'z'的坐標(biāo)變換公式為：

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

以參數(shù)d、θ、φ，以x'、y'、z'為坐標(biāo)軸建立球坐標(biāo)系，與點P1,k2相對應(yīng)的點P2,k2在x'y'z'坐標(biāo)系的直角坐標(biāo)為：

Step6-3：隨機(jī)抽樣

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

Step6-4：碰撞檢測

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

Step6-5：距離求解

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

Step6-5：收斂判斷

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

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

Step7：收斂判斷

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

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

當(dāng)滿足收斂準(zhǔn)則時，給出粒子群求解的最優(yōu)路徑；當(dāng)不滿則收斂準(zhǔn)則時，令k1=k1+1，轉(zhuǎn)到Step8繼續(xù)求解。

Step8：粒子速度更新

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

Step9：粒子群更新

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

當(dāng)獲得新的粒子群后，轉(zhuǎn)到Step2，繼續(xù)進(jìn)行迭代求解，直至滿足Step7的收斂準(zhǔn)則。

3 仿真驗證

3.1 仿真設(shè)計

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

表1 障礙物參數(shù)

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

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

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

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

3.2 仿真分析

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

表2為隨機(jī)抽樣個數(shù)和路徑點相同時，在不同種群大小下的仿真結(jié)果。由表2可知，種群大小為90時，三次仿真求解的最優(yōu)值差距最小，求解結(jié)果相對穩(wěn)定，且路徑長度的平均值最小。

表2 不同種群大小的仿真結(jié)果

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

表3 不同抽樣個數(shù)的仿真結(jié)果

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

表4 不同路徑點個數(shù)的仿真結(jié)果

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

圖5 迭代歷程

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

圖6 路徑圖

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

4 結(jié)語

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

1）依據(jù)雙工業(yè)機(jī)器人同步作業(yè)問題，建立了雙機(jī)器人無碰撞路徑規(guī)劃模型。

2）將雙機(jī)器人無碰撞路徑規(guī)劃模型分解為兩個嵌套的優(yōu)化模型。外部模型為單機(jī)器人的無碰撞路徑規(guī)劃，內(nèi)部模型為已知路徑下的距離約束路徑規(guī)劃。

3）對分解后的優(yōu)化模型進(jìn)行分析，給出了所提方法的原理與求解流程。

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

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