沈孝龍，王吉芳，郭子昇，馬 飛

(北京信息科技大學(xué)機(jī)電工程學(xué)院，北京 100192)

0 引言

近年來，機(jī)械臂已廣泛應(yīng)用于工件表面打磨、拋光和去毛刺等作業(yè)任務(wù)中，尤其是在勞動(dòng)條件惡劣、存在重大安全隱患的葉片、模具及五金用品打磨行業(yè)。此類作業(yè)任務(wù)需要機(jī)械臂與工件進(jìn)行物理接觸才能完成工作且打磨層厚度極小，一般在幾十微米內(nèi)，微小的位置偏差就會(huì)導(dǎo)致產(chǎn)品不合格，甚至損壞打磨設(shè)備，故需要獲得精確的運(yùn)動(dòng)學(xué)逆解，這是機(jī)械臂后續(xù)實(shí)現(xiàn)力位控制的關(guān)鍵之一。合適的逆解對(duì)應(yīng)機(jī)械臂最佳位姿，在使位姿誤差最小時(shí)，還具有出色的第二性能，如最佳柔順性、低能耗和避障性等。

通常，求機(jī)械臂逆運(yùn)動(dòng)學(xué)方法主要有兩種：封閉解法和數(shù)值解法?；邶R次變換矩陣和三角函數(shù)的代數(shù)法，可解所有逆解，但隨關(guān)節(jié)數(shù)增加計(jì)算復(fù)雜度驟升。根據(jù)機(jī)械臂構(gòu)型分析的幾何法，解的精度較高、計(jì)算量小，但方法通用性差。幾何法與代數(shù)法均屬封閉解法，只有當(dāng)機(jī)械臂幾何構(gòu)型滿足Pieper準(zhǔn)則[1]時(shí)才可使用，通常運(yùn)用于具有球形手腕結(jié)構(gòu)的機(jī)器人運(yùn)動(dòng)學(xué)逆解中。目前，求解復(fù)雜機(jī)械臂逆解的主要方法為數(shù)值解法，包括雅可比矩陣法、梯度投影法、加權(quán)最小范數(shù)法等[1]。數(shù)值算法耗費(fèi)計(jì)算量，有累計(jì)誤差，對(duì)迭代初值選擇敏感并且當(dāng)矩陣奇異時(shí)，運(yùn)動(dòng)學(xué)逆解無解。綜上所述，傳統(tǒng)法求機(jī)械臂運(yùn)動(dòng)學(xué)逆解依舊充滿困難。

近年來，基于群智能優(yōu)化的機(jī)器人逆運(yùn)動(dòng)學(xué)分析法為解決逆解問題提供了思路，核心思想是將運(yùn)動(dòng)學(xué)逆解問題轉(zhuǎn)化為最優(yōu)化問題。智能優(yōu)化算法不受機(jī)械臂構(gòu)型限制，求解精度高，方法通用性好且克服許多傳統(tǒng)求解方法的不足，受到眾多學(xué)者的研究。申曉寧等[2]對(duì)冗余機(jī)械手逆解使用多目標(biāo)遺傳算法，設(shè)計(jì)兩種精英策略，加快收斂速度，仿真表明末端工具可精確到達(dá)目標(biāo)位置。羅天洪等[3]為克服冗余機(jī)械臂逆解求解的大計(jì)算量和低精度困難，提出基于時(shí)變螢火蟲群算法的優(yōu)化方法，顯著提升逆解精度與穩(wěn)定性。楊惠珍等[4]針對(duì)逆解存在的多解與通用性差問題，設(shè)計(jì)一種自適應(yīng)小生境遺傳算法，仿真表明算法精度高，收斂快，可求唯一解。馮晨旭等[5]使用改進(jìn)天牛須搜索求4DOF機(jī)械臂逆解，可在短時(shí)間內(nèi)，以較高精度求得全部解。石建平等[6]基于改進(jìn)克隆選擇算法，克服了常規(guī)方法無法有效求解冗余機(jī)械臂逆解的問題。林陽等[7]運(yùn)用多種群遺傳算法處理不適用Pieper準(zhǔn)則的冗余機(jī)械臂逆解問題，有效避免了算法的局部收斂和穩(wěn)定性差的缺點(diǎn)，使收斂精度和速度顯著改善。何兵等[8]設(shè)計(jì)的復(fù)合形差分進(jìn)化算法，以較少計(jì)算開銷即可求出高精度位置逆解且能求得全部解。謝習(xí)華等[9]對(duì)9自由度機(jī)械手使用改進(jìn)差分進(jìn)化算法求解出位姿誤差最小時(shí)的運(yùn)動(dòng)學(xué)逆解。

但現(xiàn)有群智能優(yōu)化算法，基本上只是限制位置誤差最小，姿態(tài)誤差精度低且極少考慮滿足最小位姿解下的各關(guān)節(jié)柔順性與能量消耗。受上述有關(guān)智能優(yōu)化算法的啟發(fā)，在此基礎(chǔ)上提出一種改進(jìn)差分進(jìn)化算法的機(jī)械臂運(yùn)動(dòng)學(xué)逆解方法以提高位姿收斂精度，將算法用于6R機(jī)械臂逆解，與不同改進(jìn)算法進(jìn)行仿真實(shí)驗(yàn)對(duì)比，結(jié)果表明所提算法平均在173代就能獲得高精度位置逆解， 10-7m相對(duì)文獻(xiàn)[9]250代快。同時(shí)，使機(jī)械臂具有最佳柔順性與較低能量消耗。為驗(yàn)證算法通用性又對(duì)冗余機(jī)械臂進(jìn)行逆解實(shí)驗(yàn)，尋優(yōu)成功率100%，相對(duì)文獻(xiàn)[16]32%提升顯著。

1 機(jī)械臂的數(shù)學(xué)模型及代數(shù)法逆解

1.1 正運(yùn)動(dòng)學(xué)模型

仿真實(shí)驗(yàn)主要以庫(kù)卡KR210 L150-2機(jī)械臂的運(yùn)動(dòng)學(xué)逆解為研究對(duì)象，進(jìn)行算法的改進(jìn)研究與驗(yàn)證。機(jī)械臂模型及連桿坐標(biāo)系如圖1所示。

圖1 機(jī)械臂模型及連桿坐標(biāo)系

由D-H法，連桿坐標(biāo)系i-1到i的變換由關(guān)節(jié)角θi、連桿扭角αi、連桿偏距di、連桿長(zhǎng)ai4個(gè)參數(shù)描述，由圖1及產(chǎn)品數(shù)據(jù)手冊(cè)可得表1。

表1 庫(kù)卡KR 210 L150-2改進(jìn)D-H參數(shù)表

相鄰兩連桿的位姿變換過程可用i-1Ti表示，通常由式(1)的齊次變換矩陣表示。

(1)

式中，cαi-1=cos(αi-1);sαi-1=sin(αi-1);cθi=cos(θi);sθi=sin(θi);i=1,2,...n。

機(jī)械臂末端相對(duì)基坐標(biāo)系的位姿矩陣0T6，如式(2)所示。

(2)

式中，R=[n,o,a]為末端工具的姿態(tài)信息；P=(px,py,pz)為末端工具的位置信息。把表1各參數(shù)帶入式(1)和式(2)即可得到各位姿參數(shù)方程。

1.2 基于代數(shù)法求得的逆解

庫(kù)卡KR210 L150-2機(jī)械臂符合Pieper準(zhǔn)則,根據(jù)機(jī)械臂的正運(yùn)動(dòng)學(xué)方程，使用矩陣逆乘的代數(shù)法可以得到8組解。具體計(jì)算過程不在本文論述，下面給出在給定位姿為式(3)時(shí)計(jì)算得到的八組逆解，如表2所示。

(3)

表2 代數(shù)法求得給定位姿的八組逆解

2 適應(yīng)度函數(shù)

由1.2節(jié)知，當(dāng)給定庫(kù)卡機(jī)械臂一組位姿時(shí)，可得8組逆解。若為冗余機(jī)械臂，會(huì)存在無窮多組逆解，通常需要設(shè)定一些約束來求得最優(yōu)解。實(shí)際生產(chǎn)中，在滿足機(jī)械臂末端位置誤差時(shí)，通常還希望機(jī)械臂運(yùn)動(dòng)具有低能耗、柔順性等。

2.1 位姿誤差目標(biāo)函數(shù)

設(shè)機(jī)械臂末端目標(biāo)位姿矩陣為：

(4)

定義位姿誤差目標(biāo)函數(shù)為：

F=ε*|P*-P|+|R*-R|

(5)

2.2 最佳柔順描述

“最佳柔順性”即各關(guān)節(jié)角從初始位姿運(yùn)動(dòng)到目標(biāo)位姿時(shí)，所有關(guān)節(jié)轉(zhuǎn)角之和相對(duì)最小。我們知道機(jī)械臂的大臂比小臂質(zhì)量大，為使運(yùn)動(dòng)耗費(fèi)能量少且平穩(wěn)，應(yīng)多運(yùn)動(dòng)小臂，從而提升臂的工作效率。最佳柔順與能量約束[11]描述如下:

(6)

2.3 適應(yīng)度函數(shù)

將最佳柔順及能量約束與位姿目標(biāo)函數(shù)相結(jié)合，構(gòu)建適應(yīng)度函數(shù)：

F(R,P,θ)=γF(R,P)+(1-γ)f(θ)

(7)

γ=1/2×e-F(R,P)+1/2

(8)

式中，γ為位姿誤差動(dòng)態(tài)調(diào)節(jié)系數(shù)，用于控制進(jìn)化方向。差分進(jìn)化開始時(shí)，F(xiàn)(R,P)較大，γ≈0.5，隨著進(jìn)化開始，F(xiàn)(R,P)逐漸變小，γ→1。適應(yīng)度函數(shù)這樣構(gòu)建，有利于維護(hù)F(R,P)與f(θ)在進(jìn)化過程中的平衡，使進(jìn)化方向更加合理。

3 改進(jìn)的差分進(jìn)化算法

差分進(jìn)化(differential evolution,DE)是基于實(shí)數(shù)編碼的進(jìn)化算法，由變異、交叉和選擇3個(gè)基本操作組成[12]，與經(jīng)典遺傳算法一脈相承。但因其有出色的全局優(yōu)化性和魯棒性，廣泛在連桿機(jī)構(gòu)的桿長(zhǎng)優(yōu)化、機(jī)械臂軌跡優(yōu)化、混合流水車間調(diào)度等工程優(yōu)化領(lǐng)域運(yùn)用，但在機(jī)械臂逆運(yùn)動(dòng)學(xué)求解應(yīng)用少有人研究。算法通過模擬達(dá)爾文“優(yōu)勝劣汰、適者生存”思想，利用上代向量間差分的原理進(jìn)行種群的進(jìn)化更新產(chǎn)生變異新個(gè)體以求得所優(yōu)化問題的最優(yōu)解[13]。算法優(yōu)劣主要由初始種群產(chǎn)生策略和變異、交叉策略決定。

3.1 改進(jìn)的種群初始化

經(jīng)典DE算法采用隨機(jī)分布法產(chǎn)生初始種群，難以保證個(gè)體多樣性，使最優(yōu)解質(zhì)量與收斂速度并不理想[14]。混沌運(yùn)動(dòng)獨(dú)特的隨機(jī)性和遍歷不重復(fù)性對(duì)生成多樣性的個(gè)體有優(yōu)勢(shì)，可以促使初始個(gè)體盡可能均勻分布于搜索空間。為提升最優(yōu)解質(zhì)量和保持群落里個(gè)體多樣性，采用混沌映射對(duì)初始種群及子代種群進(jìn)行改造,以提升算法優(yōu)化能力。一維Logistic映射為：

xμ+1=δxμ(1-xμ),x∈[0,1]

(9)

式中，xμ為混沌變量，在δ取值為4時(shí)，系統(tǒng)表現(xiàn)為混沌態(tài)，于[0,1]范圍隨機(jī)取值。(0,0.25,0.5,0.75,1)稱作一維Logistic映射的5個(gè)不動(dòng)點(diǎn)。對(duì)群落初始化的改造其實(shí)就是將混沌映射與隨機(jī)分布法產(chǎn)生的兩個(gè)群落內(nèi)的個(gè)體適應(yīng)值逐一比較，挑出NP個(gè)最優(yōu)個(gè)體當(dāng)作初始群落，方法如下：

步驟1:隨機(jī)分布法生成群落A。

xmn,1=xnmin+rand×(xnmax-xnmin)

(10)

式中，m=1,2,...,NP；n=1,2,...,D；rand∈[0,1]；xnmax、xnmin為第n個(gè)變量的上屆與下屆。

步驟2:隨機(jī)產(chǎn)生數(shù)x1∈(0,1)，但x1不可以為上述5個(gè)不動(dòng)點(diǎn)，從x1起，根據(jù)式(9)產(chǎn)生混沌序列。

{x1,x2,...,xNP×D}

(11)

步驟3:將式(11)按行排布生成一混沌矩陣。

(12)

步驟4:把式(12)的元素映射到種群個(gè)體每一維，xmn∈[xnmin,xnmax]，得到混沌初始群落B。

xmn=xnmin+xm,n×(xnmax-xnmin)

(13)

步驟5:計(jì)算群落A∪B中所有元素適應(yīng)值，按從小到大順序排，選擇前NP個(gè)元素作為最終的初始群落P。

3.2 基于二次混沌映射的子代重構(gòu)

隨著迭代的不斷進(jìn)行，種群中隨機(jī)分布的個(gè)體會(huì)逐漸聚集，此時(shí)可能發(fā)生局部收斂情況，從而難搜索到全局最優(yōu)解。為此，本文每隔一定代數(shù)M重構(gòu)種群，在迭代過程中，剔除部分高聚集個(gè)體，通過遷移因子產(chǎn)生一些新個(gè)體補(bǔ)償，來保持種群多樣性，使搜索能跳出局部最優(yōu)解，提高全局收斂能力。

假設(shè)當(dāng)前最優(yōu)個(gè)體的適應(yīng)值為best_unit_value，M代前最優(yōu)個(gè)體的適應(yīng)值為lastbest_unit_value，用二者之比表示進(jìn)化效果，即：

(14)

σ越小，進(jìn)化效果越好，反之越差。σ>Φ時(shí)，通過二次混沌映射對(duì)子代群落進(jìn)行重建，Φ為設(shè)定的閾值，方法如下：

步驟1:執(zhí)行3.1節(jié)的步驟2與步驟3。

步驟2:把式(12)的元素映射到群落個(gè)體每一維，xmn∈[xnmin,xnmax]，產(chǎn)生混沌遷移量。

(15)

步驟3:基于子代群落P，按下式進(jìn)行混沌遷移，生成一新群落C，g∈[0,1]為遷移度。

(16)

步驟4:計(jì)算群落P∪C中所有元素適應(yīng)值，從小到大排，挑出前NP個(gè)元素到下一次迭代的父代群落PG+1。

3.3 基于對(duì)稱映射法的越界處理

經(jīng)變異操作后的試驗(yàn)個(gè)體可能會(huì)越出邊界范圍，需要對(duì)這些個(gè)體處理，使用最多的方法是隨機(jī)再初始化法，但隨機(jī)性強(qiáng)，易丟失原變異個(gè)體信息。本文選用對(duì)稱映射法將越界個(gè)體變量轉(zhuǎn)換成一個(gè)相對(duì)于邊界對(duì)稱的數(shù)值，可減小隨機(jī)性，保留原部分變異個(gè)體信息，提高算法收斂精度。方法如下：

(17)

3.4 進(jìn)化模式選擇

變異操作時(shí)，需從種群中隨機(jī)選取3個(gè)不同個(gè)體xp1、xp2、xp3，且i≠p1≠p2≠p3，DE/rand/1為：

hij(t+1)=xp1j(t)+F(xp2j(t)-xp3j(t))

(18)

DE/best/1為：

hij(t+1)=xbj(t)+F(xp2j(t)-xp3j(t))

(19)

式中，F(xiàn)∈[0,2]為縮放系數(shù)；p1、p2、p3為個(gè)體在種群中的序號(hào)；xbj(t)為當(dāng)前進(jìn)化種群中最優(yōu)個(gè)體。

DE/rand/1進(jìn)化模式雖有全局搜索的優(yōu)勢(shì)，但收斂速度慢。DE/best/1進(jìn)化模式以當(dāng)前進(jìn)化種群中最好適應(yīng)值的個(gè)體作為基矢量，具有出色的局部搜索能力和快速收斂特性，但容易搜索停滯和存在過早收斂。為此，本文設(shè)置一選擇條件，fitness>L時(shí)采用式(18)進(jìn)化形式，其它情況采用式(19)進(jìn)化形式，可充分結(jié)合兩者的優(yōu)勢(shì)，克服各自不足。L為希望達(dá)到的位姿精度，fitness為當(dāng)代種群最優(yōu)適應(yīng)值。

3.5 自適應(yīng)變異因子F

變異因子F對(duì)種群多樣性與收斂性影響較大，我們希望進(jìn)化前期，F(xiàn)值較大，有助于全局搜索，保持種群多樣化。在后期收斂時(shí)，F(xiàn)值希望較小，有助于在收斂區(qū)域進(jìn)行局部搜索，提高收斂速率和搜索精度。為此，設(shè)計(jì)F在[0.8,0.4]范圍內(nèi)動(dòng)態(tài)變化。

(20)

式中，N為當(dāng)前迭代數(shù)；G為最大迭代數(shù)；F0為最小變異因子；Fi為自適應(yīng)變異因子。

4 仿真與實(shí)驗(yàn)驗(yàn)證

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

本文將第3節(jié)改進(jìn)的差分進(jìn)化算法(CBDE)用于庫(kù)卡6R機(jī)械臂進(jìn)行逆運(yùn)動(dòng)學(xué)求解，并將其求解結(jié)果與經(jīng)典DE、CDE算法(只采用文中第3.1與3.2節(jié)改進(jìn)方法)及BDE算法(進(jìn)化模式為best/1)的求解結(jié)果進(jìn)行對(duì)比分析。為驗(yàn)證所提算法的通用性，又選取一7DOF冗余機(jī)械臂驗(yàn)證。與算法相關(guān)的參數(shù)如表3所示。

表3 算法相關(guān)參數(shù)設(shè)置

在仿真試驗(yàn)中，所有算法采用MATLAB R2016 b進(jìn)行編程，計(jì)算機(jī)配置為：Intel Core i5-4210、2.90 GHz、12 GB內(nèi)存、Windows10操作系統(tǒng)。

4.2 算法優(yōu)化性能對(duì)比分析

表4 不同算法的性能指標(biāo)統(tǒng)計(jì)

表5 不同優(yōu)化算法最優(yōu)逆解及代數(shù)法8組逆解對(duì)應(yīng)的機(jī)械臂位置

(d) 第四組逆解位姿 (e) 第五組逆解位姿(f) 第六組逆解位姿

(g) 第七組逆解位姿(h) 第八組逆解位姿(i) DE算法最優(yōu)逆解位姿

(j) BDE算法最優(yōu)逆解位姿 (k) CDE算法最優(yōu)逆解位姿 (l) CBDE算法最優(yōu)逆解位姿

圖3 各算法適應(yīng)值變化曲線

由以上實(shí)驗(yàn)數(shù)據(jù)可知，相比傳統(tǒng)代數(shù)法求得的逆解如表5所示，群智能優(yōu)化算法精度更高，且最優(yōu)逆解均為產(chǎn)生目標(biāo)位姿的角度，從位姿模型圖可以看到最優(yōu)解滿足最佳柔順性與較低能量消耗。表4在G=200時(shí)，經(jīng)典DE算法位姿與位置成功率最低，位姿誤差在萬分之一范圍內(nèi)成功率只有94%。Best/1進(jìn)化模式，雖然最低適應(yīng)值可以到10-15數(shù)量級(jí)，但位姿誤差在萬分之一范圍內(nèi)的成功率僅為92%，并且有時(shí)會(huì)陷入局部最優(yōu)，誤差很大。改進(jìn)的種群初始化與種群二次重構(gòu)CDE算法，位置成功率僅72%。而CBDE算法位姿與位置成功率及位姿誤差在萬分之一內(nèi)均達(dá)到100%，成功集合了各家優(yōu)點(diǎn)。從圖3各算法平均適應(yīng)值變化曲線看，Best/1進(jìn)化模式收斂最快，但存在局部收斂，平均適應(yīng)值0.001 865 181。CBDE算法與CDE及經(jīng)典DE算法相比雖然前期收斂速度較慢，但達(dá)到10-6適應(yīng)值平均代數(shù)最低且最終平均適應(yīng)值也最低，而且能夠滿足各項(xiàng)成功率100%。綜上所述，CBDE算法能有效求取機(jī)械臂運(yùn)動(dòng)學(xué)逆解，且求解精度高，所需迭代次數(shù)少，最優(yōu)逆解滿足最佳柔順性與較低能量消耗。

4.3 冗余機(jī)械臂逆解實(shí)驗(yàn)

為驗(yàn)證CBDE算法求冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)的有效性，選7DOF YuMi 14000 ABB機(jī)器人左臂進(jìn)行運(yùn)動(dòng)學(xué)逆解實(shí)驗(yàn)，其具體的D-H參數(shù)見文獻(xiàn)[15]。目標(biāo)函數(shù)為式(5) ，適應(yīng)度函數(shù)為式(7)。用MATLAB Robotic Toolbox求[-pi/4pi/4pi/6 -pipi/4 -pi/6pi/3]關(guān)節(jié)角下的位姿作為給定目標(biāo)位姿：

(21)

與文獻(xiàn)[16]對(duì)比，參數(shù)設(shè)置為Size=100、G=700、D=7。因YuMi機(jī)器人各連桿質(zhì)量輕，這里式(7)中ωi均為1，只要求各關(guān)節(jié)轉(zhuǎn)角差，和最小，其余參數(shù)設(shè)置與前述相同。將算法獨(dú)立連續(xù)進(jìn)行100次逆解運(yùn)算，仿真試驗(yàn)的第700代平均適應(yīng)值(F700)、位姿成功率(S1)及尋優(yōu)成功率K如表6所示。其中，

(22)

表6 冗余機(jī)械臂逆解實(shí)驗(yàn)結(jié)果

實(shí)驗(yàn)時(shí)，平均在第340代適應(yīng)值到10-5數(shù)量級(jí)，位置精度小于0.000 1 mm，姿態(tài)精度小于0.000 1 rad，成功率100%。表6中加*為文獻(xiàn)[16]結(jié)果，可以看出所提算法均優(yōu)于它。用機(jī)器人工具箱繪制在給定角度，最小適應(yīng)值及最差適應(yīng)值下的位姿圖，可以發(fā)現(xiàn)圖形及各參數(shù)數(shù)值基本一致，有一點(diǎn)點(diǎn)差異是因?yàn)榇肜L制模型圖的角度小數(shù)位數(shù)不全所致。綜上所述，CBDE算法可以求解冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)。

(a) 給定角下位姿圖

(b) 最優(yōu)適應(yīng)值下位姿圖

(c) 最差適應(yīng)值下位姿圖

5 結(jié)論

所提出的基于改進(jìn)差分進(jìn)化算法的機(jī)械臂逆運(yùn)動(dòng)學(xué)求解方法，能夠有效提升算法的收斂精度與速度，使逆解精度提升，降低求解復(fù)雜度，方法通用性強(qiáng)，對(duì)兩種機(jī)械臂逆解實(shí)驗(yàn)成功率均為100%。

(1)對(duì)6R機(jī)械臂的逆解實(shí)驗(yàn)，平均200代時(shí)適應(yīng)度值達(dá)10-10數(shù)量級(jí)，與文獻(xiàn)[9]相比，目標(biāo)函數(shù)簡(jiǎn)潔高效、易于編程實(shí)現(xiàn)。在較低迭代次數(shù)下，不僅使末端位姿誤差滿足要求，還使機(jī)械臂各關(guān)節(jié)具有最佳柔順與低能耗的第二特性。

(2)對(duì)7DOF YuMi左臂逆解實(shí)驗(yàn)，MATLAB仿真結(jié)果表明，在給定位姿精度下，算法對(duì)冗余機(jī)械臂的逆解成功率亦為100%且與文獻(xiàn)[16]對(duì)比發(fā)現(xiàn)，所提算法各評(píng)價(jià)指標(biāo)均最優(yōu)，證明算法具有較強(qiáng)通用性。