基于多項(xiàng)式擬合插值函數(shù)的碼垛機(jī)器人軌跡規(guī)劃

夏正亞，洪亮

(南京理工大學(xué)能源與動力工程學(xué)院，江蘇 南京 210094)

?

基于多項(xiàng)式擬合插值函數(shù)的碼垛機(jī)器人軌跡規(guī)劃

夏正亞，洪亮

(南京理工大學(xué)能源與動力工程學(xué)院，江蘇 南京 210094)

以四自由度碼垛機(jī)器人為研究對象，基于關(guān)節(jié)空間提出一種新型多項(xiàng)式擬合插值的運(yùn)動軌跡規(guī)劃方法。該方法不僅能夠保證其速度、加速度甚至沖擊有界且連續(xù)，還可得到不同限制條件下機(jī)器人最短執(zhí)行時間；同時根據(jù)碼垛機(jī)器人的特殊結(jié)構(gòu)和特定運(yùn)動模式，基于機(jī)器人末端運(yùn)行路徑實(shí)現(xiàn)了對關(guān)節(jié)運(yùn)動過程的進(jìn)一步合并優(yōu)化，使得編程控制簡單化?；贛ATLAB軟件對擬合曲線實(shí)現(xiàn)了仿真，分析對比了對稱型及非對稱型擬合曲線各自的性能。最后，在TRIO Motion Perfect軟件環(huán)境下實(shí)現(xiàn)了碼垛機(jī)器人實(shí)驗(yàn)運(yùn)動控制。結(jié)果表明，該運(yùn)動軌跡規(guī)劃方法準(zhǔn)確可行且工作效率高。

碼垛機(jī)器人；軌跡規(guī)劃；多項(xiàng)式擬合；最短時間規(guī)劃

碼垛機(jī)器人作為一種常見的工業(yè)機(jī)器人，其功能是代替人工進(jìn)行物件的轉(zhuǎn)移和堆垛，從而提高勞動生產(chǎn)力，實(shí)現(xiàn)生產(chǎn)過程自動化。隨著生產(chǎn)規(guī)模的擴(kuò)大和自動化水平的提高，對機(jī)器人工作效率提出了更高的要求，同時還要保證整個機(jī)器人系統(tǒng)的穩(wěn)定性和可靠性。因此，軌跡規(guī)劃越來越重要。

軌跡規(guī)劃是指操作臂在運(yùn)動過程中的位移、速度和加速度的曲線輪廓規(guī)劃[1]。在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃是指將關(guān)節(jié)變量表示為時間的函數(shù)來描述操作臂預(yù)期的運(yùn)動，在關(guān)節(jié)空間直接進(jìn)行軌跡規(guī)劃具有很好的實(shí)時性。國內(nèi)外不少學(xué)者在這方面作了大量工作。Kim 等[2]和Lin等[3]利用3次多項(xiàng)式關(guān)節(jié)軌跡規(guī)劃方法,需要解一個(n-2)×(n-2)的對角方程，其中n為軌跡的中間點(diǎn)數(shù)。這種方法的缺點(diǎn)是當(dāng)n較大時計算量大。Bazaz等[4]在3次樣條函數(shù)的基礎(chǔ)上實(shí)現(xiàn)了在線軌跡規(guī)劃，并對軌跡進(jìn)行了時間優(yōu)化設(shè)計，但這種方法不能滿足軌跡點(diǎn)之間的加速度連續(xù)性。

機(jī)器人機(jī)械系統(tǒng)在運(yùn)動過程中必須遵循一個原則，就是運(yùn)行過程盡量平滑，突變的運(yùn)動需要無窮大的動力來實(shí)現(xiàn)，而電機(jī)則因受物理限制不能提供上述動力[5]，這就需要系統(tǒng)函數(shù)具有連續(xù)的一階導(dǎo)數(shù)、二階導(dǎo)數(shù)甚至三階導(dǎo)數(shù)，同時滿足最大速度、加速度和沖擊要非常小，以減小振動[6]。為設(shè)計出性能優(yōu)越的、符合要求的軌跡規(guī)劃曲線，本文引入高階多項(xiàng)式擬合的概念，進(jìn)而推導(dǎo)出滿足速度、加速度和沖擊等運(yùn)動學(xué)約束條件的最短操作時間。同時將非對稱這個概念引入到軌跡規(guī)劃中來，將規(guī)劃推理出的對稱組合曲線進(jìn)行非對稱化，然后分析對比，力求找出滿足不同要求的、性能優(yōu)越的軌跡規(guī)劃曲線。

1 樣機(jī)結(jié)構(gòu)

本文采用的碼垛機(jī)器人實(shí)體樣機(jī)如圖1[7]所示，其機(jī)械系統(tǒng)主要由4個關(guān)節(jié)組成，能實(shí)現(xiàn)基座旋轉(zhuǎn)、水平移動、垂直移動和手腕旋轉(zhuǎn)4種運(yùn)動。機(jī)器人空間運(yùn)動范圍構(gòu)成一圓柱形空間，我們將機(jī)器人由底部至手腕的4個關(guān)節(jié)分別定義為關(guān)節(jié)A、B、C、D。根據(jù)各個關(guān)節(jié)的運(yùn)動模式，碼垛機(jī)器人坐標(biāo)可以用M(R，θ，Z，Ψ)形式表示，其中R代表C的位移量，θ代表A旋轉(zhuǎn)角度，Z代表B位移量，Ψ代表D旋轉(zhuǎn)角度。

圖1 碼垛機(jī)器人實(shí)體樣機(jī)Fig.1 Prototype of the palletizing robot

2 多項(xiàng)式擬合軌跡算法實(shí)現(xiàn)

碼垛機(jī)器人能否快速、精確、穩(wěn)定地從起始位姿到達(dá)終止位姿，軌跡規(guī)劃中控制曲線的選取至關(guān)重要[8]。在位置、速度和加速度甚至沖擊等多約束的條件下，需要高階式的多項(xiàng)式插值。因此，本文采用4次多項(xiàng)式與5次多項(xiàng)式以加減速過程1∶1等比分段擬合的方法，得到對稱型分段擬合函數(shù)。即

(1)

其中

(2)

將這個多項(xiàng)式作為比例項(xiàng)和自變量，描述各個關(guān)節(jié)變量θ在其值域范圍內(nèi)的運(yùn)動，從而有

(3)

其中θ0和θT分別是機(jī)器人在起始和終止位姿的關(guān)節(jié)變量，起始時間令t=0開始，工作時間為T。

可得

(4)

類似有

(5)

(6)

若起始和終止的關(guān)節(jié)速度為0，那么，在PTP運(yùn)動模式條件下，整個運(yùn)動過程應(yīng)滿足以下邊界條件：

(7)

(8)

(9)

(10)

對于較低共振頻率的機(jī)器人關(guān)節(jié)，要實(shí)現(xiàn)動作又要不引起沖擊、不產(chǎn)生殘留振動需要對軌跡曲線進(jìn)行非對稱化[9]。取具有代表性的加減速比2：3，希望通過在減速段將加速度及速度控制在較小值，使機(jī)器人在停止時更加平穩(wěn)，從而降低殘余振動。得到的非對稱型多項(xiàng)式擬合函數(shù)如下：

(11)

同理得到各項(xiàng)系數(shù)值如下：

(12)

若限制電機(jī)的最大轉(zhuǎn)速，由式(4)轉(zhuǎn)換可得給定操作的最短時間T為

(13)

同樣，若限制關(guān)節(jié)允許的加速度，由式(5)可求得給定操作的最短時間T為

(14)

那么，在同時限制關(guān)節(jié)允許速度及加速度的情況下，求得給定操作的最短時間T為

(15)

3 機(jī)器人末端運(yùn)行軌跡的優(yōu)化

碼垛機(jī)器人在正常工作中運(yùn)動模式為PTP運(yùn)動，即只需確定各段運(yùn)動軌跡的端點(diǎn)，端點(diǎn)之間的運(yùn)動軌跡由四軸電機(jī)分別單獨(dú)控制[10]。根據(jù)實(shí)際碼垛過程中壁障要求，將機(jī)器人工作過程的運(yùn)動分為四個階段，如圖2所示。

階段一，B軸運(yùn)動機(jī)器人抓手從生產(chǎn)線抓取位P1出發(fā)上升至一定高度至P2(低于最高點(diǎn)P3)；

階段二，A、B、C、D四軸聯(lián)動機(jī)器人上升至最高點(diǎn)P3;

階段三，A、B、C、D四軸聯(lián)動機(jī)器人下降至堆垛點(diǎn)上方P4；

階段四，B軸運(yùn)動機(jī)器人從碼垛上方下降至目標(biāo)點(diǎn)P5。

圖2 末端運(yùn)行軌跡 Fig.2 Motion track of robot end-effector

在上述四個階段中，在階段一和階段四中只有B關(guān)節(jié)運(yùn)動，而階段二和階段三4個關(guān)節(jié)同時運(yùn)動，也就意味著B關(guān)節(jié)需要做4次加減速。為提高碼垛效率，我們把B關(guān)節(jié)的4個加減速過程合并為兩個加減速過程，即P1→P3和P3→P5兩次運(yùn)動，其中從P2→P4階段四軸同時運(yùn)動不變。

4 實(shí)驗(yàn)驗(yàn)證

以機(jī)器人運(yùn)動學(xué)限制為前提，取s″0=3.5，s″1=0，并運(yùn)用MATLAB軟件編程，運(yùn)行后得到圖3所示的對稱型多項(xiàng)式擬合插值運(yùn)動軌跡曲線和非對稱型多項(xiàng)式擬合插值運(yùn)動軌跡曲線對比圖。表1顯示了機(jī)器人各個關(guān)節(jié)運(yùn)動學(xué)數(shù)據(jù)限制。

從圖3看出，兩種曲線皆具有連續(xù)的速度、加速度及沖擊，且曲線平滑過渡。相對于對稱型多項(xiàng)式擬合插值，采用非對稱型插值軌跡規(guī)劃方法將曲線后半部分進(jìn)行平緩擴(kuò)大，雖然能夠使得機(jī)器人關(guān)節(jié)達(dá)到快速運(yùn)動、緩慢停止的效果，卻是以增大速度的最值為代價的。根據(jù)式(13)，如果限定電機(jī)的最大轉(zhuǎn)速，完成給定操作對稱型擬合函數(shù)所用的時間較短，相對具有更高的工作效率。因此實(shí)際運(yùn)行中可以綜合考慮這兩種軌跡，充分利用各自優(yōu)點(diǎn)，采用最優(yōu)的軌跡。

圖3 插值多項(xiàng)式擬合曲線Fig.3 Fitting curve of the interpolation polynomial

表1 各軸運(yùn)動學(xué)約束Table 1 Kinematics constraints for each joint

把本文的理論研究應(yīng)用于四自由度機(jī)器人上，以非對稱型多項(xiàng)式擬合算法為例，驗(yàn)證其正確性。以TRIO的EURO205x型控制器為系統(tǒng)控制核心，在TRIO Motion Perfect軟件環(huán)境下進(jìn)行運(yùn)動指令編程，實(shí)現(xiàn)機(jī)器人抓包試驗(yàn)，運(yùn)動過程中抓取點(diǎn)P1及放置點(diǎn)P2的關(guān)節(jié)坐標(biāo)由控制示教點(diǎn)的位置確定。實(shí)驗(yàn)條件如下：末端采用50 kg的魚飼料包作為負(fù)載，高度為20 cm；抓取點(diǎn)A、C、D關(guān)節(jié)坐標(biāo)為(80 °，300 mm，10°)，放料托盤中心A、C、D關(guān)節(jié)坐標(biāo)為(140°，600 mm，70 °)；采用相鄰層 3×2 交錯碼垛方式；機(jī)器人上升高度h1為400 mm，下降高度h2為600 mm。Motion Perfect v3示波器實(shí)時追蹤的各關(guān)節(jié)位置以及速度軌跡曲線如圖4和圖5所示。

圖4 各關(guān)節(jié)位移軌跡曲線Fig.4 Displacement curve for each joint

圖5 各關(guān)節(jié)速度軌跡曲線Fig. 5 Velocity curve for each joint

共進(jìn)行10次測試，每次堆垛4層(搬運(yùn)20個來回)。測得碼垛過程中單包最高速度可達(dá)2.55 s ，總時間為39.56 s，平均速度為3.42 秒/包，這樣碼垛效率可達(dá)1 100包/時。通過關(guān)節(jié)運(yùn)動軌跡圖5及圖6看出，各關(guān)節(jié)位移和速度曲線光滑連續(xù)且減速過程平緩，既保證了機(jī)器人運(yùn)動的平穩(wěn)性又滿足了高效碼垛工作的要求，從而驗(yàn)證了運(yùn)動規(guī)劃方法的可行性。

5 結(jié)論

(1)在關(guān)節(jié)空間內(nèi)，通過4、5次多項(xiàng)式擬合運(yùn)動軌跡規(guī)劃保證了高階導(dǎo)數(shù)連續(xù)，避免運(yùn)動突變，同時得到多運(yùn)動學(xué)約束條件下的最短操作時間；

(2)結(jié)合碼垛機(jī)器人自身特點(diǎn)和實(shí)際運(yùn)動需求設(shè)計了抓手基本路徑規(guī)劃，對關(guān)節(jié)運(yùn)動作了合并優(yōu)化，進(jìn)一步提高了碼垛速度，同時更有利于編程控制；

(3)跟據(jù)MATLAB仿真曲線結(jié)果，分析對比本文提出的對稱及非對稱型多項(xiàng)式擬合算法各自性能優(yōu)缺點(diǎn)，提高了算法的適應(yīng)性和可行性；

(4)采用ABT碼垛機(jī)器人進(jìn)行實(shí)驗(yàn)驗(yàn)證，由示波器實(shí)時顯示的運(yùn)動軌跡曲線表明，本文的規(guī)劃方法運(yùn)用于實(shí)際項(xiàng)目中效果顯著。

[1]王小忠, 孟正大. 機(jī)器人運(yùn)動規(guī)劃方法的研究[J]. 控制工程, 2004，11(3): 280-284.

[2]KIM K W, KIM H S, CHOI Y K，et al. Optimization of cubic polynomial Joint trajectories and sliding mode controllers for robots using evolution strategy[C]// IECON97.23rd Intemational Conference on Industrial Electronies, Control and Instrumentation. IEEE ,1997, 3: 9-14.

[3]LIN C S, CHANG P R, LUH J Y S. Formulation and optimization of cubic polynomial joint trajectories for industrial robots[J]. IEEE Transactions on Automatic Control, 1983, 28(12):1066-1074.

[4]BAZAZ S A,TONDU B. Online computing of a robotic manipulator joint trajectory with velocity and acceleration constraints[C]//Proceeding of the 1997 IEEE International Sytnposium on Assembly and Task Planning. IEEE, 1997：1-6.

[5]ANGELES J. 機(jī)器人機(jī)械系統(tǒng)原理理論方法和算法[M]. 北京: 機(jī)械工業(yè)出版社, 2004.

[6]田西勇. 機(jī)器人軌跡規(guī)劃方法研究[D]. 北京: 北京郵電大學(xué), 2008.

[7]曹波, 曹其新, 童上高, 等. 基于時間最優(yōu)的搬運(yùn)機(jī)器人運(yùn)動規(guī)劃[J]. 組合機(jī)床與自動化加工技術(shù), 2012(7): 33-38.

[8]徐海黎, 解祥榮, 莊健, 等. 工業(yè)機(jī)器人的最優(yōu)時間與最優(yōu)能量軌跡規(guī)劃[J]. 機(jī)械工程學(xué)報, 2010, 46(9): 19-25.

[9]曹洋, 方帥, 徐心和. 加速度約束條件下的非完整移動機(jī)器人運(yùn)動控制[J]. 控制與決策, 2006,21(2): 193-196.

[10]邊廣韜, 葉長龍, 趙建國. 機(jī)械手的一種軌跡規(guī)劃[J]. 沈陽工業(yè)大學(xué)學(xué)報, 2001, 23(6): 490-492.

Polynomial fitting interpolation function based trajectory planning for a palletizing robot

XIA Zheng-ya，HONG Liang

( School of Energy and Power Engineering, Nanjing University of Science and Technology, Nanjing 210094, China)

∶We present a new polynomial fitting interpolation function and joint space based trajectory planning with 4-DOF palletizing robot as a subject. It can not only guarantee speed, acceleration and even jerk bound and continuation, but also acquire the shortest execution time for different restricted conditions. We also implement further combination and optimization of joint movement process based on the specific mechanical structure, movement mode and terminal movement route of the robot, so programming control is simple. We then employ MATLAB to simulate fitting curves, and investigate the performance of symmetric and asymmetric fitting curves. Motion control of palletizing robot is eventually realized with software Trio Motion Perfect. Results show that the trajectory planning is accurate, feasible and efficient.

∶palletizing robot; trajectory planning; polynomial fitting; optimal planning time

10.3976/j.issn.1002-4026.2016.05.019

2016-03-22

夏正亞(1992—)，女，碩士，研究方向?yàn)闄C(jī)器人技術(shù)及應(yīng)用。Email：506676702@qq.com

TP242

A