三自由度液壓伺服機械手軌跡優(yōu)化

劉湘琪，蒙 臻，倪 敬，朱澤飛

（1.浙江理工大學(xué)機械與自動控制學(xué)院，浙江杭州310018；2.杭州電子科技大學(xué)機械工程學(xué)院，浙江杭州310018）

三自由度液壓伺服機械手軌跡優(yōu)化

劉湘琪1，2，蒙 臻1，倪 敬2，朱澤飛2

（1.浙江理工大學(xué)機械與自動控制學(xué)院，浙江杭州310018；2.杭州電子科技大學(xué)機械工程學(xué)院，浙江杭州310018）

采用閉鏈?zhǔn)噶窟\動學(xué)模型，引入5階B樣條曲線插值逼近理論，提出一種基于非軸系驅(qū)動空間的液壓機械手運行軌跡規(guī)劃算法.針對當(dāng)滿足運動學(xué)約束時機機械手的總運行時間優(yōu)化問題，引入改進(jìn)型粒子群優(yōu)化算法，優(yōu)化求解時間步長，從而實現(xiàn)時間最優(yōu)連續(xù)運動軌跡曲線.實際應(yīng)用的實驗結(jié)果顯示：可以將實際工具中心點（TCP）軌跡和期望軌跡的絕對誤差控制在25 mm以內(nèi)，為液壓機械手運動控制提供較理想的軌跡曲線.該研究較好地解決了非關(guān)節(jié)驅(qū)動液壓機械手系統(tǒng)運動規(guī)劃問題，提高了針對復(fù)雜路徑規(guī)劃算法的效率.

閉鏈模型；軌跡規(guī)劃；B樣條曲線；粒子群優(yōu)化算法

基于電液伺服控制的多自由度液壓機械手，具有負(fù)載剛性大、功率重量比大、耐溫性強、使用維護(hù)方便等特點，廣泛應(yīng)用于大功率負(fù)載搬運的場合［1］.在應(yīng)用過程中，對于液壓伺服機械手運行精度和生產(chǎn)效率的要求愈加提高，即機械手高速、精確的運動變得十分重要［2］，因此，為了實現(xiàn)液壓機械手高效、高精度的運轉(zhuǎn)，同時保證其運行平穩(wěn)性，研究液壓伺服機械手軌跡規(guī)劃算法具有重要的實際意義.

機械手的軌跡規(guī)劃問題即根據(jù)給定的幾何路徑尋求運動學(xué)模型［3］.Abe［4］基于兩連桿串聯(lián)機構(gòu)的運動學(xué)模型，研究了機械手執(zhí)行末端軌跡規(guī)劃問題.朱世強等［5］基于六自由度串聯(lián)機械手的運動學(xué)模型，研究了速度、加速度和脈動均連續(xù)的關(guān)節(jié)軌跡.周芳等［6］基于六自由度檢修機械手的運動學(xué)模型，研究了所規(guī)劃軌跡的安全性.在上述研究中，國內(nèi)外學(xué)者主要基于常規(guī)開鏈?zhǔn)浇Y(jié)構(gòu)的機械手，采用De-navit-Hartenberg（D-H）法構(gòu)造軌跡運動學(xué)模型.在實際應(yīng)用研究中，由于多自由度液壓機械手的閉鏈?zhǔn)浇Y(jié)構(gòu)和非獨立軸系驅(qū)動形式，導(dǎo)致各關(guān)節(jié)的運動學(xué)空間具有較強的耦合作用，采用常規(guī)軌跡運動學(xué)模型較難求解液壓機械手運動軌跡，研究適用于液壓機械手的軌跡運動學(xué)模型顯得十分必要.

關(guān)于機械手的軌跡規(guī)劃算法不斷涌現(xiàn).Gasparetto等［7］同時采用了三次樣條插值函數(shù)與5階B樣條插值函數(shù)推導(dǎo)軌跡算法，并探討了軌跡執(zhí)行時間最優(yōu)與軌跡光滑性最優(yōu)的權(quán)重問題.Tian等［8］采用基于Hermite三次樣條插值函數(shù)的方法規(guī)劃機械手軌跡，并基于遺傳算法優(yōu)化軌跡插值函數(shù).Bauml等［9］研究了機器人實現(xiàn)抓取任務(wù)的問題，提出了基于梯形速度曲線的空間軌跡規(guī)劃方法.Saravanan等［10］使用NURBS曲線規(guī)劃運動軌跡.徐海黎等［11］采用三次多項式曲線構(gòu)建機器人軌跡，通過使用加權(quán)系數(shù)的代價函數(shù)，計算優(yōu)化軌跡總動作時間和總消耗能量的權(quán)重.楊玉維等［12］基于拉格朗日原理構(gòu)建系統(tǒng)動力學(xué)模型，通過線性迭代法（iterative linear programming，ILP），規(guī)劃出運動時間最優(yōu)化軌跡.史也等［13］采用量子粒子群優(yōu)化算法，基于參數(shù)化的關(guān)節(jié)軌跡函數(shù)，實現(xiàn)空間機器人軌跡規(guī)劃目標(biāo).Guo等［14］采用量子粒子群算法進(jìn)行平面冗余機械臂避碰軌跡規(guī)劃.張書濤等［15］提出基于Tau理論的仿生軌跡規(guī)劃方法.戈新生等［16］基于傅里葉基函數(shù)，通過對其系數(shù)組成的向量進(jìn)行優(yōu)化，提出了一種應(yīng)用粒子群優(yōu)化的最優(yōu)運動規(guī)劃數(shù)值算法.上述研究所涉及的應(yīng)用模型主要集中于常規(guī)機械手，其驅(qū)動空間通常位于相對獨立的關(guān)節(jié)軸系上，可通過D-H法將末端位置矢量轉(zhuǎn)化到驅(qū)動空間上.在實際研究中，液壓機械手驅(qū)動空間位于非關(guān)節(jié)軸系的組合連桿上，通過閉鏈?zhǔn)綑C構(gòu)驅(qū)動末端動作，采用常規(guī)軌跡規(guī)劃方法較難解出平滑的運動軌跡，因此研究適用于多自由度液壓機械手的軌跡規(guī)劃方法顯得十分必要.

本文以三自由度（degrees of Freedom，DOF）的液壓伺服機械手系統(tǒng)為例，基于閉鏈?zhǔn)噶窟\動學(xué)模型，引入非軸系驅(qū)動空間的5階B樣條插值方法構(gòu)建軌跡，采用改進(jìn)型粒子群優(yōu)化算法來優(yōu)化軌跡的總動作時間，并通過實驗結(jié)果說明該軌跡規(guī)劃方法的有效性.

1 機械手機構(gòu)運動學(xué)模型

三自由度液壓伺服機械手主要由回轉(zhuǎn)系統(tǒng)、俯仰系統(tǒng)和推拉系統(tǒng)組成，如圖1所示.其中：回轉(zhuǎn)系統(tǒng)主要由回轉(zhuǎn)液壓馬達(dá)實現(xiàn)機械手工具中心點（tool center point，TCP）繞Z軸轉(zhuǎn)動（O-X-Z為側(cè)視坐標(biāo)系，O-X-Y為俯視坐標(biāo)系）；俯仰系統(tǒng)由俯仰液壓缸驅(qū)動，可實現(xiàn)TCP沿Z軸運動，附帶沿著X軸運動；推拉系統(tǒng)由推拉液壓缸驅(qū)動實現(xiàn)TCP沿X軸運動，附帶沿著Z軸運動.

圖1 三自由度液壓機械手結(jié)構(gòu)圖Fig.1 Configuration of hydraulically driven 3DOF manipulator

為了精確描述工作空間中的TCP位置矢量，同時考慮機械手運動學(xué)分析的完備性，可將3DOF液壓伺服機械手的運動模型簡化為如圖2所示，Ob-Xb-Yb-Zb為機械手基座坐標(biāo)系，Ot-Xt-Yt-Zt為機械手工具坐標(biāo)系，其中基座標(biāo)系原點Ob與機械手基座回轉(zhuǎn)中心重合，工具坐標(biāo)系原點Ot與TCP重合.各桿件的鉸接點由Pi表示（i＝1，2，…，10）.如圖2所示的機械手系統(tǒng)具有閉式連桿機構(gòu)特性和非軸系驅(qū)動的運動特性，若采用常規(guī)D-H法求解運動學(xué)問題會因冗余鉸接點產(chǎn)生非唯一解，因此采用幾何矢量解析法求解3DOF液壓伺服機械手運動學(xué)問題.

根據(jù)桿件封閉矢量方程可得

基于矢量平行或共線的特性可得

圖2 3DOF液壓機械手立面圖Fig.2 Elevation of hydraulically driven 3DOF manipulator

TCP位置矢量與基座標(biāo)系的矢量關(guān)系為

將式（1）～（6）代入式（7）可得

將式（8）用復(fù)數(shù)形式可表示為

將式（9）按歐拉公式展開可得

式中：

令r＝［xt，yt，zt］T，x＝［x1，x2，θ］T，其中x1為推拉系統(tǒng)的輸出位移，x2為俯仰系統(tǒng)的輸出位移，θ為回轉(zhuǎn)系統(tǒng)的輸出位移，則TCP空間位置r可根據(jù)式（8）表示為

式中：

B l5sinθ5，l20、l30分別為l2、l3的初始長度.

2 機械手軌跡優(yōu)化

2.1 基于B樣條的軌跡規(guī)劃

液壓機械手的軌跡S由工作空間中一系列離散的TCP空間位置序列rp和相應(yīng)的時間節(jié)點序列tp組成.為了精確描述驅(qū)動輸入x與軌跡S的映射關(guān)系，根據(jù)TCP空間位置模型，通過逆運動學(xué)求解，將TCP空間位置序列rp轉(zhuǎn)換為驅(qū)動空間節(jié)點位置序列xp，k，進(jìn)而構(gòu)成驅(qū)動節(jié)點-時間序列：

式中：p＝0，1，…，n，k＝0，1，2.

基于軌跡S的連續(xù)性要求，用C3連續(xù)的4次（5階）B樣條曲線構(gòu)造驅(qū)動節(jié)點軌跡.所有節(jié)點軌跡曲線統(tǒng)一描述為

式中：Qq∈RN×1為控制頂點矢量，q＝1，2，…，n＋1，t為通過相鄰控制頂點所用的時間維度，Nq，4（t）為4次B樣條基函數(shù)：

其中，j＝2，3，4.

基于De Boor遞推關(guān)系［6］，t（t∈［tq，tq＋1］）處的節(jié)點速度、加速度和脈動分別與B樣條軌跡曲線的l階導(dǎo)數(shù)（t）對應(yīng)，l＝1，2，3.

由式（17）、（18）可知，節(jié)點速度軌跡曲線V4（t）、加速度軌跡曲線A4（t）和脈動軌跡曲線J4（t）可分別表示為

根據(jù)樣條插值特性，節(jié)點軌跡插值函數(shù)滿足以下條件：

由式（19）可以列出3（n＋3）個方程，為了求解式（14）中產(chǎn)生的3（n＋4）個未知控制頂點矢量Qq，還需3個邊界條件，假設(shè)節(jié)點初始條件為

式中：t0為上述時間維度的起點.聯(lián)立式（14）～（22），以矩陣方程的形式描述第k個驅(qū)動節(jié)點的軌跡曲線控制頂點矢量反求方程：

式中：系數(shù)矩陣Mk∈R（n＋4）×（n＋4），控制頂點矩陣

Qk＝［Qk，0，Qk，1，…，Qk，n＋3］T，位置序列矩陣xk＝［xk，0，xk，1，…，xk，n，v0，a0，J0］T.

由式（24）可以求出4次B樣條軌跡曲線的控制頂點矢量，進(jìn)而求出驅(qū)動節(jié)點在時刻ti經(jīng)過位置xi的C3連續(xù)節(jié)點軌跡.

2.2 基于粒子群的軌跡優(yōu)化

在軌跡規(guī)劃過程中，通常既要滿足運動學(xué)約束，保證軌跡的平滑性，又要降低軌跡的總運行時間，保證機械手運行效率.為了綜合權(quán)重軌跡的平滑性和執(zhí)行時間，采用B樣條軌跡的通用優(yōu)化目標(biāo)函數(shù)［4］：

式中：hq表示相鄰節(jié)點步長，hq＝tq＋1－tq；tf表示軌跡總執(zhí)行時間；kT表示時間權(quán)重系數(shù)，kJ表示脈動權(quán)重系數(shù)（kT＝0，可得到最平滑的軌跡；kJ＝0，可得到執(zhí)行最快的軌跡）.式（23）中脈動平方的積分項其結(jié)構(gòu)復(fù)雜，較難解析，通常直接采用數(shù)值積分程序（如Matlab中的Quadl函數(shù)），或者可先通過遞歸De Boor公式確定基函數(shù)的解析表達(dá)式，再積分求解，最終獲得積分表達(dá)式.

實際上，在優(yōu)化求解過程中，為了降低計算量，可將kT、kJ及脈動平方積分項設(shè)定為確定值，因此，可將B樣條軌跡的優(yōu)化問題轉(zhuǎn)化為對hi的優(yōu)化.

受改進(jìn)型粒子群算法的啟發(fā)，將時間步長hi視作粒子，通過修改慣性權(quán)重參數(shù)追蹤最優(yōu)粒子，迭代搜索全局最優(yōu)解，從而求解式（25）描述的路徑點時間步長hi優(yōu)化問題，具體優(yōu)化過程如下［17-18］.

1）選取相鄰節(jié)點時間步長hu為粒子，第u個粒子的位置矢量為xu＝［hu1，hu2，…，huD］T，其速度矢量為vu＝［vu1，vu2，…，vuD］T.它的個體極值為Pu＝［Pu1，Pu2，…，PuD］T，種群的全局極值為Pg＝［Pg1，Pg2，…，PgD］T，D為粒子維度，其中，粒子群的優(yōu)化方向可由式（25）表示.

2）初始化粒子的位置和速度，所選擇的位置和速度具有一定的隨機性.

3）根據(jù)式（26），更新每個粒子的速度和位置：

式中：u＝1，2，…，N，N為粒子總數(shù)；d＝1，2，…，D；為第k次迭代粒子u速度矢量的d維分量；為第k次迭代粒子u位置矢量的d維分量；pud為粒子u在d維的個體極值點位置pbestu；pgd為群體在d維的全局極值點位置gbest；c1、c2為加速因子；rand（）為［0，1.0］的隨機數(shù)；w為慣性因子.

4）獲得最優(yōu)解.按照如圖3所示的程序流程依次迭代得到最優(yōu)解h，根據(jù)式（26）調(diào)整粒子的速度和位置，如果達(dá)到最大迭代次數(shù)M或最優(yōu)解停滯不再發(fā)生變化，則終止迭代；否則回到步驟2）.

圖3 粒子迭代優(yōu)化程序Fig.3 Particle iterative optimization procedure

3 實驗與分析

3.1 仿真實驗

為驗證上述方法的有效性，以如圖2所示的機械手模型為基礎(chǔ)，采用Matlab／Sim Mechanics建立機械手運動學(xué)模型，以對液壓機械手軌跡規(guī)劃方法進(jìn)行仿真驗證.機械手TCP經(jīng)過的空間位置序列及逆解驅(qū)動節(jié)點位置序列如表1所示，驅(qū)動節(jié)點動力學(xué)約束條件如表2所示.表中，x1為推拉系統(tǒng)的輸出位移，x2為俯仰系統(tǒng)的輸出位移，θ為回轉(zhuǎn)系統(tǒng)的輸出位移.

表1 空間位置序列及驅(qū)動節(jié)點位置序列Tab.1 Space position sequence and position sequence of drive node

表2 機械手運動學(xué)約束條件Tab.2 Manipulator kinematics constraint conditions

采用2.1節(jié)的插值軌跡規(guī)劃及時間步長優(yōu)化算法，經(jīng)過33次迭代規(guī)劃后，終止迭代.其中，時間步長hi的優(yōu)化過程如圖4所示；第0次、第10次及第23次迭代后，時間步長hi的優(yōu)化結(jié)果如表3所示.各驅(qū)動節(jié)點軌跡迭代優(yōu)化前、后，機械手仿真TCP軌跡如圖5所示.

圖4 各時間步長粒子優(yōu)化過程Fig.4 Particle optimization process of each time step

圖5 迭代前、后的TCP軌跡圖Fig.5 TCP trajectory before and after iteration

表3 各節(jié)點運動時間對比Tab.3 Comparison of movement time period of each joint s

3.2 系統(tǒng)實驗

實驗用3DOF液壓機械手系統(tǒng)如圖6所示，主要工況及元件包括液壓油源工作壓力（8 MPa）、工作流量（100 L／min）、液壓馬達(dá)A2FM56／61W、液壓缸Φ50／（36～100）mm、絕對位置旋轉(zhuǎn)編碼器E6B2-C、力士樂比例伺服閥4WRZE10-75、脈沖計數(shù)模塊、DA模塊以及研華工控機，具體實驗步驟如下.

1）實驗工況選取.實際3DOF液壓機械手系統(tǒng)的運行參數(shù)與表1、表2所示一致.為了便于觀測，設(shè)定機械手TCP起點為r0＝［790，0，425］T，目標(biāo)點為rg＝［0，1 268，755］T.

2）機械手軌跡規(guī)劃及優(yōu)化.采用插值軌跡規(guī)劃及時間步長優(yōu)化算法，基于3.1節(jié)中的軌跡規(guī)劃及優(yōu)化步驟擬合系統(tǒng)實驗所需的驅(qū)動節(jié)點軌跡.

3）運行結(jié)果分析.為了觀測軌跡規(guī)劃效果，本實驗采用常規(guī)PID控制（只選取了比例參數(shù)為1.8.基于3.1節(jié)中第23次迭代后的期望軌跡，對各驅(qū)動節(jié)點軌跡進(jìn)行跟蹤控制，最終實現(xiàn)如圖7所示的TCP軌跡.由圖7可知，由于液壓伺服系統(tǒng)的非線性因素及實際機構(gòu)裝配間隙等，實際TCP軌跡相比期望軌跡球面絕對誤差小于25 mm.本方法所規(guī)劃的機械手軌跡相對平滑，有利于機械手的控制；而且規(guī)劃的路徑中考慮機械手位置、速度、加速度和脈動的限制，使算法更具有實用價值.

圖6 3DOF液壓機械手現(xiàn)場實物圖Fig.6 Picture of hydraulically driven 3DOF manipulator

圖7 液壓機械手TCP運行軌跡Fig.7 TCP trajectory of hydraulically driven manipulator

4 結(jié) 論

（1）提出了基于閉鏈?zhǔn)噶繖C構(gòu)的液壓機械手運動學(xué)模型，將機械手TCP空間位置矢量解析為驅(qū)動節(jié)點空間位置矢量，使液壓機械手運動學(xué)建模方法更具實效性.

（2）提出了基于5階B樣條插值曲線的液壓機械手軌跡規(guī)劃方法，應(yīng)用C4連續(xù)性特性，推導(dǎo)了液壓機械手驅(qū)動節(jié)點軌跡規(guī)劃公式，令液壓機械手的運動學(xué)約束轉(zhuǎn)化為軌跡曲線的控制頂點約束，提高了液壓機械手軌跡規(guī)劃方法的通用性.

（3）提出了基于改進(jìn)粒子群優(yōu)化算法的液壓機械手軌跡時間最優(yōu)規(guī)劃問題，改進(jìn)了常規(guī)軌跡規(guī)劃算法難以保證全局收斂、收斂速度慢等問題，可以較快地找到最優(yōu)解，在解決復(fù)雜優(yōu)化問題方面計算效率更高.

（4）仿真和應(yīng)用實驗表明：所提出的液壓機械手軌跡規(guī)劃方法，可以較好地解決非關(guān)節(jié)驅(qū)動液壓機械手系統(tǒng)運動規(guī)劃問題，驗證了軌跡規(guī)劃算法的有效性.同時，本研究對于工業(yè)液壓機械手運動控制及軌跡優(yōu)化有參考意義.

（References）：

［1］潘楚濱.液壓與氣壓傳動［M］.北京：機械工業(yè)出版社，2010：108- 110.

［2］GASPARETTO A，LANZUTTI A，VIDONI R，et al.Experimental validation and comparative analysis of optimal time-jerk algorithms for trajectory planning［J］.Robotics and Computer-Integrated Manufacturing，2012，28（2）：164- 181.

［3］GASPARETTO A，ZANOTTO V.Optimal trajectory planning for industrial robots［J］.Advances in Engineering Software，2010，41（4）：548- 556.

［4］ABE A.Trajectory planning for residual vibration sup-pression of a two-link rigid-flexible manipulator considering large deformation［J］.Mechanism and Machine Theory，2009，44（9）：1627- 1639.

［5］朱世強，劉松國，王宣銀，等.機械手時間最優(yōu)脈動連續(xù)軌跡規(guī)劃算法［J］.機械工程學(xué)報，2010，46（3）：47- 52.

ZHU Shi-qiang，LIU Song-guo，WANG Xuan-yin，et al.Time-optimal and jerk-continuous trajectory planning algorithm for manipulators［J］.Journal of Mechanical Engineering，2010，46（3）：47- 52.

［6］周芳，朱齊丹，趙國良.基于改進(jìn)快速搜索隨機樹法的機械手路徑優(yōu)化［J］.機械工程學(xué)報，2011，47（11）：30- 35.

ZHOU Fang，ZHU Qi-dan，ZHAO Guo-liang.Path optimization of manipulator based on the improved rapidlyexploring random tree algorithm［J］.Journal of Mechanical Engineering，2011，47（11）：30- 35.

［7］GASPARETTO A，ZANOTTO V.A new method for smooth trajectory planning of robot manipulators［J］.Mechanism and Machine Theory，2007，42（4）：455- 471.

［8］TIAN L F，COLLINS C.An effective robot trajectory planning method using a genetic algorithm［J］.Mechatronics，2004，14（5）：455- 470.

［9］BAUML B，WIMBOCK T，HIRZINGER G.Kinematically optimal catching a flying ball with a hand-arm-system［C］∥IEEE／RSJ International Conference on Intelligent Robots and Systems（IROS）.Taipei：IEEE／RSJ，2010：2592- 2599.

［10］SARAVANAN R，RAMABALAB S，BALAMURUGAN C.Multi-objective trajectory planner for industrial robots with payload constraints［J］.Robotica，2008，26（6）：753- 765.

［11］徐海黎，解祥榮，莊健，等.工業(yè)機器人的最優(yōu)時間與最優(yōu)能量軌跡規(guī)劃［J］.機械工程學(xué)報，2010，46（9）：19- 25.

XU Hai-li，XIE Xiang-rong，ZHUANG Jian，et al.Global time-energy optimal planning of industrial robot trajectories［J］.Journal of Mechanical Engineering，2010，46（9）：19- 25.

［12］楊玉維，趙新華，孫啟湲，等.基于多體動力學(xué)特性的機械手時間最優(yōu)軌跡規(guī)劃［J］.機械工程學(xué)報，2014，50（7）：8- 14.

YANG Yu-wei，ZHAO Xin-hua，SUN Qi-yuan，et al.Trajectory optimization of manipulator for minimum working time based on multi-body dynamic characters［J］.Journal of Mechanical Engineering，2014，50（7）：8- 14.

［13］史也，梁斌，王學(xué)謙，等.基于量子粒子群優(yōu)化算法的空間機器人非完整笛卡爾路徑規(guī)劃［J］.機械工程學(xué)報，2011，47（23）：65- 73.

SHI Ye，LIANG Bin，WANG Xue-qian，et al.Cartesian non-holonomic path planning of space robot based on quantum-behaved particle swarm optimization algorithm［J］.Journal of Mechanical Engineering，2011，47（23）：65- 73.

［14］GUO J C，WANG X J.Trajectory planning of redundant robot manipulators using QPSO algorithm［C］∥Proceedings of the 8th World Congress on Intelligent Control and Automation.Jinan：［s.n.］，2010：403- 408.

［15］張書濤，張震，錢晉武.基于Tau理論的機器人抓取運動仿生軌跡規(guī)劃［J］.機械工程學(xué)報，2014，50（13）：42- 51.

ZHANG Shu-tao，ZHANG Zhen，QIAN Jin-wu.Bioinspired trajectory planning for robot catching movements based on the Tau theory［J］.Journal of Mechanical Engineering，2014，50（13）：42- 51.

［16］戈新生，孫鵬偉.自由漂浮空間機械臂非完整運動規(guī)劃的粒子群優(yōu)化算法［J］.機械工程學(xué)報，2007，43（4）：34- 38.

GE Xin-sheng，SUN Peng-wei.Particle swarm optimization algorithm of the non-holonomic motion planning for mechanical arm in free floating space［J］.Journal of Mechanical Engineering，2007，43（4）：34- 38.

［17］倪敬，邵斌，蒙臻，等.液壓拉床雙缸IPSO-PID伺服同步驅(qū)動控制研究［J］.中國機械工程，2013，24（11）：1494- 1500.

NI Jing，SHAO Bin，MENG Zhen，et al.IPSO-PID servo synchro control on hydraulic broaching machine with dual cylinder［J］.China Mechmical Engineering，2013，24（11）：1494- 1500.

［18］黃進(jìn)，胡英，馬孜，等.基于量子粒子群優(yōu)化算法的工業(yè)機器人與外部軸標(biāo)定［J］.機械工程學(xué)報，2009，45（7）：63- 67.

HUANG Jin，HU Ying，MA Zi，et al.Industrial robot and external axle calibration based on particle swarm optimization［J］.Journal of Mechanical Engineering，2009，45（7）：63- 67.

Trajectory planning algorithm for hydraulic servo manipulator of three freedom

LIU Xiang-qi1，2，MENG Zhen1，NI Jing2，ZHU Ze-fei2
（1.School of Mechanical Engineering and Automation，Zhejiang Sci-Tech University，Hangzhou 310018，China；2.School of Mechanical Engineering，Hangzhou Dianzi University，Hangzhou 310018，China）

A hydraulic manipulator trajectory planning algorithm based on non-axis driving space was proposed by introducing the closed chain vector kinematics model and the 5-order B-spline curve interpolation approximation theory.For the optimization problem of the total running time of the manipulator when met the kinematic constraints，the improved particle swarm optimization was introduced to optimize the solving step，so as to realize the time optimal continuous trajectory curve.The experimental results of practical application showed that the absolute error was less than 25 mm between the actual tool center point（TCP）trajectory and the desired trajectory.The research can solve the motion planning of the non-joint driving hydraulic manipulator and improve the efficiency of complex path planning algorithm.

closed-chain model；trajectory planning；B-splines；particle swarm optimization

10.3785／j.issn.1008-973X.2015.09.022

TP 241.2

A

1008- 973X（2015）09- 1776- 07

2014- 11- 03. 浙江大學(xué)學(xué)報（工學(xué)版）網(wǎng)址：www.journals.zju.edu.cn／eng

劉湘琪（1975－），女，博士生，從事機械設(shè)計及理論研究.ORCID：0000-0001-8782-566X.E-mail：lxiangqi＠hdu.edu.cn

朱澤飛，男，教授，博導(dǎo).ORCID：0000-0002-6876-8257.E-mail：zzf.3691＠163.com