粒子群算法在機械手臂B樣條曲線軌跡規(guī)劃中的應用

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

(沈陽建筑大學 信息與控制工程學院，沈陽 110168)

0 引言

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

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

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

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

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

(1)

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

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

(2)

(3)

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

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

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

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

角加速度為：

角加速度變化率為：

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

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

(4)

vj≤vmax

(5)

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

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

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個粒子第k次迭代時的速度，xi(k)為第i個粒子第k次迭代的位置，pg為群體最好位置，pi為粒子最好位置，c1和c2為權(quán)重因子，r1和r2為隨機數(shù)，w為慣性權(quán)重。

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

圖1 機械手臂粒子群算法過程

3 實驗研究

3.1 實驗平臺及模型建立

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

圖2 CytonII型機械手臂

圖3 機械手臂坐標系示意圖

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

其中，坐標系0、1、3、5的Y軸垂直紙面向里，坐標系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的長度，αn為連桿n兩關(guān)節(jié)軸線之間的扭角。d1=54mm，d3=140.8mm，d5=71.8mm，a2=66mm，a4=71.8mm。各連桿參數(shù)見表1所示。

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

3.2 仿真實驗

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

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

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

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

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

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

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

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

4 結(jié)論

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