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

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

1 研究背景

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

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

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

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

2 正向運動學分析

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

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

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

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

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

機器人正向運動學方程為

式中：

3 改進PSO 算法

3.1 經典PSO 算法

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

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

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

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

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

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

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

c1、c2均為學習因子；

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

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

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

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

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

σ為步長；

cσ為pσ的更新學習率；

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

μ為中間變量；

w為慣性權重；

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

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

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

引入如下慣性權重粒子速度更新公式：

3.3 收縮學習因子

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

其中，c1+c2＞4。

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

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

4 逆運動學求解

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

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

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

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

4.2 改進PSO 求解機械臂運動學逆解

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

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

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

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

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

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

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

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

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

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

5 仿真算例

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

5.1 參數(shù)設置

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

遺傳算法種群大小Size= 500，交叉概率Pc=0.9，自適應變異概率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；各關節(jié)角范圍為[-π, π]；算法終止條件均設為進化代數(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 機器人進行仿真比較，設置初始角(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結果表明，經典PSO 算法和改進PSO 算法的求解精度和速度都優(yōu)于遺傳算法的，且改進PSO 算法比經典PSO 算法的平均收斂精度更高。

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

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

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

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

對REBot-V-6R 機器人進行仿真比較，設置初始角(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結果表明，經典PSO 算法和改進PSO 算法的求解精度和求解速度都優(yōu)于遺傳算法的，且進化曲線表明，改進PSO 算法比經典PSO 算法的全局搜索能力明顯增強；收斂速度更快，魯棒性較高。

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

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

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

6 結語

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