基于改進粒子群算法的六自由度工業機器人軌跡規劃＊

蘇 俊 熊瑞平 溫記明 李云秋 譚 平

（四川大學機械工程學院，四川成都 610041）

伴隨著中國從制造業大國邁向制造業強國的產業升級趨勢，工業機器人在大批量生產中被越來越多的使用[1]。在本文工況中，待處理工件有4處位置點需要進行去毛刺打磨操作，考慮到工件的大批量處理，因此設計使用工業機器人進行去毛刺打磨處理操作并規劃機器人運動軌跡，使得機器人運動平穩且運行時間最短，從而提高工件打磨效率。機器人路徑規劃算法是工業機器人技術的重要組成部分，常用的機器人運動軌跡規劃優化目標有運動時間最優、能量最優以及混合最優目標等優化目標[2]。本文以時間最優作為優化目標，通過優化軌跡路徑參數使得軌跡運動時間最短[3-7]。

常用的規劃軌跡有樣條軌跡、多項式軌跡和分段軌跡等方式[8-9]。其中樣條曲線軌跡求解相對復雜[10-11]，多項式軌跡常用三次、五次、七次多項式進行軌跡優化，三次多項式曲線求解簡單但加速度不連續，沿規劃路徑運動不平穩[12]；單條5次多項式軌跡能夠保證運動的穩定性，但其適應能力較差，容易因為一兩個點造成整個曲線的變形[13]；高階的7次規劃曲線，求解較復雜且會出現龍格現象，其求解出的值準確度更低[14]。本文選取3次-5次-3次三段多項式軌跡方法，該曲線在保證規劃曲線位移、速度和加速度連續，機器人運行平穩的同時，通過3段方程提高了軌跡對中間點位的適應能力，避免受限于中間點造成軌跡扭曲的問題，提高了軌跡過度點平滑性[15]；同時約束方程最高求解次數為5次，保留了多項式軌跡計算簡單的優勢。常用的軌跡優化方法有粒子群算法、蟻群算法和遺傳算法等。本文引入粒子群算法進行軌跡優化，粒子群算法成熟度高、求解能力強。但是，粒子群算法的缺點是算法較早熟，容易過早陷入局部最優。因此，本文通過融合免疫操作的方法對粒子群算法進行改進，增強粒子群算法的搜索能力，通過改進后的粒子群算法提高機器人運行效率；并利用本文的軌跡規劃問題，將融合免疫操作的改進粒子群算法與傳統粒子群算法、傳統可變參數改進粒子群算法進行對比，考察本文算法的性能。

本文研究利用工業機器人進行工件去毛刺打磨工作，針對機器人在笛卡爾空間中的4個位姿點位進行軌跡規劃，減少工作時間，提高工件處理效率。

1 運動學建模與分析

待處理工件為異型梯形工件，需要對4個柱體端面進行去毛刺打磨，如圖1所示。

圖1 打磨處理位置

4個打磨位姿點放置在機器人空間坐標系中，分布如表1所示。

表1 目標點末端位姿

其中Ti表示第i點的空間位姿。本文引入庫卡KR16-2型工業機器人進行去毛刺打磨操作，其改進DH參數如表2所示。

表2 機器人改進D-H參數

其中，i表示機器人第i軸 、αi表示連桿扭轉角、ai表示連桿長度、θi表 示關節轉角、di表示連桿偏移距離。為確定機器人的運動范圍是否滿足工況需求，使用蒙特卡洛法[16]計算機器人的工作空間，利用Matlab開源機器人運動學工具箱Robot Tool Box[17]進行運動學建模仿真驗證。算法代入大量隨機點近似遍歷機器人運動空間，仿真得到機器人工作空間范圍。代入5 000組隨機角度值計算機器人工作空間范圍，保留運算結果數據，輸出點位分布如圖2所示。

圖2 機器人工作空間范圍

機器人工作空間近似為一個高度值范圍[-1 000,1 400]mm ，半徑1 600 mm的橢球體空間，包含4個目標位置點位，符合工況需求。利用機器人學工具箱求解笛卡爾空間位姿對應的關節空間逆解，結 果如表3所示，其中Pi表 示第i點的關節角弧度值。

表3 關節空間逆解結果rad

2 機器人軌跡規劃

針對機器人關節空間軌跡規劃，使用3-5-3三段多項式曲線進行軌跡規劃，在保證曲線連續、速度和加速度連續的基礎上利用3段分段軌跡增強軌跡對位姿點位的適應能力。設3段曲線角度為q1～q3，曲線參數aij表 示第i段曲線第j個參數，3段運動時間參數分別為t1～t3，可得到關節空間中軌跡位置關系滿足函數關系式（1），軌跡速度方程滿足函數關系式（2），軌跡加速度方程滿足函數關系式（3）。

設關節空間中4個目標點轉角位置分別為Q1～Q4，為保證機器人運動軌跡平滑穩定無沖擊，根據分段軌跡約束條件，初始點Q1和 終點Q4滿足速度和加速度為0，路徑點之間的位置、速度及加速度連續，3段軌跡運行時間分別是tl1～tl3，約束表達式分別有位置連續約束方程（4）、速度連續約束方程（5）和加速度連續約束方程（6）。

將上述約束方程組寫作q=a×A，其中q為包含路徑點Q1～Q4的向量，a包含曲線參數aij的運動方程系數，A為包含運動時間t1～t3的方陣。公式變形為a=q×A-1，求解軌跡方程系數aij，即為最優狀態下的軌跡曲線方程。

3 融合免疫操作的改進粒子群算法

本文的軌跡優化目標為機器人運行時間最優，即在滿足軌跡約束條件下機器人沿軌跡運行時間最短。3-5-3多項式軌跡由3段軌跡組成，總運行時間即為3段之和，即tl=tl1+tl2+tl3，使用智能算法對軌跡的14個參數（aij）同時進行優化，使得機器人沿軌跡運行時間值最少。同時每一機器人關節有運行速度與加速度限制，得到優化目標函數表達式（7），其中目標函數f(tl)表示3段時間之和最小，限制條件為運行角速度ω值小于等于限制速度ωmax，運行角加速度α值小于等于角加速度限制 αmax。

粒子群算法是上世紀90年代提出的一種智能優化算法，通過多個粒子迭代探索最值問題的最優解[18]，其表達式如式（8）。

式中：i表示當前種群中某一粒子，t表示粒子迭代次數，d表示粒子維度；表示粒子位置值，表示粒子速度；pid表示當前粒子在迭代中的歷史最優位置；pgd表示全局最優位置；步長ω和學習因子c1和c2是分布在[0～1]之間的隨機數。為適應本文的優化目標，粒子位置xid的3個維度分別設置為3段軌跡的運行時間tl1～tl3，建立粒子群適應度公式如式（9）。

粒子群算法計算簡單、運算效率高，一經問世便被應用在各個領域。但是粒子群算法容易早熟，過早進入局部最優，往往難以求出更優秀的全局解[19-20]。改善粒子群算法早熟缺陷的常用方式是引入可變參數，例如線性遞減參數法、自適應權重法、隨機參數權重法等；可變參數法在早期降低學習因子c1和c2的值，提高步長ω的值，在早期提高算法的全局搜索能力，在后期提高算法局部搜索能力，優化算法在整體和局部的搜索能力[21-22]。參數因子設計公式如式（10）。

通過引入可變參數的粒子群算法，能夠在一定程度上改善粒子群算法容易陷入局部最優的問題，但可變參數法全局探索能力仍然限制較大、效果不夠好[23]。

為了更好地改進粒子群算法，提高軌跡規劃能力，本節通過融合免疫算法的操作對粒子群算法進行改進。免疫算法參考生物的免疫系統，其核心概念是抗體多樣性、免疫記憶與免疫接種[24]。在運用免疫算法進行問題求解時，抗原代表了滿足約束關系的最優解，抗體為當前候選解?？贵w與抗原之間的親和力值反映了當前解的優秀程度，抗體和抗體之間的親和力反映了候選解的多樣性。免疫記憶反應指從記憶庫中搜索該類問題的較優解記憶粒子作為抗體，模擬與抗原最優解反應所產生的抗體值。免疫接種模擬了接種疫苗治療相應病癥的手段，通過有指導、有目的地對種群粒子進行修改，從而提高種群粒子多樣性、抑制優化過程中的退化效應，提高算法搜索性能[25]。免疫算法操作依賴的計算參數有濃度、濃度選擇概率、適應度概率、和免疫替換概率等。粒子濃度計算如式（11），其中xi表示粒子i，D(xi)表示粒子i的抗體濃度，N表示種群數量，f(xi)表 示粒子i適應度函數值。

推導出粒子濃度概率公式（12），其中PD(xi)表示粒子i濃度概率值。

粒子i適應度概率計算式為式（13），其中PF(xi)表 示粒子適應度概率，psum表示個體最優適應度值。

免疫替換概率計算式為式（14），表明當前粒子多大程度上需要進行替換操作,rand為范圍在[0-1]的隨機數。PR(i)表 示粒子i的免疫替換概率。

免疫替換生成公式為式（15），其中rang為設置的范圍設定值。當免疫替換概率值PRi超過設定值，則進行免疫替換操作。

本文在基本粒子群算法的基礎上進一步引入免疫操作，利用免疫算法的濃度挑選機制，抑制近似的高濃度解，促進邊緣的低濃度解，同時通過免疫接種操作對種群進行有指導、有目的地替換，提高種群多樣性，從而提高改進粒子群算法的搜索能力。這一算法流程圖如圖3所示。

圖3 算法流程圖

應用改進粒子群算法對機械臂進行3-5-3分段軌跡規劃，算法主要流程步驟為：

Step1在粒子搜索空間中隨機生成N個粒子，初始化粒子位置與速度。

Step2求解粒子對應的軌跡系數，計算當前軌跡角速度與角加速度大小。若角速度和角加速度超過設定值，表明粒子不滿足角速度和角加速度限制要求，將其適應度值設置為極大值常數C。若角速度與角加速度滿足限制要求，計算粒子適應度值。

Step3計算粒子歷史最優值pid和全局最優值pgd。

Step4計算兩代間最優適應度差值是否小于設定值di f，若兩代間適應度差值小于設定值，則表明搜索算法陷入局部最優，全局搜索能力較弱，此時進行免疫操作。首先計算種群粒子濃度概率、適應度概率、免疫替換概率，若免疫替換概率大于設定值則對種群粒子進行免疫替換操作。

Step5利用粒子群算法對種群進行迭代，更新粒子位置與速度。

Step6判斷迭代次數是否滿足設定值，若滿足設定次數則輸出結果，算法結束；若還未滿足迭代次數，則返回Step2循環算法。

4 仿真實驗

為考察改進后算法的性能，選取算法測試函數對算法性能進行考察，對比傳統粒子群、傳統變參數改進粒子群和本文提出的免疫粒子群3種算法的性能，以衡量新算法的改進效果。本文選取Griewank function、Ackley function、Schaffer2 function和Levy function這4種主流的具有多局部最小值，求解難度較大的測試函數。4種測試函數全局最小值均為0，實驗以測試函數函數值作為適應度進行迭代計算。設置粒子群種群個體數為N=20，總迭代次數設置為K=100。對于傳統粒子群算法，步進參數ω 設置為0.5，學習因子c1和c2設置為 0.35；對于可變參數改進粒子群算法，步進參數ω設置最大值 ωmax=0.9，最小值ωmin=0.2，學習因子c1和c2最大值設置為c1max=c2max=0.5，最小值設置為c1min=c2min=0.2。對于融合免疫操作的改進粒子群算法，代間適應度差值閾值di f設置為di f=0.05，免疫替代概率Prep設置為Prep=0.01，免疫粒子生成范圍參數rang設 置為rang=1。對每一測試函數分別使用3種算法重復100組求解實驗，每一組總迭代次數設置為100代，同時計算第10代、20代、50代、100代平均適應度值，實驗結果如表4所示。

分析表4的實驗結果，本文所提出的融合免疫操作的改進粒子群算法在4種主流測試函數中，相較于傳統粒子群算法和傳統改進粒子群算法均體現出了較大的優勢。在第100代適應度結果中，除在Griewank function測試函數中免疫粒子群算法適應度降為改進粒子群算法一半以外，其他3種測試函數中免疫粒子群算法適應度值相較于其他兩種傳統求解算法均小于1個數量級以上，特別是在Schaffer2 function測試中小于5個數量級，體現了免疫粒子群算法求解能力的優勢。同時在第10代、20代、50代中，免疫粒子群算法適應度值均小于其他兩種傳統算法，且適應度降低速度更快，體現了免疫粒子群算法迭代能力的優勢。對比表格數據，可以得出本文所提出的融合免疫操作的改進粒子群算法在迭代求解能力、求解結果上均優于傳統粒子群算法及傳統改進粒子群算法。

表4 測試函數適應度均值

在考察本文所提出的算法對于主流測試函數求解性能后，為了進一步對比驗證改進后的算法在處理本文軌跡規劃問題上的性能，本文利用傳統粒子群算法、基于變參數的改進粒子群算法和融合免疫操作的改進粒子群算法對機器人6軸進行最短時間軌跡規劃求解運算，并進行適應度結果對比。設定機器人各軸角速度限制設置為 ωmax≤2.88 rad/s2，角加速度限制設置為αmax≤45.45 rad/s2，計算各軸適應度曲線。如圖4所示，分別計算基本粒子群方法（方法1）、可變參數改進粒子群方法（方法2）和本文提出的融合免疫操作的改進粒子群方法（方法3）3種方案的各軸適應度迭代值。如表5所示，計算機械臂6個軸迭代100次適應度最終值f1～f6，6個軸適應度均值fave和六軸適應度最大值fmax。如表6所示，對比1表示方法2相較于方法1在適應度上的改進效果，對比2表示方法3相較于方法1在適應度上的改進效果，對比3表示方法3相較于方法2在適應度上的改進效果。s1～s6表示機器人6根軸的適應度優化率，save表示平均適應度優化率，smax表示最大適應度優化率。從適應度迭代結果來看，相較于傳統粒子群算法，可變參數改進粒子群算法在機器人路徑規劃問題中的適應度平均改進1.4倍，最大值適應度改進1.522倍；而本文所提出的融合免疫操作的粒子群算法相較于傳統粒子群算法在平均適應度上改進3倍，最大值適應度改進2.831倍。根據圖表結果，本文所提出的融合免疫操作的改進粒子群算法在路徑規劃問題中相較于傳統粒子群算法和傳統改進粒子群算法迭代效率更高，適應度優化更快，適應度結果更優秀。

表5 適應度值

表6 適應度優化率

圖4 機械臂適應度曲線

在前述最大適應度規劃條件下，進行機器人各軸路徑規劃如圖5所示，得到3種不同算法方案下機械臂6個軸的角位移、角速度、角加速度曲線。其中圖5a～c分別表示方法1、方法2和方法3的角位移曲線，角位移運算終值一致，3種方法均完成了路徑規劃，機械臂抵達指定位置；圖5d～f分別表示方法1、方法2和方法3的角速度曲線，3個圖中各軸角速度最大值均滿足最大角速度2.88 rad/s限制；圖5g～i分別表示方法1、方法2和方法3的角加速度曲線，3個圖中各軸角加速度最大值均滿足最大角加速度最大值均滿足最大角加速度45.45 rad/s2限制。機械臂規劃軌跡平滑，各軸運動角速度、角加速度連續且滿足最大運行角速度和角加速限制條件。

圖5 機械臂路徑規劃曲線

5 結語

本文為解決機械臂路徑規劃最短時間問題，提出了融合免疫操作的改進粒子群算法以改善傳統粒子群算法易陷入局部最優的問題，使用此算法與傳統粒子群算法和傳統可變參數改進粒子群算法在最短時間問題上進行求解對比。所提出的改進算法相較于傳統粒子群算法適應度更佳、收斂更快，大大改善了傳統粒子群算法易于早熟、容易陷入局部最優的劣勢，縮短了機械臂路徑規劃時間。

綜上所述，本文建立機器人運動學模型，利用分段多項式曲線進行軌跡規劃，通過融合免疫算法操作改進傳統粒子群算法，控制機器人動作，實現機器人軌跡控制，提高機器人工作效率，為工業機器人的空間軌跡規劃提供了一套思路方法。