袁蒙恩，陳立家，馮子凱

（河南大學(xué)物理與電子學(xué)院，河南開封 475000）

（*通信作者電子郵箱chenlijia_just@163.com）

0 引言

在工業(yè)生產(chǎn)的過程中，大多數(shù)的工業(yè)機(jī)械臂都是按照固定示教指令完成抓取工件任務(wù)；一旦外界環(huán)境以及物體位姿發(fā)生變化，機(jī)械臂將無法實(shí)現(xiàn)抓取任務(wù)，這就需要機(jī)械手能夠根據(jù)工件具體位置完成自主抓取任務(wù)。因機(jī)器視覺獨(dú)特的優(yōu)點(diǎn)，被廣泛應(yīng)用到工業(yè)機(jī)械臂抓取工作中，幫助工業(yè)機(jī)械臂完成智能抓取任務(wù)。一般工業(yè)生產(chǎn)環(huán)境都較為復(fù)雜，盡管有機(jī)器視覺的幫助，機(jī)械臂依然很難在復(fù)雜的靜態(tài)環(huán)境中識(shí)別目標(biāo)物體位置。目前大部分有關(guān)位姿估計(jì)的算法都是在純色簡單背景或復(fù)雜運(yùn)動(dòng)背景條件下的算法，針對(duì)復(fù)雜靜態(tài)背景的物體位姿估計(jì)的算法很少，比如基于顏色的HSV（Hue-Saturation-Value）閾值分割算法［1］，該算法可濾除復(fù)雜靜態(tài)背景對(duì)目標(biāo)物體位姿獲取的影響；但這種方法缺點(diǎn)在于目標(biāo)物體顏色必須是純色的。將圖像差分與輪廓查找的方法相結(jié)合可以消除復(fù)雜靜態(tài)背景［2］，最后經(jīng)過處理之后只留下含有目標(biāo)物體區(qū)域的純色背景圖像，如文獻(xiàn)［3］中的預(yù)處理。這就將復(fù)雜背景下的物體位姿求解問題轉(zhuǎn)化成了簡單純色背景下的位姿求解問題。目前基于模型的位姿估計(jì)的方法精度較高，比如文獻(xiàn)［4］中，知道目標(biāo)物體的靶點(diǎn)像素坐標(biāo)與其對(duì)應(yīng)的3D 靶點(diǎn)在對(duì)象上的幾何關(guān)系后，利用近似透視投影求出目標(biāo)物體在機(jī)械臂底座下的位置信息，為進(jìn)一步求解出機(jī)械臂到達(dá)目標(biāo)物體處的最優(yōu)路徑奠定基礎(chǔ)。

近些年以來，為解決機(jī)械臂最優(yōu)路徑規(guī)劃問題，文獻(xiàn)［5］中提出了Gilbert-Johnson-Keerthi 算法，該算法通過計(jì)算最近距離進(jìn)行改進(jìn)，用以解決機(jī)械臂最優(yōu)路徑規(guī)劃問題，從而避免了機(jī)械臂執(zhí)行任務(wù)時(shí)的動(dòng)態(tài)障礙。文獻(xiàn)［6］中提出了三維極軌跡法，用于求解旋轉(zhuǎn)棱鏡關(guān)節(jié)和多個(gè)剛性連桿組成的旋轉(zhuǎn)三維機(jī)械手的數(shù)學(xué)建模與軌跡規(guī)劃，該方法通過極性分段插值的逆幾何變換，有效地逼近了三維公共點(diǎn)的平面gnomic 軌跡。文獻(xiàn)［7］中提出了一種新的運(yùn)動(dòng)分配和反應(yīng)式執(zhí)行方法，將傳統(tǒng)的軌跡生成技術(shù)和最優(yōu)控制策略結(jié)合到一個(gè)統(tǒng)一的同步運(yùn)動(dòng)規(guī)劃控制框架中，有效實(shí)現(xiàn)了機(jī)械臂的運(yùn)動(dòng)規(guī)劃問題。

而上述路徑規(guī)劃算法有著復(fù)雜度高、運(yùn)行耗時(shí)、誤差大等不足，無法滿足工業(yè)場景下機(jī)械臂實(shí)時(shí)抓取的要求。與上述研究不同的是，本文在研究機(jī)械臂的運(yùn)動(dòng)路徑規(guī)劃問題時(shí)，使用粒子群優(yōu)化（Particle Swarm Optimization，PSO）算法、遺傳算法（Genetic Algorithm，GA）等智能算法來解決上述問題，但這些算法在解決多關(guān)節(jié)機(jī)械臂路徑規(guī)劃問題時(shí)，容易陷入局部最優(yōu)，實(shí)時(shí)性雖然有一定的改進(jìn)，但還是無法滿足要求。經(jīng)過實(shí)驗(yàn)發(fā)現(xiàn)利用粒子群算法改進(jìn)后的多種群粒子群演化算法PSOEL（Particle Swarm Optimization based on ELite group）在一定程度上可以解決算法復(fù)雜度高、容易陷入局部最優(yōu)的問題，可有效降低算法求解誤差。PSOEL算法在一定程度上克服了傳統(tǒng)粒子群算法初始種群質(zhì)量差、搜索理想?yún)^(qū)域耗時(shí)大、算法效率低、容易陷入局部最優(yōu)解等缺點(diǎn)，可以有效地解決五自由度機(jī)械臂的運(yùn)動(dòng)規(guī)劃問題，提高工業(yè)機(jī)械臂抓取效率。

多種群粒子群算法是一種具有預(yù)選擇與交互機(jī)制的精英種群引導(dǎo)的算法。該算法通過提高初始種群的質(zhì)量以及對(duì)演化迭代過程進(jìn)行優(yōu)化，克服了傳統(tǒng)粒子群演化算法的缺點(diǎn)。

多個(gè)子種群與一個(gè)精英種群組成了多種群粒子群優(yōu)化算法。在初始化時(shí)，使用預(yù)選擇機(jī)制，將子種群之中表現(xiàn)最優(yōu)的那一部分粒子篩選出來當(dāng)作精英種群的粒子。在迭代演化的過程中，讓精英種群與子種群分別進(jìn)行演化。當(dāng)精英種群在一段時(shí)間內(nèi)的演化迭代結(jié)果一直劣于子種群或陷入了局部最優(yōu)時(shí)，那么就會(huì)觸發(fā)種群間的交互機(jī)制，PSOEL算法用子種群中表現(xiàn)最為優(yōu)秀的粒子替換掉精英種群中較劣的那部分粒子。使用預(yù)選則與交互機(jī)制可以有效地使算法跳出局部最優(yōu)，無限接近或達(dá)到全局最優(yōu)，最終求出的自適應(yīng)度誤差很小。

使用多種群粒子群算法求解出一條機(jī)械臂到達(dá)目標(biāo)物體處距離最短、消耗時(shí)間最短的一條路徑，算法將每個(gè)機(jī)械臂關(guān)節(jié)預(yù)旋轉(zhuǎn)的角度當(dāng)作粒子，經(jīng)過迭代演化后得到每個(gè)關(guān)節(jié)應(yīng)該旋轉(zhuǎn)的角度（最優(yōu)粒子）可以使得機(jī)械臂使用最短時(shí)間到達(dá)目標(biāo)物體處。

基于單目視覺的多種群粒子群機(jī)械臂路徑規(guī)劃算法能提高機(jī)械臂在復(fù)雜靜態(tài)環(huán)境下抓取的準(zhǔn)確度。

本文主要工作如下：

1）先使用棋盤圖標(biāo)定法求出相機(jī)內(nèi)參，為求出物體在參考坐標(biāo)系下的位置奠定基礎(chǔ)；將采集的圖像進(jìn)行預(yù)處理，濾除不必要的噪聲，使用背景消減算法等操作得到目標(biāo)物體區(qū)域，最后使用基于模型姿態(tài)估計(jì)的方法求出物體在參考坐標(biāo)系中的位置。

2）提出了一種多種群粒子群機(jī)械臂路徑規(guī)劃算法，通過設(shè)置不同的子種群數(shù)，觀察對(duì)機(jī)械臂運(yùn)動(dòng)規(guī)劃的影響，并選出了該算法最優(yōu)的超參數(shù)；為驗(yàn)證多種群粒子群算法在求解機(jī)械臂運(yùn)動(dòng)最優(yōu)路徑時(shí)，能很快跳出局部最優(yōu)解、求解誤差小的優(yōu)勢，將多種群粒子群算法與不同種智能算法做對(duì)比實(shí)驗(yàn)。

1 機(jī)械臂抓取系統(tǒng)

本文使用單目視覺位姿估計(jì)方法，相機(jī)安裝方式采用手眼分離式（eye-to-hand），系統(tǒng)分為4個(gè)模塊：圖像采集模塊、圖像預(yù)處理模塊、物體位姿模塊和機(jī)械臂路徑規(guī)劃模塊。首先對(duì)采集到的只有復(fù)雜靜態(tài)背景與混雜同樣背景的物體圖像一共兩張圖像進(jìn)行預(yù)處理操作，經(jīng)過預(yù)處理后可獲得純色背景的目標(biāo)物體區(qū)域；結(jié)合相機(jī)標(biāo)定得到的相機(jī)內(nèi)部參數(shù)，利用文獻(xiàn)［8］中提出的高效的透視n點(diǎn)相機(jī)姿態(tài)估計(jì)（Efficient Perspective-n-Point camera pose estimation，EPNP）算法，進(jìn)而求出物體坐標(biāo)系與相機(jī)坐標(biāo)系之間的相對(duì)位姿關(guān)系；將目標(biāo)物體的外接矩形中心作為其在圖像像素坐標(biāo)系中的位置，利用相機(jī)模型計(jì)算出物體在相機(jī)坐標(biāo)系下的三維空間位置；根據(jù)相機(jī)安裝，得到相機(jī)在機(jī)械臂底座下的位姿信息，進(jìn)而得到物體在機(jī)械臂底座下的位姿信息。將位姿信息傳給多種群粒子群機(jī)械臂路徑規(guī)劃算法，演化出每個(gè)舵機(jī)應(yīng)該旋轉(zhuǎn)多少度可以使五自由度（Degree Of Freedom，DOF）機(jī)械臂的末端機(jī)械手到達(dá)目標(biāo)物體處，機(jī)械臂完成自主抓取任務(wù)。單目五自由度機(jī)械臂抓取系統(tǒng)的工作流程如圖1所示。

圖1 單目五自由度機(jī)械臂抓取系統(tǒng)流程Fig.1 Flow chart of monocular five-degree-of-freedom manipulator grasping system

2 物體位姿獲取與定位

本文把機(jī)械臂底座當(dāng)作參考坐標(biāo)系，所進(jìn)行的位置變換都是在機(jī)械臂底座坐標(biāo)系下進(jìn)行的。當(dāng)目標(biāo)物體顏色與背景環(huán)境差異較大時(shí)，本文算法可以準(zhǔn)確求出目標(biāo)物體在機(jī)械臂底座坐標(biāo)系下的坐標(biāo)。在基于模型的姿態(tài)估計(jì)算法中，相機(jī)內(nèi)參通過文獻(xiàn)［9］中提出的棋盤圖標(biāo)定法求出。已知目標(biāo)物體的2D 靶點(diǎn)像素坐標(biāo)與其對(duì)應(yīng)的3D 靶點(diǎn)坐標(biāo)在對(duì)象上的幾何關(guān)系，本文利用近似透視投影求出目標(biāo)物體在機(jī)械臂底座下的位置信息。

2.1 相機(jī)內(nèi)參

使用單目視覺物體位姿估計(jì)算法之前，先利用張氏棋盤圖標(biāo)記法，通過相機(jī)標(biāo)定得到相機(jī)內(nèi)參，得到的相機(jī)內(nèi)參如表1所示。其中：fx、fy分別表示X和Y方向的等效焦距；u0、v0分別表示相機(jī)感光板中心在像素坐標(biāo)系下的坐標(biāo)；k1、k2、k3表示發(fā)生在相機(jī)坐標(biāo)系轉(zhuǎn)圖像物理坐標(biāo)系的過程中的相機(jī)徑向畸變系數(shù)；p1、p2表示在相機(jī)制作過程，由于感光元平面跟透鏡不平行導(dǎo)致的切向畸變系數(shù)。相機(jī)內(nèi)參的求取為下一步求出物體位置奠定基礎(chǔ)。

表1 相機(jī)內(nèi)參Tab.1 Camera intrinsic parameters

2.2 圖像預(yù)處理

如何在復(fù)雜靜態(tài)背景下獲取目標(biāo)物體的2D 特征點(diǎn)一直以來都是一個(gè)難點(diǎn)。

首先對(duì)采集到的背景圖像與帶有相同背景的物體圖像進(jìn)行高斯濾波、灰度變化處理，進(jìn)而濾除光照而導(dǎo)致的陰影等噪聲，圖2 是未經(jīng)過處理的兩張?jiān)瓐D像。在對(duì)圖像進(jìn)行差分操作之前需要先對(duì)圖像進(jìn)行伽馬變換，這樣可以有效避免圖像陰影對(duì)圖像差分結(jié)果的影響。然后調(diào)用圖像差分算法，得到帶有目標(biāo)物體并且背景為黑色的圖像；再對(duì)處理后的圖像進(jìn)行二值化處理；然后在二值化處理后的圖像上畫出物體輪廓。在實(shí)驗(yàn)中使用圖像差分處理雖然可以抑制掉復(fù)雜背景，可以將目標(biāo)物體從復(fù)雜背景下抽離出來，但是得到的目標(biāo)物體區(qū)域是粗略的，物體區(qū)域里面混合了背景顏色，導(dǎo)致最終找到的物體的輪廓區(qū)域并不是連通的。而輪廓包圍法可以幫助解決這一問題，故使用輪廓包圍法將目標(biāo)物體區(qū)域找出并畫出。具體操作如下：使用輪廓合成算法，將所有不連通的輪廓進(jìn)行合并，所有的不連通的目標(biāo)物體輪廓區(qū)域就可以被連接起來。根據(jù)凸包性質(zhì)，在一個(gè)多邊形的邊緣或內(nèi)部任意兩個(gè)點(diǎn)的連線都包含在多邊形邊界或者內(nèi)部，最后在該區(qū)域查找凸包，并畫出包含該區(qū)的凸多邊形。實(shí)驗(yàn)結(jié)果表明該多邊形為目標(biāo)物體的外輪廓，對(duì)得到的目標(biāo)區(qū)域進(jìn)行純色填充，最后可得到物體的輪廓信息。圖3 表示目標(biāo)輪廓區(qū)域獲取的過程，其中圖3（e）為調(diào)用凸包查找算法后，得到目標(biāo)區(qū)域并進(jìn)行填充。

圖2 未處理的兩張?jiān)瓐DFig.2 Two unprocessed original images

圖3 目標(biāo)輪廓區(qū)域獲取Fig.3 Target contour area acquisition

2.3 物體位姿計(jì)算

使用EPNP 算法經(jīng)過非迭代求解目標(biāo)物體的坐標(biāo)時(shí)需要6 個(gè)及以上共面或非共面的特征點(diǎn)，本文為了描述得更貼切，將這些特征點(diǎn)稱為靶點(diǎn)，物體虛擬坐標(biāo)系如圖4，在物體上將6 個(gè)靶標(biāo)分別標(biāo)出，表示為P1、P2、P3、P4、P5、P6，P5是物體的重心。

圖4 物體靶標(biāo)圖Fig.4 Object target map

將坐標(biāo)系的原點(diǎn)建在物體的重心位置，已知物體的幾何信息求出6個(gè)3D靶點(diǎn)坐標(biāo)，6個(gè)點(diǎn)的坐標(biāo)如表2。

表2 3D靶點(diǎn)Tab.2 3D target-points

假設(shè)相機(jī)的數(shù)學(xué)模型在弱仿射投影下，目標(biāo)物體在不同姿態(tài)的像面幾何畸變很小。二維方差最大時(shí)所形成的主特征軸是最穩(wěn)定的，而軸兩端的點(diǎn)是最穩(wěn)定的特征點(diǎn)。為了在不影響模式分類的情況下，最大化二維數(shù)據(jù)的方差并將其表示在一個(gè)新的空間中，引入了文獻(xiàn)［10］中提出的主成分分析（Principal Components Analysis，PCA）。PCA 是一個(gè)線性變換過程，把數(shù)據(jù)變換到一個(gè)新的坐標(biāo)系統(tǒng)中，使數(shù)據(jù)投影的第一大方差在第一主成分上，第二大方差在第二主成分上。使用PCA 算法可以求出目標(biāo)物體輪廓的主方向，在輪廓上求出代表物體主方向的四個(gè)特征點(diǎn)坐標(biāo)。使用目標(biāo)物體輪廓最小外接矩形的方法求出物體輪廓與外接矩形交點(diǎn)的兩個(gè)特征點(diǎn)的像素坐標(biāo)。將物體的主成分畫出，最終得到物體的6 個(gè)2D 特征點(diǎn)。而由PCA 求出的4 個(gè)特征點(diǎn)是經(jīng)過方差排序后得到的，將后來得到的兩個(gè)特征點(diǎn)坐標(biāo)與它們進(jìn)行排序依次標(biāo)記出這6 個(gè)2D 點(diǎn)，并將它們與3D 靶點(diǎn)一一對(duì)應(yīng)，圖5 為外接最小旋轉(zhuǎn)矩形與PCA算法處理后的帶有2D靶點(diǎn)的圖像，在圖像中將6個(gè)2D靶標(biāo)一一標(biāo)注。表3是2D靶點(diǎn)的像素坐標(biāo)。

利用EPNP 算法求出虛擬坐標(biāo)系與相機(jī)坐標(biāo)系之間的位姿關(guān)系，得到旋轉(zhuǎn)與平移矩陣。

將外接矩形的中心作為目標(biāo)物體在圖像坐標(biāo)系的位置，表3 中的P5就是外接矩形的中心，借鑒文獻(xiàn)［11］中所使用針孔相機(jī)模型求出物體在相機(jī)坐標(biāo)系下的位置(tx，ty，tz)和旋轉(zhuǎn)矩陣R，以及位姿矩陣形式T′。

式（1）表示由旋轉(zhuǎn)矩陣與坐標(biāo)組成的位姿矩陣。

圖5 靶點(diǎn)的像素位置Fig.5 Pixel positions of target-points

表3 對(duì)應(yīng)的2D靶點(diǎn)Tab.3 Corresponding 2D target-points

相機(jī)的外部參數(shù)主要指相機(jī)坐標(biāo)系到機(jī)械臂基坐標(biāo)系的轉(zhuǎn)換矩陣，在相機(jī)安裝時(shí)獲得相機(jī)外參為R′，根據(jù)矩陣變換原理，可確定目標(biāo)物在機(jī)械臂記坐標(biāo)系的姿態(tài)T=R′T′。

3 機(jī)械臂建模

3.1 機(jī)械臂的參數(shù)

若使用多種群粒子群算法求出機(jī)械臂由某一位置到達(dá)另一指定位置的最優(yōu)路徑，必須建立機(jī)械臂關(guān)節(jié)的運(yùn)動(dòng)學(xué)模型，根據(jù)機(jī)械臂運(yùn)動(dòng)學(xué)模型可以知道各個(gè)關(guān)節(jié)角之間的數(shù)學(xué)關(guān)系。根據(jù)各關(guān)節(jié)運(yùn)動(dòng)學(xué)約束關(guān)系，則可以得到機(jī)械臂末端執(zhí)行器在機(jī)械臂底座下的位姿信息。

本文采用文獻(xiàn)［12］中的機(jī)械臂運(yùn)動(dòng)學(xué)MDH（Modified Denavit Hartenbery）方法進(jìn)行建模，各個(gè)關(guān)節(jié)之間的坐標(biāo)系關(guān)系如圖6所示。

圖6 各關(guān)節(jié)坐標(biāo)系Fig.6 Joint coordinate systems

表4 為5 個(gè)機(jī)械臂關(guān)節(jié)的DH（Denavit Hartenbery）參數(shù)列表，其中：ai為機(jī)械臂第i個(gè)坐標(biāo)系到第i+1個(gè)坐標(biāo)系的平移，αi表示從第i個(gè)坐標(biāo)系旋轉(zhuǎn)到第i+1個(gè)坐標(biāo)系的角度；di從第i個(gè)坐標(biāo)系到第i+1 個(gè)坐標(biāo)系的沿y軸方向的平移距離；θi為表示機(jī)械臂的第i個(gè)關(guān)節(jié)的角度。

表4 DH參數(shù)Tab.4 DH parameters

3.2 MDH正向運(yùn)動(dòng)學(xué)方程

根據(jù)DH 模型參數(shù)可求出機(jī)械臂正向運(yùn)動(dòng)學(xué)方程，進(jìn)而可以求出機(jī)械臂末端執(zhí)行器在參考坐標(biāo)系下的空間位置信息。在機(jī)械臂的各個(gè)關(guān)節(jié)建立空間直角坐標(biāo)系，得到式（2）運(yùn)動(dòng)學(xué)方程。

其中：Ti表示從第i個(gè)坐標(biāo)系到第i+1 坐標(biāo)系之間的旋轉(zhuǎn)與平移。最后可得到5 個(gè)坐標(biāo)系之間的旋轉(zhuǎn)與平移關(guān)系。所以五自由度的機(jī)械臂末端在參考坐標(biāo)系（機(jī)械臂底座坐標(biāo)系）的位姿表示為：

因此，五自由度機(jī)械臂的運(yùn)動(dòng)學(xué)方程為：

式（4）中矩陣的前三列表示機(jī)械臂末端執(zhí)行器的姿態(tài)，第四列表示其坐標(biāo)。機(jī)械臂末端執(zhí)行器與目標(biāo)物體之間的空間距離表示收斂結(jié)果的適應(yīng)度，此收斂結(jié)果的適應(yīng)度可以衡量算法的收斂性能，如式（5）所示：

實(shí)際位置與目標(biāo)位置的均方誤差（Mean Square Error，MSE）公式如下：

3.3 基于PSOEL的機(jī)械臂運(yùn)動(dòng)規(guī)劃問題求解

3.3.1 多種群優(yōu)化算法

粒子群優(yōu)化（PSO）算法［13］是對(duì)鳥群和魚群等群體社會(huì)行為的模擬。

在粒子群優(yōu)化算法中，每個(gè)個(gè)體都是一個(gè)粒子，粒子是組成種群的單位。根據(jù)自身與其鄰域先前的位置與信息，粒子的迭代演化可將其路徑調(diào)整到最佳位置。除此之外，在全局演化的過程中，粒子在尋找最優(yōu)區(qū)域時(shí)，相互分享信息以及過往經(jīng)驗(yàn)值。為進(jìn)一步改進(jìn)傳統(tǒng)粒子群算法容易陷入局部最優(yōu)，提高其計(jì)算效率與速度，提出多種群粒子群多精英種群算法。

與傳統(tǒng)粒子群算法相比，PSOEL 算法具有精英種群與子種群交互和預(yù)選的機(jī)制。基于預(yù)選機(jī)制，在演化的過程中，精英種群與子種群分別演化，這樣在一定程度上提高了算法的計(jì)算速度與效率，在算法中加入精英種群可以幫助粒子節(jié)約時(shí)間找到目的位置。具體步驟如下：

a）子種群的初始化。該算法初始種群為多個(gè)種群，主要由n個(gè)子種群與一個(gè)精英種群組成，粒子群的每個(gè)子種群在PSOEL初始化過程中隨機(jī)產(chǎn)生通同等數(shù)量的N個(gè)粒子。

b）挑選最佳粒子。利用評(píng)價(jià)函數(shù)選擇子種群中最好的N/n個(gè)粒子，組成具有N粒子的精英種群。

c）算法迭代時(shí)兩種種群各自搜索，各自演化，粒子演化速度更新公式如式（7）：

其中：Vk+1表示粒子在k時(shí)刻的速度；c1、c2表示PSOEL 的學(xué)習(xí)因子；ωk（k=1，2，…，N）表示慣性權(quán)重。式（8）表示粒子演化位置更新公式，ak+1表示k時(shí)刻粒子的位置：

d）陷入局部最優(yōu)時(shí)的處理。當(dāng)某個(gè)子種群表現(xiàn)持續(xù)優(yōu)于精英種群表現(xiàn)且超過1 000代，或者精英種群陷入局部最優(yōu)時(shí)超過1 000，此時(shí)選出最佳的N/（n+1）粒子將會(huì)取代精英群表現(xiàn)最差的n*N/（n+1）個(gè)粒子，組成新的精英種群。

e）判斷是否有粒子達(dá)到了最優(yōu)，若不滿足全局最優(yōu)解的條件，繼續(xù)執(zhí)行c），若滿足則最后就可以得到全局最優(yōu)粒子，并且適應(yīng)度為0。通過這種方法PSOEL 可以跳出局部最優(yōu)，很易達(dá)到全局最優(yōu)或全局次優(yōu)。

如圖7為PSOEL算法的演化過程。

圖7 PSOEL算法流程Fig.7 Flow chart of PSOEL algorithm

PSOEL中的交互與選擇機(jī)制保證了精英種群粒子可在初始化和演化過程一直保持最優(yōu)，所以與傳統(tǒng)粒子算法相比，計(jì)算效率更高，更易避免陷入局部最優(yōu)的問題，收斂性能較好，求出的自適應(yīng)度較小。

3.3.2 基于PSOEL的機(jī)械臂運(yùn)動(dòng)規(guī)劃

本文的運(yùn)動(dòng)路徑規(guī)劃目標(biāo)是在約束條件下，使機(jī)械臂末端執(zhí)行器找到目標(biāo)，并使得目標(biāo)函數(shù)的適應(yīng)度趨近于0。

將機(jī)械臂各個(gè)關(guān)節(jié)在某一瞬間的角度θi當(dāng)作PSOEL 中的粒子，也就是每一個(gè)粒子表示機(jī)械臂在某一瞬間的關(guān)節(jié)角度，然后算法一直循環(huán)迭代演化，使機(jī)械臂不斷調(diào)自己的位姿，最終達(dá)到全局最優(yōu)。

多種群粒子群求解最優(yōu)路徑的步驟如下：

1）初始化。

首先對(duì)粒子群進(jìn)行初始化，機(jī)械臂各個(gè)關(guān)節(jié)初始時(shí)的角度為0°，本文的機(jī)械臂一共有5個(gè)關(guān)節(jié)，機(jī)械臂的自由度為5，所以目標(biāo)函數(shù)的自變量為5。設(shè)置PSOEL 的種群大小NP=100，實(shí)驗(yàn)的測試次數(shù)為100，最終迭代次數(shù)為10 000，學(xué)習(xí)因子c1、c2為2.0，電機(jī)轉(zhuǎn)動(dòng)范圍設(shè)為（-90°，90°），粒子最大飛行速度Vmax設(shè)為1.7，子種群數(shù)設(shè)為4，電機(jī)轉(zhuǎn)動(dòng)精度設(shè)為每步0.1°。

2）個(gè)體最優(yōu)與全局最優(yōu)。

定義粒子群優(yōu)化算法的適應(yīng)度函數(shù)Fitness如式（5）。根據(jù)評(píng)價(jià)函數(shù)的結(jié)果對(duì)n個(gè)粒子進(jìn)行排序，并從每個(gè)子種群中挑出表現(xiàn)最好的N/n個(gè)粒子，它們形成擁有N個(gè)粒子的精英種群。讓普通粒子群與精英種群各自演化，最終計(jì)算出每一個(gè)粒子的適應(yīng)度函數(shù)值，輸出個(gè)體最優(yōu)解，然后再從這些個(gè)體最優(yōu)解中找到一個(gè)能夠使得全局無限接近或者約等于最優(yōu)的解。比較全局最優(yōu)的方法為：

首先，比較適應(yīng)度值Fitness與局部粒子求出的最優(yōu)值pbest，當(dāng)Fitness＜pbest時(shí)，就用Fitness取代pbest進(jìn)行更新操作；將更新過的pbest值再與全局極值gbest比較，當(dāng)Fitness＜gbest時(shí)，就用Fitness代替gbest的值。

然后，讓粒子更新為下一代，粒子的速度與位置更新公式如式（7）～（8）。精英種群表現(xiàn)劣于子種群超過1 000代時(shí)或當(dāng)精英群體陷于局部最優(yōu)的代數(shù)超過1 000，就啟動(dòng)交互機(jī)制。交互機(jī)制將立即從每個(gè)子種群中選出表現(xiàn)最好的N/（n+1）粒子去替換精英種群中表現(xiàn)最差的n*N/（n+1）個(gè)粒子，及時(shí)更新精英種群，形成新的精英種群。

3）終止迭代。

判斷精英種群中的粒子是否滿足全局最優(yōu)，如果不滿足的話，繼續(xù)演化更新，直到得到全局最優(yōu)。最終得到的全局最優(yōu)就是能夠讓機(jī)械臂到達(dá)目標(biāo)物體處的一條最優(yōu)的路徑。

為了驗(yàn)證PSOEL 在求解機(jī)械臂路徑規(guī)劃問題上的優(yōu)勢，將它與目前一流的智能演化算法對(duì)比，如自適應(yīng)多精英引導(dǎo)的復(fù)合差分進(jìn)化算法DEAMECO（adaptive Multiple-Elitesguided Composite Differential Evolution Algorithm with a shift mechanism）［14］、改進(jìn)的多種群集成差分進(jìn)化（Improved Multi-Population Ensemble Differential evolution，IMPEDE）算法［15］、用一個(gè)成功的親本選擇框架改進(jìn)差分進(jìn)化（improving Differential Evolution with a Successful-Parent-Selecting framework，DESPS）算法［16］、差分進(jìn)化（Differential Evolution，DE）算法［17］、利用遺傳算法（Genetic Algorithm，GA）實(shí)現(xiàn)機(jī)器人路徑規(guī)劃［18］，將7種超參數(shù)調(diào)到最優(yōu)，這幾種演化算法除了學(xué)習(xí)因子、粒子最大飛行速度、子系統(tǒng)個(gè)數(shù)不一致外，其他參數(shù)的初始值都與PSOEL一致。

4 實(shí)驗(yàn)分析

4.1 位姿獲取

首先對(duì)復(fù)雜背景進(jìn)行處理，采用改進(jìn)后的圖像差分算法，由第2章可知將物體的重心看作物體，物體的重心用P5表示，在2D圖像上P5表示外接旋轉(zhuǎn)矩形的中心，把它當(dāng)作目標(biāo)物體在像素坐標(biāo)系中的位置。根據(jù)攝像機(jī)模型可得物體在攝像機(jī)坐標(biāo)系的位置（X，Y，Z）。表5 表示在參考坐標(biāo)系下的物體在三個(gè)不同位置的坐標(biāo)，包含了目標(biāo)物體在參考坐標(biāo)系下的實(shí)際測量的三個(gè)位置坐標(biāo)，經(jīng)過圖像處理算法計(jì)算出的目標(biāo)物體在參考坐標(biāo)系下的坐標(biāo)，并將實(shí)際測量值與算法計(jì)算值進(jìn)行相減得到誤差值，誤差范圍為-7.9 mm～0.2 mm。

表5 機(jī)械臂基坐標(biāo)系下物體在不同位置處坐標(biāo)Tab.5 Coordinates of objects at different positions in manipulator base coordinate system

4.2 多粒子群優(yōu)化算法在機(jī)械臂路徑規(guī)劃中的分析

本文仿真實(shí)驗(yàn)可執(zhí)行空間是開放的，只有目標(biāo)物體，該算法使機(jī)械臂以漸進(jìn)的方式到達(dá)目標(biāo)物。機(jī)械臂的復(fù)雜約束條件為：1）五個(gè)機(jī)械臂的關(guān)節(jié)轉(zhuǎn)動(dòng)范圍（-90°，90°）；2）電機(jī)的運(yùn)動(dòng)精度步長；3）種群大小。仿真實(shí)驗(yàn)部分，每一類仿真實(shí)驗(yàn)進(jìn)行100 組實(shí)驗(yàn)，每組進(jìn)行10 000 次迭代演化，每一次有10 000代結(jié)果。下面的實(shí)驗(yàn)結(jié)果包括從100 組中選取最優(yōu)的一組繪制成折線圖；從每100 組分別選出其第10 000 代演化的適應(yīng)度，并對(duì)這100 次的適應(yīng)度求標(biāo)準(zhǔn)差繪制成柱形圖，以對(duì)算法的穩(wěn)定性進(jìn)行分析。算法在第10 000 代已經(jīng)達(dá)到穩(wěn)定，得到的自適應(yīng)度Fitness是穩(wěn)定的。

4.2.1 PSOEL 在不同子種群個(gè)數(shù)下的結(jié)果分析

首先，對(duì)PSOEL 在不同超參數(shù)下的運(yùn)動(dòng)軌跡進(jìn)行仿真，選出最優(yōu)超參數(shù)。經(jīng)過機(jī)器視覺求出物體位置坐標(biāo)后，PSOEL 的仿真結(jié)果如表6，表示不同的子種群數(shù)GN時(shí)的演化結(jié)果，其中從10 000次迭代中選取了6條數(shù)據(jù)進(jìn)行展示。

表6 PSOEL在不同子種群個(gè)數(shù)下幾組適應(yīng)度比較Tab.6 Comparison of fitness of PSOEL under different numbers of sub-populations

從表6可知有子種群個(gè)數(shù)為4時(shí)，收斂結(jié)果的自適應(yīng)度最小，即求出的機(jī)械臂末端執(zhí)行器最終的位置坐標(biāo)與目標(biāo)物體的位置坐標(biāo)間的誤差最小，圖8 是PSOEL 在不同子種群個(gè)數(shù)下的適應(yīng)度比較。

圖8 PSOEL在不同子種群個(gè)數(shù)下的適應(yīng)度比較Fig.8 Comparison of fitness values of PSOEL with different sub-population numbers

4.2.2 五自由度機(jī)械臂的路徑規(guī)劃結(jié)果分析

本實(shí)驗(yàn)對(duì)PSOEL、DE、DEAMECO、DESPS、PSO、GA 和IMPEDE 共7 種算法機(jī)械臂運(yùn)動(dòng)規(guī)劃中的仿真實(shí)驗(yàn)進(jìn)行了比較分析。

三個(gè)不同的目標(biāo)位置分別為（24.19，8.30，5.52），（29.20，-0.20，1.80），（22.03，-1.80，1.73），這三個(gè)目標(biāo)都在機(jī)械臂工作范圍內(nèi)。保持系統(tǒng)其他參數(shù)不變，仿真結(jié)果如表7～10 所示。在表8～9 中，PSOEL 達(dá)到最小的適應(yīng)度，表10中PSOEL 在第二個(gè)和第三個(gè)目標(biāo)中的適應(yīng)度和均方誤差值最小。

圖9～11 是7 種算法各自10 000 次的自適應(yīng)度，對(duì)比收斂結(jié)果的適應(yīng)度可以衡量算法的準(zhǔn)確度，第二組與第三組實(shí)驗(yàn)結(jié)果說明PSOEL 接近全局最優(yōu)，適應(yīng)度和均方誤差值最小，綜合性能平均最優(yōu)，所以說明PSOEL 算法的收斂性能平均較強(qiáng)。

表7 目標(biāo)為（24.19，8.30，5.52）時(shí)五自由度機(jī)械臂在7種算法下的適應(yīng)度比較Tab.7 When target is（24.19，8.30，5.52），fitness comparison of five DOF manipulator under seven algorithms

表8 目標(biāo)為（29.20，-0.20，1.80）時(shí)五自由度機(jī)械臂在7種算法下的適應(yīng)度比較Tab.8 When target is（29.20，-0.20，1.80），fitness comparison of five DOF manipulator under seven algorithms

表9 目標(biāo)為（22.03，-1.80，1.73）時(shí)五自由度機(jī)械臂在7種算法下的適應(yīng)度比較Tab.9 When target is（22.03，-1.80，1.73），fitness comparison of five DOF manipulator under seven algorithms

表10 五自由度機(jī)械臂在不同算法及不同目標(biāo)下的結(jié)果比較Tab.10 Results comparison of five DOF manipulator under different algorithms and different targets

圖9 目標(biāo)為（24.19，8.30，5.52）時(shí)五自由度機(jī)械臂在7種算法下的適應(yīng)度比較Fig.9 When target is（24.19，8.30，5.52），fitness comparison of five DOF manipulator under seven algorithms

圖10 目標(biāo)為（29.20，-0.20，1.80）時(shí)五自由度機(jī)械臂在7種算法下的適應(yīng)度比較Fig.10 When target is（29.20，-0.20，1.80），fitness comparison of five DOF manipulator under seven algorithms

圖11 目標(biāo)為（22.03，-1.80，1.73）時(shí)五自由度機(jī)械臂在7種算法下的適應(yīng)度比較Fig.11 When target is（22.03，-1.80，1.73），fitness comparison of five DOF manipulator under seven algorithms

圖12～14 分別表示目標(biāo)位置分別為（24.19，8.30，5.52），（29.20，-0.20，1.80），（22.03，-1.80，1.73）時(shí)，從100 次實(shí)驗(yàn)中選取各自第10 000 代的自適應(yīng)度，對(duì)100 次第10 000 代自適應(yīng)度求出標(biāo)準(zhǔn)差后的柱形圖。由圖12～14 可知，PSOEL 在第二個(gè)與第三個(gè)目標(biāo)位置的100 次標(biāo)準(zhǔn)差最小，算法的穩(wěn)定性最好。

圖12 目標(biāo)為（24.19，8.30，5.52）時(shí)7種算法下的適應(yīng)度標(biāo)準(zhǔn)差比較Fig.12 When target is（24.19，8.30，5.52），fitness standard deviation comparison under seven algorithms

圖13 目標(biāo)為（29.20，-0.20，1.80）時(shí)7種算法下的適應(yīng)度標(biāo)準(zhǔn)差比較Fig.13 When target is（29.20，-0.20，1.80），fitness standard deviation comparison under seven algorithms

圖14 目標(biāo)為（22.03，-1.80，1.73）時(shí)7種算法下的適應(yīng)度標(biāo)準(zhǔn)差比較Fig.14 When target is（22.03，-1.80，1.73），fitness standard deviation comparison under seven algorithms

5 結(jié)語

本文為消除復(fù)雜背景對(duì)求解物體在參考坐標(biāo)系下位置的影響，使用圖像差分算法消除背景，利用輪廓包圍法找出目標(biāo)物體區(qū)域，再進(jìn)一步求出目標(biāo)物6 個(gè)靶點(diǎn)在圖像像素坐標(biāo)系中的位置。使用基于模型估計(jì)的位姿求解方法，通過實(shí)驗(yàn)與實(shí)際物體在某一坐標(biāo)系下的位置進(jìn)行對(duì)比，結(jié)果表明使用方本文法求出的物體位置準(zhǔn)確度較高。

針對(duì)五自由度機(jī)械臂運(yùn)動(dòng)規(guī)劃問題，多種群粒子群算法利用經(jīng)過機(jī)器視覺處理后得到的物體位置進(jìn)行演化迭代，最后演化求出一條最優(yōu)路徑使機(jī)械臂到達(dá)目標(biāo)物體處，與傳統(tǒng)的粒子群算法和其他智能算法相比，多種群粒子群算法使用精英種群提高了算法的效率與整體性能。

PSOEL 中的預(yù)選擇與種群間特殊交互機(jī)制，改進(jìn)了演化策略，提高種群的搜索性能，提高了算法的演化速度與收斂性能。實(shí)驗(yàn)結(jié)果表明，與其他演化算法相比，多種群粒子群算法收斂性效果更優(yōu)，求出的機(jī)械臂末端執(zhí)行器運(yùn)動(dòng)到目標(biāo)物體位置處，與目標(biāo)物體的坐標(biāo)誤差平均最小，由此說明多種群粒子群算法在求解機(jī)械臂最優(yōu)路徑問題時(shí)平均性能較優(yōu)。

其他用于機(jī)械臂路徑規(guī)劃的智能算法并沒有結(jié)合單目視覺，而本文將單目與機(jī)械臂相結(jié)合解決機(jī)械臂最優(yōu)路徑規(guī)劃的問題；利用單目視覺相關(guān)智能算法可解決物體在復(fù)雜靜態(tài)環(huán)境下位置難以求解的問題，另外提高了復(fù)雜靜態(tài)環(huán)境下機(jī)械臂自主抓取的效率。利用PSOEL 算法根據(jù)機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)學(xué)模型建立約束方程，解決復(fù)雜靜態(tài)環(huán)境下物體的抓取問題，解決了抓取實(shí)時(shí)性差的問題，對(duì)工業(yè)場景中機(jī)械臂自主決策抓取功能的實(shí)現(xiàn)有一定研究價(jià)值，可以有效地讓工業(yè)機(jī)械臂自主抓取物體，完全擺脫機(jī)械臂固定示教抓取模式，該種算法在工業(yè)機(jī)械臂抓取上有很高的實(shí)時(shí)性，在很大程度上，可提高工業(yè)生產(chǎn)效率?；诒疚乃惴?，未來研究方向包括解決目標(biāo)物體與背景顏色非常接近、目標(biāo)物體不規(guī)則等情況下的目標(biāo)抓取問題。