王瑞君，馬祺杰，閆金寶，李紅波

（1.中煤華晉集團(tuán)有限公司王家?guī)X礦，運(yùn)城 044000；2.中國煤炭科工集團(tuán)太原研究院有限公司，太原 030006；3.山西天地煤機(jī)裝備有限公司，太原 030006）

當(dāng)下求解逆運(yùn)動(dòng)學(xué)最常用的3 種方法分別是數(shù)值計(jì)算法[1]、代數(shù)公式法[2]和幾何分析法[3]。拆解分析法和幾何分析法求解逆運(yùn)動(dòng)學(xué)時(shí)需要考慮Pieper準(zhǔn)則。隨著機(jī)器人功能與結(jié)構(gòu)的飛速發(fā)展，逆運(yùn)動(dòng)學(xué)的求解過程更加復(fù)雜，普通的求逆解方法有可能求不出解來。因此學(xué)者們將神經(jīng)網(wǎng)絡(luò)、優(yōu)化算法等智能化求解工具運(yùn)用于求逆解的過程中。其主要思路是將機(jī)器人求逆問題轉(zhuǎn)化成多元多峰函數(shù)求最值問題[4]。主要有遺傳（Genetic Algorithm）算法[5]、粒子群（Particle Swarm Optimization）算法[6]，以及BP神經(jīng)網(wǎng)絡(luò)（Back Propagation）算法[7]等。

文獻(xiàn)[8]提出一種引入CMA-ES 步長更新的改進(jìn)Particle Swarm Optimization 算法求機(jī)器人逆運(yùn)動(dòng)學(xué)解；文獻(xiàn)[9]將沒有冗余度技術(shù)的機(jī)器人關(guān)節(jié)空間分為8 組唯一域，并用數(shù)值法求解；文獻(xiàn)[10]則給出了一種由布谷鳥算法和牛頓法組合的算法求解逆運(yùn)動(dòng)學(xué)。

文獻(xiàn)[11]針對SSA 算法迭代后期種群多樣性小，全局尋優(yōu)能力差等問題提出一種融合柯西變異和反向?qū)W習(xí)的優(yōu)化麻雀算法。經(jīng)過多次多種的對比試驗(yàn)，得到的結(jié)果表明改進(jìn)麻雀算法的極限逼近速度更快、誤差更小，全局尋優(yōu)能力更強(qiáng)?；诖?，本文給出了一種改進(jìn)麻雀搜索算法ISSA（improved sparrow search algorithm），以解決基本的麻雀搜索算法在求解過程中極限逼近速度慢、全局尋優(yōu)能力差等不足。該算法的第1 步，先通過反向?qū)W習(xí)策略生成初始種群，增加多樣性；第2 步，引入柯西-高斯變異提高算法的全局尋優(yōu)能力；最后將ISSA 應(yīng)用到求錨護(hù)機(jī)器人逆向運(yùn)動(dòng)學(xué)解中。

以CMM2-24 錨護(hù)機(jī)器人為例，使用指數(shù)積公式搭建運(yùn)動(dòng)學(xué)模型，將錨護(hù)機(jī)器人求逆解問題轉(zhuǎn)化成麻雀算法迭代求極值問題。然后分別從機(jī)器人的位置、姿態(tài)以及位姿3 個(gè)方面建立適應(yīng)度函數(shù)來進(jìn)行仿真實(shí)驗(yàn)。以此來檢驗(yàn)算法改進(jìn)的實(shí)施性。

1 麻雀搜索算法

麻雀搜索算法是根據(jù)麻雀種群捕食并規(guī)避掠食者的行為規(guī)律得到的一種群智能優(yōu)化搜索算法[12]。在麻雀搜索算法中，每代探索者位置更新如下：

式中：β 是一個(gè)服從N（0，1）的隨機(jī)數(shù)；K∈［-1，1］表示移動(dòng)方向；添加極小值c 是為了分母不為0；fi是第i 只麻雀的適應(yīng)度值；fg和fw分別是當(dāng)前階段種群的極小、極大適應(yīng)度值。

2 改進(jìn)麻雀搜索算法

2.1 反向?qū)W習(xí)策略

反向?qū)W習(xí)[13]可以使得到最優(yōu)解的概率提高，其主要思想是，通過反向?qū)W習(xí)尋到當(dāng)前解對應(yīng)的反向解，然后經(jīng)過對比保留更好的解。其數(shù)學(xué)模型：假設(shè)p=［x1，x2，x3，…，xn］為n 維空間一個(gè)點(diǎn)，其中xi∈［ai，bi］，i=1，2，…，n，則反向點(diǎn)p′=［，，，…，xn′］其中=ai+bi-xi。

2.2 高斯擾動(dòng)和柯西擾動(dòng)

在迭代尋優(yōu)過程中，把尋到的初始解進(jìn)行高斯擾動(dòng)[14]，公式為

式中：p 為初始解；N（0，1）是個(gè)隨機(jī)數(shù)，該數(shù)服從高斯分布特性?？挛髯儺悂碓从诳挛鞣植?，一維柯西分布概率密度如下[15]：

柯西擾動(dòng)類似于高斯擾動(dòng)，擾動(dòng)公式為

式中：C 表示柯西分布的隨機(jī)數(shù)?？挛鞣植茧S機(jī)變量生成函數(shù)為

柯西分布有著較好的兩翼概率，極大地提高了局部抗干擾能力和整體搜索尋優(yōu)能力。高斯分布的特性則是當(dāng)前個(gè)體附近區(qū)域的擾動(dòng)性好，所以它的局部搜索能力強(qiáng)。根據(jù)兩者的優(yōu)缺點(diǎn)，本文在使用麻雀算法求逆解時(shí)先加入柯西擾動(dòng)加大搜索范圍，提高算法的全局尋優(yōu)能力，后期投入高斯擾動(dòng)增強(qiáng)局部搜索能力，減少算法的迭代搜索時(shí)間。

3 改進(jìn)麻雀搜索算法在錨護(hù)機(jī)器人逆向運(yùn)動(dòng)學(xué)分析中的應(yīng)用

3.1 正向運(yùn)動(dòng)學(xué)建模

CMM2-24 錨護(hù)機(jī)器人的三維模型如圖1 所示?？芍^護(hù)機(jī)器人是個(gè)經(jīng)典的球腕型結(jié)構(gòu)。為了便于理解和求解該結(jié)構(gòu)的運(yùn)動(dòng)學(xué)模型，可以把6 個(gè)關(guān)節(jié)三三分類，前3 個(gè)關(guān)節(jié)確定位置，后3 個(gè)關(guān)節(jié)確定姿態(tài)。

圖1 CMM2-24 錨護(hù)機(jī)器人Fig.1 CMM2-24 anchorage robot

CMM2-24 錨護(hù)機(jī)器人的結(jié)構(gòu)簡圖如圖2 所示，采用螺旋理論[16]搭建運(yùn)動(dòng)學(xué)模型。

圖2 錨護(hù)機(jī)器人結(jié)構(gòu)簡圖Fig.2 Schematic diagram of anchorage robot

正運(yùn)動(dòng)學(xué)建模的關(guān)鍵參數(shù)如表1 所示。其中w表示軸線向量，r 表示軸線上的一點(diǎn)，i 表示關(guān)節(jié)序號。

表1 機(jī)器人各關(guān)節(jié)螺旋軸Tab.1 Spiral axes of each joint of robot

機(jī)器人的工具坐標(biāo)系建立在最后一個(gè)關(guān)節(jié)處，機(jī)器人工具坐標(biāo)系的初始位姿為

錨護(hù)機(jī)器人的正向運(yùn)動(dòng)學(xué)的指數(shù)積公式（POE）為

3.2 構(gòu)建適應(yīng)度函數(shù)

本文使用錨護(hù)機(jī)器人的位置、姿態(tài)、位姿這3種參數(shù)分別設(shè)置各自的適應(yīng)度函數(shù)[17-18]：

式中：pdes和pcur分別為目標(biāo)和實(shí)際的位置；zdes和zcur分別為目標(biāo)和實(shí)際的姿態(tài)；f3為目標(biāo)與實(shí)際的位姿誤差；α 和β 分別為位置矢量P 和姿態(tài)矢量的權(quán)值系數(shù)。使用構(gòu)建好的適應(yīng)度函數(shù)求錨護(hù)機(jī)器人逆運(yùn)動(dòng)學(xué)解的流程，如圖3 所示。

圖3 ISSA 求解流程Fig.3 ISSA solution flow chart

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

分別采用ISSA 與SSA 對錨護(hù)機(jī)器人的位置、姿態(tài)、位姿3 個(gè)方面進(jìn)行仿真實(shí)驗(yàn)，以此來驗(yàn)證改進(jìn)后的算法求解錨護(hù)機(jī)器人逆運(yùn)動(dòng)學(xué)時(shí)的可行性與有效性。

4.1 初始參數(shù)設(shè)定

對算法進(jìn)行多次試驗(yàn)后取最佳性能的初始參數(shù)：種群個(gè)數(shù)均為Size=50；均進(jìn)行150 次迭代；探索和偵察預(yù)警者個(gè)數(shù)初始值均為種群總數(shù)的20%；各關(guān)節(jié)角范圍為［-π，π］；α=0.0033，β=1；當(dāng)?shù)螖?shù)maxgen=150 或f<10-5時(shí)，算法結(jié)束。

4.2 位置仿真

對錨護(hù)機(jī)器人進(jìn)行位置仿真實(shí)驗(yàn)，設(shè)初始角（rad）為Q1=［1；0.5；2；1；1；-0.5］，錨護(hù)機(jī)器人的末端解析值位置為P1=［3452.08984；537.09178；2920.03420］。分別將ISSA、SSA 各自求解100 次，結(jié)果與解析值對比后得到2 種位置誤差進(jìn)化曲線，如圖4 所示。

圖4 位置誤差進(jìn)化曲線Fig.4 Evolution curves of position errors

結(jié)果表明在位置誤差上，SSA 算法的收斂精度保持在10～102之間，而ISSA 算法的收斂精度則保持在10-5～10-6之間，由此證明ISSA 比SSA 收斂精度更高。

ISSA、SSA 獨(dú)立運(yùn)行100 次的位置誤差，如表2所示。其結(jié)果表明，ISSA 在平均值上比SSA 提高了5 個(gè)數(shù)量級，在標(biāo)準(zhǔn)差上則提高了4 個(gè)數(shù)量級。

表2 位置誤差對比Tab.2 Position error comparison

4.3 姿態(tài)仿真

對錨護(hù)機(jī)器人進(jìn)行姿態(tài)仿真，設(shè)初始角（rad）為Q1=［1；0.5；2；1；1；-0.5］，分別將ISSA、SSA 各自求解100 次，所得2 個(gè)算法的姿態(tài)誤差如圖5 所示。

圖5 姿態(tài)誤差進(jìn)化曲線Fig.5 Evolution curve of attitude error

結(jié)果表明對于姿態(tài)誤差，SSA 算法的收斂精度保持在10-3，而ISSA 算法的收斂精度則保持在10-5。證明ISSA 的收斂精度更高，魯棒性好。

ISSA、SSA 獨(dú)立運(yùn)行100 次的姿態(tài)誤差如表3所示，結(jié)果表明，ISSA 在平均值和標(biāo)準(zhǔn)差比SSA 均提高了3 個(gè)數(shù)量級。

表3 姿態(tài)誤差對比Tab.3 Comparison of attitude error

4.4 位姿仿真

給定關(guān)節(jié)角θ（rad）=［1，2，1，-1，0.5，-0.5］，代入式（9）求得位姿矩陣T：

根據(jù)已知末端位姿，代入式（14），兩種算法設(shè)置相同的參數(shù)，分別進(jìn)行逆向運(yùn)動(dòng)學(xué)分析，其結(jié)果如表4 表示。

表4 逆運(yùn)動(dòng)學(xué)解對比Tab.4 Inverse kinematics solution comparison

兩種算法分別對位姿進(jìn)行求解得到的誤差曲線，如圖6 所示。結(jié)果顯示ISSA 的收斂精度明顯提高，SSA 在100左右就停止了搜索。對圖6 中2 條位姿誤差曲線的斜率進(jìn)行對比分析，則不難看出ISSA在剛開始迭代的時(shí)候有著比SSA 更強(qiáng)的局部及全局搜索能力。

圖6 兩種算法位姿曲線對比Fig.6 Comparison of pose curves of two algorithms

5 結(jié)語

針對錨護(hù)機(jī)器人求逆解問題，本文提出一種結(jié)合柯西-高斯變異的改進(jìn)麻雀搜索算法（ISSA），相對于傳統(tǒng)麻雀算法（SSA），該算法避免了種群單一、魯棒性差的缺點(diǎn)。將ISSA 和SSA 分別應(yīng)用到求錨護(hù)機(jī)器人逆解中，并對其位置誤差、姿態(tài)誤差以及位姿誤差3 個(gè)方面分別建立適應(yīng)度函數(shù)進(jìn)行對比仿真實(shí)驗(yàn)。使用位置誤差做適應(yīng)度函數(shù)時(shí)，ISSA 比SSA 高5 個(gè)量級的收斂精度和4 個(gè)量級的穩(wěn)定性。使用姿態(tài)誤差作為適應(yīng)度函數(shù)時(shí)，ISSA 比SSA 高3個(gè)量級的收斂精度。使用位姿誤差做適應(yīng)度函數(shù)時(shí)，ISSA 比SSA 高5 個(gè)量級的收斂精度。實(shí)驗(yàn)結(jié)果表明：改進(jìn)麻雀搜索算法擁有收斂精度高、速度快、穩(wěn)定性好等特點(diǎn)，因此充分證明了算法改進(jìn)的可行性。