基于改進麻雀搜索算法的錨護機器人逆運動學求解

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

（1.中煤華晉集團有限公司王家嶺礦，運城 044000；2.中國煤炭科工集團太原研究院有限公司，太原 030006；3.山西天地煤機裝備有限公司，太原 030006）

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

文獻[8]提出一種引入CMA-ES 步長更新的改進Particle Swarm Optimization 算法求機器人逆運動學解；文獻[9]將沒有冗余度技術的機器人關節空間分為8 組唯一域，并用數值法求解；文獻[10]則給出了一種由布谷鳥算法和牛頓法組合的算法求解逆運動學。

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

以CMM2-24 錨護機器人為例，使用指數積公式搭建運動學模型，將錨護機器人求逆解問題轉化成麻雀算法迭代求極值問題。然后分別從機器人的位置、姿態以及位姿3 個方面建立適應度函數來進行仿真實驗。以此來檢驗算法改進的實施性。

1 麻雀搜索算法

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

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

2 改進麻雀搜索算法

2.1 反向學習策略

反向學習[13]可以使得到最優解的概率提高，其主要思想是，通過反向學習尋到當前解對應的反向解，然后經過對比保留更好的解。其數學模型：假設p=［x1，x2，x3，…，xn］為n 維空間一個點，其中xi∈［ai，bi］，i=1，2，…，n，則反向點p′=［，，，…，xn′］其中=ai+bi-xi。

2.2 高斯擾動和柯西擾動

在迭代尋優過程中，把尋到的初始解進行高斯擾動[14]，公式為

式中：p 為初始解；N（0，1）是個隨機數，該數服從高斯分布特性。柯西變異來源于柯西分布，一維柯西分布概率密度如下[15]：

柯西擾動類似于高斯擾動，擾動公式為

式中：C 表示柯西分布的隨機數。柯西分布隨機變量生成函數為

柯西分布有著較好的兩翼概率，極大地提高了局部抗干擾能力和整體搜索尋優能力。高斯分布的特性則是當前個體附近區域的擾動性好，所以它的局部搜索能力強。根據兩者的優缺點，本文在使用麻雀算法求逆解時先加入柯西擾動加大搜索范圍，提高算法的全局尋優能力，后期投入高斯擾動增強局部搜索能力，減少算法的迭代搜索時間。

3 改進麻雀搜索算法在錨護機器人逆向運動學分析中的應用

3.1 正向運動學建模

CMM2-24 錨護機器人的三維模型如圖1 所示。可知錨護機器人是個經典的球腕型結構。為了便于理解和求解該結構的運動學模型，可以把6 個關節三三分類，前3 個關節確定位置，后3 個關節確定姿態。

圖1 CMM2-24 錨護機器人Fig.1 CMM2-24 anchorage robot

CMM2-24 錨護機器人的結構簡圖如圖2 所示，采用螺旋理論[16]搭建運動學模型。

圖2 錨護機器人結構簡圖Fig.2 Schematic diagram of anchorage robot

正運動學建模的關鍵參數如表1 所示。其中w表示軸線向量，r 表示軸線上的一點，i 表示關節序號。

表1 機器人各關節螺旋軸Tab.1 Spiral axes of each joint of robot

機器人的工具坐標系建立在最后一個關節處，機器人工具坐標系的初始位姿為

錨護機器人的正向運動學的指數積公式（POE）為

3.2 構建適應度函數

本文使用錨護機器人的位置、姿態、位姿這3種參數分別設置各自的適應度函數[17-18]：

式中：pdes和pcur分別為目標和實際的位置；zdes和zcur分別為目標和實際的姿態；f3為目標與實際的位姿誤差；α 和β 分別為位置矢量P 和姿態矢量的權值系數。使用構建好的適應度函數求錨護機器人逆運動學解的流程，如圖3 所示。

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

4 仿真實驗與結果分析

分別采用ISSA 與SSA 對錨護機器人的位置、姿態、位姿3 個方面進行仿真實驗，以此來驗證改進后的算法求解錨護機器人逆運動學時的可行性與有效性。

4.1 初始參數設定

對算法進行多次試驗后取最佳性能的初始參數：種群個數均為Size=50；均進行150 次迭代；探索和偵察預警者個數初始值均為種群總數的20%；各關節角范圍為［-π，π］；α=0.0033，β=1；當迭代次數maxgen=150 或f<10-5時，算法結束。

4.2 位置仿真

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

圖4 位置誤差進化曲線Fig.4 Evolution curves of position errors

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

ISSA、SSA 獨立運行100 次的位置誤差，如表2所示。其結果表明，ISSA 在平均值上比SSA 提高了5 個數量級，在標準差上則提高了4 個數量級。

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

4.3 姿態仿真

對錨護機器人進行姿態仿真，設初始角（rad）為Q1=［1；0.5；2；1；1；-0.5］，分別將ISSA、SSA 各自求解100 次，所得2 個算法的姿態誤差如圖5 所示。

圖5 姿態誤差進化曲線Fig.5 Evolution curve of attitude error

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

ISSA、SSA 獨立運行100 次的姿態誤差如表3所示，結果表明，ISSA 在平均值和標準差比SSA 均提高了3 個數量級。

表3 姿態誤差對比Tab.3 Comparison of attitude error

4.4 位姿仿真

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

根據已知末端位姿，代入式（14），兩種算法設置相同的參數，分別進行逆向運動學分析，其結果如表4 表示。

表4 逆運動學解對比Tab.4 Inverse kinematics solution comparison

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

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

5 結語

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