基于多項式擬合插值函數的碼垛機器人軌跡規劃

夏正亞，洪亮

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

?

基于多項式擬合插值函數的碼垛機器人軌跡規劃

夏正亞，洪亮

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

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

碼垛機器人；軌跡規劃；多項式擬合；最短時間規劃

碼垛機器人作為一種常見的工業機器人，其功能是代替人工進行物件的轉移和堆垛，從而提高勞動生產力，實現生產過程自動化。隨著生產規模的擴大和自動化水平的提高，對機器人工作效率提出了更高的要求，同時還要保證整個機器人系統的穩定性和可靠性。因此，軌跡規劃越來越重要。

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

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

1 樣機結構

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

圖1 碼垛機器人實體樣機Fig.1 Prototype of the palletizing robot

2 多項式擬合軌跡算法實現

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

(1)

其中

(2)

將這個多項式作為比例項和自變量，描述各個關節變量θ在其值域范圍內的運動，從而有

(3)

其中θ0和θT分別是機器人在起始和終止位姿的關節變量，起始時間令t=0開始，工作時間為T。

可得

(4)

類似有

(5)

(6)

若起始和終止的關節速度為0，那么，在PTP運動模式條件下，整個運動過程應滿足以下邊界條件：

(7)

(8)

(9)

(10)

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

(11)

同理得到各項系數值如下：

(12)

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

(13)

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

(14)

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

(15)

3 機器人末端運行軌跡的優化

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

階段一，B軸運動機器人抓手從生產線抓取位P1出發上升至一定高度至P2(低于最高點P3)；

階段二，A、B、C、D四軸聯動機器人上升至最高點P3;

階段三，A、B、C、D四軸聯動機器人下降至堆垛點上方P4；

階段四，B軸運動機器人從碼垛上方下降至目標點P5。

圖2 末端運行軌跡 Fig.2 Motion track of robot end-effector

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

4 實驗驗證

以機器人運動學限制為前提，取s″0=3.5，s″1=0，并運用MATLAB軟件編程，運行后得到圖3所示的對稱型多項式擬合插值運動軌跡曲線和非對稱型多項式擬合插值運動軌跡曲線對比圖。表1顯示了機器人各個關節運動學數據限制。

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

圖3 插值多項式擬合曲線Fig.3 Fitting curve of the interpolation polynomial

表1 各軸運動學約束Table 1 Kinematics constraints for each joint

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

圖4 各關節位移軌跡曲線Fig.4 Displacement curve for each joint

圖5 各關節速度軌跡曲線Fig. 5 Velocity curve for each joint

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

5 結論

(1)在關節空間內，通過4、5次多項式擬合運動軌跡規劃保證了高階導數連續，避免運動突變，同時得到多運動學約束條件下的最短操作時間；

(2)結合碼垛機器人自身特點和實際運動需求設計了抓手基本路徑規劃，對關節運動作了合并優化，進一步提高了碼垛速度，同時更有利于編程控制；

(3)跟據MATLAB仿真曲線結果，分析對比本文提出的對稱及非對稱型多項式擬合算法各自性能優缺點，提高了算法的適應性和可行性；

(4)采用ABT碼垛機器人進行實驗驗證，由示波器實時顯示的運動軌跡曲線表明，本文的規劃方法運用于實際項目中效果顯著。

[1]王小忠, 孟正大. 機器人運動規劃方法的研究[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. 機器人機械系統原理理論方法和算法[M]. 北京: 機械工業出版社, 2004.

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

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

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

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

[10]邊廣韜, 葉長龍, 趙建國. 機械手的一種軌跡規劃[J]. 沈陽工業大學學報, 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—)，女，碩士，研究方向為機器人技術及應用。Email：506676702@qq.com

TP242

A