基于改進PSO算法的6R機器人逆運動學(xué)分析

李西宸，孫 曉，李文杰，曾 成

1 研究背景

工業(yè)機器人是現(xiàn)代制造業(yè)中一種重要的機電一體化設(shè)備，已被廣泛地應(yīng)用于電子、物流、化工等各工業(yè)領(lǐng)域中。其中，6R 機器人因具有工作空間大、運動靈活、非冗余等特點，成為機器人應(yīng)用和研究的首要目標(biāo)[1]。逆運動學(xué)是已知機械臂末端位置，求解每個關(guān)節(jié)的角度，其解有多個，其方程組具有高維、非線性的特點，求解過程復(fù)雜且不易求出正確解[2]。逆解問題是機器人研究的重點問題之一，是機器人軌跡規(guī)劃與運動控制的基礎(chǔ)。很多學(xué)者在該領(lǐng)域進行了研究，且提出了許多理論與方法，如解析法[3]、幾何法[4]、智能算法[1]等。

機器人逆運動學(xué)的多解雖然能夠通過解析法和幾何法獲得，但是前提是其必須滿足特殊的結(jié)構(gòu)。隨著機器人結(jié)構(gòu)的復(fù)雜化和計算機技術(shù)發(fā)展的突飛猛進，許多學(xué)者將智能優(yōu)化方法應(yīng)用于機器人逆運動學(xué)求解問題中，其主要思路是將機器人運動學(xué)方程轉(zhuǎn)化為優(yōu)化控制問題加以解決[5]。此類智能算法主要包括遺傳算法、粒子群優(yōu)化算法（particle swarm optimization，PSO），以及退火算法等[6]。林陽等[7]針對6R 機器人，提出了一種基于多種群遺傳算法的逆運動學(xué)求解方法。與封閉解法和數(shù)值解法相比，多種群遺傳算法結(jié)構(gòu)簡單，無需進行矩陣逆運算以及三角函數(shù)運算，可以精確地解決任意自由度的一般機器人逆運動學(xué)問題。多種群遺傳算法雖然解決了傳統(tǒng)單種群遺傳算法的局部收斂和“早熟”等問題，提高了系統(tǒng)的收斂速度，但是在其他方面，如精度上，仍然存在較大的問題。

國內(nèi)外已有相關(guān)研究中，主要將全局學(xué)習(xí)因子和局部學(xué)習(xí)因子聯(lián)合起來，通過調(diào)整比例系數(shù)和全局學(xué)習(xí)因子的比例以獲得更快的收斂速度；或以“目標(biāo)點位姿誤差最小”為目標(biāo)函數(shù)，各關(guān)節(jié)轉(zhuǎn)角極限為約束條件，利用PSO 算法，求解機器人的最優(yōu)逆解。雖然PSO 算法具有操作簡單、參數(shù)較少、收斂速度較快等特點，在函數(shù)尋優(yōu)、圖像處理、大地測量等眾多領(lǐng)域得到了廣泛的應(yīng)用，但是在求解逆向運動學(xué)方面，其仿真實驗中仍然存在不穩(wěn)定和局部最優(yōu)與種群單一的問題[8]。

為解決傳統(tǒng)PSO 算法在求解逆向運動學(xué)中存在的不穩(wěn)定問題和易陷入局部最優(yōu)與種群單一的問題，本文提出一種改進的PSO 算法。該算法中引入動態(tài)權(quán)重因子，即動態(tài)權(quán)重調(diào)整因子結(jié)合CMA-ES（covariance matrix adaptation evolutionary strategies）算法[9]的步長更新方法，以平衡全局搜索能力和局部搜索能力，防止陷入局部最優(yōu)，并且提高種群多樣性；同時，引入收縮學(xué)習(xí)因子，以提高算法的搜索能力，得到高質(zhì)量的解，防止在迭代過程中陷入局部最優(yōu)。并以REBot-V-6R 機器人為例，首先用螺旋理論進行正向運動學(xué)建模，將機器人逆運動學(xué)求解的問題轉(zhuǎn)換為改進PSO 算法的尋優(yōu)問題；其次驗證改進PSO 算法求逆運動學(xué)解的可行性；最后分別對REBot-V-6R 機器人的位置誤差、姿態(tài)誤差進行仿真，以驗證所提算法的有效性。

2 正向運動學(xué)分析

本研究針對實驗室引進的REBot-V-6R 機器人進行仿真驗證。圖1 所示為REBot-V-6R 機器人的實物圖與其結(jié)構(gòu)簡圖。

圖1 中，{S}為慣性坐標(biāo)系，{T}為工具坐標(biāo)系，且{T}的原點與關(guān)節(jié)6 的原點重合。x、y、z為坐標(biāo)軸，d為桿長，θ為旋轉(zhuǎn)角度。

利用螺旋理論對機器人進行正向運動學(xué)建模，表1 為各關(guān)節(jié)螺旋軸的參數(shù)，其中i為桿的序號；ωi為旋轉(zhuǎn)軸的方向矢量；ri為距離；d1～d6為桿長。

表1 REBot-V-6R 的各關(guān)節(jié)螺旋軸參數(shù)Table 1 Spiral axis of each joint of REBOt-V-6R

工具坐標(biāo)系建立在機器人的第5 關(guān)節(jié)處，由圖1可知，機器人處于0 位時，工具坐標(biāo)系的初始位姿為

機器人正向運動學(xué)方程為

式中：

3 改進PSO 算法

3.1 經(jīng)典PSO 算法

PSO 算法，是通過模擬各種鳥類群體之間的其他鳥類個體進行相互覓食競爭活動的行為而進行設(shè)計和研究發(fā)現(xiàn)的，一種非常智能的群優(yōu)化粒子算法分析工具，可以通過它搜索各群體中的鳥類個體之間的相互協(xié)作或與其他各群體之間的相互競爭的活動關(guān)系，還可以幫助實現(xiàn)在復(fù)雜空間內(nèi)得到最優(yōu)化的解。

PSO 算法的收斂速度較快，易于實現(xiàn)，并且僅有少量的參數(shù)值需要進行調(diào)整，其中，具有較好的應(yīng)用前景的技術(shù)領(lǐng)域，主要包括多目標(biāo)性問題的優(yōu)化、歸納、模型識別、生物體系建模、信號處理、決策、仿真及模擬等。粒子群優(yōu)化算法起步于一種由粒子和粒子間形成的社會系統(tǒng)，該社會系統(tǒng)的研究主要側(cè)重于簡單的由個體所構(gòu)建的群落和環(huán)境之間的相互作用，以及各類人與個體之間的相互作用和行為。

PSO 的相關(guān)技術(shù)理論發(fā)展史尚短，在其技術(shù)理論的一定基礎(chǔ)和實際應(yīng)用推廣等方面仍然存在一些亟待解決的問題。如當(dāng)PSO 被應(yīng)用于高維復(fù)雜問題的優(yōu)化時，它往往會過早收斂，也就是說，在找到全局最優(yōu)解之前，種群已經(jīng)聚集到一個停滯點，這些早熟的收斂點可能是局部極小點，也可能是局部極小點附近的點[10]。因而，對于PSO 算法的早熟收斂行為的深入研究，可以在一定程度上為PSO 算法的進一步完善與發(fā)展奠定理論基礎(chǔ)。

PSO 算法是隨機地產(chǎn)生一個原始的種群，并且給這個種群中每個粒子都提供了一個隨機的位置和隨機的速度，在粒子的飛行過程中，其位置和運動速度可以通過自身和其他同伴的飛行經(jīng)驗來進行動態(tài)調(diào)整，最終使得全體種群粒子都飛向目標(biāo)位置[11]。

設(shè)xi=[xi1,xi2,…,xin]為粒子i的當(dāng)前位置；vi=[vi1,vi2,…,vin]為粒子i的當(dāng)前飛行速度；pi=[pi1,pi2,…,pin]為粒子i所經(jīng)歷的最好位置，也稱個體最好位置；pg=[pg1,pg2,…,pgn]為所有粒子經(jīng)歷的最好位置，也稱全局最優(yōu)位置。

PSO 算法的速度和位置更新公式如下：

式（4）（5）中：t為當(dāng)前迭代次數(shù)；

c1、c2均為學(xué)習(xí)因子；

r1、r2為2 個相互獨立且均勻分布的(0, 1)間的隨機數(shù)。

經(jīng)典PSO 算法主要采用“速度-位置”搜索模型來幫助解決優(yōu)化問題。每一個粒子都可以被認為是解空間里面的潛在解，并且可以通過對解的適應(yīng)值進行分析來判斷這個解的效果[12]。經(jīng)典PSO 算法在初期就具有較快的收斂特性，但隨著粒子進行了同一化，往往很容易達到局部最優(yōu)。

3.2 動態(tài)權(quán)重因子

慣性權(quán)重調(diào)整策略大多采用線性或者非線性遞減模式，這種單調(diào)的演變模式存在以下不足：首先，若算法的迭代次數(shù)很大，則每次迭代所引起的慣性權(quán)重改變量很小，以致慣性權(quán)重調(diào)節(jié)功能的發(fā)揮效果不明顯；其次，這種單一的調(diào)節(jié)模式減少了種群多樣性。因此，本研究引入如下結(jié)合CMA-ES 算法中步長更新方法的動態(tài)權(quán)重調(diào)整因子，對慣性權(quán)重進行動態(tài)調(diào)整：

式（6）～（9）中：E||N(0,1)||為歸一化進化路徑在隨機選擇下的期望長度，N為正態(tài)分布；

σ為步長；

cσ為pσ的更新學(xué)習(xí)率；

dσ為接近于1 的阻尼系數(shù)；

μ為中間變量；

w為慣性權(quán)重；

rand為(0, 1)隨機數(shù)；

r3為服從正態(tài)分布的(0, 1)隨機數(shù)；

通過式（6）～（9）動態(tài)更新慣性權(quán)重，動態(tài)平衡全局搜索和局部搜索能力，有效防止其陷入局部最優(yōu)，并提高種群多樣性。

引入如下慣性權(quán)重粒子速度更新公式：

3.3 收縮學(xué)習(xí)因子

PSO 算法能有效地解決一些簡單的和小規(guī)模的約束優(yōu)化問題，但是在面對復(fù)雜和多目標(biāo)的系統(tǒng)優(yōu)化時，很容易產(chǎn)生“趨同性”問題，從而使得種群多樣性損失過快，最終導(dǎo)致算法過早收斂和未能搜尋到最優(yōu)值。對此，可以通過設(shè)定合適的收縮因子，提高算法的搜索能力，加快收斂速度。為了有效搜索不同區(qū)域，且防止在迭代過程中陷入局部最優(yōu)，引入如下收縮學(xué)習(xí)因子。

其中，c1+c2＞4。

這種改進算法經(jīng)實驗驗證后確定，當(dāng)公式中的參數(shù)取c1=2、c2=2.1 時，收縮因子效果較好，能得到優(yōu)質(zhì)的解，且能夠滿足收斂速度與精度。

引入收縮學(xué)習(xí)因子后，粒子每次迭代按照式（5）（11）（12）來改變自身的位置和速度：

4 逆運動學(xué)求解

4.1 適應(yīng)度函數(shù)的建立

機器人手腕的位置可以由前3 個關(guān)節(jié)確定，其最終姿態(tài)由后3 個關(guān)節(jié)確定，所以采用臂腕分離的方式建立適應(yīng)度函數(shù)[2]：

式（13）（14）中：fitness1為目標(biāo)位置pdes與實際位置pcur之間的誤差；

fitness2為目標(biāo)姿態(tài)xdes與實際姿態(tài)xcur之間的誤差。

4.2 改進PSO 求解機械臂運動學(xué)逆解

在用改進PSO算法求解機械臂逆運動學(xué)過程中，粒子i由上述6 個關(guān)節(jié)角變量構(gòu)成，即粒子i的位置向量表示為，每個位置向量代表逆運動學(xué)問題的一個候選解；粒子i的速度向量用表示。

改進PSO 算法求機械臂運動學(xué)逆解流程圖如圖2 所示。

圖2 改進PSO 算法求解流程圖Fig.2 Flow chart of improved PSO algorithm for a solution

改進的PSO 算法求解機械臂運動學(xué)逆解的詳細步驟描述如下。

步驟1 初始化粒子的位置和速度，設(shè)置粒子位置與飛行速度的范圍、種群規(guī)模m、最大進化迭代次數(shù)。

步驟2 通過適應(yīng)度值計算，得到個體最優(yōu)pb和全局最優(yōu)pg；計算粒子i(i= 1, 2,…,m)的適應(yīng)度值f，且將粒子的初始位置設(shè)置為該粒子i的歷史最佳位置，將種群的初始最佳位置設(shè)置為種群全局歷史最佳位置。

步驟3 進行迭代循環(huán)，采用動態(tài)權(quán)重調(diào)整因子結(jié)合CMA-ES 算法步長更新方法，通過公式（6）～（9）更新權(quán)重w。

步驟4 引入收縮學(xué)習(xí)因子，以提高算法搜索能力和得到高質(zhì)量解，防止迭代過程中陷入局部最優(yōu)；粒子的位置和速度按公式（5）（11）（12）更新。

步驟5 計算粒子的適應(yīng)度函數(shù)值f，更新個體最優(yōu)pb和全局最優(yōu)pg。

步驟6 未達到終止條件則執(zhí)行步驟2，滿足終止條件則結(jié)束循環(huán)。

5 仿真算例

為檢驗改進PSO 算法在求解REBot-V-6R 機器人逆向運動學(xué)解的可行性與有效性，同時采用遺傳算法、經(jīng)典PSO 算法以及改進PSO 算法，分別對求解REBot-V-6R 機器人的位置和姿態(tài)逆運動學(xué)進行仿真運算與分析。

5.1 參數(shù)設(shè)置

本實驗計算機硬件環(huán)境如下：Intel I5 10400F、2.9 GHz、8 GB 內(nèi)存；軟件環(huán)境為以Matlab R2018b 進行編程，Windows 10 操作系統(tǒng)。

遺傳算法種群大小Size= 500，交叉概率Pc=0.9，自適應(yīng)變異概率Pm= 0.1-[1:1:Size]×0.01/Size，其中Size為種群大??；兩種PSO 算法粒子的種群大小Size=30；wmax=0.9，wmin=0.4；c1=2，c2=2.1；初步步長σ=0.65；各關(guān)節(jié)角范圍為[-π, π]；算法終止條件均設(shè)為進化代數(shù)genmax=100；REBot-V-6R 機器人各桿長d1=100 mm，d2=320 mm，d3=290 mm，d4=121 mm，d5=310 mm，d6=112.5 mm。

5.2 位置誤差仿真

對REBot-V-6R 機器人進行仿真比較，設(shè)置初始角(rad)Q1=[1; -1.5; 2; 1; 2; 3]；Q2=[-1; 1; 2.1;1.5; -2; 3]；末端位置為P1=[76.067 83; 118.468 63;298.079 36]；P2=[21.247 90; -33.091 64; 342.902 31]；將算法獨立計算求解100 次，所得位置誤差進化曲線見圖3。

圖3 位置誤差進化曲線圖Fig.3 Evolution curves of position errors

圖3 所示3 種算法在兩個位置獨立運行100 次的f1結(jié)果表明，經(jīng)典PSO 算法和改進PSO 算法的求解精度和速度都優(yōu)于遺傳算法的，且改進PSO 算法比經(jīng)典PSO 算法的平均收斂精度更高。

將3 種算法在兩個位置分別獨立進行100 次逆解運算，各算法尋優(yōu)結(jié)果中的最大適應(yīng)度值、最小適應(yīng)度值和平均適應(yīng)度值，即位置誤差最大值、最小值與誤差平均值的具體數(shù)據(jù)如表2 所示。

表2 3 種算法在位置1、2 獨立運行100 次的結(jié)果Table 2 Results of 100 independent runs of position 1 and position 2 under three algorithms

分析表2 所示3 種算法在位置1、2 獨立運行100 次的位置誤差值，可以得知，改進PSO 算法和經(jīng)典PSO 算法的總體誤差平均值分別為8.351 70e-12與1.175 68e-11，而遺傳算法的總體誤差平均值為0.023 05。這一結(jié)果表明，經(jīng)典PSO 算法和改進PSO算法的平均求解精度明顯優(yōu)于遺傳算法的，且改進PSO 算法比經(jīng)典PSO 算法的平均收斂精度更高。改進PSO 算法的總體平均最小誤差值和平均最大誤差值分別為1.824 99e-14 和3.514 72e-11，而經(jīng)典PSO算法的總體平均最小誤差值和平均最大誤差值分別為1.542 44e-13 和5.023 12e-11。遺傳算法的總體平均最小誤差值和平均最大誤差值分別為1.698 77e-05和1.041 13。由此可得，利用改進PSO 算法進行機器人逆運動學(xué)求解時，改進PSO 算法的穩(wěn)定性高于經(jīng)典PSO 算法的，且遠高于遺傳算法的。以上結(jié)果表明：PSO 算法在機器人逆運動學(xué)求解時，其求解精度與穩(wěn)定性上都有很大的優(yōu)勢。改進PSO 算法在尋優(yōu)穩(wěn)定性和尋優(yōu)精度上都比經(jīng)典PSO 算法的有所提升。

5.3 姿態(tài)誤差仿真

對REBot-V-6R 機器人進行仿真比較，設(shè)置初始角(rad)Q1=[1; -1.5; 2; 1; 2; 3]；Q2=[-1; 1; 2.1; 1.5; -2;3]；將算法獨立計算求解100 次，所得姿態(tài)誤差進化曲線如圖4 所示。

圖4 姿態(tài)誤差進化曲線圖Fig.4 Evolution curves of rotation errors

圖4 所示3 種算法在兩個姿態(tài)獨立運行100 次的f2結(jié)果表明，經(jīng)典PSO 算法和改進PSO 算法的求解精度和求解速度都優(yōu)于遺傳算法的，且進化曲線表明，改進PSO 算法比經(jīng)典PSO 算法的全局搜索能力明顯增強；收斂速度更快，魯棒性較高。

將3 種算法分別獨立進行100 次逆解運算，各算法尋優(yōu)結(jié)果中的最大適應(yīng)度值，最小適應(yīng)度值和平均適應(yīng)度值，即姿態(tài)誤差最大值、最小值與平均值的具體數(shù)據(jù)如表3 所示。

表3 3 種算法以姿態(tài)1、2 獨立運行100 次的結(jié)果Table 3 Results of 100 independent runs of rotation 1 and rotation 2 under three algorithms

由表3 所示的3 種算法運行100 次的姿態(tài)誤差結(jié)果，可知改進PSO 算法和經(jīng)典PSO 算法的總體平均誤差值分別為4.261 44e-17 與4.442 28e-15，而遺傳算法的總體平均誤差值為1.979 97e-03。這一結(jié)果表明，經(jīng)典PSO 算法和改進PSO 算法的求解精度要優(yōu)于遺傳算法的，且改進PSO 算法的平均收斂精度比經(jīng)典PSO 算法的更高。改進PSO 算法的總體平均最小誤差為1.162 26e-17，經(jīng)典PSO 算法的總體平均最小誤差值為1.396 44e-16，遺傳算法的總體平均最小誤差值為8.682 07e-04。改進PSO 算法和經(jīng)典PSO 算法的總體平均最大誤差值分別為1.997 68e-16與3.261 51e-14，遺傳算法的總體平均最大誤差值為3.021 06e-02。由此可得，利用改進PSO 算法進行機器人逆運動學(xué)求解時，改進PSO 算法的穩(wěn)定性高于經(jīng)典PSO 算法的，且遠高于遺傳算法的。以上結(jié)果表明：改進PSO 算法在尋優(yōu)精度上有了提升，且最小誤差可達到0，比經(jīng)典PSO 算法的求解精度都有很大優(yōu)勢。改進PSO 算法在尋優(yōu)穩(wěn)定性和尋優(yōu)精度上都比經(jīng)典PSO 算法有了提升，且與遺傳算法對比，求解精度和穩(wěn)定性上優(yōu)勢明顯。同時，也進一步證明了改進PSO 算法的可行性。

6 結(jié)語

針對經(jīng)典PSO 算法多次仿真實驗中存在的不穩(wěn)定問題、局部最優(yōu)和種群單一的問題，提出了一種改進的PSO 算法。并以REBot-V-6R 機器人為例，首先利用螺旋理論建立了機器人的正運動學(xué)模型，將機器人的逆運動學(xué)求解問題轉(zhuǎn)換為改進PSO 算法的尋優(yōu)問題；并對機器人的位置誤差、姿態(tài)誤差分別進行仿真，驗證了改進PSO 算法在求逆運動學(xué)解中的可行性。仿真實驗結(jié)果表明，所提出的改進PSO 算法在滿足精度要求的前提下，不僅能避免易陷入局部最優(yōu)解，而且穩(wěn)定性增強，收斂精度顯著提高，系統(tǒng)魯棒性強。