賀倩倩，來躍深，劉夢軍，常 宏

(1.西安工業(yè)大學 機電工程學院，西安 710021； 2.深圳市北辰億科科技有限公司 西安研發(fā)中心，西安 710065)

基于粒子群算法的拆裝機器人逆解研究*

賀倩倩1，來躍深1，劉夢軍1，常 宏2

(1.西安工業(yè)大學 機電工程學院，西安 710021； 2.深圳市北辰億科科技有限公司 西安研發(fā)中心，西安 710065)

為了使連續(xù)生產(chǎn)線工件拆裝實現(xiàn)自動化，設計了一種滿足工業(yè)需求的關節(jié)型拆裝機器人，并對其樣機加工裝配。采用D-H法將機器人的三維模型轉化為連桿坐標系，通過坐標變換求出機器人的運動學正解。由于機器人結構中連桿關聯(lián)的原因，采用常規(guī)的逆變換法和幾何法不能求出逆解，因此提出以“目標點位姿誤差最小”為目標函數(shù)，各關節(jié)轉角極限為約束條件，利用粒子群優(yōu)化算法，對機器人的逆解求最優(yōu)解。最后，通過對機器人位姿點和連續(xù)運動軌跡進行逆解求解驗證并對其運動過程仿真分析。實驗結果表明該方法求逆解速度快、精度高，能夠滿足規(guī)劃的路徑，克服了傳統(tǒng)方法僅適應于特殊構型機器人的逆解的不足，解決了該機器人的逆解求解。

機器人；運動學；逆解；粒子群算法

0 引言

隨著工業(yè)生產(chǎn)中自動化改造的需求，工業(yè)機器人已廣泛應用于各個領域的工業(yè)現(xiàn)場。對于工業(yè)機器人的結構類型大部分為串聯(lián)型，其逆解求解過程是研究過程中的關鍵部分。不同結構類型的機器人求解方法也不同，通常的逆解求解方法包括逆變換法和幾何法[1]。逆變換法為較常用的方法，利用矩陣的逆變換分別乘以已知矩陣的方法，對應元素相等求解未知解并回帶得到各關節(jié)變量的值，其特點為計算量大，求解復雜，且只能針對特定結構的機器人。幾何法是利用機器人連桿之間的數(shù)學關系，轉化成求解平面幾何問題，適應于結構較簡單直觀的機器人。由于本文設計的拆裝機器人的特殊結構，用以上方法不能很好的求出逆解，因此，提出“目標點位姿誤差最小”為目標函數(shù)，各關節(jié)轉角極限為約束條件，利用粒子群優(yōu)化算法，對機器人的逆解求最優(yōu)解。

1 拆裝機器人結構設計

本文針對現(xiàn)有連續(xù)生產(chǎn)線上的工件焊接時進行拆裝的需求，設計了一臺拆裝機器人代替人工工作。其生產(chǎn)線運動如下：工件(銅管)整齊地排列在直線式鏈條上的工裝上，相鄰工件的距離為40mm。當鏈條以10mm /s的速度勻速運行，銅管通過感應器進行加熱焊接，焊接完畢機器人對其進行抓取，水平跟蹤運行鏈條的同時把工件拆出放置在固定的位置。

根據(jù)上述連續(xù)生產(chǎn)線的實際情況，需要設計滿足工作要求和尺寸要求的拆裝機器人，其機器人的工作半徑為400mm。根據(jù)實際要求對其設計加工組裝后得到拆裝機器人的實物模型，如圖1所示。機器人整體采用串聯(lián)型連桿結構[2]，主要包括底座和腰部的旋轉、大臂的擺動、小臂的擺動和手爪相對腕部的旋轉，共4個自由度。底座與腰部之間的旋轉為了滿足工件抓取后回轉放置功能；大臂安裝在腰部的兩側，擺動實現(xiàn)末端的前后移動；小臂通過連桿機構安裝在大臂的后端，擺動實現(xiàn)末端的上下移動；手爪安裝在腕部，繞其轉動滿足在抓取工件后保證水平跟蹤一段位姿；腕部是采用雙平行四邊形連桿結構控制運動過程中的水平性[3]。另外，各個驅動電機極限位置處都裝有接近開關，在工作過程中碰到接近開關后急停保證工作的安全性。

圖1 拆裝機器人實物圖

2 運動學分析

2.1 坐標系建立

采用D-H法[4]對上述的拆裝機器人建立坐標系，如圖2所示。取機器人底座的中點為原點坐標系，其他的坐標系在相鄰關節(jié)連接處，相鄰的坐標系之間的關系用坐標變換矩陣來描述，最終得到末端點關于原點坐標系的矩陣變換。

圖2 拆裝機器人連桿坐標系

根據(jù)連桿長度參數(shù)與坐標系得到拆裝機器人的連桿變換參數(shù)，如表1所示。

其中：αi-1表示連桿扭轉角；ai-1表示連桿的長度；θi表示連桿之間的夾角；di表示連桿之間的距離；d1=138mm，d2=135mm，d3=160mm，d4=49.5mm，d5=19mm，d6=30mm。

表1 連桿坐標變換參數(shù)表

2.2 運動學正解

機器人的運動學正解指輸入各關節(jié)的轉動角度參數(shù)，得到末端的位姿。機器人連桿變換矩陣的通用表達式：

(1)

將表1中的參數(shù)代入公式(1)中,根據(jù)坐標變換公式0T5=0T1·1T2·2T3·3T4·4T5得，末端相對于原點的坐標變換矩陣為：

其中:

px=d6·c0·(c12·c3-s12·s3)·c4-d6·s0·s4+d4·c0·
c12·c3-d4·c0·s12·s3+d5·c0·c12·s3+d5·c0·
s12·c3+d5·c0·c1·c2-d5·c0·s1·s2+d2·c0·c1

(2)

py=d6·(s12·c3+c12·s3)·c4+d1+d4·s12·c3+d4·
c12·s3+d5·s12·s3-d5·c12·c3+d3·s1·c2+d3·
c1·s2+d2·s1

(3)

pz=d6·s0·(s12·s3-c12·c3)·c4-d6·c0·s4-d4·s0·
c12·c3+d4·s0·s12·s3-d5·s0·c12·s3-d5·s0·
s12·c3-d3·s0·c1·c2+d3·s0·s1·s2-d2·s0·c1

(4)

式中：si=sinθi;ci=cosθi;s12=c1·s2+s1·c2；

c12=c1·c2-s1·s2；矩陣0T5為機器人末端的位姿；Px,Py,Pz為機器人末端的位置坐標。

3 運動學逆解

機器人的逆解指的是已知機器人末端的位姿求出每個關節(jié)對應的角度，是機器人控制中必不可少的一部分。根據(jù)上述介紹的機器人結構中的連桿關聯(lián)結構導致不能用幾何法和解析法求出機器人的逆解，因此本文提出“目標點位姿誤差最小”為目標函數(shù)，各關節(jié)轉角極限為約束條件，利用粒子群優(yōu)化算法，對機器人的逆解求最優(yōu)解[5-6]。

3.1 建立數(shù)學模型

根據(jù)上述提出的“目標點位姿誤差最小”的求解目標，利用求出的機器人正解的末端位姿公式與期望的位姿的距離建立數(shù)學模型，其中位姿包括位置和姿態(tài)，由于位置和姿態(tài)在數(shù)量級上的差距，引入懲罰因子使其滿足整體誤差最小[7-8]。其目標函數(shù)為：

s.t.Px(θ0,…θ4)=Px;Py(θ0,…θ4)=Py;
Pz(θ0,…θ4)=Pz;F-160°≤θ0≤160°;10°≤θ1≤90°;
-145°≤θ2≤45°;θ3=-θ1-θ2;-120°≤θ4≤120°

(5)

3.2 算法描述

本文采用粒子群算法[9]對上述目標函數(shù)求解，首先定義一群滿足約束條件的隨機解，通過每一組解在一次迭代中的求得當前最優(yōu)值和所有組粒子的全局最優(yōu)值的比較來確定滿足目標函數(shù)的對應解。粒子的更新通過下面的速度更新式子和位置更新式子：

v(i)=w·v(i)+c1·rand()·(y(i)-x(i))+c2·
rand()·(pg-x(i))
x(i)=x(i)+v(i)

(6)

其中,v(i)為第i個粒子的速度，w為慣性權重，c1，c2為學習因子，rand()為0-1之間的隨機數(shù)，y(i)為第i個粒子的當前最優(yōu)值，x(i)為第i個粒子的當前位置，pg為全局的最優(yōu)值。

利用粒子群算法求解機器人逆解的具體步驟流程圖如圖3所示。

圖3 逆解算法流程圖

利用上述算法，采用粒子群尋優(yōu)思想，求出一組位姿對應的逆解，在matlab中編程驗證。

4 驗證

拆裝機器人的運動學逆解通過以下兩方面進行驗證。一方面是隨機取一組機器人末端位姿，通過在matlab中對逆解算法編程，經(jīng)過多次運行，觀察結果的誤差范圍；另一方面對其一個連續(xù)運動軌跡進行規(guī)劃，求出其逆解并在adams中進行驅動函數(shù)添加，得到末端位移曲線，驗證其是否滿足軌跡要求[10]。

4.1 位姿點驗證

取機器人末端一組位姿Px=247.3,Py= 155.9,Pz= -142.8,nx=0.5,oy=0.732,取粒子群數(shù)為100，迭代次數(shù)50次，運行5次后，收斂曲線如圖4所示，逆解結果如表2所示。

圖4 適應度值收斂曲線

角度12345θ029．999330．002930．006730．002929．9999θ160．000459．988759．997459．999259．9977θ2-90．0008-89．9997-89．9963-89．9976-89．9985θ40．0098-0．0325-0．0637-0．0205-0．0039f1．3422e-82．6762e-78．9268e-71．9257e-71．4159e-7

從上述的結果可以看出每次運算各個關節(jié)角度的誤差較小，都接近一個值，且整體位姿誤差可達e-7,該位姿對應的關節(jié)的角度分別為θ0=30°,θ1=60°,θ2=-90°，θ3=30°,θ4=0°，且目標函數(shù)即適應度值的曲線有很好的收斂性。

4.2 連續(xù)軌跡驗證

根據(jù)機器人實際運動的需求，對其運行路徑做分段規(guī)劃，其運動時間，末端坐標以及對應的角度如下表3所示，其中角度利用上述的求逆解方法得出的。

表3 路徑規(guī)劃逆解結果

根據(jù)表3中的路徑規(guī)劃，對其整個運動過程進行adams仿真分析，將表3求出的逆解添加在機器人的各關節(jié)的驅動函數(shù)中，整個仿真時間為4s,后處理后得到機器人末端點的x,y,z方向位移曲線如圖5所示。

圖5 末端的位移曲線

從圖5中可以看出機器人末端的x,y,z方向位移坐標均滿足整個運動規(guī)劃過程，驗證了利用上述算法求解的逆解的正確性。

5 結論

本文針對項目合作公司提出的需求設計了適應于特定生產(chǎn)線的拆裝機器人，解決了特殊行業(yè)的工件拆裝長期由人工完成的效率低、危害大、成本高的問題。其次，提出了適應于這種關聯(lián)性結構機器人的逆解求解方法，可應用于不能用常規(guī)方法很好的求出逆解的機器人。最后，設計了逆解求解算法，并編程實現(xiàn)隨機位置逆解求解驗證后，能夠保證末端達到預期的位置。該方法相對于傳統(tǒng)的求解逆解方法，其計算量較小，舍去了繁雜的公式推導；可運用于特殊構型的機器人逆解求解中；求解速度快，算法有很好的收斂性，結果滿足精度要求。

[1] 王光道，劉蔭忠，孫維堂.基于加權優(yōu)化的機器人逆向運動學求解[J].組合機床與自動化加工技術，2016(5):1-3.

[2] 葉辰雷.碼垛機器人的結構優(yōu)化及軌跡規(guī)劃研究[D].北京:北京郵電大學,2014.

[3] 姚猛，韓寶玲，羅慶生. 工業(yè)碼垛機器人機構設計與運動學分析[J]. 組合機床與自動化加工技術, 2011(5):31-33.

[4] 蔡自興.機器人學[M].北京：清華大學出版社,2009.

[5] Y Wang,L Sun,W Yan,et al. An Analytic and Optimal Inverse Kinematic Solution for a 7-DOF Space Manipulator[J]. Robot,2014(5):592-599.

[6] 王戰(zhàn)中,楊長建,劉超穎,等.MATLAB環(huán)境下六自由度焊接機器人運動學逆解及優(yōu)化[J].機械設計與制造，2013(7):182-184.

[7] 孫恒輝，趙愛武，李達,等. 基于新旋量子問題改進一類6R串聯(lián)機器人逆解算法[J].機械工程學報,2016,52(1):79-86.

[8] Damas B,Santors-Victor J.An online algorithm for simultaneously learning forward and inverse kinematics[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, USA: IEEE, 2012: 1499-1506.

[9] 劉衍民.一種求解約束優(yōu)化問題的混合粒子群算法[J].清華大學學報(自然科學版),2013，53(2):242-246.

[10] 李愛成，唐火紅，馮寶林. 關節(jié)式碼垛機器人運動學分析與動力械設計,2013,30(10):16-20.

StudyontheInverseKinematicsofDisassemblingRobotBasedonParticleSwarmOptimization

HE Qian-qian1,LAI Yue-shen1,LIU Meng-jun1,CHANG Hong2

(1.School of Mechatronic Engineering,Xi′an Technological University,Xi′an 710065,China; 2.Xi′an R & D Center, PO LARIS ETek, Xi′an 710065，China)

To realize automation for artifacts disassembled in continuous production line, designed an articulated disassembling robot which meet industrial requirements, and the prototype is processed and assembled.The three dimensional model of the robot is transformed into the link coordinate system by using D-H method, forward kinematics of the robot is obtained by means of coordinate transformation. The reason for the connection of the robot links in the structure, inverse solution can′t be obtained by using the conventional inversion method and the geometric method. Regarded “the error of target′s position-orientation is minimum” as objective function is proposed, the limit of each joint angle is the constraint condition，using particle swarm optimization algorithm to find optional solution of the robot′s inverse solution. Finally, the inverse solution is verified by the robot pose and the continuous trajectory, then simulate and analyse the motion process. The results of the experimental show that the method is fast and accurate, be able to meet the planning path, overcome the shortcomings of traditional methods which only adapt to the inverse solution of special robot, the inverse solution of the robot is solved.

robot; kinematics; inverse solution; particle swarm optimization

1001-2265(2017)11-0008-03

10.13462/j.cnki.mmtamt.2017.11.003

2017-01-23；

2017-03-17

陜西省科學技術廳工業(yè)攻關項目(2014k06-26)

賀倩倩(1994—)，女，西安人，西安工業(yè)大學碩士研究生，研究方向為機電系統(tǒng)設計及控制，(E-mail)18392066086@163.com。

TH165;TG659

A

(編輯李秀敏)