王小龍，洪東炳，賈志軍,2，朱鵬坤

1安徽理工大學(xué)機(jī)械工程學(xué)院 安徽淮南 232001 2安徽省智能礦山裝備與技術(shù)工程實驗室 安徽淮南 232001

隨 著礦山提、運、采、掘四大裝備自動化、智 能化水平日益提升，輔助安裝設(shè)備的機(jī)械化、自動化和智能化水平的提升就顯得尤為重要。井下架管機(jī)是一種重要的輔助安裝設(shè)備，通過核心部件機(jī)械臂來實現(xiàn)運管、架管、接管和固管等功能。為了提高架管效率，國內(nèi)外學(xué)者對機(jī)械臂進(jìn)行了大量研究，王君等人[1]通過 MATLAB Robotics Toolbox 對 UR5 機(jī)器人建立模型，再對其進(jìn)行軌跡規(guī)劃與仿真的研究，驗證了機(jī)器人運動模型和 3 次多項式插值算法的正確性和合理性；李威等人[2]對三節(jié)臂挖掘裝置建立運動學(xué)模型，采用遺傳算法對優(yōu)化模型求解，并對實例進(jìn)行分析，驗證了此優(yōu)化方法有較高的應(yīng)用價值；于瑞等人[3]基于雜交算法對機(jī)器人的運動時間實現(xiàn)了最優(yōu)軌跡規(guī)劃。以上述相關(guān)研究為基礎(chǔ)，結(jié)合煤礦巷道的實際工作要求，筆者采用 3-5-3 多項式插值算法對三節(jié)臂進(jìn)行軌跡規(guī)劃，結(jié)合粒子群算法，實現(xiàn)了速度約束下三節(jié)臂軌跡規(guī)劃時間的優(yōu)化。

1 綜掘架管機(jī)的結(jié)構(gòu)設(shè)計

目前大部分礦區(qū)仍然采用人工架管的方式，尤其在兩淮地區(qū)，架管效率較低。煤礦巷道斷面通常選擇直墻半圓拱形，凈斷面寬度為 4.2 m，高度為 3.4 m，管道安裝高度在 1.7～2.6 m，在沒有適合的輔助安裝設(shè)備下，工人勞動強(qiáng)度大，急需改善。

1.1 架管機(jī)的工作原理

針對上述工況，筆者設(shè)計了一種煤礦輔助設(shè)備架管機(jī)，其結(jié)構(gòu)如圖 1 所示。架管機(jī)主要由牽引機(jī)構(gòu)、行走機(jī)構(gòu)、架管機(jī)構(gòu)、鉆孔機(jī)構(gòu)、載人作業(yè)機(jī)構(gòu)和運管機(jī)構(gòu)等部分組成。架管機(jī)通過牽引機(jī)構(gòu)到達(dá)指定工作位置，由鉆孔機(jī)構(gòu)對工作區(qū)域進(jìn)行打孔作業(yè)，之后架管機(jī)構(gòu)夾取管道，將其提升到一定高度，再協(xié)同載人作業(yè)機(jī)構(gòu)將管道固定。其中，核心部件架管機(jī)構(gòu)三節(jié)臂的運動軌跡決定了架管工作的效率，因此筆者對架管機(jī)構(gòu)三節(jié)臂進(jìn)行了軌跡優(yōu)化分析。

圖1 架管機(jī)結(jié)構(gòu)Fig.1 Structure of pipe erector

1.2 架管機(jī)三節(jié)臂結(jié)構(gòu)

架管機(jī)三節(jié)臂結(jié)構(gòu)如圖 2 所示，主要由回轉(zhuǎn)裝置、第一節(jié)臂、第二節(jié)臂、第三節(jié)臂、夾管器和相應(yīng)的液壓缸組成。與兩節(jié)臂結(jié)構(gòu)相比，三節(jié)臂增加了架管的高度，通過輔助液壓缸可以調(diào)節(jié)夾管器的角度。

圖2 架管機(jī)三節(jié)臂結(jié)構(gòu)Fig.2 Structure of three-section arm of pipe erector

2 架管機(jī)三節(jié)臂的軌跡規(guī)劃

為了清楚描述三節(jié)臂工作時的路徑狀態(tài)，判斷末端位置是否滿足巷道架管要求，筆者采用粒子群優(yōu)化算法，以 3-5-3 多項式插值算法[4]對其運動過程中的關(guān)節(jié)位置、速度、加速度進(jìn)行規(guī)劃，優(yōu)化三節(jié)臂的工作時間。

2.1 多項式插值函數(shù)構(gòu)造

3-5-3 多項式規(guī)劃是指在關(guān)節(jié)規(guī)劃中，關(guān)節(jié)在時間t1采用 3 次多項式進(jìn)行規(guī)劃，在時間t2采用 5 次多項式進(jìn)行規(guī)劃，在時間t3采用 3 次多項式進(jìn)行規(guī)劃。在工作空間內(nèi)，對三節(jié)臂進(jìn)行軌跡規(guī)劃，設(shè)置其工作路徑，利用 3-5-3 次多項式對三節(jié)臂進(jìn)行插值，設(shè)置 4 個插值點，用θij表示關(guān)節(jié)i插值的角度，其中i＝1，2，…，n，n表示關(guān)節(jié)數(shù)，j＝1，2，3，4，表示插值的 4 個序號。

第i個關(guān)節(jié) 3-5-3 樣條多項式的通式為

式中：li1(t)、li2(t)、li3(t) 分別代表第i段關(guān)節(jié)在 3 次多項式的運動軌跡、5 次多項式的運動軌跡、3 次多項式的運動軌跡；其中，未知系數(shù)a可根據(jù)約束條件求出，并根據(jù)約束條件和約束邊界求出矩陣A[5]，用θ=[θi0，θi1，θi2，θi3] 代表第i個關(guān)節(jié)的 4 個插值點 (弧度值)。已知條件為第i個關(guān)節(jié)的初始點θi0、路徑點θi1和θi2、末端點θi3以及起止速度 0，則

根據(jù)已知條件，推導(dǎo)出求解未知數(shù)a與插值點的關(guān)系式為

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

粒子群算法[6]是對軌跡時間優(yōu)化的一種方法，是在鳥群捕食過程中受到的啟發(fā)，將每個個體看作 D 維空間內(nèi)一個微粒，在搜索空間內(nèi)以一定速度運動，通過個體之間相互協(xié)作和競爭來尋找最優(yōu)解。在粒子群算法中，將每個需要優(yōu)化的變數(shù)視為一個粒子，每個粒子都有一個適應(yīng)值，由被優(yōu)選函數(shù)所確定，并利用這種適應(yīng)值的大小來確定粒子的最優(yōu)值。在 D 維搜索空間內(nèi)，每個粒子都有一個位置xi和一個速度vi，粒子經(jīng)過的地方都會出現(xiàn) 2 個最優(yōu)位置，即粒子所經(jīng)歷最優(yōu)適應(yīng)值的位置pi和所有微粒群體目前所經(jīng)歷的最優(yōu)位置pg。當(dāng) 2 個位置都不存在最優(yōu)解時，第i個粒子就會通過式 (4)、(5) 繼續(xù)k+1 次迭代來更新自己的位置和速度，直到找到最優(yōu)的適應(yīng)值。

式中：vid∈[-Vmax，Vmax]；w為慣性權(quán)重；c1、c2為學(xué)習(xí)因子；r1、r2為 [0，1] 之間隨機(jī)任意值；xid∈ [-xmax，xmax]。

當(dāng)xid和vid符合約束條件時，就用邊界值代替，其約束條件為式中：t1、t2、t3為適應(yīng)值函數(shù)。

為了方便計算，將t作為自變量，搜索維度降到三維，求解位置矩陣。

粒子群算法對三節(jié)臂第i關(guān)節(jié)進(jìn)行時間最優(yōu)規(guī)劃的步驟為：

(1) 選取初始化種群規(guī)模，確定迭代次數(shù)，在三維空間內(nèi)隨機(jī)初始化粒子的位置xi和速度vi；

(2) 給出慣性權(quán)重w和學(xué)習(xí)因子c1、c2；

(3) 計算粒子的適應(yīng)值及粒子群體適應(yīng)值；

(4) 調(diào)用適應(yīng)度函數(shù) fitness Trac，計算系數(shù)矩陣A，求出關(guān)節(jié)極值速度Vmax，若滿足約束條件，則說明t1、t2、t3有效，否則說明當(dāng)前值無效，flag 返回 false；

(5) 根據(jù)式 (4)、(5) 優(yōu)化粒子的速度vi和位置xi；

(6) 判斷粒子的速度vi和位置xi是否滿足邊界條件，若超出邊界條件，則用邊界值代替；

(7) 獲取粒子個體最優(yōu)pi和粒子群體pg，判斷是否滿足結(jié)束條件，若不滿足，則回到步驟 2，反之則終止迭代。

3 仿真與分析

3.1 三節(jié)臂的運動學(xué)模型

筆者采用改進(jìn) D-H 法坐標(biāo)系[7]建立架管機(jī)三節(jié)臂的運動學(xué)模型，忽略三節(jié)臂的外形和尺寸，將其結(jié)構(gòu)進(jìn)行簡化，得到機(jī)械臂各個關(guān)節(jié)的坐標(biāo)系，如圖 3 所示。確定架管機(jī)三節(jié)臂的參數(shù)，得到 D-H 參數(shù)，如表 1 所列。

圖3 三節(jié)臂連桿坐標(biāo)系簡圖Fig.3 Sketch of coordinate system of three-section arm linkage

表1 架管機(jī)三節(jié)臂的 D-H 參數(shù)Tab.1 D-H parameters of three-section arm of pipe erector

以基座坐標(biāo)系為基準(zhǔn)，依次完成各個關(guān)節(jié)坐標(biāo)系的轉(zhuǎn)換，坐標(biāo)系 {i=1} 到 {i} 的變換矩陣ii-1T由 4 個基礎(chǔ)變換矩陣相乘得到，

其中

式中：ai-1表示從zi-1到zi沿xi-1測量的距離；αi-1表示從zi-1到zi沿xi-1旋轉(zhuǎn)的角度；θi表示從xi-1到xi沿zi旋轉(zhuǎn)的角度；di表示從xi-1到xi沿zi測量的距離。

由表 1 可知，每個相鄰連桿的位置變換矩陣相乘，即可得到三節(jié)臂的末端矩陣

3.2 仿真結(jié)果分析

給出三節(jié)臂的 4 個位置，即起始點θi0、抓取點θi1、路徑θi2和終止點θi3，利用 3-5-3 多項式插值進(jìn)行仿真，關(guān)節(jié)空間角度插值點如表 2 所列。在架管機(jī)三節(jié)臂工作時，為了架管過程中管道平穩(wěn)性和安全性，關(guān)節(jié) 1 回轉(zhuǎn)盤需要處于靜止?fàn)顟B(tài)，因此，關(guān)節(jié) 1 的 4 個位置始終為 0。

表2 關(guān)節(jié)空間角度插值點Tab.2 Interpolation points of joint space angle

根據(jù)粒子群算法最優(yōu)規(guī)劃步驟，利用基于粒子群的 3-5-3 多項式插值算法，確定每段插值的最優(yōu)時間。確定插值的粒子種群數(shù)為 50，迭代次數(shù)為 100，粒子的慣性權(quán)重w＝0.8，學(xué)習(xí)因子c1＝c2＝2，允許的最大速度Vmax＝3 rad/s，起止點的速度和加速度都為 0，得到關(guān)節(jié)位置進(jìn)化圖，如圖 4 所示。由圖 4 可以看出，在速度約束下，4 個關(guān)節(jié)最優(yōu)粒子pg位置進(jìn)化曲線中，關(guān)節(jié) 2 經(jīng)過 56 次迭代趨于收斂；關(guān)節(jié) 5 經(jīng)過 18 次迭代后快速收斂；其余收斂均在 20～40 次之間。所以關(guān)節(jié) 5 相對平穩(wěn)，說明在架管過程中末端夾管器處于穩(wěn)定運動狀態(tài)。

圖4 關(guān)節(jié)位置進(jìn)化圖Fig.4 Evolution of joint position

3-5-3 多項式插值仿真軌跡曲線如圖 5 所示。在給定各段時間具體插值準(zhǔn)則的情況下。初始和終點的速度和加速度都為 0，滿足運動學(xué)約束要求，其中t1=1，t2=1，t3=1；三節(jié)臂關(guān)節(jié)位置在每個時間段運動時間跨度大，速度未接近最大值，加速度在 2 s 左右出現(xiàn)跳變。

圖5 運動仿真軌跡曲線Fig.5 Motion simulation trajectory curve

經(jīng)過粒子群算法的 100 次迭代，得到每個關(guān)節(jié)的運動軌跡，與之前相比，時間整體縮短。優(yōu)化后末端到達(dá)指定位置時間為 0.89 s，比優(yōu)化前縮短了 2.11 s，速度在 0.21 s 達(dá)到峰值 2.97 rad/s，更接近設(shè)置的最大速度 3 rad/s。相比之前加速度的跳變，優(yōu)化后的加速度曲線連續(xù)，跳變幅度更小。

基于粒子群優(yōu)化算法，以 3-5-3 多項式插值算法對三節(jié)臂進(jìn)行規(guī)劃，其末端位置的軌跡如圖 6 所示。由圖 6 可以看出，曲線經(jīng)過設(shè)定的 4 個位置，驗證了該方法的有效性。架管機(jī)三節(jié)臂的工作空間高度可達(dá) 2.2 m，滿足巷道架管的要求。

圖6 規(guī)劃后的軌跡Fig.6 Planned trajectory

4 結(jié)語

通過設(shè)計三節(jié)機(jī)械臂架管機(jī)，并對其進(jìn)行運動學(xué)分析，運用粒子群算法對三節(jié)臂進(jìn)行運動時間優(yōu)化，得到了如下結(jié)論：通過軌跡規(guī)劃得到三節(jié)臂末端高度為 2.2 m，滿足巷道架管高度 1.7～2.6 m 的要求；通過 MATLAB 仿真得到三節(jié)臂的速度、加速度曲線，均滿足插值時間點的平滑過渡，能夠滿足三節(jié)臂各關(guān)節(jié)在速度約束下的運動平穩(wěn)性，具有較好的實用性。