基于改進正余弦算法的機械臂軌跡規(guī)劃*

潘明章,潘 雷,李昌倫,梁 科,姚博煒

(1.廣西大學(xué)機械工程學(xué)院,南寧 530004;2.柳州賽克科技發(fā)展有限公司,柳州 545005)

0 引言

軌跡規(guī)劃是機械臂運動控制的基礎(chǔ)。隨著工業(yè)自動化的不斷發(fā)展,機械臂的應(yīng)用環(huán)境也愈發(fā)復(fù)雜,現(xiàn)有的軌跡規(guī)劃算法正在向著智能化的方向發(fā)展。高性能的軌跡規(guī)劃算法使機械臂運行更平穩(wěn),工作效率更高,使用壽命更長。因此,軌跡規(guī)劃算法得到了國內(nèi)外專家和學(xué)者的廣泛關(guān)注和研究,許多以時間最優(yōu)、能量最低和沖擊最優(yōu)為目標的軌跡規(guī)劃算法被提出[1-3],而其中時間最優(yōu)軌跡規(guī)劃由于其廣泛且重要的應(yīng)用價值成為深入研究的熱門話題。

關(guān)節(jié)空間中的3次多項式插值的軌跡規(guī)劃方法由于計算簡單得到了廣泛的應(yīng)用,而其所產(chǎn)生軌跡會使機械臂的關(guān)節(jié)角加速度會突變,突變意味著驅(qū)動力的不連續(xù),這會導(dǎo)致機械振動現(xiàn)象的發(fā)生從而降低了機械臂的使用壽命和控制精度。針對這一問題,可以通過增加軌跡插值多項式的階數(shù)來解決,如5次多項式插值,但是每一段軌跡都采用5次多項式插值會導(dǎo)致運算量大幅增加。因此XU等[4]使用3-5-3多項式插值來在上述兩種插值方法中取得良好的折中,然而他們沒有對插值時間進行優(yōu)化,所生成的插值軌跡作業(yè)效率低。由于多項式插值的階次高,缺乏凸包,傳統(tǒng)方法難以對插值時間進行優(yōu)化,于是利用智能優(yōu)化算法進行時間最優(yōu)軌跡規(guī)劃成為一個有價值的研究方向。常用的智能優(yōu)化算法包括粒子群算法[5]、遺傳算法[6]和差分進化算法[7]等。但是相較于正余弦算法[8],這些算法參數(shù)結(jié)構(gòu)復(fù)雜且收斂速度較慢。

正余弦算法因其出色的性能被廣泛地應(yīng)用在各領(lǐng)域中。馬瑩瑩等[9]將正余弦算法運用于機器人的路徑規(guī)劃中以增強探索性能。郎春博等[10]提出改進的正余弦算法來解決多閾值圖像分割方法計算量大、分割精度低的問題。王慶等[11]針對含風能的經(jīng)濟排放調(diào)度問題,提出了一種混沌正余弦算法以最小化發(fā)電成本和碳排放量。正余弦算法的相關(guān)研究在眾多領(lǐng)域已取得大量成果,但尚未應(yīng)用于機械臂的軌跡規(guī)劃問題中。

本文提出了一種基于改進正余弦算法的機械臂時間最優(yōu)3-5-3多項式插值的軌跡規(guī)劃算法。該算法在迭代過程中通過平衡因子動態(tài)調(diào)整當前解和當前最優(yōu)解之間的權(quán)重,加強了局部開發(fā)的能力,此外通過引入Lévy飛行改善傳統(tǒng)正余弦算法容易陷入局部最優(yōu)的不足,提高了收斂速度。以UR5e機械臂為研究對象,根據(jù)機器人運動學(xué)確定多項式插值函數(shù),在一系列約束下使用改進正余弦算法優(yōu)化插值時間。通過實驗驗證所提出算法的有效性,并與其他先進的優(yōu)化算法進行了對比。

1 機械臂運動學(xué)模型與多項式插值

1.1 機械臂運動學(xué)建模

機械臂的運動學(xué)模型是軌跡規(guī)劃的基礎(chǔ)。在絕大多數(shù)機械臂應(yīng)用場景下,都需要根據(jù)機械臂末端執(zhí)行器的位姿來獲取各關(guān)節(jié)的角度位置,從而完成任務(wù)空間的運動。本文以UR5e協(xié)作機器人為研究對象,機器人實體如圖1所示,通過D-H參數(shù)法來對其進行運動學(xué)建模,D-H參數(shù)如表1所示。

表1 UR5e的D-H參數(shù)

圖1 UR5e機械臂

1.2 3-5-3多項式插值

為了使機械臂在任務(wù)空間中完成對指定運動,可以對任務(wù)的起點,第一個中間點,第二個中間點和終點進行逆運動學(xué)求解,得到每個關(guān)節(jié)的角度,然后通過3-5-3插值完成軌跡規(guī)劃。具體的插值方法如下:

(1)

式中:qi1(t)代表機械臂第i個關(guān)節(jié)第j段的軌跡,aijk代表機械臂第i個關(guān)節(jié)第j段軌跡插值多項式的k次項所對應(yīng)的系數(shù),t1、t2、t3分別為3段軌跡的執(zhí)行時間。插值所生成的軌跡還應(yīng)滿足以下條件:

(1)4個插值點的各關(guān)節(jié)角度已知;

(2)起點和終點對應(yīng)的各關(guān)節(jié)角速度和角加速度為0;

(3)整個軌跡過程中各關(guān)節(jié)角速度和角加速度連續(xù)。

現(xiàn)在,我們可以求解出插值多項式中所有的未知系數(shù),具體過程為:

a=A-1·θ=[a1a2a3]

(2)

式中:

θ=[0 0 0 0 0 0θi40 0θi10 0θi3θi2]T

2 改進正余弦算法

2.1 傳統(tǒng)正弦余弦算法

正弦余弦算法(SCA)是一種新穎的基于種群的隨機優(yōu)化算法,其僅利用正弦和余弦函數(shù)的波動性和隨機性作為實現(xiàn)算子的設(shè)計目標來搜索和迭代最優(yōu)解。與遺傳算法,粒子群算法,布谷鳥算法等眾多智能優(yōu)化算法相比,正弦余弦算法具有參數(shù)少、結(jié)構(gòu)簡單、易實現(xiàn)、收斂速度快等優(yōu)點,在實際應(yīng)用中具有較優(yōu)的性能。其優(yōu)化迭代過程可分為全局搜索和局部開發(fā)兩個階段。在全局搜索中,對于當前解集中的解施加較大的隨機波動來搜索解空間中的未知區(qū)域;在局部開發(fā)中,對當前解施加微弱的隨機擾動來充分搜索當前解的鄰域。具體的迭代方程分為以下正弦迭代和余弦迭代方程兩種,由式(3)所示。

(3)

(4)

式中:xi(t)表示解個體在第t次迭代時在第i維度的分量,pi(t)表示在到第t次迭代為止的所有解集合中的最優(yōu)解在第i維度的分量。參數(shù)r1由式(4)定義,用來實現(xiàn)全局搜索和局部開發(fā)的平衡和切換,其中a是一個常數(shù),通常被設(shè)置為2,t和T分別為當前迭代次數(shù)和最大迭代次數(shù)。參數(shù)r2～U[0,2π]描述了當前解向當前最優(yōu)解更新時移動的方向。參數(shù)r3是范圍為[0,2]的隨機數(shù),為當前最優(yōu)解帶來隨機權(quán)重以強調(diào)或者忽略最優(yōu)解在定義移動距離時的影響效果。參數(shù)r4是[0,1]的隨機數(shù),使迭代在正弦更新和余弦更新之間隨機切換。

2.2 改進的位置更新方程

正弦余弦算法和其他基于種群的優(yōu)化算法一樣,需要平衡全局搜索和局部開發(fā)。在傳統(tǒng)的正弦余弦算法中,解的位置更新方程僅僅將當前最優(yōu)解用于隨機確定到下一搜索區(qū)域的距離,而沒有利用當前最優(yōu)解的位置信息,導(dǎo)致SCA整體更傾向于全局搜索,算法的收斂性較差。因此,針對SCA的上述不足,本文提出了一種平衡權(quán)重更新機制,充分利用當前最優(yōu)解的位置信息來加強算法的局部開發(fā)能力,設(shè)計了一個改進的位置更新方程,由式(5)所示。

(5)

式中:λ是平衡因子,由式(6)描述,其功能是調(diào)整當前最優(yōu)解和當前解之間的權(quán)重。

(6)

式中:fit(p(t))和fit(x(t))分別為當前最優(yōu)解和當前解的適應(yīng)度值,與式(3)相比,式(5)強調(diào)了當前最優(yōu)解的位置信息,而部分淡化了當前解的位置信息,這使得SCA更加專注于當前最優(yōu)解附近的區(qū)域,加強了局部開發(fā)能力。此外,為了避免過度的局部開發(fā),引入平衡因子λ,利用當前最優(yōu)解和當前解的適應(yīng)度值來動態(tài)調(diào)整其各自權(quán)重。當λ較小時,當前解與當前最優(yōu)解之間存在較大差異,下一個搜索區(qū)域?qū)⒏嗟丶杏诋斍白顑?yōu)解附近,相反,當λ較大時,當前解很接近當前最優(yōu)解,下次搜索將加強對當前解自身位置附近區(qū)域的開發(fā)。

2.3 混合Lévy飛行策略

Lévy飛行理論自被提出以來[12],因其步長為概率分布呈重尾分布的隨機游走,已被廣泛應(yīng)用于智能優(yōu)化算法中來提高算法的搜索能力。正余弦算法通過兩個相互排斥的正余弦方程來搜索迭代尋找最優(yōu)解,然而在實際應(yīng)用中,這種搜索過程很容易陷入局部最優(yōu)的困境。因此,針對這一問題,本文將Lévy飛行的隨機游走策略融入到正余弦算法中,在式(5)的基礎(chǔ)上作出改進,將位置更新方程分為3個方程,如式(7)所示。

(7)

式中:c是常參數(shù),用來調(diào)整3個更新方程的比重;randn是步長縮放因子,為均值為0,方差為1的服從正態(tài)分布的隨機數(shù);Levy(xi(t))描述了步長服從Lévy分布的隨機游走,游走的隨機步長采用Mantegna提出用正太分布求解隨機數(shù)的方法來求解[13],其模型為:

Levy(xi(t))=s·(pi(t)-xi(t))

(8)

(9)

3 基于改進正余弦算法的軌跡優(yōu)化

時間最優(yōu)軌跡規(guī)劃的最終目標是找到一條沿著設(shè)定路徑的最快可行軌跡,來提高機械臂的工作效率。針對機械臂的最優(yōu)時間軌跡規(guī)劃問題,本文采用“3-5-3”多項式來進行軌跡插值,通過所提出的改進SCA算法來優(yōu)化每段軌跡的運行時間t1,t2,t3?？紤]到在真實環(huán)境中,機械臂受到自身的運動學(xué)和動力學(xué)的物理極限的限制,將優(yōu)化問題的約束條件設(shè)置為:

(10)

式中:qi(t)(t=1,2,…,n)代表機械臂第i個關(guān)節(jié)在t時刻的角度,n為機械臂的關(guān)節(jié)數(shù),Qimax、Wimax、Aimax分別為機械臂每個關(guān)節(jié)的最大角度,最大角速度和最大角加速度。

為了棄除不滿足約束條件的解并評價迭代過程中解的優(yōu)劣程度從而更新最優(yōu)解,本文使用如下的適應(yīng)度函數(shù):

(11)

式中:x(t)表示解個體(t1,t2,t3),inf表示無窮大。

改進SCA算法針對六軸機械臂時間最優(yōu)軌跡的優(yōu)化步驟為:

步驟1:設(shè)置算法中的基本參數(shù):種群數(shù)量N,最大迭代次數(shù)T,Lévy飛行的指數(shù)參數(shù)β,比重常參數(shù)c。設(shè)置機械臂各個關(guān)節(jié)3段軌跡的運行時間(t1,t2,t3)的上下限,隨機初始化種群。設(shè)置機械臂各關(guān)節(jié)角度,角速度和角加速度的限制;

步驟2:求解插值多項式,判斷種群中每個個體解是否超出約束限制,并計算適應(yīng)度值;

步驟3:根據(jù)適應(yīng)度更新當目前未知的最優(yōu)解,并記錄其對應(yīng)位置;

步驟4:更新算法參數(shù)r1,r2,r3,r4,λ,根據(jù)式(5)計算下一次迭代的種群中每個個體的位置;

步驟5:判斷新個體位置是否超出所設(shè)置的范圍,對超出范圍的個體重新進行初始化;

步驟6:判斷是否達到最大迭代次數(shù),若已達到,則結(jié)束迭代;若尚未達到,則返回步驟2繼續(xù)迭代;

步驟7:程序完畢,得到各關(guān)節(jié)的時間最優(yōu)軌跡,選取每段軌跡中各關(guān)節(jié)所需完成時間的最大值作為所有關(guān)節(jié)的執(zhí)行時間,使所有關(guān)節(jié)都能同步完成每一段軌跡,以保證機械臂末端能夠到達每一個所設(shè)置的任務(wù)點。

4 仿真實驗與結(jié)果分析

為了驗證所提出的算法的正確性和有效性,本文以UR5e機械臂為實驗對象進行了3-5-3多項式插值并使用上述改進SCA算法對所生成的各關(guān)節(jié)軌跡進行時間最短優(yōu)化。為了模擬實際任務(wù)需要,在任務(wù)空間中設(shè)置了起點X1、第一個中間點X2、第二個中間點X3和終點X4的位置坐標分別為(400,300,300)、(225,175,200)、(-125,-75,0)和(-300,-200,-100),姿態(tài)固定不變,并通過逆運動學(xué)得到各關(guān)節(jié)在以上4個插值點的角度,各關(guān)節(jié)經(jīng)過的角度信息如表2所示。

表2 各關(guān)節(jié)經(jīng)過的角度信息 (rad)

設(shè)置種群數(shù)量N為40,最大迭代次數(shù)T為500,(t1,t2,t3)在[0,3]的范圍內(nèi)初始化。設(shè)置機械臂關(guān)節(jié)物理限制最大角度Qimax=2π,最大角速度Wimax=3 rad/s,最大角加速度Aimax=10 rad/s2。在改進SCA算法中,Lévy飛行的指數(shù)參數(shù)β設(shè)置為1.5,取比重參數(shù)c為0.3。優(yōu)化過程中適應(yīng)度值的變化曲線如圖2所示,各關(guān)節(jié)優(yōu)化后的每段軌跡的運行時間如表3所示。優(yōu)化后機械臂各關(guān)節(jié)的角度,角速度和角加速度曲線如圖3所示。

表3 各關(guān)節(jié)優(yōu)化后的每段軌跡的運行時間 (s)

圖2 改進SCA的各關(guān)節(jié)迭代圖

根據(jù)以上實驗結(jié)果,可以看出改進SCA算法能夠在滿足所設(shè)置的機械臂各關(guān)節(jié)約束的條件下,保證所生成軌跡對應(yīng)的各關(guān)節(jié)角度、角速度、角加速度曲線平滑無突變,完成了機械臂的時間最優(yōu)軌跡規(guī)劃的任務(wù),盡可能縮短了機械臂完成指定任務(wù)所用時間,提高了工作效率,驗證了本文所提出的改進SCA算法的有效性。

為了進一步驗證改進SCA算法的優(yōu)越性,針對上述機械臂的時間最優(yōu)軌跡規(guī)劃問題,依次采用傳統(tǒng)正余弦算法(SCA)和3個較優(yōu)的智能優(yōu)化算法布谷鳥算法(CS),烏鴉算法(CSA)和粒子群算法(PSO)進行優(yōu)化,與本文所提出的改進SCA算法進行實驗對照,各關(guān)節(jié)優(yōu)化迭代過程中的適應(yīng)度變化曲線如圖4所示,完成優(yōu)化所耗時間如表4所示。所有實驗均在lntel(R) Core(TM) i5-10400F CPU @2.90 GHz 16 GB RAM計算機平臺上的MATLAB 2020b上進行。

表4 各算法完成優(yōu)化所耗時間 (s)

圖4 各關(guān)節(jié)優(yōu)化迭代圖

從圖4中可以看出,相較于傳統(tǒng)SCA算法改進SCA算法在6個關(guān)節(jié)上得到的最終軌跡運行總時間更短,具有更強的擺脫局部最優(yōu)的能力和更高的收斂精度。此外,改進SCA算法在關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3、關(guān)節(jié)5和關(guān)節(jié)6上的較上述傳統(tǒng)4個優(yōu)化算法都能在更少的迭代次數(shù)下收斂到相對最短時間,表現(xiàn)出更快的收斂速度。

從表4中可以看出,雖然改進SCA相較于傳統(tǒng)SCA由于額外運算的引入增加了部分優(yōu)化耗時,但是相較于其他算法,仍表現(xiàn)出出色的優(yōu)化運行速度。

綜合以上實驗結(jié)果,在六軸機械臂的時間最優(yōu)軌跡規(guī)劃問題中,與其他傳統(tǒng)優(yōu)化算法相比,SCA算法運算量小,運行速度快,而改進SCA算法少量犧牲了傳統(tǒng)SCA算法的優(yōu)化速度,但使得算法跳出局部最優(yōu)的能力和收斂精度得到了顯著提高,能夠快速得到更優(yōu)的機械臂運行軌跡。

5 結(jié)論

針對關(guān)節(jié)空間中六軸機械臂時間最優(yōu)軌跡規(guī)劃的問題,在3-5-3多項式軌跡插值的基礎(chǔ)上,提出了一種改進的正余弦算法來對所生成軌跡進行時間優(yōu)化。實驗表明改進正余弦算法與傳統(tǒng)正余弦算法相比,具有更強的跳出局部最優(yōu)的能力和更高的收斂精度。在與其他優(yōu)化算法的對比中,改進正余弦算法在保持較高收斂精度的同時表現(xiàn)出更快的收斂速度。經(jīng)過改進正余弦算法優(yōu)化后的機械臂軌跡,關(guān)節(jié)角度,角速度和角加速度變化平滑且均滿足所設(shè)置的約束條件,縮短了機械臂完成任務(wù)軌跡所用的時間,提高了工作效率。