基于改進自適應粒子群算法的機器人逆解研究*

武明虎，周喜悅，慶毅輝，胡 勝，劉 聰

(1．湖北工業大學電氣與電子工程學院，武漢 430068；2．湖北太陽能高效利用協同創新中心，武漢 430068)

0 引言

機器人逆解求解指的是已知機器人末端的位置和姿態求出每個關節的角度，是機器人領域的基礎問題，是實現對機器人精準實時控制的關鍵部分。目前機器人逆運動學求解方法主要分為兩大類：解析法和數值法。但解析法[1]在求解結構復雜、自由度較高的機器人逆運動學時具有較大的局限性，因此數值法已成為當下獲得機器人逆解的主要研究方法。

目前研究較多的數值方法主要是神經網絡和群體智能方法。李文博等[2]利用BP神經網絡求解機械臂逆運動學，網絡設計過程簡單但存在局部極小、收斂速度慢等缺點。李進等[3]提出一種基于RBF神經網絡間接求取運動學逆解的方法，相對于BP神經網絡，在局部極小值上有所改善，但求解精度完全取決于訓練集質量的好壞且不同結構的機器人無法共用同一訓練集。而常用的群體智能算法，如遺傳算法[4]、粒子群算法[5]等在機器人逆運動學求解的通用性上更具有優勢。王淑青等[6]提出的改進粒子群算法求逆解分別從“邊界限定、慣性權重、超界返回”三個方面來限制多解、提高求解的準確性與快速性，但求解精度仍未達到最優。因此本文在文獻[6]所提算法的基礎上，引入種群曼哈頓距離實時確定種群的進化狀態，提升算法的性能；將進化狀態分為勘探狀態與收斂狀態，進而根據狀態的不同選擇自適應學習因子，平衡局部搜索與全局搜索的能力，結果驗證了IAPSO算法在機器人逆運動學求解中的有效性。

1 運動學分析

1.1 運動學模型

為滿足工業自動化的靈活度需求，建立的機器人模型具有6個旋轉關節，等效關節分布如圖1所示。

圖1 機器人等效關節分布圖

采用改進D-H參數法[7]對上述機器人進行坐標系的建立，如圖2所示，Xi和Zi分別表示各個關節坐標系{i}的X方向和Z方向。

圖2 機器人連桿坐標系

根據連桿參數在連桿坐標系中的表示方法，得到如表1所示的D-H參數。表中：ai-1為連桿長度，αi-1為連桿扭轉角，di為連桿偏距，θi為關節角。

表1 機器人的D-H參數

表1中，d3=200 mm，a4=100 mm，a5=100 mm，d6=70 mm。

1.2 運動學正解

改進D-H參數法建系得到的兩個相鄰坐標系變換矩陣的一般表達式[8]為：

(1)

根據表1中的D-H參數和公式(1)得到坐標系{6}相對于坐標系{0}的變換矩陣：

(2)

(3)

其中，c45=cos(θ4+θ5)，s45=sin(θ4+θ5)，si=sinθi，ci=cosθi。

結合表1與式(3)在MATLAB中使用機器人工具箱建立機器人模型如圖3所示。令關節變量角q=[0，π/2，π/3，0，-π/3，π/2，0]，使用函數得到的運動學正解與式(3)得到的運動學正解相同，驗證了所建機器人模型的正確性。

圖3 本文所建機器人模型

2 改進自適應粒子群算法求逆解

2.1 已有PSO求逆解模型描述

已有PSO應用于運動學逆解的算法流程如下：

步驟1：初始化粒子的位置和速度；

步驟2：適應度值計算，得到個體最優pb和全局最優gb；

步驟3：為限制多解，采用“狹小動態邊界”策略確定搜索范圍；

步驟4：迭代循環，w采用“非線性時變權重與陷局部最優自校正結合”方法確定；

步驟5：粒子的位置和速度按照公式(4)更新；

步驟6：若粒子超界，采用“超界帶弱方向性返回擴散”方法處理；

步驟7：計算粒子的適應度函數值，個體最優和全局最優更新；

步驟8：未達到終止條件則執行步驟2，滿足終止條件則結束循環。

速度和位置更新迭代公式[9]：

其中，w為慣性權重，k為當前迭代次數，pbi是粒子i的歷史最佳位置，gb是整個群體的歷史最佳位置，c1和c2為學習因子，r1和r2為均勻分布在[0，1]上的隨機數。

上述PSO算法實現了利用“動態狹小邊界”策略來限制多解，確保解的唯一性；對w采用“非線性時變與陷局部最優自校正結合”方法提高準確性；提出“超界帶弱方向性返回擴散”策略來增強算法的快速性。

2.2 本文改進的算法模型

針對上述PSO算法尋優精度不足的缺陷，本文算法在文獻[6]的基礎上，做了兩點改進：分析種群進化狀態[10]、自適應學習因子策略[11]。

(1)種群進化狀態

種群的進化狀態隨著粒子的迭代處于不斷變化中，根據進化狀態動態調整更新迭代公式的參數更符合種群的進化需求，實現算法性能的提升。

定義種群曼哈頓距離PMD[12]表示種群中每個粒子與其他粒子之間最小曼哈頓距離的均值，粒子間的曼哈頓距離MD可以用式(5)計算：

(5)

其中，D表示種群維度，xin表示第i個粒子的n維度位置值。對MD排序獲得每個粒子與其他粒子的最小曼哈頓距離MDmin，進一步對最小曼哈頓距離求均值可得到種群曼哈頓距離PMD。算法最大迭代次數為T，則用式(6)表示第t-1代的種群曼哈頓距離PMD(t-1)，前t-2代中最小種群曼哈頓距離可以用式(7)表示：

PMD(t-1)

(6)

min(PMD[1:t-2])，1≤t≤T

(7)

通過比較式(6)與式(7)之間的大小關系，將種群的進化狀態分為以下兩種狀態：

1)種群勘探狀態：式(6)>式(7)，此時整個種群的粒子均處于積極搜索最優解的狀態，具有很強的搜索能力。

2)種群收斂狀態：式(6)≤式(7)，在這種狀態下，種群進化處于一種停滯狀態，若改變位置和速度更新參數有可能在后面的迭代過程中得到改進。

(2)自適應學習因子

從公式(4)可知，學習因子c1代表“種群個體認知”，可促進粒子獲得它歷史上最好的位置，學習因子c2代表“全局認知”，它能推進粒子向全局中最好的區域收斂。在種群勘探狀態下為盡可能得到更多最優解，增加c1、減小c2有利于種群粒子更新自身的個體最優位置；在種群收斂狀態下，為防止過早收斂陷入局部最優解應增加c1，同時也應增加c2的值使其他粒子盡快到達全局最優區域。IAPSO算法中，學習因子c1、c2初始值均為2，在種群勘探與收斂狀態的自適應策略分別由公式(8)與公式(9)給出：

(8)

(9)

學習因子的變化需緩慢進行，變化太大對種群的收斂速度與算法精度均會有所影響。因此規定在迭代循環時c1、c2變化范圍為[1.5，2.5]，若c1、c2之和大于4，則將兩個參數進行歸一化處理，歸一化公式如下：

(10)

綜上所述，本文IAPSO算法流程圖如圖4所示。

圖4 本文IAPSO算法流程圖

3 MATLAB仿真結果分析

在IAPSO算法中，確定以“目標點位姿與末端位姿距離誤差最小”為求解目標，由于位置和姿態在數量級上的差距，在位置誤差項前引入懲罰因子，使得位置誤差與姿態誤差滿足整體誤差最小。取粒子個數為60，最大迭代次數為100，慣性權重初始值為0.5，學習因子初始值為2進行仿真測試。

3.1 單點測試

隨機選取機器人末端一組已知位姿Te：

(11)

現將已有PSO算法與IAPSO算法在普通PC上運行20次，分別計算運行20次所得到的適應度值的均值，得到改進前后算法的收斂曲線對比如圖5所示，隨機抽取IAPSO算法得到5組逆解結果如表2所示。

(a) 總適應度值收斂曲線(b) 第20～100次迭代收斂曲線圖5 兩種算法的適應度值曲線對比圖

表2 給定位姿點的逆解驗證結果(單位/rad)

從表2可以看出，每次運算求得的關節角度基本恒定在一個值，最大誤差也不會超過0.005 rad，由此可見本文算法在機器人逆運動學求解中的準確性。

由圖5a可以看出，在迭代前期IAPSO算法的收斂速度明顯優于改進前，圖5b也顯示改進后的收斂精度有所提高，由此可見種群收斂狀態和自適應學習因子策略能夠增加粒子群的多樣性，提升算法的搜索性能。

3.2 連續軌跡測試

任取機器人的起始位置T0和終止位置T1，并已知機器人起始位置所對應的關節角qs=[0，π/2，-π/2，π/2，0，π/2]，T0和T1數據如下：

(12)

在起始位姿和末端位姿間利用機器人工具箱函數進行10次插值，使用IAPSO算法得到連續軌跡逆解結果如圖6所示。

圖6 連續軌跡逆解驗證

從圖6中可以看出，連續軌跡測試所得關節角曲線是光滑且唯一的，并且沒有大的跳變，表明所得解始終在上一次全局最優解附近產生，充分說明了IAPSO算法的正確性與有效性。

4 結論

本文建立了一種6自由度抓取機械手模型，通過機器人工具箱仿真驗證了所建機器人模型的正確性。IAPSO算法是針對已有PSO算法求解精度不足的缺陷進行改進，兼顧了已有PSO算法求逆解的優勢，同時仿真結果也表明IAPSO算法收斂速度快，計算精度更高。相比于傳統的逆運動學求解方法，本文算法無須復雜的公式推導過程，且求解結果比較理想，能夠應用于所有復雜結構機器人逆運動學求解。