徐尤南，劉志強，陳 潔

（華東交通大學機電與車輛工程學院，江西 南昌 330013）

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

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

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

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

1 系統(tǒng)

1.1 模型

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

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

1.2 路徑描述

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

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

2 碼垛機器人運動學正逆解

在進行機器人運動正逆解之前，應(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

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

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

關(guān)節(jié)1 坐標系的建立與初始坐標系完全相同，將關(guān)節(jié)1 坐標系和關(guān)節(jié)2 坐標系設(shè)于同一點；關(guān)節(jié)4，關(guān)節(jié)5 坐標系為腕關(guān)節(jié)坐標，位于同一點。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ù)機械臂各關(guān)節(jié)坐標變換可以建立該機器人的運動學正解算方程。

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

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

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

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

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

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

3 軌跡規(guī)劃

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

4 軌跡優(yōu)化

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

4.1 優(yōu)化目標

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

4.2 優(yōu)化過程

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

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

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

當一個碼垛周期的3 段軌跡采用3-5-3 樣條多項式插值時，其軌跡表達式為

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

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

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

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

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

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

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

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

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

Step2 求解矩陣A；

Step3 求解運動多項式， 判斷是否滿足速度約束；

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

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

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

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

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

在對機器人進行正逆解時發(fā)現(xiàn)，末端連桿的參數(shù)變化較大， 它也是碼垛工作中最最終的執(zhí)行元件，故本文以末端關(guān)節(jié)5 為例對機器人的一個碼垛周期進行時間最優(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ù)達到50 次以上，粒子是收斂的。 優(yōu)化過程中將迭代次數(shù)設(shè)置為100 次，作為優(yōu)化結(jié)束的結(jié)束條件。

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

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

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

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

5 結(jié)論

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

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

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

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

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