機(jī)械臂逆運(yùn)動(dòng)學(xué)的帶修復(fù)策略自適應(yīng)差分進(jìn)化求解*

李志敏

(無錫工藝職業(yè)技術(shù)學(xué)院機(jī)電與信息工程學(xué)院，無錫 214200)

0 引言

冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)求解問題具有非線性、高耦合性等特點(diǎn)，因此逆運(yùn)動(dòng)學(xué)求解存在較大困難。冗余機(jī)械臂逆解的精度和優(yōu)劣，將直接決定機(jī)械臂末端執(zhí)行器的定位精度、驅(qū)動(dòng)耗時(shí)和耗能[1]，因此研究機(jī)械臂逆運(yùn)動(dòng)學(xué)求解問題具有較大理論價(jià)值和實(shí)用價(jià)值。

機(jī)械臂運(yùn)動(dòng)學(xué)逆解是指根據(jù)給定的末端執(zhí)行器位姿，求解對應(yīng)的關(guān)節(jié)角空間點(diǎn)。求解方法可以分為三類，分別為幾何法、代數(shù)數(shù)值法、智能搜索法。幾何法依據(jù)機(jī)械臂的機(jī)械結(jié)構(gòu)，使用幾何知識(shí)對逆運(yùn)動(dòng)學(xué)進(jìn)行求解。胡奎等[2]引入了臂角變量的概念，將關(guān)節(jié)角限位與臂角可行范圍進(jìn)行映射，并將該映射關(guān)系進(jìn)行線性展開，得到了一種冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)求解方法。FARIA等[3]通過最小化當(dāng)前位置和自運(yùn)動(dòng)范圍邊界，實(shí)現(xiàn)避關(guān)節(jié)限位和避奇異，同時(shí)求出運(yùn)動(dòng)學(xué)逆解。代數(shù)數(shù)值法是指使用增廣雅可比矩陣、偽逆法、梯度投影法等進(jìn)行逆運(yùn)動(dòng)學(xué)求解。東輝等[4]使用傅里葉變換和卷積定理構(gòu)造非線性空間密度函數(shù)，使用迭代法求解該密度函數(shù)，從而得到運(yùn)動(dòng)學(xué)逆解。WAN等[5]在最小二乘范數(shù)法中引入了彈性場函數(shù)，從而提出了改進(jìn)最小二乘范數(shù)法的逆運(yùn)動(dòng)學(xué)求解方法?；谥悄芩阉魉惴ǖ那蠼馑悸窞?，將逆運(yùn)動(dòng)學(xué)求解問題轉(zhuǎn)化為搜索問題，包括粒子群算法、遺傳算法等。謝習(xí)華等[6]提出了權(quán)值自適應(yīng)差分進(jìn)化算法，并使用映射關(guān)系解決邊界約束問題，與傳統(tǒng)算法提高了求解精度和速度。BAYATI[7]將布谷鳥算法與帝國主義算法混合，將其應(yīng)用于機(jī)械臂逆運(yùn)動(dòng)學(xué)求解，求解性能好于單獨(dú)算法。以上研究方法在各自設(shè)置場景下均取得了較好的結(jié)果，但是仍有兩個(gè)問題需進(jìn)一步解決：一是進(jìn)一步優(yōu)化算法的搜索能力，算法搜索能力決定了問題求解深度；二是對于約束優(yōu)化問題，約束條件的轉(zhuǎn)化仍需進(jìn)一步研究。

針對冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)求解問題，本文以位姿精度為前提，以關(guān)節(jié)角變化量為優(yōu)化量。提出了帶修復(fù)策略自適應(yīng)差分進(jìn)化算法的求解方法，在保證逆解精度基礎(chǔ)上，實(shí)現(xiàn)了關(guān)節(jié)角變化量最小化的目標(biāo)。

1 問題描述與建模

1.1 研究對象

本文以KUKA LBR iiwa7自由度冗余機(jī)械臂為研究對象，該機(jī)械臂具有7個(gè)自由活動(dòng)關(guān)節(jié)，該機(jī)械臂的簡易模型如圖1所示。

圖1 機(jī)械臂簡易模型

此機(jī)械臂運(yùn)動(dòng)學(xué)的D-H參數(shù)如表1所示，表中ai為連桿長度，αi為連桿扭曲角度，di為連桿偏置，θi為關(guān)節(jié)轉(zhuǎn)角。

表1 機(jī)械臂D-H參數(shù)

1.2 問題分析與模型

在機(jī)械臂運(yùn)動(dòng)學(xué)逆解問題的研究中，一般以機(jī)械臂末端的位置誤差和姿態(tài)誤差最小為求解目標(biāo)。但是除了考慮求解的精度外，關(guān)節(jié)角的變化量也至關(guān)重要。以圖2所示情況為例，圖2中A點(diǎn)為起點(diǎn)位姿，B點(diǎn)為終點(diǎn)位姿，其中B點(diǎn)位姿對應(yīng)2種關(guān)節(jié)角組合。關(guān)節(jié)角變化量與機(jī)械臂耗能、耗時(shí)具有直接關(guān)系，因此從關(guān)節(jié)角變化量的角度講，點(diǎn)B應(yīng)當(dāng)選擇關(guān)節(jié)角組合1，而不是關(guān)節(jié)角組合2。

圖2 逆運(yùn)動(dòng)學(xué)多解舉例

一般來說，7自由度冗余機(jī)械臂末端執(zhí)行器的位姿點(diǎn)均對應(yīng)無數(shù)個(gè)關(guān)節(jié)空間點(diǎn)，因此存在運(yùn)動(dòng)學(xué)逆解選擇與優(yōu)化的問題。按照上述分析，本文將冗余機(jī)械臂逆運(yùn)動(dòng)學(xué)求解的優(yōu)化模型設(shè)置如下。

優(yōu)化目標(biāo)為關(guān)節(jié)角位移之和最小，即：

(1)

式中，y為優(yōu)化目標(biāo)函數(shù)；N為位姿點(diǎn)數(shù)量。

約束條件包括兩個(gè)方面，一是關(guān)節(jié)角運(yùn)動(dòng)范圍約束，二是末端執(zhí)行器位姿誤差應(yīng)在一定誤差范圍內(nèi)。

約束條件1：所有關(guān)節(jié)角的活動(dòng)范圍都應(yīng)在約束范圍內(nèi)，描述為：

θimin≤θi≤θimax

(2)

式中，θimin為關(guān)節(jié)i的最小關(guān)節(jié)角；θimax為關(guān)節(jié)i的最大關(guān)節(jié)角。

約束條件2：末端執(zhí)行器位姿誤差應(yīng)在一定誤差范圍內(nèi)。本文使用末端執(zhí)行器質(zhì)心位置坐標(biāo)(x,y,z)代表末端執(zhí)行器位置；姿態(tài)表示方法較多，包括歐拉角法和四元素法等，為了計(jì)算簡單和敘述方便，使用四元素(i,j,k,v)表示末端姿態(tài)。對于關(guān)節(jié)空間點(diǎn)θ，使用正運(yùn)動(dòng)學(xué)方程得到的末端執(zhí)行器位姿表示為：

(3)

式中，R3×3為3×3矩陣，表示末端執(zhí)行器姿態(tài)；P1×3=(x,y,z)T表示末端執(zhí)行器位置；03×1為零矩陣。

將期望的末端執(zhí)行器位置記為Pex=(xex,yex,zex)T，則位置誤差的約束條件為：

(4)

式中，Ep表示位置誤差；‖‖表示歐氏距離；Δp表示位置誤差閾值。

(5)

則末端執(zhí)行器姿態(tài)誤差的約束條件為：

Er≤Δr

(6)

式中，Er表示姿態(tài)誤差；Δr表示姿態(tài)誤差閾值。

結(jié)合式(1)、式(4)、式(6)，即可得到機(jī)械臂逆運(yùn)動(dòng)學(xué)求解的優(yōu)化模型。

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

機(jī)械臂正運(yùn)動(dòng)學(xué)求解一般通過旋轉(zhuǎn)和平移實(shí)現(xiàn)?？梢杂梢韵聨讉€(gè)步驟實(shí)現(xiàn)：

步驟1：建立關(guān)節(jié)坐標(biāo)系，以關(guān)節(jié)i轉(zhuǎn)向?yàn)閆i軸向；Xi軸向?yàn)閆i軸與Zi+1軸的垂直方向，并由Zi軸指向Zi+1軸；Yi軸向根據(jù)右手準(zhǔn)則確定，如圖3所示。

圖3 機(jī)械臂坐標(biāo)系

步驟2：使用RX(α)表示沿X軸旋轉(zhuǎn)α角，RY(β)、RZ(γ)具有類似含義；PX(d)表示X軸正向平移距離d，PY(t)、PZ(a)具有類似含義。則坐標(biāo)系i到坐標(biāo)系i+1的旋轉(zhuǎn)矩陣可表示為：

(7)

2 帶修復(fù)策略的自適應(yīng)差分進(jìn)化算法

2.1 差分進(jìn)化算法

差分進(jìn)化算法主要包括算法初始化、交叉、變異、選擇等操作。其中初始化使用隨機(jī)初始化方法，選擇使用一對一貪婪準(zhǔn)則，即當(dāng)子代優(yōu)于父代時(shí)選擇子代進(jìn)入下一代，當(dāng)父代優(yōu)于子代時(shí)選擇 父代進(jìn)入下一代。在此著重介紹變異和交叉操作。

DE/rand/1的變異方式為：

(8)

DE/rand/2的變異方式為：

(9)

式中，r1、r2、r3、r4、r5為種群中的5個(gè)任意個(gè)體，且r1≠r2≠r3≠r4≠r5。

DE/best/1的變異方式為：

(10)

DE/best/2的變異方式為：

(11)

DE/target-to-best/1的變異方式為：

(12)

(2)交叉操作。差分進(jìn)化算法的交叉操作是在染色體維度方面依據(jù)交叉因子進(jìn)行交叉互換，為：

(13)

式中，CR為交叉因子，取(0,1)之間的值；rand為(0,1)間的隨機(jī)數(shù)。

2.2 自適應(yīng)差分進(jìn)化算法

在第2.1節(jié)給出了多種變異策略，但是不同策略的適用情況不同[8]。比如，DE/best/1、DE/best/2和DE/target-to-best/1在解決單峰問題時(shí)，收斂性好且能快速找到最優(yōu)值，但是求解多峰問題時(shí)容易陷入局部極值；DE/rand/1和DE/rand/2的變異策略與此相反，適用于多峰問題，但是求解單峰問題時(shí)收斂性較差。

對于實(shí)際待解決的問題，一般情況下難以掌握該問題的全部性質(zhì)，因此無法確定一個(gè)適用的變異策略。為了解決這一問題，本文提出了變異策略自適應(yīng)確定方法。具體操作方法如下。

從種群中隨機(jī)選擇染色體作為測試染色體，在算法更新至k代時(shí)，第i個(gè)變異策略生成的個(gè)體被選為下一代的個(gè)數(shù)記為si,k，無法被選為下一代的個(gè)數(shù)記為fi,k，則第i種變異策略的成功得分Qi,k為：

(14)

式中，ε為一個(gè)較小值，本文取ε=0.01，防止出現(xiàn)Qi,k=0的情況。則算法迭代至k代時(shí)使用第i種變異策略的概率為：

(15)

算法依據(jù)式(15)，按照輪盤賭規(guī)則確定最終使用的變異策略。經(jīng)過以上設(shè)計(jì)，算法可以根據(jù)問題特點(diǎn)自適應(yīng)改變和選擇變異策略，使算法的變異策略可以根據(jù)問題特點(diǎn)自適應(yīng)變化。

2.3 針對約束條件的修復(fù)策略

針對帶約束的優(yōu)化問題，當(dāng)前常用方法包括可行準(zhǔn)則法、罰函數(shù)法等。本文以DE/best/1變異策略為例對上述方法進(jìn)行分析。使用圖4所示情況說明，假設(shè)圖中T點(diǎn)為位置最優(yōu)點(diǎn)，點(diǎn)A和點(diǎn)C為第k代個(gè)體，經(jīng)過變異和交叉后k+1代分別為B點(diǎn)和D點(diǎn)。

圖4 約束條件處理方法分析圖5 修復(fù)策略核心思想

若使用可行準(zhǔn)則法，B點(diǎn)不在可行域內(nèi)，因此染色體B不會(huì)進(jìn)入下一代；而點(diǎn)D函數(shù)值優(yōu)于點(diǎn)C，且點(diǎn)D在可行域內(nèi)，因此點(diǎn)D會(huì)進(jìn)入下一代。但是從實(shí)際上講，點(diǎn)B的搜索方向更優(yōu)，卻因?yàn)榭尚杏虮环艞墶ＡP函數(shù)也具有類似的缺陷，該方法得到的個(gè)體并不能維持自適應(yīng)差分進(jìn)化算法的最優(yōu)搜索方向。

為了保持自適應(yīng)差分進(jìn)化算法較優(yōu)的搜索方向，針對約束條件處理問題，本文提出了帶伸縮因子的修復(fù)策略。修復(fù)策略的核心思想如圖5所示，圖中A點(diǎn)為第k代個(gè)體，經(jīng)過變異和交叉后k+1代分別為B點(diǎn)，當(dāng)B點(diǎn)在可行域外時(shí)，使用伸縮因子將其沿原搜索方向映射到可行域中距離點(diǎn)B最近的點(diǎn)C。

具體實(shí)現(xiàn)方法為：當(dāng)點(diǎn)B超出可行域時(shí)，構(gòu)造一個(gè)子問題為：

(16)

式中，g(x)≤0為原不等式約束；k0為映射伸縮因子。式(16)含義為在原方向AB上搜索距離點(diǎn)B最近的點(diǎn)C。

3 仿真與驗(yàn)證

3.1 仿真設(shè)置

為了對上述逆運(yùn)動(dòng)學(xué)求解方法進(jìn)行驗(yàn)證，以螺旋曲線為例進(jìn)行逆運(yùn)動(dòng)學(xué)求解，螺旋曲線函數(shù)為：

在該螺旋曲線上等步長均勻選取100個(gè)點(diǎn)作為軌跡點(diǎn)，末端執(zhí)行器姿態(tài)根據(jù)任務(wù)需要設(shè)置為表2所示情況。

表2 末端姿態(tài)設(shè)置

帶修復(fù)策略的自適應(yīng)差分進(jìn)化算法參數(shù)設(shè)置為：最大迭代次數(shù)為200，縮放因子F=0.7，交叉因子CR=0.9，染色體規(guī)模為70。位置誤差閾值Δp=10-4mm，姿態(tài)誤差閾值Δr=10-4rad。

3.2 求解結(jié)果與分析

為了驗(yàn)證改進(jìn)措施的有效性，首先將帶修復(fù)策略的自適應(yīng)差分進(jìn)化算法與帶修復(fù)策略的傳統(tǒng)差分進(jìn)化算法進(jìn)行比較，傳統(tǒng)差分進(jìn)化算法參數(shù)設(shè)置與自適應(yīng)差分進(jìn)化算法參數(shù)一致。為了對比兩種算法的搜索快速性、搜索精度和搜索穩(wěn)定性，使用兩種算法各自獨(dú)立求解10次，某次的搜索過程如圖6所示。

圖6 求解的搜索過程圖7 求解結(jié)果分布

可以看出，差分進(jìn)化算法和自適應(yīng)差分進(jìn)化算法的收斂速度相近，均在迭代至50次左右時(shí)目標(biāo)函數(shù)值不再下降。

自適應(yīng)差分進(jìn)化算法和差分進(jìn)化算法各自獨(dú)立運(yùn)行10次得到的結(jié)果分布如圖7所示。

由圖7給出的10次結(jié)果分布，并結(jié)合圖6搜索結(jié)果可以看出，自適應(yīng)差分進(jìn)化算法搜索的結(jié)果整體優(yōu)于差分進(jìn)化搜索結(jié)果；從結(jié)果分布穩(wěn)定性角度分析，差分進(jìn)化算法搜索的結(jié)果存在較大波動(dòng)，而自適應(yīng)差分進(jìn)化算法搜索結(jié)果的波動(dòng)極小。綜合以上分析，自適應(yīng)差分進(jìn)化算法的搜索精度和穩(wěn)定性均優(yōu)于差分進(jìn)化算法。

為了進(jìn)一步分析本文方法對逆運(yùn)動(dòng)學(xué)問題的求解性能，同時(shí)使用本文方法、文獻(xiàn)[9]、文獻(xiàn)[10]方法對上述逆運(yùn)動(dòng)學(xué)問題進(jìn)行求解，各自獨(dú)立運(yùn)行10次，結(jié)果如表3所示。

表3 逆運(yùn)動(dòng)學(xué)求解結(jié)果 (rad)

由表3可以看出，本文提出的帶修復(fù)策略自適應(yīng)差分進(jìn)化算法求解的目標(biāo)函數(shù)值最小，為1.80×103rad，差分進(jìn)化算法、文獻(xiàn)[9]、文獻(xiàn)[10]求解的目標(biāo)函數(shù)均值均大于本文方法。從標(biāo)準(zhǔn)差的角度看，本文所提方法在10次求解中均能夠得到相對最優(yōu)解，算法穩(wěn)定性高于另外幾種方法。以上結(jié)果和分析驗(yàn)證了帶修復(fù)策略自適應(yīng)差分進(jìn)化算法在逆運(yùn)動(dòng)學(xué)求解中的優(yōu)越性。

下面對帶修復(fù)策略自適應(yīng)差分進(jìn)化算法的逆運(yùn)動(dòng)學(xué)求解精度進(jìn)行驗(yàn)證，結(jié)果如圖8所示。

圖8 求解點(diǎn)與標(biāo)準(zhǔn)軌跡

經(jīng)統(tǒng)計(jì)，100個(gè)軌跡點(diǎn)的逆運(yùn)動(dòng)學(xué)求解中，最大位置誤差為1.2×10-5mm，最大姿態(tài)誤差為1.6×10-5rad，均滿足設(shè)置的位姿誤差約束。結(jié)合圖7和圖8可知，本文提出的逆運(yùn)動(dòng)學(xué)求解精度較高，且能夠在保證精度前提下，得到角度變化量最小的逆解。以上結(jié)果和分析驗(yàn)證了帶約束自適應(yīng)差分進(jìn)化算法在逆運(yùn)動(dòng)學(xué)求解中的優(yōu)越性。

4 結(jié)論

本文研究了機(jī)械臂逆運(yùn)動(dòng)學(xué)求解問題，在保證逆解精度前提下，搜索關(guān)節(jié)角變化量最小的逆解。提出了變異策略自適應(yīng)變化策略，有效提高了算法的搜索能力。針對約束條件，給出了帶伸縮因子的修復(fù)策略，解決了約束條件問題。經(jīng)驗(yàn)證得出以下結(jié)論：①帶伸縮因子的修復(fù)策略可以有效解決約束問題；②自適應(yīng)變異策略有效改善了差分進(jìn)化算法的搜索能力；③在保證逆解精度前提下，帶修復(fù)策略的自適應(yīng)差分進(jìn)化算法可以搜索出關(guān)節(jié)角變化量較小的逆解。