王淑青， 王亞洲， 許 琛， 潘 健

(1 湖北工業(yè)大學(xué)電氣與電子工程學(xué)院，湖北 武漢 430068；2 內(nèi)蒙古電力(集團(tuán))有限責(zé)任公司烏蘭察布電業(yè)局，內(nèi)蒙古 烏蘭察布 012000)

改進(jìn)粒子群算法在機(jī)器人位置逆解上的應(yīng)用

王淑青1， 王亞洲1， 許 琛2， 潘 健1

(1 湖北工業(yè)大學(xué)電氣與電子工程學(xué)院，湖北 武漢 430068；2 內(nèi)蒙古電力(集團(tuán))有限責(zé)任公司烏蘭察布電業(yè)局，內(nèi)蒙古 烏蘭察布 012000)

為提高工業(yè)關(guān)節(jié)機(jī)器人位置逆解的快速性、準(zhǔn)確性且避免多解問題, 提出了一種改進(jìn)的粒子群算法。此算法直接從正運(yùn)動(dòng)學(xué)出發(fā)搜索數(shù)值解的逆解，并在邊界的產(chǎn)生、慣性權(quán)重的處理和超界的處理三個(gè)方面進(jìn)行了很好的改進(jìn)。單點(diǎn)仿真測(cè)試和連續(xù)軌跡仿真測(cè)試表明該改進(jìn)算法可以有效避免多解問題，同時(shí)具有良好的穩(wěn)定性、一定的實(shí)時(shí)性和較高的求解精度。此算法可用于實(shí)際的離線和在線求解，具有通用性和一定實(shí)用價(jià)值。

機(jī)器人； 逆運(yùn)動(dòng)學(xué)解； 粒子群算法； MATLAB仿真

工業(yè)機(jī)器人逆運(yùn)動(dòng)學(xué)求解的方法根據(jù)解的性質(zhì)可分為兩大類：解析法和數(shù)值法。反變換法[1]、幾何法[2]都屬于解析法。而基于搜索機(jī)制的各類智能優(yōu)化算法[3-6]、模擬輸入輸出關(guān)系的神經(jīng)網(wǎng)絡(luò)[7-8]、以及其它的迭代算法[9]則都屬于數(shù)值法。解析法在求解時(shí)存在多解，且機(jī)械臂結(jié)構(gòu)不符合Pieper準(zhǔn)則時(shí)，沒有解的表達(dá)式。其中的反變換法，表達(dá)式?jīng)]有固定的推導(dǎo)方法，而幾何法推導(dǎo)容易，但對(duì)于復(fù)雜結(jié)構(gòu)的機(jī)械臂，無法單獨(dú)求解；數(shù)值法在求解時(shí)，沒有結(jié)構(gòu)限制，但有多解、以及求解速度與精確性互斥等問題。

在數(shù)值法中，林明等[3]為避免多解使用了懲罰函數(shù)項(xiàng)的方法，但因?yàn)閼土P函數(shù)項(xiàng)取初始位置才最小，而目標(biāo)函數(shù)其余項(xiàng)取準(zhǔn)確解才最小。兩方博弈之下，最終的解只能收斂于準(zhǔn)確解和初始位置之間，使得求解精度不高；吳振宇等人[4]、金媛媛[5]和芮挺等人[6]分別將遺傳與粒子群、模擬退火與遺傳相結(jié)合。雖然使求解具有了較高的求解精度，但增加了算法復(fù)雜性，以致運(yùn)算量較大，求解速度不足，也沒有解決多解問題；DayaB.等人[7]和Jasim WM.[8]試圖以神經(jīng)網(wǎng)絡(luò)來逼近逆運(yùn)動(dòng)學(xué)的非線性關(guān)系，但由于其結(jié)構(gòu)對(duì)最終結(jié)果的不確定性及需要大量已知樣本進(jìn)行訓(xùn)練，在應(yīng)用中受到了很大的限制；王憲等人[9]使用牛頓拉夫遜迭代法算法，雖然具備了一定的求解速度，但92ms/次的求解速度還是無法滿足實(shí)時(shí)。本文采用單獨(dú)的粒子群算法，其結(jié)構(gòu)簡(jiǎn)單不需調(diào)整大量參數(shù)，且因?yàn)樗翘烊坏牟⑿兴惴?，使得?shí)現(xiàn)在線實(shí)時(shí)計(jì)算具有天然的優(yōu)勢(shì)，但簡(jiǎn)單的粒子群用于大量非線性、多峰值復(fù)雜問題的優(yōu)化時(shí)，容易陷入局部最優(yōu)。為了完成本文的求解任務(wù)，必須對(duì)簡(jiǎn)單的粒子群進(jìn)行改進(jìn)：“動(dòng)態(tài)狹小邊界”來約束多解，“非線性時(shí)變與陷局部最優(yōu)自校正結(jié)合”來增加準(zhǔn)確性和快速性，“超界帶弱方向性返回?cái)U(kuò)散與超界不限制結(jié)合”來增強(qiáng)快速性。

1 機(jī)器人模型

本文以此Puma560機(jī)器人為對(duì)象。其DH參數(shù)見表1。

表1 PUMA560的DH參數(shù)

其中a2=431.8，a3=20.32，d2=149.09，d4= 433.07

根據(jù)D-H方法，兩個(gè)相鄰坐標(biāo)系的4×4齊次變換矩陣為

i-1Ti=

(1)

把DH參數(shù)，帶入式(1)依次寫出各相鄰連桿的變換矩陣：

其中ci=cosθi，si=sinθi(i=1、2…6)。

最后，可以由式(2)得到機(jī)器人末端器的位姿矩陣：

0T6=0T1·1T2·2T3·3T4·4T5·5T6

(2)

2 改進(jìn)粒子群算法求解

2.1PSO應(yīng)用于位置逆解的問題描述

由于puma560的末端器的位置只與前三關(guān)節(jié)軸有關(guān)，故完全可以把位置求解分離出來，即僅研究前三關(guān)節(jié)θ1θ2θ3與p之間的關(guān)系。由正運(yùn)動(dòng)學(xué)式(2)可得，由前三關(guān)節(jié)變量映射到末端器的空間位置的非線性方程組為

(3)

式中，s1=sinθ1，s23=sin(θ2+θ3)，其余縮寫按同樣規(guī)則。位置逆解求解也就是在已知p后，根據(jù)方程(3)求解出θ1θ2θ3。

2.2 傳統(tǒng)粒子群的模型描述

粒子群優(yōu)化算法(PSO)最基本的算法模型是帶慣性權(quán)重的PSO模型。其模型描述為

(4)

i=x,y,z

(5)

2.3PSO算法的改進(jìn)

針對(duì)智能優(yōu)化算法解逆解時(shí)的最突出的三個(gè)方面的問題(快速性、準(zhǔn)確性、唯一性)，本文對(duì)傳統(tǒng)的粒子群優(yōu)化算法做出了三點(diǎn)改進(jìn)。

1)為了限制多解，保證唯一性，本文采用“動(dòng)態(tài)狹小邊界”法，其表達(dá)式為

(6)

其中，xmax和xmin是當(dāng)前的邊界向量，xlast是上一次的逆解向量。即每次求解空間都是以上次的解為中心上下擴(kuò)張2°為邊界的狹小空間，且隨著求解的進(jìn)行該空間在動(dòng)態(tài)移動(dòng)。由于在狹小的求解空間中不會(huì)存在大跳變的多解，所以在連續(xù)軌跡中求得的解的曲線不會(huì)因多解問題出現(xiàn)大跳變，而是較光滑的曲線。同時(shí)由于需要搜索的范圍變得很小，快速性也可以得到一定增強(qiáng)。

本方法可在真實(shí)機(jī)器人中應(yīng)用的現(xiàn)實(shí)依據(jù)：在做機(jī)器人軌跡規(guī)劃時(shí)，連續(xù)的CP控制都是用高速插值的方式來完成的。密集的笛卡爾空間插值，換算到關(guān)節(jié)空間對(duì)應(yīng)的關(guān)節(jié)變量的變化值是很小的(關(guān)節(jié)變量的變化的最大值可通過各機(jī)器人銘牌標(biāo)示的位置誤差和最大臂展值計(jì)算得到)，不會(huì)超過±1°，否則在空間軌跡上就會(huì)呈現(xiàn)出很大的誤差。所以±2°的范圍完全滿足求解需要。

2)為提高求解準(zhǔn)確性，本文提出“非線性時(shí)變權(quán)重與陷局部最優(yōu)自校正結(jié)合”的方法，表達(dá)式為

(7)

其中，i為迭代的次數(shù)，T為最大截止代數(shù)。條件①連續(xù)10代的適應(yīng)度值Q的變化量沒有超過10-5，條件②適應(yīng)度值Q還沒小于10-6(條件①確定粒子群已趨于收斂，條件②確定此時(shí)收斂的值還絕對(duì)不是全局最優(yōu)解)。

式0.5×exp(-i×((T-i+100)/T)/T)使ω隨迭代進(jìn)行從0.5衰減到0.22的處理，與其他的普通變慣性權(quán)重改進(jìn)算法的目的一樣，都是為了前期有較好的探索能力，后期有較好的開發(fā)能力。另外，本文在普通的變慣性權(quán)重改進(jìn)算法的基礎(chǔ)上增加了中期再次提升ω至0.6的處理。這樣做可以讓粒子群已經(jīng)趨于收斂局部最優(yōu)時(shí)，再次讓其小幅發(fā)散[10]，以此跳出局部最優(yōu)，增加求解準(zhǔn)確性。

3)為提高求解快速性，本文提出“超界帶弱方向性返回?cái)U(kuò)散”的方法，表達(dá)式為

(8)

式(8)中，rand為區(qū)間在[0,1]的隨機(jī)數(shù)，x為粒子的位置向量，xmax和xmin是當(dāng)前的邊界向量。在慣性權(quán)重較大時(shí)，粒子慣性較大，超界不加限制會(huì)出界較遠(yuǎn)，回到界內(nèi)可能要很久，這樣很多粒子的搜尋時(shí)間都浪費(fèi)在無效解上。且界外也存在全局最優(yōu)解，粒子還很可能收斂在界限外。所以每當(dāng)有粒子超界，則重新隨機(jī)擴(kuò)散到先前偏向的1/2區(qū)域，該處理方式既使得粒子重新隨機(jī)擴(kuò)散到解域，又保留了一定的方向性(現(xiàn)在分散為先前偏向的區(qū)域)。從理論上分析這樣既增強(qiáng)了粒子群的探索能力[10]，又盡可能不浪費(fèi)先前尋找成果來增加收斂速度；而慣性權(quán)重較小時(shí)，超界的粒子不多且不會(huì)過界太遠(yuǎn)，可以在短時(shí)間內(nèi)自行回來，所以慣性權(quán)重較小時(shí)不設(shè)界。

總結(jié)三個(gè)改進(jìn)的針對(duì)點(diǎn)，見表2。

表2 改進(jìn)粒子群總結(jié)

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

3.1 避免多解測(cè)試

“動(dòng)態(tài)狹小邊界”可以有效避免多解，保證唯一性。在笛卡爾空間中以任意一點(diǎn)為目標(biāo)，分別用本文改進(jìn)粒子群算法和僅去其改進(jìn)點(diǎn)1的算法進(jìn)行多次逆解求解。圖1就是取笛卡爾空間點(diǎn)[0.7601;0.1491;0]為目標(biāo)點(diǎn)得到的結(jié)果，圖1a為沒有使用“動(dòng)態(tài)狹小邊界”的解的情況，可以看到解出了4組解，圖1b是使用了“動(dòng)態(tài)狹小邊界”的解的情況，始終只有一組解?！皠?dòng)態(tài)狹小邊界”確實(shí)可以有效避免多解。

圖 1 位置逆解的分布圖

3.2 單點(diǎn)測(cè)試

普通改進(jìn)的例子群算法：c1=2,c2=2,T=120,N=60，機(jī)械可行范圍做邊界，ω=0.5×exp(-i×((T-i+100)/T)/T)為非線性時(shí)變，ω>0.41位置超界按本文處理，否則不限。

本文的改進(jìn)的粒子群算法：c1=2,c2=2,T=120,N=60，邊界采用改進(jìn)點(diǎn)1，ω采用改進(jìn)點(diǎn)2，超界采用改進(jìn)點(diǎn)3。

圖 2 單點(diǎn)測(cè)試20次的適應(yīng)度值收斂趨勢(shì)

現(xiàn)將兩種算法分別在普通pc機(jī)上運(yùn)行20次，記錄每次適應(yīng)度值隨著迭代次數(shù)的變化趨勢(shì)見圖2。從圖2a和b可以看出本文算法穩(wěn)定性、準(zhǔn)確性、收斂速度確實(shí)明顯優(yōu)于普通改進(jìn)粒子群算法：普通改進(jìn)粒子群算法收斂速度相對(duì)較慢，差些的需要60代才收斂到10-2以下，且容易陷入局部最優(yōu)解，目標(biāo)函數(shù)在0.1附近就是局部最優(yōu)解；而本文的改進(jìn)算法收斂迅速，40代以內(nèi)就收斂到10-3以下，每次收斂軌跡都很相似(穩(wěn)定性好)，且成功避免了普通改進(jìn)算法的局部最優(yōu)解。

3.3 連續(xù)軌跡測(cè)試

先做如下軌跡規(guī)劃：在可達(dá)空間任取起始位置pstart和終止位置pend，并已知起始位置對(duì)應(yīng)的關(guān)節(jié)角θstart。三個(gè)初始條件具體數(shù)據(jù)如下：

pstart=[0.1446,0.4527, -0.406]

θstart=[0.9425,-0.063,0.063 ]

pend=[0.361, -0.507, -0.224]

起點(diǎn)和終點(diǎn)之間的插值由robotics工具箱的函數(shù)產(chǎn)生，用ctraj()函數(shù)生成的1000個(gè)插值點(diǎn)，在笛卡爾空間中的形成軌跡見圖3。

圖 3 規(guī)劃出的空間軌跡

圖4a與b分別是Matlab工具箱自帶逆解函數(shù)與本文的改進(jìn)粒子群算法逆解出的各插值點(diǎn)對(duì)應(yīng)的前三關(guān)節(jié)的關(guān)節(jié)角度。橫坐標(biāo)是時(shí)間(4s內(nèi)有1000個(gè)插值點(diǎn))，縱坐標(biāo)是關(guān)節(jié)角度值。對(duì)比兩種方法的結(jié)果可以佐證本算法運(yùn)算結(jié)果的正確性。改進(jìn)點(diǎn)①可以讓解出來的逆解始終都與上一組解是最近的，可以保證唯一性，從而使得連續(xù)軌跡求解的值是連續(xù)光滑的，從圖4b中可見連續(xù)軌跡逆解的曲線是光滑的。

圖 4 Robotics toolbox逆解函數(shù)與本文算法的求解結(jié)果

改進(jìn)點(diǎn)1壓縮了搜索空間，使得搜索困難度降低。改進(jìn)點(diǎn)2可以有效避免陷入局部最優(yōu)解，增加準(zhǔn)確性。圖5為連續(xù)軌跡求解1000個(gè)插值點(diǎn)的目標(biāo)函數(shù)值，即每個(gè)逆解對(duì)應(yīng)的空間位置與實(shí)際期望的目標(biāo)空間位置的歐式距離，也就是誤差。從圖5中可知誤差已達(dá)到10-17數(shù)量級(jí)，而單點(diǎn)測(cè)試普通改進(jìn)粒子群僅達(dá)到了10-2，可見本文的改進(jìn)算法確實(shí)具有較高的準(zhǔn)確性。

圖 5 連續(xù)軌跡插值點(diǎn)對(duì)應(yīng)的目標(biāo)函數(shù)值

記錄該連續(xù)軌跡逆解的整個(gè)計(jì)算的耗時(shí)，除以求解次數(shù)1000得到解算平均速度為70.02ms/次，這樣的速度直接用于在線計(jì)算顯然不夠。但粒子群算法本身就是一種天然的并行算法，在每一代各個(gè)粒子的相應(yīng)計(jì)算獨(dú)立的。本文仿真受到Matlab的環(huán)境框架制約，每一代的每個(gè)粒子的計(jì)算必須挨個(gè)進(jìn)行，不能同時(shí)計(jì)算。所以如果把程序改編到FPGA或其他可以并行處理的芯片中，每一代所有的粒子的計(jì)算可以一次完成[11]。假設(shè)FPGA和普通PC處理速度一樣，那么計(jì)算速度變快為現(xiàn)在的60倍。即1.167ms一次，足以用于實(shí)時(shí)。

4 結(jié)論

為解決工業(yè)關(guān)節(jié)機(jī)器人位置逆運(yùn)動(dòng)學(xué)問題, 本文提出了一種用粒子群算法直接從正運(yùn)動(dòng)學(xué)出發(fā)搜索數(shù)值解的逆解算法。針對(duì)快速性、準(zhǔn)確性、唯一性進(jìn)行了三點(diǎn)改進(jìn)，并經(jīng)過仿真驗(yàn)證：本文的改進(jìn)粒子群算法能夠有效避免多解，保證解的唯一性；較高的求解精度，位置逆解誤差僅在10-17數(shù)量級(jí)；較快的求解速度，MATLAB仿真70.02ms一次，實(shí)際應(yīng)用時(shí)，理論上可以達(dá)到1.167ms。

[1] 蘭文寶. 基于PMAC的工業(yè)機(jī)器人控制系統(tǒng)研究與實(shí)現(xiàn)[D]. 哈爾濱：哈爾濱工程大學(xué), 2013.

[2] 李憲華，郭永存，張軍，等. 模塊化六自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)解算與驗(yàn)證[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2013，04:246-251.

[3] 林明，王冠，林永才. 改進(jìn)的遺傳算法在機(jī)器人逆解中的應(yīng)用[J]. 江蘇科技大學(xué)學(xué)報(bào)(自然科學(xué)版)，2012，04:370-375.

[4] 吳振宇，姚明江，馮林，等. 基于遺傳粒子群的PUMA機(jī)器人逆運(yùn)動(dòng)求解[J]. 計(jì)算機(jī)仿真，2011，10:173-176.

[5] 金媛媛. 機(jī)器人逆運(yùn)動(dòng)學(xué)的模擬退火自適應(yīng)遺傳算法研究[D]. 重慶：重慶大學(xué)，2007.

[6] 芮挺，朱經(jīng)緯，蔣新勝，等.PUMA機(jī)器人逆運(yùn)動(dòng)模擬退火粒子群求解方法[J]. 計(jì)算機(jī)工程與應(yīng)用，2010，03:27-29.

[7]DayaB,KhawandiS,AkoumM.ApplyingNeuralNetworkArchitectureforInverseKinematicsProbleminRobotics[J].JournalofSoftwareEngineering&Applications, 2010, 3(3):230-239.

[8]DayaB,KhawandiS,AkoumM.ApplyingNeuralNetworkArchitectureforInverseKinematicsProbleminRobotics[J].JournalofSoftwareEngineering&Applications, 2010, 3(3):230-239.

[9] 王憲，楊國梁，張方生，等. 基于牛頓-拉夫遜迭代法的6自由度機(jī)器人逆解算法[J]. 傳感器與微系統(tǒng)，2010，29(10):116-118.

[10] 潘峰，李位星，高琪著. 粒子群優(yōu)化算法與多目標(biāo)優(yōu)化[M]. 北京：北京理工大學(xué)出版社.2013.

[11] 羅犇，邵之江，徐祖華，等. 一種新的基于PSO的MPC及其硬件實(shí)現(xiàn)[J]. 控制工程，2013，20(02):227-230.

[責(zé)任編校： 張巖芳]

Robot Position Inverse kinematics based on improved PSO

WANG Shuqing1， WANG Yazhou1， XU Chen2， PAN Jian1

(1SchoolofElectricalandElectronicEngin.,HubeiUniv.ofTech.,Wuhan430068,China; 2UlaachabElectricPowerBureau,InnerMongoliaPower(Grope)CO.,LTD,Ulaanchab012000,China)

To avoid the problem of multiple solutions and enhance the performance of rapidity and accuracy in computing industrial robot position inverse kinematics, this paper presents an improved PSO. This algorithm searches for the numerical inverse solution from the forward kinematics directly and had a very good improvement on boundary generating, inertia weight handling and bound processing. Single-point and continuous path simulation tests show that the improved algorithm can effectively avoid the problem of multiple solutions, while having good stability, a certain degree of real-time and high precision solver. This algorithm can be used in offline and online practical solving and have versatile and practical value.

robot, inverse kinematics solution, PSO, MATLAB simulation

2016-03-07

王淑青(1969-)，女，河北衡水人，理學(xué)博士，湖北工業(yè)大學(xué)教授，研究方向?yàn)橛?jì)算機(jī)檢測(cè)與控制,電廠智能控制,系統(tǒng)分析與集成

1003-4684(2017)01-0046-05

TP242.2

A