基于粒子群算法的碼垛機(jī)器人時間軌跡優(yōu)化研究

徐尤南，劉志強(qiáng)，陳 潔

（華東交通大學(xué)機(jī)電與車輛工程學(xué)院，江西 南昌 330013）

隨著自動化生產(chǎn)的日益普遍，機(jī)器人的應(yīng)用越來越廣泛， 碼垛機(jī)器人一直以來也是廠家爭奪的重要市場之一， 碼垛機(jī)器人在流水作業(yè)生產(chǎn)線中的應(yīng)用十分廣泛， 是表征工業(yè)自動化水平的重要標(biāo)志[1-3]。 而在機(jī)器人的研究中，軌跡規(guī)劃是一個重要模塊，它對提高機(jī)器人的工作效率至關(guān)重要[4]。 機(jī)器人軌跡規(guī)劃中路徑的設(shè)定可在關(guān)節(jié)空間坐標(biāo)，也可在笛卡爾空間坐標(biāo)[5]。 笛卡爾空間軌跡規(guī)劃通俗易懂，但矩陣運(yùn)量大，其坐標(biāo)值不易測量，難以實(shí)時控制。 關(guān)節(jié)空間軌跡規(guī)劃可根據(jù)設(shè)計(jì)實(shí)時調(diào)整軌跡角度在時間上的變化，可避免運(yùn)動過程中機(jī)構(gòu)奇異或冗余現(xiàn)象，可控性好，故使用較為廣泛[6]。

最優(yōu)軌跡規(guī)劃是工業(yè)機(jī)器人最優(yōu)控制問題之一，規(guī)劃的任務(wù)是根據(jù)給定的路徑點(diǎn)規(guī)劃出通過這些點(diǎn)并滿足邊界約束條件的光滑的最優(yōu)運(yùn)動軌跡[7]。軌跡優(yōu)化有時間最優(yōu)和能量最優(yōu)兩種，前者使用較為廣泛。

時間最優(yōu)最早由Lin[8]中提出，其目的是最大化操作速度而最小化操作時間[9]。 機(jī)械臂關(guān)節(jié)空間軌跡規(guī)劃大多采用樣條插值的方法[10]。 三次樣條插值的最優(yōu)時間軌跡優(yōu)化使用廣泛， 計(jì)算也相對簡單。但對軌跡跟蹤精度更高，受振動影響更大等要求較高的機(jī)械手，最小脈動和脈動連續(xù)軌跡規(guī)劃受到密切關(guān)注。 朱世強(qiáng)等[11-12]對5 次多項(xiàng)式及7 次B 樣條插值方法做了研究，但計(jì)算量都較大。 提高插值平滑度和減小計(jì)算量成了一個矛盾問題。 采用分段插值由于計(jì)算量小，插值平滑等特點(diǎn)被廣泛使用。 考慮脈動連續(xù)性，Xu 等[13]采用3-5-3 多項(xiàng)式對機(jī)器人軌跡進(jìn)行規(guī)劃，何平等[14]進(jìn)行了修正。 何建成等[15]通過粒子群算法實(shí)現(xiàn)了工業(yè)機(jī)器人時間最優(yōu)、能耗最小、脈動做優(yōu)等多目標(biāo)下的最優(yōu)軌跡規(guī)劃。劉帥等[16]基于粒子群算法對工業(yè)機(jī)器人智能打磨軌跡問題進(jìn)行研究。 除此之外，粒子群算法在其他領(lǐng)域也得到廣泛使用，如黃江平等[17]通過改進(jìn)粒子群算法實(shí)現(xiàn)軌道列車節(jié)能控制優(yōu)化，楊剛等[18]的手寫數(shù)字識別研究等。

粒子群算法（PSO），其思想起源于鳥類捕食行為，模仿鳥類捕食的行為，關(guān)注個體行為與集體行為，是一種集體尋優(yōu)的優(yōu)化算法。 本文應(yīng)用POS 優(yōu)化算法，考慮運(yùn)動約束并滿足最大速度約束條件下對碼垛機(jī)器人的一個碼垛周期進(jìn)行3-5-3 多項(xiàng)式軌跡最優(yōu)時間求解。

1 系統(tǒng)

1.1 模型

本文以IRB460 碼垛機(jī)器人為參考， 應(yīng)用Matlab 工具箱Matlab Robotics 工具對碼垛機(jī)器人進(jìn)行建模， 通過機(jī)器人空間位姿及D-H 矩陣法， 通過Link 及SerialLink 構(gòu)建關(guān)節(jié)并連接起來， 構(gòu)成機(jī)器人結(jié)構(gòu)模型，如圖1 所示。

圖1 IRB460 碼垛機(jī)器人結(jié)構(gòu)示意圖Fig.1 Schematic diagram of IRB460 palletizing robot

1.2 路徑描述

機(jī)器人運(yùn)動規(guī)劃一般被劃分為兩個子問題，即路徑規(guī)劃和軌跡規(guī)劃。 前者描述的是機(jī)器人位置的空間移動軌跡，后者描述的是沿著這條路徑的機(jī)器人位姿的時間歷程。

運(yùn)動路徑的定義是軌跡規(guī)劃的前提[19]，碼垛機(jī)器人的一個碼垛周期包含4 個主要位置點(diǎn)，分別是起始抓取位置（起始點(diǎn)），抬起位置上方（路徑點(diǎn)），放置位置上方（路徑點(diǎn)），放置位置（結(jié)束點(diǎn)），本文針對該運(yùn)動周期的路徑進(jìn)行軌跡研究。

2 碼垛機(jī)器人運(yùn)動學(xué)正逆解

在進(jìn)行機(jī)器人運(yùn)動正逆解之前，應(yīng)建立其DH 參數(shù)表，如表1 所示。 表中ai為連桿長度；αi為桿之的轉(zhuǎn)動力角度；d 為桿間長度；θ 為桿間角度；ai-1和αi-1描述連桿i-1 本身特征與下一連桿i 的變化聯(lián)系。

表1 D-H 參數(shù)表Tab.1 D-H parameters

同時還需建立機(jī)器人各關(guān)節(jié)坐標(biāo)變換圖，如圖2 所示，在此基礎(chǔ)上推導(dǎo)正反解析式。

圖2 機(jī)器人各關(guān)節(jié)坐標(biāo)變換圖Fig.2 Coordinate transformation diagram of each joint of robot

關(guān)節(jié)1 坐標(biāo)系的建立與初始坐標(biāo)系完全相同，將關(guān)節(jié)1 坐標(biāo)系和關(guān)節(jié)2 坐標(biāo)系設(shè)于同一點(diǎn)；關(guān)節(jié)4，關(guān)節(jié)5 坐標(biāo)系為腕關(guān)節(jié)坐標(biāo)，位于同一點(diǎn)。5 個關(guān)節(jié)均為轉(zhuǎn)動關(guān)節(jié)，轉(zhuǎn)動變量分別為θ1，θ2，θ3，θ4，θ5。其中，a2，a3分別表示關(guān)節(jié)2，關(guān)節(jié)3 的關(guān)節(jié)長度。

根據(jù)機(jī)械臂各關(guān)節(jié)坐標(biāo)變換可以建立該機(jī)器人的運(yùn)動學(xué)正解算方程。

1） 各個連桿的變換矩陣Tii-1表示為

式中：si表示sinθi；ci表示cosθi。 sij表示sin（θi+θj）；cij表示cos（θi+θj）。

3） 機(jī)器人末端執(zhí)行器位姿T50表示為

4） 計(jì)算各軸關(guān)節(jié)角度值

機(jī)器人末端執(zhí)行器的位姿

已知機(jī)器人的期望位姿，可通過運(yùn)動學(xué)逆解求解出各關(guān)節(jié)點(diǎn)的參數(shù)，同時得到關(guān)節(jié)在不同位置空間的角度插值。

3 軌跡規(guī)劃

碼垛機(jī)器人在代替人類完成搬運(yùn)工作過程中，已知執(zhí)行末端起點(diǎn)及終點(diǎn)的位置，容易通過運(yùn)動學(xué)正逆解求出每個關(guān)節(jié)的位移， 以便對其進(jìn)行軌跡規(guī)劃，應(yīng)用MATLAB 軟件可對其在笛卡爾空間和關(guān)節(jié)空間參數(shù)進(jìn)行分析。 利用jtraj 函數(shù)對碼垛機(jī)器人4 點(diǎn)之間的3 段軌跡進(jìn)行關(guān)節(jié)空間軌跡插值， 利用ctraj函數(shù)對碼垛機(jī)器人4 點(diǎn)之間的3 段軌跡進(jìn)行笛卡爾空間軌跡插值,其操作過程較為簡單,具體從略。

4 軌跡優(yōu)化

本文基于粒子群優(yōu)化算法對碼垛機(jī)器人一個碼垛周期的軌跡進(jìn)行時間優(yōu)化。

4.1 優(yōu)化目標(biāo)

針對粒子群優(yōu)化算法對碼垛機(jī)器人進(jìn)行軌跡優(yōu)化這一問題，目標(biāo)是滿足機(jī)器人運(yùn)動學(xué)約束條件下，所有關(guān)節(jié)運(yùn)動的時間最短。 碼垛機(jī)器人工作過程中主要是最大速度的約束，故優(yōu)化問題的目標(biāo)函數(shù)如式（18）所示，該式也是POS 優(yōu)化過程中的適應(yīng)度函數(shù)。 即滿足最大速度約束的條件下，求一個碼垛周期的最短工作時間。 約束條件如式（19）所示。

4.2 優(yōu)化過程

碼垛過程中機(jī)器人機(jī)械臂末端經(jīng)過兩個中間路徑點(diǎn)，當(dāng)已知起點(diǎn)，終點(diǎn)及兩個中間路徑點(diǎn)在直角坐標(biāo)系下的位置，即給定機(jī)器人機(jī)械臂末端的期望位姿， 可得通過運(yùn)動學(xué)逆解求出各個關(guān)節(jié)參數(shù)，并將笛卡爾坐標(biāo)轉(zhuǎn)化為關(guān)節(jié)空間坐標(biāo)的各關(guān)節(jié)點(diǎn)在4 個插值點(diǎn)的關(guān)節(jié)角度，用θij表示，其中i 表示關(guān)節(jié)數(shù)，j 表示插值點(diǎn)序數(shù)。 表2 是機(jī)械手末端執(zhí)行器在笛卡爾空間的路徑插值點(diǎn)，表3 是通過運(yùn)動學(xué)逆解求出各個關(guān)節(jié)點(diǎn)在4 個插值點(diǎn)的關(guān)節(jié)角度。

表2 笛卡爾空間的路徑插值點(diǎn)Tab.2 Path interpolation points in Cartesian space

表3 關(guān)節(jié)空間的路徑插值點(diǎn)Tab.3 Path interpolation points in joint space

當(dāng)一個碼垛周期的3 段軌跡采用3-5-3 樣條多項(xiàng)式插值時，其軌跡表達(dá)式為

式中：li1（t），li2（t），li3（t）分別表示各關(guān)節(jié)3-5-3 樣條多項(xiàng)式的運(yùn)動軌跡；t1，t2，t3分別表示3-5-3 樣條多項(xiàng)式插值的時間。

由此，可將多項(xiàng)式的系數(shù)作為尋優(yōu)量。 又已知在始末點(diǎn)及中間路徑點(diǎn)的速度和加速度均為0，且3 段運(yùn)動軌跡是連續(xù)的。 得多項(xiàng)式系數(shù)與插值點(diǎn)關(guān)系為

由矩陣A 可知，尋優(yōu)量的求解只與時間t 有關(guān)，所以選擇時間變量t 作為搜索空間進(jìn)行優(yōu)化。 軌跡優(yōu)化即滿足運(yùn)動學(xué)約束條件下，所有關(guān)節(jié)運(yùn)動的時間最短。

應(yīng)用粒子群優(yōu)化算法進(jìn)行碼垛機(jī)器人碼垛周期的時間最優(yōu)求解時，將待求解的時間看作搜索空間的粒子，每個粒子都有自己的位置屬性和速度屬性，前者決定粒子的位置，后者決定粒子的飛行方向和距離。 所有粒子都會通過一個被優(yōu)化的函數(shù)來判斷適應(yīng)度值并追尋最優(yōu)。

在未找到最優(yōu)解時，粒子都會通過循環(huán)迭代不斷更新自己的速度和位置，即優(yōu)化過程。 粒子優(yōu)化包括個體粒子最優(yōu)和群體粒子最優(yōu)，個體最優(yōu)粒子的更新方式從當(dāng)前新粒子及個體最優(yōu)粒子中選擇支配粒子，兩者無法支配時任選其一，群體最優(yōu)粒子從已產(chǎn)生的最優(yōu)粒子中隨機(jī)選擇一個。 來更新自己的位置和速度，更新公式如下

式中：ω 為慣性權(quán)重；k 為迭代次數(shù)；i 代表第i 個粒子；d 為設(shè)計(jì)參數(shù)的維度；y1，y2為學(xué)習(xí)因子（也可理解為粒子加速度常數(shù)）；r1，r2為區(qū)間[0，1]之間的任意值； 粒子的位置xid∈[-xmax，xmax]， 粒子的速度vid∈[-vmax，vmax]，且都要符合該約束條件，若不符合將被邊界值替代。 速度更新函數(shù)中，右邊第1 部分是之前的速度乘以慣性權(quán)重，右邊第2 部分是粒子自身的位置優(yōu)化，反映粒子當(dāng)前位置與粒子本身歷史最優(yōu)位置的差距，右邊第3 部分粒子在全局的位置優(yōu)化，反映粒子當(dāng)前位置與群體歷史最優(yōu)位置的差距。 粒子位置更新則是上一次位置與當(dāng)前速度疊加。

應(yīng)用粒子群優(yōu)化算法對某個關(guān)節(jié)進(jìn)行時間最優(yōu)規(guī)劃流程如圖3 所示，主要步驟為：

圖3 粒子群軌跡優(yōu)化流程圖Fig.3 Flow chart of particle swarm optimization

Step1 初始化種群粒子，包括粒子位置和速度，個體最優(yōu)位置，群體最優(yōu)位置及種群規(guī)模；

Step2 求解矩陣A；

Step3 求解運(yùn)動多項(xiàng)式， 判斷是否滿足速度約束；

Step4 評價粒子適應(yīng)度；

Step5 當(dāng)前適應(yīng)度與個體歷史最優(yōu)適應(yīng)度比較，求個體最優(yōu)位置；

Step6 當(dāng)前適應(yīng)度與全局歷史最優(yōu)適應(yīng)度比較，求群體最優(yōu)位置；

Step7 更新種群的位置和速度；Step8 滿足終止條件，或返回Step2；Step9 輸出結(jié)果。

4.3 優(yōu)化結(jié)果

在對機(jī)器人進(jìn)行正逆解時發(fā)現(xiàn)，末端連桿的參數(shù)變化較大， 它也是碼垛工作中最最終的執(zhí)行元件，故本文以末端關(guān)節(jié)5 為例對機(jī)器人的一個碼垛周期進(jìn)行時間最優(yōu)求解。 結(jié)果如表4 所示。

表4 關(guān)節(jié)5 最優(yōu)插值時間Tab.4 Optimal interpolation time of Joint 5

如圖4 所示分別是關(guān)節(jié)5 速度限制為80 °/s 及110 °/s 時最優(yōu)粒子的位置變化圖。 從圖中可以看出，在迭代次數(shù)達(dá)到50 次以上，粒子是收斂的。 優(yōu)化過程中將迭代次數(shù)設(shè)置為100 次，作為優(yōu)化結(jié)束的結(jié)束條件。

從圖4 可以看出，在迭代次數(shù)達(dá)到50 次以上，粒子迅速收斂。 且收斂值在給定的速度約束下。 優(yōu)化過程中將迭代次數(shù)設(shè)置為100 次，作為優(yōu)化結(jié)束的結(jié)束條件。關(guān)節(jié)5 的兩種速度限制下經(jīng)粒子群優(yōu)化的3-5-3 多次多項(xiàng)式所需的3 段插值時間t，如圖5 所示。

圖4 最優(yōu)粒子位置變化Fig.4 Optimal particle position change

圖5 最優(yōu)時間關(guān)節(jié)5 運(yùn)動響應(yīng)曲線Fig.5 Motion response curve of optimal time Joint 5

從圖5 可以看出，關(guān)節(jié)5 的角位移s、角速度v、角加速度α 均滿足14 個運(yùn)動學(xué)約束條件， 結(jié)果證明了采用速度約束下的POS 在計(jì)算機(jī)上離線尋找機(jī)器人3 段插值最優(yōu)時間軌跡規(guī)劃的可靠性。

5 結(jié)論

經(jīng)研究得到以下結(jié)論：

1） 采用自適應(yīng)慣性權(quán)重的約束粒子群算法尋找最優(yōu)解，從而構(gòu)造出機(jī)器人關(guān)節(jié)參數(shù)化曲線；

2） 該方法可以最大限度地提高末端執(zhí)行器的可操作性；

3） 粒子群算法簡單，參數(shù)易調(diào)整，彌補(bǔ)了多項(xiàng)式計(jì)算階次高而使優(yōu)化運(yùn)算復(fù)雜的不足；

4） 粒子群算法中粒子在收斂末期容易形成振蕩，模型不同時需要重新調(diào)整參數(shù)以達(dá)到最優(yōu)結(jié)果。