郭彤穎，劉 雍，王海忱，李 峰

(沈陽(yáng)建筑大學(xué) 信息與控制工程學(xué)院，沈陽(yáng) 110168)

0 引言

機(jī)械手臂作為自動(dòng)化設(shè)備中的一個(gè)重要組成部分，以其操作靈活性，被廣泛應(yīng)用于工業(yè)生產(chǎn)、醫(yī)療服務(wù)、軍事科技等各個(gè)方面。對(duì)于不同的工作環(huán)境，我們要對(duì)機(jī)械手臂的各個(gè)關(guān)節(jié)進(jìn)行具體的軌跡規(guī)劃，從而得到機(jī)械手臂末端的位姿，因此，對(duì)機(jī)械手臂進(jìn)行精確快速的軌跡規(guī)劃就顯得尤為重要[1]。機(jī)械手臂關(guān)節(jié)空間的軌跡跟蹤就是通過控制各關(guān)節(jié)的位置、速度、加速度等變量使機(jī)械手臂沿期望軌跡運(yùn)動(dòng)[2]。

早在1971年Kahn和Roth[3]就研究過在做假設(shè)和近似的前提下，機(jī)械手臂做點(diǎn)對(duì)點(diǎn)運(yùn)動(dòng)時(shí)的最優(yōu)軌跡問題。Bazaz和Tondu[4]利用三次樣條函數(shù)進(jìn)行在線軌跡規(guī)劃，并進(jìn)行了時(shí)間最優(yōu)化的設(shè)計(jì)，其缺點(diǎn)為不能使軌跡點(diǎn)間的加速度連續(xù)。Piazzi A和Visioli A[5]設(shè)計(jì)了時(shí)間最短條件下的三次樣條軌跡規(guī)劃，其由5段三次多項(xiàng)式插值滿足位置、速度、加速度的約束條件，但是缺點(diǎn)是不能滿足脈動(dòng)連續(xù)。惲為民[6]利用遺傳算法優(yōu)化機(jī)械手臂的動(dòng)力學(xué)模型，設(shè)計(jì)了機(jī)械手臂的最優(yōu)時(shí)間軌跡規(guī)劃，缺點(diǎn)是多自由度計(jì)算時(shí)速度較慢。李俊等[7]以多關(guān)節(jié)機(jī)器人為對(duì)象，利用樣條函數(shù)和改進(jìn)的遺傳算法，避免了運(yùn)動(dòng)軌跡出現(xiàn)奇異結(jié)構(gòu)點(diǎn)和局部最優(yōu)問題，但缺點(diǎn)是結(jié)果可靠性差，不能穩(wěn)定求解。管成等[8]提出了一種基于NURBS曲線的軌跡規(guī)劃方法，能夠得到滿足物理約束的時(shí)間最優(yōu)關(guān)節(jié)軌跡，缺點(diǎn)是求導(dǎo)計(jì)算式繁瑣，求解效率低。

本文研究過路徑點(diǎn)的基于關(guān)節(jié)空間的軌跡規(guī)劃，使軌跡與B樣條曲線逼近，并運(yùn)用粒子群算法進(jìn)行優(yōu)化，最終達(dá)到的效果是使軌跡更加平滑連續(xù)，提高機(jī)械手臂的穩(wěn)定性。

1 基于B樣條曲線的軌跡規(guī)劃

Bezier曲線是由法國(guó)工程師皮埃爾·貝塞爾(Pierre Bézier)于1962年提出的一種曲線逼近方法[9]。而B樣條曲線則是Bezier曲線的一種一般化，其繼承了Bezier曲線的優(yōu)點(diǎn)并克服了其缺點(diǎn)。四階三次的B樣條曲線的表達(dá)式為：

(1)

其中，P0，P1，P2，P3為4個(gè)控制節(jié)點(diǎn)，0≤u≤1。

應(yīng)用B樣條曲線函數(shù)規(guī)劃?rùn)C(jī)械手臂路徑的基本思想如下：以機(jī)械手臂的關(guān)節(jié)空間中每個(gè)采樣區(qū)間端點(diǎn)為型值點(diǎn)，設(shè)其為V1，V2，...，Vm，根據(jù)連續(xù)性條件以及邊界條件，可以求出m個(gè)控制點(diǎn)P1，P2，...，Pm。則每?jī)蓚€(gè)相鄰型值點(diǎn)之間可用一段B曲線連接，因此，整個(gè)軌跡即為m-1段B曲線拼接而成。設(shè)第i段B曲線連接的型值點(diǎn)為Vi和Vi+1，則第i段曲線將由Pi-1，Pi，Pi+1，Pi+2這4個(gè)控制節(jié)點(diǎn)來(lái)控制。控制節(jié)點(diǎn)的坐標(biāo)可表示為Pi-1(ti-1，qi-1)，Pi(ti，qi)，Pi+1(ti+1，qi+1)，Pi+2(ti+2，qi+2)。這樣，當(dāng)路徑點(diǎn)確定后，即可通過相鄰的4個(gè)路徑點(diǎn)確定一段完整的B樣條曲線路徑，并通過Bezier函數(shù)確定各個(gè)關(guān)節(jié)的運(yùn)行情況。若t(u)，q(u)分別表示第i段曲線上任意點(diǎn)的橫縱坐標(biāo)，則機(jī)械手臂的B樣條曲線方程可表示為：

(2)

(3)

為了得到P1，Pm兩個(gè)控制點(diǎn)，B樣條函數(shù)需要附加2個(gè)邊界條件，即附加兩個(gè)控制點(diǎn)P0和Pm+1。

約束條件為對(duì)公式(1)進(jìn)行一階求導(dǎo)和二階求導(dǎo)，分別用θ(u),ω(u)表示：

(1+3u+3u2-3u3)Pi+1+u3Pi+2]

(3+6u-9u2)Pi+1+3u2Pi+2]

角加速度為：

角加速度變化率為：

2 時(shí)間最優(yōu)問題求解

B樣條函數(shù)的求解以已知各型值點(diǎn)的坐標(biāo)為前提條件[10]。本文研究的目的為如何在各段連續(xù)曲線運(yùn)動(dòng)時(shí)取得最短時(shí)間并滿足約束條件，因此，設(shè)定粒子群算法的適應(yīng)度函數(shù)為：

(4)

vj≤vmax

(5)

其中，T為機(jī)械手臂沿整條軌跡運(yùn)動(dòng)的總時(shí)間，hi(i=1,2,....m-1)為機(jī)械手臂沿每一段B曲線運(yùn)動(dòng)所需時(shí)間，m為型值點(diǎn)總數(shù)，vj(j=1,2,…,6)為各關(guān)節(jié)的速度，vmax為允許的最大關(guān)節(jié)速度。

粒子群算法[11]是由J Kennedy和R C Eberhart等于1995年開發(fā)的一種演化計(jì)算技術(shù)，來(lái)源于對(duì)一個(gè)簡(jiǎn)化社會(huì)模型的模擬。該算法中每一個(gè)粒子的位置按照下式進(jìn)行變化：

vi(k+1)=w×vi(k)+c1×r1×(pi-xi(k))+

c2×r2×(pg-xi(k))

(6)

xi(k+1)=xi(k)+vi(k+1)

(7)

其中，vi(k)為第i個(gè)粒子第k次迭代時(shí)的速度，xi(k)為第i個(gè)粒子第k次迭代的位置，pg為群體最好位置，pi為粒子最好位置，c1和c2為權(quán)重因子，r1和r2為隨機(jī)數(shù)，w為慣性權(quán)重。

粒子群算法步驟如圖1所示。

圖1 機(jī)械手臂粒子群算法過程

3 實(shí)驗(yàn)研究

3.1 實(shí)驗(yàn)平臺(tái)及模型建立

本文的研究對(duì)象CytonII型機(jī)械手臂是具有6個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)的6自由度機(jī)械手臂。如圖2所示

圖2 CytonII型機(jī)械手臂

圖3 機(jī)械手臂坐標(biāo)系示意圖

根據(jù)D-H法建立的坐標(biāo)系圖如圖3所示。

其中，坐標(biāo)系0、1、3、5的Y軸垂直紙面向里，坐標(biāo)系2、4、6的Z軸垂直紙面向外。各連桿參數(shù)中，θn為連桿n繞關(guān)節(jié)n的Zn-1軸的轉(zhuǎn)角，dn為連桿間距離，即連桿n沿關(guān)節(jié)n的Zn-1軸的位移，an為沿Xn方向上連桿n的長(zhǎng)度，αn為連桿n兩關(guān)節(jié)軸線之間的扭角。d1=54mm，d3=140.8mm，d5=71.8mm，a2=66mm，a4=71.8mm。各連桿參數(shù)見表1所示。

表1 機(jī)械手臂連桿參數(shù)表

3.2 仿真實(shí)驗(yàn)

當(dāng)機(jī)械手臂末端在直角坐標(biāo)空間中從一點(diǎn)運(yùn)動(dòng)到另一點(diǎn)時(shí)，由已知的起始點(diǎn)，終止點(diǎn)以及軌跡經(jīng)過的中間點(diǎn)，通過機(jī)械手臂運(yùn)動(dòng)學(xué)逆解可求出各關(guān)節(jié)的型值點(diǎn)。本文中各型值點(diǎn)的坐標(biāo)如表2所示。

表2 節(jié)點(diǎn)的數(shù)值表

按照上節(jié)中采用基于粒子群算法的B樣條曲線插值法進(jìn)行軌跡規(guī)劃，確定每段曲線的最優(yōu)時(shí)間。將各關(guān)節(jié)的型值點(diǎn)作為初始種群，則初始種群數(shù)為M=10，粒子飛行速度的參數(shù)設(shè)置為慣性權(quán)重w=0.5+r/2，r為0～1之間的隨機(jī)數(shù)。權(quán)重因子取為c1=0.05，c2=0.05。r1和r2為[-1,1]之間的隨機(jī)數(shù)。機(jī)械手臂關(guān)節(jié)允許的最大運(yùn)動(dòng)速度為vmax=1m/s，軌跡曲線的初始點(diǎn)和終點(diǎn)的速度以及加速度均為0，循環(huán)迭代步數(shù)取為50步。經(jīng)過Matlab仿真及計(jì)算，取機(jī)械手臂前三關(guān)節(jié)為例，其輸出的優(yōu)化結(jié)果見表3所示, 表中hi為每段軌跡運(yùn)行的時(shí)間。

表3 優(yōu)化結(jié)果表

根據(jù)實(shí)驗(yàn)數(shù)據(jù)表明，經(jīng)過粒子群算法優(yōu)化的B樣條軌跡曲線，其各段軌跡運(yùn)行總時(shí)間大約縮短2s左右，達(dá)到了實(shí)驗(yàn)的預(yù)期效果。見圖4～圖6。

圖4 關(guān)節(jié)1的角位移、角速度以及角加速度

圖5 關(guān)節(jié)2的角位移、角速度以及角加速度

圖6 關(guān)節(jié)3的角位移、角速度以及角加速度

4 結(jié)論

本文利用粒子群算法針對(duì)機(jī)械手臂B樣條曲線軌跡規(guī)劃進(jìn)行了以最優(yōu)時(shí)間為目標(biāo)的軌跡規(guī)劃。經(jīng)過Matlab仿真及實(shí)驗(yàn)分析，通過粒子群算法對(duì)各段軌跡的時(shí)間間隔進(jìn)行優(yōu)化，提高了機(jī)械手臂15%～20%的工作效率，更快速、準(zhǔn)確地規(guī)劃出工作軌跡，并能夠使機(jī)械手臂運(yùn)行平穩(wěn)，軌跡平滑連續(xù)，有效地避免了機(jī)器臂產(chǎn)生震動(dòng)或碰撞。