基于改進粒子群算法的鉆車機械手軌跡規劃研究

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

（1.河北工程大學 機械與裝備工程學院,河北 邯鄲 056038；2.冀凱河北機電科技有限公司,河北 石家莊 050000；3.河北省煤炭生態保護開采產業技術研究院,河北 邯鄲 056038；4.邯鄲市智能車輛重點實驗室,河北 邯鄲 056038）

0 引言

隨著我國煤炭開采強度加大、開采深度加深、地質條件越來越復雜，沖擊地壓、水害、瓦斯突出、火災等災害發生的概率也越來越高[1]。為了保證煤礦綠色安全開采“智能化、無人化”，國家發布了《煤礦機器人重點研發目錄》，防突鉆孔機器人與防沖鉆孔機器人是其中重點研發內容[2]。防突防沖鉆車是防突鉆孔機器人與防沖鉆孔機器人的結合體，可對煤體應力進行有效卸壓，且不會破壞原有的巷道支護，有效降低了煤與瓦斯突出、沖擊地壓等災害發生的概率。鉆車機械手作為防突防沖鉆車不可或缺的裝置，關系著鉆車是否可以正常鉆進及真正實現無人化。提高鉆車機械手的工作效率，對于提高煤礦安全生產效率具有重要意義。

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

針對上述問題，本文以鉆車機械手為研究對象，提出了一種基于改進PSO 算法的鉆車機械手時間最優軌跡規劃方法。首先，利用標準Denavit-Hartenberg（D-H）構建鉆車機械手三維模型，通過蒙特卡洛法得到鉆車機械手的工作空間，從工作空間中選取4 個途徑點作為插值點。然后，為了使鉆車機械手能夠快速平穩地到達指定位置，在關節空間中采用3-5-3 分段多項式插值構造其軌跡。最后，通過改進PSO 算法對構造的軌跡進行時間最短優化，得到鉆車機械手的時間最優軌跡規劃。Matlab 仿真結果證明了該方法的可靠性。

1 鉆車機械手運動學模型

1.1 鉆車機械手結構

本文研究的鉆車機械手為2 自由度機械手，由1 個轉動副和1 個移動副組成，其整體結構如圖1 所示。主要工作任務為從回轉托板抓取鉆桿裝入主機夾持器內，具體上桿步驟如下：①判斷機械手與托板狀態，保證順利抓桿；通過判斷仰角檢測接近開關和俯角檢測接近開關狀態，調整機械手和托板位置。② 機械手擺動油缸逆時針旋轉至憋壓狀態，機械手到達抓桿角度位置。③機械手伸縮油缸伸出至憋壓狀態，機械手到達抓桿位。④ 機械手爪夾緊，抓住鉆桿。⑤ 機械手擺動油缸順時針旋轉至憋壓狀態，機械手到達上桿位置。⑥ 夾持器（副夾持器）夾緊新鉆桿，夾緊前端已鉆入鉆桿，回轉器正轉進給，連接新加鉆桿。

圖1 鉆車機械手整體結構Fig.1 Overall structure of drill rig manipulator

1.2 鉆車機械手運動學模型

1.2.1 D-H 參數確定

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

圖2 標準D-H 坐標系Fig.2 Standard D-H coordinate system

結合圖2 的坐標系及其參數可求得機械手的D-H 參數，見表1。表1 中，θf為關節角，df為偏移距，af為桿件長度，αf扭轉角。

1.2.2 變換矩陣計算

根據圖2 坐標系和表1 中的各個參數，可求得連桿之間的齊次變換矩陣。其中，相鄰2 個連桿之間的齊次變換矩陣為[11]

表1 機械手D-H 參數Table 1 D-H parameters of manipulator

將表1 中的參數值代入式（1）可計算出2 個變換矩陣分別為

通過式（2）、式（3）可得到鉆車機械手相對于直角坐標系x，y，z軸的末端位姿矩陣[12]：

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

2 鉆車機械手關節空間軌跡規劃

軌跡規劃通常在笛卡爾空間或關節空間中進行，且兩者可互相轉換[13]。在笛卡爾空間中，可自然地描述末端執行器的軌跡，用逆運動學求解每個關節的插補點，計算相應的關節角度，計算量非常大，且容易導致運動奇異點和機械手冗余。而在關節空間中進行軌跡規劃，可實時調整各個關節的角度插值點，從而使機械手達到期望的狀態。結合實際情況，本文在關節空間中對鉆車機械手進行軌跡規劃。

2.1 分段多項式軌跡規劃

對于多項式樣條插值函數，可以保證鉆車機械手關節準確地經過各個插值點。速度的變化規律可通過對時間求1 階導來觀察。同理，對時間求2 階導可獲得加速度的變化規律。為了保證鉆車機械手的速度和加速度的連續，需要最少保證函數C3連續，同時為了保證計算量不太復雜，根據實際工況需要，將鉆車機械手的運行軌跡分為3 段，即3-5-3 多項式插值。其中，3-5-3 樣條多項式的通式為

式中：hj1(t)，hj2(t)，hj3(t) 分別為3 段多項式的軌跡，j為當前關節，t為總時間；mj為第j個關節軌跡的系數；t1，t2，t3分別為每個關節初始段、中間段和終止段的運動時間。

已知各個關節在通過抓桿點β0、路徑點β1和β2、放桿點β3時，各個端點連接處的速度和加速度都平穩連續，且在抓桿點和放桿點時速度和加速度都為0[14]，得多項式系數與插值點關系為

式中：m為關節系數矩陣；A為經過4 個插值點時3-5-3 插值多項式的系數矩陣；b為對應的插值點矩陣。

2.2 基于改進PSO 算法的軌跡優化

以時間最優為優化目標，利用改進的PSO 算法對鉆車機械手進行優化。將每個關節的3 段時間t1，t2，t3組成一個三維的粒子群[15]，其中粒子群適應度函數為

初始化之后，計算每個粒子初始化后的適應度，并將其賦值給pbest（個體最優值）。利用改進PSO 算法以式（11）為適應度函數進行迭代更新，迭代完成后，計算當前全部粒子的適應度，其中值最小的粒子作為gbest（全局最優值）。PSO 算法速度和位置更新公式為

式中：vidk+1為第i個粒子在d維時，迭代k+1 次的速度；ω為慣性權重；c1，c2為學習因子，決定粒子獲取外界信息的能力；r1，r2為[0,1]的隨機數；xid為第i個粒子在d維時的位置。

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

式中：ωmax為最大慣性權重；ωmin為最小慣性權重；k為當前迭代次數；kmax為最大迭代次數。

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

（1）參數初始化。首先明確有多少個粒子，在三維搜索空間中隨機產生M個粒子（共有3M個）構成種群，使用rand 函數隨機給定時間變化量及時間。

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

（3）式（5）-式（7）中的三段多項式軌跡可根據步驟（2）所得到的系數矩陣m得到。判斷3 段軌跡的速度函數是否全部小于各個關節允許的最大速度。如果求得的3 段軌跡的速度函數都在速度約束范圍內，則將式（11）作為其適應度函數，計算適應度，以時間最短為目標進行迭代；否則，將賦值一個極大的常數充當其適應度，在接下來迭代尋優時，會通過比較適應度的大小來淘汰該粒子。

（4）對隨機產生的M個粒子，將其自身經過迭代k次后，所產生的n個適應度值作比較，適應度值最小的位置作為pbest，再通過比較每個粒子的pbest，選取其中最小的適應度值作為gbest。

（5）根據式（12）和式（13）來更新種群的速度和位置，并將其重新構成具有新的位置和速度的M個粒子的新種群。

（6）如果達到最大迭代次數或找到局部最優解，則算法結束，否則返回步驟（2）。

3 仿真實驗與結果分析

3.1 鉆車機械手的Matlab 建模

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

圖3 鉆車機械手三維模型Fig.3 3D model of drill rig manipulator

圖4 鉆車機械手工作空間Fig.4 Workspace of drill rig manipulator

3.2 鉆車機械手的Matlab 仿真

根據鉆車機械手的工作空間及抓桿、放桿的工作特點，給定其在笛卡爾空間中的抓桿點、2 個路徑點及放桿點的位置，見表2。

根據逆運動學，可以將表2 給定的各點轉換到關節空間中，從而獲得各個關節在各點的角度插值點，見表3。其中，關節2 為伸縮關節，只改變伸縮量，所以它的角度插值不變，為0。

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

表3 關節空間的角度插值點Table 3 Angle interpolation points in joint space

3-5-3 分段插值多項式中的3 段插值時間都是根據實際情況提前給定的。在滿足抓桿點和放桿點的速度和加速度都是0 的前提下，給定總時間t=3.168 5 s，t1=0.866 5 s，t2=0.667 s，t3=1.635 s。在給定每段關節時間的插值效果如圖5-圖7 所示。

圖5 3-5-3 樣條插值機械手關節位置曲線Fig.5 Joint position curves of manipulator based on 3-5-3 spline interpolation

圖6 3-5-3 樣條插值機械手關節速度曲線Fig.6 Joint velocity curves of manipulator based on 3-5-3 spline interpolation

圖7 3-5-3 樣條插值機械手關節加速度曲線Fig.7 Joint acceleration curves of manipulator based on 3-5-3 spline interpolation

從圖5-圖7 可看出，機械手各關節位置、速度和加速度都是連續的，表明了該分段插值多項式可以很好地用于構造機械手軌跡，但是圖6 中關節的速度并未達到峰值。

根據改進的PSO 算法對防突防沖鉆車機械手進行優化，設定初始粒子數M=50，初始位置為0.1～4 s之間的任意隨機位置，粒子迭代速度在[-3，3]之間，迭代次數為50，慣性權重根據式（14）變化，速度和加速度在起始和終止時為0。優化后，適應度值和迭代次數之間的變換規律如圖8 所示。可看出粒子在迭代23 次以后幾乎趨于穩定，證明改進PSO 算法可快速收斂。

圖8 改進PSO 算法的適應度曲線Fig.8 Fitness curve of improved PSO algorithm

采用改進的PSO 算法對鉆車機械手進行時間最優軌跡規劃，以機械手關節1 為例，得到其3 段插值時間的迭代過程如圖9 所示。

圖9 關節1 最優粒子迭代過程Fig.9 Optimal particle iteration process of joint 1

從圖9 可看出，關節1 的3 段插值時間最后平穩趨于定值，即可得到關節1 優化后的時間。

由于關節2 為伸縮油缸，在進行抓桿、放桿的軌跡規劃時不改變關節位置，所以關節2 各段運行時間為0，各關節優化結果見表4。

表4 各關節優化結果Table 4 Optimization results of each joint s

為了保證關節1 和關節2 在同一時間到達放桿點的位置，同時確保每個關節運行平穩，應選取優化后各關節時間的最大值，由于關節2 各段插值時間為0，所以只采用關節1 優化后的插值時間，即0.648 7,0.391 9,1.344 8 s，總時間為2.385 4 s，與優化前相比縮短了0.783 1 s，縮短約25%，提高了機械手的工作效率。

優化后機械手各關節的位置、速度和加速度曲線如圖10-圖12 所示。

圖10 優化后的機械手關節位置曲線Fig.10 Joint position curves of optimized manipulator

圖11 優化后的機械手關節速度曲線Fig.11 Joint velocity curves of optimized manipulator

圖12 優化后的機械手關節加速度曲線Fig.12 Joint acceleration curves of optimized manipulator

從圖10 可看出，優化后的關節位置相較之前更加平滑。從圖11 可看出機械手的關節最大速度較圖6 有很大提高。從圖12 可看出，優化后的加速度雖然較之前有起伏，但其位置、速度和加速度都連續且在運動學約束范圍內。從圖11 和圖12 還可看出，起始點和終點數值都為0，符合預先假設。仿真結果顯示，通過改進的PSO 算法對鉆車機械手進行軌跡規劃，可以減少作業時間，提高其工作效率。

4 結論

（1）基于改進PSO 算法的鉆車機械手時間最優軌跡規劃方法通過構建鉆車機械手運動學模型得到其末端位姿矩陣，并利用蒙特卡洛法得到了鉆車機械手的工作空間。從工作空間中選取4 個途徑點作為插值點，采用3-5-3 分段插值多項式進行軌跡規劃，通過改進的PSO 算法對得到的軌跡進行優化，得到了鉆車機械手的時間最優軌跡規劃。

（2）基于改進PSO 算法的鉆車機械手時間最優軌跡規劃方法可在滿足鉆車機械手運動學約束的前提下，使其運行時間從3.168 5 s 減少到2.385 4 s，整體時間較優化前可縮短約25%，提高了關節運行速度。