申樹(shù)策，史艷楠,3,4，宋建鋒，任澤，王毅穎,3，王翰秋,3

（1.河北工程大學(xué) 機(jī)械與裝備工程學(xué)院,河北 邯鄲 056038；2.冀凱河北機(jī)電科技有限公司,河北 石家莊 050000；3.河北省煤炭生態(tài)保護(hù)開(kāi)采產(chǎn)業(yè)技術(shù)研究院,河北 邯鄲 056038；4.邯鄲市智能車(chē)輛重點(diǎn)實(shí)驗(yàn)室,河北 邯鄲 056038）

0 引言

隨著我國(guó)煤炭開(kāi)采強(qiáng)度加大、開(kāi)采深度加深、地質(zhì)條件越來(lái)越復(fù)雜，沖擊地壓、水害、瓦斯突出、火災(zāi)等災(zāi)害發(fā)生的概率也越來(lái)越高[1]。為了保證煤礦綠色安全開(kāi)采“智能化、無(wú)人化”，國(guó)家發(fā)布了《煤礦機(jī)器人重點(diǎn)研發(fā)目錄》，防突鉆孔機(jī)器人與防沖鉆孔機(jī)器人是其中重點(diǎn)研發(fā)內(nèi)容[2]。防突防沖鉆車(chē)是防突鉆孔機(jī)器人與防沖鉆孔機(jī)器人的結(jié)合體，可對(duì)煤體應(yīng)力進(jìn)行有效卸壓，且不會(huì)破壞原有的巷道支護(hù)，有效降低了煤與瓦斯突出、沖擊地壓等災(zāi)害發(fā)生的概率。鉆車(chē)機(jī)械手作為防突防沖鉆車(chē)不可或缺的裝置，關(guān)系著鉆車(chē)是否可以正常鉆進(jìn)及真正實(shí)現(xiàn)無(wú)人化。提高鉆車(chē)機(jī)械手的工作效率，對(duì)于提高煤礦安全生產(chǎn)效率具有重要意義。

在機(jī)械手的研究中，使機(jī)械手在避免奇異點(diǎn)的同時(shí)能夠快速、準(zhǔn)確、平穩(wěn)地到達(dá)工作點(diǎn)對(duì)于其在工業(yè)應(yīng)用中至關(guān)重要。為了使機(jī)械手快速、準(zhǔn)確、平穩(wěn)運(yùn)行，軌跡規(guī)劃優(yōu)化尤為重要。目前，在機(jī)械手軌跡規(guī)劃優(yōu)化算法中，研究較多的是如何使機(jī)械手在滿足約束條件下的時(shí)間最優(yōu)，其目的是在滿足運(yùn)動(dòng)學(xué)約束條件下使機(jī)械手各個(gè)關(guān)節(jié)的速度最大化，從而使機(jī)械臂運(yùn)行時(shí)間最短。時(shí)間優(yōu)化算法大體上可分為2 類(lèi)：①點(diǎn)到點(diǎn)的最短時(shí)間優(yōu)化算法。② 給定一條已知的連續(xù)路徑，使機(jī)械手在此路徑運(yùn)行時(shí)間最短。后者的研究方法是將路徑分段處理，再利用點(diǎn)到點(diǎn)的優(yōu)化算法對(duì)其優(yōu)化[3]。在軌跡規(guī)劃中，軌跡大多由多項(xiàng)式、樣條、貝塞爾曲線等插值函數(shù)構(gòu)造[4]。文獻(xiàn)[5]采用基于正弦變化學(xué)習(xí)因子的粒子群（Particle Swarm Optimization,PSO）算法對(duì)4-3-4多項(xiàng)式插值進(jìn)行軌跡規(guī)劃，改善了傳統(tǒng)PSO 算法收斂速度緩慢的問(wèn)題。文獻(xiàn)[6]采用5-7-5 多項(xiàng)式插值與復(fù)合形法相結(jié)合的方式對(duì)川崎FS20N 機(jī)器人進(jìn)行軌跡規(guī)劃，提高了工作效率，降低了能耗，但是階次較高，計(jì)算復(fù)雜。文獻(xiàn)[7]對(duì)六自由度機(jī)器人采用非均勻B 樣條插值對(duì)其時(shí)間進(jìn)行優(yōu)化，得到了較好的效果，但其樣條插值為五次和七次，階次較高，計(jì)算復(fù)雜。文獻(xiàn)[8]利用差分進(jìn)化算法對(duì)機(jī)械臂進(jìn)行了時(shí)間最優(yōu)軌跡規(guī)劃，但是差分進(jìn)化算法容易早熟或使算法停滯。文獻(xiàn)[9]采用五次插值多項(xiàng)式對(duì)分揀機(jī)器人進(jìn)行軌跡規(guī)劃，提高了軌跡平滑程度，但是相比于分段處理的方式，該方法耗時(shí)較多。以上研究雖然取得了一定的成果，但大多存在階次較高，易發(fā)生“龍格”現(xiàn)象且不易計(jì)算和優(yōu)化算法易早熟的問(wèn)題。

針對(duì)上述問(wèn)題，本文以鉆車(chē)機(jī)械手為研究對(duì)象，提出了一種基于改進(jìn)PSO 算法的鉆車(chē)機(jī)械手時(shí)間最優(yōu)軌跡規(guī)劃方法。首先，利用標(biāo)準(zhǔn)Denavit-Hartenberg（D-H）構(gòu)建鉆車(chē)機(jī)械手三維模型，通過(guò)蒙特卡洛法得到鉆車(chē)機(jī)械手的工作空間，從工作空間中選取4 個(gè)途徑點(diǎn)作為插值點(diǎn)。然后，為了使鉆車(chē)機(jī)械手能夠快速平穩(wěn)地到達(dá)指定位置，在關(guān)節(jié)空間中采用3-5-3 分段多項(xiàng)式插值構(gòu)造其軌跡。最后，通過(guò)改進(jìn)PSO 算法對(duì)構(gòu)造的軌跡進(jìn)行時(shí)間最短優(yōu)化，得到鉆車(chē)機(jī)械手的時(shí)間最優(yōu)軌跡規(guī)劃。Matlab 仿真結(jié)果證明了該方法的可靠性。

1 鉆車(chē)機(jī)械手運(yùn)動(dòng)學(xué)模型

1.1 鉆車(chē)機(jī)械手結(jié)構(gòu)

本文研究的鉆車(chē)機(jī)械手為2 自由度機(jī)械手，由1 個(gè)轉(zhuǎn)動(dòng)副和1 個(gè)移動(dòng)副組成，其整體結(jié)構(gòu)如圖1 所示。主要工作任務(wù)為從回轉(zhuǎn)托板抓取鉆桿裝入主機(jī)夾持器內(nèi)，具體上桿步驟如下：①判斷機(jī)械手與托板狀態(tài)，保證順利抓桿；通過(guò)判斷仰角檢測(cè)接近開(kāi)關(guān)和俯角檢測(cè)接近開(kāi)關(guān)狀態(tài)，調(diào)整機(jī)械手和托板位置。② 機(jī)械手?jǐn)[動(dòng)油缸逆時(shí)針旋轉(zhuǎn)至憋壓狀態(tài)，機(jī)械手到達(dá)抓桿角度位置。③機(jī)械手伸縮油缸伸出至憋壓狀態(tài)，機(jī)械手到達(dá)抓桿位。④ 機(jī)械手爪夾緊，抓住鉆桿。⑤ 機(jī)械手?jǐn)[動(dòng)油缸順時(shí)針旋轉(zhuǎn)至憋壓狀態(tài)，機(jī)械手到達(dá)上桿位置。⑥ 夾持器（副夾持器）夾緊新鉆桿，夾緊前端已鉆入鉆桿，回轉(zhuǎn)器正轉(zhuǎn)進(jìn)給，連接新加鉆桿。

圖1 鉆車(chē)機(jī)械手整體結(jié)構(gòu)Fig.1 Overall structure of drill rig manipulator

1.2 鉆車(chē)機(jī)械手運(yùn)動(dòng)學(xué)模型

1.2.1 D-H 參數(shù)確定

根據(jù)標(biāo)準(zhǔn)D-H 參數(shù)法以X0Y0Z0為基坐標(biāo)建立連桿坐標(biāo)系[10]，如圖2 所示。其中，XfYfZf（f=1,2）為第f個(gè)連桿的坐標(biāo)系，d1為基底與連桿1 的偏移距，d2為連桿1 和連桿2 的偏移距，且d1=267 mm，d2=702 mm。

圖2 標(biāo)準(zhǔn)D-H 坐標(biāo)系Fig.2 Standard D-H coordinate system

結(jié)合圖2 的坐標(biāo)系及其參數(shù)可求得機(jī)械手的D-H 參數(shù)，見(jiàn)表1。表1 中，θf(wàn)為關(guān)節(jié)角，df為偏移距，af為桿件長(zhǎng)度，αf扭轉(zhuǎn)角。

1.2.2 變換矩陣計(jì)算

根據(jù)圖2 坐標(biāo)系和表1 中的各個(gè)參數(shù)，可求得連桿之間的齊次變換矩陣。其中，相鄰2 個(gè)連桿之間的齊次變換矩陣為[11]

表1 機(jī)械手D-H 參數(shù)Table 1 D-H parameters of manipulator

將表1 中的參數(shù)值代入式（1）可計(jì)算出2 個(gè)變換矩陣分別為

通過(guò)式（2）、式（3）可得到鉆車(chē)機(jī)械手相對(duì)于直角坐標(biāo)系x，y，z軸的末端位姿矩陣[12]：

式中：n，o和q為位姿矩陣的姿態(tài)分量；p為位姿矩陣的位置分量。

2 鉆車(chē)機(jī)械手關(guān)節(jié)空間軌跡規(guī)劃

軌跡規(guī)劃通常在笛卡爾空間或關(guān)節(jié)空間中進(jìn)行，且兩者可互相轉(zhuǎn)換[13]。在笛卡爾空間中，可自然地描述末端執(zhí)行器的軌跡，用逆運(yùn)動(dòng)學(xué)求解每個(gè)關(guān)節(jié)的插補(bǔ)點(diǎn)，計(jì)算相應(yīng)的關(guān)節(jié)角度，計(jì)算量非常大，且容易導(dǎo)致運(yùn)動(dòng)奇異點(diǎn)和機(jī)械手冗余。而在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃，可實(shí)時(shí)調(diào)整各個(gè)關(guān)節(jié)的角度插值點(diǎn)，從而使機(jī)械手達(dá)到期望的狀態(tài)。結(jié)合實(shí)際情況，本文在關(guān)節(jié)空間中對(duì)鉆車(chē)機(jī)械手進(jìn)行軌跡規(guī)劃。

2.1 分段多項(xiàng)式軌跡規(guī)劃

對(duì)于多項(xiàng)式樣條插值函數(shù)，可以保證鉆車(chē)機(jī)械手關(guān)節(jié)準(zhǔn)確地經(jīng)過(guò)各個(gè)插值點(diǎn)。速度的變化規(guī)律可通過(guò)對(duì)時(shí)間求1 階導(dǎo)來(lái)觀察。同理，對(duì)時(shí)間求2 階導(dǎo)可獲得加速度的變化規(guī)律。為了保證鉆車(chē)機(jī)械手的速度和加速度的連續(xù)，需要最少保證函數(shù)C3連續(xù)，同時(shí)為了保證計(jì)算量不太復(fù)雜，根據(jù)實(shí)際工況需要，將鉆車(chē)機(jī)械手的運(yùn)行軌跡分為3 段，即3-5-3 多項(xiàng)式插值。其中，3-5-3 樣條多項(xiàng)式的通式為

式中：hj1(t)，hj2(t)，hj3(t) 分別為3 段多項(xiàng)式的軌跡，j為當(dāng)前關(guān)節(jié)，t為總時(shí)間；mj為第j個(gè)關(guān)節(jié)軌跡的系數(shù)；t1，t2，t3分別為每個(gè)關(guān)節(jié)初始段、中間段和終止段的運(yùn)動(dòng)時(shí)間。

已知各個(gè)關(guān)節(jié)在通過(guò)抓桿點(diǎn)β0、路徑點(diǎn)β1和β2、放桿點(diǎn)β3時(shí)，各個(gè)端點(diǎn)連接處的速度和加速度都平穩(wěn)連續(xù)，且在抓桿點(diǎn)和放桿點(diǎn)時(shí)速度和加速度都為0[14]，得多項(xiàng)式系數(shù)與插值點(diǎn)關(guān)系為

式中：m為關(guān)節(jié)系數(shù)矩陣；A為經(jīng)過(guò)4 個(gè)插值點(diǎn)時(shí)3-5-3 插值多項(xiàng)式的系數(shù)矩陣；b為對(duì)應(yīng)的插值點(diǎn)矩陣。

2.2 基于改進(jìn)PSO 算法的軌跡優(yōu)化

以時(shí)間最優(yōu)為優(yōu)化目標(biāo)，利用改進(jìn)的PSO 算法對(duì)鉆車(chē)機(jī)械手進(jìn)行優(yōu)化。將每個(gè)關(guān)節(jié)的3 段時(shí)間t1，t2，t3組成一個(gè)三維的粒子群[15]，其中粒子群適應(yīng)度函數(shù)為

初始化之后，計(jì)算每個(gè)粒子初始化后的適應(yīng)度，并將其賦值給pbest（個(gè)體最優(yōu)值）。利用改進(jìn)PSO 算法以式（11）為適應(yīng)度函數(shù)進(jìn)行迭代更新，迭代完成后，計(jì)算當(dāng)前全部粒子的適應(yīng)度，其中值最小的粒子作為gbest（全局最優(yōu)值）。PSO 算法速度和位置更新公式為

式中：vidk+1為第i個(gè)粒子在d維時(shí)，迭代k+1 次的速度；ω為慣性權(quán)重；c1，c2為學(xué)習(xí)因子，決定粒子獲取外界信息的能力；r1，r2為[0,1]的隨機(jī)數(shù)；xid為第i個(gè)粒子在d維時(shí)的位置。

固定不變的ω在PSO 算法整體求解的過(guò)程中可能會(huì)存在易陷入局部最優(yōu)解的缺陷[16]，為此本文采用一種隨迭代次數(shù)增加ω線性下降的方法來(lái)提高搜尋解的能力，其計(jì)算公式為

式中：ωmax為最大慣性權(quán)重；ωmin為最小慣性權(quán)重；k為當(dāng)前迭代次數(shù)；kmax為最大迭代次數(shù)。

式（12）和式（13）在本文中指時(shí)間變化量和時(shí)間的更新公式。以時(shí)間t為尋優(yōu)量在粒子群里尋優(yōu)，具體優(yōu)化步驟如下：

（1）參數(shù)初始化。首先明確有多少個(gè)粒子，在三維搜索空間中隨機(jī)產(chǎn)生M個(gè)粒子（共有3M個(gè)）構(gòu)成種群，使用rand 函數(shù)隨機(jī)給定時(shí)間變化量及時(shí)間。

（2）初始化完成后，將時(shí)間變量tj1，tj2，tj3代入式（8）-式（10），可求解出系數(shù)矩陣m中的元素。

（3）式（5）-式（7）中的三段多項(xiàng)式軌跡可根據(jù)步驟（2）所得到的系數(shù)矩陣m得到。判斷3 段軌跡的速度函數(shù)是否全部小于各個(gè)關(guān)節(jié)允許的最大速度。如果求得的3 段軌跡的速度函數(shù)都在速度約束范圍內(nèi)，則將式（11）作為其適應(yīng)度函數(shù)，計(jì)算適應(yīng)度，以時(shí)間最短為目標(biāo)進(jìn)行迭代；否則，將賦值一個(gè)極大的常數(shù)充當(dāng)其適應(yīng)度，在接下來(lái)迭代尋優(yōu)時(shí)，會(huì)通過(guò)比較適應(yīng)度的大小來(lái)淘汰該粒子。

（4）對(duì)隨機(jī)產(chǎn)生的M個(gè)粒子，將其自身經(jīng)過(guò)迭代k次后，所產(chǎn)生的n個(gè)適應(yīng)度值作比較，適應(yīng)度值最小的位置作為pbest，再通過(guò)比較每個(gè)粒子的pbest，選取其中最小的適應(yīng)度值作為gbest。

（5）根據(jù)式（12）和式（13）來(lái)更新種群的速度和位置，并將其重新構(gòu)成具有新的位置和速度的M個(gè)粒子的新種群。

（6）如果達(dá)到最大迭代次數(shù)或找到局部最優(yōu)解，則算法結(jié)束，否則返回步驟（2）。

3 仿真實(shí)驗(yàn)與結(jié)果分析

3.1 鉆車(chē)機(jī)械手的Matlab 建模

根據(jù)式（4）可得到坐標(biāo)系{2}相對(duì)于坐標(biāo)系{0}的變換矩陣，由此可計(jì)算出機(jī)械手末端執(zhí)行器的位姿。利用Matlab 仿真軟件內(nèi)的Robotics Toolbox工具箱對(duì)鉆車(chē)機(jī)械手進(jìn)行建模,并用蒙特卡洛法繪制出鉆車(chē)機(jī)械手的工作空間，分別如圖3、圖4 所示。

圖3 鉆車(chē)機(jī)械手三維模型Fig.3 3D model of drill rig manipulator

圖4 鉆車(chē)機(jī)械手工作空間Fig.4 Workspace of drill rig manipulator

3.2 鉆車(chē)機(jī)械手的Matlab 仿真

根據(jù)鉆車(chē)機(jī)械手的工作空間及抓桿、放桿的工作特點(diǎn)，給定其在笛卡爾空間中的抓桿點(diǎn)、2 個(gè)路徑點(diǎn)及放桿點(diǎn)的位置，見(jiàn)表2。

根據(jù)逆運(yùn)動(dòng)學(xué)，可以將表2 給定的各點(diǎn)轉(zhuǎn)換到關(guān)節(jié)空間中，從而獲得各個(gè)關(guān)節(jié)在各點(diǎn)的角度插值點(diǎn)，見(jiàn)表3。其中，關(guān)節(jié)2 為伸縮關(guān)節(jié)，只改變伸縮量，所以它的角度插值不變，為0。

表2 笛卡爾空間路徑Table 2 Cartesian space path mm

表3 關(guān)節(jié)空間的角度插值點(diǎn)Table 3 Angle interpolation points in joint space

3-5-3 分段插值多項(xiàng)式中的3 段插值時(shí)間都是根據(jù)實(shí)際情況提前給定的。在滿足抓桿點(diǎn)和放桿點(diǎn)的速度和加速度都是0 的前提下，給定總時(shí)間t=3.168 5 s，t1=0.866 5 s，t2=0.667 s，t3=1.635 s。在給定每段關(guān)節(jié)時(shí)間的插值效果如圖5-圖7 所示。

圖5 3-5-3 樣條插值機(jī)械手關(guān)節(jié)位置曲線Fig.5 Joint position curves of manipulator based on 3-5-3 spline interpolation

圖6 3-5-3 樣條插值機(jī)械手關(guān)節(jié)速度曲線Fig.6 Joint velocity curves of manipulator based on 3-5-3 spline interpolation

圖7 3-5-3 樣條插值機(jī)械手關(guān)節(jié)加速度曲線Fig.7 Joint acceleration curves of manipulator based on 3-5-3 spline interpolation

從圖5-圖7 可看出，機(jī)械手各關(guān)節(jié)位置、速度和加速度都是連續(xù)的，表明了該分段插值多項(xiàng)式可以很好地用于構(gòu)造機(jī)械手軌跡，但是圖6 中關(guān)節(jié)的速度并未達(dá)到峰值。

根據(jù)改進(jìn)的PSO 算法對(duì)防突防沖鉆車(chē)機(jī)械手進(jìn)行優(yōu)化，設(shè)定初始粒子數(shù)M=50，初始位置為0.1～4 s之間的任意隨機(jī)位置，粒子迭代速度在[-3，3]之間，迭代次數(shù)為50，慣性權(quán)重根據(jù)式（14）變化，速度和加速度在起始和終止時(shí)為0。優(yōu)化后，適應(yīng)度值和迭代次數(shù)之間的變換規(guī)律如圖8 所示?？煽闯隽Ｗ釉诘?3 次以后幾乎趨于穩(wěn)定，證明改進(jìn)PSO 算法可快速收斂。

圖8 改進(jìn)PSO 算法的適應(yīng)度曲線Fig.8 Fitness curve of improved PSO algorithm

采用改進(jìn)的PSO 算法對(duì)鉆車(chē)機(jī)械手進(jìn)行時(shí)間最優(yōu)軌跡規(guī)劃，以機(jī)械手關(guān)節(jié)1 為例，得到其3 段插值時(shí)間的迭代過(guò)程如圖9 所示。

圖9 關(guān)節(jié)1 最優(yōu)粒子迭代過(guò)程Fig.9 Optimal particle iteration process of joint 1

從圖9 可看出，關(guān)節(jié)1 的3 段插值時(shí)間最后平穩(wěn)趨于定值，即可得到關(guān)節(jié)1 優(yōu)化后的時(shí)間。

由于關(guān)節(jié)2 為伸縮油缸，在進(jìn)行抓桿、放桿的軌跡規(guī)劃時(shí)不改變關(guān)節(jié)位置，所以關(guān)節(jié)2 各段運(yùn)行時(shí)間為0，各關(guān)節(jié)優(yōu)化結(jié)果見(jiàn)表4。

表4 各關(guān)節(jié)優(yōu)化結(jié)果Table 4 Optimization results of each joint s

為了保證關(guān)節(jié)1 和關(guān)節(jié)2 在同一時(shí)間到達(dá)放桿點(diǎn)的位置，同時(shí)確保每個(gè)關(guān)節(jié)運(yùn)行平穩(wěn)，應(yīng)選取優(yōu)化后各關(guān)節(jié)時(shí)間的最大值，由于關(guān)節(jié)2 各段插值時(shí)間為0，所以只采用關(guān)節(jié)1 優(yōu)化后的插值時(shí)間，即0.648 7,0.391 9,1.344 8 s，總時(shí)間為2.385 4 s，與優(yōu)化前相比縮短了0.783 1 s，縮短約25%，提高了機(jī)械手的工作效率。

優(yōu)化后機(jī)械手各關(guān)節(jié)的位置、速度和加速度曲線如圖10-圖12 所示。

圖10 優(yōu)化后的機(jī)械手關(guān)節(jié)位置曲線Fig.10 Joint position curves of optimized manipulator

圖11 優(yōu)化后的機(jī)械手關(guān)節(jié)速度曲線Fig.11 Joint velocity curves of optimized manipulator

圖12 優(yōu)化后的機(jī)械手關(guān)節(jié)加速度曲線Fig.12 Joint acceleration curves of optimized manipulator

從圖10 可看出，優(yōu)化后的關(guān)節(jié)位置相較之前更加平滑。從圖11 可看出機(jī)械手的關(guān)節(jié)最大速度較圖6 有很大提高。從圖12 可看出，優(yōu)化后的加速度雖然較之前有起伏，但其位置、速度和加速度都連續(xù)且在運(yùn)動(dòng)學(xué)約束范圍內(nèi)。從圖11 和圖12 還可看出，起始點(diǎn)和終點(diǎn)數(shù)值都為0，符合預(yù)先假設(shè)。仿真結(jié)果顯示，通過(guò)改進(jìn)的PSO 算法對(duì)鉆車(chē)機(jī)械手進(jìn)行軌跡規(guī)劃，可以減少作業(yè)時(shí)間，提高其工作效率。

4 結(jié)論

（1）基于改進(jìn)PSO 算法的鉆車(chē)機(jī)械手時(shí)間最優(yōu)軌跡規(guī)劃方法通過(guò)構(gòu)建鉆車(chē)機(jī)械手運(yùn)動(dòng)學(xué)模型得到其末端位姿矩陣，并利用蒙特卡洛法得到了鉆車(chē)機(jī)械手的工作空間。從工作空間中選取4 個(gè)途徑點(diǎn)作為插值點(diǎn)，采用3-5-3 分段插值多項(xiàng)式進(jìn)行軌跡規(guī)劃，通過(guò)改進(jìn)的PSO 算法對(duì)得到的軌跡進(jìn)行優(yōu)化，得到了鉆車(chē)機(jī)械手的時(shí)間最優(yōu)軌跡規(guī)劃。

（2）基于改進(jìn)PSO 算法的鉆車(chē)機(jī)械手時(shí)間最優(yōu)軌跡規(guī)劃方法可在滿足鉆車(chē)機(jī)械手運(yùn)動(dòng)學(xué)約束的前提下，使其運(yùn)行時(shí)間從3.168 5 s 減少到2.385 4 s，整體時(shí)間較優(yōu)化前可縮短約25%，提高了關(guān)節(jié)運(yùn)行速度。