基于改進(jìn)粒子群算法的鑿巖機(jī)械臂軌跡規(guī)劃

黃開(kāi)啟，陳翀，劉展飛

（江西理工大學(xué)電氣工程與自動(dòng)化學(xué)院，江西 贛州 341000）

為了解決在施工中的作業(yè)環(huán)境差、耗資耗時(shí)多、勞動(dòng)強(qiáng)度大等問(wèn)題，早在20世紀(jì)70年代就開(kāi)始研究鑿巖設(shè)備代替人工作業(yè)，現(xiàn)如今鑿巖機(jī)器人的應(yīng)用更加普遍，為了使鑿巖機(jī)器人更適合復(fù)雜高精度的作業(yè)，提出了一種7自由度鑿巖機(jī)器人，隨著冗余度的增多，高性能的軌跡規(guī)劃成為了機(jī)器人領(lǐng)域研究重點(diǎn)之一。

機(jī)械臂的軌跡規(guī)劃方法很多，多項(xiàng)式插值因?yàn)槠溆?jì)算簡(jiǎn)單的優(yōu)點(diǎn)而被廣泛使用。孫玥等［1］為了解決機(jī)械臂在運(yùn)動(dòng)過(guò)程中速度和加速度的突變帶來(lái)的抖動(dòng)和不穩(wěn)定等問(wèn)題，采用了5次多項(xiàng)式插值對(duì)機(jī)械臂的軌跡擬合，確保機(jī)械臂運(yùn)行平穩(wěn)；方爽［2］等采用7次多項(xiàng)式插值方法有效地解決了加加速度不連續(xù)問(wèn)題。但隨著多項(xiàng)式插值次數(shù)的增加，凸包性質(zhì)不夠好，計(jì)算難度也越來(lái)越大，不少學(xué)者采用了分段插值方法。Guan等［3］提出了一種結(jié)合3階和4階多項(xiàng)式進(jìn)行軌跡插值的方法，對(duì)軌跡的第一段和最后一段使用兩個(gè)4次多項(xiàng)式，對(duì)其他段使用3次多項(xiàng)式；梁志鵬等［4］提出了一種4-4-4-4-5插值軌跡規(guī)劃方法，分段方法能夠較好地解決高次插值計(jì)算量大的問(wèn)題，降低了“龍格現(xiàn)象”的發(fā)生。以上研究雖然解決了傳統(tǒng)算法難以優(yōu)化高階多項(xiàng)式的問(wèn)題，但沒(méi)有考慮多項(xiàng)式插值軌跡最優(yōu)問(wèn)題，粒子群算法［5-7］在優(yōu)化問(wèn)題中得到廣泛使用。

本文提出了一種在速度和加速度約束下，利用改進(jìn)粒子群算法以時(shí)間為目標(biāo)函數(shù)，對(duì)分段多項(xiàng)式插值進(jìn)行優(yōu)化的軌跡規(guī)劃算法。以7自由度鑿巖機(jī)械臂為研究對(duì)象，在機(jī)械臂運(yùn)動(dòng)學(xué)分析的基礎(chǔ)上確定多項(xiàng)式插值函數(shù)。在滿足約束條件下，以時(shí)間最短為目的，對(duì)3-5-3分段多項(xiàng)式插值的各時(shí)間段進(jìn)行優(yōu)化。對(duì)軌跡規(guī)劃的插補(bǔ)時(shí)間引入智能算法優(yōu)化，能夠減少機(jī)械臂運(yùn)動(dòng)時(shí)間，降低機(jī)械臂運(yùn)動(dòng)能耗，并且能夠有效地避免機(jī)械臂不必要的沖擊，增加了穩(wěn)定性。

1 機(jī)械臂運(yùn)動(dòng)學(xué)分析

運(yùn)動(dòng)學(xué)分析是軌跡規(guī)劃的基礎(chǔ)。機(jī)械臂在運(yùn)動(dòng)時(shí)，通過(guò)末端位姿可以獲取各關(guān)節(jié)的位置變化，從而達(dá)到指定位姿。本文研究鑿巖機(jī)器人各關(guān)節(jié)按照［8-9］的規(guī)則建立D-H模型，如圖1所示，D-H參數(shù)表如表1所示。表中：αi-1為扭轉(zhuǎn)角；ai-1為桿件長(zhǎng)度；θi為關(guān)節(jié)角；di為偏移距。

圖1 機(jī)械臂坐標(biāo)系Fig.1 Robotic arm coordinate system

表1 機(jī)械臂的D-H參數(shù)Tab.1 D-H parameters of the manipulator

機(jī)械臂第i個(gè)和第i-1個(gè)連桿之間的變換矩陣為

根據(jù)前面給定的D-H參數(shù)表，代入公式可以得到每個(gè)連桿之間的變換矩陣01T、12T、23T、34T、45T、56T、67T，為了得到基座與末端執(zhí)行器之間的關(guān)系，將各連桿的變換矩陣相乘，得到機(jī)械臂的末端位姿矩陣為

式中：向量n、o、a分別為機(jī)械臂末端執(zhí)行機(jī)構(gòu)的法向矢量、滑動(dòng)矢量和接近矢量；向量p為機(jī)械臂的位置矢量。

機(jī)械臂的運(yùn)動(dòng)學(xué)逆解可以由文獻(xiàn)［10］所得。

2 多項(xiàng)式插值函數(shù)的構(gòu)造

三次插值多項(xiàng)式因?yàn)橛?jì)算簡(jiǎn)單而廣泛應(yīng)用于軌跡規(guī)劃，但由于機(jī)械臂運(yùn)行時(shí)穩(wěn)定性較差，可能存在關(guān)節(jié)磨損，不利于控制精度。本文提出了一種3-5-3分段多項(xiàng)式插值方法，在起點(diǎn)和終點(diǎn)中插入兩個(gè)中間點(diǎn)，在第1段和第3段使用三次多項(xiàng)式插值，第2段使用5次多項(xiàng)式插值，這樣既能保證軌跡的光滑度又能避免因?yàn)榇螖?shù)過(guò)高而引起的計(jì)算復(fù)雜。

本研究采用的3-5-3多項(xiàng)式插值方法為

式中：aim為第i個(gè)關(guān)節(jié)中第m個(gè)未知系數(shù)；i=1，2，…，n，n為關(guān)節(jié)數(shù)；m=1，2，…，14；fi1(ti1)、fi2(ti2)、fi3(ti3)為第i個(gè)關(guān)節(jié)對(duì)應(yīng)的第1、第2、第3個(gè)區(qū)間中的位置；ti1、ti2、ti3為第i個(gè)關(guān)節(jié)對(duì)應(yīng)的第1、第2、第3個(gè)區(qū)間中的時(shí)間。

為保證軌跡的光滑性，在起點(diǎn)、終點(diǎn)和2個(gè)中間點(diǎn)已知的情況下，假設(shè)起點(diǎn)速度、加速度、終點(diǎn)速度、加速度都為零；中間兩點(diǎn)位置、速度、加速度均連續(xù)，在此假設(shè)下可由以下公式計(jì)算未知系數(shù)：

式中：T為關(guān)于第i個(gè)關(guān)節(jié)的插補(bǔ)時(shí)間ti1、ti2、ti3的矩陣；A1～A8為矩陣T中對(duì)應(yīng)的分塊矩陣；Q為關(guān)節(jié)角的位置矩陣；A為系數(shù)矩陣。

3 目標(biāo)函數(shù)的選取

根據(jù)機(jī)械臂運(yùn)動(dòng)約束和工業(yè)需求，機(jī)械臂運(yùn)動(dòng)時(shí)，在保證關(guān)節(jié)平滑運(yùn)行時(shí)，關(guān)節(jié)i允許的最大速度為vmaxi(°)/s，最大加速度為amaxi（°）/s2。本文優(yōu)化的目的是在3-5-3多項(xiàng)式插值的基礎(chǔ)上提高機(jī)械手的效率和穩(wěn)定性，分別對(duì)每個(gè)關(guān)節(jié)進(jìn)行優(yōu)化，將第i個(gè)關(guān)節(jié)經(jīng)過(guò)的4個(gè)點(diǎn)分成對(duì)應(yīng)的3個(gè)時(shí)間段，在約束下對(duì)時(shí)間進(jìn)行優(yōu)化，因此以最短的運(yùn)行時(shí)間作為適應(yīng)度函數(shù)為

約束條件為

4 改進(jìn)的粒子群算法

粒子群算法（particle swarm optimization，PSO）最早是在1995年由Eberhard和Kennedy［11］共同提出，其主要思想來(lái)源于早期對(duì)很多鳥(niǎo)類的群體行為的建模與仿真。設(shè)Zj=(zj1，zj2，…，zjD)為第j個(gè)粒子的D維位置矢量，代入到根據(jù)要求設(shè)定的適應(yīng)值函數(shù)f(t)計(jì)算Zj當(dāng)前的適應(yīng)值，即可衡量粒子位置的優(yōu)劣；Vj=(vj1，vj2，…，vjd，…，vjD)為粒子j的飛行速度，即粒子移動(dòng)的距離；Pj=(pj1，pj2，…，pjd，…，pjD）為粒子迄今為止搜索到的最優(yōu)位置；Pg=(pg1，pg2，…，pgd，…，pgD)為所有粒子到目前為止搜索到的最優(yōu)位置。

在每次迭代中，粒子根據(jù)以下式子更新速度和位置：

式中：j=1，2，…，m，m為粒子數(shù)；d=1，2，…，D，D為粒子維度；k為迭代次數(shù)；r1、r2為［0，1］之間的隨機(jī)數(shù)；ω為慣性權(quán)重；c1、c2為學(xué)習(xí)因子。

為了更好地到達(dá)全局最優(yōu)位置，提出了一種自適應(yīng)慣性權(quán)重和動(dòng)態(tài)調(diào)整學(xué)習(xí)因子相結(jié)合的改進(jìn)方法。種群的全局搜索能力由全局搜索速度和慣性控制。如果ω得到一個(gè)固定的較大值，算法的收斂速度會(huì)過(guò)慢，最終解的精度也會(huì)很低。如果ω得到一個(gè)固定的較小值，算法可以檢測(cè)到局部區(qū)域，在這種情況算法容易早熟收斂。此外，還發(fā)現(xiàn)PSO的慣性加權(quán)值不宜過(guò)高也不宜過(guò)低。在0.9～0.4之間降低效果最好。更新后的慣性權(quán)重規(guī)則為

式中：ωn為ω的最大值；ωx為ω的最小值；Tx為最大迭代次數(shù)；k為當(dāng)前迭代次數(shù)。

為了增強(qiáng)算法前期粒子的搜索能力和后期的收斂能力，本文在前期保持較大的c1值和較小的c2值，在算法的后期保持c1值較小，c2值較大。文獻(xiàn)［12］中推導(dǎo)可知，要滿足PSO算法的收斂條件，算法各參數(shù)需要滿足以下條件：

更新后的學(xué)習(xí)因子規(guī)則為

粒子群優(yōu)化算法的流程圖如圖2所示。

圖2 流程Fig.2 Flowchart

5 仿真驗(yàn)證與結(jié)果分析

本例中的運(yùn)行環(huán)境為Windows10 64位，處理器為Intel（R）Core（TM）i5-7300HQ CPU@2.50GHz，RAM 8GB，軟件Matlab。

在工作范圍內(nèi)，取機(jī)械臂末端4個(gè)位姿，分別表示初始點(diǎn)、中間點(diǎn)1、中間點(diǎn)2和終點(diǎn)。表2中前3個(gè)值是位置信息，后3個(gè)值是姿態(tài)信息。利用文獻(xiàn)［10］的運(yùn)動(dòng)學(xué)逆解方法，將笛卡爾空間中的插值點(diǎn)轉(zhuǎn)換為關(guān)節(jié)空間中的插值點(diǎn)，轉(zhuǎn)換后見(jiàn)表3。

表2 機(jī)械臂末端位置和姿態(tài)信息Tab.2 Information of robotic arm terminal position and posture

表3 關(guān)節(jié)角度的插值點(diǎn)Tab.3 Interpolation points of joint angles

當(dāng)起始點(diǎn)和終點(diǎn)的速度和加速度均為零，且中間點(diǎn)1和中間點(diǎn)2速度、加速度連續(xù)時(shí)，設(shè)每段插補(bǔ)時(shí)間為5 s，用傳統(tǒng)的3-5-3多項(xiàng)式插值法可以得到優(yōu)化前的各關(guān)節(jié)運(yùn)動(dòng)的位置、速度、加速度曲線，此時(shí)運(yùn)行效率低下。

采用改進(jìn)的粒子群算法求解具有速度、加速度約束的多項(xiàng)式插值的最優(yōu)時(shí)間。以關(guān)節(jié)1最優(yōu)時(shí)間的求解為例，關(guān)節(jié)1各節(jié)段的時(shí)間進(jìn)化如圖3所示。參數(shù)設(shè)置：初始種群設(shè)置為50；最大迭代次數(shù)為100次；最大速度為25（°）/s；最大加速度為15（°）/s2；ωn=0.9；ωx=0.4。綜合考慮安全、穩(wěn)定、效率等因素，給每個(gè)關(guān)節(jié)設(shè)置合適的速度、加速度。利用改進(jìn)的粒子群算法優(yōu)化得到其他關(guān)節(jié)各節(jié)段的最優(yōu)時(shí)間，結(jié)果見(jiàn)表4。

圖3 關(guān)節(jié)1各段時(shí)間進(jìn)化Fig.3 Time evolution diagram of joint 1

表4 各個(gè)關(guān)節(jié)的最優(yōu)時(shí)間Tab.4 Optimal time for each joint

因?yàn)闄C(jī)械臂各個(gè)關(guān)節(jié)的運(yùn)動(dòng)是同步的，所以每個(gè)時(shí)間段都選取各關(guān)節(jié)中的最大時(shí)間來(lái)作為每個(gè)分段的 插 補(bǔ) 時(shí) 間。由 表4可 知：Max（ti1）=4.355 5 s，Max（ti2）=2.749 4 s，Max（ti3）=3.810 0 s。與未優(yōu)化前相比，插值時(shí)間縮短了4.085 1 s。

圖4～圖6為改進(jìn)粒子群算法優(yōu)化后得到的位置、速度、加速度曲線。由圖可以看出，優(yōu)化后的速度、加速度有所增加，但沒(méi)有突破極限并且趨向于極限，仍然滿足約束條件。

圖4 優(yōu)化后的各關(guān)節(jié)位置曲線Fig.4 The optimized position curve of each joint

圖6 優(yōu)化后的各關(guān)節(jié)加速度曲線Fig.6 The optimized acceleration curves of each joint

圖5 優(yōu)化后的各關(guān)節(jié)速度曲線Fig.5 The optimized velocity curves of each joint

6 結(jié)語(yǔ)

利用3-5-3分段多項(xiàng)式插值進(jìn)行軌跡擬合，可以保證機(jī)械臂位置、速度、加速度連續(xù)，提高運(yùn)行穩(wěn)定性，但是每個(gè)分段的插補(bǔ)時(shí)間的不確定性，嚴(yán)重影響了機(jī)械臂工作的工作效率，提出了一種以速度和加速度為約束條件，基于改進(jìn)粒子群算法的關(guān)節(jié)空間時(shí)間最優(yōu)軌跡規(guī)劃方法。仿真結(jié)果表明，改進(jìn)粒子群算法具有更快的收斂速度和更好的優(yōu)化結(jié)果。通過(guò)該算法優(yōu)化后，機(jī)械臂各關(guān)節(jié)運(yùn)行平穩(wěn)，運(yùn)行時(shí)間大大縮短，并且滿足速度和加速度約束，能夠較好地達(dá)到預(yù)期的優(yōu)化效果。