許 健， 梅江平， 段曉斌， 羅振軍， 陳落根

(1.天津大學(xué) 機構(gòu)理論與裝備設(shè)計教育部重點實驗室， 天津 300355；2.杭州娃哈哈集團(tuán)有限公司 機電研究院， 浙江 杭州 310020)

一種工業(yè)機器人連續(xù)軌跡規(guī)劃過渡算法

許 健1， 梅江平1， 段曉斌1， 羅振軍2， 陳落根2

(1.天津大學(xué) 機構(gòu)理論與裝備設(shè)計教育部重點實驗室， 天津 300355；2.杭州娃哈哈集團(tuán)有限公司 機電研究院， 浙江 杭州 310020)

提出一種用于工業(yè)機器人操作空間連續(xù)軌跡規(guī)劃的平滑曲線過渡算法，以提高工業(yè)機器人在連續(xù)軌跡過渡時的運動速度.該算法的主要目的是在保證連續(xù)軌跡過渡精度和工業(yè)機器人物理約束的條件下充分發(fā)揮工業(yè)機器人的能力，盡量提高過渡區(qū)域的速度，減少過渡所需時間，同時盡可能地使計算過程簡便，以便用于在線規(guī)劃，提高算法的通用性.該算法以給定速度為零的路徑銜接點為基準(zhǔn)，根據(jù)過渡半徑參數(shù)得到2個速度不為零的過渡節(jié)點，在過渡節(jié)點之間采用一種有限項的正弦級數(shù)進(jìn)行曲線擬合.而在非過渡區(qū)域只需采取經(jīng)典的起止速度及加速度滿足約束值的直線軌跡規(guī)劃算法或圓弧軌跡規(guī)劃算法.通過MATLAB仿真和一種兩自由度高速并聯(lián)機械手的實驗驗證了該算法的效果.該算法可以實現(xiàn)典型路徑的在線軌跡規(guī)劃，且軌跡通常不隨速度參數(shù)的更改而變化，計算過程簡便，故具有較大的應(yīng)用價值和發(fā)展空間.

連續(xù)軌跡規(guī)劃； 操作空間； 工業(yè)機器人； 路徑過渡

工業(yè)機器人軌跡規(guī)劃算法是保證工業(yè)機器人實現(xiàn)穩(wěn)定運動的核心技術(shù).對于弧焊等機器人來說，通常要求末端執(zhí)行器盡量以給定速度沿著指定示教路徑前進(jìn)，并保證在相鄰路徑之間能夠自動平滑過渡.為解決路徑平滑過渡這一難題，最早采用的是基于樣條曲線插補等方式的關(guān)節(jié)空間軌跡規(guī)劃算法[1-3]，此類算法具有約束條件少和計算速度快等優(yōu)勢，但同時存在空間軌跡不直觀和軌跡形狀會隨速度不同而改變等缺點，因此逐漸被機器人廠商棄用.近年來，國內(nèi)外學(xué)者大多轉(zhuǎn)向了操作空間軌跡規(guī)劃算法的研究.常見的工作空間軌跡規(guī)劃算法主要有3類：第1類是卷積類算法，該方法通常是給定混疊時間參數(shù)，對路徑進(jìn)行均勻離散化后采用卷積計算來實現(xiàn)加減速規(guī)劃，但此類算法存在著很多與關(guān)節(jié)空間軌跡規(guī)劃算法類似的缺點[4].第2類則是混疊類算法，包括速度混疊算法[5]和位置混疊算法[6-7]，此類算法一般需要給定加減速時間，然后通過線性、多項式或者擺線等函數(shù)形式的同倫變形獲得速度曲線或者位置曲線，缺點是加速度約束涉及時間變量，需要復(fù)雜的處理流程，其中文獻(xiàn)[7]是對ABB公司TrueMove技術(shù)的一次不完全嘗試.最后一類算法是規(guī)劃類算法，包括速度規(guī)劃算法[8]和位置規(guī)劃算法[9-10]，這一類算法通過設(shè)計過渡曲線的數(shù)學(xué)表達(dá)式來實現(xiàn).其中速度規(guī)劃算法計算流程復(fù)雜，且難以保證軌跡不隨速度參數(shù)的更改而變化.而位置規(guī)劃算法的研究中， Siciliano等將關(guān)節(jié)空間的位置規(guī)劃算法擴展到笛卡爾空間，獲得了拋物線形狀的過渡軌跡，使得加速度約束不再取決于時間常數(shù)，但是仍存在加速度矢量方向突變的問題[9].林仕高等提出五次多項式位置規(guī)劃過渡算法，但是因加速度約束是涉及時間變量的非線性函數(shù)而未能找到加速度約束處理方案[10].此外，還有一些研究通過先選取過渡路徑，然后將其細(xì)分為微小路徑段，最后采用前瞻算法實現(xiàn)速度平滑和軌跡修形，然而此類算法計算復(fù)雜，難以保證在高速運動條件下的實時性[4，11].

為此，提出一種計算效率高的基于有限項正弦級數(shù)的新型位置規(guī)劃算法來解決上述問題.

1 節(jié)點配置

操作空間中典型的連續(xù)軌跡包括直線段與直線段、直線段與圓弧段以及圓弧段與圓弧段間的銜接.如圖1所示，為簡化論述，取平面直線段Path1與直線段Path2間的銜接為代表進(jìn)行本文算法的論述，而空間連續(xù)軌跡可以很輕松地從平面算法中導(dǎo)出.

圖1 帶過渡的典型軌跡Fig.1 Typical trajectory with transition

其中：S0為Path1的起始點，S1為Path1和Path2的銜接點，S2為Path2的結(jié)束點；r1為S1處的過渡半徑；P1和P2分別為以S1為圓心、r1為半徑的圓與Path1和Path2相交而得到的過渡節(jié)點；v1和v2則分別為沿Path1和Path2的巡行速度；k1為過渡節(jié)點處待優(yōu)化的速度比例系數(shù)，且一般要求0

首先設(shè)t為完成過渡所需要的時間，點P1和P2的位置矢量分別用p0和pT表示.

采用各軸分別進(jìn)行平滑過渡的算法，故將相對位置、速度和加速度矢量分別向坐標(biāo)軸投影以獲得分量.該算法假設(shè)所有過渡節(jié)點的切向加速度均為0，從而使后續(xù)推導(dǎo)大為簡化.過渡節(jié)點P1處的相對位置、速度和加速度的坐標(biāo)軸分量配置如下：

同理，可得過渡節(jié)點P2處的相對位置、速度和加速度的坐標(biāo)軸分量配置為

由上述可知，在確定k1之后，過渡節(jié)點的實際配置將隨之確定.對于非過渡區(qū)域的軌跡只需采用經(jīng)典的起止點的速度和加速度為給定值的直線段或圓弧段的軌跡規(guī)劃算法[9-10，12-17]即可.由此可見本文過渡算法的關(guān)鍵在于確定k1.以下將具體闡述算法的原理和計算步驟.

2 算法原理

對于每一個坐標(biāo)軸方向，引入基于有限項正弦級數(shù)的位置規(guī)劃函數(shù)：

(1)

則過渡節(jié)點的運動學(xué)約束方程組如下：

(2)

為保證上述方程組解的確定性，方程數(shù)量與待定系數(shù)數(shù)量需相同，選取ns=3，ne=5.

考察待優(yōu)化的速度比例系數(shù)k1，易知其與過渡總時間T成反相關(guān)，定義為

(3)

式中s=4r1(v1+v2)-1.將式(3)代入式(2)的約束方程組，借助正弦函數(shù)的特點可消去k1，得到

Z=CV，

(4)

式中：

oi，n=2-n(i-1)un，u=πs-1，

pi，n=oi，nsin 2-(i-1)π，qi，n=oi，ncos 2-(i-1)π，

vij=Aijsin φij，wij=Aijcos φij.

故由V=C-1Z可解得向量V.而由向量V可得

(5)

下面進(jìn)一步考慮加速度性能約束.

將式(1)進(jìn)行二次求導(dǎo)可得加速度表達(dá)式為

取φ=k1ut/16，有φ∈[0，π/16]，可得

(6)

其中：

可以看出，在給定|aj(φ)|max的允許值ajmax后，對于每一個坐標(biāo)軸j，分別存在一個k1滿足式(6)，將其記為k1j，則對應(yīng)有

(7)

為獲得k1j，并避免繁雜的數(shù)值判斷，可采用近似計算方法計算|ρj(φ)|max.將ρj(φ)表達(dá)式中的正弦和余弦函數(shù)用下面的冪級數(shù)前3項近似：

可得近似表達(dá)式為

(8)

上式為五次多項式，其系數(shù)bjl為常量，求導(dǎo)可得四次多項式.可以通過解析法得到式(8)的極值，考慮邊界情況，進(jìn)而可獲得ρj(φ)的近似最大值和近似最小值，分別記為ρjmax和ρjmin.為確認(rèn)上述近似方法的有效性，需評估冪級數(shù)截斷所產(chǎn)生的近似誤差，定義為

(9)

可得近似誤差如圖2所示.

圖2 冪級數(shù)近似公式的誤差Fig.2 Errors of approximate power series

(10)

由此解得

(11)

則k1值由下式取得：

(12)

3 算法步驟

基于上述理論可以建立過渡區(qū)域的過渡算法，步驟如下：

1)根據(jù)式(4)和式(5)求解φij和Aij.

2)根據(jù)式(8)求解ρjmax和ρjmin.

3)根據(jù)式(11)和式(12)分別求解k1j和k1.

4)計算基本過渡曲線為

(13)

由式(11)可知，若對v1和v2進(jìn)行同比變化，則k1與速度成反比，使得過渡節(jié)點配置不變，進(jìn)而可知幅值系數(shù)與相位系數(shù)均不變，因此過渡軌跡的路徑和速度剖面形狀等均不改變.

5)如果(v1+v2)過小或者r1過大，可能造成k1>1.若k1>1，則可對式(13)進(jìn)行時間縮放處理：

(14)

即：將過渡時間擴大k1倍，使速度縮小k1倍,而過渡軌跡的路徑形狀不發(fā)生改變.

6)將所得相對位置矢量變換成絕對位置矢量即可得到最終過渡曲線.

但需注意，如果k1?1，則上述方法會使得過渡速度最小值過小，不利于保持運動過程中速度的相對穩(wěn)定.因此在實際機器人控制器中仍需提供類似于文獻(xiàn)[4，8，11]的分段細(xì)化和速度前瞻算法以處理(v1+v2)過小的情況.而對于r1過大的情況，通常意味著示教程序質(zhì)量不高，需要進(jìn)行一些局部調(diào)整，例如插入圓弧或增加示教點等.

4 仿真與實驗

采用辰星(天津)自動化設(shè)備有限公司的D2-350兩自由度高速并聯(lián)機械手[18-19]為工業(yè)機器人模型，并以門式軌跡(bw=305 mm，hw=25 mm)[19]為典型軌跡在MATLAB軟件上進(jìn)行算法的仿真測試.

D2-350兩自由度高速并聯(lián)機械手如圖3所示[19]，其操作空間為水平和鉛錘方向所形成的平面，具體參數(shù)見表1[19].

圖3 D2-350機械手Fig.3 The D2-350 manipulator

長度/mm高度/mm最大速度/(mm/s)最大加速度/(mm/s2)3501502004000

給定門式軌跡如圖4所示，其基本參數(shù)見表2，參考D2-350機械手設(shè)定過渡參數(shù),見表3.

圖4 給定門式軌跡Fig.4 The given gantry trajectory

b/mmbw/mmh/mmhw/mm35030515025

表3 門式軌跡過渡參數(shù)

由于該算法主要用于過渡節(jié)點的選取以及過渡曲線的生成，為更直觀地分析該算法的特性，以下主要對P1點處過渡區(qū)域進(jìn)行詳細(xì)分析.

經(jīng)過計算可得k1=0.36，則P1點處過渡區(qū)域的相對位置、速度、加速度和過渡軌跡的曲線分別如圖5、圖6、圖7和圖8所示.

圖5 P1點處過渡區(qū)域相對位置-時間曲線Fig.5 Relative position-time curve in P1 transition section

圖6 P1點處過渡區(qū)域速度-時間曲線Fig.6 Velocity-time curve in P1 transition section

圖7 P1點處過渡區(qū)域加速度-時間曲線Fig.7 Acceleration-time curve in P1 transition section

從P1過渡區(qū)域加速度曲線可以看出，用該算法求取的過渡曲線的加速度完全滿足所施加的物理約束，說明該算法可以達(dá)到預(yù)期效果.

圖8 P1點處過渡軌跡Fig.8 The P1 transition trajectory

為驗證仿真結(jié)果，進(jìn)行實驗測試.本測試采用擺線加減速方式實現(xiàn)非過渡區(qū)域軌跡規(guī)劃，同時，由于高速運動需要很高的采樣頻率，故采用編碼器反饋的關(guān)節(jié)轉(zhuǎn)角進(jìn)行運動學(xué)正解計算得到實際過渡軌跡.實際過渡軌跡、速度和加速度曲線分別如圖9、圖10和圖11所示，采樣時間為1 ms.

圖9 實際過渡軌跡與計算過渡軌跡對比Fig.9 The contrast of actual transition trajectory and computed transition trajectory

圖10 過渡區(qū)域?qū)嶋H速度-時間曲線Fig.10 The actual velocity-time curve in transition section

圖11 過渡區(qū)域?qū)嶋H加速度-時間曲線Fig.11 The actual acceleration-time curve in transition section

由圖9可知實際過渡軌跡與計算過渡軌跡基本吻合，而由圖11可知其加速度滿足機械手物理約束條件，故該算法得到驗證.

同時，用戶可以通過調(diào)整過渡半徑r1，得到不同的過渡軌跡，圖12所示過渡軌跡簇中的過渡軌跡分別為r1=2，3，4，5 mm時所生成的過渡軌跡.

圖12 過渡軌跡簇Fig.12 The transition trajectory cluster

如圖13所示：過渡軌跡1為v1=v2=200 mm/s時的過渡軌跡，而過渡軌跡2為v1=v2=100 mm/s時的過渡軌跡，兩者完全重合，證明在速度等比例變化時，所得過渡軌跡不變.

圖13 速度等比例變化過渡軌跡對比Fig.13 Comparison of transition trajectories with proportion change of velocities

5 結(jié) 論

本文給出了一種工業(yè)機器人操作空間連續(xù)軌跡規(guī)劃的過渡算法，該算法的主要目的是在保證連續(xù)軌跡過渡精度和工業(yè)機器人物理約束的條件下充分發(fā)揮工業(yè)機器人的能力，盡量提高過渡區(qū)域的速度，減少過渡所需時間，同時盡可能地使計算過程簡便，以便用于在線規(guī)劃，提高算法的通用性.通過MATLAB仿真和兩自由度高速并聯(lián)機械手的實驗驗證，證實了該算法能夠在保證連續(xù)軌跡平滑過渡的同時，保證其物理約束條件.此外，該算法具備在速度等比例變化的條件下過渡軌跡不變的特性，這種特性使其能夠更廣泛地應(yīng)用于機器人應(yīng)用領(lǐng)域.此外，該算法是位置規(guī)劃方法，容易計入位置約束條件，因此其可擴展性良好.后續(xù)研究將對姿態(tài)配置進(jìn)行類似處理，進(jìn)而實現(xiàn)包含空間姿態(tài)的軌跡過渡，推動位置規(guī)劃類連續(xù)軌跡規(guī)劃方法的創(chuàng)新和發(fā)展.

[1] FU K S, GONZALEZ R C, LEE C S G. Robotics: control, sensing, vision, and intelligence[M]. Blacklick, Ohio: McGraw-Hill Book Company, 1987: 149-200.

[2] 劉承立，欒楠. 關(guān)節(jié)空間樣條的連續(xù)點位運動規(guī)劃算法[J]. 機械與電子，2014(8)：74-78． LIU Cheng-li， LUAN Nan. Continuous point to point motion planning algorithm based on splines in joint space[J]. Machinery & Electronics, 2014(8): 74-78．

[3] 張斌. 基于多約束的機器人關(guān)節(jié)空間軌跡規(guī)劃[J]. 機械工程學(xué)報，2011，47(21): 1-6． ZHANG Bin. Joint-space trajectory planning for robots under multiple constraint[J]. Journal of Mechanical Engineering, 2011， 47(21): 1-6．

[4] SUH S-H， KANG S-K. Theory and design of CNC systems[M]. London: Springer-Verlag, 2008: 107-156.

[5] VOLPE R. Task space angular velocity blending for real-time trajectory generation: US 5602968[P]. 1997-02-11．

[6] LLOYD J， HAYWARD V. Real-time trajectory generation using blend functions [C]//Proceedings of IEEE International Conference on Robotics and Automation. Sacramento， CA, Apr.9-11， 1991: 784-789．

[7] USTYAN T， J?NSSON V. Implementation of a generic virtual robot controller [D]. Sweden: Chalmers University of Technology, Department of Signals and Systems, Division of Automatic Control, Automation and Mechatronics, 2011: 8-31.

[8] KOICHI Funaya. Method of and apparatus for robot tip trajectory control: US 5740327A[P]. 1998-04-14．

[9] SICILIANO B， SCIAVICCO L， VILLANI L. Robotics: modelling， planning and control [M]. 3th ed. London: Springer-Verlag, 2009: 161-190.

[10] 林仕高，劉曉麟，歐元賢. 機械手笛卡爾空間軌跡規(guī)劃研究[J]. 機械設(shè)計與制造，2013(3)：49-52． LIN Shi-gao， LIU Xiao-lin， OU Yuan-xian. The study of trajectory planning of manipulators in Cartesian space[J]. Machinery Design & Manufacture， 2013(3):49-52．

[11] 王允航. 連續(xù)軌跡的 S 型速度規(guī)劃的研究[D].深圳：哈爾濱工業(yè)大學(xué)深圳研究生院，2010：5-46． WANG Yun-hang. Research on S-shaped continuous trajectory planning [D]. Shenzhen: Harbin Institute of Technology, Graduate School at Shenzhen, 2010: 5-46.

[12] SONJA Macfarlane， ELIZABETH A Croft. Jerk-bounded manipulator trajectory planning: design for real-time applications[J]. IEEE Transactions on Robotics and Automation, 2003, 19(1):42-52．

[13] LUIGI Biagiotti， CLAUDIO Melchiorri. Trajectory planning for automatic machines and robots[M]. Berlin: Springer-Verlag, 2008: 341-414.

[14] JEON J W， HA Y Y. A generalized approach for the acceleration and deceleration of industrial robotics and CNC machine tools[J]. IEEE Transaction on Industrial Electronics， 2000， 47(1): 133-139．

[15] CUI J， CHU Z Y. An improved approach for the acceleration and deceleration of industrial robots and CNC machine tools[C]//Proceedings of the 2005 IEEE International Conference on Industrial Technology. Hong Kong, Dec. 14-17, 2005: 1269-1273.

[16] JEON J W. Efficient acceleration and deceleration technique for short distance movement in industrial robots and CNC machine tools[J]. Electronics Letters, 2000,36(8): 766-768．

[17] NG Y K， XIE Y， WANG C， et al. Development of a new velocity profile generation for improvement of CNC Machining Efficiency[C]//Proceedings of the 2008 IEEE International Conference on Mechatronics and Automation. Takamatsu, Aug. 5-8, 2008: 313-318.

[18] HUANG Tian， LI Zhan-xian， LI Meng， et al. Conceptual design and dimensional synthesis of a novel 2-DOF translational parallel robot for pick-and-place operations[J]. Journal of Mechanical Design, 2004， 126(3): 449-455．

[19] 辰星(天津)自動化設(shè)備有限公司. D2-350技術(shù)參數(shù)[EB/OL]. (2015-09-03)[2016-03-27]. http://www.cxrobot.com/gsby/1203_30.html. Chenxing (Tianjin) Automation Equipment Co., Ltd.. The technical parameters of D2-350[EB/OL]. (2015-09-03)[2016-03-27]. http://www.cxrobot.com/gsby/1203_30.html.

An algorithm for segment transition in continuous trajectory planning of industrial robot

XU Jian1， MEI Jiang-ping1， DUAN Xiao-bin1， LUO Zhen-jun2， CHEN Luo-gen2

(1. Key Laboratory of Mechanism Theory and Equipment Design of Ministry of Education, Tianjin University，Tianjin 300355， China; 2. Mechanical and Electrical Institute, Hangzhou Wahaha Group Co., Ltd.， Hangzhou 310020， China)

A new algorithm for obtaining concatenation curves between segments in operational space related to continuous trajectory planning of industrial robots is introduced, which is to improve motion speeds of industrial robots when they transit between segments. The main aim of the algorithm was to make full use of industrial robot, try to improve their speed and reduce the time needed in transition section with ensuring the precision in transition section and the physical constraints of industrial robot. And the computation procedure of the algorithm should be as simple as possible, so as to be easy for the online trajectory planning and improve the generality of the algorithm. The algorithm used the via-point as a reference to obtain two transition points according to the specified transition radius, and each transition point had a non-zero velocity. A novel finite sine series function was used to interpolate the trajectory between the transition points, while conventional linear or circular trajectory planning algorithms with boundary points satisfying velocity and acceleration constraints could be employed to interpolate the trajectory between transition points and other points. The effect of the algorithm was verified by simulation in MATLAB and experiment in a 2-DOFs high speed parallel manipulator. The algorithm can realize the online trajectory planning of typical robot paths. Its trajectory is fixed even when path velocity is modified, and its computation procedure is relatively simple, and hence has much potential in many applications.

continuous trajectory planning; operational space; industrial robot; path transition

2016-03-03.

本刊網(wǎng)址·在線期刊：http://www.zjujournals.com/gcsjxb.

國家自然科學(xué)基金資助項目(51135008).

許健(1991—)，男，天津人，碩士生，從事機器人控制系統(tǒng)研究，E-mail：kk53623@sina.com. http://orcid.org//0000-0003-2141-6104 通信聯(lián)系人：羅振軍(1976—)，男，湖南桂陽人，博士，總工程師，原天津大學(xué)副教授，碩士生導(dǎo)師，從事機器人機構(gòu)理論、機器人控制系統(tǒng)以及機電產(chǎn)品數(shù)字化設(shè)計等研究，E-mail：zhenjunluo@hotmail.com. http://orcid.org//0000-0003-0648-4256

10.3785/j.issn. 1006-754X.2016.06.003

TP 242.2

A

1006-754X(2016)06-0537-07