基于網(wǎng)格搜索算法的6-RUS并聯(lián)機器人時間最優(yōu)軌跡規(guī)劃

劉棟財 董廣宇 杜玉紅 李文鵬

摘要：

針對6-RUS并聯(lián)噴涂機器人再現(xiàn)軌跡不平滑、軌跡規(guī)劃效率低等問題，提出了基于優(yōu)化貝塞爾曲線節(jié)點位置的6-RUS并聯(lián)機器人時間最優(yōu)軌跡規(guī)劃方法。首先，將預(yù)處理的軌跡離散化為網(wǎng)格點，更新節(jié)點參數(shù)并優(yōu)化貝塞爾曲線弧長，進一步擬合小線段路徑獲取最優(yōu)幾何路徑；然后，計算不同粗網(wǎng)格點對應(yīng)的最佳速度以及求解時間，選擇合適的粗網(wǎng)格點，進一步以較小步長密化網(wǎng)格點間路徑，迭代求解正反向最大速度，搜索路徑的最佳速度曲線，獲取6-RUS并聯(lián)機器人的最佳運行時間。最后，在自研的6-RUS并聯(lián)機器人平臺上進行實驗。結(jié)果表明，在相同示教軌跡條件下，基于所提的改進貝塞爾曲線算法得到的路徑長為8.12 m，優(yōu)于傳統(tǒng)貝塞爾曲線算法以及G2CBS算法的結(jié)果；同時將改進的時間最優(yōu)軌跡規(guī)劃算法（TOPP）用于優(yōu)化后的示教路徑，所提算法的最優(yōu)速度曲線的求解時間為416.4 ms，與TOPP-RA算法的最優(yōu)速度曲線的求解時間相比縮短了244.7 ms，而且該算法下最優(yōu)軌跡規(guī)劃時間也優(yōu)于TOPP-RA算法，該方法提高了最佳速度的求解速率，縮短了6-RUS并聯(lián)機器人軌跡再現(xiàn)時間，提高了工作效率。

關(guān)鍵詞：6-RUS并聯(lián)機器人；改進的時間最優(yōu)軌跡規(guī)劃；貝塞爾曲線；網(wǎng)格搜索

中圖分類號：TH112

DOI：10.3969/j.issn.1004-132X.2023.13.008

Time-optimal Trajectory Planning of 6-RUS Parallel Robots Based on Grid Search Algorithm

LIU Dongcai1，2 DONG Guangyu1，2 DU Yuhong1，2 LI Wenpeng1，2

1.School of Mechanical Engineering，Tiangong University，Tianjin，300387

2.Key Laboratory of Advanced Mechatronics Equipment Technology，Tianjin，300387

Abstract： Aiming at the problems of uneven reproducing trajectory and low trajectory planning efficiency of 6-RUS parallel painting robots， a trajectory planning method of optimal time of 6-RUS parallel painting robots was proposed based on optimizing the position of Bézier curve nodes. Firstly， the preprocessed trajectory was discretized to grid points， the node parameters were updated and the arc length of Bézier curve was optimized， and the path of small line segment was further fitted to obtain the optimal geometric path. Then， the optimal velocity corresponding to different coarse grid points and the solution time were calculated， and the appropriate coarse grid points were selected. The paths between grid points were further densified by small steps， and the forward and backward maximum velocity were iteratively solved. The maximum feasible velocity curve of the path was searched to obtain the optimal running time of the 6-RUS parallel robots. Finally， the experiments were carried out on a self-developed 6-RUS parallel robot platform. The results show that under the same teaching trajectory， the path length of the improved Bessel curve algorithm herein is as 8.12 m， which is better than that of traditional Bézier curve and G2CBS（G2 continuous cubic Bézier spiral）aigorithm. Meanwhile， the improved time-optimal path parameterization algorithm herein was used for the optimized teaching path. The solution time of the optimal velocity curve of the algorithm herein is as 416.4 ms， which is 244.7 ms less than that of TOPP-RA algorithm. Moreover， the time-optimal trajectory planning time of the algorithm herein is also better than that of TOPP-RA algorithm. The method improves the solving speed of the optimal velocity， shortens the track reproduction time of 6-RUS parallel robots， and improves the working efficiency.

Key words： 6-RUS parallel robot； improved time-optimal path parameterization； Bézier curve； grid search

收稿日期：2022-08-08

基金項目：

天津市科技計劃（21YFFCYS00080）

0 引言

時間最優(yōu)軌跡規(guī)劃是機器人運動控制的重要組成部分，在最優(yōu)化算法下，機器人以最短時間從起始點運動到終點，且運動軌跡平滑。目前機器人噴涂示教再現(xiàn)領(lǐng)域，并聯(lián)機器人運動學(xué)正解復(fù)雜且要求運動精度高，導(dǎo)致計算成本極高，在給定復(fù)雜的再現(xiàn)軌跡信息情況下，機器人不能滿足笛卡兒空間下的實時規(guī)劃的最優(yōu)速度，從而無法達到時間最優(yōu)的運動，同時存在再現(xiàn)軌跡不平滑、路徑較長等問題，影響噴涂質(zhì)量與效率。

在給定約束下，尋求最優(yōu)時間規(guī)劃策略是機器人領(lǐng)域長期關(guān)注的問題。BOBROW等［1］、SHIN等［2］提出了時間最優(yōu)軌跡規(guī)劃（time-optimal path parameterization，TOPP）算法，尋求最短時間完成軌跡或任務(wù)，將機器人動力學(xué)模型表示為軌跡參數(shù)的函數(shù)，并將關(guān)節(jié)力矩的約束轉(zhuǎn)化為軌跡參數(shù)約束，建立了機器人的時間最優(yōu)問題。目前求解時間最優(yōu)問題有三種方法。一是基于龐特里亞金最大化（Pontryagins maximum）原理的數(shù)值積分法［3-4］，在平面（s，s·）下以最大速度和最小速度依次積分求得最優(yōu)軌跡，求解速度相比凸優(yōu)化更快，但存在積分后產(chǎn)物，導(dǎo)致路徑存在奇異點、機器抖動，同時該方法編程相對困難。PHAM［5］提出了一個通用的、快速的、健壯的TOPP算法，解決了動態(tài)奇異的關(guān)鍵問題。二是動態(tài)規(guī)劃法。OBERHERBER等［6］提出了一種機械手的最優(yōu)軌跡路徑動態(tài)規(guī)劃算法。近年來CONSTANTINESCU等［7］、KASERER等［8］將轉(zhuǎn)矩等約束引入動態(tài)規(guī)劃算法［6］進行改進，但約束數(shù)量的增大導(dǎo)致計算時間成倍增加。三是凸優(yōu)化法［9-10］，將時間優(yōu)化問題轉(zhuǎn)化為凸優(yōu)化問題，然后利用凸優(yōu)化包提高求解效率。PHAM等［11］提出了基于可達性分析的時間最優(yōu)軌跡規(guī)劃（time-optimal path parameterization based on reachability analysis，TOPP-RA）算法，通過求解小線性規(guī)劃（ linear programming，LP）遞歸地計算路徑上離散位置的可達集和可控集，但TOPP方法針對小線段、方向變化大且軌跡不平滑的路徑，優(yōu)化效果不明顯，同時該方法求解速度相對慢，仍不滿足實際應(yīng)用的需求。

本文根據(jù)某企業(yè)提供給定噴涂軌跡再現(xiàn)的場景，探討改進貝塞爾曲線算法，優(yōu)化機器人示教軌跡的幾何路徑，基于改進TOPP的時間最優(yōu)的示教軌跡方法，解決并聯(lián)機器人進行軌跡再現(xiàn)效率低、軌跡不平滑等問題，從而獲得6-RUS并聯(lián)機器人噴涂的最優(yōu)時間，并與已有的TOPP-RA算法進行對比分析，在自研實驗平臺上驗證方法的有效性。

1 噴涂工作站

根據(jù)某小型企業(yè)不同的汽車外觀零部件人工拖動噴涂和軌跡再現(xiàn)自動噴涂的要求，筆者設(shè)計了6-RUS并聯(lián)機器人噴涂工作站，如圖1所示。

工作站內(nèi)機器人本體采用6-RUS并聯(lián)機構(gòu)，相對6-PUS、6-PSS并聯(lián)機器人具有工作空間較大、穩(wěn)定性強（結(jié)構(gòu)對稱）、結(jié)構(gòu)載荷?。?qū)動副固連基座）等優(yōu)點。下平臺為動平臺，動平臺實現(xiàn)3個方向的平動和3個方向的轉(zhuǎn)動；上平臺為靜平臺。由6個結(jié)構(gòu)相同的驅(qū)動分支RUU連接2個平臺，機構(gòu)簡圖見圖2、圖3。圖3中，A1～A6 分別是6個轉(zhuǎn)動副位置，B1～B6為驅(qū)動桿與從動桿之間的萬向節(jié)，D1～D6為動平臺上的6個萬向節(jié)。

4 基于網(wǎng)格搜索的6-RUS并聯(lián)機器人噴涂最優(yōu)軌跡規(guī)劃實驗

4.1 實驗平臺

搭建實驗平臺如圖9所示。將預(yù)處理的示教軌跡文件錄入上位機，首先通過改進的貝塞爾曲線對示教軌跡進行平滑與優(yōu)化，然后通過改進的TOPP算法求解給定示教路徑的最佳速度曲線，與TOPP-RA算法對比，評估6-RUS并聯(lián)機器人在給定路徑下的運行時間。

4.2 示教軌跡平滑與優(yōu)化實驗

為了驗證優(yōu)化貝塞爾曲線的弧長最優(yōu)，給定6-RUS并聯(lián)機器人的工作空間、運動約束，截取一段預(yù)處理后的示教軌跡進行實驗，每組實驗重復(fù)進行50次。目前G2CBS［15］算法從運動學(xué)約束獲得約束曲率極限，在節(jié)點處使用路徑修剪和平滑，有較好的平滑和優(yōu)化路徑效果。將本文算法與文獻［15］以及3階貝塞爾曲線算法下的實驗結(jié)果進行對比分析，得到軌跡輸出結(jié)果見圖10。100次實驗的路徑弧長平均值對比見表1。

根據(jù)圖10可知，在工作空間與運動約束下，G2CBS算法和本文算法均對原始示教路徑進行了平滑與優(yōu)化，但本文算法相對于G2CBS算法得到的軌跡曲線更加平滑，G2CBS算法側(cè)重于對節(jié)

點處進行優(yōu)化，本文算法以節(jié)點為參數(shù)，迭代更新參數(shù)已達到曲線整體優(yōu)化的效果。根據(jù)表1可知，在相同約束下，本文算法相對于G2CBS算法以及3階貝塞爾曲線下6-RUS并聯(lián)機器人的示教軌跡更短，更加適合用于路徑時間最優(yōu)化的算法研究。

4.3 基于網(wǎng)格搜索的6-RUS并聯(lián)機器人軌跡參數(shù)化實驗

選取上述已優(yōu)化的示教軌跡路徑進行時間參數(shù)化實驗，給定6-RUS并聯(lián)機器人的關(guān)節(jié)、TCP的速度、加速度、加加速度、電機性能以及關(guān)節(jié)力矩約束條件，以不同的步長設(shè)置初代網(wǎng)格點M為50、100、200、500，分別進行時間參數(shù)化（即路徑離散化），計算滿足約束的每個網(wǎng)格點的最佳速度，消除冗余計算量，即給定示教軌跡信息，使用基于Seidel算法［16］的自定義LP求解器求解TOPP的LPs問題，獲得不同網(wǎng)格點Mi下軌跡的關(guān)節(jié)速度、加速度、TCP最佳速度曲線，如圖11～圖14所示，生成最優(yōu)軌跡時間對比如表2所示。

根據(jù)圖11a、圖12a、圖13a、圖14a可知，同一路徑離散化成不同數(shù)量網(wǎng)格點，6-RUS并聯(lián)機器人的6個關(guān)節(jié)電機旋轉(zhuǎn)角度基本相同，網(wǎng)格點在100之內(nèi)，關(guān)節(jié)速度曲線的曲率相對小，網(wǎng)格點在200、500時，速度曲線的曲率變大，即關(guān)節(jié)速度變快。由圖11b、圖12b可以看出，由于網(wǎng)格點的劃分稀疏，計算的步長相對較大，導(dǎo)致約束不全，使得關(guān)節(jié)加速度曲線不能快速達到理想值，同時曲線呈線性化且抖動大，0～2 ms處關(guān)節(jié)1與2的加速度直接下降，表明此處網(wǎng)格點的約束缺失導(dǎo)致加速度未呈現(xiàn)理想狀態(tài)；由圖13b、圖14b可知，隨著網(wǎng)格點的增加，加速度曲線可快速達到理想值，曲線變得更加平滑，同時加速度變化的節(jié)點的抖動、尖點也相對減少。由圖11c、圖12c、圖13c、圖14c可知，網(wǎng)格點由50到200的情況下，機器人TCP末端的速度曲線更加平滑，同時正反向計算的速度曲線更加健壯，TCP最佳速度曲線對正反向速度曲線跟隨性更好，表明隨著網(wǎng)格點的增加，路徑的約束會不斷健全，同時正反向計算的速度會更加精確，因此可以獲取更多最佳可行速度的上下限約束數(shù)值。但是網(wǎng)格點由200到500的情況下，TCP末端的最佳速度曲線并沒有變化，僅僅關(guān)節(jié)加速度曲線變得更加平滑?？紤]機器人運行成本，本文進一步分析不同網(wǎng)格點下，網(wǎng)格劃分以及求解正反向速度的時間對機器人運行時間的影響，具體如表2所示。

根據(jù)表2可知，隨著網(wǎng)格點的增加，軌跡參數(shù)化的時間基本不變，但正反向速度約束下的最優(yōu)曲線求解時間顯著增加，根據(jù)圖11c、圖12c、圖13c、圖14c可知，網(wǎng)格點從200增加到500并沒有對速度產(chǎn)生明顯的優(yōu)化，因此考慮計算機的算力以及時間成本，可選用網(wǎng)格點200作為初代最佳速度搜索的基準，進一步進行迭代最佳速度優(yōu)化。

4.4 基于網(wǎng)格搜索的6-RUS并聯(lián)機器人軌跡規(guī)劃時間優(yōu)化實驗

根據(jù)實際噴涂工況需求，拖動6-RUS并聯(lián)機器人沿工件移動并采集路徑信息，本文采用改進貝塞爾曲線的節(jié)點位置對原始路徑進行優(yōu)化，獲取最優(yōu)的幾何路徑，如圖15所示。采用本文TOPP算法對機器人軌跡再現(xiàn)時間最優(yōu)化處理，與TOPP-RA算法對比，分析6-RUS并聯(lián)機器人的關(guān)節(jié)角速度、加速度的變化曲線如圖16、圖17所示，機器人的TCP最佳速度曲線如圖18所示。

由圖15可知，基于改進的貝塞爾曲線節(jié)點位置算法的幾何路徑相對于原始路徑更加平滑，同時路徑的長度相對減少，有利于機器人軌跡再現(xiàn)時間的整體優(yōu)化。

根據(jù)圖16可知，基于TOPP-RA算法的關(guān)節(jié)運行曲線出現(xiàn)波浪式前進，本文算法下的各關(guān)節(jié)運行速度曲線相對于TOPP-RA的速度曲線均更加平滑，同時曲線幾乎沒有波動。由圖17可知，由于TOPP-RA算法的約束的冗余性，造成每個網(wǎng)格點的過度約束，導(dǎo)致6-RUS并聯(lián)機器人出現(xiàn)頻繁的加減速，使得加速度曲線的持續(xù)波動。而本文的算法采用初始粗搜索的方法，消除了冗余約束，同時保留了可控約束的完整性，從而得到圖17b所示更加平滑的加速度曲線，優(yōu)化了加減速時間，同時提高了效率。

由圖18可知，TOPP-RA算法與本文算法下的TCP的最佳速度曲線大體相同，但是由圖18a的點劃線框可看到，在速度上限約束條件改變時，TOPP-RA算法下的最佳速度的求解較慢，導(dǎo)致最佳速度的應(yīng)激變化較慢，即對速度上限的跟隨性不好，從而造成6-RUS并聯(lián)機器人的運行速度降低；機器人運行時間在500～850 ms、2000～2500 ms區(qū)間時，由于6-RUS并聯(lián)機器人關(guān)節(jié)加速度頻繁升降（圖17a），導(dǎo)致TCP的速度曲線出現(xiàn)抖動，影響機器人的運行速度，而本文算法消除了加減速帶來的影響，TCP的速度曲線均更加平滑。

在相同的幾何路徑下，TOPP-RA算法的運行時間為2986 ms，本文算法的運行時間為2440 ms。為了對比TOPP-RA算法與本文算法的最佳可行速度求解時間，本文進一步分析最佳速度求解時間與機器人運行時間，以初代搜索的200個網(wǎng)格點為例，如表3所示。

由表3可知，在相同的示教路徑下，基于本文算法的6-RUS并聯(lián)機器人運行時間相對于基于TOPP-RA算法的運行時間少506 ms；TOPP-RA算法的最佳速度曲線求解時間為661.1 ms，本文算法在初代粗搜索時，僅用21.4 ms即獲取初代網(wǎng)格點下的機器人的可行速度；進一步對網(wǎng)格段迭代計算速度曲線，用時395 ms，即本文算法最佳速度曲線的計算時間為416.4 ms，比TOPP-RA的時間少244.7 ms。

由此可知，本文算法的最佳速度求解時間相對于TOPP-RA算法的最佳速度求解時間更優(yōu)。同時由于本文的速度與加速度的持續(xù)穩(wěn)定性，優(yōu)化了最優(yōu)軌跡規(guī)劃時間，在實際應(yīng)用過程，采用本文算法的6-RUS并聯(lián)機器人的運行時間相對較短，提高了工作效率。

5 結(jié)論

（1）針對6-RUS并聯(lián)機器人示教軌跡再現(xiàn)應(yīng)用場景，提出了一種基于優(yōu)化貝塞爾曲線節(jié)點位置的改進時間最優(yōu)軌跡規(guī)劃方法。

（2）本文通過優(yōu)化貝塞爾曲線節(jié)點參數(shù)，建立弧長最小化的目標(biāo)函數(shù)，同時給定曲線相關(guān)的約束，利用模擬退火優(yōu)化求解器計算每個節(jié)點的弧長最小值，最后擬合得到最優(yōu)的示教軌跡，為進一步進行示教軌跡的時間優(yōu)化提供基礎(chǔ)。

（3）提出了一種改進的時間最優(yōu)軌跡方法，通過求解初代網(wǎng)格點下的機器人最佳速度簡化冗余約束計算，以及迭代密化求解整體路徑的最佳可行速度，提高了機器人最佳速度的計算效率，同時優(yōu)化了機器人運行速度與加速度抖動問題，縮短了6-RUS并聯(lián)機器人的最優(yōu)軌跡規(guī)劃時間。

（4）實驗結(jié)果表明，相同的示教軌跡下，基于優(yōu)化貝塞爾曲線節(jié)點位置算法得出的路徑長度為8.12 m，小于3階貝塞爾曲線和G2CBS算法下的路徑長度；在已優(yōu)化軌跡下，基于本文算法的最佳速度搜索時間為416.4 ms，優(yōu)于TOPP-RA算法的244.7 ms，基于本文算法的最優(yōu)軌跡規(guī)劃時間也優(yōu)于TOPP-RA算法的最優(yōu)軌跡規(guī)劃時間，可在最短時間內(nèi)達到預(yù)期速度并進入穩(wěn)態(tài)，提高了機器人運行效率。該方法為并聯(lián)機器人示教軌跡時間最優(yōu)規(guī)劃提供了新的思路。

參考文獻：

［1］ BOBROW J， DUBOWSKY S， GIBSON J. Time-optimal Control of Robotic Manipulators along Specified Paths［J］. The International Journal of Robotics Research， 1985， 4（3）：3-17.

［2］ SHIN K， MCKAY N. Selection of Near-minimum Time Geometric Paths for Robotic Manipulators［J］. IEEE Transactions on Automatic Control， 1986， 31（6）：501-511.

［3］ PHAM Q C. Characterizing and Addressing Dynamic Singularities in the Time-optimal Path Parameterization Algorithm［C］∥2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. Tokyo， 2013：2357-2363.

［4］ KUNZ T， STILMAN M. Time-optimal Trajectory Generation for Path Following with Bounded Acceleration and Velocity［M］∥AGARWAL P， KUMAR S， RYDE J， et al. Robotics：Science and Systems Ⅷ. Cambridge：MIT Press， 2013：209-216.

［5］ PHAM Q C. A General， Fast， and Robust Implementation of the Time-optimal Path Parameterization Algorithm［J］. IEEE Transactions on Robotics， 2014， 30（6）：1533-1540.

［6］ OBERHERBER M， GATTRINGER H， MLLER A. Successive Dynamic Programming and Subsequent Spline Optimization for Smooth Time Optimal Robot Path Tracking［J］. Mechanical Sciences， 2015， 6（2）：245-254.

［7］ CONSTANTINESCU D， CROFT E A. Smooth and Time-optimal Trajectory Planning for Industrial Manipulators along Specified Paths［J］. Journal of Robotic Systems， 2000， 17（5）：233-249.

［8］ KASERER D， GATTRINGER H， MULLER A. Nearly Optimal Path Following with Jerk and Torque Rate Limits Using Dynamic Programming［J］. IEEE Transactions on Robotics， 2019， 35（2）：521-528.

［9］ VERSCHEURE D， DEMEULENAERE B， SWEVERS J， et al. Time-optimal Trajectory Planning for Robots：a Convex Optimization Approach［J］. IEEE Transactions on Automatic Control， 2009， 54（10）：2318-2327.

［10］ HAUSER K. Fast Interpolation and Time-optimization with Contact［J］. The International Journal of Robotics Research， 2014， 33（9）：1231-1250.

［11］ PHAM H， PHAM Q C. A New Approach to Time-optimal Path Parameterization Based on Reachability Analysis［J］. IEEE Transactions on Robotics， 2018， 34（3）：645-659.

［12］ HOBBYJ D. Smooth， Easy to Compute Interpolating Splines［J］. Discrete & Computational Geometry， 1986， 1（2）：123-140.

［13］ CLAPHAM C， NICHOLSON J， NICHOLSON J R. The Concise Oxford Dictionary of Mathematics［M］. Oxford：Oxford University Press， 2014.

［14］ KIRKPATRICK S， GELATT JR C D， VECCHI M P. Optimization by Simulated Annealing［J］. Science， 1983， 220（4598）：671-680.

［15］ YANGK， JUNG D， SUKKARIEH S. Continuous Curvature Path-smoothing Algorithm Using Cubic Bezier Spiral Curves for Non-holonomic Robots［J］. Advanced Robotics， 2013， 27（4）：247-258.

［16］ SEIDEL R. Small-dimensional Linear Programming and Convex Hulls Made Easy［J］. Discrete & Computational Geometry， 1991， 6：423-434.

（編輯 王旻玥）

作者簡介：

劉棟財，男，1999年生，碩士研究生。研究方向為并聯(lián)機器人柔順控制。E-mail：Liudongcai_119591@163.com。

杜玉紅（通信作者），女，1974年生，教授、博士研究生導(dǎo)師。研究方向為機器人技術(shù)與智能控制及圖像處理，工業(yè)機器人控制。發(fā)表論文20余篇。E-mail：dyh202@163.com。