基于粒子群優(yōu)化算法的冗余機(jī)械臂運(yùn)動(dòng)學(xué)逆解

石建平

(貴陽(yáng)學(xué)院 電子與通信工程學(xué)院，貴州 貴陽(yáng) 550005)

機(jī)器人的逆運(yùn)動(dòng)學(xué)求解是指已知末端執(zhí)行器相對(duì)于參考坐標(biāo)系的位置和姿態(tài)，求機(jī)器人末端執(zhí)行器到達(dá)給定位置和姿態(tài)所需的各關(guān)節(jié)變量值。機(jī)器人運(yùn)動(dòng)學(xué)逆解運(yùn)算本質(zhì)上是一個(gè)復(fù)雜非線性方程組的求解，該問(wèn)題是機(jī)器人學(xué)領(lǐng)域的一個(gè)技術(shù)難題，同時(shí)也是研究機(jī)器人動(dòng)態(tài)特性分析、軌跡規(guī)劃和運(yùn)動(dòng)控制的基礎(chǔ)與前提。求解運(yùn)動(dòng)學(xué)逆解的傳統(tǒng)方法有封閉解法和數(shù)值解法。[1]其中封閉解法可根據(jù)公式或幾何構(gòu)型直接推導(dǎo)出關(guān)節(jié)變量表達(dá)式，該方法計(jì)算速度快、精度高，且能求出所有可能逆解，但封閉解法只能求解結(jié)構(gòu)構(gòu)型滿足Pieper準(zhǔn)則[1]的簡(jiǎn)單機(jī)器人的逆運(yùn)動(dòng)學(xué)問(wèn)題，其通用性不強(qiáng)。數(shù)值解法是冗余機(jī)器人逆運(yùn)動(dòng)學(xué)求解的主要方法，典型的數(shù)值解法有基于雅克比矩陣的迭代法，迭代法需要對(duì)雅可比矩陣進(jìn)行求逆運(yùn)算，其實(shí)現(xiàn)過(guò)程繁雜，不易編程實(shí)現(xiàn)且存在奇異性問(wèn)題。

近年來(lái)，粒子群優(yōu)化算法[2-7]、差分進(jìn)化算法[8,9]、遺傳算法[10,11]、類電磁機(jī)制優(yōu)化算法[12]、螢火蟲群算法[13]、人工蜂群算法[14]等智能優(yōu)化算法被應(yīng)用于機(jī)器人的逆運(yùn)動(dòng)學(xué)求解，研究進(jìn)展其情況較好?；谥悄軆?yōu)化算法的機(jī)器人運(yùn)動(dòng)學(xué)逆解求解方法不受機(jī)器人機(jī)構(gòu)構(gòu)型的限制，具有很強(qiáng)的通用性，很好克服了傳統(tǒng)求解方法的不足，為解決機(jī)器人逆運(yùn)動(dòng)學(xué)問(wèn)題提供了有效的解決方案。粒子群優(yōu)化算法(Particle Swarm Optimization，PSO)[15]是Kennedy和Eberhart受鳥群群體覓食行為的啟發(fā)而提出的一種群智能優(yōu)化算法，該算法由于結(jié)構(gòu)簡(jiǎn)單、控制參數(shù)少、易于實(shí)現(xiàn)等優(yōu)點(diǎn)而受到廣泛關(guān)注與研究。為改善粒子群優(yōu)化算法的優(yōu)化性能，文獻(xiàn)[16]提出了一種綜合改進(jìn)的粒子群優(yōu)化算法(Improved Particle Swarm Optimization，IPSO)，該算法已經(jīng)成功用于解決混沌系統(tǒng)的參數(shù)估計(jì)問(wèn)題。本文以平面冗余機(jī)械臂為研究對(duì)象，將IPSO算法用于機(jī)械臂運(yùn)動(dòng)學(xué)逆解的求解，進(jìn)一步拓寬該算法的工程應(yīng)用范圍。

1 運(yùn)動(dòng)學(xué)方程與目標(biāo)函數(shù)

1.1 運(yùn)動(dòng)學(xué)方程

本文采用的平面冗余機(jī)械臂如圖1所示，以機(jī)械臂的基底為參考坐標(biāo)系，則有如下的運(yùn)動(dòng)學(xué)方程:

圖1 冗余機(jī)械臂

(1)

1.2 優(yōu)化目標(biāo)函數(shù)

由于從機(jī)械臂末端執(zhí)行器的工作空間到關(guān)節(jié)空間的變換是一對(duì)多的映射關(guān)系，同一個(gè)末端執(zhí)行器的位姿將有多個(gè)逆解與之對(duì)應(yīng)。于是，如何從多解中選擇一個(gè)比較適合的逆解成為不可回避的問(wèn)題。為解決運(yùn)動(dòng)學(xué)逆解的選擇問(wèn)題，除了機(jī)械臂末端的位姿要求，通常還需要考慮機(jī)械臂從初始位置運(yùn)動(dòng)到目標(biāo)位置的能量消耗、柔順性和安全性等指標(biāo)，這就涉及運(yùn)動(dòng)學(xué)逆解的約束優(yōu)化求解。本文在機(jī)械臂的逆解運(yùn)算中，將結(jié)合末端執(zhí)行器的位置誤差和機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)的能量消耗兩個(gè)性能指標(biāo)來(lái)指導(dǎo)逆解的選擇。

(2)

(3)

式中，θ=[θ1,θ2,...,θ6]。

實(shí)際應(yīng)用中，追求機(jī)械臂末端執(zhí)行器的位置精度為零是不現(xiàn)實(shí)也是不必要的，合理的選擇應(yīng)該是在位置精度滿足實(shí)際需求的前提下使其運(yùn)行成本更低。因此，以位置誤差滿足指定精度要求為約束條件，以機(jī)械臂從初始位置運(yùn)動(dòng)到目標(biāo)位置過(guò)程中的能耗最小為優(yōu)化目標(biāo)。于是，可將機(jī)械臂的逆運(yùn)動(dòng)學(xué)問(wèn)題轉(zhuǎn)換成式(4)的最小約束優(yōu)化問(wèn)題來(lái)解決。

(4)

s.t.g(θ)=Δ-Δmax≤0

式中，Δmax為最大位置誤差。

用罰函數(shù)法將上述約束優(yōu)化問(wèn)題(4)轉(zhuǎn)換為無(wú)約束優(yōu)化問(wèn)題進(jìn)行處理，采用的約束處理方法如下:

minf′(θ)=min(f(θ)+φ(θ))

(5)

(6)

式中，f(θ)為原目標(biāo)函數(shù)(即能耗最小優(yōu)化目標(biāo)函數(shù))，φ(θ)為罰函數(shù)，f′(θ)為使用罰函數(shù)φ(θ)修正后的目標(biāo)函數(shù)，即懲罰適應(yīng)度函數(shù)，α為懲罰系數(shù)，其余參數(shù)與前述相同。

根據(jù)式(6)，機(jī)械臂末端執(zhí)行器的位置誤差越大，則對(duì)原目標(biāo)函數(shù)的懲罰力度越大。

2 用IPSO求解機(jī)械臂運(yùn)動(dòng)學(xué)逆解

在用IPSO算法求解機(jī)械臂逆運(yùn)動(dòng)學(xué)問(wèn)題中，粒子i由上述的6個(gè)關(guān)節(jié)角變量構(gòu)成，即粒子i的位置向量表示為θi=[θi1,θi2,θi3,θi4,θi5,θi6]，其中θij(j=1,2,...,6)為各關(guān)節(jié)角變量，每個(gè)位置向量代表逆運(yùn)動(dòng)學(xué)問(wèn)題的一個(gè)候選逆解；粒子i的速度向量用vi=[vi1,vi2,vi3,vi4,vi5,vi6]表示。根據(jù)參考文獻(xiàn)[16]，用IPSO算法求解機(jī)械臂運(yùn)動(dòng)學(xué)逆解的詳細(xì)步驟如下：

Step1參數(shù)設(shè)置與初始化，設(shè)置粒子位置與飛行速度的范圍、種群規(guī)模m、最大進(jìn)化迭代次數(shù)kmax、用式(7)-(9)對(duì)種群進(jìn)行混沌初始化。

(7)

(8)

(9)

式中，uθ(j)、lθ(j)分別為粒子的第j維位置分量的上邊界值與下邊界值；uv(j)、lv(j)分別為粒子的第j維飛行速度分量的上邊界值與下邊界值；xn、yn是分別由式(8)和式(9)產(chǎn)生的混沌序列，其迭代初值要求不為零。

Step2 計(jì)算粒子i(i=1,2,...,m)的適應(yīng)值f′(θi)，將粒子的初始位置設(shè)置該粒子i的歷史最佳位置，將種群的初始最佳位置設(shè)置為種群全局歷史最佳位置；設(shè)置i=1、k=1。

Step3 按照式(10)、式(11)更新粒子i的各速度分量和位置分量，用式(7)-(9)對(duì)粒子i的越界分量進(jìn)行處理。

(10)

(11)

式中，ω為慣性權(quán)重，且ω∈[ωmin,ωmax]；c1和c2為學(xué)習(xí)因子；r1、r2和r3為[0,1]內(nèi)均勻分布的隨機(jī)數(shù)；pij(k)為粒子i所經(jīng)歷的歷史最佳位置向量pi的第j維分量；gj(k)為整個(gè)粒子種群所經(jīng)歷的全局歷史最佳位置向量g的第j維分量；k為當(dāng)前進(jìn)化代數(shù)，kmax為算法最大迭代次數(shù)。

Step5 若i=m，執(zhí)行Step6；否則i=i+1，返回Step3繼續(xù)更新下一個(gè)粒子的速度和位置。

Step6 對(duì)粒子種群執(zhí)行差分變異進(jìn)化操作，設(shè)置i=1。

Step6.1按照式(12)得到粒子i對(duì)應(yīng)的變異向量u(k)，參照式(7)-(9)對(duì)越界現(xiàn)象進(jìn)行處理。

u(k)=g(k)+F(g(k)-pi(k))

(12)

式中，F(xiàn)為[0，1]內(nèi)的縮放因子。

Step6.2 若f′(u)≤f′(pi)，則pi=u；若f′(u)≤f′(g)，則g=u。

Step6.3 若i=m，執(zhí)行Step7；否則i=i+1，返回Step6.1繼續(xù)執(zhí)行差分變異進(jìn)化操作。

Step7 從當(dāng)前種群中選擇適應(yīng)值最差的粒子i進(jìn)行局部變尺度深度搜索，設(shè)置k′=1。

Step7.1 根據(jù)式(13)更新適應(yīng)值最差粒子i的位置。

(13)

Step7.2若f′(θi)≤f′(pi)，則pi=θi；若f′(θi)≤f′(g)，則g=θi。

Step8 若k=kmax，則算法尋優(yōu)過(guò)程結(jié)束并輸出尋優(yōu)結(jié)果；否則k=k+1、i=1，返回步驟Step3繼續(xù)下一代的迭代操作。

3 仿真實(shí)驗(yàn)與結(jié)果分析

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

為檢驗(yàn)IPSO算法求解機(jī)械臂運(yùn)動(dòng)學(xué)逆解的可行性與有效性，將IPSO算法的求解結(jié)果與PSO[15]、PSOTD[17]、PSOd[18]算法的求解結(jié)果進(jìn)行對(duì)比分析，所有算法采用Matlab R2013a進(jìn)行編程，計(jì)算機(jī)配置為：Intel Core(TM)i7—7700、3.6GHz、16GB內(nèi)存、Windows 10操作系統(tǒng)。

3.2 實(shí)驗(yàn)結(jié)果與分析

將各算法分別獨(dú)立連續(xù)進(jìn)行100次逆解運(yùn)算，各算法尋優(yōu)結(jié)果的最優(yōu)適應(yīng)值(Best)、平均適應(yīng)值(Mean)、最差適應(yīng)值(Worst)及標(biāo)準(zhǔn)差(Std.)分別如表1所示，各算法對(duì)應(yīng)的最優(yōu)逆解結(jié)果如表2所示，各算法對(duì)應(yīng)的最差逆解結(jié)果如表3所示，各算法的平均適應(yīng)值收斂曲線如圖2所示。

表1 不同算法的逆運(yùn)動(dòng)學(xué)優(yōu)化求解結(jié)果

表2 不同算法的最優(yōu)運(yùn)動(dòng)學(xué)逆解

表3 不同算法的最差運(yùn)動(dòng)學(xué)逆解

圖2 不同算法求解機(jī)械臂運(yùn)動(dòng)學(xué)逆解的平均收斂曲線

根據(jù)表1的尋優(yōu)結(jié)果可知，IPSO算法的最優(yōu)適應(yīng)值、平均適應(yīng)值、最差適應(yīng)值及標(biāo)準(zhǔn)差都明顯優(yōu)于PSO、PSOTD及PSOd算法，IPSO算法能夠在滿足給定約束條件的前提下對(duì)逆運(yùn)動(dòng)學(xué)問(wèn)題進(jìn)行尋優(yōu)求解；根據(jù)表2，PSOTD算法的最優(yōu)逆解結(jié)果違反了給定的約束條件，而PSO、PSOd及IPSO算法的最優(yōu)逆解結(jié)果都能夠滿足給定的位置精度要求；根據(jù)表3的最差逆解結(jié)果可知，除了IPSO算法外，其余算法的尋優(yōu)結(jié)果都違反了約束條件；在圖2中，IPSO算法的平均適應(yīng)值收斂曲線下降速度最快，并獲得了最低的下降位置，說(shuō)明與其他算法相比， IPSO算法具有收斂速度快、收斂精度高的優(yōu)勢(shì)。

綜上所述，在機(jī)械臂運(yùn)動(dòng)學(xué)逆解的約束優(yōu)化求解中，IPSO算法在4個(gè)算法中獲得了最好的求解質(zhì)量，可用于有效求解機(jī)器人的逆運(yùn)動(dòng)學(xué)問(wèn)題。

4 結(jié)論

本文將機(jī)器人的逆運(yùn)動(dòng)學(xué)問(wèn)題轉(zhuǎn)換成一個(gè)等效的最小約束優(yōu)化問(wèn)題，并利用粒子群優(yōu)化算法來(lái)對(duì)該問(wèn)題進(jìn)行有效求解。這種基于群智能優(yōu)化算法的機(jī)器人運(yùn)動(dòng)學(xué)逆解求解方法具有很強(qiáng)的通用性，對(duì)機(jī)器人的機(jī)構(gòu)構(gòu)型沒(méi)有特殊要求，可用于求解一般機(jī)器人的逆運(yùn)動(dòng)學(xué)問(wèn)題。以平面冗余機(jī)械臂為例，對(duì)PSO、PSOTD、PSOd及IPSO算法在運(yùn)動(dòng)學(xué)逆解運(yùn)算中的有效性展開(kāi)試驗(yàn)對(duì)比分析，仿真結(jié)果表明：與PSO、PSOTD及PSOd算法相比，IPSO算法獲得了最好的求解質(zhì)量，其運(yùn)動(dòng)學(xué)逆解對(duì)應(yīng)的機(jī)械臂末端執(zhí)行器的位置精度均符合約束要求，是一種有效的逆運(yùn)動(dòng)學(xué)問(wèn)題求解方法。