潘海鴻，李煥楷，何飛龍，梁 安，陳 琳

(1.廣西大學機械工程學院，南寧 530004；2. 廣西安博特智能科技有限公司，南寧 530007)

0 引言

軌跡規(guī)劃作為機器人學基礎，軌跡的優(yōu)劣對作業(yè)完成質量的好壞有著重要影響。按軌跡所在空間區(qū)分，軌跡規(guī)劃分為笛卡爾空間軌跡規(guī)劃與關節(jié)空間軌跡規(guī)劃[1-3]。

笛卡爾空間軌跡規(guī)劃對末端執(zhí)行器的運動軌跡進行規(guī)劃，具有TCP運動平滑且插補位置精度高的特點。LEE等[4]提出基于卷積的加減速算法，利用物理系統限制仿真得到滿足系統要求的合成軌跡；陳琳等[5]根據速度前瞻控制與非對稱S曲線加減速控制實現對軌跡段間銜接進給速度的平滑處理，保證加工效率并提高加工精度與平滑性；蘇婷婷等[6]利用PH曲線對笛卡爾空間軌跡進行過渡，后采用3-4-5次多項式對進行軌跡規(guī)劃，有效提高軌跡平滑性；潘海鴻等[7]提出snap連續(xù)的全類型非對稱七段式正弦曲線加減速控制算法，提高系統柔性，解決始末速度可達性校驗及修正問題。笛卡爾空間軌跡規(guī)劃雖能滿足機器人運動控制系統平滑性加工要求，但需要預測運動狀態(tài)節(jié)點，存在精插補計算較復雜的缺點。

關節(jié)空間軌跡規(guī)劃直接規(guī)劃關節(jié)運動，主要采用多項式或樣條曲線插值進行規(guī)劃。劉寶等[8]提出一種三階導數連續(xù)的改進三次樣條軌跡規(guī)劃方法，仿真驗證改進的方法有效減小了關節(jié)軸的殘余振動；朱世強等[9]采用B樣條曲線插值方法構造啟動和停止運動參數可控，且C2連續(xù)的關節(jié)軌跡，使機器人平穩(wěn)跟蹤任務空間的任意指定軌跡； ZHANG等[10]根據混合逆運動學算法計算關節(jié)角關鍵節(jié)點，采用連續(xù)五次B樣條曲線算法規(guī)劃點到點進給運動，有效規(guī)劃關節(jié)空間中的時間最優(yōu)與平滑軌跡；張程等[11]利用多項式軌跡插值算法進行關鍵點間軌跡規(guī)劃，規(guī)劃得到軌跡具備良好的平穩(wěn)性與快速性；丁云鵬等[12]改進五次多項式插值算法規(guī)劃機器人各關節(jié)運動，保證各關節(jié)準確、穩(wěn)定運動。關節(jié)空間軌跡規(guī)劃計算簡單，效率高，不用考慮關節(jié)奇異點以及冗余問題，但TCP軌跡精度較難保證。

針對機器人笛卡爾空間軌跡規(guī)劃精插補計算復雜以及關節(jié)空間軌跡規(guī)劃TCP軌跡精度較難保證問題，綜合笛卡爾空間與關節(jié)空間規(guī)劃方法，在笛卡爾空間對機器人TCP軌跡進行等距粗插補以及加減速補預處理，引入Hermite插值法[13]對機器人各關節(jié)運動進行軌跡規(guī)劃，并在自主研發(fā)的RTX64機器人運動控制器上進行驗證。

1 機器人末端連續(xù)平滑軌跡規(guī)劃

提出的Hermite插值實現機器人連續(xù)平滑軌跡規(guī)劃流程如圖1所示。

圖1 Hermite插值實現機器人連續(xù)平滑軌跡規(guī)劃流程

要使關節(jié)空間運動軌跡平滑，減少關節(jié)運動的沖擊和振動，延長伺服系統壽命，需要在笛卡爾空間軌跡預規(guī)劃的基礎上，再進行關節(jié)空間軌跡規(guī)劃。為此，先在笛卡爾空間對機器人TCP路徑進行等距粗插補與加減速預處理；而后將機器人TCP粗插補點位置與時間信息通過逆運動學映射到關節(jié)空間中；最后再對關節(jié)空間各關節(jié)運動進行Hermite插值規(guī)劃。

2 笛卡爾空間軌跡預規(guī)劃

2.1 笛卡爾空間基于弓高誤差等距粗插補處理

機器人TCP運動有位置精確、速度平滑的加工要求，為滿足位置加工要求，在笛卡爾空間對機器人路徑進行等距粗插補處理。對于存在曲率的路徑采用基于弓高誤差限制的等距粗插補，以確保位置精度，如圖2所示。

圖2 曲率路徑插補示意圖

基于弓高誤差限制的插補步距steph計算公式為：

(1)

2.2 笛卡爾空間等距加減速預處理

為獲取每個粗插補點對應運行時間信息，在笛卡爾空間采用五段式S型加減速算法對機器人TCP軌跡進行等距加減速預處理，如圖3所示。

圖3 TCP軌跡等距加減速預處理

已知TCP路徑運行總距離L，始末速度Vs、Ve，目標運行速度Vrun，最大加速度amax，最大加加速度Jmax，計算粗插補點Pi對應運行距離Li=i·step，通過S型加減速運算即可求解TCP運動至粗插補點Pi所需時間ti。

3 關節(jié)空間Hermite插值軌跡規(guī)劃

關節(jié)是機器人運動的最小單元以及實現機器人運動的基礎，為確保機器人TCP在笛卡爾空間運動平滑并且機器人各關節(jié)軌跡平滑，引入Hermite插值算法分別對各關節(jié)進行規(guī)劃，使各關節(jié)同一時刻運動至相應節(jié)點，實現各關節(jié)同步運動。

Hermite插值算法的性質：

(1)插值多項式在節(jié)點處等于給定函數值；

(2)在節(jié)點處，插值多項式的一階直至指定階的導數值與被插值函數的對應階次導數值相等。

Hermite插值等時軌跡規(guī)劃流程如圖4所示。

圖4 關節(jié)空間Hermite等時插值軌跡規(guī)劃

為防止插值多項式因次數過高產生龍格現象導致機器人各關節(jié)運動波動影響加工精度，提出采用分段式三次Hermite插值對各關節(jié)軌跡規(guī)劃。當t∈[ti,ti+1](i=0,1,2,…,n-1)，三次Hermite插值多項式如式2所示。

(2)

(3)

(4)

(5)

在以下情況需對邊界點速度進行判定修正：

4 實驗驗證與分析

4.1 實驗平臺

搭建機器人軌跡測試實驗平臺如圖5所示。該平臺以Win10+RTX64為控制系統，上位機采用RTX運動控制器，數據下發(fā)與采樣周期均為1 ms；采用清能德創(chuàng)(CDR6-A1505)伺服驅動系統，6R機器人本體型號為埃夫特ER6B-C10。

圖5 實驗平臺

4.2 直線運動

直線運動測試路徑點信息如表1所示。

表1 直線運動路徑點位姿坐標

其中系統設定TCP運行速度為30 mm/s，最大加速度120 mm/s2，最大加加速度480 mm/s3。起點A、終點B速度為0 mm/s。系統設定插補步距0.9 mm，經等距粗插補得到粗插補點位姿并通過逆運動學求解獲取對應機器人各關節(jié)位置信息，而后通過S型加減速計算粗插補點對應的時間信息。

獲取各關節(jié)對應的時間位置信息后，通過Hermite插值對各關節(jié)每個粗插補段進行軌跡規(guī)劃，測試得到Hermite規(guī)劃各關節(jié)速度曲線，如圖6所示，在加速段與減速段過渡平滑無突變，各關節(jié)運動沖擊振動小。

圖6 直線運動機器人各關節(jié)速度曲線

各關節(jié)合成的TCP運動速度曲線如圖7所示，與S型加減速曲線如圖3所示一致，TCP速度曲線在加速段與減速段過渡平滑無突變，進入勻速段TCP速度達到系統設定速度30 mm/s，TCP運動平滑。

圖7 直線運動TCP速度曲線

笛卡爾空間至關節(jié)空間的軌跡規(guī)劃存在一定耦合性，重復測試獲取直線運動精插補點位姿數據，而后進行軌跡準確度測試[14]，驗證提出方法對笛卡爾空間軌跡精度影響程度，測得軌跡準確度ATP<1 μm如圖8所示，滿足精度要求。

圖8 直線運動軌跡準確度

實驗結果表明，提出的算法在保證各關節(jié)同啟同停且運動曲線連續(xù)平滑條件下，TCP運動平滑達到系統設定條件，滿足正常加工要求。

4.3 多段連續(xù)軌跡運動

多段連續(xù)軌跡測試路徑由三段直線與一段圓弧組成如圖9所示，路徑點信息如表2所示。

圖9 連續(xù)軌跡路徑示意圖

表2 連續(xù)軌跡路徑點位姿坐標

其中系統設定各段路徑TCP運行速度為30 mm/s。起點/終點A、銜接點B和銜接點C速度為0 mm/s；銜接點E速度為8.56 mm/s。

分別采用Hermite插值、三次多項式、五次多項式對多段連續(xù)軌跡進行規(guī)劃，得到關節(jié)運動曲線，其中關節(jié)1與關節(jié)3速度曲線如圖10所示，三次和五次多項式規(guī)劃的速度曲線在銜接點B、C、E處速度存在波動，而Hermite規(guī)劃的關節(jié)速度曲線在這些銜接點處速度過渡平滑，關節(jié)運行平穩(wěn)。

圖10 連續(xù)軌跡規(guī)劃關節(jié)1與關節(jié)3速度曲線

通過3種不同規(guī)劃方法得到的TCP速度與加速度曲線如圖11和圖12所示。3種方法規(guī)劃TCP勻速運行后速度都能達到系統設定的速度30 mm/s，但Hermite規(guī)劃的TCP運動曲線在加速與減速過程中平滑無波動，銜接點B、C、E速度平滑過渡，但三次與五次多項式規(guī)劃的TCP速度曲線在這些銜接點處波動明顯(圖11)。運動過程加速度曲線變化圖12中采用Hermite規(guī)劃的TCP加速度幅值約是三次與五次多項式的1/3，且其加速度波動明顯比傳統多項式2種方法低。

圖11 連續(xù)軌跡規(guī)劃TCP速度曲線 圖12 連續(xù)軌跡規(guī)劃TCP加速度曲線

上述兩個實驗結果表明提出的Hermite插值規(guī)劃算法在保證各關節(jié)運動連續(xù)平滑條件下，合成TCP運動平滑，相較傳統多項式規(guī)劃，TCP運動在加速段與減速段速度波動明顯降低，加速度幅值降低約66%，減少了加工過程中的沖擊與振動。

5 結論

(1)為保證機器人TCP軌跡精度，提出笛卡爾空間機器人末端軌跡等距粗插補與等距加減速預處理算法，確定TCP粗插補點時間、位置信息。

(2)為保證機器人各關節(jié)運動平滑，引入Hermite插值法規(guī)劃機器人各關節(jié)運動軌跡并進行等時精插補。

(3)在自主開發(fā)的RTX64機器人運動控制系統實驗平臺上進行算法驗證可知，對比傳統多項式加減速軌跡規(guī)劃方法，所提出方法可有效規(guī)劃機器人運動平滑運動曲線，降低加速度波動，抑制機器人運動過程中產生的沖擊與振動，提高機器人加工精度，延長伺服系統壽命。